考慮變幅機(jī)構(gòu)影響的鉆機(jī)取桿機(jī)構(gòu)軌跡規(guī)劃

高志偉 ，李群明～3 ，周 圍 ，謝 帥

（1.中南大學(xué) 機(jī)電工程學(xué)院，長沙 410083；2.中南大學(xué) 輕合金研究院，長沙 410083；3.中南大學(xué) 高性能復(fù)雜制造國家重點(diǎn)實(shí)驗(yàn)室，長沙 410083）

0 引言

在鉆探過程中，由于受到鉆桿長度的限制，需要人工加裝和拆卸鉆桿，這在很大程度上延長了鉆孔作業(yè)時(shí)間，降低了施工效率[1]。因此，實(shí)現(xiàn)加裝和拆卸鉆桿的取桿機(jī)構(gòu)自動(dòng)化，可以降低工作人員的勞動(dòng)強(qiáng)度、提高工作效率、保障施工人員安全。目前，王存洋、劉銀等人針對(duì)水平鉆孔的鉆機(jī)研究了一種能夠攜帶6根鉆桿的鉆桿庫和取桿機(jī)械手[1]，沙永柏等人針對(duì)垂直鉆孔的地面科學(xué)鉆機(jī)設(shè)計(jì)了鉆桿自動(dòng)傳送系統(tǒng)，包含一個(gè)五自由度取桿機(jī)械手[2]，羅華針對(duì)水平和傾斜鉆孔的鉆機(jī)設(shè)計(jì)了一種六自由度取桿機(jī)構(gòu)[3]，阿特拉斯·科普柯Explorac100反循環(huán)鉆機(jī)配置的取桿機(jī)構(gòu)可以實(shí)現(xiàn)將鉆桿夾取到垂直或傾斜位置桅桿處，但存在夾取鉆桿時(shí)需要人工輔助定位和工作空間較小的缺點(diǎn)[4]。隨著鉆探技術(shù)的發(fā)展，實(shí)現(xiàn)鉆進(jìn)的自動(dòng)化已成為現(xiàn)代鉆機(jī)設(shè)計(jì)的主要標(biāo)志之一[5]，實(shí)現(xiàn)取桿機(jī)構(gòu)的自動(dòng)化是實(shí)現(xiàn)鉆機(jī)自動(dòng)化的關(guān)鍵技術(shù)之一。為使取桿機(jī)構(gòu)能夠高效、穩(wěn)定的自動(dòng)加裝和拆卸鉆桿，需要對(duì)其運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。胡小平等人采用五次多項(xiàng)式和Neewton相結(jié)合的方法對(duì)多工位多節(jié)點(diǎn)的作業(yè)任務(wù)進(jìn)行軌跡規(guī)劃[6]，韓江等人利用高次多項(xiàng)式與B樣條混合插值方法對(duì)機(jī)器人關(guān)節(jié)軌跡規(guī)劃，使機(jī)器人運(yùn)行更加平穩(wěn)[7]，K.Petrinec等人采用五次多項(xiàng)式和四次多項(xiàng)式結(jié)合的方法，確保了機(jī)器人速度、加速度的連續(xù)性[8]，郝雪弟等人利用五次多項(xiàng)式插值法對(duì)運(yùn)動(dòng)路徑規(guī)劃，得到各關(guān)節(jié)的運(yùn)動(dòng)軌跡方程[9]，張蕾等人利用七次多項(xiàng)式插值法對(duì)運(yùn)動(dòng)路徑進(jìn)行規(guī)劃，得到速度、加速度曲線沒有突變[10]，林建雄等人分別利用九次多項(xiàng)式和三角函數(shù)對(duì)軌跡規(guī)劃，得到的加加速度連續(xù)且有界[11]。

