一种基于精炼消失点辅助视觉惯性里程计方法

本发明公开了一种基于精炼消失点辅助的视觉惯性里程计方法及系统，适用于结构化环境下的精确定位与姿态估计。其实现过程为：首先，进行图像与惯性数据的同步采集；其次，提出“三步法”消失点提取流程，包括(1)基于两线穷举法的初始重采样，生成初始候选集合IVPs；(2)引入IMU预测姿态对消失点候选进行姿态一致性筛选，剔除与实际姿态差异较大的伪消失点，形成粗提取集合CVPs；(3)基于中值的方向聚类与距离评分，最终选出最优正交组合，构成精炼消失点集合RVP，用于后续姿态恢复。随后，基于RVP构建相机外参估计模型，实现载体姿态的还原。为提高系统在伪观测干扰下的稳定性，本发明引入IGGⅢ型方差膨胀机制。最终，将精炼消失点计算得到的姿态作为冗余观测项，联合IMU预测姿态构建误差状态Kalman滤波观测模型，完成姿态估计与状态更新。