普亞松, 史耀耀, 藺小軍, 張文斌, 陳振

(1.西北工業(yè)大學(xué) 機(jī)電學(xué)院, 陜西 西安 710072; 2.紅河學(xué)院 工學(xué)院, 云南 蒙自 661199)

工業(yè)機(jī)器人廣泛應(yīng)用于搬運(yùn)、碼垛、噴漆、焊接、裝配等領(lǐng)域，隨著工業(yè)機(jī)器人技術(shù)的發(fā)展，工業(yè)機(jī)器人逐漸用于打磨拋光、監(jiān)測、自動(dòng)檢測系統(tǒng)等領(lǐng)域，工作性能要求更高，因而對工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃提出更高要求。工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃不僅對末端執(zhí)行器的位移、速度有著嚴(yán)格的要求，而且良好的軌跡規(guī)劃還可以保證良好的運(yùn)動(dòng)平穩(wěn)性和較小的機(jī)械磨損，提高機(jī)器人工作性能[1]。

工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃分為2種類型，一種是作業(yè)空間中末端執(zhí)行器運(yùn)動(dòng)規(guī)劃，另一種是關(guān)節(jié)空間中關(guān)節(jié)運(yùn)動(dòng)規(guī)劃。作業(yè)空間中末端執(zhí)行器運(yùn)動(dòng)規(guī)劃，需要反復(fù)求解逆運(yùn)動(dòng)方程來計(jì)算各關(guān)節(jié)的角位移和角速度，計(jì)算量比較大，且有時(shí)會(huì)出現(xiàn)奇異點(diǎn)，不能正確驅(qū)動(dòng)各關(guān)節(jié)運(yùn)動(dòng)。關(guān)節(jié)空間中關(guān)節(jié)運(yùn)動(dòng)規(guī)劃，只需要進(jìn)行少數(shù)目標(biāo)點(diǎn)位姿與速度的逆運(yùn)動(dòng)學(xué)計(jì)算，極大減少計(jì)算量，且不用考慮關(guān)節(jié)奇異點(diǎn)以及冗余問題，更適合工業(yè)機(jī)器人點(diǎn)到點(diǎn)的運(yùn)動(dòng)[2]。

幾種主要的工業(yè)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)規(guī)劃方法如下：拋物線過渡的線性規(guī)劃優(yōu)勢在于線性階段關(guān)節(jié)轉(zhuǎn)速保持恒定，但是必須保證有足夠的過渡時(shí)間，并且角加速度有間斷點(diǎn)。三次多項(xiàng)式規(guī)劃方法簡單，計(jì)算量少，但對于速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)，連接點(diǎn)處的角加速度會(huì)出現(xiàn)間斷點(diǎn)，角加速度不連續(xù)，機(jī)器臂容易產(chǎn)生顫抖，影響作業(yè)性能和使命壽命。五次多項(xiàng)式規(guī)劃角加速度是連續(xù)的，但需要預(yù)先設(shè)定關(guān)節(jié)在各目標(biāo)點(diǎn)的角加速度值，如果設(shè)置不當(dāng)，角速度曲線會(huì)出現(xiàn)往復(fù)波動(dòng)現(xiàn)象，不利于關(guān)節(jié)運(yùn)動(dòng)控制。然而，預(yù)先設(shè)置合適的角加速度值較為困難[3]。并且五次多項(xiàng)次數(shù)比較高，角速度曲線和角加速度曲線變化也比較激烈，容易使角速度和角加速度峰值較大[4]。三次B樣條曲線也常用于工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃[5-6]。三次B樣條曲線用于機(jī)器人關(guān)節(jié)速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃時(shí)， 在連接點(diǎn)處角加速度會(huì)出現(xiàn)間斷點(diǎn)，角加速度不連續(xù)。有研究人員對三次B樣條曲線進(jìn)行參數(shù)控制，對B樣條曲線一階導(dǎo)數(shù)約束的同時(shí)，實(shí)現(xiàn)了各型值點(diǎn)處G2連續(xù)，但并不是C2連續(xù)，二階導(dǎo)數(shù)并不是真正的連續(xù)[7-8]。

為了進(jìn)一步解決較復(fù)雜的多個(gè)目標(biāo)點(diǎn)關(guān)節(jié)規(guī)劃，Niku[9]提出4-3-4多項(xiàng)式樣條拼接方法，該方法確保了連接點(diǎn)處角加速度的連續(xù)性，但只能指定首、末2個(gè)目標(biāo)點(diǎn)處的角速度，中間目標(biāo)點(diǎn)處的角速度不能任意指定。Xu等[10]提出3-5-3多項(xiàng)式樣條拼接方法，該方法能夠任意指定中間目標(biāo)點(diǎn)處的角速度，并且角加速度是連續(xù)的，但是當(dāng)目標(biāo)點(diǎn)超過5個(gè)時(shí)，需要預(yù)先設(shè)定目標(biāo)點(diǎn)的角加速度值，角加速度預(yù)設(shè)定值不適當(dāng)會(huì)引起角速度較大波動(dòng)。

