一种基于时序采样评价的多源数据无缝融合定位方法与流程

本发明涉及融合定位，尤其涉及一种基于时序采样评价的多源数据无缝融合定位方法。

背景技术：

1、随着室内外一体化定位技术的发展，单一定位数据源已难以满足复杂环境下对定位精度、稳定性及连续性的要求，因此越来越多的定位系统采用融合定位技术，将超宽带（uwb）的tof、tdoa定位数据、ble信号强度数据、gnss定位结果以及惯性测量单元（imu）数据等多种定位信息进行联合解算，以提高定位系统在不同场景下的适应能力。现有融合定位系统通常基于扩展卡尔曼滤波（ekf）、无迹卡尔曼滤波（ukf）等滤波算法，对不同来源的定位观测进行统一状态估计，并实现室内外定位方式之间的切换以及多种定位数据源之间的融合。

2、然而，在实际无线定位场景中，不同定位数据源的采样质量会随着环境变化而发生波动。受基站同步误差、时钟抖动、遮挡、多径传播、非视距传播、标签运动状态变化以及链路瞬时异常等因素影响，同一时刻不同数据源之间的观测质量可能存在较大差异，同一数据源连续采样之间也可能出现突变、漂移、离群或短时失真等情况。如果仍按照固定权重或固定噪声模型直接将各类原始采样输入融合滤波器，则容易造成预测状态与观测之间残差异常增大、量测噪声模型与实际采样质量不匹配，导致融合定位结果产生抖动，严重时甚至引起滤波发散。同时，现有融合定位方法通常缺乏针对采样历史稳定性、预测一致性以及物理几何合理性的综合评价机制，难以根据采样质量动态调整不同观测数据的使用方式、参与权重及量测噪声，因此难以兼顾定位精度、稳定性和连续性。

技术实现思路

1、发明目的：提出一种基于时序采样评价的多源数据无缝融合定位方法，以解决现有技术存在的上述问题。

2、本发明提出的一种基于时序采样评价的多源数据无缝融合定位方法，包括如下步骤：

3、s1、获取当前采样帧的多源定位数据，并将所述多源定位数据统一封装至ekfsample结构体中；其中，所述多源定位数据包括uwb系统的tof数据、tdoa数据、ble信号强度数据、gnss定位结果、imu数据以及约束数据；

4、s2、根据所述ekfsample结构体中的各类采样数据，调用与各类采样数据对应的量测模型，结合状态模型建立当前采样帧的融合滤波预测状态，并获得对应的预测观测值；

5、s3、对所述tof数据、ble信号强度数据以及tdoa数据建立观测实体，对每个观测实体维护对应的历史状态信息，并依据当前采样数据与历史状态信息计算时序采样评价结果；所述时序采样评价结果包括时序波动项、残差项以及物理几何项；

6、s4、依据所述时序采样评价结果确定对应采样数据的使用方式、权重大小以及量测噪声调整幅度，并根据调整后的量测噪声完成融合滤波量测更新，获得融合定位结果；

7、s5、输出所述融合定位结果，并根据所述融合定位结果输出平滑后的连续定位结果。

8、作为一个优选方案，步骤s1具体包括：

9、获取当前采样帧对应的tof数据、ble信号强度数据、tdoa数据、gnss定位结果、imu数据以及约束数据；

10、其中，所述gnss定位结果包括经纬高坐标；

11、所述imu数据包括速度、累积位移量以及参考航向角；

12、将所述多源定位数据按照统一数据接口填充至ekfsample结构体，由融合定位模块依据输入数据类型自动选择对应量测模型参与融合解算。

13、作为一个优选方案，步骤s3中，所述时序波动项的计算具体包括：

14、针对每个所述观测实体维护历史观测窗口；

15、根据当前观测值与历史观测值计算瞬时跳变指标；

16、根据当前观测值与滑动窗口中位数计算窗口异常偏离指标；

17、根据滑动窗口均值、滑动窗口标准差以及滑动窗口极差计算短时波动强度指标；

18、根据当前观测与指数加权移动平均均值之间的偏差计算观测连续性指标；

19、依据所述瞬时跳变指标、窗口异常偏离指标、短时波动强度指标以及观测连续性指标形成所述观测实体对应的时序波动项。

20、作为一个优选方案，步骤s3中，所述残差项的计算具体包括：

21、根据所述预测观测值与实际观测值计算观测残差；

22、根据所述观测残差及观测残差协方差计算归一化残差；

23、统计历史残差均值、历史残差方差、连续大残差次数以及残差变化趋势；

24、其中：

25、对于tof数据，根据所述残差项检测非视距误差；

26、对于ble信号强度数据，根据所述残差项分析残差偏置；

