動態環境下無人地面車輛點云地圖快速重定位方法

鄭壯壯, 曹萬科，鄒淵, 張旭東, 杜廣澤

(1.北京理工大學 機械與車輛學院，北京 100081； 2.北京電動車輛協同創新中心，北京 100081)

0 引言

無人車輛定位問題是無人駕駛的重要技術之一，當前大部分民用無人平臺都采用全球定位系統(GPS)與慣性導航組合定位，但在作戰環境下，GPS信號往往會缺失，需要無人作戰平臺依靠自身傳感器進行定位[1]。因此同步定位與建圖(SLAM)技術成為無人駕駛的一個研究重點[2]。SLAM技術解決了無人車輛在移動的同時邊建立地圖、邊定位的問題，但對于事先已經建好的地圖，如何在進入地圖范圍時快速確定車輛在地圖內的位置，仍缺少高效的方法，尤其是在復雜動態環境下，移動的物體會加大定位難度。在未來無人作戰體系下，各無人平臺間信息共享，新的無人作戰單元如何快速地在其他平臺所建立的地圖中確定自己的位置將是其是否能迅速發揮作戰效能的關鍵。

在無人車輛研究初期，受傳感器與計算能力限制，無人車輛主要使用二維激光雷達作為主要傳感器，通過單個平面的點云數據與二維柵格地圖進行匹配尋找初始位置[3]。文獻[4-5]提出采用蒙特卡洛方法，利用粒子濾波進行全局定位；文獻[6]通過提取二維點云特征，并通過最近鄰網絡比較特征相似性從而完成了大面積地圖下的車輛快速重定位。但二維平面數據量稀少，相似程度高，無法應對起伏路面以及復雜作戰環境。

隨著計算能力的增強，三維點云被廣泛運用，極大地增加了信息的豐富度，為復雜環境下6自由度的定位提供了可能[7]。文獻[8-9]從點云數據中提取關鍵點并計算關鍵點的不同描述子，重定位時通過尋找每個關鍵點最鄰近的地圖中關鍵點并建立投票矩陣，最終通過設定閾值進行匹配，但閾值的設定需要根據不同場景人為設定，難以通用化；文獻[10]采取對點云數據進行聚類，通過聚類大幅降低了需要匹配的數量，再提取聚類點云的包括線性、平面性、散射性、各向異性、曲率變化量等一系列特征，并訓練了一個隨機森林分類器將這些特征進行匹配以實現重定位與回環檢測。重定位算法能夠實時運行且準確率較高，證明了聚類方法的高效性。文獻[11-12]將激光雷達采集的點云轉成深度相機所采用的深度圖像，分別提取深度圖像的加速魯棒特征和法線對齊的徑向特征再進行詞袋模型匹配，證明了詞袋模型在篩選候選幀上的快速性；文獻[13]不提取特征而是直接計算整組點云的特征描述子，訓練了一個自適應增強分類器進行點云初始匹配，并最終通過迭代最近點(ICP)算法計算相對位姿，建立了一個從粗匹配到細匹配的邏輯架構；文獻[14]通過減少詞袋數量和提高加載速度加快了匹配速度。

上述方法大多面向室內環境開發，獲得了較好的效果。與室內環境相比，室外場景中包含大量的移動障礙物從而引進了大量誤導信息，因此在匹配前需要對點云進行預處理，剔除錯誤的動態障礙點[15]；并且由于點云距離數量級的擴大，導致點云的密度較低，從而物體的特征相比室內更為稀疏，導致基于局部特征描述子進行定位的方法會產生更大誤差，因此需要采用更具兼容性的描述子。并且由于無人車輛所使用的高精地圖需要覆蓋較大區域，對實時性的要求也更高，若直接對原始點云提取關鍵點或特征構成候選集將使數據量過于龐大。

