论文笔记—LOAM: Lidar Odometry and Mapping in Real-time
论文笔记—LOAM: Lidar Odometry and Mapping in Real-time
文章摘要
~~~~~~~本文提出了一种实时的测距和建图方法，该方法使用了以6-DOF运动的2轴激光雷达的距离测量值 。该问题之所以棘手，是因为在不同的时间接收到距离测量值，并且运动估计中的错误可能导致结果点云的配准错误。迄今为止，可以通过离线批处理方法来构建连贯的3D地图，通常使用闭环来校正随时间的漂移。 本文的方法可实现低漂移和低计算复杂度，而无需进行高精度测距或惯性测量。获得此性能级别的关键思想是对SLAM的复杂问题进行划分，以寻求通过两种算法同时地优化处理大量变量。 一种算法以高频率但低保真度执行测距法以估计激光雷达的速度。 另一个算法以较低的数量级频率运行，以进行点云的精确匹配和配准。两种算法的组合使该方法可以实时建图。
导语
~~~~~~~ 激光雷达的制图是很常见的，因为激光雷达可以提供高频范围的测量，而无论测得的距离如何，误差都相对恒定。在激光雷达的唯一运动是旋转激光束的情况下，点云的配准很简单。 但是，如果激光雷达本身像许多感兴趣的应用一样在运动，则准确的建图需要了解连续激光测距期间的激光雷达位姿。解决该问题的一种常用方法是使用独立的位姿估计（例如，通过GPS / INS）进行配准，使激光点指向一个固定的坐标系。另一组方法使用测距法测量，例如来自车轮编码器或视觉测距系统的测距配准激光点。由于里程表会随着时间的推移整合较小的增量运动，因此里程表势必会漂移，因此会特别注意减少漂移（例如使用闭环）。
本文考虑使用以6自由度移动的2轴激光雷达以低漂移测距法创建地图的情况。 使用激光雷达的主要优势在于它对环境光线和场景中的光学纹理不敏感。激光雷达的最新发展已减少了激光雷达的尺寸和重量。由于本文的方法旨在解决里程表估算中与最小化漂移有关的问题，因此目前不涉及回路闭合。
该方法不需要低漂移和低计算复杂度，而无需高精度测距或惯性测量。获得这种性能水平的关键思想是同时定位和建图（SLAM）这个典型的复杂问题的划分，该问题试图通过两种算法来同时优化大量变量。一种算法以高频率但低保真度执行测距法以估计激光雷达的速度;另一种算法以较低的数量级频率运行以实现点云的精确匹配和配准。尽管不是必需的，但如果有IMU，则可以提供先验运动来帮助说明高频运动。具体而言，这两种算法都提取位于尖锐边缘和平面上的特征点，并将特征点分别与边缘线段和平面斑块进行匹配。在里程表算法中，通过确保快速计算来找到特征点的对应关系。在建图算法中，通过检查关联的特征值和特征向量的局部点簇的几何分布来确定对应关系。
通过分解原始问题，更容易解决的问题是在线运动估计。 之后，将建图作为批处理优化（类似于迭代最近点（ICP））进行操作，以生成高精度的运动估计和地图。 并行算法结构确保了实时解决问题的可行性。此外，由于运动估计是在较高的频率下进行的，因此建图有足够的时间来保证精度。在较低的频率下运行时，建图算法能够合并大量特征点并使用足够多的迭代来收敛。
注释和任务说明
~~~~~~~本文所要解决的问题是对3D激光雷达感知到的点云进行自我运动估计，并为遍历的环境构建地图。我们假设激光雷达已经过预先校准，并且还假设了角速度和线速度 激光雷达的随时间变化是平滑且连续的，没有突然变化。
本文规定，我们使用大写字母表示坐标系。 当激光雷达完成一次扫描范围时，我们定义了一次扫描。 我们使用字母k，k∈Z^+表示扫描，而P_k表示在扫描k期间感知到的点云。 让我们如下定义两个坐标系。
1）激光雷达坐标系{L}是3D坐标系，其原点位于激光雷达的几何中心。 x轴指向左侧，y轴指向向上，z轴指向前方。 {L_k}中的点i，i∈P_k的坐标表示为 X_{(k,i)}^L。
2）世界坐标系{W}是一个3D坐标系，它与{L}在初始位置重合。 {W_k}中的点i，i∈P_k的坐标为X_{(k,i)}^W。
有了假设和符号，我们的激光测距和建图问题可以定义为：
给定一个激光雷达云P_k，k∈Z^+的序列，计算每次扫描k期间激光雷达的自我运动，并为遍历的环境用P_k绘制地图。
系统概述
A.雷达硬件
~~~~~~~本文的研究已基于但不限于基于Hokuyo UTM-30LX激光扫描仪的定制3D激光雷达进行了验证。 通过本文，将使用从激光雷达收集的数据来说明该方法。 激光扫描仪的视场为180°，分辨率为0.25°，扫描速度为40线/秒。激光扫描仪连接到电机，该电机被控制为在−90°之间以180°/ s的角速度旋转。 激光扫描仪的水平方向为零时为90º，然后为90º。在此特定单位下，扫描是从-90º到90º或反方向（持续1s）旋转。在此，请注意连续旋转激光雷达，扫描只是一个半球形的旋转。 板载编码器以0.25°的分辨率测量电动机的旋转角度，通过该分辨率，激光点被投影到激光雷达坐标{L}中。
B.软件系统概述
~~~~~~~图3示出了软件系统的图。 令\hat{P}为激光扫描中收到的点。 在每次扫描期间，\hat{P}在{L}中进行配准。 第k次扫描期间的组合点云形成P_k。 然后，P_k用两种算法进行处理。激光雷达里程计获取点云并计算两次连续扫描之间的激光雷达运动。 估计的运动用于校正P_k中的失真。 该算法以大约10Hz的频率运行。 输出由激光雷达建图进一步处理，将未失真的点云匹配并以1Hz的频率记录到地图上。 最后，将两种算法发布的姿态变换集成在一起，以生成相对于地图的激光雷达位姿约为10Hz的变换输出。
雷达里程计
A.特征点提取
~~~~~~~本文首先从激光雷达云P_k中提取特征点。 图2所示的激光雷达自然会在P_k中生成不均匀分布的点。 在扫描过程中，激光扫描仪的返回分辨率为0.25\degree。 这些点位于扫描平面上。 但是，随着激光扫描仪以180º/ s的角速度旋转并以40Hz的频率进行扫描，垂直于扫描平面的分辨率为180º/ 40 =4.5º。 考虑到这一事实，特征点是从P_k提取的，仅使用来自各个扫描的信息，并具有共面的几何关系。
我们选择位于尖锐边缘和平面表面块上的特征点。 设i为P_k中的一个点，i∈P_k，设S为同一扫描中激光扫描仪返回的i的连续点的集合。 由于激光扫描仪按CW或CCW顺序生成点返回，因此S在i的每一侧包含一半的点，并且在两个点之间包含0.25°的间隔。 定义一个术语c以评估局部表面的光滑度， 
扫描中的点基于c值进行排序，然后选择特征点，即具有最大c值的是边缘点和具有最小c值的是平面点。 为了在环境中均匀分布特征点，我们将扫描分为四个相同的子区域。 每个子区域最多可以提供2个边缘点和4个平面点。 仅当点i的c值大于或小于阈值且所选点的数量不超过最大值时，才可以将其选择为边缘或平面点。
在选择特征点时，我们要避免那些周围的点已经被选择的点或在局部平面上与激光束大致平行的点，这些点通常被认为是不可靠的。
总之，从最大c值开始选择特征点作为边缘点，从最小c值开始选择特征点作为平面点，如果选择了一个点，则：
1）所选边缘点或平面点的数量不能超过子区域的最大值，并且
2）尚未选择其周围的点，并且
3）它不能在与激光束大致平行的表面补丁上，也不能在被遮挡区域的边界上。
从走廊场景中提取的特征点的示例如图5所示。边缘点和平面点分别用黄色和红色标记。
B.寻找对应特征点
~~~~~~~视觉里程计算法估计扫描中激光雷达的运动。 令t_k为第k次扫描的开始时间。 在每次扫描结束时，将在扫描过程中感知到的点云P_k重新投影到时间戳t_{k + 1}，如图6所示。我们将重新投影的点云表示为\bar{P}_k。 在下一次扫描k + 1期间，\bar{P}_k与新接收的点云P_{k + 1}一起估计激光雷达的运动。
假设\bar{P}_k和P_{k + 1}现在都可用，并从找到两个激光雷达云之间的对应关系开始。 对于P_{k + 1}，我们使用上一部分中讨论的方法从激光雷达云中找到边缘点和平面点。 设{\Large\varepsilon}_{k + 1}和\mathcal{H}_{k + 1}分别为边缘点和平面点的集合。 我们将从\bar{P}_k中找到边缘线作为在{\Large\varepsilon}_{k + 1}中找到点的对应关系，而平面块作为\mathcal{H}_{k + 1}中那些点的对应关系。
请注意，在第k + 1扫描开始时，P_{k + 1}是一个空集，在扫描过程中随着接收到更多点而增长。激光雷达里程计递归估计扫描期间的6自由度运动，并逐渐随着P_{k + 1}的增加而包括更多的点。 在每次迭代中，使用当前估计的变换将{\Large\varepsilon}_{k + 1}和\mathcal{H}_{k + 1}重新投影到扫描的开始。 令\tilde{\Large\varepsilon}_{k + 1}和\tilde{\mathcal{H}}_{k + 1}为重投影点集。 对于\tilde{\Large\varepsilon}_{k + 1}和\tilde{\mathcal{H}}_{k + 1}中的每个点，我们将在\bar{P}_k找到最近的邻点。 这里，\bar{P}_k存储在3D KD树中，用于快速索引。
图7（a）表示找到边缘线作为边缘点的对应关系的过程。 设i为\tilde{\Large\varepsilon}_{k + 1}中的一个点，i∈ \tilde{\Large\varepsilon}_{k + 1}。 边缘线由两点表示。设j为i在\bar{P}_k的最接近的邻点，j∈\bar{P}_k，在连续两次扫描到j的扫描中，设l为i的最接近的邻点。（j，l）形成i的对应关系。 然后，为了验证j和l都是边缘点，我们基于表面粗糙度公式（1）来检查局部表面的光滑度。 在此，考虑到一次扫描不能包含来自同一边缘线的一个以上的点，我们特别要求j和l来自不同的扫描。 只有一个例外即边缘线位于扫描平面上。 但是，如果是这样，则边缘线将退化并在扫描平面上显示为直线，并且边缘线的特征点不应被首先提取。
图7（b）示出了寻找平面斑块作为平面点的对应关系的过程。 设i为\tilde{\mathcal{H}}_{k + 1}中的一个点，即i∈\tilde{\mathcal{H}}_{k + 1}。 平面斑块由三个点表示。类似于最后一段，我们在\bar{P}_k找到最接近i的邻点，记为j。 然后，我们找到另外两个点l和m，它们也是i的最接近邻点，一个点在j的同一次扫描中，另一个在连续两次扫描到j的扫描中，这保证了这三个是非线性的。为了验证j，l和m均为平面点，我们基于粗糙度公式（1）再次检查局部表面的光滑度。
利用找到的特征点的对应关系，现在我们导出表达式以计算从特征点到其对应关系的距离。 在下一部分中，我们将通过最小化特征点的整体距离来恢复激光雷达运动。我们从边缘点开始。 对于点i∈\tilde{\Large\varepsilon}_{k + 1}，如果（j，l）是对应的边缘线，j，l∈\bar{P}_k，则点到线的距离可以计算为:
其中\tilde{X}_{(k+1,i)}^L，\tilde{X}_{(k,j)}^L和\tilde{X}_{(k,l)}^L分别是\lbrace L \rbrace中点i，j和l的坐标。 那么，对于点i∈\tilde{\mathcal{H}}_{k + 1}，如果 （j，l，m）是对应的平面斑块，j，l，m∈\bar{P}k，则点到平面的距离为
其中，\tilde{X}{(k,m)}^L是\lbrace L \rbrace中点m$的坐标。
C.运动估计
~~~~~~~在扫描过程中以恒定的角速度和线速度对激光雷达运动进行建模。 这使我们可以在扫描中对不同时间接收的点进行线性内插位姿变换。 设t为当前时间戳，并回想t_{k + 1}是第k+1扫描的开始时间。设T_{k+1}^L是[t_{k + 1}，t]之间的激光雷达位姿变换。T_{k+1}^L包含6- DOF(自由度)的激光雷达的刚性运动。T_{k+1}^L= [t_x，t_y，t_z，θ_x，θ_y，θ_z] ^T，其中tx，ty和tz是分别沿\lbrace L \rbrace的x，y和z轴的平移，而θ_x，θ_y，θ_z是根据右手准则旋转的角度。给定点i，i∈P_{k + 1}，设t_i是其时间戳，设T_{(k+1,i)}^L是[t_{k + 1}，t_i]之间的位姿变换 。T_{(k+1,i)}^L可以通过T_{k+1}^L的线性插值来计算，
回想一下，{\Large\varepsilon}_{k + 1}和\mathcal{H}_{k + 1}是从P_{k + 1}提取的边缘点和平面点的集合，而\tilde{\Large\varepsilon}_{k + 1}和\tilde{\mathcal{H}}_{k + 1}是重新投影到扫描开始时t_{k + 1}的点的集合。 为了解决激光雷达运动，我们需要在{\Large\varepsilon}_{k + 1}和\tilde{\Large\varepsilon}_{k + 1}或者\mathcal{H}_{k + 1}和\tilde{\mathcal{H}}_{k + 1}之间建立几何关系。使用（4）中的变换，我们可以得出， 
其中{X}_{(k+1,i)}^L是{\Large\varepsilon}_{k + 1}或\mathcal{H}_{k + 1}中点i的坐标，\tilde{X}_{(k+1,i)}^L是\tilde{\Large\varepsilon}_{k + 1}或\tilde{\mathcal{H}}_{k + 1}中的对应点，T_{(k+1,i)}^L（a：b）是T_{(k+1,i)}的第a至b个条目，R是由Rodrigues公式定义的旋转矩阵，
在上式中，θ是旋转的大小，
ω是代表旋转方向的单位矢量，
\hat{ω}是ω的斜对称矩阵。
回想一下，（2）和（3）计算{\Large\varepsilon}_{k + 1}和\tilde{\Large\varepsilon}_{k + 1}中的点和他们对应点之间的距离。 结合（2）和（4）-（8），我们可以得出{\Large\varepsilon}_{k + 1}中的边缘点与相应边缘线之间的几何关系， 
类似地，结合（3）和（4）-（8），我们可以在\mathcal{H}_{k + 1}中的一个平面点和相应的平面斑块之间建立另一个几何关系， 
最后，我们用Levenberg-Marquardt方法解决了激光雷达的运动。 对{\Large\varepsilon}_{k + 1}和\mathcal{H}_{k + 1}中的每个特征点堆叠（9）和（10），我们得到一个非线性函数， 
其中f的每一行都对应一个特征点，而d包含相应的距离。 我们计算f相对于T_{k+1}^L的Jaco-bian矩阵，记为J，其中J =∂f/∂T_{k+1}^L。 然后，可以通过将d最小化为零，通过非线性迭代来求解（11）， 
λ是通过Levenberg-Marquardt方法确定的因子。 
D.激光雷达里程计算法
~~~~~~~激光雷达测距算法在算法1中显示。该算法将最后一次扫描的点云\bar{P}_k，当前扫描的增长点云P_{k + 1}和最后一次递归的位姿变换T_{k+1}^L作为输入。如果开始新的扫描，则将T_{k+1}^L设置为零（第4-6行）。然后，该算法从P_{k + 1}中提取特征点，以构造第7行中的{\Large\varepsilon}_{k + 1}和\mathcal{H}_{k + 1}。对于每个特征点，我们在\bar{P}_k中找到其对应关系（第9-19行）。运动估计适用于鲁棒拟合。在第15行中，算法为每个特征点分配双平方权重。与它们的对应关系具有较大距离的特征点被分配较小的权重，而距离大于阈值的特征点被视为离群点并被分配零权重。然后，在第16行中，位姿转换将更新一次。如果找到收敛或达到最大迭代次数，则非线性优化将终止。如果算法到达扫描结束，则使用扫描期间的运动估计将P_{k + 1}重新投影到时间戳t_{k + 2}。否则，仅返回变换T_{k+1}^L进行下一轮递归。
激光建图
~~~~~~~建图算法的运行频率比里程计算法低，并且每次扫描仅调用一次。在扫描k + 1次结束时，激光雷达里程计会生成未失真的点云\bar{P}_{k+1}，同时会产生位姿变换，T_{k+1}^L，包含扫描期间的激光雷达运动，介于[t_{k + 1}，t_{k + 2}]之间。 建图算法在世界坐标{W}中匹配并配准\bar{P}_{k+1}，如图8所示。为说明该过程，让我们在地图上定义\mathcal{Q}_k作为点云，累积到扫描第k次为止，而T_k^W为第k次扫描结束时，激光雷达在地图上的位姿，t_{k + 1}。 利用激光雷达里程计的输出，建图算法将T_k^W拓展为从t_{k + 1}到t_{k + 2}的一次扫描，以获得T_{k+1}^W，并将\bar{P}_{k+1}投影到世界坐标{W}中，表示为\bar\mathcal{Q}_{k+1} 。 接下来，该算法通过优化激光雷达位姿T_{k+1}^W使\bar\mathcal{Q}_{k+1}与\mathcal{Q}_k匹配。
以与第V -A节相同的方式提取特征点，但使用10次特征点。为了找到特征点的对应关系，我们将点云存储在10m立方区域的地图\mathcal{Q}_k上。提取与\bar\mathcal{Q}_{k+1}相交的立方体中的点，并将其存储在3D KD树中。我们在特征点周围的某个区域内找到\mathcal{Q}_k中的点。令\mathcal{S}\rq为一组周围的点。对于边缘点，我们仅将点保留在\mathcal{S}\rq中的边缘线上，对于平面点，我们仅将点保留在平面斑块上。然后，我们计算\mathcal{S}\rq的协方差矩阵，记为M，M的特征值和特征向量分别记为V和E。如果\mathcal{S}\rq分布在边缘线上，则V包含一个特征值明显大于其他两个特征值，并且E中与最大特征值相关的特征向量表示边缘线的方向;另一方面，如果\mathcal{S}\rq分布在平面斑块上， V包含两个较大的特征值，第三个特征值明显较小，并且E中与最小特征值相关的特征向量表示平面补丁的方向。边缘线或平面补丁的位置通过穿过\mathcal{S}\rq的几何中心来确定。
要计算从特征点到其对应点的距离，我们在一条边线上选择两个点，在一个平面面片上选择三个点。这允许使用与（2）和（3）相同的公式来计算距离。然后，针对每个特征点推导一个方程为（9）或（10），但是不同之处在于\bar\mathcal{Q}_{k+1}中的所有点共享相同的时间戳t_{k + 2}。通过Levenberg-Marquardt方法通过鲁棒拟合再次解决了非线性优化问题，\bar\mathcal{Q}_{k+1}记录在地图上。为了均匀分布点，使用5cm立方体的体素网格过滤器缩小了地图云的尺寸。
位姿变换的集成如图9所示。蓝色区域代表激光从雷达建图中输出的位姿T_k^W。每次扫描生成一次。橙色区域表示以10Hz左右的频率从激光雷达里程计输出的变换T_{k+1}^L。相对于地图的激光雷达位姿是两个变换的组合，其频率与激光雷达里程计相同。
