(Lidar SLAM论文)LeGO-LOAM

摘要

我们提出了一种轻量级和地面优化的里程计和建图方法，LeGO-LOAM，用于地面无人车的实时六自由度位姿估计。LeGO-LOAM是轻量级的，因为它可以在低功耗嵌入式系统上实现实时位姿估计。LeGO-LOAM是地面优化的，因为它在其分割和优化步骤中利用了地面的存在。我们首先应用点云分割来滤除噪声，然后进行特征提取来获得独特的平面和边缘特征。然后，一种两步L-M优化方法使用平面和边缘特征来求解连续扫描之间的六自由度变换的不同分量。我们使用从地面车辆的可变地形环境中收集的数据集，比较了LeGO-LOAM和最先进的方法LOAM的性能，结果表明LeGO-LOAM在降低计算代价的情况下达到了相似或更好的精度。我们还将LeGO-LOAM集成到一个SLAM框架中，以消除漂移引起的位姿估计误差，该框架使用KITTI数据集进行了测试。

引言

在智能机器人的能力中，地图构建和状态估计是最基本的先决条件之一。用基于视觉和基于激光雷达的方法实现实时6自由度同时定位和建图(SLAM)已经付出了巨大的努力。虽然基于视觉的方法在闭环检测中有优势，但如果用作唯一的导航传感器，它们对光照和视角变化的敏感性可能会使这种能力变得不可靠。另一方面，基于激光雷达的方法即使在夜间也能发挥作用，许多三维激光雷达的高分辨率允许在远距离捕捉环境的细微细节。因此，本文重点研究利用三维激光雷达支持实时状态估计和建图。

寻找两次激光雷达扫描之间变换的典型方法是迭代最近点法(ICP) [1]。通过逐点寻找对应关系，ICP迭代对齐两组点，直到满足停止标准。当扫描包含大量点时，ICP压可能会需要过高的计算成本。为了提高效率和准确性，人们提出了多种ICP的变体[2]。[3]引入了点-平面ICP的变体，它将点与局部平面相匹配。Generalized-ICP[4]提出了一种从两次扫描中匹配局部平面的方法。此外，几个ICP变体利用并行计算来提高效率[5]–[8]。

基于特征的匹配方法正在吸引更多的关注，因为它们通过从环境中提取代表性特征，需要更少的计算资源。这些特征应该适合于有效的匹配并保持视角不变性。许多检测器，例如点特征直方图(PFH) [9]和视点特征直方图(VFH) [10]已经被提出，用于使用简单有效的技术从点云中提取这些特征。在[11]中介绍了一种使用Kanade-Tomasi角点检测器从点云中提取通用特征的方法。在[12]中讨论了从密集点云中提取线和平面特征的框架。

还提出了许多使用特征进行点云配准的算法。[13]和[14]提出了一种在局部聚类中执行点的曲率计的关键点选择算法。选定的关键点用于执行匹配和位置识别。通过将点云投影到距离图像上并分析深度值的二阶导数，[15]从具有高曲率的点中选择特征用于匹配和位置识别。假设环境由平面组成，在[16]中提出了一种基于平面的配准算法。室外环境，例如森林，可能会限制这种方法的应用。在[17]中提出了一种特别为Velodyne激光雷达设计的collar line segments(CLS)方法。CLS使用来自扫描的两个连续“射线”的点随机生成线。因此，生成两个线云并用于配准。然而，这种方法遭受了由随机生成线带来的挑战。文献[18]提出了一种基于分割的配准算法。SegMatch首先将分割应用于点云。然后基于特征值和形状直方图为每个片段计算特征向量。随机森林算法用于匹配两次扫描的片段。虽然这种方法可以用于在线位姿估计，但它只能提供大于1Hz左右频率的的定位更新。

文献[19]和[20]提出了一种低漂移的实时激光雷达里成绩和建图方法(LOAM)。(LOAM)使用点特征到边缘/平面的扫描匹配，以找到扫描之间的对应关系。通过计算一个点在其局部区域的粗糙度来提取特征。具有高粗糙度值的点被选为边缘特征。类似，粗糙度值低的点被选取为平面特征。实时的性能是通过将估计问题新颖地分成两个独立的算法来实现的。一种算法以高频运行，以较低的精度来估计传感器速度。另一种算法运行频率低但返回高精度的运动估计结果。这两个估计融合在一起，以产生高频和高精度的单个运动估计。LOAM的最终精度是在KITTI里程计评分榜上仅使用激光雷达估算方法获得的最佳结果[21]。

