張付祥，　史文軍

(河北科技大學(xué) 機(jī)械工程學(xué)院， 河北 石家莊　050018)

?

雙臂掘進(jìn)鉆車自動(dòng)化鉆孔方法研究

張付祥，史文軍

(河北科技大學(xué) 機(jī)械工程學(xué)院， 河北 石家莊050018)

在對(duì)雙臂掘進(jìn)鉆車進(jìn)行運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，分析了鉆車的作業(yè)能力和理想位姿，研究了雙臂掘進(jìn)鉆車的鉆臂之間及鉆臂與巷道之間的碰撞檢測(cè)方法，提出了一種雙臂掘進(jìn)鉆車自動(dòng)化鉆孔方法。在Matlab環(huán)境下對(duì)該方法進(jìn)行了仿真驗(yàn)證，結(jié)果表明該方法能夠使鉆臂到達(dá)鉆孔位姿，實(shí)現(xiàn)自動(dòng)化鉆孔，為進(jìn)一步研究雙臂掘進(jìn)鉆車鉆臂的運(yùn)動(dòng)規(guī)劃及自動(dòng)化鉆孔奠定了基礎(chǔ)。

雙臂掘進(jìn)鉆車； 作業(yè)能力分析； 理想位姿； 碰撞檢測(cè)； 自動(dòng)化鉆孔

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/32.1627.TP.20160601.1028.011.html

0　引言

中國(guó)煤礦瓦斯爆炸事故頻發(fā)，死亡人數(shù)眾多，瓦斯事故已成為煤礦安全生產(chǎn)的最大威脅[1]。如何實(shí)現(xiàn)煤礦高效、高產(chǎn)和安全已成為煤礦進(jìn)一步發(fā)展的關(guān)鍵和核心[2]。高瓦斯礦井瓦斯防治管理的最大難題是煤層瓦斯的有效抽放[3]，而釋放瓦斯的有效途徑是鉆孔[4]。計(jì)算機(jī)控制的自動(dòng)化鉆孔和鉆爆精度等問(wèn)題的有效解決可提高瓦斯防治管理的安全性和效率。

本文針對(duì)CMJ2-27改進(jìn)型全液壓鉆井鉆車的鉆臂機(jī)械結(jié)構(gòu)進(jìn)行運(yùn)動(dòng)學(xué)分析，并對(duì)鉆車的作業(yè)能力與理想位姿進(jìn)行分析，提出鉆車自動(dòng)碰撞檢測(cè)方法，對(duì)目前常用的拱形巷道爆破方案進(jìn)行鉆孔規(guī)劃，為實(shí)現(xiàn)鉆車自動(dòng)化鉆孔奠定了技術(shù)基礎(chǔ)。

1　鉆車鉆臂機(jī)械結(jié)構(gòu)及運(yùn)動(dòng)學(xué)分析

CMJ2-27改進(jìn)型全液壓鉆井鉆車在作業(yè)過(guò)程中，所有動(dòng)作都由操作人員在駕駛室手工機(jī)械操作，效率低，鉆孔一致性較差。為了解決這些問(wèn)題，對(duì)鉆車進(jìn)行改進(jìn)，使其具備數(shù)控功能。鉆車鉆臂結(jié)構(gòu)如圖1所示。

1-鉆桿； 2-滑架； 3-推進(jìn)器； 4-擺角油缸； 5-俯仰油缸；6-回轉(zhuǎn)臂； 7-升降油缸; 8-整臂擺動(dòng)油缸

