激光雷达 LOAM 论文 解析

转自：https://blog.csdn.net/hltt3838/article/details/109261334

固态激光雷达的一段视频：https://v.qq.com/x/page/a0787ajele3.html

注意：本人实验室买的是Velodyne VLP-16激光雷和 LOAM 论文中作者用的不一样，在介绍论文之前先介绍一下激光雷达的工作原路，这样更容易理解激光雷达的工作过程，其实物图如下图1所示：

图1 Velodyne VLP-16激光雷 实物图

简介：

• Velodyne PUCK（VLP-16）16通道
• TOF测量距离，最远100m，精度3cm
• 垂直分辨率2°
• 水平分辨率600、1200RPM对应0.2°、0.4°
• 旋转速率5-20Hz
• 最大点云数据量 360/0.4161200/60=288000（30万）

相关概念

谢谢Robots.https://blog.csdn.net/i_robots/article/details/108331306#comments_13621877

首先明确作者论文中用的二维激光雷达只是水平方向旋转扫描，此时一次水平扫描完成的数据为一次scan；三维激光雷达（多线雷达，激光束在垂直方向按照时间间隔依次发射）先垂直扫描再水平旋转实现三维扫描，此时一次三维扫描称为一次scan。虽然二维激光和三维激光都有一次scan的术语，但是对应的含义只是说明一次完整的扫描过程，实际表示的数据并不一样。原论文作者使用二维激光实现三维激光，需要添加机械结构实现垂直扫描，这样就会导致多个scan才是我们需要的三维扫描，那么这多个scan称为一次sweep。也就是，这里的一次sweep对应于三维激光中的一次scan，二维激光的一次scan不等于三维激光的一次scan

疑问：16线激光雷达是同时发射的还是间隔发射？

答：

一、Velodyne VLP-16激光雷达工作原理

VLP-16型号雷达具有在垂直方向上 16线 的激光束，激光雷达在采集三维数据时，每一步的旋转（旋转频率可设定）可在空间上采集16个点的三维数据。在垂直方向上的视角范围为-15度到+15度，每个激光束对应的角度分别为如下图所示：

图2 Velodyne VLP-16 ID和原理图

16线 的激光束在每一时刻都会测量出对应的距离，而且每个激光束的角度也是知道的，那么怎么把 角度和距离 信息转化为三维坐标XYZ值，如下图所示：

其中R的值为激光雷达测得的与障碍物的直线距离，其中垂直角度ω可通过查表方法获得，每个激光束对应的角度ω是固定的本文第一张图所示，而α是旋转角度值。已知旋转角度α、垂直角度ω和R 通过图中所述的公式即可求得XYZ值，这便是3D激光雷达测量环境三维信心的原理。

注意：

vlp16的坐标系为 右前上，并且扫描时  顺时针旋转，通过安装可以将坐标系变换为 前左上，因此我们在数据处理时只需要将 前左上 坐标系的点云数据转换为 左上前 坐标系即可。因为，在LOAM中，使用的坐标系是 左上前 坐标系，也就是X指向左，Y指向上，Z指向前。

图3 Velodyne VLP-16 坐标系

二、各部分论文分析

2.1、 摘要

目标：

使用一个三维空间中运动的 两轴单线激光雷达 来构建 实时激光里程计并建图

难点：

点云不是相同时间获得的，因为点云中的点随着激光雷达运动会产生运动畸变，也就是点云中的点会相对实际环境中的物品表面上的点存在位置上的误差。这种运动畸变会造成点云在匹配时发生错误，从而不能正确获得两帧点云的相对位置关系也就无法获得正确的里程计信息。

解决算法：

一方面执行高频率的里程计但是低精度的运动估计用于 定位；另一方面算法在比定位低一个数量级的频率执行匹配和注册点云信息，实现建图和校正里程计

2.2、 Introduction

激光雷达的优势: 测距频率高、准确，并测量产生相对固定的误差且与距离无关

激光雷达的缺点: 定位和建图的复杂度、运动畸变问题，也就是 低漂移和低计算量 问题

解决方法：

1、特征点匹配

匹配问题分为scan-to-scan、scan-to-map、map-to-map。scan-to-scan匹配：优点是计算量小速度快，缺点是误差累计大，长距离误差累计后地图就无法看了; map-to-map的匹配：优点是精度高，误差累计小；缺点就是计算量大，实时性压力大。scan-to-map的匹配居中。LOAM的优点就是很好地利用力匹配的优缺点，首先，虽然scan-to-scan匹配精度差，但是我们可以只是使用它做一个获取粗的里程计，用获取的结果用于去除匀速运动造成的运动畸变，由于scan-to-scan的计算量较小因此我们可以高频执行。其次，有了里程计校正后的点云数据，接下来我们就可以做一个map-to-map的匹配了。但是map-to-map存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。

2、特征点的选取

特征点提取方法：特征向量、直方图、旋转图片等，方法好不好，好但是计算量大。 作者提出了新颖的方法就是提取边缘点和平面点作为特征点来使用，这种提取的方法就很简单只需要计算一个点前后五个点就可以得到该点的曲率，计算量就减少了很多

