基于点云聚类评估的激光雷达鲁棒定位方法

方 玮，赖际舟，吕 品，郑国庆，温烨贝

(南京航空航天大学自动化学院，南京 211106)

0 引言

工业无人车是一种具有自主定位、感知、规划和运行能力的智能机器人。目前，在园区巡检、无人物流等方面有着越来越广泛的应用。导航定位作为无人车自主运行的前提，其精度直接影响了无人车运行的可靠性以及任务完成能力。

目前,无人车大多依赖卫星进行定位，因此只能运行于卫星信号良好的开阔环境。然而,随着需求的多样化，无人车需要在一些复杂环境中(例如工业园区内)进行作业。该环境内由于受到建筑物、树木等的遮挡，卫星信号的质量难以得到保障，从而影响了无人车的定位精度。对此，一般采取的做法是增加其他类型的传感器，例如惯性测量单元(Inertial Measurement Unit，IMU)、轮式编码里程计、激光雷达(Light Detection and Ranging，LiDAR)、视觉传感器等，最终通过融合算法获取最终的载体位置和姿态。

激光雷达由于不依赖环境光照、测距精度高的特点，相较于视觉传感器有着更高的稳定性和精度，因此逐渐成为无人车和无人机导航主要的传感器[1-2]。通过激光雷达同步定位与构图(Simultaneous Localization and Mapping，SLAM)的方法对无人车进行定位，在未发生回环的情况下定位误差会随行驶里程而发散。近年来，基于先验地图的激光雷达定位方法成为无人车定位的主要方式之一，通过事先构建环境的全局无漂地图，无人车将激光雷达点云实时匹配至先验地图，从而可以消除激光雷达里程计的漂移，定位精度可以达到厘米级。

Zhang J.等[3-4]提出了基于激光雷达的SLAM框架LOAM(Lidar Odometry and Mapping)，首先基于曲率计算公式提取一帧点云中的角点和平面点，然后针对提取的特征点分别构建了点到直线以及点到平面的距离误差函数，最终采用Levenberg-Marquardt法最小化距离误差函数以获取载体的位姿。Shan T.等[5]在LOAM的基础上，针对地面车辆的应用场景进行了改进，提出了LeGO-LOAM框架，首先利用地面点云估计车辆在横滚、俯仰和高度方向的运动变化，然后在此基础上利用其他非地面点云估计另外三个自由度的变化，最终得到车辆的六自由度运动信息，相较于LOAM，它的精度相当但实时性更好。

不同于卫星的几何式定位，IMU、里程计、激光雷达或者视觉都是通过累积每两个时刻之间的载体位姿变化进行递推式的定位，因此不可避免地会存在累积误差。消除累积误差的方法是在定位过程中融合全局信息，通常采用位置信息已知的路标或者地图提供全局的位置信息。R.Mur-Artal等[6]提出了一种基于ORB(Oriented FAST and BRIEF)特征[7]的视觉里程计，但其对环境光照条件较为敏感。为了解决该问题，文献[8]首先使用激光雷达构建了全局的先验地图，然后基于该地图，提出了一种混合光束平差法获取当前相机的位姿。文献[9]同样使用激光雷达构建了场景地图作为先验信息，但其使用相机的测量匹配至激光雷达生成的地图以获取载体位姿。Koide等[10]使用基于图优化的SLAM方法融合激光雷达和全球定位系统(Global Positioning System，GPS)量测，前端使用正态分布变换(Normal Distributions Transform，NDT)[11]方法进行激光雷达点云的扫描匹配，获取激光雷达里程计信息，在后端采用GPS信息作为位姿图的约束，最终通过因子图融合的方式获取载体位姿。Zhu Y.等[12]前端采用LOAM[3]进行点云匹配，后端基于g2o库[13]融合激光雷达和GPS量测，首先构建了场景的点云地图，然后采用迭代最近点(Iterative Closest Point，ICP)[14]方法将激光雷达点云匹配至该地图对无人车进行定位，定位精度达到10cm。N.Steinke等[15]通过提取激光雷达点云中的线、面特征匹配至地理数据库，实现了厘米级的定位精度，但该方法需要首先获取目标区域内的地理数据库。相较于LOAM中简单的点特征，文献[16]通过提取结构性强、区分度高的线、面、角等特征，在将点云匹配至先验地图时得到了更精确和鲁棒的定位结果。

