劉忠澤，陳慧巖，崔星，熊光明，王羽純，陶溢

(1.北京理工大學(xué) 機(jī)械與車(chē)輛學(xué)院，北京 100081；2.中國(guó)北方車(chē)輛研究所，北京 100072；3.北京特種車(chē)輛研究所，北京 100072)

0 引言

同步定位與地圖創(chuàng)建(SLAM)在無(wú)人平臺(tái)(UGV)領(lǐng)域受到了越來(lái)越多的關(guān)注?；谝曈X(jué)的6自由度SLAM[1-2]的性能很容易受到照明和天氣變化的影響。因此，一些基于激光雷達(dá)的SLAM系統(tǒng)相繼出現(xiàn)[3-5]。Alismail等[6]、Besl等[7]將迭代最近鄰匹配(ICP)算法加以擴(kuò)展，從而獲得一種適用于考慮樣本間姿態(tài)誤差的連續(xù)軌跡估計(jì)SLAM方法。Nüchter等[8]提出一種基于三維激光雷達(dá)的室外SLAM系統(tǒng)，該算法需要為閉環(huán)檢測(cè)提供一個(gè)初始啟發(fā)值。經(jīng)典的三維地圖模型包括點(diǎn)云地圖、點(diǎn)面地圖、八叉樹(shù)地圖以及體素柵格地圖。其中點(diǎn)云地圖可以非常精細(xì)地描述三維環(huán)境[8-10]，但無(wú)法及時(shí)處理地圖數(shù)據(jù)以表達(dá)環(huán)境中的變化；點(diǎn)面地圖在表面信息豐富的場(chǎng)景中表現(xiàn)良好[11]，但越野環(huán)境中的表面信息非常少。Douillard等[12]將體素地圖與粗略的高程地圖結(jié)合起來(lái)處理三維分割問(wèn)題，但沒(méi)有良好的組織數(shù)據(jù)。Wurm等提出了一種新穎的多分辨率方法，基于八叉樹(shù)來(lái)高效地創(chuàng)建三維概率地圖[13]，該方法參考了八叉樹(shù)地圖[14]算法。鄒斌等[15]利用聚類算法，基于16線激光雷達(dá)，應(yīng)用聚類算法提取了可通行區(qū)域，但是該算法對(duì)噪聲比較敏感。主流的全局位姿優(yōu)化方法包括基于粒子濾波方法、擴(kuò)展卡爾曼濾波器(EKF)方法以及基于圖論的SLAM.Grisetti等[16]使用粒子濾波方法解決SLAM問(wèn)題，但基于粒子濾波的SLAM必須在每個(gè)粒子中表示整個(gè)系統(tǒng)狀態(tài)，不適合處理大尺度越野場(chǎng)景中的SLAM問(wèn)題?；贓KF的SLAM[17]強(qiáng)烈地依賴于無(wú)人平臺(tái)的運(yùn)動(dòng)假設(shè)以及傳感器噪聲假設(shè)，對(duì)于自然環(huán)境很難實(shí)現(xiàn)；基于EKF的SLAM還存在著非線性誤差以及計(jì)算量不斷增大的問(wèn)題。郭劍輝等[18]采用尺度無(wú)跡變換(SUT)改進(jìn)了基于EKF的SLAM系統(tǒng)，消除了3階以下的非線性誤差，但仍存在著計(jì)算量不斷增大的問(wèn)題。基于圖論的SLAM方法大多以一組代表無(wú)人平臺(tái)位姿和全局特征點(diǎn)的節(jié)點(diǎn)以及連接節(jié)點(diǎn)的約束作為處理對(duì)象，Kümmerle等[19]基于圖論提出了一個(gè)g2o優(yōu)化框架。林俊欽等[20]基于深度相機(jī)(RGBD)，利用正態(tài)分布變換算法(NDT)提出了一種混合閉環(huán)檢測(cè)方法，但三維激光雷達(dá)點(diǎn)云比較稀疏，在小尺度柵格內(nèi)提取的NDT特征誤差較大。由于環(huán)境數(shù)據(jù)大規(guī)模性及場(chǎng)景復(fù)雜性，SLAM方法很少被設(shè)計(jì)用于越野環(huán)境。

