煤礦移動機器人LiDAR/IMU緊耦合SLAM方法

李猛鋼，胡而已，朱華

（1.中國礦業大學 機電工程學院，江蘇 徐州 221116；2.江蘇省礦山智能采掘裝備協同創新中心，江蘇 徐州 221008；3.中國科學院沈陽自動化研究所 機器人學國家重點實驗室，遼寧 沈陽 110016；4.應急管理部 信息研究院，北京 100029）

0 引言

煤炭是我國的主要能源和重要戰略資源。目前我國煤礦智能化水平較低，井下作業人員仍然較多、安全風險大，加快煤炭企業對煤礦機器人的研發和應用是破解當前安全生產矛盾、實現減人增效的重要途徑和現實需求。對于各類煤礦移動機器人，精確定位與地圖構建是其推廣應用亟待實現的共性關鍵技術[1]。同步定位與地圖構建（Simultaneous Localization and Mapping，SLAM）具有在定位的同時實時重構場景模型的功能，為輔助駕駛、自主導航等提供定位建圖功能，近10 a來得到了長足的發展并已逐漸走向工業應用領域。

在煤礦井下SLAM方法方面，國外較早開展了相關研究，但主要采用手持設備[2]、移動小車[3]、探測機器人[4]、礦車[5]搭載激光雷達及慣性測量單元（Inertial Measurement Unit，IMU）等傳感器進行環境建模。D.Tardioli等[6]分析了其所在課題組多年來在地下隧道機器人領域開展的相關研究進展，主要探討了基于激光雷達、視覺相機、里程計等傳感器的SLAM方法在隧道環境的應用。美國國防部高級研究計劃局（Defense Advanced Research Projects Agency，DARPA）于2018?2021年舉辦的SubT賽事面向城市地下空間、隧道和天然洞穴環境下的機器人應用，相關研究針對光照變化[7]、場景退化[8]及巷道特征利用[9]等地下場景SLAM中的典型問題提出了新的解決方案。國內方面，馬宏偉等[10]提出了基于深度相機進行井下定位建圖實現移動機器人自主導航的方案。陳先中等[11]提出了一種煤礦地下毫米波雷達點云成像、結合深度學習處理稀疏點云的SLAM方法構想。楊健健等[12]研究了基于Hector?SLAM的掘進機器人巷道環境感知方法。Li Menggang等[13]提出了一種基于激光雷達的NDT（正態分布變換）?Graph SLAM方法，利用平面約束減輕地圖漂移，用于煤礦救援機器人的高效建圖任務。高士崗等[14]在國家能源神東煤炭榆家梁煤礦初步應用了搭載Exscan激光掃描儀的巡檢機器人，基于SLAM技術構建了動態數字化工作面，三維點云模型實測精度為0.2 m。總體來看，針對煤礦機器人SLAM的研究集中于解決激光雷達、相機、毫米波雷達等傳感器在特定場景、特定條件下的初步應用和性能改善問題，對井下多種條件耦合的復雜環境長期定位建模技術仍有待進一步攻關。

在三維激光SLAM研究方面，以迭代最近點（Iterative Closest Point，ICP）為代表的掃描匹配方法研究較早[15]。6D SLAM[16]的前端利用大量原始點云進行ICP配準，后端使用全局松弛化，實現了非實時的高精地圖構建。與使用全部點云不同，為了減少計算量、提高算法精度，另一類基于特征的方法被提出。Zhang Ji等[17]提出了一種激光里程計與建圖（LiDAR Odometry and Mapping,LOAM）方法，通過計算曲率分割邊點和面點，構建點?線和點?面優化問題，是目前諸多激光里程計采用的方法，但由于缺乏回環檢測，LOAM方法在長距離應用中仍然有較大的漂移。Shan Tixiao等[18]基于LOAM方法提出LeGO?LOAM（Light Weight and Ground?optimized LOAM）方法，采用了與LOAM方法同樣的特征提取方法，通過深度圖像分割剔除地面的干擾。J.Weingartent等[19]和A.J.B.Trevor等[20]均提出構建平面特征進行數據關聯，后端分別基于擴展卡爾曼濾波（Extended Kalman Filter，EKF）和圖優化構建SLAM。此類方法必須要求環境中有大量可提取的平面，形成稀疏平面特征地圖。

