馬宏偉，毛金根，毛清華，張旭輝，劉博興

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

0 引 言

掘進(jìn)機(jī)是煤礦巷道開(kāi)拓的主要設(shè)備，隨著快掘系統(tǒng)的深入應(yīng)用，對(duì)井下煤礦巷道掘進(jìn)的自動(dòng)化提出了更高要求，掘進(jìn)機(jī)的精確定位定向是其中最為關(guān)鍵的技術(shù)[1-2]。要從根本上改變掘進(jìn)工作面的落后狀態(tài)，必須解決掘進(jìn)機(jī)的自主精確定位定向問(wèn)題，提升掘進(jìn)機(jī)的智能化水平。

目前掘進(jìn)機(jī)的定位定向方法主要有激光指向儀、數(shù)字全站儀、超寬帶、機(jī)器視覺(jué)、和組合慣導(dǎo)等[3-5]。傳統(tǒng)的激光指向儀定位定向依賴(lài)于掘進(jìn)機(jī)司機(jī)的經(jīng)驗(yàn)，司機(jī)通過(guò)激光光斑在斷面上位置來(lái)調(diào)整掘進(jìn)機(jī)截割頭掘進(jìn)，這種掘進(jìn)定位定向方法存在較大的誤差?；跀?shù)字全站儀的定位方法已經(jīng)廣泛應(yīng)用于盾構(gòu)機(jī)形式的掘進(jìn)機(jī)中[6-7]，雖然這種方法精度高，但是對(duì)于煤礦井下復(fù)雜的地理環(huán)境，全站儀的視線將可能被擋住，無(wú)法實(shí)時(shí)獲得掘進(jìn)機(jī)的位置信息，并且無(wú)法檢測(cè)掘進(jìn)機(jī)的航向?；诔瑢拵У膶?dǎo)向和定位方法采用無(wú)線電信號(hào)的測(cè)距原理對(duì)掘進(jìn)機(jī)定位定向，其在一定的范圍優(yōu)勢(shì)明顯，缺點(diǎn)是不適合于井下長(zhǎng)距離的掘進(jìn)機(jī)定位定向[8-9]?；谝曈X(jué)定位方法采用圖像處理技術(shù)對(duì)掘進(jìn)機(jī)的位姿解算，能非接觸、實(shí)時(shí)得到掘進(jìn)機(jī)的定位信息[10]，但是容易受井下粉塵影響。捷聯(lián)慣導(dǎo)系統(tǒng)具有自主性強(qiáng)、環(huán)境適應(yīng)性好的特點(diǎn)，基于捷聯(lián)慣導(dǎo)的井下設(shè)備定位定向方法是目前的研究熱點(diǎn)[11-14]，但是隨著掘進(jìn)機(jī)長(zhǎng)時(shí)間運(yùn)行，慣導(dǎo)系統(tǒng)的定位誤差會(huì)逐漸累積，組合慣導(dǎo)的定位定向方法可以有效抑制純慣導(dǎo)累積誤差。視覺(jué)與慣導(dǎo)組合、超寬帶與慣導(dǎo)組合等方式在掘進(jìn)機(jī)定位定向應(yīng)用廣泛，但是視覺(jué)與慣導(dǎo)組合定位的精度易受煤礦井下低照度、高粉塵等掘進(jìn)工作面復(fù)雜環(huán)境影響，超寬帶與慣導(dǎo)組合定位的精度難以滿足掘進(jìn)機(jī)定位精度要求。

綜上所述，以上掘進(jìn)機(jī)的定位定向方法各有其優(yōu)缺點(diǎn)，針對(duì)煤礦井下的復(fù)雜環(huán)境，結(jié)合數(shù)字全站儀的定位精度高和光纖慣導(dǎo)自主精確定位定向的優(yōu)點(diǎn)，提出融合光纖慣導(dǎo)與數(shù)字全站儀的掘進(jìn)機(jī)自主定位定向方法，該方法利用卡爾曼濾波算法對(duì)兩者信息進(jìn)行融合，實(shí)現(xiàn)掘進(jìn)機(jī)的精確定位定向，為實(shí)現(xiàn)煤礦掘進(jìn)機(jī)的智能化奠定良好基礎(chǔ)。

1 定位定向系統(tǒng)原理

