露天煤礦環(huán)境下基于LiDAR/IMU 的緊耦合SLAM 算法研究

馬寶良 ，崔麗珍 ，李敏超 ，張清宇

（內(nèi)蒙古科技大學(xué) 信息工程學(xué)院, 內(nèi)蒙古 包頭 014010）

0 引 言

“雙碳”目標(biāo)下[1]，煤炭依舊為我國主體能源，為響應(yīng)國家生態(tài)文明建設(shè)和實現(xiàn)高質(zhì)量發(fā)展，加快推進(jìn)綠色智能煤礦建設(shè)尤為重要[2-3]。煤炭開采環(huán)境復(fù)雜、條件惡劣，大量挖掘運(yùn)輸設(shè)備相互交叉運(yùn)行在礦場中，操作人員工作強(qiáng)度大、危險系數(shù)高，安全問題難以保障[4]。因此，實現(xiàn)開采機(jī)械自動化、無人駕駛礦車智能化在煤礦開采中的應(yīng)用是建設(shè)智能礦山的首要舉措[5-6]。同時定位與地圖構(gòu)建是實現(xiàn)采礦設(shè)備自動化和采礦環(huán)境數(shù)字化的一種關(guān)鍵技術(shù)，該技術(shù)能夠讓機(jī)器人在未知的環(huán)境中，完成定位、建圖和路徑規(guī)劃[7]。

早期的SLAM 使用單一的激光雷達(dá)進(jìn)行定位，在非結(jié)構(gòu)化或復(fù)雜惡劣的環(huán)境下運(yùn)行時會造成點云地圖稀疏的問題。如REN[8]等在連續(xù)幀之間、連續(xù)關(guān)鍵幀之間和回環(huán)檢測幀之間使用GICP[9]配準(zhǔn)方法并使用了隨機(jī)抽樣一致性（Random Sample Consensus,RANSAC）對煤礦巷道點云進(jìn)行了分割，并將分割得到的巷道平面作為了觀測約束，實現(xiàn)了在結(jié)構(gòu)化的煤礦巷道環(huán)境低漂移定位與建圖。慣性測量單元能夠不受環(huán)境影響，實現(xiàn)高頻高精度位姿估計，但其在長時間工作狀態(tài)下會導(dǎo)致自身誤差累積無法精確估計，因此將LiDAR 和IMU 進(jìn)行融合能夠?qū)崿F(xiàn)在復(fù)雜惡劣環(huán)境下長時間高精度定位[10]。目前，國內(nèi)外學(xué)者在LiDAR/IMU 融合方面已經(jīng)進(jìn)行了大量研究，主要分為松耦合和緊耦合2 大類[11]。對LiDAR 和IMU 的測量數(shù)據(jù)分別處理后進(jìn)行加權(quán)融合確定運(yùn)動狀態(tài)的方法被稱為松耦合，如ZHANG等[12]提出了激光里程計與建圖（LiDAR Odometry and Mapping，LOAM），該算法使用IMU 計算的姿態(tài)作為LiDAR 掃描配準(zhǔn)的初始值，而未將其作為全局優(yōu)化的約束條件。后續(xù)對松耦合系統(tǒng)的研究大多延續(xù)了LOAM 算法框架，如SHAN 等[13]在LOAM 算法的基礎(chǔ)上增加了地面優(yōu)化，對具有相同類別的特征點進(jìn)行匹配，提高了融合計算效率，使得特征匹配更穩(wěn)定。XUE 等[14]在LeGO-LOAM 基礎(chǔ)上通過整合掃描上下文回環(huán)算法降低了點云維數(shù)，提高了在煤礦井下模擬場景下的實時性和魯棒性。

