機(jī)器手臂軌跡規(guī)劃

王凱凱，萬(wàn) 衡

WANG Kai-kai, WAN Heng

（上海應(yīng)用技術(shù)學(xué)院 電氣與電子工程學(xué)院，上海 201413）

0 引言

機(jī)器人軌跡規(guī)劃算法是目前機(jī)器人領(lǐng)域的研究熱點(diǎn)，其中主流的五次多項(xiàng)式插值法可以滿足起始點(diǎn)和結(jié)束點(diǎn)的速度與加速度為零，B樣條函數(shù)插值法的局部性特點(diǎn)較好，但其各自也存在一定缺陷，限制了使用范圍。本文給出了一種起始點(diǎn)與目標(biāo)點(diǎn)采用五次多項(xiàng)式，中間點(diǎn)采用三次B樣條插值法的混合算法——“五－B－五”軌跡規(guī)劃算法，在實(shí)際應(yīng)用中取得了較好的效果。

1 當(dāng)前機(jī)器人軌跡規(guī)劃的算法及其問題

1.1 五次多項(xiàng)式插值法

五次多項(xiàng)式插值法是機(jī)器人軌跡規(guī)劃中應(yīng)用比較廣泛的一種算法[1]。假定對(duì)機(jī)器人的運(yùn)動(dòng)軌跡要求嚴(yán)格，約束條件過高，通常采用階數(shù)比較高的五次多項(xiàng)式對(duì)運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。五次多項(xiàng)式插值法相對(duì)比較簡(jiǎn)單，運(yùn)算量不大，該算法的起始點(diǎn)與目標(biāo)點(diǎn)的速度與加速度曲線可以同時(shí)為零，且整段角位移曲線平滑，速度與加速度曲線連續(xù)。但該算法關(guān)節(jié)位移變化大，任意關(guān)節(jié)點(diǎn)參數(shù)的變化都將對(duì)整條曲線產(chǎn)生非常大的影響，使得在分段的間隔點(diǎn)處不能保證加速度的連續(xù)性，使得五次多項(xiàng)式插值法的應(yīng)用得到了一些限制。五次多項(xiàng)式可設(shè)為：

1.2 三次B樣條函數(shù)插值法

B樣條方法是Gordon、Riesenfeld等在1972年提出的[1]。B樣條函數(shù)定義如下：

其中k>1；i=0，1，…，n-k；u為系數(shù)。

由于B樣條曲線插值法對(duì)中間點(diǎn)關(guān)節(jié)角、關(guān)節(jié)角速度、關(guān)節(jié)角加速度曲線都是連續(xù)的，更主要的是B樣條曲線具有分段處理的優(yōu)點(diǎn)，各段僅由相鄰的4個(gè)控制點(diǎn)確定，因此，變動(dòng)特性多邊形的某個(gè)頂點(diǎn)，只會(huì)影響與該頂點(diǎn)有關(guān)的相鄰4段曲線，其他地方的曲線不會(huì)引起變化。但是起始點(diǎn)與目標(biāo)點(diǎn)的速度與加速度曲線不能同時(shí)為零這一不足之處，會(huì)使機(jī)器手臂在起始點(diǎn)與目標(biāo)點(diǎn)位置發(fā)生速度或加速度發(fā)生突變，必將引起機(jī)器人啟動(dòng)或者停止時(shí)的手部抖動(dòng)，影響機(jī)器人工作的平穩(wěn)性。

綜上所述，這二種算法在各自領(lǐng)域應(yīng)用都存在一些不足，因此為了解決這些不足之處，本文給出了一種新的算法。

2 五—B—五算法

通常情況下服務(wù)機(jī)器人對(duì)機(jī)器手臂的要求較高，約束條件比較多，如起始點(diǎn)與目標(biāo)點(diǎn)的位置、速度與加速度，這樣就需要滿足六個(gè)約束條件。假如把五次多項(xiàng)式看做關(guān)節(jié)角值的時(shí)間函數(shù)，則五次多項(xiàng)式的一階導(dǎo)數(shù)為關(guān)節(jié)角速度的時(shí)間函數(shù)，二階導(dǎo)數(shù)為關(guān)節(jié)角加速度的時(shí)間函數(shù)，根據(jù)下式：

求得五次多項(xiàng)式函數(shù)曲線。其中 q0、 qf分別代表起始點(diǎn)與目標(biāo)點(diǎn)關(guān)節(jié)角值，分別代表起始點(diǎn)與目標(biāo)點(diǎn)關(guān)節(jié)角速度，分別代表起始點(diǎn)與目標(biāo)點(diǎn)關(guān)節(jié)角加速度[2]。

