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

摘要

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

I. introduction

状态估计、定位和建图是成功的智能移动机器人的基本前提，需要反馈控制、避障和规划以及许多其他功能。通过使用基于视觉和基于激光雷达的传感器，人们已经付出了巨大的努力来实现高性能的SLAM，这可以支持移动机器人的六自由度状态估计。基于视觉的方法通常使用单目或双目相机，并对连续图像特征进行三角测量以确定相机运动。尽管基于视觉的方法特别适用于位置识别，但它们对初始化、照明和范围的敏感性使得它们在单独用于支持自主导航系统时不可靠。另一方面，基于激光雷达的方法对照明变化基本不变。特别是随着最近，高分辨率3D激光雷达，such as the Velodyne VLS-128 and Ouster OS1-128，激光雷达更适合直接捕获3D空间中环境的细节。因此，本文重点研究基于激光雷达的状态估计和建图方法。
在过去二十年中，人们提出了许多基于激光雷达的状态估计和建图的方法。其中，参考文献1中提出的用于低漂移和实时状态估计和建图的激光雷达里程表和建图(LOAM)方法时应用最广泛的方法之一。LOAM使用激光雷达和惯性测量单元(IMU)，实现了最先经的性能，在KITTI里程表基准站点发布以来，一直被列为顶级的基于激光雷达的方法。尽管取得了成功，LOAM仍存在一些局限性——通过将其数据保存在全局立体像素图中，通常很难进行闭环检测并结合其他绝对测量(例如GPS)进行姿态校正。当该立体像素图在特征丰富的环境中变得密集时，其在线优化过程效率降低。LOAM在大规模测试中也存在漂移，因为它的核心是基于扫描匹配的方法。
在本文中，我们提出了一种通过平滑和mapping的紧耦合激光雷达惯性里程表框架LIO-SAM，以解决上述问题。我们假设点云去偏斜的非线性运动模型，使用原始IMU测量值估计激光雷达扫描期间的传感器运动。除了解歪斜点云之外，估计的运动还可以作为激光雷达里程表优化的初始猜测。然后使用获得的激光雷达里程表的解来估计因子图中IMU的偏差。通过引入用于机器人轨迹估计的全局因子图，我们可以使用激光雷达和IMU测量有效地执行传感器融合，在机器人姿态之间结合位置识别，并在绝对测量可用时引入绝对测量(GPS定位和compass heading)。来自不同来源的因素集合用于图的联合优化。此外，我们将旧的激光雷达扫描边缘化以进行姿态优化，而不是像LOAM一样将扫描与全局地图进行匹配。以局部尺度而不是全局尺度进行扫描匹配，可以显著提高系统的实时性能，关键帧的选择性引入和添加新关键帧到固定大小的先前"子关键帧"集合的高校滑窗优化方法也是如此。我们工作的主要贡献总结如下：

• 基于因子图构建的紧耦合激光雷达惯性里程表框架，适用于多传感器融合和全局优化。
• 一种高效的、基于局部滑动窗口的扫描匹配方法，通过将选择性选择的新关键帧注册到一组固定大小的先前子关键帧来实现实时性能。
• 所提出的框架通过各种规模、车辆和环境的测试得到了广泛的验证。
  
  图1：LIO-SAM的系统结构。该系统输入来自3D激光雷达、IMU和可选的GPS的输入。引入四种类型的因子来构建因子图。(a) IMU预积分因子，(b) 激光雷达里程表因子,，© GPS因子，(d) 环路闭合因子。在Section III讨论这些因子的产生。

related work

