---

本文参考：https://blog.csdn.net/weixin_37835423/article/details/111184679?spm=1001.2014.3001.5506

作者简介：作者Tixiao Shan在2018年发表过LeGO-LOAM，当时他还在史蒂文斯理工学院读博士，19年毕业之后去了MIT做助理研究员。这篇文章LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分，然后更详细地描述了LeGO-LOAM帧图匹配部分的设计动机和细节。

论文原文：论文原文链接

开源代码：开源代码链接、

演示视频：

摘要

我们提出一个框架用于通过SAM紧耦合雷达惯性里程计LIO-SAM，可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM是基于因子图构建雷达惯性里程计，可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入雷达惯性里程计系统中。通过imu预积分获得的运动估计可以用于点云的偏斜矫正和用于雷达里程计优化的初始值。获得的雷达里程计结果又可以反过来估计imu的偏置。为了确保在实时性上有更高的性能，我们在进行位姿优化时边缘化掉了一些老的雷达数据，而不是将雷达点云与整个地图进行匹配。Scan-matching 在局部范围内进行而不是在全局范围内进行，可以有效的提高系统的实时性，原因是因为选择性引入关键帧和有效的滑动窗口策略(只将新的关键帧与固定尺寸的多个先前关键帧匹配)。提出的方法已经在拥有多种场景多种范围的三个测试平台采集的数据集上进行了广泛的评估测试。

一、引言

（这一段主要对视觉与激光SLAM方法进行了简单对比）

对于一个可以反馈控制、避障、轨迹规划和其它应用的智能移动机器人，状态估计、定位和建图是基本的要求。使用基于视觉和基于雷达的传感技术，付出巨大的努力支持移动机器人进行6自由度状态估计的高实时性的SLAM技术中。基于视觉的方法一般是使用一个单目相机或立体相机，通过连续采集的照片三角化特征来确定相机的运动。虽然基于视觉的方法非常适合于位置识别，但对初始化、光照、范围等条件特别敏感，以致于单独使用它们去支持一个自动导航系统是不可靠的。另一方面，基于雷达的方法对于光照的改变是没有影响的。特别是最近长距离高分辨率的3D激光雷达(像Velodyne VLS-128和Ouster OS1-128)已经出现了，使得雷达变得更加适合于直接在3D空间获取精细的环境信息。因此，这篇论文集中在基于雷达的状态估计和建图方法。

这一段主要介绍了LOAM方法存在的一些限制

许多的基于激光雷达的状态估计和建图方法在最近20年被提出来．其中LOAM(Lidar Odometry And Mapping)方法对于小漂移实时状态估计和建图是最被广泛使用的。LOAM使用一个雷达和一个imu实现了最好的性能，自从它发布在KITTI odometry benchmark site上，就一直排在最上面。虽然LOAM算法很成功，但依然存在一些限制。首先是它保存地图在一个全局体素地图中，其次它没法进行回环检测，也没法融入其它的像GPS这样的绝对测量数据进行位姿矫正。而且LOAM也存在在大场景测试中偏移问题，原因是因为它核心还是一个基于scan-matching的方法。

这一段介绍本文提出的LIO-SAM方法，及主要贡献

在这篇论文中，我们提出了一个通过SAM紧耦合雷达惯性里程计框架LIO-SAM，用于解决上述的问题。我们假设了一个非线性运动模型用于点云的偏斜矫正，使用原始IMU测量值估计激光雷达扫描期间的传感器运动。该运动估计除了用于点云的偏斜矫正而且还可以作为雷达里程计优化过程中的初始位姿估计。获得的雷达里程计结果反过来被用于估计因子图中imu的偏置。通过引入因子图用于机器人的轨迹估计，我们可以进行有效的基于雷达和imu作为测量值的传感器融合，比方说引入位置识别，引入GPS位置信息和指南针的朝向信息等绝对测量值。这些来自不同数据源的多种因子集合将会在因子图中共同优化。此外，我们在进行位姿优化时，边缘化掉老的雷达帧，而不是像LOAM那样将雷达帧同整个全局点云进行匹配。Scan-matching是在局部范围而不是全局范围内执行，明显的提升了系统的实时性，因为采取了关键帧的选择引入和有效的滑动窗口方法(只将新的关键帧与固定尺寸的多个先前关键帧匹配)。我们工作的主要贡献可以被总结如下：

