论文翻译：LIO-SAM：通过平滑和映射的紧耦合激光雷达惯性里程计

收录于：2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
October 25-29, 2020, Las Vegas, NV, USA (Virtual)

项目地址：https://github.com/TixiaoShan/LIO-SAM.git

摘要：我们提出了一个通过平滑和映射的紧耦合激光雷达惯性里程计框架，LIO-SAM，它实现了高精度、实时的移动机器人轨迹估计和地图构建。 LIO-SAM 在因子图上制定激光雷达惯性里程计，允许将多种相对和绝对测量（包括闭环）从不同来源作为因子合并到系统中。 惯性测量单元 (IMU) 预积分的估计运动可以消除点云的倾斜，并为激光雷达里程计优化生成初始猜测。 获得的激光雷达里程计解用于估计IMU的偏差。 为了确保实时的高性能，我们将旧的激光雷达扫描边缘化以进行姿态优化，而不是将激光雷达扫描与全局地图进行匹配。 局部尺度而不是全局尺度的扫描匹配显着提高了系统的实时性能，选择性引入关键帧以及将新关键帧注册到一组固定大小的先前关键帧的有效滑动窗口方法也是如此。 “子关键帧。” 所提出的方法在从不同规模和环境的三个平台收集的数据集上进行了广泛的评估。

1.引言

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

        在过去的二十年里，人们提出了许多基于激光雷达的状态估计和绘图方法。 其中，[1]中提出的用于低漂移、实时状态估计和建图的激光雷达里程计和建图（LOAM）方法是应用最广泛的方法之一。 LOAM 使用激光雷达和惯性测量单元 (IMU)，实现了最先进的性能，自在 KITTI 里程计基准网站上发布以来一直被评为基于激光雷达的顶级方法 [2]。 尽管取得了成功，LOAM 也存在一些局限性 - 通过将其数据保存在全局体素地图中，通常很难执行回环检测并结合其他绝对测量（例如 GPS）进行姿态校正。 当该体素图在特征丰富的环境中变得密集时，其在线优化过程的效率就会降低。 LOAM 在大规模测试中也会出现漂移，因为它的核心是基于扫描匹配的方法。

        在本文中，我们提出了一种通过平滑和映射实现紧耦合激光雷达惯性里程计的框架 LIOSAM，以解决上述问题。 我们假设一个用于点云去偏的非线性运动模型，使用原始 IMU 测量来估计激光雷达扫描期间的传感器运动。 除了去偏点云之外，估计的运动还可以作为激光雷达里程计优化的初始猜测。 然后使用获得的激光雷达里程计解来估计因子图中 IMU 的偏差。 通过引入用于机器人轨迹估计的全局因子图，我们可以使用激光雷达和 IMU 测量有效地执行传感器融合，将位置识别纳入机器人姿势之间，并引入绝对测量，例如 GPS 定位和罗盘航向（如果可用）。 来自不同来源的因素集合用于图形的联合优化。 此外，我们将旧的激光雷达扫描边缘化以进行位姿优化，而不是将扫描与 LOAM 等全局地图进行匹配。 局部尺度而不是全局尺度的扫描匹配显着提高了系统的实时性能，选择性引入关键帧以及将新关键帧注册到固定大小的先前“子帧”集的有效滑动窗口方法也是如此。 -关键帧。” 我们工作的主要贡献可概括如下：

        -构建在因子图之上的紧耦合激光雷达惯性里程计框架，适用于多传感器融合和全局优化。

        -一种高效的、基于局部滑动窗口的扫描匹配方法，通过将选择性选择的新关键帧注册到一组固定大小的先前子关键帧来实现实时性能。

        -所提出的框架通过各种规模、车辆和环境的测试进行了广泛的验证。

