LOAM全文翻译

---

摘要

我们提出了一种在[[6自由度]]范围内利用[[2轴激光雷达]]进行距离测量实现SLAM的方法。这个问题难点在于测量的距离信息是在不同的时间接收的（因为激光雷达需要通过旋转的方式采集信息），并且运动估计中的误差可能会导致产生的点云错误配准。迄今为止，通过[off-line batch（离线批处理）方法]可以构建3D地图，并使用[loop closure（循环闭合)]来校正随时间的漂移。我们实现了一种低漂移和低计算复杂度的方法，无需高精度测距或惯性测量。获得这种性能水平的关键思想是同时定位和映射的复杂问题划分为两个算法，通过两种算法同时优化大量变量。第一个算法以[高频但低精度]的性能进行里程计测量，以估计激光雷达的速度。第二个算法以[较低数量级的频率]运行，用于点云的精细匹配和对齐（registation）。通过两种算法的结合允许该方法实时建图。该方法已通过大量实验以及KITTI里程计基准进行了评估。结果表明，该方法可以达到最先进的off-line batch处理方法的精度水平。

六自由度是指刚体在三维空间中运动的自由度。特别是指刚体可以在前后、上下、左右三个互相垂直的坐标轴上平移，也可以在三个垂直轴上旋转其方向，三种旋转方向称为俯仰 (pitch)、偏摆 (yaw) 及翻滚 (roll)。加起来称为六自由度，即前后、上下、左右、翻滚 (roll)、俯仰 (pitch)、偏摆 (yaw)。

2轴激光雷达，即3D激光雷达

---

$I$ 、引言

3D建图仍然是一种主流的技术[1]–[3]。使用激光雷达进行建图是非常常见的，因为激光雷达可以提供高频的距离测量信息，无论测量的距离如何，误差都相对恒定。当激光雷达的唯一运动是旋转激光束时，其点云的配准非常简单。然而，如果激光雷达本身正在运动，如 in many applications of interest（多自由度运动），则精确的测绘需要在连续激光测距期间了解激光雷达的姿态变换。解决该问题的一种常见方法是使用独立的位置估计（例如通过GPS/INS）将激光点对齐到固定的坐标系中。另一组方法使用诸如来自车轮编码器或视觉里程计系统的里程计测量[4]、[5]来配准激光点。由于里程计随时间积分小的增量运动，因此其必然会漂移，并且许多注意力都致力于减少漂移（例如：使用闭环检测）。

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

该方法实现了低漂移和低计算复杂度，而不需要高精度测距或测量。获得这一性能水平的关键思想是将典型的复杂问题同时定位和映射（SLAM）[8]划分为两个算法，以同时优化大量变量。一种算法以高频但低精度的性能进行里程测量，以估计激光雷达的速度。另一种算法以较低数量级的频率运行，用于点云的精细匹配和对齐（registration）。尽管非必要，但如果IMU可用，则可以用于提供运动先验以帮助计算高频运动。具体来说，这两种算法都提取位于尖锐边缘和平面表面上（sharp edges and planar surfaces）的特征点（feature point），并分别将特征点与边缘线段和平面曲面片匹配（edge line segments and planar surface patches）。在里程算法中，通过确保快速计算，找到特征点的对应关系。在建图算法中，通过相关特征值和特征向量检查局部点簇的几何分布来确定对应关系。

通过对原问题进行分解，首先解决了一个较简单的问题，即在线运动估计。在此之后，映射作为批量优化（类似于迭代最近点（ICP）方法[9]）进行，以产生高精度运动估计和映射。并行算法结构确保了实时解决问题的可行性。此外，由于运动估计是在较高频率下进行的，因此给映射足够的时间来提高精度。当以较低频率运行时，映射算法能够合并大量特征点并使用足够多的迭代来收敛。

图1所示。该方法利用移动的2轴激光雷达进行运动估计和建图。由于激光雷达的运动，激光点是在不同的时间接收到的，在点云中会出现失真(如图左激光雷达点云所示)。我们提出的方法通过两个并行的算法来分解问题。一种odometry算法估计激光雷达的速度并校正点云中的失真，然后一种mapping算法匹配并对齐（registers）点云来创建地图。两种算法的结合保证了所要解决问题的实时性。

---

$II$、相关工作