图4 问题 和 改正结果

2.3、 相关工作

激光的转速 相对  激光雷达本体运动的速度  高很多的话就可以忽略由于运动造成的 运动畸变，那么直接使用标准的ICP就可以完成两帧点云匹配。实际情况下：需要有 速度估计 来去除运动造成的运动畸变。如果激光雷达的旋转速度相对较慢，则会产生非常严重的 运动畸变。特别是对两轴激光雷达，其中一个轴的速度相对较慢。通常使用其他的传感器获得运动速度用于去除运动畸变。例如可以使用视觉里程计和IMU进行插值去除运动畸变，当使用多传感器时可以使用卡尔曼进行滤波。这样就可以获得实时的地图用于机器人导航时的路径规划和避障。如果没有其它传感器辅助，那么运动估计和畸变校正就是一个难题。常用的方法是使用激光点云的强度构建图像，根据两帧数据间图像的位移估计地面车辆的速度，车辆是基于 匀速运动 模型进行计算 （也是需要改进的地方，毕竟匀速太理想了）。

所谓的畸变，如下图所示：

图5 激光雷达运动畸变图

畸变描述：单线激光雷达为例。激光雷达没旋转一周返回一帧数据，.如上图左边所示我们使用单线激光对环境进行观测。激光雷达静止的话激光返回的点云应该如上图中右边图中的 红色 点云，但是激光在获取一帧点云的过程中时不断运动的，这就造成  一帧点云数据中越早获得的点相对其对应的真实位置偏移量越大，形成上图右边的蓝色线。上图中激光雷达 朝向障碍物运动，旋向为逆时针，这样蓝色点云右边点在激光雷达运动方向上的畸变就要大于左边点的偏移量。

解释：是因为，先接受的点，距离远，由于运动产生的误差大；而近的点也会有误差，但是相对远距离来说小一点，因为激光的速度是不变的，发射激光的位置和接受的位置不在一个地方，你可以用一个极限的思想去考虑这个问题，不知道本人理解的对不对！

2.4、表示和任务描述

任务：使用激光雷达获得的点云数据进行运动估计并构建遍历过环境的地图

注意：局部坐标系中，x轴指向左边，y轴指向上，z轴指向前（这里需要注意和通常传感器安装的方向不一样，全局坐标系在初始时刻和局部坐标系相同

解决问题：给定点云计算每次扫描的运动并使用点云构建地图

2.5、系统概括

SLAM分为两个部分：10Hz的激光里程计和1Hz的建图
激光里程计使用基于特征点进行相邻点云帧数据匹配，
激光建图使用基于特征点进行校正后输入点云数据和地图拼接。

程序流程描述：

图6 系统软件结构图

解释：

和源码中代码构架一样，激光里程计主要分四部分完成。 1 、首先 是获得激光雷达坐标系下的点云数据P^， 2 、然后 把第k次扫描获得的点云组成一帧数据Pk 3 、然后 将Pk在两个算法中进行处理，也就是上面Liar Odometry节点和Lidar Mapping节点。 Liar Odometry节点的作用是获取两帧连续点云数据间的运动，估计出来的运动用于去除Pk中的运动畸变。 这个节点执行的频率为10Hz，作用相当于scan-to-scan匹配获得粗糙的运动估计用于去除匀速运动造成的运动畸变，并将处理后的结果给了Lidar Mapping节点做进一步处理。 Lidar Mapping节点使用地图去匹配和注册没有畸变的点云数据以1Hz的频率 。 4 、最后 由Transform integration节点接收前面两个节点输出的Transform信息并将其进行融合处理以活动频率为10Hz的Transform信息即里程计。

A、特征点提取

使用的特征点和对应的特征区域有两类：

1. 使用 边缘点 以及边缘点所在边缘线，
2. 使用 平面点 以及平面点所在平面。

怎么对边缘点和平面点进行分类呢？设S是第k 帧点云数据中点i 对应X(k,i)_L​附近的一个连续点集合。定义一个“距离值”c 来评估局部曲面的平滑度，用于边缘特征点、平面特征点和普通点的分类：

理解式(1)的意思，为什么式(1)的大小可以分辨出边缘点和平面点？因为如果是边缘点，那么i点与周围点的差值一定会变大，因为激光测的是距离呀，比如凹凸的地方和平滑的面显然不一样呀，理解一下；个人感觉也可以取一个窗口，或者半径的圆，计算 i点的光滑度c。这里的 X(k,j)_L 代表的是 在局部坐标系下，第j束激光 第k次sweep的点云数据  测量的位置（XYZ）， 哈哈，可惜的是程序和这个公式不是对应的，你们可以看看

• 扫描中的点根据c 值进行排序，然后用最大c 值（即边缘点）和最小c 值（即平面点）选择特征点。
• 一次扫描分为四个均等的区域。每个区域中最多可提供2个边缘点和4个平面点。
• 不能位于与激光束大致平行的曲面片上
• 不能位于遮挡区域的边界上