除了上述方法外，許多研究人員對工業(yè)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)規(guī)劃也做了相關(guān)研究。田國富等[11]采用內(nèi)插法替代五次多項(xiàng)式插值對機(jī)械臂關(guān)節(jié)進(jìn)行連續(xù)運(yùn)動(dòng)規(guī)劃，得到C2連續(xù)的平滑運(yùn)動(dòng)規(guī)劃，但該方法沒有對關(guān)節(jié)中間目標(biāo)點(diǎn)的角速度進(jìn)行約束，無法指定中間點(diǎn)的角速度。施祥玲等[12]采用五次非均勻有理樣條曲線(NURBS)對關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，構(gòu)造了高階連續(xù)的關(guān)節(jié)運(yùn)動(dòng)軌跡。該方法忽略了NURBS曲線方程可能存在分母項(xiàng)為零的情況，規(guī)劃曲線有出現(xiàn)突變值的可能性。劉承立等[13]采用三次多項(xiàng)式關(guān)節(jié)規(guī)劃，通過把2個(gè)目標(biāo)點(diǎn)按均勻時(shí)間劃分為5個(gè)點(diǎn)，1條規(guī)劃曲線變?yōu)?條規(guī)劃曲線，得到起點(diǎn)和終點(diǎn)加速度為0的平滑規(guī)劃曲線，但不能指定中間目標(biāo)點(diǎn)的角速度。Parikh等[14]采用七次多項(xiàng)式和九次多項(xiàng)式對機(jī)器人關(guān)節(jié)進(jìn)行運(yùn)動(dòng)規(guī)劃，關(guān)節(jié)運(yùn)動(dòng)路徑高階平滑，關(guān)節(jié)的速度和加速度連續(xù)，但是高階多項(xiàng)式不僅計(jì)算量較大，并且運(yùn)動(dòng)軌跡中會(huì)產(chǎn)生較大的加速度和速度峰值。

綜上所述，對于速度約束的點(diǎn)到點(diǎn)關(guān)節(jié)運(yùn)動(dòng)規(guī)劃，采用低階次運(yùn)動(dòng)規(guī)劃方法時(shí)，中間目標(biāo)點(diǎn)的角加速度不連續(xù)，會(huì)帶來附加沖擊和振動(dòng)，影響工業(yè)機(jī)器人的使用壽命和工作性能。采用高階次運(yùn)動(dòng)規(guī)劃方法時(shí)，雖然解決了角加速度連續(xù)的問題，但需要預(yù)先設(shè)定目標(biāo)點(diǎn)處的角加速度值。設(shè)定合適的角加速度值是個(gè)難點(diǎn)，容易引起角速度曲線較大波動(dòng)。再有，規(guī)劃方法的階次越高，運(yùn)動(dòng)曲線變化越激烈，角速度峰值和角加速度峰值較大，這些都不利于機(jī)器臂平穩(wěn)工作。本文提出一種三次多項(xiàng)式與四次多項(xiàng)式混合拼接規(guī)劃方法，對中間目標(biāo)點(diǎn)的速度進(jìn)行約束的同時(shí)，能夠保證中間點(diǎn)處加速度保持連續(xù)性，并且角速度波動(dòng)較小。

1 速度約束的點(diǎn)到點(diǎn)關(guān)節(jié)運(yùn)動(dòng)

工業(yè)機(jī)器人末端執(zhí)行器是最終執(zhí)行元件，是機(jī)器人作業(yè)的執(zhí)行器件。末端執(zhí)行器的運(yùn)動(dòng)模式主要有點(diǎn)到點(diǎn)(point to point,PTP)運(yùn)動(dòng)和連續(xù)路徑(continuous path,CP)運(yùn)動(dòng)。末端執(zhí)行器的運(yùn)動(dòng)是由機(jī)器人各關(guān)節(jié)共同運(yùn)動(dòng)合成的結(jié)果，末端執(zhí)行器的運(yùn)動(dòng)和關(guān)節(jié)有對應(yīng)的關(guān)系。

1.1 關(guān)節(jié)點(diǎn)到點(diǎn)運(yùn)動(dòng)

工業(yè)機(jī)器人點(diǎn)到點(diǎn)的運(yùn)動(dòng)模式適用于點(diǎn)焊、搬運(yùn)、裝卸、電路板插入元件等作業(yè)。末端執(zhí)行器從起點(diǎn)運(yùn)動(dòng)到終點(diǎn)，只需規(guī)定末端執(zhí)行器在起點(diǎn)與終點(diǎn)的位置和姿態(tài)，起點(diǎn)與終點(diǎn)之間末端執(zhí)行器的路徑和姿態(tài)并不作規(guī)定與要求。

當(dāng)末端執(zhí)行器進(jìn)行點(diǎn)到點(diǎn)的運(yùn)動(dòng)時(shí)，工業(yè)機(jī)器人轉(zhuǎn)動(dòng)的關(guān)節(jié)相應(yīng)地進(jìn)行點(diǎn)到點(diǎn)的轉(zhuǎn)動(dòng)。如圖1所示，以關(guān)節(jié)1為例,當(dāng)末端執(zhí)行器從起點(diǎn)PA運(yùn)動(dòng)到終點(diǎn)PB,關(guān)節(jié)1相應(yīng)地從起點(diǎn)A轉(zhuǎn)動(dòng)到終點(diǎn)B。

圖1 關(guān)節(jié)點(diǎn)到點(diǎn)運(yùn)動(dòng)

末端執(zhí)行器位姿與各關(guān)節(jié)角位移具有相應(yīng)函數(shù)關(guān)系,如(1)式所示。末端執(zhí)行器速度與各關(guān)節(jié)角速度也具有相應(yīng)的函數(shù)關(guān)系,如(2)式所示。

式中:[xyzαβγ]是末端執(zhí)行器的位姿;θi是各個(gè)關(guān)節(jié)角位移;f稱為各個(gè)關(guān)節(jié)轉(zhuǎn)角與末端執(zhí)行器位姿之間的正向運(yùn)動(dòng)函數(shù);[vxvyvzωxωyωz]是末端執(zhí)行器的線速度和角速度;ωi是各個(gè)關(guān)節(jié)角速度;J是速度雅克比矩陣。各種類型的機(jī)械臂正向運(yùn)動(dòng)函數(shù)f以及速度雅克比矩陣J是各不相同的。