本文提出一個(gè)由2個(gè)三維激光雷達(dá)和1個(gè)慣性測(cè)量單元(IMU)作為傳感器的6自由度激光雷達(dá)SLAM系統(tǒng)，該系統(tǒng)能夠滿足大尺度越野環(huán)境中無(wú)人平臺(tái)實(shí)時(shí)自動(dòng)駕駛的需求。2個(gè)三維激光雷達(dá)可以從一次觀測(cè)中獲得更多的環(huán)境信息，存在的盲區(qū)較小，且雷達(dá)點(diǎn)云的運(yùn)算效率比視覺(jué)也要高。該系統(tǒng)分為三維地圖創(chuàng)建以及6自由度全局姿態(tài)優(yōu)化兩個(gè)子系統(tǒng)。在地圖創(chuàng)建部分，構(gòu)建了一張結(jié)合三維柵格地圖和拓?fù)涞貓D優(yōu)點(diǎn)的全局混合地圖，用以越野環(huán)境實(shí)時(shí)提取可通行區(qū)域。在處理6自由度全局位姿優(yōu)化問(wèn)題時(shí)，采用稀疏位姿調(diào)整[3](SPA)算法來(lái)消除激光雷達(dá)里程計(jì)的累積誤差，并提出一種基于分支限界(BBS)法的旋轉(zhuǎn)直方圖最近鄰匹配(RHM-ICP)實(shí)時(shí)閉環(huán)檢測(cè)方法，通過(guò)閉環(huán)檢測(cè)獲得子地圖間約束。

1 系統(tǒng)框架

圖1 實(shí)時(shí)6自由度激光雷達(dá)SLAM系統(tǒng)框架Fig.1 6-DoF lidar SLAM system framework

本文旨在基于無(wú)人平臺(tái)實(shí)現(xiàn)一種適用于大尺度越野場(chǎng)景的實(shí)時(shí)SLAM系統(tǒng)。如圖1所示，該SLAM系統(tǒng)框架分為兩部分：三維地圖構(gòu)建以及6自由度全局姿態(tài)優(yōu)化。來(lái)自IMU的線性加速度、角速度等信息以及來(lái)自兩個(gè)32線激光雷達(dá)的點(diǎn)云共同作為該SLAM系統(tǒng)的輸入。

為了實(shí)時(shí)進(jìn)行地圖更新，全局地圖根據(jù)拓?fù)淅碚摫环指畛稍S多子地圖，每個(gè)子地圖都被認(rèn)為是全局拓?fù)浣Y(jié)構(gòu)中的1個(gè)節(jié)點(diǎn)。實(shí)際上，子地圖關(guān)聯(lián)性形成了拓?fù)鋱D中的邊，最終獲得一個(gè)完整的、結(jié)合了體素網(wǎng)格圖和拓?fù)鋱D優(yōu)點(diǎn)的全局三維混合地圖。為了保證地圖切換時(shí)的平順性，本系統(tǒng)同時(shí)維護(hù)具有一半重疊區(qū)域的兩個(gè)子地圖，并在倒數(shù)第2幀子地圖基礎(chǔ)上進(jìn)行可通行區(qū)域提取，最終獲得如圖2所示可用于局部路徑規(guī)劃的可通行區(qū)域，圖2中黑色區(qū)域代表未知區(qū)域，綠色區(qū)域代表可通行區(qū)域，白色區(qū)域代表障礙物區(qū)域，紅色區(qū)域代表懸掛障礙區(qū)域，黃色圓點(diǎn)代表當(dāng)前車(chē)輛位置，黃色矩形中心代表地圖原點(diǎn)所在位置。

圖2 可通行區(qū)域Fig.2 Transitable-area map

采用一種基于圖論的SPA[3]優(yōu)化方法處理SLAM中的6自由度全局姿態(tài)優(yōu)化問(wèn)題。稀疏位姿圖將激光雷達(dá)位姿和子地圖位姿作為待優(yōu)化的位姿節(jié)點(diǎn)，位姿節(jié)點(diǎn)間的約束被分為三類：子地圖間位姿約束、子地圖內(nèi)位姿約束以及動(dòng)力學(xué)約束。最后采用Ceres求解器來(lái)計(jì)算優(yōu)化函數(shù)。

2 三維地圖創(chuàng)建

2.1 地圖初始化

(1)

2.2 地圖更新

圖3 子地圖結(jié)構(gòu)示意圖Fig.3 Schematic diagram of submap structure