激光雷达里程表通常通过使用匹配扫描方法(ICP和GICP)查找两个连续帧之间的相对变换来执行。基于特征的匹配方法由于其计算效率而成为一种流行的替代方法，而不是匹配全点云。例如，in [5]，提出了一种用于实时激光雷达里程表的基于平面的配准方法。假设在结构化环境中进行操作，它从点云中提取平面，并通过求解最小二乘问题来匹配它们。[6]中提出了一种基于项圈线的里程表估计方法。在这种方法中，线段是从原始点云中随机生成的，稍后用于配准。然而，由于线代3D激光雷达的旋转机制和传感器的运动，扫描的点云通常是倾斜的。仅使用激光雷达进行姿态估计并不理想，因为使用倾斜点云或特征进行配准最终会导致较大的漂移。
因此，激光雷达通常与其他传感器(如IMU和GPS)结合使用，用于状态估计和地图绘制。这种利用传感器融合的设计方案通常可以分为两类：松耦合融合和紧耦合融合。在LOAM中，引入IMU来消除激光雷达扫描的偏差，并在扫描匹配之前给出运动。然而，IMU不参与算法的优化过程。因此，LOAM可以归类为松耦合方法。[7]中针对地面车辆测绘任务提出了一种轻型和地面优化的激光雷达里程表和测绘(LeGOLOAM)方法。它的IMU测量融合与LOAM相同。一种更流程的松耦合融合方法是使用扩展卡尔曼滤波器(EKF)。例如，[9]-[13]在机器人状态估计的优化阶段使用EKF集成来自激光雷达、IMU和可选GPS的测量。
紧耦合系统通常提供了改进的精度，目前是正在研究的主要焦点。In[15]，预集成的IMU测量被用于解偏斜点云。[16]中介绍了一种机器人中心激光雷达惯性状态估计器LINS。LINS专门为地面车辆设计，使用误差状态卡尔曼滤波器以紧耦合的方式递归校正机器人的状态估计。[17]中介绍了一种紧耦合的激光雷达惯性里程表和建图框架LIOM。LIOM是LIO-mapping的缩写，它联合优化了激光雷达和IMU的测量，与LOAM相比，实现了相同或更好的精度。由于LIOM设计用于处理所有传感器测量，因此无法实现实时性能，在我们的测试中，它的实时运行速度约为0.6倍。

LIDAR INERTIAL ODOMETRY VIA SMOOTHING AND MAPPING

A. System Overview

我们首先定义了本文中使用的框架和符号。我们将世界框架表示为W，将机器人主体框架表示为B。为了方便起见，我们还假设IMU框架与机器人主体框架重合。

我们提出的系统的概述图如图1所示。该系统从3D激光雷达、IMU和可选的GPS接收传感器数据。我们试图利用这些传感器的观测来估计机器人的状态及其轨迹。该状态估计问题可以被表述为最大后验(MAP)问题。我们使用因子图来建模这个问题，因为与贝叶斯网络相比，它更适合进行推理。在高斯噪声模型的假设下，我们问题的MAP推断等价于解决非线性最小二乘问题。请注意，在不失通用性的情况下，我们提出的系统还可以包括来自其他传感器的测量，例如，from an altimeter or heading from a compass。
我们介绍了四种类型的因子以及一种用于因子图构建的变量类型。这个变量表示机器人在特定时间的状态，它属于图的节点。
这四种因子类型分别是：

• IMU预积分因子
• 激光雷达里程表因子
• GPS因子
• 环路闭合因子
  当机器人姿势的变化超过用户定义的阈值时，新的机器人状态节点x被添加到图中。因子图在插入新节点时使用增量平滑和贝叶斯树映射(iSAM2)进行优化。

B. IMU Preintegration Factor

除了其效率，应用IMU预积分还自然地为因子图提供了一张类型的约束——IMU预集成因子。IMU偏差与图中的激光雷达里程表因子共同优化。

C. Lidar Odometry Factor

