VIO/LIO 入门——将 IMU 与摄像头或激光雷达结合用于自定位

视觉SLAM和激光雷达SLAM都无法单独应对所有情况。摄像头在黑暗或快速运动环境下表现不佳；激光雷达在特征稀少的环境中性能较差，且成本更高。弥补这些不足的关键在于惯性测量单元（IMU），它可以高频测量加速度和角速度。视觉惯性里程计（VIO）和激光雷达惯性里程计（LIO）——将摄像头或激光雷达与IMU结合使用——如今已成为众多自主移动机器人、无人机和AR/VR设备的实际标准配置。本文首先阐述了为什么IMU无法单独提供位置估计，然后深入探讨了传感器融合的概念、基于滤波和基于优化的两种估计设计理念，最后介绍了具有里程碑意义的算法：ROVIO、OKVIS、VINS-Mono、OpenVINS、LIO-SAM和FAST-LIO2。

IMU/INS 示例

LiDAR 示例

图片：Xsens MTi-G (Kallap85, CC BY-SA 4.0) / Livox Mid-40、Horizon 和 Tele-15 (Dllu, CC BY-SA 4.0)，Wikimedia Commons。

代表性的输入传感器，并非必需的 VIO/LIO 硬件配置。

0. 本文内容

• IMU 的测量内容，以及为什么 IMU 本身无法持续恢复位置信息

• 摄像头和激光雷达各自的局限性，以及为什么它们与 IMU 结合使用效果显著

• VIO 和 LIO 分别指代什么，以及它们之间的区别

• 基于滤波器（EKF/MSCKF）和基于优化（因子图）的设计理念之间的区别

• ROVIO、OKVIS、VINS-Mono、VINS-Fusion 和 OpenVINS（VIO）以及 LIO-SAM、FAST-LIO 和 FAST-LIO2（LIO）各自的贡献

• 如何根据实际应用场景选择 VIO 或 LIO

1. 简答：IMU、VIO 和 LIO 是什么

用一句话概括：**IMU（惯性测量单元）是这种传感器结合了加速度计和陀螺仪，无需任何外部参照物，即可高频测量自身的加速度和角速度。视觉惯性里程计 (VIO) 是将这种惯性测量单元 (IMU) 与摄像头结合，以估计自身位置和姿态的技术；激光雷达惯性里程计 (LIO) 则通过将 IMU 与激光雷达 (LiDAR) 结合来实现相同的功能。

IMU 以高频（通常为数百赫兹，最高可达约 1 千赫兹）告诉你“我当前的加速度和旋转速度有多快”，但其积分量（位置）会随时间快速漂移（我们将在第 3 节中详细讨论这一点）。摄像头或激光雷达可以通过交叉参考周围环境来提供相对位置约束，但频率较低（数十赫兹），并且在某些情况下（例如黑暗、场景特征稀少、快速运动）会完全失效。 VIO 和 LIO 的核心理念是将“高频但存在漂移的 IMU”与“具有环境相关故障模式的低频摄像头/激光雷达”相结合，从而弥补彼此的不足。

2. IMU 实际测量什么？

IMU 内部大致包含两种传感器。

加速度计测量其传感器坐标系中的特定力。理想情况下，\mathbf{f}_b=R_{wb}^{T}(\mathbf{a}_w-\mathbf{g}_w)，其中 R_{wb} 将传感器坐标旋转到世界坐标系，\mathbf{a}_w 为世界坐标系下的加速度，\mathbf{g}_w 为重力加速度。它在静止状态下读数约为 1 g，在理想自由落体状态下读数接近于零。在积分之前，需要校正偏差、旋转到世界坐标系并加上重力加速度。有关坐标和偏差的定义，请参阅 OpenVINS IMU 模型。

陀螺仪测量传感器自身的旋转速率，即角速度。同时测量三个轴的角速度，即可得到绕横滚、俯仰和偏航轴的旋转速率。

通过结合这两种传感器，惯性测量单元 (IMU) 可以高频输出六自由度运动信息（三个平移自由度 + 三个旋转自由度），且完全基于传感器自身的内部状态，无需任何外部参照物（例如地图上的地标或无线电信号）。这种“完全无需外部参照”的特性既是 IMU 的最大优势，也是其最大的劣势——优势在于其鲁棒性，即使在完全黑暗或玻璃墙房间内也能保持性能不变；劣势在于我们在第 3 节中讨论的累积误差。

