摘要

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

2轴激光雷达的理解是水平的旋转加上垂直的运动

引言

3D建图仍然是一种流行的技术[1]–[3]。使用激光雷达进行建图是常见的，因为激光雷达可以提供高频范围测量，无论测量的距离如何，误差都相对恒定。在激光雷达的唯一运动是旋转激光束的情况下，点云的配准很简单。然而，如果激光雷达本身在许多感兴趣的应用中都在移动，则精确的建图需要在连续激光测距期间了解激光雷达的姿态。解决该问题的一种常见方法是使用独立的位置估计（例如通过GPS/INS）将激光点配准到固定的坐标系中。另一组方法使用里程计测量，例如从轮速计或视觉里程计系统[4]，[5]记录激光点。由于里程计集成了随时间推移的小增量运动，因此必然会发生漂移，并且会将大量注意力放在减小漂移上（例如，使用回环）。

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

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

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

相关工作

相关工作不重要，仅做了简单的翻译

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

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

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

符号和任务描述

本文解决的问题是利用三维激光雷达感知的点云进行运动估计，并为穿越的环境绘制地图。我们假设激光雷达是预先校准的。我们还假设激光雷达的角速度和线速度随时间平滑连续，没有突变。第二个假设将通过使用第VII-B节中的IMU发布。

作为本文的惯例，我们使用右大写字母表示坐标系。我们将sweep定义为激光雷达完成一次扫描的所有scans。我们使用$k，k\in Z^{+}$表示sweeps，${\mathcal{P}}_k$示sweep $k$期间感知到的点云。让我们定义两个坐标系，如下所示。

• 激光雷达坐标系 { L } \{L\} {L}是一个三维坐标系，其原点位于激光雷达的几何中心。$x$轴指向左侧，$y$轴指向上方，$z$轴指向前方。点$i$，$i\in {\mathcal{P}}_k$，在坐标 { L k } \{L_k\} {Lk​}坐标表示为$X_{(k,i)}^L$。

当时的lidar坐标系是上述指向，目前lidar坐标系，例如velodyne是$x$指向前，$y$指向左，$z$指向上的坐标系

• 世界坐标系 { W } \{W\} {W}是一个三维坐标系，在初始位置与 { L } \{L\} {L}重合。点$i$，，$i\in {\mathcal{P}}_k$在 { W } \{W\} {W}坐标是$X_{(k,i)}^W$。

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

问题：给定一系列激光雷达云${\mathcal{P}}_k，k\in Z^{+}$，计算激光雷达在每次sweep $k$期间的自我运动，并为穿越的环境构建一张带有的${\mathcal{P}}_k$地图。

系统概述

A. Lidar Hardware

硬件对于框架理解并不重要，做简单的翻译

本文的研究在基于Hokuyo UTM-30LX激光扫描仪的定制3D激光雷达，但不限于此雷达。通过本文，我们将使用从激光雷达收集的数据来说明该方法。激光扫描仪的视场角为$180^{\circ }$， 以$0.25^{\circ }$ 分辨率和40行/秒scan速率。激光扫描仪连接到一个电机，该电机被控制以$180^{\circ }$每秒的角速度旋转介于$-90^{\circ }$和$90^{\circ }$ 激光扫描仪的水平方向为零。对于此特定单元，sweep是从$-90^{\circ }$ 到$90^{\circ }$ 或相反方向（持续1s）。这里，请注意，对于连续旋转的激光雷达，sweep只是一个半球形旋转。车载编码器以$0.25^{\circ }$的分辨率测量电机旋转角度，利用它，激光点被投影到激光雷达坐标 { L } \{L\} {L}。

B. Software System Overview

图3示出了软件系统的示意图。设$\hat{\mathcal{P}}$为激光scan中接收到的点。在每次sweep期间，$\hat{\mathcal{P}}$在 { L } \{L\} {L}坐标系中。sweep $k$期间的点云组合形成${\mathcal{P}}_k$。然后，采用两种算法对${\mathcal{P}}_k$进行处理。激光雷达里程计获取点云并计算两次连续sweep之间的激光雷达运动，估计的运动用于校正${\mathcal{P}}_k$中的失真（运动畸变），该算法以10Hz左右的频率运行。激光雷达建图对输出进行进一步处理，以1Hz的频率将未失真的点云匹配并配准添加到地图上。最后，将这两种算法发布的姿态变换进行集成，以生成关于相对于地图的激光雷达姿态的约10Hz的变换输出。第五节和第六节详细介绍了软件图中的模块。