当新的激光雷达扫描到达时，我们首先执行特征提取。通过评估局部区域上的点的粗糙度来提取边缘和平面特征。粗糙度值较大的点被分类为边缘特征。类似地，平面特征按较小的粗糙度值进行分类。我们将在时间i从激光雷达扫描中提取边缘和平面特征分别表示为$F_i^e$和$F_i^p$。在时间i提取的所有特征组成激光雷达帧$F_i$,其中$F_i=${$F_i^e,F_i^p$}。请注意，激光雷达帧F在B中表示。如果使用距离图像，可以在[1]和[7]中找到特征提取过程的更详细描述。
使用每一个激光雷达帧来计算并向图中添加因子在计算上是困难的，因此我们采用了关键帧选择的概念，这在视觉SLAM领域得到了广泛应用。使用一种简单但有效的启发式方法，当机器人位姿的变化与之前的状态$x_i$相比超过用户定义的阈值时，我们选择激光雷达帧$F_i{+}_1$作为关键帧。新保存的关键帧$F_i{+}_1$与因子图中的新机器人状态节点$x_i{+}_1$相关联。两个关键帧之间的激光雷达帧被丢弃。以这种方式添加关键帧不仅实现了建图密度和内存消耗之间的平衡，而且有助于保持相对稀疏的因子图，这适用于实时非线性优化。在我们的工作中，添加新关键帧的位置和旋转变化阈值选择为$1m$和$10^{\circ }$。
假设我们希望向因子图中添加一个新的节点$x_i{+}_1$。与此状态相关的激光雷达关键帧为$F_i{+}_1$。
激光雷达里程计系数生成的步骤如下：

D. GPS Factor

尽管我们可以通过仅使用IMU预积分和激光雷达里程表因子来获得可靠的状态估计和建图，但在长时间导航任务中，系统仍会受到漂移的影响。为了解决这个问题，我们引入提供绝对测量以消除漂移的传感器。这些传感器包括an altimeter, compass and GPS. 为了便于说明，我们讨论了GPS，因为它广泛应用于现实世界的导航系统中。
当我们接收GPS测量值时，我们首先使用[21]中提出的方法将其转换为局部笛卡尔坐标系。在因子图中添加新节点后，我们将新的GPS因子与该节点相关联。如果GPS信号与激光雷达帧不是硬件同步的，我们基于激光雷达帧的时间戳在GPS测量之间进行线性插值。
我们注意到，当GPS接收可用时，不需要不断添加GPS因子，因为激光雷达惯性里程表的漂移增长非常缓慢。在实践中，当估计的位置协方差大于接收的GPS位置协方差时，我们仅添加GPS因子。

E. Loop Closure Factor

由于使用了因子图，与LOAM和LIOM相反，闭环也可以无缝地结合到我们的系统中。我们还注意到，我们提出的框架与用于闭环检测的其他方法兼容。例如[22]和[23]，它们生成点云描述符并将其用于位置识别。
当一个新的状态$x_i{+}_1$添加到因子图中时，我们首先搜索该图，并在欧几里得空间中找到接近$x_i{+}_1$的先验状态。如图1所示，例如，$x_3$时返回的候选者之一。然后，我们尝试将$F_i{+}_1$与子关键帧{$F_3{-}_m,...,F_3,...,F_3{+}_m$}使用扫描匹配。请注意，在扫描匹配之前，$F_i{+}_1$和过去的子关键帧首先被转换为W。我们得到相对变换$\Delta T_3{，}_i{+}_1$并将其作为闭环因子添加到图中。在本文中，我们选择所以m为12，闭环的搜索距离设置为距离新状态$x_i{+}_1$ 15m。
在实践中，我们发现当GPS是唯一可用的绝对传感器时，添加环路闭合因子对于校正机器人高度的漂移特别有用。这是因为GPS的高程测量非常不准确——在我们的测试中，在没有环路闭合的情况下，会导致接近100米的高度误差。

EXPERMENTS

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