在激光?慣性融合定位建圖方面，目前主要分為松耦合和緊耦合2種方式。松耦合通過構建慣性導航獨立進行狀態傳播，利用激光進行觀測更新。緊耦合方法近年來逐漸得到關注。Ye Haoyang等[21]提出了一種雷達慣性里程計與建圖（Lidar Inertial Odometry and Mapping，LIO?mapping）方法，使用面特征構建相鄰幀之間的點?面約束，同時構建IMU預積分約束，基于因子圖聯合優化所有幀間觀測，在特征較多的大場景條件下難以滿足實時運行需求。Shan Tixiao等[22]提出了一種基于平滑建圖的雷達慣性里程計（Lidar Inertial Odometry via Smoothing and Mapping，LIO?SAM）方法，前端同樣采用LOAM進行特征提取，后端利用因子圖框架融合IMU，取得了較好的效果。除了優化方案，濾波方法也被用于激光慣性傳感器融合中。Qin Chao等[23]提出的用于魯棒和高效導航的雷達慣性狀態估計器（Lidar?Inertial State Estimator for Robust and Efficient Navigation，LINS）和Xu Wei等[24]提出的快速雷達慣性里程計（Fast Lidar Inertial Odometry,Fast?LIO）系列方法，基于迭代擴展卡爾曼濾波（Iterative Extended Kalman Filter，IEKF）實現激光和IMU緊耦合，在觀測更新時采用了類似優化方法的迭代過程，但沒有使用歷史數據，在復雜場景中的精度和魯棒性較差。楊林等[25]提出了激光慣性融合SLAM方法，前端利用IEKF設計了激光慣性里程計，通過構建關鍵幀在后端采用位姿圖優化進行聯合優化，降低了系統累計誤差。

煤礦井下機器人SLAM是當前研究熱點，但針對提高激光SLAM在井下復雜條件下的精度、魯棒性的研究仍然不足；傳統激光SLAM方法在此類環境下存在累計誤差迅速增大、旋轉過程魯棒性差、特征關聯錯誤率高等問題；現有激光?慣性融合的定位建圖緊耦合融合機制仍需進一步探討，以期進一步提高激光?慣性融合SLAM方法對煤礦井下復雜環境的適應能力。為此，本文提出了一種煤礦機器人LiDAR/IMU緊耦合SLAM方法（Lidar?Inertial SLAM，LI?SLAM方法）。基于點?線和點?面掃描匹配構建激光相對位姿約束，融合IMU預積分因子約束和回環因子約束，利用因子圖優化實現了LiDAR/IMU緊耦合的SLAM。構建了雷達相對位姿因子約束模型，解析推導了雷達相對位姿約束的殘差、雅可比和協方差矩陣；基于因子圖模型，構建了激光相對位姿約束與IMU預積分約束的緊耦合數據融合機制，提升了對煤礦井下顛簸路面、機器人劇烈旋轉等復雜工況的適應能力。該方法可幫助煤礦移動機器人在井下復雜地形與環境工況中實現魯棒精確的定位建圖，為煤礦移動機器人自主導航提供關鍵技術支撐。

1 LI?SLAM框架

LI?SLAM框架如圖1所示。激光約束構建階段包括5個步驟：①雷達數據預處理：利用IMU的短時狀態估計傳播運動狀態，校正時段內采集的雷達線掃運動畸變，進行區域濾波等預處理過程。②特征提取：提取點云中的邊和面特征。③關鍵幀與子圖構建：提取關鍵幀并構建局部子圖。④ 特征關聯：利用特征關聯和掃描匹配計算相對位姿變換。⑤雷達相對位姿因子構建：基于幀到子圖的掃描匹配建立局部雷達相對位姿因子。完成激光約束構建后，執行激光?慣性緊耦合融合機制：與同時段內慣性預積分因子在滑動窗口中進行聯合優化，實現狀態估計，利用雷達相對位姿因子限制慣導零偏等參數的漂移，實現緊耦合的聯合狀態估計，同時利用子圖關鍵幀構建全局地圖。迭代執行上述過程，可實現在線激光?慣性緊耦合SLAM。