激光雷达已成为机器人导航中有用的距离传感器[10]。对于定位和建图，大多数应用使用2D激光雷达[11]。当激光雷达扫描速率与其外部运动相比，较高时，通常可以忽略扫描内的运动失真。在这种情况下，标准ICP方法[12]可用于匹配不同扫描之间的激光回波。此外，提出了一种两步方法来消除失真[13]：基于ICP的速度估计步骤之后，是使用计算速度的失真补偿步骤。类似的技术也用于补偿单轴3D激光雷达引入的失真[14]。然而，如果扫描运动相对缓慢，则运动失真可能严重。这在使用2轴激光雷达时尤其如此，因为一轴通常比另一轴慢得多。通常，使用其他传感器来提供速度测量，从而消除失真。例如，激光雷达云可以通过与IMU集成的视觉里程计的状态估计进行配准[15]。当多个传感器（如GPS/INS和车轮编码器）同时可用时，该问题通常通过扩展卡尔曼滤波器[16]或粒子滤波器[1]解决。这些方法可以实时创建地图，以辅助机器人导航中的路径规划和碰撞避免。

如果在没有其他传感器的帮助的情况下使用2轴激光雷达，则运动估计和失真校正成为一个问题。Barfoot等人使用的一种方法是从激光强度返回创建视觉图像，并在图像之间匹配视觉上不同的特征[17]，以恢复地面车辆的运动[18]-[21]。车辆运动在[18]、[19]中被建模为恒定速度，在[20]、[21]中建模为高斯过程。我们的方法在里程计算法中使用与[18]、[19]类似的线性运动模型，但具有不同类型的特征。方法[18]-[21]涉及强度图像的视觉特征，需要密集点云。我们的方法提取和匹配笛卡尔空间中的几何特征，对云密度的要求较低。

最接近我们的方法是Bosse和Zlot[3]、[6]、[22]。他们使用2轴激光雷达获取点云，通过匹配局部点簇的几何结构进行配准[22]。此外，他们使用多个2轴激光雷达绘制地下矿山地图[3]。由同一作者提出，Zebedee是一种由2D激光雷达和通过弹簧连接到手杆的IMU组成的测绘设备[6]。通过手动点头设备进行映射。轨迹通过批量优化方法恢复，该方法处理分段数据集，并在分段之间添加边界约束。在该方法中，IMU的测量用于配准激光点，优化用于校正IMU偏差。本质上，Bosse和Zlot的方法需要批处理来开发精确的地图，因此不适用于需要实时地图的应用。相比之下，所提出的方法实时生成的映射在质量上与Bosse和Zlot的映射相似。区别在于，我们的方法可以为自主车辆的引导提供运动估计。此外，该方法利用了激光雷达扫描模式和点云分布。实现特征匹配，分别确保里程测量和映射算法的计算速度和精度。

---

$III$、符号和任务描述

本文解决的问题是使用3D激光雷达感知的点云进行自我运动估计，并为穿越环境构建地图。我们假设激光雷达是预先校准的。我们还假设激光雷达的角速度和线速度是平滑和连续的，没有突变。第二项假设将在Section VII-B中通过使用IMU提出。

作为本文中的惯例，我们使用右上角的大写字母表示坐标系。我们将扫描定义为激光雷达完成一次扫描覆盖 。我们使用右下角的小写字母 $k$, $k\in Z^{+}$ ,表示第$k$次扫描，$P_k$ 表示第 $k$ 次扫描期间感知到的点云。让我们定义如下两个坐标系。

•激光雷达坐标系 $L$ 是一个三维坐标系，其原点位于激光雷达的几何中心。x轴指向左侧，y轴指向上方，z轴指向前方。激光点云 $i$，$i\in P_k$ ，在$L_k$ 坐标系下表示为$X_{k,i}^L$ 。
•世界坐标系 $W$ 是与初始位置的 $L$ 重合的3D坐标系。激光点云 $i$ ，$i\in P_k$ ，在 $W_k$ 坐标系下表示为$X_{k,i}^W$ 。

通过假设和符号，我们的激光雷达里程计测量和建图问题可以定义为：
问题：给定激光雷达云序列 $P_k$，$k\in Z^{+}$ ，计算每次扫描 $k$ 期间激光雷达的自我运动，并使用 $Pk$ 为穿越的环境构建地图。

---

$IV$、符号和任务描述

