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

鄭壯壯, 曹萬(wàn)科，鄒淵, 張旭東, 杜廣澤

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

0 引言

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

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

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

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

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

1 算法總體框架

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

圖1 點(diǎn)云地圖重定位算法流程Fig. Flow chart of relocalization algorithm

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

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

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

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

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

2.1 點(diǎn)云反向溯源

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

(1)

(2)

式中：x、y、z為點(diǎn)的坐標(biāo)值。

圖3 球面坐標(biāo)系Fig.3 Spherical coordinate system

2.2 單幀動(dòng)態(tài)障礙概率計(jì)算

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

1)地圖點(diǎn)P在雷達(dá)坐標(biāo)系下的距離dP與當(dāng)前測(cè)得的距離dQ的差距越大，P為動(dòng)態(tài)障礙點(diǎn)的概率越大；

2)點(diǎn)P所對(duì)應(yīng)的視角與Q的視角相差越大，當(dāng)前幀判斷可信度越低，概率改變量越?。?/p>

3)dP值越大，當(dāng)前幀判斷可信度越低，概率改變量越??；

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

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

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

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

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

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

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

為量化計(jì)算每個(gè)點(diǎn)為動(dòng)態(tài)障礙物的概率，根據(jù)推論1～推論4推出如下公式計(jì)算每幀觀測(cè)值下P點(diǎn)為動(dòng)態(tài)障礙點(diǎn)的概率Pb(Dyn|α,β,δ,dP)。

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

(3)

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

(4)

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

(5)

αmax為人為設(shè)定的夾角最大值，dPmax為人為設(shè)定的距離最大值；Pb(ODyn)表示每個(gè)點(diǎn)為動(dòng)態(tài)障礙的初始概率，由實(shí)際觀測(cè)經(jīng)驗(yàn)取20%.

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

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

(6)

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

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

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

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

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

3.1 點(diǎn)云聚類與描述子計(jì)算

本算法通過(guò)對(duì)原始點(diǎn)云進(jìn)行聚類處理使每個(gè)物體作為一個(gè)單詞以對(duì)場(chǎng)景進(jìn)行描述。采用判斷相鄰兩線點(diǎn)間連線與水平面的夾角是否超過(guò)閾值來(lái)篩選出非地面點(diǎn)，如圖7所示。

圖7 地面點(diǎn)剔除方法Fig.7 Ground point culling method

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

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

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

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

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

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

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

(7)

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

3.3 計(jì)算相似度獲取初始位姿

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

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

(8)

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

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

3.4 后續(xù)精準(zhǔn)定位

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

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

4 實(shí)車實(shí)驗(yàn)

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

圖10 實(shí)驗(yàn)用無(wú)人車Fig.10 Experimental unmanned ground vehicle

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

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

4.1 動(dòng)態(tài)障礙點(diǎn)剔除與點(diǎn)云地圖建立

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

圖11 動(dòng)態(tài)障礙點(diǎn)剔除效果Fig.11 Dynamic obstacle elimination effect

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

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

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

4.2 快速初始重定位

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

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

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

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

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

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

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

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

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

結(jié)合匹配算法與開源正態(tài)分布變換(NDT)算法、采樣一致性初始配準(zhǔn)算法(SAC-IA)粗匹配+ICP細(xì)匹配算法、采用遍歷關(guān)鍵幀的幀- 幀匹配算法進(jìn)行對(duì)比，選取數(shù)據(jù)組3進(jìn)行實(shí)驗(yàn)，共包含149個(gè)關(guān)鍵幀，點(diǎn)云地圖含數(shù)據(jù)點(diǎn)29萬(wàn)個(gè)，平均匹配時(shí)間與正確率如表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算法以及幀- 幀匹配算法稍微降低，但所需的計(jì)算時(shí)間大幅減少。與NDT、自適應(yīng)蒙特卡洛定位等需提供初始位姿的點(diǎn)云配準(zhǔn)方法相比，可由算法自主計(jì)算初始位姿，因此可以在完全失去GPS定位的情況下仍舊達(dá)到高準(zhǔn)確率。與SAC-IA+ICP等點(diǎn)云全局粗匹配加局部細(xì)匹配算法相比，不僅減少了計(jì)算量，且避免了室外點(diǎn)云集法線方向不確定性的影響。與采用依次與關(guān)鍵幀匹配計(jì)算相似度的遍歷方法相比，通過(guò)聚類方法與描述子提取方法大幅降低運(yùn)算量，且待匹配的數(shù)據(jù)庫(kù)在建圖結(jié)束后就已經(jīng)計(jì)算完成，即大量的計(jì)算工作量已于重定位前完成，從而大幅減少匹配所需時(shí)間。

4.3 后續(xù)精準(zhǔn)定位

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

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

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

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

5 結(jié)論

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

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