無人平臺越野環境下同步定位與地圖創建

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

(1.北京理工大學 機械與車輛學院，北京 100081；2.中國北方車輛研究所，北京 100072；3.北京特種車輛研究所，北京 100072)

0 引言

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

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

1 系統框架

圖1 實時6自由度激光雷達SLAM系統框架Fig.1 6-DoF lidar SLAM system framework

本文旨在基于無人平臺實現一種適用于大尺度越野場景的實時SLAM系統。如圖1所示，該SLAM系統框架分為兩部分：三維地圖構建以及6自由度全局姿態優化。來自IMU的線性加速度、角速度等信息以及來自兩個32線激光雷達的點云共同作為該SLAM系統的輸入。

為了實時進行地圖更新，全局地圖根據拓撲理論被分割成許多子地圖，每個子地圖都被認為是全局拓撲結構中的1個節點。實際上，子地圖關聯性形成了拓撲圖中的邊，最終獲得一個完整的、結合了體素網格圖和拓撲圖優點的全局三維混合地圖。為了保證地圖切換時的平順性，本系統同時維護具有一半重疊區域的兩個子地圖，并在倒數第2幀子地圖基礎上進行可通行區域提取，最終獲得如圖2所示可用于局部路徑規劃的可通行區域，圖2中黑色區域代表未知區域，綠色區域代表可通行區域，白色區域代表障礙物區域，紅色區域代表懸掛障礙區域，黃色圓點代表當前車輛位置，黃色矩形中心代表地圖原點所在位置。

圖2 可通行區域Fig.2 Transitable-area map

采用一種基于圖論的SPA[3]優化方法處理SLAM中的6自由度全局姿態優化問題。稀疏位姿圖將激光雷達位姿和子地圖位姿作為待優化的位姿節點，位姿節點間的約束被分為三類：子地圖間位姿約束、子地圖內位姿約束以及動力學約束。最后采用Ceres求解器來計算優化函數。

2 三維地圖創建

2.1 地圖初始化

(1)

2.2 地圖更新

圖3 子地圖結構示意圖Fig.3 Schematic diagram of submap structure

地圖更新是一個反復將點云對齊到子地圖坐標系下的迭代過程。當兩幀相鄰點云之間超過某一運動學閾值時，將雷達坐標系中的第1幀點云通過轉換矩陣轉換到子地圖坐標系。子地圖模型為三維概率體素地圖，如圖3所示。整個子地圖的結構可以分為動態擴展層、中間數據層和體素層3層。動態擴展層中的元節點通過鏈結構進行管理，元節點數量隨著環境觀測的擴展而增加；元節點中的中間數據層節點和體素采用樹形結構進行組織，每個元節點由512個中間數據層節點組成，每個中間數據層節點由512個體素組成。通過這種方式，鏈式結構高索引效率和樹形結構高修改效率的優點被結合在一起。元節點的數量隨著環境觀測的擴展而增長。例如，當子地圖的分辨率r即體素邊長為0.1 m時，1個元節點可以表示6.4 m×6.4 m×6.4 m范圍的環境。

2.3 可通行區域提取

(2)

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

3 全局位姿優化

由于激光雷達里程計的積累誤差，全局范圍內的6自由度姿態將產生漂移，本文采用SPA方法進行全局位姿優化，從而減少累積誤差。

3.1 閉環檢測

閉環檢測是消除累積誤差的有效方法，識別觀測過的歷史場景是閉環檢測的核心。在利用當前點云位姿與子地圖原始位姿間的歐氏距離選取待檢測子地圖后[20]，本文將旋轉直方圖匹配(RHM)算法與迭代最近鄰匹配(ICP)算法相結合，實現了一種RHM-ICP算法來實現實時的閉環檢測。ICP算法用來計算當前幀點云與歷史幀子地圖之間的相對位姿ξ★，

(3)

總之，在當前全面從嚴治黨背景下，為了適應國家治理體系和治理能力現代化這一改革目標，作為黨內監督的重要形式，在立足國情的基礎上，紀檢監察派駐制度改革也要順應時代要求，實現組織理論、組織形式、組織人員、組織管理等方面的現代化，從而真正發揮“派”的權威，“駐”的優勢。

圖4 旋轉直方圖匹配示意圖Fig.4 Matching graph of rotation histogram

(4)

(5)

3.2 全局位姿優化

(6)

式中：ρ為權重系數；E為殘差；Σij為協方差矩陣，由Ceres計算得到；約束ξij包含子地圖間位姿約束和子地圖內位姿約束。

對于動力學約束，本文計算如下問題：

