A review of visual inertial odometry from filtering and optimisation perspectives

[1] Gui J, Gu D, Wang S, et al. A review of visual inertial odometry from filtering and optimisation perspectives[J]. Advanced Robotics, 2015, 29(20): 1289-1301.

红色标记为不确定是否正确，水平有限，欢迎指正。

---

 

摘要：视觉惯性里程计(VIO)是一种利用机载摄像机和IMU传感器的测量数据，估计移动平台位置和方向随时间变化的技术。近年来，VIO因其尺寸小和传感器模块价格低廉，受到了大量研究者的关注，并在各种潜在的应用中得到了广泛的应用。然而，考虑到精度、实时性、鲁棒性和运行规模，无论是技术开发还是工程实现都是非常具有挑战性的。本调查从滤波法和优化法两个方面报告了VIO技术的现状，这两种方法在研究领域占主导地位。为此，对三维刚体的各种表示形式进行了说明。先回顾了滤波法，然后是优化法。通过贝叶斯最大后验的框架来阐明这两种方法之间的联系。同时还会讨论一些其他特征，如可观测性和自校准。

---

1. 概述

定位与建图是机器人导航与控制研究领域的两个基本问题。SLAM技术和SFM技术[1]分别成为机器人和计算机视觉研究领域多年来的主要课题之一可以证明这一点。随着人们对将这些技术应用于小型平台(如小型无人机或手持移动设备)的兴趣日益浓厚，定位和建图的研究重点已转向相机和惯性测量单元传感器的使用。由于芯片和微电子机械系统(MEMS)设备的快速发展，这些传感器开始具有高精度、小型化和低成本的特点。它们之间在某种程度上是互补的，通过它们提供的冗余信息可以弥补各自的不足。此外，生物学研究表明，人类和一些动物的导航功能依赖于多种形式的运动传感模式和视觉的结合。[2-4]

定位与建图问题是机器人中的状态估计问题。对于给定噪声测量的动态系统，随机估计算法如扩展卡尔曼滤波器(EKF)或粒子滤波器(PF)的是主流工具。一些内部传感器提供了姿态随时间变化的测量，并被表示为一个数据驱动的移动平台动态模型。这些例子包括地面移动机器人中的光学编码器和飞行机器人中的imu。它们以累积误差和偏置测量而闻名。一些外部传感器，如照相机或激光测距仪，能够提供角度或距离测量。基于三角测量或三边法，他们能够估计移动平台的位置或方向。估计的可靠性在很大程度上取决于环境条件，结果对测量噪声敏感。然而，它们可以作为状态估计问题的度量模型。然后，EKF或PF通过融合动态模型中的分布和测量模型中的分布，将估计的分布由前一步传播到当前一步。在大多数情况下，EKF代替PF，因为要估计的状态数量很大。然后，估计了高斯分布的均值和协方差。

视觉运动估计是计算机视觉领域的一个优化问题。通过图像对齐迭代计算相机姿态的变化[6,7]。通常，特征是从一幅图像中提取出来，然后重投影到另一幅从不同视角捕获的图像中。在此基础上，最小化特征与重投影特征之间的误差代价，得到摄像机的姿态变化和环境结构。优化法主要是基于梯度的，如牛顿法或高斯牛顿法。在大多数情况下，利用矩阵的稀疏结构来提高计算效率。只提供估计状态的值，而没有关于估计分布的信息。

在贝叶斯推理框架下建立滤波法和优化法之间的联系。优化法被看作是一个最大似然(ML)公式，其中迭代地找到测量总概率最高的状态。滤波法被看作是一个最大后验(MAP)公式，其中平台姿态的先验分布由本体感觉传感器的测量值构成，似然分布由外感受传感器的测量值构成。对于非线性动态模型和/或非线性测量模型，迭代EKF等价于基于优化的方法，迭代更新EKF中的每一步。优化法可以通过添加来自内部传感器或其它源测量的正则化项或先验项，将ML问题重新表述为MAP问题。这些是在线或因果算法，其中当前估计依赖于当前和以前的测量。离线或非随机算法是批处理过程，其中当前估计依赖于完整的数据集，或者可以说，它们不仅依赖于当前和以前的测量，而且还依赖于未来的测量。在优化法中，批处理被分解为求解一组线性代数方程。在滤波法中，卡尔曼平滑算法可以通过前向和后向两种方法得到后向高斯分布。

VIO问题中有一长串状态变量和参数，需要通过给定的IMU和camera测量值来估计。其中一些是不可观察或不可识别的，从而导致错误性能的提高。对状态和参数的可观察性和可识别性的分析提供了对估计结果的清晰理解，并可能为通过专门的运动模式充分刺激平台提供指导。

主流的SLAM技术能够提供平台姿态和环境结构的估计结果。需要闭环检测来约束估计算法产生的累积误差[8-10]。但是，VIO技术中没有显式地执行回环闭合。因此，VIO中的估计错误将会累积，并且无法被限制。由于没有闭环检测，小型设备可以实现长时间大规模的定位。然后，研究的问题转向了如何利用这些技术来减缓误差的增加。

