一种混合视觉的车载激光雷达即时定位与地图构建方法＊

张剑锋 彭育辉 郑玮鸿 周增城 林晨浩

（福州大学，福州 350118）

主题词：激光雷达 混合视觉 即时定位与地图构建 点云 匹配

1 前言

即时定位与地图构建（Simultaneous Localization And Mapping，SLAM）是指在没有环境先验信息的前提下，搭载传感器的主体在运动中建立环境地图模型，同时估计自身在环境地图中的位置和运动[1]。随着无人驾驶技术的研究和应用日益受到政府和行业关注，SLAM算法逐渐成为国内外研究的热点。根据传感器类型不同，SLAM 算法可分为视觉SLAM（Visual SLAM）和基于雷达的SLAM（LiDAR SLAM）；根据数理模型的不同，SLAM算法可分为基于估计理论的SLAM算法和基于图优化的SLAM算法。

视觉SLAM 通过特征点法或直接法实现帧间图像配准完成位姿估计，ORB（Oriented FAST and Rotated BRIEF）-SLAM 方法为特征点法，能够合理兼顾效率和精度问题[2]。LSD（Large Scale Direct monocular）-SLAM方法则无需计算特征点来构建半稠密地图[3-4]；SVO（Semi-direct Visual Odometry）将特征点法和直接法融合使用，去除了描述子的计算，避免了半稠密法和稠密法需要处理大量信息的劣势[5]。但是，由于相机传感器光学特性的限制，视觉SLAM只能在光照充足且稳定的环境下工作。激光雷达因其不受外界光照影响，成为无人驾驶感知方案的主流。早期对3D激光SLAM的研究大都停留在对静态物体的三维重建技术上，常用迭代临近（Iterative Closest Point，ICP）法[6]。LOAM（LiDAR Odemetry And Mapping）方法是2014 年开源的3D 激光SLAM方法，以点云中的平面点和边缘点为特征点进行帧间匹配，并通过帧与地图匹配矫正位姿[7]。LVIO（Laser Visual Inertial Odometry）方法是一种多传感器融合的自身运动估计和建图数据处理方案，顺序多层地通过激光雷达、相机和惯性测量单元（Inertial Measurement Unit，IMU）获得自身运动估计[8]。LeGO（Lightweight and Ground-Optimized）-LOAM方法是改进的LOAM 方法，利用地面约束减小计算量，加入了回环检测模块，减少长时间工作导致的累计误差[9]。谷歌公司的Cartographer 利用帧间匹配，在最佳位置估计处插入子图，采取分枝定界和预先计算，在所有子图都插入后进行全局回环检测[10]。

针对搭载非均匀垂直角分辨率激光雷达的无人车的多传感器融合即时定位与地图构建问题，本文提出一种融合图像与点云密度的激光雷达SLAM方法，在特征点匹配中加入颜色信息，在优化位姿中加入密度权重，并以KITTI数据集和实车试验验证本文方法的有效性。

2 相机和激光雷达的混合标定

在三维世界中，相机能够将三维坐标点通过投影映射在二维图像平面内，该过程可以用针孔相机模型来简单描述。三维激光雷达能以三维坐标的形式直接获取周围环境信息。通过二者间的坐标变换可以赋予三维点云以颜色信息，二者标定原理如图1所示。相机坐标系为OcXcYcZc，激光雷达坐标系为OlXlYlZl。

图1 相机与雷达标定原理

由相机内参数标定获得内参矩阵，通过相机与激光雷达的外参数标定获得相机的外参矩阵，得到三维激光雷达坐标系到像素坐标系的变换关系：

式中，P为目标点在激光雷达坐标系下的齐次坐标；K为内参矩阵；T为外参矩阵；u、v为目标点在像素坐标系中的坐标。

通过雷达坐标系到像素坐标系的变换关系，可以为相机视域内的三维点云赋予颜色信息，如图2、图3所示。

图2 具有颜色信息的点云

图3 相机图片

采用HSV 颜色模型对点云进行颜色分割，通过色调（Hue）、饱和度（Saturation）、明度（Value）来表现物体颜色，其中明度在不同光照条件下变化最大，而色调基本不受光照强度影响，即通过色调判断物体的颜色。

3 帧间预匹配算法

3.1 算法基本思路

