董　輝,仲曉帆,黃　勝

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

一種六關(guān)節(jié)機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃方法

董輝,仲曉帆,黃勝

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

摘要：為提高六關(guān)節(jié)機(jī)器人運(yùn)動(dòng)效率，保證在最優(yōu)時(shí)間內(nèi)平滑地完成運(yùn)動(dòng)任務(wù)，提出一種關(guān)節(jié)空間軌跡規(guī)劃方法.該軌跡規(guī)劃方法首先通過D-H參數(shù)法，對(duì)機(jī)器人進(jìn)行建模；在末端執(zhí)行器軌跡上尋找若干個(gè)特征點(diǎn)，并通過逆運(yùn)動(dòng)學(xué)解得六個(gè)關(guān)節(jié)的關(guān)節(jié)角；采用5次非均勻B樣條曲線定義速度、加速度可控的關(guān)節(jié)軌跡，并獲得時(shí)間最優(yōu)軌跡規(guī)劃的非線性運(yùn)動(dòng)學(xué)約束條件；最后通過改進(jìn)的粒子群算法求解關(guān)于時(shí)間最優(yōu)的運(yùn)動(dòng)軌跡.

關(guān)鍵詞：軌跡規(guī)劃;時(shí)間最優(yōu);樣條;關(guān)節(jié)空間

Trajectory planning method in joint space for 6-DOF robot

DONG Hui, ZHONG Xiaofan, HUANG Shen

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

Abstract:A trajectory planning method in joint space for 6-DOF robot which will guarantee to complete the motion tasks smoothly within the optimal time and improve the efficiency of the robot motion is proposed in this paper. Firstly, the robot is modeled by using D-H method; then, a number of feature points are found on the end of actuator trajectory. The six joint angles are got through the inverse kinematics; Here, the fifth-order non-uniform b-spline curve is used to define the joint trajectory in which the speed and acceleration can be controlled. Then the non-linear kinematic constraints of the time-optimal trajectory can be obtained. Finally, by using the improved particle swarm optimization, the time-optimal trajectory with non-linear kinematic constraints is solved.

Keywords:trajectory planning; time-optimal; b-spline; joint space

關(guān)節(jié)空間的軌跡是指機(jī)器人每個(gè)自由度的位置、速度、加速度的時(shí)間歷程，它們決定了關(guān)節(jié)機(jī)器人是否能在最優(yōu)時(shí)間內(nèi)平滑地完成運(yùn)動(dòng)任務(wù).因此軌跡規(guī)劃的研究具有普遍的理論與工程意義.遺傳算法、模糊邏輯等優(yōu)化算法被廣泛運(yùn)用于具有時(shí)間最優(yōu)特性的機(jī)器人軌跡規(guī)劃中[1-2].現(xiàn)有的軌跡規(guī)劃算法大多通過求解目標(biāo)函數(shù)的最優(yōu)解，從而實(shí)現(xiàn)時(shí)間、效率、能量等最優(yōu).文獻(xiàn)中，Piazzi A等通過區(qū)間分析法找到全局加加速度最小的軌跡，但未考慮時(shí)間問題[3]；Gasparetto A等通過cubic曲線擬合關(guān)節(jié)空間軌跡，并用序列二次規(guī)劃法，找到可通過權(quán)重因子平衡選擇時(shí)間最優(yōu)或者加加速度最小的軌跡規(guī)劃，該方法設(shè)計(jì)巧妙，具有一定參考價(jià)值[4]；賈慶軒等利用啟發(fā)式的A*方法在關(guān)節(jié)空間內(nèi)進(jìn)行路徑搜索，通過算例進(jìn)行點(diǎn)到點(diǎn)路徑規(guī)劃數(shù)值仿真[5]，但未考慮效率問題；Constantinescu D等在轉(zhuǎn)矩約束條件下通過三次樣條曲線進(jìn)行規(guī)劃軌跡得到速度、加速度連續(xù)的軌跡[6]，并用可變?nèi)莶罘ㄇ蠼鈺r(shí)間最優(yōu)問題，但三次樣條無法控制關(guān)節(jié)軌跡的加加速度項(xiàng)；胡小平等采用多項(xiàng)式插值和Newton插值相結(jié)合的軌跡規(guī)劃方法[7]，該方法在起始段和終止段采用五次多項(xiàng)式插值，中間段采用Newton插值，可以獲得連續(xù)的軌跡，但無法滿足時(shí)間最優(yōu)要求.朱世強(qiáng)等通過7次非均勻B樣條曲線構(gòu)造關(guān)節(jié)軌跡，對(duì)加加速度的處理放在了約束函數(shù)中[8]，但7次非均勻B樣條曲線計(jì)算相對(duì)比較復(fù)雜.

