紧耦合LIO综述 | 6篇顶会论文看透紧耦合LIO玩儿法（LIOM/LIO-SAM）

作者 | 无疆WGH  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/567254168

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！

前言

近些年，紧耦合 LIO 逐渐引起了一些高校实验室以及业界的关注，这主要是因为紧耦合方案能够解决一些纯 LO 和松耦合 LIO 所存在的问题，比如运动畸变问题和特定场景下的松耦合 LIO 失效问题。

紧耦合 LIO 之所以叫紧耦合，是因为它会把 LiDAR 的原始观测和 IMU 的原始观测放到一起联合处理、互相影响，通常的思路是 IMU 数据首先用于 LiDAR 观测的去畸变，然后 LiDAR 观测和 IMU 观测会被一起丢到某种形式的状态估计模型中，并以最小化 LiDAR 观测和 IMU 观测的总体误差为目的，估计出最终的 Position、Velocity、Orientation、Bias、Gravity 等状态量。这其中，IMU 数据通常以预积分的形式进行处理；对 Bias 的估计相当于用 LiDAR 观测反过来影响 IMU 的状态量；对 Velocity 和 Bias 这些状态量的估计体现了紧耦合的思想之一 —— 充分考虑观测之间的内在约束，而非仅仅对 Position，Rotation 这样的结果做融合；所谓“某种形式的状态估计模型”通常是滑窗优化（Sliding Window Optimization）或者迭代误差状态卡尔曼滤波（iESKF），前者能够批量估计近期多个时刻的状态量， 后者则以迭代卡尔曼滤波（iEKF）为基础框架递归式地估计当前时刻的状态量。后文我们会看到，滑窗优化和迭代误差状态卡尔曼滤波，正是实现紧耦合 LIO 的两种主流方法。

“

事实上，滑窗优化和迭代卡尔曼滤波有着相同的数学本质，可以从基于概率图模型（Probabilistic Graphical Model）的概率推断视角下做出统一的解释，两者的主要差别在于问题的规模不同。后期笔者可能会写一篇文章证明滤波/滑窗优化/全局优化的统一性。

”

在技术落地方面，紧耦合 LIO 在自动驾驶和机器人领域都有用武之地，比如有自动驾驶厂商以 LIO 作为局部跟踪定位手段，配合多传感器融合实现自动驾驶车辆的完全定位，而 LIO 实时生成的局部地图也有助于车辆对周围环境的精确感知。紧耦合 LIO 与时下自动驾驶领域逐渐出现的「重感知，轻地图」路线非常契合，随着未来固态激光雷达成本的进一步降低，雷达上车的规模进一步扩大，紧耦合 LIO 技术可能迎来更大的需求。

了解一下「纯 LO、松耦合 LIO、紧耦合 LIO」三者的渐进式发展历程是很有必要的。

纯 LO 可以从点云配准算法讲起，通常是某种形式的 scan to scan 或者 scan to local map 配准，早期主要是基于 ICP、NDT 等的一些改良型配准算法，2014 年发表的 LOAM 通过提取几何特征来做配准，提升算法效率的同时实现了更好的配准精度，启发了后续的一系列工作；但纯 LO 的问题在于不能很好地去除点云畸变，并且存在诸多退化场景导致无法工作。

松耦合 LIO 通过融合 IMU 的数据实现点云去畸变，IMU 观测也可以为点云配准提供初值，甚至提供重力方向观测，最终的结果可以是直接输出点云配准结果、也可以是用滤波对 IMU 积分结果（预测）和点云配准结果（观测）做融合得到，相关的研究成果有 LOAM (with IMU)，LeGO-LOAM，XXX 等；但松耦合 LIO 对 LiDAR 和 IMU 的融合仅体现在结果层面，没有考虑两种观测间的内在约束，且依然无法解决一些退化场景问题。

紧耦合 LIO，正如我们上文提到的，充分考虑了 LiDAR 观测和 IMU 观测的内在约束性，互相影响，共同决定最终的结果，理论上，能够应对前两者无法解决的退化场景问题，比如长隧道环境、剧烈运动环境等。当前固态激光雷达（solid-state LiDAR）成为了一种趋势，但固态激光雷达普遍视场角（FoV）有限，最大一般不超过横向120度，在这种情况下，更能体现紧耦合的必要性。自 2019 年以来，有影响力的紧耦合 LIO 方案主要有以下 6 篇：LIOM（LIO-Mapping）、LINS、LIO-SAM、LiLi-OM、Fast-Lio1&2、Faster-Lio。有意思的是，这6项工作中多数都兼容固态激光雷达。后文中，我们将逐篇介绍这6项工作，介绍它们的核心思想和框架设计，以飨读者。

