雷达系列论文翻译（六）：LIO-SAM

LIO-SAM: Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping

摘要

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

引言

状态估计、定位和建图是成功的智能移动机器人的基本先决条件，是反馈控制、避障和规划等许多其他能力所必需的。使用基于视觉和基于激光雷达的传感，已经付出了巨大的努力来实现高性能的实时同步定位和建图(SLAM)，其可以支持移动机器人的六自由度状态估计。基于视觉的方法通常使用单目或双目相机，并在连续图像上三角化特征来确定相机的运动。虽然基于视觉的方法特别适合于位置识别，但是当它们单独用于支持自主导航系统时，它们对初始化、光照和距离的敏感性使得它们不可靠。另一方面，基于激光雷达的方法对光照变化具有强不变性。特别是随着大测量范围、高分辨率三维激光雷达的出现，如Velodyne VLS-128和 Ouster OS1-128，激光雷达更适合直接捕捉三维空间中的环境细节。因此，本文重点研究了基于激光雷达的状态估计和建图方法。

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

在本文中，我们提出了一个通过平滑和建图的紧耦合激光雷达惯性里程计框架，LIO-SAM，来解决上述问题。我们假设点云畸变的非线性运动模型，使用原始IMU测量估计激光雷达扫描期间的传感器运动。除了消除点云的畸变，估计的运动也是激光雷达里程计优化的初始位姿。获得的激光雷达里程计解随后用于估计因子图中惯性测量单元的零偏。通过为机器人轨迹估计引入全局因子图，我们可以使用激光雷达和惯性测量单元测量有效地执行传感器融合，将位置识别纳入机器人姿态，并引入绝对测量，如GPS位置和罗盘航向(如果可用)。来自各种来源的因子的集合用于图的联合优化。此外，我们将旧的激光雷达扫描边缘化，以进行姿态优化，而不是像LOAM那样将扫描与全局地图进行匹配。在局部尺度而不是全局尺度上进行扫描匹配，可以显著提高系统的实时性能，正如有选择地引入关键帧和将新的关键帧注册到一组固定大小的先前“子关键帧”中的滑动窗口方法，可以我们工作的主要贡献可以总结如下:

• 建立在因子图上的紧耦合激光雷达惯性里程计框架，适用于多传感器融合和全局优化。
• 一种高效的、基于局部滑动窗口的扫描匹配方法，通过有选择性的选择的新关键帧与固定大小的一组先前子关键帧进行配准，实现实时性能。
• 提出的框架经过了各种规模、车辆和环境的数据集上的广泛验证。

相关工作

激光雷达里程计通常通过使用扫描匹配方法(如ICP [3]和GICP[4])找到两个连续帧之间的相对变换。基于特征的匹配方法由于不需要匹配完整的点云可以获得极高的计算效率已经成为了一种流行的扫描匹配方法。例如，在[5]中，提出了一种用于实时激光雷达里程计的基于平面的配准方法。假设在结构化环境中进行操作，它从点云提取平面，并通过求解最小二乘问题来匹配它们。文献[6]提出了一种基于collar line的里程计估算方法。在这种方法中，线段从原始点云随机生成，并在之后用于配准。然而，由于现代三维激光雷达的旋转机制和传感器的运动，扫描的点云经常是带畸变的。单独使用激光雷达进行位置估计的效果并不理想，因为使用带畸变点云或特征进行配准最终会导致较大的漂移。

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

紧耦合的系统通常能提高精度，是目前正在进行的研究的方向[14]。在[15]中，预先积分的惯性测量单元的测量值被用于点云畸变去除。文献[16]介绍了一种机器人中心的激光雷达-惯性状态估计器，R-LINS。R-LINS使用误差状态卡尔曼滤波器以紧耦合的方式迭代地校正机器人的状态估计。由于缺乏其他可用于状态估计的传感器，它在长时间导航中发生漂移。文献[17]中介绍了一种紧耦合的雷达惯性里程计和建图框架LIOM。LIOM是LIO-mapping的缩写，它联合优化了激光雷达和IMU的测量结果，并在与LOAM进行比较时获得了相似或更好的精度。由于LIOM是为处理所有传感器测量而设计的，因此无法实现实时性能——在我们的测试中，它的运行速度大约为实时性能的0.6倍。

