视觉惯性导航融合算法研究进展

张宇 谢波 江一夫 王猛 王萌

摘 要：视觉惯性导航一直是无人驾驶与机器人领域重点、难点环节。首先介绍经典算法框架，从基于最优平滑算法和预积分理论两个层面介绍经典视觉惯性融合算法。然后介绍新型算法框架，主要在深度学习基础上，从对整体框架学习的程度，将新型视觉惯性融合算法分为两类，部分学习和整体学习，最后介绍了主流算法中关键技术与未来展望。

关键词：视觉惯性导航；优化；耦合算法；深度学习

0 引言

近些年来，伴随着图形处理技术、计算机视觉和元宇宙等相关技术的进步，实现无人设备高精度、强鲁棒性的定位导航始终为无人系统中的关键环节和待突破环节。美国国防研究与工程署连续数年组织机器人生存挑战赛，以求在复杂环境下探索提升导航性能，确保美军在无人领域的领先地位。中国科协也于2020年将无人车的高精度智能导航问题列为十大工程技术难题之一[1]。

无人系统导航领域一直以即时定位与地图构建（Simultaneous Localization And Mapping，SLAM）技术为核心。纯视觉导航由于缺乏深度信息导致对运动敏感薄弱，需要依赖回环检测来校正误差，这导致其鲁棒性不足。视觉导航与卫星导航系统的组合导航在复杂的城市场景下，光影变幻导致有效特征点选取难度变大，诸如地下车库等场景对卫星信号干扰强烈，而本文介绍的视觉惯性导航有着显著的优势。

一是误差的互补：视觉导航的稳定位姿估计可以弥补惯性导航自身固存的累积误差，惯性导航测量单元（Inertial Measurement Unit，IMU）高频率的动态信号可以补足相机的深度信息；二是适用速度领域的 互补：视觉导航在低速静态领域的优秀表现，可以抑制IMU的零漂。而在高速运动场景领域，惯性导航可以解决视觉导航由于图像帧之间缺乏重叠区域，导致特征提取算法失效的问题。

1 经典融合框架

经典融合框架中，松耦合算法是视觉惯性领域早期重点研究内容。松耦合算法主要依靠卡尔曼滤波器及其后续改进版本，诸如应用广泛的扩展卡尔曼滤波器（Extended Kalman Filter，EKF）、基于蒙特卡洛原理的无迹卡尔曼滤波器（Unscented Kalman Filter，UKF）、非线性非高斯领域表现不俗的粒子滤波器（Particle Filter，PF）等的松耦合方 案陆续提出。Kelly提出的无人机视觉惯性导航[2]缺少对传感器信息缺乏交叉項误差，无法全面利用传感器信息，故精度有限。

自2010年起，惯性辅助视觉的紧耦合算法大量出现，由Davison等人提出的单目SLAM[3]开辟了紧耦合领域的先河。SLAM算法框架如图1所示。

同一时间，Klein[4]等提出了平行跟踪与建图（Parallel Tracking And Mapping，PTAM）该文章也是视觉SLAM中里程碑式的文章，为视觉建图开辟了新的路径。由于早期文章均针对简单场景建模，且传感器采样率低，特征点提取成功率有限[5]。之后相关研究跟进，经典融合框架发展迅速。自此以视觉传感器为核心，惯导信息辅助优化后端环节成为了主流研究方案。

H.Ranch、F.Tung与C.Striebel合作提出了固定区间最优平滑算法[6]。该算法在数据后端处理中具有重要的应用价值，在前向估计的基础上加上反向滤波，充分利用了量测区间内的所有量测值，具有更高的估计精度，所以主流的算法大都使用该理论作为航迹推算框架。

其中以扩展卡尔曼滤波器为基础的多态融合卡尔曼滤波器（Multi-State Constraint Kalman Filter，MSCKF）最具有代表性[7]，此算法最先由Mouriki和Roumeliotis于2007年提出，MSCKF最重要的突破点在于，解决了传统松耦合算法在图像信息丰富情景下，视觉算法选取特征点超出系统处理能力的问题。

2012年，预积分理论[8]的提出，为本领域研究带来了深远影响。系统中为了减小优化求解器的负担，采用了关键帧策略，由于IMU的速率显然要快于关键帧的插入，预积分干通过重新参数化，将关键帧间的IMU测量值积分成相对运动的形式，避免了因为初始条件变化造成的无用信息。随后Foster也将其拓展到李代数上，当然也增大了预积分的计算量[9]。

在预积分理论基础上，Raul Mur-Artal等人提出了ORB-SLAM算法。在此基础上结合惯性技术，构想出了一种视觉惯性快速旋转二进制描述子算法[10]（Visual Inertial-Oriented fast and Rotated Brief SLAM，VI-ORB SLAM）。VI-ORB算法对特征提取也做出了改进，在BRIEF采点的基础上提出了Fast采点方案，提出定义特征点方向，来实现特征点的旋转不变形。

