胡釗政，李 飛,2，陶倩文，陳佳良

(1.武漢理工大學(xué) 智能交通系統(tǒng)研究中心，武漢 430063；2.武漢理工大學(xué) 重慶研究院，重慶 401120)

隨著人工智能、大數(shù)據(jù)、云計算、物聯(lián)網(wǎng)等先進科學(xué)技術(shù)的應(yīng)用和通信交互能力的增強，智能車技術(shù)已日趨成熟[1]。感知與定位技術(shù)在智能車的發(fā)展過程中占據(jù)重要地位，而由于LiDAR具有高頻、遠程、高精度的數(shù)據(jù)采集性能，使得其在智能車定位中被廣泛運用。目前基于激光的主流定位方法為SLAM(simultaneous localization and mapping)。SLAM主要分為里程計和優(yōu)化兩部分。里程計根據(jù)幀間點云匹配結(jié)果計算車輛的運動。ICP(iterative closest point)[2]是經(jīng)典的點云幀間匹配算法，通過逐點查找對應(yīng)關(guān)系，ICP不斷嘗試對齊兩組點云，直到滿足條件為止，當點云數(shù)量過多時，ICP算法會有較高的計算復(fù)雜度?；谔卣鼽c云的幀間匹配算法通過在環(huán)境中尋找具有代表性的特征而只需要較少的計算資源，備受關(guān)注。Rusu等[3]提出的PFH(point feature histogram)特征通過參數(shù)化查詢點和鄰域點的空間差異并形成一個多維直方圖來描述查詢點k鄰域的幾何屬性，該類特征提取方法簡單且具有旋轉(zhuǎn)不變性，同時對采樣密度和噪聲點具有較強的魯棒性。雖然通過幀間特征匹配可快速獲取車輛位姿，但隨著車輛行駛距離增長及幀間匹配次數(shù)增多，累計誤差逐漸增大。LOAM[4-5](lidar odometry and mapping in real-time)算法通過三維重建構(gòu)圖校正定位軌跡，但其地圖為在線生成且在制圖前需對點云進行下采樣、濾波等預(yù)處理，導(dǎo)致算法的空間復(fù)雜度和時間復(fù)雜度較高；并且隨著定位距離的增長，LOAM算法仍會存在較大的累計誤差。

本文提出了一種三維點云極化地圖表征模型，該模型以極化圖為節(jié)點，通過融合全局位置信息、點云的二維和三維特征并結(jié)合多尺度定位方法實現(xiàn)智能車定位。實驗在兩種典型場景下進行，結(jié)果表明，本文算法在節(jié)點級和度量級定位上均具有高精度，滿足智能車的定位需求，且魯棒性強、成本低、計算過程簡單。本文的創(chuàng)新點為：

1)離線制圖和在線定位均以極化圖為節(jié)點，極化圖構(gòu)造方法簡單且保留了點云的原始數(shù)據(jù)，同時將點云的二維和三維信息融合。

2)利用極化圖提取點云的二維和三維特征，實現(xiàn)制圖和定位節(jié)點的多尺度特征表征。

3)提出了一種基于點云極化地圖表征模型的多尺度定位方法，該方法采用多尺度漸進定位策略，通過基于GPS/拓撲結(jié)構(gòu)的初定位、基于點云二維特征的節(jié)點級定位和基于點云三維特征的度量級定位，獲取智能車位姿。

1 本文算法

1.1 系統(tǒng)概述

圖1為系統(tǒng)流程圖，輸入為LiDAR和普通GPS采集的激光點云及GPS數(shù)據(jù)，輸出為待定位車輛位姿。系統(tǒng)分為離線制圖、初定位、節(jié)點級定位和度量級定位4個模塊：

1)離線制圖模塊利用高精度慣導(dǎo)數(shù)據(jù)和點云數(shù)據(jù)制作極化地圖，主要包含全局位置信息、點云的二維和三維特征。

2)初定位模塊在GPS信號良好時，將待定位節(jié)點與地圖節(jié)點進行GPS匹配并根據(jù)GPS精度篩選地圖節(jié)點；若GPS信號缺失，則利用拓撲結(jié)構(gòu)進行初定位并篩選地圖節(jié)點。

