懸臂式掘進機行走軌跡及偏差感知方法

吳 淼，沈 陽，吉曉冬,2，王鵬江，姜 海，鄭偉雄，李 悅

(1.中國礦業大學(北京) 機電與信息工程學院，北京 100083; 2.石家莊煤礦機械有限公司，河北 石家莊 050033)

巷道掘進是煤礦開采中效率最低、危險度最高的前端生產環節。我國國有煤礦每年巷道掘進量超過15 000 km，絕大部分采用以懸臂式掘進機為主要開采設備的綜合機械化掘進(綜掘)方式作業[1-2]。近年來，煤礦綜掘工作面的智能化、無人化需求日益迫切，其中最關鍵的是巷道精準定向掘進[3-4]。掘進機姿態和位置信息的實時精準感知是掘進機自主行走控制和截割斷面誤差補償的基礎，是巷道定向掘進的先決條件。

目前，我國綜掘普遍采用的掘進定向方法仍舊是傳統的“激光指向儀”指示法[5]。煤礦地測部門人員利用全站儀確定指向儀在巷道頂板的安裝位置，保證其指示方向與巷道設計中線方向完全一致，這樣指向儀在大地坐標系下的絕對位置坐標和指向的絕對角度值即為已知。

激光指向儀發射的激光光斑是掘進定向的惟一依據，施工人員肉眼感知光斑位置，憑借經驗操控掘進走向。每一輪或幾輪斷面截割完成后，人工測量光斑與巷道兩幫的距離，以評估巷道走向是否存在偏差，并在此后掘進中做出相應調整，達到定向掘進的目的。

上述方式存在較多的問題:

(1)只能對巷道設計方向進行指示，與掘進機本身位姿參數無任何聯系，無法為掘進機自主導控和巷道偏差補償提供基準參數。

(2)無法解讀激光光束所攜帶的基準信息，僅憑司機肉眼觀察感知，憑經驗操控向前掘進。

(3)工作面粉塵質量濃度大，作業現場光線差，司機視線受遮擋。

(4)屬于巷道掘后檢測，無法在掘進過程中實時感知掘進偏差。

(5)需多人員協助測量，危險系數高，勞動強度大，自動化水平低。

煤巷綜掘無人化、智能化的發展趨勢亟需發展巷道智能定向掘進感知技術。許多國內外學者都對掘進機機身位姿參數的感知方法做了研究。

賈文浩等[6]提出了一種基于室內定位系統(indoor Global Positioning System,iGPS)測量原理的掘進機定位方法,實現了在狹長封閉巷道內對掘進機位置的絕對精度檢測。杜雨馨等[7-8]構建了一種基于機器視覺技術的掘進機機身位姿檢測系統，通過對十字光線成像特征的分析，在實驗室條件下實現了掘進機機身位姿的測量。陶云飛[9]將高精度全自動全站儀應用于掘進機的位姿自動檢測中，通過測量安置在機身不同位置棱鏡的坐標，解算出掘進機的位置、姿態信息，一定程度上實現了掘進機位姿參數的自動檢測。符世琛等[10-11]提出了一種基于超寬帶(Ultra Wide-Band，UWB)測距原理的掘進機定位定向方法，建立了相關解算數學模型及誤差分布模型，為掘進機行走控制提供位置狀態和位姿參數。馮大龍[12]較早的將捷聯式慣導系統(捷聯慣導)應用在掘進機的無人控制中，利用慣性測量單元測量掘進機相對于慣性空間的運動參數，使掘進機的無人作業成為了可能。

