顾及室内场景特征的多线激光雷达初始定位

史鹏程，叶 勤，张绍明，邓海峰

1. 自然资源部城市国土资源监测与仿真重点实验室，广东 深圳 518034； 2. 同济大学测绘与地理信息学院，上海 200092； 3. 武汉大学计算机学院，湖北 武汉 430072； 4. 上海华测导航技术股份有限公司，上海 201702

伴随人工智能技术快速发展，自动驾驶不仅改变了人们的出行方式，在物流、餐饮、医疗等领域发挥着重要作用，对于推动智慧化城市进程具有重要意义[1-3]。目前自动驾驶分为L0—L5共6个级别，L3级以上才能实现无须人工干预的无人驾驶。L1—L4可在限定条件下运行，无人驾驶场景分为：包括矿山、机场、港口及园区等低速封闭场景，包括高速公路等高速半封闭场景，以及包括城市中道路等的高速开放场景。无人驾驶对于定位暂无具体严格要求，但目前各个研究机构都在致力于进行高精度定位欲实现厘米级定位效果。

同时定位与地图建图(simultaneous localization and mapping，SLAM)[4]是自动驾驶与机器人导航定位的关键技术。目前根据传感器类型主要分为视觉和激光两大类。视觉SLAM以ORB-SLAM[5]和LSD-SLAM[6]为代表，但是这类方法受限于视觉传感器的量测范围，不适用于本文中的大尺度场景。激光SLAM以LOAM[7]和LeGO-LOAM[8]为代表，量测范围广、精度高，抗干扰能力强，是目前较为稳定和有效的自动驾驶方案。定位初始化是机器人导航定位SLAM中的重要环节，即当传感器初始位于某场景中，需要通过定位初始化确定其在当前场景下的初始位置，为后续导航定位提供初始信息，保证平台定位工作正常进行[9]。在机器人的连续定位过程中，若由于信号遮挡、传感器故障、目标特征缺失等某些不确定性原因造成机器人定位失效，即机器人的追踪丢失，可再次启动定位初始化来恢复机器人的实时位姿，即机器人的绑架恢复[10]。

在室内环境中，基于GNSS信号[11]、第3方开源地图[12]等辅助信号源定位方案效果不佳，全局初始定位的挑战性更大。为简化定位算法，一些方法采用视觉标志[13-14]、QR码[15]、Wi-Fi信号[16]、天花板[17]等人工辅助外源信号方案来进行定位，但是视觉标志方案易受到场景中移动目标遮挡影响，Wi-Fi定位通常无法解决朝向等问题。一些方法根据场景中存在的部分几何特征如线特征[18-19]、角点[20-21]、平面[22-23]、停车场特殊结构[24]等实现定位，但这些方法当场景纹理特征较弱时定位效果欠佳。一些方法利用特殊环境条件如磁场[25]、门牌号[26]等实现定位，但是对于场景有着严格要求，方法适用性有待提高。一些研究[27-28]中采用蒙特卡洛结合神经网络的方法可在全局范围内搜索出最优全局位姿，但在这种方法中一定程度上受限于模型训练效果，并需要数据清洗工作，前期工作量较大。部分多传感器融合方案如相机与LiDAR[29]、IMU与LiDAR[30]等，虽然能增加数据丰富度，但多源数据的融合增加了算法挑战性。

综上关于定位初始化的相关研究，仍存在部分待优化问题：①对于大尺度室内场景的定位初始化应用研究相对较少，其效率与准确度有待提高；②在GNSS信号不佳的室内场景下，利用人工标志的方法，需要提前布设，同时易在遮挡时失效，不依赖于外源信息的定位方案有待研究；③基于特征的定位初始化方法依赖于特征提取效果，稳健的特征提取方法有待研究。大尺度室内环境更有利于机器人导航及自动驾驶产业落地推广，但此类场景占地面积大、地物特征信息较少，光照条件不佳等问题对定位算法挑战性较大。为此，本文提出一种在大尺度室内场景下基于特征模式的机器人定位初始化方法。通过对室内场景特征分析，探索场景中一些稳定的人工构筑物结构及结构组合，并将其定义为特征模式，提高特征提取准确度，进一步改善室内机器人全局定位初始化性能。

