基于組合方式的掘進機導(dǎo)航系統(tǒng)研究

田立勇，孫業(yè)新，于 寧，陳洪月，馬春瑩

（遼寧工程技術(shù)大學(xué)機械工程學(xué)院，遼寧 阜新 123000）

煤炭資源是支撐國家繁榮和經(jīng)濟可持續(xù)發(fā)展的基礎(chǔ)，開采技術(shù)智能化是煤炭工業(yè)發(fā)展的重要方向［1-2］。隨著我國科學(xué)技術(shù)的發(fā)展，對智能化煤炭開采技術(shù)的要求不斷提高。掘進作業(yè)作為煤炭開采過程中的重要環(huán)節(jié)，其面臨的主要難題是“采掘失調(diào)”。因此，亟須實現(xiàn)煤礦巷道掘進的自動化、智能化［3-4］，而自動定向掘進是實現(xiàn)掘進工作面自動化的核心技術(shù)［5］。

掘進機定向掘進的關(guān)鍵是實現(xiàn)掘進機位姿的自主測量。朱信平等［6］通過采用全站儀檢測目標(biāo)棱鏡，實現(xiàn)了對掘進機機身位姿參數(shù)的實時測量。楊文娟等［7］利用機器視覺方法來測量掘進機機身及截割頭的位姿，其以激光束與紅外光斑為特征，通過防爆相機采集圖像以實現(xiàn)掘進機位姿的測量。杜雨馨等［8］通過分析十字激光束在激光標(biāo)靶上的成像特征，解算得到掘進機機身位姿的相關(guān)參數(shù)，完成了掘進機機身位姿的自主測量。吳淼等［9］利用空間交匯測量技術(shù)來獲取掘進機的位姿，即通過激光接收器識別旋轉(zhuǎn)激光平面交匯區(qū)域，解算得到機身位姿參數(shù)，從而獲得掘進機的工作狀態(tài)。張旭輝等［10］結(jié)合超聲波傳感器和慣性導(dǎo)航系統(tǒng)來測量掘進機機身的位姿信息。毛清華等［11］將激光測距傳感器、超聲波測距傳感器、地磁傳感器和慣性導(dǎo)航系統(tǒng)組合，用于測量掘進機的位姿參數(shù)。Hargrave等［12］利用雷達定位技術(shù)來測量掘進機的位姿，即通過雷達的反射信號得到掘進機的位置和運行軌跡信息。綜上所述，井下掘進機位姿的自主測量大多以機器視覺技術(shù)和全站儀為主，但由于煤礦巷道的環(huán)境復(fù)雜、惡劣，照明不足，易對位姿測量結(jié)果造成影響。為此，筆者基于多傳感器融合思想［13］，將多種測量技術(shù)相結(jié)合，以期能夠進一步提高掘進機位姿的測量精度，從而實現(xiàn)其定向掘進。

結(jié)合上述學(xué)者的研究成果，筆者提出一種基于組合方式的掘進機導(dǎo)航系統(tǒng)（即組合導(dǎo)航系統(tǒng)）：將自主研制的光電傳感器導(dǎo)航系統(tǒng)與光纖慣性導(dǎo)航系統(tǒng)組合，用于測量掘進機機身的位姿。首先，以十字激光束作為光電傳感器導(dǎo)航系統(tǒng)的識別特征，建立掘進機機身位姿解算模型；然后，利用遞推最小二乘（recursive least square,RLS）算法對光電傳感器導(dǎo)航系統(tǒng)和光纖慣性導(dǎo)航系統(tǒng)的測量數(shù)據(jù)進行融合處理，以完成掘進機機身位姿解算；最后，利用PLC（programmable logic controller，可編程邏輯控制器）對掘進機機身進行糾偏控制，實現(xiàn)其定向掘進。

1 掘進機組合導(dǎo)航系統(tǒng)總體方案

鑒于光電傳感器導(dǎo)航系統(tǒng)是基于絕對基準(zhǔn)進行測量的，無誤差累積影響，而光纖慣性導(dǎo)航系統(tǒng)對姿態(tài)角的測量精度高且不受外部信號干擾，但對位置的測量誤差會隨時間累積，本文將兩者組合使用，以彌補各導(dǎo)航系統(tǒng)的不足，從而提高對掘進機機身位姿的測量精度。

