相关VI-SLAM算法内容总结

本文为作者在从事Slam相关工作中对这几年遇到以及改进过相关VIO算法内容总结。

1.背景介绍

一个完整的 SLAM（simultaneous localization and mapping） 框架包括传感器数据、 前端、 后端、 回环检测与建图，如图1所示，其中，前端将传感器的数据抽象成适用于估计的模型，回环检测判断机器人是否经过已知的位置。而后端接受不同时刻前端测量的位姿和回环检测的信息并对它们进行优化，从而得到全局一致的轨迹。建图则是根据状态估计得到的轨迹建立与任务要求相对应的地图。通常，仅含有前端和局部后端的框架被称为里程计， 而带有回环检测和全局后端的完整框架被称为 SLAM。

近十年来，SLAM 及其相关技术的研究取得了快速进展，研究的重点从开始的激光雷达到相机和IMU，与此同时芯片和MEMS器件的也取得快速发展，算力得到的极大提高，相机和IMU 等传感器实现了高精度化、小型化和低成本化。这使得SLAM技术在移动端能够实时的运用。而视觉传感器因其体积小、 成本低和易于进行硬件设置而获得了广泛的关注，大量基于视觉传感器的 SLAM 方法被提出，但纯视觉 SLAM 方法存在无法在图像纹理少的区域工作、快速运动时图像模糊等问题；而IMU可测量角速度和加速度，其功能可以与相机进行互补，并且在融合之后能够得到更加完善的SLAM 系统。

采用相机和 IMU 的 SLAM 方法被称为视觉惯性 SLAM（visual-inertial SLAM，VI-SLAM），只含有很小的漂移。本文主要介绍VI-SLAM，对激光和纯视觉SLAM不重点介绍。目前VI-SLAM数据融合的的方法分为两类：紧耦合和松耦合，其中，紧耦合是指把 IMU 的状态与相机的状态合并在一起进行位姿估计。松耦合是指相机和 IMU 分别进行自身的位姿估计，然后对它们的估计结果进行融合。

而VI-SLAM根据后端优化方法的不同，分为基于滤波和基于优化 2 类方法，在滤波方法中，传统的 EKF（扩展卡尔曼滤波器） 、UKF（无迹卡尔曼滤波器）、改进的 MSCKF（多状态约束卡尔曼滤波器） 和OpenVins都取得了一定的成果。现阶段基于优化的方法则慢慢占据了主流。

2. 基于优化的方案

基于优化的方法主要依靠图像处理技术进行特征提取和图像匹配，而 IMU 数据则被视为先验项或者正则化项。自 PTAM（parallel tracking and map-ping） 之后，批量非线性优化方法一般都分为2 个线程：跟踪和建图。在跟踪线，通过各种特征检测器从图像中提取 3 维空间中的点、线或其他路标特征。然后，针对检测到的所有特征，在 2 个图像之间定义重投影误差。之后用该误差建立优化的代价函数，以便找到特征或地标的坐标，即光束平差法（bundle adjustment，BA） 。在建图线程，地图中特征和路标的坐标被用于定义 2 幅图像之间的重投影误差，然后再次应用优化算法来发现移动机器人位姿的变化。将优化问题分成 2 个并行线程的目的是使跟踪部分实时响应图像数据，以便快速获得跟踪结果。而地图的优化没有必要实时计算，可以放在后端缓慢运行。基于优化的方法通常使用g 2 o 、Ceres 、GTSAM 等非线性优化库实现优化。

基于优化的方法有两种类型，一种是含有局部优化（LBA）和全局优化（GBA），一种是只有全局优化（GBA）

LBA通过设定固定的窗口，将在这个窗口内的系统状态进行估计，同时边缘化旧的状态维持实时优化窗口。相比于滤波方法，基于优化的方法精度更高，因为它们可以对过去的测量结果重新进行线性化处理，同时在优化时可以设置鲁棒核函数来剔除一些异常值。从而提高精度；但是缺点是，边缘化旧的一帧时带来了稠密的先验信息，这在一定程度上降低了效率，针对这个问题，一些文献 中提出了为了稀疏性而放弃某些测量的方法。此外，由于采用了边缘化的方法，LBA方法具有与部分滤波方法相似的问题，如一致性，线性化误差的累积等。

