基于慣性／航跡推測的煤礦掘錨機(jī)組合導(dǎo)航方法

倫華江，徐 楓，黃 喆，許東旭，張曉日

(1.中國航天科技集團(tuán)有限公司物聯(lián)網(wǎng)技術(shù)應(yīng)用研究院，北京100094；2.力信測量(上海）有限公司，上海201300；3.天津科技大學(xué)電子信息與自動化學(xué)院，天津300222)

0 引言

煤炭是我國主體能源，在我國能源消費(fèi)總量中占比60%[1]。煤炭的獲取依賴于安全開采[2]，而實(shí)現(xiàn)工作面安全開采離不開先進(jìn)、可靠的掘進(jìn)裝備[3]，其中掘錨機(jī)因其掘進(jìn)速度快、安全性好和巷道成形效率高等特點(diǎn)而廣泛應(yīng)用于我國礦井煤巷施工中[4-5]。煤礦巷道中使用激光指示和人工觀察相結(jié)合的方法來指導(dǎo)掘進(jìn)，但這種方法勞動強(qiáng)度大，工人安全得不到有效保障，極大降低了施工質(zhì)量和掘進(jìn)效率。因此，研究高可靠性、高精度的導(dǎo)航定位方法在掘錨機(jī)煤巷施工中具有很高的工程應(yīng)用價(jià)值。

近年來，國內(nèi)學(xué)者在煤礦掘進(jìn)機(jī)導(dǎo)航定位方面取得了較多研究成果。王以忠等[6]提出了一種基于空間矢量約束的實(shí)時(shí)位姿組合測量方法，通過構(gòu)建空間四邊形的方式實(shí)現(xiàn)了煤礦掘進(jìn)機(jī)的六自由度測量，但是該方法不能解算空間四邊形中有兩邊平行的特殊情況。田原[7-8]基于四點(diǎn)式光靶的掘進(jìn)機(jī)定位方法實(shí)現(xiàn)了機(jī)器視覺和慣性的組合導(dǎo)航，但該方法在巷道進(jìn)尺方向精度較差。而后該學(xué)者從掘進(jìn)機(jī)特點(diǎn)出發(fā)，提出了一種基于零速修正的掘進(jìn)機(jī)慣性導(dǎo)航定位方法，解決了環(huán)境適應(yīng)性差、慣性導(dǎo)航長時(shí)定位精度低等問題。薛光輝等[9]針對慣導(dǎo)誤差累積的特點(diǎn)提出了一種基于激光靶向跟蹤的掘進(jìn)機(jī)位姿測量方法，可以在標(biāo)靶單點(diǎn)布站的情況下完成掘進(jìn)機(jī)所有位姿參數(shù)的測量，但該方法受限于煤塵問題，不能長距離使用。就目前技術(shù)而言，由于掘進(jìn)機(jī)工作環(huán)境復(fù)雜，基于單一傳感器的導(dǎo)航定位技術(shù)存在局限性，多傳感多信息融合的導(dǎo)航技術(shù)更有希望解決掘進(jìn)機(jī)的導(dǎo)航定位問題[10]。文獻(xiàn)[11]～文獻(xiàn)[15]進(jìn)行了里程計(jì)＋捷聯(lián)慣導(dǎo)組合導(dǎo)航技術(shù)在管道、自動駕駛及井下車輛等場景中的應(yīng)用研究。掘進(jìn)機(jī)定位技術(shù)關(guān)鍵在于建立掘進(jìn)機(jī)的實(shí)時(shí)坐標(biāo)[16]，然而上述文獻(xiàn)中未給出載體坐標(biāo)的解算方法?；诹阄粶y量實(shí)時(shí)更新載體坐標(biāo)和姿態(tài)的方法在盾構(gòu)導(dǎo)向中應(yīng)用比較成熟[17]，其基于載體與傳感器之間的剛體關(guān)系，在已知傳感器坐標(biāo)和姿態(tài)角的情況下實(shí)時(shí)解算載體坐標(biāo)。

為了實(shí)現(xiàn)掘錨機(jī)定位測量和掘進(jìn)導(dǎo)向，進(jìn)一步豐富導(dǎo)航參數(shù)，本文基于對現(xiàn)有的技術(shù)研究，提出了基于慣性／航跡推測的組合導(dǎo)航方法。通過對慣導(dǎo)實(shí)時(shí)位姿測量，結(jié)合掘錨機(jī)剛體零位標(biāo)定數(shù)據(jù)、全站儀測量數(shù)據(jù)和計(jì)劃線數(shù)據(jù)等，解算出掘錨機(jī)實(shí)時(shí)坐標(biāo)及導(dǎo)向參數(shù)，實(shí)現(xiàn)了精確導(dǎo)向功能。

1 定位和偏差算法

1.1 坐標(biāo)系定義

位姿測量系統(tǒng)坐標(biāo)系的定義如圖1所示。OnXnYnZn(簡稱n系)為參考坐標(biāo)系，OnXn軸指向正東方向，OnYn軸指向正北方向(方位角為零度的方向)，OnZn軸垂直向上，原點(diǎn)由城市規(guī)劃設(shè)計(jì)部門設(shè)定；ObXbYbZb(簡稱b系)為掘錨機(jī)坐標(biāo)系，原點(diǎn)位于機(jī)體中線上，ObXb軸指向機(jī)體右側(cè)，ObYb軸由機(jī)尾指向機(jī)頭，ObZb軸滿足正交坐標(biāo)右手系；OmXmYmZm(簡稱m系)為慣性測量單元(Inertial Measurement Unit，IMU)坐標(biāo)系，原點(diǎn)在 IMU中心，坐標(biāo)軸為IMU自身出廠的坐標(biāo)軸。

圖1 測量系統(tǒng)坐標(biāo)系定義及掘錨機(jī)與設(shè)計(jì)軸線空間位置關(guān)系Fig.1 Definition of the measurement system coordinate system and spatial position relationship between the roadheader and the design line

在掘錨機(jī)施工階段，進(jìn)行測量的主要任務(wù)是獲取導(dǎo)向參數(shù)，即測量機(jī)體與計(jì)劃線的偏差量，從而控制機(jī)器前進(jìn)方向。圖1描述了掘錨機(jī)與設(shè)計(jì)軸線在空間中的位置關(guān)系，過機(jī)頭中心點(diǎn)和機(jī)尾中心點(diǎn)分別做垂線垂直于設(shè)計(jì)軸線，則垂線段在水平面上的投影為水平偏移量，在豎直面上的投影為垂直偏移量。始終將水平偏差和垂直偏差控制在規(guī)定的范圍，就可以使掘錨機(jī)沿著計(jì)劃線前進(jìn)，獲得良好的直線度。

為了求解掘錨機(jī)相對于計(jì)劃軸線的絕對位置偏差，需要實(shí)時(shí)解算機(jī)頭中心點(diǎn)和機(jī)尾中心點(diǎn)的坐標(biāo)，然后根據(jù)點(diǎn)與直線的相對位置關(guān)系計(jì)算偏差量。解算的數(shù)據(jù)流程圖如圖2所示，主要分三步來實(shí)現(xiàn)偏差量計(jì)算：

圖2 數(shù)據(jù)流程示意圖Fig.2 Flowchart of data

第一步為零位標(biāo)定，即確定掘錨機(jī)與IMU之間的相對位置關(guān)系和姿態(tài)關(guān)系。其中，位置關(guān)系通過標(biāo)定機(jī)頭中心點(diǎn)和機(jī)尾中心點(diǎn)在n系下的坐標(biāo)確定，姿態(tài)關(guān)系通過標(biāo)定b系和m系的旋轉(zhuǎn)矩陣確定。

第二步為坐標(biāo)解算，即根據(jù)里程計(jì)和IMU獲取機(jī)頭中心點(diǎn)和機(jī)尾中心點(diǎn)在n系下的坐標(biāo)。

第三步為偏差計(jì)算，即根據(jù)空間中點(diǎn)到直線最短距離原理計(jì)算水平和垂直偏差值。

1.2 零位標(biāo)定

在零位處獲取機(jī)尾、機(jī)頭和IMU在n系下的坐標(biāo)Pa0、Pb0和P0，由IMU解算出自身三個(gè)姿態(tài)角，即俯仰角α0、橫滾角β0和航向角γ0。 則可得機(jī)尾、機(jī)頭在m系下的坐標(biāo)Qa0和Qb0，表達(dá)式如下

由于IMU與掘錨機(jī)是剛性連接的，所以在以后的機(jī)體運(yùn)動過程中，Qa0、Qb0以及保持不變。

1.3 坐標(biāo)解算

分兩個(gè)步驟進(jìn)行求解：1)基于里程計(jì)測量的位移數(shù)據(jù)和姿態(tài)信息計(jì)算IMU實(shí)時(shí)坐標(biāo)；2)結(jié)合IMU實(shí)時(shí)姿態(tài)角數(shù)據(jù)和零位數(shù)據(jù)計(jì)算機(jī)頭機(jī)尾實(shí)時(shí)坐標(biāo)。

