六自由度鉆桿裝卸機(jī)械手運(yùn)動(dòng)學(xué)研究

陳光柱，鄧 熠，邱士安 ，付 林

（1.成都理工大學(xué)核技術(shù)與自動(dòng)化工程學(xué)院，四川 成都 610059；2.成都工業(yè)學(xué)院材料工程學(xué)院，四川 成都 611730）

1 引言

工程鉆機(jī)機(jī)器人是一種主要應(yīng)用于建筑工程施工、地質(zhì)勘探、煤礦開(kāi)采等領(lǐng)域的工程鉆探設(shè)備，其具有自動(dòng)化程度高，生產(chǎn)作業(yè)快速，支護(hù)能力強(qiáng)等特點(diǎn)。工程鉆機(jī)機(jī)器人的使用大幅度提高了工程行業(yè)的施工效率，同時(shí)讓工人從惡劣的工作環(huán)境和繁重的工作作業(yè)中解放出來(lái)，極大地降低了鉆進(jìn)過(guò)程中的操作危險(xiǎn)性[1]。鉆桿裝卸是鉆機(jī)機(jī)器人極為重要的工作環(huán)節(jié)，該環(huán)節(jié)的效率高低直接影響整機(jī)的工作效率，故國(guó)內(nèi)外學(xué)者針對(duì)鉆桿自動(dòng)裝卸技術(shù)開(kāi)展了大量研究，并且在實(shí)際生產(chǎn)中取得了許多應(yīng)用成果。在20世紀(jì)初，國(guó)外Williamson公司的Rovdrill鉆機(jī)、澳大利亞PROD鉆機(jī)、德國(guó)不萊梅MeBo鉆機(jī)安裝了鉆機(jī)鉆桿自動(dòng)存取系統(tǒng)，極大地提高了工作效率[2-4]。1999年，寶雞石油機(jī)械廠在研制的GWM1000鉆機(jī)上首先開(kāi)展關(guān)于鉆桿自動(dòng)拆卸技術(shù)與系統(tǒng)的研究。2001年，蘭州石油化工機(jī)器廠生產(chǎn)的陸地沙漠鉆機(jī)采用了鉆桿存取系統(tǒng)。2010年，煙臺(tái)中集公司在深水半潛式鉆井平臺(tái)，配備國(guó)外的鉆桿自動(dòng)存取裝置，實(shí)現(xiàn)了全自動(dòng)化鉆進(jìn)和起下鉆具等功能。2011年，我國(guó)首次自助設(shè)計(jì)的“海洋石油981”上配備HR柱形存取系統(tǒng)以及鉆桿移送裝置。2014年，某科技大學(xué)機(jī)電工程學(xué)院進(jìn)行了液壓鉆車鉆桿自動(dòng)裝卸裝置的設(shè)計(jì)[5-7]。

可以看出，目前鉆桿自動(dòng)裝卸系統(tǒng)及技術(shù)在鉆井平臺(tái)上已經(jīng)得到了較為廣泛的應(yīng)用。然而，由于工作環(huán)境的特殊性及復(fù)雜性，目前工程鉆機(jī)的鉆桿裝卸還未實(shí)現(xiàn)自動(dòng)化，仍采用手動(dòng)換接方式，其不但工作效率低下，而且安全隱患大。鑒于此，設(shè)計(jì)一種六自由度鉆桿裝卸機(jī)械手，以實(shí)現(xiàn)工程鉆機(jī)鉆桿的自動(dòng)換接。在此基礎(chǔ)上，建立機(jī)械手的運(yùn)動(dòng)學(xué)模型，并進(jìn)行仿真研究。

2 鉆桿拆卸機(jī)械手運(yùn)動(dòng)學(xué)方程建立

2.1 三維模型建立