根據(jù)末端執(zhí)行器的位姿和速度可以反求各關(guān)節(jié)的角位移和角速度,如(3)～(4)式所示,稱之為逆運(yùn)動(dòng)學(xué)[9]。

根據(jù)末端執(zhí)行器起點(diǎn)和終點(diǎn)的位姿和速度,通過逆運(yùn)動(dòng)學(xué)解可求得各關(guān)節(jié)的起點(diǎn)和終點(diǎn)的角位移值和角速度值,稱之為關(guān)節(jié)起點(diǎn)和終點(diǎn)的邊界約束條件。根據(jù)起點(diǎn)和終點(diǎn)的邊界約束條件,進(jìn)行各個(gè)關(guān)節(jié)運(yùn)動(dòng)規(guī)劃,各關(guān)節(jié)運(yùn)動(dòng)規(guī)劃最終合成為末端執(zhí)行器的運(yùn)動(dòng),流程如圖2所示。這種點(diǎn)到點(diǎn)運(yùn)動(dòng)模式下,末端執(zhí)行器在起點(diǎn)和終點(diǎn)的速度都為0,所以各關(guān)節(jié)對應(yīng)的起點(diǎn)和終點(diǎn)的角速度也為0,以圖1的關(guān)節(jié)1為例,ωa=ωb=0。

圖2 末端執(zhí)行器點(diǎn)到點(diǎn)運(yùn)動(dòng)與關(guān)節(jié)點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃

1.2 速度約束的點(diǎn)到點(diǎn)關(guān)節(jié)運(yùn)動(dòng)

當(dāng)末端執(zhí)行器點(diǎn)到點(diǎn)運(yùn)動(dòng)的起點(diǎn)和終點(diǎn)之間存在多個(gè)中間目標(biāo)點(diǎn),并且末端執(zhí)行器需要以規(guī)定的速度經(jīng)過中間目標(biāo)點(diǎn),對目標(biāo)點(diǎn)的位置和速度都要進(jìn)行約束。末端執(zhí)行器進(jìn)行速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)模式時(shí),各運(yùn)動(dòng)關(guān)節(jié)相應(yīng)進(jìn)行速度約束的點(diǎn)到點(diǎn)轉(zhuǎn)動(dòng),對關(guān)節(jié)中間目標(biāo)點(diǎn)的角位移和角速度進(jìn)行約束,才能滿足末端執(zhí)行器在目標(biāo)點(diǎn)處的位置和速度要求。

如圖3所示,末端執(zhí)行器從起點(diǎn)PA運(yùn)動(dòng)到終點(diǎn)PD,途中以指定的速度經(jīng)過2個(gè)中間目標(biāo)點(diǎn)PB和PC,各運(yùn)動(dòng)關(guān)節(jié)要進(jìn)行相應(yīng)的轉(zhuǎn)動(dòng)。以關(guān)節(jié)1為例,關(guān)節(jié)1對應(yīng)地從起點(diǎn)A轉(zhuǎn)動(dòng)到終點(diǎn)D,途經(jīng)中間目標(biāo)點(diǎn)B和C,在中間目標(biāo)點(diǎn)B點(diǎn)和C點(diǎn)角位移和角速度需要進(jìn)行約束,角速度值一般不為0,因此,關(guān)節(jié)1進(jìn)行的是角速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)。通過逆運(yùn)動(dòng)學(xué)求解,可以通過末端執(zhí)行器的位姿和速度求得關(guān)節(jié)各目標(biāo)點(diǎn)的角位移和角速度邊界約束條件,就可以對關(guān)節(jié)1的運(yùn)動(dòng)軌跡進(jìn)行分段規(guī)劃,對于其他關(guān)節(jié)也是同樣如此,最后各個(gè)關(guān)節(jié)共同合成末端執(zhí)行器的運(yùn)動(dòng)。

圖3 速度約束的關(guān)節(jié)點(diǎn)到點(diǎn)運(yùn)動(dòng)

2 關(guān)節(jié)運(yùn)動(dòng)規(guī)劃方法

工業(yè)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)規(guī)劃最常用的方法主要有三次多項(xiàng)式規(guī)劃、五次多項(xiàng)式規(guī)劃。

2.1 三次多項(xiàng)式運(yùn)動(dòng)規(guī)劃

多項(xiàng)式插值是一種較為常見的插值方法,用于離散點(diǎn)之間的插值,這與工業(yè)機(jī)器人點(diǎn)到點(diǎn)運(yùn)動(dòng)特點(diǎn)相符合,因此常用于工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃。三次多項(xiàng)式插值是工業(yè)機(jī)器人關(guān)節(jié)常采用的一種運(yùn)動(dòng)規(guī)劃方法,三次多項(xiàng)式插值方程如(5)式所示

θ(t)=a0+a1t+a2t2+a3t3

(5)

式中:t為時(shí)間變量;a0,a1,a2,a3是三次多項(xiàng)式插值方程的4個(gè)系數(shù),需要根據(jù)2個(gè)目標(biāo)插值點(diǎn)邊界約束條件求解這4個(gè)系數(shù)。

將初始時(shí)間、結(jié)束時(shí)間以及2個(gè)目標(biāo)點(diǎn)的角位移、角速度約束條件分別代入(5)式及其一階導(dǎo)數(shù)中,經(jīng)過經(jīng)整理可以得到求解系數(shù)的(6)式。

