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

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

1 引言

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

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

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

2.1 三維模型建立

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

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

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

對六自由度鉆桿裝卸機(jī)械手進(jìn)行運(yùn)動學(xué)研究是機(jī)械手設(shè)計(jì)一個重要步驟。當(dāng)已知機(jī)械手所有關(guān)節(jié)變量時(shí)，求出機(jī)械手的運(yùn)動學(xué)方程，就能描述出鉆桿抓取器在空間中的準(zhǔn)確位姿，此為運(yùn)動學(xué)正解。在鉆桿抓取器特定的位姿下對機(jī)械手的運(yùn)動學(xué)方程求解，得出鉆桿抓取器在此位姿下的關(guān)節(jié)變量，此為運(yùn)動學(xué)逆解。之后可以進(jìn)一步得出機(jī)械手每個運(yùn)動構(gòu)件的速度，加速度，關(guān)節(jié)變量等參數(shù)，能夠?yàn)闄C(jī)械手的動態(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ī)械手各個連桿D-H坐標(biāo)變化矩陣為：

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

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

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

3.1 遺傳算法介紹

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

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

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

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

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

設(shè)s1=45mm，s2=42mm，由式（2）計(jì)算出鉆桿與鉆桿安裝孔間允許的最大同心度誤差pmax=0.9mm。為使鉆桿抓取器運(yùn)動到預(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)系。因此將三個關(guān)節(jié)變量θ2=［40°，50°］、θ3=［55°，60°］、θ4=［90°，100°］作為約束條件。對式（3）求解可以得出能夠使鉆桿抓取器到達(dá)指定位置的關(guān)節(jié)變量。

3.2.2 算法過程

采用二進(jìn)制編碼，將解的精度設(shè)定為0.001。關(guān)節(jié)變量θ2、θ3、θ4的編碼長度分別為 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。則總的編碼長度為n1+n2+n3=41。

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

在圖2中，各連桿長度分別為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ī)的工作角度，將這兩個角度脫離機(jī)械手的控制，由鉆機(jī)的控制程序進(jìn)行控制。因此θ1，θ5和相關(guān)參數(shù)d1=55mm不納入算法程序中，利用遺傳算法進(jìn)行運(yùn)動學(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é)問題時(shí)具有較高的穩(wěn)定性。取同心度誤差最小的一組解（第五組解）作為此次逆解求解的最優(yōu)解，以作為后續(xù)控制參數(shù)。

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

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

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

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

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

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

5 結(jié)論

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

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

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