松耦合算法僅停留在數(shù)據(jù)結(jié)果層面分析，無法解決退化場景問題，為此，緊耦合算法使用圖優(yōu)化或者濾波的方式將LiDAR 與IMU 觀測數(shù)據(jù)融合，充分考慮了兩者間的內(nèi)在約束，系統(tǒng)的魯棒性和準(zhǔn)確性得到提升。YE 等提出的LIO-Mapping[15]使用VINS-Mono[16]中的優(yōu)化過程來最小化IMU 殘差和LiDAR 測量誤差，并以一種旋轉(zhuǎn)約束的方法來細(xì)化最終的地圖，但由于該約束的計算復(fù)雜性，系統(tǒng)難以實時運(yùn)行。SHAN 等[17]提出了基于圖優(yōu)化的LIOSAM 使用劃窗的方式來完成幀到局部子圖的掃描配準(zhǔn)，關(guān)鍵幀的選擇以及對舊幀的邊緣化有效降低了計算復(fù)雜性，提高了系統(tǒng)運(yùn)行速度。QIN 等基于濾波的LINS[18]提出了一種以機(jī)器人為中心，以誤差狀態(tài)進(jìn)行迭代卡爾曼濾波的融合算法。YANG 等[19]利用擾動模型對煤礦長巷道退化環(huán)境進(jìn)行了檢測和補(bǔ)償，IMU 預(yù)積分用于補(bǔ)償旋轉(zhuǎn)狀態(tài)退化，LiDAR 與IMU 融合產(chǎn)生的新姿態(tài)用于補(bǔ)償平移狀態(tài)退化，提高了煤礦巷道退化場景的魯棒性。XU 等在FAST-LIO[20]中提出了一個新的計算卡爾曼增益公式，顯著提高了計算效率。在此基礎(chǔ)之上的FASTLIO2[21]提出了一種新的數(shù)據(jù)結(jié)構(gòu)ikd-tree[22]，能夠高效動態(tài)地對數(shù)據(jù)結(jié)構(gòu)進(jìn)行劃分，實現(xiàn)增量更新地圖，極大地提高了地圖管理效率。

盡管當(dāng)前SLAM 解決方案眾多，但是在露天煤礦環(huán)境下使用時的表現(xiàn)卻不盡如人意。該環(huán)境存在大量斜坡、碎石以及開采后產(chǎn)生的不規(guī)則單側(cè)石壁，算法會隨運(yùn)行距離增長產(chǎn)生累計漂移問題，而且大多數(shù)SLAM 算法需要提取邊緣和平面等幾何特征進(jìn)行配準(zhǔn)，均采用LOAM 框架對海量點云進(jìn)行多次降采樣，導(dǎo)致配準(zhǔn)結(jié)果不準(zhǔn)確，影響定位精度以及建圖效果。另外，在這樣的環(huán)境下會出現(xiàn)分段式的相似環(huán)境，當(dāng)無法檢測到新的特征時LiDAR 會出現(xiàn)退化現(xiàn)象，一旦配準(zhǔn)失敗且經(jīng)過IMU 積分發(fā)散，SLAM的定位性能將迅速下降，導(dǎo)致建圖失敗。綜上所述，SLAM 如何在露天煤礦環(huán)境下保持系統(tǒng)的魯棒性和準(zhǔn)確性仍是一大難題。為此，提出了一種基于濾波與圖優(yōu)化相結(jié)合的緊耦合SLAM 算法，前端使用迭代擴(kuò)展卡爾曼濾波（Iterater Extended Kalman Filter,IEKF）來實現(xiàn)激光雷達(dá)與IMU 數(shù)據(jù)的融合，后端分別設(shè)計了雷達(dá)相對位姿因子和回環(huán)檢測因子來完成全局優(yōu)化，增強(qiáng)了在露天煤礦環(huán)境下對顛簸不平路況的魯棒性，實現(xiàn)了復(fù)雜環(huán)境下精確的定位建圖，為煤礦設(shè)備智能化提供了一種關(guān)鍵技術(shù)支撐。

1 系統(tǒng)框架

