基于擴展卡爾曼濾波的清倉機器人位姿識別方法

摘要：煤礦水倉巷道光照強度不均勻且結構化特征明顯，傳統基于視覺的機器人位姿識別方法識別不準確，而單一的機器人定位技術如自適應蒙特卡洛（AMCL）方法隨著清倉機器人的長時間運行，輸出的位姿信息存在較大累計誤差，易出現煤泥清理不干凈、與兩側巷道發生碰撞的情況。針對上述問題，提出了一種基于擴展卡爾曼濾波的多傳感器融合清倉機器人位姿識別方法。首先搭建多傳感器融合算法框架，建立里程計、慣性測量裝置、激光雷達數據采集模型；其次基于擴展卡爾曼濾波原理，以慣性測量裝置角度信息建立觀測方程，結合里程計位姿信息，得到第1 次融合的清倉機器人位姿矩陣，利用激光雷達的位置信息與之前的位姿矩陣進行迭代，得到第2 次融合的清倉機器人位姿矩陣；最后采用互補濾波算法對融合后的清倉機器人位姿矩陣進行處理，輸出最終的清倉機器人位姿矩陣。實驗結果表明：在直線位姿識別中2 次的最大位置誤差為0.04 m，最大姿態角誤差為0.05 rad；在模擬巷道實驗中的最大位置誤差為0.1 m，最大姿態角誤差為0.085 rad；與AMCL 方法相比，基于擴展卡爾曼濾波的清倉機器人位姿識別方法在減少清倉機器人運行過程中的累計誤差方面表現出顯著的有效性。

關鍵詞：煤礦水倉清理；清倉機器人；機器人位姿識別；多傳感器融合；擴展卡爾曼濾波；煤泥自主清理

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

0 引言

根據國家統計局2022 年數據顯示，煤炭在我國能源結構中占比為56%，是我國能源體系中的主要供應來源[1]。在煤炭開采過程中，水害被列為五大災害之一[2]。水倉是防止水害事故發生的重要措施，水倉沉積物的累積會降低水倉容積，使水倉失去蓄水作用，提高水害發生風險。《煤礦安全規程》規定：每年雨季前必須清理一次水倉。

目前常見的清倉方式主要有人工清倉、水力清倉、機械清挖等。人工清倉是目前主流的清倉方式[3]，需10～12 名清倉工人連續清理2～3 個月才能完成，清倉效率低且存在沖倉、漫倉等風險。水力清倉是利用高壓水槍不斷沖擊煤泥淤積層，將煤泥進行稀釋后通過排水泵排出，這種清倉方式在一定程度上降低了工人勞動強度，操作簡單，但需在煤泥清理過程中注入大量水，進一步增加了水害發生的風險。機械清挖是通過裝載機、挖掘機等設備將煤泥裝入礦車，或采用壓濾工藝將煤泥脫水后通過帶式輸送機運輸，這種清倉方式清理效率高，但在清理過程中使用了大量設備，極大占用巷道工作空間，易對煤礦正常開采造成影響。因此，設計一種體積小、自動化程度高的煤礦水倉清理機器人勢在必行。隨著國家“機械化換人、自動化減人、安全至上”戰略的提出，越來越多學者投入到清倉機器人研發過程中。郭培紅等[4]設計了一種煤泥清挖泵送一體的清倉機，實現了水倉煤泥清理機械化，但在使用過程中需不斷鋪設管道，管道拆裝成本高。宋峰等[5]設計了一款集煤泥清理、輸送為一體的清倉機器人組，將清倉機定點安置在水倉口，通過裝載機對煤泥進行轉運，無需在水倉中鋪設管道，降低了清倉成本，但整套系統體積龐大，易對煤礦正常開采造成影響。 姚貴英等[6]提出了一種可自行行走的水倉清理機器人，實現了對煤泥水的自主篩分、脫水，但整機自動化程度低。以上研究主要針對清倉機器人機械結構方面，清倉機器人在工作過程中仍需要依靠工人跟機操作，無法實現對煤泥的自主清理。

