线面特征结合的三维激光SLAM算法及性能评估

王雅仪，余 萌，朱 锋

（武汉大学 测绘学院，武汉 430079）

0 引言

自动驾驶是1个集精准定位、环境感知、决策控制等多功能于一体的复杂系统，而定位又是这些功能发挥作用的先决条件。当前的定位定姿技术众多，但均存在缺陷，如卫星导航容易在信号遮挡的环境下失效；高精度的惯导设备成本极高、且定位误差随着时间增加而快速发散；视觉导航易受光照和视角变化的影响，基于无线信号即无线保真（wireless fidelity, WiFi）、蓝牙、发光二极管（light emitting diode, LED）等的定位方案，须预先布置使用场景[1]。文献[2-3]指出：激光雷达（light detection and ranging, LiDAR）具有测量精度高、时间和空间分辨率精细、无需事先布置场景、能够快速响应环境变化、可融合多传感器等优点，与基于视觉同步定位和制图（simultaneous location and mapping, SLAM）技术相比，激光SLAM自主定位更加安全、稳健，成为了国内外研究的热点。

激光SLAM框架，一般分为前端扫描匹配、后端优化、回环检测、地图构建等4个关键模块。前端扫描匹配是SLAM最核心的功能，实现前后两帧点云的相对位姿恢复，也被称为前端里程计。前端里程计的主流匹配算法有：迭代最临近点法（iterative closest point, ICP）及其变种、相关性扫描匹配法（correlation scan match, CSM）、正态分布变换（normal distribution transformation, NDT）、基于特征的匹配等等。ICP是由文献[4]在1992年提出的，迭代转换矩阵使得待匹配的两帧点云欧氏距离最小化，但是传统的ICP需要对全点云进行优化，存储和计算消耗大；基于特征的匹配，通过抽象出3维点云的几何信息，能更好地辅助前端进行点云匹配，在相同时间下准确度更高，是当下的研究热点。

2009年，文献[5]首次在点特征直方图（point feature histograms, PFH）的基础上，提出了更加稳定的快速点特征直方图（fast point feature histograms,FPFH），类似于视觉特征子的描述符一样，描述了3维（3D）点云集中1个点周围的局部几何特征，通过直方图的相似度确定同名点对，再进行基于同名点的ICP配准；文献[6]提出利用建筑物边界约束进行点云数据配准，通过探测建筑物垂直外墙和地面相交处的边角点，预先构建出地图，然后通过ICP实现地图匹配来校正车辆位置；文献[7]采用分开—合并框架提取点线特征，还融合了不同观测值的置信度；文献[8]采用“点－切平面”的特征对重叠点云进行配准；2016年，文献[9]提出的激光雷达里程计与地图构建（LiDAR odometry and mapping, LOAM）成为3维激光SLAM最流行的点云匹配方法，它基于线面特征进行了点云特征提取，并将激光雷达里程计的输出和点云子地图进行特征匹配，对里程计恢复的位姿进行优化，但系统实时性差；基于LOAM的基本思想，列戈-洛姆（LeGO-LOAM）[10]引入了基于历史位置的最近邻搜索做闭环检测，采用两步优化的方式降低设计矩阵的维数，可以满足系统实时性的要求。

相对于点线面特征，更加抽象的语义特征能如实地反映3维场景，提供正确的特征匹配。苏黎世联邦理工大学开发的LeGO-LOAM软件[11]，则基于分割算法进行回环检测，首先从3D点云中聚类并标注分割物，将它们用于匹配已经走过的地方和使用几何验证对比的方法选出回环检测的候选点云。文献[12]提出的塞格·马特赫（SegMatch），采用漫水填充方法消除错误的类别标签，识别动态物体并剔除，构建带有语义约束的ICP模型，提高了定位的稳定性。

本文采用1种线面特征相结合的3维激光SLAM算法，通过基于特征的ICP变种算法，对在不同时刻扫描的重叠点云进行配准，恢复行驶车辆的6个自由度位姿；为提高解算精度，提出全球卫星导航系统（global satellite navigation system,GNSS）数据融合激光SLAM的方案。

1 算法理论及实现

