懸臂式掘進機視覺導航與定向掘進控制技術

張旭輝，趙建勛，楊文娟，張 超

(1.西安科技大學 機械工程學院,陜西 西安 710054; 2.陜西省礦山機電裝備智能監測重點實驗室,陜西 西安 710054)

煤礦生產裝備智能化是煤炭綠色安全高效發展的關鍵[1-2]。隨著我國綜采技術和配套設備的快速發展，“采掘失衡”矛盾日益突出，巷道掘進自動化、智能化是目前國內攻關的關鍵技術[3]。懸臂式掘進機是綜掘工作面的重要裝備，其智能化技術對目前其他形式的快速掘進裝備發展具有代表性，自主定位、智能控制和遠程監控等技術發展決定未來掘進生產的發展[4-5]。自動定向掘進是實現掘進工作面自動化的核心技術。

懸臂式掘進機的定向掘進機控制包括掘進機機身位姿測量和掘進機的定向糾偏控制兩部分，前者是實現定向糾偏控制的基礎。文獻[6]利用超寬帶技術，結合TW-TOF測距法實現了井下巷道中機身位姿測量，但脈沖信號在井下環境中易受干擾，且測量誤差較大。文獻[7]提出零速修正的掘進機慣性導航定位方法，利用二次曲線擬合法進行零速修正，得到較高精度的機身位姿，試驗結果表明該定位方法可達到厘米級。文獻[8]設計了一種基于雷達系統的井下設備定位系統，可利用井下環境生成雷達返回信號，獲得井下設備軌跡及位置信息。文獻[9]利用機器視覺測量方法，以十字激光器與激光標靶為特征，利用相機采集激光標靶圖像,通過建立機身位姿視覺測量模型，實現對懸臂式掘進機機身位姿的檢測。文獻[10]采用機器視覺測量方法，以激光束為特征，建立了基于兩點三線的掘進機位姿解算模型，解算得到巷道中機身位姿。文獻[11]利用全站儀測量機身位姿，通過建立掘進機軌跡跟蹤的數學模型，結合行走機構液壓系統，提出了基于雙層模糊控制的掘進機軌跡跟蹤的控制方法。全站儀在使用過程中需要考慮移站問題，文獻[12]根據激光測量原理研究了全站儀測站移站后的自主標定原理，構建了系統測量誤差模型，驗證了激光導向系統自主標定的可行性，可實現對掘進機的位姿測量。文獻[13]利用懸臂式掘進行走機構運動學確定了機身位姿誤差和機身移動速度間的關系，設計了掘進機行走控制系統，仿真驗證了系統有效性。目前針對定向掘進控制系統的研究，以視覺、慣導和全站儀等測量方法為主要方式，通過建立掘進機軌跡跟蹤控制模型實現對掘進機機身的跟蹤控制。煤礦井下高粉塵、低照度工作環境對各測量方法提出了挑戰，上述方法掘進機位姿測量的精度和可靠性尚待進一步研究和應用驗證。

筆者提出一種基于機器視覺的懸臂式掘進機精確定位和軌跡跟蹤控制的定向掘進方法，利用單目視覺測量原理，以井下激光指向儀為特征解決井下特征獲取問題，通過建立懸臂式掘進機位姿視覺測量模型計算機身位姿;研究掘進機在井下巷道的糾偏機理，建立定向掘進運動控制模型，設計軌跡跟蹤控制器實現對懸臂式掘進機定向掘進的過程控制。

1 懸臂式掘進機定向掘進控制方案

懸臂式掘進機自動定向掘進控制原理如圖1所示。系統由懸臂式掘進機、防爆計算機、防爆工業相機、激光指向儀、DSP控制器和傳感器構成;軟件系統實現圖像采集、特征提取、機身位姿解算、機身軌跡跟蹤控制和數據存儲顯示等功能，可完成巷道中機身位姿精確測量以及掘進機軌跡跟蹤控制。

圖1 懸臂式掘進機自動定向掘進控制總體方案Fig.1 Overall scheme of automatic directional heading control for boom-type roadheader