所提出的算法主要由前端迭代擴(kuò)展卡爾曼濾波和后端因子圖優(yōu)化兩部分組成。該算法使用LiDAR 和IMU 兩種傳感器作為數(shù)據(jù)輸入信息源。前端使用緊耦合的迭代擴(kuò)展卡爾曼濾波器將LiDAR 特征點與IMU 數(shù)據(jù)相融合，使用后向傳播來補(bǔ)償LiDAR 運(yùn)動失真，經(jīng)過運(yùn)動補(bǔ)償后與該時刻的特征點來構(gòu)造殘差，完成狀態(tài)更新。后端分別設(shè)計了雷達(dá)相對位姿因子和回環(huán)檢測因子，相對位姿因子作用于全局提供新的關(guān)位姿與局部子圖內(nèi)關(guān)鍵幀位姿之間的約束，并對當(dāng)前幀與局部子圖進(jìn)行匹配；回環(huán)檢測因子同樣作用于全局，檢測是否與歷史位姿重合并對相鄰關(guān)鍵幀進(jìn)行調(diào)整來，完成全局優(yōu)化。最后輸出優(yōu)化后的機(jī)器人軌跡，位姿以及全局點云地圖，系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System framework

2 系統(tǒng)模型描述

使用了文獻(xiàn)[23]中定義封裝了兩個算子?和?，?：S×Rn→S 及其逆?：S×S →Rn，其中 S為維數(shù)為n的流形（S=S O(3) ）。IMU 坐標(biāo)系記為I，一般假設(shè)IMU 與LiDAR 以外參ITL=(IRL,I pL)剛性連接在一起，則IMU 運(yùn)動學(xué)模型如下：

其中，Δt為IMU 采樣時間間隔；f為函數(shù)；x為狀態(tài)；u為輸入；w為噪聲；定義如下：

3 激光慣性里程計模型構(gòu)建

使用IEKF 來估計式(2) 中的狀態(tài)。假設(shè)LiDAR 在時刻的掃描最優(yōu)狀態(tài)估計值是，則系統(tǒng)的隨機(jī)誤差狀態(tài)向量為

3.1 狀態(tài)傳播

IMU 測量時，按照式(2)將過程噪聲設(shè)置為零進(jìn)行傳播得到下式：

傳播協(xié)方差以迭代誤差動態(tài)模型來處理，定義如下[19]：

傳播一直進(jìn)行到tk這一幀結(jié)束，傳播狀態(tài)以及協(xié)方差分別記為和，將白噪聲w的協(xié)方差定義為Q，則傳播協(xié)方差可以通過下式計算得出：

當(dāng)點云累積時間間隔達(dá)到tk時，將新產(chǎn)生的特征點集與式(9)中的傳播狀態(tài)和協(xié)方差進(jìn)行融合來產(chǎn)生最優(yōu)的狀態(tài)。對于采集一幀點云時間內(nèi)產(chǎn)生的時間偏差所造成的運(yùn)動畸變，可以用高于IMU 測量值的后向傳播進(jìn)行補(bǔ)償。后向傳播可以在采樣時間ρj以及掃描結(jié)束時間tk之間，產(chǎn)生一個相對位姿。該相對位姿可以將局部測量投影到掃描結(jié)束時的測量位置：

3.2 狀態(tài)更新

xk先驗分布由前向傳播得到：

其中，K為卡爾曼增益，隨著狀態(tài)更新，每個特征點（）將會投影到全局地圖中，實現(xiàn)地圖更新。

4 全局因子圖優(yōu)化

理論上前端的狀態(tài)更新后已經(jīng)可以獲得完整的激光雷達(dá)慣性里程計，但在實際試驗過程中，上述得到的定位與建圖結(jié)果隨著系統(tǒng)運(yùn)行會產(chǎn)生偏移，并且這種偏移是不可逆的。這個問題在露天煤礦環(huán)境下顯得尤為突出，圖優(yōu)化的方式可以非常簡潔方便的描述各個誤差項之間的關(guān)聯(lián)，因此本文后端使用因子圖來進(jìn)行全局位姿圖優(yōu)化，通過對殘差模型進(jìn)行分析，最終目的就是最小化一個殘差項。構(gòu)建了雷達(dá)相對位姿和回環(huán)檢測兩個因子來實現(xiàn)全局位姿優(yōu)化。