在这项工作中，我们以一种易于在小规模嵌入式系统上有效实现的方式，为装备有3D激光雷达的地面车辆提供可靠、实时的六自由度位姿估计。由于几个原因，这样的任务并不是微不足道的。由于尺寸有限，许多无人地面车辆没有悬架或强大的计算单元。在多变地形上行驶的小型无人驾驶车辆经常会遇到非平滑运动，因此，采集的数据往往会存在畸变。在覆盖范围重叠有限的大运动情况下，在两次连续扫描之间也很难找到可靠的特征对应。除此之外，从三维激光雷达接收的大量点对使用有限的机载计算资源进行实时处理提出了挑战。
当我们为这样的任务实现LOAM时，当UGV进行可以获得稳定特征的平滑运动并且有足够的计算资源支持时，我们可以获得低漂移的运动估计。然而，当计算资源有限时，LOAM的性能会下降。由于需要计算密集三维点云中每个点的粗糙度，轻量级嵌入式系统中特征提取的更新频率不能总是跟上传感器的更新频率。UGV在存在噪声环境中的运行也给LOAM带来了挑战，由于在小型UGV上激光雷达的安装位置通常靠近地面，来自地面的传感器噪声可能是一个持续的噪声。例如，从草地返回的距离测量可能导致高粗糙度值。因此，可能会从这些点提取不可靠的边缘特征。类似地，也可以从树叶返回的点中提取边缘或平面特征。这样的特征对于扫描匹配通常是不可靠的，因为在两次连续扫描中可能不会出现相同的草地或者树叶。使用这些特征可能导致不准确的配准和大的漂移。

因此，我们提出了一种轻量级和地面优化的LOAM(LeGO-LOAM)，用于在复杂多变的地形环境下对UGV进行位姿估计。LeGO-LOAM是轻量级的，因为实时位姿估计和建图可以在嵌入式系统上实现。执行点云分割是为了在地面分离后丢弃可能代表不可靠特征的点。LeGO-LOAM也是地面优化的，因为我们引入了一个两步优化的位姿估计。从地面提取的平面特征用于在第一步中获得$\left[t_z,{\theta }_{\text{roll }},{\theta }_{\text{pitch }}\right]$。在第二步中，剩余的变换$\left[t_x,t_y,{\theta }_{yaw}\right]$ 通过匹配从分割点云提取的边缘特征来获得。我们还集成了执行闭环检测的能力，以校正运动估计漂移。

系统硬件

略

轻量级的雷达里程计与建图

A. 系统概述

该系统接收来自三维激光雷达的输入，并输出6自由度位姿估计。整个系统分为五个模块。第一个是分割模块，它将单个扫描的点云投影到距离图像上进行分割。分割后的点云然后被发送到特征提取模块。然后，激光雷达里程计模块使用从先前的模块中提取到的特征来找到连续两帧扫描之间的相对位姿变换。这些特征在雷达建图模块中进一步处理，雷达建图模块将它们注册到全局点云地图中。最后，变换集成模块融合来自雷达里程计和雷达建图的位姿估计结果并输出最终的位姿估计。相对于[19]和[20]的原始、通用的LOAM框架，本文所提出的系统致力于提高地面车辆的效率和精度。这些模块的细节介绍如下。

图1

LeGO-LOAM 的硬件和系统概述

B. 分割模块

令 P t = { p 1 , p 2 , … , p n } P_{t}=\left\{p_{1}, p_{2}, \ldots, p_{n}\right\} Pt​={p1​,p2​,…,pn​}是在$t$时刻获取的点云，其中$p_i$是$P_t$中的一个点。$P_t$首先投影到一幅距离图像上。投影距离图像的分辨率是1800×16，因为VLP-16的水平和垂直角分辨率分别为 $0.2°$和$2°$，每个 $P_t$中的有效点$p_i$由距离图像中唯一的像素来表示。距离值$r_i$表示从相应点$p_i$到传感器的欧几里得距离。由于斜坡地形在任何环境下都很常见，我们不假设地面是平坦的。在分割之前，对距离图像进行列式评估（可视为地平面估计[22])，用于地面点提取。在这个过程之后，可能代表地面的点被标记为地面点，而不用于分割。

距离图像就是深度图

然后，将基于图像的分割方法[23]应用于距离图像，以将点分组为许多簇（聚类）。来自同一簇的点被分配一个唯一的标签。请注意，接地点是一种特殊类型的簇。对点云进行分割可以提高处理效率和特征提取精度。假设机器人在嘈杂的环境中工作，小物体，例如树叶，可能会形成琐碎和不可靠的特征，因为在两次连续扫描中不可能看到相同的树叶。为了使用分割的点云执行快速可靠的特征提取，我们忽略了少于30个点的簇。分割前后点云的可视化如下图所示。

