三维点云极化地图表征模型与智能车定位方法

胡钊政，李 飞,2，陶倩文，陈佳良

(1.武汉理工大学 智能交通系统研究中心，武汉 430063；2.武汉理工大学 重庆研究院，重庆 401120)

随着人工智能、大数据、云计算、物联网等先进科学技术的应用和通信交互能力的增强，智能车技术已日趋成熟[1]。感知与定位技术在智能车的发展过程中占据重要地位，而由于LiDAR具有高频、远程、高精度的数据采集性能，使得其在智能车定位中被广泛运用。目前基于激光的主流定位方法为SLAM(simultaneous localization and mapping)。SLAM主要分为里程计和优化两部分。里程计根据帧间点云匹配结果计算车辆的运动。ICP(iterative closest point)[2]是经典的点云帧间匹配算法，通过逐点查找对应关系，ICP不断尝试对齐两组点云，直到满足条件为止，当点云数量过多时，ICP算法会有较高的计算复杂度。基于特征点云的帧间匹配算法通过在环境中寻找具有代表性的特征而只需要较少的计算资源，备受关注。Rusu等[3]提出的PFH(point feature histogram)特征通过参数化查询点和邻域点的空间差异并形成一个多维直方图来描述查询点k邻域的几何属性，该类特征提取方法简单且具有旋转不变性，同时对采样密度和噪声点具有较强的鲁棒性。虽然通过帧间特征匹配可快速获取车辆位姿，但随着车辆行驶距离增长及帧间匹配次数增多，累计误差逐渐增大。LOAM[4-5](lidar odometry and mapping in real-time)算法通过三维重建构图校正定位轨迹，但其地图为在线生成且在制图前需对点云进行下采样、滤波等预处理，导致算法的空间复杂度和时间复杂度较高；并且随着定位距离的增长，LOAM算法仍会存在较大的累计误差。

本文提出了一种三维点云极化地图表征模型，该模型以极化图为节点，通过融合全局位置信息、点云的二维和三维特征并结合多尺度定位方法实现智能车定位。实验在两种典型场景下进行，结果表明，本文算法在节点级和度量级定位上均具有高精度，满足智能车的定位需求，且鲁棒性强、成本低、计算过程简单。本文的创新点为：

1)离线制图和在线定位均以极化图为节点，极化图构造方法简单且保留了点云的原始数据，同时将点云的二维和三维信息融合。

2)利用极化图提取点云的二维和三维特征，实现制图和定位节点的多尺度特征表征。

3)提出了一种基于点云极化地图表征模型的多尺度定位方法，该方法采用多尺度渐进定位策略，通过基于GPS/拓扑结构的初定位、基于点云二维特征的节点级定位和基于点云三维特征的度量级定位，获取智能车位姿。

1 本文算法

1.1 系统概述

图1为系统流程图，输入为LiDAR和普通GPS采集的激光点云及GPS数据，输出为待定位车辆位姿。系统分为离线制图、初定位、节点级定位和度量级定位4个模块：

1)离线制图模块利用高精度惯导数据和点云数据制作极化地图，主要包含全局位置信息、点云的二维和三维特征。

2)初定位模块在GPS信号良好时，将待定位节点与地图节点进行GPS匹配并根据GPS精度筛选地图节点；若GPS信号缺失，则利用拓扑结构进行初定位并筛选地图节点。

3)节点级定位模块通过极化表征生成点云极化图并利用加权特征匹配算法与初定位结果进行SURF(speeded up robust features)[6]和ORB(oriented FAST and rotated BRIEF)[7]全局特征匹配，结合WH-KNN(weighted hybridk-nearest neighbor)[8]算法挑选最近地图节点。

4)度量级定位利用极化图提取点云角特征点和面特征点并与最近地图节点进行特征点云匹配，结合L-M(Levenberg-Marquardt)[9]算法计算待定位车辆位姿。

1.2 基于三维点云极化地图表征模型的地图构建

三维点云极化地图表征模型以极化图为节点，融合了全局位置信息、点云的二维和三维特征，地图构建示意图见图2。利用数据采集车辆搭载LiDAR和组合惯导并结合RTK(real-time kinematic)技术在待定位路段采集激光点云和高精度惯导数据。在数据采集之前，需通过LiDAR对组合惯导定位接收器进行标定以获取其在LiDAR坐标系中的位置，通过标定获取的位置参数用于在线定位中的坐标修正。设PI={PI1,PI2,…,PIi,…}为LiDAR扫描在惯导定位接收器上的点云集合，计算PI的质心坐标PINS并以此作为组合惯导定位接收器在LiDAR坐标系中的位置，计算公式为