2.相关工作

        激光雷达里程计通常通过使用 ICP [3] 和 GICP [4] 等扫描匹配方法查找两个连续帧之间的相对变换来执行。基于特征的匹配方法由于其计算效率而成为流行的替代方法，而不是匹配完整的点云。例如，在[5]中，提出了一种用于实时激光雷达里程计的基于平面的配准方法。假设在结构化环境中进行操作，它从点云中提取平面并通过解决最小二乘问题来匹配它们。文献[6]提出了一种基于领线的方法来进行里程估计。在这种方法中，线段是从原始点云中随机生成的，并用于稍后的配准。然而，由于现代 3D 激光雷达的旋转机制和传感器运动，扫描的点云经常会倾斜。单独使用激光雷达进行姿态估计并不理想，因为使用倾斜的点云或特征进行配准最终会导致较大的漂移。

        因此，激光雷达通常与其他传感器（例如 IMU 和 GPS）结合使用，用于状态估计和绘图。 这种利用传感器融合的设计方案通常可以分为两类：松耦合融合和紧耦合融合。在 LOAM [1] 中，引入 IMU 来消除激光雷达扫描的倾斜，并为扫描匹配提供运动先验。 然而，IMU不参与算法的优化过程。因此，LOAM 可以归类为松散耦合方法。[7] 中提出了一种用于地面车辆测绘任务的轻量级地面优化激光雷达里程计和测绘（LeGOLOAM）方法 [8]。它对 IMU 测量的融合与 LOAM 相同。松耦合融合的一种更流行的方法是使用扩展卡尔曼滤波器（EKF）。例如，[9][13]在机器人状态估计的优化阶段使用 EKF 集成来自激光雷达、IMU 和可选 GPS 的测量结果。

        紧耦合系统通常可以提供更高的准确性，并且是目前正在进行的研究的主要焦点[14]。在[15]中，预积分 IMU 测量被用于去偏点云。[16] 中提出了一种以机器人为中心的激光雷达惯性状态估计器 LINS。LINS 专为地面车辆设计，使用错误状态卡尔曼滤波器以紧密耦合的方式递归地校正机器人的状态估计。[17] 中介绍了一种紧密耦合的激光雷达惯性里程计和绘图框架 LIOM。 LIOM 是 LIO-mapping 的缩写，联合优化激光雷达和 IMU 的测量，与 LOAM 相比，可实现相似或更好的精度。由于 LIOM 设计用于处理所有传感器测量结果，因此无法实现实时性能 - 在我们的测试中，其运行速度约为 0.6 倍实时。

3.通过平滑和映射进行激光雷达惯性里程测量

A. 系统概述

        我们首先定义整篇论文中使用的框架和符号。我们将世界坐标系表示为W，将机器人本体坐标系表示为B。为了方便起见，我们还假设IMU坐标系与机器人本体坐标系重合。机器人状态x可以写为：

其中 R ∈ SO(3) 是旋转矩阵，p ∈ R3 是位置向量，v 是速度，b 是 IMU 偏差。从 B 到 W 的变换 T ∈ SE(3) 表示为 T = [R | p]。

图 1 显示了所提议系统的概述。该系统从 3D 激光雷达、IMU 和可选的 GPS 接收传感器数据。我们试图利用这些传感器的观察来估计机器人的状态及其轨迹。该状态估计问题可以表述为最大后验（MAP）问题。我们使用因子图来模拟这个问题，因为与贝叶斯网络相比，它更适合执行推理。 假设高斯噪声模型，我们问题的 MAP 推断相当于求解非线性最小二乘问题 [18]。请注意，在不失一般性的情况下，所提出的系统还可以合并来自其他传感器的测量结果，例如来自高度计的海拔或来自指南针的航向。

        我们引入了四种类型的因子以及一种变量类型来构建因子图。该变量代表机器人在特定时间的状态，归因于图的节点。这四种类型的因子是：(a) IMU 预积分因子、(b) 激光雷达里程计因子、(c) GPS 因子和 (d) 环路闭合因子。当机器人位姿的变化超过用户定义的阈值时，新的机器人状态节点 x 就会添加到图中。使用增量平滑和贝叶斯树映射 (iSAM2) [19]，在插入新节点时优化因子图。以下各节描述了生成这些因素的过程。