光纖慣導(dǎo)系統(tǒng)主要由陀螺儀和加速度計(jì)，以及數(shù)學(xué)解算平臺(tái)等構(gòu)成，它通過(guò)二次積分的方式將加速度計(jì)測(cè)量出的載體的加速度轉(zhuǎn)化為位移，再將它變換到由陀螺儀建立的導(dǎo)航坐標(biāo)系中，從而得到載體在導(dǎo)航坐標(biāo)系中的速度、姿態(tài)角、位置信息；數(shù)字全站儀利用激光反射形成的光點(diǎn)，由內(nèi)置CCD相機(jī)接收，其位置以CCD相機(jī)的中心作為參考點(diǎn)來(lái)確定[15]。光纖慣導(dǎo)與數(shù)字全站儀融合定位定向原理如圖1所示，利用煤礦巷道設(shè)計(jì)的中線上2個(gè)地理坐標(biāo)系下坐標(biāo)點(diǎn)，數(shù)字全站儀根據(jù)這兩點(diǎn)的坐標(biāo)建立數(shù)字全站儀的坐標(biāo)系，光纖慣導(dǎo)從而獲取掘進(jìn)機(jī)在地理坐標(biāo)系的位置信息。通過(guò)自身的數(shù)學(xué)解算平臺(tái)，輸出掘進(jìn)機(jī)的位姿信息。采用卡爾曼濾波方法對(duì)這兩者數(shù)據(jù)進(jìn)行融合，實(shí)時(shí)輸出掘進(jìn)機(jī)的精準(zhǔn)位姿信息，實(shí)現(xiàn)掘進(jìn)機(jī)的精確定位定向。

圖1 基于光纖慣導(dǎo)與數(shù)字全站儀的組合定位定向原理Fig.1 Schematic diagram of combined positioning and orientation of strapdown inertial navigation and digital total station

2 定位定向系統(tǒng)誤差模型

選取光纖慣導(dǎo)與數(shù)字全站儀的系統(tǒng)誤差作為定位定向的狀態(tài)量、位置差值作為量測(cè)量，并采用卡爾曼濾波算法對(duì)狀態(tài)量和量測(cè)量進(jìn)行預(yù)測(cè)，獲取二者融合后的位姿。因此，需要對(duì)光纖慣導(dǎo)和數(shù)字全站儀的誤差進(jìn)行分析與建模。光纖慣導(dǎo)系統(tǒng)誤差建模已經(jīng)有過(guò)很多研究[16-19]，主要分析數(shù)字全站儀的誤差來(lái)源與數(shù)字全站儀的誤差建模。

在光纖慣導(dǎo)與數(shù)字全站儀的定位定向系統(tǒng)中，數(shù)字全站儀的位置誤差直接決定了定位定向系統(tǒng)的精度，通過(guò)分析數(shù)字全站儀的輸出位置解算公式，得出數(shù)字全站儀的誤差，數(shù)字全站儀輸出位置解算公式如下：

(1)

式中：x0,y0,z0為測(cè)站點(diǎn)的N、E、Z坐標(biāo)(已知)；D為斜距；a為目標(biāo)點(diǎn)與測(cè)站點(diǎn)連線與z軸的夾角；b為方位角；hi為儀器高；ht為目標(biāo)高。

一般測(cè)站點(diǎn)、儀器高和目標(biāo)高為定值，而且目標(biāo)點(diǎn)與測(cè)站點(diǎn)連線與z軸的夾角變化很小，誤差可忽略，所以數(shù)字全站儀主要誤差源為全站儀的斜距距測(cè)量誤差和方位角誤差。

設(shè)數(shù)字全站儀的兩點(diǎn)斜距測(cè)量誤差為mD，方位角誤差δb，則測(cè)量誤差公式如下：

(2)

式中：δP為數(shù)字全站儀測(cè)量誤差；ρ為弧度化秒因子。

3 定位定向系統(tǒng)融合算法

采用融合光纖慣導(dǎo)與數(shù)字全站儀的定位定向算法。首先，建立定位定向的系統(tǒng)狀態(tài)方程和量測(cè)方程；然后，利用卡爾曼濾波器進(jìn)行濾波處理，由此獲取光纖慣導(dǎo)和數(shù)字全站儀的融合位姿信息。

3.1 定位定向系統(tǒng)狀態(tài)方程

光纖慣導(dǎo)與數(shù)字全站儀融合的系統(tǒng)狀態(tài)方程主要包括：捷聯(lián)慣導(dǎo)的數(shù)學(xué)平臺(tái)姿態(tài)誤差角φE、φN、φu，速度誤差δE、δN、δu，位置誤差δL，陀螺漂移δλ，加速度計(jì)誤差εE、εN、εu，以及數(shù)字全站儀的位置誤差?E、?N、?u，如式(3)所示。