然而，当前的激光雷达定位方案仅仅通过点层次的几何特征来筛选用于匹配的点云，一方面易受激光雷达测量噪声的影响；另一方面，在动态物体较多的环境中，也无法区分动态点云和静态点云，从而影响最终的定位精度。本文从聚类的层次考虑，对点云聚类的整体误差进行评估，从而不易受到测量噪声的影响，同时可以将对应于动态物体的点云进行整体性地辨识和移除，从而提高了最终的定位精度。

此外，在车辆实际运行过程中，也会存在很多意外情况导致点云匹配结果出错，例如车辆线/角速度变化过快导致点云匹配初值偏差过大而无法收敛至正确解，传感器视野大面积受遮挡导致用于匹配的有效点过少而无法正确解算位姿变换，传感器数据异常导致的解算异常，环境变化过大导致实时点云和地图点云之间相差过大而无法正确匹配至地图。这些情况下点云匹配常常会收敛至错误解导致无人车的定位结果完全错误，如果无法及时发现并做出应对措施，可能会导致无人车运行至错误路线，甚至导致无人车发生碰撞等较为严重的事故。因此，如何评估点云匹配结果是否正确，对于无人车的稳定运行是必不可少的。

基于以上考虑，本文提出了一种动态环境下基于先验地图的三维激光雷达鲁棒定位方法，主要包含以下工作：

1)提出了一种基于角度和距离双阈值的点云深度图像分割方法，相较于传统仅基于角度或者距离阈值的分割方法，分割结果对点云噪声更为鲁棒；

2)提出了一种基于点云聚类评估的鲁棒点云匹配方法，首先对原始扫描点云进行聚类分割，然后对点云聚类的匹配度进行评估，通过剔除匹配度较差的聚类，保留匹配度较好的聚类进行匹配定位，提高了在动态场景中点云匹配定位结果的精度；

3)提出了一种基于聚类评估的两阶段点云匹配结果评估方法，第一个阶段首先评估每一个聚类的匹配度，第二个阶段基于所有聚类的匹配度判断匹配结果成功或者失败，相较于传统的评估方法，提高了评估结果的准确度；

4)构建了面向动态场景的基于先验地图的三维激光雷达鲁棒点云匹配与容错定位系统，并在公开数据集和实际试验中验证了本文方法的有效性。

1 动态环境下基于点云聚类评估的三维激光雷达鲁棒定位方法

本文提出的动态环境下基于点云聚类评估的三维激光雷达鲁棒定位方法，相较于传统方法仅仅只是提取特征点并进行点云匹配的简单过程，增加了点云聚类分割以及基于分割后聚类匹配度评估的二次匹配过程，使得匹配结果更为准确。方法主要分为点云分割、点云匹配I、聚类评估、点云匹配II、匹配评估、全局位姿初始化六个模块(见图1)。相较于传统方法(如LOAM)只是从原始点云提取特征点后直接匹配至地图点云的方式，本文提出的方法首先将原始扫描点云分割为地面点和若干聚类，经降采样后通过点云匹配I模块将扫描点云匹配至地图，然后通过聚类评估模块对每个点云聚类的匹配度进行评估，通过点云匹配II对地面点和匹配度良好的聚类点云再次进行匹配，得到最终的位姿估计。通过这样的两阶段匹配方法，剔除原始点云中匹配不良的动态点或是噪点，从而可以获得更精确的匹配结果。最后，通过匹配评估模块对点云匹配的结果进行评估，判断匹配结果是否出错，若正确，则继续进行下一帧点云的匹配，若错误，则通过全局位姿初始化模块重新初始化传感器的位姿。相较于传统的激光雷达定位框架无法在定位结果错误后自行修正的现状，本文算法通过计算扫描点云和地图点云之间的匹配度，从而对匹配结果的正确性进行评估，并在匹配错误时通过全局重定位进行修正。