通过平滑和建图实现的雷达惯性里程计

A.系统概述

我们首先定义我们在整个论文中使用的坐标系和符号。我们将世界坐标系表示为$W$，将机器人机体坐标系表示为$B$。为了方便起见，我们还假设惯性测量单元坐标系与机器人机体坐标系是一致的。机器人状态$x$可以写成:
$$x=[R^T,p^T,v^T,b^T{]}^T\text{\hspace{1em}(1)}$$
其中$R\in SO(3)$为旋转矩阵，$p\in {\mathbb{R}}^3$是位置向量，$v$是速度，$b$是惯性测量单元的零偏。从$B$到$W$的变换矩阵$T\in SE(3)$被表示为$T=[R|p]$。

图1展示了本文中提出的系统的概述。

该系统从3D激光雷达、惯性测量单元和可选的GPS系统接收传感器数据。我们试图利用这些传感器的观测来估计机器人的状态及其轨迹。这个状态估计问题可以表述为最大后验概率(MAP)问题。我们使用因子图来建模这个问题，因为与贝叶斯网络相比，它更适合执行推理。在高斯噪声模型的假设下，我们问题的MAP推断相当于求解一个非线性最小二乘问题[18]。请注意，不失一般性，本文提出的系统也可以结合其他传感器的测量值，如高度计的高度值或罗盘的航向。

我们为因子图的构造引入了四种带有单变量类型的因子。这个代表机器人在特定时间的状态的变量被归为因子图的节点。这四种类型的因子是:(a)惯性测量单元预积分因子，(b)激光雷达里程计因子，© GPS因子，以及(d)闭环因子。当机器人姿态的变化超过用户定义的阈值时，一个新的机器人状态节点被添加到因子图中。因子图在插入新节点时使用增量平滑和建图的贝叶斯树(iSAM2)进行优化[19]。产生这些因子的过程将在以下章节中描述。

B.IMU预积分因子

来自惯性测量单元的角速度和加速度的测量值用方程2和3定义:
$${\hat{\omega }}_t={\omega }_t+b_t^{\omega }+n_t^{\omega }\text{\hspace{1em}(2)}$$
$${\hat{a}}_t=R_t^{BW}(a_t-g)+b_t^a+n_t^a\text{\hspace{1em}(3)}$$

其中${\hat{\omega }}_t$和${\hat{a}}_t$是指在$B$坐标系下时刻$t$时原始的惯性测量单元测量值。${\hat{\omega }}_t$和${\hat{a}}_t$受到缓慢变化的零偏$b_t$和白噪声$n_t$的影响。$R_t^{BW}$是从$W$到$B$的旋转矩阵，$g$是$W$坐标系下的恒定重力矢量。

我们现在可以使用来自惯性测量单元的测量来推测机器人的运动。机器人在时间$t+\Delta t$时的速度、位置和旋转可以计算如下:
$$v_{t+\Delta t}=v_t+g\Delta t+R_t({\hat{a}}_t-b_t^a-n_t^a)\Delta t\text{\hspace{1em}(4)}$$
$$p_{t+\Delta t}=p_t+v_t\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}{R}_t({\hat{a}}_t-b_t^a-n_t^a)\Delta t^2.\text{\hspace{1em}(5)}$$
$$R_{t+\Delta t}=R_{t}exp(({\hat{\omega }}_t-b_t^{\omega }-n_t^{\omega })\Delta t^2).\text{\hspace{1em}(6)}$$

其中$R_t=R_t^{WB}={R_t^{BW}}^T$ 。这里我们假设在上述积分过程中，$B$坐标系下的角速度和加速度保持不变。

然后，我们应用文献[20]中提出的惯性测量单元预积分方法来获得两个时间步长之间的相对机体运动。两个时刻$i$和$j$之间的预积分测量值$\Delta v_{ij}，\Delta p_{ij}$和$\Delta R_{ij}$可使用以下公式计算:
$$\Delta v_{ij}=R_i^T(v_j-v_i-g\Delta t_{ij})\text{\hspace{1em}(7)}$$
$$\Delta p_{ij}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2).\text{\hspace{1em}(8)}$$
$$\Delta R_{ij}=R_i^T{R}_j\text{\hspace{1em}(9)}$$