• 一个基于因子图的紧耦合的雷达惯性里程计框架，适合多传感器融合和全局优化；
• 一个有效的基于局部滑窗的scan-matching方法，其通过匹配选择的关键帧和估计数量的先前关键帧策略，确保了系统的实时性；
• 提出的框架已在多范围、多设备和多种环境下进行了广泛的测试验证。

二、相关工作

雷达里程计典型实现是通过使用scan-matching方法找到两个相邻帧之间的相对转换关系，这样的方法有ICP、GICP等。除了匹配完整点云，基于特征匹配的方法由于其计算的有效性已经变得流行起来。例如，在[5]中提出了一个基于平面特征匹配的雷达里程计。假设运行场景在结构化的环境中，其从点云中提取平面特征，并使用最小二乘法对特征进行匹配。[6]是一个基于线特征的雷达里程计，其是从原始点云里面随机分割线特征，然后再进行匹配。然而由于现代3D激光雷达使用旋转机械件和设备的移动，导致点云经常会有偏斜。因此，仅仅使用激光雷达进行位姿估计是不理想的，原因是因为使用偏斜的点云或特征进行匹配必将会造成大的漂移。

因此，雷达通常是与其它传感器(例如IMU、GPS)融合使用来进行状态估计和建图。这样一种利用传感器融合思路的方法可以分为两类:松耦合融合和紧耦合融合。例如LOAM[1]算法，其使用IMU进行点云的偏斜矫正和为扫描匹配提供运动先验，然而，IMU并没有引入到算法的优化处理过程中，因此LOAM可以被归类为松耦合方法。另一个轻量的、基于地面优化的雷达里程计和建图算法LeGO-LOAM[7]被用于地面车辆的建图任务，该算法融合IMU测量的方法同LOAM是一致的。一种更加流行的松耦合方法是使用EKF，例如，[9]-[13]在优化阶段使用EKF集成了来自激光雷达、IMU和选择性GPS的测量数据，用于机器人状态估计。

紧耦合系统通常能够提供更高的精度，并且是当前的主要研究方向。在[15]方法中，预积分的IMU测量值被用于对点云进行偏斜矫正。[16]提出了一种以机器人为中心的雷达惯性里程计LINS，其用一种紧耦合的方式使用ESKF算法对机器人的状态进行矫正，由于缺乏其它可用于状态估计的传感器，该算法在经过长时间导航后会有漂移问题。[17]提出了一个紧耦合的雷达惯性里程计框架LIOM(雷达惯性里程计建图)，其共同优化雷达和IMU的测试值，实现了比LOAM更好的精度。因为LIOM的设计是处理所有的传感器测量，因此，它不能实时运行——在我们的测试中，它以大约0.6×的实时速度运行。

三、基于SAM的雷达惯性里程计

A.系统概述

我们首先定义了我们在整个论文中使用的坐标系和符号。我们定义W为世界坐标系，B为机器人坐标系。为了方便，我们也假设IMU坐标系和机器人本体坐标系是一致的。机器人的状态x可按下面定义:


我们提出的系统概述如图1所示。系统接收来自3D雷达、IMU和可选择的GPS等传感器的数据。我们争取使用这些传感器数据估计出机器人的状态和轨迹。这个状态估计问题可以被构造为一个最大化后验问题(MAP)。我们使用一个因子图去刻画这个问题，因为它是最适合进行贝叶斯网络的推断。由于高斯模型假设，我们的MAP推断问题与求解非线性最小二乘问题是等价的。注意，我们的系统可以不失一般性地引入其它传感器的测量数据，例如高度计的海拔数据、指南针的朝向数据。