本文主要針對(duì)工程鉆機(jī)鉆探過程中工作范圍小和加裝鉆桿自動(dòng)化的需求，提出一種結(jié)合桅桿帶有變幅機(jī)構(gòu)的鉆機(jī)取桿機(jī)構(gòu)，實(shí)現(xiàn)自動(dòng)化加裝鉆桿的工作和不改變鉆機(jī)位置就能實(shí)現(xiàn)鉆探不同位置的需求，增加了鉆機(jī)的工作范圍。在保證取桿機(jī)構(gòu)末端手抓準(zhǔn)確到達(dá)抓桿點(diǎn)的情況下，對(duì)變幅機(jī)構(gòu)各油缸行程區(qū)間進(jìn)行分析，確定變幅機(jī)構(gòu)各油缸行程的適用范圍；確定取桿機(jī)構(gòu)各個(gè)關(guān)節(jié)運(yùn)動(dòng)順序，避免取桿機(jī)構(gòu)運(yùn)動(dòng)過程中與送桿機(jī)構(gòu)和動(dòng)力頭相碰撞，對(duì)各個(gè)關(guān)節(jié)運(yùn)動(dòng)進(jìn)行規(guī)劃，并得到末端手抓運(yùn)動(dòng)軌跡光滑、連續(xù)。

1 鉆機(jī)取桿機(jī)構(gòu)工作原理：

取桿機(jī)構(gòu)7安裝在桅桿1一側(cè)（桅桿與送桿機(jī)構(gòu)之間），如圖1所示。鉆機(jī)在工作時(shí)，經(jīng)履帶底盤5運(yùn)動(dòng)到工作位置，由變幅機(jī)構(gòu)8調(diào)節(jié)桅桿傾角和位置，使動(dòng)力頭2中心線對(duì)準(zhǔn)孔口后固定不動(dòng)。在自動(dòng)控制模式下，送桿機(jī)構(gòu)6依此將鉆桿箱中的鉆桿輸送到固定位置給予取桿機(jī)構(gòu)7的末端手抓抓取，經(jīng)取桿機(jī)構(gòu)各關(guān)節(jié)先后順序運(yùn)動(dòng)將鉆桿輸送到雙夾持器4內(nèi)夾緊。

圖1 鉆機(jī)主要組成部分布置示意圖

圖2 變幅機(jī)構(gòu)結(jié)構(gòu)圖

變幅機(jī)構(gòu)主要由機(jī)架連接板4、連桿5、連桿7、連桿連接板8、桅桿滑移導(dǎo)軌11等主要結(jié)構(gòu)組成，如圖2所示。通過連桿油缸6、變幅油缸9、滑移油缸10的伸縮，伸縮連桿油缸6可以對(duì)桅桿上下微調(diào)和左右微調(diào)，伸縮變幅油缸9可以調(diào)節(jié)桅桿的傾角大小，伸縮滑移油缸10可以對(duì)桅桿上下位置進(jìn)行調(diào)節(jié)。

鉆機(jī)取桿機(jī)構(gòu)結(jié)構(gòu)如圖3所示，取桿機(jī)構(gòu)通過上底座2和下底座3安裝在桅桿上。在擺動(dòng)油缸1的作用下，取桿機(jī)構(gòu)整體繞擺動(dòng)油缸1中心旋轉(zhuǎn)；液壓馬達(dá)9通過鏈輪鏈條機(jī)構(gòu)帶動(dòng)末端手抓上下移動(dòng)；旋轉(zhuǎn)擺動(dòng)油缸8控制左右末端手抓中心線與鉆桿或雙夾持器平行，方便抓取和夾持器夾持鉆桿；旋轉(zhuǎn)擺動(dòng)油缸7控制左右末端手抓的開口方向；伸縮油缸6調(diào)節(jié)末端手抓的抓桿點(diǎn)；左右末端手抓夾持鉆桿兩個(gè)不同位置，保證鉆桿移動(dòng)過程中平穩(wěn)。

根據(jù)上述分析，該取桿機(jī)構(gòu)共有五個(gè)自由度，三個(gè)旋轉(zhuǎn)自由度，兩個(gè)移動(dòng)自由度，分別由擺動(dòng)油缸1、擺動(dòng)油缸7、擺動(dòng)油缸8、液壓馬達(dá)9、伸縮油缸6驅(qū)動(dòng)。

圖3 鉆機(jī)取桿機(jī)構(gòu)結(jié)構(gòu)圖

2 鉆機(jī)取桿機(jī)構(gòu)運(yùn)動(dòng)學(xué)建模

變幅機(jī)構(gòu)各油缸伸長量變化，導(dǎo)致桅桿相對(duì)于機(jī)架的空間位置變化，進(jìn)而影響取桿機(jī)構(gòu)上下底座在空間中的位置。為得到取桿機(jī)構(gòu)末端手抓相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型。首先，以取桿機(jī)構(gòu)上底座為研究對(duì)象，建立上底座坐標(biāo)系相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型；其次，建立取桿機(jī)構(gòu)末端手抓中心坐標(biāo)系相對(duì)于上底座坐標(biāo)系的運(yùn)動(dòng)學(xué)模型；最后，得到取桿機(jī)構(gòu)末端手抓相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型。

