□ 王 勇 □ 劉樹(shù)林 □ 杜 偉

上海大學(xué) 機(jī)電工程與自動(dòng)化學(xué)院 上海 200072

在海洋石油鉆井以及開(kāi)采過(guò)程中，鉆桿、油管及套管的搬運(yùn)是最為繁重的工作任務(wù)之一，為此各種管子移運(yùn)裝置應(yīng)運(yùn)而生，最為典型的有大范圍三維空間移運(yùn)機(jī)械手。該機(jī)械手涉及到力學(xué)、機(jī)械學(xué)、電器液壓技術(shù)、自動(dòng)控制技術(shù)、傳感器技術(shù)和計(jì)算機(jī)技術(shù)等科學(xué)領(lǐng)域［1］，能夠?qū)崿F(xiàn)管子的定點(diǎn)移運(yùn)及隨機(jī)移運(yùn)，擁有自動(dòng)控制及手動(dòng)控制等多種方式。雖然該類(lèi)型機(jī)械手已投入使用，但由于缺乏運(yùn)動(dòng)學(xué)理論研究，管子定點(diǎn)移運(yùn)及自動(dòng)控制模式未能得到很好地應(yīng)用。

筆者分析了一種自動(dòng)控制機(jī)械手，能夠?qū)崿F(xiàn)管子從堆放處至動(dòng)力貓道或者從動(dòng)力貓道至堆放處的智能搬運(yùn)。討論了機(jī)械手正運(yùn)動(dòng)學(xué)、逆運(yùn)動(dòng)學(xué)的求解方法，在MATLAB中建立其運(yùn)動(dòng)學(xué)仿真模型，驗(yàn)證了機(jī)械手結(jié)構(gòu)設(shè)計(jì)的合理性，并利用蒙特卡洛法求出機(jī)械手的工作空間，為后續(xù)研究奠定了理論基礎(chǔ)。

1 管子移運(yùn)機(jī)械手結(jié)構(gòu)分析

通常情況下，機(jī)械手的主要技術(shù)參數(shù)包括自由度、最大工作速度、承載能力、定位精度和重復(fù)定位精度等［2］。針對(duì)不同任務(wù)機(jī)械手的技術(shù)參數(shù)，結(jié)構(gòu)設(shè)計(jì)應(yīng)該各有側(cè)重。

1.1 機(jī)械手總體結(jié)構(gòu)

根據(jù)海洋鉆井平臺(tái)現(xiàn)場(chǎng)工作空間大，管子重量不一，直徑粗細(xì)不均、長(zhǎng)度也不相等的特征，移運(yùn)機(jī)械手采取位置與姿態(tài)分開(kāi)操作的結(jié)構(gòu)。位置控制部分參照液壓挖掘機(jī)等液壓工程機(jī)械，采用液壓缸驅(qū)動(dòng)各機(jī)械臂來(lái)完成末端的定位。姿態(tài)部分（稱(chēng)之為叉臂）具有5個(gè)自由度，可靈活抓取方向放置不同的管子。管子移運(yùn)機(jī)械手總體結(jié)構(gòu)見(jiàn)圖1，叉臂結(jié)構(gòu)見(jiàn)圖2。

1.2 機(jī)械手結(jié)構(gòu)分析

參照?qǐng)D1、圖2，機(jī)械手共有9個(gè)自由度，分析如下。

（1）整個(gè)機(jī)械手外部包括旋轉(zhuǎn)平臺(tái)，控制室繞固定基座轉(zhuǎn)動(dòng)，安裝的位置傳感器能實(shí)時(shí)精確地反饋旋轉(zhuǎn)位置。

（2）內(nèi)部臂繞旋轉(zhuǎn)平臺(tái)上下擺動(dòng)；中間臂繞內(nèi)部臂上下擺動(dòng)；外部臂繞中間臂上下擺動(dòng)；各擺動(dòng)均通過(guò)液壓缸實(shí)現(xiàn)，安裝的位置傳感器能實(shí)時(shí)精確地反饋各個(gè)液壓缸伸出長(zhǎng)度。

（3）外部臂與叉臂之間的轉(zhuǎn)動(dòng)，此自由度利用旋轉(zhuǎn)脈沖編碼器保持叉臂始終處于豎直狀態(tài)。

