李振拯, 丁恩杰, 王戈琛

(1.中國礦業(yè)大學(xué) 物聯(lián)網(wǎng)(感知礦山)研究中心,江蘇 徐州 221008； 2.中國礦業(yè)大學(xué) 信息與控制工程學(xué)院,江蘇 徐州 221008)

0 引 言

近年來,隨著人工智能、計算機視覺、傳感器等技術(shù)的飛速發(fā)展,自主化無人設(shè)備受到了科學(xué)界和工業(yè)界的廣泛關(guān)注。定位和建圖[1～3]是無人車自主導(dǎo)航的兩個關(guān)鍵問題,因此,同時定位與建圖(simultaneous localization and mapping,SLAM)[4]作為環(huán)境感知和運動導(dǎo)航的基礎(chǔ),已成為一個研究熱點。SLAM是指在未知的位置和環(huán)境中,利用其傳感器逐步構(gòu)建與環(huán)境一致的地圖,并在地圖中定位。SLAM一般分為基于視覺傳感器和基于激光雷達(LiDAR)傳感器兩大類?；谝曈X傳感器[5,6]的方法實現(xiàn)了實時定位和建圖,但由于視覺傳感器對光照和視角變化的敏感性,視覺SLAM算法[7]并不可靠。隨著LiDAR傳感器價格的不斷降低,LiDAR SLAM[8]以其計算量小、應(yīng)用環(huán)境廣等優(yōu)點越來越受到人們的重視。

許多研究人員已經(jīng)使用3D LiDAR傳感器[9]完成了高精度定位和建圖。LOAM[10]是一個采用LiDAR作為傳感器的SLAM算法,它將SLAM分為兩個線程,即里程計和建圖。里程計線程使用特征提取和特征配準以獲得粗定位；建圖線程采用掃描與地圖配準策略,解決了高漂移和高計算量的問題。LeGO-LOAM[11]則在LOAM的基礎(chǔ)上進行了改進,通過分割和優(yōu)化地面點來降低計算成本,并通過兩步優(yōu)化策略實現(xiàn)精確定位，完成了LOAM的輕量級轉(zhuǎn)換,并成功地將其部署在低功耗嵌入式系統(tǒng)中。由于單一的傳感器容易跟蹤失效,因此LIO-SAM[12]則在LeGO-LOAM的基礎(chǔ)上,通過因子圖融合LiDAR和慣性測量單元(LiDAR-IMU)的測量,來實現(xiàn)精確的狀態(tài)估計,并且提高了系統(tǒng)的魯棒性。有研究者[13]利用區(qū)塊的多特征信息對SLAM算法的匹配策略進行改進,并對多特征信息的加權(quán)系數(shù)進行自適應(yīng)調(diào)整,保證了匹配精度。復(fù)雜的環(huán)境因素導(dǎo)致LiDAR傳感器測量數(shù)據(jù)失真,從而引起數(shù)據(jù)關(guān)聯(lián)錯誤,這給SLAM算法的精度帶來了挑戰(zhàn)。本文提出了一種LiDAR-IMU傳感器松耦合的SLAM方法,并通過實驗結(jié)果證明本文方法的精度比單一傳感器和其他松耦合方法更高。

1 算法流程

算法流程如圖1所示。IMU測量用于狀態(tài)預(yù)測和預(yù)積分。使用預(yù)測的狀態(tài)對LiDAR測量進行去畸變，隨后從去畸變的LiDAR測量中提取特征，局部地圖中查找特征匹配構(gòu)建代價函數(shù)，通過最小化代價函數(shù)計算激光里程計，選擇關(guān)鍵幀特征構(gòu)成基于關(guān)鍵幀的局部地圖,而其他幀則構(gòu)成基于普通幀的局部地圖，關(guān)鍵幀進一步用于回環(huán)檢測。最后在非線性優(yōu)化階段通過最小化關(guān)鍵幀約束、IMU預(yù)積分約束和回環(huán)檢測約束的代價函數(shù),得到了最終的定位和建圖結(jié)果。

圖1 基于LiDAR—IMU松耦合的SLAM系統(tǒng)框架

2 本文算法模型

2.1 IMU狀態(tài)預(yù)測與預(yù)積分

本文采用[14]中提出的IMU預(yù)積分方法來獲得IMU測量約束

(1)

