一种用于非平坦环境的鲁棒的两阶段激光惯性里程计

本公开提供了一种用于非平坦环境的鲁棒的两阶段激光惯性里程计，该里程计中，激光里程计模块基于激光点云信息，采用基于NDT的粗匹配和基于KISS‑ICP的精确匹配获得无人平台的位姿；NDT解决旋转运动精匹配，KISS‑ICP算法解决平移运动精匹配。回环检测与优化模块对激光里程计模块输出的位姿进行回环检测和补偿；多传感器数据融合模块根据回环检测与优化模块输出的位姿和惯性传感器输出的IMU数据，采用基于误差状态的卡尔曼滤波器进行数据融合，得到当前时刻无人平台的最终位姿结果。卡尔曼滤波器的误差量和观测量加入了无人平台的速度误差。使用本发明能够提升非平坦环境下位姿估计的精确度、鲁棒性。