1 特征模式提取

图1为本文室内场景下基于特征模式的定位初始化流程。

图1 定位初始化流程Fig.1 Flow chart of localization initialization

1.1 特征模式类别

室内场景中存在的一些稳定的人工构筑物如墙壁、柱体等，这些构筑物位置发生变化的概率极低，同时作为场景中的显著标志，可标示场景的总体布局。将这些稳健结构以及结构组合定义为特征模式。基于文献[31]的研究，本文增加了对称结构模式。如图2为定义的(a)—(e) 5类特征模式。根据结构的几何显著性将特征模式分为3个级别，即图2(a)为1级结构，图2(b)—图2(d)为2级结构，图2(e)为3级结构。图2中第1行为实际场景结构，第2行为结构示意图，第3行为简化后的特征模式表达形式。

图2 特征模式Fig.2 Feature pattern

1.2 实时帧特征模式提取

多线激光雷达是自动驾驶中常用的传感器，具有距离分辨率高、抗干扰能力强等优点。其数据具有如下特征：①组织形式，一系列具有三维坐标信息的点；②几何形态，呈现多条线状分布；③空间分布，不规则离散分布，不同扫描线点云之间距离差不同；④空间视场角，水平视角为360°，竖直方向存在一定视角限制。

1.2.1 预处理

在获取实时点云S后，需对其预处理为后续特征提取做准备。主要包括以下步骤。

(1) 直通滤波：通过两个高度阈值(H1,H2)滤除部分干扰点云，得到滤波点云SF。

(1)

(2)

利用奇异值分解依次得出点云3个正交方向向量(k,l,m)，令k、l所成平面与XOY面对齐，实现校正。

(3) 点云投影：将校正后点云正射投影至XOY平面，得到投影点云SJ。

1.2.2 一级结构提取

室内场景中最为显著的结构为多段墙体结构。基于美观和布局考虑，室内墙体通常会在方向上保持一致性。因此，采用一种循环分割的策略来提取场景中线状分布的分段墙体结构。主要包括以下步骤。

(1) 直线分割。如式(3)所示

(3)

(4)

(5)

式中，i为当前分割索引，se为1时停止分割，为0时继续分割；Nlp为百分比阈值，ci为第i次分割累积分割次数，Cm为分割次数阈值。

(6)

(7)

式中，ec为点云聚类的有效性；Nlc为点云数量阈值；l(·)为计算点云长度，采用最小外接立方体的对角线长度近似，Llc为长度阈值。

图3 截断误差Fig.3 Truncation error

图4 线段拟合Fig.4 Line fitting

(4) 干扰线段剔除。如图4(b)在直线分割时，由于参数设置等问题，某点云线段可能会被分割为双线段结构(L1,L2)。这些双线段结具有如下特征：①距离较近;②方向近似平行；③彼此重叠度较高。基于以上特征，寻找出双线段结构，并保留其中较长线段得到L3。

1.2.3 二级结构提取

二级结构中包含平行、对称和垂直结构(图2(b)—图2(d))。相比于单线段结构，二级结构(线段对)在特征描述时更加稳健，具有更多属性，在后续匹配阶段效率更高。其提取主要包括以下步骤。

(1) 确定线段对。单线段结构两两组合为线段对集合Lp={(Li,Lj)i≠j,i,j∈nL}，nL为L数量。

(2) 夹角计算。设定平行和垂直余弦阈值Cp、Cv筛选候选平行和垂直线段对Lpp、Lpv。

(3) 距离计算。式(8)为计算近似平行线段对的距离；式(9)为根据线段距离对平行线段对分类

(8)

(9)

(4) 重叠度计算。如图5(a)在线段对中计算两线段重叠率o12o21,图5(b)为无重叠线段对距离dn计算方式。

图5 重叠度计算Fig.5 The calculation of overlapping degree

在式(10)中对候选平行和对称结构筛选得到AL和PL

(10)

(5) 垂足计算。根据式(11)计算两线段的垂足点(x,y)

(11)

式中a1、b1、c1、a2、b2、c2为线段的直线参数。

1.2.4 3级结构提取

3级结构中定义了圆弧结构(图2(e))。AS对应的是场景中圆柱等柱面结构。其提取采用一种结合量化分析的序列化提取方法，主要包含以下4个步骤。

