https://www.zhihu.com/video/1577308476673560576
作者：自动驾驶人 | 原文出处： 公众号【自动驾驶专栏】
摘要
本文提出了一种使用6自由度运动的两轴激光雷达的距离测量结果来实现实时里程计和建图的方法。这个问题很困难，因为激光雷达的距离测量结果是在不同时刻收到的，并且运动估计的误差将导致目标点云误匹配。至今为止，离线的批量方法能够建立清晰的3D地图，通常使用闭环检测来校正随时间的漂移。本文提出的方法能够在不需要高精度测距或者惯性测量单元的情况下，实现低漂移和低计算复杂度的定位和建图。为了取得这样的性能，本文的核心思想是将同时定位和建图这个同时优化大量变量的复杂问题分离为两种算法。一种算法是实现高频率但低精度的里程计来估计激光雷达速度，另一种算法则实现低频率但高精度的点云匹配与配准。这两种算法的组合使得建图方法可以实时运行。该方法已经被大量实验和 KITTI 里程计数据集所评估和验证。结果表明，该方法能够与目前（论文创作时）最先进的离线批量方法达到同级别的精度。
介绍
3D建图是一种常用的技术。使用激光雷达建图很常见，因为激光雷达能够提供高频率的距离测量，且该测量误差相对固定，不受测量距离的影响。如果激光雷达唯一的运动只是旋转一条激光束时，点云配准就较为简单。然而，如果激光雷达在很多有趣的应用中需要运动那样，精确的建图需要在连续的激光测距过程中获取激光雷达的位姿。一种常用的解决该问题的方式为使用独立的位置估计（例如通过GPS/INS）将激光点云配准到一个固定的坐标系统。另外的一些位置估计方法通过使用来自轮子编码器或者视觉里程计系统的里程计测量来配准激光点云。由于里程计累计了随时间变化的微小增量运动，因此必定会发生漂移，更多的关注应该致力于减少累计漂移（例如使用回环检测）。
本文考虑了使用6自由度运动的2轴激光雷达通过低漂移的里程计来创建地图的情况。使用激光雷达的一个关键优势在于它对场景中的环境光照和光学纹理不敏感。激光雷达近来的发展已经在减小它们的尺寸和重量。激光雷达可以被人手持来穿过环境，或者装载在无人机上。由于本文提出的方法目的在于解决与最小化里程计漂移相关的问题，因此该方法目前没有包含回环检测模块。
本文提出的方法在没有高精度测距或者惯性测量的情况下达到了低漂移且低复杂度的性能。获取这样性能的关键思想是将同时定位和建图这个同时优化大量变量的典型复杂问题分离为两种算法（里程计算法和建图算法）。一种算法（里程计算法）是实现高频率但低精度的里程计来估计激光雷达速度，另一种算法（建图算法）则实现低频率但高精度的点云匹配与配准。尽管本文提出的方法不需要IMU，但是如果有IMU可用，则可以为高频运动提供运动先验。具体而言，这两种算法提取位于边缘和平面上的特征点，并且将特征点分别与边缘线段和平面块进行匹配。里程计算法通过快速计算来寻找特征点的匹配关系，建图算法通过与局部点聚类的几何分布相关的特征值和特征向量来寻找特征点的匹配关系。
通过分解原始问题，可以获得更简单的问题，该问题是可以通过在线运动估计（里程计算法）来求解的。在此之后，建图算法作为批量优化算法（类似于迭代最近点 ICP 方法）来得到高精度的运动估计和地图。这种并行的算法结构保证了问题实时求解的可行性。进一步，由于运动估计（里程计算法）是高频运行的，建图算法就有充足时间来计算以保证精度；建图算法由于运行频率较低，因此能够包含大量特征点，并且通过足够多的迭代次数来达到收敛。
符号约定和任务描述
本文解决的问题是通过3D激光雷达感知到的点云来实现自身运动估计并且建立穿行环境的地图。需要确保激光雷达预先标定过，同时也要保证激光雷达的角速度和线速度是随时间光滑且连续的，没有发生突变。作为本文的符号约定，使用大写的右上角标来表示坐标系统，并且定义一个sweep为激光雷达完成一次扫描收敛（即一周圈360°）。本文使用右下角标来表示第次sweep，并且使用表示第次sweep接收到的点云。下面定义两种坐标系统：
1）激光雷达坐标系统{}是一个3D坐标系统，它的原点位于激光雷达的几何中心。该坐标系统的x轴指向左方，y轴指向上方，z轴指向前方。表示在坐标系统{}中点云中第i个点的坐标；
2）世界坐标系统{}是一个与激光雷达坐标系统{}在初始位置对齐的3D坐标系统。表示在坐标系统{}中点云中第i个点的坐标。
在上面的约定和表示下，激光雷达里程计和建图问题被定义为：给定一系列的激光雷达点云，计算第次sweep的激光雷达自身运动，并且通过建立穿行环境的地图。
系统概述
A.激光雷达硬件
本文的研究验证于但不受限于Hokuyo UTM-30LX激光雷达，通过它采集数据来验证本文提出的方法。该激光雷达具有180°的视场角，0.25°的分辨率以及40线/秒的扫描频率，它与一个马达连接，马达在-90°和90°之间以180°/s的角速度控制激光雷达旋转。基于这样特定的装置，一个sweep就是从-90°到90°的一次旋转（或者相反方向）。这里值得注意的是，对于一个连续旋转的激光雷达，一个sweep仅仅只是一个半球形旋转。一个装载在机体上的编码器以0.25°的角分辨率测量马达的旋转角度，根据旋转角度可以将激光点云投影到激光雷达坐标系{}中。
B.软件系统概述
下图为软件系统示意图。使用表示某一时刻激光扫描接收到的点云。在每个sweep中，点云是在激光雷达坐标系{}中获取的。将第k次sweep中所有时刻的点云组合起来形成点云，它在两种算法（里程计算法和建图算法）中进行处理。激光雷达里程计算法获取点云，并且计算两个连续sweep之间激光雷达的运动，使用估计得到的运动来纠正点云中的畸变，该算法以10Hz频率运行。里程计算法的输出结果被激光建图算法进一步处理，建图算法以1Hz的频率将去畸变后的点云与地图进行匹配和配准。最后，将两种算法得到的位姿变换进行融合，以10Hz的频率输出融合后的激光雷达相对于地图的位姿变换。
激光雷达里程计算法
A.特征点提取
本文从激光雷达点云中提取特征点开始介绍。激光雷达自然地生成非均匀分布点云，它在一次扫描中返回点云结果的分辨率为0.25°，这些点云位于一个扫描平面内。然而，当激光雷达以180°/s的角速度旋转并且以40Hz的频率进行扫描时，在垂直于扫描平面方向的精度为180°/40=4.5°。根据这一事实，从点云中提取的特征点根据共面几何关系，仅使用来自单次扫描的信息。 本文从边缘线和平面小块提取特征点。定义为点云中的一个点，并定义为激光雷达返回的与点在同一条扫描线上的连续点。由于激光雷达以顺时针或者逆时针顺序生成点，在点两边各包含一半点，两个连续点之间的间隔为0.25°。定义如下项来评估局部表面的平滑度（用来提取边缘点和平面点）：将一条扫描线上的点按值大小进行排序，将值最大的那些点作为边缘特征点，值最小的那些点作为平面特征点。为了使环境中的特征点均匀分布，将一条扫描线划分为4个相同子区域（代码中为6个子区域），从每个子区域中提取出最多2个边缘点和4个平面点。点能够被作为边缘点或者平面点，当且仅当它的值大于或者小于一个阈值并且所选特征点的数量不超过设定的最大值。当选择特征点时，一般避免在已选特征点附近进行选择，或者避免在与激光束大致平行的局部平面表面进行选择（例如下图(a)中点B），这些点通常认为是不可靠的。同样的，也需要避免选择遮挡区域边界上的点作为特征点（例如下图(b)中点A），点A被误认为激光雷达点云中边缘点，因为它的连接平面（虚线段）被其它物体所遮挡。然而，如果激光雷达移动到另一视角下，被遮挡区域就会发生变化并且变得可观。为了避免提取到之前提到的这些点，对中的点进行检查，只有当形成的平面与激光束不平行，并且中不存在比点离激光雷达更近且与点连线在激光束方向的点。
综上所述，从最大的值开始选择边缘点，从最小的值开始选择平面点，并且这些特征点还需满足如下条件：1）所选边缘点或者平面点的数量不超过子区域内设定的最大数量；2）已选特征点附近的点不能作为特征点；3）特征点不能在与激光束几乎平行的平面上，并且不能在被遮挡区域的边界上提取。在一个走廊场景下提取特征点的示例如下图所示，图中边缘点和平面点分别标记为黄色和红色。
B.特征点匹配
里程计算法估计一个sweep中激光雷达的运动，记为第次sweep的起始时间。在每次sweep结束的时候，将这次sweep接收到的点云重投影到时刻，如下图所示。将投影后的点云记作。在下个次sweep中，和新接收到的点云一起用来估计激光雷达的运动。
假设现在同时有和，下面开始寻找这两个点云之间的匹配关系。使用上一小节讨论的方法提取激光雷达点云中的边缘点和平面点，记和分别为提取到的边缘点和平面点。寻找点云中的边缘线作为的匹配关系，以及点云中的平面小块作为的匹配关系。注意在次sweep开始的时候，是个空的集合，它的规模随着扫描过程中接收到更多的点而增长。激光雷达里程计在sweep过程中迭代地估计自身6自由度运动。在每次迭代过程中，和使用当前估计的位姿变换投影到当前sweep的起始时刻，记和为投影后的点集。对于和中的每个点，寻找中对应的最近邻点，这里被存储在3D KD树中以实现快速索引。下图展示了寻找边缘点对应的边缘线的过程，记为中的一个点，边缘线可以用两个点表示。记点为中离点最近邻的点，点为中与点在不同扫描线且离点最近邻的点。组成了与点对应的边缘线，为了验证和均为边缘点，需要使用公式（1）检查局部表面的平滑性。这里特别要求点和点来自不同的扫描线，因为同一条扫描线不可能包含相同边缘线上超过一个点。唯一的例外就是边缘线就在扫描平面上，如果是这种情况，则边缘线发生退化，并且在扫描平面上为一条直线，这样在这条边缘线上的特征点就不会被提取到（因为平滑性的计算是基于同一条扫描线的）。
下图展示了寻找平面点对应的平面小块的过程，记为中的一个点，平面小块可以用三个点表示。类似于之前寻找边缘点对应关系，记点为中离点最近邻的点，接着寻找另外两个点（点和点）作为点的最近邻点，点要求和点在同一条扫描线上，点要求在与点在相邻的扫描线上，这样保证了三个点非共线。为了验证点，点和点均为平面点，再次检查局部表面的平滑性。
找到特征点的匹配关系后，就可以获得计算特征点到其匹配关系的距离表达式。在下一小节中将通过最小化所有特征点距离误差来估计激光雷达运动。下面先从边缘点开始，对于一个点，如果是其对应的边缘线，则边缘点到边缘线的距离可以通过下式计算：其中，，和分别为激光雷达坐标系{}中点，和的坐标。 对于一个点，如果是其对应的平面小块，则平面点到平面小块的距离可以通过下式计算：其中，为激光雷达坐标系{}中点的坐标。
C.运动估计
激光雷达在一次sweep中运动由恒定角速度和线速度来建模，这使得对于一次sweep中在不同时刻接收到的点云可以通过线性插值位姿变换来统一到同一坐标系下（点云去畸变）。记为当前时间戳，为第次sweep的起始时间，为时间段内的激光雷达位姿变换，它是6自由度的刚体运动，。其中，，和分别为激光雷达坐标系{}下沿，和方向的移动，，和为旋转角度（遵循右手规则）。给定中一个点，记为它的时间戳，并且记为时间段内的激光雷达位姿变换。能够通过对线性插值得到：回忆上一小节，和分别是从中提取的边缘点和平面点集合，和是投影到sweep起始时刻的点集。为了求解激光雷达的运动，需要建立和，或者和之间的几何关系。根据公式(4)中的变换关系，可以计算：其中，为集合和中点的坐标，为集合和中对应点的坐标，为中第个到第个元素，为通过罗德里格斯公式定义的旋转矩阵：在上式中，是旋转向量的模长：表示旋转方向的单位向量：是的反对称矩阵。公式(2)和(3)计算了和中的点和它们对应关系之间的距离。根据公式(2)和公式(4)-(8)，可以得到中的边缘点和对应边缘线之间的几何关系：类似地，根据公式(3)和公式(4)-(8)，可以建立另一个中平面点和对应平面小块之间的几何关系：最后，使用Levenberg-Marquardt方法求解激光雷达的运动。根据和中每个特征点将公式(9)和公式(10)堆叠成向量形式，获得一个非线性函数：上式中，每一行对应一个特征点，表示对应的距离。下面计算相对于的雅可比矩阵，。接着就可以通过非线性迭代最小化为0来求解公式(11)：上式中，是Levenberg-Marquardt方法的一个因子。
D.激光雷达里程计算法
激光雷达里程计算法如上表所示。该算法的输入为来自上一次sweep的点云，当前sweep中逐渐增长的点云，以及上一次迭代的位姿变换。如果一个新的sweep刚开始，设置为0（上表中4-6行）。接着，算法从点云中提取特征点来构建边缘点和平面点（上表中第7行）。对于每个特征点，寻找它在点云中的对应关系（上表中9-14行）。在上表中第15行，算法给每个特征点分配了平方权重，特征点和它们对应关系的距离误差越大则分配权重越小，如果该距离误差大于一定阈值，则认为该特征点是外点并分配权重为0。在上表中第16行，位姿变换经过一次迭代更新，如果满足收敛条件或者超过最大迭代次数，则非线性优化终止。如果算法结束了一次sweep，使用这个sweep内估计的运动将点云投影到时间戳，否则返回位姿变换用于下一次迭代。
