柴夢娜,劉元盛,任麗軍

(1.北京聯(lián)合大學(xué) 智慧城市學(xué)院,北京 100101；2.北京聯(lián)合大學(xué) 機(jī)器人學(xué)院,北京 100101；3.北京聯(lián)合大學(xué) 北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室,北京 100101)

1 引 言

同時定位與建圖(simultaneous localization and mapping,SLAM)問題可以描述為機(jī)器人在未知環(huán)境中從一個未知位置開始移動,在移動過程中根據(jù)位置估計和地圖進(jìn)行自身定位,同時在自身定位的基礎(chǔ)上建造增量式地圖,實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航。1986年，在舊金山舉行的ICRA會議上首次提出將基于概率的估算方法應(yīng)用于局部地圖構(gòu)建[1]。1985年Cheeseman和Smith撰寫的文章創(chuàng)立的原理可作為求解SLAM問題的數(shù)學(xué)基礎(chǔ)[2]。1987年,Smith和Cheeseman等人提出基于卡爾曼濾波的SLAM算法理論框架,該理論被廣泛應(yīng)用[3]。隨著技術(shù)的逐漸發(fā)展,衍生出許多基于概率的方法,主要包括:擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)方法[4-7]、無跡卡爾曼濾波(Unscented Kalman Filter,UKF)方法[8-10]、粒子濾波器(Particle Filter,PF)方法[11-13]等基于濾波的算法。基于卡爾曼濾波的方法要求估計的問題必須基于高斯假設(shè)下,不能很好地應(yīng)用于車輛的非線性系統(tǒng)。粒子濾波在重采樣過程可能導(dǎo)致粒子耗盡,使得無法進(jìn)行很好的位姿估計。

1992年,Besl P J等人提出迭代最近點(diǎn)(Iterative Closest Point,ICP)算法[14]。相對于基于濾波的方法。該算法忽略點(diǎn)云運(yùn)動方程直接采用點(diǎn)云的特征進(jìn)行配準(zhǔn)。其通過尋找兩個點(diǎn)云集的匹配點(diǎn),并計算出兩個點(diǎn)云的變換參數(shù),來進(jìn)行位姿估計與建圖。2003年,Peter Biber等人首次提出二維正態(tài)分布變換(Normal Distributions Transform,NDT)數(shù)據(jù)配準(zhǔn)[15]。該算法將掃描所占用的空間細(xì)分為單元格,根據(jù)單元內(nèi)的點(diǎn)分布,為每個單元計算概率密度函數(shù)。通過評價各單元格映射點(diǎn)的概率分布的和得到變換參數(shù)的得分函數(shù),并使用牛頓迭代算法最小化得分函數(shù),從而得到最佳變換。NDT算法是一種整體因素決定的匹配。其在配準(zhǔn)過程中,避免點(diǎn)與點(diǎn)對應(yīng)的復(fù)雜問題,相比于ICP算法,該算法配準(zhǔn)效率高,速度快。2007年,Martin Magnusson提出三維NDT算法,為三維點(diǎn)云的空間變換提供了理論基礎(chǔ)[16]。

在SLAM問題中,位姿的估計往往是一個遞推的過程,即由上一幀位姿解算當(dāng)前幀的位姿,因此會造成較大的累計誤差?；丨h(huán)檢測最初應(yīng)用于視覺SLAM,在激光雷達(dá)的應(yīng)用中較少。它能夠使得機(jī)器人更加準(zhǔn)確地去識別自己曾經(jīng)去過的位置,通過進(jìn)行辨識過去的位置,檢測回環(huán)來解決位姿的漂移問題,使得地圖的建立更加準(zhǔn)確。傳統(tǒng)的視覺SLAM 中回環(huán)檢測方法為視覺詞帶模型。董蕊芳等人提出一種基于改進(jìn)TF-IDF的視覺SLAM回環(huán)檢測算法[17]。首先,采用圖像中的直線作為特征來進(jìn)行回環(huán)檢測,接下來通過二進(jìn)制線特征描述子來進(jìn)行視覺詞典的構(gòu)建。徐建鵬等人提出一種基于Fast-RCNN神經(jīng)網(wǎng)絡(luò)的回環(huán)檢測方法,利用圖像語義特征等信息構(gòu)建二維語義特征向量圖,并利用非線性累計誤差計算兩幀圖像間的相似度[18]。張劍華等提出點(diǎn)云片段匹配約束提升回環(huán)檢測的方法[19]。T.R?hling等人提出一種對三維點(diǎn)云數(shù)據(jù)進(jìn)行基于直方圖的相似測量的回環(huán)檢測方法[20]。