3维激光SLAM的任务是估计载体自身的位姿，同时建立周围的环境地图。因为激光雷达可以准确测量障碍点的角度与距离，环境地图通过对正确的位姿进行点云叠加即可获得，可见激光SLAM的关键在于提高位姿估计的精度。本文提出的SLAM基本框架可以分为：预处理、前端里程计和后端优化3个模块，如图1所示，图1中的ICP是指基于特征的迭代最临近点法，实验中采用的激光点云采样率设为10 Hz，而GNSS接收机位置先验信息则以低频1 Hz的速度输入系统，最终的位姿估计的频率与点云采样率一致，在GNSS接收机良好的场景下可以用GNSS接收机辅助构建点云地图。

1.1 预处理

预处理分为点云分割和特征提取2个步骤，点云分割能提高特征提取的速度和精度。对原始点云进行聚类，去除冗余点，仅留下包含结构化特征的分割点云，包括地面点和大型物体上的点（如树干）。采用基于密度的聚类方法，以某一点为启发点向四周探索，计算相邻方位角、相邻扫描线上邻点的距离和角度，足够靠近的一群点云组成1个簇，只有点云簇含30个点以上才被认为是候选点云。接着，从分割点云中再提取对应的线特征点和面特征点。

图1 本文提出的3维激光SLAM算法流程

本文采用文献[9]提出的线面特征提取方法，定义激光点的局部平滑度为

式中：n为选取与点iP同一激光扫描线的邻点的个数；ic为点iP与其相邻2n个激光点形成的单位向量之和的模。

当激光束打在“面”上，iP与其邻点分布在近似一条直线上，因此这些邻点与iP构成的向量相互抵消，ic值较小。当激光束打在“线”上，邻点与iP形成的向量无法相互抵消，ic值较大。

为获取空间均匀分布的特征，将每根扫描线等分为6段，在段内依据激光点的局部平滑度进行排序，每一段选取平滑度大于0.1的最大2个点为线特征点，选取平滑度小于0.1的最小4个点为面特征点。特征提取的效果如图2所示。

图2 线面特征提取示意图

原始激光点云数约为11.7万个，提取特征后，线特征点数量为667个，面特征点数量为845个，总特征点数为1 512个，待估观测数据大大减少，并且如图2所示，提取出的线特征点主要聚集在树干、路沿、车辆边缘、房屋角点等处，而面特征点聚集在地面，能如实反映3维环境的结构化特征。

1.2 前端里程计

里程计主要负责每帧点云的位姿估计，当结构化线面特征输入系统后，采用畸变校正和扫描匹配并行的算法，恢复出相邻帧的位姿关系。

考虑到常用的激光雷达都是旋转扫描的工作方式，车辆在激光雷达一帧扫描的周期内，会行驶过一段距离，导致一帧内的所有激光点并不是处于同一个局部坐标系，经过畸变校正可以恢复到同一局部坐标系下。点云的去畸变需要已知帧间运动量，而求解帧间运动量又依赖于去畸变后扫描匹配，因此可以用位姿插值的办法将畸变校正和扫描匹配放在1个算法中同时处理。假设扫描周期内汽车的运动是平滑的，基于匀速运动模型，不同时刻对应的位姿可以插值表示，设在第k帧期间，车体的运动量等价于帧尾时刻的局部坐标系{Lk+1}相对于帧头时刻的局部坐标系{Lk}的旋转和平移，用Tk表示，Tk=[θx,θy,θz,t x,t y,tz]T。

如果已知某点在一帧扫描内的相对时间，记为s，那么该点对应{Lk}的旋转和平移量为（以绕Y轴旋转和平移为例）

在对每帧点云进行特征提取操作后，本文采用PCL库中的FLANN匹配器快速查找前后两帧点云的匹配特征点对，线特征点只和线特征点进行最近邻匹配，面特征点只和面特征点进行最近邻匹配。对于线特征点而言，虽然最近邻点和它可能不是同一个点，但是它们在同一条结构线上的可能性很大，因此在目标点云中找到最近邻点和次近邻点，观测方程就变成了最小化点到线/面的距离之和的函数，相比于传统全点云ICP，线面-ICP能够建立更准确的观测方程。