B. IMU 预积分因子

        IMU 的角速度和加速度测量值使用等式 2 和 3 定义:

其中  和  是时间 t 时 B 中的原始 IMU 测量值。  和  受到缓慢变化的偏差  和白噪声 的影响。  是从W到B的旋转矩阵，g是W中的恒定重力矢量。

        现在，我们可以使用 IMU 的测量值来推断机器人的运动。 机器人在时间 t +Δt 时的速度、位置和旋转可以计算如下：

其中  。 这里我们假设在上述积分过程中B的角速度和加速度保持恒定。

然后，我们应用[20]中提出的IMU预积分方法来获得两个时间步之间的相对身体运动。 时间 i 和 j 之间的预积分测量值 ， 和  可以使用以下公式计算：

由于篇幅限制，我们建议读者参考[20]中的描述来详细推导方程 7—9. 除了效率之外，应用 IMU 预积分也自然地为我们提供了因子图的一种约束 - IMU 预积分因子。IMU 偏差与图中的激光雷达里程计因素共同优化。

C. 激光雷达里程计因子

        当新的激光雷达扫描到达时，我们首先执行特征提取。 通过评估局部区域上点的粗糙度来提取边缘和平面特征。 具有较大粗糙度值的点被归类为边缘特征。 类似地，平面特征按较小的粗糙度值进行分类。 我们将在时间 i 的激光雷达扫描中提取的边缘和平面特征分别表示为和 。在时间i提取的所有特征组成激光雷达帧，其中。注意，激光雷达帧在B中表示。特征提取过程的更详细描述可以在[1]中找到，或者如果使用距离图像，则在[7]中。

        使用每个激光雷达帧进行计算并向图中添加因子在计算上是很困难的，因此我们采用关键帧选择的概念，该概念广泛应用于视觉 SLAM 领域。 使用简单但有效的启发式方法，当机器人姿态的变化与之前的状态 xi 相比超过用户定义的阈值时，我们选择激光雷达帧作为关键帧。 新保存的关键帧与因子图中的新机器人状态节点相关联。 两个关键帧之间的激光雷达帧将被丢弃。 通过这种方式添加关键帧不仅可以实现地图密度和内存消耗之间的平衡，而且有助于保持相对稀疏的因子图，适合实时非线性优化。 在我们的工作中，添加新关键帧的位置和旋转变化阈值选择为 1m 和 10°。

        假设我们希望向因子图中添加一个新的状态节点 。 与此状态关联的激光雷达关键帧是 。 激光雷达里程计因子的生成按以下步骤描述：

        1）体素图的子关键帧：我们实现了一个滑动窗口的方法来创建一个点云地图包含固定数量的最近激光雷达扫描。我们不是优化两个连续激光雷达扫描之间的变换，而是提取n个最近的关键帧（我们称之为子关键帧）进行估计。然后，使用与子关键帧  相关联的变换将该子关键帧集变换到帧W。变换的子关键帧被合并在一起成为体素贴图 。由于我们在前面的特征提取步骤中提取了两种类型的特征， 由两个子体素映射组成，分别表示为，边缘特征体素映射和 ，平面特征体素映射。激光雷达帧和体素贴图相互关联，如下所示：

        2）扫描匹配：

        3）相对变换：

D. GPS 因子

        尽管我们可以仅利用 IMU 预积分和激光雷达里程计因子来获得可靠的状态估计和映射，但系统在长时间导航任务中仍然会出现漂移。为了解决这个问题，我们可以引入提供绝对测量的传感器以消除漂移。此类传感器包括高度计、指南针和 GPS。 为了便于说明，我们在此讨论 GPS，因为它广泛应用于现实世界的导航系统中。

        当我们收到 GPS 测量结果时，我们首先使用[21]中提出的方法将它们转换为本地笛卡尔坐标系。在向因子图中添加新节点后，我们将新的 GPS 因子与该节点相关联。如果 GPS 信号未与激光雷达帧硬件同步，我们会根据激光雷达帧的时间戳在 GPS 测量值之间进行线性插值。

        我们注意到，当 GPS 接收可用时，没有必要不断添加 GPS 因子，因为激光雷达惯性里程计的漂移增长非常缓慢。实际上，只有当估计的位置协方差大于接收到的 GPS 位置协方差时，我们才添加 GPS 因子。

