賀建偉

(1.中國(guó)煤炭科工集團(tuán)太原研究院有限公司， 山西 太原 030006；2.山西天地煤機(jī)裝備有限公司， 山西 太原 030006)

0 引言

連續(xù)采煤機(jī)適用于“三下”壓煤、不規(guī)則塊段及殘采區(qū)的采煤作業(yè)，也可用于巷道掘進(jìn)。連續(xù)采煤機(jī)在掘進(jìn)或采煤過(guò)程中大多使用傳統(tǒng)的點(diǎn)激光光源對(duì)其方向進(jìn)行指示，采掘司機(jī)通過(guò)目視斷面上的激光光斑控制截割[1]。該方法由于掘進(jìn)工作面粉塵較大、作業(yè)現(xiàn)場(chǎng)光線差、操作人員視線受裝備機(jī)身遮擋等，難以準(zhǔn)確判斷截割部位置。同時(shí)隨著掘進(jìn)裝備的掘進(jìn)深入，點(diǎn)激光光源需要多次移站跟進(jìn)，指示精度降低，且測(cè)量工作量大，無(wú)法滿足工作面無(wú)人化遠(yuǎn)程操控的要求，因此，如何實(shí)現(xiàn)連續(xù)采煤機(jī)井下精準(zhǔn)定位定向成為目前亟需解決的關(guān)鍵問(wèn)題。

煤礦井下由于環(huán)境特殊且條件惡劣，GPS等地面應(yīng)用較多的導(dǎo)航定位系統(tǒng)無(wú)法在井下完全發(fā)揮作用[2]。慣性導(dǎo)航系統(tǒng)作為一種自主式定位定向系統(tǒng)，適用范圍廣，不依賴外界信息便可獲得載體的姿態(tài)、速度及位置信息，對(duì)煤礦井下采掘裝備進(jìn)行定位定向指示具有較好的效果。

當(dāng)前較為主流的井下采掘裝備定位方法大多依附于慣導(dǎo)系統(tǒng)進(jìn)行[2]，但慣導(dǎo)系統(tǒng)需通過(guò)對(duì)測(cè)量得到的加速度信息進(jìn)行二次積分才可獲得載體的位置信息，存在隨時(shí)間累積會(huì)產(chǎn)生較大位置漂移誤差的缺陷[3]。目前多使用基于慣導(dǎo)的多傳感器組合定位方法解決單獨(dú)使用慣導(dǎo)系統(tǒng)進(jìn)行定位存在的位置漂移誤差問(wèn)題。文獻(xiàn)[4]將慣導(dǎo)和里程計(jì)相結(jié)合進(jìn)行井下采掘裝備的定位研究，利用卡爾曼濾波結(jié)合里程計(jì)測(cè)得的里程信息對(duì)慣導(dǎo)相應(yīng)測(cè)量數(shù)據(jù)進(jìn)行校正，但采掘設(shè)備在井下易受浮煤影響造成履帶打滑及滑移現(xiàn)象，此時(shí)里程計(jì)的測(cè)量誤差會(huì)遠(yuǎn)大于慣導(dǎo)自身的漂移誤差，僅依靠里程計(jì)與慣導(dǎo)進(jìn)行組合會(huì)降低整個(gè)定位方法的測(cè)量精度。文獻(xiàn)[5]采用聯(lián)邦濾波將慣性測(cè)量單元(IMU)分別與立體相機(jī)、里程計(jì)構(gòu)成子濾波器，避免了因里程計(jì)打滑或滑移失效產(chǎn)生的較大測(cè)量誤差污染整個(gè)定位系統(tǒng)[6]，但井下視覺(jué)定位方法目前還處于理論研究階段，未出現(xiàn)成功應(yīng)用的工業(yè)性案例，故其可實(shí)施性與可靠性均屬未知。文獻(xiàn)[7]采用集中式濾波處理慣性導(dǎo)航系統(tǒng)、里程計(jì)及測(cè)距雷達(dá)所獲取的各類傳感器的測(cè)量信息，在進(jìn)行卡爾曼濾波狀態(tài)方程建模時(shí)維數(shù)較高，整個(gè)系統(tǒng)計(jì)算量較大且容錯(cuò)性差。