地圖更新是一個(gè)反復(fù)將點(diǎn)云對(duì)齊到子地圖坐標(biāo)系下的迭代過(guò)程。當(dāng)兩幀相鄰點(diǎn)云之間超過(guò)某一運(yùn)動(dòng)學(xué)閾值時(shí)，將雷達(dá)坐標(biāo)系中的第1幀點(diǎn)云通過(guò)轉(zhuǎn)換矩陣轉(zhuǎn)換到子地圖坐標(biāo)系。子地圖模型為三維概率體素地圖，如圖3所示。整個(gè)子地圖的結(jié)構(gòu)可以分為動(dòng)態(tài)擴(kuò)展層、中間數(shù)據(jù)層和體素層3層。動(dòng)態(tài)擴(kuò)展層中的元節(jié)點(diǎn)通過(guò)鏈結(jié)構(gòu)進(jìn)行管理，元節(jié)點(diǎn)數(shù)量隨著環(huán)境觀測(cè)的擴(kuò)展而增加；元節(jié)點(diǎn)中的中間數(shù)據(jù)層節(jié)點(diǎn)和體素采用樹(shù)形結(jié)構(gòu)進(jìn)行組織，每個(gè)元節(jié)點(diǎn)由512個(gè)中間數(shù)據(jù)層節(jié)點(diǎn)組成，每個(gè)中間數(shù)據(jù)層節(jié)點(diǎn)由512個(gè)體素組成。通過(guò)這種方式，鏈?zhǔn)浇Y(jié)構(gòu)高索引效率和樹(shù)形結(jié)構(gòu)高修改效率的優(yōu)點(diǎn)被結(jié)合在一起。元節(jié)點(diǎn)的數(shù)量隨著環(huán)境觀測(cè)的擴(kuò)展而增長(zhǎng)。例如，當(dāng)子地圖的分辨率r即體素邊長(zhǎng)為0.1 m時(shí)，1個(gè)元節(jié)點(diǎn)可以表示6.4 m×6.4 m×6.4 m范圍的環(huán)境。

2.3 可通行區(qū)域提取

(2)

式中：Zt為最大高度差閾值；ρt為體素密度閾值。

3 全局位姿優(yōu)化

由于激光雷達(dá)里程計(jì)的積累誤差，全局范圍內(nèi)的6自由度姿態(tài)將產(chǎn)生漂移，本文采用SPA方法進(jìn)行全局位姿優(yōu)化，從而減少累積誤差。

3.1 閉環(huán)檢測(cè)

閉環(huán)檢測(cè)是消除累積誤差的有效方法，識(shí)別觀測(cè)過(guò)的歷史場(chǎng)景是閉環(huán)檢測(cè)的核心。在利用當(dāng)前點(diǎn)云位姿與子地圖原始位姿間的歐氏距離選取待檢測(cè)子地圖后[20]，本文將旋轉(zhuǎn)直方圖匹配(RHM)算法與迭代最近鄰匹配(ICP)算法相結(jié)合，實(shí)現(xiàn)了一種RHM-ICP算法來(lái)實(shí)現(xiàn)實(shí)時(shí)的閉環(huán)檢測(cè)。ICP算法用來(lái)計(jì)算當(dāng)前幀點(diǎn)云與歷史幀子地圖之間的相對(duì)位姿ξ★，

(3)

總之，在當(dāng)前全面從嚴(yán)治黨背景下，為了適應(yīng)國(guó)家治理體系和治理能力現(xiàn)代化這一改革目標(biāo)，作為黨內(nèi)監(jiān)督的重要形式，在立足國(guó)情的基礎(chǔ)上，紀(jì)檢監(jiān)察派駐制度改革也要順應(yīng)時(shí)代要求，實(shí)現(xiàn)組織理論、組織形式、組織人員、組織管理等方面的現(xiàn)代化，從而真正發(fā)揮“派”的權(quán)威，“駐”的優(yōu)勢(shì)。

圖4 旋轉(zhuǎn)直方圖匹配示意圖Fig.4 Matching graph of rotation histogram

(4)

(5)

3.2 全局位姿優(yōu)化

(6)

式中：ρ為權(quán)重系數(shù)；E為殘差；Σij為協(xié)方差矩陣，由Ceres計(jì)算得到；約束ξij包含子地圖間位姿約束和子地圖內(nèi)位姿約束。