苏 黎 世 联 邦 理 工 学 院 ASL 实 验 室 Leutenegger 等 提 出 了 OKVIS（open keyframe-based visual-inertial SLAM）；利用基于关键帧的滑动窗口进行批量非线性优化，先于滑动窗口的关键帧被边缘化，不用来进行估计。系统前端使用多尺度 Harris 特征检测器 来提取特征，然后在其基础上计算 BRISK（binary robust invariant scalable keypoint）描述子，以便在帧与帧之间进行数据关联。。如图 3 所示

香港科技大学飞行机器人实验室提出的 VINS-Mono（monocular visual-inertial system）方法类似于 OKVIS，是一种基于非线性优化的 VI-SLAM方法。VINS-Mono 方法为这类框架引入了几个全新的功能，其完整系统包括观测值预处理、 初始化、 局部视觉惯性联合优化、全局图优化和回环检测 5 个部分， 前端提取 Harris 特征点，并采用 LK（Lucas-Kanade）光流（opticalflow）法跟踪相邻帧。光流法是一种描述像素随时间在图像之间运动的方法。LK 光流法计算部分像素，在 SLAM 中用于跟踪特征点的位置。

VINS-Mono 方法只计算特征点，不计算描述子，同时使用光流法跟踪特征点的运动。这样就减少了计算和匹配描述子的时间和资源，只需要计算光流。在初始化部分，作者使用了一种松耦合的传感器融合初始化程序，称为动态初始化，先用图像信息构建SFM，后面结合IMU信息，进行在线VIO标定。使得系统可以在优化之前从任意初始状态引入估计器。同时采用预积分的处理方法处理IMU信息，得到两帧之间的IMU相对运动，而不是随着某一时刻位姿改变而要重新处理IMU信息，减少计算量。系统采用与 OKVIS相似的基于滑动窗口的紧耦合位姿估计方法，并且加入了基于DBoW2（bag of binary words 2）的回环检测线程，使系统具有重定位功能，而全局优化则采用4Dof优化。

香港科技大学飞行机器人实验室随后又推出了 VINS-Mobile 和 VINS-Fusion。VINS-Mobile 在 iOS 设备上运行，并为增强现实（AR）应用程序提供本地化服务。VINS-Mobile 对 VINS-Mono 框架作了许多轻量化处理，并采用了一种基于小滑动窗口的联合优化方案。VINS-Fusion 在完善 VINS 框架的同时，提供了 4 个不同硬件的版本，包括单目 IMU、 双目 IMU、 纯双目和双目 IMU 与GPS 松耦合。

百度 AR 技术小组提出的 ICE-BA（incremental，consistent and efficient bundle adjustment） 沿 用了 OKVIS 以及 VINS-Mono 这一类框架。前端提取Harris 特征点 并采用 LK 光流法 跟踪相邻帧，与 VINS-Mono 相似。后端则是论文中提出的增量式 BA，主要分为 3 个部分：局部 BA（LBA）、全局 BA（GBA） 以及相对边缘化（Relative-Marginalization），前两者采用增量式方法提升了后端速度，后者保证了LBA 和GBA 的一致性。其中，G BA 在建立增量方程时，对系统已经计算过且不变的状态向量不进行计算。

因为即使重新计算，精度也不会有太大的改善。对于LBA 来说，由于几乎每个点都被滑动窗口里面所有帧看到，因此即使只重新线性化一部分点，也会带来较大的改动。因此在LBA这个部分，系统中采用了一个更高效的针对局部 BA 的后端设计。这个方法把长期跟踪的特征点分成很多短期跟踪段，这样每次重新计算特征点的时候，重新线性化的区域减小，从而提高了效率。见图5；对于滑窗系统，往往都有边缘化这一步，把有些本应该出了滑动窗口的信息以另一种形式存储下来。但是当有GBA 存在时，边缘化的效果未必会有很大的提升，反而可能会下降。ICE-BA 中改进了边缘化方法，保持了边缘化和全局 BA 的一致性，见图6。但在测试的时候，去掉相对边缘化有些数据集效果会更好。