图2

噪声环境中scan的特征提取过程。原始点云如（a）所示。在（b）中，红色点标记为接地点。其余的点是分割后保留的点。在（c）中，蓝色和黄色点表示$F_e$和$F_p$中的边缘和平面特征。在（d)中，绿色和粉色点分别表示${\mathbb{F}}_e$和中的边${\mathbb{F}}_p$ 缘和平面特征。

原始点云包括许多点，这些点是从可能产生不可靠特征的周围植被中获得的。

在这个过程之后，只有可能代表大物体的点(图2(b))例如树干和接地点被保留用于进一步处理。同时，只有这些点保存在距离图像中。我们还获取了每个点的3个属性:(1)它作为接地点或分割点的标签，(2)它在范围图像中的列和行索引，以及(3)它的范围值。这些属性将在以下模块中使用。

C.特征提取模块

特征提取过程类似于[20]中使用的方法。然而，我们不是从原始点云提取特征，而是从地面点和分割点提取特征。让我们从距离图像的同一行中选择一组连续的点$S$，各有一半的点在$p_i$的两侧。在本文中，我们将$|S|$ 设置为10。使用在分割期间计算的距离值，我们可以评估$S$ 中点$p_i$的粗糙度：
$$c=\frac{1}{|S|\cdot \Vert r_i\Vert }\Vert \sum _{j\in S,j\ne i}\left(r_j-r_i\right)\Vert \text{\hspace{1em}(1)}$$
为了从各个方向均匀地提取特征，我们将距离图像水平分成几个相等的子图像。然后，我们根据粗糙度值$c$对子图像每行中的点进行排序。类似于LOAM，我们使用距离阈值来区分不同类型的特征。我们称$c$大于$c_{th}$的点为边缘特征，$c$小于$c_{th}$的点为平面特征。从子图像的每一行中选择不属于地面的具有最大$c$值的边缘特征点 $n_{{\mathbb{F}}_e}$。以相同的方式选择具有最小$c$值的其他特征点$n_{{\mathbb{F}}_p}$，这些特征点可以被标记为地面点或分割点。让 ${\mathbb{F}}_e$ 和${\mathbb{F}}_p$ 为从所有子图像中获取的所有边缘和平面特征的集合。这些特征如图2(d)所示。然后，我们从子图像的每一行中提取不属于地面的具有最大$c$值的$n_{F_e}$个边缘特征。类似地，我们用最小$c$值从子图像的每一行中提取 $n_{F_p}$ 个平面特征，这些点必须是接地点。让我们用$F_e$ 和$F_p$来表示这个过程该子图像中获得的所有边缘和平面特征的集合。在这里，我们有$F_e\subset {\mathbb{F}}_e$ ，$F_p\subset {\mathbb{F}}_p$，图2（c）显示了输入和输出的特征。本文将 $360^{\circ }$范围图像分成6个子图像。每个子图像的分辨率为300×16。 $n_{F_e},n_{F_p},n_{{\mathbb{F}}_e}$ 和$n_{{\mathbb{F}}_p}$ 分别选择为2、4、40和80。

D. Lidar里程计模块

雷达里程计模块估计两次连续扫描之间的传感器运动。两次扫描之间的变换通过执行点到边缘和点到平面的扫描匹配来实现。换句话说，我们需要为在 $F_e^t$ 和$F_p^t$ 从前一次扫描的点${\mathbb{F}}_e^{t-1}$ 和${\mathbb{F}}_p^{t-1}$中找到相应的特征。为了简洁起见，找到这些对应关系的详细过程可以在[20]中找到。
但是，我们注意到可以进行一些更改来提高特征匹配的准确性和效率:

1） 标签匹配：

由于分割后在$F_e^t$ 和$F_p^t$ 中的每个特征都用其标签进行编码，因此我们只能从 ${\mathbb{F}}_e^{t-1}$ 和${\mathbb{F}}_p^{t-1}$中找到具有相同标签的对应关系。对于$F_p^t$中的平面特征，只有在${\mathbb{F}}_p^{t-1}$中标记为接地点的点被用于寻找对应的平面匹配。对于$F_e^t$ 中的边缘特征，其对应的边缘线只在 ${\mathbb{F}}_e^{t-1}$ 中找到。用这种方法找到对应关系有助于提高匹配精度。换句话说，在两次扫描之间更有可能找到同一物体的匹配或对应关系。这个过程也缩小了潜在的匹配选人的范围。

2） 两步L-M优化：

