面向煤礦巷道環境的LiDAR與IMU融合定位與建圖方法

馬艾強，姚頑強，藺小虎，張聯隊，鄭俊良，武謀達，楊鑫

（1.西安科技大學 測繪科學與技術學院，陜西 西安 710054；2.陜西彬長礦業集團有限公司，陜西 咸陽 712000）

0 引言

煤礦事故發生后，井下存在不確定和危險因素，依靠人工救援成功率低，煤礦井下移動機器人可替代救援人員深入礦井探測災害現場情況，檢查事故現場，并在第一時間將獲得的環境信息傳輸到指揮中心，制定救援策略[1]，以提高救援效率[2]。然而，傳統的導線測量[3]、貫通設計、三維激光掃描等技術方案測量速度慢、效率低，嚴重制約了煤礦井下環境建模，且實施過程中難以保證建模精度[4]。近年來，激光雷達（Light Detection and Ranging，LiDAR）、慣性測量單元（Inertial Measurement Unit，IMU）和全球導航 衛 星 系 統（Global Navigation Satellite System，GNSS）[5]的移動激光雷達平臺被廣泛用于獲取三維地理信息數據[6]。煤礦井下環境復雜，無GNSS信號，工況惡劣，地面常規定位與建圖技術無法直接應用，因此采用同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）[7]技術輔助煤礦井下移動機器人執行路徑規劃、自主探索、自主導航任務[8]。基于SLAM技術礦井機器人可以實時準確地構建煤礦井下三維地圖，該地圖可以靈活、可靠地輔助礦井機器人進行智能導航和避障，并應用于礦井下危險區作業、自動巡檢、遠程調度等領域，對實現智能化定位導航起著至關重要作用[9-10]。然而，在煤礦井下噴漿表面、對稱巷道環境中，單一傳感器的SLAM技術已不能滿足智能感知精度及可靠性的需求。因此，多源傳感器融合的SLAM技術應用在煤礦井下成為了當前學術研究與工程應用的聚焦點。

目前，LiDAR和IMU的融合方法主要分為松耦合和緊耦合2種方法。松耦合類方法[11]分別處理LiDAR和IMU的位姿估計，然后再進行融合。文獻[12]提出了利用IMU信息來輔助估計激光里程計，IMU積分狀態為激光點云匹配提供位姿初值，該方法缺乏回環抑制漂移誤差。緊耦合類方法[13]對LiDAR和IMU的數據進行互相估計與更新。文獻[14]將IMU預積分因子、激光里程計因子、GNSS因子和閉環因子進行聯合優化，取得了較好的建圖效果，但并未考慮煤礦環境對稱巷道退化對SLAM結果的影響。文獻[15]提出用誤差卡爾曼濾波對IMU進行預測，并與LiDAR進行緊融合的方法，該方法用激光雷達高程圖和先驗數字高程圖進行匹配，并用匹配值對狀態進行更新，該方法展示了在長距離無GNSS信號場景下的導航能力，但需要先驗地圖，無法應對無先驗地圖的場景。文獻[16]提出了一種基于因子圖的LiDAR和IMU緊耦合算法，通過最小化IMU預積分和LiDAR點云匹配結果的殘差來優化LiDAR位姿和IMU偏置，根據IMU的預積分值對LiDAR位姿進行線性插值，去除點云畸變，實驗表明該方法可應對LiDAR退化的場景和快速運動的情況，但效率較低，實時性較差，而且基于勻速假設的去畸變策略無法適用于運動變化較快的場景。文獻[17-18]提出了一種緊耦合算法，在畸變去除部分，該算法使用高斯過程回歸對IMU預積分測量值進行采樣，利用采樣值對LiDAR位姿進行插值，完成畸變去除，取得了很好效果，但該算法計算效率較低。文獻[19]提出了一種MC2SLAM的3D LiDAR和IMU緊耦合里程計算法，該算法使用因子圖融合3D LiDAR和IMU，在畸變去除部分，使用一種基于勻速假設的非剛性點云匹配算法，通過最小化點云匹配的殘差優化LiDAR位姿，再對優化得到的位姿進行線性插值，但是該算法的特征點選取策略過于簡單且存在隨機性，精度不夠穩定。

為解決發生煤礦事故后救援機器人難以自主導航定位的問題，本文提出了一種面向煤礦巷道環境的LiDAR與IMU融合的實時定位與建圖方法，用于探測礦井災害現場情況，提高煤礦事故救援效率。該方法通過數據預處理、點云掃描匹配和后端因子圖優化等多層次的數據融合，降低軌跡漂移，增強救援機器人系統在煤礦巷道環境中快速移動時的魯棒性，實現高精度定位與建圖。

1 SLAM基本框架

