新型优化策略在视觉惯性导航系统中的应用

为了解决视觉惯性组合导航系统状态估计耗时长、实时性差的问题,提出一种改进dogleg算法作为求解算法。首先推导了完整的视觉惯性融合模型:视觉信息利用相机模型投影到像素平面,使用光流法进行特征点的跟踪,并利用PNP优化方法计算视觉位姿,惯性信息利用惯性预积分获得视觉间的相对位姿,有效提高计算效率;然后基于上述信息推导了融合算法中用于非线性优化计算的雅克比矩阵,以及视觉惯性组合系统模型损失函数;最后应用改进dogleg算法进行状态估计。经过数据集实验以及跑车实验可以得出,改进算法优于传统LM算法,平均耗时降低约60%,具有更好的收敛速度,满足实时性要求;同时对精度有17%左右的提高,具有更好的实用性。