SLAM论文翻译(1) - LOAM: Lidar Odometry and Mapping in Real-time

几度春风里

已于 2023-07-09 21:18:17 修改

阅读量631

点赞数 3

文章标签：计算机视觉 c++ 自动驾驶

于 2023-03-23 22:20:49 首次发布

原文链接：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf

版权

LOAM: Lidar Odometry and Mapping in Real-time

C Tests with KITTI Datasets

8 结论和未来的工作

原文链接：https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf

LOAM: Lidar Odometry and Mapping in Real-time

摘要：我们提出了一种实时测距和映射的方法，使用2轴激光雷达在6自由度运动的距离测量。这个问题很困难，因为距离测量是在不同的时间接收的，运动估计中的错误会导致所得到的点云的错误匹配。迄今为止，连续3D地图可以通过离线批处理方法构建，通常使用闭环来校正随时间推移的漂移。我们的方法实现了低漂移和低计算复杂度，不需要高精度测距或惯性测量。获得这种性能水平的关键思想是同时定位和映射的复杂问题的划分，它寻求通过两种算法同时优化大量变量。一种算法在高频率低保真的情况下进行测程来估计激光雷达的速度。另一种算法以低一个数量级的频率运行，用于点云的精细匹配和配准。结合这两种算法，可以实现实时映射。该方法已通过大量实验以及KITTI里程计基准进行了评估。结果表明，该方法可以达到目前离线批处理方法的精度水平。

1 引言

3D建图是流行的技术。使用激光雷达进行建图是常见的，因为激光雷达可以提供高频范围测量，其中误差相对恒定，与测量的距离无关。激光雷达的唯一运动是激光束的旋转，点云的匹配很简单。然而，如果激光雷达本身是移动的，在许多应用中，精确的建图需要连续激光测距过程中激光雷达的位姿。解决这个问题的一种常见方法是使用独立的位姿估计(例如通过GPS/INS)将激光点变换到一个固定的坐标系中。另一组方法使用里程计测量，如轮数计或视觉里程计来记录激光点。由于里程计在一段时间内集成了小的增量运动，因此它必然会漂移，因此很多注意力都用于减少漂移(例如使用闭环)。

图1所示。该方法的目标是利用移动两轴激光雷达进行运动估计和映射。由于激光点在不同的时间接收，由于激光雷达的运动，点云中存在失真(如左侧激光雷达云所示)。我们提出的方法通过并行运行的两个算法来分解问题。一种测程算法估计激光雷达的速度并校正点云中的失真，然后一种映射算法匹配并注册点云以创建地图。两种算法的结合保证了问题实时解决的可行性。

在这里，我们考虑使用在6自由度移动的2轴激光雷达创建低漂移里程计地图的情况。使用激光雷达的一个关键优势是它对环境光线和场景中的光学纹理不敏感。最新的激光雷达已经有较小的尺寸和重量。激光雷达可以由在环境中的人手持，甚至可以连接到微型飞行器上。由于我们的方法旨在推动与最小化里程计估计漂移相关的问题，因此目前不涉及回环。

该方法实现了低漂移和低计算复杂度，不需要高精度的测距或测量惯性测量。获得这种性能水平的关键思想是SLAM的典型复杂问题的划分，它寻求通过两个算法同时优化大量变量。一种算法在高频率低精度的情况下进行测程来估计激光雷达的速度。另一种算法以低频率高精度的匹配。虽然没有必要，但如果IMU可用，则可以提供一个运动先验来帮助解释高频运动。具体而言，这两种算法分别提取位于尖锐边缘和平面表面的特征点，并将特征点与边缘线段和平面表面块进行匹配。在测程算法中，通过快速计算，找到特征点之间的对应关系。在建图算法中，通过相关的特征值和特征向量，通过检查局部点簇的几何分布来确定对应关系。