掘進機組合導(dǎo)航系統(tǒng)的總體方案如圖1所示，其硬件結(jié)構(gòu)主要由光電傳感器導(dǎo)航系統(tǒng)（包含十字激光指向儀和激光接收標(biāo)靶）、光纖慣性導(dǎo)航系統(tǒng)（包含光纖陀螺儀和加速度計）和輔助配套設(shè)備構(gòu)成；軟件程序可完成位姿解算，數(shù)據(jù)存儲、顯示與PLC控制等功能，以實現(xiàn)掘進機機身位姿的自主測量與糾偏調(diào)控。

圖1 掘進機組合導(dǎo)航系統(tǒng)總體方案Fig.1 Overall scheme of integrated navigation system of roadheader

該組合導(dǎo)航系統(tǒng)的組成模塊及安裝方式如下。

1）光電傳感器導(dǎo)航系統(tǒng)。將激光接收標(biāo)靶安裝在掘進機機身上，十字激光指向儀固定于掘進機后方的巷道頂板上，利用激光接收標(biāo)靶對定位激光束進行接收和識別。

2）光纖慣性導(dǎo)航系統(tǒng)。光纖陀螺儀和加速度計分別沿掘進機前進方向、偏移方向及垂直于底板方向安裝，用于測量掘進機機身在這3個方向上的運動角速度和線加速度。

3）通信傳輸模塊。測量數(shù)據(jù)通過485總線傳輸?shù)絇LC，由狀態(tài)顯示界面顯示輸出數(shù)據(jù)及誤差提示，通過人工調(diào)控來控制掘進機的運行軌跡。

在開始測量前，測量人員將光電傳感器導(dǎo)航系統(tǒng)中的激光接收標(biāo)靶和光纖慣性導(dǎo)航系統(tǒng)固定安裝在掘進機機身上，并根據(jù)煤礦巷道設(shè)計信息確定十字激光指向儀在掘進機巷道后方頂板上的安裝位置，以保證定位激光束發(fā)射的精確度。十字激光指向儀發(fā)射2個相互垂直的激光束，分別為沿掘進方向的水平激光束和垂直激光束。掘進機組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)如圖2所示。

圖2 掘進機組合導(dǎo)航系統(tǒng)結(jié)構(gòu)Fig.2 Structure of integrated navigation system of roadheader

光電傳感器導(dǎo)航系統(tǒng)包含2個激光接收橫向標(biāo)靶和1個激光接收縱向標(biāo)靶，分別用于接收垂直和水平激光束。每臺激光接收標(biāo)靶由若干個光電傳感器線性陣列組成，各光電傳感器的間距為3 mm，如圖3所示。均勻排布的光電傳感器將識別的光信號轉(zhuǎn)化為電信號后輸出，以確定激光束的落點位置，結(jié)合構(gòu)建的位姿解算模型獲取掘進機機身的位姿參數(shù)。

圖3 激光接收標(biāo)靶組成示意Fig.3 Composition of laser receiving target

光纖慣性導(dǎo)航系統(tǒng)用于測量掘進機沿不同方向的角速度和線加速度，通過解算可得到其機身的姿態(tài)角和實時位置。將光電傳感器導(dǎo)航系統(tǒng)和光纖慣性導(dǎo)航系統(tǒng)的測量數(shù)據(jù)進行融合處理，可實現(xiàn)對掘進機機身位姿的自主、準(zhǔn)確測量。

2 掘進機組合導(dǎo)航系統(tǒng)測量原理

2.1 坐標(biāo)系定義

為實現(xiàn)掘進機機身位姿的測量，建立如圖4所示的空間直角坐標(biāo)系。其中：十字激光指向儀坐標(biāo)系O0-X0Y0Z0以激光束發(fā)射點為原點；巷道坐標(biāo)系O-XYZ以十字激光指向儀安裝位置在巷道底板上的投影點為原點；掘進機機身坐標(biāo)系Ob-XbYbZb以其中心為原點。掘進機機身坐標(biāo)系與巷道坐標(biāo)系的3個坐標(biāo)軸方向在起始條件下保持一致，即：Yb軸沿機身中軸線指向掘進機前進方向，Zb軸垂直機身中軸線指向巷道頂板方向，Xb、Yb、Zb軸的關(guān)系符合左手定則。在巷道坐標(biāo)系下，激光束發(fā)射點O0的坐標(biāo)為(0，0，z0)，掘進機機身中心Ob的坐標(biāo)為(x，y，z)。

圖4 掘進機機身位姿測量相關(guān)坐標(biāo)系定義Fig.4 Definition of coordinate systems related to position and attitude measurement of roadheader body

