论文链接

LIO-SAM

---

0. Abstract

• 提出了一种通过平滑和映射进行紧耦合激光雷达惯性里程计的框架 LIO-SAM，它实现了高精度、实时的移动机器人轨迹估计和地图构建
  • LIO-SAM 在因子图上制定激光雷达惯性里程计，允许将多种相对和绝对测量（包括闭环）从不同来源作为因子合并到系统中
  • 惯性测量单元 (IMU) 预积分的估计运动可以消除点云的倾斜，并为激光雷达里程计优化生成初始猜测
  • 对姿势优化进行了边缘化处理，而不是将激光雷达扫描与全局地图匹配
  • 在局部尺度而不是全局尺度上进行扫描匹配显著提高了系统的实时性能，关键帧的选择性引入以及将新的关键帧注册到一组固定大小的先前“子关键帧”的高效滑动窗口方法也有助于提高系统性能

---

1. Intro

状态估计、定位和地图构建是实现智能移动机器人成功运行的基本先决条件，对于反馈控制、避障和规划等许多功能都是必需的

• 基于视觉的方法通常使用单目或双目摄像头。虽然基于视觉的方法特别适用于地点识别，但它们对初始化、光照和距离的敏感性使得当它们单独用于支持自主导航系统时不够可靠
• 基于激光雷达的方法在光照变化方面具有较大的不变性。特别是随着长距离、高分辨率3D激光雷达的最近可用，激光雷达更适用于直接捕捉三维空间环境的精细细节

主要贡献

• 建立一个紧密耦合的激光雷达惯性测距法框架，建立在因子图之上，适用于多传感器融合和全局优化
• 提出一种高效的、基于本地滑动窗口的扫描匹配方法，通过将选择性选择的新关键帧注册到一组固定大小的先前子关键帧，实现实时性能
• 所提出的框架经过广泛验证，通过在不同尺度、车辆和环境上进行测试

---

2. Related Work

激光雷达里程计通常通过使用 ICP 和 GICP 等扫描匹配方法查找两个连续帧之间的相对变换来执行

• 基于特征的匹配方法由于其计算效率而成为流行的替代方法，而不是匹配完整的点云
• 激光雷达通常与其他传感器（例如 IMU 和 GPS）结合使用，用于状态估计和绘图。这种利用传感器融合的设计方案通常可以分为两类：松耦合融合和紧耦合融合
  • 松耦合融合的一种更流行的方法是使用扩展卡尔曼滤波器（EKF）
  • 紧耦合系统通常可以提供更高的准确性，并且是目前正在进行的研究的主要焦点

---

3. Lidar Internal Odometry Via Smoothing and Mapping

A. 系统概述

将世界坐标系表示为 $\mathbf{W}$，将机器人本体坐标系表示为 $\mathbf{B}$。为了方便起见，z假设IMU坐标系与机器人本体坐标系重合。机器人状态 $\mathbf{x}$ 可以写为
$$\mathbf{x}=[{\mathbf{R}}^{\mathbf{T}},{\mathbf{p}}^{\mathbf{T}},{\mathbf{v}}^{\mathbf{T}},{\mathbf{b}}^{\mathbf{T}}{]}^{\mathbf{T}},\text{\hspace{1em}(1)}$$
其中 $\mathbf{R}\in SO(3)$ 是旋转矩阵，$\mathbf{p}\in {\mathbb{R}}^3$ 是位置向量，$\mathbf{v}$ 是速度，$\mathbf{b}$ 是 IMU 偏差。从 $\text{B}$ 到 $\text{W}$ 的变换 $\text{T}\in SE(3)$ 表示为 $\text{T}=[\text{R}|\text{p}]$

Fig. 1 LIO-SAM的系统结构。该系统接收来自 3D 激光雷达、IMU 和可选 GPS 的输入。引入四种类型的因子来构建因子图：(a) IMU 预积分因子、(b) 激光雷达里程计因子、© GPS 因子和 (d) 环路闭合因子

引入了四种类型的因子以及一种变量类型来构建因子图。该变量代表机器人在特定时间的状态，归因于图的节点

(a) IMU 预积分因子、(b) 激光雷达里程计因子、© GPS 因子和 (d) 环路闭合因子

