石金龍，馬宏偉，毛清華，張羽飛，華洪濤

(1.西安科技大學(xué) 電氣與控制工程學(xué)院，陜西 西安 710054；2.陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054；3.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054)

采煤機(jī)精準(zhǔn)定位是實(shí)現(xiàn)煤礦綜采工作面智能化開(kāi)采的關(guān)鍵[1]。由于井下環(huán)境復(fù)雜，一般傳感器容易受外部影響，定位效果不理想[2，3]，基于捷聯(lián)慣導(dǎo)的采煤機(jī)定位系統(tǒng)，受到國(guó)內(nèi)外專家學(xué)者的廣泛關(guān)注。楊海[4]等提出一種基于捷聯(lián)慣導(dǎo)的采煤機(jī)定位方法，利用陀螺儀與加速度計(jì)采集的三軸角速度和三軸加速度信息，解算得到采煤機(jī)的位姿信息。雖然慣導(dǎo)系統(tǒng)可以在短期內(nèi)檢測(cè)采煤機(jī)位姿，但在長(zhǎng)期運(yùn)行過(guò)程中，受到傳感器漂移與累積誤差的影響，定位精度不足。李楚[5]提出一種慣導(dǎo)與視覺(jué)相機(jī)組合定位的方法，通過(guò)采集采煤機(jī)姿態(tài)信息與位置信息，視覺(jué)攝像機(jī)采集信標(biāo)修正慣導(dǎo)的累計(jì)誤差，提高采煤機(jī)定位精度，但采煤機(jī)運(yùn)行過(guò)程中灰塵過(guò)多，影響視覺(jué)相機(jī)定位精度；夏婷[6]提出了一種慣導(dǎo)與超聲波組合的采煤機(jī)定位方法，運(yùn)用容積卡爾曼濾波算法對(duì)慣導(dǎo)和無(wú)線傳感器的位置信息進(jìn)行濾波，輸出可信的位置信息，但超聲定位系統(tǒng)需要信標(biāo)安裝數(shù)量過(guò)多，節(jié)點(diǎn)出現(xiàn)異常后定位精度不足。李昂[7]等應(yīng)用慣導(dǎo)與軸編碼器組合定位方法，構(gòu)建采煤機(jī)定位方程，驗(yàn)證了方法的可行性，雖然慣導(dǎo)與里程計(jì)兩者受外界因素干擾較小，也不向外輻射能量，自主定位能力強(qiáng)等特點(diǎn)，但采用的慣導(dǎo)精度較低，組合后定位精度較差。澳大利亞聯(lián)邦科學(xué)與工業(yè)研究組織(CSIRO)的LASC系統(tǒng)[8]，使用高精度光纖陀螺儀與里程計(jì)，結(jié)合找直算法，分析采煤機(jī)空間位置信息，但國(guó)外慣導(dǎo)領(lǐng)域長(zhǎng)期對(duì)我國(guó)處于封鎖狀態(tài)，技術(shù)不對(duì)外開(kāi)放，使用價(jià)格高昂。

綜上所述，針對(duì)當(dāng)前國(guó)內(nèi)采煤機(jī)定位精度不足，穩(wěn)定性較差的問(wèn)題，本文采用高精度國(guó)產(chǎn)光纖捷聯(lián)慣導(dǎo)與里程計(jì)組合，測(cè)量采煤機(jī)位姿，建立慣導(dǎo)與里程計(jì)組合定位模型，獲得采煤機(jī)運(yùn)行軌跡。通過(guò)仿真實(shí)驗(yàn)與井下綜采工作面實(shí)驗(yàn)，驗(yàn)證了“慣導(dǎo)+里程計(jì)”組合慣導(dǎo)在煤礦井下的定位精度與可靠性。

1 “慣導(dǎo)+里程計(jì)”采煤機(jī)位姿測(cè)量原理

綜采工作面中的“三機(jī)”有采煤機(jī)、刮板輸送機(jī)和液壓支架，采煤機(jī)騎行在刮板輸送機(jī)上，刮板輸送機(jī)沿工作面截割方向放置，置于液壓支架前，液壓支架通過(guò)液壓缸推動(dòng)刮板輸送機(jī)移動(dòng)，向前推進(jìn)。捷聯(lián)慣導(dǎo)系統(tǒng)安裝在采煤機(jī)內(nèi)部，里程計(jì)與采煤機(jī)齒軌輪軸連接。通過(guò)慣導(dǎo)解算采煤機(jī)位姿信息，卡爾曼濾波融合慣導(dǎo)與里程計(jì)單位時(shí)間內(nèi)位置信息，利用閉環(huán)反饋的方式校正慣導(dǎo)與里程計(jì)的誤差參數(shù)，輸出采煤機(jī)的位姿，工作原理如圖1所示。