根據(jù)D-H法建立坐標(biāo)系的原則[12～14]，對(duì)坐標(biāo)系的建立做如下規(guī)定：以變幅機(jī)構(gòu)連桿6和機(jī)架連接板3在桅桿對(duì)稱中心面上投影的交點(diǎn)作為基坐標(biāo)系O'0x'0y'0z'0，取桿機(jī)構(gòu)上底座2與擺動(dòng)油缸1軸線在上底座2上下對(duì)稱面上投影的交點(diǎn)作為取桿機(jī)構(gòu)上底座坐標(biāo)系O0x0y0z0，取桿機(jī)構(gòu)末端手抓中心坐標(biāo)系為O5x5y5z5，取桿機(jī)構(gòu)末端手抓坐標(biāo)系為OHnoa，連桿i的坐標(biāo)系為Oixiyizi，連桿i的坐標(biāo)原點(diǎn)位于關(guān)節(jié)i軸線與關(guān)節(jié)i+1軸線的交點(diǎn)處或兩軸線公垂線與關(guān)節(jié)i+1軸線的交點(diǎn)處，連桿i的z軸與關(guān)節(jié)關(guān)節(jié)i+1軸線重合，連桿i的x軸在關(guān)節(jié)i和關(guān)節(jié)i+1軸線的公垂線上，方向指向關(guān)節(jié)i+1，連桿i的y軸由右手法則確定。

根據(jù)機(jī)器人學(xué)原理[15]，機(jī)械手抓坐標(biāo)系相對(duì)與基坐標(biāo)系可以用n、o、a、p來表示，如圖4所示。其中，p表示手抓坐標(biāo)系原點(diǎn)相對(duì)與基坐標(biāo)原點(diǎn)位置的矢量；a稱為接近矢量，表示z向矢量處于手抓進(jìn)入物體的方向；0稱為方向矢量，表示y向矢量的方向從一個(gè)指尖指向另一個(gè)指尖；n稱為法線矢量，與a、o構(gòu)成右手矢量集合，且由矢量叉乘規(guī)定：n=o×a。

圖4 機(jī)械手抓坐標(biāo)系示意圖

2.1 取桿機(jī)構(gòu)上底座相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型

變幅機(jī)構(gòu)可看成一個(gè)多關(guān)節(jié)機(jī)械臂[12]，利用上述規(guī)定，建立桅桿和變幅機(jī)構(gòu)的各個(gè)關(guān)節(jié)坐標(biāo)系，圖5所示為桅桿和變幅機(jī)構(gòu)D-H坐標(biāo)系。其中：O'0A代表連桿油缸長度；O'0O'1、BC代表兩連桿長度；四邊形O'0ACO'2表示連桿連接板；CD表示變幅油缸的長度；CD'表示變幅油缸初始長度（行程為0），EF表示滑移油缸，d0為滑移油缸的長度。由變幅機(jī)構(gòu)和桅桿的實(shí)際尺寸得到其D-H參數(shù)表，如表1所示，其中：θi為相鄰兩連桿的夾角；di為相鄰兩連桿距離；ai為連桿長度；αi表示連桿扭角，θ'i和θ'2均和連桿油缸長度有關(guān)。

圖5 桅桿和變幅機(jī)構(gòu)的D-H坐標(biāo)系

根據(jù)坐標(biāo)系圖5中幾何關(guān)系可得出：

表1 桅桿和變幅機(jī)構(gòu)D-H參數(shù)表

在三角形O'0O'1A中，由余弦定理可求得：

三角形CO'2D'中，根據(jù)余弦定理求得：

在三角形CO'2D中，根據(jù)余弦定理求得：

將式(2)～式(4)代入式(1)中，可以得出關(guān)節(jié)變量θ與各油缸長度之間的關(guān)系。

將表1參數(shù)分別代入式(5)中，可得到取桿機(jī)構(gòu)上底座相對(duì)于基座標(biāo)系O'0的運(yùn)動(dòng)學(xué)模型：