由于 VI-SLAM 系统实现了优异的效果，一些著名的视觉 SLAM 系统 相继推出了自己系统的 VI 版本，VI-ORB SLAM 就是其中最有代表性的系统之一。在这里首先介绍纯视觉ORB-SLAM ，其首次使用 3 个线程：实时跟踪特征点的线程、局部建图的优化线程（co-visibility graph）和全局回环检测与优化线程（essential graph），来完成 SLAM。跟踪线程对每幅图像提取 ORB 特征点，并与最近的关键帧比较，计算特征点位置并估计位姿。局部 BA 线程求解更精细的相机位姿和特征点空间位置。全局回环检测线程采用 DBoW2 对全局的地图与关键帧进行回环检测，消除累积误差。ORB-SLAM 的三线程结构取得了优异的跟踪和建图效果，保证了移动机器人轨迹与地图的全局一致性，该算法框架在开源数据集测试的结果是效果最好的开源代码。

VI-ORB SLAM 改进自 ORB-SLAM2，作者引入 IMU 尝试解决在快速运动时丢失特征点的问题。VI ORB-SLAM 分别对 3 个线程作了修改，用以融合 IMU 信息。在跟踪线程，基于重投影误差和 IMU 预积分，建立帧与帧之间的约束关系来构造代价函数，从而得到当前帧位姿的最优估计。在局部建图线程，有了新的关键帧之后，将会对前 N 个关键帧进行优化，当前的关键帧（第 N + 1帧）将固定不变，提供 IMU 预积分约束。如图 7所示，P、v、b 分别为优化的位姿、IMU 速度与偏差。在全局回环检测线程，由于 IMU 提供了尺度信息，因此全局优化将从 7 个自由度下降到 6 个自由度。全局位姿优化将忽略 IMU 信息，因此不再优化速度和偏差，当完成全局位姿优化后，再根据矫正后的位姿对速度进行矫正。

在2020年，ORB-SLAM3开源，其第一个能够执行视觉、视觉惯性和多地图重用的系统，相比于ORB-SLAM2，ORB-SLAM3新增元素有以下面这几个：

1. 单、双目的vo/vio slam系统，并支持鱼眼相机。

2. 地图复用，拥有合并地图的功能。

3. 高召回的地点识别（占用较低的计算资源换得高召回与精度）

4. 不限制相机模型，只需提供投影，反投影及Jacobian方程（程序里提供了针孔与鱼眼模型）

其主要的创新点分为：

1.“完全”基于最大后验估计MAP的VI-SLAM ，无论在初始化阶段还是运行阶段，都采用了MAP进行状态估计，因此ORB-SLAM3在室内外、大小场景中鲁棒性很好，且精确度是其他方法的2~5倍； 如文中所讲，本文的IMU和视觉的组合系统是extremely robust的。

2.多地图系统，当定位丢失即lost时，ORB-SLAM3会自动建立一个新的小地图，并在revisit两张地图上的同一地点的时候进行地图的seamlessly merge，因此，这一算法能够使用不仅仅几帧之前的信息，而是运用了全局的信息，能够在bundle adjustment中利用视差较大的帧来增加BA求解的准确性（因为当视察较小时，求解不准确，且优化容易进入局部极值）。这种方法里对同一特征的观测可能在时间上的间隔较大，因为我们对其上一次的观测甚至可能出现在之前一张小地图中（中间过程里lost了至少一次）。

