LIO论文阅读：Tightly Coupled 3D Lidar Inertial Odometry and Mapping

论文地址与代码地址

论文：arxiv
代码：GitHub

摘要

Ego-motion 在移动机器人应用中很重要。传感器融合可以补偿单传感器缺陷。这篇文章主要介绍了一种LiDAR-IMU 融合的方法，通过联合优化IMU和LiDAR的测量数据，可以做到在LiDAR退化的情况下也没有明显漂移。
另外，本文提出了一种rotation-constrained refinement 算法（LIO-mapping），用于配准LiDAR位姿和全局地图。
实验结果显示，所提出的方法可以做到以IMU的速率估计传感器的位置，即使在快速移动和缺少特征情况下也能工作。

I. INTRODUCTION

LiDAR优点：在机器人领域广泛应用，频率10Hz，水平的 field of view(FOV)是360 $°$。受光照影响小，高精度和高可靠性。

LiDAR的缺点：竖直方向上分辨率太小，太过于稀疏，因此能提供的特征也很少，让特征跟踪变得棘手。
安装在机器人上的LiDAR由于机器人的运动，会使测量的数据变差。
另外，在一个狭窄的走廊环境中，只能得到部分天花板，墙和底板的点云。特征过少导致跟踪的丢失。
还有一个问题：更新频率太低。

在本文中，提出IMU-LiDAR紧耦合的位姿估计算法来解决上述问题。
使用了 fix-lag smoothing 和 原有位姿边缘化的方法达到实时和一致性的估计，然后再进行 rotation-constrained refinement。

主要工作如下：

• LiDAR-IMU紧耦合算法提供高频的，实时的，准确的位姿估计。
• rotational constrained refinement 方法优化最终的位姿并且产生点云地图，提高一致性和鲁棒性的估计。
• 算法在室内外环境都进行了测试，效果比松耦合算法优异。
• 算法开源，业界第一个LiDAR-IMU紧耦合算法。

本文总体结构：
II 介绍相关工作；
III 介绍标记与注记；
IV 介绍里程计算法；
V 介绍 refinement （配准）算法；
VI 和 VII 介绍了实现与测试结果；
VIII 结论。

II. RELATED WORKS

松耦合算法：将LiDAR和IMU的状态估计分别单独考虑。
[1] 中的雷达里程计算法带有IMU的辅助信息。使用加速度信息时会假设它是 zero velocity 的。它将 IMU 和雷达的数据分离，并且将IMU信息作为整个系统的先验，因此在优化中，它没有最大化 IMU 信息的利用。
[2] 中使用EKF来融合2D 情况下的 LiDAR 和 IMU 的信息，但它不能处理3D或者更复杂的环境。
[3] 讲了一种模块化的IMU融合方式，通过 EKF 进行融合。这种松耦合的方式虽然计算效率很高，但是和紧耦合的相比准确度不够，因为它只把里程计作为一个黑盒，却没有通过IMU来更新它。
[4]采用了紧耦合的方式。
[5] 是 Soloviev 等人提出的。针对2D平面运动估计，他们提出了一种方法：抽取和匹配2D平面的线特征。通过 IMU 的预测方位补偿雷达信息。过程中应用了 卡尔曼滤波器。
[6] Hemann 等人提出了以误差状态的卡尔曼滤波器形式，紧耦合IMU 传播误差和 LiDAR heightmap。这种方式在环境地图已知时，没有GPS信息也能长时间工作，但没有地图无法工作。
[7] 和 [8] 未处理数据和 IMU 的预测数据直接用于计算残差和进行优化。这种方式没有牵涉转化和状态估计，在系统快速运动时，系统会不可用。

受到其他 VI 方式启发，如[10] ,[11] ，我们基于预积分和紧耦合的方式设计了整个系统。使用未处理的IMU数据和LiDAR数据进行状态优化。
整个系统可以在雷达退化或者运动十分快速时也能工作得很好。
据我们所知，我们的系统是少数几个适合复杂3D环境的。

III. NOTATIONS AND PRELIMINARIES

A. Notations

雷达每一线的测量记为 $scanC$。
包含所有 $scan$ 的一次测量记为 $sweepS$。16线雷达在一次sweep中包含16个scan。

变换矩阵${\mathbf{T}}_b^a\in SE(3)$,表示把点 ${\mathbf{x}}^{\mathbf{b}}\in {\mathbb{R}}^3$ 从坐标系 $F_b$ 变换到坐标系 $F_a$；
${\bar{\mathbf{T}}}_b^a$表示IMU预测的变换；
${\mathbf{R}}_b^a\in SO(3)$ 和 ${\mathbf{p}}_b^a$ 分别是${\mathbf{T}}_b^a$的旋转和平移部分；
四元素${\mathbf{q}}_b^a$对应于${\mathbf{R}}_b^a$；

${\hat{\mathbf{a}}}_k$和${\hat{\omega }}_k$表示在时刻 $k$ 时，IMU的加速度和角速度测量；

在坐标系 $F_a$下抽取的特征表示为 ${\mathbf{F}}_a$， ${\mathbf{F}}_a$可以通过 ${\mathbf{F}}_a^b$变换到坐标系 $F_b$。

A. IMU Dynamics

1）状态
$F_{B_i}$ 和 $F_{L_i}$ 分别是IMU和雷达的坐标系；
雷达在离散时刻 $i$ 时接收 $sweep{S}_i$；

我们要进行估计的是IMU在世界坐标系下 $F^W$ 的状态 ${\mathbf{X}}_{B_i}^W$以及雷达和IMU之间的外参 ${\mathbf{T}}_B^L$，具体如下：
$$\begin{gathered} {\mathbf{X}}_{B_i}^W=[{{\mathbf{p}}_{B_i}^W}^T{{\mathbf{v}}_{B_i}^W}^T{{\mathbf{q}}_{a_i}^W}^T{{\mathbf{b}}_{g_i}^W}^T{]}^T, \\ {\mathbf{T}}_B^L= \end{gathered}$$