我们将建议的LIO-SAM框架与LOAM和LIOM进行了比较。在所有实验中，LOAM和LIO-SAM都必须实时运行。另一方面，LIOM有无限的时间来处理每个传感器测量。所有这些方法都是用C++实现的，并在配备了Intel i7-10710U CPU的笔记本电脑上使用Ubuntu Linux中的机器人操作系统(ROS)执行。我们注意到，只有CPU用于计算，而没有启用并行计算。我们的LIO-SAM实现可在Github上免费获得。执行的实验的补充细节，包括所有测试的完整可视化，可在这个链接中找到。

A. Rotation Dataset

在本测试中，我们重点评估了在因子图中仅添加IMU预积分和激光雷达里程表因子时框架的稳健性。旋转数据集由手持传感器套件并在静止状态下执行一系列积极旋转动作的用户收集。本试验中遇到的最大转速为$133.7^{\circ }/s$、由结构填充的测试环境如图3(a)所示。从LOAM和LIO-SAM获得的地图如图。如图3(b)和©所示。由于LIOM使用了[25]中相同的初始化流水线。它继承了视觉惯性SLAM的相同初始化灵敏度，因此无法使用该数据集正确初始化。由于未能产生有意义的结果，LIOM地图未显示。如图所示，与LOAM地图相比，LIO-SAM地图保留了更精细的环境结构细节。这是因为LIO-SAM能够在$SO(3)$中精确地记录每个激光雷达帧，即使机器人经历快速旋转。

B. Walking Dataset

该测试旨在评估系统在$SE(3)$中发生剧烈平移和旋转时我们方法的性能。该数据集的最大平移和旋转速度分别为$1.8m/s$和$213.9^{\circ }/s$。在数据收集过程中，用户手持图2(a)所示的传感器套件，快速穿过MIT校园。在本测试中，如图4(b)所示，当遇到剧烈旋转时，LOAM图在多个位置发散。LIOM在该测试中优于LOAM。然而，如图4©所示，它的地图在不同位置仍略有差异，并由许多模糊结构组成。因为LIOM设计用于处理所有传感器测量，所有它仅以0.56X实时运行，而其他方法则实时运行。最后，LIO-SAM优于这两种方法，生成的地图与可用的谷歌地球图像一致。

C. Campus Dataset

该测试旨在展示引入GPS和环路闭合因子的好处。为了做到这一点，我们特意禁止在图形中插入GPS和环路闭合因子。当GPS和环路闭合因子都被禁用时，我们的方法被称为LIO-odom，它只使用IMU预积分和激光雷达测速因子。当使用GPS因子时，我们的方法称为LIO-GPS，它使用IMU预积分、激光雷达里程表和GPS因子进行图形构建。LIO-SAM使用所有可用的因素。
为了收集这个数据集，用户使用手持设备在麻省理工学院校园内走动，然后返回同一位置。由于绘图区域内有许多建筑物和树木，GPS接收很少可用，而且大多数时间都不准确。过滤掉不一致的GPS测量值后，可用GPS的区域如图5(a)所示为绿色部分。这些区域对应于少数未被建筑物或树木包围的区域。

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

D. Park Dataset

在这个测试中，我们将传感器安装在UGV上，并沿着森林徒步小径驾驶车辆。机器人在行驶40分钟后返回其初始位置。UGV在三个路面上行驶：沥青路面、草覆盖的地面和泥土覆盖的小道。由于缺乏悬架，机器人在非沥青路面上行驶时会经历低振幅但高频率的振动。
为了模拟具有挑战性的地图场景，我们仅在机器人处于开阔区域时使用GPS测量，如图6(a)中的绿色部分所示。这样的映射场景代表了机器人必须映射多个GPS被拒绝的区域并且周期性地返回到具有GPS可用性的区域以校正漂移的任务。

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

E. Amsterdam Dataset