設(shè)計(jì)的六自由度鉆桿裝卸機(jī)械手，如圖1所示。主要由基座、鉆桿抓取器和五個(gè)通過(guò)R副和P副實(shí)現(xiàn)連接的連桿組成。機(jī)械手腰部、機(jī)械手大臂、機(jī)械手中臂、機(jī)械手小臂之間的關(guān)節(jié)運(yùn)動(dòng)方式為旋轉(zhuǎn)運(yùn)動(dòng)，具有3個(gè)自由度，機(jī)械手腰部自身帶旋轉(zhuǎn)功能，具有1個(gè)自由度，而旋轉(zhuǎn)移動(dòng)部件能實(shí)現(xiàn)轉(zhuǎn)動(dòng)和移動(dòng)，具有2個(gè)自由度，故機(jī)械手總共擁有6個(gè)自由度。鉆桿抓取器在各關(guān)節(jié)液壓馬達(dá)的驅(qū)動(dòng)下，按照預(yù)定的路徑將鉆桿送至鉆機(jī)或者將鉆桿從鉆機(jī)中取出放入鉆桿庫(kù)。由于該機(jī)械手自由度數(shù)較多，相對(duì)來(lái)說(shuō)具有更加開(kāi)闊的工作壞境。因此，鉆桿抓取器能夠運(yùn)動(dòng)至空間中任意位置，具有較高的靈敏度，同時(shí)也易于實(shí)現(xiàn)避障。

圖1 六自由度鉆桿裝卸機(jī)械手Fig.1 Six DOF Drill Pipe Handing Manipulator

2.2 鉆桿拆卸機(jī)械手運(yùn)動(dòng)學(xué)模型建立

對(duì)六自由度鉆桿裝卸機(jī)械手進(jìn)行運(yùn)動(dòng)學(xué)研究是機(jī)械手設(shè)計(jì)一個(gè)重要步驟。當(dāng)已知機(jī)械手所有關(guān)節(jié)變量時(shí)，求出機(jī)械手的運(yùn)動(dòng)學(xué)方程，就能描述出鉆桿抓取器在空間中的準(zhǔn)確位姿，此為運(yùn)動(dòng)學(xué)正解。在鉆桿抓取器特定的位姿下對(duì)機(jī)械手的運(yùn)動(dòng)學(xué)方程求解，得出鉆桿抓取器在此位姿下的關(guān)節(jié)變量，此為運(yùn)動(dòng)學(xué)逆解。之后可以進(jìn)一步得出機(jī)械手每個(gè)運(yùn)動(dòng)構(gòu)件的速度，加速度，關(guān)節(jié)變量等參數(shù)，能夠?yàn)闄C(jī)械手的動(dòng)態(tài)性能優(yōu)化，結(jié)構(gòu)參數(shù)優(yōu)化，整機(jī)控制等后續(xù)研究工作提供理論基礎(chǔ)和參考依據(jù)。Denavit和Hartenberg在1955年提出一種D-H連桿坐標(biāo)變化法，這種方法在機(jī)器人設(shè)計(jì)領(lǐng)域內(nèi)得到了廣泛的應(yīng)用[8]。建立六自由度鉆桿裝卸機(jī)械手D-H坐標(biāo)系，如圖2所示。建立D-H參數(shù)，如表1所示。

圖2 六自由度鉆桿裝卸機(jī)械手D-H坐標(biāo)系Fig.2 Six DOF Drill Pipe Handing Manipulator D-H Coordinate

表1 六自由度鉆桿裝卸機(jī)械手D-H參數(shù)Tab.1 Six DOF Drill Pipe Handing Manipulator D-H CoordinateParameters

根據(jù)表1中的參數(shù)，機(jī)械手各個(gè)連桿D-H坐標(biāo)變化矩陣為：

將上述六個(gè)矩陣相乘得到機(jī)械手的運(yùn)動(dòng)學(xué)方程為：

如果已知每一個(gè)關(guān)節(jié)變量θn，將其代入式（1）就得出可以機(jī)械手在空間中的確定位姿。

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

3.1 遺傳算法介紹

工程鉆機(jī)機(jī)器人在鉆進(jìn)過(guò)程中，鉆桿裝卸機(jī)械手需要根據(jù)鉆機(jī)主軸孔的位置確定各關(guān)節(jié)變量值，然后驅(qū)動(dòng)各關(guān)節(jié)運(yùn)動(dòng)，完成鉆桿的抓取、移動(dòng)、放置等動(dòng)作，這實(shí)際上是對(duì)鉆桿裝卸機(jī)械手進(jìn)行逆運(yùn)動(dòng)學(xué)分析。遺傳算法通過(guò)模仿自然界中選擇與遺傳的機(jī)理尋找最優(yōu)解。算法求解過(guò)程中引入選擇、交叉和變異三個(gè)遺傳算子作為核心操作步驟，采用概率的變遷規(guī)則指導(dǎo)目標(biāo)函數(shù)進(jìn)行求解。通過(guò)引入概率的搜索方法，整個(gè)算法的搜索過(guò)程始終朝著搜索空間更優(yōu)化的解區(qū)域移動(dòng)[9-10]。在評(píng)價(jià)基因個(gè)體時(shí)采用適應(yīng)度函數(shù)對(duì)其進(jìn)行評(píng)價(jià)，不需要外界輔助信息，也不容易陷入局部最優(yōu)解，是一種全局范圍內(nèi)的最優(yōu)搜索算法。