為解決上述問題，本文通過點云反向溯源概率更新方法消除了室外環境中大量動態障礙物帶來的影響；通過聚類對整個物體進行特征描述子提取并構建詞袋模型匹配，消除了稀疏點云的影響，大幅減少了數據庫中的數據量，滿足了實時性的要求；最后通過在城市無車道線、無道路邊界、無標識牌輔助定位的動態復雜環境下實驗，驗證了新算法的有效性與實時性。

1 算法總體框架

本文提出了基于激光雷達點云反向溯源的動態障礙物剔除方法與基于聚類詞袋模型的三維點云地圖內快速重定位方法，如圖1所示。

圖1 點云地圖重定位算法流程Fig. Flow chart of relocalization algorithm

首先遍歷每幀點云中的每個點，進行反向溯源找尋方位角最接近的光束。引入距離比、角度比、航向差等參數，通過貝葉斯公式計算其為靜態障礙點的概率，連續比較多幀后去除低于閾值的點從而達到剔除動態障礙物的目的。在建圖階段對三維點云地圖進行拆分、聚類，提取描述子以訓練詞袋模型詞典，構建描述子數據集，并存儲建圖產生的點云地圖。在匹配定位階段通過匹配數據集將重合度最高的子地圖作為匹配結果實現快速初始定位，最后采用改進實時激光定位與建圖(LOAM)[16-17]算法進行后續的精準定位。

2 基于貝葉斯概率的動態障礙剔除

在動態環境下建圖和重定位過程中，動態障礙物會對匹配過程造成干擾，其在多個時刻于不同位置被激光雷達檢測到，若這些點被存入點云地圖中，地圖內將會儲存多個本不該存在的障礙物。現有方法主要基于柵格地圖設計，對于三維點云地圖缺少有效方法。

本算法根據激光雷達一個角度下只會存在一個點的特性設計，流程如圖2所示。通過遍歷所存點云進行反向溯源，計算其相對當前幀激光雷達位置所在的視角與距離，比較當前幀在該視角下測得的距離值與計算所得的距離值大小，得到該點為動態障礙點的概率。通過貝葉斯公式計算多次修正后動態障礙點的概率，當超過設定的閾值時予以剔除。

圖2 動態障礙剔除方法流程Fig.2 Flow chart of dynamic obstacle filtering method

2.1 點云反向溯源

為快速找到已存地圖點在當前激光雷達坐標系下的視角，本文采用(1)式和(2)式求得地圖點與當前幀點云在球面坐標系下對應的水平面方位角φ與相對水平面的仰角θ，通過kd-Tree搜索方法找到與地圖內點云P具有相近方位角與仰角值的當前幀點云Q，如圖3所示，圖3中O點為激光雷達坐標系原點。即每個P點會分入以OQ連線為軸線的一個圓錐體范圍內，kd-Tree設定的閾值大小即為圓錐體的頂角大小。

(1)

(2)

式中：x、y、z為點的坐標值。

圖3 球面坐標系Fig.3 Spherical coordinate system

2.2 單幀動態障礙概率計算

依據激光雷達特性與無人車實際工況，做出以下推論：

1)地圖點P在雷達坐標系下的距離dP與當前測得的距離dQ的差距越大，P為動態障礙點的概率越大；

2)點P所對應的視角與Q的視角相差越大，當前幀判斷可信度越低，概率改變量越小；

3)dP值越大，當前幀判斷可信度越低，概率改變量越小；

4)點P所對應的視角與車輛航向夾角越大，概率改變量越小。

圖4 因障礙物移動造成距離、角度發生變化Fig.4 Distance and angle change due to obstacle movement

推論1、推論2、推論3可從圖4中推出，對于時刻t，激光雷達掃到障礙物S上返回點P. 對于時刻t+m，點P在此時與激光雷達的距離為dP，點Q為當前點云中與點P視角最接近的點，測得其距離為dQ；若障礙物S為動態障礙物，此時激光將打在其他物體上，從而得到不同的距離值，因此dP與dQ的差距越大，P為動態概率點的概率也越大。