对于线特征点，其算法模型为

以同一数据测试全点云，ICP算法和线面特征结合的ICP算法，得到全点云ICP算法每一帧的计算时间平均为3.183 s，而改进后的ICP算法只需要0.292 s，这主要是因为特征提取减小了匹配点对的数量，提高了匹配速率；另外，考虑到用最小二采估计位姿速度慢，本文采用分层优化的方法[10]减小设计矩阵求逆的维度，如图3所示，在同等精度下可以减少35%的计算时间。

图3 分层优化算法

1.3 后端优化

前端里程计每一步的解算都存在误差，随着航位推算的距离增加，全局位姿的误差也在累积，根据位姿拼接起来的地图也会发生畸变。在SLAM领域，又提出了后端优化的关键技术，对位姿进行优化，得到全局一致的轨迹和地图。始终假设后一时刻的估计较前一时刻带有更多的误差，因此对已经构建的地图的信任程度远高于相邻帧点云配准后的运动估计。

本文通过帧间距离和角度的关系来划分关键帧，距离相隔1 m或者角度转过30°就向后端输入1个关键帧；一旦前端判断当前帧为关键帧后，从后端的关键帧库中查询在当前位置80 m范围内的关键帧轨迹点；并将对应的点云叠加、构建局部地图，进行基于地图的线面-ICP匹配；最后将优化后的位姿更新到关键帧点云库和轨迹库中。

如果系统中还有GNSS数据融合，考虑到GNSS 数据具有不发散的良好特性，可以用来抑制激光SLAM的长时性漂移，保证了SLAM轨迹漂移在1个可控范围之内。将车辆的初始时间和位置和GNSS 时间和坐标系对准后，设定每1 Hz得到对应的GNSS先验数据。系统首先进行3维激光点云配准，然后融合GNSS先验位置数据和对应位姿点，经过GNSS融合后的位姿估计才被存储到关键帧点云库和轨迹库中。基于式（3）的观测模型，观测值为源点云到最近邻目标点云形成的线/面的欧氏距离，可以引入1个附加先验信息的最小二乘估计器，假设观测方程线性化后得到系数矩阵为B，观测值为L，观测值权阵为P，则附加先验信息的最小二乘法方程为

式中：待估的6自由度的位姿(R t)是当前帧相对于全局坐标系的旋转、平移量；N、W的值由公式N=BTPB，W=BTPL求得；PG是具有先验信息的平移量t的权阵，根据经验设定；LG表示t的GNSS先验值减去前端ICP位姿估计值的差。

2 实验与结果分析

为了对激光SLAM系统的车载导航性能进行测试和评估，采用不同道路环境的数据对算法的整体定位定姿性能以及GNSS融合对定位结果的影响进行分析。本文测试了卡尔斯鲁厄工学院和丰田技术学院联合采集的开源数据集基蒂（KITTI）[13]，其采集环境包括公路、城市及乡村。采集设备有一套OXTS RT3003的GNSS与惯性测量系统（inertial measurement unit, IMU）组合导航系统和一台Velodyne 64线激光雷达。激光雷达以10 Hz的频率进行旋转扫描，测距精度0.02 m（＜1δ），水平视角360°，水平分辨率0.09°，竖直视角为26.8 °；涵盖三类环境，“公路”道路宽、车速快；“城市”道路两旁是建筑物，“乡村”道路两旁是大量植被。其实验设备、采集环境如图4所示。

2.1 整体精度分析

把GNSS/INS的解算结果视为参考真值，和纯激光SLAM实时输出的定位定姿结果进行误差分析，水平轨迹误差的大小由灰度深浅来表示，灰度越深代表偏差越大，如图5所示。

图4 实验装备与采集环境

KITTI共有11组数据，但是由于公路场景下车速达到80 km/h，点云变化速度快，特征提取稀疏，SLAM位姿误差随时间很快地发散了，在图5中没有展示出来。由图5可知，大部分的轨迹和参考真值重合度高，但是仍明显存在着累计误差随行驶距离增大而增大的情况。这里采用平均相对误差进行精度分析，每行驶100 m计算1次误差，得到整体的定位定姿精度如表1所示。

