鉆桿排放機(jī)械手運(yùn)動(dòng)學(xué)分析及軌跡控制仿真

黃川，蘭凱，袁煥宇，陳凱凱

(1.中國石油大學(xué)機(jī)電工程學(xué)院，山東青島266580 ;2.中國化工集團(tuán)桂林橡膠機(jī)械廠，廣西桂林541002)

0 前言

傳統(tǒng)鉆機(jī)主要依靠二層臺的井架工人手持管夾，通過體力勞動(dòng)的方式對鉆桿立根進(jìn)行排放操作。這些鉆機(jī)在日益要求健康、安全、環(huán)保的石油石化行業(yè)中，需要進(jìn)行升級和改造，安裝鉆桿自動(dòng)排放裝置。鉆桿自動(dòng)排放裝置是一種能夠在井口與管架之間往返移送和存取鉆桿立根的自動(dòng)化設(shè)備［1－2］。目前，機(jī)械手式的排放裝置以其結(jié)構(gòu)緊湊、質(zhì)量輕、靈活度高的優(yōu)點(diǎn)，已被國外鉆機(jī)廣泛采用［3］。對此，經(jīng)過初步探索和研究，設(shè)計(jì)了一種新型二層臺舌臺前置式鉆桿排放裝置(中國專利:201320019851.1)。作者針對該裝置的總體方案及工作原理進(jìn)行了簡要介紹，并通過建立其運(yùn)動(dòng)學(xué)模型和雅克比矩陣，得到末端夾持器與動(dòng)作關(guān)節(jié)之間的位置、速度變換關(guān)系，在此基礎(chǔ)上利用Simulink 進(jìn)行軌跡控制仿真，獲取了相關(guān)運(yùn)動(dòng)學(xué)參數(shù)并驗(yàn)證了控制方法的可行性。

1 總體結(jié)構(gòu)與工作原理

如圖1所示，排放機(jī)械手結(jié)構(gòu)上主要包括旋轉(zhuǎn)臂、大臂、小臂、伸縮臂4 段手臂連桿，末端夾持器、扶正鉗兩個(gè)動(dòng)力夾鉗以及液壓驅(qū)動(dòng)器等部分。旋轉(zhuǎn)臂懸掛于二層臺下方，能以舌臺為基座360°回轉(zhuǎn);大臂與旋轉(zhuǎn)臂，小臂與大臂通過轉(zhuǎn)動(dòng)關(guān)節(jié)連接，最大轉(zhuǎn)角范圍35°;伸縮臂與小臂通過移動(dòng)關(guān)節(jié)連接，最大伸長1.4 m。綜上，排放機(jī)械手共有3 個(gè)旋轉(zhuǎn)關(guān)節(jié)和1 個(gè)移動(dòng)關(guān)節(jié)，每個(gè)關(guān)節(jié)都由液壓執(zhí)行器驅(qū)動(dòng)，總共4 個(gè)自由度。

圖1 鉆桿排放機(jī)械手總體結(jié)構(gòu)示意圖

鉆機(jī)二層臺距鉆臺高24.5 ～26.5 m，與井架固連。如圖2，鉆桿排放機(jī)械手取鉆桿作業(yè)示意圖所示，排放機(jī)械手在取鉆桿作業(yè)時(shí)，末端夾持器先以水平姿態(tài)夾持鉆桿。接著，小臂內(nèi)置伸縮缸單獨(dú)動(dòng)作將鉆桿提升至適當(dāng)高度。然后，所有關(guān)節(jié)按一定規(guī)律聯(lián)動(dòng)，使鉆桿以豎直姿態(tài)沿著期望軌跡ABCD 運(yùn)動(dòng)出二層臺(管架)并送至井口上方。最后，小臂下降，把鉆桿交由其他裝置處理。在整個(gè)運(yùn)動(dòng)過程中，扶正鉗始終限制鉆桿的水平偏移量，一直使AA'，BB'，…，DD'保持豎直。排放機(jī)械手存鉆桿作業(yè)的動(dòng)作流程與取鉆桿相逆。

圖2 鉆桿排放機(jī)械手取鉆桿作業(yè)示意圖

2 建立位置關(guān)系

2.1 數(shù)學(xué)描述

采用D-H 方法［4］建立排放機(jī)械手的連桿坐標(biāo)系如圖3所示，D-H 參數(shù)見表1。