在掘錨機(jī)運(yùn)動過程中，里程計(jì)輸出空間位移數(shù)據(jù)，給定初始位置，可以推算實(shí)時(shí)位置信息。設(shè)里程計(jì)第i采樣間隔的里程增量在b系下的矢量表示為ΔSbi， IMU姿態(tài)角構(gòu)成的旋轉(zhuǎn)矩陣為，根據(jù)航位推算原理，IMU實(shí)時(shí)坐標(biāo)可表示為[19-20]

式(5)中，Pat為機(jī)尾中心在n系下的坐標(biāo)，Pbt為機(jī)頭中心在n系下的坐標(biāo)。

1.4 偏差計(jì)算

機(jī)尾、機(jī)頭偏差量求解原理如圖3所示。將掘錨機(jī)機(jī)尾中心A和機(jī)頭中心B在平面XnOnYn和平面ZnOnYn投影， 得到投影點(diǎn)A′、A″、B′和B″，將設(shè)計(jì)軸線起點(diǎn)C和終點(diǎn)D在平面XnOnYn和平面ZnOnYn投影， 得到投影點(diǎn)C′、C″、D′和D″， 平面A′AA″交線段CD、C′D′和C″D″于點(diǎn)E、E′和E″，平面B′BB″交線段CD、C′D′和C″D″于點(diǎn)F、F′和F″。則按照本文偏差量的定義，A′E′即為機(jī)尾中心點(diǎn)水平偏差量，A″E″為機(jī)尾中心點(diǎn)垂直偏差量，B′F′為機(jī)頭中心點(diǎn)水平偏差量，B″F″為機(jī)頭中心點(diǎn)垂直偏差量。