LIDAR 里程计

A. Feature Point Extraction

我们首先从激光雷达云${\mathcal{P}}_k$中提取特征点。图2所示的激光雷达自然会在${\mathcal{P}}_k$中生成分布不均匀的点。在scan中，激光扫描仪返回的分辨率为$0.25^{\circ }$（水平分辨率） 。这些点位于scan平面上。但是，由于激光扫描仪以$180^{\circ }/s$的角速度旋转并以40Hz的频率生成scans，垂直于scan平面的分辨率为$180^{\circ }/40=4.5^{\circ }$（垂直分辨率）. 考虑到这一事实，特征点仅使用从${\mathcal{P}}_k$中提取的单个scan的信息，具有共面几何关系。

图1：

该方法旨在使用移动的2轴激光雷达进行运动估计和建图。由于在不同的时间接收激光点，由于激光雷达的运动（如左激光雷达云中所示），点云中会出现失真。我们提出的方法将问题分解为两个并行运行的算法。里程计算法估计激光雷达的速度并校正点云中的失真，然后，建图算法匹配并配准点云以创建地图。这两种算法的结合保证了实时求解问题的可行性。

我们选择位于锐边和平面曲面片上的特征点。让$i$成为中${\mathcal{P}}_k$的一点，$i\in {\mathcal{P}}_k$，设$\mathcal{S}$为同一scan中返回的$i$的连续点集。激光扫描仪以CW或CCW顺序生成点（CW为顺时针，CCW逆时针，目前的lidar大部分是顺时针旋转），$\mathcal{S}$在$i$的每一侧都包含其点集的一半，两点之间的间隔为$0.25^{\circ }$。定义一个术语以评估局部曲面的平滑度（也叫曲率），
$$c=\frac{1}{|\mathcal{S}|\cdot \Vert X_{(k,i)}^L\Vert }\Vert \sum _{j\in \mathcal{S},j\ne i}\left(X_{(k,i)}^L-X_{(k,j)}^L\right)\Vert \text{\hspace{1em}(1)}$$

理解式(1)的意思，为什么式(1)的大小可以分辨出边缘点和平面点？因为如果是边缘点，那么$i$点与周围点的差值一定会变大，因为激光测的是距离呀，比如凹凸的地方和平滑的面显然不一样呀，理解一下；个人感觉也可以取一个窗口，或者半径的圆，计算 $i$点的光滑度$c$。

scan中的点根据$c$值进行排序，然后选择具有最大$c$值（即边缘点）和最小$c$值（即平面点）的特征点。

为了在环境中均匀分布特征点，我们将scan分为四个相同的子区域。每个子区域最多可提供2个边缘点和4个平面点。仅当点$i$的c值大于或小于阈值且所选点的数量不超过最大值时，才可以将点$i$选择为边缘或平面点。

在选择特征点时，我们希望避免已经选择过的点周围的点（同一个scan），或局部平面上与激光束大致平行的点（图4（a）中的点B）。这些点通常被认为是不可靠的。此外，我们还希望避免位于遮挡区域边界上的点[23]。图4（b）中示出了一个示例。点A是激光雷达云中的边缘点，因为其连接的曲面（虚线段）被另一个对象阻挡。但是，如果激光雷达移动到另一个视角，则遮挡区域可能会发生变化并变得可见。

图4$(a)$的点B因为于激光束平行，微笑运动后可能就看不见了，图4$(b)$的点A并不是边缘点，而是被遮挡的平面点，但是由于被遮挡断层，导致曲率较大，可能错误认为是边缘点。

