張子悅,符世琛,劉 超,李永澤,蘭 玉,吳 淼

(中國(guó)礦業(yè)大學(xué)(北京)機(jī)電與信息工程學(xué)院，北京 100083)

0 引 言

無(wú)人采掘裝備是當(dāng)前國(guó)際煤炭領(lǐng)域的發(fā)展前沿[1]。采煤機(jī)是井下綜采工作面的核心設(shè)備，綜采工作面的工況非常復(fù)雜，常存在高溫高灰塵以及其他的干擾情況，導(dǎo)致駕駛員操控采煤機(jī)作業(yè)難以保證精度，采煤效率大幅降低，采煤機(jī)的位姿檢測(cè)是實(shí)現(xiàn)采煤機(jī)無(wú)人化、自動(dòng)化的關(guān)鍵。近年來(lái)，已有多位學(xué)者對(duì)采煤機(jī)的位姿檢測(cè)進(jìn)行了研究：張博淵[2]提出了捷聯(lián)慣導(dǎo)系統(tǒng)與里程計(jì)相融合的采煤機(jī)定位技術(shù)，分析了影響采煤機(jī)定位精度的因素，建立了最優(yōu)估計(jì)誤差補(bǔ)償模型，但慣導(dǎo)會(huì)產(chǎn)生較大的累積誤差；方新秋等[3]提出了微機(jī)械陀螺和加速度計(jì)傳感器組合自主定位系統(tǒng)的技術(shù)，根據(jù)采煤機(jī)的運(yùn)動(dòng)特征建立了動(dòng)力學(xué)模型，仿真結(jié)果表明慣性傳感器的誤差是導(dǎo)致自主定位系統(tǒng)精度低的主要因素；李振平[4]提出了運(yùn)用電子地圖顯示采煤機(jī)位置的技術(shù)，通過(guò)采煤機(jī)在巷道中的投影以及簡(jiǎn)單的數(shù)學(xué)計(jì)算，可消除采煤機(jī)定位系統(tǒng)計(jì)算過(guò)程中的累積誤差；郝尚清等[5]提出了以采區(qū)坐標(biāo)系作為參考坐標(biāo)來(lái)確定采煤機(jī)坐標(biāo)的技術(shù)，將此技術(shù)應(yīng)用到采煤機(jī)自動(dòng)調(diào)高和工作面自動(dòng)調(diào)直中，但并未測(cè)得采煤機(jī)的姿態(tài)角。目前的采煤機(jī)位姿檢測(cè)方法僅能獲取采煤機(jī)的局部坐標(biāo)系坐標(biāo)，無(wú)法獲取絕對(duì)坐標(biāo)系坐標(biāo)，所以無(wú)法滿足采煤機(jī)定位的精度要求。

全站儀具有數(shù)據(jù)處理快速準(zhǔn)確、定方位角快捷和快速自動(dòng)測(cè)距的優(yōu)點(diǎn)，可滿足檢測(cè)采煤機(jī)位姿實(shí)效性和精度的要求。UWB定位技術(shù)通過(guò)檢測(cè)超寬帶信號(hào)波到達(dá)時(shí)間來(lái)解算距離[6-8]，可以減小井下高溫高灰塵工作環(huán)境對(duì)采煤機(jī)定位檢測(cè)精度的影響。將尋北儀和傾角傳感器安裝至采煤機(jī)機(jī)身，尋北儀和傾角傳感器是完全獨(dú)立自主的，可測(cè)出采煤機(jī)的姿態(tài)角[9-10]。

筆者基于多種感知方式提出了一種采煤機(jī)的位姿檢測(cè)方法，運(yùn)用全站儀與UWB定位系統(tǒng)，對(duì)采煤機(jī)的空間坐標(biāo)進(jìn)行組合測(cè)量。運(yùn)用傾角傳感器和尋北儀測(cè)量采煤機(jī)的姿態(tài)角。重點(diǎn)介紹了UWB定位系統(tǒng)基于TOA定位模型的CT算法，推導(dǎo)了UWB固定基站群坐標(biāo)系與全站儀坐標(biāo)系間的坐標(biāo)變換矩陣，介紹了傾角傳感器和尋北儀測(cè)量采煤機(jī)姿態(tài)角的工作原理，分析了該系統(tǒng)在實(shí)際應(yīng)用中的誤差來(lái)源，提出了相應(yīng)的改善方案。