3)節(jié)點級定位模塊通過極化表征生成點云極化圖并利用加權(quán)特征匹配算法與初定位結(jié)果進行SURF(speeded up robust features)[6]和ORB(oriented FAST and rotated BRIEF)[7]全局特征匹配，結(jié)合WH-KNN(weighted hybridk-nearest neighbor)[8]算法挑選最近地圖節(jié)點。

4)度量級定位利用極化圖提取點云角特征點和面特征點并與最近地圖節(jié)點進行特征點云匹配，結(jié)合L-M(Levenberg-Marquardt)[9]算法計算待定位車輛位姿。

1.2 基于三維點云極化地圖表征模型的地圖構(gòu)建

三維點云極化地圖表征模型以極化圖為節(jié)點，融合了全局位置信息、點云的二維和三維特征，地圖構(gòu)建示意圖見圖2。利用數(shù)據(jù)采集車輛搭載LiDAR和組合慣導(dǎo)并結(jié)合RTK(real-time kinematic)技術(shù)在待定位路段采集激光點云和高精度慣導(dǎo)數(shù)據(jù)。在數(shù)據(jù)采集之前，需通過LiDAR對組合慣導(dǎo)定位接收器進行標定以獲取其在LiDAR坐標系中的位置，通過標定獲取的位置參數(shù)用于在線定位中的坐標修正。設(shè)PI={PI1,PI2,…,PIi,…}為LiDAR掃描在慣導(dǎo)定位接收器上的點云集合，計算PI的質(zhì)心坐標PINS并以此作為組合慣導(dǎo)定位接收器在LiDAR坐標系中的位置，計算公式為

圖2 極化地圖Fig.2 Polarization map

(1)

式中nI為PI中點云數(shù)量。標定完成后，以等距的方式在待定位路段進行數(shù)據(jù)采集，采集頻率1 m/次，每次數(shù)據(jù)采集包含一幀點云數(shù)據(jù)和一條高精度慣導(dǎo)數(shù)據(jù)。值得提出的是，組合慣導(dǎo)結(jié)合RTK僅用于構(gòu)建極化地圖，在線定位時采用普通GPS接收機即可。數(shù)據(jù)采集完成后，離線處理每個數(shù)據(jù)節(jié)點，處理步驟為：1)全局位置表征；2)二維特征表征；3)三維特征表征。具體實施如下。

從高精度慣導(dǎo)數(shù)據(jù)中提取全局位置信息。全局位置信息包含GPS信息和歐拉角，其中歐拉角由翻滾角(roll)、俯仰角(pitch)和航向角(yaw)組成。

點云二維特征表征。首先通過極化表征將激光點云數(shù)據(jù)轉(zhuǎn)化成極化圖，本文所選用VLP-16 LiDAR的激光掃描線數(shù)、水平視場和水平角分辨率分別為16、360°、0.2°，故極化圖的分辨率為1 800×16，根據(jù)每個點云所在激光掃描線位置及水平視場角度確認其像素坐標并以其與LiDAR的歐式距離作為灰度值進行投影，生成極化圖。其次對極化圖進行切割及預(yù)處理并提取預(yù)處理圖像子塊的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]完成地面點云提取并將其作為一類特殊的聚類點簇，地面點云對后續(xù)的面特征點提取具有重要意義。其次基于圖像的點云聚類方法[11]對極化圖上非地面點云進行聚類，需要注意的是，小物體(例如樹葉)點云會在后續(xù)的特征點云提取中形成瑣碎且不可靠的特征點云，而車輛在兩次不同的激光數(shù)據(jù)采集中，很難捕捉到同一個小物體，為了提取到可靠的特征點云，通過設(shè)置聚類點云數(shù)量閾值kp篩除小物體聚類點簇，本文kp=30。聚類結(jié)果如圖4(b)所示，圖4(a)為原始點云。最后提取點云三維特征，即面特征點云和角特征點云，特征提取方式與LOAM[5]類似，不同的是，本文在聚類點云中完成特征提取，具體而言：1)計算點云Pi的曲率ci，計算公式為