圖3 機(jī)尾、機(jī)頭偏差量求解原理Fig.3 Principle of solving the deviation of the tail and head

根據(jù)空間點(diǎn)與直線的位置關(guān)系[21]，容易得到

計(jì)劃線起點(diǎn)和終點(diǎn)在n系下坐標(biāo)已知，記為C(xc，yc，zc)和D(xd，yd，zd)， 則可得C′(xc，yc，0)、C″(0，yc，zc)、D′(xd，yd， 0)、D″(0，yd，zd)。掘錨機(jī)機(jī)頭、 機(jī)尾坐標(biāo)可由式求出，記為A(xa，ya，za) 和B(xb，yb，zb)，則可 得A′(xa，ya， 0)、A″(0，ya，za)、B′(xb，yb， 0)、B″(0，yb，zb)。 將上述點(diǎn)坐標(biāo)帶入式(6)，得到

2 導(dǎo)航系統(tǒng)實(shí)驗(yàn)

為了驗(yàn)證組合導(dǎo)航方法的準(zhǔn)確性，將慣性測量單元(IMU)和里程計(jì)安裝在移動小車上進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)原理如圖4所示。

圖4 實(shí)驗(yàn)原理Fig.4 Diagram of experiment principle

讓測試小車沿著設(shè)定好的計(jì)劃線前進(jìn)，在小車運(yùn)行到觀察位置點(diǎn)時(shí)，用全站儀測量小車位置，同時(shí)記錄慣導(dǎo)＋里程計(jì)組合導(dǎo)航系統(tǒng)的定位坐標(biāo)。結(jié)合初始零位及計(jì)劃線參數(shù)，計(jì)算機(jī)頭、機(jī)尾在水平方向和豎直方向上的偏移量，通過比較解算值和實(shí)測值，獲得系統(tǒng)測量誤差。本次實(shí)驗(yàn)所用的設(shè)備如表1所示。