(6)

式中：t0為初始時(shí)間；t1為結(jié)束時(shí)間。θ0與θ1分別為2個(gè)目標(biāo)點(diǎn)的角位移；ω0與ω1分別為2個(gè)目標(biāo)點(diǎn)的角速度,這些值可以通過逆運(yùn)動(dòng)學(xué)求得。對于普通的點(diǎn)到點(diǎn)運(yùn)動(dòng),ω0和ω1都為0。對于速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng),ω0和ω1可以不為0。求得各系數(shù)之后,就可以得到滿足2個(gè)目標(biāo)點(diǎn)邊界約束條件的三次多項(xiàng)式規(guī)劃方程。

三次多項(xiàng)式規(guī)劃原理簡單明了,運(yùn)算量小,因此廣泛用于關(guān)節(jié)點(diǎn)到點(diǎn)的運(yùn)動(dòng)規(guī)劃。但是,三次多項(xiàng)式用于速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃時(shí),各段規(guī)劃連接點(diǎn)處會(huì)出現(xiàn)角加速度不連續(xù)的情況,如圖4所示。加速度出現(xiàn)間斷點(diǎn)會(huì)引起振動(dòng)和沖擊,影響工業(yè)機(jī)器人的工作性能和使用壽命。

圖4 速度約束的點(diǎn)到點(diǎn)三次多項(xiàng)式角加速度規(guī)劃

2.2 五次多項(xiàng)式運(yùn)動(dòng)規(guī)劃

五次多項(xiàng)式插值也常用于工業(yè)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)規(guī)劃,采用五次多項(xiàng)式規(guī)劃能保證在各規(guī)劃段的連接點(diǎn)處角加速度是連續(xù)的。五次多項(xiàng)式插值方程如(7)式所示

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(7)

式中：t為時(shí)間變量；a0,a1,a2,a3,a4,a5是五次多項(xiàng)式方程的6個(gè)系數(shù),需要根據(jù)關(guān)節(jié)2個(gè)目標(biāo)插值點(diǎn)的角位移、角速度和角加速度邊界約束條件求解方程中的6個(gè)系數(shù)。

將初始時(shí)間、結(jié)束時(shí)間、2個(gè)目標(biāo)點(diǎn)的角位移和角速度的約束條件以及預(yù)設(shè)定的角加速度分別代入(7)式及其一階導(dǎo)數(shù)和二階導(dǎo)數(shù)中,經(jīng)過經(jīng)整理可以得到求解系數(shù)的(8)式。

(8)

式中：t0,t1,θ0,θ1,ω0和ω1與(6)式中的物理意義相同；ε0和ε1分別為2個(gè)目標(biāo)點(diǎn)的角加速度,需要預(yù)先設(shè)定。求得各系數(shù)之后,就可以得到滿足2個(gè)目標(biāo)點(diǎn)約束邊界條件的五次多項(xiàng)式規(guī)劃方程。從(8)式中可以看出,只要在前、后段規(guī)劃運(yùn)動(dòng)的連接點(diǎn)處設(shè)置相等的角加速度值就能保證加速度是連續(xù)的。

五次多項(xiàng)式運(yùn)動(dòng)規(guī)劃需要預(yù)先設(shè)定目標(biāo)點(diǎn)處關(guān)節(jié)角加速度的值,如果角加速度數(shù)值設(shè)定不合適則會(huì)引起角速度較大的波動(dòng)。如圖5所示,由于在點(diǎn)A點(diǎn)和B點(diǎn)處的角加速度設(shè)定不合適,導(dǎo)致角速度規(guī)劃曲線ω1(t)′和曲線ω2(t)″出現(xiàn)角速度往復(fù)波動(dòng),關(guān)節(jié)電機(jī)需要來回加減速,不利于關(guān)節(jié)角速度的控制。但角加速度要找到合適的預(yù)設(shè)定值并不容易,因此容易出現(xiàn)角速度波動(dòng)的情況。

圖5 五次多項(xiàng)式規(guī)劃角速度波動(dòng)

3 混合多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)規(guī)劃

三次多項(xiàng)式規(guī)劃用于速度約束的多目標(biāo)點(diǎn)規(guī)劃時(shí),角加速度不連續(xù),出現(xiàn)間斷點(diǎn)。五次多項(xiàng)式規(guī)劃時(shí)角加速度是連續(xù)的,但會(huì)出現(xiàn)角速度波動(dòng)大的問題。因此,出現(xiàn)多項(xiàng)式混合插值規(guī)劃的方法。

3.1 多項(xiàng)式拼接規(guī)劃

雖然三次多項(xiàng)式規(guī)劃簡單實(shí)用,速度平穩(wěn),但是單一的三次多項(xiàng)式規(guī)劃方法顯然難以滿足工業(yè)機(jī)器人日益提高的性能要求,于是出現(xiàn)三次多項(xiàng)式與其他多項(xiàng)式混合使用的方法。