3.2 算法設(shè)計(jì)

3.2.1 目標(biāo)函數(shù)設(shè)計(jì)

將鉆桿與鉆桿安裝孔間的裝配關(guān)系視為一個(gè)孔軸配合從而可以利用孔軸配合的同心度誤差對(duì)鉆桿與鉆桿安裝孔間的相對(duì)位置關(guān)系進(jìn)行評(píng)價(jià)。鉆桿與鉆桿安裝孔間允許的最大同心度誤差可用下式計(jì)算：

式中：p—同心度誤差；s1—孔的基準(zhǔn)尺寸，此處為鉆桿安裝孔的直徑；s2—配合軸的最大尺寸，此處為鉆桿直徑。

設(shè)s1=45mm，s2=42mm，由式（2）計(jì)算出鉆桿與鉆桿安裝孔間允許的最大同心度誤差pmax=0.9mm。為使鉆桿抓取器運(yùn)動(dòng)到預(yù)定位置且滿足最大同心度誤差要求，可將鉆桿與安裝孔間的同心度誤差的幾何關(guān)系表達(dá)式作為遺傳算法的目標(biāo)函數(shù)，定義如下：

式中：Px，Py，Pz—鉆桿抓取器中心位置坐標(biāo)；Pox，Poy，Poz—主軸孔指定點(diǎn)位置坐標(biāo)。從式（1）的參數(shù)解釋中可以看出Px，Py，Pz與關(guān)節(jié)變量θ2，θ3，θ4成互相耦合的關(guān)系。因此將三個(gè)關(guān)節(jié)變量θ2=［40°，50°］、θ3=［55°，60°］、θ4=［90°，100°］作為約束條件。對(duì)式（3）求解可以得出能夠使鉆桿抓取器到達(dá)指定位置的關(guān)節(jié)變量。

3.2.2 算法過(guò)程

采用二進(jìn)制編碼，將解的精度設(shè)定為0.001。關(guān)節(jié)變量θ2、θ3、θ4的編碼長(zhǎng)度分別為 n1，n2，n3。n1=log2［（50-40）/0.001+1］=14，n2=log2［（60-55）/0.001+1］=13，n3=log2［（100-90）/0.001+1］=14。則總的編碼長(zhǎng)度為n1+n2+n3=41。

算法通過(guò)下述幾個(gè)步驟完成：（1）初始化種群。隨機(jī)生成50*41的矩陣，即個(gè)體數(shù)目為50，個(gè)體長(zhǎng)度為41。（2）確定選擇算子。選擇出符合條件的父體。（3）確定交叉算子。設(shè)置交叉概率Pc為0.8，通過(guò)上一個(gè)選擇步驟得出的所有父體進(jìn)行兩兩隨機(jī)配對(duì)并進(jìn)行交叉，得到新的個(gè)體。（4）確定變異算子。將變異概率Pm設(shè)定為0.01，使上一步驟產(chǎn)生的新個(gè)體進(jìn)行變異，再一次產(chǎn)生新的個(gè)體。（5）如果步驟（4）產(chǎn)生的新的個(gè)體數(shù)達(dá)到50，則形成一個(gè)新群體；否則，返回步驟（2），再次運(yùn)行算法程序。

在圖2中，各連桿長(zhǎng)度分別為a1=227.62mm，a2=363.10mm，a3=340mm，d1=55mm，L5=380.47mm，鉆桿需要到達(dá)的點(diǎn)坐標(biāo)為（-578.08mm，0，755.62mm）。種群規(guī)模取 50，交叉概率取 0.8，變異概率取0.01。由于機(jī)械手參數(shù)θ1和θ5的值取決于鉆機(jī)的工作角度，將這兩個(gè)角度脫離機(jī)械手的控制，由鉆機(jī)的控制程序進(jìn)行控制。因此θ1，θ5和相關(guān)參數(shù)d1=55mm不納入算法程序中，利用遺傳算法進(jìn)行運(yùn)動(dòng)學(xué)逆解求解，運(yùn)行十次得到結(jié)果，如表2所示。