為解決大規(guī)模場景下進(jìn)行同時定位與建圖存在較大累積誤差的問題,本文提出一種基于激光點(diǎn)云NDT特征的兩步回環(huán)檢測方法,并將所提方法應(yīng)用于完整SLAM框架。首先對點(diǎn)云進(jìn)行預(yù)處理,濾除噪點(diǎn),之后采用重疊網(wǎng)格對點(diǎn)云進(jìn)行三維NDT配準(zhǔn)與回環(huán)檢測。回環(huán)檢測中,充分利用NDT配準(zhǔn)中網(wǎng)格特性,采用兩步法。第一步,利用各網(wǎng)格點(diǎn)云方差矩陣特征值對點(diǎn)云進(jìn)行外觀描述,并進(jìn)行分類,通過各類數(shù)量為各類型分配權(quán)重,構(gòu)造兩幀間相似函數(shù),進(jìn)行粗回環(huán)檢測。第二步,通過網(wǎng)格點(diǎn)云均值點(diǎn)距原點(diǎn)距離保證點(diǎn)云具有旋轉(zhuǎn)不變性,進(jìn)行精確回環(huán)檢測。最后,在“小旋風(fēng)”智能車平臺進(jìn)行驗(yàn)證。

2 改進(jìn)NDT點(diǎn)云匹配SLAM方法

首先給出了完整SLAM系統(tǒng)流程。之后,介紹了傳統(tǒng)NDT算法,并給出三維點(diǎn)云NDT配準(zhǔn)流程。最后,提出對三維點(diǎn)云的NDT匹配的回環(huán)檢測算法,并進(jìn)行理論分析。

2.1 基于NDT特征的SLAM完整框架

本文采用完整SLAM框架,首先對點(diǎn)云進(jìn)行預(yù)處理。然后,對相鄰點(diǎn)云進(jìn)行配準(zhǔn),同時利用網(wǎng)格正態(tài)分布信息進(jìn)行兩步回環(huán)檢測?；丨h(huán)檢測中,首先判斷兩幀點(diǎn)云是否符合粗回環(huán)檢測配準(zhǔn)條件,符合粗回環(huán)檢測條件下,再進(jìn)行精確閉環(huán)檢測。采用非線性優(yōu)化方法進(jìn)行優(yōu)化,完成最終建圖。文中采用SLAM系統(tǒng)流程圖如圖1所示。

2.2 傳統(tǒng)NDT算法

NDT算法是一種配準(zhǔn)算法,應(yīng)用于多維點(diǎn)的統(tǒng)計模型。將曲面用一組局部概率密度函數(shù)進(jìn)行表示,每個函數(shù)描述曲面的一部分。當(dāng)使用NDT進(jìn)行掃描配準(zhǔn)時,目標(biāo)是找到當(dāng)前掃描的位置,使當(dāng)前掃描點(diǎn)位于參考掃描表面的可能性最大化。待優(yōu)化的參數(shù)為當(dāng)前掃描的姿態(tài)估計,包括旋轉(zhuǎn)矩陣與平移矩陣。最好的姿態(tài)估計是使似然函數(shù)最大化,對應(yīng)地最小化其負(fù)對數(shù)函數(shù)的概率。由于其在配準(zhǔn)過程中不利用對應(yīng)點(diǎn)的特征計算和匹配,所以時間比其他方法快,提高了算法的運(yùn)行效率。對于不同分辨率的點(diǎn)云,配準(zhǔn)效果也比較好。