我们还要再强调一下，紧耦合 LIO 的核心在于状态估计器的设计，无非是优化（滑窗优化）和滤波（迭代卡尔曼滤波）两条路线；滑窗优化的优点在于可以联合估计多个时刻的状态量，精度更高，但付出的代价是运算效率低，早期基于滑窗优化的 LIO 甚至不能保证实时性；迭代卡尔曼滤波的优点在于计算效率很高，有着良好的实时性，但受限于观测的数量和过早地边缘化掉了早期状态量，精度不如滑窗优化高；两种不同路线体现的是精度和效率的权衡。

下文中，我们将先介绍基于滑窗优化的两项工作（LIOM、LiLi-OM），然后介绍基于迭代卡尔曼滤波的工作（LINS、Fast-Lio1&2 和 Faster-Lio），最后还要补充介绍一下 LIO-SAM，LIO-SAM 相对比较特殊，虽然也是紧耦合方案，但它的具体做法是将滑窗仅用于优化 IMU 的 bias，然后有一个额外的后端，该后端将预积分因子、激光里程计因子、回环因子等放到一个因子图优化模型中联合优化。

LIOM (LIO-Mapping)

论文链接：https://arxiv.org/abs/1904.06993

开源代码地址：https://github.com/hyye/lio-mapping

发表日期&作者：2019年4月，港科大 RAM-LAB 实验室

LIOM 是将滑窗优化用于 LIO 的有影响力的工作之一，非常适合作为第一篇来介绍。众所周知，VINS-mono 是视觉 SLAM 领域中将滑窗用的非常好的工作之一，是诸多技巧的集大成者。LIOM 的思路在于，能否将 VINS-mono 中的滑窗优化思想移植到激光 SLAM 领域？答案是可以，但又有所不同。

在 VINS-mono 的滑窗中，相邻帧之间可以通过共视特征点建立帧间约束，但 LIO 中却不具备构建这种帧间约束的基础 —— 激光点云无法实现帧间特征点追踪，而 scan to scan 配准又太不靠谱，也不足以构建帧间约束。怎么办呢？scan to scan 配准不靠谱，但是 scan to local map 配准则是靠谱的！

于是 LIOM 将滑窗中的所有帧投影到最老帧的坐标系下，构建一个表达在最老帧坐标系下的 local map，剩下的每一帧都和 local map 做配准，从而每一帧都可以构建一个与最老帧的约束关系，而相邻帧之间本来就有来自 IMU 预积分的约束关系，从而形成了完整的滑窗优化问题。

思路讲清楚了，我们再看一下细节，下图是 LIOM 论文中的滑窗优化问题模型，从中可以清晰地看到：优化的状态量为各个关键帧时刻的 pose、speed 和 bias（橙色圆和蓝色五角星），在最新帧到来之时，用滑窗内的所有关键帧构建了表达在最老帧坐标系下的 local map（褐色块），然后每一帧都和 local map 建立了约束（浅蓝色连线），所有的相邻帧之间又有来自 IMU 预积分的约束（红色块）。

LIOM 中的滑窗优化问题

除了紧耦合 LIO 之外，LIOM 还在 mapping 部分通过 scan to global map 配准进一步提升位姿精度，且在 scan to global map 配准的优化问题求解中，仅优化 yaw，不优化 roll 和 pitch；这样做的出发点是更相信 LIO 部分给出的重力方向，后续的调整无需再改变 roll 和 pitch。

LiLi-OM

论文链接：https://arxiv.org/abs/2010.13150

开源代码地址：https://github.com/KIT-ISAS/lili-om

发表日期&作者：2022年10月，Kailai Li & Meng Li，卡尔斯鲁厄理工学院[德国]

这篇工作于 2022 年 10 月首先上传到 arxiv 预印本网站上，后续发表在了 RAL-2021。这是一篇兼容旋转式机械雷达和 Livox Horizon 固态激光雷达的工作，但本文的重点在于紧耦合方法的设计，与点云形态相关的部分不再多说。

LiLi-OM 的总体 pipeline 如下图所示，由 3 部分构成：一个轻量化的激光里程计（绿色区域），一个基于滑窗优化的紧耦合 LIO（红色区域），一个负责回环检测和全局优化的真后端（紫色区域）。我们只重点分析基于滑窗优化的紧耦合 LIO 部分。

LiLi-OM 的算法 pipeline

下图所示为滑窗优化问题的示意图，我们只需关注两个问题：滑窗内的状态量有哪些？滑窗内的约束有哪些？

LiLi-OM 的滑窗优化问题模型

滑窗内的状态量为各个关键帧时刻的 Position、Velocity、Orientation、Bias。

滑窗内的约束有3种，分别是边缘化约束（黄色因子），IMU 预积分约束（蓝色因子），点云配准约束（紫色因子）。边缘化约束和 IMU 预积分约束不再多说；正如在 LIOM 中提到的，LIO 中无法通过 scan to scan 配准提供帧间约束，LiLi-OM 将最新的 30 个关键帧拼接为 local map，用 scan to local map 的配准结果作为点云配准约束。在每轮滑窗优化过后，local map 都会根据新的位姿做更新。