最后，我们将传感器安装在船上，沿着阿姆斯特丹运河巡航了3个小时。尽管在本次测试中传感器的移动相对平稳，但由于几个原因，绘制运河地图仍然具有挑战性。运河上的许多桥梁构成了退化的场景，因为当船在其下方时，几乎没有什么有用的特征，类似于穿过一条长而无特征的走廊。由于地面不存在，平面特征的数量也显著减少。当阳光直射传感器视场时，我们观察到激光雷达的许多错误检测，这在数据收集期间大约20%的时间发生。由于头顶上有桥梁和城市建筑，我们只能获得间歇性GPS接收。
由于这些挑战，LOAM、LIOM和LIO-odom都未能在该测试中产生有意义的结果。与Park数据集中遇到的问题类似，LIO-GPS在返回机器人初始位置时无法闭合环路，因为高度漂移，这进一步促使我们在LIO-SAM中使用环路闭合因子。

F. Benchmarking Results

由于全GPS覆盖仅在Park数据集中可用，因此我们显示了与GPS测量历史相关的均方根误差（RMSE）结果，该结果被视为地面真实情况。该RMSE误差不考虑沿z轴的误差。如表III所示，LIO-GPS和LIO-SAM相对于GPS地面实况实现了相似的RMSE误差。请注意，我们可以通过让这两种方法完全访问所有GPS测量结果，将其误差至少降低一个数量级。然而，在许多地图设置中，完全GPS接入并不总是可用的。我们的目标是设计一个能够在各种挑战性环境中运行的稳健系统。

在所有五个数据集中注册一个激光雷达帧的三种竞争方法的平均运行时间如表IV所示。在所有测试中，LOAM和LIO-SAM都必须实时运行。换言之，当激光雷达旋转速率为10Hz时，如果运行时间超过100ms，则会丢弃一些激光雷达帧。LIOM有无限的时间处理每个激光雷达帧。如图所示，LIO-SAM使用的运行时间比其他两种方法少得多，这使得它更适合部署在低功耗嵌入式系统上。

我们还对LIO-SAM进行压力测试，以比实时更快的速度提供数据。与数据回放速度为1×实时时的结果相比，LIO-SAM在无故障情况下实现类似性能时，最大数据回放速度记录并显示在表IV的最后一列中。如图所示，LIO-SAM处理数据的速度比实时快13倍。

我们注意到，LIO-SAM的运行时间受特征图密度的影响更为显著，而受因子图中节点数和因子数的影响较小。例如，公园数据集是在特征丰富的环境中收集的，在该环境中，植被产生了大量的特征，而阿姆斯特丹数据集产生了更稀疏的特征图。虽然Park测试的因子图由4573个节点和9365个因子组成，但Amsterdam测试的图有23304个节点和49617个因子。尽管如此，LIO-SAM在阿姆斯特丹测试中使用的时间比Park测试中的运行时间少。

Conclusions and Discussion

我们提出了LIO-SAM，一种通过平滑和映射实现紧耦合的激光雷达惯性里程测量的框架，用于在复杂环境中进行实时状态估计和映射。通过在因子图上表示激光雷达惯性里程表，LIO-SAM特别适用于多传感器融合。附加的传感器测量可以很容易地作为新的因素并入框架中。提供绝对测量值的传感器，如GPS、罗盘或高度计，可用于消除激光雷达惯性里程表在长时间内或在特征较差的环境中累积的漂移。地点识别也可以很容易地并入系统。为了提高系统的实时性能，我们提出了一种滑动窗口方法，将旧的激光雷达帧边缘化以进行扫描匹配。关键帧被选择性地添加到因子图中，并且当生成激光雷达里程表和环路闭合因子时，新的关键帧仅注册到一组固定大小的子关键帧中。这种局部规模而非全局规模的扫描匹配有助于LIO-SAM框架的实时性能。在跨各种环境的三个平台上收集的数据集上，对所提出的方法进行了全面评估。结果表明，与LOAM和LIOM相比，LIO-SAM可以达到类似或更好的精度。未来的工作包括在无人机上测试拟议的系统。