圖1 “慣導(dǎo)+里程計(jì)”測(cè)量位姿原理圖

2 “慣導(dǎo)+里程計(jì)”組合定位模型

對(duì)慣導(dǎo)的位姿信息進(jìn)行解算是是慣性導(dǎo)航系統(tǒng)的核心部分，是影響整個(gè)慣導(dǎo)系統(tǒng)精度的主要因素之一。采煤機(jī)的位姿解算更新算法主要有歐拉角法、四元數(shù)法、方向余弦矩陣法、Rodrigues法，其中四元數(shù)法計(jì)算量小，精度高，應(yīng)用范圍較廣，本文將采用四元數(shù)法解算慣導(dǎo)位姿。

捷聯(lián)慣導(dǎo)固定在采煤機(jī)上，設(shè)采煤機(jī)坐標(biāo)系為載體坐標(biāo)系(b系)，采煤機(jī)前進(jìn)方向?yàn)閤b軸，機(jī)身平面上與移動(dòng)方向垂直的為yb軸，與機(jī)身平面垂直的方向?yàn)閦b軸。選取地理坐標(biāo)系(g系)為導(dǎo)航坐標(biāo)系(n系)，以采煤機(jī)重心為中心，規(guī)定xn，yn，zn分別指向東、北、天方向。其中，采煤機(jī)的航向角為φ，俯仰角為θ，橫滾角為γ。

采煤機(jī)的姿態(tài)四元數(shù)微分方程為：

(1)

(2)

根據(jù)采煤機(jī)姿態(tài)轉(zhuǎn)換矩陣：

(3)

得到姿態(tài)角為：

θ=sin-1(T32)

(4)

γ=-tan(T31/T33)

(5)

φ=tan-1(T12/T22)

(6)

采煤機(jī)速度更新的微分方程為：

(7)

采煤機(jī)位置更新微分方程為：

(8)

通過(guò)上述方法，即可得到慣導(dǎo)解算后的姿態(tài)、速度、位置方程，接下來(lái)分析里程計(jì)輸出模型。

里程計(jì)通常采用脈沖方式輸出采樣時(shí)間間隔內(nèi)的里程增量，里程計(jì)與采煤機(jī)齒軌輪連接，使里程計(jì)的速度輸出方向時(shí)刻指向采煤機(jī)運(yùn)行方向。假設(shè)采煤機(jī)在運(yùn)行過(guò)程中正常行駛，與齒軌齒合，里程計(jì)的輸出值為采煤機(jī)的運(yùn)行方向的速度量。設(shè)采煤機(jī)上里程計(jì)的坐標(biāo)系為m系，是與采煤機(jī)固連的右-前-上坐標(biāo)系，即ox軸沿采煤機(jī)縱軸向前，oy軸沿采煤機(jī)橫軸向右，oz軸沿垂直地面向上，里程計(jì)在采樣時(shí)間間隔內(nèi)輸出的脈沖數(shù)N在m系投影矢量為：

(9)

(10)

進(jìn)一步整理后，計(jì)算得到里程計(jì)的位置增量為：

(11)

式中，αφ、αθ為采煤機(jī)坐標(biāo)系相對(duì)于里程計(jì)坐標(biāo)系的航向、俯仰安裝角；i為輸出軸到齒軌輪傳動(dòng)比；N為里程計(jì)旋轉(zhuǎn)一周產(chǎn)生的脈沖數(shù)目；n(t)為單位時(shí)間t內(nèi)產(chǎn)生的脈沖數(shù)目，行走輪的分度圓半徑為R。

只考慮標(biāo)度因數(shù)誤差δKD、安裝誤差角δαφ、δαθ，則里程計(jì)實(shí)際輸出的采煤機(jī)坐標(biāo)系下分量可簡(jiǎn)化為：

(12)

式中，wd為隨即噪聲干擾。

