掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位方法

張旭輝， 劉博興， 張超， 楊文娟， 趙建勛

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

0 引言

2050年以前煤炭仍將是我國的主導(dǎo)能源[1-2]。煤礦巷道掘進(jìn)是煤礦開采的重要部分，但掘進(jìn)工作面環(huán)境惡劣，危險性高，因此實(shí)現(xiàn)掘進(jìn)機(jī)智能化，進(jìn)而實(shí)現(xiàn)掘進(jìn)工作面少人甚至無人化，對于煤礦安全生產(chǎn)具有重要意義。

掘進(jìn)機(jī)定姿定位是實(shí)現(xiàn)智能掘進(jìn)機(jī)的關(guān)鍵技術(shù)之一，近年來得到了很多研究人員的關(guān)注。文獻(xiàn)[3-4]提出了基于視覺的掘進(jìn)機(jī)位姿測量方法，但該方法易受井下粉塵等因素的影響。文獻(xiàn)[5]利用超寬帶技術(shù)對掘進(jìn)機(jī)進(jìn)行定姿定位檢測，該方法系統(tǒng)結(jié)構(gòu)簡單，但因煤礦井下環(huán)境中金屬設(shè)備較多，對超寬帶信號傳輸造成很大影響。文獻(xiàn)[6]提出采用空間交匯技術(shù)對懸臂式掘進(jìn)機(jī)進(jìn)行位姿測量，該方法靈活性強(qiáng)，但開發(fā)難度較大。文獻(xiàn)[7]采用捷聯(lián)慣導(dǎo)技術(shù)實(shí)現(xiàn)掘進(jìn)機(jī)定位，該方法環(huán)境適應(yīng)性強(qiáng)、短時間內(nèi)精度高[8]，但累計(jì)誤差隨時間推移逐漸增大[9]。文獻(xiàn)[10]采用全站儀進(jìn)行掘進(jìn)機(jī)定位，測量精度較高，但會因粉塵過大或其他原因?qū)е氯緝x光路被遮擋而無法進(jìn)行定位。

本文綜合利用全站儀的高精度及捷聯(lián)慣導(dǎo)的無源性特點(diǎn)，提出一種掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位方法。該方法采用卡爾曼濾波器融合全站儀測量的掘進(jìn)機(jī)位置信息和捷聯(lián)慣導(dǎo)測量的位姿信息，實(shí)現(xiàn)掘進(jìn)機(jī)組合定位，可在全站儀光路被遮擋的情況下實(shí)現(xiàn)掘進(jìn)機(jī)精準(zhǔn)定位。

1 總體方案

1.1 組合定位坐標(biāo)系

掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位涉及的坐標(biāo)系如圖1所示。

(1) 東-北-天坐標(biāo)系(簡稱T系)OtXtYtZt，表示載體所在位置。T系以載體重心為原點(diǎn)，指向東、北、天(也稱北-東-地坐標(biāo)系，滿足“右手定則”)。

(2) 掘進(jìn)機(jī)機(jī)體坐標(biāo)系(簡稱b系)ObXbYbZb。b系以掘進(jìn)機(jī)機(jī)身重心為原點(diǎn),Xb軸平行于巷道底面指向左，Yb軸指向掘進(jìn)機(jī)前進(jìn)方向，Zb軸指向掘進(jìn)機(jī)機(jī)身豎軸方向。

(3) 導(dǎo)航坐標(biāo)系(簡稱n系)OnXnYnZn。n系是導(dǎo)航過程中的基準(zhǔn)，一般根據(jù)載體導(dǎo)航需要選取。本文采用指北方位系統(tǒng)作為n系，即與T系重合。

(4) 全站儀測量坐標(biāo)系(簡稱c系)OcXcYcZc。c系以全站儀測站中心為原點(diǎn)，Xc軸為后視方向，Zc軸豎直向上，Yc軸根據(jù)“右手定則”確定。

1.2 組合定位原理

掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位過程包括基于全站儀的機(jī)身位置參數(shù)測量、基于捷聯(lián)慣導(dǎo)的機(jī)身位姿參數(shù)測量、基于卡爾曼濾波的機(jī)身位置參數(shù)與位姿參數(shù)融合。

