LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

文章：https://arxiv.org/pdf/2007.00258.pdf
代码：https://github.com/TixiaoShan/LIO-SAM

1. 主要工作

本文提出了一种紧耦合激光-惯性里程计方法，通过优化包含LiDAR里程计因子，IMU预积分因子，GPS因子和回环因子来得到机器人的全局一致的位姿。作者使用帧-局部地图匹配代替LOAM的帧-全局地图匹配，提高了帧图匹配的效率（这并不算一个新的创新点，因为作者在LeGO-LOAM中已经用了帧-局部地图匹配）。

2. 研究背景

LOAM是目前为止激光里程计（LO）领域最经典最广泛使用的方法。但是它存在一些问题：

1. 直接存储全局体素地图而不是局部地图
2. 很难执行回环检测以修正漂移
3. 没有组合GPS、指南针等绝对测量进行位姿修正
4. 当体素地图变得稠密时，体素地图的使用效率会随时间降低，从而使得在线优化过程的效率降低

为了克服该问题，作者只独立地存储每个关键帧的特征，而不是在位姿估计完成后就将特征加入到全局地图中。

另一方面，IMU和LiDAR的联合位姿估计已经被广泛研究，大致分为两类：
第一类是松耦合的方法，例如LOAM和LeGO-LOAM中使用IMU去除LiDAR点云的运动畸变，以及[1]-[5]使用EKF整合LiDAR和IMU的测量。

第二类是紧耦合的方法，例如R-LINS[6]，使用误差状态卡尔曼滤波器迭代地修正机器人的位姿估计，再比如LIOM [7]联合优化LiDAR和IMU测量。但是LIOM一次性处理所有测量，因此不能实时运行。而本文也属于紧耦合的激光-惯性里程计方法，只是采用了因子图优化而不是滤波的方法。

3. 方法

状态变量$\mathbf{x}$:
$$\mathbf{x}={\left[{\mathbf{R}}^{\mathbf{T}},{\mathbf{p}}^{\mathbf{T}},{\mathbf{v}}^{\mathbf{T}},{\mathbf{b}}^{\mathbf{T}}\right]}^{\mathbf{T}}$$

$\mathbf{R}\in SO(3)$表示旋转矩阵，$\mathbf{p}\in {\mathbb{R}}^3$表示位置向量，$\mathbf{v}$表示速度，$\mathbf{b}$表示IMU偏差，$\mathbf{T}\in SE(3)$表示从机体坐标系$\mathbf{B}$到世界坐标系$\mathbf{W}$的变换，表示为$\mathbf{T}=[\mathbf{R}\mid \mathbf{p}]$.

使用因子图的思想优化激光SLAM，引入四种因子。
1.IMU预积分因子
2.激光雷达里程计因子
3.GPS因子
4.闭环因子

图中主要包含四种因子。第一种是IMU预积分因子（橙色），由两个相邻关键帧之间的IMU测量积分得到。第二种是激光里程计因子（绿色），由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。第三种是GPS因子（黄色），由每个关键帧的GPS测量得到。第四种是回环因子（黑色），由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。

当机器人姿态变化超过用户定义的阈值时，将在图中添加一个新的机器人状态节点x。因子图在插入新节点时使用贝叶斯树(iSAM2)的增量平滑和映射进行优化.

3.1 IMU预积分因子