將多種傳感器組合可以實現優勢互補，以應對綜掘工作面粉塵大、空間小、振動強、光線弱的惡劣環境。因此，很多學者提出了多傳感器信息融合的掘進機位姿感知方法。毛君等[13]提出了將陀螺儀與全站儀組合的掘進機導向系統，實現了掘進機姿態和位置信息的連續獲取。童敏明等[14]采用三軸加速度傳感器、單軸陀螺儀和超聲波測距傳感器構建了掘進機定位系統，將機身位姿、偏轉角度和位移等信息融合處理，得到了掘進機的位置信息。黃東等[15]提出了一種基于機器視覺/捷聯慣導的掘進機位姿組合測量方法，捷聯慣導測得姿態信息，單目視覺測得位置信息，從而實現掘進機實時位姿5個自由度的測量。

雖然上述研究對于掘進機位姿感知水平有了大幅提升，為巷道的精準定向掘進做出了很大貢獻，但由于綜掘工作面環境惡劣(高溫、高濕、高粉塵、光線昏暗、底板起伏大)，災害頻發(瓦斯、冒頂、片幫、底臌)，裝備繁多(掘進機、轉載機、帶式輸送機、刮板輸送機、臨時支架、鉆錨設備等)，空間狹小(狹長封閉空間)、工藝離散(掘-支-網-鉆-錨-運六大主要工藝環節彼此獨立)，目前還不能夠在綜掘流程中實現實時、自主、精準感知掘進機姿態和位置，生成行走軌跡為掘進機的動態調整提供參考的實用方法。

另外，懸臂式掘進機為主的綜掘工作面是一種間斷式的循環截割掘進，可以由斷面截割補償掘進機機身位姿的偏差。而上述研究只是檢測掘進機的位姿參數，并未建立機身位姿參數與巷道定向掘進的關系，不能為截割斷面誤差補償提供偏差參數。

再者，掘-支同步式快速掘進新工藝已在普遍探索之中，有可能成為一種發展趨勢，新型快掘機組中的臨時支護設備將嚴重遮擋綜掘作業空間，造成 “外置式”位姿感知裝置嚴重受限，因此 “機載式”的位姿感知方法成為必然需求[16-17]。

針對上述問題，筆者提出一種能夠適用于量大面廣的懸臂式掘進機的行走軌跡及偏差感知技術，可以實時、自主、準確、直接地獲取掘進機行走軌跡，同時給出掘進機縱向軸線與巷道設計中線的偏差角、掘進機質心與巷道中線的偏差距離等位姿偏差信息。根據行走軌跡動態調整掘進機位姿并對位姿殘余偏差進行反向截割補償，保證巷道定向掘進精度。

1 偏差的描述及影響分析

獲取掘進機機身位姿參數是為了給巷道精準掘進提供糾偏參數。以往的研究[18]已經提出了一套符合實際工況且精準、簡便、易測的掘進機位姿參數體系，定義了橫滾角、俯仰角、車前距、偏角、偏距5個掘進機機身位姿參數，它們可全面描述掘進機機身在煤巷內的位置和姿態。

偏角和偏距2個參數描述了懸臂式掘進機在掘進方位上的偏差，準確地反映了掘進機實際掘進方向偏離巷道設計中線的情況，是定向掘進最重要的參數。將偏角和偏距統稱為掘進機方位偏差。

1.1 坐標系定義

1.1.1 地心慣性坐標系

地心慣性坐標系記為i系(Xi,Yi,Zi)，其坐標原點為地心，Zi軸沿地球自轉軸指向北極，Xi,Yi軸在赤道平面內，分別指向慣性空間的兩顆恒星。慣性坐標系不參與地球自轉，其3個坐標軸在慣性空間內的指向固定不變。

1.1.2 大地坐標系

大地坐標系記為d系(Xd,Yd,Zd)，其坐標原點為地心，Zd軸沿地球自轉軸指向北極，Xd,Yd軸在赤道平面內分別指向本初子午線和東經90°。大地坐標系固聯在地球上，隨地球自轉。煤礦中巷道設計的位置和方向都是在大地坐標系中設計并標識的。

1.1.3 巷道坐標系

巷道坐標系記為h系(Xh,Yh,Zh)，其坐標原點在巷道設計中線上，并由掘進機初始位置確定，Xh軸指向巷道左側，Yh軸沿巷道設計中線指向掘進方向，Zh軸豎直向上與Xh,Yh軸符合右手定則。