1）雷達(dá)相對位姿因子。在前端激光雷達(dá)慣性里程計中采用了幀到子圖的掃描配準(zhǔn)方式，并且通過滑動窗口的方式來創(chuàng)建固定關(guān)鍵幀數(shù)量的子圖。機(jī)械式雷達(dá)的運(yùn)行頻率一般為10 Hz，相鄰幀之間變化量極小但卻需要消耗海量的計算資源，將每一幀都添加到因子圖中進(jìn)行優(yōu)化顯然是低效的，因此本文采用了選取關(guān)鍵幀的策略。關(guān)鍵幀的選取主要遵循兩個閾值指標(biāo)進(jìn)行確立：①最小平移閾值d當(dāng)系統(tǒng)接收到的當(dāng)前幀與上一幀的最小平移距離超過d時選取當(dāng)前幀為關(guān)鍵幀，這樣可以避免設(shè)備移動緩慢甚至?xí)和r產(chǎn)生大量冗余激光幀。②最小旋轉(zhuǎn)角 α，當(dāng)接受到的當(dāng)前幀與上一幀的姿態(tài)旋轉(zhuǎn)角度變化值大于 α?xí)r選取該幀為關(guān)鍵幀，由于機(jī)械雷達(dá)掃描覆蓋范圍較大，該閾值的設(shè)定可避免設(shè)備在顛簸路面的微量轉(zhuǎn)動提取到太多的激光點云數(shù)據(jù)。滿足上述任一指標(biāo)即可確立當(dāng)前幀為關(guān)鍵幀，利用關(guān)鍵幀來維護(hù)一個固定滑窗，即局部子圖。接著對新到來的關(guān)鍵幀進(jìn)行特征提取，利用該幀以及局部子圖來構(gòu)建以下代價函數(shù)求解位姿變換關(guān)系：

de和dp分別為新一幀提取到的邊緣特征和平面特征與其在局部子圖中相鄰幀提取到對應(yīng)特征之間的距離，以此構(gòu)建相鄰位姿間的優(yōu)化目標(biāo)：

其中，x為待優(yōu)化量；和分別為第幀和相鄰第j幀對應(yīng)運(yùn)因子動狀態(tài)。

2）回環(huán)檢測。SLAM 系統(tǒng)運(yùn)行時，各個傳感器誤差會不斷積累，這一誤差難以消除并且會逐漸增大，導(dǎo)致定位結(jié)果漂移。回環(huán)檢測因子的使用對于糾正漂移誤差以及構(gòu)建全局一致的地圖具有重要作用[25]。此外，在露天煤礦環(huán)境下往來礦用卡車基本會走固定路線進(jìn)行運(yùn)輸，重復(fù)運(yùn)輸?shù)穆肪€使得檢測到回環(huán)的幾率大幅增加，因此本文將回環(huán)檢測因子加入因子圖，與雷達(dá)相對位姿因子共同進(jìn)行對全局位姿的優(yōu)化。

當(dāng)新的關(guān)鍵幀確立后，首先設(shè)定最小距離閾值，對因子圖內(nèi)的全部歷史關(guān)鍵幀進(jìn)行搜索，找到與新關(guān)鍵幀在歐氏距離上滿足最小閾值的關(guān)鍵幀，設(shè)其為待匹配幀。使用GICP 對新幀與對應(yīng)待匹配幀進(jìn)行配準(zhǔn)，通過式(19)計算出最近點均方根的最小距離獲得置信分?jǐn)?shù)，同時與設(shè)定的最小距離閾值進(jìn)行比較，確認(rèn)該幀是否為回環(huán)關(guān)鍵幀，確定后可利用式(20)計算出該位姿的相對變換并添加到因子圖中。該步驟需要嚴(yán)格設(shè)置最小距離閾值，一旦回環(huán)檢測匹配到錯誤的關(guān)鍵幀就會導(dǎo)致地圖崩潰。

5 試驗和討論

