基于雙側鉆臂位姿協同約束的鑿巖臺車車體定位方法

吳昊駿，龔 敏

(北京科技大學 土木與資源工程學院，北京 100083)

鑿巖臺車鉆孔自動定位是掘進技術發展的重要方向和近年來研究熱點[1-4]。鉆孔自動定位首先需要解決車體定位(臺車與巷道位置關系)問題，車體定位后計算機才能據此確定各鉆臂移動參數和實施動作控制[5-6]，因此具有非常重要的作用。

由于產權保護原因國外車體定位文獻很少，可供借鑒參考的資料有限。李軍利等[7]研究了與臺車類似的掘進機車體定位，通過坐標變換矩陣并基于閉環尺寸鏈進行定位，然而每次掘進前需用全站儀測量的方式較麻煩并影響施工;ELSRUD Rolf[8]介紹了阿特拉斯臺車自動定位RCS控制系統，首次提出利用鉆臂穿過巷道定向激光的車體定位方法，但文獻沒有闡述具體解算過程;對此何清華，謝習華等[9-10]詳細推導了該方法的計算求解步驟，為我國首臺隧道鑿巖機器人研發奠定了重要基礎，一些學者[11-13]據此進行了相關研究，這也是目前普遍認可的定位方法。但這一方法在實際應用時因堆碴高度變化(現場常見情況)誤差很大，采用車體橫向角度傳感器作補償，在現場應用時準確性較差。

為解決上述方法無法適應堆碴高度變化的難題，筆者提出雙光束雙鉆臂車體定位的新方法，從理論上證明了車體位姿解的惟一性，給出了車體定位計算求解過程，并在綠水洞煤礦成功實現井下鉆孔自動定位工業性試驗。

1 雙鉆臂法與車體位姿惟一性證明

1.1 車體定位問題的理論表述

車體定位即確定臺車與巷道位置關系。雙鉆臂臺車如圖1所示，左右鉆臂對稱，各有5個轉動軸和1個移動軸。臺車停在工作面前適當位置，以設計姿態鉆孔，根據臺車移動特點建立兩空間坐標系如圖2所示，坐標系分別為:車體基坐標系{OC}和工作面坐標系{OG}。要求將鉆具軸線向量stick在{OC}下的坐標表達實時轉換到其在{OG}下的坐標表達，直到其與{OG}內炮孔設計向量hole共線，且當stick末端與hole始端重合時，鉆孔自動定位過程結束，可以開始鉆孔。圖2中坐標系{O0L}和{O6L}，{O0R}和{O6R}分別為按照D-H法[14-15]確定的左側鉆臂首端和末端坐標系，右側鉆臂首端和末端坐標系。

圖1 車體結構簡圖Fig.1 Organization graph of drill rig

各工作循環停車位置是隨機的，{OC}和{OG}的關系不固定。鉆孔前應以簡易辦法快速確定本次{OC}和{OG}的變換關系，即車體定位。

1.2 雙鉆臂法與位姿解惟一性證明

針對原定位法存在的問題，本文提出雙鉆臂車體定位方法(以下簡稱雙鉆臂法):利用已有傳感器，增加一路井下定向激光，以臺車雙側鉆臂同時穿過兩束激光進行車體定位。該方法不存在車體雙側定位誤差傳遞問題，且一定程度上可反映工作面左右不平整情況。

下面將分析雙鉆臂法能否得到車體定位的惟一解，是否可解決車體橫向角度變化的問題。

雙鉆臂法原理如圖3所示:提供兩束平行激光，在工作面上的投影OL和OR分別作為左側和右側激光坐標系{OL}，{OR}的原點。定位過程中{OL}的YL軸和{O6L}的Y6L軸形成角變量θL，{OR}的YR軸和{O6R}的Y6R軸形成角變量θR。在θL和θR的值隨機變動的過程中，{OC}的原點OC到激光的距離保持不變(點繞軸轉動，點到軸的距離不變)。圖3中h為OC到履帶底板高度。

圖2 車體定位的數學坐標系表述Fig.2 Mathematical description of carriage positioning

圖3 新車體定位方法示意Fig.3 New carriage positioning method

