基于多源信息融合的井下無人駕駛建圖與定位方法

中圖分類號：TD525 文獻(xiàn)標(biāo)志碼：A

Abstract：Due to the harsh environment in coal mines underground，mapping methods based on singlesource odometry information are prone to issues such as drift，occlusion，and missing semantic features.Existing mainstream localization algorithms applied underground in coal mines often encounter localization errors.To addressthese issues，this paper proposed an underground autonomous mapping and localization method basedon multi-source information fusion.The mapping was performed using themulti-source information fusion-based RTAB-Map algorithm，which significantly reduced mapping driftand improved featurecapture ability by fusing point cloud and image data.Precise localization was achieved using the Adaptive Monte Carlo Localization （AMCL）algorithm，which combined LiDAR and motion information and employed particle filtering，pose prediction and resampling to achieve adaptive localization，thereby reducing localization inaccuracies and mapping drift.Simulationand experimental resultsshowed that，compared withasingle wheel odometry，the absolute value of the relative error of RTAB-Map mapping based on multi-source information fusion was reduced to within 1% ，and the map matching accuracy was higher，improving mapping reliability.Particles using the AMCLalgorithm converged rapidly within 2 meters，meeting the localization requirements of autonomous auxiliary transport vehicles.

Key Words： underground autonomous driving；SLAM;multi-source information fusion；RTAB-Map algorithm;AMCL algorithm

0引言

