吳昊駿，龔 敏

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖4 真實(shí)解的惟一性Fig.4 Uniqueness of true solution

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

雙鉆臂法計(jì)算流程如圖5所示。

圖5 計(jì)算流程Fig.5 Process of calculation

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

2.1 鉆臂基坐標(biāo)系和末端坐標(biāo)系位姿矩陣

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

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

沿各軸方向確定各坐標(biāo)系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軸，按右手法則確定各坐標(biāo)系Y軸。

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

(1)

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

式中，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軸，原點(diǎn){O6L}在{O0L}下的坐標(biāo)表達(dá)。

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

(3)

2.2 車體基坐標(biāo)系和激光坐標(biāo)系位姿矩陣

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

(4)

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

(5)

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

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

(6)

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

(7)

2.3 解車體基坐標(biāo)系原點(diǎn)位置

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

圖7 求車體基坐標(biāo)系原點(diǎn)坐標(biāo)Fig.7 Find the origin coordinates of the carriage coordinate system

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

(8)

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

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

(9)

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

(10)

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

(11)

3 車體定位實(shí)例

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

3.1 全站儀測量車體實(shí)際位姿

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

圖9 全站儀測量布點(diǎn)Fig.9 Measuring points of total station

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

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

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

表1簡化機(jī)構(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 各軸基準(zhǔn)位置Fig.10 Reference position of each axis

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

車體兩側(cè)激光:OL位于{OG}左上方(1 645,1 875)位置，OR位于{OG}右上方(-1 580,1 840)位置。移動(dòng)雙側(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在工作面上的投影坐標(biāo)(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種定位方法對(duì)定位效果影響的討論

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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