Niku[9]提出4個(gè)目標(biāo)點(diǎn)的4-3-4多項(xiàng)式拼接方法用于關(guān)節(jié)運(yùn)動(dòng)規(guī)劃,該方法可以擴(kuò)展到高于4個(gè)目標(biāo)點(diǎn)的多項(xiàng)式拼接4-3-…-3-4。這種拼接方法雖然能確保目標(biāo)點(diǎn)處角加速度是連續(xù)的,但是只能指定首、末2個(gè)目標(biāo)點(diǎn)處的角速度,中間目標(biāo)點(diǎn)處的角速度不能任意指定。Xu等[10]提出3-5-3多項(xiàng)式樣條拼接方法,該方法能夠任意指定中間目標(biāo)點(diǎn)處的角速度,并且角加速度是連續(xù)的,該方法因?yàn)槲宕味囗?xiàng)式的角速度由前后2段三次多項(xiàng)得到,所以不需要進(jìn)行角加速度的設(shè)置。但是當(dāng)目標(biāo)點(diǎn)超過5個(gè)時(shí),拼接方法成為3-5-…-5-3,此時(shí)就需要對五次多項(xiàng)式的加速度進(jìn)行預(yù)先設(shè)定,角加速度預(yù)設(shè)定值不適當(dāng)時(shí)會(huì)引起角速度較大波動(dòng),角速度運(yùn)動(dòng)不平穩(wěn)。

本文提出的3-4-…-4-4的混合多項(xiàng)拼接方法,可以任意指定各目標(biāo)點(diǎn)的角速度,確保角加速度的連續(xù)性,并且無需預(yù)先設(shè)定角加速度的值,避免角加速度預(yù)設(shè)定值不適當(dāng)帶來的角速度較大波動(dòng),解決了現(xiàn)有的多項(xiàng)式樣條拼接方法存在的不足。

3.2 四次多項(xiàng)式運(yùn)動(dòng)規(guī)劃

本方法運(yùn)用到四次多項(xiàng)式插值,四次多項(xiàng)式插值方程如(9)式所示

θ(t)=a0+a1t+a2t2+a3t3+a4t4

(9)

式中：t為時(shí)間變量；a0,a1,a2,a3,a4是四次多項(xiàng)式插值方程的5個(gè)系數(shù),需要根據(jù)關(guān)節(jié)2個(gè)目標(biāo)插值點(diǎn)約束要求求解各系數(shù)。

將初始時(shí)間、結(jié)束時(shí)間、2個(gè)目標(biāo)點(diǎn)的角位移和角速度的約束條件以及1個(gè)目標(biāo)點(diǎn)的角加速度代入(9)式及一階導(dǎo)數(shù)和二階導(dǎo)數(shù)中,經(jīng)過經(jīng)整理可以得到求解系數(shù)的公式(10)。

(10)

式中：t0,t1,θ0,θ1,ω0和ω1與(6)式中的物理意義是一樣的；ε0為2個(gè)目標(biāo)點(diǎn)的任意一個(gè)目標(biāo)點(diǎn)的角加速度。這里規(guī)定ε0為兩個(gè)目標(biāo)點(diǎn)中前一個(gè)目標(biāo)點(diǎn)的角加速度,目的是為了與上一段運(yùn)動(dòng)規(guī)劃的加速度在連接點(diǎn)處相等,也即讓加速度在連接點(diǎn)處保持連續(xù)。因此,ε0可從上一段運(yùn)動(dòng)規(guī)劃得到,而不需要設(shè)先設(shè)定,避免角加速度預(yù)設(shè)定不合適帶來速度波動(dòng)的問題。求得各系數(shù)之后,就可以得到滿足2個(gè)目標(biāo)點(diǎn)約束邊界條件的四次多項(xiàng)式規(guī)劃方程。

3.3 運(yùn)動(dòng)規(guī)劃時(shí)間的計(jì)算

關(guān)節(jié)的運(yùn)動(dòng)時(shí)間與末端執(zhí)行器的運(yùn)動(dòng)時(shí)間是同步、相等的。對于連續(xù)路徑運(yùn)動(dòng),末端執(zhí)行器的運(yùn)動(dòng)時(shí)間等于位移量除以工作運(yùn)動(dòng)速度,關(guān)節(jié)運(yùn)動(dòng)時(shí)間也等于該時(shí)間。對于有時(shí)間要求的點(diǎn)到點(diǎn)運(yùn)動(dòng),例如生產(chǎn)流水線分撿、工裝等,末端執(zhí)行器運(yùn)動(dòng)時(shí)間是根據(jù)工作要求而進(jìn)行設(shè)定,關(guān)節(jié)運(yùn)動(dòng)時(shí)間也與之同步。

對于沒有時(shí)間約束的點(diǎn)到點(diǎn)運(yùn)動(dòng),關(guān)節(jié)運(yùn)動(dòng)時(shí)間不是唯一的,需要根據(jù)一定的條件進(jìn)行設(shè)定。對于關(guān)節(jié)點(diǎn)到點(diǎn)的運(yùn)動(dòng),目標(biāo)點(diǎn)中間的速度沒有進(jìn)行約束,如果運(yùn)動(dòng)時(shí)間非常短,目標(biāo)點(diǎn)的位移又比較大,過程轉(zhuǎn)動(dòng)中可能出現(xiàn)關(guān)節(jié)速度過大,甚至超過了電機(jī)允許的最大轉(zhuǎn)速,即“飛車”現(xiàn)象,這是不允許的,因此在關(guān)節(jié)最大轉(zhuǎn)速小于電機(jī)允許的最大轉(zhuǎn)速的前提下,設(shè)定合適的關(guān)節(jié)運(yùn)動(dòng)時(shí)間。

根據(jù)求極值的原理,函數(shù)的導(dǎo)數(shù)為0處存在極值。對于三次多項(xiàng)式規(guī)劃方程,二階導(dǎo)數(shù)為0處,其一階導(dǎo)數(shù)存在極值,即角速度存在極大值或極小值,暫且以極大值敘述。根據(jù)三次多項(xiàng)規(guī)劃方程(5),令關(guān)節(jié)角位移的二階導(dǎo)數(shù)等于0,如(11)式所示,可以得到關(guān)節(jié)運(yùn)動(dòng)出現(xiàn)最大角速度的時(shí)間tm,如(12)式所示。

