【论文阅读：激光SLAM】 LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

摘要

论文提出了一种通过平滑和映射（SAM：smoothing and mapping）紧耦合的激光雷达惯性里程计（LIO-SAM），可实现高精度、实时的移动机器人轨迹估计和地图构建。

LIO-SAM 将激光雷达-惯性里程测量置于因子图之上，允许将来自不同来源的大量相对和绝对测量（包括环路闭合）作为因子纳入系统。

来自惯性测量单元（IMU）预积分的估计运动会对点云进行纠偏，并生成用于雷达里程计优化的初始值。获得的雷达里程计又用于估算惯性单元的偏置（bias）。

为了确保高性能的实时性，我们将当前帧-局部地图匹配用于姿态优化，而不是将当前帧-全局地图匹配。在局部尺度而非全局尺度上进行扫描匹配可显著提高系统的实时性能，选择性地引入关键帧和高效的滑动窗口方法也是如此，这种方法将新的关键帧注册到一组固定大小的先前 "子关键帧组 "中。

简介

• 状态估计、定位和绘图是智能移动机器人取得成功的基本前提，是反馈控制、避障和规划等许多功能所必需的。利用基于视觉和激光雷达的传感技术，人们致力于实现高性能的实时同步定位和绘图（SLAM），以支持移动机器人的六自由度状态估计。
• 基于视觉的方法通常使用单目或立体摄像机，通过对连续图像进行三角测量来确定摄像机的运动特征。虽然基于视觉的方法特别适用于地点识别，但它们对初始化、光照和范围的敏感性使其在单独用于支持自主导航系统时并不可靠。
• 另一方面，基于激光雷达的方法在很大程度上不受光照变化的影响。尤其是最近出现了长距离、高分辨率的三维激光雷达，如 Velodyne VLS-128 和 Ouster OS1-128，激光雷达更适合直接捕捉三维空间环境的细节。因此，本文重点讨论基于激光雷达的状态估计和绘图方法。

在过去二十年中，提出了许多基于激光雷达的状态估计和测绘方法。其中，用于低漂移和实时状态估计与测绘的激光雷达里程测量与测绘（LOAM）方法是应用最广泛的方法之一。

• LOAM 系列算法使用激光雷达和惯性测量单元 (IMU)，达到了最先进的性能，自其在 KITTI 里程测量基准网站上发布以来，一直被评为基于激光雷达的顶级方法。
• 尽管取得了成功，LOAM 仍然存在一些局限性–由于其数据保存在全局体素图中，因此通常难以执行闭环检测，也难以结合其他绝对测量数据（如 GPS）进行姿态校正。
• 当这种体素图在特征丰富的环境中变得密集时，其在线优化过程的效率就会降低。由于 LOAM 的核心是基于扫描匹配的方法，因此在大规模测试中也会出现漂移。

在本文中，提出了一个通过平滑和映射实现紧耦合激光雷达惯性里程测量的框架（LIO-SAM），以解决上述问题。

• 我们为点云去畸变假设了一个非线性运动模型，在激光雷达扫描过程中使用原始的 IMU 测量来估计传感器的运动。除了对点云进行去畸变外，运动估计还可作为雷达里程计优化的初始值。
• 获得的雷达里程计可用于估计因子图中 IMU 的偏差。通过为机器人轨迹估算引入全局因子图，我们可以利用激光雷达和 IMU 测量有效地进行传感器融合，在机器人姿势中加入位置识别，并在可用时引入 GPS 定位和罗盘航向等绝对测量。这些来自不同来源的因子集合将用于联合图优化。
• 此外，我们将当前帧-局部地图匹配，用于姿势优化，而不是像 LOAM 那样将当前帧-全局地图匹配。在局部尺度而非全局尺度上进行扫描匹配大大提高了系统的实时性能，选择性地引入关键帧和高效的滑动窗口方法也是如此，这种方法将新的关键帧注册到一组固定大小的先前 "子关键帧组 "中。

相关工作

激光雷达测距通常是通过使用 ICP 和 GICP 等扫描点云匹配方法找到两个连续帧之间的相对变换。

由于计算效率高，基于特征的匹配方法已成为替代全点云匹配的流行方法。例如，基于平面的配准方法，假定在结构化环境中进行操作，从点云中提取平面，并通过求解最小二乘问题对其进行匹配。基于线的测距方法，线段从原始点云中生成，随后用于配准。