激光雷達豎直方向上相鄰兩線間存在一定夾角，且點云預處理時對原始點云進行了降采樣操作，因此kd-Tree搜索的閾值需保證大部分點都能夠獲得匹配，但也導致P點與雷達連線OP和Q點與雷達連線OQ會存在一定的角度偏差，兩點可能為不同障礙物返回的點，因此當二者間視角相差越大，此次判斷的可信度也越低。

隨著時差m的增加，P點距離激光雷達越遠，被其他障礙物遮擋的可能性越高，且對于距離較遠的點，相同角度差對應的橫向偏差也越大，屬于同一障礙物的概率也越低，因此dP值越大時，此次判斷可信度越低。

推論4針對無人車實際工況所提出。無人車輛行駛時與周邊動態障礙物運動方向多為一致，因此在航向上動態障礙物所帶來的距離變化量最大，且激光束在同一障礙上的概率最大。如圖5所示，對于越偏離車輛航向的障礙物，在t時刻記入地圖的點P，在t+m時刻容易被兩側其他障礙物遮擋，造成dP與dQ存在較大差異。因此OP與車輛航向夾角越大，可信度越低，概率改變量越小。

圖5 兩側障礙物易遮擋造成誤檢Fig.5 False detection due to obstacles on both sides

為量化計算每個點為動態障礙物的概率，根據推論1～推論4推出如下公式計算每幀觀測值下P點為動態障礙點的概率Pb(Dyn|α,β,δ,dP)。

Pb(Dyn|α,β,δ,dP)∝
Pb(ODyn)[1+C(δ)U(α,β,dP)]，

(3)

式中：α為OP與OQ的夾角，即兩點的視角差；β為OP與車輛航向的夾角；δ為dP與dQ的差值相對于dP的比值，即|dP-dQ|/dP;C為基于δ得出的概率改變量，取值為

(4)

U為此幀判斷的可信度，取值為

(5)

αmax為人為設定的夾角最大值，dPmax為人為設定的距離最大值；Pb(ODyn)表示每個點為動態障礙的初始概率，由實際觀測經驗取20%.

2.3 連續多幀對比概率更新

通過貝葉斯(6)式可以計算每個點經過10次判斷后為動態障礙點的概率Pb(Dyn|z1:10)。

(6)

式中：zt為第t幀判斷結果；Pb(Dyn|zt)即根據當前幀所進行的判斷得出的概率值，即由(3)式計算所得；Pb(Dyn|z1:t-1)為根據前幾幀判斷所累積的概率；Pb(Dyn)表示點為動態障礙點的先驗概率，即Pb(ODyn)=0.2.

對連續多幀進行累積計算，當最后的動態概率高于設定的閾值時，即認為該點為動態障礙點，從而從地圖中予以剔除，完成地圖的更新。

3 基于詞袋模型的快速重定位

對于重定位問題，其關鍵是如何有效地檢測出無人車是否經過同一地點。因此需要比對點云之間的相似性，最直接的方法即進行特征匹配。但這些匹配算法需耗費較長的計算時間，難以滿足實時性的要求。本文提出基于聚類與詞袋模型的點云地圖快速重定位算法，其流程如圖6所示。

圖6 快速重定位方法流程圖Fig.6 Flow chart of fast relocalization method

3.1 點云聚類與描述子計算

本算法通過對原始點云進行聚類處理使每個物體作為一個單詞以對場景進行描述。采用判斷相鄰兩線點間連線與水平面的夾角是否超過閾值來篩選出非地面點，如圖7所示。

圖7 地面點剔除方法Fig.7 Ground point culling method

采用自適應閾值的區域生長算法進行聚類。隨機選取點云中的一點作為種子點進行區域生長，通過kd-Tree快速搜索算法尋找該點的鄰近點，將歐式距離小于自適應閾值的鄰近點劃分為種子點的同一點集(同一物體)，自適應閾值隨著點與激光雷達距離的增大而增大。若鄰域內不存在滿足閾值的臨近點，則這組點集搜索完畢，再從剩余點云中隨機選取新的種子點進行搜索，直至點云中的所有點都劃分入相應的點集中，完成聚類，如圖8所示為聚類完成的5個不同物體。