由于組合導(dǎo)航系統(tǒng)固定安裝在掘進機機身上，須確定機身坐標(biāo)系與巷道坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。以掘進機機身姿態(tài)角為轉(zhuǎn)換角，用歐拉角α、β、γ表示。其中：α為機身偏向角，β為機身俯仰角，γ為機身滾動角。則機身坐標(biāo)系相對于巷道坐標(biāo)系的轉(zhuǎn)換矩陣Jb可表示為：

2.2 光電傳感器導(dǎo)航系統(tǒng)的測量原理

在光電傳感器導(dǎo)航系統(tǒng)中，激光接收標(biāo)靶通過識別定位激光束來測量掘進機機身的位姿參數(shù)。首先，定義各個激光接收標(biāo)靶在掘進機機身坐標(biāo)系下的坐標(biāo)：激光接收橫向標(biāo)靶1中心Ob1的坐標(biāo)為(xb1，yb1，zb1)，激光接收橫向標(biāo)靶2中心Ob2的坐標(biāo)為(xb2，yb2，zb2)，其中xb2=xb1，yb2=yb1+e1，zb2=zb1+e2，e1和e2分別為2個激光接收橫向標(biāo)靶之間的水平和豎直距離，其具體數(shù)值與激光接收標(biāo)靶的安裝位置有關(guān)；激光接收縱向標(biāo)靶中心Ob3的坐標(biāo)為(xb3，yb3，zb3)。光電傳感器導(dǎo)航系統(tǒng)的測量原理如圖5所示。

圖5 光電傳感器導(dǎo)航系統(tǒng)的測量原理示意Fig.5 Schematic diagram of measurement principle of photoelectric sensor navigation system

當(dāng)掘進機開始作業(yè)后，激光接收標(biāo)靶在巷道中的位置隨掘進機的運動而發(fā)生變化。在掘進機行進過程中，2個激光接收橫向標(biāo)靶中心在巷道坐標(biāo)系下的坐標(biāo)（x1，y1，z1）、（x2，y2，z2）可分別表示為：

激光接收橫向標(biāo)靶1和2分別用矢量l1和l2來表示，垂直激光束所在平面的方程為X=p，則垂直激光束落點P1和P2分別為l1和l2與平面X=p的交點。l1和l2的表達式為：

式中：μk為比例系數(shù)。

將X=p代入式（4），求得落點P1與落點P2的坐標(biāo)（xPk，yPk，zPk）（k=1，2），可表示為：

在巷道坐標(biāo)系下，垂直激光束在激光接收橫向標(biāo)靶上的落點與該標(biāo)靶中心之間的距離（d1和d2）可用兩點間距離公式計算，為：

在掘進機行進過程中，激光接收橫向標(biāo)靶在巷道坐標(biāo)系中的橫向（X方向）位移等于其中心與垂直激光束落點的距離。聯(lián)立式（2）、式（3）和式（6），可得偏向角α：

整理可得，在巷道坐標(biāo)系下掘進機機身的橫向、垂向位置分別為：

式中：d3為水平激光束在激光接收縱向標(biāo)靶上的落點與該標(biāo)靶中心之間的距離。

2.3 光纖慣性導(dǎo)航系統(tǒng)的測量原理

光纖慣性導(dǎo)航系統(tǒng)測量得到的是掘進機機身在地理坐標(biāo)系下的位姿信息，則須進行掘進機機身坐標(biāo)系與地理坐標(biāo)系的轉(zhuǎn)換，其轉(zhuǎn)換矩陣可表示為［14］：

綜合考慮姿態(tài)角解算的復(fù)雜度、計算量以及更新速率，本文采用四元數(shù)法［15］來解算掘進機機身的姿態(tài)角，并通過四階龍格庫塔方法實時求解四元數(shù)。定義四元數(shù)q0、q1、q2和q3，則轉(zhuǎn)換矩陣可表示為：

通過解算可得掘進機機身的姿態(tài)角：

通過比力積分運算得到掘進機在地理坐標(biāo)系下的運動速度，進一步對速度進行積分運算獲得其地理位置［16-17］。掘進機機身在地理坐標(biāo)系下的位置可表示為：