首先，根据激光雷达技术参数对点云数据进行结构化排序，并剔除匹配数据中的不稳定点。然后，选取相邻帧点云之间的特征点，利用先前获取的激光雷达与相机之间的变换矩阵赋予颜色信息，并根据特征点的性质制定匹配策略。最后，对匹配点采取未解耦的位姿求解算法，获取两帧间的变换矩阵作为LeGO-LOAM方法中帧间匹配模块的匹配初值，流程如图4所示。

图4 算法基本流程

3.2 点云预处理

激光雷达的技术参数如表1所示，通过扫描获取的三维点云包含断点、遮挡点等无效点。为有效减少点云数量并降低错误匹配出现的概率，必须对点云进行预处理。

表1 Pandar40P激光雷达技术参数

借鉴LeGO-LOAM 方法，将点云进行40 行1 800 列的结构化排序并分割为有效聚类点、离群点和地面点。基本处理步骤为：

a.进行地面点分割，通过观察点云图获取地面点所占的行数来确定预估的地面点云，在此类点云中计算列方向上相邻两点的斜率来确定地面点。

b.采取临近点聚类的方法分割剩余点云，如果某点满足临近点数量大于30个或竖直分布的临近点行数大于3行，则认为是有效聚类点，否则判断为离群点。

c.处理后的点云仍存在帧间匹配中不稳定存在的易遮挡点和断点，需要利用相邻两点与激光雷达的距离进行深度估计来完成这两类点的去除。

3.3 特征点的选取

由于点云数据的稀疏性，激光SLAM中的特征点往往是在点云的行方向上通过计算某类特征进行选取，为此，定义一个类粗糙度特征c来分割平面点和边缘点：

地面点云中，c值较小的点标记为平面点；有效聚类点云中，c值较大的点标记为边缘点，并通过标定得到的变换矩阵获取着色特征点。

3.4 特征点的匹配

ICP算法是帧间匹配中最常用的算法，但是传统的ICP算法效率低，对点云三维旋转不敏感且易陷入局部极值。改进的ICP 算法，如PL-ICP（Point-to-Line ICP）算法[11]和PP-ICP（Point-to-Plane ICP）算法[12]有效提升了传统ICP 算法的性能。针对边缘点和平面点特性不同，通常在边缘特征上采用PL-ICP算法，在平面特征上采用PP-ICP算法。

考虑到实际应用中，地面点的颜色往往是固定的，因此在平面点匹配中插入颜色要素能明显提高匹配点的选择精确度。本文提出的匹配策略如图5所示，针对某帧t平面点云中的点a，在上一帧平面点云中寻找与a颜色相同的最近点b1，在b1同一线上寻找颜色相同的最近点b2，在b2上、下两线中按照同样方法搜索点b3，并利用非共线点b1、b2、b3来拟合平面，计算点到平面的距离dp：

图5 平面点匹配

边缘点从非地面点中选取，包括建筑物、树木、停靠车辆等。由于实际环境中，这类物体的颜色往往不是单一色调，所以采取的匹配策略与平面点有所区别。如图6 所示，针对某帧边缘点云中的点a，在上一帧的边缘点云中寻找与a颜色相同的最近点b1，在b1的上、下两线中寻找距离最近点b2，利用b1和b2构造直线，计算点到线的距离de：

图6 边缘点匹配

在得到匹配的特征点后，采取非线性优化方法来求解ICP，将匹配点之间的距离范数和作为误差函数f(Pk-1,Pk)，并通过最小二乘法求解：

式中，Pk为第k帧的点云；Pk,i为第k帧第i点的三维坐标；τ()为将三维坐标转换为齐次坐标的转换函数；Tk为帧间位姿变换矩阵。

带有颜色信息的特征点相对较少，所以在预匹配过程中采用未解耦的位姿求解算法以保证求解精度。用Sk表示和，d表示dp和de，用ΔT表示第k帧到第(k-1)帧的位姿变换，误差函数表示为：

求其对变换矩阵的雅可比矩阵：

式中，J为雅可比矩阵；f为误差函数；p为点云中的点。

此外，匹配距离d因素对ICP 算法也会造成影响，距离越大，置信度s越低。由于Pandar40P 激光雷达的线束分配不均匀，引入线束密度因子h，取中间线束密集区域的因子为1，往两侧越稀疏越小，最外侧不参与匹配：

式中，dp2l为目标点到激光雷达的距离。

最后得到高斯-牛顿（Gauss-Newton）方程组为：