機器人位姿識別是實現清倉機器人自主清理煤泥的前提。目前位姿識別方法大多基于視覺識別：姚立健等[7]提出了一種基于改進型廣義Hough 變換的位姿檢測方法，通過“形狀相似度”法縮小檢測范圍，提高檢測效率，但擴大了小目標物體的垂直視差；陳東旭等[8]針對焊接機器人位姿識別問題，提出了一種基于線結構光的位姿識別方法，結合圖像處理工具獲得位姿信息，這種方法位姿識別精度高，但適用范圍有限，無法對復雜曲面進行有效檢測；馬斌等[9]采用單目視覺技術實現對鉆桿的位姿識別， 提高了鉆桿裝卸自動化水平，但單目視覺容易出現圖形畸變，導致偏差范圍增加。由于水倉巷道中光照強度不均勻，視覺環境受光源布置影響較大，所以基于視覺的位姿識別方法在大多數情況下無法滿足清倉機器人的使用要求。

機器人定位技術是提高清倉機器人位姿識別精度的關鍵技術。目前常見的機器人定位方法主要包括GPS 定位、地磁定位、無限載波通信定位等。井下處于高巖土覆蓋層，無法接收GPS 信號 [10-11]。地磁定位對外界磁場要求較高，由于井下電磁設備對磁場干擾較大，無法滿足機器人定位精度需求[12]。無線載波通信抗干擾能力強、靈活性高，適用于大多數場景的機器人定位[13]，但在使用過程中需提前布置無線網絡，成本較高。

受井下復雜環境的影響，傳統定位方法無法使用，通過傳感器對井下環境識別并進行同步定位與建圖（Simultaneous Localization and Mapping， SLAM）是解決井下定位困難問題的有效手段[14]。D. Fox等[15]首先將粒子濾波應用于機器人定位中，提出自適應蒙特卡羅定位（Adaptive Monte Carlo Localization，AMCL）方法，該方法在機器人定位過程中對采樣粒子依賴嚴重，當采樣粒子出現退化時，機器人本體姿態會出現無法彌補的誤差，魯棒性差。王寧等[16]利用重采樣與相對熵交替采樣對AMCL 進行改進，有效降低了粒子退化，但無法消除機器人長期運行過程中產生的累計誤差。馮佳萌等[17]通過增加掃描匹配和傅里葉濾波來優化AMCL 算法，提高了算法的定位精度和魯棒性，算法粒子退化現象得到了緩解，但該方法只適用于初始位姿精確的模型，對于初始位姿信息缺失的機器人定位易出現無法收斂的情況。

為了提高清倉機器人位姿識別的準確性和減少累計誤差，本文提出了一種基于擴展卡爾曼濾波（Extended Kalman Filter，EKF）的多傳感器融合的清倉機器人位姿識別方法。首先選擇里程計、慣性測量裝置和激光雷達作為數據來源，搭建位姿識別系統框架，設計傳感器布置方式并建立傳感器觀測模型，然后綜合考慮不同傳感器性質建立觀測方程，基于EKF 算法對里程計、慣性測量裝置、激光雷達數據進行2 次融合，最后結合2 次融合的位姿信息，通過互補濾波的方式得到清倉機器人高精度位姿。

1 清倉機器人位姿識別系統結構

本身位姿進行識別，其位姿識別系統由硬件層、軟件層和工具層組成，如圖1 所示。硬件層是機器人的眼睛，包括里程計、激光雷達和慣性測量裝置3 個部分，負責數據采集。軟件層是位姿識別系統的核心，基于機器人操作系統（Robot Operating System，ROS）對傳感器驅動進行配置并實現位姿識別算法。工具層用于顯示定位結果、測算定位精度等，包括日志輸出、人機交互、可視化、數據錄制、數據處理等工具。

2 傳感器數據采集