本文將IMU的測量轉(zhuǎn)換到LiDAR參考系下。此處兩參考系的幾何變換通過LiDAR與IMU標定獲取。為了避免符號濫用,本文忽略了一些符號的變化。為了獲得狀態(tài)預(yù)測,本文使用中值法對IMU的測量進行數(shù)值積分

2g)Δt2]

(2)

相對運動以及當(dāng)前幀的預(yù)測位姿由式(3)表示

(3)

式中TWBi為i時刻的位姿。

2.2 點云去畸變與特征提取

特征提取是在去畸變的LiDAR點云上執(zhí)行的。如文獻[10]中所述,定義c值來評估局部區(qū)域上點的粗糙度

(4)

2.3 局部地圖與激光里程計

使用每個LiDAR掃描幀為后端非線性優(yōu)化添加約束是十分消耗計算資源的,而僅采用關(guān)鍵幀拋棄其他幀(普通幀)則會造成信息損失。因此,本文提出了基于關(guān)鍵幀和基于普通幀的局部地圖匹配方法,避免了信息損失同時維持了后端優(yōu)化的低計算資源需求。

局部地圖管理和特征匹配如圖2所示。其中基于關(guān)鍵幀的局部地圖由關(guān)鍵幀的特征所構(gòu)建,基于普通幀的局部地圖由普通幀的特征所構(gòu)建。對于當(dāng)前幀的特征,首先在基于關(guān)鍵幀的局部地圖中查找匹配,若匹配失敗則進一步在基于普通幀的局部地圖中查找匹配。使用特征到匹配的距離構(gòu)建代價函數(shù)

(5)

圖2 局部地圖管理與特征匹配

為避免環(huán)境因素干擾引起的特征匹配錯誤等,本文使用M估計以減少錯誤的特征匹配對與狀態(tài)估計的影響。對兩個局部地圖的特征匹配代價函數(shù)分別采用Cauchy魯棒函數(shù)和Geman-McClure魯棒函數(shù)處理

C(x)=

(6)

式中x為上一次迭代的當(dāng)前幀位姿值。

最后,通過迭代加權(quán)最小二乘法來求解代價函數(shù)C(x)最小化問題計算激光里程計,從而得到當(dāng)前幀的位姿。

2.4 回環(huán)檢測

許多相關(guān)工作使用基于歐氏距離的回環(huán)檢測方法,如LeGO-LOAM[11]和LIO-SAM[12]?；丨h(huán)的目的是消除SLAM的累積誤差。但當(dāng)誤差積累過大時,基于歐氏距離的回環(huán)檢測會失效。因此,本文使用Scan-Context[16]來檢測回環(huán)。

本文首先在LiDAR坐標系中以等間距方式為點云劃分為Nr×Ns個子區(qū)域。點云的Scan-Context描述子表示為矩陣形式

(7)

式中P**為子區(qū)域,φ(P**)為子區(qū)域內(nèi)高度最高的點的z軸坐標值。

給定一個查詢點云Scan-Context描述子Iq和一個候選點云Scan-Context描述子Ic,計算兩個描述子的距離來度量兩者的相似性

(8)

若查詢點云和候選點云描述子的距離小于閾值,則進一步執(zhí)行ICP點云匹配來驗證。如果查詢點云和候選點云執(zhí)行ICP匹配成功,則在后端非線性優(yōu)化添加回環(huán)約束。

2.5 后端非線性優(yōu)化

為了能實時運行,本文方法僅優(yōu)化關(guān)鍵幀的狀態(tài)。設(shè)Xi=[TWBivibaibgi]為關(guān)鍵幀的狀態(tài),其中,TWBi為位姿,vi為速度,bai和bgi分別為加速度計零偏和陀螺儀零偏。構(gòu)建LiDAR約束、IMU測量約束和回環(huán)約束的非線性代價函數(shù)

(9)

式中X為滑窗內(nèi)關(guān)鍵幀的狀態(tài),rIi為IMU測量約束殘差,rLi為LiDAR約束殘差,rij為回環(huán)約束殘差。最后通過優(yōu)化非線性代價函數(shù)實時獲得準確的狀態(tài)估計,得到精確的定位和建圖結(jié)果。

3 實驗與結(jié)果分析

3.1 仿真與實驗