2.2 取桿機(jī)構(gòu)末端手抓中心相對(duì)于取桿機(jī)構(gòu)上底座坐標(biāo)系的運(yùn)動(dòng)學(xué)模型

利用D-H法建立坐標(biāo)系的原則，建立取桿機(jī)構(gòu)各個(gè)關(guān)節(jié)的坐標(biāo)系，如圖6所示；根據(jù)取桿機(jī)構(gòu)實(shí)際尺寸參數(shù)，得出取桿機(jī)構(gòu)D-H參數(shù)表，如表2所示。

圖6 取桿機(jī)構(gòu)D-H坐標(biāo)系

表2 取桿機(jī)構(gòu)D-H參數(shù)表

將表2參數(shù)分別代入式(5)中，得到取桿機(jī)構(gòu)末端手抓中心相對(duì)于取桿機(jī)構(gòu)上底座坐標(biāo)系O0的運(yùn)動(dòng)學(xué)模型為：

式中：ci表示cosθi；si表示sinθi。

2.3 取桿機(jī)構(gòu)相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型

由圖6中可知，取桿機(jī)構(gòu)末端手抓中心坐標(biāo)系繞自身y軸旋轉(zhuǎn)90°，即可得到取桿機(jī)構(gòu)末端手抓坐標(biāo)系。繞當(dāng)前坐標(biāo)系y軸旋轉(zhuǎn)θ角度的旋轉(zhuǎn)矩陣可表示為：

將式(6)與式(7)相乘，并右乘矩陣(8)得到取桿機(jī)構(gòu)相對(duì)于基坐標(biāo)系的運(yùn)動(dòng)學(xué)模型：

式中：[nxnynz]、[oxoyoz]、[axayaz]分別為取桿機(jī)構(gòu)末端手抓相對(duì)于基坐標(biāo)系的姿態(tài)向量；[pxpypz]為取桿機(jī)構(gòu)末端手抓相對(duì)于基坐標(biāo)系的位置向量。

式(9)中：

取桿機(jī)構(gòu)的運(yùn)動(dòng)學(xué)方程為：

3 變幅機(jī)構(gòu)對(duì)抓桿點(diǎn)的影響及變幅機(jī)構(gòu)油缸行程區(qū)間分析

從式(10)中可以看出，取桿機(jī)構(gòu)末端手抓的姿態(tài)向量只與桅桿傾斜角度θ'3有關(guān)，即只與變幅油缸伸長長度有關(guān)。從式(11)中可以看出，取桿機(jī)構(gòu)末端手抓的位置向量px、py與θ'1和θ'3有關(guān)，而pz與θ'1和θ'3無關(guān)。

3.1 變幅機(jī)構(gòu)對(duì)抓桿點(diǎn)的影響

取桿機(jī)構(gòu)工作時(shí)，送桿機(jī)構(gòu)將鉆桿輸送到固定位置給予取桿機(jī)構(gòu)抓取。抓桿時(shí)，取桿機(jī)構(gòu)左右末端手抓中心線與鉆桿平行，手抓開口方向指向鉆桿，即取桿機(jī)構(gòu)各關(guān)節(jié)變量θ為：θ1=90°；θ3=-θ'3；θ4=0°?？梢?，變幅機(jī)構(gòu)對(duì)抓桿時(shí)姿態(tài)向量影響，反映在關(guān)節(jié)變量θ3上。將θ1、θ3、θ4值代入式(10)和式(11)中得到：

從式(12)得出，抓桿時(shí)px、py與變幅機(jī)構(gòu)參數(shù)θ'1、θ'3和d0有關(guān)，pz為固定值。由于，送桿機(jī)構(gòu)將鉆桿送到固定位置，即鉆桿在空間的位置一定，對(duì)于抓桿點(diǎn)py、pz值一定，為保證能將鉆桿精確的送入夾持器中夾緊，因此，對(duì)抓桿時(shí)px位置有一定要求。

3.2 變幅機(jī)構(gòu)油缸行程區(qū)間分析

由式(12)中px、py，得到d2和d5的表達(dá)式：

已知：O '0A=300 mm ， O '1O '0=860 ，O'2D'=1260mm；CD'=1000mm；CO'2=410.4mm。利用控制變量法，改變O'0A、O'2D'、CD的值代入式(1)、式(2)、式(3)、式(4)、式(13)中，求出對(duì)應(yīng)的d2和d5的值，判斷是否都在各自取值范圍內(nèi)，程序框圖如圖7所示。