表1 實(shí)驗(yàn)設(shè)備Table 1 List of experiment apparatus

實(shí)驗(yàn)步驟如下：

1)組裝實(shí)驗(yàn)裝置和搭建實(shí)驗(yàn)環(huán)境。采用鋁合金型材、行走輪組裝成測試小車主體；在里程計(jì)上加裝計(jì)米輪并通過安裝支架固連到車體尾部，其中安裝支架中帶彈簧機(jī)構(gòu)保證計(jì)米輪貼地；在IMU上加裝反射棱鏡，并通過螺絲固連到車體的右前部位；將全站儀架設(shè)在視野寬闊的位置，調(diào)平并設(shè)站定向。實(shí)驗(yàn)裝置和實(shí)驗(yàn)環(huán)境如圖5所示。

圖5 實(shí)驗(yàn)裝置與實(shí)驗(yàn)環(huán)境Fig.5 Diagram of experiment devices and experiment environment

2)標(biāo)定剛體零位和初始化線路數(shù)據(jù)。通過全站儀測量小車上的特征點(diǎn)以及陀螺特征點(diǎn)坐標(biāo)，得到零位坐標(biāo)數(shù)據(jù)和零位姿態(tài)數(shù)據(jù)，如表2、表3所示。根據(jù)小車運(yùn)動范圍設(shè)定沿正北方向150m為前進(jìn)軸線，得到計(jì)劃線參數(shù)，如表4所示。

表2 零位坐標(biāo)數(shù)據(jù)Table 2 Data of coordinate at zero-position

表3 零位姿態(tài)數(shù)據(jù)Table 3 Data of attitude angle at zero-position

表4 計(jì)劃線數(shù)據(jù)Table 4 Data of design line

3)根據(jù)安裝偏差校準(zhǔn)算法[19]獲取補(bǔ)償誤差：航向角偏差為0.196°，俯仰角偏差為－0.002°，并補(bǔ)償?shù)搅阄坏陌惭b偏差中。

4)推動小車向前移動，每間隔約2m用全站儀測量慣導(dǎo)特征點(diǎn)坐標(biāo)，同時(shí)記錄慣導(dǎo)自身解算坐標(biāo)，得到的IMU軌跡如圖6所示。通過零位關(guān)系求解移動小車上機(jī)頭、機(jī)尾點(diǎn)與計(jì)劃線的偏差，結(jié)合全站儀測量得到的偏差計(jì)算誤差，得到的偏差變化曲線和誤差曲線如圖7所示。

圖6 慣導(dǎo)定位軌跡與全站儀定位軌跡比較Fig.6 Comparison between inertial navigation positioning track and total station positioning track

圖7 解算與實(shí)測偏差對比及誤差曲線Fig.7 Comparison of calculated and measured deviations and error curves

由圖6可知，小車在前進(jìn)過程中，北向和東向位移增大，天向位移逐漸減小，對比全站儀測量坐標(biāo)和慣導(dǎo)解算坐標(biāo)，兩者的變化趨勢一致。但隨著時(shí)間和距離的變化，誤差逐漸增大，在東北天方向的最大定位誤差分別為37.9mm、46.0mm和29.5mm。在圖7的解算與實(shí)測偏差比較曲線中，機(jī)頭機(jī)尾中心與計(jì)劃線偏移量隨著里程變化逐漸增大，且頭尾變化保持一致。從解算誤差曲線圖中可以發(fā)現(xiàn)，誤差隨著時(shí)間和距離變化累積，在里程108.6481m處，水平方向上機(jī)頭中心和機(jī)尾中心的偏差量誤差分別為－44mm和－44mm，在里程114.2266m處，豎直方向上機(jī)頭中心和機(jī)尾中心的偏差量誤差分別為32mm和30mm，如表5所示。

表5 實(shí)驗(yàn)小車與計(jì)劃線偏差的最大測量誤差Table 5 Maximum measurement error of the deviation between the experiment car and the design line