如果说ORB-SLAM是导航领域的一颗明珠，那么香港科技大学的沈劭劼的单目视觉惯性系统算法[12]（Monocular Visual-Inertial System，VINS-Mono）无疑是另一个代表性成果。VINS-Mono提出了一种健壮的基于紧耦合滑动窗口非线性优化的单目视觉惯性里程计，初始化策略，包括对相机与IMU间的外参、数据间时间戳进行在线校准。该算法在前端的处理上使用光流追踪特征点，当读取到新图像后提取Harris角点[13]，然后使用光流法在更新环节迭代计算，最后利用RANSAC[14]算法对跟踪错误的特征点进行剔除，其框架如如图2所示。

总的来说，视觉惯性导航算法已经经过了长足的发展，在前端和后端上的大量改进使得导航精度不断提升，伴随着性能的提升，其计算量也在大幅提升，对于实际应用仍是一个考验。

2 新型融合框架

为提高图像 识别准确度，斯坦福大学李飞飞团队举办了图像识别挑战赛，Alex等人设计的Alex Net模型[15]以领先第2名近10%的准确度夺冠，迅速席卷了深度学习领域，使得卷积神经网络声名大噪，随后的卷积神经网路的快速算法也广泛应用于本领域[16]。

Huang提出了使用循环神经网络（Recurrent Neural Network，RNN）网络学习光流法的视觉里程计[17]，该算法就是一种典型的用深度学习替代前端视觉里程计的计算过程的方法。作者立足于单目视觉里 程计，提出了用于估计相机的自我运动的序列到序列算法。该网络学习光流的潜在空间并建立图像序列动力学模型，并且计算连续图像的光流场，最终以自编码方式提取潜在的光流表示。

Li提出了端到端EKF的深度视觉-惯导里程计[18]（Towards End-to-end Learning of Visual Inertial Odometry with an EKF）。在耦合思路上仿照专家系统，将传统算法中表现良好的环节予以保留。采用深度学习得到的姿态构建量测方程，整体上采用RTS算法模型，在正反双方向的计算上优化结果。在数据集上测试并证明了系统的有效性。

牛津大学计算机科学系Clark团队提出了一种基于流形的序列训练方法VINet[19]。其改进之处是在中间特征表示水平上進行数据融合。作者提出使用长短期记忆（Long Short-Term Memory，LSTM）来弥补RNN网络对全局信息的把握不足。VINet整体框图如图3所示。

除此以外，Chen提出了一种新颖的端对端选择性传感器融合框架[20]，主要应用于复杂环境下数据丢失问题，以及处理野值的框架。由于单目相机图像信息较少，且缺乏深度信息，在预测中常出现误差较大的问题。故构架了不同掩蔽策略的方案。将丢失信息的程度划分为两类工作环境，对应不同环境采用不同的策略。结合故障诊断思想，分为软、硬两种融合方案。后验型视觉惯性导航算法对比表如表1所示。

综合来看，深度学习在纯视觉SLAM方向广泛使用，在视觉惯性领域仍有较大可发展空间。对于深度学习在整体框架中使用的比例不尽相同，整体使用框架虽然性能更好但是计算量过大，局部使用计算量更小但是性能表现稍弱。

3 展望未来

3.1 多传感器融合

视觉与惯性耦合算法在机器人领域取得了显著的成果，在自动驾驶领域，多传感器融合算法已经得到了广泛应用，诸如全球定位系统（Global Positioning System，GPS）等等。

Lee提出一种有效而且鲁棒的GPS辅助的视觉惯导里程计系统（GPS-VIO）[21]，该系统使用低频的GPS测量数据融合相机IMU数据，作者提出一种在线标定的方法对于外参、时间偏移以及对传感器噪声具有鲁棒性的参考帧初始化步骤。香港大学张富团队提出了一种快速、稳健且通用的激光惯性里程计框架[22]。创新之处是不提取特征直接将原始点配准到地图，而这使得环境中的细微特征能够被使用.

3.2 仿生导航

仿生导航作为一种多学科交叉的新兴学科。动物行为学和生理学的研究成果对新型科技领域如计算机、通信领域一直有促进推动作用。Hao结合了昆虫复眼结构和人类视觉器官结构，研制了一种混合型的仿生复眼系统[23]。国防科技大学胡小平教授团队研制了六通道点测量式仿生光罗盘[24]。Milford等基于吸引子网络模型，构建了包含位置细胞和方向细胞模型的RatSLAM算法[25]。

4 结束语

将视觉导航系统与惯性导航系统融合，互相取长补短，加以耦合的算法使低成本传感器获得了高精度和高稳定性，让整个系统迈向小型化、轻量化。视觉惯性导航是一个多学科交叉，对算法和硬件要求都很高的综合性平台。其进步必然得益于算法效率的提升与计算机算力的进步。所以仍需要后继者开阔思路，进一步探索与挖掘。

