一种数-模混合估计的多源融合定位方法及系统

本技术涉及融合定位，具体涉及一种数-模混合估计的多源融合定位方法及系统。

背景技术：

1、在当今自动驾驶和机器人领域，实现准确、鲁棒的定位是关键问题之一。随着自主移动平台的迅速发展，特别是在复杂和未知环境中，高精度的定位和导航成为了确保安全和可靠性的关键任务。为了实现这一目标，同时构建未知环境的三维地图并进行实时定位，即同步定位与地图构建（slam），已经成为一种广泛采用的策略。

2、光探测和测距（lidar）传感器由于其能够直接提供高精度的三维信息，受到了自动驾驶和机器人领域的广泛应用。相较于基于相机的方法，lidar-slam方法在光照条件不稳定的情况下仍然能够实现准确感知，因此在场景感知中具备更高的鲁棒性。然而，当环境中的场景特征退化或缺失时，传统的lidar定位方法容易产生漂移误差，导致定位不准确。

3、为了克服这些局限性，近年来，紧密耦合lidar-imu融合技术受到了广泛研究。该技术将激光雷达和惯性测量单元（imu）的测量数据结合，利用imu的加速度计和陀螺仪数据来提供姿态信息和运动状态的估计，从而弥补lidar在一些情况下的不足。然而，现有的lidar-imu融合方法主要基于滤波或优化技术进行定位估计。这些方法通常仅使用传感器的几何信息，如深度、加速度、陀螺仪等，未充分挖掘传感器数据的高层次观测特征信息。而基于模型驱动的方法虽然具有较高的稳定性和效率，但过于依赖领域知识和模型假设的准确性，并且通常需要手动调整参数。同时，现有的定位方法很少考虑到结果的不确定度问题。当传感器故障发生或者环境缺乏足够的特征时，由于缺少不确定度估计，系统可能无条件地接受不准确的预测结果，从而导致依赖该输出的系统容易出现故障。

技术实现思路

1、本技术提供一种数-模混合估计的多源融合定位方法及系统，其可自适应调整滤波器参数，增强了定位系统在复杂场景和运动状态下的鲁棒性，且综合考虑了传感器数据和模型的不确定度，能提供更可靠的结果置信度。

2、第一方面，本技术实施例提供一种数-模混合估计的多源融合定位方法，所述一种数-模混合估计的多源融合定位方法包括：

3、根据imu数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、imu不确定度因子和伪观测因子；

4、基于相邻两次采集的点云数据，利用lidar数据驱动模块提取不同激光之间的变化特征以提供lidar场景全局观测，并计算当前时刻的场景特征退化率；

5、根据所述imu不确定度因子、场景特征退化率以及基于ekf模型驱动的协方差，确定imu数据驱动模块、lidar数据驱动模块和ekf模型驱动模块的不确定度；

6、根据所述传播调整因子动态调整ekf模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行ekf模型驱动模块的观测更新，并结合不确定度以进行位姿估计。

7、结合第一方面，在一种实施方式中，所述基于相邻两次采集的点云数据，利用lidar数据驱动模块提取不同激光之间的变化特征以提供lidar场景全局观测，并计算当前时刻的场景特征退化率，包括：

8、同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；

9、将二维距离图像输入至lidar数据驱动模块的lidar全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。

10、结合第一方面，在一种实施方式中，所述根据所述imu不确定度因子、场景特征退化率以及基于ekf模型驱动的协方差，确定imu数据驱动模块、lidar数据驱动模块和ekf模型驱动模块的不确定度，包括：

11、根据公式：，计算所述不确定度 u，其中， a 2为imu不确定度因子， p为场景特征退化率， σ为基于ekf模型驱动的协方差。

12、结合第一方面，在一种实施方式中，所述根据所述传播调整因子动态调整ekf模型驱动模块传播预测时的噪声协方差，包括：

13、根据公式： q bw ′   = q bw × 10^(a 1 w )，更新陀螺仪噪声 b w的噪声协方差 q bw；

14、根据公式： q ba ′   = q ba × 10^(a 1 a )，更新加速度计噪声 b a的噪声协方差 q ba；

15、其中， a 1 w为陀螺仪传播调整因子， a 1 a为加速度计传播调整因子。

16、结合第一方面，在一种实施方式中，所述当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行ekf模型驱动模块的观测更新，包括：

17、构造速度约束：

18、 y k hor = h hor (x k ) + n k hor

19、 y k ver = h ver (x k ) + n k ver

20、其中， y k hor和 y k ver分别为伪测量值 y k水平方向和垂直方向的分量， h(·)为ekf模型驱动模块的观测函数， n k为服从高斯分布的第 k个观测噪声，对应的协方差矩阵为 r 2×2，且 r 2 ×2满足：