其中：

353+841/sinθ'3>2158用來判斷末端手抓上升出送桿機(jī)構(gòu)的距離是否足夠。

根據(jù)連桿油缸的行程0～190mm、變幅油缸行程0～572mm、滑移油缸0～800mm，得到O'0A的取值范圍為：607mm～797mm；CD的取值范圍為：997mm～1569mm；d0的取值范圍為：1747mm～2547mm。

圖7 變幅機(jī)構(gòu)油缸行程區(qū)間計(jì)算流程圖

保證能將鉆桿精確的送入夾持器中夾緊的px范圍為1032mm～1207mm，py=224mm。經(jīng)過上述流程圖計(jì)算可以得到，能夠使取桿機(jī)構(gòu)末端手抓正確抓取鉆桿的變幅機(jī)構(gòu)各油缸行程參數(shù)，如圖8所示，由于計(jì)算出的數(shù)據(jù)比較大，圖中僅給出能夠使末端手抓正常抓取鉆桿的各油缸行程極限值。

圖8 變幅機(jī)構(gòu)油缸行程參數(shù)圖

圖8中各名稱代表的含義是：連桿油缸行程一定時(shí)，變幅油缸行程為L0時(shí)，對(duì)應(yīng)滑移油缸行程最大值為L1，變幅油缸行程為L01時(shí)，對(duì)應(yīng)滑移油缸行程最大值為L11。

從圖8中可以看出：連桿油缸伸出長度一定時(shí)，變幅油缸伸出越長，滑移油缸行程可用區(qū)間越?。贿B桿油缸伸出越長，對(duì)應(yīng)的滑移油缸和變幅油缸的行程可用區(qū)間就越小。

4 取桿機(jī)構(gòu)運(yùn)動(dòng)過程分析與軌跡規(guī)劃

通過對(duì)取桿機(jī)構(gòu)末端手抓抓桿點(diǎn)的分析，得到變幅機(jī)構(gòu)各油缸對(duì)應(yīng)行程區(qū)間，但取桿機(jī)構(gòu)在輸送鉆桿的過程中，受到鉆桿長度的影響容易出現(xiàn)與動(dòng)力頭和送桿機(jī)構(gòu)發(fā)生碰撞的現(xiàn)象，因此，還需對(duì)取桿機(jī)構(gòu)的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，以實(shí)現(xiàn)取桿機(jī)構(gòu)在自主運(yùn)動(dòng)過程中精確加裝和拆卸鉆桿、不發(fā)生碰撞的功能，確定取桿機(jī)構(gòu)運(yùn)動(dòng)過程，滿足不同變幅機(jī)構(gòu)狀態(tài)下，鉆桿無碰撞運(yùn)動(dòng)。

4.1 取桿機(jī)構(gòu)運(yùn)動(dòng)過程分析

由圖1及表2可以看出，由于送桿機(jī)構(gòu)和動(dòng)力頭的位置以及各關(guān)節(jié)的運(yùn)動(dòng)范圍限制了取桿機(jī)構(gòu)的運(yùn)動(dòng)空間，且變幅機(jī)構(gòu)影響取桿機(jī)構(gòu)各關(guān)節(jié)變量值，因此，需要確定取桿機(jī)構(gòu)運(yùn)動(dòng)過程。輸送鉆桿過程運(yùn)動(dòng)過程如圖9所示，考慮變幅機(jī)構(gòu)的影響，若桅桿處于傾斜狀態(tài)，圖9中鉆桿狀態(tài)2、3、4運(yùn)動(dòng)到與桅桿平行狀態(tài)。其中：A表示旋轉(zhuǎn)關(guān)節(jié)1，B表示移動(dòng)關(guān)節(jié)2，C表示旋轉(zhuǎn)關(guān)節(jié)3，D表示旋轉(zhuǎn)關(guān)節(jié)4，E表示移動(dòng)關(guān)節(jié)5，圖中箭頭為關(guān)節(jié)的運(yùn)動(dòng)方向。

圖9 鉆桿運(yùn)動(dòng)過程

根據(jù)實(shí)際工況，設(shè)定輸送鉆桿的時(shí)間為24s。根據(jù)圖中鉆桿的運(yùn)動(dòng)過程，將取桿機(jī)構(gòu)的運(yùn)動(dòng)分為三個(gè)過程：