为了避免选择上述点，我们再次找到点集$\mathcal{S}$。仅当$\mathcal{S}$未形成大致平行于激光束的曲面片时，才能选择点$i$。还有就是在$\mathcal{S}$中，没有一个点于点$i$间隙断开，同时比点$i$更靠近激光雷达（例如，图4$(b)$中的点B)。

点$i$为边缘点时要最靠近lidar

图4:

$(a)$ 实线线段表示局部曲面面片。点A位于与激光束（橙色虚线段）成一定角度的曲面片上。点B位于大致平行于激光束的表面贴片上。我们将B视为不可靠点，不选择它作为特征点。$(b)$实线段是激光可观察的对象。点A位于遮挡区域（橙色虚线段）的边界上，可以检测为边缘点。然而，如果从不同的角度观察，被遮挡的区域可能会改变并变得可见，边缘点A就会消失，也是不可靠点。

总之，特征点被选择为从最大c值开始的边缘点和从最小c值开始的平面点，如果选择了点，

• 选定边缘点或平面点的数量不能超过子面域的最大值，以及
• 未选择其周围的任何点（同一个scan），并且
• 它不能位于大致平行于激光束的表面贴片上，也不能位于遮挡区域的边界上。

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

图5:

从道路中获取的激光雷达云中提取边缘点（黄色）和平面点（红色）的示例。同时，激光雷达以0.5m/s的速度向图形左侧的墙壁移动，这会导致墙壁上的运动变形。

B. Finding Feature Point Correspondence

里程计算法估计激光雷达在sweep范围内的运动。设$t_k$为sweep $k$的开始时间。在每次sweep结束时，sweep期间内感知到所有的点云${\mathcal{P}}_k$被重新投影到时间戳$t_{k+1}$，如图6所示。我们将重新投影的点云表示为${\overline{\mathcal{P}}}_k$。在下一次sweep $k+1$期间，${\overline{\mathcal{P}}}_k$与新接收的点云${\mathcal{P}}_{k+1}$一起用于估计激光雷达的运动。

图6：

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

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

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

图7$(a)$表示寻找作为边缘点的对应的边缘线的过程。点$i$为 ${\tilde{\mathcal{E}}}_{k+1}$中的一个点，边线最少由两点表示。设点$j\in {\overline{\mathcal{P}}}_k$，$j$是$i$的最近邻点，并且点$l$也是点$i$的最近邻点，和$j$是两次连续scans。线$(j，l)$构成$i$的对应边缘线。然后，为了验证$j$和$l$都是边缘点，我们根据$(1)$式检查局部曲面的平滑度。这里，要求$j$和$l$来自不同的scans，因为单个scan不能包含来自同一边缘线的多个点。

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

如果三个点在同一个scan对导致平面退化为直线，如果三个点在不同scan会存在三个点位于共线的风险，所以像$(b)$那样寻找对应平面面片

图7：

$(a)$求${\tilde{\mathcal{E}}}_{k+1}$边缘点的对应边线，$(b)$求${\tilde{\mathcal{H}}}_{k+1}$中平面点的对应平面面块。在$(a)$和$(b)$中，$j$是在${\overline{\mathcal{P}}}_k$中找到的特征点距离最近的点。橙色线表示$j$的相同scan，蓝色线表示两次连续scan。为了找到$(a)$中的边线对应关系，我们在蓝色线上找到另一个点$l$，对应边线表示为$(j，l)$。为了找到$(b)$中的平面面片，我们分别在橙色和蓝色线上找到另外两个点$l$和$m$，表示为$(j，l，m)$。

根据找到的特征点的对应关系，现在我们导出表达式来计算从特征点到其对应关系的距离。在下一节中，我们将通过最小化特征点的总距离来恢复激光雷达运动。