(12)

式中，Ix(·)索引函数，可保留三维点索引。

(4) 弧弦分析。如图6在圆弧点云中寻找3点关键几何点q1q2q3，其中S为激光雷达，q2为圆弧中点，是当前圆弧点云中距离S最近的点。q2q3为两圆弧端点，是圆弧上q2两侧距S最远的点，q1q2q3近似构成一个等腰三角形即式(13)中弧弦约束，在几何形态上不满足该弧弦条件的圆弧结构将会被剔除掉，最后得到AS。

图6 弧弦分析Fig.6 Chord analysis

(13)

式中，d(A,B)表示AB的距离；a(A,B,C)表示ABC所形成的角度；Dch是距离误差阈值；Ach是角度误差阈值。

1.3 点云地图特征模式提取

相比于单帧激光雷达数据，地图点云数据较为稠密，在实时数据中适用的特征模式提取方法不适用于点云地图。因此为提高点云地图M上特征模式提取准确度，提出一种半自动化方法提取地图特征模式。点云地图特征模式提取流程如下(图7)。

(1) 地图拼接。采用激光SLAM算法LOAM计算LiDAR数据帧间相对位姿，通过选取关键帧Sk的方式进行地图数据拼接，完成了地图点云的初步拼接。

图7 地图数据处理流程Fig.7 Map data processing

(2) 地图预处理。采用与文献[32]相同的预处理方法，即分别使用离群点去除，统计滤波等滤除部分噪声点云，并用直通滤波滤除部分干扰结构。

(3) 第1次地图裁剪。采用CloudCompare软件的编辑裁剪功能，手动勾选部分拼接偏差点，通过目视检查场景结构的整体精度控制地图数据修剪过程。

(4) 地图校正投影。采用1.2.1节中点云校正的方法对修剪后的地图数据进行校正， 并投影到XOY平面内，得到投影点云MJ。

(5) 第2次地图裁剪。手动修剪MJ，裁剪出每一特征模式对应点云聚类。

(6) 特征模式提取。根据模式类别采用Ransac在对应点云聚类中拟合特征模式。

2 地图数据管理

为提高定位初始化的效率，本文提出一种高效的地图数据管理方案。以文本的方式对地图相关数进行存储与管理，同一场景下只需完一次地图数据处理并离线保存，避免了重复烦琐的地图数据操作。仅在场景发生变化更新时，重复一次地图数据处理流程，更新地图数据即可。如图8为地图数据存储格式，其中主要包括以下两方面。

(1) 地图特征模式：根据特征模式初始化方式，对各类特征模式单独采用离线文本方式进行存储。其存储格式如图8中L、AL、PL、VL、AS等特征模式数据存储格式。

图8 地图数据管理格式Fig.8 Management format of map data

(2) 地图格网：根据格网分辨率r和投影点云MJ生成二值化格网地图(图9)。如图8中MG行记录格网非0元素位置，1为行号，从2开始依次为列号。

图9 地图网格Fig.9 Map grid

3 位姿求解

由于室内场景地面起伏波动较小，6自由度(degree of freedom，DOF)的位姿可退化为3自由度(tx、ty、θ)即可完成室内的定位与建图任务。其中tx和ty分别为X轴和Y轴方向平移量，θ则为绕着Z轴旋转角度量。通过构建对应各特征模式的误差方程组合及匹配与配准策略，实现定位初始化，主要包含以下步骤。

(2) 误差方程构建。采用两类误差方程，即点到点距离与点到线段距离。在各类特征模中，构建适应其几何属性的多个误差方程组合。

(3) 位姿参数求解。主要包括以下步骤：①位姿参数初始化为0；②采用L-M梯度下降方法对误差方程进行优化，推导最终得到关于位姿增量x的线性方程如式(14)所示

(H+λDTD)Δx=g

(14)

式中，H=J(x)TJ(x)，g=-J(x)Tf(x)，f(x)为误差方程，D取对角矩阵即JTJ的对角元素平方根。采用QR分解计算出Δx的值；③计算ρ

(15)

若ρ>0.75，令μ=2μ,若ρ<0.25，令μ=0.5μ，其中μ为优化半径，若ρ大于0.5，则xk+1=xk+Δxk；④若Δxk足够小，则停止，否则返回步骤②。

