面向点云退化的隧道环境的无人车激光SLAM方法

李帅鑫，李九人，田 滨，陈 龙，王 力，李广云

1. 信息工程大学地理空间信息学院，河南 郑州 450000； 2. 慧拓无限科技有限公司，北京 100089； 3. 中国科学院自动化研究所复杂系统管理与控制国家重点实验室，北京 100190； 4. 中山大学数据科学与计算机学院，广东 广州 510275

在大数据和人工智能等新技术浪潮的席卷下，全球新一轮的科技革命和产业革命已悄然而至，传统测绘技术的转型升级势在必行[1]。打破传统格局，研究更智能高效的地理信息数据获取新方法，实现地理信息数据对现实环境的支持和增强，是现代测绘发展的重要方向之一[2]。近年来，自动驾驶产业正以前所未有的速度迅猛发展，这一切都离不开高精度地图(high definition map,HD Map)的强有力支持。然而，无论是大规模城市环境数据的快速采集还是高精度地图的生产制备，在传统测绘技术下都是极富挑战的任务[3-4]。

激光雷达(light detection and ranging，LiDAR)能够快速、精准地获取环境信息，现已被广泛应用于机器人、无人驾驶和测绘导航等领域，被视为无人系统感知外部环境的“眼睛”。基于LiDAR的SLAM技术具有不受环境遮挡和光照变化影响的特点，能够很好地弥补全球导航卫星系统(Global Navigation Satellite System，GNSS)的信号在室内环境下的失锁[5-6]。过去的20年来，已涌现出一批优秀的2D激光SLAM解决方案，如：Gmapping[7]、Hector SLAM[8]、Cartograper[9]、Spline SLAM[10]等，它们已在扫地机器人、仓库物流车等诸多商业化产品中得以应用。随着传感器技术的不断发展和用户对产品需求的不断升级，3D SLAM技术近年来备受关注。文献[11]提出的LiDAR定位与地图构建系统(LiDAR odometry and mapping，LOAM)是最经典的3D SLAM系统之一。它采用线和面特征配准点云，并在两个并行线程上分别以高频和低频运行激光里程计与地图构建模块，保证了系统的时效性。鉴于LOAM出色的表现，后续的诸多工作均在其基础上展开，重点是解决一些极端或特殊场景下的挑战性问题。文献[12]提出了面向无人小车(unmanned ground vehicle，UGV)的轻量级LOAM(lightweight and ground-optimized LOAM，LeGO-LOAM)，通过优化特征提取并采用两步法配准点云，降低系统运算量，使其能够在轻量级运算平台上实时运行。文献[13]针对非机械转动的固态激光雷达提出Livox-LOAM，将LOAM算法扩展到Livox Mid-40 LiDAR的应用中。文献[14]提出M-LOAM (multi-LiDAR LOAM，M-LOAM)，实现了多LiDAR的同时定位与地图构建。以上这些方法均是LOAM的扩展，它们都沿用了线面特征匹配方法进行运动估计。还有一些学者尝试采用其他点云配准方法，如：文献[15]在SLAM系统中采用经典的迭代最近邻点(iterative closest point，ICP)算法；文献[16]采用表面元(surfel)模型构建点云地图，并采用正态分布变换算法(normal distribution transformation，NDT)实现点到地图的配准；文献[17]提出基于Surfel的SLAM方法(surfel-based SLAM，SuMa)，在系统中采用surfel ICP配准点云并利用GPU加速；文献[18]提出多分辨率栅格(multi-resolution map，MRS-Map)地图表达方法，并采用概率点集配准算法(Coherent Point Drift，CPD)匹配点云。点云强度能够反映目标的表面材质，可以帮助系统更全面的感知环境。然而，上述系统都只考虑了点云的几何信息而忽视了强度信息，信息利用不够充分。