在这里简单介绍一下iSAM，iSAM是增量式图优化，该算法可简单的理解为iSAM可以自适应的判断当前测量是否影响某个历史状态，从而判断是否要计算更新，从而能够减少不必要的计算；在2012年，iSAM2 用贝叶斯树+在线启发式排序重新解决iSAM问题，后端部分原理相似与ICE-BA相似，都是增量式BA，该技术是GBA的一项突破，利用因子图来保持稀疏性，并仅识别和更新受新度量影响的变量的一小部分。使用贝叶斯树数据结构来获得有效的变量排序，从而最大程度地减少了计算时间。由于新的测量结果只对估计值的局部产生影响，通过分析受影响的区域，可以减少一些没必要的计算，加速优化流程。

基 于 优 化 的 方 法 是 目 前 VI-SLAM 领 域 以 及SLAM 相关领域的热点之一，非线性优化方法实现了相较于滤波方法更精确的状态估计效果。但由于计算资源有限，目前的方法往往以牺牲一部分精度和路标点为代价换取系统的实时性。新型的优化器以及并行计算方法的出现有望进一步减少对计算资源的占用，但大范围在线系统的实时性仍然是一个有待解决的问题。

3. 基于滤波的方案

不同滤波方法的分类如图 9 所示，卡尔曼滤波器作为一种解决状态估计问题的经典模型，主要存在 2 个问题。首先，它要求时序和测量方程是线性的，这个问题可以由 EKF 和 UKF 解决。其次，卡尔曼滤波器假设后验分布是单峰分布的，而且需要通过均值和协方差来表达，因此，它针对物体的位姿只能有一个假设，并不能保证关于状态的多个假设，这个问题可以由粒子滤波器（particle filter，PF） 解决。

一个完整的 EKF 框架包括预测步骤和更新步骤。对于基于滤波的 VI-SLAM 方法，惯性传感器能够提供 3 轴的加速度和角速度，用于计算 3 维刚体运动的动态模型并在预测步骤中进行预测。相机能够提供在特征和移动机器人之间的角度和距离测量结果，并在更新步骤中更新预测结果。

早期的 SLAM 工作主要是基于扩展卡尔曼滤波器，由 Smith 等基于早期的工作实现。Jones 等将 EKF 框架引入 VI-SLAM 系统。该系统实现了将状态和参数作为在线程序的一部分进行估计，并用 EKF 框架进行了有效实现.Kelly 等将 UKF 框架引入 VI-SLAM 系统。该方法能够在线、随时校准更新位姿，比如正在进行的导航或建图任务。

MSCKF 是 Mourikis 在 2007 年 提 出 的，MSCKF 2.0 也在随后由 Mourikis 和 Li 提出 ，是目前许多 VI-SLAM 系统的基础。MSCKF 是一种基于扩展卡尔曼滤波器的 VI-SLAM 框架。在传统EKF 框架中，特征点信息会加入到状态向量和协方差矩阵里，这会加大矩阵维度，增加计算量，如图 10 所示。而MSCKF 则在状态向量里只添加窗口内的相机状态，而忽略特征点，减少计算量。同时系统一般采用IMU静态初始化，系统启动后需要静止一段时间，才能初始化成功，该系统没有回环检测功能。MSCKF 工作的最主要贡献在于推导出一种测量模型，该模型能够表达从多个相机位姿观察到静态特征时出现的几何约束。系统中维护一个位姿的滑动窗口，如果一个特征点在滑动窗口内的几个位姿都被观察到的话就会在这几个位姿间建立约束。这种方法用一个特征点约束多个相机位姿，从而进行 KF 的更新，旧的特征点和滑动窗口之外的相机姿态则被丢弃。近几年，许多基于 MSCKF 的工作相继提出，框架整体的精度和鲁棒性得到了不断的提升。