综上所述：
从最大c 值开始选择特征点作为边缘点，从最小c 值开始选择平面点，每个区域中边缘点不超过2个，平面点不超过4个，选中点周围的点不能被选择 且 不能选择位于与激光束大致平行的曲面上的点 以及位于遮挡区域的边界上的点

图7 遮挡

图8 平面点和边缘点（红色为平面特征点，黄色为边缘特征点）

B、特征点匹配关联

Pk​表示第k次sweep的点云数据, Pk​开始的时间戳为tk ​，结束的时间戳为t k + 1，经过畸变校正之后将Pk 重投影到t k + 1 得到P ˉ k ​。估计激光雷达运动时，使用的是重投影点云数据(经过畸变校正)P ˉ k 和新的点云数据（未经过畸变校正）P k + 1 , 他们代表的线如下图所示：

图9 Reprojecting point cloud to the end of a sweep. The blue colored line segment represents the point cloud perceived during sweep k , P k . At the end of sweep k , P k is reprojected to time stamp t k +1 to obtain P¯k (the green colored line segment). Then, during sweep k + 1 , P¯k and the newly perceived point cloud P k +1 (the orange colored line segment) are used together to estimate the lidar motion.

图10 Finding an edge line as the correspondence for an edge point in E˜k+1 (a), and a planar patch as the correspondence for a planar point in H˜k+1 (b). In both (a) and (b), j is the closest point to the feature point, found in P¯k. The orange colored lines represent the same scan of j, and the blue colored lines are the two consecutive scans. To fifind the edge line correspondence in (a), we fifind another point, l, on the blue colored lines, and the correspondence is represented as (j, l). To fifind the planar patch correspondence in (b), we fifind another two points, l and m, on the orange and blue colored lines, respectively. The correspondence is (j, l, m).

有了点到线和点到面的对应关系，接下来就要求点到线和点到面的 距离。首先求点到线段的距离，公式如下图所示：

1、点到线的理解图

图11 点到线的理解图

接下来求平面点到对应平面的距离，距离公式如下图所示：

公式的分子包括两部分，上边是获得一个向量，下边也是获得一个向量，但两个向量叉乘再取模就表示的求下边得到三角形面积上面表示立方体的高，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面条，得到的高就是平面点到平面的距离。直观上的理解如下图所示:

2、 点到面的理解图

图12 点到面的理解图

C、运动估计

为了获得这一帧数据中的点和上一帧数据中点的对应关系，我们使用一个旋转矩阵R和一个平移量T表示。

这里需要把时间点对齐，才能求解上面的点到线和点到面的距离d ！

由于旋转矩阵求导非常复杂，所以用罗德里杰斯公式将旋转矩阵R展开为:     
                                                              
这样就很容易对旋转矩阵进行求导。
到此我们有了点到线和面的距离，并获得用于优化的误差函数：
                                                                                         

这是一个非线性优化问题，可以通过 GN 求解，直到d最小化

f中的每一行代表一个特征点，接下就要求解雅克比矩阵，最后使用LM的方法进行优化。
                                                            
优化的过程中是对每一次迭代都寻找一次最近邻的点。
此时如果当前帧计算完成，则将当前的P k + 1 P_{k+1}Pk+1​点云则在t k + 2 t_{k+2}tk+2​时刻 转换到激光世界坐标系下。

激光雷达里程计具体的流程

图13激光雷达里程计具体的流程

2.6、建图

累计一定帧数目的点云数据才会进行建图，频率较低，但是精度很高。目的是将点云数据融入到世界地图中，精确估计激光在世界坐标系的位姿。与里程计相比，这里构建边缘线，平面时使用的点数大大增加，因此需要采用其他方法得到线向量，法向量。

因为里程计使用的点数较少，因此对于线，使用两个点就可以确定这个线向量，但是对于建图部分使用的点数较多，不能直接得到线向量，对于这个问题如何结果，作者使用了协方差矩阵。

根据数学知识，可以得到一个区域的三维点坐标分布与这些点三维坐标形成的协方差矩阵是存在一定关系的。对应的话就是

对于在近似一条线上的点，它们的协方差矩阵中特征值存在两小一大（λ 1 > > λ 2 > λ 3 ），其中λ 1  对应的特征向量就是这些点所在的线向量。对于在近似平面的点，它们的协方差矩阵中特征值存在两大一小（λ 1 > λ 2 > > λ 3 ​），其中λ 3 ​对应的特征向量就是这些点所在平面的法向量。

LOAM的优点：

将里程计和建图分隔开，一个高频低精(处理每次帧数据)，一个低频高精(累积一定次数)
整理框架是串行结构，将整个问题逐步划分为多个层次。
可实时建图的开源3D Lidar SLAM

LOAM的缺点：

点云特征处理可进一步改善：运动的人等物体
回环检测
假设匀速运动模型

三、参考资料

https://www.zhihu.com/people/li-shuai-xin-36/posts

https://blog.csdn.net/i_robots/article/details/108331306