綜合以上軌跡規(guī)劃方法，首先根據(jù)徳布爾算法構(gòu)造關(guān)節(jié)空間加加速度連續(xù)的5次非均勻B樣條函數(shù)Pk(t)，并將時(shí)間最優(yōu)問題表述為關(guān)節(jié)運(yùn)動(dòng)累積時(shí)間最小的問題；通過對(duì)目標(biāo)函數(shù)求導(dǎo)得到速度、加速度、加加速度關(guān)于位置變量的函數(shù)表達(dá)式，并將非線性運(yùn)動(dòng)學(xué)約束條件加入到計(jì)算控制頂點(diǎn)的過程中；最后根據(jù)改進(jìn)的粒子群算法求解具有非線性約束條件的時(shí)間最優(yōu)問題，得到最優(yōu)的時(shí)間序列.

1構(gòu)造目標(biāo)函數(shù)

通過次非均勻B樣條曲線表示目標(biāo)函數(shù)，樣條曲線由若干多項(xiàng)式Ni,k(t)線性組合而成.這些多項(xiàng)式都具有一個(gè)相應(yīng)的控制頂點(diǎn)di作為加權(quán)系數(shù).

為構(gòu)造B樣條函數(shù)，首先需在笛卡爾空間中的幾何路徑上尋找n個(gè)點(diǎn).再通過逆運(yùn)動(dòng)學(xué)求解得到笛卡爾空間中的點(diǎn)對(duì)應(yīng)的各個(gè)關(guān)節(jié)的位置矢量p1,p2,…,pn，此處pi泛指任意關(guān)節(jié)在第i個(gè)點(diǎn)處的關(guān)節(jié)位置矢量.關(guān)節(jié)空間中通過次非均勻B樣條曲線Pk(t)描述關(guān)節(jié)軌跡為

(1)

其中基函數(shù)Ni,k(t)定義為

(2)

因?yàn)闃訔l函數(shù)的局域性，在時(shí)間段[tl,tl+1)的樣條曲線Pk(t)由控制頂點(diǎn)dl-k,dl-k+1,…,dl決定，而與其他控制頂點(diǎn)無關(guān).因此，式(1)可表示為

(3)

由式(2，3)可知：關(guān)節(jié)空間軌跡規(guī)劃的時(shí)間最優(yōu)問題是以每個(gè)時(shí)間段的樣條曲線Pk(t)累計(jì)執(zhí)行時(shí)間最短為控制目標(biāo)，從而獲得目標(biāo)函數(shù)的n+k-1個(gè)控制頂點(diǎn)di.在時(shí)間節(jié)點(diǎn)序列兩邊各有k個(gè)重節(jié)點(diǎn).假設(shè)每個(gè)時(shí)間段為hi=ti+1-ti，則關(guān)于時(shí)間最優(yōu)的問題可描述為尋找滿足min∑hi的控制頂點(diǎn)的問題，對(duì)應(yīng)的時(shí)間節(jié)點(diǎn)ti，有

(4)

2分析運(yùn)動(dòng)學(xué)約束

2.1邊界條件處理

機(jī)器人的運(yùn)動(dòng)必須滿足運(yùn)動(dòng)學(xué)約束條件，本節(jié)假設(shè)機(jī)器人任意關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束條件描述，包括速度v、加速度a，加加速度j約束項(xiàng).具體約束條件為v≤Vmax,a≤Amax,j≤Jmax.

(5)

容易得到關(guān)節(jié)速度、加速度以及加加速度的函數(shù)表達(dá)分別為

(6)