圖4 點云聚類Fig.4 Point cloud clustering

(2)

式中：S為與Pi在極化圖上呈行連續(xù)的點集且均勻分布在Pi兩側(cè)，ri為Pi在極化圖中對應(yīng)點的灰度值，rj為S中第j個點云在極化圖中對應(yīng)點灰度值；2)點云分類，通過設(shè)置曲率閾值cth將點云劃分為面特征點(cicth)；3)特征篩選，為了讓特征點云均勻分布，將極化圖沿水平方向劃分為數(shù)個相等大小的子圖像，從每行點云中分別挑選Ne個具有最大曲率ci的角特征點和Np個具有最小曲率ci的面特征點，本文將極化圖分為6個子圖像，Ne=20，Np=40。

1.3 基于極化地圖的多尺度定位

本文在線定位采用多尺度漸進定位策略，通過基于GPS/拓撲結(jié)構(gòu)的初定位，基于點云二維特征的節(jié)點級定位和基于點云三維特征的度量級定位，獲取智能車位姿，具體實施如下。

基于GPS/拓撲結(jié)構(gòu)的初定位。定位之前需利用LiDAR對GPS接收機進行標定以獲取其在LiDAR坐標系中的位置PGPS，方法與制圖所用相同。

標定完成后，根據(jù)以下兩種情況進行初定位：

1)GPS信號良好時，匹配待定位節(jié)點與地圖節(jié)點的GPS，完成初定位。設(shè)待定位節(jié)點L的粗GPS為gl=(al,bl)，地圖節(jié)點序列Mi的高精度GPS為Gm={gm1,gm2,…,gmi,…}={(am1,bm1),(am2,bm2),…,(ami,bmi),…}，計算待定位節(jié)點與地圖節(jié)點的歐氏距離并通過設(shè)置距離閾值kg對地圖節(jié)點進行篩選，計算公式為：

(3)

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

(4)

式中：Ul=(Xl,Yl)為待定位節(jié)點的粗GPS數(shù)據(jù)經(jīng)轉(zhuǎn)化后的歐氏坐標，Umi=(Xmi,Ymi)為第i個地圖節(jié)點的高精度GPS數(shù)據(jù)經(jīng)轉(zhuǎn)化后的歐氏坐標，trans()為GPS坐標轉(zhuǎn)歐氏坐標的公式，dist()為歐氏距離計算公式，Dl為符合要求的地圖節(jié)點集合。由于普通GPS的定位精度為10 m，故kg=10。

2)GPS信號缺失時，利用拓撲結(jié)構(gòu)完成初定位，示意圖見圖5。在很短的時間內(nèi)，智能車被認為勻速運動，可通過前一位置來預(yù)測智能車當前位置范圍。智能車在ti-1時刻的位置為Ui-1，di-1為智能車從ti-2到ti-1時刻的移動距離，由于之前位置已知，故智能車在ti時刻的位置可用下式推導(dǎo)

圖5 拓撲初定位Fig.5 Coarse localization based on topology

(5)

由于拓撲定位存在誤差dε，則智能車的位置范圍為Ui±dε，以此范圍篩選地圖節(jié)點。dε根據(jù)實際設(shè)置，本文dε=10 m。

基于點云二維特征的節(jié)點級定位。首先基于極化表征將待定位節(jié)點激光點云數(shù)據(jù)轉(zhuǎn)化成極化圖并進行切割及預(yù)處理，提取預(yù)處理圖像子塊的SURF和ORB全局特征，方法與制圖所用相同。其次匹配待定位節(jié)點和初定位結(jié)果的SURF和ORB全局特征，為了更準確表達不同極化圖之間的相似度，本文提出了加權(quán)特征匹配算法，具體而言：1)待定位節(jié)點與初定位結(jié)果分別進行SURF和ORB全局特征匹配；2)計算SURF和ORB特征距離加權(quán)均值，距離值越小，說明不同極化圖之間的相似度越高，具體計算公式為

(6)