1）抓取桿件過程：取桿機(jī)構(gòu)在抓取鉆桿之后，末端機(jī)械手不能通過旋轉(zhuǎn)進(jìn)行姿態(tài)變換，因此，需要將末端機(jī)械手提升至送桿機(jī)構(gòu)上方。鉆桿狀態(tài)從0～1，關(guān)節(jié)運(yùn)動(dòng)B1，B1上升到關(guān)節(jié)2的極限位置，運(yùn)動(dòng)時(shí)間設(shè)定0～4s。抓桿時(shí)手抓末端到送桿機(jī)構(gòu)的最大距離為841mm，B1運(yùn)動(dòng)距離必須大于841mm。

2）輸送桿件過程：將桿件從送桿機(jī)構(gòu)上方，傳遞到動(dòng)力頭中心位置。鉆桿狀態(tài)從1-2-3。鉆桿狀態(tài)從1～2過程中，關(guān)節(jié)運(yùn)動(dòng)C和D，D運(yùn)動(dòng)時(shí)間設(shè)定4s～12s，C運(yùn)動(dòng)時(shí)間設(shè)定8s～12s；鉆桿狀態(tài)從2～3過程中，關(guān)節(jié)運(yùn)動(dòng)A和B2，A運(yùn)動(dòng)時(shí)間設(shè)定12s～20s，B2運(yùn)動(dòng)時(shí)間設(shè)定為14-18s。

3）夾持桿件過程：將桿件輸送到夾持器中夾緊。鉆桿狀態(tài)從3～4，關(guān)節(jié)運(yùn)動(dòng)E，運(yùn)動(dòng)時(shí)間設(shè)定為20s～24s。

以變幅機(jī)構(gòu)參數(shù)O'0A=611；CD=1569；d0=1747為例，即桅桿垂直（θ'3=90°）情況下，驗(yàn)證上述運(yùn)動(dòng)的正確性。

在該變幅機(jī)構(gòu)參數(shù)下，末端機(jī)械手起始點(diǎn)設(shè)為：

末端機(jī)械手終止點(diǎn)設(shè)為：

通過運(yùn)動(dòng)學(xué)方程可以得出起始點(diǎn)和終止點(diǎn)各關(guān)節(jié)的參數(shù)：

起始點(diǎn)各關(guān)節(jié)參數(shù)：θ1=90°，d2=-1931mm，θ3=90°，θ4=0，d5=1343mm；

終止點(diǎn)各關(guān)節(jié)參數(shù)：θ1=90°，d2=-2158mm，θ3=0，θ4=180°，d5=885mm；

按照上述運(yùn)動(dòng)過程，得到末端手抓的運(yùn)動(dòng)軌跡如圖10所示，圖中0、1、2、3、4分別與圖9中鉆桿運(yùn)動(dòng)狀態(tài)相對(duì)應(yīng)。

圖10 末端手抓運(yùn)動(dòng)軌跡

4.2 取桿機(jī)構(gòu)軌跡規(guī)劃

根據(jù)上一小節(jié)取桿機(jī)構(gòu)運(yùn)動(dòng)過程的分析，取桿機(jī)構(gòu)主要是將送桿機(jī)構(gòu)送出的桿件輸送到夾持機(jī)構(gòu)夾緊，屬于兩點(diǎn)之間的運(yùn)動(dòng)，適合采用點(diǎn)到點(diǎn)（PTP）軌跡規(guī)劃方法。五次多項(xiàng)式插值算法相比于三次多項(xiàng)式插值法、擺線法、三次B樣條曲線，具有計(jì)算量適中、曲線平滑、較好的動(dòng)態(tài)特性的優(yōu)勢[16]。因此，本文采用五次多項(xiàng)式插值法對(duì)取桿機(jī)構(gòu)的運(yùn)動(dòng)進(jìn)行規(guī)劃，并利用取桿機(jī)構(gòu)運(yùn)動(dòng)學(xué)方程，得到取桿機(jī)構(gòu)末端機(jī)械手的運(yùn)動(dòng)軌跡。

1）五次多項(xiàng)式插值法軌跡規(guī)劃

假設(shè)取桿機(jī)構(gòu)某一關(guān)節(jié)開始運(yùn)動(dòng)時(shí)刻ti的角度值為θ1或位移值為di，速度為加速度為在終止時(shí)刻tf的角度值為θf或位移值為df，速度為加速度為可得到關(guān)節(jié)的初始和末端條件：

