论文笔记—LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

论文链接
代码链接

文章摘要

~~~~ ~~~ 本文提出了一种用于地面车辆的实时六自由度姿态估计的轻型且经地面优化的激光雷达里程计和建图方法LeGO-LOAM。LeGO-LOAM重量轻，因为它可以在低功耗嵌入式系统上实现实时姿势估计。LeGOLOAM经过地面优化，因为它在分割和优化步骤中充分利用了地平面的存在。本文首先应用点云分割来滤除噪声，然后进行特征提取以获得独特的平面和边缘特征。然后，两步Levenberg-Marquardt优化方法使用平面和边缘特征来解决连续扫描中六自由度变换的不同分量。

导语

~~~~ ~~~ 在智能机器人的功能中，地图构建和状态估计是最基本的先决条件。致力于通过基于视觉和基于激光雷达的方法实现实时6自由度同时定位和制图（SLAM）。尽管基于视觉的方法在闭环检测中具有优势，但是如果用作唯一的导航传感器，它们对照明和视点变化的敏感性可能会使这种功能不可靠。另一方面，基于激光雷达的方法甚至可以在晚上使用，并且许多3D激光雷达的高分辨率都可以在较宽的光圈范围内捕获远距离环境的精细细节。因此，本文着重于使用3D激光雷达来支持实时状态估计和映射。
因此，本文提出了一种轻巧且经地面优化的LOAM（LeGO-LOAM），用于在地形复杂的复杂环境中对UGV进行姿态估计。LeGO-LOAM是轻量级的，因为可以在嵌入式系统上实现实时姿态估计和建图。执行点云分割以丢弃可能代表地面分离后不可靠特征的点。由于引入了两步优化的姿势估计，因此LeGO-LOAM也进行了地面优化。在第一步中，从地面提取的平面特征用于获得[ t z ， θ r o l l ， θ p i t c h t_z，θ_{roll}，θ_{pitch} tz，θroll，θpitch]。在第二步中，其余的变换[ t x ， t y ， θ y a w t_x，t_y，θ_{yaw} tx，ty，θyaw]通过匹配从分段点云中提取的边缘特征来获得。此外本文还集成了执行闭环以纠正运动估计漂移的功能。

轻型LOAM

A.系统概述

~~~~ ~~~ 上述所提出框架的概述如图１所示。系统从3D激光雷达接收输入，并输出6 DOF（自由度）姿态估计。整个系统分为五个模块。第一种是分割，它会进行一次扫描的点云并将其投影到范围图像上进行分割。分割的点云然后被送到特征提取模块。然后，激光雷达里程计使用从先前模块中提取的特征来查找与连续扫描的位姿变换。这些特征会在激光建图中被进一步处理，将其与全局点云图进行配准。最后，变换集成模块将来自激光雷达里程计和激光建图的的位姿估计结果融合在一起，并输出最终的姿态估计。

Ｂ. 分割

~~~~ ~~~ 设 P t = { p 1 ， p 2 ， . . . ， p n } P_t =\{p_1，p_2，...，p_n\} Pt={p1，p2，...，pn} 是在时间t获取的点云，其中 p i 是 P t p_i是P_t pi是Pt中的一个点。 P t P_t Pt首先投影到范围图像上。投影范围图像的分辨率为1800×16。现在， P t P_t Pt中的每个有效点 p i p_i pi可以由范围图像中的唯一像素表示。与 p i p_i pi相关联的范围值 r i r_i ri表示对应点 p i p_i pi到传感器的欧几里德距离。由于倾斜地形在许多环境中是常见的，因此我们不认为地面是平的。范围图像的列优先评估可以被视为是对地平面估计，可以用作在分割之前对地面点的提取。在此过程之后，可以表示地面的点被标记为接地点并且不用于分割。
　　然后，将基于图像的分割方法应用于范围图像，以将大量点分成许多集簇。将来自同一集簇的点分配以唯一的标签。为了使用分割点云执行快速且可靠的特征提取，本文省略了少于30个点的集簇。在图2中示出了分割前和之后的点云的可视化。
　　在此过程之后，仅提供可以代表大物体的点，例如树干和地面点以进行进一步处理。同时，只有这些点保存在范围图像中。此外还可以获取每个点的三个属性：（1）其标签作为地面点或分段点；（2）其在范围图像中的列和行索引；以及（3）其范围值。这些属性将在以下模块中使用。

C.特征提取

~~~~ ~~~ 本文从地面点和分割点中进行提取特征。设S是范围图像中来自同一行的连续点 p i p_i pi的集合，S中的一半点位于 p i p_i pi的两侧。使用在分段期间计算的范围值，我们可以评估S中 p i p_i pi点的粗糙度，记为c。

