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

Application of novel optimization strategy in visual inertial navigation system

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

In order to solve the problems of time-consuming of state estimation and poor real-time performance of visual inertial integrated navigation system,an improved dogleg algorithm was proposed as a solution algorithm.First of all,a complete visual inertial fusion model was derived:visual information was projected to the pixel plane using camera model,and the feature points were tracked by optical flow method,then the Perspective-N-Point(PNP)optimization method was used to calculate visual position,inertial information was used to obtain the relative pose between visual frames,which effectively increased calculation efficiency.Based on above information,Jacobi matrix was derived for nonlinear optimization calculation,and system loss function was presented for fusion algorithm.Finally,the improved dogleg algorithm was used for state estimation.Through the dataset test and dynamic car test,it can be concluded that the proposed algorithm is better than traditional Levenberg-Marquadt(LM)algorithm.The improved doglet algorithm educes average time consumption by about 60%,has better convergence speed and meets the realtime requirements,and its accuracy is improvied by about 17%,and it has better practicability.