图2 极化地图Fig.2 Polarization map

(1)

式中nI为PI中点云数量。标定完成后，以等距的方式在待定位路段进行数据采集，采集频率1 m/次，每次数据采集包含一帧点云数据和一条高精度惯导数据。值得提出的是，组合惯导结合RTK仅用于构建极化地图，在线定位时采用普通GPS接收机即可。数据采集完成后，离线处理每个数据节点，处理步骤为：1)全局位置表征；2)二维特征表征；3)三维特征表征。具体实施如下。

从高精度惯导数据中提取全局位置信息。全局位置信息包含GPS信息和欧拉角，其中欧拉角由翻滚角(roll)、俯仰角(pitch)和航向角(yaw)组成。

点云二维特征表征。首先通过极化表征将激光点云数据转化成极化图，本文所选用VLP-16 LiDAR的激光扫描线数、水平视场和水平角分辨率分别为16、360°、0.2°，故极化图的分辨率为1 800×16，根据每个点云所在激光扫描线位置及水平视场角度确认其像素坐标并以其与LiDAR的欧式距离作为灰度值进行投影，生成极化图。其次对极化图进行切割及预处理并提取预处理图像子块的SURF和ORB全局特征，具体而言：1)将极化图切割成N个图像子块，本文N为30，图像子块分辨率为60×16；2)对图像子块进行直方图均衡化和尺寸归一化(63×63)处理；3)将处理后的图像子块中心作为特征点，整个图像子块作为特征点的邻域；4)计算特征描述符，并将此描述符作为图像子块的全局特征描述符。最终得到的SURF全局特征描述符是一个64维的向量，ORB全局特征是一个256位的二进制字符串，见图3。

图3 SURF(上方)和ORB(下方)全局特征Fig.3 SURF (above)and ORB (below)holistic features

点云三维特征表征。首先基于极化图像的列式评估方法[10]完成地面点云提取并将其作为一类特殊的聚类点簇，地面点云对后续的面特征点提取具有重要意义。其次基于图像的点云聚类方法[11]对极化图上非地面点云进行聚类，需要注意的是，小物体(例如树叶)点云会在后续的特征点云提取中形成琐碎且不可靠的特征点云，而车辆在两次不同的激光数据采集中，很难捕捉到同一个小物体，为了提取到可靠的特征点云，通过设置聚类点云数量阈值kp筛除小物体聚类点簇，本文kp=30。聚类结果如图4(b)所示，图4(a)为原始点云。最后提取点云三维特征，即面特征点云和角特征点云，特征提取方式与LOAM[5]类似，不同的是，本文在聚类点云中完成特征提取，具体而言：1)计算点云Pi的曲率ci，计算公式为

图4 点云聚类Fig.4 Point cloud clustering

(2)

式中：S为与Pi在极化图上呈行连续的点集且均匀分布在Pi两侧，ri为Pi在极化图中对应点的灰度值，rj为S中第j个点云在极化图中对应点灰度值；2)点云分类，通过设置曲率阈值cth将点云划分为面特征点(cicth)；3)特征筛选，为了让特征点云均匀分布，将极化图沿水平方向划分为数个相等大小的子图像，从每行点云中分别挑选Ne个具有最大曲率ci的角特征点和Np个具有最小曲率ci的面特征点，本文将极化图分为6个子图像，Ne=20，Np=40。

1.3 基于极化地图的多尺度定位

本文在线定位采用多尺度渐进定位策略，通过基于GPS/拓扑结构的初定位，基于点云二维特征的节点级定位和基于点云三维特征的度量级定位，获取智能车位姿，具体实施如下。

基于GPS/拓扑结构的初定位。定位之前需利用LiDAR对GPS接收机进行标定以获取其在LiDAR坐标系中的位置PGPS，方法与制图所用相同。

标定完成后，根据以下两种情况进行初定位：