式中：nf為相匹配的特征點對數(shù)目，dfi為第i個相匹配特征點對之間的距離，df為不同極化圖之間的特征距離。加權(quán)特征匹配算法通過對圖像進行區(qū)域劃分，將區(qū)域圖像作為特征并進行匹配，利用匹配特征間的距離加權(quán)均值作為不同圖像的特征距離，更加準確地表達了不同圖像的相似度。最后通過WH-KNN算法挑選最近的地圖節(jié)點，WH-KNN算法通過引入權(quán)值解決識別模糊性問題，結(jié)合加權(quán)特征匹配算法，極大提高了最近節(jié)點檢測的正確率。

基于點云三維特征的度量級定位。首先通過待定位節(jié)點極化圖完成點云聚類、特征點云提取，方法與制圖所用相同，需要注意的是，待定位節(jié)點只需從子圖像每行點云中挑選較少數(shù)量的角特征點和面特征點即可，本文挑選數(shù)量分別為2和4。其次匹配待定位節(jié)點和最近地圖節(jié)點的角特征點和面特征點，匹配方式分別為點到線和點到面的特征匹配方法[5]。再次利用標定獲取的GPS接收機位置參數(shù)PGPS和組合慣導(dǎo)定位接收器位置參數(shù)PINS對匹配點云對進行坐標修正，地圖數(shù)據(jù)采集時，以組合慣導(dǎo)定位接收器代替車輛的位置，而在線定位時，以GPS接收機代替車輛的位置，所以應(yīng)將點云坐標原點分別移至兩者處，計算公式為

(7)

式中：Pbefore={xbefore,ybefore,zbefore}為修正前的點云坐標，Pafter={xafter,yafter,zafter}為修正后的點云坐標，Pc={xc,yc,zc}為GPS接收機或組合慣導(dǎo)定位接收器位置參數(shù)。最后通過修正后的匹配點云對坐標及L-M算法計算待定位節(jié)點與最近地圖節(jié)點的旋轉(zhuǎn)向量R和平移向量T，結(jié)合最近地圖節(jié)點的全局位置信息，即可獲取待定位車輛的位置和姿態(tài)，完成定位。

2 實驗結(jié)果

實驗在兩種典型場景下進行：1)某高校園區(qū)閉環(huán)路線，見圖6(a)，該路線長為600 m，覆蓋面積為15 000 m2；2)某工業(yè)園區(qū)閉環(huán)路線，見圖6(b)，該路線長為891 m，覆蓋面積為20 900 m2。極化地圖構(gòu)建采用Velodyne VLP-16 LiDAR與組合慣導(dǎo)結(jié)合RTK技術(shù)來完成，在線定位采用Velodyne VLP-16 LiDAR與普通GPS接收機來完成。VLP-16 LiDAR的最大測量范圍為100 m，精度為3 cm，16條激光掃描線，垂直視場為30°(±15°)，垂直角分辨為2°，水平視場為360°，水平角分辨率可在0.1°～0.4°中進行選擇，本文設(shè)置的掃描速率為10 Hz，水平角分辨率為0.2°；組合慣導(dǎo)的工作速率為10 Hz，結(jié)合RTK技術(shù)后的定位精度為1 cm；普通GPS接收機的定位精度為10 m；本文系統(tǒng)的運算平臺為具有2.8 GHz i7-7700HQ CPU的筆記本電腦。場景1采用比亞迪E5電動車作為測試車，實驗平臺見圖6(c)；場景2采用智能物流車作為測試車，實驗平臺見圖6(d)。

圖6 實驗場景與平臺Fig.6 Experimental scenarios and platforms

2.1 高校園區(qū)實驗

