張子悅,符世琛,劉 超,李永澤,蘭 玉,吳 淼
(中國礦業(yè)大學(北京)機電與信息工程學院,北京 100083)
無人采掘裝備是當前國際煤炭領域的發(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)在實際應用中的誤差來源,提出了相應的改善方案。
為實現(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
由于區(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固定基站群坐標系之間的坐標變換矩陣。
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固定基站群所建立的坐標系中的空間坐標位置。
已知采煤機機身定位節(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)
傾角傳感器將所有模塊集中至小電路板,其中包括微控制單元、加速度計、數(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和夾角β呈線性關系,“固體擺”式傳感器便基于此原理。在實際應用中,傾角傳感器的測量精度易受振動的影響,故將傾角傳感器安裝至機身振動相對較小的位置,并在下一階段研究中設計減震裝置。
陀螺尋北儀是由雙自由度動力調(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)可知
此方法可消除陀螺的零偏,快速地測得采煤機的航向角。在實際應用中,尋北儀的測量精度易受振動的影響,故將傾角傳感器安裝至機身振動相對較小的位置,并在下一階段研究中設計減震裝置。
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)仿真分析和實驗平臺搭建提供了理論參考。