李永梅，張錦濤，馬鎰明

(東南大學(xué)成賢學(xué)院，江蘇 南京 210088)

0 引言

直角碼垛機(jī)器人是現(xiàn)代工廠中比較常見(jiàn)的一種機(jī)器人，它可以搬運(yùn)比較重或者種類繁雜的物體，不論是靜態(tài)搬運(yùn)還是流水線上的動(dòng)態(tài)搬運(yùn)，都能夠節(jié)省人力成本，提高工作效率。對(duì)于一些重復(fù)枯燥的搬運(yùn)活動(dòng)，只需要給機(jī)器人示教或者通過(guò)程序控制，便可以使其快速、高效地完成。軌跡規(guī)劃對(duì)機(jī)器人的運(yùn)動(dòng)以及性能有重大影響，也是機(jī)器人的重要研究部分。1997年，為了驗(yàn)證工業(yè)機(jī)器人在焊接過(guò)程中軌跡的完整性，H.J.Yeh與K.Abdel-Malek提出軌跡規(guī)劃的概念。盧軍等[1]提出了一種基于視覺(jué)引導(dǎo)的軌跡規(guī)劃算法，可以自行調(diào)整躍度。楊利達(dá)[2]提出了一種梯形加減速控制算法與三次B樣條結(jié)合的方法，解決了機(jī)器人運(yùn)行時(shí)不平穩(wěn)、沖擊大等問(wèn)題。尚和平[3]提出了一種基于弦截法的S型曲線加減速規(guī)劃算法，提高了碼垛的效率和運(yùn)行的平穩(wěn)性。沈悅[4]利用PSO優(yōu)化算法對(duì)五次B樣條軌跡規(guī)劃進(jìn)行修改，能夠得到脈動(dòng)-時(shí)間較優(yōu)的軌跡，提高了六自由度機(jī)器人的運(yùn)行效率，改善了機(jī)器人運(yùn)行的平穩(wěn)性。姜爽[5]采用改進(jìn)差分進(jìn)化算法來(lái)解決機(jī)器人時(shí)間最優(yōu)規(guī)劃問(wèn)題。本文通過(guò)以上幾種方法對(duì)機(jī)器人軌跡進(jìn)行研究比較，旨在得到機(jī)器人末端平穩(wěn)快速的軌跡。

1 直角坐標(biāo)碼垛機(jī)器人運(yùn)動(dòng)過(guò)程

3自由度直角坐標(biāo)碼垛機(jī)器人分為X軸、Y軸、Z軸，末端執(zhí)行器為單個(gè)真空吸盤(pán)(與真空泵配合使用，利用真空泵抽走吸盤(pán)與工件之間的空氣，產(chǎn)生壓強(qiáng)差，從而將工件提起)[6]。此處工件為30 mm×30 mm×30 mm的方塊。碼垛機(jī)器人抓取物塊的工作路徑如圖1所示。

圖1 機(jī)器人工作路徑

其工作的整個(gè)流程為：①機(jī)器人從起始A點(diǎn)正上方的合適位置，沿Z軸向下運(yùn)動(dòng)到B點(diǎn)抓取方塊；②機(jī)器人跟小方塊一起從B點(diǎn)沿Z軸上升到A點(diǎn)；③機(jī)器人與小方塊從A點(diǎn)沿弧線或近圓曲線經(jīng)過(guò)最高點(diǎn)C點(diǎn)運(yùn)動(dòng)到D點(diǎn)；④機(jī)器人與小方塊從D點(diǎn)沿著Z軸下降到E點(diǎn)，放置小方塊；⑤機(jī)器人從E點(diǎn)沿著Z軸上升到D點(diǎn)；⑥運(yùn)動(dòng)回到起始A點(diǎn)正上方。

流程②到流程④，這三個(gè)流程是整個(gè)碼垛工作的核心。機(jī)器人在B點(diǎn)和E點(diǎn)的速度和加速度最好為零，這樣機(jī)器人才能可靠地從B點(diǎn)抓取目標(biāo)方塊以及將目標(biāo)方塊放到E點(diǎn)。

2 不同軌跡規(guī)劃方法比較

常用軌跡規(guī)劃方法有三次多項(xiàng)式、五次多項(xiàng)式,其一個(gè)周期T內(nèi)的位移、速度、加速度的曲線示意圖分別如圖2、圖3所示。三次多項(xiàng)式軌跡規(guī)劃是比較簡(jiǎn)單的一種規(guī)劃算法，根據(jù)其位移的表達(dá)式是三次多項(xiàng)式而得名。圖2和圖3中，d代表位移，v代表速度，a代表加速度。

圖2 三次多項(xiàng)式位移、速度、加速度曲線 圖3 五次多項(xiàng)式位移、速度、加速度曲線

從圖2中可以看出三次多項(xiàng)式的位移、速度、加速度曲線都是光滑連續(xù)的，說(shuō)明其在運(yùn)動(dòng)過(guò)程中的沖擊較小，但是其加速度在初始點(diǎn)和終止點(diǎn)時(shí)不為零，導(dǎo)致在這兩處的沖擊較大并且會(huì)使機(jī)器人產(chǎn)生抖振現(xiàn)象。從圖3中可以看出五次多項(xiàng)式得出的曲線更加光滑，且不存在加速度突變的問(wèn)題。但是由于它的高次方，很容易出現(xiàn)位移、速度、加速度超調(diào)的現(xiàn)象。

