董　輝,仲曉帆,黃　勝

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

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

董輝,仲曉帆,黃勝

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

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

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

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

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

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

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

(1)

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

(2)

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

(3)

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

(4)

2分析運動學(xué)約束

2.1邊界條件處理

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

(5)

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

(6)

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

Ad=P

(7)

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

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

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é)啟停速度、加速度、加加速度確定，即實現(xiàn)對邊界條件的處理.

2.2運動學(xué)約束條件

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

(10)

3最優(yōu)化求解

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

(11)

(12)

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

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

改進思想是結(jié)合外點罰函數(shù)法和基本粒子群算法，將問題的約束條件和目標(biāo)函數(shù)構(gòu)造一個新目標(biāo)函數(shù)F(t)=f(t)+max{0,gr(t)},(r=1,2,3).對那些違反約束條件的點給以懲罰，再通過粒子群算法思想將得到的新目標(biāo)函數(shù)求最優(yōu)解.改進的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ù)和約束條件，確定外點罰函數(shù)，得出增廣的新目標(biāo)函數(shù)fitness ().將初始時間序列帶入計算，并要求計算過程滿足運動學(xué)約束，再計算每個粒子的適應(yīng)度.

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

4) 將每個粒子適應(yīng)度值與整個粒子群所經(jīng)歷的全局歷史最好適應(yīng)度值作比較，如果這個位置比全局歷史最優(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) 若達到停止條件，算法停止，返回目標(biāo)函數(shù)取最小值時的自變量的值及目標(biāo)函數(shù)最小值作為解輸出；否則，轉(zhuǎn)步驟2.

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

4實驗步驟與分析

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

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

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

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

表2　運動學(xué)約束

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

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

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

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

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

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

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

參考文獻：

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

[2]董輝,黃文嘉.變增益反饋D型迭代學(xué)習(xí)控制系統(tǒng)分析與仿真[J].浙江工業(yè)大學(xué)學(xué)報,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*算法的空間機械臂避障路徑規(guī)劃明[J].機械工程學(xué)報,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]胡小平,彭濤.一種基于多項式和Newton插值法的機械手軌跡規(guī)劃方法[J].中國機械工程,2012,23(24):2946-2949.

[8]朱世強,劉松國.機械手時間最優(yōu)脈動連續(xù)軌跡規(guī)劃算法[J].機械工程學(xué)報,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é)任編輯：劉巖)

中圖分類號：TP242.2

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

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

作者簡介：董輝(1979—)，男，浙江永康人，副教授，研究方向為嵌人式系統(tǒng)技術(shù)，E-mail:hdong@zjut.edu.cn.

收稿日期：2014-12-15