對(duì)于動(dòng)力學(xué)約束，本文計(jì)算如下問(wèn)題：

(7)

(8)

4 實(shí)驗(yàn)結(jié)果

本文在真實(shí)越野環(huán)境下進(jìn)行了大量試驗(yàn)。圖5所示為某實(shí)驗(yàn)場(chǎng)景(見(jiàn)圖5(a))以及本文使用的實(shí)驗(yàn)平臺(tái)(見(jiàn)圖5(b))，圖5中紅色曲線根據(jù)實(shí)時(shí)動(dòng)態(tài)差分全球定位系統(tǒng)(RTK-GPS)獲得的數(shù)據(jù)繪制獲得。在該實(shí)驗(yàn)路線中包括顛簸道路、匝道、岔路、混凝土路面和橋梁等多種地形。2臺(tái)美國(guó)Velodyne公司生產(chǎn)的32線激光雷達(dá)安裝在平臺(tái)頂部?jī)蓚?cè)，以獲得與實(shí)驗(yàn)場(chǎng)景相對(duì)應(yīng)的點(diǎn)云，一個(gè)IMU被安裝在平臺(tái)的質(zhì)心位置，用以實(shí)時(shí)測(cè)量無(wú)人平臺(tái)的線性加速度和角速度。此外，還安裝一部RTK-GPS用以獲取平臺(tái)的精確位置，并采用另一個(gè)高精度慣性導(dǎo)航系統(tǒng)(INS)獲取參考姿態(tài)，以評(píng)估姿態(tài)優(yōu)化性能。

圖5 實(shí)驗(yàn)場(chǎng)景和平臺(tái)Fig.5 Experimental scene and platform

4.1 三維地圖創(chuàng)建及可通行區(qū)域提取

無(wú)人平臺(tái)以20 km/h的平均速度運(yùn)動(dòng)，在三維地圖創(chuàng)建過(guò)程中重建三維環(huán)境。圖6所示為某越野實(shí)驗(yàn)環(huán)境中顛簸路面環(huán)境下該算法對(duì)于墻體、岔路口、橋、懸空障礙等進(jìn)行三維地圖創(chuàng)建以及可通行區(qū)域提取良好的結(jié)果。圖6(c)、圖6(f)、圖6(i)中，黑色區(qū)域?yàn)槲粗獏^(qū)域，綠色區(qū)域?yàn)榭赏ㄐ袇^(qū)域，白色區(qū)域?yàn)檎系K物區(qū)域，紅色區(qū)域?yàn)閼铱照系K區(qū)域，黃色圓點(diǎn)為當(dāng)前車(chē)輛位置，黃色矩形框中心為可通行區(qū)域原點(diǎn)，藍(lán)色網(wǎng)格為標(biāo)尺線，網(wǎng)格的邊長(zhǎng)為10 m.圖6(f)顯示了對(duì)于樹(shù)良好的懸空檢測(cè)效果，正是由于懸空檢測(cè)使得平臺(tái)不會(huì)將樹(shù)冠區(qū)域視為不可通行，從而保障了平臺(tái)可以順利通過(guò)此岔路口。

圖6 天然地形、地圖創(chuàng)建結(jié)果以及可通行區(qū)域提取結(jié)果Fig.6 Natural terrains,mapping results and transitable-area extraction results

圖7 地圖創(chuàng)建及更新系統(tǒng)運(yùn)行周期統(tǒng)計(jì)圖Fig.7 Operational cycle of mapping and system updating

為了測(cè)量地圖創(chuàng)建及更新系統(tǒng)的實(shí)時(shí)性，本文統(tǒng)計(jì)了從接收到點(diǎn)云數(shù)據(jù)到地圖結(jié)果輸出所用的時(shí)間，如圖7所示。算法平均運(yùn)行周期為(0.093 4±0.029 3)s，更新頻率約為10 Hz，按最高行駛速度36 km/h計(jì)算，在一個(gè)運(yùn)行周期內(nèi)平臺(tái)可運(yùn)動(dòng)(0.934±0.293)m，可基本滿足無(wú)人平臺(tái)日常行駛的需求。

4.2 全局位姿優(yōu)化