系統采用單目視覺測量方法，通過構建井下巷道懸臂式掘進機機身位姿測量模型，求解機身實時位姿并傳輸到DSP控制器，獲得機身當前位姿與期望位姿之間的位姿誤差，利用DSP控制器輸出控制指令控制掘進機移動，進行軌跡跟蹤控制，同時進行掘進機機身及截割頭實時位姿顯示，為操控人員提供超挖預警報警。各模塊原理如下:

(1)機身位姿視覺測量。根據掘進機、攝像機和激光指向儀在巷道中的位置關系構建巷道全局坐標系，固定在機身上的防爆工業相機采集后視巷道圖像信息，通過圖像處理、激光特征提取以及位姿解算等過程求解機身位姿數據。

(2)數據交互模塊。系統工作時需要各模塊之間協同工作，其中DSP控制器利用RS485串行通信模塊與計算機通信，獲取巷道參數和機身位姿數據，利用A/D模塊采集流量、壓力和電流等模擬量傳感器數據，通過各數據庫中數據交互，實現各工作模塊之間的數據交互。

(3)軌跡跟蹤控制。根據懸臂式掘進機軌跡糾偏控制策略構建掘進機定向掘進運動控制模型，比較實時反饋的機身位姿數據和機身移動期望位置得到機身位姿誤差，利用DSP控制器D/A模塊向掘進機行走機構液壓系統發送控制指令，通過控制比例換向閥的開口，控制掘進機左右履帶運動速度，使掘進機沿規劃路徑移動，通過不斷調整機身位姿，實現對巷道中掘進機的軌跡跟蹤控制。

2 懸臂式掘進機位姿視覺測量方法

2.1 掘進機機身位姿視覺測量方案

2.1.1 測量特征選取

圖2(a)為井下高粉塵、低照度和多雜光的工作環境，普通視覺測量特征難以在該環境中準確識別測量特征，嚴重影響視覺測量的可靠性。圖2(b)為巷道定位激光指向儀，具有體積小、結構簡單、巷道環境中具有明顯對比特征，適用于井下高粉塵的復雜工作環境。本文借助激光指向儀包含巷道坐標系位置信息的特點，以巷道布置的激光指向儀光斑和激光束為特征，為巷道中掘進機定位提供基準信息，建立掘進機位姿視覺測量模型，解算掘進機機身實時位姿。

圖2 井下環境中激光指向儀特征圖像Fig.2 Feature image of laser pointer in downhole environment

2.1.2 掘進機位姿視覺測量系統原理

懸臂式掘進機位姿視覺測量系統如圖3所示，由防爆計算機、防爆工業相機和兩激光指向儀組成。系統進行掘進機位姿測量之前，將防爆工業相機方向向后固定于掘進機機體上，在煤礦地測科確定的激光指向儀(包含巷道設計信息)基礎上再平行固定一個激光指向儀，利用特殊設計的輔助裝置保證兩個激光束沿巷道設計方向平行。利用工業相機采集激光指向儀圖像，傳輸到計算機進行圖像預處理、激光點線特征提取，結合已建立的懸臂式掘進機機身位姿解算模型，解算掘進機在巷道坐標系中的位姿，實現懸臂式掘進機的位姿測量。機身位姿測量流程如圖4所示。

圖3 懸臂式掘進機視覺測量系統結構Fig.3 Visual measurement system structure of boom-type roadheader

圖4 懸臂式掘進機機身位姿測量流程Fig.4 Flow chart of fuselage pose measurement of boom-type roadheader

2.2 機身位姿視覺測量模型

(1)

2.3 掘進機機身位姿解算方法

相機成像模型是對攝像機成像的一種幾何描述，攝像機的成像模型一般為小孔成像模型[14]，該模型可以看作為線性幾何模型。激光指向儀包括左右兩激光光斑和兩條激光束直線，結合兩光斑的連線可將激光指向儀的兩點三線特征簡化為如圖5所示的基于門形結構的掘進機機身位姿測量模型[15]。

圖5 基于門形結構的機身位姿視覺測量原理Fig.5 Visual measurement schematic diagram of fuselage posture based on portal structure