本文建立位置觀測(cè)模式下的系統(tǒng)模型，與傳統(tǒng)速度觀測(cè)模式下的組合算法相比矩陣維數(shù)更少，計(jì)算的精度更高[9]。組合系統(tǒng)中包含的誤差主要為慣導(dǎo)的姿態(tài)、速度、位置誤差，陀螺儀與角速度計(jì)零漂誤差，里程計(jì)刻度因數(shù)誤差和俯仰角航向角安裝誤差，建立的組合定位系統(tǒng)與這些誤差之間存在著互相關(guān)聯(lián)，構(gòu)成了一個(gè)閉環(huán)反饋系統(tǒng)，對(duì)誤差進(jìn)行優(yōu)化修正，提高定位精度。

建立Kalman濾波器系統(tǒng)方程為：

(14)

式中，F(xiàn)(t)為系統(tǒng)矩陣；w(t)為系統(tǒng)噪聲；G(t)為系統(tǒng)噪聲轉(zhuǎn)移矩陣，二者一般為白噪聲。

其中該組合導(dǎo)航狀態(tài)變量為18維：

X=[(δvn)T，(φn)T，(δp)T，(εb)T，

(b)T，δKD，δαθ，δαφ]T

(13)

式中，vn為速度誤差；φn為姿態(tài)誤差；δp為位置誤差；εb為陀螺儀零漂；b為加速度計(jì)零漂；δKD為里程計(jì)標(biāo)度因數(shù)誤差；δαφ、δαθ為俯仰、航向安裝誤差角。

其中里程計(jì)刻度因數(shù)誤差、安裝誤差角均可視為隨即常數(shù)。因此，對(duì)應(yīng)的系統(tǒng)矩陣為：

(15)

式中，F(xiàn)1為慣導(dǎo)系統(tǒng)誤差方程對(duì)應(yīng)的轉(zhuǎn)移矩陣，計(jì)算方法如文獻(xiàn)[10]所示。

慣導(dǎo)位置更新需要計(jì)算每個(gè)周期導(dǎo)航坐標(biāo)系下的位置增量，位置增量的計(jì)算公式為：

(16)

由里程計(jì)輸出的航位推算位置更新算法公式為：

(17)

利用單位時(shí)間內(nèi)慣導(dǎo)和航位推算位置增量之差作為量測(cè)，將航位推算誤差擴(kuò)充到狀態(tài)中。由于慣導(dǎo)位置解算使用微分計(jì)算，隨著時(shí)間的增加誤差累積增大，因此采用導(dǎo)航坐標(biāo)系下慣導(dǎo)位置增量和航位推算位置增量的每秒位置之差作為量測(cè)值：

(18)

式中，K為1s時(shí)間內(nèi)位置更新次數(shù)。

在采煤機(jī)運(yùn)行過(guò)程中慣導(dǎo)解算的真實(shí)位置和里程計(jì)航位推算的位置相同，根據(jù)公式進(jìn)一步整理，即可得到觀測(cè)矩陣H(t)。

(19)

(20)

F(k|k-1)=F(k，k-1)P(k-1)×

FT(k，k-1)+Q(k-1)

(21)

量測(cè)更新方程為：

K(k)=P(k|k-1)HT(k)[H(k)×

P(k|k-1)HT(k)+R(k)]-1

(22)

(23)

P(k)=[I-K(k)H(k)]P(k|k-1)

(24)

對(duì)于確定的濾波方程，選取合適的初始值X0和均方誤差初值P0分別進(jìn)行濾波回路與增益回路的時(shí)間更新與量測(cè)更新，利用遞推算法，隨著濾波次數(shù)增加，提取的被估計(jì)誤差值的信息濃度增加，逐漸接近真實(shí)值，提高系統(tǒng)誤差補(bǔ)償精度。

3 實(shí)驗(yàn)驗(yàn)證

3.1 仿真實(shí)驗(yàn)

通過(guò)模擬慣導(dǎo)與里程計(jì)數(shù)據(jù)，應(yīng)用閉環(huán)卡爾曼濾波組合導(dǎo)航算法，對(duì)運(yùn)行軌跡進(jìn)行仿真，加入了爬坡轉(zhuǎn)向等情況，其中選取的誤差設(shè)定如下：光纖陀螺儀漂移為0.01°/h，加速度計(jì)零偏為5×10-5g，井下采煤機(jī)運(yùn)行速度較慢，打滑情況較少，因此設(shè)里程計(jì)刻度系數(shù)誤差為0.01%；初始位置誤差為0，慣導(dǎo)水平姿態(tài)角初始誤差為0.5′，航向角誤差為0.5′。仿真軌跡為：首先以0.1m/s2加速度運(yùn)行10s，上行15s，保持不變10s，下行15s，保持勻速運(yùn)行150s，向左運(yùn)行15s，向右運(yùn)行15s，保持勻速運(yùn)行100s，最后以-0.1m/s2加速度運(yùn)行10s，對(duì)比純慣導(dǎo)系統(tǒng)運(yùn)行軌跡、理論軌跡和測(cè)量軌跡對(duì)比，仿真軌跡如圖2所示。