SLAM基本框架由預處理、激光里程計、地圖構建3個部分組成，如圖1所示。首先對原始點云進行分割，利用IMU預積分位姿去除原始點云非線性運動畸變，并對得到的點云進行線、面特征提取。然后將相鄰幀的線、面特征進行匹配，在分層位姿估計過程中融合IMU預積分所得到的位姿初值，減少計算迭代次數，提高特征點匹配的精度，解算出當前幀的位姿。最后向因子圖中插入局部地圖因子、IMU因子、關鍵幀因子，對位姿進行優化約束，對關鍵幀與局部地圖進行匹配，通過八叉樹結構實現地圖構建。

圖1 SLAM基本框架Fig.1 SLAM basic framework

2 基于特征點匹配的相對位姿估計

2.1 預處理

在實際作業環境中，搭載LiDAR的移動機器人不可避免產生非線性運動，導致掃描的激光點云存在非線性運動畸變，因此，利用高頻率的IMU狀態對激光點進行補償校正[16]。因為煤礦巷道地面特征點和墻壁特征點對于垂直方向有一定的約束，且平面特征對水平方向的平移和旋轉有較好的約束，所以先用基于隨機樣本一致性（Random Sample Consensus，RANSAC）[20]的快速點云分割算法提取環境中的巷道地面點和墻壁的平面特征。特征提取部分利用深度圖計算某一點的曲率，將曲率較大的非地面點標記為線特征點，將曲率較小的非地面點標記為平面特征點[21]。

2.2 激光里程計

在提取特征的前后幀點云中，首先利用KDtree[22]搜索方法尋找最近鄰相似屬性的特征點，并將法向和主方向夾角均小于5°且距離小于1 cm的特征點作為同一類特征點。建立點到線和點到面距離最小的約束，進行分層位姿估計，如圖2所示。由于煤礦巷道的面特征比線特征多，占據位姿權重較大，數據可信度高，因此，選擇源點云面特征和目標點云面特征點去對應匹配位姿，確定垂向平移量、橫滾角、俯仰角，得出一個較準確的位姿。再根據源點云線特征和目標點云線特征匹配確定側向平移量、前向平移量和航向角，從而達到整體優化位姿精度。

圖2 分層位姿估計Fig.2 Hierarchical pose estimation

3 多因子聯合圖優化及地圖構建

3.1 關鍵幀因子提取

為確保本文算法在煤礦井下實時運行，在前端里程計中部署了關鍵幀選取策略，T1—T6為連續幀，?T為連續幀T1—T6之間的位姿變化，選擇T1和T6為關鍵幀，如圖3所示。

圖3 關鍵幀選取Fig.3 Key frames selection

關鍵幀的選取可極大地提高計算效率，確保點云能夠快速匹配。選取關鍵幀的標準是給歐氏距離?S 、關鍵幀旋轉矩陣?R、時間間隔?t設定一個閾值，任意滿足其一，則當前第k幀點云被選取為候選關鍵幀。選擇關鍵幀時，必須減少匹配錯誤幀和冗余關鍵幀，以達到節省計算量的目的。在保證精度的前提下減少數據冗余，以提高運算效率。因此，基于實時點云關鍵幀選擇標準，將第1幀點云作為判斷準則開始選擇關鍵幀。連續關鍵幀的相對變換位姿為

式中：?Tk?1,k為第 k?1幀到第k幀關鍵幀的位姿變化；Tk?1為 第k?1幀 的關鍵幀；Tk為第 k 幀的關鍵幀；?q為第 k?1幀 到第 k 幀關鍵幀的旋轉矩陣；?d，?e，?f分別為在X軸、Y軸、Z軸的位移變化；tk為第k幀關鍵幀的時間戳。

在確定關鍵幀后，再以初始相對變換位姿優化點云配準，最終變換的位姿應用于位姿和位姿圖中位置之間的約束。

3.2 多因子位姿優化及地圖構建

假設LiDAR第i（i=1,2，…，j，j為關鍵幀總次數）幀關鍵幀的掃描起始時刻為ti，掃描得到的全部點云為Pi；IMU在內采集的數據為I(i,j)。系統在ti時刻的姿態、位置、速度和IMU零點偏置為式中：xi為關鍵幀在ti時刻的狀態；Ri為李群表示的旋轉矩陣；vi為速度；bi為零點偏差。

設在tk時刻的所有關鍵幀為Kk，其對應的狀態量其對應的所有觀測量

在給定觀測量Zk和先驗信息 p(χ0)的條件下，預測關鍵幀Kk對 應的初始狀態量χ0的后驗概率問題為狀態估計問題。

變量因子最大后驗概率為

式中：χt為t時刻的狀態量；r為觀測模型與實際觀測的殘差，是關于狀態量χk的函數；Kt為t時刻的關鍵幀；Γ為對應的協方差矩陣。