通过对原始问题的分解，首先解决一个更简单的问题，即在线运动估计。之后，通过批量优化(类似于迭代最接近点(ICP)方法建图，得到高精度的运动估计和地图。并行算法结构保证了问题实时求解的可行性。此外，由于运动估计是在更高的频率下进行的，因此映射有足够的时间来提高精度。当以较低的频率运行时，映射算法能够合并大量的特征点，并使用足够多的迭代进行收敛。

2 相关工作

3 符号和任务描述

本文所要解决的问题是利用三维激光雷达感知的点云进行位姿估计，并对环境建立地图。我们假设激光雷达是预校准的。我们还假设激光雷达的角速度和线速度随时间是平滑连续的，没有突变。第二种假设将在第7- B节中通过使用IMU来介绍。

作为本文的惯例，我们使用右大写字母表示坐标系。我们定义一次扫描为激光雷达完成一次扫描覆盖。我们使用 $k$ , $k\epsilon Z^{+}$ 表示扫描次数， $P_{k}$ 表示扫描 $k$ 次感知到的点云。我们定义如下两个坐标系。

激光雷达坐标系{L}，是一个原点在激光雷达几何中心的三维坐标系。x轴是向左的，y轴是向上的，z轴是向前的。 ${L_{k}}$ 中点的坐标记为。
世界坐标系{W}，是与{L}在初始位置重合的三维坐标系。点，在中的坐标为。

通过假设和记号，我们的激光雷达测程和建图问题可以定义为：

问题：给定激光雷达云序列，计算每次扫描 k 时激光雷达的自我运动，对遍历的环境用建立地图。

4 系统概述

A 激光雷达硬件

图2所示。本研究中使用的3D激光雷达由电机驱动的用于旋转运动的Hokuyo激光扫描仪和测量旋转角度的编码器组成。激光扫描仪的视野为180◦分辨率为0.25◦。扫描速率为40行/秒。控制电机从−90◦旋转到90◦，激光扫描仪的水平方向为零。

图3所示。激光雷达测程与测图软件系统框图

B 软件系统概述

软件系统示意图如图3所示。以激光扫描中接收到的点为。在每次扫描期间，在{L}中注册了。扫描k时组合的点云形成，然后用两种算法处理。激光雷达里程计取点云，并计算两次连续扫描之间激光雷达的运动。估计的运动用于校正中的失真。算法运行在10Hz左右的频率。输出由激光雷达建图进一步处理，它以1Hz的频率将未失真的云匹配并注册到地图上。最后，将两种算法位姿集成在一起，生成一个大约10Hz的关于激光雷达相对于地图的位姿变换输出。第五节和第六节详细介绍了软件框图中的模块。

5 激光雷达里程计

A 特征点提取

我们从激光雷达云中提取特征点开始，图2所示的激光雷达在中自然生成分布不均匀的点。激光扫描仪在一次扫描中返回的分辨率为0.25◦。这些点位于扫描平面上。然而，当激光扫描仪以180◦s的角速度旋转并以40Hz的速度生成扫描时，与扫描平面垂直方向的分辨率为180◦/40 = 4.5◦。考虑到这一点，从中提取特征点仅使用来自单个扫描的信息，具有共平面几何关系。

我们选择在边缘和平面上的特征点（角点和面点）。设 i 是中的一个点，，设S是激光扫描仪在同一次扫描中返回的 i 的连续点的集合。由于激光扫描仪以CW或CCW顺序生成点返回，S包含一半的点在 i 和两点之间的0.25◦间隔上。定义一个术语来评估局部表面的平滑度（曲率），

根据c值对扫描点进行排序，然后选择c值最大的特征点即角点，c值最小的特征点即面点。为了在环境中均匀分布特征点，我们将扫描分为四个相同的子区域。每个子区域最多可以提供2个角点和4个面点。当点 i 的c值大于或小于阈值，且所选点个数不超过最大值时，点 i 才能被选为角点或面点（特征点的均匀化）。

在选择特征点时，我们要避免选择周围点被选中的点，或者局部平面上与激光束大致平行的点(图4(a)中的B点)。这些点通常被认为是不可靠的。同样，我们希望避免在被遮挡区域边界上的点。如图4(b)所示。点A是激光雷达云中的一个角点，因为它的连接表面(虚线段)被另一个物体挡住了。然而，如果激光雷达移动到另一个视角，被遮挡的区域就会发生变化，变得可观察到。

图4所示。(a)实线段代表局部表面块。点A位于与激光束有一定角度的表面上(橙色虚线段)。点B位于与激光束大致平行的表面上。我们认为B是一个不可靠的激光返回点，不选择它作为特征点。(b)实线段是激光可观测的物体。点A在被遮挡区域的边界上(橙色虚线段)，可以检测到角点。然而，如果从不同的角度观察，被遮挡的区域可以改变并变得可观察到。我们不把A作为显著边点，也不选择A作为特征点。

总之，选从最大 c 值择特征点作为角点,从最小c为平面点，选择点需要满足如下条件，

选择角点或平面点的数量不能超过最大的次区域，
周边点都已经被选中，
不能在一个表面上大致平行于激光线，或者在一个区域的边界。

从走廊场景中提取特征点的示例如图5所示。边缘点和平面点分别用黄色和红色标记。

图5所示。从走廊内的激光雷达云中提取边缘点(黄色)和平面点(红色)的示例。同时，激光雷达以0.5m/s的速度向人物左侧的墙壁移动，这导致了墙壁的运动失真。

B 寻找特征点对应关系

里程计算法用于估计激光雷达在一次扫描中的运动。设为一次扫描 k 的开始时间，每次扫描结束时，扫描过程中感知到的点云，重投影到时间戳，如图6所示。我们将重新投影的点云标记为。在下一个扫描 k+1 中，与新接收到的点云一起使用，来估计激光雷达的运动。

图6所示。重新投影点云到扫描结束。蓝色线段表示扫描k中感知到的点云。在扫描k结束时，被重投影到时间戳，以获得(绿色线段)。然后，在扫描k+1期间，和新感知的点云(橙色线段)一起估计激光雷达的运动。

如图所示。重新投影点云到扫描结束。蓝色线段表示扫描k中感知到的点云。在扫描k结束时，被重投影到时间戳，以获得(绿色线段)。然后，在扫描k+1期间，和新感知的点云(橙色线段)一起估计激光雷达的运动。

我们假设和现在都是可用的，然后从寻找两个激光雷达云之间的对应关系开始。对于，我们使用上一节讨论的方法从激光雷达云中找到角点和面点。设为角点集合，为面点集合。我们将的边线作为中的点的对应点，平面集合作为中的点的对应点。

请注意，在扫描k+1开始时，是一个空集，在扫描过程中，随着接收到更多的点，它会增长。激光雷达里程计递归估计扫描过程中的 6-DOF 运动，并随着的增加逐渐包含更多的点。在每次迭代中，和使用当前估计的变换重新投影到扫描的开始。让和为重投影点集。对于和中的每个点，我们将找到中最近的邻居点。在这里，存储在3D-kd树中，用于快速索引。

图7(a)表示作为边点对应的边线的查找过程。设 i 是的一个点。边线由两点表示。让 j 是邻近点在，并让是最最近邻点，我的两个连续扫描的扫描 j。的关联 i 。然后,以验证 j 和都是边缘点，我们检查当地的平滑表面基于(1)。在这里,我们特别要求j 和 l 不同扫描正在考虑单个扫描不能包含多个点相同的边缘线。只有一个例外，即边缘线在扫描平面。但如果是这样，则边缘线退化，在扫描平面上呈现为一条直线，边缘线上的特征点首先就不能提取。

图7(b)为面点对应寻找平面贴片的过程。让 i 是。平面贴片用三个点表示。与上一段类似，我们在中找到 i 的最近邻点，记为 j。然后，我们找到另外两个点 l 和 m，作为 i 的最近邻居，一个在对 j 的同一扫描中，另一个在对 j 的连续两次扫描中。这保证了这三个点是非共线的。为了验证j、l和m都是平面点，我们根据(1)再次检查局部曲面的平滑度。

在找到特征点的对应关系后，我们推导出计算特征点到对应关系的距离的表达式。在下一节中，我们将通过最小化特征点的总体距离来恢复激光雷达运动。我们从边点开始。对于点，如果是对应的边线，则点到线的距离可计算为：

其中和分别是{ L }中点i, j 和的坐标。那么，对于点，如果是对应的平面贴片，，则点到平面的距离为：

其中是点m在{L}中的坐标。

C 运动估计

在扫描过程中，激光雷达的运动建模为恒定的角速度和线速度。这允许我们在扫描中对不同时间接收到的点进行线性插值。设 t 为当前时间戳，是扫描的开始时间 k + 1。

设为之间的激光雷达姿态变换。包含激光雷达在6-DOF中的刚性运动，，其中 tx, ty, tz 分别为沿{L}的 x-， y-， z- 轴的平移，θx， θy， θz为旋转角度，遵循右手定则。给定点，设为其时间戳，设为之间的位姿变换。可通过的线性插值计算，

回想一下，和是从中提取的角点和面点的集合，而和是重新投影到扫描开始的点的集合。为了解决激光雷达运动，我们需要在和或和之间建立几何关系。利用式(4)中的变换，我们可以推导出，

其中是或中点 i 的坐标，是或中的对应点，是的第 a 到第 b 个参数，R是由Rodrigues公式定义的旋转矩阵，

在上面的方程中，θ是旋转的幅度，

ω是表示旋转方向的单位向量，

而是ω的斜对称矩阵，

回想(2)和(3)计算和点之间的距离及其对应关系。结合(2)和(4)-(8)，我们可以得到中的边点与相应边线之间的几何关系，

同样地，结合(3)和(4)-(8)，我们可以建立另一个平面点与对应的平面的几何关系，

最后用LevenbergMarquardt方法求解激光雷达运动。对和中的每个特征点进行(9)和(10)叠加，得到一个非线性函数，

其中 f 的每一行对应一个特征点，d包含相应的距离。我们计算 f 关于的雅可比矩阵，记为，其中。(11)可通过非线性迭代，使 d 趋近于0，

λ是由Levenberg-Marquardt方法确定的因子。

D 激光雷达里程计算法

激光雷达里程计算法如算法1所示。该算法将上一次扫描的点云，当前扫描的增长点云，以及上一次递归的姿态变换作为输入。如果开始新的扫描，被设置为零(第4-6行)。然后从中提取特征点，构造第7行和。对于每个特征点，我们在中找到对应的内容(第9-19行)。运动估计适应于一个鲁棒拟合。在第15行中，算法为每个特征点分配了一个二方权重。与对应点之间距离较大的特征点被赋予较小的权重，距离大于阈值的特征点被认为是异常值，并赋予零权重。然后，在第16行中，为一次迭代更新了姿态变换。如果找到收敛点或满足最大迭代次数，非线性优化就终止。如果算法到达扫描的结束，则使用扫描期间估计的运动将重新投影到时间戳。否则，下一轮递归只返回变换。

6 激光雷达地图

映射算法的运行频率比里程计算法低，并且每次扫描只调用一次。在扫描k+1结束时，激光雷达里程计生成一个未变形的点云，同时生成一个姿态变换，包含扫描过程中在之间激光雷达运动。映射算法匹配并在世界坐标 {W} 中注册，如图(8)所示。为了解释这个过程，我们定义为地图上的点云，累积到扫描k，让为扫描k结束时激光雷达在地图上的姿态。根据激光雷达测程的输出，映射算法将从扩展到，得到，并将投影到世界坐标{W}，记为。接下来，算法通过优化激光雷达姿态来匹配与。

图8所示。映射过程的说明。蓝色曲线表示扫描k处映射算法生成的激光雷达在地图上的姿态。

橙色曲线表示用里程计算法计算的扫描k+1, 期间激光雷达的运动。使用和，将里程计算法发布的未失真点云投影到地图上，记为(绿色线段)，并与地图上已有的云(黑色线段)进行匹配。

特征点提取方法与Section V -A相同，但使用了10倍的特征点。为了找到特征点的对应关系，我们将点云存储在地图上，以10m立方的面积存储。立方体中与相交的点被提取并存储在3D KD-tree 中。我们在特征点周围的一定区域内找到中的点。设是周围点的集合。对于一个边点，我们只在的边线上保留点，而对于一个平面点，我们只在平面块上保留点。然后，我们计算的协方差矩阵，记为M, M的特征值和特征向量，分别记为V和E。如果分布在边线上，V包含一个显著大于另外两个的特征值，E中与最大特征值相关联的特征向量表示边线的方向。

另一方面，如果分布在平面块上，V包含两个较大的特征值，第三个特征值明显较小，E中与最小特征值相关的特征向量表示平面块的方向。边缘线或平面块的位置是通过的几何中心来确定的。

为了计算从特征点到其对应点的距离，我们在边缘线上选择两点，在平面贴片上选择三个点。这允许使用与(2)和(3)相同的公式来计算距离。然后，为每个特征点推导出一个方程(9)或(10)，但不同之处在于中的所有点共享相同的时间。通过Levenberg-Marquardt方法进行鲁棒拟合再次求解非线性优化，并在地图上注册。为了均匀分布点，地图云通过体素网格过滤器缩小体素大小为5cm立方体。

姿态变换的积分如图9所示。蓝色区域表示激光雷达映射的姿态输出，每次扫描生成一次。橙色区域表示激光雷达测程在频率约为10Hz时的变换输出。激光雷达相对于地图的姿态是两个变换的组合，与激光雷达测程频率相同。

图9所示。姿态变换的积分。蓝色区域说明了映射算法每次扫描生成一次的激光雷达姿态。橙色区域为当前扫描范围内的激光雷达运动，由里程计算法计算。激光雷达的运动估计是两种变换的组合，频率为。

7 实验

在实验中，处理激光雷达数据的算法在一台2.5GHz四核6Gib内存的笔记本电脑上运行，在Linux下的机器人操作系统(ROS)上运行。该方法总共消耗两个核心，里程计和映射程序在两个独立的核心上运行。

A 室内和室外测试

图10所示。地图生成于(a)-(b)狭长的走廊，(c)-(d)大型大厅，(e)-(f)植被覆盖的道路，以及(g)-(h)两行树之间的果园。激光雷达在室内测试中放置在一辆推车上，在室外测试中安装在一辆地面车辆上。所有测试都使用0.5m/s的速度。

该方法已在室内和室外环境中进行了测试。在室内测试中，激光雷达与电池和笔记本电脑一起放置在一辆推车上。一个人推着车走。图10(a)和图10(c)分别是在两个具有代表性的室内环境中绘制的地图，分别是狭长的走廊和宽敞的大厅。图10(b)和图10(d)是在同一场景拍摄的两张照片。在室外测试中，激光雷达被安装在地面车辆的前部。图10(e)和图10(g)分别为植被道路和两排树木之间的果园生成的地图，图10(f)和图10(h)分别给出照片。在所有测试中，激光雷达以0.5m/s的速度移动。

为了评估地图的局部准确性，我们从相同的环境中收集了第二组激光雷达云。在数据选择期间，激光雷达保持静止，并放置在每个环境中的几个不同的地方。采用点对面ICP方法对两个点云进行匹配比较。匹配完成后，一个之间的距离将点云和第二点云中对应的planer patch视为匹配误差。图(11)显示了误差分布的密度。说明室内环境的匹配误差小于室外环境。结果是合理的，因为在自然环境中的特征匹配不如在人造环境中精确。

图11所示。走廊(红色)、大堂(绿色)、植被道路(蓝色)、果园(黑色)的匹配误差，分别对应图10中的四个场景。

此外，我们进行测试来测量运动估计的累积漂移。我们选择走廊进行室内实验，走廊包含一个闭环。这使得我们可以在同一个地方开始和结束。运动估计在开始和结束位置之间产生一个间隙，这表明漂移的量。室外实验，我们选择了果园环境。携带激光雷达的地面车辆配备高精度GPS/INS用于地面真相采集。将测量的漂移与行进的距离作为相对精度进行比较，并在表I中列出。具体来说，测试1使用与图10(a)和图10(g)相同的数据集。一般来说，室内测试的相对准确度约为1%，室外测试的相对准确度约为2.5%

B IMU的帮助

我们将Xsens MTi-10 IMU附加到激光雷达上以处理快速速度变化。在将点云发送到所提出的方法之前，以两种方式对点云进行预处理，1)根据IMU的方向，旋转一次扫描中接收到的点云，使其与该次扫描中激光雷达的初始方向对齐，2)通过加速度测量，部分消除运动失真，就像激光雷达在扫描过程中以常量速度移动一样。然后通过激光雷达测程和绘图程序对点云进行处理。