对于因子图的构造，我们对一个变量引入四种因子。该表示在指定时刻机器人状态的变量对应因子图中的节点。四种类型的因子分别是：(a) IMU预积分因子，(b) 雷达里程计因子，©GPS因子和(d)回环因子。当机器人的位姿改变超过人为设定的阈值，则添加新的机器人状态节点x到因子图中。一旦插入了新的节点，因子图就会使用iSAM2进行优化。下面部分将介绍产生这些因子的过程。

B. IMU预积分因子

来自于IMU的角速度和加速度测量值定义如等式(2)和(3):


我们现在可以使用IMU测量值进行机器人运动的推断。机器人在时刻 t + Δt 的速度、位置和姿态计算方法如下：

式中Rt为从坐标系B到坐标系W的旋转，并且我们假设基准坐标系B的角速度和加速度在上述积分过程中是不变的。

由于篇幅的限制，读者们可以阅读[20]获得更多的关于公式(7)~(9)的推导。除了有效性外，应用IMU预积分也自然的给我们一种类型的因子图约束-IMU预积分因子。IMU的偏置是同雷达里程计因子一起在因子图中共同优化。

C. 雷达里程计因子

这段介绍了基于边和面的特征提取

当接收到一个新的雷达扫描数据，我们首先通过评估一个局部区域中点的凹凸程度来提取边和面特征点，当点的凹凸程度较大，则将这些点分类为边特征点。类似的，当凹凸程度较小时，这些点将被分类为面特征点。我们将从在时刻i获取的雷达点云中提取的边特征和面特征的集合表示为F ie,F ip。在时刻i ii提取的所有特征点构成了雷达帧F i = [ F i e , F i p ]注意雷达帧F是表达在B坐标系下．关于特征点提取的详细描述可查看[1][7]。

这一段介绍了本文添加关键帧的方法

如果将每一个雷达帧进行计算并添加因子到因子图中进行优化是很难计算的，因此我们使用关键帧策略，该策略已在视觉SLAM领域广泛使用。当机器人的位姿相对于先前状态x i 的改变超过用户设定的阈值，则选择对应的雷达帧Fi+1作为新的关键帧，新的关键帧与因子图中的一个新的机器人状态节点x i+1相关联的，相邻两个关键帧之间的雷达帧被丢弃。这样添加关键帧的方法不仅实现了地图密度和内存消耗的平衡，而且帮助维护了一个相对稀疏的适用于实时非线性优化的因子图。在我们的工作中，用于添加关键帧的位置和旋转改变阈值参数分别设置为1米和10度。

下面介绍雷达里程计因子的产生步骤

假设我们在因子图中添加一个新的状态节点xi+1其相关的雷达关键帧为Fi+1则产生一个雷达里程计因子的步骤如下：

1)体素地图的子关键帧: 我们使用滑窗的方法产生一个点云地图，该地图包含固定数量的最近的雷达帧。而不是直接通过两个相邻的雷达帧来获得相对转换关系，我们提取了n个最近的关键帧，我们称之为子关键帧，用于估计相对转换关系。子关键帧的集合{Fi−n , . . . , Fi }然后使用和他们相关的转换关系{Ti−n , . . . , Ti }将他们转换到世界坐标系下。转换过的子关键帧融合到一起构成一个体素地图Mi（体素地图是在世界坐标系下的）。因为我们在先前特征提取步骤提取了两种类型的特征，因此 Mi 是由两个子体素地图组成，分别是边特征体素地图Mie和面特征体素地图Mip，雷达帧和体素地图是按下面彼此相关的：

使用降采样去删除重复特征点

2)扫描匹配：

论文[1]：J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and
Mapping,” Autonomous Robots, vol. 41(2): 401-416, 2017.

​3) 相对转换：一个特征和它相关的边或者平面之间的距离可按下式计算：

D. GPS因子