（4）軛繞豎直方向的旋轉(zhuǎn)運(yùn)動(dòng)，此運(yùn)動(dòng)通過(guò)液壓馬達(dá)實(shí)現(xiàn)，安裝的位置傳感器能實(shí)時(shí)精確地反饋旋轉(zhuǎn)位置。

▲圖1 管子移運(yùn)機(jī)械手總體結(jié)構(gòu)簡(jiǎn)圖

▲圖2 管子移運(yùn)機(jī)械手叉臂結(jié)構(gòu)簡(jiǎn)圖

（5）軛沿豎直方向的上下運(yùn)動(dòng)，運(yùn)動(dòng)通過(guò)緩沖液壓缸實(shí)現(xiàn)，可以緩沖機(jī)械臂對(duì)手爪的沖擊力，安裝的位置傳感器、重力傳感器能實(shí)時(shí)精確地反饋液壓缸伸出長(zhǎng)度、下面負(fù)載的重量。

（6）軛的兩側(cè)擺動(dòng)，通過(guò)擺動(dòng)液壓缸實(shí)現(xiàn)，安裝的位置傳感器能實(shí)時(shí)精確地反饋液壓缸伸出長(zhǎng)度。

（7）抓手的張開(kāi)閉合，此運(yùn)動(dòng)用于抓取管子，抓手有3種規(guī)格可以方便更換，軛的下面裝有傳感器，用于探測(cè)管子。

綜上所述，管子移運(yùn)機(jī)械手具有9個(gè)自由度，能夠方便、快捷地完成管子在動(dòng)力貓道與存放處之間的運(yùn)輸。9個(gè)自由度中，前4個(gè)自由度控制機(jī)械手終端的位置，后5個(gè)自由度控制機(jī)械手終端的姿態(tài)。液壓動(dòng)力單元集中安裝于基座中，電控操作單元以及終端人機(jī)界面安裝于控制室中，各傳感器分布在執(zhí)行零部件上。

2 管子移運(yùn)機(jī)械手的運(yùn)動(dòng)學(xué)分析與仿真

機(jī)械手運(yùn)動(dòng)學(xué)研究是分析路徑軌跡規(guī)劃及運(yùn)動(dòng)控制的基礎(chǔ)［3］，包括了正運(yùn)動(dòng)學(xué)分析和逆運(yùn)動(dòng)學(xué)分析。已知連桿參數(shù)和各個(gè)關(guān)節(jié)變量來(lái)求解指尖末端相對(duì)于給定坐標(biāo)系的位置和姿態(tài)稱(chēng)之為正運(yùn)動(dòng)學(xué)，已知機(jī)械手連桿參數(shù)和指尖末端相對(duì)于固定坐標(biāo)系的位置和姿態(tài)，來(lái)求解機(jī)械手各個(gè)關(guān)節(jié)變量的大小稱(chēng)之為運(yùn)動(dòng)學(xué)逆問(wèn)題［4］。通過(guò)上節(jié)對(duì)機(jī)械手的結(jié)構(gòu)分析，其末端抓手姿態(tài)主要靠傳感器得到的實(shí)時(shí)反饋信息進(jìn)行調(diào)整，不需要事先進(jìn)行規(guī)劃，所以對(duì)管子移運(yùn)機(jī)械手的運(yùn)動(dòng)學(xué)分析及工作空間求解可以集中于前4個(gè)自由度，即從基座到外臂末端機(jī)械臂的運(yùn)動(dòng)學(xué)分析和外臂末端的工作空間求解。

2.1 機(jī)械手運(yùn)動(dòng)學(xué)方程建立及正運(yùn)動(dòng)學(xué)求解

建立機(jī)械手運(yùn)動(dòng)學(xué)方程常用的方法主要是標(biāo)準(zhǔn)的D-H方法和改進(jìn)的D-H方法，兩種方法基本原理相同，連桿分析思想一致，但坐標(biāo)系表示方法不同，前者建立的連桿坐標(biāo)系位于連桿末端，后者建立的連桿坐標(biāo)系位于連桿前端［5］。本文中采用標(biāo)準(zhǔn)的D-H方法，建立管子移運(yùn)機(jī)械手的連桿坐標(biāo)系如圖3。