苏黎世联邦理工学院无人系统实验室（Autono-mous Systems Lab，ASL）的 Bloesch 等提出了一种基于迭代扩展卡尔曼滤波器（IEKF）的直接法单目视觉惯性里程计，简称 ROVIO（robust visual inertialodometry） 。如图 11所示，该算法将视觉信息和IMU信息进行紧耦合的一种视觉惯性测量单元。数据融合的方法主要是通过迭代卡尔曼滤波来进行的。对于视觉方面的信息，作者主要是通过将路标点在图像中对应的点周围的图像块做为路标点的描述子，从而得到光度误差。然后将光度误差进行变换得到IEKF中的innovation term，进而进行滤波状态的更新。

整体的滤波方程的构造是以机器人为中心进行构造的（fully robocentric）——实际上这个robocentric就是以IMU为原点固连在IMU上的坐标系——从而保证能观状态不受不断增长的全局协方差的影响，这样可以减小因非线性而造成的误差。此外，作者还将路标点的空间位置信息拆开成了两项。一项是bearing vector（二维向量），还有一项是distance（这里实际上用的是逆深度）。并在 IMU 预测阶段对路标点进行预测，在视觉更新时对其修正，不像其他框架一般仅在视觉阶段去计算。这样的构造方式可以避免不能观状态量，并且使得初始化没有延迟。由于使用了基于 QR 分解的测量空间缩减方法，并执行每个路标点的更新迭代，因此系统具有较高的效率和准确性。

ARL 实 验 室 随 后 又 推 出 了 Maplab 框 架，与 ROVIO 不同的是，Maplab 带有一个完整的 VI-SLAM 系统，具有回环检测和重定位功能。系统由两部分组成，一部分是 ROVIOLI（ROVIO with lo-calization integration），一个在线的视觉惯性全局定位系统，其接收图像与惯性传感器数据作为输入，输出全局的位姿估计，并建立地图。另一部分是离线的 Maplab 控制台，可以让使用者以离线批处理方式在地图上应用各种算法。

在 松 耦 合 方 法 中， Faessler 等使 用 了 一种基于 SVO（semi-direct visual odometry）和 MSF（multi-sensor fusion）的方案。SVO 是一种计算量较小的稀疏直接算法。其通过跟踪 FAST（fromaccelerated segment test） 特征并最小化周围图像块的光度误差来匹配帧间图像，使用非线性最小二乘法来最小化特征中的重投影误差，得到仅由相机估计的位姿。MSF是一种通用的 EKF 框架，用于在位姿估计中融合来自不同传感器的数据。系统将纯视觉 SVO 得到的位姿作为通用位姿传感器的输出提供给 MSF，然后与 IMU 数据融合。由于是一种松耦合框架，位姿的尺度需要近似正确，因而时常要手动初始化。

Open VINS是黄国权老师团队在2019年8月份开源的一套基于MSCKF的VINS算法。主要提供了流形滑动窗口卡尔曼滤波器、在线摄像机内、外标定、摄像机-惯性传感器时间偏移标定、具有不同表示和一致第一估计的SLAM地标（FEJ）处理、用于状态管理的模块化系统、可扩展视觉惯性系统模拟器、用于算法评估的广泛工具箱等功能。其在前端图像信息处理中，结合了EKF-SLAM、MSCKF和Hybird SLAM等算法处理方式，提高了系统的稳定性，在一些开源数据集上其精度可以媲美基于优化的SLAM。该开源代码的主要贡献

1. 提供了一个可拓展、开源的代码库

2. 提供了许多视觉视觉惯性的基础，包括多相机，多IMU，视觉惯性运动对象跟踪，Schmidt-based visual-inertial SLAM，点平面和点线视觉惯性导航等。

开源代码大致流程如图12

图12 openvins算法流程示意图

表1 代表性 VI-SLAM 框架对比

4.滤波方法与优化方法的联系与对比

利用贝叶斯推断可以建立基于滤波和基于优化这 2 类方法之间的联系。基于滤波的方法，位姿的先验分布由内感受型传感器的测量构建，似然分布由外感受型传感器的测量建立，故可以视为最大后验估计（maximum a posteriori，MAP）问题。基于优化的方法，通过迭代找到测量总概率最高的状态，故可视为最大似然估计（maximum likelihood，ML）问题。基于优化的方法，可以通过向传感器的测量值中添加正则化项或先验项，使其由 ML 问题转化为 MAP 问题。如果把 EKF 框架看作是非线性高斯-牛顿法或高斯法的近似，那么它的表现无疑是达不到要求的。