然而，由于目前三维激光雷达的旋转机制和传感器的运动，扫描点云往往是畸变的。仅使用激光雷达进行姿态估计并不理想，因为使用畸变的点云或特征进行注册最终会导致较大的漂移。

因此，激光雷达通常与 IMU 和 GPS 等其他传感器结合使用，用于状态估计和绘图。这种利用传感器融合的设计方案通常可分为两类：松耦合融合和紧耦合融合。

• 在 LOAM 中，IMU 被引入对激光雷达扫描进行去畸变，并为扫描匹配提供运动先验。但是，IMU 并不参与算法的优化过程。因此，LOAM 可以归类为一种松耦合方法。LeGO-LOAM 对 IMU 测量的融合与 LOAM 相同。
• 使用扩展卡尔曼滤波器（EKF）进行松耦合融合是一种更为流行的方法。例如，在机器人状态估计的优化阶段使用 EKF 对激光雷达、IMU 和 GPS 的测量输出（即分开计算，仅对结果进行 EKF）进行融合。

紧耦合系统通常能提高精度，是目前研究的重点方向。

• R-LINS 使用误差状态卡尔曼滤波器，以紧耦合的方式递归修正机器人的状态估计。但由于缺乏用于状态估计的其他可用传感器，它在长时间导航过程中会出现漂移。
• LIOM 紧耦合联合优化了激光雷达和IMU的测量结果，与 LOAM 相比，LIOM 能达到相似甚至更高的精度。由于 LIOM 是为处理所有传感器数据而设计的，因此无法实现实时性能。

方法论

系统概述

首先定义在整个论文中使用的框架和符号。我们将世界坐标系表示为 $\mathbf{W}$，机器人本体坐标系表示为 $\mathbf{B}$，为方便起见，我们还假设 IMU 坐标系与机器人本体坐标系重合。机器人状态 $\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 S O(3)$ 是旋转矩阵， $\mathbf{p} \in \mathbb{R}^3$ 是位移向量，$\mathbf{v}$ 为速度，$\mathbf{b}$ 为 IMU 的偏置。从 $\mathbf{B}$ 到 $\mathbf{W}$ 的坐标变换表示为 $\mathbf{T}=[\mathbf{R} \mid \mathbf{p}]$，$\mathbf{T} \in S E(3)$。

如上图所示，系统接收来自 3D 激光雷达、IMU 和 GPS 的传感器数据。我们试图利用这些传感器的观测数据来估计机器人状态及其轨迹。

这个状态估计问题可以表述为最大后验（MAP）问题。与贝叶斯网相比，因子图更适合进行推理，因此我们使用因子图来模拟这一问题。

在高斯噪声模型的假设下，我们问题的 MAP 推理等同于求解非线性最小二乘问题。需要注意的是，在不失一般性的前提下，所提出的系统还可以结合其他传感器的测量结果，如高度计的海拔高度或罗盘的航向。

我们引入了四种类型的因子，以及一种用于因子图构建的变量类型。该变量代表机器人在特定时间的状态，被归属于图节点。分别是：(a) IMU 预积分因子，(b) 激光雷达里程因子，© GPS 因子，以及 (d) 闭环因子。

当姿态变化超过定义的阈值时，就会在图中添加一个新的机器人状态节点 $\mathbf{x}$。在插入新节点时，会使用贝叶斯树（iSAM2）进行增量平滑和映射，对因子图进行优化。这些因子的生成过程将在以下章节中介绍。

IMU 预积分因子

IMU 的角速度和加速度测量值由公式 2 和 3 确定：

$$\begin{aligned} & \hat{\omega}_t=\omega_t+\mathbf{b}_t^\omega+\mathbf{n}_t^\omega \\ & \hat{\mathbf{a}}_{\ell}=\mathbf{R}_t^{\mathrm{BW}}\left(\mathbf{a}_t-\mathbf{g}\right)+\mathbf{b}_t^{\mathbf{a}}+\mathbf{n}_t^{\mathbf{a}}, \end{aligned}$$

其中 $\hat{\boldsymbol{\omega}}_t$ 和 $\hat{\mathbf{a}}_t$ 是 IMU 在坐标系 $\mathbf{B}$ 下 $t$ 时刻的原始测量数据，而 $\hat{\boldsymbol{\omega}}_t$ 和 $\hat{\mathbf{a}}_t$ 同样都受到缓慢变化的偏置 $\mathbf{b}_t$ 和白噪声
$\mathbf{n}_t$ 的影响。$\mathbf{R}_t^{\mathbf{B W}}$ 是从坐标系 $\mathbf{W}$ 到 $\mathbf{B}$ 的转换变换矩阵。$\mathrm{g}$ 是坐标系 $\mathbf{W}$ 下的常数重力值。

