王凱凱，萬 衡

WANG Kai-kai, WAN Heng

（上海應用技術學院 電氣與電子工程學院，上海 201413）

0 引言

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

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

1.1 五次多項式插值法

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

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

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

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

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

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

2 五—B—五算法

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

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

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

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

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

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

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

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

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

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

具體算法如下：

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

2 ）識別開始段X-Y1、中間段Y1-Yi和目標段Yi-Z；

3）計算出V1、V2、V3、V4、A1、A2、A3、A4；

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

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

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

7）結束。

五—B—五算法結構圖如圖1所示。

圖1 五—B—五算法結構圖

3 仿真驗證

本文選取機器人其中一個關節(jié)為例來驗證算法的可行性與穩(wěn)定性，假設關節(jié)起始點x(0，-20)，中間點為y1(5，0)、y2(10，20)、y3(15，-30)、y4(20，50)，目標點為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 關節(jié)角速度曲線

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

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

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

與目標點的速度、加速度均為0，算法得到了很好的改善。

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

4 結束語

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

[1] 孫秀萍.機器人手臂軌跡規(guī)劃的研究[J].內(nèi)蒙古師范大學學報(自然科學漢文版),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] 石忠,王永智,胡慶雷.基于多項式插值的自由漂浮空間機器人軌跡規(guī)劃粒子群優(yōu)化算法[J].四川宇航學報,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] 王幼民,徐蔚鴻.機器人連續(xù)控制中B樣條軌跡優(yōu)化[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è)機器人建模、運動仿真與軌跡優(yōu)化[D].內(nèi)蒙古大學,2012.

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