鉆車為雙臂操作，且2個(gè)機(jī)械臂的結(jié)構(gòu)基本一致，每個(gè)機(jī)械臂有5個(gè)轉(zhuǎn)動(dòng)自由度、1個(gè)移動(dòng)自由度。在2個(gè)機(jī)械臂的各個(gè)關(guān)節(jié)上分別建立關(guān)節(jié)坐標(biāo)系。關(guān)節(jié)坐標(biāo)系建立規(guī)則：每個(gè)機(jī)械臂按關(guān)節(jié)和鉆車車體物理連接的先后次序在各個(gè)關(guān)節(jié)上依次建立關(guān)節(jié)坐標(biāo)系O1X1Y1Z1，O2X2Y2Z2，O3X3Y3Z3，O4X4Y4Z4，O5X5Y5Z5，O6X6Y6Z6，其中第6個(gè)自由度為鉆桿的移動(dòng)，坐標(biāo)系O6X6Y6Z6為建立在鉆桿末端的鉆桿坐標(biāo)系。

采用傳統(tǒng)的D-H (Denavit-Hartenbergma-trix)法對(duì)鉆車鉆臂進(jìn)行正向運(yùn)動(dòng)學(xué)分析，即通過(guò)已知的機(jī)械臂關(guān)節(jié)坐標(biāo)求取鉆桿坐標(biāo)系的位姿。鉆臂的每個(gè)連桿上都固定一個(gè)坐標(biāo)系，用4×4的齊次變換矩陣來(lái)描述相鄰2個(gè)連桿的空間關(guān)系[5]。通過(guò)依次變換可最終推導(dǎo)出鉆桿相對(duì)于基坐標(biāo)系的位姿，建立鉆臂的運(yùn)動(dòng)學(xué)方程。

逆運(yùn)動(dòng)學(xué)問(wèn)題是已知鉆桿坐標(biāo)系的位姿，求相應(yīng)的關(guān)節(jié)坐標(biāo)。運(yùn)動(dòng)學(xué)逆解解法主要有解析法和代數(shù)法。代數(shù)法工作量大，往往需要直接觀察和經(jīng)驗(yàn)[6]，而解析法可避免這個(gè)問(wèn)題，因此采用解析法中的分離變量法求解析解。分離變量法首先利用運(yùn)動(dòng)方程的不同形式，找出矩陣中簡(jiǎn)單表達(dá)某個(gè)未知數(shù)的元素，力求得到未知數(shù)較少的方程，然后分離出變量，逐個(gè)求解。

雙臂掘進(jìn)鉆車的運(yùn)動(dòng)學(xué)分析目的是求解鉆車鉆桿期望位姿對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)，手部位姿為6個(gè)自由度，需要6軸聯(lián)動(dòng)。鉆孔作業(yè)采用計(jì)算機(jī)控制，使其能夠根據(jù)作業(yè)點(diǎn)坐標(biāo)通過(guò)逆運(yùn)動(dòng)學(xué)求解關(guān)節(jié)運(yùn)動(dòng)控制參數(shù)，從而實(shí)現(xiàn)鉆臂的全方位數(shù)字控制作業(yè)。

2　鉆車作業(yè)能力與理想位姿分析

鉆車鉆臂的空間分析與機(jī)器人的空間分析類似[7]。目前大多采用蒙特卡羅法求解機(jī)器人工作空間，其為利用隨機(jī)抽樣來(lái)求解數(shù)學(xué)問(wèn)題的一種數(shù)值方法。

實(shí)際工作中，最為理想的狀態(tài)是理想鉆車位姿時(shí)鉆孔，即末端姿態(tài)為鉆臂垂直斷面。利用Matlab可求得鉆車距離斷面1 010 mm時(shí)鉆孔效果最佳，然后根據(jù)鉆臂垂直斷面的邊界條件和最佳距離條件得出理想狀態(tài)工作空間，如圖2所示，(px，py)為用戶坐標(biāo)系上鉆孔的坐標(biāo)。

圖2　雙臂掘進(jìn)鉆車?yán)硐霠顟B(tài)工作空間

由圖2可知，理想鉆車位姿鉆孔時(shí)，內(nèi)部包含一些空心區(qū)域，即有部分孔位置鉆臂達(dá)不到。另外，鉆車2個(gè)鉆臂的結(jié)構(gòu)完全相同，工作過(guò)程中有相互重疊的區(qū)域，在同時(shí)移位過(guò)程中鉆臂之間可能產(chǎn)生相互碰撞。因此，鉆車工作時(shí)要考慮鉆車位姿調(diào)整。