E. 环路闭合因子

        我们注意到，当 GPS 接收可用时，没有必要不断添加 GPS 因子，因为激光雷达惯性里程计的漂移增长非常缓慢。实际上，只有当估计的位置协方差大于接收到的 GPS 位置协方差时，我们才添加 GPS 因子。由于使用了因子图，与 LOAM 和 LIOM 不同，闭环也可以无缝地合并到所提出的系统中。为了说明的目的，我们描述并实现了一种朴素但有效的基于欧几里德距离的闭环检测方法。 我们还注意到，我们提出的框架与其他闭环检测方法兼容，例如[22]和[23]，它们生成点云描述符并将其用于位置识别。

        当一个新的状态添加到因子图中时，我们首先搜索图并找到欧几里德空间中接近 的先验状态。例如，如图 1 所示，是返回的候选之一。 然后，我们尝试使用扫描匹配将与子关键帧匹配。请注意，和过去的子关键帧在扫描匹配之前首先转换为 W。我们获得相对变换并将其作为闭环因子添加到图中。 在本文中，我们选择索引 m 为 12，并且从新状态开始的闭环搜索距离设置为 15m。

        在实践中，我们发现当 GPS 是唯一可用的绝对传感器时，添加闭环因子对于纠正机器人高度的漂移特别有用。这是因为 GPS 的海拔测量非常不准确——在我们的测试中，在没有环路闭合的情况下，海拔高度误差接近 100m。

4.实验

        我们现在描述一系列实验来定性和定量分析我们提出的框架。 本文使用的传感器套件包括 Velodyne VLP16 激光雷达、MicroStrain 3DM-GX5-25 IMU 和 Reach M GPS。 为了进行验证，我们收集了 5 个不同规模、平台和环境的不同数据集。这些数据集分别称为“旋转”、“步行”、“校园”、“公园”和“阿姆斯特丹”。传感器安装平台如图 2 所示。前三个数据集是使用麻省理工学院校园内定制的手持设备收集的。公园数据集是使用无人地面车辆 (UGV) - Clearpath Jackal 在一个植被覆盖的公园中收集的。最后一个数据集“阿姆斯特丹”是通过将传感器安装在船上并在阿姆斯特丹运河中巡航来收集的。 这些数据集的详细信息如表 I 所示。

        我们将所提出的 LIO-SAM 框架与 LOAM 和 LIOM 进行比较。 在所有实验中，LOAM 和 LIOSAM 都被迫实时运行。 另一方面，LIOM 有无限的时间来处理每个传感器测量结果。 所有方法均以 C++ 实现，并在配备 Intel i7-10710U CPU 的笔记本电脑上使用 Ubuntu Linux 中的机器人操作系统 (ROS) [24] 执行。 我们注意到，只有CPU用于计算，没有启用并行计算。 我们的 LIO-SAM 实现可以在Github1上免费获得。 所执行实验的补充详细信息，包括所有测试的完整可视化，可以在下面的链接2中找到。

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

A.旋转数据集

        在此测试中，我们重点评估当仅将 IMU 预积分和激光雷达里程计因子添加到因子图中时框架的稳健性。 旋转数据集是由用户手持传感器套件并在静止状态下执行一系列激进的旋转操作来收集的。 本次测试中遇到的最大转速为133.7°/s。 充满结构的测试环境如图3（a）所示。 从 LOAM 和 LIO-SAM 获得的图谱如图 1 和 2 所示。 分别如图3(b)和(c)所示。 由于 LIOM 使用与[25]相同的初始化管道，它继承了视觉惯性 SLAM 相同的初始化敏感性，并且无法使用该数据集正确初始化。 由于未能产生有意义的结果，LIOM 的地图未显示。 如图所示，与 LOAM 地图相比，LIO-SAM 地图保留了更精细的环境结构细节。 这是因为即使机器人快速旋转，LIO-SAM 也能够在 SO(3) 中精确记录每个激光雷达帧。