27、对于tdoa数据，根据所述残差项分析基站对残差持续偏移及参考体系一致性；

28、依据所述观测残差、归一化残差、历史残差均值、历史残差方差、连续大残差次数以及残差变化趋势形成所述残差项。

29、作为一个优选方案，步骤s3中，所述物理几何项的计算具体包括：

30、判断当前观测值是否位于对应数据源允许的观测范围内；

31、判断观测变化是否满足最大运动速度约束；

32、判断观测是否满足对应的物理关系；

33、计算观测模型雅可比矩阵对应的观测灵敏度；

34、根据观测方向筛选空间方向差异较大的观测；

35、计算观测子集的空间分布均衡性；

36、计算估计点到目标曲线的最短距离；

37、依据所述观测范围、运动速度约束、物理关系、观测灵敏度、观测方向、空间分布均衡性以及估计点到目标曲线的最短距离形成所述物理几何项。

38、作为一个优选方案，步骤s4具体包括：

39、根据所述时序波动项、残差项以及物理几何项形成所述观测实体对应的时序采样评价结果；

40、依据所述时序采样评价结果确定对应采样数据是否参与本次滤波更新；

41、对于参与滤波更新的采样数据，根据所述时序采样评价结果确定对应采样数据的量测权重；

42、依据所述量测权重调整量测噪声矩阵；

43、采用调整后的量测噪声矩阵完成量测更新，输出融合定位结果。

44、作为一个优选方案，步骤s5具体包括：

45、将所述融合定位结果输入平滑器；

46、采用二维pv状态模型作为平滑器状态模型；

47、以所述融合定位结果作为量测量；

48、根据融合质量自适应调节观测噪声；

49、根据静止识别结果切换平滑等级；

50、根据连续性机制限制每帧输出位移；

51、输出平滑后的连续定位结果。

52、作为一个优选方案，所述gnss定位结果参与融合滤波之前，还包括：

53、获取多个控制点对应的uwb坐标及经纬高坐标；

54、将所述uwb坐标与所述经纬高坐标借助站心坐标系进行转换；

55、计算对应的旋转矩阵、平移向量及缩放因子；

56、依据所述旋转矩阵、平移向量及缩放因子，将所述gnss定位结果转换至uwb坐标系。

57、作为一个优选方案，在步骤s1获取当前采样帧的多源定位数据之后，还包括初始点估计步骤，所述初始点估计步骤具体包括：

58、获取当前可用的多源定位数据；

59、当存在可用tof数据时，根据多条tof数据进行三角定位获得初始点，并通过迭代算法逼近测距交点坐标；

60、当不存在可用tof数据且存在可用tdoa数据时，根据tdoa数据计算基站均值坐标作为初始点，并通过迭代算法逼近交点坐标；

61、当不存在可用uwb数据且存在满足条件的ble信号强度数据时，根据ble信标坐标确定初始点；

62、当不存在可用ble信号强度数据时，根据gnss定位结果确定初始点；

63、将所述初始点作为所述状态模型的初始状态。

64、作为一个优选方案，所述约束数据包括0维点约束和1维线约束，

65、所述约束数据具有对应的作用范围及约束程度；

66、当估计点位于约束作用范围内时，将所述约束数据作为量测信息参与融合滤波；

67、估计点与对应约束的距离越近，约束程度越高；

68、多个所述约束数据能够同时作用于所述估计点，以降低点位波动。

69、与现有技术相比，本发明至少具备如下有益效果：

70、（1）本发明在融合滤波量测更新之前引入时序采样评价机制，结合采样的时序波动特征、预测残差以及物理几何特性，对各类观测数据进行综合质量评价，并依据评价结果确定对应采样数据的使用方式、参与权重及量测噪声调整幅度，从而能够根据采样质量动态优化融合滤波过程，提高不同定位数据源的利用效率。

71、（2）本发明通过对tof、ble信号强度及tdoa等多源观测分别维护历史观测信息及历史状态信息，综合计算时序波动项、残差项及物理几何项，实现对观测数据连续稳定的质量评估，能够有效降低异常采样、离群采样及短时失真采样对融合定位结果的影响，提高融合定位结果的稳定性。

72、（3）本发明支持多种定位数据源统一融合处理，并根据当前采样情况自动调用对应量测模型参与融合解算，实现不同定位方式之间的平滑切换以及室内外定位场景之间的稳定切换，提高复杂环境下融合定位系统的适用性。

73、（4）本发明通过融合定位结果输出后的平滑处理机制，在保持融合定位实时性的基础上，提高静止场景、短时中断恢复场景及轨迹跳变场景下定位轨迹的连续性和显示稳定性，进一步改善定位结果的观感。