由于篇幅有限，我们请读者参考[20]的描述，以获得方程7-9的详细推导。除了效率之外，应用惯性测量单元预积分也自然地给我们提供了一种对因子图的约束——惯性测量单元预积分因子。在因子图中，惯性测量单元零偏与激光雷达里程计因子一起进行了联合优化。

C.雷达里程计因子

当新的激光雷达扫描到达时，我们首先执行特征提取。通过评估局部区域上的点的粗糙度来提取边缘和平面特征。粗糙度值较大的点被归类为边缘特征。类似地，平面特征对应于粗糙度值较小的点。我们以$F_i^e$和$F_i^p$来表示分别在时刻$i$时从激光雷达扫描中提取的边缘和平面特征。在时刻$i$时提取的所有特征组成一个雷达帧${\mathbb{F}}_i$，这里${\mathbb{F}}_i=\{F_i^e,F_i^p\}$。请注意，激光雷达帧$\mathbb{F}$被表示在$B$系中。如果使用距离图像，特征提取过程的更详细描述可以在[1]或[7]中找到。

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

假设我们希望在因子图中添加一个新的状态节点$x_{i+1}$。与此状态相关联的激光雷达关键帧为${\mathbb{F}}_{i+1}$。激光雷达里程计因子的生成描述如下:

1) 用于体素地图的子关键帧:

我们实现了一种滑动窗口方法来创建包含固定数量的最近激光雷达扫描的点云图。我们没有优化两次连续激光雷达扫描之间的变换，而是提取$n$个最近的关键帧，我们称之为子关键帧，来用于估计。该组子关键帧$\{{\mathbb{F}}_{i-n},...,{\mathbb{F}}_i\}$然后被使用与他们相对应的变换$T_{i-n},...,T_i$转换到世界坐标系$W$。转换后的子关键帧被合并到一个体素地图$M_i$中。由于我们在先前的特征提取步骤中提取了两种类型的特征，所以$M_i$由两个子体素地图组成，它们是边缘特征体素地图$M_i^e$和平面特征体素地图$M_i^p$。激光雷达帧和对应的体素地图如下所示:
$$M_i=\{M_i^e,M_i^p\}$$
这里
$$\begin{gathered} M_i^e{=}^{\prime }{F}_i^e{\cup }^{\prime }{F}_{i-1}^e\cup ...{\cup }^{\prime }{F}_{i-n}^e \\ {M}_i^p{=}^{\prime }{F}_i^p{\cup }^{\prime }{F}_{i-1}^p\cup ...{\cup }^{\prime }{F}_{i-n}^p \end{gathered}$$
${}^{\prime }{F}_i^e$和${}^{\prime }{F}_i^p$是变换后的边缘和平面特征。然后，对$M_i^e$和$M_i^p$进行下采样，以消除落在同一个体素单元中的重复特征。在本文中，$n$被选择为25。下采样分辨率分别为$0.2m$和$0.4m$。

2）扫描匹配

我们通过扫描匹配来匹配一个新获得的雷达帧${\mathbb{F}}_{i+1}$，也就是$\{F_{i+1}^e,F_{i+1}^p\}$，和$M_i$。各种扫描匹配方法，如[3]和[4]，可用于此目的。在这里，我们选择使用[1]中提出的方法，因为它在各种挑战性环境中的计算效率和鲁棒性。

我们首先将$\{F_{i+1}^e,F_{i+1}^p\}$从$B$坐标系转换到$W$坐标系，并得到${\{}^{\prime }{F}_{i+1}^e{,}^{\prime }{F}_{i+1}^p\}$。这个初始变换是通过使用来自IMU预测的机器人运动̃${\tilde{T}}_{i+1}$获得的。为${}^{\prime }{F}_{i+1}^e$’或${}^{\prime }{F}_{i+1}^p$中的每个特征，我们可以在$M_i^e$和$M_i^p$中找到其边缘或平面对应关系。为了简洁起见，这里省略了了寻找这些对应关系的详细步骤，但在[1]中有详细描述。

3）相对变换

特征与其对应的边缘或平面特征之间的距离可以用以下公式计算:
$$d_{e_k}=\frac{|(p_{i+1,k}^e-p_{i,u}^e)\times (p_{i+1,k}^e-p_{i,v}^e)|}{|p_{i,u}^e-p_{i,v}^e|}\text{\hspace{1em}(10)}$$