虽然我们可以只利用IMU预积分因子和雷达里程计因子获得可靠的状态估计和建图，但系统还是会在长时间导航任务中遇到漂移问题。为了解决这个问题，我们可以引入能够提供绝对测量的传感器来消除漂移。这样的传感器包括高度计、指南针和GPS。在这为了说明目的，我们讨论GPS，因为它在真实世界的导航系统中被广泛的使用。

当我们接收到GPS测量值时，我们首先使用[21]中提出的方法将这些值转换到局部笛卡尔坐标系。当添加一个新的节点到因子图中时，我们然后将一个新的GPS因子关联到这个节点。如果GPS信号不是同雷达帧时间同步的，我们基于雷达帧的时间戳线性插值出GPS测量值。

我们注意到当GPS数据可利用时，我们没有必要不断地添加GPS因子，因为雷达惯性里程计的漂移增长缓慢。在实际应用中，我们只当估计的位置协方差是比接收到的GPS位置协方差大的时候，我们才添加GPS因子。

E. 回环因子

由于因子图的利用，回环因子可以无缝引入我们提出的系统中，不像LOAM和LIOM。为了说明的目的，我们介绍了和实现了一个简单有效的基于欧拉距离的回环检测方法。我们也注意到我们提出的框架是兼容其它回环检测方法的，例如[22]和[23]，其产生点云描述子并使用这些描述子进行位置识别。

这一段引出回环因子

添加回环因子的作用

在实际应用中，当GPS是可得到的唯一的绝对传感器，我们发现添加回环因子在矫正机器人的海拔漂移问题非常有效。这是因为GPS的高程测量非常不准确，在没有回环的情况下，我们的测试中出现的高度误差接近100m。

四. 实验

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


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

A. 旋转数据集

在这个测试中，我们的重点是评估当只有IMU预集成和激光雷达测速因子被添加到因子图中时，我们的框架的鲁棒性。旋转数据集由手持传感器套件并在静止不动时执行一系列积极的旋转操作的用户收集。本试验中遇到的最大转速为133.7◦/s。用结构填充的测试环境如图3(a)所示。从LOAM和LIO-SAM中获得的地图分别如图3(b)和©所示。因为LIOM使用了来自[25]的相同的初始化途径，所以它继承了与视觉-惯性SLAM相同的初始化敏感性，并且不能使用这个数据集正确地初始化。由于它未能产生有意义的结果，LIOM的地图没有显示出来。如图所示，与LOAM相比，LIO-SAM地图保留了更精细的环境结构细节。这是因为LIO-SAM能够在SO（3）中精确地注册每个激光雷达帧，即使机器人经历了快速旋转。

B.步行数据集

该测试旨在评估我们的方法在SE（3）中进行正向平移和旋转时的性能。在这个数据集中遇到的最大平移速度和旋转速度分别为1.8m/s和213.9◦/s。在数据收集过程中，用户持有如图2(a)所示的传感器套件。快速穿过麻省理工学院校园。（图4(a))。在本测试中，LOAM地图，如图4(b)所示。当遇到主动旋转时，会在多个位置发散。LIOM在此测试中优于LOAM。然而，其地图，如图4©所示。在不同的位置仍然略有不同，并由许多模糊的结构组成。因为LIOM被设计为处理所有的传感器测量值，所以它只在0.56×下实时运行，而其他方法则是实时运行的。最后，LIO-SAM优于这两种方法，并产生了一个与可用的谷歌地球图像一致的地图。

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未能正确初始化并产生有意义的结果，因此没有显示LIOM的结果。如图所示，与其他所有方法相比，LOAM的轨迹显著漂移。如果没有对GPS数据的校正，lio-odom的轨迹开始在地图的右下角出现明显的漂移。在GPS数据的帮助下，LIO-GPS可以在存在时纠正漂移。然而，在数据集的后面部分中没有GPS数据。因此，当由于漂移导致机器人返回到起始位置时，LIO-GPS无法关闭循环。另一方面，LIO-SAM可以通过向图中添加环路闭合因子来消除漂移。LIO-SAM的地图与谷歌地图对齐一致，如图5(b)所示。当机器人返回起点时，所有方法的相对平移误差如表二所示。

