一种基于MSCKF的激光雷达惯性里程计融合方法

本发明提供一种基于MSCKF的激光雷达惯性里程计融合方法。涉及定位技术领域。本申请获取惯性测量单元解算的位姿、速度以及陀螺仪偏置和加速度计偏置，激光雷达解算的位姿状态信息；创建惯性测量单元的运动模型来使用惯性测量单元的加速度计和陀螺仪数据进行状态预测，将状态向量更新到下一时刻；预处理激光雷达点云数据，从激光雷达帧中提取关键帧；基于惯性测量单元计算的位姿和根据各关键帧的静态特征点匹配结果计算采集关键帧时激光雷达处于全局坐标系G的位姿；基于激光雷达相对于全局坐标系G的位姿估计对所有被检测静态特征点的量测残差进行建模得到量测模型；通过最小化重投影误差优化量测模型，在卡尔曼滤波的更新步骤来校正系统状态。