IMU方向是通过积分陀螺的角速率和卡尔曼滤波器[1]中加速度计的读数来获得的。图12(a)显示了示例结果。一个人拿着激光雷达走在楼梯上。当计算红色曲线时，我们使用IMU提供的方向，我们的方法只估计平移。取向漂移超过25◦在5分钟的数据收集。绿色曲线仅依赖于我们方法中的优化，假设没有可用的IMU。蓝色曲线使用IMU数据进行预处理，然后采用所提出的方法。我们观察到绿色和蓝色曲线之间的差异很小。图12(b)为蓝色曲线对应的地图。在图12(c)中，我们比较了图12(b)中黄色矩形中的地图的两个封闭视图。上面和下面的图形分别对应蓝色和绿色曲线。仔细比较可以发现上图的边缘更锐利。

图12所示。比较有/没有IMU辅助的结果。一个人拿着激光雷达走在楼梯上。黑点是起点。在(a)中，红色曲线使用IMU的方向和我们的方法估计的平移来计算，绿色曲线仅依赖于我们方法中的优化，蓝色曲线使用IMU数据进行预处理，然后使用该方法。(b)为蓝色曲线对应的地图。在(c)中，上下两幅图分别对应蓝色曲线和绿色曲线，使用(b)中黄色矩形标记的区域，上面的图边缘更清晰，在地图上的精度更高。