給定速度、加速度的初始值與終止值，再代入式(7)，可得各個(gè)關(guān)節(jié)的起始和終止速度、加速度、加加速度，從而實(shí)現(xiàn)對(duì)邊界條件的處理.其中d1,d2,d3,dn+k-3,dn+k-2,dn+k-1，以及由式(3)求解n+k-1個(gè)控制頂點(diǎn).對(duì)于六關(guān)節(jié)機(jī)器人系統(tǒng)，以上方程組可由矩陣形式表達(dá)，則各個(gè)關(guān)節(jié)的控制頂點(diǎn)可表示為

Ad=P

(7)

其中：P泛指各個(gè)關(guān)節(jié)空間的關(guān)節(jié)角序列以及關(guān)節(jié)啟停速度、加速度、加加速度組成的序列；A為Rn+5n+5的方陣；d為各個(gè)關(guān)節(jié)空間的控制頂點(diǎn)序列.

通過矩陣運(yùn)算可得到各個(gè)關(guān)節(jié)空間的控制頂點(diǎn)序列為

d=A-1P

(8)

其中方陣A中元素確定可以表示為

A(i+4,i+j+2)=Ni+j+2,5(ui+3)

(9)

式中：i=0,1,…,n-3；j=0,1,…,k-1.其他行元素根據(jù)關(guān)節(jié)啟停速度、加速度、加加速度確定，即實(shí)現(xiàn)對(duì)邊界條件的處理.

2.2運(yùn)動(dòng)學(xué)約束條件

根據(jù)機(jī)器人各個(gè)關(guān)節(jié)的速度、加速度上限，機(jī)器人關(guān)節(jié)空間的六個(gè)關(guān)節(jié)均要滿足各自的約束條件，其運(yùn)動(dòng)學(xué)約束由式(5)表述.因此，該運(yùn)動(dòng)學(xué)約束可描述為：對(duì)于任意一個(gè)關(guān)節(jié)運(yùn)動(dòng)軌跡，只需要保證該關(guān)節(jié)運(yùn)動(dòng)速度、加速度、加加速度曲線最大值的絕對(duì)值能滿足相應(yīng)關(guān)節(jié)的約束條件即可.關(guān)節(jié)空間軌跡規(guī)劃的時(shí)間最優(yōu)問題的非線性運(yùn)動(dòng)學(xué)約束可由式(5，7)聯(lián)合得到，即

(10)

3最優(yōu)化求解

在滿足運(yùn)動(dòng)學(xué)約束條件下，關(guān)節(jié)空間具有時(shí)間最優(yōu)化特性的軌跡規(guī)劃問題數(shù)學(xué)模型可描述為

(11)

(12)

其中：gr(ti)為復(fù)雜函數(shù)，可通過式(2，6，9，10)聯(lián)合求的.

該最優(yōu)化問題是一個(gè)具有非顯式非線性約束條件的優(yōu)化問題，常規(guī)最優(yōu)化方法基本難以求解該類問題.由于粒子群算法(PSO, Particle swarm optimization)計(jì)算簡(jiǎn)單、魯棒性好、收斂速度快[9-10]，因此在普通PSO算法的基礎(chǔ)上進(jìn)行適當(dāng)改進(jìn)，將非線性約束條件加入搜索最優(yōu)解的過程中，改進(jìn)后的PSO算法即可用于計(jì)算具有非線性約束條件的時(shí)間最優(yōu)平滑軌跡規(guī)劃問題.

改進(jìn)思想是結(jié)合外點(diǎn)罰函數(shù)法和基本粒子群算法，將問題的約束條件和目標(biāo)函數(shù)構(gòu)造一個(gè)新目標(biāo)函數(shù)F(t)=f(t)+max{0,gr(t)},(r=1,2,3).對(duì)那些違反約束條件的點(diǎn)給以懲罰，再通過粒子群算法思想將得到的新目標(biāo)函數(shù)求最優(yōu)解.改進(jìn)的PSO算法步驟如下：

1) 初始化粒子群參數(shù):設(shè)定慣性權(quán)值ω=0.3，學(xué)習(xí)因子c1=1,c2=2，粒子數(shù)N=20，最大迭代次數(shù)M=20，最優(yōu)化問題的維數(shù)D=n-1等.

2) 根據(jù)目標(biāo)函數(shù)和約束條件，確定外點(diǎn)罰函數(shù)，得出增廣的新目標(biāo)函數(shù)fitness ().將初始時(shí)間序列帶入計(jì)算，并要求計(jì)算過程滿足運(yùn)動(dòng)學(xué)約束，再計(jì)算每個(gè)粒子的適應(yīng)度.