　　为了均匀地从所有方向提取特征，本文将范围图像水平地分成几个相等的子图像。然后我们基于其粗糙度值c对子图像的每一行中的点进行排序。与SLAM类似，我们使用阈值 c t h c_{th} cth来区分不同类型的特征。称粗糙程度c大于阈值 c t h c_{th} cth的点为边缘特征，粗糙程度c小于阈值 c t h c_{th} cth的点为平面特征。此后，具有最大粗糙程度c的 n F e n_{\mathbb{F}_e} nFe边缘特征不属于地面，可以在子图像中的每一行进行选取。具有最小粗糙程度c的 n F p n_{\mathbb{F}_p} nFp平面特征可以将其标记为平面点或者分段点，以相同的方式进行选择。设 F e \mathbb{F}_e Fe和 F p \mathbb{F}_p Fp是所有子图像中的边缘特征和平面特征集合。之后我们提取具有最大粗糙程度c的 n F e n_{F_e} nFe边缘特征,其不属于地面点且来自子图像的每一行。同样地，我们提取具有最小粗糙程度c的 n F p n_{F_p} nFp边缘特征,其一定属于地面点且来自子图像的每一行。设 F e F_e Fe和 F p F_p Fp是在这个过程中得到的边缘和平面特征集合。这里我们有 F e ⊂ F e F_e\subset\mathbb{F}_e Fe⊂Fe以及 F p ⊂ F p F_p\subset\mathbb{F}_p Fp⊂Fp。本文将360°的范围图像分为6个分辨率为300×16的子图像， n F e n_{F_e} nFe, n F p n_{F_p} nFp, n F e n_{\mathbb{F}_e} nFe, n F p n_{\mathbb{F}_p} nFp分别被选择为2,4,40和80。

D.激光雷达里程计

~~~~ ~~~ 激光雷达里程计模块执行两个连续扫描之间的传感器运动估计。通过执行点对边和点对平面扫描匹配来找到两个扫描之间的运动变换。换句话说，就是需要对特征集合 F e t \mathbb{F}_e^t Fet和 F p t \mathbb{F}_p^t Fpt中的点找到在上一次扫描的特征集合 F e t − 1 \mathbb{F}_e^{t-1} Fet−1和 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1中的对应特征。以下的一些方法可以改善特征匹配准确性和效率：
　　1）标签匹配:由于在 F e t F_e^t Fet和 F p t F_p^t Fpt中的每个特征在分割之后都被编码贴上了标签，因此只需要在 F e t − 1 \mathbb{F}_e^{t-1} Fet−1和 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1找到有相同标签的对应特征。
　　2）两步L-M优化：本文介绍一种两步的L-M优化方法。通过两个步骤找到最优的位姿变换T。通过匹配 F p t \mathbb{F}_p^t Fpt和 F p t − 1 \mathbb{F}_p^{t-1} Fpt−1中对应的平面特征来估计位姿变换中的[ t z ， θ r o l l ， θ p i t c h t_z，θ_{roll}，θ_{pitch} tz，θroll，θpitch]。剩下的[ t x ， t y ， θ y a w t_x，t_y，θ_{yaw} tx，ty，θyaw]通过匹配 F e t \mathbb{F}_e^t Fet和 F e t − 1 \mathbb{F}_e^{t-1} Fet−1中对应的边缘特征来估计，同时使用[ t z ， θ r o l l ， θ p i t c h t_z，θ_{roll}，θ_{pitch} tz，θroll，θpitch]作为约束条件。

E.激光建图

~~~~ ~~~ 激光建图模块将{ F e t ， F p t F_e^t，F_p^t Fet，Fpt}中的特征与周围点云地图 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1相匹配来进一步改善位姿变换，但是只能在一个较低的频率下运行。之后在此处再次使用L-M方法以获得最终的位姿变换。Lego-Loam的主要区别就是最终的点云地图是如何存储的。本文的方法是保存每个特征集合{ F e t ， F p t F_e^t，F_p^t Fet，Fpt}而不是保存单独的点云地图。设 M t − 1 M^{t-1} Mt−1={{ F e 1 ， F p 1 F_e^1，F_p^1 Fe1，Fp1},…,{ F e t − 1 ， F p t − 1 F_e^{t-1}，F_p^{t-1} Fet−1，Fpt−1}}是存储先前所有特征集的集合，当进行扫描时， M t − 1 M^{t-1} Mt−1中的每个特征集也和传感器的位姿相关联。之后，可以通过两种方法从 M t − 1 M^{t-1} Mt−1中获得 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1。
　　在第一种方法中，通过选择在传感器视野中的特征集来获得 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1。为简单起见，我们可以选择传感器位姿在传感器的当前位置100米范围内的特征集。然后将被选中的特征集进行变换并融合到单独的周围地图 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1中。
　　第二种方法是我们可以将位姿图SLAM集成到LeGO-LOAM中。每个特征集的传感器位姿可以作为位姿图中的节点建模。特征集{ F e t ， F p t F_e^t，F_p^t Fet，Fpt}可以被视为此节点的传感器测量。由于激光建图模块的位姿估计漂移非常低，因此我们可以假设在短时间内没有漂移。通过这种方法，可以选择最近的一组特征集合来构成 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1，即 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1={{ F e t − k ， F p t − k F_e^{t-k}，F_p^{t-k} Fet−k，Fpt−k},…,{ F e t − 1 ， F p t − 1 F_e^{t-1}，F_p^{t-1} Fet−1，Fpt−1}}，其中k定义了 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1的尺度。然后，使用在L-M优化之后获得的位姿变换来添加新节点和在 Q ‾ t − 1 \overline{Q}^{t-1} Qt−1中所选节点之间的空间约束。我们可以通过执行循环闭合检测来进一步消除对该模块的漂移。在这种情况下，如果在当前功能集和使用ICP的先前功能集之间找到匹配项，则添加新约束。然后通过将位姿图发送到优化系统来更新传感器的估计位姿。
　　

实验结果

~~~~ ~~~ 本文将LEGO-LOAM集成到SLAM框架中，以消除由漂移引起的位姿估计误差，使用KITTI数据集进行测试。