圖1 SLAM系統(tǒng)流程圖

(1)

(2)

NDT算法流程圖如圖2所示。三維NDT算法配準(zhǔn)步驟如下:

(3)

(4)

(5)

(3)對于要配準(zhǔn)的點(diǎn)云,通過變換T將其轉(zhuǎn)換到參考點(diǎn)云的網(wǎng)格中,如式(6)所示:

(6)

圖2 NDT算法流程圖

(4)根據(jù)正態(tài)分布參數(shù)計算每個轉(zhuǎn)換點(diǎn)的概率密度,如式(7)所示:

(7)

(5)NDT配準(zhǔn)得分通過對每個轉(zhuǎn)換點(diǎn)計算出的概率密度相加得到,如式(8)所示:

(8)

(9)

(10)

(11)

(12)

2.3 基于激光點(diǎn)云NDT特征的回環(huán)檢測

本文所提回環(huán)檢測中,使用重疊的NDT單元以增加每個網(wǎng)格中激光點(diǎn)數(shù),并最小化點(diǎn)云單元空間離散問題。如果每個體素的邊長為b,則體素中心點(diǎn)之間的距離為b/2。圖3為4個NDT單元排列圖。

圖3 NDT單元排列圖

回環(huán)檢測中分兩步,第一步:利用NDT各網(wǎng)格的特征值屬性,對網(wǎng)格外觀進(jìn)行分類,包括線性,平面和球形,通過各類數(shù)量分配權(quán)重,并構(gòu)造兩幀間相似函數(shù),進(jìn)行粗回環(huán)檢測。在滿足第一步的條件下執(zhí)行第二步。第二步:利用各網(wǎng)格均值到坐標(biāo)原點(diǎn)距離總和進(jìn)行精確回環(huán)檢測,以此保證點(diǎn)云的旋轉(zhuǎn)不變性。

2.3.1 點(diǎn)云外觀描述進(jìn)行粗回環(huán)檢測

圖4 單元格類型圖

·如果λ2/λ3≤te,分布是線性的；

·如果是非線性的并且λ1/λ2≤te,分布為平面的；

·如果是非線性、非平面的分布為球形(換句話說,如果沒有特征值比另一個特征值大1/te倍)。

根據(jù)球形類、平面類和線性類,將每個網(wǎng)格分為三大子類,ns球面子類、np平面子類和nl線性子類。

(13)

每種類型在掃描的點(diǎn)云中占據(jù)的比例不同,因此不同的類型應(yīng)該占據(jù)不同的權(quán)重。用wi表示i類型所占權(quán)重。用式(14)進(jìn)行計算。將每幀圖像由類型與其權(quán)重表示如式(15)。

(14)

F={(ws,S),(wp,P),(wl,L)}?vF

(15)

對F和G兩幅圖像的相似度評分,采用其L1范數(shù)形式,值越大表明兩幅圖像越相似。設(shè)置閾值,至此完成了粗回環(huán)檢測。

(16)

2.3.2 網(wǎng)格均值進(jìn)行精確回環(huán)檢測

激光雷達(dá)位置到特定表面的距離也是重要的信息。在符合粗回環(huán)條件后,根據(jù)網(wǎng)格類型,將各網(wǎng)格均值點(diǎn)到坐標(biāo)原點(diǎn)間的距離進(jìn)行存儲。

(17)

與具有很少網(wǎng)格的掃描相比,具有許多網(wǎng)格的掃描傾向于具有更大的差異值。為了量化兩次掃描F和G之間的差異,使用每次掃描中占用的NDT網(wǎng)格的總數(shù),對F和G各類型距離進(jìn)行歸一化。SFi表示F掃描中i類型外觀網(wǎng)格均值距原點(diǎn)的歐幾里得距離之和。max(SF,SG)/min(SF,SG)作為歸一化因子。設(shè)置誤差閾值,確定是否兩幀間重合度高,確定是否為回環(huán)。

(18)

3 實(shí)驗(yàn)結(jié)果與分析

