＊趙洪輝 李耀龍 張業(yè)奔

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

關(guān)鍵字：煤礦智能化；巡檢機(jī)器人；擴(kuò)展卡爾曼濾波；自主定位

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

1.傳感器模型建立

本文使用輪速計和IMU實現(xiàn)巡檢機(jī)器人在煤礦井下的定位，因此必須通過對這兩種傳感器構(gòu)建合理的數(shù)學(xué)模型，以便于后續(xù)的信息融合。

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

表1 驅(qū)動電機(jī)參數(shù)Tab.1 Driving motor parameters

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

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

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

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

式中，ωk表示在k時刻IMU所測量的角速度值；Rk為在k時刻時IMU在世界坐標(biāo)系下的姿態(tài)。

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

對于輪速計和IMU模型進(jìn)行建立后，本文選用擴(kuò)展卡爾曼濾波算法實現(xiàn)融合輪速計和IMU數(shù)據(jù)信息，擴(kuò)展卡爾曼濾波一種強(qiáng)大的定位工具，它可以接受不同類型的數(shù)據(jù)，求解大量的參數(shù)，并產(chǎn)生可靠和準(zhǔn)確的結(jié)果，根據(jù)模型的線性或非線性特性，它們被應(yīng)用于軌跡跟蹤、機(jī)械手機(jī)器人的位置估計、SLAM（同步定位和繪圖）和物體檢測等。這種算法的靈活性使得來自不同類型的傳感器和技術(shù)的信息集成成為可能，例如里程計、GNSS（全球?qū)Ш叫l(wèi)星系統(tǒng)）、激光雷達(dá)和超聲波傳感器。整體系統(tǒng)的算法流程如圖1所示：

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

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

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

3.巡檢機(jī)器人定位實驗

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

式中，δ代表定位精度，m；（xt,yt,zt）代表估算位置；（xe,ye,ze）代表從基準(zhǔn)中測得的點(diǎn)的坐標(biāo)數(shù)據(jù)。

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

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

圖2 巡檢機(jī)器人行駛軌跡對比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融合策略在定位精度和穩(wěn)定性方面均優(yōu)于其他兩種方法。

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

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

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

4.結(jié)論

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

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