將兩激光束簡化為直線段L1和L3，且L1//L3。圖中點P1和P2為兩激光指向儀的光斑中心，P3和P4分別為激光指向儀所發出的激光線上的距離激光光斑一定距離的點，L2為過兩光斑中心的直線。點q1和q2為兩激光光斑中心點在圖像坐標系上的投影點，l1，l2和l3為L1，L2和L3在圖像坐標系上的投影直線。假設投影直線l1,l2和l3的直線方程為

aix+biy+ci=0 (i=1,2,3)

(2)

式中，ai，bi，ci分別為直線li的直線方程一般式的系數。

根據直線方程可得到投影直線l1，l2和l3的方向向量vi(-bi,ai,0)。圖中相機光心Oc、圖像平面中的投影直線l1，l2，l3和空間直線L1，L2，L3圍成OcP2P3，OcP1P2，OcP1P4三個平面。設3個平面的法向量分別為N1，N2和N3。在平面OcP2P3中，設t1為投影直線l1上的任意一點，則可以得到平面OcP2P3的法向量N1為

N1=Oct1×V1

(3)

同理可確定平面OcP1P2和平面OcP1P4的法向量N2和N3。由于L1//L3，N3⊥L3，則可得N3⊥L1，N1⊥L1。可得空間直線L1，L2和L3的方向向量V1，V2和V3存在如下關系:

V3=V1=N1×N3

(4)

V2=V1×N2

(5)

V′2=k2P2/M2-k1P1/M1

(6)

(7)

設圖像中l1和l3兩投影直線交于消失點q3(x3,y3)，則根據消失點特性[16]可知向量Ocq3的單位向量與Zd軸的方向單位向量一致，可得Zd軸的方向向量nz為

(8)

通過nx和nz叉乘可確定Yd軸的方向向量ny為

(9)

(10)

則可計算得到相機在激光測量坐標系中的偏航角αc、橫滾角βc、俯仰角γc為

(11)

(12)

相機在激光測量坐標系下的位置(Xc,Yc,Zc)為

(13)

(14)

將式(14)代入式(1)中，根據相機在激光測量坐標系下的位置和姿態角信息，結合通過事先標定得到的掘進機機體坐標系和相機坐標系之間的變換關系，可計算得到掘進機在激光測量坐標系下的位姿。利用全站儀測量激光指向儀在巷道坐標系下的坐標，獲取激光測量坐標系和巷道坐標系之間的轉換關系，最后便可求得掘進機在巷道坐標系下的偏航角、橫滾角、俯仰角和位置信息。

3 懸臂式掘進機定向掘進控制模型

3.1 掘進機糾偏控制策略

巷道掘進施工中，懸臂式掘進機糾偏主要包括整平和對中，其中整平過程通過控制掘進機前后鏟板和支撐的升降調整掘進機的角度;對中過程是控制掘進機左右履帶運動使掘進機機身位置與期望位置重合，實現對機身位置的調整[17]。通過掘進機對中糾偏過程，實現對掘進機機身偏航角偏差α和位置偏差的糾偏控制。圖6為掘進機的對中示意圖，掘進機糾偏步驟如下:

(1)首先比較視覺測量得到的機身位姿數據和期望的機身位姿數據，確定機身偏航角偏差α和中心距偏差d。

(2)機身存在中心距為d時，當d>0時，控制掘進機左履帶反轉，右履帶正轉，使機身存在一定偏向角，然后控制左右履帶同時正轉，直至中心距小于限定值;當d<0時，則方向相反。

(3)機身存在偏航角偏差α時，當α>0時，控制掘進機左履帶反轉，右履帶正轉;α<0時，控制掘進機左履帶正轉，右履帶反轉，直至偏航角小于限定值。

通過上述掘進機糾偏控制步驟可實現巷道中掘進機的定向掘進控制。

3.2 懸臂式掘進機定向掘進運動模型

