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

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

煤炭生產(chǎn)裝備智能化是煤炭綠色安全高效發(fā)展的關(guān)鍵[1]。針對煤礦巷道掘進(jìn)存在地質(zhì)復(fù)雜、工藝裝備落后，作業(yè)人員偏多，操作環(huán)境惡劣，安全風(fēng)險(xiǎn)嚴(yán)峻，采快掘慢失衡等問題[2]，深入研究綜掘工作面的智能化問題顯得十分重要，而掘進(jìn)機(jī)的位姿測量是綜掘工作面智能化的重要前提。掘進(jìn)機(jī)工作時(shí)傳統(tǒng)的定向方法是借助一束與巷道預(yù)設(shè)方向平行的激光來指向，掘進(jìn)機(jī)司機(jī)依照該方向手動(dòng)控制掘進(jìn)機(jī)完成糾偏，該方法掘進(jìn)精度低，測量工作量大且人員安全得不到保障。針對掘進(jìn)機(jī)的位姿測量提出了很多新的方法，大致可以總結(jié)為基于激光導(dǎo)向器測量系統(tǒng)[3，4]、基于機(jī)器視覺測量系統(tǒng)[5，6]和基于慣性測量系統(tǒng)[7]。

激光導(dǎo)向器測量系統(tǒng)是目前應(yīng)用最廣泛的導(dǎo)向系統(tǒng)，其原理是通過對掘進(jìn)機(jī)的距離和角度進(jìn)行測量，再經(jīng)過解算得到掘進(jìn)機(jī)位姿數(shù)據(jù)，例如德國VMT公司研發(fā)的SLS-T系統(tǒng)[8]，該系統(tǒng)通過對透過旋轉(zhuǎn)陰屏的激光強(qiáng)度進(jìn)行檢測，當(dāng)光強(qiáng)達(dá)到最大時(shí)，根據(jù)此時(shí)陰屏的旋轉(zhuǎn)角度來計(jì)算掘進(jìn)機(jī)的方向角，該系統(tǒng)實(shí)時(shí)性好，但需要的操作人員數(shù)量較多。機(jī)器視覺測量系統(tǒng)是通過采集待測物體的幾何特征，利用成像模型從而解算出目標(biāo)物體的位姿信息，在文獻(xiàn)[9]中，卡內(nèi)基梅隆大學(xué)基于機(jī)器視覺研發(fā)了一套掘進(jìn)機(jī)導(dǎo)向系統(tǒng)，但由于煤礦井下環(huán)境惡劣，粉塵大，導(dǎo)致該系統(tǒng)無法在大傾角煤礦巷道內(nèi)使用；在文獻(xiàn)[10]中，作者通過將目標(biāo)光靶與掘進(jìn)機(jī)進(jìn)行固連，使用攝像機(jī)識(shí)別出光靶上的特征點(diǎn)在圖像中的位置，從而解算出掘進(jìn)機(jī)的位姿，但該方法的測量精度受限于光靶與攝像機(jī)之間的距離，并不適用于遠(yuǎn)程位姿測量。慣性測量系統(tǒng)原理是通過慣性測量元件陀螺儀和加速度計(jì)采集掘進(jìn)機(jī)的角速度和加速度數(shù)據(jù)，通過數(shù)值積分運(yùn)算得到掘進(jìn)機(jī)的位姿信息，在文獻(xiàn)[11]中，作者通過對純慣性導(dǎo)航系統(tǒng)進(jìn)行仿真，結(jié)果表明該系統(tǒng)測量得到的姿態(tài)角數(shù)據(jù)滿足國家煤礦井巷工程驗(yàn)收標(biāo)準(zhǔn)，而測量得到的位置誤差遠(yuǎn)遠(yuǎn)超出了國家標(biāo)準(zhǔn)，所以慣性導(dǎo)航系統(tǒng)往往需要搭配其他位姿傳感器構(gòu)成組合導(dǎo)航系統(tǒng)來提高定位精度。在文獻(xiàn)[12]中，作者驗(yàn)證了零速修正的方法可以提高捷聯(lián)慣導(dǎo)系統(tǒng)對掘進(jìn)機(jī)定位的精度。在文獻(xiàn)[13]中，作者將油缸行程、激光、超聲、慣導(dǎo)和地磁構(gòu)成組合測量系統(tǒng)，實(shí)現(xiàn)對掘進(jìn)機(jī)位姿實(shí)時(shí)精確的測量。在文獻(xiàn)[14]中，為了對礦下或無GPS信號(hào)環(huán)境中的設(shè)備進(jìn)行定位，作者提出將雙目視覺和慣性技術(shù)構(gòu)成組合導(dǎo)航系統(tǒng)，利用視覺導(dǎo)航技術(shù)得到的導(dǎo)航參數(shù)來對慣導(dǎo)的數(shù)據(jù)進(jìn)行修正，從而提高定位精度。

