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

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

0引言

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

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

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

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

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

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

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

3算法性能測試

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

3.1仿真巷道性能測試

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

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

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

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

3種算法規(guī)劃的二維軌跡如圖8 所示。可看出本文算法在x，y 軸上的規(guī)劃軌跡幾乎與真實軌跡重合，且預測的巷道長度與真實值差別不大，但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 算法定位誤差明顯，且誤差波動更大，表明其在巷道環(huán)境中可靠性較低；LIO?SAM 算法性能優(yōu)于LeGO?LOAM算法，但定位誤差仍有波動，且累計誤差隨時間增長而增大；本文算法的ATE 和RPE 最低且最穩(wěn)定，定位精度、穩(wěn)定性和魯棒性最優(yōu)。

3.2真實環(huán)境性能測試

在一條長約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 系統(tǒng)上運行[20]。

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

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

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

4結論

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

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

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