面向礦井無人駕駛的IMU與激光雷達融合SLAM技術

關鍵詞：無人駕駛；同時定位與地圖構建；SLAM；激光雷達；慣性測量單元；環境信息輔助；因子圖優化

中圖分類號：TD67 文獻標志碼：A

0引言

智能化、無人化運輸是提高井工煤礦運輸安全性和效率的重要保障[1]。近年來，礦井無人駕駛技術研究主要集中在環境感知、路徑規劃、自主導航等方面，其中環境感知和自主導航是實現無人駕駛的關鍵[2]。同時定位與地圖構建（SimultaneousLocalization and Mapping，SLAM）技術可使移動機器人在未知環境中實時定位，同時構建環境地圖[3]，是無人駕駛、災后救援等領域的核心技術[4]。然而，礦井生產環境惡劣，地形復雜、巷道狹窄、光照條件差、信號衰減大等因素增加了SLAM 技術在井下應用的難度。

SLAM 技術常用傳感器包括激光雷達、攝像頭（單目、立體、RGB?D、魚眼）等[5]。視覺傳感器提供的信息更豐富，但對環境照明條件非常敏感，在黑暗區域或紋理較弱的場景中可能無法工作。激光雷達作為一種主動式傳感器，不受環境照明的影響，具有分辨率高、抗干擾能力強等優點，常被用作SLAM 首選傳感器[6]。但激光雷達存在運動失真、低頻更新、稀疏點云等問題。與激光雷達不同，慣性測量單元（Inertial Measurement Unit，IMU）不受結構特征或環境劇烈變化的影響，可在短時間內提供高精度、高頻率的姿態估計，但其累計誤差隨時間漂移[7]。

IMU 與激光雷達融合[8-10]是實現復雜環境中SLAM 技術的重要方法。二者的融合方式主要有松耦合和緊耦合。松耦合是將IMU 提供的運動估計結果作為先驗信息，用于輔助激光雷達的SLAM 過程；緊耦合是用IMU 和激光雷達數據互相估計與更新。針對礦井巷道特殊環境，文獻[11]提出了IMU 與激光雷達融合的SLAM 方法，通過選取關鍵幀、采用因子圖優化方式對激光雷達和IMU 數據進行整體優化，降低了SLAM 累計誤差，但未充分利用巷道環境信息的輔助作用。文獻[12]采用回環檢測、掃描匹配、IMU 預積分分別構建約束因子圖來優化約束因子，完成激光雷達和IMU 的緊耦合，但在狹長巷道內可能不存在機器人的回環，此時定位與建圖效果會受到影響。文獻[13]在前端加入卡爾曼濾波算法，后端使用位姿圖優化算法，實現了激光雷達和IMU耦合，并引入巷道的地面約束，但只考慮地面約束對定位精度和魯棒性的提高有限。

本文提出一種巷道環境特征輔助的IMU 與激光雷達融合SLAM 算法，用于礦井無人駕駛的高精度定位。該算法由IMU 提供運動狀態的實時觀測數據，通過激光雷達獲取環境中的點云信息，進而提取側壁和地面信息，結合巷道環境特征來提高SLAM精確性和魯棒性。

1IMU與激光雷達融合SLAM算法框架

巷道環境特征輔助的IMU 與激光雷達融合SLAM 算法框架如圖1 所示。對激光雷達獲取的原始點云數據進行去畸變處理，以消除傳感器運動而引入的誤差；對IMU 觀測數據進行預積分，為激光雷達點云匹配提供初值。選取關鍵幀構成局部地圖，對局部地圖中的點云進行平面特征提取，通過點云匹配算法與已知局部地圖進行匹配，以確定相鄰關鍵幀之間的位姿變換關系。通過激光雷達里程計約束、IMU 預積分和巷道環境約束共同進行因子圖優化，限制慣導零偏等參數的漂移，優化位姿變化，實現對無人駕駛車輛運動和環境的精確估計。

2 IMU與激光雷達融合SLAM算法關鍵技術

3算法性能測試

利用Gazebo 仿真平臺對本文算法的性能進行仿真驗證，在涵洞和車庫2 個場景進行實測驗證。

3.1仿真巷道性能測試

在Gazebo 軟件中搭建一個長200 m、寬5 m、高3m的巷道模型，其包括直道、彎道、岔口等復雜情況。在巷道中部署一個ScoutV2 輪式機器人模型代替無人駕駛小車，其配備Velodyne VLP?16 激光雷達和9 軸IMU 傳感器，用于采集仿真環境數據。部分仿真場景如圖4 所示。

小車在平坦地面和側壁處凸起地面上行駛時采集的點云如圖5所示。