我们从边缘点开始，就这边缘点$i\in {\tilde{\mathcal{E}}}_{k+1}$而言，如果$(j，l)$是对应的边线，$j,l\in {\overline{\mathcal{P}}}_k$,点到线的距离可计算为
$$d_{\mathcal{E}}=\frac{\left|\left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L\right)\times \left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,l)}^L\right)\right|}{\left|{\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right|}\text{\hspace{1em}(2)}$$

其中${\tilde{X}}_{(k+1,i)}^L,{\tilde{X}}_{(k,j)}^L,{\tilde{X}}_{(k,l)}^L$是 { L } \{L\} {L}坐标系下的点$i,j$和$l$的坐标。

然后，对于平面点$i\in {\tilde{\mathcal{H}}}_{k+1}$，如果是$(j,l,m)$它对应的平面面块，$j,l,m\in {\overline{\mathcal{P}}}_k$，则点到面的距离为：

$$d_{\mathcal{H}}=\frac{\left|\begin{array}{c}\left({\tilde{\mathbf{X}}}_{(k+1,i)}^L-{\overline{\mathbf{X}}}_{(k,j)}^L\right)\\ \left(\left({\overline{\mathbf{X}}}_{(k,j)}^L-{\overline{\mathbf{X}}}_{(k,l)}^L\right)\times \left({\overline{\mathbf{X}}}_{(k,j)}^L-{\overline{\mathbf{X}}}_{(k,m)}^L\right)\right)\end{array}\right|}{\left|\left({\overline{\mathbf{X}}}_{(k,j)}^L-{\overline{\mathbf{X}}}_{(k,l)}^L\right)\times \left({\overline{\mathbf{X}}}_{(k,j)}^L-{\overline{\mathbf{X}}}_{(k,m)}^L\right)\right|},\text{\hspace{1em}(3)}$$

其中${\tilde{X}}_{(k,m)}^L$是 { L } \{L\} {L}坐标系下点$m$的坐标，上式中分子是内积运算。

C. Motion Estimation
激光雷达运动在sweep过程中以恒定的角速度和线速度进行建模。这允许我们在sweep中对在不同时间接收到的点进行线性插值姿势变换。假设$t$是当前时间戳，$t_{k+1}$是sweep $k+1$的开始时间。假设$T_{k+1}^L$是$[t_{k+1},t]$之间的激光雷达姿态变换。$T_{k+1}^L$包含六自由度激光雷达的刚体运动，$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 } \{L\} {L}的$x，y$和$z$轴的平移，${\theta }_x,{\theta }_y,{\theta }_z$为旋转角度，遵循右手规则。$[t_{k+1},t]$检测到任意一个点$i$，$i\in {\mathcal{P}}_{k+1}$，设$t_i$为其时间戳，$T_{(k+1,i)}^L$为$[t_{k+1},t_i]$之间的姿势变换，$T_{(k+1,i)}^L$可通过$T_{k+1}^L$的线性插值计算:
$$T_{(k+1,i)}^L=\frac{t_i-t_{k+1}}{t-t_{k+1}}{T}_{k+1}^L\text{\hspace{1em}(4)}$$

计算sweep $k+1$期间的一个点的姿态

