一种自适应多模态里程计动态融合方法

本发明提供了一种自适应多模态里程计动态融合方法，具体包括：提取激光点云的边缘和平面特征，利用迭代运动补偿和残差优化生成激光位姿因子；从原始图像提取多尺度角点特征，结合光流跟踪和局部光束平差生成视觉位姿因子。构建动态因子图，将激光与视觉位姿因子作为约束，运用iSAM2引擎优化求解，得到全局位姿。实时计算拉格朗日因子残差均值和方差，基于卡方检验动态检验传感器状态，连续超限时触发失效响应，逐步移除异常因子，保持历史数据，通过SE(3)流形插值对齐时间戳，并优化外参标定。传感器恢复后，渐进式权重融合重新引入因子，采用滑动窗口机制平滑轨迹。该方法为机器人和自动驾驶领域提供了一种鲁棒、高精度的融合算法。