1.1 基于角度和距离双阈值的点云深度图像分割

点云分割模块从每一帧点云中提取出地面点云和分别属于每一个物体的点云，并剔除其他无法被聚为一类的单独的点。

定义FL为激光雷达坐标系，其原点OL位于激光雷达中心，其三轴XL、YL、ZL分别指向激光雷达的前、左、上。记t时刻获取的一帧原始激光雷达点云为Pt={pi},i=1,2,…,n，其中，n为一帧点云的总点数，pi=(xi,yi,zi)T为每一个点在FL下的三维坐标。首先，类似于LeGO-LOAM[5]，将原始的激光雷达点云Pt投影至深度图像It。

对于地面无人车来说，地面是一种稳定的特征，但由于激光雷达扫描点云的稀疏性，深度图像It同一列内的相邻像素点对应到实际地面上的两点相距很远。因此，直接基于欧式距离准则的方法难以稳定地对地面点进行分割，类似于文献[5]和文献[17]，本文基于深度图像It，采用如下的阈值分割方法提取地面点：

对于It中的任意一点It(ui,vi)，其对应的FL系下的三维点坐标pi=(xi,yi,zi)T，并且设与其处于It中同一列的上一个点It(ui,vi-1)对应的三维点坐标pup=(xup,yup,zup)T，与其处于It中同一列的下一个点It(ui,vi+1)对应的三维点坐标pdown=(xdown,ydown,zdown)T，如果满足如下的判断条件，则将其标记为地面点

|zi-hg|<δgh

(1)

(2)

(3)

其中，式(1)保证了只有在一定高度范围内的点才会被标记为地面点；hg代表激光雷达相对于地面的高度；δgh、δgα代表了地面的平坦度；式(2)和式(3)保证了只有高度的梯度变化在一定范围内的点才会被标记为地面点，设提取的地面点集为Gt。

图2所示为一室外复杂场景中各方法的地面点提取效果对比，图2(a)为激光雷达原始扫描点云，其中蓝色框内为地面凸起的路旁人行道，黄色框内为较低矮的灌木丛，红色圈内为路上的行人和车辆；图2(b)为本文方法的地面点提取结果；图2(c)、图2(d)分别为文献[5]和基于随机抽样一致(Random Sample Consensus, RANSAC)平面拟合的地面点提取结果。可以看到，图2(c)中文献[5]采用方法提取的地面点中包含了较多的灌木丛返回的点云，一部分行人、车辆返回的点云也被包含在了地面点中；图2(d)中基于RANSAC平面拟合方法提取的地面点虽然剔除了行人、车辆和灌木丛的点云，但其只提取出了地面较为平坦的部分，未提取到蓝色框内的地面凸起部分；而本文方法通过综合考虑地面的平坦度和激光雷达安装高度，在剔除行人、车辆和灌木丛的点云的同时，成功提取了凸起部分的地面点云，取得了更准确的地面点分割效果。

图2 各方法地面点提取效果对比Fig.2 Comparison of ground points extraction effects of the methods

地面点分割后，不同于文献[18]中仅基于角度阈值的分割方法，本文对区域增长的判断准则上同时设定了角度和距离阈值，做了如下改进：

(4)

(5)

其中，式(4)为文献[18]中的判断准则，该准则考虑了扫描点随距离的增大而变稀疏的非均匀特点，通过相邻扫描点间距和扫描距离的比值作为区域增长的判断条件，有效地缓解了由于点云稀疏性而引起的过分割现象。然而，在扫描距离较短时，β值的大小会显著受到激光雷达测距误差的影响，常常会由于测距误差而导致过分割，因此本文加入了式(5)作为另一判断准则，如果两点之间的欧氏距离小于一定阈值，同样也被标记为同一聚类，即只需满足式(4)或式(5)之一，(ui,vi)和(uj,vj)即被标记为同一聚类。图3展现了改进前后分割结果的对比，其中不同的颜色代表不同的聚类，要注意的是这里展示的分割结果已经经过了上述地面点提取的过程，图3(a)和图3(b)分别为同一场景改进前后的分割结果，可以看到在对具有一定噪声的物体(如植被)进行分割后，图3(b)相对于图3(a)获得了更完整和连续的分割结果(图3(a)、图3(b)中蓝色方框内)；图3(c)和图3(d)分别为另一场景改进前后的分割结果，右下角的卡车对应的点云在图3(c)中被分割为了若干个聚类，而在图3(d)中则被分割为了一个整体(图3(c)、图3(d)中蓝色方框内)。