通過巷道側壁信息提取方法對采集的點云進行處理，結果如圖6所示。

為驗證本文算法的準確性和魯棒性，將其與應用廣泛的LeGO?LOAM[19]， LIO?SAM 算法進行對比。3種算法規劃的三維軌跡如圖7 所示。可看出本文算法規劃的軌跡與真實軌跡的偏差較小， 而LeGO?LOAM算法和LIO?SAM 算法規劃的軌跡出現了不同程度的偏差，尤其在運動幅度較大和較長時間后，偏差更明顯。

3種算法規劃的二維軌跡如圖8 所示。可看出本文算法在x，y 軸上的規劃軌跡幾乎與真實軌跡重合，且預測的巷道長度與真實值差別不大，但LeGO?LOAM 算法和LIO?SAM 算法在定位上有較大偏差。在z 軸方向，LeGO?LOAM，LIO?SAM 算法的定位值漂移較大，特別是1 000 s 后（運動較長時間），漂移更加顯著。而本文算法的定位結果雖然也不可避免地存在漂移、誤差累計情況，但明顯小于其他2 種算法，驗證了本文算法在垂直方向上具有強魯棒性。

3 種算法的絕對軌跡誤差（Absolute TrajectoryError， ATE） 和相對軌跡誤差（Relative Pose Error，RPE）如圖9 所示，具體值見表1、表2。

從圖9、表1、表2 可看出，LeGO?LOAM 算法定位誤差明顯，且誤差波動更大，表明其在巷道環境中可靠性較低；LIO?SAM 算法性能優于LeGO?LOAM算法，但定位誤差仍有波動，且累計誤差隨時間增長而增大；本文算法的ATE 和RPE 最低且最穩定，定位精度、穩定性和魯棒性最優。

3.2真實環境性能測試

在一條長約600 m、寬6 m 的復雜巷道（包含上下坡、大拐角等場景）中對本文算法進行測試，定性分析建圖效果。測試場景如圖10 所示。

實驗平臺采用Autolabor 機器人代替無人駕駛車輛，如圖11所示。機器人配備hipnuc?CH104M 9 軸IMU 和Velodyne 16線激光雷達。雷達、IMU 的采樣頻率分別為10，200 Hz。平臺搭載的計算機為IntelCore i7，8 個內核，16 GiB 內存。算法用C++編寫，用ROS 的melodic 版本在Ubuntu 18.04 系統上運行[20]。

采用LeGO?LOAM 算法、LIO?SAM 算法和本文算法進行SLAM 測試，結果如圖12 所示。可看出使用LeGO?LOAM 算法時，巷道邊緣呈模糊狀態，并伴隨大量雜亂點云，表明該算法在巷道環境中具有局限性。LIO?SAM 算法針對長巷道邊緣的對齊精度較低。本文算法在巷道邊緣建圖方面表現優異，即使在大拐角處也能保持良好效果，證明了其在復雜環境中的適應性和有效性。

在與礦井巷道具有相似性的2 個代表性場景中對本文算法進行實測。① 涵洞場景，如圖13（a）所示。該場景具有地面坑洼、側壁粗糙等特點，用于測試算法在不平整地面和復雜壁面環境中的表現。② 地下車庫場景，如圖13（b）所示。該場景存在較多的車輛和其他障礙物，用于測試算法在復雜動態環境中的定位與建圖能力。

在涵洞和地下車庫測試場景中分別進行多次實驗，在不同路徑和不同障礙物分布情況下，本文算法的SLAM 結果如圖14 所示。可看出本文算法在涵洞和地下車庫場景中均表現出良好的適應性和魯棒性。在涵洞場景中，小車在地面坑洼和側壁粗糙條件下能準確定位并建圖，且側壁為一條直線，沒有出現彎曲、扭曲效果。在地下車庫較多障礙物的情況下，本文算法能實現準確定位和建圖，效果良好，未出現拖尾、漂移現象。

4結論

1） 巷道環境特征輔助的IMU 與激光雷達融合SLAM 算法通過IMU 預積分校正、側壁和地面平面信息提取、因子圖優化等，實現復雜礦井環境的高精度定位與建圖。

2） 仿真及真實環境測試結果表明，該算法的定位精度和魯棒性較常用的LeGO?LOAM、LIO?SAM算法有明顯提升，在復雜巷道、涵洞、地下車庫場景中均具有較好的建圖效果，未出現漂移和拖尾現象，定位精度和魯棒性較高。

3） 今后將進一步提升算法的計算效率，并對SLAM 框架進行優化擴展，以適應更復雜的環境和任務需求。