圖3 鉆桿排放機(jī)械手連桿坐標(biāo)系

表1 鉆桿排放機(jī)械手D-H 參數(shù)表

圖3 中，1、2、3、4 表示手臂關(guān)節(jié)，其中1、2、3 為轉(zhuǎn)動(dòng)關(guān)節(jié)、4 為移動(dòng)關(guān)節(jié);①、②、③、④表示手臂連桿，依次表示旋轉(zhuǎn)臂、主臂、副臂和伸縮臂。坐標(biāo)系0、1、2、3、4 分別與基座①、②、③、④固連。

2.2 正向位置關(guān)系

建立排放機(jī)械手連桿坐標(biāo)系后，由已知量θ =［θ1θ2θ3d4］T確定各連桿之間的變換矩陣i－1Ai(θi/di)(i=1，2，3，4)，將這些矩陣連乘得到排放機(jī)械手的運(yùn)動(dòng)學(xué)方程

A=0A1(θ1)1A2(θ2)2A3(θ3)3A4(d4)

A 即是包含了位置矢量(px，py，pz)和姿態(tài)矢量R 的信息的四階方陣。由于該機(jī)械手的姿態(tài)矢量R可在其位置矢量已經(jīng)確定的情況下由繞y4軸的俯仰角ωy唯一表示。由于繞z4軸的ωz與px、py、pz線性相關(guān)，繞x4軸的ωx受結(jié)構(gòu)限制而恒為0，固可不建立它們的關(guān)系。因此，正向位置關(guān)系為

式中:s(i+ j)= sin(θi+ θj)，si= sinθi，c(i + j)=cos(θi+θj)，ci=cosθi。

2.3 逆向位置關(guān)系

采用幾何法求解:首先根據(jù)px和py易求得θ1，然后利用ΔO1O2O'和ΔO2O3O4在空間中的幾何關(guān)系列方程解出d4和θ2，最后易知θ3。求解過程中出現(xiàn)的重根由排放機(jī)械手的結(jié)構(gòu)限制予以排除。得反向位置關(guān)系為

式中:ci=cosθi。

3 建立速度關(guān)系

由于排放機(jī)械手的工作區(qū)域受到鉆桿、管架和舌臺的限制，允許夾持器的運(yùn)動(dòng)空間非常小。因此，必須設(shè)計(jì)具有較高品質(zhì)的控制系統(tǒng)，這就需要進(jìn)一步分析其末端夾持器與動(dòng)作關(guān)節(jié)之間的速度關(guān)系。

3.1 數(shù)學(xué)描述［5］

排放機(jī)械手的雅克比矩陣是它的末端夾持器速度與關(guān)節(jié)速度之間的線性變換，也是從關(guān)節(jié)空間到操作空間的傳動(dòng)比，其定義為

直接將式(5)—(8)代入上式求解并簡化，得該機(jī)械手的雅克比矩陣J，此處把J 視為一個(gè)4 階的方陣。

3.2 速度關(guān)系

排放機(jī)械手關(guān)節(jié)空間與操作空間的速度關(guān)系為

因此，只要J 可逆，在已知末端夾持器位姿和速度的情況下，利用式(9)即可求出關(guān)節(jié)速度。J 若不可逆，則說明末端夾持器處于該位姿則關(guān)節(jié)處于無法達(dá)到的位置或速度，這個(gè)位姿在軌跡規(guī)劃時(shí)就應(yīng)當(dāng)予以避免。

4 軌跡控制仿真

4.1 軌跡控制原理

關(guān)節(jié)控制量的產(chǎn)生以式(9)為核心:先將反饋的關(guān)節(jié)實(shí)際位置經(jīng)過正向位置關(guān)系式(1)—(4)轉(zhuǎn)換成夾持器在操作空間中的位姿，再與期望的夾持器位姿進(jìn)行比較，形成的偏差與逆雅克比矩陣相乘，得到關(guān)節(jié)的控制量。該運(yùn)動(dòng)控制方法的軌跡規(guī)劃和關(guān)節(jié)控制都是在操作空間內(nèi)進(jìn)行的，因此具有比關(guān)節(jié)空間運(yùn)動(dòng)控制方法更加直接的特性［6］。圖4 為利用Simulink在該運(yùn)動(dòng)控制方法下的運(yùn)動(dòng)學(xué)仿真框圖:圖中xyzpry_T軌跡期望模塊把設(shè)定的位置期望px，py，pz和姿態(tài)量期望wy構(gòu)成的夾持器期望位姿矩陣A*輸出;tr_diff 位姿比較模塊把由forward 模塊反饋來的夾持器實(shí)際位姿A 與期望位姿A*進(jìn)行比較，輸出偏差值;Integrator 模擬關(guān)節(jié)運(yùn)動(dòng)模塊將輸入的關(guān)節(jié)速度積分得到關(guān)節(jié)的位置并反饋到Jacobian、Inv_ jacobian 模塊求出雅克比逆矩陣J－1。期望為如圖2所示的軌跡ABCD，其中AB、BC 直線軌跡各需時(shí)8 s，CD 圓弧軌跡需時(shí)6 s。仿真加入PID 控制器以提高控制精度。