图3 点云分割结果对比Fig.3 Comparison of point cloud segmentation results

图3(a)、图3(b)为同一帧原始扫描点云改进前后的分割结果，图3(c)、图3(d)为另一帧原始扫描点云改进前后的分割结果，注意这里展示的分割结果不包含地面点云。图3(a)、图3(b)中的蓝色方框内为一排相连植被返回的点云，尽管相较于墙面等的平坦表面具有更大的起伏，但仍属于一个物体，可以看到图3(a)中改进前算法将该部分点云分割为了许多细小的点云聚类，但图3(b)中改进后算法则成功地将其分割为了一个完整的点云聚类。图3(c)、图3(d)中的蓝色方框内为一辆卡车返回的点云，图3(c)中改进前算法将同属于一辆卡车的点云分割为了若干的点云聚类，而图3(d)中改进后算法则分割为了一个完整的点云聚类。

1.2 基于残差卡方检验的点云聚类匹配度评估方法

由于地图变化或环境中存在动态物体的情况，这些变化或动态的物体所返回的点云会对匹配结果产生不良影响，最终影响位姿估计的精度。因此，本文在初次点云匹配的结果下，对每一聚类的匹配良好度进行评估，剔除匹配不良的聚类，之后在点云匹配II模块中对剩余匹配良好的聚类点云进行二次匹配，进一步优化位姿估计。

上文提到，原始点云Pt经过点云分割模块处理后，被分割为地面点集以及若干个聚类，Pt={Gt,Ci,t},i=1,2,…,k，对于任一聚类Ci,t(不包含地面点集)，通过如下的卡方检验步骤评估其匹配良好度：

(6)

(7)

在本文中，取α=0.05。

1.3 基于点云聚类评估的鲁棒点云匹配方法

相较于传统点云匹配方法直接从原始点云Pt中提取特征点进行匹配，本文方法仅使用地面点以及满足聚类评估模块检验的聚类所包含的点，通过剔除匹配度较差的聚类进行进一步的点云匹配，以优化位姿估计结果，具体步骤如下：

设分割后点云Pt={Gt,{Ci,t}},i=1,2,…,k是地面点以及所有分割后的聚类所包含的点，Gt为地面点云，Ci,t表示第i个聚类的点云，k为聚类个数。

首先，类似于LOAM，从Pt中提取特征点构建点到面的距离残差，通过高斯牛顿迭代得到粗匹配位姿Trough。将Pt通过粗匹配位姿Trough进行变换得到Pt,Tr。

Pt,Tr=Trough·Pt

(8)

1.4 基于聚类评估的两阶段点云匹配结果评估方法

相较于传统仅基于单个点对的距离设定阈值的匹配评估方法，本文提出了一种基于聚类评估的两阶段点云匹配结果评估方法。第一个阶段首先评估每一个聚类的匹配度，如果一个聚类匹配成功，就认为该聚类内的所有点匹配成功，反之则聚类内的所有点匹配失败;第二个阶段基于所有聚类的匹配度判断匹配结果成功或者失败，计算所有成功匹配的聚类包含的点数与总点数的比值，具体过程如下。

经过点云匹配II模块后，通过将满足检验评估的聚类点云匹配至地图得到传感器估计位姿Tfine，然后按照聚类评估模块的步骤，首先对地面点集进行同样的评估，如果评估失败，即地面点集匹配度较差，则直接判定点云匹配结果错误；如果地面点集匹配度良好，即在地面点集通过检验评估的前提下，再进行如下的评估步骤：