(7)

(8)

4 實驗結果

本文在真實越野環境下進行了大量試驗。圖5所示為某實驗場景(見圖5(a))以及本文使用的實驗平臺(見圖5(b))，圖5中紅色曲線根據實時動態差分全球定位系統(RTK-GPS)獲得的數據繪制獲得。在該實驗路線中包括顛簸道路、匝道、岔路、混凝土路面和橋梁等多種地形。2臺美國Velodyne公司生產的32線激光雷達安裝在平臺頂部兩側，以獲得與實驗場景相對應的點云，一個IMU被安裝在平臺的質心位置，用以實時測量無人平臺的線性加速度和角速度。此外，還安裝一部RTK-GPS用以獲取平臺的精確位置，并采用另一個高精度慣性導航系統(INS)獲取參考姿態，以評估姿態優化性能。

圖5 實驗場景和平臺Fig.5 Experimental scene and platform

4.1 三維地圖創建及可通行區域提取

無人平臺以20 km/h的平均速度運動，在三維地圖創建過程中重建三維環境。圖6所示為某越野實驗環境中顛簸路面環境下該算法對于墻體、岔路口、橋、懸空障礙等進行三維地圖創建以及可通行區域提取良好的結果。圖6(c)、圖6(f)、圖6(i)中，黑色區域為未知區域，綠色區域為可通行區域，白色區域為障礙物區域，紅色區域為懸空障礙區域，黃色圓點為當前車輛位置，黃色矩形框中心為可通行區域原點，藍色網格為標尺線，網格的邊長為10 m.圖6(f)顯示了對于樹良好的懸空檢測效果，正是由于懸空檢測使得平臺不會將樹冠區域視為不可通行，從而保障了平臺可以順利通過此岔路口。

圖6 天然地形、地圖創建結果以及可通行區域提取結果Fig.6 Natural terrains,mapping results and transitable-area extraction results

圖7 地圖創建及更新系統運行周期統計圖Fig.7 Operational cycle of mapping and system updating

為了測量地圖創建及更新系統的實時性，本文統計了從接收到點云數據到地圖結果輸出所用的時間，如圖7所示。算法平均運行周期為(0.093 4±0.029 3)s，更新頻率約為10 Hz，按最高行駛速度36 km/h計算，在一個運行周期內平臺可運動(0.934±0.293)m，可基本滿足無人平臺日常行駛的需求。

4.2 全局位姿優化

本文在圖5所示實際大尺度自然地形中評估了全局位姿優化的性能。圖8所示為位姿優化結果中的位置優化結果。

圖8 位置優化結果圖Fig.8 Optimized results of position

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

本文也對姿態優化性能進行了評估，結果如圖9所示。從圖9中可以看出，激光雷達里程計的姿態估計結果在局部范圍內是準確的，但累積誤差隨著時間的推移而增加。當無人平臺返回至起始位置時，累積航向角誤差為9.0°，俯仰角誤差為0.7°，側傾角誤差為2.2°.優化后的姿態與高精度INS測量結果幾乎一致，不過在時間上稍有延遲。

為了評價閉環檢測算法的實時性，本文將開源SLAM算法Cartographer以及基于NDT檢測算法的閉環檢測時間進行了對比，結果如表1所示。

從表1中可以看出，RHM-ICP閉環檢測的時間最短，基本在0.4 s左右。由于NDT算法在閉環檢測時需要迭代統計當前點云分布并計算概率，而RHM-ICP主要使用建圖生成的概率柵格，無需另外計算概率，因此NDT算法實時性稍差；Cartographer算法實時性較差，這是因為其在進行閉環檢測時沒有對待檢測的子地圖進行篩選，導致待匹配的子地圖隨著平臺運動不斷增加，且由于實時性不夠、容易造成計算量累積，從而進一步降低了閉環檢測的實時性。

圖9 姿態對比圖Fig.9 Attitude contrast graph

表1 閉環檢測時間對比表Tab.1 Closed loop detection time table s

5 結論

本文提出了一種適用于大尺度越野環境的無人平臺實時SLAM系統。通過三維地圖創建獲取了豐富的真實環境信息，在此基礎上提取了可服務于無人平臺路徑規劃的可通行區域；采用基于BBS的RHM-ICP算法實現了三維越野環境下實時的閉環檢測；利用IMU添加動力學約束，通過SPA算法對6自由度全局位姿進行了優化。通過實驗證明了三維地圖創建和可通行區域提取良好的精確性和實時性，全局位姿優化使無人平臺在GPS信號較弱甚至丟失時，可以獲得精確的位姿信息。