1.1.4 機體坐標系

機體坐標系記為b系(Xb,Yb,Zb)，其坐標原點在機身幾何重心處，Xb軸沿機身橫軸指向機身左側，Yb軸沿機身縱軸指向機頭方向，Zb與Xb,Yb軸符合右手定則。機體坐標系固聯在掘進機機身，跟隨掘進機移動。

1.2 掘進機方位偏差的描述

為簡便起見，在上述巷道坐標系中只保留掘進機履帶部分來表示掘進機機身。

1.2.1 偏角

掘進機機身中線與煤巷設計中線在煤巷底板平面XOY上投影的夾角α,定義為掘進機的偏角，如圖1所示。偏角反映了掘進機實際朝向與煤巷設計方向之間的角度偏差值。

圖1 偏角Fig.1 Deviation angle

1.2.2 偏距

機身中心點C與煤巷設計中線在煤巷底板平面XOY上投影的距離l，定義為掘進機的偏距，如圖2所示。

圖2 偏距Fig.2 Deviation distance

1.3 掘進機方位偏差與巷道定向掘進的關系

偏角和偏距體現了掘進機偏離巷道設計中線的情況，一旦出現偏差，將導致實際掘進方向偏離設計掘進方向，影響整個采區的正常生產，嚴重時還可能引發巷道內地質災難，造成井下安全事故。李睿[18]通過激光位姿檢測系統實現了偏角和偏距的檢測，但不能形成掘進機動態行走軌跡，不能為掘進機前后調動提供實時參考。

動態的掘進機行走軌跡和方位偏差信息可以在掘進機向截割斷面行走過程中實時為掘進機機身位姿調整提供參考，以盡可能減小掘進機方位偏差。由于巷道內空間有限以及驅動掘進機行走的液壓馬達難以精確控制，掘進機機身方位偏差很難徹底消除?？筛鶕堄嗑蜻M機方位偏差信息調整巷道截割斷面的位置以反向補償巷道掘進偏差，從而達到巷道精準定向掘進的目的。

因此，需要研發一種可以自主動態感知掘進機行走軌跡、同時給出掘進機偏角和偏距的位姿感知系統。

2 行走軌跡及偏差感知系統組成

掘進機行走軌跡及偏差感知系統主要由激光偏距感知系統、捷聯慣導、二維里程計、解算主機等組成，如圖3所示。

2.1 激光偏距感知系統

激光偏距感知系統在以往基礎上加以改進，由一臺扇形激光發射器、一臺激光標靶組成，如圖3所示。地測人員按照巷道設計要求將扇形激光發射器精確安裝在后方巷道頂部中線上(已知精確偏距亦可)，發出的扇形激光作為巷道設計中線基準傳遞媒介。激光標靶平行安裝在掘進機機身已知位置且與機身中線垂直。扇形激光在激光標靶上形成線型光斑，被光敏元件感知，通過內部控制器解算，得到掘進機相對于巷道中線的偏差距離。偏距感知系統僅在掘進初始位置使用，提供巷道坐標系下初始位置的橫坐標。

圖3 系統組成Fig.3 System compositions

2.2 捷聯慣導

捷聯慣導由三軸光纖陀螺儀、三軸加速度計以及解算芯片組成，安裝在掘進機機身頂部中心，可自主測得大地坐標系下掘進機相對于正北方向的航向角，還可解算出掘進機在大地坐標系下的航向、姿態、速度、位置等信息[19]。其航向角及姿態角的測量精度很高，而速度、位置的誤差隨時間增加會逐漸增大。捷聯慣導使用前需輸入當地的經緯度坐標，經過3 min的初始自對準(自對準過程中保證掘進機靜止)，自主尋得掘進機相對于正北方向的航向角，以及初始的俯仰角和橫滾角;初始自對準完成后，可實時精準測得掘進機在大地坐標系下的運動信息，從而實現“靜態對準，動態測量”。