参考文献：

[1] 中国科学技术协会.面向未来的科技—2020重大科学问题和工程技术难题解读[M]. 北京：中国科学技术出版社，2020.

[2] KELLY J， SUKHATME G S. Visual-inertial sensor fusion： localization， mapping and sensor-to-sensor selfcalibration[J].International Journal of Robotics Research， 2011， 30（1）： 56-79.

[3] A. J. DAVISON， I. D. Reid， N. D. MOLTON. Mono SLAM： Real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007，29（6）：1052-1067.

[4] KLEIN G， MURRAY D. Parallel tracking and mapping for small AR work-spaces [C].// Proc of the 6th IEEE/ACM International Symposium on Mixed and Augmented Reality. Washington DC： IEEE Computer Society， 2007.

[5] CARRILLO L R. LOPEZ A E. Lozano R. et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV [J].Journal of Intelligent and Robotic Systems， 2012， 65 （1） ： 373-387.

[6] 嚴恭敏，邓瑀.传统组合导航中的实用Kalman滤波技术评述[J].导航定位与授时，2020，7（2）： 19-24.

[7] MOURIKIS A I， ROUMELIOTIS S I. A multistate constraint Kalman filter for vision-aided inertial navigation[C].// Proceedings of IEEE International Conference on Robotics and Automation. Piscataway， USA：IEEE， 2007.

[8] LUPTON T， SUKKARIEH S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics， 2012， 28（1） ：61-76.

[9] FORSTER C， CARLONE L， DELLAERT F， et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]. Proceedings of Robotics Science and Systems，2015.

[10] MUR-ARTAL R， TARDOS J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters， 2017，2（2）：796-803.

[11] CARLOS CAMPOS， RICHARD ELVIRA. An accurate open-source library for visual， visual-inertial and multi-map SLAM[C].IEEE Transactions on Robotics 37（6）：1874-1890， Dec.2021.

[12] SHEN S J， MICHAEL N， KUMAR V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C].IEEE International Conference on Robotics and Automation. Piscataway， USA：IEEE 2015：5303-5310. DOI： 10.1145/3065386.

[13] SEMMA ABDELILLAH， HANNAD YAACOUB et al. Writer identification using deep learning with FAST key points and Harris corner detector[J].Expert Systems With Applications，2021，184.

[14] PAN Z T， WEN Z W. Analysis of image registration algorithm RANSAC and its improved algorithm[J]. IOP Conference Series： Earth and Environmental Science，2021（1）：783.

[15] ALEX K， ILYA S， GEOFFREY E H. Image net classification with deep convolutional neural networks[J]. Communications of The ACM. 2017.

[16] A.LAVIN， S.GRAY. Fast algorithms for convolutional neural networks[C].// Proceedings of Computer Vision and Pattern Recognition （CVPR）， 2016.

[17] ZHAN H， C.S.WEERASEKERA， J.W.BIAN. Visual odometry revisited： what should be learnt？[C].International Conference on Robotics and Automation （ICRA）， 2020.

[18] LI C， S.L.WASLANDER. Towards end-to-end learning of visual inertial odometry with an EKF[C].Conference on Computer and Robot Vision （CRV）， 2020：190-197.

[19] CLARK R， WANG S， WEN H， et al. VINet： visual inertial odometry as a sequence to sequence learning problem[C]. Proceedings of International Conference on Artificial Intelligence， 2017.

[20] CHEN C， ROSA S， MIAO Y， et al. Selective sensor fusion for neural visual inertial odometry[C].Proceedings of Computer Vision and Pattern Recognition （CVPR）， 2019.

[21] LEE W， K. ECKENHOFF. Intermittent GPS-aided VIO： online initialization and calibration[C].International Conference on Robotics and Automation （ICRA）， 2020.

[22] XU W， CAI Y. FAST-LIO2： fast direct Li DAR-inertial odometry[C].Transactions on Robotics. DOI： 10.1109.

[23] HAO Q， WANG Z， CAO J， et al. A hybrid bionic image sensor achieving FOV extension and foveated imaging[J]. Sensors， 2018， 18（4）：1042.

[24] XIAN Z， HU X， LIAN J， et al. A novel angle computation and calibration algorithm of bio-inspired sky-light polarization navigation sensor[J].Sensors，2014，14（9）：17068-17088.

[25] MILFORD M J， WYETH G F. Mapping a suburb with a single camera using a biologically inspired SLAM system[J]. IEEE Transactions on Robotics， 2008， 24（5）：1038-1053.

[26] S.W.CHEN et al. SLOAM： semantic Lidar odometry and Mapping for Forest Inventory[J]. Robotics and Automation Letters， 2020.

[27] WANG H， XUE C， ZHOU Y et al. Visual semantic localization based on HD map for autonomous vehicles in urban scenarios. International Conference on Robotics and Automation （ICRA）， 2021.