(11)

(12)

(13)

(14)

根據(jù)(6)式,系數(shù)a1,a2,a3是關(guān)于t0,t1,θ0,θ1,ω0,ω1的函數(shù),θ0,θ1,ω0,ω1是已知量。t0是開始時(shí)間,對于第一段運(yùn)動(dòng)規(guī)劃t0=0,對于其他運(yùn)動(dòng)規(guī)劃段,t0等于前一段運(yùn)動(dòng)規(guī)劃的結(jié)束時(shí)間,所以t0是已知量。t1是結(jié)束時(shí)間,是未知待求量。將系數(shù)a1,a2,a3的表達(dá)式代入(14)式中,得到(15)式。

ωmax=

(15)

ωmax=

(16)

(16)式中只有t1是未知變量,其他都是已知量,因此可以求解得到結(jié)束時(shí)間t1。如果從時(shí)間效率方面考慮,在電機(jī)最高轉(zhuǎn)速范圍內(nèi),將關(guān)節(jié)運(yùn)動(dòng)的最大角速度ωmax設(shè)定得越高,則關(guān)節(jié)運(yùn)動(dòng)結(jié)束時(shí)間越短,且不會(huì)出現(xiàn)因關(guān)節(jié)角速度超出電機(jī)允許的最高轉(zhuǎn)速而引起的“飛車”現(xiàn)象。如果從轉(zhuǎn)動(dòng)平穩(wěn)方面考慮,關(guān)節(jié)最大角速度ωmax越接近2個(gè)目標(biāo)點(diǎn)中較高的角速度,則速度波動(dòng)越小。結(jié)束時(shí)間t1隨著ωmax變化而變化,可根據(jù)不同的需求設(shè)定ωmax的值,求解得到相應(yīng)的結(jié)束時(shí)間。

3.4 三次多項(xiàng)式與四次多項(xiàng)式混合插值的運(yùn)動(dòng)規(guī)劃原理

對于速度約束的點(diǎn)到點(diǎn)關(guān)節(jié)運(yùn)動(dòng)規(guī)劃,本文提出一種三次多項(xiàng)式和四次多項(xiàng)式混合插值進(jìn)行速度約束的點(diǎn)到點(diǎn)關(guān)節(jié)運(yùn)動(dòng)規(guī)劃,第一段采用三次多項(xiàng)式規(guī)劃,其余都采用四次多項(xiàng)式規(guī)劃,其原理與過程配合圖6敘述如下。為了便于描述,把目標(biāo)點(diǎn)依次標(biāo)記為A,B,C,D…。

圖6 3-4-…-4-4混合多項(xiàng)式運(yùn)動(dòng)規(guī)劃流程圖

1) 第一段運(yùn)動(dòng)規(guī)劃采用三次多項(xiàng)式插值。根據(jù)目標(biāo)點(diǎn)A和B的角位移和角速度約束條件θA,θB,ωA,ωB,初始時(shí)間tA(tA=0),并設(shè)定關(guān)節(jié)最高轉(zhuǎn)速ω1max,求解運(yùn)動(dòng)結(jié)束時(shí)間tB。

2) 根據(jù)θA,θB,ωA,ωB,tA,tB,運(yùn)用(6)式,求解三次多項(xiàng)式插值方程的系數(shù),得到第一段運(yùn)動(dòng)規(guī)劃方程。

3) 根據(jù)第一段運(yùn)動(dòng)規(guī)劃方程,求得目標(biāo)點(diǎn)B的角加速度εB,以供第二段運(yùn)動(dòng)規(guī)劃使用。

4) 第二段運(yùn)動(dòng)規(guī)劃采用四次多項(xiàng)式插值。根據(jù)目標(biāo)點(diǎn)B和C的角位移和角速度約束條件θB,θC,ωB,ωC,初始時(shí)間tB,B點(diǎn)角加速度εB,并設(shè)定關(guān)節(jié)最高轉(zhuǎn)速ω2max,求解結(jié)束時(shí)間tC。

5) 根據(jù)θB,θC,ωB,ωC,tB,tC,εB,運(yùn)用(10)式,求解四次多項(xiàng)式插值方程的系數(shù),得到第二段運(yùn)動(dòng)規(guī)劃方程。因?yàn)榈谝欢我?guī)劃與第二段規(guī)劃在連接點(diǎn)B處角加速度相等,所以確保這兩段規(guī)劃曲線在連接點(diǎn)B處角加速度是連續(xù)的。

6) 根據(jù)第二段運(yùn)動(dòng)規(guī)劃方程,求得目標(biāo)點(diǎn)C的角加速度εC,以供第三段運(yùn)動(dòng)規(guī)劃使用。

7) 如果還有后續(xù)目標(biāo)點(diǎn),后面的運(yùn)動(dòng)規(guī)劃都采用四次多項(xiàng)式插值,重復(fù)步驟4)～6),直到完成所有目標(biāo)點(diǎn)的運(yùn)動(dòng)規(guī)劃。

從上述原理與步驟中可以看出,這種規(guī)劃方法不但能保證連接點(diǎn)處角加速度的連續(xù)性,且不用預(yù)先設(shè)定目標(biāo)點(diǎn)處角加速度的值,避免不合理角加速度預(yù)定值帶來的角速度波動(dòng)。

4 算例驗(yàn)證與分析

為了證驗(yàn)本文提出方法的合理性和有效性,運(yùn)用相關(guān)算例進(jìn)行驗(yàn)證與分析說明。

