LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping论文解读

abstract

LIO-SAM将lidar-惯性里程计置于一个因子图之上，允许从不同来源将多种相对和绝对测量，包括环闭检测，作为因子合并到系统中。由惯性测量单元（IMU）预积分估计的运动消除点云畸变，并为激光雷达里程计优化提供初始猜测。所得到的激光里程计解被用来估计IMU的偏差。为了确保实时的高性能，我们将旧的激光雷达扫描边缘化以优化姿态，而不是将激光雷达扫描与全局地图匹配。局部尺度的扫描匹配代替了全局尺度的扫描匹配，大大提高了系统的实时性。还有选择性地引入关键帧，以及将新关键帧注册到固定大小的先前“子关键帧”集合的高效滑动窗口方法。

一、Introduction

1.1 LOAM的缺点：

LOAM将其数据保存在全局体素图中，通常很难执行环路闭合检测和合并其他绝对测量值（例如GPS）进行姿态校正。当这个体素图在一个特征丰富的环境中变得密集时，它的在线优化过程就变得不那么有效了。LOAM在大规模测试中也会出现漂移，因为它的核心是基于扫描匹配的方法。

1.2 LIO-SAM的改进：

我们假设一个非线性运动模型对点云去畸变，使用原始的IMU测量数据估计传感器在一个激光雷达扫描过程中的运动。除了点云去畸变，估计的运动还作为激光雷达里程计优化的初始猜测。得到的激光里程计解用于估计因子图中IMU的偏差。通过引入全局因子图进行机器人轨迹估计，可以有效地利用lidar和IMU测量进行传感器融合，在机器人姿态之间进行位置识别，并引入绝对测量，如GPS定位和罗盘航向。这些来自不同来源的因子集合用于图的联合优化。此外，我们将旧的激光雷达扫描边缘化以优化姿势，而不是将扫描与全局地图（如loam）匹配。局部尺度的扫描匹配而不是全局尺度的扫描匹配显著地提高了系统的实时性能，关键帧的选择性引入也是如此，以及一种有效的滑动窗口方法，它将一个新的关键帧注册到一组固定大小的先验“子关键帧”中。
我们的主要贡献可概括如下：

1. 一种建立在因子图之上的紧密耦合的激光雷达惯性里程计框架，适用于多传感器融合和全局优化。
2. 一种高效的、基于局部滑动窗口的扫描匹配方法，通过将有选择地选择的新关键帧注册到固定大小的先验子关键帧集来实现实时性能。
3. 所提出的框架通过各种规模、车辆和环境的测试得到了广泛的验证。

二、Related Work

这种利用传感器融合的设计方案通常可分为两类：

1. 松耦合：引入LOAM和IMU对激光雷达扫描进行去畸变，并给出扫描匹配的运动先验。然而，IMU不参与算法的优化过程。因此，LOAM可归类为松散耦合方法。lego-loam也一样。一种更流行的松耦合融合方法是使用扩展卡尔曼滤波器（EKF）。例如，在机器人状态估计的优化阶段，使用EKF整合来自lidar、IMU和GPS的测量值。
2. 紧耦合：紧耦合系统通常提供更高的精度，目前是正在进行的研究的主要焦点。在[15]中预积分的IMU测量被用于点云去畸变。==文献[16]提出了一种机器人中心激光雷达惯性状态估计器R-LINS。R-LINS使用错误状态卡尔曼滤波器以紧耦合的方式递归修正机器人的状态估计。==由于缺少其他可用的传感器来进行状态估计，它在长时间航行中会产生漂移。[17]中介绍了一种紧耦合激光雷达惯性测程与测图框架LIOM。LIOM是LIO-mapping的缩写，联合优化从激光雷达和IMU的测量，与LOAM相比获得类似或更好的准确性。

三、LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING（实际工作）

A. 符号约定和文章综述

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

B IMU预积分因子

、

C. 激光里程计因子

1. 特征提取：
   通过评估局部区域上的点的粗糙度来提取边缘和平面特征。粗糙度值较大的点被划分为边缘特征。同样，粗糙度小的被分类为平面特征。我们将第$i$时刻激光雷达扫描提取的边缘和平面特征分别表示为$F_i^e$和$F_i^p$。我将在同一时刻提取的所有特征组成一个激光雷达帧${\mathbb{F}}_i$，其中${\mathbb{F}}_i=\left\{{\mathrm{F}}_i^e,{\mathrm{F}}_i^p\right\}$
   对于特征提取过程的更详细的描述可以在1中找到，如果使用的是距离图像，则可以在7中找到。
2. 关键帧选择
   使用一种简单而有效的启发式方法：与之前的状态$x_i$相比，当机器人姿态变化超过用户定义的阈值时，我们选择激光雷达帧$F_{i+1}$作为关键帧。在因子图中，新保存的关键帧$F_{i+1}$与一个新的机器人状态节点$x_{i+1}$关联。两个关键帧之间的激光雷达帧被丢弃。通过这种方式添加关键帧，既能平衡映射密度和内存消耗，又能保持相对稀疏的因子图，适用于实时非线性优化。在我们的工作中，增加一个新的关键帧的位置和旋转变化阈值被选择为1米和10◦。
3. 激光雷达测程因子的生成步骤如下：
   1. 体素地图的子关键帧（Sub-keyframes）:我们实现了一个滑动窗口的方法来创建一个包含固定数量的最近激光雷达扫描的点云地图。我们没有优化两个连续的激光雷达扫描之间的转换，而是提取n个最近的关键帧，我们称之为子关键帧，用于估计。然后子关键帧集合$\left\{{\mathbb{F}}_{i-n},\dots ,{\mathbb{F}}_i\right\}$使用与之关联的变换矩阵$T_{i-n},...,T_i$转换到世界坐标系W。转换后的子关键帧被合并到一个体素地图$M_i$中。于我们在前面的特征提取步骤中提取了两种类型的特征，因此$M_i$由两个亚体素地图组成，分别是边缘特征体素地图$M_i^e$和平面特征体素地图$M_i^p$。激光雷达帧和体素地图之间的关系如下:
      
      ${}^{\prime }{\mathrm{F}}_i^e$ 和 ${}^{\prime }{\mathrm{F}}_i^p$ 是在世界坐标系 $\mathbf{W}$下的经过变换的边特征和面特征。然后对${\mathbf{M}}_i^e$ 和 ${\mathbf{M}}_i^p$ 进行下采样来去除同一体素中的重复特征。在这篇文章中, $n$ 为选为$25$。 ${\mathbf{M}}_i^e$ 和${\mathbf{M}}_i^p$ 的降采样率分别是 $0.2m$和 $0.4m$
   2. Scan-matching：（这里比较简单，就直接上原文了）
      
      

3. Relative transformation:和loam相同，不再赘述
   
   提出了另一种可选的方法，用scan-map的匹配，代替原来scan-scan的匹配

D. GPS因子

当我们接收到GPS测量值时，我们首先使用[21]中提出的方法将其转换到局部笛卡尔坐标系中。在因子图中添加一个新节点之后，我们将一个新的GPS因子与这个节点关联起来。如果GPS信号与激光雷达帧之间没有硬件同步，则根据激光雷达帧的时间戳对GPS测量值进行线性插值。

我们注意到，在GPS接收可用时不断增加GPS因子是不必要的，因为激光雷达惯性测程法的漂移增长非常缓慢。在实际应用中，我们只在估计的GPS位置协方差大于接收到的GPS位置协方差时添加一个GPS因子。

E. 闭环因子

和lego-loam一样使用的基于欧式距离的闭环检测，不再赘述