無人駕駛輔助運輸車輛為煤礦智能化的關(guān)鍵部分。無人駕駛技術(shù)包含感知、建圖與定位、決策和控制等[1-3]。其中建圖與定位技術(shù)是通過傳感器收集四周環(huán)境數(shù)據(jù)并進(jìn)行分析處理，得到周圍環(huán)境信息與自身位姿的一種技術(shù)[4-5]。建圖與定位信息可為車輛后續(xù)決策提供關(guān)鍵判斷依據(jù)。由于井下環(huán)境復(fù)雜，全球定位系統(tǒng)（GlobalPositioningSystem，GPS）無法適用[；同時，井下無線信號傳播受到地形和障礙物的限制，增加了建圖與定位難度[7]。將即時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM技術(shù)引入井下無人駕駛定位系統(tǒng)中，可有效構(gòu)建車輛運行環(huán)境地圖，實現(xiàn)車輛精確定位[8]。然而，SLAM本身也面臨計算復(fù)雜度高、對動態(tài)變化的環(huán)境適應(yīng)性差等挑戰(zhàn)[9-10]

SLAM技術(shù)包含建圖和定位2個部分。目前主流的建圖算法主要有Gmapping算法、HectorSLAM算法、Cartorgrapher算法、KartoSLAM算法等[11-15]，都是基于單激光雷達(dá)或激光雷達(dá)融合慣性測量單元（InertialMeasurementUnit，IMU）實現(xiàn)建圖。但針對井下狹長、高相似、低光照、高粉塵環(huán)境，建圖迭代易出現(xiàn)偏移現(xiàn)象，基于激光雷達(dá)建圖易缺失語義特征，基于單相機(jī)建圖易出現(xiàn)光照不穩(wěn)定及遮擋現(xiàn)象。基于外觀的實時建圖（Real-TimeAppearance-BasedMapping，RTAB-Map）算法采用漸進(jìn)式閉環(huán)檢測策略，可顯著降低大規(guī)模建圖復(fù)雜度，將點云信息和圖像深度信息融合，能夠捕捉更多特征點，更加適用于井下惡劣環(huán)境。因此，本文采用RTAB-Map建圖技術(shù)。

目前主流的定位算法包括擴(kuò)展卡爾曼濾波（ExtendedKalmanFilter，EKF）/無損卡爾曼濾波（UnscentedKalmanFilter，UKF）算法、ORB-SLAM算法、PoseNet算法、KartoSLAM算法[16-19]，這些算法對環(huán)境的要求較高，如特征豐富場景、低動態(tài)、光照穩(wěn)定等，應(yīng)用于煤礦井下時存在定位失準(zhǔn)、建圖漂移等現(xiàn)象。自適應(yīng)蒙特卡羅定位（AdaptiveMonteCarloLocalization，AMCL）算法使用車輛激光雷達(dá)獲取地圖信息，結(jié)合車輛自身的運動信息（通常由里程計提供）實現(xiàn)定位，能夠自適應(yīng)調(diào)整定位精度來適應(yīng)車輛的不穩(wěn)定性[20]。因此，本文采用AMCL算法實現(xiàn)井下精確定位，并將RTAB-Map建圖技術(shù)與AMCL算法相結(jié)合，提出一種基于多源信息融合的井下無人駕駛建圖與定位方法。

1SLAM算法原理

1.1基于RTAB-Map的建圖算法

RTAB-Map算法基于優(yōu)化策略求解SLAM問題[21]，其支持RGB-D視覺信息、位姿、激光點云的多源信息輸入，構(gòu)建的地圖具有多樣性。RTAB-Map系統(tǒng)框架如圖1所示，主要包含里程信息估計和建圖兩大部分。為實現(xiàn)車輛里程信息的高精度計算，利用激光雷達(dá)、深度相機(jī)和IMU獲取點云信息、圖像信息和車輛位姿信息，將3種信息進(jìn)行時空同步，對圖像和激光點云源信息進(jìn)行特征提取和特征匹配，并基于IMU與積分參數(shù)對畸變參數(shù)進(jìn)行校正，最后使用EKF實現(xiàn)多源信息融合，實現(xiàn)車輛里程信息高精度估計。為實現(xiàn)煤礦井下巷道的高精度建圖，基于激光雷達(dá)采集的點云信息，構(gòu)建巷道局部地圖，然后基于里程信息實現(xiàn)全局地圖的增量式構(gòu)建[22]；設(shè)計閉環(huán)檢測算法，利用圖形優(yōu)化策略避免無效的閉環(huán)檢測，實現(xiàn)高可靠閉環(huán)優(yōu)化；構(gòu)建全局高匹配地圖，最終輸出實際場景的地圖。

1.2基于AMCL的定位算法

AMCL算法主要由粒子初始化、預(yù)測粒子位姿、更新粒子位姿、重采樣及將重采樣的粒子放入直方圖5個部分組成。

1）粒子初始化。基于傳感器采集的信息確定初始位置，在初始位置周圍構(gòu)建一組粒子，粒子根據(jù)初始位置（均值）和協(xié)方差矩陣定義的高斯分布進(jìn)行采樣，將粒子的權(quán)重值初始化為統(tǒng)一值。AMCL算法性能高度依賴于粒子初始化時設(shè)定的初始位置和粒子數(shù)量。初始位置的選取應(yīng)盡量與實際初始位置吻合。粒子數(shù)量的選取需綜合考慮實時性與定位精度。基于全局坐標(biāo)系OXY，設(shè)置位姿初值為0，粒子初始空間分布如圖2所示。

2）預(yù)測粒子位姿。AMCL粒子預(yù)測的核心是利用車輛當(dāng)前運動位姿推斷下一時刻位姿，包含2個關(guān)鍵步驟：運動更新和采樣。運動更新階段，利用車輛的運動模型推算每個粒子下一時刻的位姿。再從預(yù)測的粒子中采樣，得到新的粒子位姿。粒子位姿預(yù)測如圖3所示，灰色粒子群從車輛里程計信息中獲取，然后轉(zhuǎn)換為紫色粒子群分布。由于估計誤差的存在，紫色粒子群分布發(fā)散性較高。

3）粒子位姿更新。通過傳感器模型計算每個粒子的似然值，更新粒子權(quán)重。粒子位姿更新如圖4所示，粒子顏色越暗，表明權(quán)重越大。

4）粒子重采樣。粒子重采樣前，設(shè)定粒子群短期權(quán)重和長期權(quán)重的平均值不變，并增加隨機(jī)粒子。重采樣階段需維持粒子總數(shù)恒定，依據(jù)權(quán)重值進(jìn)行篩選，淘汰低權(quán)重粒子，同時復(fù)制高權(quán)重粒子。粒子重采樣如圖5所示。

2仿真分析

2.1基于RTAB-Map的建圖仿真

在Gazebo仿真平臺中構(gòu)建井下巷道模擬環(huán)境，開展無人駕駛輔助運輸車輛的地圖構(gòu)建與導(dǎo)航仿真試驗。模擬巷道包含封閉空間的聯(lián)絡(luò)巷、輔運大巷及運輸巷，如圖6所示， L₁-L₆ 為各巷道寬度。

在仿真環(huán)境中定義車輛模型，且加裝激光雷達(dá)、深度相機(jī)、輪式里程計。采用輪式里程計和多源信息融合2種方式獲取車輛里程信息，進(jìn)行對比分析，建圖效果分別如圖7和圖8所示。

印由圖7可看出，基于輪式里程計所建地圖存在錯位，存在多處圖像重影與重疊現(xiàn)象。這是由于低精度車輛輪式里程計信息導(dǎo)致構(gòu)建的立體圖與柵格圖無法統(tǒng)一位置，降低了地圖的準(zhǔn)確性與匹配度。由圖8可看出，基于多源信息融合的RTAB-Map建圖效果顯著改善，有效抑制了漂移現(xiàn)象。

對2種方式的建圖效果進(jìn)行定量分析，結(jié)果見表1與表2，誤差對比如圖9所示。可看出在模擬巷道不同位置處，基于多源信息融合的方法圖測值更加接近實際值，絕對誤差和相對誤差更小。

2.2基于AMCL的定位仿真

在井下巷道仿真環(huán)境中，采用AMCL算法進(jìn)行定位仿真，并通過Rviz可視化平臺驗證定位性能，如圖10所示。其中紅色粒子群（箭頭集）表示無人車可能的位姿，箭頭起點和方向分別對應(yīng)位置和朝向；綠色為預(yù)行駛軌跡。圖10（a）顯示，初始位置粒子分布較發(fā)散，定位精度較低，此時AMCL算法尚無法為無人車提供精確位姿信息。無人車行進(jìn)約 1m 時，定位粒子開始顯著收斂（圖10（b））;行進(jìn) 3m 后，粒子向車輛中心聚集，表明此時位姿已基本確定；圖10（d）所示的穩(wěn)定跟蹤階段，粒子持續(xù)收縮于較小區(qū)域，全程保持高度穩(wěn)定。

車輛行駛過程中，AMCL算法可快速收斂，獲取無人車在地圖中的精確位姿，并在行駛?cè)瘫３至己玫姆€(wěn)定性，滿足井下無人駕駛定位要求。AMCL算法在不同階段的仿真收斂效果見表3，可看出粒子抵達(dá)終點時方差最小，收斂性最好。

