基于流形预积分的因子图的导航方法、装置及系统

本申请涉及导航，具体而言，涉及一种基于流形预积分的因子图的导航方法、装置及系统。

背景技术：

1、随着科学技术的不断发展，导航定位需求逐渐增加，自动驾驶汽车进入大众的视野，成为人们生活出行的主流选择。车辆导航技术主要包含车辆的姿态、速度、位置等信息，这些信息的精度决定了自动驾驶汽车行驶的准确性与安全性。然而，现在的无人驾驶系统多依赖全球卫星导航系统(global navigation satellite system,gnss)，但单一的导航定位系统多少会存在程度上的限制，例如在途径隧道或者高楼大厦等复杂环境时，会出现gnss信息中断的问题。

2、当卫星信号不可用时，可以通过集成多传感器的信息来获取精确可靠的组合导航结果。目前常见的组合导航系统都是以惯性导航系统(inertial navigation system,ins)为基准，卫星导航系统以及其他类型的导航系统协助的组合模式。ins是一种自主式导航系统，可以在不依赖任何外部信标的情况下，通过加速度计和陀螺仪提供行进过程中的压力、磁力、加速度、角速度等量测数据。然而，ins存在定位误差随时间积累而增长的缺点，进而影响导航的精度。因此，ins通常与gnss集成在一起，通过优势互补提高定位的性能[]。gps提供全局环境下的导航位置信息，ins的测量信息采样频率很高可以达到200hz，并且其在短期内定位精度也较高，可以为gps系统提供丰富的与高精度定位关联的测量数据。因此，基于gps和ins的组合导航算法备受人们关注，对其行驶过程中的可靠性、安全性要求更高。目前的算法研究多以卡尔曼滤波及其改进滤波(扩展卡尔曼滤波ekf、无迹卡尔曼滤波ukf及联邦卡尔曼滤波fkf)为主。但是基于卡尔曼滤波的方法存在处理信息不同步、非线性估计存在较大误差、无法追溯历史信息等较多问题，单靠滤波算法实现多传感器组合导航难以实现，往往需要结合神经网络辅助滤波算法。

3、现有技术提出了一种输入延迟神经网络方法，该方法考虑了ins位置和速度信息的当前时刻与过去时刻的采样，以帮助gnss/ins的集成。现有技术还提出了一种elman神经网络辅助的组合导航滤波算法，在gps信号丢失前训练神经网络，gps信号丢失后则利用神经网络进行实时预测，有效地减小了imu积累误差对精度的影响。但人工智能方法实时性较差，并且在缺少训练样本的情况下效果不佳，难以适应环境变化的情况。

4、相比于传统滤波方法，因子图方法在非线性量测条件下可以获得更好效果。因子图推理中的高斯牛顿迭代过程，其在对待求解的状态变量进行估计时进行多次的线性化。与扩展卡尔曼滤波的方法相比，线性化过程中产生误差更小。此外，当传感器的可用性出现变化时，联邦滤波器的系统重构较为困难，而因子图将系统量测信息体现为因子节点，各个传感器因子节点工作相互独立，只需要不再添加该类传感器因子节点即可，相比于滤波的方法结构变化更为灵活，拥有即插即用的优点。

5、针对上述的问题，目前尚未提出有效的解决方案。

技术实现思路

1、本发明实施例提供了一种基于流形预积分的因子图的导航方法、装置及系统，以至少解决现有技术中导航不精确的技术问题。

2、根据本发明实施例的一个方面，提供了一种基于流形预积分的因子图的导航方法，包括：构建因子节点模型，并基于所述因子节点模型创建因子节点，其中，所述因子节点模型包括imu流形预积分因子模型、gnss因子模型、和先验因子模型，所述因子节点包括imu流形预积分因子、gnss因子和先验因子；基于所述因子节点，利用滑动窗口的最大后验估计优化算法，来对全局路径导航进行优化。

3、根据本发明实施例的另一方面，还提供了一种基于流形预积分的因子图的导航装置，包括：模型构建模块，被配置为构建因子节点模型，并基于所述因子节点模型创建因子节点，其中，所述因子节点模型包括imu流形预积分因子模型、gnss因子模型、和先验因子模型，所述因子节点包括imu流形预积分因子、gnss因子和先验因子；导航模块，被配置为基于所述因子节点，利用滑动窗口的最大后验估计优化算法，来对全局路径导航进行优化。

4、根据本发明实施例的另一方面，还提供了一种基于流形预积分的因子图的导航系统，包括：无人车；如上所述的导航装置，设置在所述无人车上，被配置为对所述无人车的全局路径导航进行优化。

5、在本发明实施例中，构建因子节点模型，并基于所述因子节点模型创建因子节点，其中，所述因子节点模型包括imu流形预积分因子模型、gnss因子模型、和先验因子模型，所述因子节点包括imu流形预积分因子、gnss因子和先验因子；基于所述因子节点，利用滑动窗口的最大后验估计优化算法，来对全局路径导航进行优化，解决了现有技术中导航不精确的技术问题。

技术特征：

1.一种基于流形预积分的因子图的导航方法，其特征在于，包括：

2.根据权利要求1所述的方法，其特征在于，构建因子节点模型，并基于所述因子节点模型创建因子节点，包括：

3.根据权利要求2所述的方法，其特征在于，基于所述imu数据构建所述imu预积分模型，包括：

4.根据权利要求3所述的方法，其特征在于，分别对所述旋转速率和所述加速度测量值进行积分，得到下一时刻的状态，包括：

5.根据权利要求1所述的方法，其特征在于，构建因子节点模型，并基于所述因子节点模型创建因子节点，包括：

6.根据权利要求1所述的方法，其特征在于，基于所述因子节点，利用滑动窗口的最大后验估计优化算法，来对全局路径导航进行优化，包括：

7.一种基于流形预积分的因子图的导航装置，其特征在于，包括：

8.一种基于流形预积分的因子图的导航系统，其特征在于，包括：

9.一种电子设备，其特征在于，包括：

10.一种计算机可读存储介质，其上存储有程序，其特征在于，在所述程序运行时，使得计算机执行如权利要求1至6中任一项所述的方法。

技术总结本申请提供了一种基于流形预积分的因子图的导航方法、装置及系统，其中，该方法包括：构建因子节点模型，并基于所述因子节点模型创建因子节点，其中，所述因子节点模型包括IMU流形预积分因子模型、GNSS因子模型、和先验因子模型，所述因子节点包括IMU流形预积分因子、GNSS因子和先验因子；基于所述因子节点，利用滑动窗口的最大后验估计优化算法，来对全局路径导航进行优化。本申请解决了现有技术中导航不精确的技术问题。技术研发人员：刘宁,胡彬,袁超杰,范军芳,赵旭,赵辉,刘福朝,苏中受保护的技术使用者：北京信息科技大学技术研发日：技术公布日：2024/8/16