此外，IMU 的输出始终包含偏差和噪声。偏置是传感器固有的偏移误差——即使真实值为零，输出值也不会为零——并且由于温度变化等因素，它会随时间缓慢漂移（偏置不稳定性）。噪声是每次读取时都会新增的随机波动。价格较低的MEMS（微机电系统）IMU往往具有更大的偏置和噪声，这是接下来讨论的漂移的主要原因。

3. 仅使用IMU获取位置（积分和漂移）

从IMU输出恢复位置需要对加速度进行两次积分：速度是加速度的积分，位置是速度的积分。

其中，p_0是初始位置，v_0是初始速度，a(s)是时间s时的加速度。公式本身很简单，但实际应用中的加速度 a(s) 总是带有第 2 节讨论的偏差 b。对包含偏差的加速度 a(s) + b 进行双重积分，会产生一个由偏差驱动的误差项，其形式为

该误差项随时间平方成正比发散。此外，陀螺仪偏差会逐渐使姿态估计产生偏差，而该姿态误差会影响加速度的积分方向——因此，整体位置误差往往会增长到 O(t^2) 到 O(t^3) 的量级。这种误差随时间无限累积的现象被称为漂移。

一个具体的数值有助于理解这一点。假设一个廉价的 MEMS IMU 的加速度计偏差约为 0.01\,\mathrm{m/s^2}。 10 秒后，位置误差约为 \frac{1}{2} \times 0.01 \times 10^2 = 0.5\,\mathrm{m}。1 分钟后，误差约为 18\,\mathrm{m}。IMU 仅能在极短的时间尺度内（几秒到几十秒）提供可靠的位置估计，这也是 IMU 无法作为自定位系统的根本原因。换句话说，IMU 的优势在于“短期、高频变化”，而劣势在于“长期、绝对位置”——这与相机或激光雷达的优势和劣势正好相反。

4. 摄像头和激光雷达也有弱点

如果仅使用惯性测量单元 (IMU) 无法恢复位置，人们自然会想到使用摄像头（参见视觉 SLAM 入门）或激光雷达（参见激光雷达 SLAM 技术趋势）来代替。这在某种程度上是正确的，但摄像头和激光雷达各自都有其独特的弱点。

摄像头的弱点包括：低光照条件下的图像质量下降（特征点或直接法所依赖的亮度梯度将无法计算）、低纹理环境（纯白墙壁或纯色地板根本无法产生对应点）以及快速运动时的运动模糊（图像在曝光窗口期间会模糊，导致特征描述符不稳定）。所有这些问题的根本原因都相同：摄像头接收到的视觉信号不足。

激光雷达的弱点在于其在特征稀疏的环境中表现不佳（例如在长隧道或开阔的平面上，像ICP/NDT这样的点云配准方法无法收敛到唯一解——这被称为几何退化），以及传感器本身的成本较高。激光雷达通过主动发射激光来测量距离，因此它对黑暗和逆光环境具有较强的鲁棒性，但在环境几何特征稀疏的地方，它的表现与摄像头类似。

惯性测量单元（IMU）的弱点，如第3节所述，在于其会随着时间推移而累积无限大的漂移，并且它本身完全无法恢复绝对位置。

关键在于，这三种传感器失效的情况几乎没有重叠。在相机难以工作的黑暗环境中，惯性测量单元 (IMU) 仍能正常工作，并且即使相机或激光雷达 (LiDAR) 失去信号，IMU 也能持续提供高频运动信息。相机和激光雷达的观测利用环境几何结构来约束漂移，但如果没有已知地图、全球导航卫星系统 (GNSS) 或闭环等约束条件，它们无法保证长期的绝对位置精度。这种互补性是下一节讨论的传感器融合的出发点。

5. 传感器融合的概念

传感器融合是指将多个传感器的不同误差特性和优势领域结合起来，以获得比任何单个传感器更精确、频率更高、更稳健的估计结果（有关融合的总体概念，请参阅传感器融合入门）。 VIO 和 LIO 共享的框架可以组织成一个预测-更新循环：

Diagram 1 · Use the button to switch views

Diagram 2 · Use the button to switch views

图1 — IMU（高速率）持续预测通过传播（积分）过程更新状态。在更新步骤中，利用特征/点匹配和相机/激光雷达（低频）观测数据修正预测结果。修正过程中估计的IMU偏差被反馈到下一次传播过程中，持续抑制漂移累积。

