基于雙側(cè)鉆臂位姿協(xié)同約束的鑿巖臺車車體定位方法

吳昊駿，龔 敏

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

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

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

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

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

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

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

圖1 車體結(jié)構(gòu)簡圖Fig.1 Organization graph of drill rig

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

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

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

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

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

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

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

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

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

2 雙鉆臂法公式推導(dǎo)

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

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

以下將根據(jù)流程闡述詳細計算過程。

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

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

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

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

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

(1)

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

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

設(shè){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 車體基坐標系和激光坐標系位姿矩陣

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

(4)

式中，ε為雙側(cè)鉆臂間距一半，為正數(shù)。

(5)

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

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

(6)

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

(7)

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

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

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

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

(8)

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

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

(9)

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

(10)

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

(11)

3 車體定位實例

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

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

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

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

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

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

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

表1簡化機構(gòu)連桿參數(shù)表(右側(cè))
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

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

車體兩側(cè)激光:OL位于{OG}左上方(1 645,1 875)位置，OR位于{OG}右上方(-1 580,1 840)位置。移動雙側(cè)鉆臂使鉆具軸線與激光平行。完成后，兩側(cè)各傳感器讀數(shù)為:(-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，與全站儀測量結(jié)果吻合。

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

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

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

在車體定位參數(shù)中，以E和H為例，進一步分析θR，θL變化時，引起的定位參數(shù)變化?；鶞势矫嬖O(shè)置為H=420 mm，基準偏中距設(shè)置為E=45 mm。

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

圖11 角變量和車體位置參數(shù)關(guān)系Fig.11 Relationship between angle variable and body position parameter

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

(3)采用原有方法，為將精度控制在5 cm以內(nèi)需將車體橫向角度控制1.5°以下，這對井下清碴要求很高。雙鉆臂法不僅從理論上對堆碴高度沒有限制，而且將隨機變化的堆碴條件轉(zhuǎn)變?yōu)榍蠼鈱ο螅瑥亩ㄎ辉砩媳苊饬诉^去方法的缺陷。

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