利用IMU角度信息进行SLAM漂移诊断与补偿方法

本发明提供利用IMU角度信息进行SLAM漂移诊断与补偿方法，涉及SLAM技术领域。利用稀疏点云特征点配准来估计位姿；确定稀疏点云关键帧及其估计位姿；在IMU处于零速或匀速时，分析IMU数据计算的当前关键帧俯仰角，配准的当前关键帧的俯仰角，当前关键帧相对当前关键帧前第h个历史关键帧俯仰角和IMU数据计算的当前关键帧俯仰角之差判断是否发生漂移；漂移时，取当前关键帧的xy坐标和航向角保持不变，将IMU数据计算的当前关键帧的横滚角、俯仰角和当前关键帧前第h个历史关键帧的z坐标赋值给当前关键帧；未漂移时，用位姿误差和漂移误差构建G2O图优化的代价函数，利用G2O图优化进行优化迭代。