圖1 LI?SLAM方法框架Fig.1 Framework of the LI-SLAM method

定義慣導坐標系B，雷達坐標系L，激光慣性里程計坐標系O。LI?SLAM的因子圖模型如圖2所示。待估計的變量xOBi是優化窗口內所有的IMU相對于局部坐標系下的位置p、速度v、姿態q、加速度零偏ba（a為加速度）、角速度零偏bω（ω為角速度）及從慣導到雷達坐標系間的外參TBL。從i，i+1，…，j時刻所有的待估計變量xOBi，xOBi+1，…， xOBj組 成的集合為χ：

圖2 因子圖模型Fig.2 Factor graph model

優化窗中第i時刻對應的狀態變量和外參分別表示為

采用子圖進行掃描匹配過程。在局部優化過程中，僅滑動窗口以內的各類因子參與優化過程，窗口外的狀態利用邊緣化技術固定。基于因子圖模型的優化目標函數為

2 激光約束構建

2.1 雷達數據預處理

點云畸變在機械式激光雷達高速運動問題中是必須解決的問題。現有手段通常基于勻速運動假設，利用線性插值重新部署激光點云的位置。這種方法對于劇烈的非線性運動適應性差。本文使用高頻IMU進行狀態傳播，以恢復每個點的運動。設定第i時刻和第j時刻對應的雷達觀測幀為Si和Sj。對第Si幀與第Sj幀之間的IMU觀測進行積分，作為激光掃描匹配的初值。相鄰采樣時刻間認為加速度和角速度未發生變化，加速度、角速度零偏是固定值。IMU加速度觀測值為，角速度為從t（t=i，i+1，…， j）時刻傳播的狀態包括位置、速度方向。

式中：vWBt為t時刻世界坐標系下的速度；gO為里程計坐標系下的重力加速度；和為t時刻相對于世界坐標系和里程計坐標系的旋轉矩陣。

2.2 特征提取

采用LOAM[17]的特征提取方法設計點云粗糙度指標作為線特征和面特征的區分方式。設定某個雷達掃描周期K內點云為PK，點云PK中2個點(PA,PU)∈PK，在雷達坐標系下的位置坐標可以表示為()。s為點云PK中點PA所在的行中的點組成的連續點集，則點PA所在局部曲面的粗糙度為

式中A，U為點云中的2個點對應的序號。

根據以下規則提取特征：①邊緣點：取粗糙度C最大的某些個點排序即為曲率較大的邊緣點。② 平面點：取粗糙度C最大的某些個點排序即為曲率較小的平面點。③為了使點云特征在雷達坐標系下的各個區域分布盡可能均勻，將每個掃描幀等分為若干區域，每個區域提取2個邊點和4個平面點。

2.3 關鍵幀與子圖構建

為處理高頻激光觀測數據，采用關鍵幀進行特征匹配和雷達約束的構建。預設最小平移距離閾值、最小旋轉角度閾值，若達到其中一個條件，則設置當前幀為關鍵幀。關鍵幀位姿變化計算公式可以參考文獻[13]。

在前端匹配中，采用幀到子圖的方式進行掃描匹配，組合固定數量的關鍵幀形成子圖。設關鍵幀k的邊和面特征集合為Fek和Fhk（e，h分別表示邊和面，下同），n個關鍵幀組成的特征集合為{Fk?n,Fk?n+1···,Fk},Fk=Fek∪Fhk。利用各關鍵幀的變換可以將特征轉換到局部里程計坐標系O下。轉換后，拼接關鍵幀，組成局部子圖Mk，其中包含變換到里程計坐標系后的邊和面兩類特征， Mk={Mke∪Mkh}。采用下采樣方法剔除同一個體素柵格中的重復特征，避免冗余的特征匹配。Mke和Mkh的下采樣精度根據環境特點、計算量按照經驗設置，此處設為0.1 m和0.2 m，子圖內關鍵幀數量為n=20。

2.4 幀到子圖的特征關聯與掃描匹配

圖3 2類特征點數據關聯方法Fig.3 Data association method for two kinds of features

至此，構建點到線的距離de：