$$d_{p_k}=\frac{|(p_{i+1,k}^p-p_{i,u}^p)(p_{i,u}^p-p_{i,v}^p)\times (p_{i,u}^p-p_{i,w}^p)|}{|(p_{i,u}^p-p_{i,v}^p)\times (p_{i,u}^p-p_{i,w}^p)|}.\text{\hspace{1em}(11)}$$

其中$k$、$u$、$v$和是它们对应集合中的特征索引。对于在${}^{\prime }{F}_{i+1}^e$中的边特征$p_{i+1,k}^e$，$p_{i,u}^e$和$p_{i,v}^e$是在$M_i^e$中改变形成相应边缘线的点。对于${}^{\prime }{F}_{i+1}^p$中的平面特征$p_{i+1,k}^p$，$p_{i,u}^p$,$p_{i,v}^p$和$p_{i,w}^p$是在$M_i^p$中形成相应的平面的点。然后使用高斯牛顿法通过最小化以下代价函数来求解最优变换:
$$\underset{T_{i+1}}{\min }\{\sum _{p_{i+1,k}^e{\in }^{\prime }{F}_{i+1}^e}{d}_{e_k}+\sum _{p_{i+1,k}^p{\in }^{\prime }{F}_{i+1}^p}{d}_{p_k}\}$$
最后，我们可以得到$x_i$和$x_{i+1}$之间的相对变换$\Delta T_{i,i+1}=T_i^T{T}_{i+1}$，这是连接这两个位姿的激光雷达里程计因子:

我们注意到获得$\Delta T_{i,i+1}$的另一种方法是将子关键帧转换到$x_i$的坐标系下。换句话说，我们匹配${\mathbb{F}}_{i+1}$与在到$x_i$坐标系下表示的体素地图。这样就可以直接得到相对变换$\Delta T_{i,i+1}$。因为变换后的特征${}^{\prime }{F}_i^p$和${}^{\prime }{F}_i^p$可以被多次重用，所以为了效率我们选择使用1)中描述的方法。

D.GPS因子

尽管仅利用惯性测量单元预积分和激光雷达里程计因子就能获得可靠的状态估计和地图绘制，但在长时间导航任务中，系统仍存在漂移问题。为了解决这个问题，我们可以引入为消除漂移提供绝对测量的传感器。这种传感器包括高度计、指南针和GPS。出于说明的目的，我们在这里讨论GPS，因为它广泛用于现实世界的导航系统。

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

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

E.闭环因子

由于使用了因子图，闭环也可以无缝地集成到本文提出的的系统中，这与LOAM和LIOM不同。出于说明的目的，我们描述并实现了一种简单但有效的基于距离的闭环检测方法。我们还注意到，我们提出的框架与用于闭环检测的其他方法兼容，例如[22]和[23]，它们生成点云描述子并将其用于位置识别。

当一个新的状态$x_{i+1}$被添加到因子图中时，我们首先搜索该图，并在欧氏空间中找到与状态$x_{i+1}$接近的先前状态。如图1所示，例如，$x_3$是返回的候选之一。

然后，我们尝试将${\mathbb{F}}_{i+1}$与子关键帧$\{{\mathbb{F}}_{3-m},...,{\mathbb{F}}_{3+m}\}$进行扫描匹配。请注意，在扫描匹配之前，${\mathbb{F}}_{i+1}$和过去的子关键帧首先被转换到世界坐标系$W$下。我们得到相对变换$\Delta T_{3,i+1}$，并把它作为一个闭环因子加到因子图中。在本文中，我们选择索引$m$为12，并且从新的状态$x_{i+1}$开始，闭环的搜索距离设置为$15m$。

在实践中，我们发现当GPS是唯一可用的绝对传感器时，增加闭环因子对于校正机器人高度的漂移特别有用。这是因为GPS的相关测量非常不准确——在我们的测试中，缺乏闭环的情况下，高度误差接近100米。

实验

我们现在描述一系列实验来定性和定量分析我们提出的框架。本文使用的传感器套件包括一台Velodyne VLP-16激光雷达、一台MicroStrain 3DM-GX5-25惯性测量单元和一台Reach M GPS。为了验证，我们收集了5个不同规模、平台和环境的不同数据集。这些数据集分别被称为旋转、步行、校园、公园和水坝。传感器安装平台如图2所示。