本文提出的框架使用從Velodyne16線激光雷達(dá)收集的數(shù)據(jù)集進(jìn)行驗(yàn)證。實(shí)驗(yàn)平臺為“小旋風(fēng)”無人駕駛智能車,如圖5所示。圖6為實(shí)驗(yàn)環(huán)境和無人駕駛汽車的行駛路線,全程約800 m。圖6中A點(diǎn)為起點(diǎn)和終點(diǎn),起點(diǎn)與終點(diǎn)形成閉合,此環(huán)境可以很好檢驗(yàn)算法對于回環(huán)回路的性能。為驗(yàn)證算法的有效性,根據(jù)上述的地圖構(gòu)建方案,本文對真實(shí)的環(huán)境進(jìn)行驗(yàn)證,并對采集數(shù)據(jù)結(jié)果進(jìn)行了分析。為了保證本文實(shí)驗(yàn)的可對比性,在同一場景用傳統(tǒng)NDT算法和文獻(xiàn)[20]方法進(jìn)行實(shí)驗(yàn),并與本文算法進(jìn)行對比分析。

圖5 “小旋風(fēng)”無人駕駛智能車

采集數(shù)據(jù)時,車輛行駛速度約為10 km/h,整個場景包含3507幀,路面較平坦。將對于激光雷達(dá)向前方向定為X軸,垂直激光雷達(dá)方向定為Y軸,垂直地面方向定義為Z軸。SLAM建圖時,以起點(diǎn)為坐標(biāo)原點(diǎn),進(jìn)行兩幀之間的配準(zhǔn),將新的點(diǎn)云轉(zhuǎn)換到局部坐標(biāo)系下,實(shí)現(xiàn)增量式地圖創(chuàng)建。

圖6 實(shí)驗(yàn)場景

傳統(tǒng)NDT建圖結(jié)果，采用的網(wǎng)格為不重疊網(wǎng)格,網(wǎng)格邊長設(shè)置為2米,最大補(bǔ)償設(shè)置為0.1,最大迭代次數(shù)設(shè)置為30,最小轉(zhuǎn)換差異為0.01。根據(jù)式(3)到式(12)的流程進(jìn)行傳統(tǒng)三維NDT配準(zhǔn)。圖7為傳統(tǒng)三維NDT建圖場景俯視圖,圖8為側(cè)面圖和回環(huán)位置對比圖。

圖7 傳統(tǒng)NDT建圖(俯視圖)

圖8 傳統(tǒng)NDT建圖(局部圖)

本文算法建圖結(jié)果，在根據(jù)傳統(tǒng)方法進(jìn)行三維NDT點(diǎn)云配準(zhǔn)的同時,采用本文所提方法進(jìn)行實(shí)時回環(huán)檢測。在進(jìn)行NDT配準(zhǔn)時,采用的點(diǎn)云網(wǎng)格邊長為2,最大步長設(shè)置為0.1,最大迭代次數(shù)設(shè)置為30,最小轉(zhuǎn)換差異設(shè)置為0.01。在進(jìn)行粗回環(huán)檢測時,網(wǎng)格分類閾值te設(shè)置為e-5,L1范數(shù)設(shè)置為0.3。進(jìn)行精確回環(huán)檢測時,兩幀距原點(diǎn)距離之和差異與上一幀點(diǎn)云距離比例閾值為0.02。圖9為本文算法所建圖的俯視圖,圖10中給出了回環(huán)處的細(xì)節(jié)圖。

圖9 改進(jìn)算法建圖(俯視圖)

圖10 改進(jìn)算法建圖(局部圖)

將傳統(tǒng)NDT建圖軌跡和改進(jìn)算法建圖軌跡與GPS軌跡進(jìn)行對比,如圖11所示。圖12將回環(huán)處誤差放大,原點(diǎn)處為回環(huán)處。

圖11 三種算法與GPS軌跡對比圖

圖12 回環(huán)處局部圖