针对这一问题，文献[19]将同名点强度差值与空间欧氏距离同时作为约束加入ICP的优化求解中，大大缩减了ICP的迭代次数，从而降低了2D SLAM前端配准的计算消耗。文献[20]提出一种基于统计方法的LiDAR强度信息标定方法，并在Hector SLAM[8]中加入同名点强度差值约束，试验结果显示系统的定位精度有所提升。类似情况，Intensity SLAM[21]构建局部强度栅格图，在点到局部地图匹配时融入强度信息。文献[22]针对LOAM的特征点匹配，提出一种强度差定权方法，为具有相似强度值的同名点赋予更高的权。事实上，在结构性特征明显的环境下，仅依靠点云几何信息进行匹配已足够满足精度要求，无须消耗更多计算资源加入点云强度信息。而在结构特征较弱但强度特征丰富的环境下，则需要利用强度信息辅助点云匹配。综上，系统应具备自适应强度特征提取以及强度融合判断的能力，而这些均是上述方法所缺乏的。

本文针对现有激光SLAM在几何结构不明显的隧道环境下存在点云配准退化的问题，在文献[11]和[12]的基础上提出一种面向点云退化的隧道环境的无人车激光SLAM方法。本文在以下方面对LOAM进行了优化和改进。

(1) 特征提取部分。一是在LOAM基于点云的特征提取方法基础上，改进为基于点云柱面投影图的特征提取方法。相较于在三维空间中对离散点云搜索，在投影图中点云的空间位置关系更加明确，无须使用树形结构对点云进行组织和管理。二是提出基于统计方法的自适应强度特征提取方法，能够根据环境情况自适应提取线、面、地面、反射标志物等4种空间特征。

(2) 地图构建与位姿优化部分。一是采用基于统计的退化检测方法，能够自动评估位姿估计退化情况，并确定退化方向；二是结合无人车的特性提出一种基于三次B样条栅格图的强度点云匹配方法对平面方向的退化位姿进行修正。

试验结果表明，采用本文方法能够在几何特征较弱但强度特征丰富的长直隧道内实现无人车的鲁棒定位与地图构建。

1 系统概述

系统接收点云数据，实现自主位姿推估和环境地图构建。整体分为3个部分：①点云数据处理。从原始点云中分割地面点，对剩余非地面点聚类并从中自适应提取线、面和强度特征点。②激光里程计。将相邻帧的线、面特征配准，估计车辆的相对运动并推估在地图中的位姿。③地图构建。将提取的几何特征与局部地图配准修正位姿，检测位姿估计的退化情况并确定退化方向，采用强度特征匹配修正出现退化的位姿估计，最后更新局部特征地图。本文系统的整体框架如图1所示。

图1 系统框架Fig.1 The overview of the system

2 方法介绍

2.1 点云数据处理

2.1.1 地面分割

结合KITTI数据集[23-24]的处理实例对地面分割算法进行说明，主要步骤如下。

(1)

(2)

图2 点云投影深度图实例Fig.2 Example of points-to-depth image projection

(2) 点云分块。将点云划分为i个扇形区域Bk,i，并以水平距离ρ划分为j个扇环Ck,i,j(图3)

(3)

(3) 直线拟合。为避免噪点影响，采用增量式拟合方法对Bk,i中各Ck,i,j进行直线l=aρ+b拟合，a和b分别表示拟合直线的斜率和截距。首先由Ck,i,1和Ck,i,2拟合直线参数a1和b1，再加入Ck,i,3将直线参数更新为a2和b2，依此类推。若更新后引起拟合直线斜率出现较大变化，则判定Ck,i,3为噪声数据，将其跳过。最终拟合得到Bk,i的地表直线(如图3中红色虚线所示)。

图3 点云地面分割实例Fig.3 Example of ground points segmentation

rk,n=ak,iρk,n+bk,i-zk,n

(4)

式中，ak,i和bk,i为Bk,i的拟合直线参数。当ak,i和bk,i存在时，将满足rk,n

2.1.2 特征提取

特征点的自适应提取算法基于深度图实现，主要步骤如下。

