1. 摘要

本文提出一种实现激光雷达里程计与建图方法，使用6自由度的双轴激光雷达进行距离测量。问题的难点在于激光雷达的每一个点的时间戳都不同，运动估计的误差会影响激光点云的配准。目前相关的3D地图一般使用离线批处理方法构建，且使用闭环来较正漂移。本文在不需要高精度测量和惯性测量的情况下实现了低漂移和低计算复杂度。获取如此高性能的关键是把复杂SLAM问题拆分。SLAM需要同时优化大量的变量。用两种算法，其一对激光雷达的但低置信度的速度估计；另一个算法低频运行，完成点云的精细匹配与对准。它们共同实现了系统的实时运行。

2. 引言

本文创新地提出了通过两种算法对SLAM问题进行划分。里程计算法通过前后两帧点云之间的匹配，高频率但低精度地估计机器人位姿。建图算法低频但高精度地匹配当前帧点云与地图，得到精度更高的里程计数据，并构造点云地图

本算法所要解决的问题描述：给定一个点云序列，包含每次扫描的点云，据此计算激光雷达的自运动，并构建环境地图。

3. 系统概述

本节介绍了实验的硬件和软件流程

3.1. 激光雷达硬件

论文采用的激光雷达比较特殊，是一个2D激光雷达加一个电机扫描得到3维点云。

2D激光雷达具有180°视野，0.25°分辨率，40行/秒的扫描速度。电机每秒在-90°到90°之间旋转，因此该“3D”激光雷达为40线，线间角分辨率为4.5°，半球形扫描面。

3.2. 软件系统概述

对雷达扫描得到的点云，首先进行畸变去除，再提取特征点，组合成一帧点云。

之后通过两帧之间匹配，得到雷达里程计数据，以10Hz发布。

接下来，将当前帧点云与局部子地图进行匹配，得到更精确的里程计数据，并更新地图，以1Hz发布融合雷达里程计数据和地图匹配数据，得到位姿变换，并以10Hz速率发布。

4. 雷达里程计（Lidar Odometry）

4.1. 特征点提取

首先从激光点云 $P_k$ 中提取特征点。激光雷达产生非均匀分布的激光点云 $P_k$ 。激光雷达单次扫描的分辨率是0.25度。这些激光点在一个扫描平面内。由于激光雷达旋转的角速度是180度/秒，采样频率是40Hz，故垂直方向的分辨率是4.5度。仅使用单次扫描的点云提取到的特征点有共面几何关系。

选择比较锋利的边缘和平面块作为特征点。设i为 $P_k$ 中的一个点，S是它的一个邻域。定义平滑性如下

根据平滑度进行排序，平滑度值最大的即为边缘点，平滑度值最小的即为平面点。

将一次扫描分为四个子区域，每个区域最多选择两个边缘点，四个平面点，且要保证点的平滑度符合设定的阈值。

选要点了避免选取的特征点相邻较近，又要避免平面与激光方向平行。这些点通常都是不可靠的。

不选择与激光束方向平行的点，如下图(a)中的B点。

在激光束方向上，点集S中没有点与点i有缝隙（不选择可能有遮挡的点），如下图(b)中的A点。

总的来说，根据c值最大选择边缘点，根据c值最小选择平面点，且需要满足下列要求：

局部范围内特征点的数量不能超过阈值。
该点的附近没有其它特征点。
它不能是平面于激光的方向的平面点，也不能是被遮挡造成的边缘点。

4.2. 寻找特征点的对应关系

里程计算法利用激光雷达的单次扫描来估计激光雷达的运动。设 $t_k$ 是扫描k的起始时刻。每次扫描结束之后，把产生的点云 $P_k$ 投影到时刻 $t_{k+1}$ ，记为 $\bar{P}_{k+1}$ ，然后和 $P_{k+1}$ 进行匹配来估计激光雷达的运行。

下图中(a)表示寻找边线与边点对应的过程。假设已经有了 $\bar{P_k}$ 和 $P_{k+1}$ ，需要寻找两帧激光点云之间的对应关系。对于 $P_{k+1}$ ，按上一节方法找到边缘点和平面点，分别用 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 表示。接下会在 $\bar{P_k}$ 中寻找与 $\mathcal{E}_{k+1}$ 相对应的边缘线和与 $\mathcal{H}_{k+1}$ 相对应的平面块。

注意到，在扫描开始时 $P_{k+1}$ 是空集，在扫描过程中收到的点数逐渐增长。在扫描过程中，激光里程计迭代地估计6自由度的运动并逐步地扩充 $P_{k+1}$ 所包含的点数。在每次迭代时利用刚刚估计的位姿把 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 重投影回扫描的初始时刻。用 $\widetilde{\mathcal{E}}_{k+1}$ 和 $\widetilde{\mathcal{H}}_{k+1}$ 代码重投影后的边缘点集合和平面点集合。对于它们中的每一个点，都在 $\bar{P_k}$ 之中搜索最近邻点，使用的方法是KD树。

下图表示搜索边缘线和边缘点对应关系的过程。用i表示 $\widetilde{\mathcal{E}}_{k+1}$ 中的点，边线用两个点表示。用j表示在 $\bar{P_k}$ 中i的最近邻点，用l表示j的两个连续的扫续中i的最近邻，故(j, l)形成i的一个关联。接下来验证j和l是边缘点，只需检查局部平滑性。这里特别要求，j和l来源于不同的扫描，因为一个扫描不能包含来自同一条边线的两个点。有一种异常情况：边缘线在一个扫描平面上。这种情况下，边缘化会友扫描平面上退化为一条直线，它上面的特征点不会被提取。

下图中(b)表示寻找平面块与平面点对应的过程。用i表示 $\widetilde{\mathcal{H}}_{k+1}$ 中的点。平面块用三个点表示。与边缘线部分类似，在 $\bar{P_k}$ 中寻找i的最近邻点，记为j。接下面寻找另外两个点l和m，它们是i的最近邻点，一个与j处于相同的扫描，另一个在j的相信扫描上。这保证了三个点是非共线的。为了验证三个点是共面点，需要检查局部平面的平滑性。

利用找到的特征点关联关系，计算关联的特征点之间的距离。用最小化总距离的方式求解激光雷达的运动。首先计算边缘点距离，对于边缘点 $i \in \widetilde{\mathcal{E}}_{k+1}$ ，如果(j, l)是对应的边缘线， $j, l \in \bar{P}_k$ ，那么点到线的距离是

同理，平面点的距离是

4.3. 运动估计

把激光雷达运动建模为均速平移且均角速度旋转。因为在计算不同时间戳的点的位姿转换时可以使用线性插值。设t是当前时间戳， $t_{k+1}$ 是k＋1采样的初始时刻。用 $T^L_{k+1}$ 表示 $[t_{k+1}, t]$ 之间的位净转换。 $T^L_{k+1}$ 包含激光雷达的6自由度运动， $T^L_{k+1} = [t_x, t_y, t_z, \theta_x, \theta_y, \theta_z]^T$ 。线性插值得到 $t_i$ 时刻的位姿是