2. 慧拓无限科技有限公司, 北京 100089;
3. 中国科学院自动化研究所复杂系统管理与控制国家重点实验室, 北京 100190;
4. 中山大学数据科学与计算机学院, 广东 广州 510275
2. Waytous Infinity Inc. Co., Ltd., Beijing 100089, China;
3. The State Key Laboratory of Management and Control for Complex Systems, Institute of Automation, Chinese Academy of Science, Beijing 100190, China;
4. School of Data and Computer Science, Sun Yat-sen University, Guangzhou 510275, China
在大数据和人工智能等新技术浪潮的席卷下，全球新一轮的科技革命和产业革命已悄然而至，传统测绘技术的转型升级势在必行[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) 点云投影。根据3D LiDAR的扫描特性，将tk的点云Pk={pk, 1, …, pk, n}, pk, n=
(1)
(2)
式中，[u, v]表示点p=[x y z]T在Dk上的像素坐标；Δϕ和Δθ分别为投影图像的水平和垂直角分辨率。图 2为投影示意图，投影圆柱面和深度图上的不同线条展示了投影面展开前后的对应关系。
|
| 图 2 点云投影深度图实例 Fig. 2 Example of points-to-depth image projection |
(2) 点云分块。将点云划分为i个扇形区域Bk, i，并以水平距离ρ划分为j个扇环Ck, i, j(图 3)
(3)
|
| 图 3 点云地面分割实例 Fig. 3 Example of ground points segmentation |
以扇环Ck, i, j内所有点的最小z值和对应水平距离ρ构成二维特征表示Ck, i, j，即
(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中红色虚线所示)。
(4) 地面点标记。在Dk上遍历所有点，将点pk, n代入式，映射为
(4)
式中，ak, i和bk, i为Bk, i的拟合直线参数。当ak, i和bk, i存在时，将满足rk, n < rthr的点pk, n被标记为地面点，如图 3中绿色点即为分割的地面点Gk。
2.1.2 特征提取特征点的自适应提取算法基于深度图实现，主要步骤如下。
(1) 目标分割。根据上节所述的地面点分割结果得到非地面点深度图Vk=Dk-Gk。采用基于夹角值的快速目标分割方法将点云聚类，算法细节可参考文献[25]，聚类后得到如图 4的聚类点云Lk。若Lk中某一类的点数少于阈值nthr，则该类的所有点将作为野点从Pk中剔除。
|
| 图 4 目标聚类结构结果 Fig. 4 Result of the object clustering |
(2) 线、面特征点提取。在Dk上计算各像素点的粗糙度s，方法与文献[11]中相同
(5)
式中，d表示深度；Pneig为深度图上的邻域像素。遍历Vk，将满足s>sE的nE个像素标记为线特征Ek, 满足s < sS的nS个像素标记为面特征Sk。
(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 点云配准将tk-1和tk时刻的点云Pk-1和Pk配准，估计相对运动Tkk-1。本文采用LOAM中点到线和点到面的特征匹配方法。即在Ek-1中搜索pk, n∈Ek的一对匹配点(pk-1, i, pk-1, j)，在Sk-1中搜索面特征点pk, n∈Sk的3个匹配点(pk-1, i, pk-1, j, pk-1, l)，分别建立点到线和点到面距离最小的约束
(6)
(7)
得到距离d关于Tkk-1的函数，从而由几何特征点构建目标方程
(8)
采用Levenberg-Maquardt算法可迭代求解三维变换Tkk-1的估值，优化计算时采用李代数的方式表达三维变换Tkk-1[26]。
2.2.2 位姿推估定义初始时刻t0的位姿为单位阵I，并以其为参考。根据tk-1时刻的位姿Tk-10和相对运动Tkk-1，可推估tk的位姿Tk0
(9)
为限制激光里程计的误差累计和位姿漂移，将Pk={pk, 1, pk, 2, …, pk, n}根据激光里程计推估的位姿Tk0进行变换，得到里程计参考系下的点云
(10)
地图配准仅采用几何特征，即Ek到MEk和Sk到MSk的配准，与激光里程计中所用方法一致，算法细节可参考文献[11]。
2.3.2 退化检测车载点云配准时，地面点可以提供俯仰角、横滚角及z轴方向的约束，非地面目标则可以提供偏航角、x轴及y轴方向的约束。在空旷场地或长直隧道等环境下，由于难以建立准确的同名点数据关联，导致状态空间上某些方向约束不足，点云配准解算的法方程矩阵呈现病态，产生退化现象。
本文采用文献[27]中提出的退化因子(degeneracy factor，DF)算法进行退化检测及退化方向判断。对于方程Ax=b，退化因子rD定义为一个仅与ATA的特征值λi相关的量
(11)
式中，λmin表示最小特征值。在本文点云配准问题中，A即为目标函数的一阶雅克比矩阵
(12)
采用奇异值分解方法得到ATA的6个特征值diag{λ1…λ6}，分别对应Tkk-1的x，y，z，横滚，俯仰，偏航等6个状态方向。取λmin=min{λ1, λ2, …, λ6}代入式计算rD，当rD < rthr时，表明配准出现退化，λmin对应的状态方向即为退化方向。rthr可根据实测数据统计得到，数据处理实例见3.2.2节。
2.3.3 配准优化无人车点云数据包含大量地面点，因此对俯仰、横滚及z方向的约束是充分的，一般不会出现退化问题，而偏航、x及y方向则不然。图 7为长直隧道中相隔约10 m的两帧点云的俯视图和侧视图。显然仅利用隧道几何特征很难得到车辆前进方向的准确估计，即配准在x方向出现退化。
|
| 图 7 点云配准退化 Fig. 7 The points registration |
结合无人车点云数据的这一特点，并考虑到点云强度受测量距离、反射角度等多方面因素影响的现实情况，本文提出采用基于三次B样条概率地图的2D点云匹配算法，当发生水平方向退化时，对位姿估计
(1) 点云压缩。由于位姿估计
(13)
(2) 强度特征与地图匹配。强度地图由三次B样条概率图表示，相较于离散格网的占据栅格图它具有出色的连续性和更好的稳定性。强度地图MIk在任意一点τ=[x, y]T处的概率可由映射函数s(τ): R2→R表示
(14)
式中，cij∈R3×3表示离散的控制点；bix(x)和bjy(y)表示在第i和j维度的三次B样条函数。将式(14)中各项展开为列向量，并改写为向量相乘的形式
(15)
点云到地图的配准可表示为点云在强度概率地图上对应位置的概率和最大，由此写出强度目标方程
(16)
采用L-M算法可求解位姿优化量ΔT2D。
(3) 位姿优化。2D位姿修正量ΔT2D和位姿估计量
(17)
需要说明的是，式(17)并非严格意义的向量相加，其中⊕表示对应项相加。
2.3.4 地图更新本文采用多尺度地图模型建立线、面特征的局部地图MEk和MSk[28]，采用三次B样条概率地图模型建立强度特征的局部地图MLk。局部地图均以传感器为中心，随车辆运动而滑动更新。
(1) 局部几何地图更新。几何地图分为3个尺度，各尺度下栅格分辨率由粗到细，相互嵌套。栅格中的点存储于环形容器中，维持栅格内点个数不变，以保持地图大小基本固定。将Ek和Sk根据
图 8为多尺度栅格地图的示意图，红黄蓝色栅格分别表示不同尺度的栅格地图；黑色点表示栅格中的点；LiDAR运动时，地图随之移动，地图中心由Oold向Onew移动。
|
| 图 8 局部栅格地图 Fig. 8 Local voxel map |
(2) 局部强度地图更新。压缩后的二维强度特征Ĩ0={τn}作为遮挡点{τocc, n}，各激光点通路上的连通区域均为未遮挡区域，即非遮挡点{τfree, n}
(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中绿色为检测到的退化帧，蓝色为非退化帧。结合实际数据采集情况，红色虚线段内的区域为进入隧道的帧段，在这两段区域均自动判定为退化，与实际情况具有较好的契合度，反映出退化检测的有效性。
|
| 图 13 退化因子及退化检测结果 Fig. 13 Degeneracy factor detection |
时效性分析：当检测到退化时，系统将利用强度特征进行2D激光匹配，对LiDAR的位姿估计进一步修正。图 14为发生退化时的数据处理时耗，4组箱线图分别代表DF计算、基于强度特征的二维激光匹配、LOAM地图构建线程及本文所提方法的地图构建线程等部分的耗时情况。由图可以看出，DF的计算及退化检测几乎不耗时，2D激光匹配约需20 ms。LOAM的地图构建线程数据处理时间约为100 ms，本文所提方法约为120 ms。本文系统的地图构建线程以不超过5 Hz的频率接收数据，即该线程的响应间隔为200 ms，低于平均处理时长的120 ms，因此能够达到实时运行。
|
| 图 14 时效性分析 Fig. 14 Timecosts analysis |
3.2.3 轨迹估计结果
本文试验采用实测数据集，从相对精度和绝对精度的角度分析定位轨迹精度。
在实测数据试验中，将OxTS与LiDAR的轨迹统一至同一参考系，以OxTS的输出轨迹为真值，内插对应时刻的LiDAR位姿估计，并采用蒙特卡洛试验对位姿估计进行定量评价。选择LOAM和HDL-Graph-SLAM两个具有代表性的系统为比较对象，各系统分别运行10次取各位姿点平均值，轨迹如图 15所示。
|
| 图 15 定位轨迹对比 Fig. 15 Comparison of the estimated trajectories |
由图 15中可以看出，基于点云NDT匹配的HDL-Graph-SLAM和基于几何特征匹配的LOAM在隧道内沿车辆前进方向出现不同程度的退化，导致位置估计在该方向出现严重偏差。而采用本文方法可以有效修正偏差，得到准确的定位结果。
相对定位精度：采用文献[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。
| 方法 | 位置误差/m | 姿态误差 | |||
| x | y | 整体误差 | 偏航角/(°) | ||
| LOAM | 29.478 | 18.220 | 34.654 | 1.586 | |
| HDL-SLAM | 119.756 | 75.368 | 141.498 | 2.408 | |
| 本文方法 | 2.541 | 3.985 | 4.726 | 0.747 | |
由于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 相对位姿变换结果为进一步说明3种LiDAR SLAM方法在隧道内产生如此巨大偏差的原因，并利用多种数据验证本文所提方法的优越性，采用仿真数据对相对位姿变换精度定量评估。试验中，计算上述3种LiDAR SLAM方法定位结果在tk与tk+1时刻的相对位姿变换Tkk+1，并与仿真真值TGT比较。以欧拉角和平移向量描述相对位姿变换误差rk=vec(Tkk+1·TGT-1)，并分别计算平移和旋转误差的模，结果如图 18所示。
|
| 图 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的融合方面，为系统提供更多车辆运动状态的信息，以帮助校正动态条件下点云的畸变，并为点云配准提供可靠的初始估计。
| [1] |
宁津生. 测绘科学与技术转型升级发展战略研究[J]. 武汉大学学报(信息科学版), 2019, 44(1): 1-9. NING Jinsheng. Research on the development strategy of surveying and mapping science and technology transformation and upgrading[J]. Geomatics and Information Science of Wuhan University, 2019, 44(1): 1-9. |
| [2] |
李德仁, 龚健雅, 邵振峰. 从数字地球到智慧地球[J]. 武汉大学学报(信息科学版), 2010, 35(2): 127-132, 253. LI Deren, GONG Jianya, SHAO Zhenfeng. From digital earth to smart earth[J]. Geomatics and Information Science of Wuhan University, 2010, 35(2): 127-132, 253. |
| [3] |
卢秀山, 滕腾, 刘如飞. 移动测量、地理信息更新与城市管理智能化[J]. 测绘学报, 2017, 46(10): 1592-1597. LU Xiushan, TENG Teng, LIU Rufei. Mobile mapping, geographic information update and urban management intelligence[J]. Acta Geodaetica et Cartographica Sinica, 2017, 46(10): 1592-1597. DOI:10.11947/j.AGCS.2017.20170327 |
| [4] |
陈驰, 杨必胜, 田茂, 等. 车载MMS激光点云与序列全景影像自动配准方法[J]. 测绘学报, 2018, 47(2): 215-224. CHEN Chi, YANG Bisheng, TIAN Mao, et al. Automatic registration of vehicle-borne mobile mapping laser point cloud and sequent panoramas[J]. Acta Geodaetica et Cartographica Sinica, 2018, 47(2): 215-224. DOI:10.11947/j.AGCS.2018.20170520 |
| [5] |
SINGANDHUPE A, LA H M. A review of SLAM techniques and security in autonomous driving[C]//Proceedings of the 3rd IEEE International Conference on Robotic Computing (IRC). 2019, Naples, Italy: IEEE, 2019: 602-607.
|
| [6] |
ZAFFAR M, EHSAN S, STOLKIN R, et al. Sensors, SLAM and long-term autonomy: a review[C]//Proceedings of 2018 NASA/ESA Conference on Adaptive Hardware and Systems (AHS). Edinburgh, UK: IEEE, 2018: 285-290.
|
| [7] |
GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with Rao-blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46. DOI:10.1109/TRO.2006.889486 |
| [8] |
KOHLBRECHER S, VON STRYK O, MEYER J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//Proceedings of 2011 IEEE International Symposium on Safety, Security, and Rescue Robotics. Kyoto, Japan: IEEE, 2011: 155-160.
|
| [9] |
HESS W, KOHLER D, RAPP H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//Proceedings of 2016 IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden: IEEE, 2016: 1271-1278.
|
| [10] |
RODRIGUES R T, TSIOGKAS N, AGUIAR A P, et al. B-spline surfaces for range-based environment mapping[C]//Proceedings of 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Las Vegas, NV, USA: IEEE, 2021: 10774-10779.
|
| [11] |
ZHANG Ji, SINGH S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots, 2017, 41(2): 401-416. DOI:10.1007/s10514-016-9548-2 |
| [12] |
SHAN Tixiao, ENGLOT B. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Madrid, Spain: IEEE, 2018: 4758-4765.
|
| [13] |
LIN Jiarong, ZHANG Fu. Loam livox: a fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV[C]//Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris, France: IEEE, 2020: 3126-3131.
|
| [14] |
JIAO Jianhao, YE Haoyang, ZHU Yilong, et al. Robust odometry and mapping for multi-LiDAR systems with online extrinsic calibration[J]. IEEE Transactions on Robotics, 8287, PP(99): 1-10. |
| [15] |
MOOSMANN F, STILLER C. Velodyne SLAM[C]//Proceedings of 2011 IEEE Intelligent Vehicles Symposium (Ⅳ). Baden-Baden, Germany: IEEE, 2011: 393-398.
|
| [16] |
KOIDE K, JUN M, EMANUELE M. A portable 3D LIDAR-based system for long-term and wide-area people behavior measurement[J]. International Journal of Advanced Robotic Systems, 2019, 16(2): 1-16. |
| [17] |
BEHLEY J, STACHNISS C. Efficient surfel-based SLAM using 3D laser range data in urban environments[C]//Proceedings of Robotics: Science and Systems ⅩⅣ. [S. l. ]: Science and Systems Foundation, 2018: 16-25.
|
| [18] |
DROESCHEL D, SCHWARZ M, BEHNKE S. Continuous mapping and localization for autonomous navigation in rough terrain using a 3D laser scanner[J]. Robotics and Autonomous Systems, 2017, 88: 104-115. DOI:10.1016/j.robot.2016.10.017 |
| [19] |
TIAN Y, LIU X, LI L, et al. Intensity-assisted ICP for fast registration of 2D-LiDAR[J]. Sensors (Basel, Switzerland), 2019, 19(9). DOI:10.3390/s19092124 |
| [20] |
KHAN S, WOLLHERR D, BUSS M. Modeling laser intensities for simultaneous localization and mapping[J]. IEEE Robotics and Automation Letters, 2016, 1(2): 692-699. DOI:10.1109/LRA.2016.2516592 |
| [21] |
WANG Han, WANG Chen, XIE Lihua. Intensity-SLAM: intensity assisted localization and mapping for large scale environment[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1715-1721. DOI:10.1109/LRA.2021.3059567 |
| [22] |
PARK Y S, JANG H, KIM A. I-LOAM: intensity enhanced LiDAR odometry and mapping[C]//Proceedings of the 17th International Conference on Ubiquitous Robots (UR). Kyoto, Japan: IEEE, 2020: 455-458.
|
| [23] |
GEIGER A, LENZ P, URTASUN R. Are we ready for autonomous driving? The KITTI vision benchmark suite[C]//Proceedings of 2012 IEEE Conference on Computer Vision and Pattern Recognition. Providence, RI, USA: IEEE, 2012: 3354-3361.
|
| [24] |
GEIGER A, LENZ P, STILLER C, et al. Vision meets robotics: The KITTI dataset[J]. The International Journal of Robotics Research, 2013, 32(11): 1231-1237. DOI:10.1177/0278364913491297 |
| [25] |
BOGOSLAVSKYI I, STACHNISS C. Efficient online segmentation for sparse 3D laser scans[J]. PFG - Journal of Photogrammetry, Remote Sensing and Geoinformation Science, 2017, 85(1): 41-52. DOI:10.1007/s41064-016-0003-y |
| [26] |
BARFOOT T D. State estimation for robotics[M]. Cambridge: Cambridge University Press, 2017.
|
| [27] |
ZHANG Ji, KAESS M, SINGH S. On degeneracy of optimization-based state estimation problems[C]//Proceedings of 2016 IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden: IEEE, 2016: 809-816.
|
| [28] |
李帅鑫, 李广云, 王力, 等. LiDAR/IMU紧耦合的实时定位方法[J]. 自动化学报, 2021, 47(6): 1377-1389. LI Shuaixin, LI Guangyun, WANG Li, et al. LiDAR/IMU tightly coupled real-time localization method[J]. Acta Automatica Sinica, 2021, 47(6): 1377-1389. |