3) 將每個(gè)粒子的適應(yīng)度值與其歷史最好適應(yīng)度值Pbest進(jìn)行比較，若當(dāng)前適應(yīng)度值更優(yōu)，則用當(dāng)前位置更新Pbest.

4) 將每個(gè)粒子適應(yīng)度值與整個(gè)粒子群所經(jīng)歷的全局歷史最好適應(yīng)度值作比較，如果這個(gè)位置比全局歷史最優(yōu)更好，則用當(dāng)前位置更新gbest.

5) 粒子的速度、位置更新公式分別為

v(t+1)=ωv(t)+c1rand(Pbest-x(t))+

c2rand(gbest-x(t))

x(t+1)=x(t)+v(t)

6) 若達(dá)到停止條件，算法停止，返回目標(biāo)函數(shù)取最小值時(shí)的自變量的值及目標(biāo)函數(shù)最小值作為解輸出；否則，轉(zhuǎn)步驟2.

在最優(yōu)化問題求解過程中，為提高求解效率，將速度、加速度、加加速度曲線離散化.根據(jù)B樣條曲線凸包性，選取各個(gè)關(guān)節(jié)速度、加速度、加加速度曲線的控制頂點(diǎn)作為約束條件的局部最值.因此max{0,gr(t)}可直接從控制頂點(diǎn)處選取.

4實(shí)驗(yàn)步驟與分析

為驗(yàn)證通過改進(jìn)后的粒子群算法計(jì)算速度、加速度、加加速度連續(xù)的六關(guān)節(jié)機(jī)器人時(shí)間最優(yōu)化軌跡規(guī)劃方法的可行性，使用MATLAB對(duì)六自由度串聯(lián)關(guān)節(jié)式機(jī)器人RBT-6T/S03S機(jī)器人進(jìn)行仿真.以下實(shí)驗(yàn)所取參數(shù)來源于該機(jī)器人參數(shù)，實(shí)驗(yàn)步驟如下：

1) 通過逆運(yùn)動(dòng)學(xué)將笛卡爾空間型值點(diǎn)換算為6個(gè)關(guān)節(jié)空間的關(guān)節(jié)變量，再通過5次非均勻B樣條曲線插補(bǔ)6個(gè)關(guān)節(jié)的軌跡.從而獲得6個(gè)關(guān)節(jié)關(guān)于時(shí)間節(jié)點(diǎn)的曲線函數(shù).為方便實(shí)驗(yàn)，此處直接給定六個(gè)關(guān)節(jié)的關(guān)節(jié)變量，關(guān)節(jié)變量如表1所示.

表1　關(guān)節(jié)空間各關(guān)節(jié)角

2) 通過結(jié)合了罰函數(shù)法的粒子群算法，求解在運(yùn)動(dòng)學(xué)約束下，步驟2所得到的目標(biāo)函數(shù)的最優(yōu)化解.運(yùn)動(dòng)學(xué)約束如表2所示.

表2　運(yùn)動(dòng)學(xué)約束

3) 將步驟2得到的粒子群尋優(yōu)的時(shí)間節(jié)點(diǎn)序列：h1=3.738 9 s，h2=2.966 8 s，h3=3.222 4 s，帶入式(2，6，9，10)，得到關(guān)于時(shí)間節(jié)點(diǎn)的關(guān)節(jié)空間角度規(guī)劃曲線、速度規(guī)劃曲線、加速度規(guī)劃曲線以及加加速度規(guī)劃曲線，如圖1~4所示.

圖1　時(shí)間最優(yōu)關(guān)節(jié)角度規(guī)劃曲線Fig.1　Time-optimal angle trajectory planning curve

4) 與未使用粒子群優(yōu)化算法情況下，滿足運(yùn)動(dòng)學(xué)約束的五次B樣條曲線軌跡規(guī)劃結(jié)果做比較，如圖5所示關(guān)節(jié)角度軌跡規(guī)劃曲線圖.