B. IMU 预积分因子

IMU 的角速度和加速度测量值使用如下定义
$${\hat{\omega }}_t={\omega }_t+{\text{b}}_t^{\omega }+{\text{n}}_t^{\omega }\text{\hspace{1em}(2)}$$

$${\hat{\mathbf{a}}}_t={\text{R}}_t^{\text{BW}}({\mathbf{a}}_t-\mathbf{g})+{\mathbf{b}}_t^{\mathrm{a}}+{\mathbf{n}}_t^{\mathrm{a}}\text{\hspace{1em}(3)}$$

其中 ${\hat{\omega }}_t$ 和 ${\hat{\mathbf{a}}}_t$ 是时间 $t$ 时 $\mathbf{B}$ 中的原始 IMU 测量值。 ${\hat{\omega }}_t$ 和 ${\hat{\mathbf{a}}}_t$ 受到缓慢变化的偏差 ${\text{b}}_t$ 和白噪声 ${\text{n}}_t$ 的影响。 ${\text{R}}_t^{\text{BW}}$ 是从 $\text{W}$ 到 $\text{B}$ 的旋转矩阵。$\mathbf{g}$ 是 $\mathbf{W}$ 中的恒定重力矢量

使用 IMU 的测量值来推断机器人的运动。机器人在时间 $t+\Delta t$ 时的速度、位置和旋转可以计算如下
$${\mathbf{v}}_{t+\Delta t}={\mathbf{v}}_t+\mathbf{g}\Delta t+{\mathbf{R}}_t({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathbf{a}}-{\mathbf{n}}_t^{\mathbf{a}})\Delta t\text{\hspace{1em}(4)}$$