1 多感知方式組合測(cè)量系統(tǒng)結(jié)構(gòu)

為實(shí)現(xiàn)采煤機(jī)的位姿檢測(cè)，提出了一種基于多感知方式組合測(cè)量的采煤機(jī)遠(yuǎn)程位姿檢測(cè)系統(tǒng)，采煤機(jī)位姿檢測(cè)系統(tǒng)布置如圖1所示。

圖1 采煤機(jī)位姿檢測(cè)系統(tǒng)布置Fig.1 Shearer position and attitude detection system layout

本系統(tǒng)根據(jù)綜采工作面不同的環(huán)境，設(shè)計(jì)了不同的檢測(cè)方法，一定程度減小了環(huán)境對(duì)采煤機(jī)位姿檢測(cè)精度的影響。區(qū)段運(yùn)輸平巷光照充足、粉塵少，但巷道狹長(zhǎng)，不便采用UWB定位系統(tǒng)定位，故采用全站儀對(duì)UWB固定基站群定位。綜采工作面工況復(fù)雜，采煤機(jī)工作時(shí)液壓支架需移動(dòng)，UWB定位較為靈活，基站之間可實(shí)現(xiàn)自主連續(xù)標(biāo)定，故采用UWB定位系統(tǒng)對(duì)UWB定位節(jié)點(diǎn)定位。采用UWB系統(tǒng)解算采煤機(jī)的姿態(tài)角需3個(gè)定位節(jié)點(diǎn)，解算過(guò)程會(huì)產(chǎn)生累積誤差，影響測(cè)量精度，故采用傾角傳感器和尋北儀，通過(guò)儀器內(nèi)部數(shù)據(jù)處理直接測(cè)得采煤機(jī)的姿態(tài)角。

采煤機(jī)位姿檢測(cè)系統(tǒng)結(jié)構(gòu)如圖2所示，將UWB定位系統(tǒng)固定基站群安裝在綜采工作面與運(yùn)輸平巷交點(diǎn)處，為實(shí)現(xiàn)UWB移動(dòng)基站移站時(shí)的自主標(biāo)定，液壓支架的頂端和底端分別安裝UWB模塊，安裝UWB模塊的液壓支架和未安裝UWB模塊的液壓支架間隔布置。將全站儀安裝在運(yùn)輸上山與區(qū)段運(yùn)輸平巷處，以全站儀為采煤機(jī)位姿檢測(cè)系統(tǒng)的定位基點(diǎn)，建立大地絕對(duì)坐標(biāo)系，由地測(cè)人員進(jìn)行坐標(biāo)標(biāo)定。將尋北儀、傾角傳感器及UWB定位節(jié)點(diǎn)安裝至采煤機(jī)機(jī)身，根據(jù)采煤機(jī)型號(hào)的尺寸規(guī)格可將定位節(jié)點(diǎn)安裝在機(jī)身上部或側(cè)部，將UWB定位節(jié)點(diǎn)安裝在采煤機(jī)坐標(biāo)系中便于測(cè)量且振動(dòng)較小的位置，由于UWB信號(hào)易受非視距傳播的影響，須保證定位節(jié)點(diǎn)與液壓支架上的UWB基站視距傳播。

圖2 采煤機(jī)位姿檢測(cè)系統(tǒng)結(jié)構(gòu)Fig.2 Shearer position and attitude detection system structure

采煤機(jī)位姿檢測(cè)系統(tǒng)原理如圖3所示，本系統(tǒng)采用UWB定位技術(shù)測(cè)量UWB定位節(jié)點(diǎn)在UWB固定基站群坐標(biāo)系中的坐標(biāo)位置，采用全站儀技術(shù)測(cè)量UWB固定基站群在定位基點(diǎn)坐標(biāo)系中的坐標(biāo)位置。當(dāng)采煤機(jī)工作時(shí)，截割頭載荷的變化會(huì)導(dǎo)致采煤機(jī)機(jī)身振動(dòng)，從而引起采煤機(jī)采煤方向以及自身水平方向的變化，采用傾角傳感器測(cè)量采煤機(jī)的橫滾角和俯仰角，采用尋北儀測(cè)得航向角。

圖3 采煤機(jī)位姿檢測(cè)系統(tǒng)原理Fig.3 Schematic of shearer posture detection system