算例工業(yè)機(jī)器人為了完成某項(xiàng)工作任務(wù),關(guān)節(jié)1需要進(jìn)行速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng),從起始點(diǎn)A開始轉(zhuǎn)動(dòng),依次經(jīng)過B,C,D,E點(diǎn),各目標(biāo)點(diǎn)的轉(zhuǎn)動(dòng)量如表1所示。使用本文提出的三、四次多項(xiàng)式混合插值,對該關(guān)節(jié)進(jìn)行速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃。

表1 關(guān)節(jié)1目標(biāo)點(diǎn)約束條件

1) 第一段運(yùn)動(dòng)規(guī)劃,三次多項(xiàng)式插值。從速度平穩(wěn)性考慮,對于三次多項(xiàng)插值,取2個(gè)目標(biāo)點(diǎn)較大速度為最大速度,ω1max=8,求得第一段運(yùn)動(dòng)結(jié)束時(shí)間為5.625 s,適當(dāng)取整之后,取結(jié)束時(shí)間tB=5 s。

根據(jù)(6)式求解第一段運(yùn)動(dòng)規(guī)劃三次多項(xiàng)式的系數(shù),得到第一段運(yùn)動(dòng)規(guī)劃方程,如(17)式所示。

θ1(t)=2t2-0.16t30≤t≤5

(17)

根據(jù)該方程,得到第一段規(guī)劃結(jié)束點(diǎn)B的角加速度εB=-0.8°/s2以供第二段運(yùn)動(dòng)規(guī)劃使用。

2) 第二段運(yùn)動(dòng)規(guī)劃,四次多項(xiàng)式插值。從速度平穩(wěn)性考慮,對于四次多項(xiàng)插值,最大速度設(shè)定為略高于2個(gè)目標(biāo)點(diǎn)較大轉(zhuǎn)速,ω2max=8.5°/s,并求解第二段運(yùn)動(dòng)結(jié)束時(shí)間tC,去除復(fù)數(shù)解及異常解,有2個(gè)合適解12.55和13.29,適當(dāng)取整之后,取結(jié)束時(shí)間tC為13 s。

根據(jù)(10)式求解第二段運(yùn)動(dòng)規(guī)劃四次多項(xiàng)式的系數(shù),得到第二段運(yùn)動(dòng)規(guī)劃方程,如(18)式所示。

5≤t≤13 (18)

根據(jù)該方程,得到第二段規(guī)劃結(jié)束點(diǎn)C的角加速度εC=-0.05°/s2以供第三段運(yùn)動(dòng)規(guī)劃使用。

3) 第三段運(yùn)動(dòng)規(guī)劃,四次多項(xiàng)式插值。從速度平穩(wěn)出發(fā),取關(guān)節(jié)最大速度為ω3max=8.5°/s,并求解第三段運(yùn)動(dòng)結(jié)束時(shí)間tD,去除異常解,適當(dāng)取整之后,取結(jié)束時(shí)間tD為22 s。

根據(jù)(10)式求解第三段運(yùn)動(dòng)規(guī)劃四次多項(xiàng)式的系數(shù),得到第三段運(yùn)動(dòng)規(guī)劃方程,如(19)式所示。

13≤t≤22 (19)

根據(jù)該方程,求得第三段規(guī)劃結(jié)束點(diǎn)D的角加速度εD=0.246°/s2以供第四段運(yùn)動(dòng)規(guī)劃使用。

4) 第四段運(yùn)動(dòng)規(guī)劃,四次多項(xiàng)式插值。從速度平穩(wěn)出發(fā),取關(guān)節(jié)最大速度為ω4max=8.5°/s,求解第四段運(yùn)動(dòng)結(jié)束時(shí)間tE,去除復(fù)數(shù)解和異常解,取整之后,取結(jié)束時(shí)間tE為35 s。

根據(jù)(10)式求解第四段運(yùn)動(dòng)規(guī)劃四次多項(xiàng)式的系數(shù),得到第四段運(yùn)動(dòng)規(guī)劃方程,如(20)式所示。

22≤t≤35 (20)

至此,關(guān)節(jié)的運(yùn)動(dòng)規(guī)劃完畢。驗(yàn)證運(yùn)動(dòng)規(guī)劃的目標(biāo)點(diǎn)是否滿足邊界約束條件,見表2。從表2中可以看出,4段運(yùn)動(dòng)規(guī)劃的目標(biāo)點(diǎn)都滿足算例給定的角位移和角速度約束條件,并且在各段連接點(diǎn)處角加速度相等,保證了角加速度的連續(xù)性。

表2 目標(biāo)點(diǎn)運(yùn)動(dòng)量檢驗(yàn)

根據(jù)求得的4段運(yùn)動(dòng)規(guī)劃方程,繪制如圖7所示的運(yùn)動(dòng)規(guī)劃曲線。如圖7a)所示,角位移規(guī)劃曲線變化平緩,并且目標(biāo)點(diǎn)滿足角位移的約束條件。如圖7b)所示,目標(biāo)點(diǎn)滿足角速度約束條件,并且整個(gè)角速度曲線光滑平穩(wěn),波動(dòng)較小,并且沒有出現(xiàn)大的角速度峰值,說明所提出的運(yùn)動(dòng)時(shí)間計(jì)算方法是合理可行的。如圖7c)所示,角加速度變化平穩(wěn),在各段連接點(diǎn)處是連續(xù)的，無間斷點(diǎn)。算例驗(yàn)證了本文提出的運(yùn)動(dòng)時(shí)間計(jì)算方法的合理性以及所提出的3-4-…-4-4混合多項(xiàng)式規(guī)劃方法的合理性和有效性。