图5 整体轨迹误差结果

表1 KITTI数据描述以及整体解算精度

由表1可知，本文给出的线面特征结合的纯激光SLAM在车速20～50 km/h的结构化环境下，能较好地维持车辆的位姿，整体定位精度约为1.98%，即行驶100 m，偏离真值1.98 m，角度误差为0.005 1(°)/m。分析几个相对误差较大的轨迹，如序号4、序号8的轨迹，明显是误差随着行驶距离的增加而累积，所以激光雷达必须融合其他具备绝对定位能力的传感器数据，才能抑制自身长时性漂移；对序号7的异常轨迹分析发现，车辆经过了多次快速转弯、环境退化（大量相似及不稳定特征干扰导致）的场景，在平面上产生了严重漂移。

2.2 后端优化对定位定姿结果的影响

后端优化将当前关键帧和距离80 m以内的历史关键帧组成的地图关联起来，用构建好的地图对当前帧的位姿进行精化。为了具体分析后端地图优化模块对整体定位定姿精度的影响，以序列6数据为例，这是1组城市道路的数据，全长700 m左右。分析不加地图优化和加入地图优化后，对比两种模式下估计出来的轨迹和定位定姿结果的误差序列，结果如图6至图8所示。

图6 后端优化对整体精度的影响

图7 后端优化对位置估计的影响

图8 后端优化对姿态估计的影响

由图6可以看出：没有进行后端优化的轨迹尺度漂移明显，各方向的位置、角度逐渐偏离真值。反之，成功的地图优化可使得起点和终点高度重合，最大偏差显著降低。本文采用均方根误差（root mean square error, RMSE）作为评价标准，来定量地分析优化前后相对精度变化，其结果如表2所示。

表2 后端优化前后RMSE

水平定位精度提升至1～2 m，角度精度约为2°，提高了66.7%的姿态精度，说明优化后整体精度有了显著提高，验证了后端优化的有效性。在没有其他传感器的融合辅助下，成功的后端优化可以在一定程度上抑制激光SLAM自身的长时性漂移。

2.3 GNSS融合对定位定姿结果的影响

图9 GNSS融合对整体精度的影响

为进一步分析多传感器融合对3维激光SLAM位姿估计的影响，在后端优化中引入GNSS位置先验数据，采用有先验约束的整体平差法进行数据融合，这是1种松组合的方法，便于多传感器的拓展。将车辆的初始时间和位置与 GNSS 时间和坐标系对准后，参考真值转换为GNSS约束条件，模拟实时GNSS约束输入。以长路程的序列1、序列7的数据为例，GNSS融合后估计出来的轨迹和建图效果如图9所示。

由图9可知，融合了GNSS数据的计算轨迹和参考真值轨迹的重合度更高：当有GNSS数据约束时，可抑制激光雷达里程计的漂移发散；没有GNSS数据约束时，激光雷达里程计依赖自身航位推算进行位姿估计与建图。

表3分析了车辆行驶中水平方向上的定位精度。

表3 KITTI数据定位结果分析

由表3可知，通过精度对比，GNSS/激光SLAM的融合算法明显优于纯SLAM位姿优化算法。该定位方法不存在由里程计引起的累积误差，在30 km/h左右的速度下，表现出良好的定位性能，适用于在GNSS条件良好下的点云地图构建。定位精度提高至25 cm左右时，和纯GNSS定位精度相当，但融合算法能输出更高频率的位姿结果，有助于生成点云密度更高、一致性良好的点云地图。

3 结束语

本文针对传统全点云ICP算法存储和运算消耗大、点云误匹配的问题，提出了1种线面特征结合的点云精配准算法。通过对获取的点云进行分割和特征提取，再对结构化的线、面特征点云进行精配准，并在精配准的过程中，同时实验姿点云运动的畸变校正，最后通过KITTI测试，对算法的可行性和有效性进行了评估，验证了该算法的整体定位相对精度约为1.98%，定姿相对精度约为0.005 1(°)/m；后端优化在一定程度上可抑制激光SLAM自身的长时性漂移；当有GNSS约束时，轨迹无发散，定位精度约为25 cm，达到分米级定位精度。