清倉機器人上的傳感器布置如圖2 所示，將激光雷達安裝在清倉機器人算法控制箱上方，采集激光點云數據；在履帶兩側馬達處分別安裝2 個里程計，收集清倉機器人行走過程中的速度、姿態數據；慣性測量裝置安裝在算法控制箱中，負責測量清倉機器人運行過程中的加速度、角速度數據；控制主板處理激光點云數據、里程計數據和慣性測量數據，得到清倉機器人位姿信息，控制清倉機器人對水倉進行清理。在清倉機器人位姿識別過程中選擇慣性測量裝置安裝平面中心點作為衡量位姿變化的觀測點。

2.1 里程計數據采集

里程計采樣頻率較高且在短時間內具有較高的定位精度，是機器人定位常用的傳感器之一。里程計通過檢測霍爾編碼器高低電平的個數計算里程計運動圈數，從而判斷機器人的位移，并結合左右兩側履帶移動距離差推算機器人位姿變化值[18]。

清倉機器人里程計運動模型如圖3 所示，將機器人看作巷道平面內運動的一個剛體[19]，以巷道一端中心點為原點，建立全局坐標系XwYwOw；以機器人質心M 為原點，建立本體坐標系XRYROR，2 個坐標系夾角為姿態角，將機器人位姿信息寫成1×3矩陣。

t 時刻機器人在M 點處的速度為

式中： v （t）為機器人在 M 點的線速度； vL; vR分別為左、右輪線速度，通過霍爾編碼器測量；ω（t）為機器人在M 點的角速度； 為機器人寬度。

里程計輸出的機器人位姿為

式中：qod為里程計輸出的位姿矩陣；εod為里程計誤差。

2.2 慣性測量裝置數據采集

慣性測量裝置主要通過加速度計和陀螺儀獲得清倉機器人的加速度、角速度信息，通過對加速度的2 次積分獲得清倉機器人的位置信息[20]，采用方向余弦矩陣對姿態角信息進行解算。

對加速度進行第1 次積分得到清倉機器人運行速度信息。

式中：vx， vy， vz分別為清倉機器人在x，y，z 方向速度；ax，ay，az分別為清倉機器人繞x，y，z 軸的加速度，可通過加速度計測量得到。

由于清倉機器人在巷道平面內運動，在z 方向上速度為0，只需對x， y 方向上的速度進行第2 次積分，即可得到機器人位置信息。

式中ximu， yimu為慣性測量裝置輸出的位置信息。

采用方向余弦矩陣對姿態角信息進行解算，將清倉機器人看成對坐標軸的旋轉，對方向余弦矩陣求導得到姿態角導數的微分方程。

式中：α，β，γ為姿態角的導數；ωx，ωy，ωz分別為清倉機器人繞x，y，z 軸的角速度，可通過陀螺儀測量得到；α，β，γ分別為清倉機器人繞x，y，z 軸旋轉的姿態角。

清倉機器人初始姿態角α0，β0，γ0為

式中g為重力加速度。

根據清倉機器人初始姿態角，采用龍格庫塔法對微分方程進行求解，得到清倉機器人的姿態角。

選取清倉機器人在x，y 方向上的位置數據和繞z 軸旋轉角作為姿態數據，建立位姿矩陣，慣性測量裝置輸出的機器人位姿為

式中：qimu為慣性測量裝置輸出的位姿矩陣；εimu為慣性測量誤差。

2.3 激光雷達數據采集

激光雷達通過發射激光束并收集返回數據獲取環境中物體的位置信息，實現高精度環境地圖的構建，為機器人提供精確、可靠的位置信息，在機器人位姿識別過程中起著重要的作用[21]。激光雷達輸出值為極坐標系下激光雷達障礙物距離與角度信息。