该循环有两个关键点。首先，IMU驱动的传播过程持续高频运行，即使在没有相机/激光雷达观测数据的情况下（例如黑暗、纹理稀疏、特征稀少、快速运动），也从未停止位姿预测。其次，更新步骤（仅在收到相机/激光雷达观测数据时触发）减少了仅靠IMU预测会无限累积的漂移，并同时更新IMU自身偏差的估计值，将其反馈到传播过程中。这种“持续高频运行的预测”加上“在观测数据约束的方向上减少误差的修正”的组合，是VIO和LIO共同的设计理念。

6. VIO （视觉惯性里程计）

视觉惯性里程计 (VIO) 是一种配置，其中摄像头在此框架中扮演更新端传感器的角色。它将惯性测量单元 (IMU) 的高频预测与摄像头的特征点观测相结合，利用 IMU 的度量加速度信息，解决了困扰单目视觉里程计的尺度模糊问题（如第 4 节所述——单目图像本身无法恢复真实世界的度量距离）。在摄像头难以捕捉的快速运动中，IMU 可以持续高频地提供姿态变化信息，从而大大降低跟踪失败的可能性。

根据摄像头和 IMU 的连接方式，VIO 进一步分为松耦合和紧耦合两种配置。松耦合系统分别独立计算摄像头端的姿态估计和 IMU 端的姿态估计，然后在事后融合这两个结果；它易于实现，但缺点是…… IMU 端无法利用相机端估计的内部误差结构（即特征观测的不确定性），因此精度会降低。紧耦合系统从一开始就将 IMU 的原始数据和图像的特征观测视为一个单一的联合状态估计问题；虽然实现起来更复杂，但两个传感器的每一位信息都会被利用，从而获得更高的精度。我们在第 8、9 和 10 节中介绍的具有代表性的现代 VIO 实现（OKVIS、VINS-Mono、OpenVINS 等）都采用了紧耦合。

7. LIO（激光雷达惯性里程计）

LIO（激光雷达惯性里程计） 框架与上述相同，只是将激光雷达作为更新端。激光雷达点云可以直接高精度地捕捉环境几何形状，但单次扫描需要数十到数百毫秒，如果传感器本身在此期间发生移动，则生成的点云会发生变形（运动）。畸变）。利用IMU的高频姿态信息，可以校正扫描过程中的运动，从而重建无畸变的点云。

与第6节中的VIO类似，在LIO难以处理的几何退化环境中（例如长隧道、开阔的平面），IMU持续的短期位姿预测使得点云配准（例如ICP/NDT等方法）的偏差大大降低。LIO在实践中也主要以紧耦合系统的形式实现——第10节中介绍的LIO-SAM和FAST-LIO/FAST-LIO2均采用了这种紧耦合设计。

8. 基于滤波器的方法（卡尔曼滤波器/扩展卡尔曼滤波器/多级卡尔曼滤波器）

VIO和LIO的核心——融合预测和观测——基于两种主要的设计理念之一：基于滤波器的方法，或下一节中介绍的基于优化的方法。章节。

基于滤波器的方法的基础是卡尔曼滤波器，它是一种框架，通过交替进行预测和基于观测的更新，递归地推导出具有线性动力学系统的最优状态估计。对于像VIO/LIO这样的问题，由于IMU积分（位姿合成）和相机投影的组合本质上是非线性的，因此使用非线性扩展：扩展卡尔曼滤波器（EKF）。EKF将每个非线性函数围绕当前估计值线性化（一阶泰勒展开），然后应用与普通卡尔曼滤波器相同的更新方程。

其中，\hat{x}_k^{-}是传播预测的状态，z_k是实际观测值，h(\cdot)是根据状态预测观测值的观测模型，而K_k是卡尔曼增益——决定权重大小的因素。预测值比观测值更可靠。P_k^{-} 是预测状态的协方差（不确定性），H_k 是观测模型的雅可比矩阵线性化，R 是观测噪声的协方差。直观地说，预测不确定性 P_k^{-} 相对于观测噪声 R 越大，卡尔曼增益 K_k 就越大，观测信息 z_k 就越能更好地融入状态校正中。