(1)

(2)

式中，L，g和ωie分別為當地緯度、重力加速度和地球自轉角速率;ωiecosL和ωiesinL分別為地球自轉角速度在北向和天向的分量。

(3)

整合可得

(4)

式(4)展開為

(5)

(6)

式中，γ，θ，φ分別為載體的滾轉角、俯仰角和航向角。

將Tij的值與式(6)中的元素對應，即可求得掘進機的初始航向和姿態信息。

經過上述粗對準過程，由于陀螺儀和加速度計會受到角晃動和線晃動的干擾以及慣性器件本身的測量誤差，導致捷聯慣導獲得的航向角和姿態角存在一定的失準角誤差。通過進一步的濾波精對準過程，可減小失準角誤差的影響，得到精準的航向、姿態信息[20-21]。

初始自對準完成后，掘進機開始向前行走，可用四元數法對掘進機進行實時姿態角更新[22]。

在捷聯慣導解算中常用四元數表示載體的姿態。四元數由1個實數部分和3個虛數部分組成，它將載體的三維姿態表示為載體繞固定點的旋轉，可完整描述載體的姿態信息。四元數Q的形式為

Q=q0+q1i+q2j+q3k

(7)

式中，q0，q1，q2，q3為實數;i，j，k既是相互正交的單位向量，又是虛數單位。

捷聯慣導姿態更新可由下式表示:

Q(tk+1)=Q(tk)?q(h)

(8)

式中，Q(tk+1)和Q(tk)分別為tk+1時刻和tk時刻的姿態四元數;?為四元數的乘法;q(h)為[tk，tk+1]時間段內的姿態變化四元數。

(9)

式中，Ф為機體坐標系tk時刻和tk+1時刻的等效旋轉矢量;Ф為Ф的模。

(10)

利用式(6)，(10)中對應元素相等，可得姿態角與四元數的關系為

(11)

θ=arcsin(2q0q1+2q2q3)

(12)

(13)

這樣得到了掘進機在大地坐標系下動態的航向角、橫滾角和俯仰角。由于高精度的捷聯慣導價格昂貴，而掘進機行走軌跡及偏差的感知只需利用其航向信息，因此，可研發航向角單一參數測量裝置及其解算模型，大大降低成本，滿足綜掘工作面的需求。

2.3 二維里程計

二維里程計主要由編碼器、測量輪、支架、張緊裝置組成，如圖4所示，安裝在掘進機底部，處于捷聯慣導的正下方，其兩測量輪的對稱線與掘進機縱軸線重合，測量輪與掘進機縱軸線呈45°，如圖5所示。兩測量輪跟隨掘進機的前進轉動，帶動編碼器旋轉，得到測量輪的旋轉圈數，通過計算可得掘進機在單位時間內沿機身縱軸前進方向和橫軸方向的里程增量。二維里程的精準感知有效解決了傳統里程計無法感知掘進機發生側向滑動的難題。

圖4 二維里程計結構Fig.4 Structure diagram of two-dimensional odometer

圖5 二維里程計安裝位置Fig.5 Installation location diagram of two-dimensional odometer

3 行走軌跡及偏差感知系統原理

3.1 系統原理

巷道中線方向角以大地坐標系為基準，而捷聯慣導輸出大地坐標系下的航向角，兩者的差即為掘進機的偏角，如圖6所示。二維里程計通過編碼器可實時感知掘進機在機身縱軸和橫軸2個方向的里程增量。將偏角和里程增量信息結合，利用航位推算算法對二維里程增量進行累加，可得掘進機在巷道坐標系下的行走軌跡，其縱坐標為掘進距離，橫坐標為偏距，并將所得的位置、方向、姿態等信息發送至遠端顯示，為掘進糾偏提供基準參數。在實際工況下，也可以用上述偏角和激光測出的偏距作為輸入值計算掘進糾偏量對巷道兩幫進行修正，實現誤差補償。系統原理如圖7所示。