綜上可知，目前多采用里程計(jì)、視覺(jué)測(cè)量、測(cè)距雷達(dá)等作為輔助測(cè)量方式，與慣導(dǎo)組合測(cè)量降低慣導(dǎo)隨時(shí)間累積所產(chǎn)生的位置漂移誤差，其中輔助測(cè)量方式要求可靠性與測(cè)量精度均較高。對(duì)不同測(cè)量方式得到的數(shù)據(jù)目前多使用集中式卡爾曼濾波進(jìn)行濾波解算，但集中式卡爾曼濾波的計(jì)算量較大且容錯(cuò)性較差。針對(duì)上述問(wèn)題，本文提出了一種由捷聯(lián)式慣導(dǎo)、測(cè)距雷達(dá)及里程計(jì)組成的連續(xù)采煤機(jī)定位方法，使用測(cè)距雷達(dá)與里程計(jì)測(cè)量連續(xù)采煤機(jī)的位移量，通過(guò)航位推算法推算出機(jī)身的二維位置信息，利用二維位置信息對(duì)慣導(dǎo)位置解算信息進(jìn)行反饋修正，減小慣導(dǎo)隨時(shí)間累積所產(chǎn)生的位置漂移誤差[8]。采用聯(lián)邦式卡爾曼濾波對(duì)各傳感器采集的位姿信息進(jìn)行濾波與融合[9-11]，通過(guò)降低方程維數(shù)減少計(jì)算量,在保證精度的同時(shí)還具備一定的容錯(cuò)性。該定位方法可為連續(xù)采煤機(jī)在巷道約束環(huán)境下的位姿精確測(cè)量、巷道自動(dòng)成形控制和遠(yuǎn)程控制奠定基礎(chǔ)。

1 連續(xù)采煤機(jī)定位方法

連續(xù)采煤機(jī)定位方法結(jié)構(gòu)組成如圖1所示。使用測(cè)距雷達(dá)與里程計(jì)同時(shí)輔助修正慣導(dǎo)系統(tǒng)，采用聯(lián)邦式卡爾曼濾波將慣導(dǎo)分別與測(cè)距雷達(dá)、里程計(jì)組成子濾波器。在里程計(jì)因履帶打滑或滑移產(chǎn)生較大的測(cè)量誤差時(shí)，提高測(cè)距雷達(dá)與慣導(dǎo)所組成的子濾波器1的置信度；在測(cè)距雷達(dá)因人員移動(dòng)或其他障礙物遮擋而導(dǎo)致測(cè)量誤差較大時(shí)，里程計(jì)及慣導(dǎo)所組成的子濾波器2采取較高的置信度，防止因個(gè)別輔助傳感器測(cè)量失效產(chǎn)生較大誤差而降低定位方法的精度。

1-連續(xù)采煤機(jī)本體； 2-慣導(dǎo)組件； 3-電控箱； 4-里程計(jì)；

1.1 慣導(dǎo)系統(tǒng)

慣導(dǎo)系統(tǒng)根據(jù)安裝方式可分為平臺(tái)式與捷聯(lián)式，根據(jù)所采用的陀螺儀種類不同又可分為光纖級(jí)、激光級(jí)等[12]。本文采用某型號(hào)光纖級(jí)捷聯(lián)式慣導(dǎo)，慣導(dǎo)本體及交換機(jī)直接固連于連續(xù)采煤機(jī)機(jī)身中部右側(cè)位置，緊鄰電控箱，可實(shí)時(shí)測(cè)量機(jī)身的姿態(tài)、速度及位置信息。該定位方法取測(cè)距雷達(dá)、里程計(jì)與慣導(dǎo)系統(tǒng)所推算得到的機(jī)身位置信息差值作為濾波輸入來(lái)進(jìn)行濾波處理[13]，各傳感器所采集到的位姿信息可通過(guò)交換機(jī)傳送至巷道后方安全位置或地面控制室內(nèi)的上位機(jī)進(jìn)行顯示與控制。

