礦用巡檢機器人自主定位技術研究

＊趙洪輝 李耀龍 張業奔

（1.陜西陜煤榆北煤業有限公司榆林信息化運維分公司 陜西 719000 2.天地（常州）自動化股份有限公司 江蘇 213015 3.中煤科工集團常州研究院有限公司 江蘇 213015）

關鍵字：煤礦智能化；巡檢機器人；擴展卡爾曼濾波；自主定位

煤礦智能化建設[1-4]的最根本的目標是煤礦的安全生產，其中對井下設備和環境進行巡檢是安全生產的重要保障[5-7]。由于煤礦井下環境復雜，特別是在一些采掘工作面和運輸巷道，存在水霧粉塵彌漫、通風條件不良等一系列惡劣因素，導致人工巡檢難度大[8]。為確保煤礦的安全生產，通過使用巡檢機器人可以有效地減小人工巡檢所帶來的風險[9]。礦用巡檢機器人在工作時，需要準確且實時的獲取自身當前位置，但由于地下環境無法穩定的接收GNSS信號，使得RTK等定位技術難以得到部署和應用[10-12]。針對這一問題，本文通過輪速計和IMU的組合定位方式，以IMU高頻數據對輪速計信息進行校正，防止因單一傳感器帶來的累計誤差問題，并使用擴展卡爾曼濾波算法融合兩者信息，實現煤礦井下的高精度定位。

1.傳感器模型建立

本文使用輪速計和IMU實現巡檢機器人在煤礦井下的定位，因此必須通過對這兩種傳感器構建合理的數學模型，以便于后續的信息融合。

輪速計是靠編碼器計算車輪轉動信息來計算巡檢機器人位姿的變化量，其速度信息的導數用于計算俯仰角和滾轉角，同時輪速計的速度信息用于與IMU一起計算位姿。實驗中使用的巡檢機器人所配備的驅動電機參數如表1所示。

表1 驅動電機參數Tab.1 Driving motor parameters

機器人所涉及到的各參量表示如下：△dL為左輪位移，△dR為右輪位移。u=[△D,△θ]為輪速計的輸入量。位移的相對變化量△D和偏航角的相對變化量△θ的轉化關系如式（1）所示：

由上式可以近似得出巡檢機器人的輪速計模型為：

輪速計定位采用伺服電機速度反饋信號來推算巡檢機器人運行過程中的實時位置，通過速度積分方法實時獲取其位置。輪速定位的主要優勢在于它可以為移動機器人提供自主定位，而無需依賴外部信息。但輪速計會因為運動時間的增長導致其定位產生誤差也會不斷累積。

為了解決該問題，需要使用IMU對輪速計信息進行補償，旨在通過IMU提供的高頻姿態信息對輪速計的累積誤差進行消除。IMU是一種組合傳感器設備，生成的數據（取決于IMU的類型）分別涉及加速度、角速度以及三個方位，對IMU數據解算后就可以獲取由IMU提供的里程計信息。假設巡檢機器人的初始位置為p0=(0,0,0)T，初始姿態表示為φ0=(0,0,0)T，R(φ0)表示初始姿態對應的旋轉矩陣。歐拉角對應的旋轉矩陣為R(△φ)。進行多次迭代后，可以得出IMU模型為：

式中，ωk表示在k時刻IMU所測量的角速度值；Rk為在k時刻時IMU在世界坐標系下的姿態。

2.傳感器信息融合算法研究

對于輪速計和IMU模型進行建立后，本文選用擴展卡爾曼濾波算法實現融合輪速計和IMU數據信息，擴展卡爾曼濾波一種強大的定位工具，它可以接受不同類型的數據，求解大量的參數，并產生可靠和準確的結果，根據模型的線性或非線性特性，它們被應用于軌跡跟蹤、機械手機器人的位置估計、SLAM（同步定位和繪圖）和物體檢測等。這種算法的靈活性使得來自不同類型的傳感器和技術的信息集成成為可能，例如里程計、GNSS（全球導航衛星系統）、激光雷達和超聲波傳感器。整體系統的算法流程如圖1所示：

圖1 擴展卡爾曼濾波算法框架Fig.1 Extended Kalman filter algorithm framework

該算法是一種遞歸算法，因此預測過程是一個實時進行的過程，當算法接收到新狀態，就會增加校正項作為下一時刻的初始條件。利用擴展卡爾曼濾波器融合IMU和輪速計降低運動隨機誤差，系統狀態方程矩陣形式為：

然后通過更新擴展卡爾曼濾波器可計算得到最終的融合結果：

3.巡檢機器人定位實驗

為了驗證本文所使用的巡檢機器人定位算法，通過在Ubuntu 18.04系統中的ROS Melodic框架下對輪速計和IMU進行了姿態估計和信息融合，并在巡檢機器人試驗平臺上完成定位實驗，通過展現實際軌跡并分析絕對位姿誤差驗證了算法的可行性與有效性。其中，絕對位姿誤差是指巡檢機器人的預估位置與實際位置之間的偏差，表達式如下所示：

式中，δ代表定位精度，m；（xt,yt,zt）代表估算位置；（xe,ye,ze）代表從基準中測得的點的坐標數據。

測試地點為曹家灘煤礦，測試路線以曹家灘煤礦副斜井井口為起點，全長約為625m，其中水平距離42m，其余路段為坡度7%的斜坡，有3個轉彎路口。

其中，ground_turth由巷道施工圖聯合場景內布置的定位二維碼采集解算，代表巡檢機器人真實軌跡，VSS和IMU分別表示由輪速計以及IMU解算得到的里程計信息。VSS+IMU表示使用擴展卡爾曼濾波融合后的里程計信息。圖2為軌跡圖，從中可以直觀看出使用VSS+IMU的定位軌跡最貼合機器人真實軌跡，僅采用VSS或IMU會存在不同程度的噪聲，導致定位存在著明顯波動。從圖3可以看出使用VSS+IMU組合定位方式幾乎在整個巡檢過程中的絕對位姿誤差保持最低。

圖2 巡檢機器人行駛軌跡對比Fig.2 Comparison of driving trajectories of patrol robot

圖3 絕對位姿誤差對比Fig.3 Comparison of absolute pose errors

圖4展示了三種方法的絕對位姿誤差分布情況，通過小提琴圖和箱線圖表明使用VSS+IMU組合定位的誤差箱體介于0.5～0.9為最低，使用VSS定位誤差介于1.2～2.6次之，使用IMU的定位誤差介于2.5～3.8為最高，因此本文所使用的VSS+IMU融合策略在定位精度和穩定性方面均優于其他兩種方法。

圖4 絕對位姿誤差分布Fig.4 Absolute pose error distribution

通過表2可以看出使用擴展卡爾曼濾波融合輪速計和IMU后的平均位姿誤差為0.401，位姿誤差的標準差為0.247，兩項指標均為三者最低，表明了使用VSS+IMU的融合模式能夠穩定降低巡檢機器人的位姿誤差。

表2 三種定位方式的誤差對比Tab.2 Error comparison of three Self-localization methods

4.結論

（1）對巡檢機器人的定位系統進行設計，對定位過程中所使用輪速計、IMU分別進行了建模分析，通過擴展卡爾曼濾波融合輪速計信息和IMU信息解決了單一傳感器定位的不足，有效降低了傳感器的噪聲。

（2）完成了巡檢機器人定位實驗，結果表明多傳感器系統的平均位姿誤差約為0.401，相較于IMU和輪速計的定位誤差分別降低了2.542和1.791，更好的實現了巡檢機器人的自主定位功能。