(1) 目标分割。根据上节所述的地面点分割结果得到非地面点深度图Vk=Dk-Gk。采用基于夹角值的快速目标分割方法将点云聚类，算法细节可参考文献[25]，聚类后得到如图4的聚类点云Lk。若Lk中某一类的点数少于阈值nthr，则该类的所有点将作为野点从Pk中剔除。

(2) 线、面特征点提取。在Dk上计算各像素点的粗糙度s，方法与文献[11]中相同

(5)

式中，d表示深度；Pneig为深度图上的邻域像素。遍历Vk，将满足s>sE的nE个像素标记为线特征Ek,满足s

图4 目标聚类结构结果Fig.4 Result of the object clustering

(3) 强度特征点自适应提取。首先将非地面点投影为强度图Ik，像素位置与Vk一一对应。为自适应提取局部范围内的强度特征，将Ik分为M×N个子块，并统计子块内各强度区间的占比，建立直方图,如图6所示。直方图横轴为强度区间，区间间隔ΔI，范围为[0,255]；纵轴为该强度区间占比。在直方图中取中值作为该子块强度特征提取的阈值Ithr，将强度满足I>Ithr且大于最小阈值I>Imin的像素标记为强度特征Rk。图6中彩色像素为提取的强度特征，颜色越红表示反射强度越强。

图5 强度特征提取结果Fig.5 Result of the extraction of intensity features

图6 自适应阈值设置Fig.6 Adaptive intensity threshold setting

强度特征提取结果与真实环境的对照见图5，从中可以看出，环境中存在许多诸如车牌、尾灯、路标、钢架结构等稳定的强度特征，能够为点云配准提供良好的约束。

2.2 激光里程计

2.2.1 点云配准

(6)

(7)

(8)

2.2.2 位姿推估

(9)

2.3 地图构建

2.3.1 地图配准

(10)

地图配准仅采用几何特征，即Ek到MEk和Sk到MSk的配准，与激光里程计中所用方法一致，算法细节可参考文献[11]。

2.3.2 退化检测

车载点云配准时，地面点可以提供俯仰角、横滚角及z轴方向的约束，非地面目标则可以提供偏航角、x轴及y轴方向的约束。在空旷场地或长直隧道等环境下，由于难以建立准确的同名点数据关联，导致状态空间上某些方向约束不足，点云配准解算的法方程矩阵呈现病态，产生退化现象。

本文采用文献[27]中提出的退化因子(degeneracy factor，DF)算法进行退化检测及退化方向判断。对于方程Ax=b，退化因子rD定义为一个仅与ATA的特征值λi相关的量

rD=λmin+1

(11)

式中，λmin表示最小特征值。在本文点云配准问题中，A即为目标函数的一阶雅克比矩阵

(12)

2.3.3 配准优化

无人车点云数据包含大量地面点，因此对俯仰、横滚及z方向的约束是充分的，一般不会出现退化问题，而偏航、x及y方向则不然。图7为长直隧道中相隔约10 m的两帧点云的俯视图和侧视图。显然仅利用隧道几何特征很难得到车辆前进方向的准确估计，即配准在x方向出现退化。

图7 点云配准退化Fig.7 The points registration

(13)

(14)

s(τ)=φ(τ)Tcφ(τ)T=vec(bx(x)·by(y))

(15)

点云到地图的配准可表示为点云在强度概率地图上对应位置的概率和最大，由此写出强度目标方程

(16)

采用L-M算法可求解位姿优化量ΔT2D。

(17)

需要说明的是，式(17)并非严格意义的向量相加，其中⊕表示对应项相加。

2.3.4 地图更新

本文采用多尺度地图模型建立线、面特征的局部地图MEk和MSk[28]，采用三次B样条概率地图模型建立强度特征的局部地图MLk。局部地图均以传感器为中心，随车辆运动而滑动更新。

图8为多尺度栅格地图的示意图，红黄蓝色栅格分别表示不同尺度的栅格地图；黑色点表示栅格中的点；LiDAR运动时，地图随之移动，地图中心由Oold向Onew移动。

