基于混合多項式插值的工業(yè)機器人關(guān)節(jié)運動規(guī)劃

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

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

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

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

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

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

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

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

1 速度約束的點到點關(guān)節(jié)運動

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

1.1 關(guān)節(jié)點到點運動

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

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

圖1 關(guān)節(jié)點到點運動

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

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

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

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

圖2 末端執(zhí)行器點到點運動與關(guān)節(jié)點到點運動規(guī)劃

1.2 速度約束的點到點關(guān)節(jié)運動

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

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

圖3 速度約束的關(guān)節(jié)點到點運動

2 關(guān)節(jié)運動規(guī)劃方法

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

2.1 三次多項式運動規(guī)劃

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

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

(5)

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

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

(6)

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

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

圖4 速度約束的點到點三次多項式角加速度規(guī)劃

2.2 五次多項式運動規(guī)劃

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

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

(7)

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

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

(8)

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

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

圖5 五次多項式規(guī)劃角速度波動

3 混合多項式插值的關(guān)節(jié)運動規(guī)劃

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

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

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

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

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

3.2 四次多項式運動規(guī)劃

本方法運用到四次多項式插值,四次多項式插值方程如(9)式所示

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

(9)

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

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

(10)

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

3.3 運動規(guī)劃時間的計算

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

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

根據(jù)求極值的原理,函數(shù)的導數(shù)為0處存在極值。對于三次多項式規(guī)劃方程,二階導數(shù)為0處,其一階導數(shù)存在極值,即角速度存在極大值或極小值,暫且以極大值敘述。根據(jù)三次多項規(guī)劃方程(5),令關(guān)節(jié)角位移的二階導數(shù)等于0,如(11)式所示,可以得到關(guān)節(jié)運動出現(xiàn)最大角速度的時間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是開始時間,對于第一段運動規(guī)劃t0=0,對于其他運動規(guī)劃段,t0等于前一段運動規(guī)劃的結(jié)束時間,所以t0是已知量。t1是結(jié)束時間,是未知待求量。將系數(shù)a1,a2,a3的表達式代入(14)式中,得到(15)式。

ωmax=

(15)

ωmax=

(16)

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

3.4 三次多項式與四次多項式混合插值的運動規(guī)劃原理

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

圖6 3-4-…-4-4混合多項式運動規(guī)劃流程圖

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

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

3) 根據(jù)第一段運動規(guī)劃方程,求得目標點B的角加速度εB,以供第二段運動規(guī)劃使用。

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

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

6) 根據(jù)第二段運動規(guī)劃方程,求得目標點C的角加速度εC,以供第三段運動規(guī)劃使用。

7) 如果還有后續(xù)目標點,后面的運動規(guī)劃都采用四次多項式插值,重復步驟4)～6),直到完成所有目標點的運動規(guī)劃。

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

4 算例驗證與分析

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

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

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

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

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

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

(17)

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

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

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

5≤t≤13 (18)

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

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

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

13≤t≤22 (19)

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

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

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

22≤t≤35 (20)

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

表2 目標點運動量檢驗

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

圖7 3-4-4-4混合多項式運動規(guī)劃

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

圖8 3種運動規(guī)劃的角位移曲線

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

圖9 3種運動規(guī)劃的角速度曲線

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

圖10 3種運動規(guī)劃的角加速度曲線

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

5 結(jié) 論

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

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