圖6 巷道坐標系與大地坐標系關系Fig.6 Schematic diagram of the relationship between the road-way coordinate system and the geodetic coordinate system

圖7 系統原理Fig.7 Diagram of system principle

設由二維里程測量裝置所得里程增量分別為ΔSOD1和ΔSOD2，根據圖5中測量裝置與掘進機的位置關系，將兩里程增量合并，可得掘進機機體坐標系二維里程增量矢量ΔSOD為

(14)

將機體坐標系里程增量轉換到巷道坐標系h系:

(15)

將巷道坐標系下的里程增量累加，可得掘進機在巷道中的軌跡坐標:

(16)

式中，Ph0為h系下掘進機的位置坐標，其橫坐標由激光偏距感知系統提供的初始偏距，縱坐標設為0;ΔPh(i-1)掘進機位置坐標增量。

其中，初始位置P0的橫坐標由激光偏距感知系統提供的初始偏距確定，縱坐標設為0。

3.2 系統流程

本系統的流程如下:掘進機行走前，捷聯慣導進行3 min的初始對準，自主感知掘進機的初始航向角;激光偏距感知系統提供初始偏距信息，作為掘進機初始位置的橫坐標，其初始縱坐標記為0。掘進機開始向前行走后，捷聯慣導、二維里程計將測得的原始數據實時傳輸到機載控制器進行位姿參數計算，得到掘進機實時位置坐標，橫坐標為實時偏距，縱坐標為豎直掘進距離，同時可得掘進機的偏角、航向角和姿態角等信息。掘進機行駛至巷道斷面開始掏槽前，通過偏角和偏距等信息判斷掘進機位姿偏差程度，若偏差較大，對掘進機位置、方向進行粗調整后，再進行掏槽;掏槽完成后，根據此時的偏角和偏距調整截割參數以反向補償掘進機位姿偏差，保證巷道精準掘進。圖8為掘進機實時行走軌跡及偏差示意圖。

圖8 掘進機行進軌跡及偏差示意Fig.8 Diagram of roadheader trajectory and deviation

4 實驗驗證

為了驗證所提出的掘進機行走軌跡及偏差感知系統的有效性，并探究其對行走軌跡追蹤和掘進方位偏差感知精度，搭建了行走軌跡及偏差感知精度驗證實驗系統，在模擬煤礦綜掘工作面真實工況的條件下對本文所提系統輸出的偏角、偏距、軌跡坐標精度進行了實驗驗證。

4.1 實驗配置

行走軌跡及偏差實驗驗證系統主要包括激光偏距感知系統、捷聯慣導、二維里程計、機載控制器、井上和井下可視化遠程控制平臺、全站儀等。

(1)激光偏距感知系統。激光偏距感知系統中的激光標靶和扇形激光發射器均為自主研發，如圖9，10所示。其基本性能參數見表1。

圖9 激光標靶Fig.9 Laser target

(2)光纖捷聯慣導。捷聯慣導選用光纖型捷聯慣導，如圖11所示。其基本性能參數見表2。

圖10 扇形激光發射器Fig.10 Fan laser transmitter

表 1 激光偏距感知系統性能參數Table 1 Performance parameters of laser deviation distance perception system

表2 捷聯慣導性能參數Table 2 Performance parameters of strapdown inertial navigation

(3)二維里程測量裝置。二維里程計為自主研發，如圖12所示。其中編碼器選用光電多圈高精度絕對值編碼器，測量輪選用大承重的全向輪，基本參數見表3，4。

表3 編碼器性能參數Table 3 Performance parameters of Encoder

圖12 二維里程計Fig.12 Two-dimensional odometer