$${\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({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathbf{a}}-{\mathbf{n}}_t^{\mathbf{a}})\Delta t^2\text{\hspace{1em}(5)}$$

$${\mathbf{R}}_{t+\Delta t}={\mathbf{R}}_t\exp (({\hat{\omega }}_t-{\text{b}}_t^{\omega }-{\text{n}}_t^{\omega })\Delta t)\text{\hspace{1em}(6)}$$

其中 ${\mathbf{R}}_t={\mathbf{R}}_t^{WB}={\mathbf{R}}_t^{B{W}^{\top }}$。这里假设 B 的角速度和加速度在上述积分过程中保持恒定

应用IMU预积分方法来获得两个时间步之间的相对身体运动。时间 $i$ 和 $j$ 之间的预积分测量值 $\Delta {\mathbf{v}}_{ij}$、$\Delta {\mathbf{p}}_{ij}$ 和 $\Delta {\mathbf{R}}_{ij}$ 可以使用以下公式计算
$$\begin{array}{rl}\Delta {\mathbf{v}}_{ij}& ={\mathbf{R}}_i^{\top }({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})\\ \Delta {\mathbf{p}}_{ij}& ={\mathbf{R}}_i^{\top }({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)\\ \Delta {\mathbf{R}}_{ij}& ={\mathbf{R}}_i^{\top }{\mathbf{R}}_j\end{array}\text{\hspace{1em}(7)(8)(9)}$$

C. 激光雷达里程计系数

当新的激光雷达扫描到达时，我们首先执行特征提取。通过评估局部区域上点的粗糙度来提取边缘和平面特征。具有较大粗糙度值的点被归类为边缘特征

• 将在时间 $i$ 的激光雷达扫描中提取的边缘和平面特征分别表示为 ${\text{F}}_i^e$ 和 ${\text{F}}_i^p$。在第 i 时刻提取的所有特征组成一个激光雷达帧 ${\text{F}}_i$，其中 F i = { F i e , F i p } \mathbb{F}_i = \{\textbf{F}^e_i , \textbf{F}^p_i\} Fi​={Fie​,Fip​} ，激光雷达框架 $\mathbb{F}$ 在 $\mathbf{B}$ 中表示

• 采用关键帧选择的概念

  • 当机器人姿态的变化与之前的状态 ${\text{x}}_i$ 相比超过用户定义的阈值时，我们选择激光雷达帧 ${\mathbb{F}}_{i+1}$ 作为关键帧
  • 新保存的关键帧 ${\mathbb{F}}_{i+1}$ 与因子图中的新机器人状态节点 ${\text{x}}_{i+1}$ 相关联
  • 两个关键帧之间的激光雷达帧将被丢弃

  通过这种方式添加关键帧不仅可以实现地图密度和内存消耗之间的平衡，而且有助于保持相对稀疏的因子图，适合实时非线性优化。在本文工作中，添加新关键帧的位置和旋转变化阈值选择为 1m 和 10°

假设向因子图中添加一个新的状态节点 ${\text{x}}_{i+1}$。与此状态关联的激光雷达关键帧是 ${\mathbb{F}}_{i+1}$。激光雷达里程计因子的生成描述如下步骤

• 体素地图的子关键帧：取 $n$ 个最近的关键帧，我们称之为子关键帧，用于估计。子关键帧集合 { F i − n , . . . , F i } \{\mathbb{F}_{i−n}, ..., \mathbb{F}_i\} {Fi−n​,...,Fi​} 然后使用与它们相关联的变换 { T i − n , . . . , T i } \{\mathbf{T}_{i−n}, ..., \mathbf{T}_i\} {Ti−n​,...,Ti​} 转换为帧 $\mathbf{W}$。转换后的子关键帧被合并成一个体素地图 ${\mathbf{M}}_i$。由于在先前的特征提取步骤中提取了两种类型的特征，${\mathbf{M}}_i$由两个子体素地图组成，分别表示为 ${\mathbf{M}}_i^e$，边缘特征体素地图，和 ${\mathbf{M}}_i^p$，平面特征体素地图。激光雷达帧和体素图彼此相关，如下所示
  M i = { M i e , M i p } w h e r e :   M i e =   ′ F i e ∪   ′ F i − 1 e ∪ . . . ∪   ′ F i − n e M i p =   ′ F i p ∪   ′ F i − 1 p ∪ . . . ∪   ′ F i − n p \mathbf{M_i}=\{\mathbf{M}^e_i,\mathbf{M}^p_i\}\\ where:\ \mathbf{M}^e_i=\ '\mathbf{F}^e_i∪\ '\mathbf{F}^e_{i−1} ∪ ... ∪\ '\mathbf{F}^e_{i−n}\\ \mathbf{M}^p_i=\ '\mathbf{F}^p_i∪\ '\mathbf{F}^p_{i−1} ∪ ... ∪\ '\mathbf{F}^p_{i−n} Mi​={Mie​,Mip​}where: Mie​= ′Fie​∪ ′Fi−1e​∪...∪ ′Fi−ne​Mip​= ′Fip​∪ ′Fi−1p​∪...∪ ′Fi−np​

  ${}^{\prime }{\mathbf{F}}_i^e$ 和 ${}^{\prime }{\mathbf{F}}_i^p$ 是W 中变换后的边缘和平面特征。然后对 ${\mathbf{M}}_i^e$ 和 ${\mathbf{M}}_i^p$ 进行下采样以消除落在同一体素单元中的重复特征

• 扫描匹配：通过扫描匹配将新获得的激光雷达帧 ${\mathbb{F}}_{i+1}$（也是 { F i + 1 e ， F i + 1 p } \{\mathbf{F}^e_{i+1}，\mathbf{F}^p_{i+1}\} {Fi+1e​，Fi+1p​}）与 ${\mathbf{M}}_i$ 进行匹配

  • 首先将 { F i + 1 e ， F i + 1 p } \{\mathbf{F}^e_{i+1}，\mathbf{F}^p_{i+1}\} {Fi+1e​，Fi+1p​} 从 $\mathbf{B}$ 变换到 $\mathbf{W}$ 并得到 { ′ F i + 1 e ， ′ F i + 1 p } \{'\mathbf{F}^e_{i+1}，'\mathbf{F}^p_{i+1}\} {′Fi+1e​，′Fi+1p​} 。该初始变换是通过使用 IMU 预测的机器人运动 ${\tilde{\mathbf{T}}}_{i+1}$ 获得的
  • 对于${}^{\prime }{\mathbf{F}}_{i+1}^e$ 或 ${}^{\prime }{\mathbf{F}}_{i+1}^p$ 中的每个特征，我们然后在 ${\mathbf{M}}_i^e$ 或 ${\mathbf{M}}_i^p$ 中找到其边缘或平面对应关系

• 相对变换：特征与其边缘或平面块对应关系之间的距离可以使用以下方程计算
  $${\mathbf{d}}_{e_k}=\frac{\left|({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,u}^e)\times ({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,v}^e)\right|}{\left|{\mathbf{p}}_{i,u}^e-{\mathbf{p}}_{i,v}^e\right|}\text{\hspace{1em}(10)}$$

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

  其中 $k$、$u$、$v$ 和 $w$ 是其对应集合中的特征索引。对于${}^{\prime }{\mathbf{F}}_{i+1}^e$ 中的边缘特征 ${\mathbf{p}}_{i+1,k}^e$，${\mathbf{p}}_{i,u}^e$ 和 ${\mathbf{p}}_{i,v}^e$ 是形成 ${\mathbf{M}}_i^e$ 中相应边缘线的点。对于 ${}^{\prime }{\mathbf{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}}_i^p$ 中相应的平面块

  然后使用高斯牛顿方法通过最小化来求解最优变换
  $$\underset{{\mathbf{T}}_{i+1}}{\min }=\left\{\sum _{{\mathbf{p}}_{i+1,k}^e{\in }^{\prime }{\mathbf{F}}_{i+1}^e}{\mathbf{d}}_{e_k}+\sum _{{\mathbf{p}}_{i+1,k}^p{\in }^{\prime }{\mathbf{F}}_{i+1}^p}{\mathbf{d}}_{p_k}\right\}$$
  最后，可以得到 ${\text{x}}_i$ 和 ${\text{x}}_{i+1}$ 之间的相对变换 $\Delta {\mathbf{T}}_{i,i+1}$，这是连接这两个位姿的激光雷达里程计因子
  $$\Delta {\mathbf{T}}_{i,i+1}={\mathbf{T}}_i^{\top }{\mathbf{T}}_{i+1}\text{\hspace{1em}(12)}$$

D. GPS 因子

尽管可以仅利用 IMU 预积分和激光雷达里程计因子来获得可靠的状态估计和映射，但系统在长时间导航任务中仍然会出现漂移。为了解决这个问题，可以引入提供绝对测量的传感器以消除漂移，如 GPS

• 当收到 GPS 测量结果时，首先将它们转换为本地笛卡尔坐标系
• 当 GPS 接收可用时，没有必要不断添加 GPS 因子，因为激光雷达惯性里程计的漂移增长非常缓慢
• 只有当估计的位置协方差大于接收到的 GPS 位置协方差时，我们才添加 GPS 因子

E. 环路闭合因子

实现了一种朴素但有效的基于欧几里德距离的闭环检测方法

• 当一个新的状态 ${\mathrm{x}}_{i+1}$ 添加到因子图中时，首先搜索图并找到欧几里德空间中接近 ${\mathrm{x}}_{i+1}$ 的先验状态
• 尝试使用扫描匹配将 ${\mathbb{F}}_{i+1}$ 与子关键帧 { F 3 − m , . . . , F 3 , . . . , F 3 + m } \{\mathbb{F}_{3−m}, ..., \mathbb{F}_3, ..., \mathbb{F}_{3+m}\} {F3−m​,...,F3​,...,F3+m​} 匹配
• ${\mathbb{F}}_{i+1}$ 和过去的子关键帧在扫描匹配之前首先转换为 $\mathbf{W}$
• 获得相对变换 $\Delta {\mathrm{T}}_{3,i+1}$ 并将其作为闭环因子添加到图中

在实践中发现当 GPS 是唯一可用的绝对传感器时，添加闭环因子对于纠正机器人高度的漂移特别有用

---

4. Experiments

• 使用的传感器套件包括 Velodyne VLP16 激光雷达、MicroStrain 3DM-GX5-25 IMU 和 Reach M GPS

  

  Fig. 2 数据集在 3 个平台上收集：(a) 定制手持设备，(b) 无人驾驶地面车辆 - Clearpath Jackal，© 电动船 - Duffy 21

  数据集的详细信息如表 I 所示

  

A. 旋转数据集

在此测试中，重点评估仅将 IMU 预积分和激光雷达里程计因子添加到因子图中时框架的稳健性。旋转数据集是由用户手持传感器套件并在静止状态下执行一系列激进的旋转操作来收集的

Fig. 3 旋转测试中 LOAM 和 LIO-SAM 的绘图结果。 LIOM 未能产生有意义的结果

B. 步行数据集

该测试旨在评估当系统在 SE(3) 中经历剧烈平移和旋转时方法的性能。该数据集遇到的最大平移速度和旋转速度分别为 1.8 m/s 和 213.9 °/s

Fig. 4 使用 Walking 数据集的 LOAM、LIOM 和 LIO-SAM 的绘图结果。当遇到剧烈旋转时，(b) 中的 LOAM 地图会多次发散。 LIOM 优于 LOAM。然而，由于点云配准不准确，其地图显示出许多模糊的结构。 LIO-SAM 无需使用 GPS 即可生成与 Google Earth 图像一致的地图

C. 校园数据集

该测试旨在展示引入 GPS 和环路闭合因素的好处。为了做到这一点，特意禁止在图中插入 GPS 和闭环因子。当 GPS 和环路闭合因子均被禁用时，方法称为 LIO-odom，它仅利用 IMU 预积分和激光雷达里程计因子。当使用 GPS 因子时，方法被称为 LIO-GPS，它使用 IMU 预积分、激光雷达里程计和 GPS 因子来构建图形

Fig. 5 使用在麻省理工学院校园收集的校园数据集的各种方法的结果。红点表示开始和结束位置。轨迹方向为顺时针方向。 LIOM 未显示，因为它无法产生有意义的结果

Tab. 2 机器人返回起点时所有方法的相对平移误差

D. 公园数据集

在本次测试中，将传感器安装在 UGV 上，并沿着森林远足小径驾驶车辆。行驶40分钟后，机器人返回初始位置

Fig. 6 使用在新泽西州 Pleasant Valley Park 收集的 Park 数据集的各种方法的结果。红点表示开始和结束位置。轨迹方向为顺时针方向。

由于没有可靠的绝对高度测量，LIO-GPS 会出现高度漂移，并且在返回机器人初始位置时无法关闭环路。 LIO-SAM不存在这样的问题，因为它利用闭环因子来消除漂移

E. 阿姆斯特丹数据集

将传感器套件安装在船上，并沿着阿姆斯特丹运河航行了 3 个小时。尽管在本次测试中传感器的移动相对平稳，但由于多种原因，绘制运河图仍然具有挑战性

• 当直射阳光位于传感器视场中时，激光雷达出现许多错误检测，这种情况在数据收集期间大约 20% 的时间发生
• LOAM、LIOM 和 LIO-odom 都未能在此测试中产生有意义的结果

F. 基准测试结果

由于完整的 GPS 覆盖范围仅在 Park 数据集中可用，因此我们显示了与 GPS 测量历史记录相关的均方根误差 (RMSE) 结果，该结果被视为地面实况

Tab. 3 LIO-GPS 和 LIO-SAM 相对于 GPS 地面实况实现了类似的 RMSE 误差

Tab. 4 三种竞争方法在所有五个数据集中注册一个激光雷达帧的平均运行时间

LIO-SAM 的运行时间受特征图密度的影响更显着，而受因子图中节点和因子数量的影响较小

---

5. Conclusions And Discussion

• 提出了 LIO-SAM，这是一种通过平滑和映射实现紧耦合激光雷达惯性里程计的框架，用于在复杂环境中执行实时状态估计和映射
• 通过在因子图上制定激光雷达惯性里程计，LIO-SAM 特别适合多传感器融合。额外的传感器测量可以作为新因素轻松纳入框架中
• 提供绝对测量的传感器（例如 GPS、指南针或高度计）可用于消除激光雷达惯性里程计在长时间内或在功能匮乏的环境中累积的漂移
• 提出了一种滑动窗口方法，可以边缘化旧的激光雷达帧以进行扫描匹配
• 关键帧有选择地添加到因子图中，并且当生成激光雷达里程计和回环因子时，新关键帧仅注册到固定大小的子关键帧集