基于激光雷达的轻量化定位与建图方法

龚柯阳，侯宝龙，史志飞，王孝宇

（长安大学 汽车学院，陕西 西安 710064）

随着自动驾驶技术的发展，作为其关键技术之一的定位与建图技术越发重要。现有的定位方案主要为全球定位系统（Global Positioning Ststem, GPS）+惯性测量单元（Inertial Measurement Unit, IMU）的方案，然而在高楼林立、隧道等环境中GPS存在信号缺失问题，在这些场景中上述的定位方案无法达到良好的效果。激光雷达是稳定性强的且能感知周围环境的传感器，在GPS信号不好的环境中，通过激光雷达的辅助定位可以提高定位系统的鲁棒性，且通过其感知的周围环境可以构建全局地图。

在现有的高等级自动驾驶解决方案中，高精度地图是必不可少的，而激光雷达正是目前自动驾驶汽车构建高精度地图必不可少的传感器之一。激光雷达在进行定位与建图时，随着时间的推移，其定位误差会逐渐累积，通过回环检测的方法可以将轨迹拉回真实情况，提高定位精度从而提高建图精度。与此同时，自动驾驶模块众多，在有限算力的情况下，定位与建图模块的运算轻量化尤为重要。

1 整体框架

本文提出的基于激光雷达的轻量化定位与建图方法主要由前端配准、回环检测、后端优化、建图等四个部分组成。其流程如图1所示。

图1 整体框架流程图

其主要实现流程简述如下：雷达通过扫描接收到点云数据后通过前端配准经过点云分割、特征提取得到粗略的里程计位姿，同时将点云发送给回环检测模块，如检测到回环则将雷达里程计和回环约束同时加入到后端进行优化，得到较为精确的位姿。最后通过无人车的位姿以及雷达的点云构建全局的点云地图。Velodyne64线激光雷达如图2所示。

图2 Velodyne64线激光雷达

2 前端配准

2.1 点云分割

P={,,...,P}是在时刻接收到的雷达点云，其P={x,y,z}。将P映射到距离图像中，该距离图像的行是360度除以雷达的水平角分辨率、列是雷达的线束。每个P在距离图像中对应一个像素，每个像素的值r为该点距离雷达发射中心的距离。通过距离图像的同列相邻行的坐标值的比较可以得出当前点的地面属性，当小于某一阈值时将其标记为地面点。

式中，为当前点的行标；为当前点的列标。

根据距离图像的距离信息，将非地面点进行聚类处理，当属于同一类的点的数量小于一定阈值时将其标记为异常点云。实际应用中，异常点云通常是较小的物体、飘散的树叶等，通过剔除这些不稳定的特征有助于提高前端里程计的精度。

经过上述过程的处理，原始的激光雷达点云被划分为几个部分：（1）地面点云；（2）较大聚类数量的非地面点云；（3）带有行列索引的距离图像。

2.2 特征提取

特征提取部分与基于雷达的里程计与建图（Lidar Odometry and Mapping in Real-time, LOAM）中所用的方法类似。由于点云数量过多，为了在保证匹配精度的基础上减少计算，分别从已得到的地面点和非地面点中提取出更具特征性的平面点和角点。根据当前点和其周围点的距离信息，可以得到当前点的曲率信息，根据曲率信息进行平面点和角点的提取。

式中，是当前点附近的点云的总数；为当前点的索引；为附近点的索引。

为了避免提取的特征点聚集影响匹配精度，将点云的距离图像均匀划分为多个子图像，分别在子图像中提取平面点和角点。分别设定平面点阈值和角点阈值。每个子图像中，选取曲率小于的pS个地面点为主平面点，曲率大于的个地面点为次平面点，选取曲率大于的个非地面点为主角点，曲率小于的个非地面点为次角点。主平面点和次平面点合称平面点，其集合为，主角点和次角点合称角点，其集合为，主平面点的集合为，主s角点的集合为。特征提取前后点云的情况如图3所示，其中为未处理的点云，为提取的平面点，为提取的角点。

图3 特征提取前后点云的鸟瞰图

2.3 雷达里程计

为求解该位姿估计问题，我们采用两步L-M优化算法，流程图如图4所示。待优化的位姿为，其中优化的过程为（1）由于其平面特征的匹配对于高程、横滚角、俯仰角具有较好的约束，因此第一步先通过平面特征的匹配优化得到 [t,,]。（2）将第一步优化得到的[t,,]作为第二步优化的约束，通过对角点特征的匹配进一步优化得到[t,t,]。

图4 两步L-M优化流程图

3 回环检测

3.1 SC描述子

回环检测部分，我们采用扫描上下文SC描述子来进行户外场景的回环识别。根据方位角和半径，我们将雷达一帧点云划分为多个子块，并将其展开为行列的形式，分别记行数为，列数为。取每个子块的属性定义如下

式中，()·为返回点云的轴坐标。

因此，一帧点云的SC描述子可以用一个×的矩阵表示为

点云子块示意如图5所示。

图5 点云子块示意图

3.2 相似度分数

当得到一对扫描上下文SC描述子时，我们需要一个尺度去衡量两个位置的相似度。这里令当前帧点云的描述子为，候选帧点云的描述子为。将两个描述子按列进行比较并计算其距离尺度，其计算公式为

3.3 旋转不变性

当智能汽车在行驶的过程中回到同一地点时，其朝向与之前到达时未必一致，通过上述得到的描述子的距离来判断回环则无法检测到上述存在转向的情况。

图6、图7以距离图像的差别来表示每帧点云的旋转差异，这两幅图表示汽车处于同一位置只是转向发生了变化。考虑到这一问题，需要对描述子按列进行移动，从而找到最小的距离尺度，其公式为

图6 当前帧的距离图像

图7 候选帧的距离图像

式中，用于与距离阈值进行比较从而判断回环是否存在。

4 后端优化与建图

后端优化采用图优化的方法，将雷达里程计和回环检测联合优化得到全局的位姿。

图8 局部地图与全局地图示意图

5 实验及验证

本文采用自动驾驶KITTI数据集00序列进行验证，其采用Velodyne 64线激光雷达进行数据采集。分别用带回环检测和不带回环检测的定位与建图方法跑数据集，得到的点云地图如图9、图10所示。

图9 带回环检测的点云地图

图10 不带回环检测的点云地图

从上图可以得出，本文所提出的基于激光雷达的定位与建图方法在较大的场景下具有良好的建图效果，且加入回环检测能显著提高定位与建图的精度。

6 结论

基于激光雷达的轻量化定位与建图方法克服了采集到的点云数量过多而占用过多的计算资源的问题，能够进行轻量化定位与建图，且保证其定位与建图精度。与此同时，利用SC描述子构建的回环检测的引入极大地提高了整个方法的性能。通过该方法建立的点云地图可作为自动驾驶高精度地图的底层模板，从而加快高精度地图的开发应用。

仅通过激光雷达进行定位不可避免地会引起高程误差从而导致垂直于地面方向的地图存在偏移，本文提出的方法并没有很好地解决这一方面的问题。考虑多传感器融合是未来研究的方向。