(3)

根據(jù)上述組合定位定向系統(tǒng)誤差模型，可得到融合定位定向狀態(tài)方程，如式(4)所示：

(4)

式中：F(t)、X(t)、G(t)、W(t)分別為系統(tǒng)狀態(tài)矩陣、系統(tǒng)的狀態(tài)、系統(tǒng)噪聲系數(shù)陣、系統(tǒng)噪聲。

3.2 定位定向系統(tǒng)量測(cè)方程

選取光纖慣導(dǎo)輸出的位置與數(shù)字全站儀位置的差值作為量測(cè)值，設(shè)光纖慣導(dǎo)解算的位置和數(shù)字全站儀解算位置分別為xSINS、ySINS、xTS、yTS，則光纖慣導(dǎo)與數(shù)字全站儀的量測(cè)方程如下式：

Z(t)=H(t)X(t)+V(t)

(5)

3.3 卡爾曼濾波器設(shè)計(jì)

根據(jù)上述融合定位定向系統(tǒng)的狀態(tài)方程和量測(cè)方程，采用卡爾曼濾波算法將光纖慣導(dǎo)與數(shù)字全站儀信息融合，得到準(zhǔn)確的定位定向信息，卡爾曼算法流程如下：

1)將上述融合系統(tǒng)的狀態(tài)量與量測(cè)量離散化處理，獲得離散型狀態(tài)方程和量測(cè)方程，如式(6)所示：

(6)

式中：A為狀態(tài)轉(zhuǎn)移矩陣；xk-1,xk分別為k-1和k時(shí)刻的狀態(tài)量；uk為k時(shí)刻系統(tǒng)的輸入量；wk為過(guò)程狀態(tài)噪聲，服從均值為0，方差為Q的正態(tài)分布；B為輸入增益矩陣；zk為k時(shí)刻觀測(cè)量；H為測(cè)量矩陣；vk為量測(cè)噪聲，服從均值為0，方差為R的正態(tài)分布。

2)根據(jù)系統(tǒng)上一時(shí)刻的狀態(tài)量預(yù)測(cè)下一時(shí)刻的狀態(tài)量，公式如下：

(7)

3)利用數(shù)字全站儀和光纖慣導(dǎo)的位置差值作為量測(cè)量來(lái)校正融合定位定向系統(tǒng)的位置誤差，達(dá)到融合目的，公式如下：

(8)

4)協(xié)方差進(jìn)行更新，公式如下：

(9)

4 試驗(yàn)驗(yàn)證與分析

搭建如圖2所示的光纖慣導(dǎo)與數(shù)字全站儀的定位定向試驗(yàn)平臺(tái)，主要包括光纖慣導(dǎo)，數(shù)字全站儀，360°棱鏡、協(xié)同控制器、開(kāi)關(guān)電源和上位機(jī)等。高精度的光纖慣導(dǎo)和數(shù)字全站儀主要參數(shù)見(jiàn)表1。

圖2 定位定向試驗(yàn)平臺(tái)Fig.2 Experimental platform of positioning and orientation

表1 光纖慣導(dǎo)與數(shù)字全站儀的主要參數(shù)Table 1 Main parameters of fiber optic inertial navigation and digital total station

模擬井下掘進(jìn)的實(shí)際情況，取巷道中線上2點(diǎn)，并利用這兩點(diǎn)建立數(shù)字全站儀的坐標(biāo)系，此時(shí)數(shù)字全站儀輸出的位置即反映了掘進(jìn)機(jī)在井下掘進(jìn)巷道坐標(biāo)系(地理坐標(biāo)系)的坐標(biāo)值，從而將光纖慣導(dǎo)的導(dǎo)航坐標(biāo)系(地理坐標(biāo)系)與數(shù)字全站儀建立的坐標(biāo)系進(jìn)行統(tǒng)一。在這次試驗(yàn)中，將這2點(diǎn)連線作為x軸(與巷道中線平行)，與之垂直的方向?yàn)閥軸，建立坐標(biāo)系o-x-y-z，并以掘進(jìn)機(jī)機(jī)身的中線為x1軸，與之垂直的方向?yàn)閥1，建立載體坐標(biāo)系o1-x1-y1-z1，掘進(jìn)機(jī)在oxy平面的轉(zhuǎn)動(dòng)角為航向角，掘進(jìn)機(jī)在oyz平面的轉(zhuǎn)動(dòng)角為橫滾角，掘進(jìn)機(jī)在oxz平面的轉(zhuǎn)動(dòng)角為俯仰角，如圖3所示。棱鏡和慣導(dǎo)放在一起，首先進(jìn)行光纖慣導(dǎo)初始對(duì)準(zhǔn)，然后掘進(jìn)機(jī)前進(jìn)，通過(guò)無(wú)線路由器實(shí)時(shí)將協(xié)同控制器獲得的數(shù)字全站儀位置發(fā)送給慣導(dǎo)系統(tǒng)進(jìn)行融合，最后輸出兩者融合的位置和姿態(tài)信息。