(9)

如果δ低于设定阈值δ0，则判定点云匹配结果错误。本文设置δ0为0.5。

在判定点云匹配结果错误后，尝试重新初始化传感器的位姿，本文采用一种基于激光扫描描述子的方法[19]对传感器的位姿进行全局初始化，通过在构建地图时存储下各位置处的扫描描述子构成描述子库，再在地图的描述子库中搜寻与当前扫描描述子最相似的描述子对应的位置，即为全局初始化的结果，关于描述子的构建方法可以参考文献[19]，在此不做赘述。

2 验证与分析

为验证本文所提定位框架相对于传统匹配方法在定位精度上的提升，以及匹配评估方法相对于传统评估方法的优势，在公开数据集和实际试验中都进行了验证。算法运行的配置如下，选择Intel CORE i9 8950处理器和8GB的内存，算法基于C++实现，运行环境为Linux Ubuntu下的机器人操作系统ROS。

本文用于对比的方法为LOAM和Lio-sam[20]，不同的是去除了LOAM和Lio-sam中的地图更新部分，改为和已知先验地图匹配；用于对比的传统点云匹配评估方法为基于点云匹配残差的评估方法，以及另一种基于点云匹配成功率的方法，它们的评估指标分别计算如下：

设P、Q为两帧待匹配点云，经点云匹配将P匹配至Q后，获得了P到Q的位姿估计T，设P中总点数为n，配对成功的m对点对为{pi,qi},i=1,2,…,m。其中pi与qi已形成对应关系，满足|T*pi-qi|

(10)

若ε值小于设定的阈值εthre，ε<εthre，则认为匹配成功，否则匹配失败。基于点云匹配成功率的方法计算P中成功匹配的点数占P中总点数的比例

(11)

若匹配成功的点数的比例η大于设定的阈值ηthre，η>ηthre，则认为匹配成功，否则匹配失败。

2.1 数据集验证与分析

本文采用文献[21]中公开的动态环境数据集，该数据集包含了室内大厅、火车站、步行区等各种存在大量行人的环境，通过手持Ouster OS1-64激光雷达以10Hz的频率采集点云序列，选取了其中在步行区采集的数据验证本文算法。图4中展示了其中的一帧扫描点云的强度图像，用红色标注的点为场景中的行人返回的点云。

图4 数据集Ouster激光雷达扫描点云强度图像Fig.4 Intensity image by Ouster lidar scan in dataset

首先通过因子图[20]融合点云数据与参考真值构建场景的先验地图(图5中白色点云)，然后将激光雷达实时点云匹配至先验地图获取传感器位姿。图5中蓝色点云为激光雷达原始扫描点云，红色点云为通过本文算法提取的匹配度良好的点云，可以看到，本文算法提取的红色点云中已经不包含行人返回的点云(图5中黄色方框内的蓝色点云)，成功剔除了场景中动态行人的点云，因此定位结果相较于LOAM和Lio-sam更为平稳和准确。

图5 数据集点云地图(白色)和实时点云(红色和蓝色)Fig.5 Point cloud map (white) and real-time point cloud (red and blue) in dataset

图6展示了LOAM、Lio-sam和本文所提方法的定位结果的误差对比，表1列出了均方根误差(Root Mean Square Error，RMSE)的对比结果，可以看到，相较于LOAM，本文算法的定位误差减小了近50%，相较于Lio-sam，本文算法的定位误差减小了27.5%。

(a) X轴误差

表1 定位均方根误差(RMSE)对比

2.2 试验验证与分析

2.2.1 匹配定位试验

本文选择的试验场地为南航校园主干道，无人车搭载有Velodyne VLP-16激光雷达传感器和实时差分GPS(RTK)，RTK用作定位基准，以及辅助构建全局无漂的先验地图(见图7)。通过因子图[21]融合激光雷达和RTK构建先验地图(图8(b))。无人车的行进路线在图8(a)中用蓝线表示。

(a) 无人车定位验证平台

(a) 试验场景鸟瞰图