B.步行数据集

        该测试旨在评估当系统在 SE(3) 中经历剧烈平移和旋转时我们方法的性能。该数据集遇到的最大平移速度和旋转速度分别为 1.8 m/s 和 213.9 °/s。在数据收集过程中，用户手持图2（a）所示的传感器套件并快速穿过麻省理工学院校园（图4（a））。在此测试中，当遇到剧烈旋转时，LOAM 图（如图 4（b）所示）在多个位置发散。在此测试中，LIOM 的表现优于 LOAM。 然而，如图 4（c）所示，它的地图在不同位置仍然略有不同，并且由许多模糊结构组成。由于 LIOM 旨在处理所有传感器测量结果，因此它仅以 0.56× 实时速度运行，而其他方法则实时运行。最后，LIO-SAM 优于这两种方法，并生成与可用的 Google 地球图像一致的地图。

C.校园数据集

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

        为了收集此数据集，用户使用手持设备在麻省理工学院校园内走动并返回到同一位置。由于测绘区域有大量建筑物和树木，GPS 接收很少可用，而且大多数时候都不准确。过滤掉不一致的 GPS 测量结果后，GPS 可用的区域如图 5（a）中绿色部分所示。这些区域对应于少数没有被建筑物或树木包围的区域。

        LOAM、LIO-odom、LIOGPS 和 LIO-SAM 的估计轨迹如图 5（a）所示。由于未能正确初始化并产生有意义的结果，LIOM 的结果未显示。如图所示，与所有其他方法相比，LOAM 的轨迹显着漂移。 在没有GPS数据校正的情况下，LIO-odom的轨迹在地图右下角开始明显漂移。借助 GPS 数据，LIO-GPS 可以在可用时纠正漂移。然而，GPS 数据在数据集的后面部分不可用。因此，当机器人由于漂移返回起始位置时，LIOGPS无法关闭环路。另一方面，LIOSAM 可以通过在图中添加闭环因子来消除漂移。LIO-SAM 的地图与 Google Earth 图像非常吻合，如图 5(b) 所示。机器人返回起点时所有方法的相对平移误差如表2所示。

D.公园数据集

        在本次测试中，我们将传感器安装在 UGV 上，并沿着森林远足小路驾驶车辆。行驶40分钟后，机器人返回初始位置。UGV 在三种路面上行驶：沥青、草地和泥土覆盖的小道。由于没有悬架，机器人在非沥青路面上行驶时会经历低振幅但高频的振动。

        为了模拟具有挑战性的地图场景，我们仅在机器人处于开阔区域时才使用 GPS 测量，如图 6(a) 中的绿色部分所示。这种地图绘制场景代表了这样的任务：机器人必须绘制多个 GPS 无法使用的区域，并定期返回具有 GPS 可用性的区域以纠正漂移。

        与之前测试的结果类似，LOAM、LIOM 和 LIO-odom 都存在显着的漂移，因为没有可用的绝对校正数据。此外，LIOM 仅以 0.67× 实时运行，而其他方法都是实时运行。尽管LIO-GPS和LIO-SAM的轨迹在水平面内重合，但它们的相对平移误差不同（表2）。由于没有可靠的绝对高度测量，LIO-GPS 会出现高度漂移，并且在返回机器人初始位置时无法关闭环路。LIO-SAM不存在这样的问题，因为它利用闭环因子来消除漂移。