通過對上述三類測量方式進(jìn)行對比，捷聯(lián)慣導(dǎo)系統(tǒng)憑借著不依賴外部環(huán)境的特點(diǎn)，使得其成為掘進(jìn)機(jī)位姿測量的首選。但是，捷聯(lián)慣導(dǎo)隨著運(yùn)行時(shí)間的增長，其解算結(jié)果的誤差會(huì)越來越大。因此，本文將捷聯(lián)慣導(dǎo)和推移油缸構(gòu)成組合測量系統(tǒng)，通過標(biāo)準(zhǔn)卡爾曼濾波算法將慣導(dǎo)數(shù)據(jù)和油缸位移數(shù)據(jù)進(jìn)行融合，旨在克服慣導(dǎo)位姿解算結(jié)果隨時(shí)間產(chǎn)生累計(jì)誤差的難題。

1 掘進(jìn)機(jī)行走及定位方案

本文以筆者團(tuán)隊(duì)聯(lián)合相關(guān)企業(yè)研發(fā)的煤礦智能掘進(jìn)機(jī)器人系統(tǒng)(如圖1所示)的位姿測量為例。

圖1 煤礦智能掘進(jìn)機(jī)器人系統(tǒng)

該護(hù)盾式智能掘進(jìn)機(jī)器人系統(tǒng)將截割機(jī)器人集成于臨時(shí)支護(hù)機(jī)器人內(nèi)，其行駛原理是臨時(shí)支護(hù)機(jī)器人Ⅰ和臨時(shí)支護(hù)機(jī)器人Ⅱ通過4個(gè)油缸連接，通過油缸前推后拉，使得盾體按照截距要求間歇前進(jìn)，其結(jié)構(gòu)如圖2所示。將捷聯(lián)慣導(dǎo)安裝在臨時(shí)支護(hù)機(jī)器人Ⅰ上，捷聯(lián)慣導(dǎo)采集臨時(shí)支護(hù)機(jī)器人Ⅰ的加速度和角速度信息，對其解算得到臨時(shí)支護(hù)機(jī)器人Ⅰ的姿態(tài)和位置信息。再將油缸推移前進(jìn)的位移數(shù)據(jù)與慣導(dǎo)解算得到的位置數(shù)據(jù)進(jìn)行融合，提高臨時(shí)支護(hù)機(jī)器人Ⅰ位姿精度。捷聯(lián)慣導(dǎo)與油缸組合定位原理如圖3所示。

圖2 臨時(shí)支護(hù)機(jī)器人結(jié)構(gòu)示意圖

圖3 捷聯(lián)慣導(dǎo)與油缸組合定位原理圖

2 智能掘進(jìn)機(jī)器人位姿解算

將臨時(shí)支護(hù)機(jī)器人Ⅰ所在位置的地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，并規(guī)定東向?yàn)閤n，北向?yàn)閥n，天向?yàn)閦n。以臨時(shí)支護(hù)機(jī)器人Ⅰ的重心位置為坐標(biāo)原點(diǎn)建立載體坐標(biāo)系，以臨時(shí)支護(hù)機(jī)器人Ⅰ前進(jìn)方向作為xb軸正方向，臨時(shí)支護(hù)機(jī)器人Ⅰ左側(cè)垂直xb軸方向作為yb軸正方向，垂直臨時(shí)支護(hù)機(jī)器人Ⅰ向上方向作為zb軸正方向。兩坐標(biāo)系可以通過繞三個(gè)方位軸旋轉(zhuǎn)而得到，例如導(dǎo)航坐標(biāo)系首先繞zn軸旋轉(zhuǎn)φ角度，得到坐標(biāo)系ox1y1z1，然后坐標(biāo)系ox1y1z1繞x1軸旋轉(zhuǎn)γ角度，得到坐標(biāo)系ox2y2z2，最后坐標(biāo)系ox2y2z2繞y2軸旋轉(zhuǎn)θ角度，得到載體坐標(biāo)系oxbybzb。其中φ、γ、θ分別為臨時(shí)支護(hù)機(jī)器人Ⅰ的航向角、橫滾角和俯仰角。

圖4 坐標(biāo)系示意圖

2.1 姿態(tài)解算模型

捷聯(lián)慣導(dǎo)系統(tǒng)算法是將前一時(shí)刻臨時(shí)支護(hù)機(jī)器人Ⅰ的位姿數(shù)據(jù)和最近采樣周期內(nèi)慣性測量元件的采樣值作為輸入，通過遞歸迭代的形式對系統(tǒng)的微分方程進(jìn)行求解，從而計(jì)算出當(dāng)前時(shí)刻臨時(shí)支護(hù)機(jī)器人Ⅰ的位姿信息。