圖8 自適應閾值聚類結果Fig.8 Adaptive threshold clustering results

為防止同一物體在多幀點云地圖中具有相似度較高的描述向量，對同一物體在不同幀下的點集描述子應該區分不同的觀測視角。因此本文選取視點特征直方圖(VFH)作為聚類結果的描述子。VFH描述子為一個由308個浮點數組成的向量，其在快速點特征直方圖的基礎上增加了視點方向與聚類點集中每個點估計法線之間的統計信息。因此在不同視角下會有不同的VFH描述子，利于區分不同位姿。

3.2 訓練詞典與構建數據庫

訓練詞典并構建匹配數據庫與建圖同步進行。通過將相鄰多幀點云進行疊加，將疊加后的點云集作為關鍵幀進行點云聚類和描述子計算。每組局部點云對應于n×308的描述向量，n為物體數量。當建圖完成后，每個關鍵幀都擁有其對應的描述向量與全局坐標以構成數據庫，而所有單詞被放入詞典中。

采用詞袋模型函數庫DBoW3完成詞典與數據庫的構建，可最多容納kl個單詞，k為每層分類數，l為層數。匹配時，每個單詞只需比較l次即可找到最相近的單詞。

數據庫中的每組關鍵幀可由其擁有的單詞在詞典中的分布來表示。詞袋模型函數庫DBoW3為詞典中每個單詞的重要性加以評估，通過詞頻- 逆文本頻率指數(7)式賦予每個單詞以權重。

(7)

式中：ωi為第i個單詞的權重；nih為第i個單詞在關鍵幀h中出現的次數；nh為關鍵幀h中單詞的數量；N為構建詞典所使用的關鍵幀數量；Ni為包含第i個單詞的關鍵幀數目。

3.3 計算相似度獲取初始位姿

通過比較當前點云描述向量與數據庫中存儲的描述向量，找尋相似度最大的關鍵幀，初步確認當前車輛所在地圖中的位置，此步驟在重定位過程中只執行一次。

通過L1范數形式(8)式計算向量與數據庫中每一個向量的相似度，選取相似度最高的關鍵幀。

(8)

式中：s為最后所得相似度；vc、vhj分別為當前點云與數據庫中第j個關鍵幀hj的描述向量。

為提高匹配準確率，先由詞袋模型選取相似度排名前幾的關鍵幀，再采用ICP匹配方式對這幾組候選點云進行細匹配，最終每組候選點云的匹配分數由詞袋模型相似度與ICP匹配相似度相乘而得，選取分數最高的作為最終匹配點云，從而得到在地圖中更精確的初始位姿。

3.4 后續精準定位

在得到車輛在地圖中初始位置后，通過更改LOAM算法實現在地圖中的后續精準定位。定位流程如圖9所示。

激光里程計以10 Hz高頻運算，為以1 Hz低頻進行的與地圖匹配節點提供初始參考位姿變換。以初始位姿為圓心，將一定范圍內的地圖點云作為匹配點云參與低頻匹配，從而獲得與建圖精度相同的定位精度。

4 實車實驗

本文所用的實驗車輛由北京汽車股份有限公司生產的EU260電動車改裝而成，如圖10所示。該車配備有激光雷達、毫米波雷達、Mobileye攝像頭，以及網絡差分慣性導航系統，以獲取周圍環境信息及定位信息。本文以Velodyne-16線雷達作為傳感器，參數如表1所示，水平安裝于車輛頂部，離地距離為2.1 m，x軸方向與車輛前進方向保持一致。用作實驗數據比對的差分慣性導航設備主機安裝于接近車輛中心位置，雙天線前后安置于車輛頂部，可達到厘米級定位精度。其內置慣性測量單元，加速度計量程達4g，偏差穩定性達20×10-6g，陀螺儀量程達2 000°/s，零偏穩定性達3°/h. 實驗使用安裝Inter i7-7700HQ CPU,8 G內存的筆記本電腦作為處理器，負責建圖與重定位匹配。軟件編程環境基于Ubuntu 16.04系統下的機器人操作系統ROS開發，