1)GPS信号良好时，匹配待定位节点与地图节点的GPS，完成初定位。设待定位节点L的粗GPS为gl=(al,bl)，地图节点序列Mi的高精度GPS为Gm={gm1,gm2,…,gmi,…}={(am1,bm1),(am2,bm2),…,(ami,bmi),…}，计算待定位节点与地图节点的欧氏距离并通过设置距离阈值kg对地图节点进行筛选，计算公式为：

(3)

Dl={Umi|dist(Ul,Umi)≤kg}

(4)

式中：Ul=(Xl,Yl)为待定位节点的粗GPS数据经转化后的欧氏坐标，Umi=(Xmi,Ymi)为第i个地图节点的高精度GPS数据经转化后的欧氏坐标，trans()为GPS坐标转欧氏坐标的公式，dist()为欧氏距离计算公式，Dl为符合要求的地图节点集合。由于普通GPS的定位精度为10 m，故kg=10。

2)GPS信号缺失时，利用拓扑结构完成初定位，示意图见图5。在很短的时间内，智能车被认为匀速运动，可通过前一位置来预测智能车当前位置范围。智能车在ti-1时刻的位置为Ui-1，di-1为智能车从ti-2到ti-1时刻的移动距离，由于之前位置已知，故智能车在ti时刻的位置可用下式推导

图5 拓扑初定位Fig.5 Coarse localization based on topology

(5)

由于拓扑定位存在误差dε，则智能车的位置范围为Ui±dε，以此范围筛选地图节点。dε根据实际设置，本文dε=10 m。

基于点云二维特征的节点级定位。首先基于极化表征将待定位节点激光点云数据转化成极化图并进行切割及预处理，提取预处理图像子块的SURF和ORB全局特征，方法与制图所用相同。其次匹配待定位节点和初定位结果的SURF和ORB全局特征，为了更准确表达不同极化图之间的相似度，本文提出了加权特征匹配算法，具体而言：1)待定位节点与初定位结果分别进行SURF和ORB全局特征匹配；2)计算SURF和ORB特征距离加权均值，距离值越小，说明不同极化图之间的相似度越高，具体计算公式为

(6)

式中：nf为相匹配的特征点对数目，dfi为第i个相匹配特征点对之间的距离，df为不同极化图之间的特征距离。加权特征匹配算法通过对图像进行区域划分，将区域图像作为特征并进行匹配，利用匹配特征间的距离加权均值作为不同图像的特征距离，更加准确地表达了不同图像的相似度。最后通过WH-KNN算法挑选最近的地图节点，WH-KNN算法通过引入权值解决识别模糊性问题，结合加权特征匹配算法，极大提高了最近节点检测的正确率。

基于点云三维特征的度量级定位。首先通过待定位节点极化图完成点云聚类、特征点云提取，方法与制图所用相同，需要注意的是，待定位节点只需从子图像每行点云中挑选较少数量的角特征点和面特征点即可，本文挑选数量分别为2和4。其次匹配待定位节点和最近地图节点的角特征点和面特征点，匹配方式分别为点到线和点到面的特征匹配方法[5]。再次利用标定获取的GPS接收机位置参数PGPS和组合惯导定位接收器位置参数PINS对匹配点云对进行坐标修正，地图数据采集时，以组合惯导定位接收器代替车辆的位置，而在线定位时，以GPS接收机代替车辆的位置，所以应将点云坐标原点分别移至两者处，计算公式为

(7)

式中：Pbefore={xbefore,ybefore,zbefore}为修正前的点云坐标，Pafter={xafter,yafter,zafter}为修正后的点云坐标，Pc={xc,yc,zc}为GPS接收机或组合惯导定位接收器位置参数。最后通过修正后的匹配点云对坐标及L-M算法计算待定位节点与最近地图节点的旋转向量R和平移向量T，结合最近地图节点的全局位置信息，即可获取待定位车辆的位置和姿态，完成定位。

2 实验结果

实验在两种典型场景下进行：1)某高校园区闭环路线，见图6(a)，该路线长为600 m，覆盖面积为15 000 m2；2)某工业园区闭环路线，见图6(b)，该路线长为891 m，覆盖面积为20 900 m2。极化地图构建采用Velodyne VLP-16 LiDAR与组合惯导结合RTK技术来完成，在线定位采用Velodyne VLP-16 LiDAR与普通GPS接收机来完成。VLP-16 LiDAR的最大测量范围为100 m，精度为3 cm，16条激光扫描线，垂直视场为30°(±15°)，垂直角分辨为2°，水平视场为360°，水平角分辨率可在0.1°～0.4°中进行选择，本文设置的扫描速率为10 Hz，水平角分辨率为0.2°；组合惯导的工作速率为10 Hz，结合RTK技术后的定位精度为1 cm；普通GPS接收机的定位精度为10 m；本文系统的运算平台为具有2.8 GHz i7-7700HQ CPU的笔记本电脑。场景1采用比亚迪E5电动车作为测试车，实验平台见图6(c)；场景2采用智能物流车作为测试车，实验平台见图6(d)。