在[20]中，当前扫描的边缘和平面特征点与它们在前一次扫描中对应关系距离的一系列非线性表达式被组合成单个综合距离向量。应用L-M方法来寻找两次连续扫描之间的最小距离变换。

这里我们介绍一种两步L-M优化方法。最优变换$T$的寻找分两步进行:(1)通过匹配在$F_p^t$中的平面特征和它们在${\mathbb{F}}_p^{t-1}$中的对应关系来估计$\left[t_z,{\theta }_{\text{roll }},{\theta }_{\text{pitch }}\right]$ ，(2)然后使用在$F_e^t$中的边缘特征和它们在${\mathbb{F}}_e^{t-1}$中的对应关系来估计剩余的 $\left[t_x,t_y,{\theta }_{yaw}\right]$ ，同时使用$\left[t_z,{\theta }_{\text{roll }},{\theta }_{\text{pitch }}\right]$作为约束。应该注意的是，虽然$\left[t_x,t_y,{\theta }_{yaw}\right]$也可以从第一个优化步骤中获得，但它们不太精确。最后，通过融合$\left[t_z,{\theta }_{\text{roll }},{\theta }_{\text{pitch }}\right]$和 $\left[t_x,t_y,{\theta }_{yaw}\right]$ 找到两次连续扫描之间的6D变换。通过使用所提出的两步优化方法，我们观察到，在计算时间减少约35%的情况下，可以实现类似的精度(III)。

表III

LeGO-LOAM 的迭代次数比较

E. Lidar建图模块

雷达建图模块将$\left\{{\mathbb{F}}_e^t,{\mathbb{F}}_p^t\right\}$中的特征与周围的局部点云地图${\bar{Q}}^{t-1}$进行匹配，以进一步细化位姿变换，但运行频率较低。然后在这里再次使用L-M方法来获得最终的变换。关于详细的匹配和优化程序，请读者参考[20]中的描述。
LeGO-LOAM的主要区别在于最终点云地图的存储方式。我们不保存单个点云地图，而是保存每个单独的特征集合 $\left\{{\mathbb{F}}_e^t,{\mathbb{F}}_p^t\right\}$。令 $M^{t-1}=\left\{\left\{{\mathbb{F}}_e^1,{\mathbb{F}}_p^1\right\},\dots ,\left\{{\mathbb{F}}_e^{t-1},{\mathbb{F}}_p^{t-1}\right\}\right\}$是保存所有之前特征集合的集合。$M^{t-1}$中的每个特征集合也与进行扫描时传感器的位姿相关联。那么可以通过两种方式从 $M^{t-1}$ 获得 ${\bar{Q}}^{t-1}$ 。
在第一种方法中，${\bar{Q}}^{t-1}$是通过选择传感器视野内的特征集获得的。简单来说，我们可以选择特征集对应的传感器位置在当前传感器位置100米以内的特征集。然后，将所选特征集转换并融合成单个局部地图 ${\bar{Q}}^{t-1}$。这种地图选择技术类似于[20]中使用的方法。

我们还可以将位姿图SLAM集成到LeGO-LOAM中。每个特征集的传感器位姿可以表示为姿态图中的一个节点。特征集 $\left\{{\mathbb{F}}_e^t,{\mathbb{F}}_p^t\right\}$可视为此节点的传感器测量。由于雷达建图模块的位姿估计漂移非常低，我们可以保证在短时间内没有漂移。这样， ${\bar{Q}}^{t-1}$可以通过选择最近的一组特征集来形成，即 ${\bar{Q}}^{t-1}=$ $\left\{\left\{{\mathbb{F}}_e^{t-k},{\mathbb{F}}_p^{t-k}\right\},\dots ,\left\{{\mathbb{F}}_e^{t-1},{\mathbb{F}}_p^{t-1}\right\}\right\}$，其中$k$定义了 ${\bar{Q}}^{t-1}$的大小。然后，可以使用L-M优化后获得的变换来添加新节点和 ${\bar{Q}}^{t-1}$中所选节点之间的空间约束。我们可以通过执行闭环检测来进一步消除该模块的漂移。在这种情况下，如果使用ICP在当前特征集和前一个特征集之间找到了匹配，则会添加新的约束。然后，通过将位姿图发送到优化系统(如[24])来更新传感器的估计姿态。请注意，只有实验四中使用这种技术来创建其周围的地图。

总结：

1. 对于lidar安装位置较低的场景，聚类剔除无用噪声
2. 分割地面点，地面点做三个自由度优化，分割出剩余的点提取出特征点，匹配优化另外三个自由度，实现轻量级lidar里程计