紧耦合LIO笔记

一、前言

近些年，紧耦合 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 的两种主流方法。

NOTE:事实上，滑窗优化和迭代卡尔曼滤波有着相同的数学本质，可以从基于概率图模型（Probabilistic Graphical Model）的概率推断视角下做出统一的解释，两者的主要差别在于问题的规模不同。

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

二、LO到松耦合LIO再到紧耦合LIO

1、LO

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

2、松耦合LIO

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

3、紧耦合LIO

紧耦合 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项工作中多数都兼容固态激光雷达。
紧耦合 LIO 的核心在于状态估计器的设计，无非是优化（滑窗优化）和滤波（迭代卡尔曼滤波）两条路线；滑窗优化的优点在于可以联合估计多个时刻的状态量，精度更高，但付出的代价是运算效率低，早期基于滑窗优化的 LIO 甚至不能保证实时性；迭代卡尔曼滤波的优点在于计算效率很高，有着良好的实时性，但受限于观测的数量和过早地边缘化掉了早期状态量，精度不如滑窗优化高；两种不同路线体现的是精度和效率的权衡。

三、经典LIO工作

1、基于滑窗优化的LIO

（1）LIOM（LIO-Mapping）

论文链接：Tightly Coupled 3D Lidar Inertial Odometry and Mapping (arxiv)

开源代码地址：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 预积分的约束（红色块）。

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

（2）LiLi-OM

论文链接：Towards High-Performance Solid-State-LiDAR-Inertial Odometry and Mapping (arxiv)

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

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

 这是一篇兼容旋转式机械雷达和 Livox Horizon 固态激光雷达的工作。LiLi-OM 的总体 pipeline 如下图所示，由 3 部分构成：一个轻量化的激光里程计（绿色区域），一个基于滑窗优化的紧耦合 LIO（红色区域），一个负责回环检测和全局优化的真后端（紫色区域）。我们只重点分析基于滑窗优化的紧耦合 LIO 部分

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

滑窗内的状态量为各个关键帧时刻的 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 等，实验部分与其它算法的对比也很充分，详见论文。

（3）LIO-SAM

论文链接：LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

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

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

 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 预积分因子，输入到后端的因子图优化问题中（如下图所示），参与整个轨迹的优化。

2、基于迭代卡尔曼滤波的LIO

（1）LINS

论文链接：LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation (arxiv)

开源代码地址：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 时刻的状态递推为例，我们的目标状态量表示为：

【1】Quaternion kinematics for the error-state Kalman filter https://arxiv.org/abs/1711.02508

 （2）Fast-Lio(1&2)

论文链接1：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

论文链接2：FAST-LIO2: Fast Direct LiDAR-inertial Odometry

发表日期&作者：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-Lio2 相比于 Fast-Lio 使用了增量式的 ikd-Tree 数据结构（中文解析）维护 local map，有效降低了对激光点观测查询近邻的耗时，进一步提升了效率；此外，不再提取特征点，所有的点都根据点到面的方式寻找关联；而在算法部分，两者差别不大，Fast-Lio2 的 pipeline 如下图所示。

 

 （3）Faster-Lio

论文链接：https://ieeexplore.ieee.org/sta

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

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

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