本文将介绍VIO技术的最新发展和进展。从滤波和优化的角度，说明了它们的基本模型、算法和最近的结果。调查还将突出两种方法之间的联系，并对VIO技术进行深入的介绍。对状态可观测性和参数可辨识性进行了讨论。总结了我们对这一研究领域的展望。

第2节介绍滤波法，第3节介绍优化法。它们之间的联系总结在第4节。可观察性和可识别性的分析将在第5节中讨论。第6节简要介绍了结论和今后的工作。

---

2. 滤波法

EKF框架一般由预测步骤和更新步骤组成。对于滤波法VIO，惯性传感器能够提供三轴加速度和角速度测量值，作为三维刚体运动的数据驱动动态模型或先验分布，并在预测步骤中进行运动预测。相机能够提供特征与移动平台之间的角度和距离测量，作为测量模型或似然分布，并在更新步骤中更新预测结果。

我们假设一个只有一个摄像头和一个IMU的移动平台，在未知环境中移动。相机与IMU之间的空间关系是固定的，可以表示为已知的位置和姿态。基于EKF的VIO算法目标是利用惯性测量和未知环境的视觉观测提供平台的位置和方向信息。接下来，将基于[11]对EKF框架进行全面的描述，包括状态表示、IMU数据驱动的动态和可视化观察。这将为下文的进一步分析和讨论铺平道路。

2.1. IMU数据驱动动态模型

三维刚体任意时刻的IMU状态向量可由一个16×1的向量定义：

q是描述从世界坐标系到IMU坐标系旋转的单位四元数，p和v是相对于世界坐标系的位置和速度，bg和ba是一个3×1的向量，分别描述陀螺仪和加速度计的bias，图1所示为两个坐标系之间的空间关系。

 

2.2 误差状态表示与更新

 

2.3 视觉测量模型与更新

 

---

3. 优化法

基于优化的方法主要依靠图像处理技术进行特征提取和图像对齐优化，而惯性测量被视为先验、正则化项或完全忽略。在大多数情况下，基于优化的方法有两个阶段:映射和跟踪。在映射阶段，通过各种特征检测器从图像中提取角、边等三维空间的特征。然后在两幅图像之间为检测到的所有特征定义一个重投影误差。误差作为一个成本函数被用来优化，以便找到特征或地标的坐标。在跟踪阶段，利用地图中地物和地标的坐标来定义两幅图像之间的重投影误差，并再次应用优化算法来寻找移动平台的位置和方向的变化。将估计问题分解为两个阶段的思想是为了在映射过程耗时的情况下获得快速的跟踪结果。[14,15]利用3d特征坐标和移动平台姿态变化在两幅图像之间重新投影误差来定义成本，[19]可以同时进行优化，而利用关键帧的概念可以将旧状态边缘化，从而维持一个有限的优化窗口进行实时操作。

 

3.1 特征对齐

 

3.2 稠密对齐

 

3.3 惯性测量项

 

 

---

4. 滤波法和优化法之间的关联

滤波法和优化法都可以用贝叶斯推理来表示。当需要连续的近似线性化时，可以通过迭代EKF显式地建立它们之间的联系。当近似线性化只是一个单一的步骤时，包含正向传递和反向传递的平滑法 等价于 通过最小二乘问题的信息矩阵的 Cholesky 分解求解的优化法。为了降低计算复杂度，通过只维护关键帧或滑动窗口来减少待估计状态变量。特别是，滑窗法或动时域估计法将成本分成两部分，实现两部分的一种流行方法是在一部分中优化结果，在另一部分中使用EKF将最老的状态边缘化。

4.1 迭代EKF更新

滤波法的核心是卡尔曼滤波，优化法的核心是高斯牛顿法。它们之间的联系是迭代EKF (IEKF)。EKF有两个步骤：预测和更新。设当前时刻 k 的预测步骤结果为 ...， EKF与IEKF的区别在于IEKF的更新步骤中存在迭代循环，而EKF的更新步骤中只执行一个循环。在优化法中，IEKF的迭代循环能够使模型线性化引起的误差尽可能接近于相应的误差。

下面，我们将展示IEKF更新步骤中的迭代循环与高斯牛顿优化方法(ML)的等价性。

 

4.2 平滑法

滤波法是一个序列迭代过程，其中当前的估计结果依赖于当前的测量值和过去的估计结果(过去的测量值被压缩为过去的估计结果)，即未来的测量对当前的估计结果没有任何贡献。如果情况并非如此，即未来的测量确实对当前的估计结果有贡献，则应采用基于平滑的方法。这更像是一个批处理算法，当所有的测量值都可用时，才进行估计。

 

4.3 关键帧边缘化

 

4.4 动时域估计

 

 

 

---

5.状态可观测性与参数可识别性

VIO解决的问题是：在给定惯性和视觉传感器测量值的情况下，在线恢复全局帧中的平台轨迹。然而，从工程实现的角度来看，这个问题并不容易解决。除了滤波法和优化法的复杂性外，一些参数对于表示平台轨迹动态状态估计的成功与否也起着至关重要的作用。这些参数包括：