通過對懸臂式掘進機糾偏分析可知，掘進機是通過左右兩側履帶做差速運動實現的運動控制。為建立掘進機機身的運動學模型，假設:① 懸臂式掘進機在二維平面內運動;② 懸臂式掘進機左右兩側履帶與地面完全接觸;③ 兩側履帶中心連線與掘進機前后運動方向垂直[18]。則懸臂式掘進機運動學模型如圖7所示。

圖7中坐標系XOZ為巷道二維平面坐標系;k點為參考點，xk，yk為該點的坐標;θ為懸臂式掘進機的航向角;坐標系X0O0Z0為以掘進機質心為原點的掘進機二維平面坐標系，設(x0,y0)為掘進機質心在巷道坐標系XOZ中的坐標;vL和vR為掘進機運動時其左右履帶速度;b為單側履帶的寬度，D為機身寬度。則機身在巷道坐標系XOZ中的位姿可表示為P=[x0,y0,θ]T。根據圖中的幾何關系可推得懸臂式掘進機運動學模型為

(15)

設機身運動線速度為v，角速度為ω，則左右履帶速度和機身之間速度可表示為

(16)

結合式(15)，(16)可得掘進機從當前位置移動到k點時的運動學方程為

(17)

(18)

其中，xe為掘進機在巷道中的橫向誤差;ye為掘進機在巷道中的縱向誤差;θe為掘進機航向角誤差。其位姿誤差微分方程描述為

(19)

式中，ωA為當前位姿狀態下的機身運動角速度;vA為當前位姿狀態下的機身運動線速度;vB和ωB為期望位姿狀態下的機身運動線速度和角速度;偏航角誤差θe=θB-θA。

4 軌跡跟蹤控制器設計

4.1 掘進機定向掘進控制系統

懸臂式掘進機通過左右兩側履帶差速運動實現機身的移動，屬于典型的非完整移動機器人。對于非完整移動機器人系統需要設計控制輸入作用的控制器，使機器人可以以給定的速度沿規劃軌跡運動。掘進機定向掘進控制系統具有高度非線性，經典線性控制理論難以直接應用于系統的控制器設計。Back-stepping方法[19]是一種非線性系統的穩定設計理論，針對非線性系統中的參數不確定性，通過逐步修正算法設計控制器，可實現系統的全局調節。因此，論文采用Back-stepping方法，采用根據懸臂式掘進機運動學模型設計軌跡跟蹤控制器，懸臂式掘進機定向掘進控制系統結構如圖8所示。

圖8 懸臂式掘進機定向掘進控制結構框Fig.8 Block diagram of directional heading control structure of boom-type roadheader

系統輸入掘進機期望位姿Pr=[xr,yr,θr]T和跟蹤控制時的期望運動速度，即參考速度qr=[vr,ωr]T;輸出為掘進機當前時刻位姿Pc=[xc,yc,θc]T。通過設計系統速度控制器使當前位姿與期望位姿之間的位姿誤差Pe確定掘進機的運動控制速度Pc，通過反復調節控制使得掘進機位姿誤差Pe趨近于0。

4.2 掘進機軌跡跟蹤控制器設計

筆者結合Back-Stepping設計方法，根據Lyapunov函數設計具有全局收斂特性的掘進機軌跡跟蹤控制器。分別以Pr和qr為掘進機期望位姿及速度，則根據式(19)掘進機位姿微分方程有

(20)

引理:對任意x∈R，并且|x|<∞，則有f(x)=xsin(arctan(x))≥0，當且僅當x=0時，f(x)=0成立[20]。根據上述引理可知，懸臂式掘進機軌跡跟蹤控制器設計步驟如下:

第1步:根據掘進機位姿微分方程可知，當xe=0時可選Lyapunov函數為

(21)

對式(21)微分可得

(22)

根據上述引理可知，當θe=a=arctan(vrye)時可使ye收斂，此時若xe收斂于0，則θe收斂到a，便有ye收斂于0。掘進機軌跡跟蹤的控制律輸入為v和ω，可取偏向角誤差變量為

(23)

第2步:在Vy基礎上選擇全局Lyapunov函數V為

(24)

對上式微分可得

(25)

則可得懸臂式掘進機軌跡跟蹤控制律為

(26)