為驗證文中算法的有效性以及魯棒性，我們分別在公共開源數(shù)據(jù)集以及露天煤礦實測數(shù)據(jù)集上進(jìn)行了試驗。開源數(shù)據(jù)集選擇了M2DGR[26]：一個帶有全套傳感器的地面機(jī)器人對上海交通大學(xué)校園內(nèi)不同場景采集的大規(guī)模數(shù)據(jù)集，所有傳感器都經(jīng)過良好的外部校準(zhǔn)和時間同步。露天煤礦實測數(shù)據(jù)集使用的數(shù)據(jù)采集設(shè)備如圖2 所示，該設(shè)備主要搭載傳感器為速騰聚創(chuàng)RS-LiDAR-16 型激光雷達(dá)，10 Hz工作模式下水平分辨率為0.2°，垂直分辨率為2°；超核電子CH110 型9 軸IMU，設(shè)置采樣頻率為400 Hz；北斗星通C200 衛(wèi)星接收機(jī)；機(jī)載計算機(jī)為NVIDIA Jetson AGX Xavier，8 核ARM 處理器，主頻2.26 GHz，內(nèi)存32 G。提出的SLAM 算法均使用C++實現(xiàn)，運(yùn)行于Ubuntu20.04，ROS 版本為noetic。

圖2 數(shù)據(jù)采集設(shè)備Fig.2 Data collection equipment

5.1 開源數(shù)據(jù)集試驗

使用開源數(shù)據(jù)集對目前主流激光SLAM 算法：A-LOAM、LeGO-LOAM、FAST-LIO2、LIO-SAM 與本文所提算法進(jìn)行了測試，通過計算最終絕對位姿誤差(Absolute Position Error，APE)來計算各個算法的定位精度。所有算法均在相同配置環(huán)境下使用，運(yùn)行設(shè)備為Intel(R) i9-10850K 3.60 GHz。分別選取了3 個不同場景下采集的數(shù)據(jù)street_08、door_02和gate_02 序列進(jìn)行試驗，使用EVO[27]作為最終結(jié)果評估工具。其中street08 序列為戶外環(huán)境，采集過程中存在大量連續(xù)轉(zhuǎn)彎，圖3 所示為各個算法在該序列上的定位軌跡對比結(jié)果。

圖3 street_08 序列各算法軌跡對比Fig.3 Trajectory comparison of each algorithm for the street_08 sequence

從圖3 中可以看出A-LOAM、LeGO-LOAM 表現(xiàn)較差，運(yùn)行一段時間后開始出現(xiàn)明顯漂移，LIOSAM 同樣存在該問題，但繞行一周后成功檢測到了回環(huán)，糾正了累計漂移，仍取得較好的結(jié)果，F(xiàn)ASTLIO2 對新的數(shù)據(jù)結(jié)構(gòu)的應(yīng)用可以在不提取特征的情況下可以快速直接地將原始點配準(zhǔn)到地圖上，在短距離結(jié)構(gòu)化的環(huán)境下，精度較高。算法所設(shè)計的相對位姿因子有效抑制了IMU 累計誤差，同時后期檢測到了回環(huán)，表現(xiàn)良好。各算法在所選序列試驗結(jié)果的APE 見表1，其中加粗項表示該項測試中誤差項最小的結(jié)果，對應(yīng)箱型圖如圖4 所示。

表1 開源數(shù)據(jù)集APE 誤差Table 1 Open source dataset APE error

圖4 開源數(shù)據(jù)集APE 誤差數(shù)據(jù)集箱型圖Fig.4 Open source dataset APE box plot

5.2 露天礦山實測數(shù)據(jù)集試驗

為進(jìn)一步驗證算法的有效性以及魯棒性，對內(nèi)蒙古哈爾烏素露天煤礦進(jìn)行了數(shù)據(jù)采集，實地環(huán)境如圖5 所示。該環(huán)境道路崎嶇不平，沿途布滿山石碎塊，呈現(xiàn)分段相似性，且結(jié)構(gòu)復(fù)雜無明顯的幾何特征。在采集的數(shù)據(jù)集中選取了兩個序列進(jìn)行試驗分析，各算法配置均與開源數(shù)據(jù)集試驗配置相同。