本文在圖5所示實(shí)際大尺度自然地形中評(píng)估了全局位姿優(yōu)化的性能。圖8所示為位姿優(yōu)化結(jié)果中的位置優(yōu)化結(jié)果。

圖8 位置優(yōu)化結(jié)果圖Fig.8 Optimized results of position

在某一1.5 km長(zhǎng)環(huán)形路線上，當(dāng)無(wú)人平臺(tái)再次返回到起始位置時(shí)，累積的Oxy平面上誤差達(dá)到21.5 m，并且z軸方向的累積誤差在沒(méi)有優(yōu)化情況下達(dá)到9.5 m.在成功檢測(cè)到閉環(huán)后，Oxy平面以及z軸方向的誤差都被優(yōu)化在1 m以內(nèi)。從圖8(a)中可以看到，在(25 m，250 m)處優(yōu)化后的位置距離參考位置更遠(yuǎn)，這是全局位姿優(yōu)化算法為了實(shí)現(xiàn)總體位姿誤差最小而對(duì)局部位置精度的犧牲。

本文也對(duì)姿態(tài)優(yōu)化性能進(jìn)行了評(píng)估，結(jié)果如圖9所示。從圖9中可以看出，激光雷達(dá)里程計(jì)的姿態(tài)估計(jì)結(jié)果在局部范圍內(nèi)是準(zhǔn)確的，但累積誤差隨著時(shí)間的推移而增加。當(dāng)無(wú)人平臺(tái)返回至起始位置時(shí)，累積航向角誤差為9.0°，俯仰角誤差為0.7°，側(cè)傾角誤差為2.2°.優(yōu)化后的姿態(tài)與高精度INS測(cè)量結(jié)果幾乎一致，不過(guò)在時(shí)間上稍有延遲。

為了評(píng)價(jià)閉環(huán)檢測(cè)算法的實(shí)時(shí)性，本文將開(kāi)源SLAM算法Cartographer以及基于NDT檢測(cè)算法的閉環(huán)檢測(cè)時(shí)間進(jìn)行了對(duì)比，結(jié)果如表1所示。

從表1中可以看出，RHM-ICP閉環(huán)檢測(cè)的時(shí)間最短，基本在0.4 s左右。由于NDT算法在閉環(huán)檢測(cè)時(shí)需要迭代統(tǒng)計(jì)當(dāng)前點(diǎn)云分布并計(jì)算概率，而RHM-ICP主要使用建圖生成的概率柵格，無(wú)需另外計(jì)算概率，因此NDT算法實(shí)時(shí)性稍差；Cartographer算法實(shí)時(shí)性較差，這是因?yàn)槠湓谶M(jìn)行閉環(huán)檢測(cè)時(shí)沒(méi)有對(duì)待檢測(cè)的子地圖進(jìn)行篩選，導(dǎo)致待匹配的子地圖隨著平臺(tái)運(yùn)動(dòng)不斷增加，且由于實(shí)時(shí)性不夠、容易造成計(jì)算量累積，從而進(jìn)一步降低了閉環(huán)檢測(cè)的實(shí)時(shí)性。

圖9 姿態(tài)對(duì)比圖Fig.9 Attitude contrast graph

表1 閉環(huán)檢測(cè)時(shí)間對(duì)比表Tab.1 Closed loop detection time table s

5 結(jié)論

本文提出了一種適用于大尺度越野環(huán)境的無(wú)人平臺(tái)實(shí)時(shí)SLAM系統(tǒng)。通過(guò)三維地圖創(chuàng)建獲取了豐富的真實(shí)環(huán)境信息，在此基礎(chǔ)上提取了可服務(wù)于無(wú)人平臺(tái)路徑規(guī)劃的可通行區(qū)域；采用基于BBS的RHM-ICP算法實(shí)現(xiàn)了三維越野環(huán)境下實(shí)時(shí)的閉環(huán)檢測(cè)；利用IMU添加動(dòng)力學(xué)約束，通過(guò)SPA算法對(duì)6自由度全局位姿進(jìn)行了優(yōu)化。通過(guò)實(shí)驗(yàn)證明了三維地圖創(chuàng)建和可通行區(qū)域提取良好的精確性和實(shí)時(shí)性，全局位姿優(yōu)化使無(wú)人平臺(tái)在GPS信號(hào)較弱甚至丟失時(shí)，可以獲得精確的位姿信息。