图8 局部栅格地图Fig.8 Local voxel map

(18)

式中，Δl表示采样间隔。地图更新时采用递归策略更新离散的概率值控制点

(19)

式中，c-和c+分别表示τocc,n对应控制点的先验和后验概率；κ为更新因子，对于遮挡点该值取正，对于非遮挡点该值取负。

图9为隧道内的局部强度地图实例。红色和蓝色区域分别表示强度地图的占据和非占据连通区域，颜色越深表示该点置信度越高。绿色点为当前观测到的强度特征点。局部地图随LiDAR而运动，使传感器始终保持在地图中心。

图9 局部B样条地图实例Fig.9 Example of local B-spline map

3 试验结果和分析

3.1 试验介绍

本文试验采用实测和仿真数据。仿真数据由16线LiDAR采集，输出频率10 Hz；垂直视场为[+15°,-15°]，水平视场为360°；垂直分辨率为2°，水平分辨率为0.2°；测距的最小和最大范围分别是0.1 m和180 m(如图10(b))。实测数据由车载Robosense RS-32 LiDAR(如图11(a))采集，其输出频率为10 Hz；垂直视场为[+15°,-25°]，水平视场为360°；垂直分辨率由0.33°至2°，水平分辨率为0.18°；测距的最小和最大范围分别是0.2 m和200 m；支持输出各点的三维坐标和反射率。相较于64线LiDAR，32线更专注于车辆行驶区域的点云采集，多用于无人驾驶领域。

图10 仿真平台Fig.10 Simulation platform

图11 实测数据采集平台Fig.11 Real-world data collection platforms

实测试验数据在封闭无人驾驶测试场采集，时间约5 min，车辆行驶速度为30～60 km/h，全长约1.5 km并包含一段约150 m的长直隧道。定位轨迹的参考值由高精度GNSS辅助惯性导航系统Oxford Technical Solution(OxTS) RT3000获得(图11(b))，其定位精度优于10 cm，可直接输出通用横墨卡托格网系统(universal transverse mercator grid system，UTM)下的坐标。需要说明的是，在进行本文所述的激光定位与地图构建的数据处理过程中，OxTS数据未参与任何形式的数据融合，它仅在试验中作为位姿估计的参考值用于定量评估轨迹精度。仿真平台以DARPA地下城市挑战赛提供的仿真环境为基础，模拟了真实井下长直隧道的场景(如图10(a)和(c))。隧道长约500 m，墙壁两侧以30 m的间隔交错设置反射标志，仿真车辆运动过程中受到的地面摩擦力、传感器温湿度等均最大程度地与实际情况保持一致。数据采集时行车速度保持15～20 km/h，共采集1000帧点云数据，车辆在各个时刻的精确位姿可直接仿真输出。本文所述方法输出频率约为5 Hz，运行环境为Ubantu16.04，CPU为Intel Core i5-6300@2.3 GHz，内存8 GB。

3.2 试验结果和分析

3.2.1 特征提取结果

本文试验采用实测数据集，线特征提取阈值设置为sE=0.3，面特征提取阈值设置为sS=0.1，强度特征自适应提取的格网化为16×4。如图12分别为开阔区域，城市道路，绿化带和狭长封闭隧道环境下的点云及特征提取结果。图中分别用红、绿、蓝色表示提取的强度、面和线特征，白色为当前帧的全部激光扫描点。线和强度特征点与实际场景的对应物由相应的颜色框框出。如图12(a)橙色框内的地面断点，在地面分割的作用下，可防止因其粗糙度s过大而被误标记为线特征；如图12(b)，在一般城市道路中，具有十分丰富的强度特征点云，且这些特征往往源自静态环境；如图12(c)，隧道中的有效线特征较面特征少很多，但存在稳定的强度特征；如图12(d)，在目标聚类的作用下，橙色框内散乱的植被点作为散点被剔除，避免因s过大而被误标记为线特征。

