基于捷聯(lián)慣導(dǎo)和推移油缸信息融合的智能掘進機器人位姿測量方法

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

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

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

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

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

1 掘進機行走及定位方案

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

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

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

圖2 臨時支護機器人結(jié)構(gòu)示意圖

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

2 智能掘進機器人位姿解算

將臨時支護機器人Ⅰ所在位置的地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，并規(guī)定東向為xn，北向為yn，天向為zn。以臨時支護機器人Ⅰ的重心位置為坐標(biāo)原點建立載體坐標(biāo)系，以臨時支護機器人Ⅰ前進方向作為xb軸正方向，臨時支護機器人Ⅰ左側(cè)垂直xb軸方向作為yb軸正方向，垂直臨時支護機器人Ⅰ向上方向作為zb軸正方向。兩坐標(biāo)系可以通過繞三個方位軸旋轉(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。其中φ、γ、θ分別為臨時支護機器人Ⅰ的航向角、橫滾角和俯仰角。

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

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

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

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

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

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

θ=sin-1(T32)

2.2 位置解算模型

臨時支護機器人Ⅰ三個方向的速度更新微分方程分別為：

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

臨時支護機器人Ⅰ位置更新微分方程為：

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

3 智能掘進機器人位移模型建立

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

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

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

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

圖6 油缸糾偏俯視圖

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

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

4.1 航位推算算法

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

由于在捷聯(lián)慣導(dǎo)更新算法中已經(jīng)對姿態(tài)信息進行了求解，所以在航位推算算法中無需再次對姿態(tài)信息進行更新，而是使用同一姿態(tài)矩陣對油缸推移量進行坐標(biāo)轉(zhuǎ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為計算導(dǎo)航坐標(biāo)系與理想導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角誤差；(δv)T為臨時支護機器人Ⅰ的速度誤差；(δp)T為臨時支護機器人Ⅰ的位置誤差；(δpD)T為航位推算時的位置誤差；(εb)T為陀螺儀零偏誤差；(b)T為加速度計零偏誤差；其中αθ、αφ分別為捷聯(lián)慣導(dǎo)安裝時在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的計算方法如文獻[16]所示；W為陀螺儀和加速度計的測量白噪聲；V為測量噪聲。

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

5 實驗驗證與數(shù)據(jù)分析

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

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

首先智能掘進機器人系統(tǒng)保持靜止?fàn)顟B(tài)，捷聯(lián)慣導(dǎo)進行初始對準(zhǔn)，待初始對準(zhǔn)完成后得到智能掘進機器人系統(tǒng)的初始姿態(tài)角數(shù)據(jù)，其中俯仰角為-0.184514°，橫滾角為0.170776°，航向角為180.369611°。智能掘進機器人系統(tǒng)繼續(xù)保持靜止，并采集其1h內(nèi)的姿態(tài)角數(shù)據(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)與油缸組合定位實驗

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

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

圖10 組合定位下智能掘進機器人位置曲線

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

6 結(jié) 論

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

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