一种基于因子图的多位姿源融合定位方法

本发明一种基于因子图的多位姿源融合定位方法，所述方法包括：S1、因子图建立：将融合开始时刻的RTK状态信息作为当前状态下的先验因子，建立因子图；S2、点云地图重定位里程计信息优化：利用IMU数据进行前向传播，建立状态量误差模型；利用前向传播的预测值对LiDAR原始点云进行后向传播来进行去畸变操作，利用去畸变后的点云建立观测方程；再采用IEKF进行更新；S3、轮速‑IMU里程计信息优化：通过误差卡尔曼优化IMU提供的旋转信息，结合轮速数据计算IMU数据时刻下的状态量；S4、多位姿源融合：将RTK先验因子、点云地图重定位里程计因子和轮速‑IMU里程计因子进行累乘，得到最大概率的系统状态。通过对多个传感器数据进行融合优化，得到矿卡精确的自主定位信息。