圖7 3-4-4-4混合多項(xiàng)式運(yùn)動(dòng)規(guī)劃

本算例又分別采用了三次多項(xiàng)式和3-5-5-3混合多項(xiàng)式進(jìn)行規(guī)劃,與本文提出的3-4-4-4多項(xiàng)式規(guī)劃進(jìn)行比較。圖8是3種運(yùn)動(dòng)規(guī)劃的角位移曲線,3種規(guī)劃都能滿足目標(biāo)點(diǎn)角位移要求,角位移曲線基本相似,沒有太大差別。

圖8 3種運(yùn)動(dòng)規(guī)劃的角位移曲線

圖9是3種運(yùn)動(dòng)規(guī)劃的角速度曲線。如圖9b)所示,三次多項(xiàng)式規(guī)劃的角速度曲線整體變化平穩(wěn),角速度波動(dòng)不大,但是連接點(diǎn)處有明顯的尖銳點(diǎn),在C點(diǎn)尤為明顯,說明在連接點(diǎn)處角加速度雖然連續(xù)但是并不平滑，如圖9c)所示,3-5-5-3規(guī)劃的角速度曲線在第二段和第三段中波動(dòng)較大,說明在連接點(diǎn)C的角加速度預(yù)設(shè)定的值不適當(dāng),因此造成角速度曲線有較大的波動(dòng),角速度不平穩(wěn)。如圖9a)所示,采用本文提出的3-4-4-4規(guī)劃方法,角速度曲線整體變化平穩(wěn),在連接點(diǎn)處角速度光滑平穩(wěn)。

圖9 3種運(yùn)動(dòng)規(guī)劃的角速度曲線

圖10是3種運(yùn)動(dòng)規(guī)劃的角加速度曲線。如圖10b)所示,三次多項(xiàng)式規(guī)劃在連接點(diǎn)B,C,D處角加速度出現(xiàn)明顯間斷點(diǎn),在連接點(diǎn)處角加速度不連續(xù),將會(huì)產(chǎn)生振動(dòng)和沖擊,不利于工業(yè)機(jī)器人的工作性能和使用壽命。如圖10c)所示,3-5-5-3規(guī)劃方式的角加速度在連接點(diǎn)處是連續(xù)的,但是在五次多項(xiàng)式規(guī)劃段中,角加速度波動(dòng)較大。如圖10a)所示,采用本文提出的3-4-4-4規(guī)劃方法,角加速度在連接點(diǎn)是連續(xù)的,并且整個(gè)加速度曲線變化較平穩(wěn)。

圖10 3種運(yùn)動(dòng)規(guī)劃的角加速度曲線

從上述比較分析中可以看出，本文提出的方法進(jìn)行3-4-4-4混合多項(xiàng)式規(guī)劃，相比于三次多項(xiàng)式和3-5-5-3多項(xiàng)式規(guī)劃，能更好地應(yīng)用于速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃中，滿足目標(biāo)點(diǎn)角位移與角速度約束要求的同時(shí)，保證了運(yùn)動(dòng)規(guī)劃角加速度的連續(xù)性，且不用預(yù)先設(shè)定目標(biāo)點(diǎn)角加速度的值，減小角速度變化波動(dòng)。

5 結(jié) 論

對于速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)，三次多項(xiàng)規(guī)劃連接點(diǎn)處角加速度不連續(xù)，出現(xiàn)間斷點(diǎn)，會(huì)引起振動(dòng)和沖擊，不利于機(jī)器人的使用壽命和工作性能。五次多項(xiàng)式規(guī)劃解決了角加速度不連續(xù)問題，但需要預(yù)先設(shè)定目標(biāo)點(diǎn)處的角加速度，如果預(yù)先設(shè)定的角加速度值不合適則會(huì)引起角速度來回波動(dòng)，不利于控制。對于現(xiàn)有的多項(xiàng)式樣條曲線拼接方法，4-3-…-3-4多項(xiàng)式規(guī)劃的角加速度雖然連續(xù)，但是不能指定中間目標(biāo)點(diǎn)的角速度值，3-5-…-5-3多項(xiàng)式規(guī)劃方法因?yàn)槲宕味囗?xiàng)式的存在，也需要預(yù)先設(shè)定角加速度，同樣容易引起角速度較大的波動(dòng)。

針對這些存在問題，本文提出了3-4-…-4-4混合多項(xiàng)式插值規(guī)劃，這種規(guī)劃方法主要有以下幾個(gè)優(yōu)勢：①在指定中間目標(biāo)點(diǎn)角速度的同時(shí)，能夠確保在連接點(diǎn)處的角加速度是連續(xù)的，不會(huì)出現(xiàn)角加速度間斷點(diǎn)；②不需要預(yù)先設(shè)定目標(biāo)點(diǎn)的角加速度，避免預(yù)先設(shè)定的加速度值不適當(dāng)引起角速度波動(dòng)；③提出運(yùn)動(dòng)時(shí)間的計(jì)算方法，避免運(yùn)動(dòng)規(guī)劃中出現(xiàn)速度峰值過大的現(xiàn)象，并且使規(guī)劃的角速度波動(dòng)較小；④相關(guān)算例驗(yàn)證了所提方法的有效性和合理性，較好地解決了現(xiàn)有規(guī)劃方法存在的問題，能夠很好應(yīng)用到速度約束的點(diǎn)到點(diǎn)運(yùn)動(dòng)規(guī)劃中，有利工業(yè)機(jī)器人的工作性能和使用壽命。