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

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

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

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

1.傳感器模型建立

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

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

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

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

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

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

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

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

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

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

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

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

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

3.巡檢機(jī)器人定位實(shí)驗(yàn)

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

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

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

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

圖2 巡檢機(jī)器人行駛軌跡對(duì)比Fig.2 Comparison of driving trajectories of patrol robot

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

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

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

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

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

4.結(jié)論

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

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