假設(shè)機(jī)器手臂在關(guān)節(jié)空間中有一組型值點(diǎn)(任意時(shí)刻的關(guān)節(jié)角)P1,P2,…,Pm根據(jù)邊界條件和連續(xù)條件可以求出m+2個(gè)控制點(diǎn)V0,V1,…,Vm+1任意的兩個(gè)相鄰的型值點(diǎn)可以用一條B樣條曲線連接起來，這樣整條軌跡曲線就由m-1 條B 樣條曲線組合而成，第i 段曲線可以由四個(gè)控制點(diǎn)來控制[3,4]。

設(shè)四階三次B樣條函數(shù)表達(dá)式為：

其中u(0 ≤ u ≤1)為參數(shù)；Qi(u)為第i段B樣條曲線上的點(diǎn)；Xi(u)為參數(shù)多項(xiàng)式；V為控制點(diǎn)。根據(jù)文獻(xiàn)[5,6]可得：

從而可得第i段B樣條曲線為：

其中i=1，2，…，m；0≤u≤ 1；當(dāng)u取遍0～1的所有數(shù)值時(shí)，即可求得一段完整的B樣條曲線。由于選擇的邊界條件的限制，使速度曲線與加速度曲線的初始點(diǎn)與目標(biāo)點(diǎn)必然不能同時(shí)為零。為了求解式(6)，我們采用邊界條件選擇為加速度曲線的起始點(diǎn)與目標(biāo)點(diǎn)為零，即用為解控制點(diǎn)的邊界條件。根據(jù)式(7)就可以求出完整的第i段B樣條曲線。

式(6)中u為未知參數(shù)，其從0～1遍歷取值就可以得到第i段的B樣條曲線，其中t(u)與q(u)分別表示該段曲線的橫縱坐標(biāo)。要繪制速度曲線和加速度曲線必須知道B樣條的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)，令一階導(dǎo)數(shù)為二階導(dǎo)數(shù)為則：

起始段與中間段及目標(biāo)段的連接尤為重要，處理好可以保證整條曲線的連續(xù)平穩(wěn)，處理不好則整個(gè)曲線則沒有價(jià)值，達(dá)不到預(yù)期目標(biāo)。在速度曲線中，起始段的結(jié)束速度V2必須與中間段的初始速度相同，終止段的初始速度V4必須與中間段的結(jié)束速度相同。在加速度曲線中，起始段的結(jié)束速度A2必須與中間段的初始速度相同，結(jié)束段的起始速度A4必須與中間段的結(jié)束速度相同，否則曲線不連續(xù)。

連接點(diǎn)的各參數(shù)設(shè)置如下：在關(guān)節(jié)角速度曲線中，起始段五次多項(xiàng)式曲線的起始速度取V1=0，結(jié)束速度取中間段B樣條曲線開始時(shí)刻t1的速度V2，終止段五次多項(xiàng)式曲線的終止速度取V3=0，起始速度取中間段B樣條函數(shù)曲線的結(jié)束點(diǎn)時(shí)刻t2速度V4。在關(guān)節(jié)角加速度曲線中，起始段五次多項(xiàng)式曲線的起始加速度取A1=0，結(jié)束加速度取中間段B樣條的開始點(diǎn)加速度V2，終止段五次多項(xiàng)式曲線的終止加速度取A3=0，起始速度取中間段B樣條函數(shù)曲線的結(jié)束點(diǎn)加速度A4。

具體算法如下：

1）輸入起始點(diǎn)X、中間點(diǎn)Y1...Yi（i為整數(shù)）與目標(biāo)點(diǎn)Z；

2 ）識(shí)別開始段X-Y1、中間段Y1-Yi和目標(biāo)段Yi-Z；

3）計(jì)算出V1、V2、V3、V4、A1、A2、A3、A4；

4）判斷若為起始點(diǎn)，連接起始點(diǎn)X與中間點(diǎn)Y1，調(diào)用公式(4)和公式(1)；

5）連接Y1于Y2，Y2與Y3，Yi-1與Yi，每段分別調(diào)用公式(6)；

6）連接Yi與目標(biāo)點(diǎn)Z，調(diào)用公式(4)和公式(1)；

7）結(jié)束。

五—B—五算法結(jié)構(gòu)圖如圖1所示。

圖1 五—B—五算法結(jié)構(gòu)圖

3 仿真驗(yàn)證

本文選取機(jī)器人其中一個(gè)關(guān)節(jié)為例來驗(yàn)證算法的可行性與穩(wěn)定性，假設(shè)關(guān)節(jié)起始點(diǎn)x(0，-20)，中間點(diǎn)為y1(5，0)、y2(10，20)、y3(15，-30)、y4(20，50)，目標(biāo)點(diǎn)為z(25，90)，根據(jù)上述所述可得到V1=0、V2=9.6667、V3=23.6667、V4=0，A1=A2=A3=A4=0。利用MATLAB編程[8,9]可得到下面的仿真圖2、圖3、圖4。

圖2 角位移曲線