圖10 實驗用無人車Fig.10 Experimental unmanned ground vehicle

表1 激光雷達參數

采用C++編寫。實驗過程中車輛速度保持在10～20 km/h的范圍內。

4.1 動態障礙點剔除與點云地圖建立

實驗場景選擇在北京理工大學校區非結構化動態環境下進行測試，園區內道路較為狹窄，沒有車道線與明顯道路邊界，且存在大量行人與車輛等動態障礙物。在點云地圖構建階段，采用改進LOAM算法對車輛位姿進行估計與點云地圖的生成，在其基礎上添加動態障礙點剔除算法。動態障礙點剔除算法結果如圖11所示：深色點為所剔除的點云，淺色點云為存入地圖的靜態障礙點。由道路上的動態障礙物造成的錯誤點云信息都被算法予以剔除，墻面、路邊停車等靜態障礙物上的少部分點云被誤刪。誤刪動態點多出現在與路邊車輛、樹等障礙距離較遠的墻面上，此時距離改變量較大從而造成概率變化大。并且因為動態障礙點的漏檢比誤刪所造成的危害更大，所以在閾值設定時往往會設置得較低以盡量剔除動態點。但是由于采用了聚類提取描述子的方法，誤刪的點云對障礙物整體成型與描述子計算影響不大。

圖11 動態障礙點剔除效果Fig.11 Dynamic obstacle elimination effect

圖12 實驗場地衛星照片Fig.12 Satellite photo of experimental site

實驗場地衛星照片如圖12所示，最終SLAM效果如圖13所示。圖13中藍色點云為地面分割算法所分割出的道路地面點云，黑色點云為高于水平地面的障礙點。對比點云圖與衛星照片，看出建圖過程很好地還原了園區環境，且成功地剔除了道路上的動態障礙點。

圖13 SLAM結果Fig.13 SLAM result

4.2 快速初始重定位

本文共采集600 m×400 m的北京理工大學校區環境區域，共6組不同路段數據。其中數據組1、數據組2中無重復路徑，數據組3～數據組6中都含有回環路徑。動態障礙點剔除，聚類分割與數據庫建立與數據采集同時進行，采用10 Hz更新頻率，聚類分割與描述子計算由另一線程完成，由移動距離觸發。在當前計算環境中均能滿足實時性計算的要求。

實驗中每隔5 m生成一幅關鍵幀，每組關鍵幀點云包含前后共20幀連續點云的組合結果以保證關鍵幀的點云密度。對關鍵幀進行自適應閾值聚類，圖14為其中一組關鍵幀點云的聚類結果，圖14中不同顏色的點云代表不同物體。

圖14 關鍵幀聚類結果Fig.14 Key frame clustering result

分隔較遠的物體都被明確區分，只有個別距離很近的物體被識別為同一物體，但因為構建數據庫與重定位時采用相同的聚類算法，因此并不影響匹配結果。圖15為同一物體在相鄰兩個關鍵幀下所計算出來的描述子，具有相同的變化趨勢，證明VFH描述子既能明確區分不同物體，還能對激光雷達位置的變化做出響應。

圖15 不同視角下同一物體的VFH描述向量Fig.15 VFH description vector of the same object from different perspectives

重定位實驗時，將車輛重新駛上先前采集數據的道路上，通過聚類詞袋模型求得車輛在地圖中的初始位置并通過對比車載高精定位數據判斷是否匹配正確。數據庫建立時每個關鍵幀位姿之間相隔5 m，因此在初始匹配時當結果與實際位姿距離在2.5 m內即認為初始匹配成功，初始匹配過程在整個定位過程中只執行一次。