3　鉆車鉆臂碰撞檢測(cè)方法

在實(shí)際工作中，由于雙臂掘進(jìn)鉆車的串聯(lián)式機(jī)械臂處于動(dòng)態(tài)的、不確定的非結(jié)構(gòu)性環(huán)境中，碰撞現(xiàn)象可能經(jīng)常發(fā)生。如何判斷和避免串聯(lián)式機(jī)械臂的碰撞，對(duì)于雙臂掘進(jìn)鉆車的實(shí)際應(yīng)用非常重要。

鉆臂碰撞檢測(cè)方法：① 針對(duì)鉆車的工作情況進(jìn)行碰撞檢測(cè)理論分析，利用機(jī)器人運(yùn)動(dòng)學(xué)理論，推導(dǎo)出各個(gè)關(guān)節(jié)中心點(diǎn)的坐標(biāo)表達(dá)式，求得任意2個(gè)連桿之間的最小距離，采用包絡(luò)半徑法對(duì)2個(gè)連桿之間的最小距離與2個(gè)連桿的包絡(luò)半徑之和進(jìn)行比較，得到碰撞檢測(cè)方程；② 針對(duì)單臂作業(yè)情況進(jìn)行分析，利用Matlab工具，結(jié)合蒙特卡羅法獲取單機(jī)械臂作業(yè)可能發(fā)生碰撞的情況；③ 針對(duì)雙臂同時(shí)工作的情況進(jìn)行分析，利用Matlab工具，結(jié)合蒙特卡羅法獲取雙臂同時(shí)作業(yè)可能發(fā)生碰撞的情況；④ 針對(duì)鉆車2個(gè)機(jī)械臂鉆桿的目標(biāo)位姿，利用Matlab工具及鉆車的解析逆解表達(dá)式獲取2個(gè)機(jī)械臂的各個(gè)關(guān)節(jié)變量；⑤ 單臂作業(yè)時(shí)，針對(duì)蒙特卡羅法檢測(cè)出的單機(jī)械臂各連桿之間可能發(fā)生的碰撞情況進(jìn)行單臂碰撞檢測(cè)，雙臂同時(shí)作業(yè)時(shí)，針對(duì)蒙特卡羅法檢測(cè)出的2個(gè)機(jī)械臂各連桿之間可能發(fā)生的碰撞情況進(jìn)行雙臂碰撞檢測(cè)。

在鉆車實(shí)際工作中，鉆臂與巷道也可能發(fā)生碰撞，加劇鉆臂的磨損，需盡量避免。鉆臂與巷道的碰撞檢測(cè)方法：采用兩端具有半徑為r的半球的圓柱體將連桿包絡(luò)起來(lái)[8]，當(dāng)鉆車鉆臂工作時(shí)，如果圓柱體兩端球心位置(即連桿關(guān)節(jié)坐標(biāo)原點(diǎn))不與巷道碰撞，則整個(gè)圓柱體就不會(huì)與巷道發(fā)生碰撞，即鉆臂連桿與巷道之間不發(fā)生碰撞。因此只要檢測(cè)關(guān)節(jié)坐標(biāo)原點(diǎn)是否與巷道發(fā)生碰撞即可。

通過(guò)分析，可將關(guān)節(jié)坐標(biāo)原點(diǎn)是否與巷道發(fā)生碰撞的問(wèn)題轉(zhuǎn)換為求關(guān)節(jié)坐標(biāo)原點(diǎn)到巷道的最小距離問(wèn)題：如果關(guān)節(jié)坐標(biāo)原點(diǎn)到巷道的最小距離大于包絡(luò)連桿的半球半徑r，則鉆臂與巷道之間不發(fā)生碰撞，否則發(fā)生碰撞。