简单地实现基于扩展卡尔曼滤波器 (EKF) 的视觉信息优化 (VIO) 需要将图像中的每个特征点都包含在状态向量中，而随着特征点数量的增加，计算成本会迅速膨胀。Mourikis 和 Roumeliotis 于 2007 年提出的 多状态约束卡尔曼滤波器 (MSCKF) 正好解决了这个问题。 MSCKF 滤波器的状态中不包含特征点本身；相反，它将多个相机位姿（滑动窗口）保存在状态中，并仅使用几何约束（即同一特征点在多个位姿中被观测到）来更新状态。通过从状态中排除特征点，它避免了特征数量增加导致的计算量爆炸，同时仍然利用了跨越多个帧的观测数据——这是一种高效的基于滤波器的 VIO 设计，目前已被广泛应用。

9. 基于优化的方法（捆绑调整/因子图）

基于滤波器的估计更新是递归的，但像 MSCKF 这样的方法可以暂时保留克隆的过去位姿。基于优化的方法将状态和观测数据保存在一个滑动窗口或图上，并求解非线性优化问题以获得它们的联合一致性。可以将其理解为扩展了 Visual-SLAM 的捆绑调整（参见Visual-SLAM 入门，第 10 节），使其也包含 IMU。信息。

IMU 加速度和角速率是已知的观测值，而非优化变量。在每个 IMU 时间戳处添加状态，或在每次重新线性化时重新积分每个时间间隔，都会增加计算成本。IMU 预积分由 Forster 等人于 2015 年提出（arXiv:1512.02363），它将关键帧之间的测量值汇总为一个可进行偏差校正的相对运动因子。该状态可以包含速度和偏差，在某些系统中还可以包含地标；计算成本由窗口设计和重新线性化控制，而不仅仅是由 IMU 采样率控制。

由此得到的 IMU 预积分因子，连同从相机/激光雷达观测值（重投影误差或扫描匹配误差）导出的因子，被放置在一个公共的因子图上，并作为一个最大似然（或最大后验概率）估计问题一起求解——这是基于优化的方法的基本形式。

\mathcal{X} 是待估计的位姿、速度、偏差以及（必要时）地图点的集合；r_{\mathrm{IMU}} 是预积分因子的残差。基于滤波器的估计通过顺序更新和有界历史来控制成本；基于优化的估计可以对滑动窗口进行重新线性化。精度和成本取决于窗口长度、初始化、重新线性化方案、模型和计算预算，因此两者没有绝对的优劣之分。

Diagram 3 · Use the button to switch views

Diagram 4 · Use the button to switch views

i Keyframe i{+}1 High-rate IMU measurements (hundreds of Hz) Integrated together over the interval → a single relative-motion Factor

图 2 — 在关键帧 i 和 i{+}1 之间到达的大量 IMU 测量值（数百赫兹）通过预积分预先积分，并在因子图上作为单个相对运动因子进行处理。这使得优化变量的数量与 IMU 的采样率无关，而仅取决于关键帧的数量。

10. 标志性算法

VIO

ROVIO（鲁棒视觉惯性里程计），由 Bloesch 等人在 IROS 2015 上提出，是一种基于 EKF 的 VIO。它并非匹配特征点描述符，而是将图像块亮度信息直接输入到扩展卡尔曼滤波器（EKF）的观测模型中——这种直接法式的方法使其成为将基于滤波器的估计与直接法相结合的典型示例。

OKVIS（基于关键帧的开放式视觉惯性SLAM）由Leutenegger等人发表于2015年国际雷达研究杂志（IJRR 2015），是一种基于非线性优化的关键帧视觉惯性定位（VIO）方法。它结合了惯性测量单元（IMU）预积分（第9节）和光束法平差式重投影误差最小化，是早期基于优化的VIO的里程碑式实现之一。

由香港科技大学的秦、李和沈开发的VINS-Mono是一款用于单目相机的VIO算法，它基于IMU预积分的非线性优化（第9节），并增加了回环检测以进行全局校正，同时提供了一种灵活的设计，允许每个组件在松耦合和紧耦合操作之间切换。VINS-Fusion是VINS-Mono的后续版本，它扩展了VINS-Mono的功能，支持多种传感器配置——立体相机、GPS——并优先考虑在更多样化的硬件设置中的实用性。

由Geneva、Eckenhoff、Lee、Yang、Huang及其合作者开发的OpenVINS是一个基于MSCKF的开源VIO研究平台。它基于MSCKF高效的滤波器设计（第8节），并因其易于扩展和验证的特性而被广泛用于研究目的。