前三个数据集是使用定制的手持设备在麻省理工学院校园内收集的。公园数据集是在一个被植被覆盖的公园中收集的，使用的是无人驾驶地面车辆(UGV)——Clearpath Jackal。最后一个数据集，Amsterdam，是通过在船上安装传感器并在阿姆斯特丹的运河中巡航收集的。这些数据集的详情见表一

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

A.旋转数据集

在本次测试中，我们重点评估了只有惯性测量单元预积分因子和激光雷达里程计因子被添加到因子图时我们框架的鲁棒性。用户手持传感器套件并在站立时执行一系列激进的旋转动作来收集旋转数据集。在这个测试中遇到的最大转速为$133.7°/s$。试验环境如图3(a)所示。

图3(b)和©分别显示了从LOAM和LIO-SAM获得的结果图。
在这里插入图片描述

因为LIOM使用与[25]相同的初始化流程，所以它继承了与视觉惯性SLAM相同的初始化灵敏度，并且无法使用该数据集正确初始化。由于未能产生有意义的结果，因此没有显示LIOM的地图。如图所示，与LOAM地图相比，LIO-SAM地图保留了更精细的环境结构细节。这是因为LIO-SAM能够在$SO(3)$中精确配准每一个激光雷达帧，即使当机器人经历快速旋转时。

B.步行数据集

该测试旨在评估我们的方法在系统经历强烈的平移和旋转时的性能。遇到的最大平移和旋转速度分别是$1.8m/s$和$213.9°/s$。在数据收集过程中，用户手持图2(a)所示的传感器套件，快速穿过麻省理工学院校园(图4(a))。

在这个测试中，如图4(b)所示，当遇到强烈的旋转时，LOAM的地图在多个位置发散。

在这次测试中，LIOM的表现优于LOAM。然而，它的地图，如图4©所示，仍然在不同的位置轻微发散，由无数模糊的结构组成。

因为LIOM被设计为处理所有传感器测量，所以它只以0.56倍的实时速率运行，而其他方法是实时运行的。最后，LIO-SAM优于这两种方法，并产生了一个与谷歌地球图像一致的地图。

C.校园数据集

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

为了收集这个数据集，用户使用手持设备在MIT校园周围走动，然后返回到相同的位置。因为在建图区域有无数的建筑和树木，GPS接收很少可用，而且大部分时间都不准确。在过滤掉不一致的GPS测量后，GPS可用的区域在图5(a)中显示为绿色部分。这些地区相当于少数没有被建筑物或树木包围的区域。

LOAM、LIO-odom、LIO-GPS和LIO-SAM的估计轨迹如上图所示。由于无法正确初始化和产生有意义的结果，所以没有显示LIOM的结果。如图所示，与所有其他方法相比，LOAM的轨迹漂移非常明显。没有GPS数据的校正，LIO-odom的轨迹在地图右下角开始明显漂移。在GPS数据的帮助下，当GPS可用时LIO-GPS可以矫正漂移。然而，GPS数据在数据集的后半部分不可用。结果，当机器人由于漂移而返回起始位置时，LIO-GPS无法完成闭环。另一方面，LIO-SAM可以通过在因子图中增加闭环因子来消除漂移。LIO-SAM的地图与谷歌地球的图像非常吻合，如图5(b)所示。

当机器人返回起点时，所有方法的相对平移误差如表二所示。

D.公园数据集

在这次测试中，我们将传感器安装在UGV上，并驾驶车辆沿着森林覆盖的徒步旅行路线行驶。行驶40分钟后，机器人回到初始位置。UGV在三个路面上行驶：柏油路、被草覆盖的地面和被泥土覆盖的小路。由于没有悬架，机器人在非沥青路面上行驶时会受到低振幅但高频率的振动。

为了模拟一个具有挑战性的建图场景，我们仅在机器人处于开阔区域时使用GPS测量，这在图6(a)中用绿色线段表示。

这种映建图场景代表了一种任务，在这种任务中，机器人必须建图多个拒绝GPS的区域，并周期性地返回GPS可用的区域，以校正漂移。