組合定位原理如圖2所示。將捷聯(lián)慣導(dǎo)和目標(biāo)棱鏡安裝在掘進(jìn)機(jī)機(jī)身上，使其隨掘進(jìn)機(jī)移動。開始定位前，測繪人員測量掘進(jìn)機(jī)所在位置的經(jīng)緯度，并根據(jù)巷道走向確定西安80坐標(biāo)系下任意2個坐標(biāo)點(diǎn)，一個坐標(biāo)點(diǎn)作為全站儀測站點(diǎn)，另一個作為后視點(diǎn)。根據(jù)2個坐標(biāo)點(diǎn)構(gòu)建c系，全站儀通過測量掘進(jìn)機(jī)機(jī)身上的目標(biāo)棱鏡，解算出掘進(jìn)機(jī)機(jī)身在西安80坐標(biāo)系下的坐標(biāo)。捷聯(lián)慣導(dǎo)根據(jù)初始經(jīng)緯度進(jìn)行初始對準(zhǔn)，得到初始轉(zhuǎn)換矩陣。隨著掘進(jìn)機(jī)移動，捷聯(lián)慣導(dǎo)結(jié)合掘進(jìn)機(jī)初始經(jīng)緯度及初始姿態(tài)角，可實(shí)時測量出掘進(jìn)機(jī)位姿和速度。將捷聯(lián)慣導(dǎo)測量的經(jīng)緯度轉(zhuǎn)換為西安80坐標(biāo)系下的坐標(biāo)值，利用卡爾曼濾波器將捷聯(lián)慣導(dǎo)測量的位置、速度及姿態(tài)角信息與全站儀測量的掘進(jìn)機(jī)位置信息融合，得出掘進(jìn)機(jī)位姿信息。

圖2 掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位原理Fig.2 Roadheader positioning principle combining total station and strapdown inertial navigation system

2 基于捷聯(lián)慣導(dǎo)的掘進(jìn)機(jī)位姿參數(shù)測量

2.1 捷聯(lián)慣導(dǎo)初始對準(zhǔn)

捷聯(lián)慣導(dǎo)在工作前須進(jìn)行初始對準(zhǔn)，包括粗對準(zhǔn)和精對準(zhǔn)[11]。

初始粗對準(zhǔn)的目的是利用捷聯(lián)慣導(dǎo)三軸加速度計(jì)測量的重力加速度和陀螺儀測量的地球自轉(zhuǎn)速度計(jì)算出n系到b系的轉(zhuǎn)換矩陣。采用文獻(xiàn)[12]中方法實(shí)現(xiàn)捷聯(lián)慣導(dǎo)粗對準(zhǔn)。

完成粗對準(zhǔn)后，捷聯(lián)慣導(dǎo)獲取粗略的姿態(tài)轉(zhuǎn)換矩陣。為了獲取更準(zhǔn)確的姿態(tài)轉(zhuǎn)換矩陣，采用卡爾曼濾波器進(jìn)行精對準(zhǔn)，利用系統(tǒng)誤差作為濾波狀態(tài)，構(gòu)建卡爾曼濾波方程和觀測方程，實(shí)現(xiàn)精對準(zhǔn)[12-13]。

初始對準(zhǔn)時，掘進(jìn)機(jī)的速度為0，因此有

(1)

式中：VE，VN，VU分別為n系下掘進(jìn)機(jī)東、北、天方向速度；fE，fN，fU分別為三軸加速度計(jì)測量的東、北、天方向比力；g為重力加速度。

采用卡爾曼濾波器進(jìn)行精對準(zhǔn)時，通常利用外部觀測值(如位置、速度、姿態(tài)角等)確定失準(zhǔn)角[14]。本文以速度作為觀測值設(shè)計(jì)卡爾曼濾波器[15]。捷聯(lián)慣導(dǎo)在靜止?fàn)顟B(tài)下的系統(tǒng)誤差方程為

(2)

通過分析，可觀測值為ΔVE，ΔVN，ΔVU，φE，φN，φU，εN，εU,因此建立8個狀態(tài)模型，對式(2)進(jìn)行離散化處理，之后采用卡爾曼濾波器進(jìn)行最優(yōu)化估計(jì)，從而獲取準(zhǔn)確的姿態(tài)轉(zhuǎn)換矩陣。