圖3 關(guān)節(jié)角速度曲線

圖4 關(guān)節(jié)角角加速度曲線

圖2 (a)為三次B樣條角位移曲線，圖2(b)為五—B—五算法角位移曲線由圖2(a)可以看出，機(jī)器人該關(guān)節(jié)角位移從起始點(diǎn)-20°開始，在第5秒達(dá)到第一個(gè)中間點(diǎn)0°，這段曲線為起始段，采用五次多項(xiàng)式插值法；在第10秒達(dá)到第二個(gè)中間點(diǎn)20°，在第15秒達(dá)到第三個(gè)中間點(diǎn)-30°，在第20秒達(dá)到第四個(gè)中間點(diǎn)50°，這三段曲線稱為中間段，采用三次B樣條曲線插值法；在第25秒達(dá)到目標(biāo)點(diǎn)90°，這段曲線稱為結(jié)束段，采用五次多項(xiàng)式插值法。整段曲線平滑、連續(xù)，變化平緩，不存在跳變，很好的達(dá)到預(yù)期的效果。

圖3(a)與4(a)分別為三次B樣條曲線速度與加速度曲線，圖3(b)、4(b)分別為五—B—五算法速度與加速度曲線。由圖3(a)與4(a)可以看出單獨(dú)的三次B樣條曲線的起始速度與起始加速度不同時(shí)為零，而由3(b)、4(b)可以看出，機(jī)器人該關(guān)節(jié)角速度曲線、加速度曲線連續(xù)，變化平緩，不存在跳變，且在起始點(diǎn)

與目標(biāo)點(diǎn)的速度、加速度均為0，算法得到了很好的改善。

綜上所述，該機(jī)器人關(guān)節(jié)的角位移曲線平滑連續(xù)、角速度和角加速度曲線連續(xù)，說明該關(guān)節(jié)在工作過程中運(yùn)行平穩(wěn)，不會(huì)產(chǎn)生較大的震蕩。

4 結(jié)束語(yǔ)

本文針對(duì)當(dāng)前一些機(jī)器手臂軌跡規(guī)劃算法存在的不足，結(jié)合以往算法的優(yōu)點(diǎn)，給出了一種“五—B—五”軌跡規(guī)劃算法，該算法的角位移曲線連續(xù)，角速度曲線和角加速度曲線的起始點(diǎn)和結(jié)束點(diǎn)的速度和加速度都為零且連續(xù)，保證了機(jī)器人在運(yùn)行過程中沒有劇烈的抖動(dòng)，延長(zhǎng)了機(jī)器人的壽命。另外該算法的中間曲線部分采用四個(gè)控制點(diǎn)控制一段曲線的生成，在型值點(diǎn)發(fā)生意外變化時(shí)只能改變?cè)撔《蔚那€變化，減少了誤差。最后通過MATLAB對(duì)該軌跡規(guī)劃算法仿真，取得了預(yù)期目標(biāo)，為以后的機(jī)器人軌跡規(guī)劃研究提供了很好的參考價(jià)值。

[1] 孫秀萍.機(jī)器人手臂軌跡規(guī)劃的研究[J].內(nèi)蒙古師范大學(xué)學(xué)報(bào)(自然科學(xué)漢文版),2009,38(2):174-177.

[2] A. Oasparettot ,V.Zanotto. A. new method for smooth trajectory planning of robot manipulators[J].Mechanism and Machine Theory,2007,42:455-471.

[3] 石忠,王永智,胡慶雷.基于多項(xiàng)式插值的自由漂浮空間機(jī)器人軌跡規(guī)劃粒子群優(yōu)化算法[J].四川宇航學(xué)報(bào),2011,32(7):1516-1521.

[4] Jeng-Yih Chiou Robot Control System Based on Distributed Embedded Systems[J].Second International Conference on Innovative Computing,Information and Control(ICICIC 2007),2007:266-269.

[5] 王幼民,徐蔚鴻.機(jī)器人連續(xù)控制中B樣條軌跡優(yōu)化[J].機(jī)械設(shè)計(jì).2000,10(10):33-36.

[6] Tan Guanzheng,Wang Yuechao. theoretical and experimental research on time-optimal trajectory planning and control of industrial robots[J].Control theory &Applications,2003(2):185-192.

[7] Lin Dingding,Liu Li,Zhao Jiandong,et al.ZMP-CoP Testing and Research of Biped Walking Robot[J].Robot,2004,26(4):368-379.

[8] 李雙雙.工業(yè)機(jī)器人建模、運(yùn)動(dòng)仿真與軌跡優(yōu)化[D].內(nèi)蒙古大學(xué),2012.

[9] 王永龍,張兆忠,張桂紅.MATLAB語(yǔ)言基礎(chǔ)與應(yīng)用[M].北京:電子工業(yè)出版社,2010.