现在可以使用 IMU 的测量值来推断机器人的运动。机器人在 $t+\Delta t$ 时刻的速度、位置和旋转可计算为:

$$\begin{aligned} \mathbf{v}_{t+\Delta t}=\mathbf{v}_t & +\mathbf{g} \Delta t+\mathbf{R}_t\left(\hat{\mathbf{a}}_t-\mathbf{b}_t^{\mathbf{a}}-\mathbf{n}_t^{\mathbf{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^{\mathbf{a}}-\mathbf{n}_t^{\mathbf{a}}\right) \Delta t^2 \\\mathbf{R}_{t+\Delta t}= & \mathbf{R}_t \exp \left(\left(\hat{\boldsymbol{\omega}}_t-\mathbf{b}_t^{\boldsymbol{\omega}}-\mathbf{n}_t^{\boldsymbol{\omega}}\right) \Delta t\right), \end{aligned}$$

其中 $\mathbf{R}_t=\mathbf{R}_t^{\mathbf{W B}}=\mathbf{R}_t^{\mathbf{B W} \mathbf{W}^{\top}}$。 这里我们假设在上述 $\Delta t$ 积分过程中，$\mathbf{B}$ 的角速度和加速度保持不变。

然后，我们使用 IMU 预积分方法来获得两个时间帧之间的物体相对运动。时间 $i$ 和 $j$ 之间的预积分测量 $\Delta \mathbf{v}_{i j}, \Delta \mathbf{p}_{i j}$ 和 $\Delta \mathbf{R}_{i j}$ 可以使用以下方法计算:

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

除了效率之外，应用 IMU 预积分还自然而然地为我们提供了一种因子图约束–IMU 预积分因子。在因子图中，IMU 偏置与激光雷达里程因子共同优化。

激光雷达里程因子

当新的激光雷达扫描帧到达时，首先进行特征提取。通过评估局部区域内各点的粗糙度来提取边缘和平面特征。粗糙度值较大的点被归类为边缘特征。同样，粗糙度值较小的点被归类为平面特征。

我们将第 $i$ 时间激光雷达扫描提取的边缘和平面特征分别称为 $\mathrm{F} _{i}^{e}$ 和 $\mathrm{F} _{i}^{p}$。第 $i$ 个时间点提取的所有特征组成一个激光雷达帧 $\mathbb{F} _{i}$ ，其中 $\mathbb{F} _{i} = \{\mathrm{F} _{i}^{e} , \mathrm{F} _{i}^{p}\}$。请注意，激光雷达帧 $\mathbb{F}$ 是用 $\mathbf{B}$ 表示的。关于特征提取过程的更详细说明，请参阅 LOAM 文献，如果使用的是测距图像，则请参阅 LeGO-LOAM 文献。

使用所有激光雷达帧来计算并向图中添加因子在计算上是难以实现的，因此我们采用了关键帧选择的概念，这一概念在视觉 SLAM 领域得到了广泛应用。我们采用一种简单而有效的启发式方法，当机器人姿势变化与之前的状态 $\mathbf{x}_i$ 相比超过用户定义的阈值时，就会选择一个激光雷达帧 $\mathbb{F} _{i+1}$ 作为关键帧。新保存的关键帧 $\mathbb{F} _{i+1}$ 与因子图中新的机器人状态节点 $\mathbf{x}_{i+1}$ 相关联。两个关键帧之间的激光雷达帧将被丢弃。以这种方式添加关键帧不仅能在地图密度和内存消耗之间取得平衡，还有助于保持相对稀疏的因子图，适合实时非线性优化。在我们的工作中，添加新关键帧的位置和旋转变化阈值分别选为 1m 和 10°。

假设希望在因子图中添加一个新的状态节点 $\mathbf{x}_{i+1}$。与该状态相关的激光雷达关键帧为 $\mathbb{F} _{i+1}$。激光雷达测距因子的生成步骤如下：

1、子关键帧组合成体素图：采用滑动窗口法创建点云图，其中包含固定数量的最近激光雷达扫描帧。我们不对两个连续激光雷达扫描之间的变换进行优化，而是提取最近的 $n$ 个关键帧（称之为子关键帧）进行估计。然后使用与之相关的变换 $\{ {\mathbf{T}}_{i-n},\dots , {\mathbf{T}}_{i}\}$ 将子关键帧集合 $\{ {\mathbb{F}}_{i-n},\dots , {\mathbb{F}}_{i}\}$ 变换为帧 $\mathbf{W}$。转换后的子关键帧合并成一个体素图 $\mathbf{M}_i$。由于我们在前面的特征提取步骤中提取了两种类型的特征，因此 $\mathbf{M}_i$ 由两个子体素图组成，分别表示为边缘特征体素图 $\mathbf{M}^e_i$ 和平面特征体素图 $\mathbf{M}^p_i$。激光雷达帧和体素图之间的关系如下：

$$\mathbf{M}_i = \{\mathbf{M}^e_i, \mathbf{M}^p_i\} \\ where : \begin{aligned}\mathbf{M}^e_i= '\mathrm{F}^e_i\cup'\mathrm{F}^e_{i-1}\cup\dots\cup'\mathrm{F}^e_{i-1} \\ \mathbf{M}^p_i= '\mathrm{F}^p_i\cup'\mathrm{F}^p_{i-1}\cup\dots\cup'\mathrm{F}^p_{i-1}\end{aligned}$$

$'\mathrm{F}^e_i$ 和 $'\mathrm{F}^p_i$ 是 $\mathbf{W}$ 中经过转换的边缘特征和平面特征。然后对 $\mathbf{M}_i$ 和 $\mathbf{M}^e_i$ 进行降采样，以消除位于同一体素单元中的重复特征。本文选择 $n$ 为 25。$\mathbf{M}_i$ 和 $\mathbf{M}^e_i$ 的下采样分辨率分别为 0.2m 和 0.4m。

2、扫描匹配：我们通过扫描匹配将新获得的激光雷达帧 $\mathbb{F}_{i+1}$（$\{\mathrm{F} _{i+1}^{e} , \mathrm{F} _{i+1}^{p}\}$与 $\mathbf{M}_i$ 匹配。有多种扫描匹配方法可用于此目的，如 ICP 和 GICP。在这里，我们选择使用 LOAM 中提出的方法，因为该方法计算效率高，而且在各种具有挑战性的环境中都很稳健。

我们首先将 $\{\mathrm{F} _{i+1}^{e} , \mathrm{F} _{i+1}^{p}\}$ 从 $\mathbf{B}$ 变换到 $\mathbf{W}$，得到 $\{'\mathrm{F} _{i+1}^{e} , '\mathrm{F} _{i+1}^{p}\}$。这个初始变换是利用 IMU 预测的机器人 $\tilde{\mathbf{T}}_{i+1}$ 的运动得出的。对于 $'\mathrm{F} _{i+1}^{e}$ 或 $'\mathrm{F} _{i+1}^{p}$ 中的每个特征，我们都要在 $\mathbf{M}^p_i$ 或 $\mathbf{M}^e_i$ 中找到其边缘或平面对应关系。

3、相对变换： 特征与其边缘或平面块对应关系之间的距离可以通过以下公式计算出来：

$$\begin{gathered}\mathbf{d}_{e_k}=\frac{\left|\left(\mathbf{p}_{i+1, k}^e-\mathbf{p}_{i, u}^e\right) \times\left(\mathbf{p}_{i+1, k}^e-\mathbf{p}_{i, v}^e\right)\right|}{\left|\mathbf{p}_{i, u}^e-\mathbf{p}_{i, v}^e\right|} \\ \mathbf{d}_{p_k}=\frac{\left|\begin{array}{c}\left(\mathbf{p}_{i+1, k}^p-\mathbf{p}_{i, u}^p\right) \\ \left(\mathbf{p}_{i, u}^p-\mathbf{p}_{i, v}^p\right) \times\left(\mathbf{p}_{i, u}^p-\mathbf{p}_{i, w}^p\right) \end{array}\right|}{\left|\left(\mathbf{p}_{i, u}^p-\mathbf{p}_{i, v}^p\right) \times\left(\mathbf{p}_{i, u}^p-\mathbf{p}_{i, w}^p\right)\right|},\end{gathered}$$

其中 $k, u, v$, 和 $w$ 是相应集合中的特征索引。 对于${ }^{\prime} \mathrm{F}_{i+1}^e$ 中的边缘特征 $\mathbf{p}_{i+1, k}^e$ , $\mathbf{p}_{i, u}^e$ 和 $\mathbf{p}_{i, v}^e$是 $\mathbf{M}^e_i$ 中构成相应边缘线的点。 对于${ }^{\prime} \mathrm{F}_{i+1}^p$ 中的平面特征 $\mathbf{p}_{i+1, k}^p$ , $\mathbf{p}_{i, u}^p, \mathbf{p}_{i, v}^p$, 和 $\mathbf{p}_{i, w}^p$ 在 $\mathbf{M}^p_i$ 中构成相应的平面块。然后使用高斯牛顿法求解最优变换，即最小化：

$$\min _{\mathbf{T}_{i+1}}\left\{\sum_{\mathbf{p}_{i+1, k}^e \in \in^{\prime} \mathbf{F}_{i+1}^e} \mathbf{d}_{e_k}+\sum_{\mathbf{p}_{i+1, k}^p \in \mathrm{F}_{i+1}^p} \mathbf{d}_{p_k}\right\} .$$

最后，我们可以得到 $\mathrm{x}_i$ 和 $\mathrm{x}_{i+1}$ 之间的相对变换 $\Delta \mathbf{T}_{i, i+1}$，它是连接这两个位置的激光雷达因子：

$$\Delta \mathbf{T}_{i, i+1}=\mathbf{T}_i^{\top} \mathbf{T}_{i+1}$$

我们注意到，获取 $\Delta \mathbf{T}_{i, i+1}$ 的另一种方法是将子关键帧转换为 $\mathrm{x}_i$ 帧。换句话说，我们将 $\mathbb{F}_{i+1}$ 与 $\mathrm{x}_i$ 帧所代表的体素图相匹配。这样，就可以直接得到真正的相对变换 $\Delta \mathbf{T}_{i, i+1}$。由于变换后的特征 $\mathbf{M}^p_i$ 或 $\mathbf{M}^e_i$ 可以多次重复使用，因此我们选择使用上上节中描述的方法，以提高计算效率。

GPS因子

虽然我们可以仅利用 IMU 预积分和激光雷达测距因素来获得可靠的状态估计和绘图，但系统在长时间导航任务中仍会受到漂移的影响。为了解决这个问题，我们可以引入能够提供绝对测量值的传感器来消除漂移。这类传感器包括高度计、指南针和全球定位系统。为了便于说明，我们在此讨论 GPS，因为它在现实世界的导航系统中得到了广泛应用。

当我们接收到 GPS 测量值时，我们将其转换为本地直角坐标。在因子图中添加一个新节点后，我们就会将一个新的 GPS 因子与该节点关联起来。如果 GPS 信号与激光雷达帧没有硬件同步，我们将根据激光雷达帧的时间戳对 GPS 测量值进行线性插值。

我们注意到，由于激光雷达惯性里程测量的漂移增长非常缓慢，因此在有 GPS 接收信号的情况下无需不断添加 GPS 因子。实际上，我们只在估计的位置协方差大于接收到的 GPS 位置协方差时才添加 GPS 因子。

闭环因子

与 LOAM 和 LIOM 不同的是，由于使用了因子图，闭环也可以无缝地纳入到所提出的系统中。为了便于说明，我们描述并实现了一种基于欧氏距离的简单但有效的闭环检测方法。我们还注意到，我们提出的框架与其他环路闭合检测方法兼容，它们生成点云描述符并用于地点识别。

当一个新的运动状态 $\mathrm{x}_{i+1}$ 被添加到因子图中时，我们首先搜索因子图，找出在欧几里得空间中与 $\mathrm{x}_{i+1}$ 相近的先验状态。例如，如图所示，$x_3$ 就是返回的候选状态之一。然后，我们尝试使用扫描匹配法将 $\mathbb{F}_{i+1}$ 与子关键帧 $\{\mathbb{F}_{3-m},\cdots,\mathbb{F}_{3}\,\cdots,\mathbb{F}_{3+m}\}$ 匹配。需要注意的是，在扫描匹配之前，$\mathbb{F}_{i+1}$ 和过去的子关键帧首先要转换成 $\mathbf{W}$。我们得到相对变换 $\Delta \mathbf{T}_{3, i+1}$ 并将其作为循环闭合因子添加到图中。在本文中，我们选择指数 m 为 12，并将从新状态 $\mathrm{x}_{i+1}$ 开始的闭环搜索距离设定为 15m。

在实践中，我们发现当全球定位系统是唯一可用的绝对传感器时，添加闭环因子对修正机器人高度漂移特别有用。这是因为全球定位系统的海拔测量非常不准确–在我们的测试中，在没有闭环的情况下，海拔误差接近 100 米。