三維點云極化地圖表征模型與智能車定位方法

胡釗政，李 飛,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)平均定位誤差只達到了分米級，尚未達到厘米級。利用定位過程中的誤差信息作為反饋對制圖過程進行干預，即利用眾包技術提高地圖表征能力，降低定位誤差，將是本文下一階段的重點研究計劃。