对于预积分因子，计算采用常见的角速度，加速度，线速度.
角速度、加速度的测量值：
$${\hat{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\mathbf{b}}_t^{\mathbf{\omega }}+{\mathbf{n}}_t^{\mathbf{\omega }}$$ $${\hat{\mathbf{a}}}_t={\mathbf{R}}_t^{\mathbf{B}\mathbf{W}}\left({\mathbf{a}}_t-\mathbf{g}\right)+{\mathbf{b}}_t^{\mathbf{a}}+{\mathbf{n}}_t^{\mathbf{a}}$$
速度、位置、旋转的预测值：
$${\mathbf{v}}_{t+\Delta t}={\mathbf{v}}_t+\mathbf{g}\Delta t+{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathrm{a}}-{\mathbf{n}}_t^{\mathrm{a}}\right)\Delta t$$$${\mathbf{p}}_{t+\Delta t}={\mathbf{p}}_t+{\mathbf{v}}_t\Delta t+\frac{1}{2}\mathbf{g}\Delta t^2$$$$+\frac{1}{2}{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathrm{a}}-{\mathbf{n}}_t^{\mathrm{a}}\right)\Delta t^2$$$${\mathbf{R}}_{t+\Delta t}={\mathbf{R}}_t\exp \left(\left({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_t^{\omega }-{\mathbf{n}}_t^{\omega }\right)\Delta t\right)$$

时刻$i$ 和 $j$之间的预积分：

$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij}\right)\\ \Delta {\mathbf{p}}_{ij}& ={\mathbf{R}}_i^{\top }\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2\right)\\ \Delta {\mathbf{R}}_{ij}& ={\mathbf{R}}_i^{\top }{\mathbf{R}}_j\end{array}$$

3.2 激光雷达里程计因子

在本文中，只使用关键帧的特征，非关键帧的特征全部被抛弃。采用LOAM或LeGO-LOAM的方法提取第$i$个关键帧的特征： F i = { F i e , F i p } \mathbb{F}_{i}=\left\{\mathrm{F}_{i}^{e}, \mathrm{F}_{i}^{p}\right\} Fi​={Fie​,Fip​}，前者是边缘特征，后者是平面特征。当新的关键帧${\mathbb{F}}_{i+1}$到来时，利用之前$n$+1个关键帧的特征集合 { F i − n , … , F i } \left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\} {Fi−n​,…,Fi​}和位姿估计 { T i − n , … , T i } \left\{\mathbf{T}_{i-n}, \ldots, \mathbf{T}_{i}\right\} {Ti−n​,…,Ti​}构建局部地图${\mathbf{M}}_i$，其中
M i = { M i e , M i p } \mathbf{M}_{i}=\left\{\mathbf{M}_{i}^{e}, \mathbf{M}_{i}^{p}\right\} Mi​={Mie​,Mip​}where$${\mathbf{M}}_i^e={}^{\prime }{\mathrm{F}}_i^e{\cup }^{\prime }{\mathrm{F}}_{i-1}^e\cup \dots {\cup }^{\prime }{\mathrm{F}}_{i-n}^e$$$${\mathbf{M}}_i^p={}^{\prime }{\mathrm{F}}_i^p{\cup }^{\prime }{\mathrm{F}}_{i-1}^p\cup \dots {\cup }^{\prime }{\mathrm{F}}_{i-n}^p$$

${}^{\prime }{\mathrm{F}}_i^e$ 和 ${}^{\prime }{\mathrm{F}}_i^p$ 是变换到世界坐标系中的第 $i$ 个关键帧的特征。

${\mathbf{M}}_i^e$ 和 ${\mathbf{M}}_i^p$ 是世界坐标系中的局部边缘特征地图和局部平面特征地图。

之后，利用LOAM中的方法匹配 ${\mathrm{F}}_{i+1}^e$ 和 ${\mathbf{M}}_i^e,$ 以及 ${\mathrm{F}}_{i+1}^p$ 和 ${\mathbf{M}}_i^p,$ 建立点线和点面距离约束。通过最小化所有约束，优化 T ${}_{i+1}$ 。

点云匹配和位姿优化过程与LOAM/LeGO-LOAM 相同 。不同的是, 这里使用过去n+1坝关键帧的组合点云而不是全局点云和最新帧进行匹配。

3.3 GPS因子

当接收到GPS测量后，首先使用[8]的方法将它们变换到笛卡尔坐标系中。当一个新的位姿节点被插入到因子图后，关联GPS因子到该位姿节点中去。

对于GPS因子，作者提出：

1.GPS测量值的时间戳根据里程计时间戳进行线性插值。
2.无需不断添加GPS因子。
3.当估计的位置协方差大于接收的GPS位置协方差时，添加GPS因子。
4.GPS在z方向可信度较低。