將兩種方法與GPS軌跡進(jìn)行逐幀對比。圖13為各幀距離誤差對比圖,圖14為各幀X方向距離誤差對比圖,圖15為各幀Y方向距離誤差對比圖。圖16為各幀配準(zhǔn)用時對比圖。

圖13 距離誤差對比圖

圖14 X方向距離誤差對比圖

從圖7的俯視圖可以看出在回環(huán)處X和Y方向存在明顯誤差,從圖8中可以看出傳統(tǒng)方法建圖在Z方向同樣存在極大誤差。與傳統(tǒng)方法相比,在保證其他參數(shù)設(shè)置不變的情況下,采用重疊網(wǎng)格,并加入本文的回環(huán)檢測方法。圖9中回環(huán)處已沒有明顯誤差,圖10中可以看出在Z處同樣不存在明顯誤差。在3486幀處為回環(huán)處,三種方法與GPS軌跡誤差對比如表1所示。

圖15 Y方向距離誤差對比圖

圖16 各幀配準(zhǔn)用時對比圖

表1 與GPS軌跡誤差對比

從圖13到圖15可看出本文提出方法,相對于傳統(tǒng)方法,已明顯減小累積誤差。與文獻(xiàn)[20]方法相比,本文精度整體配準(zhǔn)提高31 %。在采用本文算法進(jìn)行回環(huán)檢測時,進(jìn)去精確回環(huán)檢測次數(shù)為351幀,運(yùn)用粗回環(huán)檢測已排除90 %不存在回環(huán)檢測幀,極大提高了算法的運(yùn)行效率。如圖16三種方法配準(zhǔn)時長對比中可看出,相對于傳統(tǒng)方法,加入回環(huán)檢測后,相應(yīng)的配準(zhǔn)時間邊長,但是相對于文獻(xiàn)[20]采用基于直方圖的方法,由于本文算法充分利用了NDT網(wǎng)格特征,并在回環(huán)檢測時分兩步走,極大地降低了配準(zhǔn)時長。表2為三種算法配準(zhǔn)用時對比。本文方法能夠在保證效率的前提下達(dá)到更加精確的回環(huán)檢測；與使用直方圖方法進(jìn)行回環(huán)檢測相比,本文中的方法在運(yùn)算效率上有了極大提高,在建圖精度上也提高了30 %結(jié)果。充分考慮到NDT建圖特性,本文方法更適合NDT條件下的回環(huán)檢測。

表2 建圖時長與回環(huán)檢測用時對比

4 結(jié) 論

本文針對進(jìn)行大規(guī)模同時定位與建圖存在較大累積誤差的問題,提出一種基于激光點(diǎn)云NDT特征的兩步回環(huán)檢測方法,并將其運(yùn)用于完整SLAM框架中進(jìn)行實(shí)驗(yàn)與分析。該方法充分利用了NDT配準(zhǔn)時各網(wǎng)格均值與方差特征,實(shí)現(xiàn)了同一場景下對傳統(tǒng)建圖結(jié)果的回環(huán)檢測,有效減少了累積誤差。并采用重疊網(wǎng)格進(jìn)行點(diǎn)云配準(zhǔn)與回環(huán)檢測?；丨h(huán)檢測的第一步:通過網(wǎng)格點(diǎn)云方差矩陣特征值對點(diǎn)云進(jìn)行外觀描述,并進(jìn)行分類,通過各類數(shù)量為各類型分配權(quán)重,構(gòu)造兩幀間相似函數(shù),進(jìn)行粗回環(huán)檢測。第二步:通過網(wǎng)格點(diǎn)云均值點(diǎn)距原點(diǎn)距離,進(jìn)行精確回環(huán)檢測,保證點(diǎn)云的旋轉(zhuǎn)不變性。與基于直方圖的回環(huán)檢測相比,極大提高了運(yùn)算速度。論文的研究結(jié)果對基于NDT的大規(guī)模同時定位與建圖具有指導(dǎo)意義。后期將在其中加入回環(huán)預(yù)測,僅在回環(huán)概率高的地方進(jìn)行回環(huán)檢測,進(jìn)一步提高回環(huán)檢測效率。