根據(jù)所建立的連桿坐標(biāo)系，按照標(biāo)準(zhǔn)D-H方法，確定各桿件結(jié)構(gòu)參數(shù)和關(guān)節(jié)變量，見(jiàn)表1，表中：θ為關(guān)節(jié)角，是唯一變量；α為連桿轉(zhuǎn)角；a為連桿長(zhǎng)度；d為連桿偏距。

表1 機(jī)械手連桿參數(shù)和關(guān)節(jié)變量

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

基于表1中各參數(shù)，參照標(biāo)準(zhǔn)的D-H坐標(biāo)轉(zhuǎn)換公式，得出連桿坐標(biāo)變換矩陣通式為：

式中：cθn+1=cosθn+1=cn+1；sθn+1=sinθn+1=sn+1。 另 c（θ2+θ3）=cos（θ2+θ3）=c23，s（θ2+θ3）=sin（θ2+θ3）=s23，以下各式簡(jiǎn)寫(xiě)類(lèi)似。

各連桿的變換矩陣相乘，得出末端坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣，即正運(yùn)動(dòng)學(xué)方程為：

2.2 機(jī)械手逆運(yùn)動(dòng)學(xué)求解

機(jī)械手逆運(yùn)動(dòng)學(xué)求解方法主要分為數(shù)值方法和封閉解法［6］，對(duì)于簡(jiǎn)單的機(jī)械手一般用封閉解法，封閉解法又有反變換法、幾何法等。本文采用反變換法求解如下。

將式（2）兩邊同時(shí)乘A1的逆，代入az=0可得：

式（3）兩邊等式中第3行第4列元素相等，可得:

式（3）兩邊等式第2行第1列元素及第2行第2列元素分別相等，可得：

式（3）兩邊等式第1行第4列元素以及第2行第4列元素分別相等，可得：

整理、變換、化簡(jiǎn)上面兩個(gè)方程，可得：

最后可得θ4:

2.3 機(jī)械手運(yùn)動(dòng)學(xué)MATLAB仿真驗(yàn)證

對(duì)正運(yùn)動(dòng)學(xué)仿真，給出結(jié)構(gòu)設(shè)計(jì)參數(shù)：L1=8 m，L2=8 m，L3=2 m，L4=8 m，θ1＝0°，θ2＝0°，θ3＝-45°，θ4＝-45°。 在MATLAB程序中，為編程方便，用q表示關(guān)節(jié)角度。

（2）在MATLAB中建立機(jī)械手仿真圖，如圖4所示。

通過(guò)正運(yùn)動(dòng)學(xué)編程求解，得機(jī)械手末端X、Y、Z坐標(biāo) （仿真圖 4 （b） 中 x、y、z 值） 分別為：9.4142，0，-1.414 2，與幾何關(guān)系得出的結(jié)果相符，驗(yàn)證了正運(yùn)動(dòng)學(xué)的正確性。

逆運(yùn)動(dòng)學(xué)仿真，重新對(duì)關(guān)節(jié)角度賦值為：

在MATLAB/ROBOTICS工具箱下，編寫(xiě)逆運(yùn)動(dòng)學(xué)驗(yàn)證程序：

q= ［-pi/6 pi/6-pi/3-pi/3］;

drivebot（r,q）;

T=fkine（r,q）//%對(duì)關(guān)節(jié)值q求正解（末端位置）

q0=ikine（r,T）//%對(duì)末端位置求逆解（關(guān)節(jié)值）

MATLAB求解末端位置的逆解結(jié)果為：

▲圖4機(jī)械手正運(yùn)動(dòng)學(xué)仿真圖

▲圖5 機(jī)械手逆運(yùn)動(dòng)學(xué)仿真圖

q0=-0.523 6 0.523 6 -1.047 2 -1.047 2

可見(jiàn)，逆解結(jié)果q0與關(guān)節(jié)角度q所賦初值相等，驗(yàn)證了逆運(yùn)動(dòng)學(xué)的正確性。仿真圖見(jiàn)圖5。

3 管子移運(yùn)機(jī)械手末端工作空間的求解