图9展示了本文算法对点云的筛选作用，图9(a)中的白色点为地图点云，彩色点为聚类后的点云(除地面点)，每种颜色代表一个聚类；图9(b)中的蓝色点云为图9(a)中所有聚类构成的点云，红色点云为经过聚类评估模块提取的点云，可以看到本文算法成功地剔除了扫描点云中和地图不匹配的部分。

(a) 点云分割结果

图10展示了LOAM、Lio-sam和本文所提改进匹配方法的定位结果的误差对比，表2列出了均方根误差(RMSE)的对比结果，可以看到，在剔除了匹配度较差的聚类点云后，相较于LOAM，本文算法的定位误差减小了近70%，相较于Lio-sam，本文算法的定位误差减小了48%。

(a) 东向误差

表2 试验场景下定位均方根误差(RMSE)对比

2.2.2 算法运行时间分析

图11所示为本文算法各主要模块运行时间，表3列出了各模块运行的平均时间，可以看到，加入的点云分割模块平均耗时为6.59ms，聚类评估模块平均耗时为4.54ms，点云匹配I和II的总时间为62.58ms，算法总体平均运行时间为73.71ms，激光雷达扫描频率为10Hz，因此本文算法可以实时处理每帧点云数据。

(a)

表3 各模块运行平均时间

2.2.3 匹配评估试验

本文选取了8个典型场景对比本文匹配评估方法和上述两种传统方法，如图12所示，1～4号为匹配成功的情况，5～8号为匹配失败的情况，表4列出了对应的评估指标的计算值。在诸如1号的一般情形下，三种方法均能正确进行评估；然而在2、3、4中，当传感器附近经过大型车辆，视野被大量遮挡或环境点云较为嘈杂时，基于残差或匹配率的方法出现了较大波动，而本文算法则受影响较小。5～8中模拟了点云匹配失败的情形，在5中当点云和环境差别较大时，三种方法均能正确评估出匹配失败；然而在6、7、8中，通过将点云从正确位置平移数米模拟匹配失败的情形中(实际运行过程中通过卫星定位或是其他全局定位算法可能会出现的情况)，误匹配的点云和环境相似度较高，此时6、7中点云匹配的残差甚至低于在3、4中的残差值，7、8中的点云匹配率高于2、4中的匹配率，从而无法正确评估出匹配结果是否成功，而本文算法在匹配失败时的匹配率明显小于匹配成功时的匹配率，即使是点云和环境的相似度较高时，也能正确做出评估。图12中5(b)～8(b)的黄色点云为传统仅基于距离阈值筛选所提取的成功匹配的点云，红色点云为本文算法经过聚类评估后提取的成功匹配的点云。可以看到，如果仅仅基于距离阈值进行筛选，哪怕是在匹配错误的情况下，依旧有很多点会被认为是正确配对的点云，从而可能出现最终将匹配错误的情况误判断为正确；但在经过聚类评估后，尽管单独的某些点在匹配错误时可能仍能找到距离较近的点作为对应点，但聚类中所有点的总体误差在匹配错误时通常会显著变大，从而能够剔除多数误匹配的点云，最终可以更为准确地对匹配结果进行评估。

图12 点云匹配成功(1～4)和失败(5～8)情况Fig.12 Matching success (1～4) and failure (5～8) situations

表4 图12中各情况对应评估指标的计算值

3 结论

本文针对动态环境下的激光雷达定位问题，提出了一种基于点云聚类评估的三维激光雷达鲁棒定位方法。主要工作为以下几点：

1)基于双阈值的区域增长法对点云进行分割聚类，提高了聚类结果对噪声的鲁棒性；

2)通过卡方检验剔除误匹配的动态点云聚类，以提高匹配精度；

3)通过聚类的匹配成功比例，评估匹配结果的正确性。

经过公开数据集与实际试验验证，结果表明，相较于传统匹配方法，本文方法有效提高了在行人、车辆等移动物体较多的动态场景下的定位精度和匹配结果评估的准确度。在后续工作中，将会针对激光雷达全局重定位方法开展研究，以提高无人车全局位姿初始化的速度和准确率。