与之前测试的结果类似，由于没有绝对校正数据，LOAM、LIOM和LIOM-odom都存在明显的漂移。此外，LIOM仅以0.67倍的实时速率运行，而其他方法可以实时运行。尽管LIO-GPS和LIO-SAM的轨迹在水平面上重合，但它们的相对平移误差是不同的(表二)。

由于没有可靠的绝对高度测量数据，LIO-GPS在返回机器人初始位置时，会受到系统漂移的影响，无法完成闭环。LIO-SAM没有这样的问题，因为它利用闭环因子来消除漂移。

E.阿姆斯特丹数据集

最后，我们将传感器组件安装在船上，在阿姆斯特丹的运河上巡航了3个小时。尽管在这次测试中，传感器的移动相对平稳，但由于几个原因，绘制运河地图仍然是一项挑战。运河上的许多桥梁构成了退化的场景，因为当船在它们下面时，很少有有用的特征，类似于穿过一条长而无特征的走廊。平面特征的数量也显著减少，因为地面不存在。当太阳光直射在传感器视场中时，我们从激光雷达中获得许多错误观测，这种情况在数据收集过程中20%的时间发生。由于头顶上有桥梁和城市建筑，我们也只能间歇性的获得GPS信号。

由于这些挑战，LOAM、LIOM和LIOM-odom未能在该测试中产生有意义的结果。类似于在公园数据集中遇到的问题，由于高度漂移，LIO-GPS在返回机器人的初始位置时无法完成闭环，这进一步推动了我们在LIO-SAM中使用闭环。

F.基准数据集

由于完整的GPS覆盖仅在公园数据集中可用，我们显示了GPS测量历史的均方根误差(RMSE)结果，它被视为真值。这个RMSE误差没有考虑沿$z$轴的误差。如表三所示，相对于GPS真值，LIO-GPS和LIO-SAM获得了类似的RMSE误差。

请注意，我们可以通过让这两种方法完全访问所有的GPS测量，将它们的误差进一步减小至少一个数量级。然而，在许多建图场景中，并不总是可以使用完整的GPS访问。我们的目标是设计一个能够在各种挑战性环境中运行的鲁棒系统。

表四显示了三种竞争方法在所有五个数据集上配准一个激光雷达帧的平均运行时间。

在所有测试中，LOAM和LIO-SAM被迫实时运行。换句话说，当激光雷达旋转速率为$10Hz$时，如果运行时间超过$100ms$，则会丢弃一些激光雷达帧。LIOM有无限的时间来处理每个激光雷达帧。如图所示，与其他两种方法相比，LIO-SAM使用更少的运行时间，这使得它更适合在低功耗的嵌入式系统中部署。

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

我们注意到，LIO-SAM算法的运行时间受特征图密度的影响更大，而不受因子图中节点数和因子的影响。例如，公园数据集是在要素丰富的环境中收集的，在这种环境中，植被会产生大量的特征，而阿姆斯特丹数据集会产生更稀疏的特征地图。虽然公园数据集的因子图由4573个节点和9365个因子组成，而阿姆斯特丹数据集的因子图有23304个节点和49617个因子。尽管如此，相比起公园数据集，LIO-SAM在阿姆斯特丹数据集中运行使用的时间更少。

结论和讨论

我们已经提出了LIO-SAM，一个通过平滑和建图进行紧耦合激光雷达惯性里程计的框架，用于在复杂环境中执行实时状态估计和建图。通过在因子图上建立激光雷达-惯性里程计，LIO-SAM特别适用于多传感器融合。额外的传感器测量可以作为新的因子纳入框架。额外的提供绝对测量的传感器，如GPS、指南针或高度计，可用于消除长期积累的雷达惯性里程计漂移，或在特征缺乏的环境中。位置识别也可以很容易地结合到系统中。为了提高系统的实时性能，我们提出了一种滑动窗口方法，将旧的激光雷达帧边缘化以进行扫描匹配。关键帧被选择性地添加到因子图中，并且当生成激光雷达里程计和闭环因子时，新的关键帧仅被配准到固定大小的一组子关键帧中。这种局部尺度而非全局尺度的扫描匹配有助于LIO-SAM框架的实时性能。所提出的方法在不同环境下的三个平台上收集的数据集上进行了全面评估。结果表明，与LOAM和LIOM相比，LIO-SAM可以达到相似或更好的精度。未来的工作包括在无人驾驶飞行器上测试提出的系统。