图12 特征提取结果Fig.12 Result of the features extraction

根据多种场景下的特征点云提取结果可以得出以下结论：①面特征数量往往远多于线特征数量，尤其在相对开阔的区域。将所有特征联合解算，如LOAM，可能会存在约束不均衡的问题。②强度特征广泛存在，并且这些特征往往都是静态目标，如路标或其他的反光标志等，能够提供稳定的匹配，不应被忽略。③提出的自适应强度特征提取方法为在弱结构环境下人工加设强度目标提供可能。

3.2.2 退化检测及时效性结果

本文试验采用实测数据集，分别分析退化检测结果和本文方法的时效性。

退化检测结果：如图13所示为退化因子及退化检测示意图。图13(a)为DF统计结果，其中横轴表示DF的统计区间，纵轴表示该区间的占比。可以看出，DF统计数据呈现一定的聚类效应，可取聚类分界值为阈值rthr，本文试验中取rthr=300。图13(b)为计算DF的关键帧帧号， 纵轴为DF数值。图13中绿色为检测到的退化帧，蓝色为非退化帧。结合实际数据采集情况，红色虚线段内的区域为进入隧道的帧段，在这两段区域均自动判定为退化，与实际情况具有较好的契合度，反映出退化检测的有效性。

时效性分析：当检测到退化时，系统将利用强度特征进行2D激光匹配，对LiDAR的位姿估计进一步修正。图14为发生退化时的数据处理时耗，4组箱线图分别代表DF计算、基于强度特征的二维激光匹配、LOAM地图构建线程及本文所提方法的地图构建线程等部分的耗时情况。由图可以看出，DF的计算及退化检测几乎不耗时，2D激光匹配约需20 ms。LOAM的地图构建线程数据处理时间约为100 ms，本文所提方法约为120 ms。本文系统的地图构建线程以不超过5 Hz的频率接收数据，即该线程的响应间隔为200 ms，低于平均处理时长的120 ms，因此能够达到实时运行。

图13 退化因子及退化检测结果Fig.13 Degeneracy factor detection

图14 时效性分析Fig.14 Timecosts analysis

3.2.3 轨迹估计结果

本文试验采用实测数据集，从相对精度和绝对精度的角度分析定位轨迹精度。

在实测数据试验中，将OxTS与LiDAR的轨迹统一至同一参考系，以OxTS的输出轨迹为真值，内插对应时刻的LiDAR位姿估计，并采用蒙特卡洛试验对位姿估计进行定量评价。选择LOAM和HDL-Graph-SLAM两个具有代表性的系统为比较对象，各系统分别运行10次取各位姿点平均值，轨迹如图15所示。

由图15中可以看出，基于点云NDT匹配的HDL-Graph-SLAM和基于几何特征匹配的LOAM在隧道内沿车辆前进方向出现不同程度的退化，导致位置估计在该方向出现严重偏差。而采用本文方法可以有效修正偏差，得到准确的定位结果。

图15 定位轨迹对比Fig.15 Comparison of the estimated trajectories

相对定位精度：采用文献[23]中的相对精度评估方法，相对误差由一段距离区间内的相对位姿变化估值与真值的差值计算得到，相对误差曲线如图16所示。

图16 固定距离的相对定位误差Fig.16 Relative errors over the fixed lengths

由图16可以看出，3种方法在1.5 km的范围内相对位置误差均优于0.5%，相对角度误差优于0.014°/m。本文方法的相对位置误差明显优于其余两者，且在不同距离区间内保持稳定。LOAM和HDL-Graph-SLAM在某些距离区间内误差有波动，反映出轨迹在某一区域出现严重偏移。由于隧道仅为直线，因此并未造成过大的姿态偏差，3种方法的相对姿态曲线差异不大。

绝对轨迹精度：采用均方根误差(root mean square error,RMSE)作为绝对精度的评价指标，结果见表1。

表1 绝对定位误差