⑤解算出当前匹配元素所确定的位姿Ti=(txi,tyi,θi)。若当前匹配元素存在角度不确定时，按照式(16)更新θ角度作为初始值

(16)

再次进行求解，得到Ti+1=(txi+1,tyi+1,θi+1)，计算出当前特征模式匹配对所得到的所有位姿集合{Ti}k，k为当前特征模式匹配对索引。

(17)

(18)

(5) 遍历所有候选特征模式匹配对，重复步骤(1)—步骤(4)，得到匹配位姿集合序列，{Ti}0,{Ti}1…{Ti}m。在所有初始化成功的匹配位姿中，选出得分最高者作为最终的初始化匹配位姿。

4 试 验

4.1 试验数据

图10(a)—图10(c)选取走廊、大厅、地下停车场3种代表性的大尺度室内结构化场景进行试验。图10(a)为室内走廊，占地约750 m2，包含多段墙壁、柱子、和围栏扶手等。图10(b)是室内大厅，占地约800 m2，场景中包含多个圆形柱子、展台桌、墙壁、盆栽和长楼梯等，整个场景相对空旷。作为挑战性之一，以上两个场景都存在部分玻璃结构，对数据获取存在一定影响。图10(c)为地下停车场，占地约3000 m2，除包含多段墙壁、柱子、警示标志，还包括众多车辆(干扰)等。停车场挑战性相对较大，光照条件不佳同时场景中存在大量相似的车体等结构，对于墙壁等稳健结构的提取存在一定的干扰。

试验采用RoboSense16线激光雷达如图10(d)所示。将激光雷达一段时间内采集的点云数据保存，并进行离线全局定位初始化试验。试验中建图与定位使用同一组数据，但由于地图数据在拼接过程中采用关键帧策略，同时地图数据在处理过程中通过采样、滤波、裁剪等操作，完全不同于实时帧数据处理流程，因此可使用同组数据在全局范围内测试本文定位方法。试验中采用激光SLAM算法LOAM计算匹配位姿作为相对真值。LOAM具有0.55%的平移精度，0.001 3 deg/m的角度精度，是目前KITTI数据中排名前三的SLAM算法。当计算位姿与相对真值误差在2 m认为初始化成功，因为在大尺度室内场景中2 m左右初始位置误差可在后续定位与建图过程中逐步降低，不影响整体自动驾驶性能。试验环境为Intel Core i7-5500U CPU @2.40 GHz 2.39 GHz和8.0 GB of RAM。

4.2 试验结果

由于单线段结构L匹配效率较低，实际场景应用性不强，因此，试验中未统计基于L结构的初始化结果。图11在3个场景中分别选取某帧数据进行初始化效果显示，图11(a)为地图中提取的特征模式，并用不同颜色表示。图11(b)为定位初始化匹配效果的侧视图。黄色为地图点云，红色为初始定位前单帧激光雷达点云，绿色为初始定位后，利用匹配位姿将单帧点云转入地图坐标下的点云。图11为了提高显示效果，对地图数据进行降采样显示。在初始定位后，单帧点云与地图点云目视匹配效果较好，可有效确定传感器在全局地图中的初始位姿。

图10 试验场景与设备Fig.10 Experimental scene and equipment

图11 定位初始化效果Fig.11 The effect of localization initialization

在3个场景中各取3000、548、1015帧数据进行全局定位初始化测试，见表1。并统计了包括地图模式数、定位初始化准确率、召回率、一次成功初始化时间、定位误差均值等各指标。由于文中初始定位位姿为3自由度位姿，统计的初始定位误差为水平定位精度。

(1) 场景1(走廊)。走廊场景中，相互垂直的墙壁、门窗、柱子等所形成的对称、圆弧模式较为显著。地图特征模式数量分别为：PL-41、VL-53、AL-14、AS-8。在定位准确性方面，VL准确率达到94%以上，AS和AL分别达到76%和69%左右，PL相对较低。各类特征模式在完成一次成功初始化时所需时间分别为245.419 ms、457.3 ms、177.796 ms、870.941 ms，其中PL的定位误差相对较大达到0.28 m，其他特征模式的定位误差均在0.03 m以内。