由以上實(shí)驗(yàn)結(jié)果可知：通過改進(jìn)的粒子群算法獲得了最優(yōu)化時(shí)間序列，從而求解得到的關(guān)節(jié)空間角度、速度、加速度以及加加速度軌跡曲線均連續(xù)、平滑，并且滿足表2所示的運(yùn)動(dòng)學(xué)約束條件.

圖2　時(shí)間最優(yōu)關(guān)節(jié)角速度軌跡規(guī)劃曲線Fig.2　Time-optimal angle velocity trajectory planning curve

圖3　時(shí)間最優(yōu)關(guān)節(jié)角加速度軌跡規(guī)劃曲線Fig.3　Time-optimal angle acceleration trajectory planning curve

圖4　時(shí)間最優(yōu)關(guān)節(jié)角加加速度軌跡規(guī)劃曲線Fig.4　Time-optimal angle jerk trajectory planning curve

圖5　普通關(guān)節(jié)角度規(guī)劃曲線Fig.5　Normal angle trajectory planning curve

5結(jié)論

在5次非均勻B樣條曲線插值的基礎(chǔ)上，利用改進(jìn)的粒子群算法進(jìn)行軌跡優(yōu)化，使得六關(guān)節(jié)機(jī)器人在保證速度、加速度及加加速度都在約束范圍之內(nèi)光滑變化的前提下，實(shí)現(xiàn)最短時(shí)間軌跡規(guī)劃.利用MATLAB進(jìn)行軌跡仿真，仿真結(jié)果表明：基于粒子群算法的軌跡規(guī)劃保證了機(jī)器人運(yùn)動(dòng)時(shí)間節(jié)點(diǎn)的最優(yōu)化，而5次非均勻B樣條曲線保證了關(guān)節(jié)角度、速度、加速度的連續(xù)性和平滑性，兩者的結(jié)合得到了六關(guān)節(jié)機(jī)器人關(guān)節(jié)空間的最優(yōu)軌跡.

參考文獻(xiàn)：

[1]JOHN J CRAIG.機(jī)器人學(xué)導(dǎo)論[M].贠超,譯.3版.北京:機(jī)械工業(yè)出版社,2010.

[2]董輝,黃文嘉.變?cè)鲆娣答丏型迭代學(xué)習(xí)控制系統(tǒng)分析與仿真[J].浙江工業(yè)大學(xué)學(xué)報(bào),2014,42(2):214-218.

[3]PIAZZI A, VISIOLI A. Global minimum-jerk trajectory planning of robot manipulators[J]. Industrial Electronics, IEEE Transactions on,2000,47(1):140-149.

[4]GASPARETTO A, ZANOTTO V. Optimal trajectory planning for industrial robots[J]. Advances in Engineering Software,2010,41(4):548-556.

[5]賈慶軒,陳鋼,孫漢旭,等.基于A*算法的空間機(jī)械臂避障路徑規(guī)劃明[J].機(jī)械工程學(xué)報(bào),20l0,46(13):109-115.

[6]COSTANTINESCU D, CROFT E A. Smooth and time-optimal trajectory planning for industrial manipulators along specified paths[J]. Journal of Robotic Systems,2000,17(5):233-249.

[7]胡小平,彭濤.一種基于多項(xiàng)式和Newton插值法的機(jī)械手軌跡規(guī)劃方法[J].中國機(jī)械工程,2012,23(24):2946-2949.

[8]朱世強(qiáng),劉松國.機(jī)械手時(shí)間最優(yōu)脈動(dòng)連續(xù)軌跡規(guī)劃算法[J].機(jī)械工程學(xué)報(bào),2010,43(3):47-52.

[9]KENNEDY J, EBERHART R. Particle swarm optimization[C]//IEEE International Conference on Neural Networks. Piscataway: IEEE Service Center,1995:1942-1948.

[10]HUANG P, XU Y. Pso-based time-optimal trajectory planning for space robot with dynamic constraints[C]// IEEE International Conference on Robotics and Biomimetics. Kunming: IEEE,2006:1402-1407.

(責(zé)任編輯：劉巖)

中圖分類號(hào)：TP242.2

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1006-4303(2015)03-0336-04

作者簡(jiǎn)介：董輝(1979—)，男，浙江永康人，副教授，研究方向?yàn)榍度耸较到y(tǒng)技術(shù)，E-mail:hdong@zjut.edu.cn.

收稿日期：2014-12-15