此時兩側激光之間距離確定，OC到兩側激光的兩個距離不變。則這3個距離將構成一個三邊長度已知的穩定三角形。因此求解θL和θR值的問題轉化為求解OC位置的問題，并進一步轉化為已知三角形兩頂點坐標和三條邊長，求另一頂點坐標的問題。該問題具有惟一解如圖4所示，到OL固定距離的點構成解集A，到OR固定距離的點構成解集B，同時滿足兩固定距離的解惟一確定為A∩B，因此OC，OL，OR構成一穩定三角形。

圖4 真實解的惟一性Fig.4 Uniqueness of true solution

2 雙鉆臂法公式推導

雙鉆臂法計算流程如圖5所示。

圖5 計算流程Fig.5 Process of calculation

以下將根據流程闡述詳細計算過程。

2.1 鉆臂基坐標系和末端坐標系位姿矩陣

以左側鉆臂為例，基于D-H法在各軸建系{OiL}(i=1,2,…,6)如圖6所示，并在激光投影位置建立{OL}，{OC}與{O0L}在車體側視圖內重合。圖6中a1,a2,d3,a4,d5,d6為對應位置鉆臂的桿件尺寸。首先通過相鄰軸坐標系之間的變換關系逐級推導出{O0L}和{O6L}的位姿矩陣。

圖6 左側鉆臂連桿坐標系Fig.6 Linkage coordinate system of left boom

沿各軸方向確定各坐標系Z軸，ZiL為{OiL}的Z軸，Z(i-1)L為{O(i-1)L}的Z軸。{O0L}的X0L軸方向人為設定，其余XiL=Z(i-1)LZiL(i=1,2,…,6)，XiL為{OiL}的X軸，按右手法則確定各坐標系Y軸。

按D-H法位姿矩陣通式Ai確定相鄰兩坐標系(兩軸)間的位姿矩陣:

(1)

式中，ai-1，di為連桿長度參數;αi-1，θi為連桿間角度參數;(xiL,yiL,ziL)為點或向量在坐標系{OiL}下的坐標表達;(x(i-1)L,y(i-1)L,z(i-1)L)為點或向量在坐標系{O(i-1)L}下的坐標表達。

式中，A1L為左側{O0L}和{O1L}的位姿矩陣，…，A6L為左側{O5L}和{O6L}的位姿矩陣。

設{O6L}在{O0L}下的位姿矩陣:

(2)

式中，(nxL,nyL,nzL)，(oxL,oyL,ozL)，(axL,ayL,azL)，(pxL,pyL,pzL)分別為{O6L}的X6L軸，Y6L軸，Z6L軸，原點{O6L}在{O0L}下的坐標表達。

同理，{O6R}在{O0R}下的位姿矩陣:

(3)

2.2 車體基坐標系和激光坐標系位姿矩陣

設{OC}位于{O0L}和{O0R}中間，三者僅有平移變換而無旋轉變換。設平移距離為ε，則{O6L}，{O6R}在{OC}下的位姿矩陣:

(4)

式中，ε為雙側鉆臂間距一半，為正數。

(5)

再考慮與兩側鉆具軸線平行的激光。定位過程中為保證平行關系成立，兩側各特制了兩枚激光靶，確保OL，OR在{O6L}，{O6R}下的X，Y坐標分別為:(εx,εy)，(-εx,εy)。

θR=0，θL=0時，可得到{OC}在{OL}，{OR}下的位姿矩陣:

(6)

設OC到兩束激光距離分別為LL，LR。LL，LR不會隨θL，θR取值變化而變化。由式(6)得:

(7)

2.3 解車體基坐標系原點位置

因{OL}，{OR}的ZL軸，ZR軸與{OG}的ZG軸同向，在不涉及Z坐標時，可認為三坐標系的XOY平面共面。設OL，OR在{OG}的XOY平面內的坐標分別為(UL,VL)，(UR,VR)，利用MATLAB編程求解到OL點距離LL，同時到OR點距離LR的點的坐標(U,V)，作為OC在{OG}的XOY平面內的投影點坐標如圖7所示(舍去V值較大的解)。