(4)全站儀。全站儀作為掘進機位姿參數基準參考測量儀器，基本參數見表5。全站儀和3個棱鏡的布置方式如圖13所示，測量棱鏡 1，2，3 固定在掘進機機身不共線的3個位置上，其中棱鏡1固定在激光標靶中點上部，可用棱鏡1的位置變化代表掘進機的運動。全站儀放置在掘進機正后方，可測得全站儀與機載棱鏡的距離S、水平角β、垂直角γ，并通過式(17)完成棱鏡相對于全站儀的坐標測量工作[23]。

圖13 全站儀及棱鏡布置Fig.13 Layout of total station and prisms

表4 測量輪性能參數Table 4 Performance parameters of measuring wheel

表5 全站儀性能參數Table 5 Performance parameters of total station

(17)

根據以上原理測得3個棱鏡的相對全站儀的坐標，并將它們轉換到巷道坐標系，得到3個棱鏡在巷道坐標系下的坐標分別為(xh1,yh1,zh1),(xh2,yh2,zh2)和(xh3,yh3,zh3)。全站儀測得的基準機身偏角α′可通過式(18)求出;基準軌跡為棱鏡1的水平坐標，其中橫坐標xh1為基準偏距。

(18)

4.2 實驗方案

實驗地點地理位置為北緯37.92°,東經114.52°。以掘進機起點為坐標原點，初始朝向為巷道設計方向，按照一般巷道外形尺寸在地面確定掘進機移動邊界，并建立巷道坐標系。通過井上可視化控制平臺操控掘進機按井下實際工況所需操縱掘進機行走，同時系統傳感器所采集到的原始數據在機載控制器計算后，通過光纖傳輸回井上可視化控制平臺，實時顯示出掘進機行進軌跡及方位偏差值，并將所測數據保存。

掘進機行走過程中，通過全站儀測量掘進機機身上3個棱鏡坐標，以作為掘進機機身位姿基準參考點。全站儀進行坐標測量時掘進機需要短暫停止。人為不定時控制掘進機停止，并記錄相應時刻，可得到一系列掘進機位姿基準參考點。一共進行5組實驗。

4.3 實驗結果

圖14為掘進機實際軌跡與系統測量顯示軌跡對比圖，左邊紅色軌跡為掘進機實際軌跡，右圖為本文所提系統測得軌跡在遠程可視化控制平臺的顯示，從定性的角度來看本文所提系統能夠較好地感知掘進機機身的實時位姿。將以本文所提方法測得的5組位姿數據與相對應基準參考點數據進行比較分析，結果見表6。

圖14 掘進機實際軌跡與系統測量顯示軌跡對比Fig.14 Comparison diagram of the actual trajectory of roadheader and the measured trajectory of the system

表6 測量誤差統計特性Table 6 Statistical characteristics of measurement errors

偏角測量誤差的最大值為0.104°，均值為0.052°，方差為0.100°;偏距(橫坐標)測量誤差的最值為0.084 m，均值為0.048 m,方差為0.077 m;縱坐標測量誤差的最大值為0.099 m，均值為0.055 m,方差為0.068 m。可以得出，本文所提方法的偏差感知精度滿足巷道定向掘進對于掘進機定位定向要求，可使所掘巷道達到國家煤礦井巷工程驗收標準[24-25]。

5 結 論

(1)基于已往研究，在巷道坐標系下描述了偏角和偏距，闡述了掘進機方位偏差與巷道定向掘進之間的關系。

(2)搭建了一種可以自主、動態感知掘進機行走軌跡、偏角和偏距的位姿感知系統，提出了基于激光偏距感知、捷聯慣導、二維里程計的掘進機方位偏差感知方法及相應的自主精準定向掘進策略。

(3)實驗結果表明，所提方法實現了掘進機機身偏角0.052°、偏距(橫坐標)0.048 m、縱坐標0.055 m的感知精度，驗證了所提方法的可行性及優越性，感知精度滿足巷道定向掘進對于掘進機定位定向要求。

(4)基于所提系統開發了遠程可視化操控平臺，實現了掘進機位姿狀態在井上、下的實時顯示。