1.2 測(cè)距雷達(dá)

測(cè)距雷達(dá)安裝于連續(xù)采煤機(jī)機(jī)身尾部右后下方，將規(guī)格為1 m×1 m的標(biāo)靶放置于與測(cè)距雷達(dá)處于同一水平線的正后方。連續(xù)采煤機(jī)行進(jìn)過(guò)程中通過(guò)測(cè)距雷達(dá)不斷測(cè)量與后方標(biāo)靶的距離，所得的當(dāng)前測(cè)量值與前一次采樣頻率下的測(cè)量值作差便可得到連續(xù)采煤機(jī)的相對(duì)位置增量。利用航位推算法對(duì)相對(duì)位置增量進(jìn)行解算便可得到機(jī)體坐標(biāo)系下的位置信息，借助慣導(dǎo)解算中的姿態(tài)矩陣實(shí)時(shí)地將機(jī)體坐標(biāo)系下的位置信息轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的信息后再進(jìn)行濾波解算[14]。

1.3 里程計(jì)

里程計(jì)安裝于履帶驅(qū)動(dòng)齒輪處，輸出量為連續(xù)采煤機(jī)短時(shí)間內(nèi)路程增量，通過(guò)航位推算法得出所需要的位置信息[15]。里程計(jì)由于固連于車體履帶轉(zhuǎn)軸處，所測(cè)得的信息量依舊是在機(jī)體坐標(biāo)系下的，同樣需借助慣導(dǎo)解算中的姿態(tài)矩陣實(shí)時(shí)地將其轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的信息量后再進(jìn)行濾波解算。

2 航位推算法

航位推算法是一種使用航向和里程信息來(lái)計(jì)算運(yùn)載體相對(duì)于起點(diǎn)的相對(duì)位置的定位方法，原理如圖2所示。已知起始位置坐標(biāo)(x0，y0)及起始方位角θ0，通過(guò)測(cè)量行駛距離Si及方位角變化值Δθi，結(jié)合二者值便可推算出下一時(shí)刻位置坐標(biāo)(xk，yk)和方位角θk。

(1)

(2)

θk=θk-1+Δθk-1k=1,2,…

(3)

式中：θk-1為上一時(shí)刻方位角，k為不同時(shí)刻；Δθk-1為上一時(shí)刻至當(dāng)前時(shí)刻方位角變化。

圖2 航位推算法原理Fig.2 Principle of dead reckoning algorithm

3 濾波器模型構(gòu)建

3.1 聯(lián)邦式卡爾曼濾波原理

(4)

圖3 聯(lián)邦式卡爾曼濾波器原理Fig.3 Federated Kalman filter principle

為了簡(jiǎn)化計(jì)算，在信息分配上，使用平均分配方式分配子濾波器與主濾波器間的方差矩陣值，即

βj=βm=1/3

(5)

3.2 誤差分析

定位方法中的誤差主要來(lái)源于捷聯(lián)慣導(dǎo)自身器件誤差及算法更新誤差、里程計(jì)及測(cè)距雷達(dá)測(cè)量誤差，通過(guò)誤差分析可為濾波器建模奠定基礎(chǔ)。

3.2.1 慣性導(dǎo)航誤差

其中關(guān)于捷聯(lián)式慣導(dǎo)更新算法中的姿態(tài)、速度及位置誤差推導(dǎo)見(jiàn)文獻(xiàn)[16]。

3.2.2 測(cè)距雷達(dá)測(cè)量誤差

測(cè)距雷達(dá)測(cè)量誤差主要為自身固有誤差δρ及初始輸入誤差δη。經(jīng)分析這2類誤差特性，同時(shí)為簡(jiǎn)化模型及降低計(jì)算量，將其設(shè)置為固定常值對(duì)誤差進(jìn)行補(bǔ)償，以消除其對(duì)實(shí)驗(yàn)結(jié)果的影響。故在后續(xù)建模過(guò)程中，將測(cè)距雷達(dá)這2類誤差均設(shè)為固定常值。