图6 实验场景与平台Fig.6 Experimental scenarios and platforms

2.1 高校园区实验

首先在闭环路线上采集数据构建极化地图，数据采集频率1 m/次，每次采集数据包括一帧高精度惯导数据和一帧点云数据。极化地图构建完成后，进行在线定位实验，该闭环路线上共786个待定位节点，地图采集和在线定位时间间隔超过24 h。本文选取Tao等[12]提出的算法作为节点级定位对比算法，该算法首先将待定位节点和地图节点的点云俯视投影图进行ORB特征匹配并利用RANSAC(random sample consensus)算法去除错误匹配点对，其次根据匹配点对数量选取最近地图节点。若通过节点定位获取的地图节点与待定位节点的GPS真值(ground truth)距离最短，则认为节点定位正确。同时，本文选取经典的SLAM算法LOAM[5]作为度量级定位对比算法。本文算法在该闭环路线上的定位轨迹见图7(a)，定位误差见图7(b)，对比实验结果见表1。

表1 高校园区对比实验Tab.1 Comparative experiment on campus

图7 高校园区定位结果Fig.7 Localization results of campus

在该闭环路线上，本文算法正确定位最近地图节点个数为773，节点定位准确率为98.3%，度量级定位误差为33.2 cm，最大定位误差为42.9 cm，满足智能车高精度定位需求。与Tao等提出的算法和LOAM算法相比，本文算法节点级定位准确率提高了51.2%，度量级定位误差降低了43.1 cm。

2.2 工业园区实验

与高校园区的定位实验相似，首先在该闭环路线上采集数据构建极化地图，其次进行在线定位，该闭环路线上共5 205个待定位节点，构图与定位的时间间隔超过24 h，分别选取Tao等提出的算法和LOAM算法作为节点级定位和度量级定位的对比算法。本文算法在该闭环路线上的定位轨迹和定位误差分别如图8(a)和8(b)所示，对比实验结果见表2。

表2 工业园区对比实验Tab.2 Comparative experiment on factory

图8 工业园区定位结果Fig.8 Localization results of factory

在该闭环路线上，本文算法正确定位最近地图节点个数为5 138，节点定位准确率为98.7%，度量级定位误差为19.6 cm，最大定位误差为38.4 cm，满足智能车高精度定位需求。与Tao等提出的算法和LOAM算法相比，本文算法节点级定位准确率提高了52.4%，度量级定位误差降低了77.8 cm。

3 结 论

本文提出一种基于三维点云极化地图表征模型的智能车定位算法，以点云极化图为节点，采用多尺度渐进定位策略，首先利用GPS/拓扑结构完成初定位，其次利用点云二维特征完成节点级定位，最后利用点云三维特征完成度量级定位，通过两种典型场景的实验验证，结论如下：

1)通过极化地图和点云二维特征定位最近地图节点，整体节点定位准确率，即两种实验场景的平均节点定位准确率，为98.7%，证明该方法可有效定位最近地图节点。

2)通过基于三维点云极化地图表征模型的多尺度定位算法实现智能车定位，整体定位误差，即两种实验场景的平均定位误差，为21.4 cm，最大定位误差为42.9 cm，满足智能车高精度定位需求。

3)实验中选取了不同的场景和实验车进行测试，均实现了高精度定位，证明本文算法在不同的场景和实验车中具有良好的鲁棒性。同时，本文算法在线定位时，只需要LiDAR和普通GPS接收机，降低了定位成本。

本文算法仍存在以下不足之处：1)在相似且特征单一的场景中，定位误差较大，如对称的车道等；2)平均定位误差只达到了分米级，尚未达到厘米级。利用定位过程中的误差信息作为反馈对制图过程进行干预，即利用众包技术提高地图表征能力，降低定位误差，将是本文下一阶段的重点研究计划。