關(guān)于速度、加速度超調(diào)問(wèn)題，可以采用梯形速度規(guī)劃來(lái)控制，如圖4所示。假設(shè)允許最大加速度為amax，允許最大速度為vmax，工作時(shí)間為t3。梯形速度曲線有勻加速運(yùn)動(dòng)、勻速運(yùn)動(dòng)、勻減速運(yùn)動(dòng)3段。其中勻減速段和勻加速段的加速度的絕對(duì)值相同，令其為a均；勻速段的速度為v均；只要保證a均≤amax，v均≤vmax即可。

從圖4中可以看出，雖然梯形速度規(guī)劃能夠調(diào)節(jié)速度和加速度，但是其速度曲線不光滑，加速度曲線不連續(xù)，在初始時(shí)刻和終止時(shí)刻t3，以及t1和t2處均會(huì)引起沖擊和抖振。

圖4 梯形速度規(guī)劃的速度和加速度曲線 圖5 基于正弦函數(shù)的S型軌跡規(guī)劃位移、速度、加速度曲線

3 基于正弦函數(shù)的S型曲線規(guī)劃

由于正弦函數(shù)的各階導(dǎo)函數(shù)及其積分的曲線都是光滑連續(xù)的，在軌跡規(guī)劃中應(yīng)用廣泛。如果用正弦函數(shù)曲線將梯形規(guī)劃的加速曲線開(kāi)始處的突變變?yōu)閺牧氵B續(xù)緩步地上升到加速度a均值，在t1到t2之間的曲線變?yōu)閺募铀俣萢均連續(xù)緩步地下降到加速度-a均，t3處的突變變?yōu)閺募铀俣?a均連續(xù)緩步地上升到0，就能使加速度曲線變?yōu)檫B續(xù)的。這樣既能解決梯形規(guī)劃中沖擊和抖振的問(wèn)題，又能凸顯它的優(yōu)勢(shì)。

本文通過(guò)研究S型曲線規(guī)劃的一個(gè)周期來(lái)探討。設(shè)規(guī)劃曲線的一個(gè)周期為T，初始位移值為d0，終止位移值為de，時(shí)間用t表示。對(duì)位移d、時(shí)間t進(jìn)行歸一化處理，即：

(1)

(2)

其中：d(ρ)為位移的母函數(shù)，ρ為時(shí)間的歸一值，且0≤d(ρ)≤1，0≤ρ≤1。

那么位移為：

d=(de-d0)d(ρ)+d0.

(3)

對(duì)位移求一階導(dǎo)數(shù)得速度為：

(4)

對(duì)位移求二階導(dǎo)數(shù)得加速度為：

(5)

4 碼垛機(jī)器人末端和軸的軌跡規(guī)劃及仿真

對(duì)機(jī)器人碼垛工作的3個(gè)核心流程②～④進(jìn)行仿真。規(guī)定機(jī)器人末端必須依次經(jīng)過(guò)點(diǎn)B(86，450，30)、點(diǎn)A(86，450，140)、點(diǎn)C(70，300，250)、點(diǎn)D(54，150，140)以及點(diǎn)E(54，150，30)。假設(shè)3個(gè)流程的總時(shí)長(zhǎng)為3 s。對(duì)于直角坐標(biāo)碼垛機(jī)器人而言，末端的軌跡可以看成是3個(gè)軸的軌跡的合成。因此，對(duì)末端進(jìn)行軌跡規(guī)劃本質(zhì)上就是對(duì)X、Y、Z三個(gè)軸進(jìn)行軌跡規(guī)劃。Z軸在三個(gè)流程中始終在運(yùn)動(dòng)，因此設(shè)Z軸在三個(gè)流程的運(yùn)動(dòng)時(shí)間各為1 s。其中針對(duì)第③個(gè)流程，將其分為兩個(gè)部分：從點(diǎn)A到最高點(diǎn)C和從最高點(diǎn)C到點(diǎn)D，每個(gè)部分各用時(shí)0.5 s。根據(jù)上一節(jié)所述的S型曲線規(guī)劃，令其在B點(diǎn)、A點(diǎn)、C點(diǎn)、D點(diǎn)和E點(diǎn)的Z軸速度以及加速度都為零，可以將每一部分軌跡的每一個(gè)點(diǎn)的X、Y、Z坐標(biāo)都表示出來(lái)，然后將每一部分的點(diǎn)連起來(lái)就能得到末端的軌跡?；谶@個(gè)思想，可以先用MATLAB分別計(jì)算出B-A-C-D-E五個(gè)點(diǎn)形成的四段軌跡上的若干點(diǎn)，最后用plot3函數(shù)將這些點(diǎn)擬合成末端軌跡，如圖6所示。

圖6 機(jī)器人末端軌跡

5 結(jié)論

對(duì)直角坐標(biāo)碼垛機(jī)器人選擇了運(yùn)動(dòng)路徑，通過(guò)分析碼垛流程確定出重點(diǎn)研究的3個(gè)流程及其軌跡，通過(guò)比較常用的軌跡規(guī)劃算法的優(yōu)缺點(diǎn)，引出基于正弦函數(shù)的S型曲線規(guī)劃方法。將該算法運(yùn)用到直角坐標(biāo)碼垛機(jī)器人的軌跡規(guī)劃上，讓機(jī)器人末端通過(guò)指定的B-A-C-D-E五個(gè)點(diǎn)，分別求出這五個(gè)點(diǎn)形成的四段軌跡上的三個(gè)軸的軌跡規(guī)劃公式，并根據(jù)得出的公式利用MATLAB求出四段軌跡的若干點(diǎn)的三維坐標(biāo)，通過(guò)plot3函數(shù)擬合這些點(diǎn)，最終得到末端平穩(wěn)快速且正確的軌跡規(guī)劃。