卡爾曼濾波器狀態(tài)一步預(yù)測方程為

(3)

狀態(tài)估計(jì)方程為

(4)

式中：Kk為濾波增益；Zk為離散觀測值；Hk為離散觀測矩陣。

(5)

式中：Pk為狀態(tài)估計(jì)均方差；Rk為觀測噪聲方差。

一步預(yù)測均方差為

(6)

式中：Γk-1為系統(tǒng)離散噪聲驅(qū)動矩陣；Qk-1為噪聲方差。

狀態(tài)估計(jì)均方差為

Pk=(I-KkHk)Pk/k-1

(7)

式中I為單位矩陣。

2.2 掘進(jìn)機(jī)位姿解算

2.2.1 掘進(jìn)機(jī)姿態(tài)解算

采用四元數(shù)法進(jìn)行掘進(jìn)機(jī)姿態(tài)解算。掘進(jìn)機(jī)機(jī)身姿態(tài)角包括俯仰角γ、航向角α和橫滾角β。n系到b系的轉(zhuǎn)換矩陣為

(8)

n系到b系的旋轉(zhuǎn)四元數(shù)為

(9)

式中：Q為四元數(shù)，Q=[q0q1q2q3]；un為旋轉(zhuǎn)的軸和旋轉(zhuǎn)方向；θ為旋轉(zhuǎn)角度。

四元數(shù)基體坐標(biāo)系(n系)到b系的轉(zhuǎn)換矩陣為

(10)

由式(8)、式(10)可得掘進(jìn)機(jī)機(jī)身姿態(tài)角：

(11)

2.2.2 掘進(jìn)機(jī)位置解算

根據(jù)式(12)、式(13)計(jì)算掘進(jìn)機(jī)速度。由于掘進(jìn)機(jī)的垂直方向速度遠(yuǎn)小于水平方向速度，所以在計(jì)算掘進(jìn)機(jī)速度時，忽略垂直方向速度，同時忽略掘進(jìn)機(jī)高度。

(12)

(13)

式中：ωie為掘進(jìn)機(jī)跟隨地球的旋轉(zhuǎn)角速度；L為掘進(jìn)機(jī)所在位置的緯度；RN為地球卯酉曲率半徑。

則掘進(jìn)機(jī)位置更新微分方程為

(14)

式中：RM為子午圈主曲率半徑；λ為掘進(jìn)機(jī)所在位置的經(jīng)度。

2.3 經(jīng)緯度與西安80坐標(biāo)系之間轉(zhuǎn)換

進(jìn)行組合定位數(shù)據(jù)融合之前，需要將捷聯(lián)慣導(dǎo)測量的實(shí)時經(jīng)緯度轉(zhuǎn)換為西安80坐標(biāo)系[16-17]下的坐標(biāo)，之后與全站儀測量的掘進(jìn)機(jī)機(jī)身在西安80坐標(biāo)系下的位置數(shù)據(jù)進(jìn)行融合計(jì)算，得出機(jī)身在西安80坐標(biāo)系下的位姿數(shù)據(jù)。實(shí)時經(jīng)緯度與西安80坐標(biāo)系的轉(zhuǎn)換公式為[18]

(61-58tan2L+tan4L+270η2tan2L)

(15)

58η2tan2L)

(16)

式中：(x，y)為掘進(jìn)機(jī)在西安80坐標(biāo)系下的坐標(biāo)；s為從赤道到掘進(jìn)機(jī)所在緯度的子午線弧長；η=0.082 094 469cosL。

3 基于卡爾曼濾波的數(shù)據(jù)融合

采用卡爾曼濾波器實(shí)現(xiàn)全站儀和捷聯(lián)慣導(dǎo)數(shù)據(jù)融合，以全站儀、捷聯(lián)慣導(dǎo)測得的位置誤差作為融合狀態(tài)向量和觀測值，建立組合定位狀態(tài)方程和觀測方程，最終解算出組合定位下的掘進(jìn)機(jī)位置和姿態(tài)角。

組合定位狀態(tài)方程為

(17)

以捷聯(lián)慣導(dǎo)解算的位置數(shù)據(jù)與全站儀測量的位置數(shù)據(jù)之差為觀測值，得觀測方程：

Z=ΔpINS-ΔpT

(18)