回想一下，${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$是从${\mathcal{P}}_{k+1}$中提取的边缘点和平面点集，${\tilde{\mathcal{E}}}_{k+1}$ 和 ${\tilde{\mathcal{H}}}_{k+1}$是重新投影到sweep $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=\mathbf{R}{\tilde{\mathbf{X}}}_{(k+1,i)}^L+{\mathbf{T}}_{(k+1,i)}^L(1:3)\text{\hspace{1em}(5)}$$
其中$X_{(k+1,i)}^L$是${\mathcal{E}}_{k+1}$ 或者 ${\mathcal{H}}_{k+1}$中的点$i$的坐标，${\tilde{X}}_{(k+1,i)}^L$是${\tilde{\mathcal{E}}}_{k+1}$ 或者 ${\tilde{\mathcal{H}}}_{k+1}$的坐标，${\mathbf{T}}_{(k+1,i)}^L(1:3)={\left[t_x,t_y,t_z\right]}^T$。

旋转矩阵计算：

$$\mathbf{R}=e^{\hat{\omega }\theta }=\mathbf{I}+\hat{\omega }\sin \theta +{\hat{\omega }}^2(1-\cos \theta )\text{\hspace{1em}(6)}$$
在上面的方程中，$\theta$是旋转的大小，
$$\theta =\Vert T_{(k+1,i)}^L(4:6)\Vert \text{\hspace{1em}(7)}$$
$\omega$是表示旋转方向的单位向量，
$$\omega =T_{(k+1,i)}^L(4:6)/\Vert T_{(k+1,i)}^L(4:6)\Vert \text{\hspace{1em}(8)}$$
其中$\hat{\omega }$是$\omega$的反对称矩阵。

这就是旋转的角轴（李群）的基本知识使用

回想$(2)$和$(3)$中，${\tilde{\mathcal{E}}}_{k+1}$ 和 ${\tilde{\mathcal{H}}}_{k+1}$点和它对应关系之间的距离。结合$(2)$和$(4)-(8)$。

我们可以导出${\mathcal{E}}_{k+1}$中的边缘点与相应边线之间的几何关系:
$$f_{\mathcal{E}}\left({\mathbf{X}}_{(k+1,i)}^L,{\mathbf{T}}_{k+1}^L\right)=d_{\mathcal{E}},i\in {\mathcal{E}}_{k+1}\text{\hspace{1em}(9)}$$
类似地，结合$(3)$和$(4)-(8)$，我们可以在 ${\mathcal{H}}_{k+1}$平面点和相应的平面面片之间建立另一个几何关系：
$$f_{\mathcal{H}}\left({\mathbf{X}}_{(k+1,i)}^L,{\mathbf{T}}_{k+1}^L\right)=d_{\mathcal{H}},i\in {\mathcal{H}}_{k+1}\text{\hspace{1em}(10)}$$
最后，我们使用Levenberg-Marquardt方法求解激光雷达运动[26]。叠加$(9)$和$(10)$对${\mathcal{E}}_{k+1}$ 和 ${\mathcal{H}}_{k+1}$中的每个特征点，我们得到一个非线性函数，

在代码中使用的是高斯牛顿法

$$f\left(T_{k+1}^L\right)=d\text{\hspace{1em}(11)}$$

其中，$f$的每一行对应一个特征点，$d$包含相应的距离。我们计算$f$关于${\mathbf{T}}_{k+1}^L$雅可比矩阵，表示为$\mathbf{J}$，其中$\mathbf{J}=\partial f/\partial {\mathbf{T}}_{k+1}^L$。然后，$(11)$可以通过非线性迭代求解，通过将$d$最小化到零，
$${\mathbf{T}}_{k+1}^L\leftarrow {\mathbf{T}}_{k+1}^L-{\left({\mathbf{J}}^T\mathbf{J}+\lambda \mathrm{diag}\left({\mathbf{J}}^{\mathrm{T}}\mathbf{J}\right)\right)}^{-1}{\mathbf{J}}^{\mathrm{T}}\mathbf{d}\text{\hspace{1em}(12)}$$
$\lambda$是由Levenberg-Marquardt 方法确定的因子。

D. Lidar Odometry Algorithm

激光雷达里程计算法如算法1所示。该算法将上一次sweep的点云${\overline{\mathcal{P}}}_k$、当前sweep 的增长点云${\mathcal{P}}_{k+1}$和上一次sweep得到的姿势变换作为当前sweep的输入姿态变换${\mathbf{T}}_{k+1}^L$（匀速模型）。如果开始是新sweep，则${\mathbf{T}}_{k+1}^L$设置为零（第4-6行）。然后，该算法从${\mathcal{P}}_{k+1}$特征点，在第7行构建${\mathcal{E}}_{k+1}$和${\mathcal{H}}_{k+1}$。对于每个特征点，我们从${\overline{\mathcal{P}}}_k$中找到其对应关系（边线和平面面块）。运动估计适用于稳健拟合[27]。在第15行中，算法为每个特征点分配一个双平方权重，对距离其对应关系较大的特征点分配较小的权重，距离大于阈值的特征点视为异常值并分配零权重（类似于核函数）。然后，在第16行中，姿势变换更新一次迭代。如果发现收敛或满足最大迭代次数，则非线性优化终止。如果算法到达扫描结束，则使用扫描期间的估计运动将${\mathcal{P}}_{k+1}$重新投影到时间戳$t_{k+2}$（当前lidar直接获得一帧数据，此处判断可以忽略）。否则，下一轮递归只返回变换${\mathbf{T}}_{k+1}^L$。

LIDAR 建图

建图算法的运行频率低于里程计算法，每次sweep只调用一次。在sweep $k+1$结束时，激光雷达里程计生成一个未失真的点${\overline{\mathcal{P}}}_{k+1}$，同时生成一个姿态变换${\mathbf{T}}_{k+1}^L$，也就是sweep 介于$[t_{k+1},t_{k+2}]$之间期间的激光雷达运动。建图算法在世界坐标 { W } \{W\} {W}中匹配并配准${\overline{\mathcal{P}}}_{k+1}$，如图8所示。为了解释该过程，让我们将${\mathcal{Q}}_k$定义为原有地图的点云，一直累积到sweep $k$，并将${\mathbf{T}}_k^W$定义为sweep $k$结束时刻$t_{k+1}$在地图上激光雷达的姿态。利用激光雷达里程计的输出，建图算法将${\mathbf{T}}_k^W$从$t_{k+1}$扩展到$t_{k+2}$，以获得${\mathbf{T}}_{k+1}^W$，并将${\overline{\mathcal{P}}}_{k+1}$投影到世界坐标 { W } \{W\} {W}，表示为${\overline{\mathcal{Q}}}_{k+1}$。接下来，该算法通过匹配${\mathcal{Q}}_k$和${\overline{\mathcal{Q}}}_{k+1}$来优化激光雷达姿态${\mathbf{T}}_{k+1}^W$。

图8:

建图过程的图示。蓝色曲线表示地图上的激光雷达姿态${\mathbf{T}}_k^W$，由sweep $k$处的建图算法生成。橙色曲线表示由里程计算法计算的sweep $k+1$期间的激光雷达运动${\mathbf{T}}_{k+1}^L$。通过${\mathbf{T}}_k^W$和${\mathbf{T}}_{k+1}^L$，将里程计算法发布的未失真点云投影到地图上，表示为${\overline{\mathcal{Q}}}_{k+1}$（绿色线段），并与地图上现有的点云${\mathcal{Q}}_k$（黑色线段)匹配。