由于LOAM和HDL-SLAM在隧道中出现点云配准退化情况，分别造成了高达34.654 m和141.498 m的绝对位置误差。本文算法的定位精度远高于其余两者，1.5 km的路径范围绝对位置误差为4.726 m，偏航角误差0.747°，在该场景下定位精度较LOAM和HDL-SLAM有大幅提升。统计各位姿点绝对误差得到如图17的累计误差分布图。

图17 绝对定位误差累积分布Fig.17 Cumulative distribution of the absolute errors

由图17中可以看出，在该场景下利用强度信息优化点云配准结果，保证了点云配准在全轨迹下的稳定与可靠。全局范围内定位误差逐步累积，最大不超过10 m，姿态误差最大不超过4°。对比方法的姿态精度与本文所述方法相差不大，但位置精度差异明显。两对比方法的曲线并非呈平滑递增，而是在某些误差区间内陡增，反映出点云配准在某些区域的异常，这与隧道内点云配准退化的事实相吻合。

3.2.4 相对位姿变换结果

图18 相对位姿变换误差Fig.18 Relative transformation errors

由图18中可以看出，在本文的仿真隧道环境内，3种方法的相对旋转变换误差都很很小，整体均小于0.04°，其中本文方法和HDL-Graph-SLAM小于0.01°。说明隧道内的地面和墙壁点云足以保证姿态估计的准确。相对平移变换误差分层明显，其中本文方法的平移误差0.02 m以下，远小于LOAM的0.4 m和HDL-Graph-SLAM的0.08 m，说明了基于强度特征匹配的正确性和有效性，也反映出在这种退化环境下融合强度特征的必要性。

3.2.5 地图构建结果

采用实测数据构建点云地图，通过地图结果对比可直观地看出本文方法在隧道环境下性能的提升。仅采用所采集数据的隧道部分，用3种方法分别构建点云地图，结果如图19所示。

图19 隧道点云构建结果Fig.19 Points map mapping on the remote sensing image

通过对比可以明显看出，HDL-Graph-SLAM和LOAM出现所谓的“长直走廊困境”，导致构建的隧道比实际隧道出现几十米的偏差(LOAM为168.299 m，HDL-Graph-SLAM为90.952 m)。本文方法通过自适应特征提取方法获得更多隧道中的强度特征，以此来弥补点云几何配准时可靠约束的缺失。从地图构建结果上看，本文方法能够在该环境下取得较好的结果，整体隧道结构保持完好。需要注意的是，匹配结果的正确必须建立在有足够的特征的基础上，因此对于某些极端环境，可通过在墙面张贴反光标志或涂抹反光漆等方式人为设置强度标志，本文提出的自适应特征提取算法同样能够识别并提取出这些强度目标用于配准。

为更直观的展示本文方法构建地图的准确性，将构建的完整地图映射在该场景的遥感影像上，如图20所示。从图20中可以看出，所构建的地图结果与实际场景的契合度良好。

图20 遥感影像映射点云Fig.20 Points map mapping on the remote sensing image

3.3 待改进之处

车辆动态运动时采集的点云数据质量往往存在较大问题，尤其是在车辆快速转弯时，点云畸变尤为明显。这可能会导致转弯后的方向偏移，造成较大累积误差。如图15和图20左侧，车辆在十字路口转弯后出现方向偏差。

4 结 语

本文针对隧道环境下基于激光的车辆定位与地图构建问题，提出了强度信息增强的激光SLAM系统。该系统通过自适应算法提取环境中的强度特征，并自动化判断点云配准的退化情况，在几何匹配的基础上增加强度特征的匹配，为点云配准提供更多可靠的约束信息，解决隧道环境下点云匹配的退化问题，使系统能够在长直隧道环境下稳定运行。本文提出的方法是对国际上主流的LOAM系统性能的补充和完善。后期的工作将集中于与IMU的融合方面，为系统提供更多车辆运动状态的信息，以帮助校正动态条件下点云的畸变，并为点云配准提供可靠的初始估计。