1. 相机内参：焦距、主点和镜头畸变
2. IMU参数：加速度计和陀螺仪偏差
3. 空间参数：IMU与相机之间的转换。
4. 时间参数：IMU与相机测量之间的时间延迟

除IMU偏置和系统参数外，它们被视为时不变变量。相反，平台轨迹由时变变量表示：状态。

通过对可观测性和可识别性的分析，仅考虑IMU和摄像机的测量值，就可以回答这些状态和参数是否可以恢复的问题。传统意义上，状态是动态变量，用可观测性分析状态估计问题，参数是静态变量，用可辨识性分析状态标定问题。利用随机游走过程对参数进行建模，可以利用可观测性(如IMU偏差)对参数进行分析。

5.1 状态可观测性

可观测性是一种基本属性，它反映了根据输入/输出数据估计状态的可能性。用 y 表示初始状态 x0 的输出轨迹，初始时间 t0 和测量读数 z(t) 。两个初始状态 x01 和 x02 ... 如果状态是可区分的，则可以从输出和已知测量值中估计它们。如果状态是无法区分的，那么它们就被称为不可观测的，并且它们相应的方差将无限制地增长和膨胀。

线性系统的可观测性是一个全局性质，它既可以由可观测矩阵的秩决定，也可以由格拉曼矩阵的秩决定。然而，非线性系统的可观测性是由给定状态的局部决定的[36]。局部可观测性强于可观测性。局部可观测性将状态与其相邻状态区分开来。局部微弱的可观测性瞬间就把状态与其邻居区分开来，而没有大的扰动[37]。

局部弱可观测性的优点是李代数检验的可用性。对于由式(2)和式(6)组成的非线性系统。

 

如果O在x0处具有全列秩，则系统在x0处是局部弱可观测的。由于状态方程和测量方程是无限光滑的，可观测矩阵可以有无限行。如果能找到足够多的线性无关的行，则O是满秩的，系统是局部弱可观测的。

基于可观测秩条件，已有文献表明平台在全局坐标系中的位姿是不可观测的，绕重力矢量(yaw)的旋转也是不可观测的。其他一些状态是可以观测到的，包括初始相机帧中的平台位姿、初始相机帧中的重力矢量以及相机帧中的特征。直观的解释是，视觉摄像头只是一个方位传感器，IMU只是位姿的双积分器，无法在全局帧中提供位姿和偏航信息。

可观测性分析使我们能够找到充分激发系统的方法，如使用给定世界坐标的三个非共线特征，或用伪量方程使一些不可观测的东西成为可观测的[1]。此外，它有潜力提供线索，如何使不可观测的爆炸尽可能慢。

对于滤波法，EKF采用线性化系统。然而，线性化系统的可观测性不同于相应的非线性系统。更具体地说，在线性系统中偏航是可观察到的。这种非预期的可观测DOF导致了不一致的结果：估计的协方差优于实际测量结果。提高一致性的一个方向是由于模型线性化而在错误可观测方向施加约束[38]。

利用连续对称的概念对可观测性进行了分析，该概念能够求出可观测状态和可识别参数的解析推导。

5.2 参数可识别性

在大多数与视觉相关的定位技术中，相机内参都是预先知道的。它们也可以在线校准[40]。

IMU偏差随时间而变化，在大多数情况下被建模为时变状态。可观测性秩条件表明它们是可观测的。

IMU与摄像机之间的空间参数为六自由度变换，必须精确地知道，才能稳定估计结果。它们被建模为随机行走过程，并被视为时变状态。通过对可观测等级条件的分析，发现六自由度摄像机变换，以及imu偏置、重力矢量和场景结构尺度都是可观测的。充足的可观测性要求足够的激励，即平台至少沿两个IMU轴经历旋转和加速度[41-43]。

惯性和视觉传感器测量之间的时间参数是时间延迟。通过构造包含时滞和其它量的约束方程，建立了时滞的局部可辨识性。对约束的雅可比矩阵秩进行局部可辨识性检验。结果表明，在一般轨迹上，时滞是局部可识别的。描述了导致可识别性损失的临界轨迹。利用[45]中的ICP算法估计了惯性和视觉传感器测量之间的时间延迟。

---

6. 结论

本文综述了视觉惯性里程计的研究现状。从基于滤波和基于优化的两个方面进行描述。同时也通过基于特征和基于密度的图像处理方法对其进行了描述。将EKF和图像对齐的广泛知识统一到一个框架中。这两种方法之间的联系通过迭代EKF、平滑化和边缘化得到了描述，并对这两种方法进行了深入的研究。分析了状态可观测性和参数可辨识性。本文还总结了该领域的一些技术，包括EKF、MAP、IEKF、BA、ICP、MHE等。在未来，探索这些变体的计算复杂性将为该领域的从业者提供更直观的指导。应该提供更多的实现细节作为这项工作的集成部分。