圖7 求車體基坐標系原點坐標Fig.7 Find the origin coordinates of the carriage coordinate system

由坐標(U,V)可得車體偏離巷道中線距離E，并根據h計算履帶底板下方堆碴高度H。

(8)

式中，E為車體偏中距;H為履帶底板下方堆碴高度。

2.4 求角變量和車體位姿矩陣

(9)

圖8 求角變量Fig.8 Calculate the rotation angle

(10)

式中，Rot(ZL,θL)表示繞ZL軸旋轉θL所對應矩陣;Rot(ZR,θR)表示繞ZR軸旋轉θR所對應矩陣。

(11)

3 車體定位實例

在實驗廠房內驗證雙鉆臂法。模擬巷道高3.6 m，寬4.8 m。臺車停于工作面前，h=1 256 mm，H=420 mm，穩定后不再移動。分別使用全站儀測量方法和雙鉆臂法計算車體位姿，將2組結果進行對比。

3.1 全站儀測量車體實際位姿

用全站儀測量車體位姿，在車體四周合適位置設四控制點D,G,E,F如圖9所示，線段DG⊥DE且DG⊥GF。DE距巷道中線1 645 mm，與左側激光指向重合，GF距巷道中線1 580 mm，與右側激光指向重合。

圖9 全站儀測量布點Fig.9 Measuring points of total station

在F點架設全站儀，設置向量FG方向為0角度方向。在車體左右兩側各找一點WL，WR，測得兩點坐標為(1.481,2.157,0.969)，(1.490,1.093,0.978)，計算得到E=(2 157+1 093)/2-1 580=45 mm，車體左右翻轉角-0.48°。在車體右側豎直面板上找前后兩點W1，W2，測量得到兩點坐標為(2.710，1.071，0.873)，(1.587，1.061，0.884)，計算得到前后俯仰角-0.56°。

3.2 建立雙鉆臂法的位姿矩陣

使用雙鉆臂法計算車體位姿，根據臺車尺寸確定D-H法參數列于表1，尺寸變量單位為mm，角度變量單位為(°)。表1中di，θi兩列中θ1～θ5以及d6為軸變量，其值由傳感器實時確定。小括號內數據為各軸位于基準位置時變量取值如圖10所示。

表1簡化機構連桿參數表(右側)
Table1Linkparametersofsimplifymechanism(rightside)

連桿iαi-1ai-1diθi取值范圍1000θ1(0)(-47,14)2-901800θ2(90)(35,106)390532 598θ3(180)(70,320)49000θ4(90)(75,180)590136529θ5(90)(55,135)6900d6(1 739)0(1 739,3 039)

圖10 各軸基準位置Fig.10 Reference position of each axis

臺車雙側鉆臂寬2ε=530 mm;激光靶參數εx=60 mm，εy=120 mm。

車體兩側激光:OL位于{OG}左上方(1 645,1 875)位置，OR位于{OG}右上方(-1 580,1 840)位置。移動雙側鉆臂使鉆具軸線與激光平行。完成后，兩側各傳感器讀數為:(-25.840°，-9.624°，0.309°，9.185°，28.330°，1 183.680 mm)，(25.356°，-10.063°，0.369°，9.053°，-28.969°，1 097.002 mm)。

3.3 求車體的位姿矩陣解

MATLAB編程求得OC在工作面上的投影坐標(U,V)=(49.4,1 662.4)，由式(8)得:E=U=49.4 mm，H=V-h=1 662.4-1 256=406.4 mm，與全站儀測量結果吻合。

由式(9)得:θL=3.384°;θR=-1.687°。

根據位姿矩陣求得車體前后俯仰角，左右翻轉角為(-0.76°,-0.43°)。與全站儀測量結果基本一致。

3.4 2種定位方法對定位效果影響的討論

在車體定位參數中，以E和H為例，進一步分析θR，θL變化時，引起的定位參數變化。基準平面設置為H=420 mm，基準偏中距設置為E=45 mm。

以左側為例，首先計算θL取值不同造成車體位置計算結果與實際結果的差異如圖11所示。

圖11 角變量和車體位置參數關系Fig.11 Relationship between angle variable and body position parameter