3.4 闭环因子

对于闭环因子，作者提出：

1.使用的是一种简单但有效的基于欧氏距离的闭环检测方法。
2.闭环系数对于校正机器人高度的漂移特别有用，因为GPS的海拔高度测量非常不准确。

4. 实验结果

4.1 精度对比

4.2 运行时间对比

特征多的区域即使scan少也更耗时。

5. 总结

5.1 核心思想

基于激光雷达，IMU和GPS多种传感器的因子图优化方案，以及在帧图匹配中使用帧-局部地图取代帧-全局地图。

5.2 优缺点

优点：

1. 紧耦合了多种不同传感器，运动估计结果更鲁棒。
2. 使用增量平滑和建图方法iSAM2执行因子图优化

缺点：

关键帧之间的特征被完全丢弃。能否设计一种简单高效的方法同时利用关键帧之间的激光帧的（部分）特征？这样的好处是信息损失更少，有利于提高精度。

5.3 展望

1. 为了提速，仅使用关键帧，剔除中间帧的想法借鉴自视觉SLAM，以后在激光SLAM中也会逐渐流行起来。
2. LIDAR点云的特征点提取这种基础工作有待进一步改进，无论是LOAM还是LeGO-LOAM，特征点提取方法都非常简单。能够提取出更加通用，稳定，高效的特征点的方法是非常关键的。因为后续的匹配算法对特征点的质量非常敏感，虽然短期可能看不出来，但是长期来看，好的特征非常重要。
3. 如何处理高动态环境也是一个开放问题。包括高动态环境中准确，鲁棒的定位方法，以及高动态环境的建图方法，是否对动态物体建图？如何去除动态物体的残影？如何建立纯静态场景地图？以及基于语义，物体，拓扑的建图方法等。

参考：

https://zhuanlan.zhihu.com/p/153394930?from_voters_page=true

https://blog.csdn.net/unlimitedai/article/details/107378759

参考文献：

[1] T. Shan, J. Wang, K. Doherty, and B. Englot, “Bayesian Generalized Kernel Inference for Terrain Traversability Mapping,” In Conference on Robot Learning, pp. 829-838, 2018.
[2] S. Lynen, M.W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, “A
Robust and Modular Multi-sensor Fusion Approach Applied to MA V
Navigation,” IEEE/RSJ International Conference on Intelligent Robots
and Systems, pp. 3923-3929, 2013.
[3] S. Yang, X. Zhu, X. Nian, L. Feng, X. Qu, and T. Mal, “A Robust
Pose Graph Approach for City Scale LiDAR Mapping,” IEEE/RSJ
International Conference on Intelligent Robots and Systems, pp. 1175-
1182, 2018.
[4] M. Demir and K. Fujimura, “Robust Localization with Low-Mounted
Multiple LiDARs in Urban Environments,” IEEE Intelligent Trans-
portation Systems Conference, pp. 3288-3293, 2019.
[5] Y . Gao, S. Liu, M. Atia, and A. Noureldin, “INS/GPS/LiDAR Inte-
grated Navigation System for Urban and Indoor Environments using
Hybrid Scan Matching Algorithm,” Sensors, vol. 15(9): 23286-23302,2015.
[6] C. Qin, H. Ye, C.E. Pranata, J. Han, S. Zhang, and Ming Liu, “R-
LINS: A Robocentric Lidar-Inertial State Estimator for Robust and
Efficient Navigation,” arXiv:1907.02233, 2019.
[7] H. Ye, Y . Chen, and M. Liu, “Tightly Coupled 3D Lidar Inertial
Odometry and Mapping,” IEEE International Conference on Robotics
and Automation, pp. 3144-3150, 2019.
[8] T. Moore and D. Stouch, “A Generalized Extended Kalman Filter
Implementation for The Robot Operating System,” Intelligent Au-
tonomous Systems, vol. 13: 335-348, 2016.