使用高斯-牛顿方法迭代优化求解，直至收敛或者迭代次数达到上限，得到两帧间的位姿变换矩阵T。将初匹配后的变换矩阵传至LeGO-LOAM 方法的帧间匹配模块，完成着色点云帧间预匹配算法。

4 算法验证

4.1 KITTI数据集验证

KITTI 数据集是目前国际上最大的自动驾驶场景下的计算机视觉算法评测数据集[13-14]。当前SLAM算法往往只适用于低速环境，所以选用KITTI 数据集Odometry 数据中住宅区与城市场景进行验证。选用LeGO-LOAM 方法作为原始方法，对比着色点云帧间初匹配方法的性能，并采用GitHub上的SLAM精度测量工具evo[15]进行误差分析，各组仿真结果如表2所示。

表2中，本文方法对00、07和10号数据性能提升较为明显，均方根误差上平均优化了6.84%。00号数据的路程较长，为3 772 m，需要通过回环检测来保证长时间工作时地图的准确性，从回环次数结果可知，本文方法相对原始方法增加了12次。图7、图8所示分别为00号数据在LeGO-LOAM 方法下和本文方法下的局部建图结果。由图7和图8对比可知，原始方法在标记处出现了无法回环的情况，这是由于汽车在经过此场景前有一段长时间无回环检测的路程，原始方法的累计误差超过回环检测范围，导致汽车无法与已建立的地图匹配造成回环失败。而在图8中可以发现，标记处成功实现回环检测，说明本文方法相比于原始方法累计误差小，建图效果如图9所示。

表2 KITTI数据集结果对比

图7 LeGO-LOAM方法建图结果

图8 本文方法建图结果

图9 本文方法00号数据建图结果

由10 号数据可以看出，二者的回环次数均为0，说明在没有发生回环的情况下，加入着色点云帧间初匹配能有效提高建图精度。本文方法相比于原始方法均方根误差减小了12.5%，2 种方法的绝对位置误差以及标准差、均方根误差等参数对比如图10、图11 所示，表明本文方法的精度相比原始方法有所提升。

4.2 实车验证

试验车辆为自行研制的福州大学纯电动无人驾驶测试平台，如图12 所示。工控机采用宸曜科技Nuvo-7160GC，激光雷达采用禾赛Pandar40P，摄像头采用小觅双目深度版，并以ROS 机器人操作系统实现传感器的数据通讯。

图10 KITTI数据集00号数据绝对位置误差对比

图11 KITTI数据集00号数据其他参数对比

图12 试验车辆

首先，获取差分GPS 输出的经纬度信息，转换为平面坐标并作为行驶轨迹的真值。由于轨迹真值的姿态与试验车辆不一致，所以采用Umeyama 方法配准真值与测量值。测试场地选取在福州大学旗山校区，如图13 所示的4 条试验路线。其中，序号1、2、3 为3种不同的典型路线类型，序号4 为福州大学北门区域路线图。对比本文方法与LeGO-LOAM 方法，各组结果如表3 所示。

由表3 可以看出，本文提出的方法相比于LeGOLOAM 算法有一定的优化，均方根误差平均减小了4.53%。以优化最大的序号04为例，本文方法均方根误差减小了11.7%。2种方法的绝对位置误差对比如图14所示，可以看出本文方法的绝对误差总体上小于原始方法。其他对比参数如图15 所示，本文方法的标准差为0.386 m，平均值为0.906 m，原始方法的标准差为0.558 m，平均值为0.965 m，分别减小了30%和6%，绝对误差最大值为2.472 m，最小为0.017 m。最终获得的福州大学北门区域地图如图16所示。

图13 试验路线

表3 实车试验结果对比

图14 路线4绝对位置误差对比

图15 路线4其他参数对比

图16 福州大学北门区域地图

5 结束语

传统激光SLAM 方法通常基于单一激光雷达传感器，通过匹配点云获取位姿变换关系。本文在纯激光SLAM方法上增加相机颜色信息，提出了基于着色点云的预匹配算法。同时，针对竖直线束非均匀分布的雷达，提出基于点云线束密度的位姿求解优化方法。通过KITTI数据集和实车试验对LeGO-LOAM方法和本文方法的精度进行对比，结果表明本文方法在2个数据集上轨迹点均方根误差分别优化了6.84%和4.53%，相对于原算法精度有所提高，并且能够满足搭载垂直角分辨率非均匀雷达的无人车的多传感器即时定位与地图构建要求。