针对弱结构长隧道环境的激光惯性里程计算方法及系统

本发明公开一种针对弱结构长隧道环境的激光惯性里程计算方法，包括：建立激光惯性里程计系统运动状态的预测方程、误差状态的协方差传播方程；建立反射强度残差和几何观测残差的观测方程；将状态变量的先验分布、几何观测残差和强度观测残差融合，形成最大后验优化目标函数；基于最大后验优化目标函数，迭代更新系统运动状态和误差状态的协方差直至收敛，得到最优系统位姿。本发明利用低成本的传感器组合，充分利用激光雷达回波反射强度信息，增强传感器在长隧道、弱结构约束场景下的定位建图准确性和算法鲁棒性。