機(jī)械手工作空間指手臂末端所能達(dá)到的空間目標(biāo)的集合，代表了機(jī)械手的活動(dòng)范圍，是衡量機(jī)械手工作能力一個(gè)重要的運(yùn)動(dòng)學(xué)指標(biāo)［7］。通過(guò)對(duì)油管移運(yùn)機(jī)械手的分析，關(guān)節(jié)角度θ1是作回轉(zhuǎn)運(yùn)動(dòng)，所以本機(jī)械手工作空間可以只求解XOZ平面，之后關(guān)節(jié)角度θ1旋轉(zhuǎn)相應(yīng)角度范圍即可得到整個(gè)工作空間。

本文采用蒙特卡洛法［8］求解管子移運(yùn)機(jī)械手的工作空間。蒙特卡洛法是一種借助于隨機(jī)抽樣來(lái)解決數(shù)學(xué)問(wèn)題的數(shù)值方法，廣泛應(yīng)用于描述某些隨機(jī)的物理現(xiàn)象，其具體步驟如下。

（1）求解機(jī)械手運(yùn)動(dòng)學(xué)正解，得到機(jī)械手末端在參考坐標(biāo)系中的位置向量。

（2）在MATLAB環(huán)境下，利用 Rand（）函數(shù)產(chǎn)生隨機(jī)數(shù)形成各關(guān)節(jié)變量的隨機(jī)值為：

▲圖6 蒙特卡洛法末端工作空間圖

式 中 ：θimin為 變量 θi轉(zhuǎn)動(dòng)范圍的 最 小 值 ；θimax為變量 θi轉(zhuǎn)動(dòng)范圍的最大值。

（3） 編寫(xiě)算法程序，使 式 （9） 循環(huán)N次 （此處N取80 000）， 得到每個(gè)關(guān)節(jié)變量的N個(gè)隨機(jī)值。

（4） 將得到的隨機(jī)值代入正運(yùn)動(dòng)學(xué)方程中，在MATLAB中調(diào)用繪圖命令求解末端坐標(biāo)點(diǎn)集，結(jié)果如圖6所示。

4 結(jié)論

基于海洋平臺(tái)管子移運(yùn)的特殊性，針對(duì)具有9個(gè)自由度的移運(yùn)機(jī)械手結(jié)構(gòu)，分析了機(jī)構(gòu)的正運(yùn)動(dòng)學(xué)解法、逆運(yùn)動(dòng)學(xué)解法，并在MATLAB中仿真驗(yàn)證了機(jī)構(gòu)模型和解法的正確性；利用蒙特卡洛法求解了機(jī)械手末端在XOZ平面中的工作空間。一方面為進(jìn)一步的軌跡規(guī)劃和控制理論打下基礎(chǔ)，更為重要的是對(duì)同性質(zhì)機(jī)械手的設(shè)計(jì)和研究提供參照。

［1］ 熊有倫.機(jī)器人學(xué)［M］．北京：機(jī)械工業(yè)出版社，1993．

［2］ 馬江.六自由度機(jī)械臂控制系統(tǒng)設(shè)計(jì)與運(yùn)動(dòng)學(xué)仿真［D］.北京：北京工業(yè)大學(xué),2009.

［3］ 趙亮亮,馬振書(shū),穆喜輝，等.危險(xiǎn)作業(yè)機(jī)器人機(jī)械臂設(shè)計(jì)及其運(yùn)動(dòng)學(xué)分析［J］.機(jī)械設(shè)計(jì)與制造,2011（8）:155-157.

［4］ 羅家佳,胡國(guó)清.基于 MATLAB的機(jī)器人運(yùn)動(dòng)仿真研究［J］.廈門(mén)大學(xué)學(xué)報(bào)（自然科學(xué)版）,2005,44（5）:640-644.

［5］ Corke P I.A Robotics Toolbox for MATLAB［J］.Robotics&Automation,IEEE,1996,3（1）:24-32.

［6］ 劉萍，陳瑩.五自由度關(guān)節(jié)式機(jī)械手運(yùn)動(dòng)學(xué)分析及仿真［J］.制造業(yè)自動(dòng)化,2011,33（19）:9-11.

［7］ Rastegar J,Fardanesh B.Manipulation Workspace Analysis Using the Monte Carlo Method［J］.Mechanism and Machine Theory,1990,25（2）:233-239.

［8］ 田海波,馬宏偉.井下排爆機(jī)器人機(jī)械臂運(yùn)動(dòng)學(xué)及工作空間的分析［J］.礦山機(jī)械，2012,40（9）:105-108.