本次試驗(yàn)分為2個(gè)部分進(jìn)行驗(yàn)證：

1)如圖2所示，運(yùn)用光纖慣導(dǎo)靜止2 h輸出的姿態(tài)角變化情況如圖4、5、6所示，試驗(yàn)結(jié)果表明：航向角、俯仰角和橫滾角檢測(cè)誤差小于0.005°，能夠提供準(zhǔn)確的定向信息。

圖4 俯仰角變化Fig.4 Change of pitch angle

圖5 橫滾角變化Fig.5 Change of roll angle

圖6 航向角變化Fig.6 Change of heading angle

2)通過(guò)GNSS衛(wèi)星定位接收機(jī)檢測(cè)得到初始位置為:x0=3 818 661.12 m，y0=592 377.82 m，z0=

346.47 m。融合定位系統(tǒng)中方差R值對(duì)融合定位精度影響很大，為驗(yàn)證R值對(duì)融合定位精度的影響，將數(shù)字全站儀的定位精度的平方作為組合慣導(dǎo)系統(tǒng)的量測(cè)量的方差R值。分別采用R=0.12和R=0.012兩種方差對(duì)掘進(jìn)機(jī)直線行駛進(jìn)行定位對(duì)比試驗(yàn)，其中R=0.012為數(shù)字全站儀的定位精度，選取部分采樣點(diǎn)進(jìn)行分析，試驗(yàn)結(jié)果分別如圖7和圖8所示。通過(guò)圖7與圖8的x方向、y方向定位對(duì)比可以得出：根據(jù)數(shù)字全站儀的定位精度確定方差R值后，光纖慣導(dǎo)與數(shù)字全站儀融合后的x方向誤差在3 cm以?xún)?nèi)，y方向的誤差在2 cm以?xún)?nèi)，能夠?qū)崿F(xiàn)掘進(jìn)機(jī)精確定位。

圖7 x方向位置解算對(duì)比Fig.7 Comparison of position calculation in x direction

圖8 y方向位置解算對(duì)比Fig.8 Comparison of position resolution in y direction

通過(guò)上述數(shù)據(jù)分析，基于光纖慣導(dǎo)與數(shù)字全站儀的掘進(jìn)機(jī)的定位定向方法，克服了純慣導(dǎo)定位誤差發(fā)散和數(shù)字全站儀的無(wú)法實(shí)時(shí)獲取掘進(jìn)機(jī)位置信息等問(wèn)題，實(shí)現(xiàn)了掘進(jìn)機(jī)的自主精確定位定向。

5 結(jié) 論

1)針對(duì)掘進(jìn)機(jī)自主定位定向不準(zhǔn)確問(wèn)題，提出了一種基于光纖慣導(dǎo)與數(shù)字全站儀組合的自主定位定向方法，該方法發(fā)揮了數(shù)字全站儀和光纖捷聯(lián)慣導(dǎo)的各自?xún)?yōu)勢(shì)，實(shí)現(xiàn)了掘進(jìn)機(jī)的自主定位定向。

2)通過(guò)建立光纖慣導(dǎo)與數(shù)字全站儀組合定位定向系統(tǒng)的狀態(tài)方程和量測(cè)方程，以及運(yùn)用卡爾曼濾波器對(duì)系統(tǒng)的狀態(tài)量與量測(cè)量進(jìn)行預(yù)測(cè)，可以獲得慣導(dǎo)與數(shù)字全站儀融合后的位姿信息。

3)根據(jù)數(shù)字全站儀定位精度確定系統(tǒng)量測(cè)量的噪聲方差R值，可以獲得最佳的融合定位定向精度，定位誤差小于3 cm，定向誤差小于0.005°，能夠?qū)崿F(xiàn)掘進(jìn)機(jī)精確定位定向，對(duì)提升掘進(jìn)工作面的智能化水平奠定了良好基礎(chǔ)。