構建點到面的距離dh：

式中：Xkh+1,I為關鍵幀k+1對應的面特征I的坐標；為關鍵幀k對應的面特征A，U，V對應的坐標。

利用幀與子圖中的點到線的距離de與點到面的距離dh，可以組成距離矩陣d，基于掃描匹配方法，可以建立以下代價函數 f來求解幀到子圖的最優位姿變換關系

利用列文伯格馬切爾特（LM）方法可以構建以下優化方程來求解幀間位姿的變換：

式中：J為雅可比矩陣，J=?f/?Tk+1；D為系數矩陣；μ為信賴區域半徑。

迭代推導出最優估計：

式中λ為阻尼因子。

采用以上列文伯格馬切爾特優化方法收斂后可以得到當前位姿估計值Tk+1=。位姿Tk和Tk+1之間的相對位姿變換為

2.5 相對位姿因子構建

利用雷達相對位姿構建關鍵幀位姿之間的約束關系，在滑窗中聯合慣性預積分一起參與優化。相鄰位姿間的優化目標項為

式中：x為待優化狀態變量；rL為相對位姿因子殘差；xi和xj分別為i和j時刻對應的運動狀態；Σ為約束不確定度的協方差矩陣， Σ∈R6×6，R為旋轉矩陣。由于估計過程是基于IMU坐標系，相對位姿觀測需要轉換到LiDAR坐標系：

2.5.1 殘差計算

從i時刻關鍵幀到j時刻關鍵幀之間的位姿變換的觀測和期望之間的殘差可以表示為

式中rL(χ)為 雷達坐標系下相對于觀測zBBij 和待估計變量χ的殘差，為從j時刻到i時刻IMU坐標系下的觀測。

推導可得相對位姿因子殘差的旋轉部分rθ和平移部分rξ：

2.5.2 雅可比矩陣計算

（1）向量空間：平移殘差向量在向量空間參數塊的雅可比矩陣為

（2）流形空間：平移與旋轉殘差向量在流形空間參數塊的雅可比為

2.5.3 協方差計算

由于前端配準采用了掃描到子圖的方法，因此可通過計算所有匹配成功特征點來計算整體掃描匹配的協方差。計算當前掃描幀c的特征點u的坐標Xuc在子圖w中對應特征點u的坐標Xuw：

所有匹配成功的雷達特征計算的協方差矩陣H為

定義噪聲矩陣Λ為

式中σx，σy，σz為x，y，z方向的的噪聲西格瑪值，受到2個點Xuw和Xuc噪聲的影響，參考激光雷達的噪聲參數，設置噪聲典型值為5 cm。

協方差矩陣對應的信息矩陣?為

利用式（22）從初始時刻的06×6矩陣開始迭代計算信息矩陣，隨著運動距離增加，信息矩陣的不確定度逐漸增長。

3 IMU預積分約束構建

IMU預積分作為一種高頻慣性觀測數據處理方法，在視覺慣性里程計、激光慣性里程計中逐漸得到應用，其核心思想是將相鄰關鍵幀之間的大量慣性觀測進行集成，在關鍵幀的局部慣性坐標系下開始積分，獲得獨立的相對運動約束，與其他約束聯合優化，實現完整的狀態估計。預積分原理如圖4所示，橫坐標代表時間戳。預積分過程就是將關鍵幀So、Si和Sj之間的高頻IMU觀測獨立建立為IMU運動約束。本文采用視覺慣性導航系統（Visual Inertial Navigation System,VINS）的預積分方法對IMU進行積分約束，預積分觀測方程、殘差、雅可比矩陣和協方差矩陣推導參考文獻[26]。

圖4 預積分原理Fig.4 Principleof pre-integration

4 回環檢測約束構建

回環檢測優化對于獲得全局一致的地圖有很大作用。本文采用文獻[13]提出的回環檢測方法，通過以下3個步驟實現回環檢測與約束構建。

（1）里程判斷：當新的關鍵幀建立后，首先搜索因子圖并找到與新關鍵幀位姿在歐氏距離上接近的關鍵幀。小于設定距離閾值的歷史關鍵幀將被作為回環候選關鍵幀。

