化肥碼垛機器人的軌跡規(guī)劃及仿真試驗

郭瑞峰， 彭戰(zhàn)奎， 張文輝， 彭光宇

(西安建筑科技大學(xué)機電工程學(xué)院，陜西西安 710055)

目前化肥行業(yè)常見的碼垛方式有3種，分別是人工碼垛、傳統(tǒng)機械碼垛、碼垛機器人碼垛[1]。雖然化肥碼垛機器人的價格比前面2種要高，但在后期的使用過程中它更能節(jié)省企業(yè)的生產(chǎn)成本。因為碼垛機器人有以下7個特點：(1)化肥碼垛機器人工作8 h可代替人工3～4人，1年可節(jié)省人力成本數(shù)十萬元。(2)能耗低。傳統(tǒng)機械碼垛的耗能約 26 kW，而碼垛機器人的耗能僅需5 kW左右。(3)結(jié)構(gòu)簡單、零部件少。因此其故障率低、魯棒性好、易于保養(yǎng)和維修[1]。(4)適應(yīng)性好。當(dāng)抓取物品的尺寸、形狀、空間位置發(fā)生改變或末端操作器更換時，只要修改控制程序或重新標(biāo)定即可實現(xiàn)，而不會影響正常的生產(chǎn)效率[2]。(5)人機界面，便于操作。(6)占用空間少，便于流水化生產(chǎn)線的布置。(7)碼垛機器人碼垛精度更高，可達(dá)±3 mm，減少塌垛現(xiàn)象。

綜合以上因素，本研究以實驗室自主設(shè)計的四自由度化肥碼垛機器人為例，通過幾何法與Modified-DH模型組合求解的方式，推導(dǎo)該機器人正運動學(xué)方程。在逆運動學(xué)求解時，通過Atan2函數(shù)及各關(guān)節(jié)運動范圍，求出其可行解；針對逆解的多解問題，利用最短行程的原則進(jìn)行優(yōu)化，得到各關(guān)節(jié)角的最優(yōu)解[3-4]。為了保證機器人精確、流暢、連續(xù)、平穩(wěn)地碼放化肥包裝袋，采用“5-3-5”法進(jìn)行軌跡規(guī)劃。最后通過ADAMS仿真分析驗證了理論方法的正確性及機械手設(shè)計的合理性，完成化肥碼垛機器人的試制，結(jié)果表明該碼垛機器人可以滿足設(shè)計要求。

1 化肥碼垛機器人結(jié)構(gòu)分析

該機器人可碼垛化肥包裝袋尺寸參數(shù)為960 mm×600 mm，一次性抓取總質(zhì)量達(dá)到160 kg，一次性抓取包裝袋的數(shù)量為2袋。其結(jié)構(gòu)示意見圖1。

該化肥碼垛機器人有4個自由度，具有如下優(yōu)點[5-6]：(1)四邊形機構(gòu)具有誤差自補償作用，Ⅱ驅(qū)動軸的運動通過四邊形機構(gòu)傳遞到末端執(zhí)行器會更精確，在一定程度上保證了機器人末端執(zhí)行器定位位置的準(zhǔn)確性；(2)整機質(zhì)量分布得到了優(yōu)化，增強了碼垛機器人的穩(wěn)定性；(3)伺服電機下移到回轉(zhuǎn)平臺上，增加了電機安裝位置的靈活性，減小了臂部關(guān)節(jié)的重力和轉(zhuǎn)動慣量，實現(xiàn)小臂輕量化；(4)在該機構(gòu)的作用下，機器人的承載能力得以提高[7]。

2 Modified-DH模型

機器人運動學(xué)研究中，運動學(xué)正解的求解常使用 Denavit和Hartenberg在1955年提出的4參數(shù)DH模型[2]。Paul在1981年證明了其對機器人運動學(xué)分析的價值，并提出Paul系統(tǒng)，由于被機器人廣泛使用而發(fā)展成為了標(biāo)準(zhǔn)DH模型[2-3]。然而標(biāo)準(zhǔn)DH模型主要是針對串行結(jié)構(gòu)的機器人進(jìn)行運動學(xué)分析，當(dāng)用標(biāo)準(zhǔn)DH模型來處理樹狀結(jié)構(gòu)或者閉環(huán)結(jié)構(gòu)機器人的時候會產(chǎn)生歧義[4，8]。1984年Craig提出一種Modified-DH模型[4]，其使用Craig連桿約束，因此又稱為Craig系統(tǒng)。該模型具有以下優(yōu)點：(1)可以用統(tǒng)一的定義來處理串行結(jié)構(gòu)、樹結(jié)構(gòu)和閉環(huán)結(jié)構(gòu)，具有更好的通用性。(2)其使用修改的DH參數(shù)，從機械結(jié)構(gòu)的簡化模型方面來講，其參數(shù)更加清晰、真實。但由于國內(nèi)機器人行業(yè)普遍采用標(biāo)準(zhǔn)DH模型處理串聯(lián)機器人，較少涉及樹結(jié)構(gòu)和閉環(huán)結(jié)構(gòu)的機器人，因此Modified-DH模型的論述較少。

