基于非线性优化的单目视觉/惯性组合导航算法

针对自主移动机器人在GPS拒止环境下准确实时定位的问题,提出一种单目视觉/惯性组合导航算法。为解决视觉/惯导工作频率不一致问题,利用预积分技术预先处理惯性测量值;引入了一种快速高精度线性初始化方法,分步估计初始系统尺度、重力方向、速度及零偏;结合全局地图三维点约束削弱累积误差,基于是否更新地图点,构建了两种基于非线性优化的单目视觉/惯性紧耦合模型,以保证导航的局部精度及全局一致性。实验结果表明:初始化方法可快速(15 s内)实现高精度状态初始化;与单目视觉导航算法相比,提出的算法不仅可获取绝对尺度信息,且导航精度更高;与传统滑动窗口非线性优化方法相比,提出的算法在窗口优化过程中加入全局地图三维点约束,可有效削弱累积误差,验证了算法的正确性和可行性。

Monocular visual inertial navigation based on nonlinear optimization

For the problem of autonomous mobile robots' real-time precise localization in GPS-denied environments,a monocular visual inertial navigation algorithm based on nonlinear optimization is proposed.To handle multi-rate measurements in visual inertial navigation system,the IMU pre-integration technique is utilized to process inertial measurements.A fast accurate linear initialization method is introduced to estimate the initial scale,gravity direction,velocity,and gyroscope and acceleration biases.To guarantee the local accuracy and global consistency of visual inertial navigation system,the global map point constraint is utilized to decrease the influence of accumulated errors,and two kinds of nonlinear optimization based visual inertial tightly coupled model are built according to whether the map points are updated.Experimental results indicate that the initialization method can obtain accurate initial states in 15 s.Compared with the visual navigation only,the proposed algorithm can get the absolute scale and outperforms the visual navigation in accuracy.Compared with classical sliding window methods,the proposed algorithm can effectively decrease the influence of accumulated errors,which validates the correctness and feasibility of the proposed method.