A. 激光雷达硬件

本论文的研究在基于Hokuyo UTM-30LX激光扫描仪的定制[[3D激光雷达]]上进行了验证，但不限于此。通过本文，我们将使用从激光雷达收集的数据来说明该方法。激光扫描仪的视场为180度（水平）,分辨率为0.25度（垂直），扫描速率为40行/秒。激光扫描仪与电机相连，电机被控制以180度/s (−90◦到90◦)的角速度旋转,激光扫描仪的水平角度为零。对于此特定单元，扫描是从−90◦到90◦或相反方向进行的（持续1s）。请注意，对于连续旋转的激光雷达，扫描只是一个半球形旋转。机载编码器以0.25度的分辨率测量电机旋转角度, 激光点被投影到激光雷达坐标 $L$ 。

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

B. 软件系统预览

图3为软件系统框图。设 $\hat{P}$ ,为激光扫描中接收的点。在每次扫描期间，$\hat{P}$ 被登记在 $L$ 中。扫描 $k$ 期间的组合点云形成 $P_k$ 帧。然后，用两种算法处理 $P_k$ 。激光雷达里程计通过点云，计算激光雷达在两次连续扫描之间的运动。估计的运动被用来校正在 $P_k$ 中的畸变。该算法的运行频率约为10Hz。输出的结果被激光雷达mapping算法施行进一步的处理，它以1Hz的频率将去畸变的点云匹配并对齐到地图上。最后，将两种算法发布的姿态变换进行集成，得到激光雷达姿态相对于地图的约10Hz的变换输出。第五节和第六节对软件框图中的模块进行了详细介绍。

---

$V$、激光雷达里程计

A. 特征点提取

我们首先从激光雷达云 $P_k$ 中提取特征点。图2所示的激光雷达在 $P_k$ 中会产生分布不均匀的点。激光扫描仪返回的分辨率为0.25°。这些点位于一个扫描平面上。但是，由于激光扫描仪以180°/s的角速度旋转并以40Hz的频率生成扫描，垂直于扫描平面方向的分辨率为180°/40 = 4.5°。考虑到这一事实，仅使用来自具有共面几何关系的各个扫描的信息，从 $P_k$ 中提取特征点。

我们选择尖锐边缘和平面曲面片上的特征点。设 $i$ 是 $p_k$ 中的点，$i\in P_k$，并且设 S 是由激光扫描器在同一扫描中返回的 $i$ 的连续点的集合。由于激光扫描仪以CW或CCW顺序（顺时针或逆时针）返回生成点，S 在 $i$ 的每一侧包含其点的一半，两个点之间的间隔为0.25°。定义一个术语来评估局部表面的平滑度(曲率)。

基于c值对扫描中的点进行排序，然后选择具有最大c的特征点，即边缘点，以及最小c，即平面点。为了在环境中均匀分布特征点，我们将扫描分为四个相同的子区域（代码中是6等分）。每个子区域最多可提供2个边缘点和4个平面点(全是平面点或边缘点也可以)。仅当点i的c值大于或小于阈值且所选点的数量不超过最大值时，点i才可以被选为边或平面点。

在选择特征点时，我们希望避免选择其周围点已经被选择的点，或局部平面上与激光束大致平行的点（图4（a）中的点B）。这些点通常被认为是不可靠的。此外，我们还希望避免位于遮挡区域边界上的点[23]。图4（b）中示出了一个示例。Point A是激光雷达云中的一个边缘点，因为其连接表面（虚线段）被另一个对象阻挡。但是，如果激光雷达移动到另一个视角，则遮挡区域可能会发生变化并变得可见。为了避免选择上述点，我们再次找到一组点S。仅当S未形成大致平行于激光束的曲面片(surface patch)，并且S中没有任何点通过激光束方向上的间隙与 $i$ 断开，同时更接近激光雷达，然后是点i时，才能选择点 $i$（例如，图4（B）中的点B）。

图4所示。(a)实线段表示局部曲面片(local surface patches)。Point A在一个surface patch上，它与激光束(橙色点线段)有一定的角度。point B位于一个与激光束大致平行的surface patch上。我们将B视为不可靠的激光回波，不选择它作为特征点。(b)实线段是激光的可观测对象。point A位于被遮挡区域(橙色点线段)的边界上，可以被检测为边缘点。然而，如果从不同的角度看，被遮挡的区域可以改变并变得可见。我们不把A作为一个显著的边缘点，也不把它作为一个特征点。