Modified-DH模型的連桿坐標(biāo)系描述如圖2所示，將它與標(biāo)準(zhǔn)DH模型進(jìn)行對比得到其基本差異如下：(1)坐標(biāo)系依附到連桿的位置不同。Modified-DH模型坐標(biāo)系i-1固連于連桿i-1上，坐標(biāo)原點Oi-1位于關(guān)節(jié)軸i-1上，而不像標(biāo)準(zhǔn)DH那樣坐標(biāo)系i-1固連于連桿i上，坐標(biāo)原點Oi-1位于關(guān)節(jié)軸i上。(2)所執(zhí)行的轉(zhuǎn)換順序不同。根據(jù)Craig的連桿約定描述，變換矩陣由以下操作順序給出。

相鄰連桿坐標(biāo)系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齊次變換步驟如下：(1)繞xi-1軸旋轉(zhuǎn)αi-1，使zi-1和zi平行；(2)沿xi-1軸平移ai-1，使zi-1和zi共線；(3)繞zi-1軸旋轉(zhuǎn)θi，使xi-1與Oi和Oi+1之間的連線重合；(4)沿zi軸平移di，使xi-1與xi重合。

因此Modified-DH模型的連桿坐標(biāo)系Oi-1-xi-1yi-1zi-1到Oi-xiyizi的齊次變換通式為

i-1Ti=Rot(xi-1,αi-1)Trans(xi-1,ai-1)Trans(zi,θi)Rot(zi,di)；

(1)

(2)

3 化肥碼垛機器人運動學(xué)分析

3.1 化肥碼垛機器人正運動學(xué)計算

由于Modified-DH模型可以用統(tǒng)一的定義來處理該化肥碼垛機器人，因此本節(jié)采用Modified-DH模型求解的方式，推導(dǎo)該機器人正運動學(xué)方程。

機器人機構(gòu)運動簡圖如圖3所示，圖中虛線所示為小臂處于水平位置時的機器人機構(gòu)狀態(tài)。主傳動系統(tǒng)包括大臂前臂AF、短桿AD、大臂后臂DE和小臂EF組成的平行四邊形機構(gòu)ADEF，以及短桿AD、連桿CD和曲柄BC組成的鉸鏈四桿機構(gòu)ABCD2個并聯(lián)機構(gòu)。為將這2個并聯(lián)機構(gòu)串聯(lián)化，須要知道E點相對于B點的坐標(biāo)[7-9]，具體做法如下。

首先如圖3所示建立局部坐標(biāo)系B-xy。設(shè)各關(guān)節(jié)點坐標(biāo)：A(300，300)，B(0，0)，C(xC，yC)，E(xE，yE)。根據(jù)幾何法可得：

(3)

(4)

(5)

聯(lián)立公式(3)～(5)，可得：

(6)

式中：α等于連桿CD與坐標(biāo)系y的夾角，且α∈(0，90°)。

在鉸鏈四桿機構(gòu)ABCD中可得γ角的正余弦如下：

(7)

根據(jù)三角函數(shù)關(guān)系，消去變量γ得：

(8)

根據(jù)公式(8)及A、D2點坐標(biāo)，求出夾角α的正余弦：

(9)

通過公式(6)、(9)可以得出E點相對于B點的關(guān)系式僅與關(guān)節(jié)角θ2及θ3相關(guān)，因此可以將并聯(lián)機構(gòu)串聯(lián)化。

根據(jù)圖3建立機器人Modified-DH連桿坐標(biāo)系見圖4，對于Modified-DH模型，基坐標(biāo)系O-x0y0z0可以任意設(shè)定[4]，通常為了簡化計算，設(shè)定z0軸沿D(xD,yD)關(guān)節(jié)軸1的方向。各關(guān)節(jié)Modified-DH模型連桿參數(shù)見表1。

表1 Modofied-DH模型連桿參數(shù)

將表1中Modified-DH模型連桿參數(shù)帶入公式(2)，可得0A1、1A2、2A3、3A4、4AG。將各連桿矩陣連乘得到0TG。

0TG=0A1·1A2·2A3·3A4·4AG。

(10)

所以，機器人末端執(zhí)行器的位置為下列矩陣的第4列：

(11)

機器人末端執(zhí)行器的姿態(tài)矩陣為

(12)

式中：R0為基坐標(biāo)系的初始姿態(tài)矩陣。

聯(lián)立式(6)、(7)、(10)～(12)，可得化肥碼垛機器人正運動學(xué)方程如下：

(13)

式(13)中：

nx=-s(θ1+θ4)；

ox=ny=c(θ1+θ4)；

oy=s(θ1+θ4)；

(14)

(15)

(16)

3.2 化肥碼垛機器人逆運動學(xué)計算