3.2.3 里程計(jì)測(cè)量誤差

里程計(jì)的測(cè)量誤差主要包括標(biāo)度因數(shù)誤差δKD和安裝誤差角δα，設(shè)δα=[δαθδαγδαψ]，δαθ,δαγ和δαψ分別為安裝角誤差中的俯仰角、橫滾角和航向角誤差。其中里程計(jì)的橫滾角安裝誤差一般不會(huì)對(duì)測(cè)距雷達(dá)的位置輸出量造成干擾，故這里不考慮橫滾角安裝誤差。俯仰角安裝誤差及航向角安裝誤差對(duì)里程計(jì)測(cè)量誤差有影響，將δKD、δαθ、δαψ三者均設(shè)為隨機(jī)常值。

3.3 濾波器模型構(gòu)建

聯(lián)邦式卡爾曼濾波作為卡爾曼濾波的變式，其與集中式卡爾曼濾波的區(qū)別在于聯(lián)邦式卡爾曼濾波子濾波器先進(jìn)行一級(jí)濾波，而后進(jìn)入聯(lián)邦式卡爾曼濾波主濾波器進(jìn)行二級(jí)濾波，同時(shí)將全局濾波最優(yōu)估計(jì)及誤差方差矩陣放大后反饋給子濾波器進(jìn)行下一次濾波[18-19]。

2個(gè)子濾波器均采用間接法進(jìn)行卡爾曼濾波器數(shù)學(xué)建模，即狀態(tài)變量由導(dǎo)航參數(shù)的誤差構(gòu)成，同時(shí)對(duì)該連續(xù)變量信息進(jìn)行離散化，以便于卡爾曼濾波進(jìn)行分步迭代更新[20]，本文為描述簡(jiǎn)便直接給出離散化后的數(shù)學(xué)模型。

3.3.1 子濾波器1數(shù)學(xué)模型

狀態(tài)方程為

Xk1=Φk1/k1-1Xk1-1+Γk1-1Wk1-1

(6)

式中：Φk1/k1-1為子濾波器1的十五維狀態(tài)轉(zhuǎn)移矩陣；Γk1-1為子濾波器1的噪聲驅(qū)動(dòng)矩陣；Wk1-1為子濾波器1的陀螺與加速度計(jì)的白噪聲矩陣。

狀態(tài)向量為

(7)

量測(cè)方程為

Zk1=Hk1Xk1+Vk1

(8)

式中：Zk1為子濾波器1以慣導(dǎo)與測(cè)距雷達(dá)所輸出的位置量作差得到的觀測(cè)量；Hk1為子濾波器1的量測(cè)矩陣；Vk1為子濾波器1的測(cè)距雷達(dá)的位置量測(cè)白噪聲矩陣。

3.3.2 子濾波器2數(shù)學(xué)模型

狀態(tài)方程為

Xk2=Φk2/k2-1Xk2-1+Γk2-1Wk2-1

(9)

式中：Φk2/k2-1為子濾波器2的二十一維狀態(tài)轉(zhuǎn)移矩陣；Γk2-1為子濾波器2的噪聲驅(qū)動(dòng)矩陣；Wk2-1為子濾波器2的陀螺與加速度計(jì)的白噪聲矩陣[20]。

狀態(tài)向量為

(10)

量測(cè)方程為

Zk2=Hk2Xk+Vk2

(11)

式中：Zk2為觀測(cè)量，子濾波器2以慣導(dǎo)與里程計(jì)根據(jù)航位推算法推算出的位置量作差作為觀測(cè)量；Hk2為子濾波器2的量測(cè)矩陣；Vk2為子濾波器2的里程計(jì)的位置量測(cè)白噪聲矩陣。

4 仿真

