基于多感知方式組合測量的采煤機位姿檢測系統(tǒng)研究

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

(中國礦業(yè)大學(北京)機電與信息工程學院，北京 100083)

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖4 全站儀及坐標測量Fig.4 Total station coordinate measurement chart

已知全站儀C到目標點A的距離為SAC，待定邊AC的垂直角為α1，則全站儀C到目標點A的平距[11]為

DAC=SACcosα1

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

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

2.2 UWB定位系統(tǒng)

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

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

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

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

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

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

(1)

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

(2)

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

(3)

將式(1)在(xM,yM,zM)點用Taylor級數(shù)展開，并忽略二次及以上項，整理可得：

(4)

對式(4)進行重復迭代計算，當|δx|+|δy|+|δz|小于設定的門限時，則停止迭代，采煤機機身定位節(jié)點M的最終估計值(xM1,yM1,zM1)為

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

2.3 坐標系間的變換矩陣

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

圖6 坐標系變換Fig.6 Transformation of coordinate system

(5)

(6)

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

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

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

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

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

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

F=Gsinβ=mgsinβ

則有

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

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

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

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

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

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

(7)

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

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

(8)

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

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

(9)

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

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

(10)

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

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

4 結(jié) 論

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

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

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