2 采煤機(jī)的位置檢測(cè)系統(tǒng)

2.1 全站儀檢測(cè)系統(tǒng)

由于區(qū)段運(yùn)輸平巷空間狹窄、距離長(zhǎng)、不便用UWB系統(tǒng)定位，筆者采用全站儀對(duì)UWB固定基站群進(jìn)行空間定位，全站儀的三維坐標(biāo)測(cè)量原理如圖4所示。根據(jù)區(qū)段運(yùn)輸平巷的服務(wù)年限，由地測(cè)人員每隔半年到一年對(duì)設(shè)站點(diǎn)進(jìn)行坐標(biāo)標(biāo)定，將設(shè)站點(diǎn)坐標(biāo)C(xC,yC,zC)作為測(cè)站點(diǎn)的絕對(duì)坐標(biāo)，以此坐標(biāo)為基準(zhǔn)建立空間坐標(biāo)系，輸入后視點(diǎn)B的絕對(duì)坐標(biāo)B(xB,yB,zB)。

圖4 全站儀及坐標(biāo)測(cè)量Fig.4 Total station coordinate measurement chart

已知全站儀C到目標(biāo)點(diǎn)A的距離為SAC，待定邊AC的垂直角為α1，則全站儀C到目標(biāo)點(diǎn)A的平距[11]為

DAC=SACcosα1

已知邊BC的方位角為αBC，待定方向與已知方向的水平角為αAB，全站儀C的高程為HC，則目標(biāo)點(diǎn)A的三維坐標(biāo)為

根據(jù)全站儀的性能，可以快捷地測(cè)出目標(biāo)點(diǎn)(xA,yA,zA)的坐標(biāo)位置以及角度，從而可推導(dǎo)出全站儀坐標(biāo)系與UWB固定基站群坐標(biāo)系之間的坐標(biāo)變換矩陣。

2.2 UWB定位系統(tǒng)

UWB技術(shù)通過(guò)檢測(cè)超寬帶信號(hào)波到達(dá)時(shí)間來(lái)解算距離，可以減小井下高溫高灰塵工作環(huán)境對(duì)采煤機(jī)定位檢測(cè)精度的影響，UWB基站布置示意如圖5所示。UWB定位技術(shù)具有傳輸速率高、穿透力強(qiáng)、實(shí)時(shí)性好等優(yōu)點(diǎn)，為實(shí)現(xiàn)UWB基站的自主連續(xù)標(biāo)定，頂端和底端分別安裝UWB基站的液壓支架與未安裝基站的液壓支架間隔布置，通過(guò)各個(gè)UWB基站之間的信號(hào)傳輸，將UWB定位節(jié)點(diǎn)的坐標(biāo)位置傳輸至UWB固定基站群[12-14]。

圖5 UWB模塊及基站布置示意Fig.5 UWB base station layout diagram

根據(jù)采煤機(jī)機(jī)身定位節(jié)點(diǎn)與定位基站群之間的超寬帶信號(hào)飛行時(shí)間參量建立TOA定位模型。通過(guò)測(cè)量采煤機(jī)機(jī)身定位節(jié)點(diǎn)到個(gè)基站的空間直線距離，利用“在測(cè)量無(wú)誤差的條件下，目標(biāo)一定位于以各個(gè)定位基站為核心，以測(cè)量距離為半徑的球面上”這一幾何特征[15-16]，4個(gè)球位置面相交的交點(diǎn)就是采煤機(jī)機(jī)身定位節(jié)點(diǎn)的空間位置坐標(biāo)。

根據(jù)TOA定位模型建立了將Caffery算法和Taylor級(jí)數(shù)法相結(jié)合的算法，利用Caffery算法確定一個(gè)初始點(diǎn)坐標(biāo)[17]，進(jìn)而使用Taylor級(jí)數(shù)展開(kāi)法進(jìn)行迭代消除誤差[18-20]，可精確確定采煤機(jī)在空間內(nèi)的坐標(biāo)位置。

假設(shè)采煤機(jī)機(jī)身定位節(jié)點(diǎn)為M點(diǎn)，坐標(biāo)為(xM,yM,zM)，安裝在液壓支架的4個(gè)基站分別為1,2,3,4，坐標(biāo)為(xi,yi,zi)(i=1,2,3,4)，則基站i與定位節(jié)點(diǎn)M的空間距離為diM。