臨時(shí)支護(hù)機(jī)器人Ⅰ姿態(tài)更新的四元數(shù)微分方程為[15]：

對上述四元數(shù)微分方程進(jìn)行求解，得到當(dāng)前時(shí)刻臨時(shí)支護(hù)機(jī)器人Ⅰ姿態(tài)信息的四元數(shù)表達(dá)，進(jìn)而可以求出姿態(tài)變換矩陣，其轉(zhuǎn)換公式如下：

計(jì)算得到姿態(tài)角：

θ=sin-1(T32)

2.2 位置解算模型

臨時(shí)支護(hù)機(jī)器人Ⅰ三個(gè)方向的速度更新微分方程分別為：

式中，fx、fy、fz分別為加速度計(jì)測量的比力，vx、vy、vz分別為臨時(shí)支護(hù)機(jī)器人Ⅰ在導(dǎo)航坐標(biāo)系下東向、北向和天向速度分量，g為臨時(shí)支護(hù)機(jī)器人Ⅰ所在位置重力加速度。

臨時(shí)支護(hù)機(jī)器人Ⅰ位置更新微分方程為：

通過對位置微分方程進(jìn)行積分可以得到臨時(shí)支護(hù)機(jī)器人Ⅰ所在位置的經(jīng)緯度和高度信息。

3 智能掘進(jìn)機(jī)器人位移模型建立

當(dāng)臨時(shí)支護(hù)機(jī)器人Ⅰ直線前進(jìn)時(shí)，其兩側(cè)油缸推移量相等，此時(shí)臨時(shí)支護(hù)機(jī)器人Ⅰ的位移量與油缸推移量相等。

當(dāng)臨時(shí)支護(hù)機(jī)器人Ⅰ處于糾偏狀態(tài)時(shí)，此時(shí)兩側(cè)油缸推移量不等，如圖5所示。由于捷聯(lián)慣導(dǎo)與臨時(shí)支護(hù)機(jī)器人Ⅰ固連，所以臨時(shí)支護(hù)機(jī)器人Ⅰ的位移與捷聯(lián)慣導(dǎo)的位移等效，而捷聯(lián)慣導(dǎo)的位移可以用其在臨時(shí)支護(hù)機(jī)器人Ⅰ后面的投影位移近似等效。

圖5 智能掘進(jìn)機(jī)器人系統(tǒng)糾偏示意圖

智能掘進(jìn)機(jī)器人處于糾偏轉(zhuǎn)態(tài)時(shí)，其俯視圖如圖6所示。

圖6 油缸糾偏俯視圖

圖中LAB、LCD分別表示左側(cè)油缸和右側(cè)油缸，兩者相互平行，a、b分別為捷聯(lián)慣導(dǎo)與臨時(shí)支護(hù)機(jī)器人Ⅰ盾體左右面之間的距離，此時(shí)智能掘進(jìn)機(jī)器人的位移計(jì)算公式為：

4 捷聯(lián)慣導(dǎo)與油缸推移量數(shù)據(jù)融合算法

4.1 航位推算算法

在油缸推移智能掘進(jìn)機(jī)器人前進(jìn)的過程中，其路程增量在載體坐標(biāo)系的投影為：

由于在捷聯(lián)慣導(dǎo)更新算法中已經(jīng)對姿態(tài)信息進(jìn)行了求解，所以在航位推算算法中無需再次對姿態(tài)信息進(jìn)行更新，而是使用同一姿態(tài)矩陣對油缸推移量進(jìn)行坐標(biāo)轉(zhuǎn)換，進(jìn)而對航位推算時(shí)智能掘進(jìn)機(jī)器人位置進(jìn)行更新。

4.2 捷聯(lián)慣導(dǎo)/航位推算組合算法

根據(jù)捷聯(lián)慣導(dǎo)誤差和航位推算誤差方程[16]，構(gòu)造如下狀態(tài)向量：