根據IMU動力學模型，采用離散化積分方法對加速度和角速度在IMU采樣間隔時間?t內積分，將高頻輸出的加速度和角速度觀測量轉換為狀態量間的位姿變換，構成關鍵幀狀態量之間的約束因子。

式中：?Rij，?vij，?sij分 別為在?t 內第i ～j幀關鍵幀之間的旋轉矩陣、速度、位移；Rj為第j幀關鍵幀的旋轉矩陣；vi，vj分別為第i，j幀關鍵幀的速度；?Rik為?t 內第i～k幀關鍵幀之間的旋轉矩陣；分別為第k幀關鍵幀的角速度、加速度；bak， bgk分別為IMU的零偏加速度與零偏角速度；si，sj分別為第i，j幀關鍵幀的位移；g為重力加速度；η為觀測噪聲。

假設關鍵幀在狀態xi與xj的IMU零點偏置保持不變，則可由式（9）?式（11）得預積分觀測模型。

由式（12）?式（14）可推導出殘差項 rI(i,j)。

多因子圖優化模塊是在系統中維護一個全局因子圖，因子圖結構如圖4所示。設給定初始狀態為x0，采用數據預處理模塊計算相鄰關鍵幀之間的IMU預積分，并向因子圖插入IMU預積分因子rI(i,j)。

圖4 多因子圖優化Fig.4 Multi-factor graph optimization

式中mj為第j幀的關鍵幀局部地圖。

在因子圖中插入關鍵幀因子rp(i,j)。

當提取一個連續幀作為關鍵幀時，根據連續關鍵幀的位姿估計，將關鍵幀點云匹配進行地圖的更新。首先利用線、面特征匹配求得LiDAR坐標中連續關鍵幀之間的位姿，然后通過坐標變換更新關鍵幀位姿，并將其點云轉換到全局坐標系中，最后通過八叉樹結構對點云圖進行更新。

4 實驗結果和分析

4.1 實驗設備及數據介紹

為了測試本文所提方法性能，自主搭建Autolabor 、VLP-16 LiDAR和Ellipse-N IMU的實驗平臺，如圖5所示。本次實驗中LiDAR頻率為10 Hz，IMU頻率為200 Hz，2個傳感器采用硬同步方式進行時間對齊。煤礦井下移動機器人可進行原地旋轉，最大旋轉角速度為0.523 5 rad/s，最大位移速度為1.5 m/s。移動機器人系統采用C++實現，在機器人操作系統（Robot operation system，ROS）中運行。利用全站儀記錄移動機器人經過參考點時的絕對位置，定量分析移動機器人位姿精度。實驗所采集的數據分別有煤礦巷道和樓道數據，具體信息見表1。

圖5 實驗平臺Fig.5 Experimental platform

表1 實測數據Table1 Measured data

4.2 實驗結果分析

為驗證本文所提方法的定位性能與建圖效果，在煤礦巷道A和樓道走廊B中選取起點和終點相同的閉環環境，定量計算各方案的累計位姿誤差，定性評價各方案的建圖效果及起點和終點的激光里程計軌跡閉合情況。

為了評價不同方法的絕對定位精度，對煤礦巷道場景A所示的10個控制點（A0?A9）和樓道走廊場景B所示的6個控制點（B0?B5）的坐標真值（圖6）進行測量。分別與目前主流的LeGO?LOAM，LIO?SAM方法進行對比。通過全站儀測量真值，并記錄移動機器人在對應控制點位置停止區間時刻的起點和終點，將其時間段內的位置估計結果求取均值，作為當前方法在該點的觀測值。全站儀實測數據作為判斷移動機器人定位精度的依據。

圖6 場景A（參考點A0?A9）和場景B（參考點B0?B5）的坐標真值Fig.6 Coordinate ture value of scenario A （reference points A0-A9）and scenario B（reference points B0-B5）

4.2.1 定位精度分析

為分析移動機器人在運動過程中的定位精度，給移動機器人粘貼標志點，分別在場景A（A0?A9）、場景B（B0?B5）處，使用全站儀測量其真值，并對3種方法絕對定位誤差分布效果進行分析，如圖7所示。在場景A中可看出，LeGO?LOAM方法定位精度較差，在X方向上最大絕對定位誤差為300 cm；LIO?SAM方法定位精度雖高于LeGO?LOAM方法，但在三軸方向的絕對定位誤差在50 cm附近；本文方法在三軸方向的絕對定位誤差的均值和中值均小于32 cm。這是因為點云匹配之后進行了分層位姿估計，多因子優化可有效降低全局累計誤差，對軌跡精度和地圖的一致性的提升具有重要作用。在場景B中可看出，LeGO?LOAM方法在Z軸的誤差均值為50～55 cm，X、Y軸誤差均值為30～35 cm；LIO?SAM方法三軸誤差均值為15～25 cm；本文方法三軸的誤差均值為5～15 cm，誤差范圍小，精度高。