初始和末端條件可確定一個(gè)五次多項(xiàng)式方程：

將初始和末端條件代入上述方程，可解得五次多項(xiàng)式方程中的各項(xiàng)系數(shù)為：

將上述運(yùn)動(dòng)分析過程中的參數(shù)代入式(14)中，解出方程的各項(xiàng)系數(shù)，得到各關(guān)節(jié)的運(yùn)動(dòng)函數(shù)分別為：

2）仿真結(jié)果

圖11 末端手抓的五次多項(xiàng)式插值運(yùn)動(dòng)軌跡

圖12 鉆桿空間運(yùn)動(dòng)軌跡y-z方向視圖和x-y方向視圖

從圖11中可以看出，采用五次多項(xiàng)式插值法得到的末端機(jī)械手的運(yùn)動(dòng)軌跡平滑、連續(xù)。從圖12中可以看出，y-z方向視圖中0、1、2、3、4分別對(duì)應(yīng)圖9中鉆桿的運(yùn)動(dòng)狀態(tài)，鉆桿狀態(tài)從0～2過程中，關(guān)節(jié)運(yùn)動(dòng)B1、C、D使鉆桿與動(dòng)力頭中心線平行，從圖12中可以看出，運(yùn)動(dòng)過程中不會(huì)與送桿機(jī)構(gòu)發(fā)生碰撞，同樣，在變幅機(jī)構(gòu)使桅桿傾斜情況下，關(guān)節(jié)運(yùn)動(dòng)C屬于垂直狀態(tài)下關(guān)節(jié)運(yùn)動(dòng)C的一部分，關(guān)節(jié)運(yùn)動(dòng)D與垂直狀態(tài)下關(guān)節(jié)運(yùn)動(dòng)D相同，因此，運(yùn)動(dòng)過程也不會(huì)與送桿機(jī)構(gòu)發(fā)生碰撞，鉆桿運(yùn)動(dòng)狀態(tài)從2～3過程中，桅桿傾斜和垂直，關(guān)節(jié)運(yùn)動(dòng)A、B2相同，圖12中鉆桿頂端與動(dòng)力頭之間的最小距離為200mm，滿足不會(huì)與動(dòng)力頭發(fā)生碰撞要求。因此，按照上述運(yùn)動(dòng)過程對(duì)于不同變幅機(jī)構(gòu)狀態(tài)下取桿機(jī)構(gòu)輸送鉆桿均不會(huì)與動(dòng)力頭和送桿機(jī)構(gòu)發(fā)生碰撞。

5 結(jié)語

1）提出一種結(jié)合桅桿帶有變幅機(jī)構(gòu)的鉆機(jī)取桿機(jī)構(gòu)。通過D-H法，建立了取桿機(jī)構(gòu)運(yùn)動(dòng)學(xué)模型，分析了能夠使末端機(jī)械手正確到達(dá)抓桿點(diǎn)的變幅機(jī)構(gòu)各油缸行程區(qū)間。仿真結(jié)果表明：連桿油缸伸出長度一定時(shí)，變幅油缸伸出越長，滑移油缸行程可用區(qū)間越?。贿B桿油缸伸出越長，對(duì)應(yīng)的滑移油缸和變幅油缸的行程可用區(qū)間就越小。

2）對(duì)取桿機(jī)構(gòu)的運(yùn)動(dòng)關(guān)節(jié)進(jìn)行了軌跡規(guī)劃，確定各關(guān)節(jié)運(yùn)動(dòng)的順序。利用五次多項(xiàng)式插值法得到各關(guān)節(jié)的運(yùn)動(dòng)函數(shù)，代入取桿機(jī)構(gòu)末端手抓的運(yùn)動(dòng)學(xué)方程中，得到末端手抓的運(yùn)動(dòng)軌跡。仿真結(jié)果表明：五次多項(xiàng)式插值法得到的末端手抓運(yùn)動(dòng)軌跡平滑、連續(xù)；并且，在鉆桿運(yùn)動(dòng)過程中沒有與送桿機(jī)構(gòu)和動(dòng)力頭發(fā)生碰撞，滿足不同變幅機(jī)構(gòu)狀態(tài)下的取桿機(jī)構(gòu)運(yùn)動(dòng)。