里程计得到的位姿只能作为最后建图的初值，从图中绿色线段就可以看出存在较大的误差，LOAM中是通过采用帧-地图的匹配优化${\mathbf{T}}_{k+1}^W$，至于**${\mathbf{T}}_k^W$是通过第一帧作为世界坐标，所有相对运动（位姿）${\mathbf{T}}_k^L$累乘即可得到。**

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

为了计算从特征点到其对应点的距离，我们在边线上选择两个点，在平面面片上选择三个点。这允许使用与$(2)$和$(3)$相同的公式计算距离。然后，将每个特征点的方程导出为$(9)$或$(10)$，但不同之处在于${\overline{\mathcal{Q}}}_{k+1}$中的所有点共享相同的时间戳$t_{k+2}$。通过Levenberg-Marquardt方法[26]，通过稳健拟合[27]再次解决非线性优化问题，并将${\overline{\mathcal{Q}}}_{k+1}$添加到地图上。为了点的均匀分布，地图云通过体素网格滤波[28]缩小，体素大小为边长5cm立方体。

后端就改变了匹配的方式，相对于帧-帧匹配，帧-地图的匹配精度更高。由于地图较大，所有通过体素滤波降采样。

姿势变换的集成如图9所示。蓝色区域表示每次sweep生成一次的激光雷达地图的姿势输出，${\mathbf{T}}_k^W$。橙色区域表示激光雷达里程计的变换输出，${\mathbf{T}}_{k+1}^L$，频率约为10Hz。地图的激光雷达姿态是两个变换的组合，频率与激光雷达里程计相同。

图9:
姿势变换的集成。蓝色区域显示了每次sweep通过建图算法得到的激光雷达姿态，${\mathbf{T}}_k^W$。橙色区域是由里程计算法计算的当前sweep 内的激光雷达运动（相对运动），${\mathbf{T}}_{k+1}^L$。激光雷达的运动估计是两种变换的组合，频率与${\mathbf{T}}_{k+1}^L$相同。