為檢驗(yàn)連續(xù)采煤機(jī)定位方法的定位精度及2類卡爾曼濾波的容錯(cuò)性，本文進(jìn)行了Matlab 仿真實(shí)驗(yàn)。仿真時(shí)長(zhǎng)取20 min即1 200 s，模擬連續(xù)采煤機(jī)實(shí)際掘進(jìn)過(guò)程中不停機(jī)生產(chǎn)時(shí)長(zhǎng)。在仿真時(shí)間為5 min即300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)，持續(xù)時(shí)間為30 s，模擬連續(xù)采煤機(jī)打滑情況。慣導(dǎo)關(guān)鍵精度參數(shù)設(shè)置：陀螺儀的常值漂移εb設(shè)置為0.001 °/h, 加速度計(jì)零偏設(shè)置為1×10-4g(g為重力加速度)。

4.1 姿態(tài)誤差

集中式與聯(lián)邦式卡爾曼濾波航向角誤差對(duì)比如圖4所示。從圖4可看出，在前200 s 時(shí)，2類濾波波動(dòng)均較大，其中集中式卡爾曼濾波最大波動(dòng)可達(dá)10′。在200 s后，2類濾波的濾波值逐漸趨于平穩(wěn)，在0～-2.5′內(nèi)小幅波動(dòng)。在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，與聯(lián)邦式卡爾曼濾波相比，集中式卡爾曼濾波出現(xiàn)的突變值較大，最大可達(dá)-8′，且在30 s內(nèi)加入誤差的過(guò)程中持續(xù)出現(xiàn)突變值，而聯(lián)邦式卡爾曼濾波出現(xiàn)突變后，短時(shí)間內(nèi)便恢復(fù)至正常濾波值范圍。

圖4 航向角誤差對(duì)比Fig.4 Comparison of heading angle error

集中式與聯(lián)邦式卡爾曼濾波橫滾角誤差對(duì)比如圖5所示。從圖5可看出，在前80 s時(shí)，2類濾波的濾波值波動(dòng)均較大，集中式卡爾曼濾波波動(dòng)范圍更大，最高可達(dá)14′；在80 s后，2類濾波的濾波值恢復(fù)至-1～1′內(nèi)波動(dòng)。在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，2類濾波均出現(xiàn)了瞬時(shí)突變，但聯(lián)邦式卡爾曼濾波突變值較小且持續(xù)時(shí)間較短，且在較短時(shí)間內(nèi)恢復(fù)到了正常值。

圖5 橫滾角誤差對(duì)比 Fig.5 Comparison of roll angle error

集中式與聯(lián)邦式卡爾曼濾波俯仰角誤差對(duì)比如圖6所示。從圖6可看出，在濾波初期240 s內(nèi)，2類濾波的濾波值波動(dòng)均較大，且與航向角、橫滾角相比，波動(dòng)峰值更大，最大處可達(dá)270′。在240 s后，濾波波動(dòng)值穩(wěn)定于15′附近。在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，與前2個(gè)姿態(tài)角波動(dòng)相類似，集中式卡爾曼濾波產(chǎn)生的瞬時(shí)突變值較大且持續(xù)時(shí)間長(zhǎng)，而聯(lián)邦式卡爾曼濾波則可以在短時(shí)間內(nèi)恢復(fù)至正常波動(dòng)范圍內(nèi)。

由3個(gè)姿態(tài)角誤差可得出，誤差角在濾波穩(wěn)定后可達(dá)到角分級(jí)，天向通道中的俯仰角誤差在前期濾波過(guò)程中波動(dòng)較大，最大波動(dòng)值可達(dá)270′，穩(wěn)定后在15′內(nèi)波動(dòng)；東向通道中的航向角和北向通道中的橫滾角仿真誤差值較小，穩(wěn)定后波動(dòng)值為2.5′左右。注入里程計(jì)測(cè)量誤差擾動(dòng)后，與集中式卡爾曼濾波相對(duì)比，聯(lián)邦式卡爾曼濾波出現(xiàn)的瞬時(shí)波動(dòng)值較小且持續(xù)時(shí)間短，容錯(cuò)能力更強(qiáng)。