在鉆車實(shí)際工作中，還要考慮實(shí)時(shí)碰撞檢測(cè)問(wèn)題，需給出一個(gè)安全范圍δ(鉆臂實(shí)際工作時(shí)與巷道壁的最小距離)。本文采用煤礦作業(yè)時(shí)最常用的拱形巷道，巷道作業(yè)面寬度為W，高度為H，如圖3所示。鉆孔規(guī)劃碰撞檢測(cè)時(shí)δ=0，實(shí)時(shí)碰撞檢測(cè)時(shí)δ≠0。

圖3　巷道作業(yè)面結(jié)構(gòu)

4　鉆車自動(dòng)化鉆孔規(guī)劃方法

已知鉆桿坐標(biāo)系O6X6Y6Z6、用戶坐標(biāo)系OuXuYuZu，根據(jù)鉆孔工藝可確定各個(gè)鉆孔在用戶坐標(biāo)系上的坐標(biāo)(px，py)，而對(duì)于各孔的姿態(tài)確定，理論上需要滿足關(guān)系：

(1)

式中：nx，ny，nz分別為X6在Xu，Yu，Zu軸上的投影；ox，oy，oz分別為Y6在Xu，Yu，Zu軸上的投影；ax，ay，az分別為Z6在Xu，Yu，Zu軸上的投影。

針對(duì)煤礦常見(jiàn)的拱形巷道，在作業(yè)面上建立用戶坐標(biāo)系，如圖4所示。Ou在底部直線的中點(diǎn)，Xu在底部向右為正，Yu在作業(yè)面的左右對(duì)稱線上向上為正，Zu垂直于工作面向外為正。確定鉆臂目標(biāo)姿態(tài)前需要先找?guī)讉€(gè)特征孔(鉆車初始位，即鉆桿水平的理想位姿時(shí)，鉆頭正對(duì)工作面的2個(gè)孔；最上面的1個(gè)孔，最下面的3個(gè)孔；圓弧左右側(cè)45°處2個(gè)孔)。然后將所有孔分為16個(gè)區(qū)域，其中Yu軸左側(cè)的孔由左側(cè)鉆臂鉆孔，Yu軸右側(cè)的孔由右側(cè)鉆臂鉆孔，Yu軸上的孔可根據(jù)情況選擇由左側(cè)或右側(cè)鉆臂鉆孔。鉆孔方案如圖4所示。對(duì)這16個(gè)區(qū)域分別設(shè)定鉆桿末端的目標(biāo)位姿，見(jiàn)表1。

圖4　鉆孔方案

5　計(jì)算結(jié)果與分析

利用某礦用88孔方案驗(yàn)證理論分析結(jié)果，以其中1個(gè)孔為例進(jìn)行說(shuō)明。

首先根據(jù)特定的鉆孔方案獲取鉆孔數(shù)據(jù)，即作業(yè)孔的用戶坐標(biāo)。取其中1個(gè)孔，假定作業(yè)孔在用戶坐標(biāo)系中的位姿為

T中第1列數(shù)據(jù)為作業(yè)孔X6軸姿態(tài)，第2列數(shù)據(jù)為作業(yè)孔Y6軸姿態(tài)，第3列數(shù)據(jù)為作業(yè)孔Z6軸姿態(tài)，第4列數(shù)據(jù)為鉆孔的用戶坐標(biāo)系坐標(biāo)。

假定鉆車位姿矩陣為

表1　鉆桿末端的目標(biāo)位姿

將數(shù)據(jù)輸入程序，得關(guān)節(jié)坐標(biāo)θ1=-15.057 0，θ2=-95.902 0，θ3=-137.677 2，θ4=79.780 0，θ5=-97.213 1，d6=-1 984.1。

通過(guò)程序計(jì)算出作業(yè)孔的關(guān)節(jié)坐標(biāo)后，即可通過(guò)控制系統(tǒng)控制關(guān)節(jié)運(yùn)動(dòng)，使鉆臂到達(dá)鉆孔位姿，實(shí)現(xiàn)自動(dòng)化鉆孔。