（2）形貌相似度判斷：利用ICP掃描匹配方法匹配當前關鍵幀的點云，與回環候選關鍵幀前后設定的某些幀構成局部子圖。利用最近點對的均方根最小距離計算擬合率，與設定閾值比較來確定是否為真實回環關鍵幀。

（3）回環因子構建：利用以上雙重判斷選擇出回環關鍵幀后，通過式（12）計算相對變換因子，并將相對變換因子添加到全局因子圖中。

5 實驗分析

5.1 野外顛簸路面試驗

為了驗證LI?SLAM方法在顛簸路面和復雜場景的精度與魯棒性，基于輪式移動機器人試驗平臺在野外開展了試驗。機器人搭載VLP?16 LiDAR，MTi?G710 IMU。輪式移動機器人搭載的運算平臺配置為i7?8700k，32G內存。基于輪式移動機器人的試驗平臺和3個試驗場地如圖5所示。

圖5 輪式移動機器人試驗平臺及野外試驗場地Fig.5 Wheeled mobile robot test platform and field testsenvironment

5.1.1 精度分析

為了分析LI?SLAM方法的優越性，將其與當前最優的激光算法LOAM方法、LINS方法、LIO?mapping方法進行了對比。幾種方法的軌跡對比及其對齊到衛星地圖上的結果如圖6?圖8所示。可看出：在所有試驗場地中，LI?SLAM方法和LOAM方法的地圖一致性最好，與真實路線基本吻合，LI?SLAM方法對旋轉有更佳的適應能力。LIO?mapping方法無法實時運行，在0.5倍速下可以獲得完整軌跡，但在初始運動階段出現了較大程度的方向偏移，初始化過程容易失敗。LINS方法由于僅利用了最新的觀測信息，在復雜地形下出現了漂移。

圖6 試驗場地1中各種方法的運行軌跡Fig.6 Trajectoriesfor different methods in different test scenario 1

圖8 試驗場地3中各種方法的運行軌跡Fig.8 Trajectoriesfor different methods in different test scenario 3

不同方法在機器人起點、終點間測量距離與真值的誤差見表1，測量距離通過圖上的坐標差獲得，距離真值可以通過全站儀等測量和計算獲得。從表1可看出：LI?SLAM方法在試驗場地1、試驗場地2中誤差最小，在試驗場地3中與LOAM方法的誤差接近，均明顯優于其他方法。

圖7 試驗場地2中各種方法的運行軌跡Fig.7 Trajectoriesfor different methods in different test scenario 2

表1 不同方法起點、終點間距離與真值的誤差Table 1 Thedistance and truth valueerror between thestarting point and the end point of different methods

5.1.2 運算速度分析

LI?SLAM方法各個模塊耗時的均值和最大值見表2，可看出回環檢測模塊耗時最長，其次是建圖模塊。實際應用中，回環檢測和地圖構建模塊本身不需要實時，可以根據環境和需求人為設定。其余模塊均滿足10 Hz雷達采樣頻率的實時運行需求。

表2 野外試驗時LI?SLAM方法中各模塊耗時均值與最大值Table 2 The averageand maximum time consumption of each moduleof LI-SLAM method in field tests ms

5.1.3 回環檢測效果分析

在試驗場地1中機器人起始和終止位置發生的回環優化前后的效果如圖9所示。圖9（a）中回環尚未發生，在位置2的樹木和道路出現明顯的模糊，圖9（b）中回環優化后樹木辨別更加清晰，獲得了全局一致性的地圖，證明了回環檢測的有效性。

圖9 回環優化效果Fig.9 Effect of loop optimization

5.2 地下車庫少結構環境試驗

為了驗證LI?SLAM方法在少結構的井底車場等環境的應用效果，采用自主研發的CUMTV?C煤礦救援機器人平臺[27]進行了地下車庫的模擬試驗，如圖10所示，在機器人上方水平和傾斜30°各部署1個16線激光雷達，分別用于定位和構建稠密地圖。機器人搭載Xsens MTi?G?710 IMU，用于慣性數據測量。

圖10 地下車庫環境及機器人平臺Fig.10 Underground garage environment and robot platform