基站1,2,3,4與定位節(jié)點(diǎn)M之間的TOA定位觀測(cè)方程組為

(1)

面向采煤機(jī)的超寬帶位姿檢測(cè)系統(tǒng)的Caffery定位算法如下，使式(1)中的第n個(gè)方程減去第n+1個(gè)方程，得到關(guān)于(xM,yM,xM)線性方程組：

(2)

將式(2)中計(jì)算得出的 (xM,yM,xM)作為對(duì)M點(diǎn)的初始估計(jì)值，其與估計(jì)值(xM0,yM0,zM0)的誤差設(shè)為:δx、δy、δz，則有：

(3)

將式(1)在(xM,yM,zM)點(diǎn)用Taylor級(jí)數(shù)展開(kāi)，并忽略二次及以上項(xiàng)，整理可得：

(4)

對(duì)式(4)進(jìn)行重復(fù)迭代計(jì)算，當(dāng)|δx|+|δy|+|δz|小于設(shè)定的門限時(shí)，則停止迭代，采煤機(jī)機(jī)身定位節(jié)點(diǎn)M的最終估計(jì)值(xM1,yM1,zM1)為

運(yùn)用CT算法以同樣原理可算出位于液壓支架頂端和底端的各個(gè)UWB基站的坐標(biāo)位置，最終結(jié)算出采煤機(jī)機(jī)身定位節(jié)點(diǎn)在UWB固定基站群所建立的坐標(biāo)系中的空間坐標(biāo)位置。

2.3 坐標(biāo)系間的變換矩陣

已知采煤機(jī)機(jī)身定位節(jié)點(diǎn)M在UWB固定基站群坐標(biāo)系中的坐標(biāo)為(xM,yM,zM)，通過(guò)全站儀的測(cè)量已知由全站儀坐標(biāo)系OXYZ到UWB固定基站群坐標(biāo)系O′X′Y′Z′沿X軸移動(dòng)了dX，沿Y軸移動(dòng)了dY，沿Z軸移動(dòng)了dZ，繞X軸旋轉(zhuǎn)了θ角，繞Y軸旋轉(zhuǎn)了α角，繞Z軸旋轉(zhuǎn)了γ角，坐標(biāo)系變換如圖6所示，則兩坐標(biāo)系之間的旋轉(zhuǎn)矩陣為式(5)。由于兩坐標(biāo)系間沿X軸移動(dòng)了dX，沿Y軸移動(dòng)了dY，沿Z軸移動(dòng)了dZ距離，所以兩坐標(biāo)系之間的變換矩陣為式(6)。

圖6 坐標(biāo)系變換Fig.6 Transformation of coordinate system

(5)

(6)

3 采煤機(jī)姿態(tài)檢測(cè)系統(tǒng)

3.1 傾角傳感器結(jié)構(gòu)及工作原理

傾角傳感器將所有模塊集中至小電路板，其中包括微控制單元、加速度計(jì)、數(shù)模轉(zhuǎn)換電路以及通信單元，可直接輸出所要測(cè)量的角度。

傾角傳感器運(yùn)用了牛頓第二定律，在一個(gè)系統(tǒng)內(nèi)部可測(cè)量出加速度，根據(jù)初速度和加速度，可通過(guò)積分計(jì)算出線速度。當(dāng)傾角傳感器在水平狀態(tài)時(shí)，沒(méi)有橫向和垂直的加速度，只有一個(gè)重力加速度，重力垂直軸與靈敏軸之間的夾角為傾斜角。

“固體擺”式精度更高，應(yīng)用較為廣泛，其工作原理如圖7所示。系統(tǒng)由支架、擺錘和擺線組成，擺錘受重力G和拉力T的作用，β為擺線與重力方向的夾角，則所受合外力為

圖7 “固體擺”式傾角傳感器原理Fig.7 “Solid pendulum”tilt sensor principle

F=Gsinβ=mgsinβ

則有

在小角度測(cè)量范圍測(cè)量時(shí)，可認(rèn)為合外力F和夾角β呈線性關(guān)系，“固體擺”式傳感器便基于此原理。在實(shí)際應(yīng)用中，傾角傳感器的測(cè)量精度易受振動(dòng)的影響，故將傾角傳感器安裝至機(jī)身振動(dòng)相對(duì)較小的位置，并在下一階段研究中設(shè)計(jì)減震裝置。