LiLi-OM 的算法 pipeline 里还有很多细节可以讲，比如固态激光雷达点云的配准算法，自适应计算配准结果的权重，滑窗优化使用 ceres solver，而后端全局优化使用 gtsam 等，实验部分与其它算法的对比也很充分，详见论文。

LiLi-OM 建图效果

LINS

论文链接：https://arxiv.org/abs/1907.02233

开源代码地址：https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM

发表日期&作者：2019年7月，港科大 RAM-LAB 实验室

LINS 同样出自港科大 RAM-LAB 实验室，作为滤波系的工作，和 LIOM 互相呼应。LINS 的算法 pipeline 如下。

滤波形式的问题并没有什么框图可以细说，我们粗略地讲一下 LINS 中迭代误差状态卡尔曼滤波（iESKF）的范式。

首先，LINS 中 iESKF 估计的状态量为最新时刻的 Position、Velocity、Orientation、Bias 和 Gravity，并且，这些状态量都是表达在上一帧坐标系下的，也即，LINS 是一种 robocentric 的方法，以 k 时刻到 k+1 时刻的状态递推为例，我们的目标状态量表示为：

由于采用了误差状态，因此实际目标状态量为：

Fast-Lio(1&2)

论文链接1：https://arxiv.org/abs/2010.08196

论文链接2：https://arxiv.org/abs/2107.06829

开源代码地址：https://github.com/hku-mars/FAST_LIO

发表日期&作者：2020年10月/2021年7月，港大 MaRS-Lab 实验室

Fast-Lio 同样采用迭代误差状态卡尔曼滤波（iESKF）来实现紧耦合的 LIO。在滤波器的设计上，Fast-Lio 和 LINS 大体相同，但在卡尔曼增益的计算部分有所区别。众所周知，计算卡尔曼增益时需要对观测对误差状态的雅可比矩阵与自身转置的乘积求逆，当观测的维度较大时求逆将变得耗时，比如有 1000 个点时就需要对  大小的矩阵求逆。针对此问题，Fast-Lio 采用的新的卡尔曼增益计算方法，需要求逆的矩阵的规模仅与状态量的规模相关，而非观测的规模。

Fast-Lio 的 pipeline 如下所示，Fast-Lio 兼容旋转式机械激光雷达和 Livox 固态激光雷达，首先对点云做特征提取，并用 IMU 观测对特征点做去畸变，然后将激光观测和 IMU 观测放到 iESKF 中估计当前时刻的 Position、Velocity、Orientation、Bias 和 Gravity 状态量，所有这些状态量都是位于世界坐标系下的。

Fast-Lio pipeline

Fast-Lio2 相比于 Fast-Lio 使用了增量式的 ikd-Tree 数据结构（中文解析）维护 local map，有效降低了对激光点观测查询近邻的耗时，进一步提升了效率；除此之外，在算法部分差别不大，pipeline 如下图所示。

Fast-Lio2 pipeline

Faster-Lio

论文链接：https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=9718203

开源代码地址：https://github.com/gaoxiang12/faster-lio

发表日期&作者：2022年，智行者&清华

Faster-Lio 是智行者高博团队在 Fast-Lio2 的基础上发表的工作，和 Fast-Lio2 类似，也致力于通过使用增量式的数据结构（iVox）维护 local map，来提升算法的效率。由于 Faster-Lio 的主要工作在于 iVox，在 iESKF 部分完全同 Fast-Lio系列，因此本文不再赘述。

Faster-Lio 中的 iVox 数据结构

LIO-SAM

论文链接：https://arxiv.org/abs/2007.00258

开源代码地址：https://github.com/TixiaoShan/LIO-SAM

发表日期&作者：2020年7月，Tixiao Shan, etc

LIO-SAM 作为非常有影响力的工作，相信很多同学都已经很熟悉了。LIO-SAM 并非一个单纯的 LIO，其后端的因子图优化支持融合 GPS 等不同的传感器数据，框架如下图所示，网上也有很多解读。

这里主要提一下 LIO-SAM 中的紧耦合 LIO 部分：LIO-SAM 中的 imu_preintegration 节点专门对 IMU 做预积分，预积分结果给到后端的 scan to local map 部分做去畸变和配准初值，配准结果反过来还会返回至 imu_preintegration 节点，该节点内基于 gtsam 构建一个不超过N帧的滑窗优化问题，专门优化 IMU 当前的 bias，新的 bias 将用于后续时刻的 IMU 预积分。同时，IMU 预积分结果也作为 IMU 预积分因子，输入到后端的因子图优化问题中（如下图所示），参与整个轨迹的优化。

LIO-SAM 后端因子图优化 

往期回顾

港大重磅SLAM新作！R3LIVE++：一个实时鲁棒的紧耦合激光雷达-惯性-视觉融合框架

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

加入我们：自动驾驶之心技术交流群汇总！

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！