总之，特征点被选择为从最大c值开始的边缘点，以及从最小c值开始选择的平面点，如果选择了一个点:
•所选边缘点或平面点的数量不能超过子区域的最大值，并且
•其周围的点都没有被选择，并且
•它不能位于大致平行于激光束的表面贴片上，也不能位于遮挡区域的边界上。

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

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

B. 寻找特征点关联

里程计算法估计激光雷达在扫描（sweep）范围内的运动。设 $t_k$为扫描k的开始时间。在每次扫描结束时，扫描期间感知到的点云 $P_k$ 被重新投影到时间戳 $t_{k+1}$ 上，如图6所示。

图6所示。将点云重新投影到扫描的末尾。蓝色线段表示第 $k$ 次扫描期间感知到的点云 $P_k$ 。在第 $k$ 次扫描结束时，将 $P_k$ 重新投影到时间戳 $t_{k+1}$ 上， 以获得 ${\overline{P}}_k$ （绿色线段）。然后，在第 k+1 次扫描期间，${\overline{P}}_k$ 和新感知的点云 $P_{k+1}$ （橙色线段）一起用于估计激光雷达运动。

让我们假设 ${\overline{P}}_K$ 和 $P_{K+1}$ 现在都可用，并从查找两个激光雷达云之间的对应关系开始。对于 $P_{k+1}$
我们使用上一节讨论的方法从激光雷达云中查找边缘点和平面点。设 ${\mathcal{E}}_{K+1}$ 和 ${\mathcal{H}}_{K+1}$ 分别为边点集和平面点集。我们将从 ${\overline{P}}_k$ 中找到边线(edge lines)作为 ${\mathcal{E}}_{k+1}$ 中的点的对应，找到平面面片(planar patches)作为 ${\mathcal{H}}_{k+1}$ 中的点的对应。