X=[φT(δvn)T(δp)T(δpD)T

式中，φT為計(jì)算導(dǎo)航坐標(biāo)系與理想導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角誤差；(δv)T為臨時(shí)支護(hù)機(jī)器人Ⅰ的速度誤差；(δp)T為臨時(shí)支護(hù)機(jī)器人Ⅰ的位置誤差；(δpD)T為航位推算時(shí)的位置誤差；(εb)T為陀螺儀零偏誤差；(b)T為加速度計(jì)零偏誤差；其中αθ、αφ分別為捷聯(lián)慣導(dǎo)安裝時(shí)在x軸和y軸方向上存在的安裝偏角，δKD為油缸的刻度系數(shù)誤差。

以捷聯(lián)慣導(dǎo)解算算法得到的位置數(shù)據(jù)與航位推算得到的位置數(shù)據(jù)之差構(gòu)建觀測量：

Z=δp-δpD(13)

綜上，可以建立基于捷聯(lián)慣導(dǎo)和油缸的組合定位狀態(tài)空間模型為：

其中：

式中，F(xiàn)為慣導(dǎo)系統(tǒng)誤差轉(zhuǎn)移矩陣；F11和F22的計(jì)算方法如文獻(xiàn)[16]所示；W為陀螺儀和加速度計(jì)的測量白噪聲；V為測量噪聲。

根據(jù)上述狀態(tài)方程和量測方程，將k-1時(shí)刻的導(dǎo)航參數(shù)信息作為輸入，計(jì)算得到k時(shí)刻的位置和姿態(tài)數(shù)據(jù)，隨著濾波次數(shù)的增加，得到的位姿數(shù)據(jù)會(huì)越來越接近真實(shí)值，進(jìn)而提高系統(tǒng)的測量精度。

5 實(shí)驗(yàn)驗(yàn)證與數(shù)據(jù)分析

在榆北煤業(yè)小保當(dāng)煤礦對煤礦智能掘進(jìn)機(jī)器人系統(tǒng)進(jìn)行了井下工業(yè)性試驗(yàn)，試驗(yàn)驗(yàn)證了本文提出的定位方法。

5.1 慣導(dǎo)姿態(tài)角實(shí)驗(yàn)

首先智能掘進(jìn)機(jī)器人系統(tǒng)保持靜止?fàn)顟B(tài)，捷聯(lián)慣導(dǎo)進(jìn)行初始對準(zhǔn)，待初始對準(zhǔn)完成后得到智能掘進(jìn)機(jī)器人系統(tǒng)的初始姿態(tài)角數(shù)據(jù)，其中俯仰角為-0.184514°，橫滾角為0.170776°，航向角為180.369611°。智能掘進(jìn)機(jī)器人系統(tǒng)繼續(xù)保持靜止，并采集其1h內(nèi)的姿態(tài)角數(shù)據(jù)，觀察其變化趨勢。智能掘進(jìn)機(jī)器人系統(tǒng)的俯仰角、橫滾角和航向角在1h內(nèi)相對初始姿態(tài)角的漂移量分別如圖7、圖8和圖9所示，從圖中可以看出1h后俯仰角漂移了0.018596°，橫滾角漂移了0.004635°，航向角漂移了0.011898°。

圖7 俯仰角漂移量

圖8 橫滾角漂移量

圖9 航向角漂移量

5.2 慣導(dǎo)與油缸組合定位實(shí)驗(yàn)

智能掘進(jìn)機(jī)器人系統(tǒng)按照初始方向直線前進(jìn)21m，油缸每次推移量為1m，在油缸中安裝了位移傳感器，其推移誤差為千分之一，即±1mm。

捷聯(lián)慣導(dǎo)和油缸組合測量下智能掘進(jìn)機(jī)器人位置曲線和位置誤差曲線分別如圖10和圖11所示。圖10可以看出，組合定位得到的位置曲線可以精確的反應(yīng)智能掘進(jìn)機(jī)器人的實(shí)際位置。從圖11可以看出，組合定位系統(tǒng)下智能掘進(jìn)機(jī)器人在X軸和Y軸方向上的位置誤差分別控制在3.5cm和2cm的范圍內(nèi)，有效消除了純慣導(dǎo)解算造成的累計(jì)誤差，顯著提高了定位的精度。

圖10 組合定位下智能掘進(jìn)機(jī)器人位置曲線

圖11 組合定位位置誤差曲線

6 結(jié) 論

1)提出了一種基于捷聯(lián)慣導(dǎo)和推移油缸信息融合的煤礦智能智能掘進(jìn)機(jī)器人位姿測量方法：利用捷聯(lián)慣導(dǎo)測量得到智能掘進(jìn)機(jī)器人的姿態(tài)和位置信息；利用油缸推移量可以精確控制的特點(diǎn)，分別建立煤礦智能掘進(jìn)機(jī)器人在直行和糾偏狀態(tài)下的位移模型；利用標(biāo)準(zhǔn)卡爾曼濾波算法實(shí)現(xiàn)捷聯(lián)慣導(dǎo)數(shù)據(jù)和油缸數(shù)據(jù)的融合，抑制捷聯(lián)慣導(dǎo)在位置解算結(jié)果上的累計(jì)誤差，提高測量精度。

2)在煤礦智能掘進(jìn)機(jī)器人系統(tǒng)上分別進(jìn)行姿態(tài)角測量和組合定位實(shí)驗(yàn)，結(jié)果表明俯仰角、橫滾角、航向角、X軸和Y軸誤差均滿足設(shè)計(jì)要求，測量精度高。