其中，(xe,ye,θe)T為掘進機的位姿誤差;(vr,ωr)為掘進機的參考速度;k1和k2為常數。掘進機軌跡跟蹤控制律以掘進機位姿誤差和參考速度為輸入，以掘進機運動的控制速度為輸出，通過控制器的反復調節，使掘進機的位姿誤差最終趨近于0。

5 測量系統性能評價及實驗結果

5.1 機身位姿視覺測量實驗

5.1.1 懸臂式掘進機位姿視覺測量實驗

搭建了懸臂式掘進機位姿視覺測量實驗平臺，進行功能驗證和測量性能評價，如圖9所示。平臺由懸臂式掘進機模型、2個激光指向儀、MV-EMV510M防爆工業相機和防爆計算機等組成。

圖9 懸臂式掘進機機身位姿測量實驗平臺Fig.9 Boom-type roadheader body posture measurement experimental platform

懸臂式掘進機位姿視覺測量精度測試實驗步驟如下:

(1)搭建平臺時，將防爆計算機固定于懸臂式掘進機機體上，使相機采集激光指向儀激光圖像，在原有激光指向儀旁再平行固定一個激光指向儀，為掘進機機身位姿視覺測量提供測量基準。

(2)通過軟觸發方式控制防爆攝像機采集激光指向儀光斑和光束圖像，并在計算機中進行圖像預處理、激光光斑中心定位、激光束直線特征提取以及位姿解算等過程，計算得到掘進機在激光測量坐標系中的位姿。

(3)將懸臂式掘進機移動至不同位置，利用全站儀獲得掘進機機身在激光測量坐標系下的位姿實際值，與視覺測量系統測量結果進行對比，獲取掘進機位姿視覺測量誤差。

設定兩激光之間距離為55 cm，根據上述實驗步驟進行實驗，得到實驗結果和誤差如表1，圖10，11所示。

表1 懸臂式掘進機機身位姿測量部分實驗結果Table 1 Experimental results of boom-type roadheader body posture measurement

圖10 機身姿態角測量誤差Fig.10 Error of measurement of fuselage attitude Angle

實驗結果表明本文所述的懸臂式掘進機位姿視覺測量方法可有效實現機身位姿的獲取。圖10為測量得到的掘進機姿態角角度誤差，其中機身偏航角角度誤差小于0.3°，俯仰角角度誤差小于0.25°，橫滾角角度誤差小于0.3°。圖11為測量得到掘進機位置誤差，x，y，z軸方向的誤差分別為20，25,18 mm。

圖11 機身位置測量結果誤差Fig.11 Error of airframe position measurement

5.1.2 懸臂式掘進機可視化截割井下測試

基于筆者提出的機身定位方法研發了懸臂式掘進機可視化截割控制系統，圖12為在山西煤礦井下環境中搭建的懸臂式掘進機可視化輔助截割系統試驗平臺，平臺由掘進機位姿視覺測量系統、捷聯慣導和防爆計算機等組成。以多傳感器實現機身和截割頭位姿測量，借助機器人控制方法實現結果過程的可視化導引和超挖預警。

圖12 懸臂式掘進機可視化輔助截割系統試驗平臺Fig.12 Visual auxiliary cutting system test platform of boom-type roadheader

該系統在某礦回風掘進巷道進行了懸臂式掘進機可視化輔助截割測試。測試時移動安裝在機身上的防爆工業相機來模擬機身的移動，通過人工測量方式獲取相機在巷道中的移動數據為真實值，最后將人工測量結果與視覺測量結果比較獲取機身位姿測量誤差。表2及圖13為得到的測量結果及誤差，考慮井下環境中機身姿態角角度的真實值測量困難，故僅給出計算得到的掘進機姿態信息。

表2 懸臂式掘進機井下位姿測量部分實驗結果Table 2 Part of experimental results of underground pose measurement of boom-type roadheader

圖13 懸臂式掘進機位置測量誤差Fig.13 Measurement error of boom-type roadheader