表2 求解的關(guān)節(jié)變量值Tab.2 The Solved Joint Variable Value

由于遺傳算法具有一定的隨機(jī)性，因此表2中的十組解有一定的差異。但是可以看出，每一組解的同心度誤差均小于標(biāo)限定值0.9，均符合設(shè)計(jì)要求。這同時(shí)也表明了遺傳算法在處理該類數(shù)學(xué)問(wèn)題時(shí)具有較高的穩(wěn)定性。取同心度誤差最小的一組解（第五組解）作為此次逆解求解的最優(yōu)解，以作為后續(xù)控制參數(shù)。

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

為使機(jī)械手的運(yùn)動(dòng)過(guò)程直觀化，對(duì)采用遺傳算法求出的逆解進(jìn)行檢測(cè)，將機(jī)械手幾何模型導(dǎo)入U(xiǎn)G軟件運(yùn)動(dòng)學(xué)仿真模塊中進(jìn)行運(yùn)動(dòng)學(xué)仿真，采用點(diǎn)到點(diǎn)運(yùn)動(dòng)軌跡規(guī)劃，取機(jī)械手中每個(gè)連桿的質(zhì)心為跟蹤點(diǎn)，設(shè)置運(yùn)動(dòng)時(shí)間為160s，第5s到110s內(nèi)為各連桿繞自身關(guān)節(jié)轉(zhuǎn)動(dòng)，110s到140s內(nèi)為抓取器通過(guò)移動(dòng)，將鉆桿送上鉆機(jī)[11]。將2.2節(jié)求出的最優(yōu)解作為機(jī)械手的關(guān)節(jié)輸入量，通過(guò)UG軟件運(yùn)動(dòng)學(xué)模塊進(jìn)行位置仿真。通過(guò)觀察仿真結(jié)果，得出鉆桿抓取器停止時(shí)的中心點(diǎn)位置坐標(biāo)為（-578.08mm，0，756.07mm）。將其帶入式（3）中進(jìn)行計(jì)算，得出同心度誤差為p1=0.45mm，而遺傳算法計(jì)算出的同心度誤差p2=0.442mm，說(shuō)明通過(guò)遺傳算法求出的逆解可以使得鉆桿抓取器到達(dá)指定位置，且同心度誤差較小。同時(shí)該逆解值可以作為機(jī)器人后續(xù)控制研究工作的理論研究依據(jù)。

進(jìn)一步設(shè)定兩種驅(qū)動(dòng)方式，一種為機(jī)械手所有關(guān)節(jié)依次運(yùn)動(dòng)，另一種為機(jī)械手所有關(guān)節(jié)同步運(yùn)動(dòng)。將逆解分別作為兩種驅(qū)動(dòng)方式的輸入源進(jìn)行仿真，得出兩種驅(qū)動(dòng)方式下的運(yùn)動(dòng)參數(shù)曲線，如圖3、圖4所示。

圖3 各運(yùn)動(dòng)部件質(zhì)心速度Fig.3 Centroid Velocity of Moving Parts

圖4 各運(yùn)動(dòng)部件質(zhì)心速度Fig.4 Centroid Acceleration of Moving Parts

