导航定位学报
Journal of Navigation and Positioning 第9卷第1期2021年2月Vol.9, No. 1Feb., 2021
引文格式：王雅仪，余萌，朱锋•线面特征结合的三维激光SLAM 算法及性能评估[J] •导航定位学报,2021,9(1): 122-129. ( WANG Yayi, YU Meng, ZHU feng.3D LiDAR SLAM algorithm and performance evaluation based on edge and surface features [J].Journal of Navigation and
Positioning,2021,9(1): 122-129.)DOI: 10.ki. 10-1096.20210118.
线面特征结合的三维激光SLAM 算法及性能评估
王雅仪，余萌，朱锋
(武汉大学 测绘学院，武汉 430079)
摘要：针对自动驾驶的高精度定位中，传统激光同步定位和制图(SLAM )点云配准采用的迭代最临近点法(ICP ) 计算效率低、易误匹配的问题，提出1种3维激光SLAM 点云配准方法：通过基于特征的ICP 变种算法，对在不同时刻 扫描的重叠点云进行配准；并分析有无全球卫星导航系统(GNSS )数据融合下的激光SLAM 自主定位性能。实验结果表
明，纯激光SLAM 的位置误差存在线性发散，其值约为行车里程的1.98%，融入GNSS 数据后，可以提高SLAM 全局位
姿的一致性，达到分米级定位精度。
关键词：激光同步定位与制图；特征提取；点云配准；迭代最临近点法；全球卫星导航系统
中图分类号：P 228 文献标志码：A 文章编号：2095-4999(2021)01-0122-08
3D LiDAR SLAM algorithm and performance evaluation based on edge and surface features
WANG Yayi, YU Meng, ZHU f eng
(School of Geodesy and Geomatics, Wuhan University, Wuhan 430079, China )
Abstract ： Aiming at the problems of low computational efficiency and mismatching of the Iterative Closest Point (ICP) method used by the traditional Light Detection and Ranging (LiD
AR) Simultaneous Location and Mapping (SLAM) point cloud registration algorithm in the high-precise positioning of automatic driving, the paper proposed a point cloud registration method
of 3D LiDAR SLAM: the overlapping point clouds scanned at different times were registered by ICP variant algorithm based on
features; and the autonomic positioning performances of LiDAR SLAM with or without Global Navigation Satellite System (GNSS) fusion were analyzed. Experimental result showed that the linear divergence of the positioning error of LiDAR SLAM could be 1.98% of the driving mileages, while the integration of GNSS data could help improve the overall position and attitude consistency
of SLAM with the positioning accuracy of decimeter level.
Keywords: light detection and ranging simultaneous location and mapping; feature extraction; point cloud registration; iterative closest point; global navigation satellite system
0引言
自动驾驶是1个集精准定位、环境感知、决 策控制等多功能于一体的复杂系统，而定位又是
这些功能发挥作用的先决条件。当前的定位定姿 技术众多，但均存在缺陷，如卫星导航容易在信号
遮挡的环境下失效；高精度的惯导设备成本极高、
且定位误差随着时间增加而快速发散；视觉导航
易受光照和视角变化的影响，基于无线信号即无
线保真(wireless fidelity, WiFi )、蓝牙、发光二极
管(light emitting diode, LED )等的定位方案，须
预先布置使用场景［1］o 文献［2-3］指出：激光雷达
收稿日期：2020-05-20
基金项目：湖北省技术创新专项(2019AAA 043)。
第一作者简介：王雅仪(1996—)，女，四川成都人，硕士研究生，研究方向为视觉/激光点云数据处理和组合导航与定位。
第1期王雅仪，等.线面特征结合的三维激光SLAM算法及性能评估123
(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是指基于特征的迭代最临近点法，实验中采用的激光点云采样率设为10Hz，而GNSS接收机位置先验信息则以低频1Hz的速度输入系统，最终的位姿估计的频率与点云采样率一致，在GNSS接收机良好的场景下可以用GNSS接收机辅助构建点云地图。
1.1预处理
预处理分为点云分割和特征提取2个步骤，点云分割能提高特征提取的速度和精度。对原始点云进行聚类，去除冗余点，仅留下包含结构化特征的分割点云，包括地面点和大型物体上的点(如树干)。采用基于密度的聚类方法，以某一点为启发点向四周探索，计算相邻方位角、相邻扫描线上邻点的距离和角度，足够靠近的一点云组成1个簇，只有点云簇含30个点以上才被认为是候选点云。接着，从分割点云中再提取对应的线特征点和面特征点。
124导航定位学报2021年2月
图1本文提出的3维激光SLAM算法流程
本文采用文献[9]提出的线面特征提取方法,
定义激光点的局部平滑度为
1 c i=一i2n y(-P j
-n,i+n]j幻||p—
(1)
式中：n为选取与点同一激光扫描线的邻点的个数；c为点P与其相邻2n个激光点形成的单位向量之和的模。
当激光束打在“面”上，£与其邻点分布在近似一条直线上，因此这些邻点与£构成的向量相互抵消，q值较小。当激光束打在“线”上，邻点与£形成的向量无法相互抵消，q值较大。
为获取空间均匀分布的特征，将每根扫描线等分为6段，在段内依据激光点的局部平滑度进行排序，每一段选取平滑度大于0.1的最大2个点为线特征点，选取平滑度小于0.1的最小4个点为面特征点。特征提取的效果如图2所示。
(a)原始激光点云
(b)提取后的线面特征点云
图2线面特征提取示意图
原始激光点云数约为11.7万个，提取特征后，
线特征点数量为667个，面特征点数量为845个，总特征点数为1512个，待估观测数据大大减少，并且如图2所示，提取出的线特征点主要聚集在树干、路沿、车辆边缘、房屋角点等处，而面特征点聚集在地面，能如实反映3维环境的结构化特征。1.2前端里程计
里程计主要负责每帧点云的位姿估计，当结构化线面特征输入系统后，采用畸变校正和扫描匹配并行的算法，恢复出相邻帧的位姿关系。
考虑到常用的激光雷达都是旋转扫描的工作方式，车辆在激光雷达一帧扫描的周期内，会行驶过一段距离，导致一帧内的所有激光点并不是处于同一个局部坐标系，经过畸变校正可以恢复到同一局部坐标系下。点云的去畸变需要已知帧间运动量，而求解帧间运动量又依赖于去畸变后扫描匹配，因此可以用位姿插值的办法将畸变校正和扫描匹配放在1个算法中同时处理。假设扫描周期内汽车的运动是平滑的，基于匀速运动模型，不同时刻对应的位姿可以插值表示，设在第k帧期间，车体的运动量等价于帧尾时刻的局部坐标系｛L k+1｝相对于帧头时刻的局部坐标系｛L k｝的旋转和平移，用T表示，T=[2，&y,2，tx，ty,t z]T。
如果已知某点在一帧扫描内的相对时间，记为s，那么该点对应｛L k｝的旋转和平移量为(以绕Y 轴旋转和平移为例)
'cos(s y)0一sin(s-O y)]
r,y
)=o i o
,(2)
I sin(s y0cos(s
0=s-t y
在对每帧点云进行特征提取操作后，本文采用PCL库中的FLANN匹配器快速查前后两帧点云的匹配特征点对，线特征点只和线特征点进行最近邻匹配，面特征点只和面特征点进行最近邻匹配。对于线特征点而言，虽然最近邻点和它可能不是同一个点，但是它们在同一条结构线上的可能性很大，因此在目标点云中到最近邻点和次近邻点，观测方程就变成了最小化点到线/面的距离
第1期王雅仪，等.线面特征结合的三维激光SLAM 算法及性能评估
125
之和的函数，相比于传统全点云ICP，线面-ICP 能 够建立更准确的观测方程。
对于线特征点，其算法模型为
n
{7?, t } = arg min 工 Function (Proj e ct (R £, s J , d ；，必)
i =1
(3)
式中：s ,. =[ s x , s y , s iz 是当前源点云中的点；
d =[d ”, d y , d i z 是目标点云中的匹配点；R ,、t , 代表经过位姿插值后的旋转矩阵和平移矩阵；R 、 t 代表旋转矩阵和平移矩阵；函数Project (•)是将源
点云中的线特征点s ，归算到帧头时刻对应的局部
坐标系下，记为s '，和目标点云所在局部坐标系一
致；函数Function (•)是指计算点s ；到d 1和d ，所形成 的线的距离。相似地，对于当前帧的面特征点，需
要在参考帧中到最近邻的3个面特征点，误差函
数表示为点s ；到d ；、d ,、d ,3所形成的面的距离。
以同一数据测试全点云，ICP 算法和线面特征
结合的ICP 算法，得到全点云ICP 算法每一帧的
计算时间平均为3.183 s ，而改进后的ICP 算法只
需要0.292 s ，这主要是因为特征提取减小了匹配
点对的数量，提高了匹配速率；另外，考虑到用最 小二采估计位姿速度慢，本文采用分层优化的方
法[10]减小设计矩阵求逆的维度，如图3所示，在同
等精度下可以减少35%的计算时间。
前端位
i 姿输出
源点云面特征点
-►
垂向平移量 横滚角 俯仰角
目标点云面特征点
特征提取
1
i 源点云线特征点
侧向平移量前向平移量目标点云线特征点
航向角
图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 = B T PB ，W =B t PL 求得；P G 是具有先验信息 的平移量t 的权阵，根据经验设定；L g 表示t 的
GNSS 先验值减去前端ICP 位姿估计值的差。
2实验与结果分析
为了对激光SLAM 系统的车载导航性能进行
测试和评估，采用不同道路环境的数据对算法的整 体定位定姿性能以及GNSS 融合对定位结果的影 响进行分析。本文测试了卡尔斯鲁厄工学院和丰田
技术学院联合采集的开源数据集基蒂(KITTI ) [13]， 其采集环境包括公路、城市及乡村。采集设备有一
套OXTS RT 3003的GNSS 与惯性测量系统(inertial
measurement unit, IMU )组合导航系统和一台
Velodyne 64线激光雷达。激光雷达以10 Hz 的频 率进行旋转扫描，测距精度0.02 m (<1 6)，水平
视角360°，水平分辨率0.09°，竖直视角为26.8 °；
涵盖三类环境，“公路”道路宽、车速快；“城市" 道路两旁是建筑物，“乡村”道路两旁是大量植
被。其实验设备、采集环境如图4所示。
2.1整体精度分析
把GNSS/INS 的解算结果视为参考真值，和纯
激光SLAM 实时输出的定位定姿结果进行误差分
126
导航定位学报2021年2月
(a)实验采集设备(b)公路采集环境
(c)城市采集环境(d)乡村采集环境
图4实验装备与采集环境
析，水平轨迹误差的大小由灰度深浅来表示，灰度 越深代表偏差越大，如图5所示。
KITTI 共有11组数据，但是由于公路场景下车 速达到80 km/h ，点云变化速度快，特征提取稀疏，
SLAM 位姿误差随时间很快地发散了，在图5中没
有展示出来。由图5可知，大部分的轨迹和参考真
值重合度高，但是仍明显存在着累计误差随行驶距
离增大而增大的情况。这里采用平均相对误差进行
精度分析，每行驶100 m 计算1次误差，得到整体
的定位定姿精度如表1所示。
(a) 1号序列轨迹(b) 2号序列轨迹(c) 3号序列轨迹
A7m (d) 4号序列轨迹
-20 -10 0
150
10
Xlm
(g) 7号序列轨迹
X/m
(e) 5号序列轨迹
Xlm
(h) 8号序列轨迹
A7m
图5整体轨迹误差结
果
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议