21、 r ’ 11 = r 11 × 10 ^ (a 3 hor )

22、 r ’ 22 = r 22 × 10 ^ (a 3 ver )

23、其中， a 3 hor为伪观测因子水平方向的分量， a 3 ver为伪观测因子垂直方向的分量；

24、使用伪测量值[ y k hor , y k ver] =[ 0,0]为状态更新提供过滤器。

25、第二方面，本技术实施例提供一种数-模混合估计的多源融合定位系统，所述数-模混合估计的多源融合定位系统包括：

26、imu数据驱动模块，其用于采集连续多帧采集的陀螺仪的角速度和加速度计的加速度，以回归当前时刻的传播调整因子、imu不确定度因子和伪观测因子；

27、lidar数据驱动模块，其基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供lidar场景全局观测，并计算当前时刻的场景特征退化率；

28、ekf模型驱动模块，其根据所述传播调整因子动态调整传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行观测更新；

29、所述ekf模型驱动模块还根据所述imu不确定度因子、场景特征退化率以及基于ekf模型驱动的协方差，确定imu数据驱动模块、lidar数据驱动模块和ekf模型驱动模块的不确定度。

30、结合第二方面，在一种实施方式中，所述lidar数据驱动模块基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供lidar场景全局观测，并计算当前时刻的场景特征退化率，包括：

31、同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；

32、将二维距离图像输入至lidar数据驱动模块的lidar全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。

33、结合第二方面，在一种实施方式中，所述ekf模型驱动模块根据所述imu不确定度因子、场景特征退化率以及基于ekf模型驱动的协方差，确定imu数据驱动模块、lidar数据驱动模块和ekf模型驱动模块的不确定度，包括：

34、根据公式：，计算所述不确定度 u，其中， a 2为imu不确定度因子， p为场景特征退化率， σ为基于ekf模型驱动的协方差。

35、结合第二方面，在一种实施方式中，所述ekf模型驱动模块根据所述传播调整因子动态调整传播预测时的噪声协方差，包括：

36、根据公式： q bw ′   = q bw × 10^(a 1 w )，更新陀螺仪噪声 b w的噪声协方差 q bw；

37、根据公式： q ba ′   = q ba × 10^(a 1 a )，更新加速度计噪声 b a的噪声协方差 q ba；

38、其中， a 1 w为陀螺仪传播调整因子， a 1 a为加速度计传播调整因子。

39、结合第二方面，在一种实施方式中，所述当场景特征退化率超过设定阈值时，ekf模型驱动模块仅使用所述伪观测因子进行观测更新，包括：

40、构造速度约束：

41、 y k hor = h hor (x k ) + n k hor

42、 y k ver = h ver (x k ) + n k ver

43、其中， y k hor和 y k ver分别为伪测量值 y k水平方向和垂直方向的分量， h(·)为ekf模型驱动模块的观测函数， n k为服从高斯分布的第 k个观测噪声，对应的协方差矩阵为 r 2×2，且 r 2 ×2满足：

44、 r ’ 11 = r 11 × 10 ^ (a 3 hor )

45、 r ’ 22 = r 22 × 10 ^ (a 3 ver )

46、其中， a 3 hor为伪观测因子水平方向的分量， a 3 ver为伪观测因子垂直方向的分量；

47、使用伪测量值[ y k hor , y k ver] =[ 0,0]为状态更新提供过滤器。

48、本技术实施例提供的技术方案带来的有益效果至少包括：

49、本技术中的数-模混合估计的多源融合定位方法，其根据imu数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、imu不确定度因子和伪观测因子；基于相邻两次采集的点云数据，利用lidar数据驱动模块提取不同激光之间的变化特征以提供lidar场景全局观测，并计算当前时刻的场景特征退化率；根据所述imu不确定度因子、场景特征退化率以及基于ekf模型驱动的协方差，确定imu数据驱动模块、lidar数据驱动模块和ekf模型驱动模块的不确定度；根据所述传播调整因子动态调整ekf模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行ekf模型驱动模块的观测更新，并结合不确定度以进行位姿估计。

50、和现有技术相比，本技术将lidar和imu的数据驱动与基于ekf的模型驱动相结合，充分发挥两者的优势，实现更准确的定位；通过imu数据驱动模块实时感知运动状态，以及通过lidar数据驱动模块感知场景特征变化，进而通过两者共同作用来自适应调整基于模型驱动的滤波器参数，也即通过基于数据驱动的方法为基于模型驱动的滤波器系统提供自适应的参数调整，来适应不同场景和不同运动状态的变化。增强了定位系统在复杂场景和运动下的鲁棒性，此外，还引入了不确定度模型，综合考虑传感器数据和系统模型的不确定度，提供更可靠的结果置信度。