3試驗驗證

3.1無人駕駛試驗車及試驗場景搭建

結(jié)合井下巷道真實環(huán)境與樓道模擬巷道環(huán)境，對寬窄樓道進(jìn)行測量，用走廊模擬運輸巷，用房間模擬煤層，如圖11所示。自主搭建輔助運輸車輛模型，如圖12所示，包括激光雷達(dá)、RGB-D相機(jī)、IMU、主控模塊、移動底盤、驅(qū)動電源等。

3.2基于RTAB-Map的建圖試驗

基于輪式里程計和多源信息融合的RTAB-Map建圖試驗效果分別如圖13、圖14所示。由圖13可看出，基于輪式里程計所建地圖整體上與原場景相吻合，但在部分區(qū)域出現(xiàn)漂移現(xiàn)象，這主要是由于里程計存在累計誤差。輪式里程計驅(qū)動的RTAB-Map在短距離內(nèi)建圖精準(zhǔn)，但隨著距離增加，里程計累計誤差導(dǎo)致位姿估計偏差線性增大，最終引發(fā)地圖漂移。由圖14可看出，基于多源信息融合的RTAB-Map建圖整體精度較高，與原場景吻合度較高。

為了對比分析融合多源信息前后RTAB-Map算法的建圖效果，計算絕對誤差和相對誤差絕對值，結(jié)果見表4和表5，誤差對比如圖15所示。可看出融合多源信息后的測量誤差更小。