橫坐標為θL，θL在0±10°內變化。圖11中位置較高曲線表示θL與H的關系;位置較低的表示θL與E的關系。

以H=420 mm為基準面，θL=-10°時對應H=782 mm，二者相差86%;θL=10°時對應H=224 mm，二者相差47%;即使按照θL=0°計算該比例也達到19%。采用雙鉆臂法計算出θL=3.384°，誤差比例為3.3%。

全站儀測得E=45 mm，θL=10°時對應E=85 mm，二者相差88%;而采用雙鉆臂法計算出θL=3.384°，誤差比例為9.7%。

可見θL對車體位置計算結果的影響很大。其中H在后續計算中更為重要。通過雙鉆臂法將該值誤差控制在3.3%，較為可靠。

下面討論θL變化造成車體定位誤差對鉆孔定位的影響。假設右側激光所指位置存在一待鉆目標孔。車體精確定位后，按照右側各傳感器讀數移動右側鉆臂，鉆具末端仍能定位到該位置。當θL取不同值，系統仍按傳感器讀數進行鉆孔定位后，該炮孔實際定位結果與設計孔位的偏離量如圖12所示。

圖12 角變量和炮孔位置關系Fig.12 Relationship between angle variable and position of hole

橫坐標仍為θL，縱坐標為炮孔定位位置與設計位置距離。θL=-10°時，鉆孔定位誤差達到75 cm。為控制鉆孔定位精度在5 cm以內，θL計算誤差應在1.5°內，這在井下清碴很難做到，因此不考慮車體橫向轉角將對最終鉆孔定位精度影響較大，采用雙鉆臂法則可避免這種情況。

對比采用①全站儀+原方法(左側定位)和②雙鉆臂法時的鉆孔定位精度(均不進行定位誤差補償)如圖13所示。圖中2條黑線為①法左右兩側鉆孔定位誤差結果，紅線為②法左右兩側結果，實線為左側炮孔，虛線為右側炮孔，因巷道中線上有3孔由左側鉆臂完成，歸為左側孔，故圖中實線較長。橫坐標代表孔序。

圖13 2種方法鉆孔定位結果對比Fig.13 Comparison of the results of hole positioning between two methods

由圖13可知:原方法兩側炮孔誤差明顯不同，對側右側(虛線)誤差比定位側左側(實線)誤差大，前者集中在12～20 cm，平均誤差16.7 cm，后者集中在0～12 cm，平均誤差9.3 cm。而雙鉆臂法任一側誤差均與原方法定位側誤差精度一致，平均誤差8.6 cm。

原方法對對側炮孔存在誤差傳遞并放大的問題。全站儀測量準確的情況下，原方法定位側炮孔誤差精度與雙鉆臂法雙側炮孔誤差精度一致。未補償前精度均在0～12 cm內。原方法對側平均誤差比定位側及雙鉆臂法任一側平均誤差高94%，補償難度大。

4 結 論

(1)針對原有車體定位方法在工作面堆碴高度變化時定位誤差很大甚至無法使用的問題，提出了雙鉆臂車體定位法，通過數學推導證明了解的惟一性，這一方法已在井下工業性試驗得到成功驗證，具有普遍性，可用于同類型鑿巖臺車車體定位。

(2)車體繞激光橫向旋轉的角變量是求解車體位置參數的關鍵因素。當橫向偏轉角為10°時，用過去定位法的車體豎向位置誤差達86%。雙鉆臂法通過準確求解定位過程中的角度值，將豎方向位置誤差控制在5%以內，準確度較高。

(3)采用原有方法，為將精度控制在5 cm以內需將車體橫向角度控制1.5°以下，這對井下清碴要求很高。雙鉆臂法不僅從理論上對堆碴高度沒有限制，而且將隨機變化的堆碴條件轉變為求解對象，從定位原理上避免了過去方法的缺陷。

(4)單鉆臂定位存在誤差向非定位側鉆臂傳遞的現象，非定位側炮孔平均定位誤差較定位側高出94%。雙鉆臂法定位則不存在這一問題，工作面左右兩側鉆孔定位精度均在12 cm內，有利于全斷面炮孔高效率補償。