圖 3 和圖 4 每幅圖中的（a）（b）（c）（d）描述了機(jī)械手各個(gè)連桿和鉆桿抓取器質(zhì)心的運(yùn)動(dòng)狀態(tài)。而每幅圖中的（d）最后一部分加速度曲線線形發(fā)生了變化，是因?yàn)樵诘?10s時(shí)鉆桿抓取器的運(yùn)動(dòng)方式發(fā)生了改變。經(jīng)過(guò)對(duì)比可以看出在相同的時(shí)間段內(nèi)，采用關(guān)節(jié)依次運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，各個(gè)運(yùn)動(dòng)部件的速度與加速度呈現(xiàn)疊加狀態(tài)，加速度的數(shù)值整體偏高。驅(qū)動(dòng)方式下連桿1的加速度最大值為1.6mm/s2，連桿2加速度最大值為1.78mm/s2，連桿3加速度最大值為1.93mm/s2，鉆桿抓取器加速度最大值為3.2mm/s2。而當(dāng)采用關(guān)節(jié)同步運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，所有的連桿和鉆桿抓取器的最大加速度值不到0.5mm/s2。由加速度曲線線形可以看出，采用關(guān)節(jié)依次運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，曲線變化極為不平滑，整機(jī)在運(yùn)動(dòng)過(guò)程中有可能因?yàn)闃?gòu)件加速度突變次數(shù)較多而顯得運(yùn)動(dòng)不穩(wěn)定。同時(shí)加之外界工況和振動(dòng)的影響會(huì)使得整機(jī)結(jié)構(gòu)的動(dòng)態(tài)穩(wěn)定性較差，長(zhǎng)期下去會(huì)嚴(yán)重影響設(shè)備零部件的剛性和整機(jī)結(jié)構(gòu)的穩(wěn)定性，最終導(dǎo)致設(shè)備提前損壞。相比之下，采用關(guān)節(jié)同步運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，速度和加速度曲線變化平穩(wěn)，且加速度數(shù)值較?。ㄋ械倪B桿和鉆桿抓取器的最高加速度不到0.5mm/s2），同時(shí)加速度線形平滑且數(shù)值分布均勻，故機(jī)械手的整個(gè)運(yùn)動(dòng)過(guò)程將更為平穩(wěn)。從圖4的加速度曲線對(duì)比中還可以看出，采用關(guān)節(jié)同步運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，工作時(shí)間段內(nèi)加速度的突變次數(shù)遠(yuǎn)低于采用關(guān)節(jié)依次運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)的加速度突變次數(shù)，這意味著在同等的工作時(shí)間段內(nèi)，關(guān)節(jié)所受到的剛性沖擊較小，其不會(huì)因自身運(yùn)動(dòng)沖擊頻繁而過(guò)早發(fā)生壞損。綜上分析可知，采用關(guān)節(jié)同步運(yùn)動(dòng)驅(qū)動(dòng)方式時(shí)，機(jī)械手的運(yùn)動(dòng)狀態(tài)相對(duì)較好。

5 結(jié)論

（1）結(jié)合工程鉆機(jī)機(jī)器人的結(jié)構(gòu)和工況條件，設(shè)計(jì)了一種擁有5個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)和1個(gè)移動(dòng)關(guān)節(jié)的六自由度全液壓驅(qū)動(dòng)鉆桿裝卸機(jī)械手，實(shí)現(xiàn)了鉆機(jī)鉆桿裝卸的自動(dòng)化，提高了鉆機(jī)的總體工作效率。

（2）采用D-H方法建立了六自由度鉆桿裝卸機(jī)械手的運(yùn)動(dòng)學(xué)方程，以鉆桿與鉆桿安裝孔間的同心度誤差作為目標(biāo)函數(shù)，利用遺傳算法對(duì)運(yùn)動(dòng)學(xué)方程進(jìn)行了逆解求解。進(jìn)一步將逆解作為輸入?yún)?shù)，在UG軟件中對(duì)機(jī)械手的運(yùn)動(dòng)學(xué)進(jìn)行了仿真分析，仿真所得機(jī)械手鉆桿抓取器器中心點(diǎn)位移曲線規(guī)律驗(yàn)證了運(yùn)動(dòng)學(xué)方程的正確性和算法設(shè)計(jì)的合理性，為機(jī)械手的控制研究提供了理論基礎(chǔ)。

（3）提出了同步運(yùn)動(dòng)和依次運(yùn)動(dòng)兩種機(jī)械手關(guān)節(jié)驅(qū)動(dòng)方案，對(duì)兩種方案下鉆桿裝卸機(jī)械手的運(yùn)動(dòng)學(xué)過(guò)程進(jìn)行了仿真分析，通過(guò)對(duì)比分析表明，同步運(yùn)動(dòng)方式可使鉆桿裝卸機(jī)械手獲得較好的速度和加速度性能，優(yōu)于依次運(yùn)動(dòng)方式，為關(guān)節(jié)驅(qū)動(dòng)方式的選取提供了依據(jù)。