3.3基于AMCL的定位試驗

基于AMCL的定位試驗效果如圖16所示。由圖16（a）可知，初始位姿時粒子分布松散且范圍大，定位精度較低；行駛約 1m 后，粒子迅速收斂（圖16（b））；移動 2m 后，粒子聚集至車輛中心，表明位姿已基本確定（圖16（c））；圖16（d）所示的跟蹤階段，粒子持續(xù)收縮于較小區(qū)域，全程保持穩(wěn)定。試驗結(jié)果表明：基于AMCL算法的定位粒子能夠快速收斂至較小區(qū)域，具有較高的定位精度。在狹長環(huán)境內(nèi)，依然能夠準(zhǔn)確估計出無人車位姿，滿足無人駕駛輔助運輸車輛的實際定位需求。

AMCL算法在不同階段的收斂效果見表6，可看出粒子抵達(dá)終點時方差最小，收斂性最好。

3.4不同條件下的建圖效果分析

3.4.1不同信息源建圖效果

采用相機(jī)融合激光雷達(dá)、激光雷達(dá)融合IMU、相機(jī)融合IMU的方式采集信息，建圖效果如圖17所示。可看出拐角處易出現(xiàn)不吻合現(xiàn)象，局部存在扭曲問題（圖中紅圈所示）。不同信息源下的建圖誤差見表7，可看出本文提出的多源信息融合方法誤差最小。

3.4.2 不同速度下的建圖效果

參照《煤礦安全規(guī)程》規(guī)定的井下運輸車輛速度限制（運人 25km/h ，運物 40km/h） ，對比速度為10，3 0，40km/h 時的建圖效果，如圖18所示。可看出低速行駛時建圖效果最佳，正常速度下整體建圖效果與低速時差別不大，符合實際工礦場景，能夠滿足正常使用需求。不同速度條件下的建圖誤差見表8。

可看出不同行駛速度下的建圖和定位結(jié)果較接近，不會對車輛安全行駛造成較大干擾。

4結(jié)論

1）針對煤礦井下巷道環(huán)境與實際行駛需求，提出一種基于多源信息融合的井下無人駕駛建圖與定位方法。采用基于多源信息融合的RTAB-Map算法建圖，將點云信息和圖像深度信息融合，實現(xiàn)煤礦井下巷道的高精度建圖。采用AMCL算法實現(xiàn)無人駕駛運輸車輛的精準(zhǔn)定位。

2）仿真及試驗結(jié)果表明：相較單一輪式里程計，基于多源信息融合的RTAB-Map建圖相對誤差絕對值縮減到 1% 以內(nèi)，地圖匹配度更高，提升了建圖可靠性；基于AMCL算法的定位粒子能夠在 2m 內(nèi)迅速收斂，滿足無人駕駛輔助運輸車輛的定位要求。

參考文獻(xiàn)（References）：

[1]王國法.煤礦智能化最新技術(shù)進(jìn)展與問題探討[J].煤 炭科學(xué)技術(shù)，2022，50（1）：1-27. WANG Guofa. New technological progress of coal mine intelligence and its problems[J].Coal Science and Technology，2022，50（1）：1-27.

[2]王陳，鮑久圣，袁曉明，等.無軌膠輪車井下無人駕駛 系統(tǒng)設(shè)計及控制策略研究[J].煤炭學(xué)報，2021，46（增 刊1）：520-528. WANG Chen， BAO Jiusheng，YUAN Xiaoming， et al. Design and control strategy of underground driverless system for trackless rubber tire vehicle[J]. Journal of China Coal Society，2021，46（S1）：520-528.

[3]鮑久圣，章全利，葛世榮，等.煤礦井下無人化輔助運 輸系統(tǒng)關(guān)鍵基礎(chǔ)研究及應(yīng)用實踐[J].煤炭學(xué)報， 2023，48（2）： 1085-1098. BAO Jiusheng，ZHANG Quanli，GE Shirong，et al. Basic research and application practice of unmanned auxiliary transportation system in coal mine[J]. Journal of China Coal Society，2023，48（2）：1085-1098.