表II比较了使用IMU和不使用IMU时运动估计的相对误差。激光雷达由一个人以0.5m/s的速度行走，并以0.5m左右的幅度上下移动激光雷达。地面真实值是用卷尺手工测量的。在所有四次测试中，在IMU的帮助下使用所提出的方法得到了最高的精度，而使用IMU的定向只导致了最低的精度。实验结果表明，该方法能有效地消除非线性运动，并能有效地处理线性运动。

C Tests with KITTI Datasets

我们还使用KITTI里程计基准的数据集评估了我们的方法。在结构化道路上行驶的乘用车(图13(a))顶部安装的传感器仔细注册数据集。车辆配备360◦V激光雷达，彩色/单色立体摄像机，高精度GPS/INS地面真相目的。激光雷达数据以10Hz记录，并被我们的方法用于里程估计。由于篇幅问题，我们无法将结果包括在内。但是，我们鼓励读者在基准网站上查看我们的结果。

图13所示。(a) KITTI基准测试使用的传感器配置和车辆。车辆安装有Velodyne激光雷达、立体摄像机和用于地面真相获取的高精度GPS/INS。我们的方法只使用来自Velodyne激光雷达的数据。(b)来自城市场景的激光雷达云样本(上图)和相应的视觉图像(下图)。

数据集主要覆盖三种类型的环境:“城市”，周围有建筑物，“乡村”，道路狭窄，场景中有植被，“高速公路”，道路宽阔，周围环境相对干净。图13(b)显示了一个样本激光雷达云和来自城市环境的相应视觉图像。数据集中包含的总驾驶距离为39.2公里。车辆轨迹上传后，基准测试服务器自动计算精度和排名。我们的方法在基准评估的所有方法中排名第一，不考虑传感方式，包括最先进的立体视觉测程[32]，[33]。平均位置误差为行进距离的0.88%，使用100m, 200m，…的轨迹段生成。，三维坐标长度为800米。

8 结论和未来的工作

使用旋转激光扫描仪的点云进行运动估计和映射可能很困难，因为问题涉及到激光雷达云中运动的恢复和运动失真的修正。该方法通过并行运行的两种算法对该问题进行划分和解决:激光雷达测程算法进行粗处理，在较高频率下估计速度;激光雷达测绘算法进行细处理，在较低频率下创建地图。两种算法的合作可以实现精确的实时运动估计和映射。此外，该方法可以利用激光雷达扫描模式和点云分布。在里程计算法中进行特征匹配以保证计算速度，在映射算法中保证精度。该方法已在室内和室外以及KITTI测程基准上进行了测试。

由于目前的方法不能识别闭环，我们未来的工作包括开发一种通过闭环来修复运动估计漂移的方法。此外，我们将把我们的方法的输出与卡尔曼滤波器中的IMU集成在一起，以进一步减少运动估计漂移。