式中：T為解算周期；λ、L和h分別為地理坐標(biāo)系下掘進機機身在經(jīng)度、緯度和高度方向上的位置；λ0、L0和h0分別為地理坐標(biāo)系下掘進機機身在經(jīng)度、緯度和高度方向上的初始位置；RN、RM分別為地球卯酉圈和子午圈的曲率半徑；vx、vy和vz分別為掘進機運動速度沿地理坐標(biāo)系的3個坐標(biāo)軸方向的分量。

設(shè)地理坐標(biāo)系與巷道坐標(biāo)系之間的轉(zhuǎn)換矩陣為M，則掘進機機身在巷道坐標(biāo)系下的位置可表示為：

3 基于RLS算法的數(shù)據(jù)融合處理

光電傳感器導(dǎo)航系統(tǒng)在解算掘進機機身位姿時采用的俯仰角β和滾動角γ由光纖慣性導(dǎo)航系統(tǒng)測得，這是因為十字激光指向儀發(fā)射的激光束會被遮擋，從而無法測量這2個姿態(tài)角，結(jié)合光纖慣性導(dǎo)航系統(tǒng)可彌補該缺陷。因此須對2個導(dǎo)航系統(tǒng)的測量數(shù)據(jù)進行融合處理，實現(xiàn)掘進機機身位姿的準(zhǔn)確測量。本文采用RLS算法對測量數(shù)據(jù)進行融合處理。

3.1 RLS算法原理

RLS算法是一種自適應(yīng)高斯-牛頓算法，其可使實際及期望信號與融合后輸出信號之差的平方和最小，每次融合過程均直接求解最佳權(quán)重系數(shù)［18］。

經(jīng)RLS算法融合處理后，輸出信號y(i)可表示為［19-20］：

式中：X(i)為i時刻的輸入信號，i=1，2，…，n；w(n)為權(quán)重系數(shù)向量。

定義誤差e(i)為i時刻期望信號d(i)與融合后輸出信號y(i)之間的誤差，構(gòu)建RLS算法的代價函數(shù)J（n）：

式中：η為遺忘因子，0<η<1。

對代價函數(shù)關(guān)于權(quán)重求導(dǎo)，并令導(dǎo)數(shù)值為0。由此可得，權(quán)重系數(shù)向量的最優(yōu)解為：

根據(jù)Q(n)定義增益向量K(n)：

則Q-1(n)可表示為：

則權(quán)重系數(shù)向量的最優(yōu)解可表示為：

式中：e′(n)為先驗估計誤差。

綜上，基于RLS算法的數(shù)據(jù)融合流程為：首先計算先驗估計誤差e′(n)，并將輸入信號作為期望信號；然后更新增益向量K(n)，并更新權(quán)重系數(shù)向量w(n)；最后更新Q-1(n)。

3.2 數(shù)據(jù)融合效果分析

構(gòu)建初始模擬信號（數(shù)據(jù)個數(shù)m=2，遺忘因子η=0.97），融合當(dāng)前時刻與前一時刻的數(shù)據(jù)，對應(yīng)權(quán)重系數(shù)分別為w1和w2。采用二階自回歸方程生成非平穩(wěn)隨機輸入信號，采樣點數(shù)為3 000個，得到如圖6所示的初始模擬信號和RLS融合信號的均方誤差曲線。由圖可見，相較于初始模擬信號，RLS融合信號的均方誤差明顯下降。

圖6 初始模擬信號和RLS融合信號的均方誤差對比Fig.6 Comparison of mean square error between initial analog signal and RLS fusion signal

鑒于RLS算法中遺忘因子η的取值不同時會得到不同的融合處理結(jié)果，設(shè)定3個不同的遺忘因子，通過仿真分析其對權(quán)重系數(shù)的影響（已知最優(yōu)權(quán)重系數(shù)為1.4）。不同遺忘因子下權(quán)重系數(shù)的收斂曲線如圖7所示。

圖7 不同遺忘因子下權(quán)重系數(shù)的收斂曲線Fig.7 Convergence curves of weight coefficient under different forgetting factors

由圖7可知：η取值越大，權(quán)重系數(shù)越收斂于最優(yōu)權(quán)重系數(shù)；當(dāng)η=0.99時，權(quán)重系數(shù)最接近于最優(yōu)權(quán)重系數(shù)，故本文取η=0.99。仿真結(jié)果表明：RLS算法具有較高的收斂速度和估計精度，在迭代過程中產(chǎn)生的均方誤差較小，能夠精準(zhǔn)快速地從帶噪信號中恢復(fù)真值，對非平穩(wěn)隨機信號的適應(yīng)性強。故選取RLS算法作為組合導(dǎo)航系統(tǒng)測量數(shù)據(jù)的融合處理方法可行。