[4]韋曉琴.基于激光雷達(dá)的AGV機(jī)器人SLAM與定位 導(dǎo)航研究[D].廣州：華南理工大學(xué)，2019. WEI Xiaoqin. Research on SLAM，localization and navigation of an AGV robot based on laser sensors [D]. Guangzhou： South China University of Technology， 2019.

[5]王陳.井下無人駕駛無軌膠輪車自主建圖與導(dǎo)航技術(shù) 研究[D].徐州：中國礦業(yè)大學(xué)，2022. WANG Chen. Research on autonomous mapping and navigationtechnologyofundergrounddriverless tracklessrubber-tyred vehicle[D]. Xuzhou：China University of Mining and Technology， 2022.

[6]李仲強(qiáng).煤礦救援機(jī)器人自主建圖和導(dǎo)航技術(shù)研究 [D].淮南：安徽理工大學(xué)，2019. LI Zhongqiang. Research on self-construction and navigation technology of coal mine rescue robot[D]. Huainan： Anhui University of Science amp; Technology， 2019.

[7］劉停.基于機(jī)器視覺的井下救援探測機(jī)器人路徑規(guī)劃 研究[D].徐州：中國礦業(yè)大學(xué)，2017. LIU Ting. Research on path planning of mine rescue detection robot based on machine vision[D]. Xuzhou： China University of Mining and Technology， 2017.

[8]胡青松，李敬雯，張元生，等.面向礦井無人駕駛的 IMU與激光雷達(dá)融合SLAM技術(shù)[J].工礦自動化， 2024，50（10）：21-28. HU Qingsong，LI Jingwen， ZHANG Yuansheng， et al. IMU-LiDARintegratedSLAMtechnologyfor unmanned driving in mines[J]. Journal of Mine Automation，2024，50（10）： 21-28.

[9]田野，陳宏巍，王法勝，等.室內(nèi)移動機(jī)器人的 SLAM 算法綜述[J].計算機(jī)科學(xué)，2021，48（9）：223-234. IIAN Ye，CHEN HongWeI， WANG Fasheng，et al. Overview of SLAM algorithms for mobile robots[J]. Computer Science，2021，48（9）： 223-234.

[10]MARTIN F，MORENO L，BLANCO D， et al. Kullback-Leibler divergence-based global localization formobile robots[J]．Roboticsand Autonomous Systems，2014， 62（2）：120-130.

[11]韓彥峰，李君君，肖科.基于優(yōu)化Gmapping 算法的巷 道噴漿機(jī)器人建圖研究[J].湖南大學(xué)學(xué)報（自然科學(xué) 版），2023，50（6）：118-126. HAN Yanfeng，LI Junjun，XIAO Ke. Research on mapping of shotcrete robot in roadway based on optimized gmapping algorithm[J]. Journal of Hunan University（Natural Sciences），2023，50（6）： 118-126.

[12]汪建華，黃磊，石雨婷，等.基于優(yōu)化Hector-SLAM算 法的機(jī)器人自主導(dǎo)航系統(tǒng)設(shè)計[J].工程設(shè)計學(xué)報， 2023，30（6）：678-686. WANG Jianhua，HUANG Lei， SHI Yuting，et al. Design of robot autonomous navigation system based on optimized Hector-SLAM algorithm[J]. Chinese Jourmal of Engineering Design， 2023，30（6）： 678-686.

[13]李芳威，鮑久圣，王陳，等．基于LD改進(jìn) Cartographer建圖算法的無人駕駛無軌膠輪車井下 SLAM自主導(dǎo)航方法及試驗[J].煤炭學(xué)報，2024， 49（增刊2）：1271-1284. LI Fangwei，BAO Jiusheng，WANG Chen，et al. Unmanned trackless rubber wheeler based onLD improved Cartographer mapping algorithm underground SLAM autonomous navigation method and test[J]. Journal of China Coal Society，2024，49（S2）： 1271-1284.

[14]侯可馨，李霽翔，鄒騰躍，等.基于Karto 算法的割草 機(jī)器人建圖及其運動畸變校正[J].南方農(nóng)機(jī)，2022， 53（14）： 1-4. HOU Kexin， LI Jixiang， ZOU Tengyue， et al. Drawing and motion distortion correction of mowing robot based on Karto algorithm[J]. China Southern Agricultural Machinery，2022， 53（14）： 1-4.

[15]崔邵云，鮑久圣，胡德平，等.SLAM技術(shù)及其在礦山 無人駕駛領(lǐng)域的研究現(xiàn)狀與發(fā)展趨勢[J].工礦自動 化，2024，50（10）：38-52. CUI Shaoyun， BAO Jiusheng，HU Deping， et al. Research status and development trends of SLAM technology in autonomous mining field[J]. Journal of Mine Automation，2024，50（10）：38-52.

[16]劉燕，張健，肖慶高，等.UKF與EKF在導(dǎo)航定位中的 對比研究[J].微處理機(jī)，2023，44（4）：30-33. LIU Yan，ZHANG Jian，XIAO Qinggao，et al．A Upaiauve stuuy UI UNr' anu LΛr I Havigauun anu positioning[J].Microprocessors，2023，44（4）： 30-33.

[17]尚明姝，王克朝，高玉寶.一種基于 SURF-ORB 的改 進(jìn)圖像配準(zhǔn)算法[J].計量學(xué)報，2025，46（3）：323-328. SHANG Mingshu，WANG Kechao，GAO Yubao. An improved image registration algorithm based on SURFORB[J]．Acta Metrologica Sinica，2025，46（3）： 323-328.

[18]王珊，張雙寒，高靖楠，等.基于PoseNet 模型實現(xiàn)人 體姿態(tài)識別[J].電腦編程技巧與維護(hù)，2022（1）：112- 113， 135. WANG Shan， ZHANG Shuanghan， GAO Jingnan， et al. Realization of human posture recognition based on PoseNet model[J].Computer Programming Skillsamp; Maintenance，2022（1）： 112-113，135.

[19]李澤龍，張小俊，王曉陽，等.基于改進(jìn)AMCL的 AGV激光定位技術(shù)研究[J/OL].激光雜志：1-10 [2025-03-16]. htp： //kns.cnki.net/kcms/detail/50.1085. TN.20240926.1432.008.html. LI Zelong， ZHANG Xiaojun， WANG Xiaoyang， et al. Research on AGV laser positioning technology based on improved AMCL[J/OL]. Jourmal of Lasers： 1-10[2025- 03-16].htp：/ns.cnki.net/kcms/detail/50.1085.TN. 20240926.1432.008.html.

[20]薛光輝，李瑞雪，張鈣昊，等.基于3D激光雷達(dá)的 SLAM算法研究現(xiàn)狀與發(fā)展趨勢[J].信息與控制， 2023， 52（1）： 18-36. XUE Guanghui， LI Ruixue， ZHANG Zhenghao，et al. State-of-the-art and tendency of SLAM algorithms based on3D LiDAR[J].Information and Control，2023， 52（1）： 18-36.

[21]趙文金，朱子恒，吳云雁.基于 SLAM導(dǎo)航的煤礦井下 機(jī)器人設(shè)計[J].山東煤炭科技，2024，42（2）：161-165. ZHAO Wenjin， ZHU Ziheng， WU Yunyan. Design of coalminedownholerobotbasedonSLAM navigation[J]. Shandong Coal Science and Technology， 2024，42（2）：161-165.

[22]崔邵云，鮑久圣，李芳威，等.基于多源里程融合的井 下無人駕駛自主導(dǎo)航SLAM方法［J/OL].煤炭科學(xué) 技術(shù)：1-10[2025-03-13].http：//kns.cnki.net/kcms/detail/ 11.2402.td.20240925.1231.002.html. CUI Shaoyun， BAO Jiusheng，LI Fangwei，et al. Autonomous navigation SLAM method for underground unmanned driving based on multi-source mileage fusion scenarios[J/OL]. Coal Science and Technology：1-10 [2025-03-13]. htp：//kns.cnki.net/ kcms/detail/11.2402. td.20240925.1231.002.html.