運動學(xué)逆解時，文獻(xiàn)[7]采用單變量反正切函數(shù)，這樣不僅可能會造成解的丟失，而且角的精度也難以保證[9]。這里通過先確定各關(guān)節(jié)角的正弦和余弦，然后應(yīng)用Atan2函數(shù)求關(guān)節(jié)角。

將公式(14)、(15)兩邊對應(yīng)相除可得：

θ1=Atan2(Hy,Hx)。

(17)

將公式(14)、(16)移位化簡，可得：

(18)

對公式(18)利用三角函數(shù)關(guān)系，可以消去變量θ3。結(jié)合公式(9)、(17)可求出sθ2、cθ2：

θ2=Atan2(sθ2,cθ2)。

(19)

此時公式(18)只有θ3仍是未知量，因此可以求出sθ3、cθ3：

θ3=Atan2(sθ3,cθ3)。

(20)

假設(shè)工作過程中末端執(zhí)行器的姿態(tài)保持不變，則：

θ1+θ4=0；
θ4=-Atan2(Hy0,Hx0)。

(21)

4 軌跡規(guī)劃及ADAMS仿真分析

4.1 軌跡規(guī)劃

碼垛作業(yè)如下：1 h碼放900袋，從生產(chǎn)線指定位置A(2 500，0，1 200)抓取化肥包裝袋、經(jīng)提升位置B(2 500，0，1 200)、下降位置C(3 500，0，1 200)將化肥包裝袋碼放在位置D(3 500，0，800)，軌跡規(guī)劃按照“PTP”的運動形式，要求整個碼放過程連續(xù)、平穩(wěn)運行?；蚀a垛機器人尺寸參數(shù)[7]見表2。

碼垛作業(yè)的時間分布如下：

ΓA-B=2 s、ΓB-C=4 s、ΓC-D=2 s。

通過Matlab編寫逆解程序，得到各關(guān)節(jié)角的可行解見表3。

對表3中各關(guān)節(jié)角的可行解，按照最短行程的原則(每個關(guān)節(jié)總的移動量最小)進(jìn)行優(yōu)化，可以得到各關(guān)節(jié)角的最優(yōu)解見表4。

表2 化肥碼垛機器人尺寸參數(shù)

表3 各關(guān)節(jié)角的可行解

表4 各關(guān)節(jié)角的最優(yōu)解

為了保證機器人精確、流暢、連續(xù)、平穩(wěn)的碼放化肥包裝袋[5]，須要對表4中的數(shù)據(jù)進(jìn)行連續(xù)化處理。本研究采用“5-3-5”法對軌跡進(jìn)行插補運算[5，10]，最終得到各關(guān)節(jié)軸角位移曲線見圖5。

4.2 ADAMS仿真分析

(1)利用SolidWorks建立機器人三維模型；然后導(dǎo)入ADAMS軟件中進(jìn)行裝配并添加相應(yīng)的約束[10-11]。(2)將圖5中各關(guān)節(jié)角位移曲線的數(shù)據(jù)導(dǎo)入ADAMS中；利用AKISPL函數(shù)進(jìn)行關(guān)節(jié)角位移擬合，然后添加到相應(yīng)的關(guān)節(jié)角驅(qū)動中。(3)根據(jù)作業(yè)要求，設(shè)置求解器仿真時間為8 s，進(jìn)行運動學(xué)仿真，仿真過程中的某一時刻狀態(tài)見圖6。(4)利用ADAMS的測量功能及后處理功能，得到機器人末端執(zhí)行器的仿真位移曲線見圖7-a。(5)將圖5中的數(shù)據(jù)代入到公式(14)、(15)、(16)當(dāng)中，計算得到理論位移曲線見圖7-b。(6)將ADAMS仿真位移曲線與理論位移曲線進(jìn)行對比得到圖7-c。運用Modified-DH模型計算得到的理論位移曲線與ADAMS仿真位移曲線基本重合，驗證了該化肥碼垛機器人運動學(xué)求解的正確性。

5 結(jié)論

本研究以實驗室自主設(shè)計的四自由度化肥碼垛機器人為例，通過幾何法與Modified-DH模型組合求解的方式，推導(dǎo)該機器人正運動學(xué)方程。在逆運動學(xué)求解時，通過Atan2函數(shù)及各關(guān)節(jié)運動范圍，求出其可行解；針對逆解的多解問題，利用最短行程的原則進(jìn)行優(yōu)化，得到各關(guān)節(jié)角的最優(yōu)解。為了保證機器人精確、流暢、連續(xù)、平穩(wěn)地碼放化肥包裝袋采用“5-3-5”法進(jìn)行軌跡規(guī)劃；通過ADAMS進(jìn)行單周期仿真分析，驗證了理論方法的正確性及機械手設(shè)計的合理性。最后完成化肥碼垛機器人的試制，結(jié)果表明該碼垛機器人可以滿足設(shè)計要求。