主要原因是，EKF 没有迭代至收敛的过程，其雅可比矩阵只计算 1 次，可能会远离所期望的最优估计。但由于 EKF 没有一次性计算所有的雅可比矩阵，EKF 的结果比单次的高斯-牛顿法迭代更精确，它的缺陷只在于没有迭代这个步骤。从优化的角度来看，这是一个显而易见的问题，因为优化是需要最后迭代至收敛的。EKF 使用了马尔可夫假设来实现其递归形式，由于使用了马尔可夫假设，一旦滤波器建立在该假设上就无法摆脱它。包括IEKF 在内，虽然 IEKF 一次迭代了一个时间步长，但它仍然依赖于马尔可夫假设，而且仅在一个时刻上进行了迭代，并非在整个轨迹上。

本文比较了几种代表性的 VI-SLAM 框架，如表 1 所示。可以看出，目前主流的 VI-SLAM 实现方法以紧耦合的优化方法为主。相较于松耦合的方法，把 IMU 状态与相机状态合并在一起进行状态估计的紧耦合方法具有更高的精度。而由于基于滤波的方法具有马尔可夫性，无法考虑到某时刻状态与之前所有时刻状态的关系，目前普遍认为在计算资源足够的情况下，基于优化的方法会得到更精确的效果。但是在计算资源受限，或者移动机器人位姿轨迹比较简单的场合里，基于滤波的方法仍然是一种有效的方式。

5. 展望

5.1 与深度学习结合

构建语义地图、在帧间匹配和回环检测中采用 深 度 学 习 的 方 法 是 目 前 SLAM 的 研 究 热 点 之一 。深度学习方法的引入使移动机器人可以理解周围环境的语义信息。完成一些更复杂的任务。但深度学习方法对于计算资源的需求巨大，在硬件条件较差的嵌入式场景难以使用，需要研究人员进一步地改进网络。

5.2 轻量化

SLAM 本身是为了给上层应用提供自身位姿估计，在实际应用中，研究人员并不希望算法占用太多运算资源。使用者希望 SLAM 框架能够轻量化，不影响移动机器人或者手机移动端所要完成的其他工作。相比于激光 SLAM 等成本高昂的方法，由于 VI-SLAM 系统只使用相机和惯性传感器，在无人机或手持移动设备上良好运行 SLAM 程序成为可能。通过继续改进算法，现在很多大厂均能够在移动端实现效果良好的定位效果。

5.3 与更多传感器融合

视觉与 IMU 的融合已经在不同的领域取得了较理想的效果，但是当场景特别大时，视觉和 IMU传感器的效果不太理想，这时就需要用到其他传感器。在自动驾驶领域，将激光雷达、 GPS/GNSS（全球卫星导航系统）、相机以及 IMU 等多传感器进行融合已经成为一种趋势，引入激光雷达等传感器会为系统提供更准确的深度信息。但是更多的传感器会带来更复杂的标定与融合方面的问题，有待研究人员加以解决。

5.4 嵌入式系统

移动机器人通常使用嵌入式设备作为平台的计算硬件，而随着计算能力的大力发展，嵌入式设备的使用频率逐渐增多。新型 GPU（图形处理器）的出现使得嵌入式系统的性能更加接近 PC 机，但是计算能力仍然有限。一些嵌入式硬件，例如Jetson Nano、TX1/2 的出现使得研究人员可以在嵌入式系统中使用 GPU，便于移动机器人在恶劣的条件下应用 VI-SLAM 算法。随着一些代表性产品 Google Project Tango、Microsoft Hololens、MagicLeap、Nreal等的应用，VI-SLAM 技术会逐步融入到平常生活之中。

责任编辑：haq