圖4 排放機(jī)械手運(yùn)動(dòng)學(xué)Simulink 仿真框圖

4.2 仿真結(jié)果及分析

由圖5 末端夾持器位置變化曲線，圖6 末端夾持器x－y 平面的運(yùn)動(dòng)軌跡可見:運(yùn)動(dòng)過程中pz保持不變，px，py合成了預(yù)期軌跡。

圖5 末端夾持器位置變化曲線

圖6 末端夾持器x－y平面軌跡

由圖7 末端夾持器姿態(tài)變化曲線可見:運(yùn)動(dòng)過程中ωy保持不變，ωx由于機(jī)械手結(jié)構(gòu)的限制也維持不變;由圖8 手臂各關(guān)節(jié)位置變化曲線可見:關(guān)節(jié)2、3 的變化范圍分別為87.97° ～116.03°和－116.03° ～－87.97°，關(guān)節(jié)4 的變化范圍為－0.59 m ～－0.12 m。

圖7 末端夾持器姿態(tài)變化曲線

上述結(jié)果表明，末端夾持器能夠按照期望軌跡運(yùn)動(dòng)，控制方法有效，關(guān)節(jié)運(yùn)動(dòng)也均在設(shè)計(jì)范圍以內(nèi)。另外還可發(fā)現(xiàn)，由于期望的姿態(tài)量ωy恒為零，導(dǎo)致θ2與θ3恒互為相反數(shù)。故可以采用碼垛機(jī)器人所常見的平行四邊形機(jī)構(gòu)降低控制的難度［7］。

5 結(jié)論

(1)建立了新型鉆桿排放機(jī)械手末端夾持器與動(dòng)作關(guān)節(jié)之間的位置和速度變換關(guān)系，利用Simulink進(jìn)行了操作空間運(yùn)動(dòng)控制方法下的軌跡控制仿真，得到了控制結(jié)果及相關(guān)運(yùn)動(dòng)學(xué)參數(shù)。

(2)仿真結(jié)果表明排放機(jī)械手的機(jī)構(gòu)設(shè)計(jì)合理，軌跡控制方法有效，運(yùn)動(dòng)能夠滿足排放工藝的要求，分析和仿真可為詳細(xì)設(shè)計(jì)提供思路和參考。

［1］WOELFEL S R，VOLLANDS P T.Breakthroughs in Casing Running Efficiency Utilizing Modern Pipe Handling Technology［R］.Netherlands:SPE/IADC，2001.

［2］黃川，姜菁杰，蘭凱.二層臺鉆桿自動(dòng)排放裝置運(yùn)動(dòng)控制建模與仿真［J］.石油礦場機(jī)械，2013，42(7):4－7.

［3］姜鳴，曾言悌，周聲強(qiáng).陸地鉆機(jī)鉆桿自動(dòng)排放系統(tǒng)的設(shè)計(jì)方案［J］.石油機(jī)械，2008，36(8):95－98.

［4］霍偉，機(jī)器人動(dòng)力學(xué)與控制［M］.北京:高等教育出版社，2005:16－23.

［5］陳懇，楊向東，劉莉，等.機(jī)器人技術(shù)與應(yīng)用［M］.北京:清華大學(xué)出版社，2006:44－52.

［6］毛新濤，包鋼，楊慶俊，等.三自由度氣動(dòng)串聯(lián)機(jī)械手空間路徑規(guī)劃及軌跡控制研究［J］.機(jī)床與液壓，2009，36(4):61－63.

［7］張志強(qiáng)，臧冀原，贠超.混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真［J］.機(jī)械設(shè)計(jì)，2010，27(11):47－51.