結果表明，該方法可以在掘進工作面高粉塵、水霧以及雜光的環境下穩定獲取激光圖像特征，計算出懸臂式掘進機機身位置信息。由于井下巷道地勢環境影響視覺標定精度，以及惡劣環境等影響，系統的測量誤差與實驗室相比有所增大，X，Y，Z軸方向的誤差分別為±38，±40，±40 mm。值得注意的是，考慮了視覺測量可能存在的遮擋、丟幀等問題，可視化截割系統中組合了低成本MEMS慣導，多傳感器融合保證了懸臂式掘進機機身定位的精度和穩定性。

5.2 掘進機軌跡跟蹤控制實驗

為了驗證定向掘進控制系統的可行性和穩定性，搭建定向掘進控制實驗平臺，如圖14所示,為方便對履帶轉動速度的控制，采用電機驅動的履帶式移動機器人代替懸臂式掘進機進行實驗。實驗平臺由履帶式移動機器人、內置DSP控制器、MV-EMV510M防爆工業相機、YHJ-800激光指向儀和計算機組成。

圖14 定向掘進控制實驗平臺Fig.14 Directional heading control experiment platform

實驗時以樓道模擬巷道環境，確定巷道坐標系，首先采用全站儀測量履帶機器人和激光測量坐標系在巷道坐標系中的位姿，并對相機進行外參標定。在運動過程中，借助機身位姿視覺測量系統實時測量機器人位姿，并將機身位姿數據反饋至DSP控制器內，根據當前機身位姿誤差確定輸出控制命令，控制左右履帶轉動，通過不斷調整使機身移動至期望位置。

設定以巷道中線為掘進機定向移動的參考軌跡，利用全站儀測量得到移動機器人在巷道平面坐標系的初始位置為(-0.4，0)，偏航角為-5.21°，設移動機器人運動終點為(0，5)，偏航角為0°，機身位置誤差允許范圍在3 cm以內，角度誤差允許范圍在0.2°以內。根據上述實驗步驟進行實驗，如圖15，16所示為軌跡跟蹤控制結果及誤差。

圖15 機身軌跡跟蹤結果Fig.15 Airframe trajectory tracking results

圖15機身軌跡跟蹤結果中，藍色線為設定的機身參考運動軌跡，紅色線為機身的實際運動軌跡，機身從起始位置開始運動時存在起始位置偏差，隨著運動進行，機身逐漸沿參考軌跡進行運動，通過不斷調整機身運動方向，使機身最終運動至期望點。由圖16機身軌跡跟蹤誤差可得，機身位置和偏航角誤差最終都收斂，且機身在X方向的位置誤差為±15 mm以內，在Y方向的位置誤差為±20 mm以內，機身偏向角角度誤差在0.15°以內，均滿足允許范圍。

圖16 機身軌跡跟蹤誤差Fig.16 Fuselage trajectory tracking error

6 結 論

(1)提出基于圖像測量的激光指向儀精確定位方案，由于相機安裝于機身且兩束激光線構建門型三線特征實現機身位姿測量，與相關研究對比，具有不易脫靶、標靶移動少等優點。

(2)基于單目視覺測量原理建立懸臂式掘進機位姿視覺測量模型，以2個激光指向儀為參考基準，實現掘進機機身的相對位姿測量，具有系統簡單經濟的優勢。實驗表明，利用視覺測量方法進行機身位姿測量時，姿態角角度誤差在0.3°以內，位置誤差在25 mm以內。實際在井下工業試驗時，考慮了視覺測量存在的遮擋、丟幀等問題，組合了低成本MEMS慣導，獲得了穩定的測量效果。

(3)根據懸臂式掘進機運動特點設計糾偏策略，構建了懸臂式掘進機定向掘進運動控制模型，采用Back-stepping方法實現掘進軌跡的跟蹤控制，對實現綜掘工作面自動化提供了借鑒。軌跡跟蹤控制實驗表明，控制器按照視覺測量反饋的信息控制機身沿設定的參考軌跡運動，運動至終點時，掘進機機身偏航角角度誤差在0.15°以內，機身位置誤差在20 mm以內，可在誤差允許范圍內實現對移動機器人的軌跡跟蹤控制。