LI?SLAM方法的建模效果如圖11所示，綠色到紅色漸變的球形表示機器人的關鍵幀位姿，球形之間連線為相對位姿約束與回環約束。從圖11可看出，LI?SLAM方法具有較好的建模精度，局部精細化程度高。

圖11 LI?SLAM方法在地下車庫建模效果Fig.11 Modeling resultsof LI?SLAM method in underground garage

不同方法的運動軌跡如圖12所示，由于地面較為平坦，LINS、LOAM、LI?SLAM方法均獲得了平滑的運動軌跡，LIO?Mapping方法在初始化時出現了偏移，表明該方法在缺乏足夠運動激勵時難以獲得魯棒的狀態估計。

圖12 不同方法軌跡對比Fig.12 Comparison of trajectoriesof different methods

不同方法起點、終點間距離的誤差見表3，可看出LOAM和LI?SLAM方法的誤差最小，證明了緊耦合方法可以提升定位精度。

表3 不同方法起點、終點間距離的誤差Table3 The distance error between the starting point and theend point of different methods

LI?SLAM方法各模塊耗時均值與最大值見表4，可以看出：去畸變、特征提取、優化模塊均可滿足10 Hz運行條件，回環檢測與地圖構建模塊無高頻運行需求。

表4 地下車庫試驗時LI?SLAM方法中各模塊耗時均值與最大值Table4 Theaverage and maximum time consumption of each module in LI-SLAM method in underground garage tests ms

5.3 煤礦井下現場工業性測試

2020年11月2日，在晉能控股山西煤業股份有限公司塔山煤礦開展了煤礦救援機器人工業性驗證試驗，如圖13所示。采用自主研發的CUMTV?C煤礦救援機器人平臺進行試驗[27]，機器人實時運行LI?SLAM方法，通過無線信號傳輸（帶寬大于120 Mbit/s）實時顯示構建的地圖，用于人員遙控和自主導航。

圖13 煤礦井下現場試驗Fig.13 Field testsin underground coal mine

機器人在行走過程遇到了各類復雜工況，包括大量水汽、斜坡、岔口、顛簸路面等，如圖14所示。搭載LI?SLAM方法的煤礦救援機器人在各類地形環境中均可穩定運行，說明LI?SLAM方法滿足魯棒性、實時性需求。

圖14 機器人在井下各類復雜環境中運行情況Fig.14 Running process of the robot in several complex environment in underground coal mine

LI?SLAM方法實際運行中的一組建模結果如圖15所示。機器人行駛巷道直線距離為273 m，將地圖上的距離作為測量值，利用全站儀測量獲得人工標志點的定位結果之間的距離作為真值進行對比，分析30組距離結果表明，平均誤差小于15 cm，具有較高的定位和建模精度，基本滿足煤礦移動機器人的定位建模精度需求。

圖15 煤礦井下建模效果Fig.15 Modeling resultsin underground coal mine

6 結論

（1）提出了一種基于因子圖優化框架的LiDAR和IMU緊耦合SLAM（LI?SLAM）方法，基于點?線和點?面掃描匹配構建激光相對位姿約束，融合IMU預積分因子約束和回環因子約束，利用因子圖優化實現了LiDAR/IMU緊耦合的SLAM。

（2）詳細分析了激光約束的構建過程，設計了點云去畸變、特征提取、關鍵幀與子圖構建、特征關聯策略，解析推導了雷達相對位姿因子的殘差、雅可比與協方差矩陣。

（3）設計了雷達相對位姿因子、慣性預積分因子、回環檢測因子的代價函數，構建了激光慣性里程計因子圖緊耦合模型，并設計了無約束優化目標函數。

（4）在野外顛簸路面、地下車庫進行了大量測試，在真實煤礦井下現場進行了工業性試驗。結果表明：與LOAM方法、LINS方法、LIO?mapping方法相比，LI?SLAM方法在精度、魯棒性方面表現更佳，對于顛簸、少結構、水汽等復雜煤礦井下環境的移動機器人精確定位與地圖構建有更好的適用性，具有較高的定位和建模精度，基本滿足煤礦移動機器人的定位與建模精度需求，對井下各類移動機器人的定位建圖與自主導航具有較強的指導意義和參考價值。