6　結(jié)語(yǔ)

對(duì)CMJ2-27改進(jìn)型全液壓鉆井鉆車進(jìn)行作業(yè)能力分析、碰撞檢測(cè)和鉆孔規(guī)劃，得到了自動(dòng)化鉆孔方法，為進(jìn)一步研究雙臂掘進(jìn)鉆車鉆臂的軌跡規(guī)劃奠定了理論基礎(chǔ)，為實(shí)現(xiàn)CMJ2-27改進(jìn)型鉆車自動(dòng)化鉆孔奠定了技術(shù)基礎(chǔ)。

[1]代勇.煤礦用液壓鉆車鉆臂系統(tǒng)的研究[D].洛陽(yáng)：河南科技大學(xué), 2012.

[2]HAN S M, HWANG Y K, WOO N S, et al. Solid-liquid hydrodynamics in a slim hole drilling annulus [J].Journal of Petroleum Science and Engineering, 2010, 70(3/4): 308-319.

[3]柏思忠.瓦斯抽放管道設(shè)備ia等級(jí)防爆型式探討[J].工礦自動(dòng)化,2015,41(5)：55-57.

[4]史建.露天潛孔鉆機(jī)鉆孔自動(dòng)定位研究[D].長(zhǎng)沙：中南大學(xué),2014.

[5]蘇學(xué)滿,孫麗麗,楊明,等.基于matlab的六自由度機(jī)器人運(yùn)動(dòng)特性分析[J].機(jī)械設(shè)計(jì)與制造,2013(1):78-80.

[6]賀昱曜.機(jī)器人PUMA560逆運(yùn)動(dòng)方程的新解法[J].機(jī)器人,1989,10(3):19-26.

[7]馬小蒙,沈惠平,黃濤,等.冗余擬人雙臂機(jī)器人的運(yùn)動(dòng)學(xué)及工作空間分析[J].機(jī)械科學(xué)與技術(shù),2014(11):25-29.

[8]陳靖波,趙猛,張珩.空間機(jī)械臂在線實(shí)時(shí)避障路徑規(guī)劃研究[J].控制工程,2007，14(4):445-447.

[9]姚寧平，張杰，李泉新，等.煤礦井下定向鉆孔軌跡設(shè)計(jì)與控制技術(shù)[J].煤炭科學(xué)技術(shù),2013,41(3):7-11.

Research on an automatic drilling method for dual-arm drill rig

ZHANG Fuxiang,SHI Wenjun

(School of Mechanical Engineering, Hebei University of Science and Technology,Shijiazhuang 050018, China)

Based on kinematics analysis of dual-arm drill rig, operation ability and ideal position of dual-arm drill rig were analyzed, collision detection methods of dual-arm drill rig between the arms and between the arms and roadway were studied, and an automatic drilling method for dual-arim drill rig was put forward. A simulation experiment was carried out in Matlab. The experimental result shows that the method can make the arms of dirll rig arrive the ideal position and realize automatic drilling, which lays a foundation for further research on trajectory planning of the arms of dual-arm drill rig and automatic drilling.

dual-arm drill rig; operation ability analysis; ideal position; collision detection; automatic drilling

1671-251X(2016)06-0042-04

10.13272/j.issn.1671-251x.2016.06.011

2015-12-31；

2016-04-17；責(zé)任編輯：李明。

河北省科技攻關(guān)計(jì)劃資助項(xiàng)目(14211902D)；河北省研究生創(chuàng)新資助項(xiàng)目。

張付祥(1973-)，男，河北青縣人，副教授，博士，主要研究方向?yàn)闄C(jī)電裝備與機(jī)器人，E-mail:zhangfx@hebust.edu.cn。

TD421

A網(wǎng)絡(luò)出版時(shí)間：2016-06-01 10:28

張付祥，史文軍.雙臂掘進(jìn)鉆車自動(dòng)化鉆孔方法研究[J].工礦自動(dòng)化，2016,42(6)：42-45.