柴夢(mèng)娜,劉元盛,任麗軍

(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 引 言

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

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

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

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

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

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

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

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

2.2 傳統(tǒng)NDT算法

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

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

(1)

(2)

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

(3)

(4)

(5)

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

(6)

圖2 NDT算法流程圖

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

(7)

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

(8)

(9)

(10)

(11)

(12)

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

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

圖3 NDT單元排列圖

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

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

圖4 單元格類型圖

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

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

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

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

(13)

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

(14)

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

(15)

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

(16)

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

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

(17)

與具有很少網(wǎng)格的掃描相比,具有許多網(wǎng)格的掃描傾向于具有更大的差異值。為了量化兩次掃描F和G之間的差異,使用每次掃描中占用的NDT網(wǎng)格的總數(shù),對(duì)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)平臺(tái)為“小旋風(fēng)”無(wú)人駕駛智能車,如圖5所示。圖6為實(shí)驗(yàn)環(huán)境和無(wú)人駕駛汽車的行駛路線,全程約800 m。圖6中A點(diǎn)為起點(diǎn)和終點(diǎn),起點(diǎn)與終點(diǎn)形成閉合,此環(huán)境可以很好檢驗(yàn)算法對(duì)于回環(huán)回路的性能。為驗(yàn)證算法的有效性,根據(jù)上述的地圖構(gòu)建方案,本文對(duì)真實(shí)的環(huán)境進(jìn)行驗(yàn)證,并對(duì)采集數(shù)據(jù)結(jié)果進(jìn)行了分析。為了保證本文實(shí)驗(yàn)的可對(duì)比性,在同一場(chǎng)景用傳統(tǒng)NDT算法和文獻(xiàn)[20]方法進(jìn)行實(shí)驗(yàn),并與本文算法進(jìn)行對(duì)比分析。

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

采集數(shù)據(jù)時(shí),車輛行駛速度約為10 km/h,整個(gè)場(chǎng)景包含3507幀,路面較平坦。將對(duì)于激光雷達(dá)向前方向定為X軸,垂直激光雷達(dá)方向定為Y軸,垂直地面方向定義為Z軸。SLAM建圖時(shí),以起點(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)場(chǎng)景

傳統(tǒng)NDT建圖結(jié)果，采用的網(wǎng)格為不重疊網(wǎng)格,網(wǎng)格邊長(zhǎ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建圖場(chǎng)景俯視圖,圖8為側(cè)面圖和回環(huán)位置對(duì)比圖。

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

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

本文算法建圖結(jié)果，在根據(jù)傳統(tǒng)方法進(jìn)行三維NDT點(diǎn)云配準(zhǔn)的同時(shí),采用本文所提方法進(jìn)行實(shí)時(shí)回環(huán)檢測(cè)。在進(jìn)行NDT配準(zhǔn)時(shí),采用的點(diǎn)云網(wǎng)格邊長(zhǎng)為2,最大步長(zhǎng)設(shè)置為0.1,最大迭代次數(shù)設(shè)置為30,最小轉(zhuǎn)換差異設(shè)置為0.01。在進(jìn)行粗回環(huán)檢測(cè)時(shí),網(wǎng)格分類閾值te設(shè)置為e-5,L1范數(shù)設(shè)置為0.3。進(jìn)行精確回環(huán)檢測(cè)時(shí),兩幀距原點(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)行對(duì)比,如圖11所示。圖12將回環(huán)處誤差放大,原點(diǎn)處為回環(huán)處。

圖11 三種算法與GPS軌跡對(duì)比圖

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

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

圖13 距離誤差對(duì)比圖

圖14 X方向距離誤差對(duì)比圖

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

圖15 Y方向距離誤差對(duì)比圖

圖16 各幀配準(zhǔn)用時(shí)對(duì)比圖

表1 與GPS軌跡誤差對(duì)比

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

表2 建圖時(shí)長(zhǎng)與回環(huán)檢測(cè)用時(shí)對(duì)比

4 結(jié) 論

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