(2) 场景2(大厅)。与走廊场景相似，大厅中的特征模式均较为显著。地图特征模式数量分别为：PL-23、VL-47、AL-11、AS-11。各类模式的定位初始化准确率均达到了85%以上，平均初始化速度分别为209.908 ms、382.1 ms、82.638 ms、874.508 ms，定位误差分别为0.048 5 m、0.028 3 m、0.026 4 m、0.057 6 m均保持在0.06 m以内。各类特征模式在该场景下的定位初始化效果良好。

(3) 场景3(停车场)。停车场是挑战性较大的一个场景，地图特征模式数量分别为：PL-358、VL-562、AL-37。场景地图中未出现AS模式，因此未对AS进行定位初始化统计。由于该场景中存在多个车辆结构，造成地图中的特征模式相对较多，定位初始化效率相对较低。VL与AL的准确率在62%和66%左右，但定位误差均在0.04 m以内，PL的准确率在34%左右，定位误差在0.136 7 m左右。相比于前两个场景，整体定位准确率稍有下降，但垂直和对称模式仍具有良好的定位精度。

表1 全局定位初始化试验统计

4.3 讨论与分析

试验结果表明，本文方法可有效完成室内大尺度场景的定位初始化，且速度较快。试验选取的典型的室内大场景具备一定的挑战性，如长走廊和大厅玻璃结构等，场景空旷特征信息不足，地下停车场光照不佳，存在车辆等大型动态障碍物。目前实际的机器人导航或自动驾驶中初始定位方法通常采用室外GNSS、室内WIFI、人工标志等辅助信号源或2D LiDAR、相机等传感器，这些方法在信号不佳、标志遮挡时效果欠佳、同时辅助信号源的设置需要较多人工干预，自动化程度有待提高，在典型室内场景中定位效果不佳。

在定位召回率方面，所选取的室内试验场景占地面积较大，某些数据存在遮挡或在某些位置不存在某些特定特征模式，如在走廊场景中，试验中激光雷达曾在电梯附近小空间内运行一段时间，此处位置特征模式不足，类似情况也存在于大厅和停车场场景，因此造成初始定位试验的整体召回率较低。在实际机器人自主导航或自动驾驶时，可以避开像电梯附近这种人流量大、空间小的位置，选择视野开阔位置进行定位初始化，若定位初始化失败时，可适当移动当前传感器位置，重新定位即可。

本文方法召回率虽然不高，但定位误差相对较低，可以较好地完成定位初始化任务。由于地图数据为单独离线处理，因此在处理时可有选择地剔除部分对初始化意义不大的特征数据，以提高整体定位速度。如试验场景3中由于车辆信息的存在导致提取的地图模式(干扰特征)较其他场景多，因此在试验中，本文在地图特征模式提取中有选择地删除掉部分(干扰)特征模式，以提高定位初始化的效率。同时在实际场景中，还可通过特征模式间相互组合的方式以提高定位初始化的召回率。

5 结 论

本文提出一种大尺度室内环境下基于特征模式的室内机器人定位初始化方法。在分析室内场景常见结构类别与特性基础上，提出了特征模式的概念及多线激光雷达点云中特征模式提取的方法，并对其分级管理，采用轻量级的表达方式显著提高特征表达效率。本文的半自动化地图特征模式提取流程，保证了地图数据准确度，并设计一种高效的地图数据管理方案实现对地图数据的高效管理，提高了全局初始定位效率。针对各类特征模式构建自适应匹配与配准策略，并利用地图网格作为初始化评价指标，实现室内场景下传感器的快速、准确的定位初始化，为室内机器人的快速初始定位提供一种新方法。为验证本文方法的有效性，选取了3类代表性场景进行定位初始化试验，并对试验结果进行评价分析。本文方法不局限于16线LiDAR数据，可应用于自动驾驶中其他线数的多线激光雷达数据。本文全局定位初始化性能基本满足实际室内自动驾驶性能需求，可应用于其初始定位模块中。在进一步的研究工作中，将在更多类型场景中探索典型地物特征，扩充特征模式库。并研究融合IMU等传感器的实时定位系统构建，以应对更加复杂的室内大场景下定位工作。