由此建立組合定位狀態(tài)方程：

(19)

式中V為觀測噪聲。

對于全站儀和捷聯(lián)慣導(dǎo)組合定位數(shù)據(jù)融合的卡爾曼濾波方法同式(3)—式(7)。

4 組合定位試驗(yàn)

在西安煤礦機(jī)械有限公司搭建試驗(yàn)平臺，對掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位方法進(jìn)行驗(yàn)證，如圖3所示。試驗(yàn)采用Xi-1000Q自動全站儀(測量精度為1 mm)和民用航空捷聯(lián)慣性基準(zhǔn)系統(tǒng)。將捷聯(lián)慣導(dǎo)放置在手推車上，棱鏡置于捷聯(lián)慣導(dǎo)中心位置。將全站儀設(shè)置為跟蹤模式，連續(xù)測量手推車實(shí)時位置。人為推動手推車模擬掘進(jìn)機(jī)在設(shè)計(jì)路線上行走，記錄全站儀、捷聯(lián)慣導(dǎo)測量的位置信息及組合定位信息。假設(shè)手推車高度始終為全站儀初始測量值。試驗(yàn)以全站儀測量的位置數(shù)據(jù)為實(shí)際值。

(a) 試驗(yàn)方案

采用經(jīng)緯儀測得手推車初始位置：L=34°29′29.846 820 8″，λ=109°21.374 304 7″，h=346.005 5 m。用全站儀測得手推車起點(diǎn)和終點(diǎn)的高斯坐標(biāo)(x，y)分別為(3 818 663.24 m,592 368.64 m)，(3 818 661.12 m, 592 377.82 m)。全站儀和捷聯(lián)慣導(dǎo)輸出速率均為9 600 bit/s，全站儀采樣頻率為3 Hz。人為推動手推車從起點(diǎn)到終點(diǎn)運(yùn)動，每運(yùn)動5 s暫停1次，記錄全站儀、捷聯(lián)慣導(dǎo)及組合定位方法測量的該位置高斯坐標(biāo)。試驗(yàn)共測得9個點(diǎn)的位置數(shù)據(jù)，如圖4所示。可看出手推車在運(yùn)動過程中，x，y方向的組合定位數(shù)據(jù)與實(shí)際值基本相同，而捷聯(lián)慣導(dǎo)測量數(shù)據(jù)與實(shí)際值偏差隨時間延長逐漸增大。

(a) x方向

組合定位數(shù)據(jù)與實(shí)際值誤差見表1，誤差曲線如圖5所示?？煽闯鼋M合定位誤差較?。涸趚方向的定位誤差最大值為0.029 1 m，最小值為0.010 0 m，平均值為0.019 93 m；在y方向定位誤差最大值為0.029 5 m，最小值為0.011 0 m，平均值為0.018 26 m。

表1 組合定位數(shù)據(jù)與實(shí)際值誤差Table 1 Errors between combined positioning data and the actual ones

全站儀和組合定位方法獲取的手推車軌跡如圖6所示。可看出手推車在運(yùn)動過程中，組合定位方法獲取的軌跡與實(shí)際運(yùn)動軌跡基本重合，驗(yàn)證了組合定位方法具有較高的定位精度。

圖5 組合定位誤差曲線Fig.5 Combined positioning error curves

圖6 手推車軌跡Fig.6 Trolley track

組合定位方法測得的姿態(tài)角如圖7所示。

(a) 俯仰角

5 結(jié)語

掘進(jìn)機(jī)全站儀與捷聯(lián)慣導(dǎo)組合定位方法基于捷聯(lián)慣導(dǎo)測量的掘進(jìn)機(jī)位姿參數(shù)和全站儀測量的掘進(jìn)機(jī)位置參數(shù)，采用卡爾曼濾波方法進(jìn)行數(shù)據(jù)融合，實(shí)現(xiàn)了掘進(jìn)機(jī)精確定位。試驗(yàn)結(jié)果表明該方法定位精度較高：在x方向的定位誤差最大值為0.029 1 m，最小值為0.010 0 m，平均值為0.019 93 m；在y方向的定位誤差最大值為0.029 5 m，最小值為0.011 0 m，平均值為0.018 26 m。下一步將研究該方法用于井下的實(shí)際定位精度。