E.阿姆斯特丹数据集

        最后，我们将传感器套件安装在船上，并沿着阿姆斯特丹运河航行了 3 个小时。尽管在本次测试中传感器的移动相对平稳，但由于多种原因，绘制运河图仍然具有挑战性。运河上的许多桥梁都会造成退化场景，因为当船在桥下时，几乎没有有用的功能，类似于穿过一条长而无特色的走廊。由于不存在地面，平面特征的数量也明显减少。当直射阳光位于传感器视场中时，我们观察到激光雷达出现许多错误检测，这种情况在数据收集期间大约 20% 的时间发生。由于头顶上有桥梁和城市建筑，我们只能获得间歇性的 GPS 接收。

        由于这些挑战，LOAM、LIOM 和 LIO-odom 都未能在此测试中产生有意义的结果。与 Park 数据集中遇到的问题类似，由于高度漂移，LIO-GPS 在返回机器人初始位置时无法闭合环路，这进一步激发了我们在 LIO-SAM 中使用环路闭合因子。

F.基准测试结果

        由于完整的GPS覆盖仅在 PARK 数据集中可用，我们将均方根误差(RMSE)结果显示给GPS测量历史，这被视为地面真实。该RMSE误差不考虑沿z轴的误差。如表三所示，Lio-GPS和Lio-SAM在GPS地面实况方面实现了类似的均方根误差。请注意，我们可以通过允许这两种方法完全访问所有GPS测量来进一步减少这两种方法的误差至少一个数量级。但是，在许多映射设置中并不总是提供完全GPS访问权限。我们的目标是设计一个强大的系统，能够在各种具有挑战性的环境中运行。

        三种相互竞争的方法在所有五个数据集中注册一个激光雷达帧的平均运行时间如表IV所示。在所有测试中，LAAM和Lio-SAM被迫实时运行。换句话说，当激光雷达旋转速率为10赫兹时，如果运行时间超过100ms，则会丢弃一些激光雷达帧。Liom被给予无限的时间来处理每一帧激光雷达图像。结果表明，lio-SAM的运行时间明显少于其他两种方法，更适合在低功耗的嵌入式系统上部署。

        我们还通过比实时更快地向lio-SAM提供数据来对lio-SAM执行压力测试。与数据回放速度为1倍实时时的结果相比，当Lio-SAM无故障地实现类似的性能时，最大数据回放速度被记录并显示在表IV的最后一栏中。如图所示，Lio-SAM处理数据的速度比实时处理快13倍。

        我们注意到，Lio-SAM的运行时间受特征映射密度的影响更大，而受因子图中节点数和因子的影响较小。例如，Park数据集是在特征丰富的环境中收集的，其中植被产生了大量的特征，而阿姆斯特丹的数据集产生了更稀疏的特征地图。Park测试的因素图由4,573个节点和9,365个因素组成，而阿姆斯特丹测试的因素图由23,304个节点和49,617个因素组成。尽管如此，与Park测试中的运行时相比，lio-SAM在阿姆斯特丹测试中使用的时间更少。

5.结论与讨论

        我们提出了 LIO-SAM，这是一种通过平滑和映射实现紧耦合激光雷达惯性里程计的框架，用于在复杂环境中执行实时状态估计和映射。 通过在因子图上制定激光雷达惯性里程计，LIO-SAM 特别适合多传感器融合。 额外的传感器测量可以作为新因素轻松纳入框架中。 提供绝对测量的传感器（例如 GPS、指南针或高度计）可用于消除激光雷达惯性里程计在长时间内或在功能匮乏的环境中累积的漂移。 地点识别也可以轻松地集成到系统中。 为了提高系统的实时性能，我们提出了一种滑动窗口方法，可以边缘化旧的激光雷达帧以进行扫描匹配。 关键帧有选择地添加到因子图中，并且当生成激光雷达里程计和回环因子时，新关键帧仅注册到固定大小的子关键帧集。 这种局部范围而不是全局范围的扫​​描匹配有利于 LIO-SAM 框架的实时性能。 所提出的方法在跨各种环境的三个平台上收集的数据集上进行了彻底评估。 结果表明，与 LOAM 和 LIOM 相比，LIO-SAM 可以获得相似或更好的精度。 未来的工作包括在无人机上测试拟议的系统。

参考文献

        见原文。