本次實(shí)驗(yàn)中，由于在組裝時(shí)行走輪與機(jī)身軸線有偏角，加之推行小車的力位于小車左后方，小車并沒有完全沿著預(yù)定的計(jì)劃線前進(jìn)，而是朝北偏東方向前進(jìn)，所以小車在前進(jìn)過程中東坐標(biāo)和北坐標(biāo)逐漸增大，且機(jī)頭機(jī)尾中心向右偏離計(jì)劃線；計(jì)劃線設(shè)定在高程為8.82m的水平面上，而實(shí)際小車下坡前進(jìn)，故機(jī)頭機(jī)尾中心向下偏離計(jì)劃線。此外，由于慣性系統(tǒng)存在累積誤差，全站儀實(shí)測值與組合導(dǎo)航系統(tǒng)解算值之間存在誤差，直接體現(xiàn)在慣導(dǎo)定位誤差中，間接影響測試小車機(jī)頭機(jī)尾偏離計(jì)劃線的解算誤差。

該組合導(dǎo)航系統(tǒng)的現(xiàn)場應(yīng)用如圖8所示，先在廠里做完零位標(biāo)定，待掘進(jìn)裝備運(yùn)到施工現(xiàn)場后做零位復(fù)測。某煤礦應(yīng)用中，巷道長度約300m，掘進(jìn)過程中對機(jī)頭水平偏差解算誤差進(jìn)行驗(yàn)證，在同一掘進(jìn)里程下使用全站儀人工測量機(jī)頭與設(shè)計(jì)線的水平偏差，記錄測量系統(tǒng)解算的機(jī)頭水平偏差，并統(tǒng)一編號作對比。表6為掘進(jìn)里程位于122m～127m區(qū)間內(nèi)的三組人工復(fù)核數(shù)據(jù)和測量系統(tǒng)解算數(shù)據(jù)?？捎杀?知，比對誤差在130mm以內(nèi)，滿足應(yīng)用需求。

圖8 測量系統(tǒng)現(xiàn)場應(yīng)用Fig.8 Field application of measurement system

表6 機(jī)頭中心水平偏差實(shí)測值與計(jì)算值比較Table 6 Comparison of measured value and calculated value of head center horizontal deviation

上述結(jié)果驗(yàn)證了本文提出的組合導(dǎo)航方法的可行性，在實(shí)驗(yàn)條件下，該方法定位精度達(dá)到46mm(116m前進(jìn)距離)，最終計(jì)算的位置偏差精度為：偏差量誤差／前進(jìn)里程＝0.044／116.4287＝0.038%。在實(shí)踐應(yīng)用中，水平位置偏差精度約為：0.129／300＝0.043%。相信將該導(dǎo)航方法應(yīng)用于掘錨機(jī)施工中，將為機(jī)器設(shè)備的導(dǎo)航定位工作提供有力保障。另一方面，為了減小慣性誤差累積的影響，保證定位精度和位置偏差精度，通常在載體前進(jìn)100m左右要做一次坐標(biāo)修正。

3 結(jié)論

基于煤礦巷道施工現(xiàn)場掘進(jìn)導(dǎo)向的迫切需求，本文提出了一種基于慣性導(dǎo)航和航跡推測的導(dǎo)航定位方法。該方法根據(jù)慣導(dǎo)姿態(tài)數(shù)據(jù)、里程計(jì)位移數(shù)據(jù)以及載體零位數(shù)據(jù)，解算機(jī)頭／機(jī)尾相對于計(jì)劃線在水平和豎直方向上的偏移量，實(shí)現(xiàn)了掘錨機(jī)的導(dǎo)向測量功能。在實(shí)驗(yàn)環(huán)境下進(jìn)行了定位精度和偏差精度測試，同時(shí)進(jìn)行了在現(xiàn)場施工環(huán)境下的應(yīng)用測試。實(shí)驗(yàn)結(jié)果表明，該方法偏差解算精度為0.038D%(D為前進(jìn)距離)，具有很好的工程推廣和應(yīng)用價(jià)值。在接下來的工作中，需要研究慣導(dǎo)、里程計(jì)的標(biāo)定方法以及振動環(huán)境下的穩(wěn)定性測試，以提高系統(tǒng)精度。