圖5 露天礦山環(huán)境Fig.5 Open pit mine environmental

序列1 全長2 024 m，為實時觀測到礦山數(shù)據(jù)誤差累積情況，對序列1 進(jìn)行了分段切片處理，分別截取為500、1 000 和2 000 m。圖6 分別展示了不同算法在兩處不同地點的建圖效果。

圖6 序列1 建圖效果Fig.6 Sequence 1 mapping results

從圖6 建圖效果中能夠看出LeGO-LOAM 和FAST-LIO2 存在明顯的漂移現(xiàn)象，雖完成了最終建圖，但是誤差較大。A-LOAM 與LIO-SAM 無直觀地漂移現(xiàn)象，但累計定位誤差較大。本文所提出的算法完整地構(gòu)建出了高精度點云地圖，對碎石巖壁結(jié)構(gòu)建圖清晰，掃描到的礦卡輪廓直觀無重影，具有較高的一致性。針對露天煤礦環(huán)境體現(xiàn)出了較高的魯棒性。

序列2 全長2 120 m，各算法運(yùn)行出的定位軌跡如圖7 所示。A-LOAM 和LeGO-LOAM 屬于單一激光雷達(dá)建圖算法，缺少IMU 的輔助出現(xiàn)了較大的累計誤差，尤其是輕量化LiDAR 里程計LeGOLOAM 嚴(yán)重依賴地面特征提取，在露天煤礦這樣的環(huán)境下，大量斜坡的存在導(dǎo)致該算法嚴(yán)重失效，狀態(tài)估計出現(xiàn)了較大誤差。FAST-LIO2 默認(rèn)不使用特征提取的方法，降采樣之后直接將原始點云配準(zhǔn)到地圖上來體現(xiàn)全局一致性，但是在碎石巖壁與顛簸道路環(huán)境中，原始點云經(jīng)過降采樣后呈現(xiàn)高度一致性，導(dǎo)致幾處相似場景出現(xiàn)了較大的漂移現(xiàn)象。

圖7 序列2 軌跡對比Fig.7 Sequence 2 trajectory comparison result

為避免這些問題，提出的算法將激光雷達(dá)幀間配準(zhǔn)結(jié)果作為約束因子在后端與回環(huán)因子進(jìn)行聯(lián)合優(yōu)化，進(jìn)一步提升了SLAM 系統(tǒng)的魯棒性，同時提高了定位建圖精度。各算法在所選序列試驗結(jié)果的APE 見表2，其中加粗項表示該項測試中誤差項最小的結(jié)果，其對應(yīng)箱型圖如圖8 所示。

表2 露天煤礦數(shù)據(jù)集APE 誤差Table 2 open pit coal mine dataset APE error

圖8 露天煤礦數(shù)據(jù)集APE 誤差箱線圖Fig.8 Open pit coal mine dataset APE box plot

6 結(jié) 論

1）提出了一種針對露天煤礦復(fù)雜環(huán)境下具有高精度高魯棒性的LiDAR/IMU 緊耦合SLAM 算法，前端使用緊耦合的迭代擴(kuò)展卡爾曼將LiDAR 特征點與IMU 數(shù)據(jù)融合，后端使用因子圖來接收關(guān)鍵幀位姿狀態(tài)，將激光雷達(dá)幀間配準(zhǔn)結(jié)果作為約束因子并與回環(huán)檢測因子共同完成全局優(yōu)化。

2）利用該算法在開源數(shù)據(jù)集M2DGR 的3 個不同場景和露天煤礦實地環(huán)境進(jìn)行了試驗測試，結(jié)果表明，算法在開源數(shù)據(jù)集的城市化環(huán)境下精度表現(xiàn)與當(dāng)前的SOAT 激光SLAM 算法保持一致，在長達(dá)兩千多米的露天煤礦實地環(huán)境下所提算法較FASTLIO2、LIO-SAM 緊耦合算法在定位精度上分別提高了46.00%和23.15%，提高了復(fù)雜環(huán)境下的定位精度與魯棒性。