Llaser = {（li，φi） ，"i = 1，2，…}"（9）

式中：li 為激光雷達與障礙物距離； φi 為激光雷達與障礙物的偏轉角度，即測量姿態角。

將Llaser轉換至直角坐標系：

式中xlaser; ylaser分別為激光雷達輸出的直角坐標系下坐標。

激光雷達輸出的機器人位姿為

式中： qlaser為激光雷達輸出的位姿矩陣； εlaser為激光雷達測量誤差。

3 多傳感器融合機器人位姿識別方法

3.1 AMCL 算法

AMCL 算法將機器人位姿看成粒子在地圖上的概率分布，利用里程計運動模型和激光雷達數據獲取清倉機器人在已知地圖環境下的位姿信息[22]。

在實際應用中，首先對AMCL 系統進行初始化，設置粒子濾波器的狀態及粒子權重，利用里程計得到機器人初始位姿信息，使用里程計數據更新粒子濾波器狀態，然后通過激光雷達數據與環境地圖信息進行對比，更新粒子濾波器中粒子權重，最后根據粒子權重進行重采樣，使用高權重粒子替代低權重粒子，得到機器人位姿信息。

3.2 基于EKF 的多傳感器融合定位算法

由于AMCL 在粒子采集過程中選用正態分布假設，易導致采集粒子出現偏差，這種偏差在短時間內對位姿影響不大，但隨著清倉機器人長時間運行，這種偏差會逐漸累加，導致定位精度差，甚至是無法定位。針對上述問題，本文基于EKF[23]提出一種多傳感器融合的位姿識別方法，以解決粒子濾波中粒子采集偏差問題，實現清倉機器人在長時間運行中對位姿的高精度識別。

3.2.1 EKF 基本原理

EKF 基本原理如圖4 所示，首先將非線性系統通過Taylor 公式得到近似線性化系統，其次通過預測步建立預測狀態方程并計算預測誤差協方差矩陣，然后通過更新步計算卡爾曼增益，并更新后驗狀態矩陣和后驗誤差協方差矩陣，最后得到系統最優狀態估計。

3.2.2 多傳感器融合算法設計

多傳感器融合算法原理如圖5 所示，基于EKF原理，對里程計、慣性測量裝置、激光雷達數據進行2 次融合，然后通過一階互補濾波結合融合后的數據得到清倉機器人的位姿矩陣。

具體步驟如下：

1） 對里程計數據和慣性測量數據進行處理，減小里程計打滑引起的誤差；根據EKF 基本原理，得到里程計系統狀態函數。

qt+1 = Aqodt + But （12）

式中：qt+1為t+1時刻清倉機器人狀態矩陣；A為系統狀態矩陣，將里程計數據轉換成清倉機器人位姿信息；qodt為t時刻清倉機器人狀態矩陣；B為控制變量矩陣，將系統控制向量轉換成位姿信息；ut為系統控制向量，包括t時刻里程計輸出的線速度、角速度等。

采用慣性測量裝置采集的數據作為觀測數據，建立觀測方程：

式中himu為慣性測量裝置觀測矩陣。

式中I為單位矩陣。

2） 將里程計與慣性測量裝置融合后的數據作為清倉機器人的狀態信息，建立激光雷達觀測方程：

式中σ1，σ2分別為第1 次融合和第2 次融合的權重因子， 由于第2 次融合得到的位姿信息中包含有第1 次融合信息，且激光雷達測距精度高，取σ1= 0.2，σ2= 0.8。

4 實驗研究

4.1 實驗平臺搭建

實驗系統選擇Ubuntu18.04 長期支持版，ROS 版本選擇Melodic，上位機采用VirtualBox 燒錄Ubuntu18.04系統安裝ROS-Noetic 長期支持版，機器人由小車底盤、激光雷達及慣性測量裝置等組成。激光雷達頻率為5 Hz，慣性測量裝置頻率為100 Hz，里程計采用霍爾編碼器，實驗平臺如圖6 所示。

選用長為16.8 m、最大寬度為5 m、最小寬度為2.8 m 的實驗環境模擬井下巷道環境，在燈光昏暗的條件下進行清倉機器人位姿識別實驗，環境平面圖如圖7 所示。

4.2 機器人位姿識別實驗

將樹莓派與上位機連接在同一局域網，上位機通過遠程登錄服務連接樹莓派并通過本機遠程桌面進行登錄。在Rviz 中分別訂閱Map， Pose-array，Path，Odom，Scan 等相關節點對機器人上的傳感器數據進行可視化顯示。實驗主要包括沿直線方向位姿識別精度測試和沿模擬巷道位姿識別精度測試。

基于Gmapping 算法[24]構建全局一致性地圖（圖8），紅色坐標軸為x 軸，綠色坐標軸為y 軸；黑色輪廓線為定位前建立的靜態地圖，青色包裹層區為膨脹層，其目的是防止機器人與兩側巷道發生碰撞。

4.2.1 沿直線位姿識別精度測試

為測試實驗小車硬件系統的可靠性和位姿識別算法的穩定性，選擇在x 方向上進行位姿識別精度測試（圖9） ，控制機器人沿x 軸行走10 m 并返回。由圖9（a） 可看出， 在機器人運行到10 m 位置時，AMCL 定位結果為10.132 6 m，本文方法定位結果為10.045 2 m；由圖9（b）可看出，AMCL 在y 方向上的最大定位誤差為0.17 m，平均位置誤差為0.08 m，本文方法可將位置誤差控制在0.04 m 之內；由圖9（c）可看出，采用AMCL 獲得的最大姿態角誤差為0.15 rad，平均姿態角誤差為0. 08 rad，采用本文方法獲得最大姿態角誤差為 0.05 rad，平均姿態角誤差為0.03 rad。

4.2.2 模擬巷道位姿識別精度測試

為模擬清倉機器人實際運行過程中可能遇到的各種情況，提高位姿識別算法的有效性和實用性，在模擬巷道中進行位姿識別精度測試。

在模擬巷道進行軌跡預設，如圖10 所示。通過上位機鍵盤控制節點來控制清倉機器人沿著預設軌跡運動，在模擬巷道中選取5 個觀測點對位姿識別結果進行定量分析，結果見表1、表2。

位置誤差采用歐氏距離公式計算，姿態角誤差用真實姿態角與測量姿態角差的絕對值表示。

式中：d 為位置誤差；（xi，yi）為測量坐標；（x，y）真實坐標； 為角度誤差；φ為真實姿態角。

位置誤差和姿態角誤差對比分析見表3，可看出AMCL 定位的最大位置誤差為0.19 m，最大姿態角誤差為0.165 rad，平均位置誤差為0.126 m，平均姿態角誤差為0.104 rad；本文方法的最大位置誤差為0.1 m，最大姿態角誤差為0.085 rad，平均位置誤差為0.049 m，平均姿態角誤差為0.045 rad。說明多傳感器融合與位姿優化方法能有效提高清倉機器人位姿識別精度。

5 結論

1） 搭建位姿識別算法框架，建立里程計、慣性測量裝置、激光雷達數據采集模型，基于EKF 對傳感器數據進行2 次融合，通過一階互補濾波算法對2 次融合的位姿結果進行處理，得到高精度清倉機器人位姿。

2） 沿直線位姿識別精度的實驗結果表明，多傳感器融合與位姿優化的方法在清倉機器人直線行走中可將位置誤差控制在0.04 m 之內，將姿態角誤差控制在0.05 rad 之內。

3） 模擬巷道位姿識別精度的實驗結果表明，多傳感器融合與位姿優化的方法在模擬巷道實驗中，平均位置誤差為0.049 m，平均姿態角誤差為0.045 rad。

參考文獻（References）：

[ 1 ]賈建稱，賈茜，桑向陽，等. 我國煤礦地質保障系統建設30 年： 回顧與展望[J]. 煤田地質與勘探， 2023，51（1）：86-106.

JIA Jiancheng， JIA Qian， SANG Xiangyang， et al.Review and prospect of coal mine geological guaranteesystem in China during 30 years of construction[J]. CoalGeology amp; Exploration，2023，51（1）：86-106.

[ 2 ]曾一凡，武強，趙蘇啟，等. 我國煤礦水害事故特征、致因與防治對策[J]. 煤炭科學技術，2023，51（7）：1-14.

ZENG Yifan， WU Qiang， ZHAO Suqi， et al.Characteristics，causes，and prevention measures of coalmine water hazard accidents in China[J]. Coal Scienceand Technology，2023，51（7）：1-14.

[ 3 ]石軍杰，高貴軍，游青山，等. 煤礦井下水倉清理機器人系統設計與應用[J]. 煤炭工程， 2022， 54（11） ：205-208.

SHI Junjie， GAO Guijun， YOU Qingshan， et al. Waterbin cleaning robot system for underground coal mine[J].Coal Engineering，2022，54（11）：205-208.

[ 4 ]郭培紅，薛蛟生，朱建安，等. 全液壓水倉煤泥清挖泵送一體機研制[J]. 煤礦機械，2015，36（1）：148-150.

GUO Peihong， XUE Jiaosheng， ZHU Jian'an， et al.Development of cleaning-pumping combined machinewith full hydraulic control for coal slime in watersump[J]. Coal Mine Machinery，2015，36（1）：148-150.

[ 5 ]宋峰，蒲仁利，鐘靈敏，等. 一種礦用水倉清淤系統：CN202210801796.5[P]. 2022-09-02.

SONG Feng，PU Renli，ZHONG Lingmin，et al. A minewater bin dredging system： CN202210801796.5[P].2022-09-02.

[ 6 ]姚貴英，曹夢媛，馬婧. 煤礦水倉清理機器人研究與設計[J]. 煤礦機械，2020，41（2）：4-6.

YAO Guiying，CAO Mengyuan，MA Jing. Research anddesign of coal mine sump cleaning robot[J]. Coal MineMachinery，2020，41（2）：4-6.

[ 7 ]姚立健， 丁為民， 張培培， 等. 基于改進型廣義Hough 變換的茄子果實位姿識別方法[J]. 農業工程學報，2009，25（12）：128-132.

YAO Lijian， DING Weimin， ZHANG Peipei， et al.Recognition method of position and attitude of eggplantfruits based on improved generalized Houghtransforms[J]. Transactions of the Chinese Society ofAgricultural Engineering，2009，25（12）：128-132.

[ 8 ]陳東旭，趙鐵軍，喬赫廷. 基于線結構光的焊縫位姿識別研究[J]. 機械工程與自動化，2021（4）：38-40，43.

CHEN Dongxu，ZHAO Tiejun，QIAO Heting. Researchon weld position-posture recognition based on linestructured light[J]. Mechanical Engineering amp;Automation，2021（4）：38-40，43.

[ 9 ]馬斌，彭光宇. 基于單目視覺的鉆桿位姿識別技術研究[J]. 煤田地質與勘探，2022，50（10）：171-178.

MA Bin， PENG Guangyu. Research on drill pipe poserecognition technology based on monocular vision[J].Coal Geology amp; Exploration，2022，50（10）：171-178.

[10]KEDONG W. A new algorithm for fine acquisition ofGPS carrier frequency[J]. GPS Solutions，2014，18（4）：581-592.

[11]黃西平，楊飛. 綜采工作面巡檢機器人自主定位方法[J]. 工礦自動化，2023，49（4）：86-91.

HUANG Xiping， YANG Fei. Autonomous positioningmethod for inspection robots in fully mechanizedworking face[J]. Journal of Mine Automation， 2023，49（4）：86-91.

[12]陸一，魏東巖，紀新春，等. 地磁定位方法綜述[J]. 導航定位與授時，2022，9（2）：118-130.

LU Yi， WEI Dongyan， JI Xinchun， et al. Review ofgeomagnetic positioning method[J]. NavigationPositioning and Timing，2022，9（2）：118-130.

[13]崔苗，喻鑫，李學易，等. 多載波無線攜能通信的上下行鏈路聯合資源分配[J]. 通信學報，2019，40（3）：206-214.

CUI Miao，YU Xin，LI Xueyi，et al. Joint downlink anduplink resource allocation for multi-carrier SWIPTsystem[J]. Journal on Communications， 2019， 40（3） ：206-214.

[14]SMITH R， SELF M， CHEESEMAN P. Estimatinguncertain spatial relationships in robotics[J]. MachineIntelligence amp; Pattern Recognition， 1988， 5（5） ：435-461.

[15]FOX D， BURGARD W， DELLAERT F， et al. Montecarlo localization： efficient position estimation formobile robots[C]. Sixteenth National Conference onArtificial Intelligence，Orland，1999.

[16]王寧，王堅，李麗華. 一種改進的AMCL 機器人定位方法[J]. 導航定位學報，2019，7（3）：31-37.

WANG Ning， WANG Jian， LI Lihua. An improvedadaptive monte carlo localization method for robot[J].Journal of Navigation and Positioning， 2019， 7（3） ：31-37.

[17]馮佳萌，裴東，鄒勇，等. 基于機器人激光定位的一種改進AMCL 算法[J]. 激光與光電子學進展， 2021，58（20）：479-487.

FENG Jiameng， PEI Dong， ZOU Yong， et al. Animproved AMCL algorithm based on robot laserlocalization[J]. Laser amp; Optoelectronics Progress，2021，58（20）：479-487.

[18]馬先重. 基于多傳感器融合的室內移動機器人定位及障礙物檢測與測量研究[D]. 武漢：武漢科技大學，2021.

MA Xianchong. Research on localization and obstacledetection and measurement of indoor mobile robot basedon multi sensor fusion[D]. Wuhan：Wuhan University ofScience and Technology，2021.

[19]金書奎，寇子明，吳娟. 煤礦水泵房巡檢機器人路徑規劃與跟蹤算法的研究[J]. 煤炭科學技術， 2022，50（5）：253-262.

JIN Shukui， KOU Ziming，WU Juan. Research on pathplanning and tracking algorithm of inspection robot incoal mine water[J]. Coal Science and Technology，2022，50（5）：253-262.

[20]宗意凱，蘇淑靖，高瑜宏. 基于多源IMU 和粒子濾波優化的姿態融合算法[J]. 儀表技術與傳感器，2023（8）：88-95.

ZONG Yikai， SU Shujing， GAO Yuhong. Attitudefusion algorithm based on multi-source IMU and particlefilter optimization[J]. Instrument Technique andSensor，2023（8）：88-95.

[21]沈斯杰， 田昕， 魏國亮， 等. 基于2D 激光雷達的SLAM 算法研究綜述[J]. 計算機技術與發展，2022，32（1）：13-18，46.

SHEN Sijie，TIAN Xin，WEI Guoliang，et al. Review ofSLAM algorithm based on 2D lidar[J]. ComputerTechnology and Development，2022，32（1）：13-18，46.

[22]PENG Gang， ZHENG Wei， LU Zezao， et al. Animproved AMCL algorithm based on laser scanningmatch in a complex and unstructured environment[J].Complexity，2018（5）：1-11.

[23]ARASARATNAM I， HAYKIN S， ELLIOTT R J.Discrete-time nonlinear filtering algorithms using gausshermitequadrature[J]. Proceedings of the IEEE， 2007，95（5）：953-977.

[24]SU Zhifeng， ZHOU Jiehua， DAI Jiyang， et al.Optimization design and experimental study ofgmapping algorithm[C]. Chinese Control and DecisionConference， Hefei， 2020. DOI： 10.1109/CCDC49329.2020.9164603.

基金項目：山西省科技合作交流專項項目（202104041101005）；山西省重點研發計劃項目（202102100401004）。