圖6 俯仰角誤差對(duì)比 Fig.6 Comparison of pitch angle error

4.2 位置誤差

集中式與聯(lián)邦式卡爾曼濾波經(jīng)度誤差對(duì)比如圖7所示。從圖7可看出，在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，集中式卡爾曼濾波較聯(lián)邦式卡爾曼濾波出現(xiàn)的瞬時(shí)波動(dòng)值更大且持續(xù)時(shí)間更長(zhǎng)，可達(dá)30 s，與注入里程計(jì)擾動(dòng)時(shí)長(zhǎng)相符。待穩(wěn)定后，2類濾波的波動(dòng)值為-1～1 cm。

圖7 經(jīng)度誤差對(duì)比 Fig.7 Comparison of longitude error

集中式與聯(lián)邦式卡爾曼濾波緯度誤差對(duì)比如圖8所示。從圖8可看出，在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，2類濾波均出現(xiàn)了相應(yīng)波動(dòng)，其中集中式卡爾曼濾波的波動(dòng)值較大且持續(xù)時(shí)間較長(zhǎng)，濾波穩(wěn)定后，2類濾波的波動(dòng)值為-0.5～0.5 cm。

圖8 緯度誤差對(duì)比Fig.8 Comparison of latitude error

集中式與聯(lián)邦式卡爾曼濾波高度誤差對(duì)比如圖9所示。從圖9可看出，在300 s時(shí)加入里程計(jì)測(cè)量誤差擾動(dòng)后，集中式卡爾曼濾波出現(xiàn)了較大瞬時(shí)波動(dòng)值，最高達(dá)-4.5 cm，且波動(dòng)持續(xù)時(shí)間較長(zhǎng)，濾波穩(wěn)定后，2類濾波的波動(dòng)值為-2～2 cm。

圖9 高度誤差對(duì)比Fig.9 Comparison of height error

由上述分析可得出，位置誤差在濾波穩(wěn)定后可達(dá)厘米級(jí)精度，天向通道的高度誤差相較于經(jīng)緯度誤差較大，達(dá)到10 cm，穩(wěn)定后在-2～2 cm內(nèi)波動(dòng)。經(jīng)度誤差穩(wěn)定后為-1～1 cm，緯度誤差穩(wěn)定后為-0.5～0.5 cm。與姿態(tài)誤差最終結(jié)果表現(xiàn)相同，聯(lián)邦式卡爾曼濾波在加入誤差擾動(dòng)后出現(xiàn)的瞬時(shí)波動(dòng)值較小且持續(xù)時(shí)間短，容錯(cuò)能力強(qiáng)。

5 結(jié)論

(1) 使用捷聯(lián)式慣導(dǎo)、里程計(jì)及測(cè)距雷達(dá)組合測(cè)量，可實(shí)現(xiàn)煤礦井下連續(xù)采煤機(jī)較高精度的位姿信息獲取。

(2) 通過(guò)測(cè)距雷達(dá)與里程計(jì)共同進(jìn)行位置推算輔助修正捷聯(lián)慣導(dǎo)系統(tǒng)，可有效抑制慣導(dǎo)隨時(shí)間積累所產(chǎn)生的位置漂移誤差。

(3) 聯(lián)邦式卡爾曼濾波與集中式卡爾曼濾波相比，2類濾波在精度基本一致的前提下，聯(lián)邦式卡爾曼濾波在受到較大測(cè)量誤差干擾后出現(xiàn)的瞬時(shí)波動(dòng)值小且波動(dòng)時(shí)間短，具備一定的容錯(cuò)性。

(4) 采用聯(lián)邦式卡爾曼濾波的定位方法的定位精度可達(dá)±2 cm，定向精度可達(dá)15′，可靠性較高，抗干擾能力較強(qiáng)，可為下一步井下現(xiàn)場(chǎng)試驗(yàn)提供理論與實(shí)驗(yàn)依據(jù)。

DOI：10.3390/s19173812.

DOI：10.3390/s17020239.