首先在一般化的道路環(huán)境數(shù)據(jù)集如KITTI數(shù)據(jù)集評估本文方法性能,以便與其他方法比較,最后使用礦山隧道測量數(shù)據(jù)展示本文方法的建圖結(jié)果。KITTI數(shù)據(jù)集的點云數(shù)據(jù)由車載Velodyne HDL—64E S2 LiDAR收集。在LiDAR的左后方,有一個組合導(dǎo)航系統(tǒng)(OxTS RT 3003),可以輸出RTK/IMU組合導(dǎo)航結(jié)果,包括經(jīng)度、緯度和姿態(tài),以及IMU原始數(shù)據(jù)。KITTI數(shù)據(jù)集的全球衛(wèi)星導(dǎo)航系統(tǒng)提供了Ground Truth用于評估定位的精度。礦山隧道測量數(shù)據(jù)使用Ouster OS1—64激光雷達和Microstrain 3DM—GX5—25 IMU記錄了礦山的數(shù)據(jù)。本文選擇了一些先進的算法作為比較,包括SuMa[17]、LeGO-LOAM、LOAM、LIO-SAM。對于KITTI數(shù)據(jù)集,根據(jù)絕對軌跡誤差(absolute trajectory error,ATE)進行評估。

3.2 實驗結(jié)果分析

第一組實驗,表1為絕對軌跡誤差(ATE)的實驗對比結(jié)果,相較于這些單一傳感器方法,本文方法獲得了更優(yōu)的結(jié)果。SuMa的實驗結(jié)果是根據(jù)作者發(fā)布在項目頁面上的軌跡進行評估的。對于LOAM、LeGO-LOAM,則使用其開源代碼獲取軌跡進行評估。Seq.02和Seq.08中存在大量回環(huán),結(jié)果給出了本文采用的回環(huán)檢測方法的良好性能,即使在大量誤差累積時,仍能檢測到回環(huán)并對位姿進行優(yōu)化。對于其他的場景,本文提出的基于關(guān)鍵幀和基于普通幀的局部地圖匹配方法,避免了信息損失,同時也能有效處理錯誤特征關(guān)聯(lián)的問題。

表1 ATE實驗評估結(jié)果

第二組實驗,使用LIO-SAM的開源代碼獲取軌跡。圖3給出了LIO-SAM與本文方法在KITTI數(shù)據(jù)集Seq.09上軌跡的比較。由于長期運行造成的誤差累積使得基于歐氏距離的回環(huán)檢測無效,LIO-SAM的軌跡存在明顯的漂移,而本文方法則通過回環(huán)檢測來校正位姿。

圖3 Seq.09軌跡評估結(jié)果

圖4則是兩者建圖結(jié)果的比較。顯然,在紅色矩形框處,LIO-SAM的建圖結(jié)果有明顯殘影,而本文方法則建立了與環(huán)境一致的地圖。

圖4 二種方法在Seq.09上的建圖結(jié)果

第三組實驗,使用礦山隧道測量數(shù)據(jù)測試本文方法的建圖效果。圖5為本文方法在地下礦山隧道上的建圖結(jié)果,為了便于展示,僅顯示了部分地圖細節(jié)。

圖5 本文方法在礦山隧道數(shù)據(jù)上的建圖結(jié)果

4 結(jié) 論

本文提出了一種基于LiDAR IMU松耦合的SLAM算法。IMU的狀態(tài)預(yù)測用于消除點云的畸變,并為LiDAR里程計的計算提供初始值。提出了基于關(guān)鍵幀和基于普通幀的局部地圖匹配方法,引入M估計修正代價函數(shù)的形狀減少錯誤數(shù)據(jù)關(guān)聯(lián)的影響,并采用迭代加權(quán)最小二乘法求解,避免了信息損失維持了后端非線性優(yōu)化的低計算資源需求,同時也能有效處理錯誤特征關(guān)聯(lián)的問題。采用了基于Scan-Context的回環(huán)檢測方法,以獲得更魯棒的回環(huán)檢測結(jié)果。通過最小化關(guān)鍵幀約束、IMU測量約束和回環(huán)約束的非線性代價函數(shù)實時獲取精確的狀態(tài)估計,得到精確的定位和建圖結(jié)果。最后使用KITTI數(shù)據(jù)集評估了本文方法的性能。大量實驗結(jié)果表明：在有很多導(dǎo)致錯誤數(shù)據(jù)關(guān)聯(lián)的樹木或灌木叢和動態(tài)對象的場景中,本文方法比幾種當(dāng)前先進的方法更準確,能有效處理此類噪點。在長期運行導(dǎo)致的累積錯誤過大時,采用基于歐氏距離回環(huán)檢測的對比算法難以進行回環(huán)矯正,本文方法仍能正確檢測回環(huán),建立與環(huán)境一致的地圖。