動態(tài)環(huán)境下無人地面車輛點云地圖快速重定位方法

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

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

0 引言

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

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

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

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

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

1 算法總體框架

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

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

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

2 基于貝葉斯概率的動態(tài)障礙剔除

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

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

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

2.1 點云反向溯源

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

(1)

(2)

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

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

2.2 單幀動態(tài)障礙概率計算

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

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

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

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

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

圖4 因障礙物移動造成距離、角度發(fā)生變化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為動態(tài)障礙物，此時激光將打在其他物體上，從而得到不同的距離值，因此dP與dQ的差距越大，P為動態(tài)概率點的概率也越大。

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

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

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

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

為量化計算每個點為動態(tài)障礙物的概率，根據(jù)推論1～推論4推出如下公式計算每幀觀測值下P點為動態(tài)障礙點的概率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為人為設(shè)定的夾角最大值，dPmax為人為設(shè)定的距離最大值；Pb(ODyn)表示每個點為動態(tài)障礙的初始概率，由實際觀測經(jīng)驗取20%.

2.3 連續(xù)多幀對比概率更新

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

(6)

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

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

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

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

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

3.1 點云聚類與描述子計算

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

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

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

圖8 自適應(yīng)閾值聚類結(jié)果Fig.8 Adaptive threshold clustering results

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

3.2 訓練詞典與構(gòu)建數(shù)據(jù)庫

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

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

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

(7)

式中：ωi為第i個單詞的權(quán)重；nih為第i個單詞在關(guān)鍵幀h中出現(xiàn)的次數(shù)；nh為關(guān)鍵幀h中單詞的數(shù)量；N為構(gòu)建詞典所使用的關(guān)鍵幀數(shù)量；Ni為包含第i個單詞的關(guān)鍵幀數(shù)目。

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

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

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

(8)

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

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

3.4 后續(xù)精準定位

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

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

4 實車實驗

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

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

表1 激光雷達參數(shù)

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

4.1 動態(tài)障礙點剔除與點云地圖建立

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

圖11 動態(tài)障礙點剔除效果Fig.11 Dynamic obstacle elimination effect

圖12 實驗場地衛(wèi)星照片F(xiàn)ig.12 Satellite photo of experimental site

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

圖13 SLAM結(jié)果Fig.13 SLAM result

4.2 快速初始重定位

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

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

圖14 關(guān)鍵幀聚類結(jié)果Fig.14 Key frame clustering result

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

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

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

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

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

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

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

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

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

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

4.3 后續(xù)精準定位

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

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

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

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

5 結(jié)論

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

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