圖2 三維軌跡仿真

純慣導(dǎo)測(cè)量軌跡隨著運(yùn)行時(shí)間推移，東向最大誤差13.5m，北向最大誤差5.02m，天向最大誤差2.46m，軌跡發(fā)散較為嚴(yán)重。組合定位軌跡中，東向最大誤差2.23m，北向最大誤差0.04m，天向最大誤差0.19m，抑制了定位誤差發(fā)散。因此采煤機(jī)采用高精度光纖陀螺儀與里程計(jì)組合定位系統(tǒng)，應(yīng)用閉環(huán)卡爾曼濾波修正方法有效的提高了定位精度與穩(wěn)定性。

3.2 井下實(shí)驗(yàn)

為了測(cè)試在井下復(fù)雜環(huán)境下的定位精度與穩(wěn)定性，將此組合定位系統(tǒng)安裝到某煤礦綜采工作面的采煤機(jī)上進(jìn)行井下實(shí)驗(yàn)，慣導(dǎo)安裝于弱電箱內(nèi)，慣導(dǎo)系統(tǒng)由高精度國(guó)產(chǎn)光纖慣導(dǎo)、任務(wù)機(jī)、UPS電源組成。

采煤機(jī)低速在工作面113.43m處到139.37m處反復(fù)空刀運(yùn)行，單刀運(yùn)行約25min，共用時(shí)40min，保持液壓支架不動(dòng)，驗(yàn)證采煤機(jī)定位精度。其中，井下每個(gè)液壓支架寬度為1.5m，采煤機(jī)型號(hào)為MG650/1630-WD。建立采煤機(jī)坐標(biāo)系，x軸指向采煤機(jī)牽引方向，y軸指向工作面推進(jìn)方向，z軸指向高度方向。慣導(dǎo)對(duì)準(zhǔn)后，陀螺儀精度為0.01°/h，加速度計(jì)精度為5×10-5g，采煤機(jī)往復(fù)運(yùn)行過(guò)程中，俯仰角、橫滾角、航向角初始值分別為3.69°、6.27°、181.6°。采煤機(jī)在連續(xù)兩刀往復(fù)行走，觀測(cè)值與實(shí)驗(yàn)值的角度對(duì)比如圖3所示，俯仰角最大誤差為0.07°，橫滾角最大誤差為0.184°，航向角最大誤差為0.22°，角度誤差如圖4所示。

圖3 角度對(duì)比

圖4 角度差

觀測(cè)軌跡與實(shí)驗(yàn)軌跡的位置對(duì)比如圖5所示，x方向最大誤差為2.046cm，y方向最大誤差為2.8cm，高度方向最大誤差為1.4cm，誤差曲線如圖6所示。實(shí)驗(yàn)過(guò)程中存在一定誤差，主要原因在于采煤機(jī)往復(fù)行走過(guò)程中一是受浮煤阻力及振動(dòng)等影響，產(chǎn)生位姿變化；二是刮板輸送機(jī)中部槽與液壓缸之間銷耳間隙，導(dǎo)致位姿變化。綜合分析，“慣導(dǎo)+里程計(jì)”測(cè)量系統(tǒng)位姿檢測(cè)精度與穩(wěn)定性較好，在復(fù)雜環(huán)境下定位誤差小于5cm。

圖5 運(yùn)行軌跡對(duì)比

圖6 位置誤差

4 結(jié) 論

1)“慣導(dǎo)+里程計(jì)”組合測(cè)量系統(tǒng)應(yīng)用閉環(huán)卡爾曼濾波修正方法，反饋組合導(dǎo)航內(nèi)部參數(shù)，提高了測(cè)量精度。

2)慣導(dǎo)測(cè)得的采煤機(jī)位置誤差會(huì)隨時(shí)間累積，應(yīng)用“慣導(dǎo)+里程計(jì)”組合定位方法，能夠抑制誤差的發(fā)散，保證測(cè)量系統(tǒng)的穩(wěn)定性。