首先在閉環(huán)路線上采集數(shù)據(jù)構(gòu)建極化地圖，數(shù)據(jù)采集頻率1 m/次，每次采集數(shù)據(jù)包括一幀高精度慣導(dǎo)數(shù)據(jù)和一幀點云數(shù)據(jù)。極化地圖構(gòu)建完成后，進行在線定位實驗，該閉環(huán)路線上共786個待定位節(jié)點，地圖采集和在線定位時間間隔超過24 h。本文選取Tao等[12]提出的算法作為節(jié)點級定位對比算法，該算法首先將待定位節(jié)點和地圖節(jié)點的點云俯視投影圖進行ORB特征匹配并利用RANSAC(random sample consensus)算法去除錯誤匹配點對，其次根據(jù)匹配點對數(shù)量選取最近地圖節(jié)點。若通過節(jié)點定位獲取的地圖節(jié)點與待定位節(jié)點的GPS真值(ground truth)距離最短，則認為節(jié)點定位正確。同時，本文選取經(jīng)典的SLAM算法LOAM[5]作為度量級定位對比算法。本文算法在該閉環(huán)路線上的定位軌跡見圖7(a)，定位誤差見圖7(b)，對比實驗結(jié)果見表1。

表1 高校園區(qū)對比實驗Tab.1 Comparative experiment on campus

圖7 高校園區(qū)定位結(jié)果Fig.7 Localization results of campus

在該閉環(huán)路線上，本文算法正確定位最近地圖節(jié)點個數(shù)為773，節(jié)點定位準確率為98.3%，度量級定位誤差為33.2 cm，最大定位誤差為42.9 cm，滿足智能車高精度定位需求。與Tao等提出的算法和LOAM算法相比，本文算法節(jié)點級定位準確率提高了51.2%，度量級定位誤差降低了43.1 cm。

2.2 工業(yè)園區(qū)實驗

與高校園區(qū)的定位實驗相似，首先在該閉環(huán)路線上采集數(shù)據(jù)構(gòu)建極化地圖，其次進行在線定位，該閉環(huán)路線上共5 205個待定位節(jié)點，構(gòu)圖與定位的時間間隔超過24 h，分別選取Tao等提出的算法和LOAM算法作為節(jié)點級定位和度量級定位的對比算法。本文算法在該閉環(huán)路線上的定位軌跡和定位誤差分別如圖8(a)和8(b)所示，對比實驗結(jié)果見表2。

表2 工業(yè)園區(qū)對比實驗Tab.2 Comparative experiment on factory

圖8 工業(yè)園區(qū)定位結(jié)果Fig.8 Localization results of factory

在該閉環(huán)路線上，本文算法正確定位最近地圖節(jié)點個數(shù)為5 138，節(jié)點定位準確率為98.7%，度量級定位誤差為19.6 cm，最大定位誤差為38.4 cm，滿足智能車高精度定位需求。與Tao等提出的算法和LOAM算法相比，本文算法節(jié)點級定位準確率提高了52.4%，度量級定位誤差降低了77.8 cm。

3 結(jié) 論

本文提出一種基于三維點云極化地圖表征模型的智能車定位算法，以點云極化圖為節(jié)點，采用多尺度漸進定位策略，首先利用GPS/拓撲結(jié)構(gòu)完成初定位，其次利用點云二維特征完成節(jié)點級定位，最后利用點云三維特征完成度量級定位，通過兩種典型場景的實驗驗證，結(jié)論如下：

1)通過極化地圖和點云二維特征定位最近地圖節(jié)點，整體節(jié)點定位準確率，即兩種實驗場景的平均節(jié)點定位準確率，為98.7%，證明該方法可有效定位最近地圖節(jié)點。

2)通過基于三維點云極化地圖表征模型的多尺度定位算法實現(xiàn)智能車定位，整體定位誤差，即兩種實驗場景的平均定位誤差，為21.4 cm，最大定位誤差為42.9 cm，滿足智能車高精度定位需求。

3)實驗中選取了不同的場景和實驗車進行測試，均實現(xiàn)了高精度定位，證明本文算法在不同的場景和實驗車中具有良好的魯棒性。同時，本文算法在線定位時，只需要LiDAR和普通GPS接收機，降低了定位成本。

本文算法仍存在以下不足之處：1)在相似且特征單一的場景中，定位誤差較大，如對稱的車道等；2)平均定位誤差只達到了分米級，尚未達到厘米級。利用定位過程中的誤差信息作為反饋對制圖過程進行干預(yù)，即利用眾包技術(shù)提高地圖表征能力，降低定位誤差，將是本文下一階段的重點研究計劃。