D. 公园数据集

在这个测试中，我们将传感器安装在UGV上，并驾驶车辆沿着森林徒步旅行路线行驶。机器人在驾驶40分钟后返回到初始位置。UGV在三个路面上驱动：沥青、草地覆盖的地面和泥土覆盖的小径。由于缺乏悬挂系统，该机器人在非沥青路面上行驶时，会经历低振幅但高频的振动。

为了模拟一个具有挑战性的映射场景，我们只在机器人处于开阔区域时使用GPS测量，如图6(a)中的绿色段表示。这种映射场景代表了一项任务，其中机器人必须映射多个GPS拒绝的区域，并定期返回具有GPS可用性的区域，以纠正漂移。

与之前的测试结果相似，LOAM、LIOM和LIO-odom存在显著的漂移，因为没有绝对的校正数据。此外，LIOM只在0.67×下实时运行，而其他方法则实时运行。虽然LIO-GPS和LIO-SAM的运动轨迹在水平平面上一致，它们的相对翻译误差是不同的（表二）。由于没有可靠的绝对高程测量值，LIO-GPS存在高度漂移问题，在返回机器人的初始位置时无法关闭回路。LIO-SAM没有这样的问题，因为它利用环路闭合因子来消除漂移。

E. 阿姆斯特丹数据集

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

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

F. 基准测试结果

由于完整的GPS覆盖仅在Park数据集中可用，我们将GPS测量历史的均方根误差结果视为地面真值。这个RMSE误差没有考虑到沿z轴的误差。正如图表3所示，LIO-GPS和LIO-SAM对GPS地面真值实现类似的RMSE误差。请注意，我们可以通过完全访问这两种方法的所有GPS测量，使它们完全减少至少一个数量级。然而，在许多映射设置中并不总是可用。我们的目的是设计一个可以在各种具有挑战性的环境中运行的健壮的系统。

在所有五个数据集上注册一个激光雷达帧的平均运行时如表四所示。在所有的测试过程中，LOAM和LIO-SAM被迫实时运行。换句话说，当激光雷达的旋转速率为10Hz时，如果运行时需要超过100ms，那么一些激光雷达帧就会被丢弃。LIOM有无限的时间来处理每个激光雷达帧。如图中所示，LIO-SAM使用了明显不少于其他两种方法的运行时间，这使得它更适合部署在低功耗嵌入式系统上。

我们还对LIO-SAM进行压力测试，比实时快。最大数据回放速度被记录和展示在表四的最后一列中当LIO-SAM也实现了类似的性能与当数据回放速度为1×实时性相比。如图所示，LIO-SAM能够比实时处理数据快到13×。

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

五. 总结

我们提出了一个基于SAM的紧耦合雷达惯性里程计框架LIO-SAM，为了实现在复杂环境中实时的状态估计和建图。通过在因子图上构造雷达惯性里程计，LIO-SAM是特别适合于多传感器的融合。除了传感器的测量可以轻松的作为新的因子添加到框架中。提供绝对测量值的传感器(高度计、指南针、GPS)可以被使用去消除长期运行引入的漂移，特别是在特征缺少的环境中。位置识别也可以轻松的融入系统。为了提高系统的实时性，我们提出了一种滑窗方法，我们在扫描匹配过程中，边缘化掉老的雷达帧。关键帧是被选择性的添加到因子图中，并且当雷达里程计和回环因子产生时，新的关键帧只会同固定数量的子关键帧进行匹配。这种扫描匹配在局部范围而不是全局范围的策略，使得LIO-SAM框架能够实时运行。我们提出的方法是在三个平台的数据集上全面的进行评估过，结果显示当同LOAM和LIOM比较时，LIO-SAM可以实现更高的精度。