请注意，在扫描 $k+1$ 开始时，$P_{k+1}$ 是空集，其在扫描过程中随着接收到更多的点而增长。激光雷达里程计递归估计扫描期间的6自由度运动，并随着 $P_{k+1}$ 的增加逐渐包括更多点。在每次迭代中，使用当前估计的变换将 ${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$ 重新投影到扫描的开始。设 ${\tilde{\mathcal{E}}}_{k+1}$ 和 ${\tilde{\mathcal{H}}}_{k+1}$ 为重投影点集。对于$\tilde{\mathcal{E}}k+1$ 和 $\tilde{\mathcal{H}}k+1$ 中的每个点，我们将在 ${\overline{P}}_k$ 中找到最近的邻居点，我们将 ${\overline{P}}_k$ 存储在3D-KD树[24]中，用于快速索引。

图7。（a）表示找到一条边缘线作为 ${\tilde{\mathcal{E}}}_{k+1}$ 中边缘点的对应，（b）表示找到一个平面面片作为 ${\tilde{\mathcal{H}}}_{k+1}$ 中平面点的对应。在（a）和（b）中，$j$ 是距离特征点最近的点，位于 $P_k$ 中。橙色线条表示 $j$ 的同一次扫描，蓝色线条表示两次连续扫描。为了找到（a）中的边线对应关系，我们在蓝色线上找到另一个点，$l$ ，对应关系表示为 $(j,l)$ 。为了找到（b）中的平面面片对应关系，我们在橙色和蓝色线上分别找到另外两个点，$l$ 和 $m$ 。对应关系为 $(j,l,m)$ 。

图7（a）表示寻找一个边缘点的对应边缘线的过程。设 $i$ 是 ${\tilde{\mathcal{E}}}_{k+1}$ 中的一个点，$i\in {\tilde{\mathcal{E}}}_{k+1}$ 。边线由两点表示。定义 $j$ 表示 ${\overline{P}}_k$ 中 $i$ 的最近邻居点，$j\in {\overline{P}}_k$ ，并定义 $l$ 是 $i$ 在两次连续扫描中( $t_k$ 和 $t_{k+1}$ )与 $j$ 的最近邻居点。$(j,l)$ 形成 $i$ 的对应关系。为了验证 $j$ 和 $l$ 都是边点，我们基于（1）检查局部曲面的平滑度。这里，我们特别要求 $j$ 和 $l$ 来自不同的扫描，因为单个扫描不能包含来自同一边缘线的多个点（保证线特征与扫描线不是同一条线）。只有一个例外，即边缘线位于扫描平面上。但是，如果是这样，则边缘线将退化并在扫描平面上显示为一条直线，并且不应首先提取边缘线上的特征点。

图7（b）示出了寻找作为平面点对应的平面面片的过程。设 $i$ 为 ${\tilde{H}}_{k+1}$ 中的一个点， $i\in {\tilde{\mathcal{H}}}_{k+1}$ 。平面面片由三个点表示。与上一段类似，我们在 ${\overline{\mathcal{P}}}_k$ 中找到 $i$ 的最近邻，表示为 $j$ 。然后，我们发现另外两个点，$l$ 和 $m$ 是 $i$ 的最近邻点，一个在 $j$ 的相同扫描中 $(l)$，另一个在 $j$ 的两个连续扫描中 $(m)$。这保证了三个点不共线。为了验证 $j$ 、$l$ 和 $m$ 都是平面点，我们根据（1）再次检查局部曲面的平滑度。

根据找到的特征点的对应关系，现在我们导出表达式来计算从特征点到其对应关系的距离。在下一节中，我们将通过最小化特征点的总距离来恢复激光雷达运动。我们从边缘点开始。对于点 $i\in {\tilde{\mathcal{E}}}_{k+1}$ ，如果 $(j,l)$ 是对应的边线，$j,l\in {\overline{\mathcal{P}}}_k$，则点到线的距离可以计算为


式(2)中，分子为[[外积]]，也称叉乘。 a·b 内积描述两向量之间的投影关系，结果是一个数 |a||b|cos<a,b> axb 外积描述两向量的法向量，结果是一个向量，垂直于a和b向量构成的四边形，大小为|a||b|sin<a,b>，也是这个四边形的面积

其中 ${\tilde{\mathbf{X}}}_{(k+1,i)}^L$, ${\overline{\mathbf{X}}}_{(k,j)}^L$ , 和 ${\overline{\mathbf{X}}}_{(k,l)}^L$ 分别是 L 中点 $i$ ， $j$ 和 $l$ 的坐标。然后，对于点 $i\in {\tilde{\mathcal{H}}}_{k+1}$ ，如果 $(j,l,m)$ 是对应的平面面片，$j,l,m\in {\overline{\mathcal{P}}}_k$ ，则点到平面的距离为

$j,l,m$ 构成两个向量 $\vec {jl}$ 和 $\vec{jm}$ ,然后做外积得到垂直于这个平面的法向量，然后用该法向量与 $\vec{ji}$ 做内积可以得到 $\vec{ji}$ 在法向量上的投影，即可得到 $i$ 到平面的距离 $d_H$

其中 ${\overline{\mathbf{X}}}_{(k,m)}^L$ 是点 $m$ 在 L 中的坐标。

C. 运动估计

激光雷达运动在扫描期间以恒定的角速度和线速度建模。这允许我们在扫描中对在不同时间接收到的点进行线性插值计算姿势变换。设 $t$ 为当前时间戳，并记住 $t_{k+1}$ 是第 $k+1$ 扫描的开始时间。设 ${\mathbf{T}}_{k+1}^L$ 为 [ $t_{k+1}$ ， $t$ ] 之间的激光雷达姿态变换。${\mathbf{T}}_{k+1}^L$ 包含激光雷达在6-DOF中的刚体运动，${\mathbf{T}}_{k+1}^L$ = ${\left[t_x,t_y,t_z,{\theta }_x,{\theta }_y,{\theta }_z\right]}^T$,其中 $t_x,t_y$ 和 $t_z$ 分别表示为沿着 $L$ 的x，y和z轴的平移， ${\theta }_x,{\theta }_y$ , 和 ${\theta }_z$ 表示为旋转角度，遵循右手规则。给定一个点 $i$ ， $i\in P_{k+1}$ ，让 $t_i$ 作为它的时间戳，让 ${\mathbf{T}}_{(k+1,i)}^L$ 作为$\left[t_{k+1},t_i\right]$ 之间的姿势变换。${\mathbf{T}}_{(k+1,i)}^L$可以通过 ${\mathbf{T}}_{k+1}^L$ 的线性插值来计算： （$t_i$ < $t$ ）

---

想一下， ${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$ 是从 $P_{k+1}$ 中提取的边点和平面点集，而 ${\tilde{\mathcal{E}}}_{k+1}$ 和 ${\tilde{\mathcal{H}}}_{k+1}$ 是重新投影到扫描开始的点集。为了求解激光雷达的运动，我们需要在${\mathcal{E}}_{k+1}$ 和 ${\tilde{\mathcal{E}}}_{k+1}$ 或者 ${\mathcal{H}}_{k+1}$ 和${\tilde{\mathcal{H}}}_{k+1}$ 之间建立几何关系。使用（4）中的变换，我们可以导出：

其中 ${\mathbf{X}}_{(k+1,i)}^L$ 是点 $i$ 在 ${\mathcal{E}}_{k+1}$ 或 ${\mathcal{H}}_{k+1}$ 的坐标，${\tilde{\mathbf{X}}}_{(k+1,i)}^L$ 是点 $i$ 在 ${\tilde{\mathcal{E}}}_{k+1}$ 或 ${\tilde{\mathcal{H}}}_{k+1}$ 中的对应点。${\mathbf{T}}_{(k+1,i)}^L(a:b)$ 是 ${\mathbf{T}}_{(k+1,i)}^L$ 的第 $a$ 到第 $b$ 个条目（即平移量 $t_x,t_y$ 和 $t_z$），而 $R$ 是由罗德里格斯公式定义的旋转矩阵[25]

---

在上面的等式中，$\theta$ 是旋转的幅度(即旋转量 ${\theta }_x,{\theta }_y$ , 和 ${\theta }_z$ )

$\omega$ 是表示旋转方向的单位向量

$\hat{\omega }$ 是 $\omega$ [25]的斜对称矩阵。

---

回想一下，（2）和（3）计算 ${\tilde{E}}_{k+1}$ 和 ${\tilde{\mathcal{H}}}_{k+1}$ 中点之间的距离及其对应关系。
结合 （2）和 (4)-(8)，我们可以导出我们可以导出 ${\mathcal{E}}_{k+1}$ 中的边缘点与对应的边缘线之间的几何关系，

类似地，结合（3）和（4）-（8），我们可以在 ${\mathcal{H}}_{k+1}$ 中的平面点和对应的planar patch之间建立另一个几何关系，

---

最后，我们使用LevenbergMarquardt方法求解激光雷达运动[26]。对 ${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$ 中的每个特征点叠加（9）和（10），我们得到一个非线性函数，

求解变换矩阵T从而让两个d(残差:点到线的距离和点到面的距离)最小
其中，$f$ 的每一行对应一个特征点，$d$ 包含相应的距离。我们计算 $\mathbf{f}$ 关于 ${\mathbf{T}}_{k+1}^L$ 的雅可比矩阵，表示为 $\mathbf{J}$ ，其中 $\mathbf{J}=\partial f/\partial {\mathbf{T}}_{k+1}^L$ 。然后，（11）可以通过非线性迭代求解，将 $\mathbf{d}$ 最小化为零，

λ是由Levenberg-Marquardt方法确定的系数。

---

D. 激光雷达里程测量算法

激光雷达里程计算法如Algorithm 1 所示。该算法将最后一次扫描的点云 $\overline{\mathcal{P}}k$ 、当前扫描的增长点云 ${\mathcal{P}}_{k+1}$ 和最后一次递归的姿势变换$T_{k+1}^L$ 作为输入。如果开始新的扫描，$T_{k+1}^L$ 被设置为零（第4-6行）。然后，该算法从 $P_{K+1}$ 中提取特征点，在第7行构造 ${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$ 。对于每个特征点，我们在${\overline{P}}_k$ （第9-19行）中找到其对应关系。运动估计适用于鲁棒拟合（robust fitting）[27]。在第15行中，算法为每个特征点分配一个双平方权重。与其对应关系具有较大距离的特征点被分配有较小的权重，并且对于大于阈值距离的特征点被认为是异常值并且被分配有零权重。然后，在第16行中，姿势变换更新一次迭代。如果发现收敛或满足最大迭代次数，则非线性优化终止。如果算法到达扫描结束时，${\mathcal{P}}_{k+1}$ 将使用扫描期间的估计运动重新投影到时间戳 $t_{k+2}$ 。否则，下一轮递归只返回转换$T_{k+1}^L$。

---

$VI$、激光雷达建图

mapping算法的运行频率低于里程计算法，每次扫描只调用一次。在第 $k+1$ 次扫描结束时，激光雷达里程计生成一个去畸变的点云 ${\overline{P}}_{K+1}$ ，同时生成一个 $\left[t_{k+1},t_{k+2}\right]$ 之间的姿势变换 $T_{k+1}^L$ ，包含扫描期间的激光雷达运动。mapping算法将 ${\overline{P}}_{K+1}$ 在世界坐标 $W$ 中匹配并对齐，如图8所示。为了解释这个过程，我们将 ${\mathcal{Q}}_K$ 定义为地图上的点云，累积到第 $k$ 次扫描，并将 ${\mathbf{T}}_K^W$ 定义为 第 $k$ 次扫描，$t_{k+1}$ 结束时地图上激光雷达的姿态。利用激光雷达里程计的输出，mapping算法将 $T_k^W$ 从 $t_{k+1}$ 扩展到 $t_{k+2}$ ，以获得 $T_{k+1}^W$ ，并将 ${\overline{P}}_{k+1}$ 投影到世界坐标 $W$ ，表示为 ${\overline{\mathcal{Q}}}_{k+1}$ 。接下来，该算法通过优化Lidar姿态 $T_{k+1}^W$ 将 ${\overline{\mathcal{Q}}}_{k+1}$ 与 ${\mathcal{Q}}_k$ 进行匹配。

图8.mapping过程的图示。蓝色曲线表示由扫描k处的建图算法生成的地图上的激光雷达姿态 $T_k^W$。橙色曲线表示由里程计算法计算的第 $k+1$ 次扫描期间的激光雷达运动 $T_{k+1}^L$ 。使用 ${\mathbf{T}}_k^W$ 和${\mathbf{T}}_{k+1}^L$ 将里程计算法发布的去畸变后的点云投影到地图上，表示为 ${\mathcal{Q}}_{k+1}$ （绿色线段），并与地图上的现有点云 ${\mathcal{Q}}_K$ （黑色线段）匹配。

特征点的提取方法与第V-A节相同（但是是在地图中找匹配点），但使用了10倍的特征点。为了找到特征点的对应关系，我们将点云存储在地图 $Q_k$ 上，以10立方米的大小存储。立方体中与${\overline{Q}}_{k+1}$ 相交的点被提取并存储在3D KD树中[24]。我们在 $Q_k$ 中寻找特征点周围的一定区域内的点。设 $S^{\prime }$ 为一组周围点。对于边缘点，我们只在 $S^{\prime }$ 中保留边线上的点，对于平面点，我们只保留在planar patches上的点。然后，我们计算 $S^{\prime }$ 的协方差矩阵，记为 $M$ ，$M$ 的特征值和特征向量，表示为 $V$ 和 $E$。如果 $S^{\prime }$ 分布在边缘线上，$V$ 包含一个显著大于其他两个的特征值，$E$ 中与最大特征值相关的特征向量表示边缘线的方向。另一方面，如果 $S^{\prime }$ 分布在一个 planar patch上，$V$ 包含两个较大的特征值，第三个特征值明显较小，$E$ 中与最小特征值相关的特征向量表示平面patch的方向（法向量）。边缘线或 planar patch的位置通过 $S^{\prime }$ 的几何中心确定。

为了计算从特征点到其对应点的距离，我们在边缘线上选择两个点，在planar patch上选择三个点。这允许使用与（2）和（3）相同的公式计算距离。然后，基于（9）或（10）将每个特征点的方程导出，但不同之处在于 ${\overline{Q}}_{k+1}$ 中的所有点都具有相同的时间戳 $t_{k+2}$ ​。通过Levenberg-Marquardt方法[26]robust拟合[27]可以再次解决非线性优化问题，并且 ${\overline{Q}}_{k+1}$ 在地图上registered。为了均匀分布点，地图点云通过体素网格过滤器[28]缩小为体素大小为5cm的立方体（下采样，每个立方体中保留一个点）

姿势变换的集成(intergration)如图9所示。蓝色区域表示激光雷达mapping输出的姿势，$T_k^W$ ，每次扫描生成一次。橙色区域表示激光雷达里程计输出的变换（transform），$T_{k+1}^L$ ，频率约为10Hz。相对于地图的激光雷达姿态是两个变换的组合，频率与激光雷达里程计相同。

图9. 姿势变换的集成。蓝色区域表示激光雷达mapping输出的姿势，$T_k^W$ ，每次扫描生成一次。橙色区域是当前扫描范围内的激光雷达运动，），$T_{k+1}^L$ ，由里程计算法计算。激光雷达的运动估计是两种变换的组合，频率与 $T_{k+1}^L$ 相同。

---

$VII$、实验

A. 室内和室外试验

在实验期间，处理激光雷达数据的算法在具有2.5GHz四核和6Gib内存的笔记本电脑上运行，在Linux中的机器人操作系统（ROS）[29]上运行。该方法总共消耗两个核，里程计和建图程序在两个单独的核上运行。我们的软件代码和数据集是公开的1，2。

图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方法[9]匹配并比较两个点云。匹配完成后，一个点云与第二点云中对应的平面面片之间的距离被视为匹配误差。图11示出了误差分布的密度。它表明室内环境中的匹配误差比室外环境中的小。结果是合理的，因为自然环境中的特征匹配不如人工环境中的精确。

图11。走廊（红色）、大厅（绿色）、植被道路（蓝色）和果园（黑色）的匹配误差，对应于图4中的四个场景。

此外，我们还进行了测试，以测量运动估计的累积漂移。我们选择包含闭环的室内走廊进行实验。这使我们可以在同一个地方开始和结束。运动估计在开始位置和结束位置之间生成间隙，该间隙指示漂移量。对于室外实验，我们选择果园环境。携带激光雷达的地面车辆配备了高精度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提供的方向，并且我们的方法仅估计平移。在5分钟的数据收集期间，方向漂移超过25 °。绿色曲线仅依赖于我们方法中的优化，假设没有IMU可用。蓝色曲线使用IMU数据进行预处理，然后使用所提出的方法。我们观察到绿色和蓝色曲线之间的微小差异。图12（b）示出了对应于蓝色曲线的图。在图12（c）中，我们比较了图12（b）中黄色矩形中地图的两个闭合视图。上图和下图分别对应于蓝色和绿色曲线。仔细比较发现，上图中的边缘更尖锐。

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

C. 通过使用KITTI数据集进行测试

我们还使用来自KITTI里程测量基准[30]、[31]的数据集评估了我们的方法。这些数据集被安装在结构化道路上行驶的乘用车顶部的传感器（图13（a））小心地完全收集。车辆配备了360◦ Velodyne激光雷达、彩色/单色立体相机和用于地面实况的高精度GPS/INS。激光雷达数据以10Hz记录，并由我们的方法用于里程估计。由于空间问题，我们无法将结果包括在内。但是，我们鼓励读者在基准网站3上查看我们的结果。

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

数据集主要涵盖三种类型的环境：“城市”周围有建筑物，“乡村”周围有植被的小型道路，以及“公路”，道路较宽，周围环境相对清洁。图13（b）示出了来自城市环境的样本激光雷达云和相应的视觉图像。数据集中包括的总行驶距离为39.2km。上传车辆轨迹后，基准服务器将自动计算精度和排名。们的方法在由基准评估的所有方法中排名第一，无论传感模式如何，包括最先进的立体视觉里程计[32]，[33]。平均位置误差为行驶距离的0.88%，使用三维坐标中100m、200m、…、800m长度的轨迹段生成。

---

$VIII$ 总结和未来的工作**

使用旋转激光扫描仪的点云进行运动估计和建图可能很困难，因为该问题涉及激光雷达云中的运动恢复和运动失真校正。所提出的方法将问题分为两个并行运行的算法：激光雷达里程计进行粗处理以估计较高频率的速度，而激光雷达映射执行精细处理以创建较低频率的地图。两种算法的协作允许实时精确的运动估计和建图。此外，该方法可以利用激光雷达扫描模式和点云分布，进行特征匹配以确保里程计算法中的快速计算，并增强映射算法的准确性。该方法已在室内和室外以及KITTI里程基准上进行了测试。由于当前方法不识别环路闭合，我们未来的工作涉及开发一种通过闭合环路来固定运动估计漂移的方法。此外，我们将在卡尔曼滤波器中将我们方法的输出与IMU集成，以进一步减少运动估计漂移。

参考：
LOAM原文