圖7 場景A、B的絕對定位誤差分布Fig.7 Absolute positioning error distribution of scenarios A and B

3種方法的累計誤差見表2。在場景A中可看出，LeGO?LOAM方法在狹小的煤礦巷道點云匹配發生退化，造成三軸的累計誤差很大，位置偏差達到49.16 m，使得建圖效果發生了漂移；本文方法和LIO?SAM方法的定位精度相對較高，而本文方法對X軸的位姿估計精度最高，其累計誤差為1.65 m，位置偏差為2.97 m，從三軸或累計誤差來看，本文方法建圖效果整體良好，建圖軌跡未發生漂移。在場景B中可看出，LeGO?LOAM方法在X、Y軸的累計誤差均小于1.5 m，Z軸誤差達到了2.33 m，位置累計偏差為2.95 m；LIO?SAM方法在X、Y軸的累計誤差分別為2.21，3.15 m，Z軸誤差僅為1.05 m，位置偏差為3.99 m；本文方法在X、Y、Z軸的誤差均小于1.01 m，累計位置偏差僅為1.67 m。綜合分析得出，在較少特征的樓道中，LeGO?LOAM方法與LIO?SAM方法點云匹配時發生退化，導致X、Y、Z軸的平移誤差較大，點云模型相差較大，建圖定位精度較差。本文方法應用于樓道退化場景依舊有較強的魯棒性，定位精度高，有良好的建圖效果。

表2 3種方法的累計誤差Table 2 Cumulative error of there methods

4.2.2 建圖效果分析

為了進一步說明3種方法在煤礦巷道和樓道定位的建圖效果，分別在煤礦巷道與樓道環境進行驗證，建圖結果與軌跡如圖8和圖9所示。

圖8 煤礦巷道場景A 中3種方法建圖結果與軌跡Fig.8 Mapping resultsand tracksof three methods in coal mine roadway scenarios A

圖9 樓道走廊場景B中3種方法建圖結果與軌跡Fig.9 Mapping results and tracks of three methods in corridor scenarios B

由圖8可看出，LeGO?LOAM方法由于點云誤匹配出現大的漂移現象，建圖失效；LIO?SAM方法可以構建出大致輪廓的點云地圖，但由于Z軸上出現整體明顯漂移，導致移動機器人所生成的點云地圖結構變形且不完整；本文方法構建的點云地圖，在完整性和幾何結構真實性方面均有著優秀的表現，可以直觀地反映巷道環境的實際情況，在煤礦井下環境具有良好的魯棒性。

由圖9可看出，采用LeGO?LOAM方法進行建圖時，軌跡重復，整體發生偏移，移動機器人在回到終點時，軌跡無法閉合，建圖失效；采用LIO?SAM方法進行建圖時，隨著激光里程計里程的增加，由于部分場景相似，導致建立了錯誤的回環約束，發生漂移，以致點云地圖構建失敗；采用本文方法進行建圖時，地圖完整性與環境匹配均有良好的性能，這是由于通過增加關鍵幀因子，插入因子圖對其新增節點相關變量進行優化，降低了位姿估計漂移，定位與建圖精度相對較高。

5 結論

（1）提出了一種LiDAR與IMU融合的實時定位與建圖方法。該方法首先采用面特征匹配估計位姿，然后進行線特征匹配，實現激光序列幀的相對位姿估計，提高點云匹配精度，最后在因子圖優化與地圖構建過程中插入關鍵幀因子、IMU預積分因子、局部地圖配準因子，對LiDAR和IMU數據進行整體優化，提高了實時定位與建圖的精度，降低了煤礦環境下SLAM的累計誤差。

（2）該方法在自采數據集上分別進行了定量定性分析，并與現有的LeGO?LOAM，LIO?SAM方法進行比較。結果表明：①在煤礦巷道環境中，本文方法三軸的絕對定位誤差的均值和中值均小于32 cm；X軸的位姿估計精度最高，其累計誤差為1.65 m，位置偏差為2.97 m，建圖效果整體良好，建圖軌跡未發生漂移；構建的點云地圖，在完整性和幾何結構真實性方面均有著優秀的表現，可以直觀地反映巷道環境的實際情況，具有良好的魯棒性。②在樓道走廊環境中，本文方法三軸的誤差均小于1.01 m，誤差均值為5～15 cm，誤差范圍小，精度高；累計位置偏差僅為1.67 m；地圖完整性與環境匹配均有良好的性能。