純詞袋模型匹配結果如表2所示，匹配時間均在150 ms以內，皆低于采用遍歷匹配的定位方法所耗費的時間。6組數據匹配正確率均在82%以上。無重復路段的數據正確率在90%以上。

表2 純詞袋模型匹配結果Tab.2 Matching results only using bag-of-words model

當加上ICP匹配計算，每次匹配選取相似度前5的點云作為候選，再分別進行ICP匹配計算最終匹配分數，結果如表3所示，最終6組匹配正確率均在91%以上，且均在1.8 s內計算完成。

結合匹配算法與開源正態分布變換(NDT)算法、采樣一致性初始配準算法(SAC-IA)粗匹配+ICP細匹配算法、采用遍歷關鍵幀的幀- 幀匹配算法進行對比，選取數據組3進行實驗，共包含149個關鍵幀，點云地圖含數據點29萬個，平均匹配時間與正確率如表4所示。

表3 詞袋模型與ICP結合匹配結果Tab.3 Matching results using bag-of-wordsmodel and ICP

表4 不同算法的匹配結果Tab.4 Matching results of different methods

本文提出的算法雖然正確率相比提供了初始位姿的NDT算法以及幀- 幀匹配算法稍微降低，但所需的計算時間大幅減少。與NDT、自適應蒙特卡洛定位等需提供初始位姿的點云配準方法相比，可由算法自主計算初始位姿，因此可以在完全失去GPS定位的情況下仍舊達到高準確率。與SAC-IA+ICP等點云全局粗匹配加局部細匹配算法相比，不僅減少了計算量，且避免了室外點云集法線方向不確定性的影響。與采用依次與關鍵幀匹配計算相似度的遍歷方法相比，通過聚類方法與描述子提取方法大幅降低運算量，且待匹配的數據庫在建圖結束后就已經計算完成，即大量的計算工作量已于重定位前完成，從而大幅減少匹配所需時間。

4.3 后續精準定位

為比較算法在先建地圖中的定位精度，將建圖時的定位軌跡作為參考軌跡，通過回放截取所記錄的點云數據，比較算法所得出的重定位軌跡與參考軌跡的偏差從而評價算法的精度。

圖16與圖17為經過初始定位與全局匹配后的重定位軌跡與參考軌跡對比。由圖16和圖17可知：詞袋模型的初始定位偏差在1.5 m左右，后續的精準匹配迅速將車輛位置匹配到參考軌跡上。從全局匹配結果來看，重定位軌跡與參考軌跡重合良好。對匹配定位結果和參考值進行誤差評估，通過計算得到全程匹配定位的均方根誤差均在15 cm以內，每幀匹配的時間都在100 ms以內，證明本算法能實現在高精度地圖下的實時精準定位。

圖16 數據組1重定位軌跡與參考軌跡對比Fig.16 Comparison of Data 1 relocalization and reference trajectories

圖17 數據組3重定位軌跡與參考軌跡對比Fig.17 Comparison of Data 3 relocalization and reference trajectories

5 結論

在預先生成的地圖中快速找到自身位置是實現無人車輛自主規劃和導航的基礎，而如何在無GPS定位的復雜動態環境下實現快速精準定位仍是較大難題。本文所提出的算法依靠自身車載激光雷達實現了在復雜動態環境下的快速重定位，在保證實時性的同時也實現了高精度定位。該算法通過激光雷達點云反向溯源，進行貝葉斯概率更新實現動態障礙物剔除，通過構建點云聚類詞袋模型進行快速初始定位確定初始位姿，最終通過改進LOAM算法實現后續精準定位。實車實驗結果表明，本文算法能去除動態環境下移動物體的干擾，能實現快速且準確地選取初始位姿，在后續精準定位中達到了高精度效果。

后續工作需要進一步提升初始定位的準確率以及點云地圖的通用性。