4 掘進機組合導(dǎo)航系統(tǒng)實驗研究

為驗證所設(shè)計的組合導(dǎo)航系統(tǒng)的合理性與精確度，以陜煤集團張家峁煤礦15212掘進工作面為依托，以SANDNIK公司生產(chǎn)的MB670型掘錨一體機為掘進設(shè)備，開展組合導(dǎo)航系統(tǒng)測試實驗，實驗現(xiàn)場如圖8所示。

圖8 掘進機組合導(dǎo)航系統(tǒng)測試現(xiàn)場Fig.8 Test site of integrated navigation system of roadheader

按照MB670型掘錨一體機機身的空間尺寸，激光接收標(biāo)靶的長度設(shè)定為600 mm，經(jīng)過地面安裝調(diào)試后與光纖慣性導(dǎo)航系統(tǒng)一起固定安裝在機身上，保證其對定位激光束的接收功能；十字激光指向儀安裝在機身后方的巷道頂板上，發(fā)射波長為650 nm的紅色激光束。在組合導(dǎo)航系統(tǒng)測量的同時，利用全站儀對MB670型掘錨一體機進行位姿標(biāo)定。

通過對比組合導(dǎo)航系統(tǒng)的測量結(jié)果與全站儀的標(biāo)定結(jié)果，分析組合導(dǎo)航系統(tǒng)的測量誤差。在實驗過程中，MB670型掘錨一體機共行進50 m，每間隔10 m進行一次標(biāo)定；組合導(dǎo)航系統(tǒng)在同一位置處的測量次數(shù)不少于30次。MB670型掘錨一體機機身位姿的標(biāo)定值與測量值（平均值）的對比如表1所示。相同位置處MB670型掘錨一體機機身的位置測量誤差和姿態(tài)角測量誤差分別如圖9和圖10所示。考慮到在井下測量機身姿態(tài)角較為困難，故僅給出30組測量誤差，結(jié)果由人機交互界面輸出顯示。

圖9 MB670型掘錨一體機機身位置測量誤差Fig.9 Measurement error of position of MB670 anchor digging machine body

圖10 MB670型掘錨一體機機身姿態(tài)角測量誤差Fig.10 Measurement error of attitude angle of MB670 anchor digging machine body

表1 MB670型掘錨一體機機身位姿的標(biāo)定值與測量值對比Table 1 Comparison between calibrated and measured values of position and attitude of MB670 anchor digging machine body

由圖9可知：對于MB670型掘錨一體機機身的位置，X方向的最大測量誤差為17.5 mm，Y方向的最大測量誤差為14.8 mm，Z方向的最大測量誤差為13.9 mm，3個方向上的位置測量誤差均在±20 mm內(nèi)波動。由圖10可知：對于MB670型掘錨一體機機身的姿態(tài)角，偏向角的最大測量誤差為0.13°，俯仰角的最大測量誤差為0.13°，滾動角的最大測量誤差為0.14°，3個姿態(tài)角的測量誤差均在±0.15°內(nèi)波動。實驗結(jié)果表明，所設(shè)計的組合導(dǎo)航系統(tǒng)可實現(xiàn)對掘進機機身位姿的實時、準(zhǔn)確測量。

5 結(jié) 論

本文采用理論推導(dǎo)和實驗驗證的方法對掘進機組合導(dǎo)航系統(tǒng)進行了研究，獲得了以下結(jié)論。

1）基于激光接收標(biāo)靶測量技術(shù)研制了光電傳感器導(dǎo)航系統(tǒng)，并結(jié)合光纖慣性導(dǎo)航系統(tǒng)構(gòu)建了組合導(dǎo)航系統(tǒng)，其對掘進機機身位姿參數(shù)的測量精度高且可靠性好。

2）推導(dǎo)了掘進機機身的位姿解算模型，并采用RLS算法對組合導(dǎo)航系統(tǒng)的測量數(shù)據(jù)進行融合處理，有效提高了位姿的測量精度。

3）依托于陜煤集團張家峁煤礦進行現(xiàn)場實驗。結(jié)果表明：組合導(dǎo)航系統(tǒng)對掘進機機身的位置測量誤差在±20 mm以內(nèi)，對姿態(tài)角的測量誤差在±0.15°以內(nèi)，滿足巷道施工精度要求，能夠?qū)崿F(xiàn)掘進機定向掘進。