3.2 尋北儀結(jié)構(gòu)及工作原理

陀螺尋北儀是由雙自由度動(dòng)力調(diào)諧陀螺，機(jī)械轉(zhuǎn)動(dòng)裝置和信號(hào)解算電路組成的，是一種利用陀螺原理測(cè)定地球自轉(zhuǎn)角速率在當(dāng)?shù)卣姹狈轿坏膽T性測(cè)量系統(tǒng)。

尋北儀測(cè)角如圖8所示，在尋北過(guò)程中，將陀螺置于動(dòng)基座上，測(cè)出陀螺在位置1處的角速度ω1，假設(shè)初始方向與真北方向的夾角為ψ，地球自轉(zhuǎn)速度為ωe，地球自轉(zhuǎn)在陀螺敏感軸的投影速度為ωecosφ，陀螺輸出的零點(diǎn)漂移為ε(t1)，則有

圖8 尋北儀及其測(cè)角示意Fig.8 North finder metering diagram

ω1=ωecosφcosψ+ε(t1)

(7)

轉(zhuǎn)動(dòng)基座90°到達(dá)位置2處，陀螺在位置2處的角速度ω2，陀螺輸出的零點(diǎn)漂移為ε(t2)，則有

ω2=ωecosφcos(ψ+90°)+ε(t2)

(8)

在轉(zhuǎn)動(dòng)基座90°到達(dá)位置3處，陀螺在位置3處的角速度ω3，陀螺輸出的零點(diǎn)漂移為ε(t3)，則有

ω3=ωecosφcos(ψ+180°)+ε(t3)

(9)

在轉(zhuǎn)動(dòng)基座90°到達(dá)位置4處，陀螺在位置4處的角速度ω4，陀螺輸出的零點(diǎn)漂移為ε(t4)，則有

ω4=ωecosφcos(ψ+270°)+ε(t4)

(10)

在短時(shí)間內(nèi)可將陀螺的零點(diǎn)漂移設(shè)為常量，即ε(t1)=ε(t2)=ε(t3)=ε(t4)，則由式(7)—式(10)可知

此方法可消除陀螺的零偏，快速地測(cè)得采煤機(jī)的航向角。在實(shí)際應(yīng)用中，尋北儀的測(cè)量精度易受振動(dòng)的影響，故將傾角傳感器安裝至機(jī)身振動(dòng)相對(duì)較小的位置，并在下一階段研究中設(shè)計(jì)減震裝置。

4 結(jié) 論

1)建立了一種基于多感知方式組合測(cè)量的采煤機(jī)位姿檢測(cè)系統(tǒng)，通過(guò)全站儀確定UWB固定基站群的位置坐標(biāo)，通過(guò)UWB定位模型確定UWB定位節(jié)點(diǎn)相對(duì)于UWB固定基站群的位置坐標(biāo)；通過(guò)傾角傳感器和尋北儀對(duì)采煤機(jī)姿態(tài)角進(jìn)行測(cè)量標(biāo)定，從而實(shí)現(xiàn)采煤機(jī)的位姿檢測(cè)。

2)完整推導(dǎo)了UWB定位系統(tǒng)基于TOA定位模型的CT算法，完整推導(dǎo)了全站儀坐標(biāo)系與UWB固定基站群坐標(biāo)系間的坐標(biāo)變換矩陣，介紹了全站儀、尋北儀及傾角傳感器的工作原理。分析了基于多感知方式組合測(cè)量的采煤機(jī)位姿檢測(cè)系統(tǒng)在實(shí)際應(yīng)用中的受限因素，并簡(jiǎn)要提出了改善方案。本系統(tǒng)在理論上可實(shí)現(xiàn)對(duì)采煤機(jī)綜采工作面中的實(shí)時(shí)位姿檢測(cè)，為系統(tǒng)仿真分析和實(shí)驗(yàn)平臺(tái)搭建提供了理論參考。

3)基于多感知方式組合測(cè)量的采煤機(jī)位姿檢測(cè)系統(tǒng)理論上實(shí)現(xiàn)了對(duì)采煤機(jī)在綜采工作面中的實(shí)時(shí)位姿檢測(cè)，為系統(tǒng)仿真分析和實(shí)驗(yàn)平臺(tái)搭建提供了理論參考。