LIO

LIO-SAM（基于平滑和建图的紧耦合激光雷达惯性里程计），由 Shan、Englot、Meyers、Wang、Ratti、Rus 及其合作者在 IROS 2020 会议上提出，是一种基于因子图的紧耦合 LIO。它将多种因子类型——IMU 预积分因子、来自激光雷达扫描匹配的里程计因子、GPS 因子和回环闭合因子——统一到同一个因子图上，是应用基于优化的设计理念（第 9 节）于激光雷达的典型示例。

FAST-LIO（快速激光雷达惯性里程计），由香港大学 MARS 实验室的 Xu、Zhang 及其合作者提出，是一种基于紧耦合迭代卡尔曼滤波器的快速 LIO。与基于优化的 LIO-SAM 不同，FAST-LIO2 基于滤波器，旨在直接处理高速率激光雷达点云，并优先考虑在计算能力有限的平台上进行实时操作。FAST-LIO2 是其后续版本：它通过使用增量式最近邻搜索结构（iKD 树）直接管理点云，显著降低了地图重建的成本，从而实现了更快、更精确的紧耦合激光雷达成像 (LIO)。

11. VIO 与 LIO 的比较

VIO 的优势在于其传感器成本低，以及颜色和模式识别带来的高层次环境感知能力（与物体识别天然契合）；LIO 的优势在于几何精度高，且对黑暗和恶劣天气具有鲁棒性——这就是二者的基本分工。

12. 实际选择

选择 VIO、LIO 还是二者结合，取决于您可携带的传感器、预算以及预期运行环境。

• 室内服务机器人/扫地机器人：如果成本是首要考虑因素，VIO（或松耦合的摄像头+IMU配置）往往更胜一筹；如果精度和几何精确的地图是首要考虑因素，则倾向于选择 LIO。在走廊和墙壁较多的普通室内环境中，激光雷达带来的精度提升更容易被接受。

• 无人机：由于有效载荷和功率限制严格，轻量级摄像头加IMU的视觉惯性里程计（VIO）配置往往占据主导地位。在开阔的户外空间，通常还会与GPS配合使用。

• 增强现实/虚拟现实（AR/VR）：使用头显内置摄像头和IMU的视觉惯性里程计（VIO）是事实上的标准；除了少数配备激光雷达（LiDAR）的高端型号外，成本和重量的限制使得VIO成为默认的首选方案。

• 自动驾驶/户外自主移动机器人：考虑到对恶劣天气、逆光和夜间驾驶的鲁棒性要求，激光惯性里程计（LIO）通常是主干系统，并与VIO和全球导航卫星系统（GNSS）分层叠加（参见传感器融合入门）。

• 几何退化空间——例如隧道、长直走廊：单独使用激光雷达（LiDAR）或激光成像轨道（LIO）在配准方面往往不稳定，因此需要添加一个独立的信息源——例如车距信息轨道（VIO）或轮式里程计——来弥补几何退化。

基本原则是：如果预算和传感器重量允许，且几何精度是首要考虑因素，则选择激光成像轨道（LIO）；如果成本和轻量化是首要考虑因素，并且您还希望利用基于颜色/图案的环境感知，则选择车距信息轨道（VIO）。在实际产品开发中，很少会单独选择这两种方法——相反，它们往往会演变为多传感器融合（参见传感器融合入门），根据具体情况对车距信息轨道（VIO）、激光成像轨道（LIO）、全球导航卫星系统（GNSS）和轮式里程计进行加权。

13. 总结

惯性测量单元 (IMU) 提供高频、完全独立的运动信息，但通过积分恢复的位置会迅速漂移，漂移量与经过时间的平方到立方成正比。相机 (VIO) 或激光雷达 (LIO) 会周期性地校正这种累积误差，但它们各自存在不同的弱点——相机在黑暗和低纹理环境下表现不佳，而激光雷达则在几何退化环境中表现不佳。VIO 和 LIO 通过预测（高频 IMU 预测）- 更新（相机/激光雷达校正）循环来结合这些互补的弱点，该循环在基于滤波器（EKF/MSCKF）或基于优化（IMU 预积分 + 因子图）的框架下实现，从而达到任何单个传感器都无法单独达到的鲁棒性和精度。