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

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

0 前言

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

1 總體結構與工作原理

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

圖1 鉆桿排放機械手總體結構示意圖

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

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

2 建立位置關系

2.1 數學描述

采用D-H 方法［4］建立排放機械手的連桿坐標系如圖3所示，D-H 參數見表1。

圖3 鉆桿排放機械手連桿坐標系

表1 鉆桿排放機械手D-H 參數表

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

2.2 正向位置關系

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

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

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

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

2.3 逆向位置關系

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

式中:ci=cosθi。

3 建立速度關系

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

3.1 數學描述［5］

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

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

3.2 速度關系

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

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

4 軌跡控制仿真

4.1 軌跡控制原理

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

圖4 排放機械手運動學Simulink 仿真框圖

4.2 仿真結果及分析

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

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

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

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

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

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

5 結論

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

(2)仿真結果表明排放機械手的機構設計合理，軌跡控制方法有效，運動能夠滿足排放工藝的要求，分析和仿真可為詳細設計提供思路和參考。

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

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

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

［4］霍偉，機器人動力學與控制［M］.北京:高等教育出版社，2005:16－23.

［5］陳懇，楊向東，劉莉，等.機器人技術與應用［M］.北京:清華大學出版社，2006:44－52.

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

［7］張志強，臧冀原，贠超.混聯(lián)碼垛機器人運動學分析及仿真［J］.機械設計，2010，27(11):47－51.