網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150526.0853.001.html

速度約束下PSO的六自由度機械臂時間最優(yōu)軌跡規(guī)劃

李小為，胡立坤，王琥

(廣西大學(xué) 電氣工程學(xué)院，廣西 南寧 530004)

摘要：以六自由度機械臂的運動學(xué)正逆解為前提條件，在關(guān)節(jié)空間中根據(jù)插值點設(shè)計機器人的運動軌跡，為使機械臂在不同的速度約束下以最短時間運行，提出了粒子群優(yōu)化速度約束下的時間最優(yōu)3-5-3多項式插值軌跡規(guī)劃方法。粒子群算法結(jié)構(gòu)簡單、實現(xiàn)容易，參數(shù)易調(diào)整，直接選擇以多項式插值時間為變量的搜索空間中進行PSO優(yōu)化，并且對不符合速度約束條件的插值時間進行篩選。通過離化得到六自由度機械臂限速運行下的最短時間，在機器人控制平臺上進行實時實驗，得到關(guān)節(jié)運動位置、速度、加速度曲線，證明了該方法能夠準(zhǔn)確地實現(xiàn)任意速度約束的時間最優(yōu)軌跡規(guī)劃。

關(guān)鍵詞：機器人；六自由度機械臂；粒子群優(yōu)化；軌跡規(guī)劃；多項式插值；速度約束；時間最優(yōu)

DOI:10.3969/j.issn.1673-4785.201404035

中圖分類號：TP183 文獻標(biāo)志碼：A

收稿日期：2014-04-18. 網(wǎng)絡(luò)出版日期：2015-05-26.

基金項目：廣西自然科學(xué)基金資助項目(2012GXNSF BA053144).

作者簡介：

中文引用格式：李小為，胡立坤，王琥. 速度約束下PSO的六自由度機械臂時間最優(yōu)軌跡規(guī)劃[J]. 智能系統(tǒng)學(xué)報, 2015, 10(3): 393-398.

英文引用格式：LI Xiaowei, HU Likun，WANG Hu. PSO-based time optimal trajectory planning for six degrees of freedom robot manipulators with speed constraints [J]. CAAI Transactions on Intelligent Systems, 2015, 10(3): 393-398.

PSO-based time optimal trajectory planning for

six degrees of freedom robot manipulators with speed constraints

LI Xiaowei, HU Likun, WANG Hu

(College of Electrical Engineering, Guangxi University, Nanning 530004, China)

Abstract：The trajectory planning is designed for robot manipulators in the joint space according to interpolation points on the premise of the solutions to the forward and inverse kinematics problems of 6-DOF (depth of field) manipulators. This paper puts forward the particle swarm optimization (PSO)-based time optimal trajectory planning of the 3-5-3 polynomial interpolation method in order to make mechanical arms run in the shortest time at a constrained speed. The PSO is proposed to optimize run time due to its simple structure and easily adjustable parameters. The polynomial interpolation time rather than coefficient is selected as searching variable in PSO optimization. If the interpolation time of three polynomials doesn't meet the constraints, the particle will be excluded by comparison. The shortest time of the 6-DOF manipulators running at different speeds is obtained by offline PSO. Real-time experiments are conducted on the robot control platform. It shows that this method can accurately realize time optimal trajectory planning at any speed through its position, velocity and acceleration curves.

Keywords：robot; 6 DOF manipulators; particle swarm optimization; trajectory planning; polynomial interpolation; speed constraints; time optimal

通信作者：李小為. E-mail: maxwelllxw@163.com.

目前軌跡規(guī)劃主要從2個方面進行優(yōu)化，一是對時間的優(yōu)化；二是對系統(tǒng)能量的優(yōu)化；其中對最優(yōu)時間的研究最多。機器人時間最優(yōu)軌跡規(guī)劃是指在滿足各種約束條件下，以機器人運動時間最短為目標(biāo)來規(guī)劃機器人的運動軌跡[1]。對軌跡規(guī)劃的時間最優(yōu)，一般從2個方面考慮，一是以速度和加速度為約束條件；二是以運動學(xué)或動力學(xué)為約束，采用各種非線性約束的最優(yōu)化算法實現(xiàn)機器人最優(yōu)軌跡規(guī)劃[2]。文獻[3]以機器人的位置、速度、加速度和二階加速度的運動學(xué)約束為前提，路徑點之間用高次多項式作曲線軌跡，采用柔性多面體搜索的算法求解最優(yōu)時間。由于在連接點處引入某一確定的加速度值使得抑制振動變得十分困難。為了避免機械手振動，延長關(guān)節(jié)使用壽命，關(guān)節(jié)軌跡都采用二次多項式加余弦函數(shù)的形式[4]，不僅可以保證各關(guān)節(jié)運動的位移、速度、加速度連續(xù)，而且還可以保證各關(guān)節(jié)運動的二階加速度連續(xù)。但是采用二次多項式加余弦函數(shù)的形式增加了計算的復(fù)雜度，需要耗費大量的計算時間。利用具有光滑特性的三次樣條曲線段來連接路徑點是最簡單的多項式方法，在速度和加速度的約束下能夠減小兩點之間的運行時間，同時避免振蕩[5]?；诙囗検讲逯档能壽E規(guī)劃，具有階次高、沒有凸包性質(zhì)等特點，很難用傳統(tǒng)方法優(yōu)化[6]。文獻[7]采用遺傳算法對關(guān)節(jié)空間中各關(guān)鍵點間的運行時間間隔進行規(guī)劃。但是遺傳算法需要進行編碼、解碼過程，操作復(fù)雜。粒子群優(yōu)化方法具有算法簡單、參數(shù)設(shè)置少、實現(xiàn)簡單，能夠有效解決真實世界的復(fù)雜優(yōu)化問題，與其他求解約束優(yōu)化問題的方法相比有一定的優(yōu)勢[8]。文獻[9-10]采用粒子群優(yōu)化算法對關(guān)節(jié)角軌跡進行時間最優(yōu)化求解。

不同于以往對速度的單一限制，本文采用粒子群算法對任意關(guān)節(jié)速度下的機械臂進行運行時間的優(yōu)化。

1多項式插值函數(shù)的構(gòu)造

已知機器人在笛卡爾坐標(biāo)系下起始點，2個路徑點以及終點的空間坐標(biāo)，通過逆運動學(xué)求解得到各個關(guān)節(jié)在4個插值點處的關(guān)節(jié)角度，用θij表示關(guān)節(jié)i插值的角度，其中i=1,2,…,n，n表示關(guān)節(jié)個數(shù)，j=1,2,3,4代表4個插值點的序號。點與點之間采用3-5-3多項式插值的曲線軌跡。其約束條件是：已知第i個關(guān)節(jié)各段的初始點θi0、路徑點θi1和θi2、末端點θi3，路徑點之間的位置、速度與加速度連續(xù)以及初始點和終點的速度、加速度均為0。

第i段關(guān)節(jié)3-5-3樣條多項式的通式為

(1)

式中：hi1(t)、hi2(t)、hi3(t)分別代表3段多項式的軌跡，未知系數(shù)ai1j、ai2j、ai3j為3-5-3樣條多項式的第i個關(guān)節(jié)軌跡各段插值函數(shù)的第j個系數(shù)。用以上的14個邊界和過渡的約束條件，可以推導(dǎo)求解出3-5-3多項式的14個未知系數(shù)aij，推導(dǎo)公式為式(2)~(4)。式(2)中A僅與第i個關(guān)節(jié)3段多項式的插值時間ti1、ti2、ti3有關(guān)，式(3)代表第i個關(guān)節(jié)角的位置矩陣[6]。

(2)

(3)

(4)

式中：

2PSO求解速度約束下的最優(yōu)時間

粒子群算法是一種基于群體智能的全局進化優(yōu)化算法，它源于對鳥類捕食行為的模擬，將每個個體看作D維搜索空間中一個沒有體積和質(zhì)量的微粒[11-12]，且在搜索空間中以一定的速度飛行，通過群體中粒子之間的合作和競爭來尋找最優(yōu)解。假設(shè)一個包含m個微粒的微粒群在其D維搜索空間中飛行，第i個粒子在D維空間的位置為xi=(xi1,xi2,…,xiD)，飛行速度為vi=(vi1,vi2,…,viD)，每個粒子都具有一個由被優(yōu)化的目標(biāo)函數(shù)決定的適應(yīng)值，微粒i所經(jīng)歷的最好位置pi=(pi1,pi2,…,piD)，也就是微粒i所經(jīng)歷過的具有最好適應(yīng)值的位置，群體所有微粒經(jīng)歷的最好位置為pg。對每一次迭代，第i個粒子的第d維分量(1≤d≤D)在第k+1次迭代時的速度和位置按式(5)和(6)變化：

(5)

(6)

如果選擇多項式的系數(shù)aij作為待尋優(yōu)量，則根據(jù)式(2)~(4)可以得到時間變量ti1、ti2、ti3，這時候粒子群的維數(shù)為14維。如果直接選擇在時間變量ti1、ti2、ti3的搜索空間進行優(yōu)化，可以將維數(shù)降低，大大減少了粒子群尋優(yōu)的復(fù)雜性和困難性。優(yōu)化目標(biāo)是使各個關(guān)節(jié)在約束的速度范圍內(nèi)以最短的時間運行。其適應(yīng)度函數(shù)為

(7)

(8)

式中：Vi和Vimax分別是第i個關(guān)節(jié)的實時速度和最大限制速度，利用粒子群算法對復(fù)雜的約束優(yōu)化問題求解。

粒子群算法對機器人第i個關(guān)節(jié)進行最優(yōu)時間軌跡規(guī)劃，具體步驟如下：

1)選定種群的規(guī)模m(一般為20)，在插值時間的3維搜索空間中隨機產(chǎn)生m個粒子構(gòu)成初始種群，并初始化粒子的位置和速度。

2)由m組時間變量ti1、ti2、ti3代入式(2)~(4)中可得出3-5-3多項式的未知系數(shù)aij。

3)將3-5-3多項式的系數(shù)aij代入式(1)并對時間求導(dǎo)，得到關(guān)節(jié)角度的速度函數(shù)，判斷實時速度是在否滿足式(8)。

4)計算每個微粒的適應(yīng)度值。對步驟3)的計算結(jié)果進行篩選，如果3段中的任何一段速度不符合式(8)，則將該粒子的適應(yīng)度值設(shè)置為極大的常數(shù)，在尋找最優(yōu)粒子時，通過適應(yīng)度值的比較，將會排除這個適應(yīng)度值較大的粒子，不被篩選為最優(yōu)粒子。而這個粒子本身也會慢慢向最佳值進行靠攏，直到滿足速度的約束。如果3段的最大速度都符合式(8)，則采用式(7)作為適應(yīng)度函數(shù)，粒子群算法迭代以獲得最小插值時間為目標(biāo)。

5)對每個微粒，將其適應(yīng)度值與其經(jīng)歷過的最好位置pi的適應(yīng)度值作比較，如果較好，則將其替換為當(dāng)前的最好位置pi。

6)比較每個微粒當(dāng)前最好位置的適應(yīng)度值，得到當(dāng)前整體最優(yōu)粒子，再與群體所經(jīng)歷的全局最好位置pg作比較，如果較好，則替換pg。

7)根據(jù)式(5)~(6)變化微粒的速度和位置，重新整合成新的由m個粒子構(gòu)成的種群。

8)如滿足終止條件(通常為足夠好的適應(yīng)值或達到一個預(yù)設(shè)最大迭代次數(shù)(Nmax)則算法結(jié)束，否則返回步驟2)。

本文設(shè)定粒子群的個數(shù)m為20，初始的粒子位置為[0.1,4.0]的任意隨機數(shù)，粒子的最大飛行速度在[-2,2]之間。粒子飛行速度的參數(shù)設(shè)置為慣性權(quán)重w=(Wmax-i×(Wmax-Wmin)/Nmax)，Wmax=0.9，Wmin=0.4，i為迭代次數(shù)，循環(huán)迭代步數(shù)Nmax為50。權(quán)重因子c1=2,c2=2，r1和r2為[0,1]的隨機數(shù)。

3機器人建模與PSO仿真

本文主要研究工業(yè)機器人六自由度機械臂，采用標(biāo)準(zhǔn)D-H坐標(biāo)系法進行運動學(xué)建模[14]，如圖1所示。6個關(guān)節(jié)的D-H參數(shù)見表1。

圖1　機械臂的D-H坐標(biāo)系 Fig. 1　The D-H coordinates of robot manipulators

關(guān)節(jié)idi/mmai/mmai/(°)θi/(°)關(guān)節(jié)變量范圍/(°)10150900[-150,150]20570090[-80,65]30150900[-80,80]46500-900[-175,175]500900[-110,110]6010500[-200,200]

通過解析法用4×4的齊次變換矩陣可以求解出機械臂位置控制和方向控制的各關(guān)節(jié)角度的逆運動學(xué)解析解。在直角坐標(biāo)系下給定機械臂末端的軌跡插值點，如表2。由逆運動學(xué)將各空間笛卡爾位置插值點轉(zhuǎn)化為關(guān)節(jié)空間的角度插值點。

表 2　笛卡爾空間的路徑表

通過逆運動學(xué)解，得到1~6關(guān)節(jié)的初始位置，路徑點和終點所對應(yīng)的角度如表3所示。

表 3　關(guān)節(jié)空間的角度插值點

按照第2節(jié)中的方法，在不同速度約束下，關(guān)節(jié)1采用PSO求解最優(yōu)時間，跟蹤群體最好位置pg在每次迭代過程中的位置變化，得到關(guān)節(jié)1的最優(yōu)粒子pg位置進化圖，如圖2?？梢钥闯觯诓煌俣燃s束下，關(guān)節(jié)1最優(yōu)粒子pg最多經(jīng)過30次迭代就快速收斂。其各個收斂值即為在特定的速度約束之下，關(guān)節(jié)1運行三段插值多項式所需要的最短時間為t11、t12和t13，如表4所示。

+t 11, ?t 12, *t 13 圖2　關(guān)節(jié)1的最優(yōu)粒子p g位置進化 Fig. 2　The optimal particle p gof joints 1 evolution

速度范圍(°/s)t11/st12/st13/s(-115,115)0.22290.29490.1384(-57,57)0.44490.59240.2856(-20,20)1.27471.63610.8485(-10,10)2.56713.24581.7209

對于其他關(guān)節(jié)也同樣采用上述方法進行尋優(yōu)，從而得到各個關(guān)節(jié)在不同速度約束下的運行最優(yōu)時間。由于研究目標(biāo)是大型的工業(yè)機器人，關(guān)節(jié)速度不宜采取過大，否則會產(chǎn)生強烈的振動，不利于機器人長期、高效、穩(wěn)定的運行。所以取關(guān)節(jié)速度在[-20,20](°/s)為例，按照與關(guān)節(jié)1同樣的優(yōu)化方法，得到各個關(guān)節(jié)的最短插值時間，如表5。從表中可以看出，第4關(guān)節(jié)的位置與機器人的姿態(tài)完全相關(guān)，因為機器人姿態(tài)在運動過程中始終保持不變，所以關(guān)節(jié)4的位置也沒有發(fā)生變化，插值時間和速度都為0。關(guān)節(jié)1和關(guān)節(jié)6的運動狀態(tài)完全重合。

表 5　各關(guān)節(jié)在速度[-20,20]下的最優(yōu)時間

4實驗結(jié)果與分析

由于機器人各個關(guān)節(jié)是在同一時間內(nèi)運動，所以每段插值時間取各關(guān)節(jié)該段插值時間的最大值，t1=max{ti1},t2=max{ti2},t3=max{ti3}(i=1,2,…,6)。則t1=3.116 2，t2=3.997 7，t3=3.280 0。

通過離線優(yōu)化得到最短的插值時間，在機器人控制平臺上編程實現(xiàn)3-5-3多項式時間最優(yōu)軌跡規(guī)劃，并進行實驗。圖3~5分別是各關(guān)節(jié)的位置、速度、加速度的實驗曲線。從圖中可以看出關(guān)節(jié)位置，速度，加速度均滿足14個運動學(xué)約束條件。圖4中，各個關(guān)節(jié)速度的最大值和最小值都在第2段插值時出現(xiàn)，第2關(guān)節(jié)和第3關(guān)節(jié)的速度在第2段時趨近于-Vmax，第5關(guān)節(jié)的速度在第2段時趨近于Vmax。

圖3　粒子群優(yōu)化的機械臂關(guān)節(jié)位置曲線 Fig. 3　Mechanical arms’ joint position curves of PSO

圖4　粒子群優(yōu)化的機械臂關(guān)節(jié)速度曲線 Fig. 4　Mechanical arms’ joint speed curves of PSO

圖5　粒子群優(yōu)化的機械臂關(guān)節(jié)加速度曲線 Fig. 5　Mechanical arms’ joint acceleration curves of PSO

圖6是粒子群優(yōu)化的機械臂關(guān)節(jié)脈動曲線，由于3-5-3多項式3次導(dǎo)數(shù)只有在某些特殊的情況下才能保持連續(xù)。機械手的軌跡精度和關(guān)節(jié)脈動有密切關(guān)系，關(guān)節(jié)脈動越小則機械手的軌跡精度越高[15]。此時，機械手的軌跡精度較低，誤差較大，同時使用壽命也會減少。如果要使脈動連續(xù)，則要以提高階次或復(fù)雜度為代價。在采集關(guān)節(jié)角度數(shù)據(jù)的同時采集機器人的實時空間坐標(biāo)，圖7是在基于粒子群優(yōu)化的機械臂笛卡爾坐標(biāo)下三維立體實驗軌跡曲線?？梢钥闯銮€經(jīng)過預(yù)設(shè)的點。實驗驗證了速度約束下的PSO六自由度機械臂多項式插值軌跡規(guī)化的有效性。

圖6　粒子群優(yōu)化的機械臂關(guān)節(jié)脈動曲線 Fig. 6　Mechanical arms’ joint pulsation curves of PSO

圖7　基于粒子群優(yōu)化的機械臂軌跡曲線 Fig. 7　Mechanical arms’ joint trajectory curve of PSO

5結(jié)束語

采用速度約束下的PSO算法在計算機上離線尋優(yōu)出機器人3段插值最優(yōu)時間，在機器人控制平臺上進行編程實驗，實驗結(jié)果證明了速度約束下的PSO六自由度機械臂時間最優(yōu)軌跡規(guī)劃的可靠性。

傳統(tǒng)3-5-3多項式插值，沒有確定各段插值時間的準(zhǔn)則，插值時間是隨便選取的已知數(shù)。根據(jù)14個已知條件，求出各個關(guān)節(jié)在3段插值時間內(nèi)的多項式，對于3-5-3多項式求導(dǎo)得到速度函數(shù)2-4-2多項式，要求各個關(guān)節(jié)的速度滿足一定的約束條件，即要使這些多項式的值在約束范圍內(nèi)，要求高次多項式滿足限定條件，這本身就是非常復(fù)雜的數(shù)學(xué)問題，加之多項式的系數(shù)本身就與插值時間有關(guān)，在滿足速度約束的條件下同時還要進行時間最優(yōu)，就更是一個復(fù)雜的難題，難以用傳統(tǒng)的方法解決。而粒子群算法能有效地解決這種復(fù)雜的約束優(yōu)化問題。

在計算式(2)時，由于粒子群中的每個粒子都是隨機產(chǎn)生的，可能造成矩陣不滿秩，不利于矩陣求逆。但是對尋優(yōu)過程并不會有太大影響，如果矩陣不滿秩，求解式(4)時使得a的各個系數(shù)為無窮大，在后續(xù)粒子群尋優(yōu)過程中，這個粒子會不滿足速度的約束條件從而被篩選出來。

本文在研究過程中并沒有考慮脈動不連續(xù)對機器人造成的不利影響，下一步的工作就是在分析脈動對系統(tǒng)的作用基礎(chǔ)之上考慮脈動不連續(xù)的解決方案。

參考文獻:

[1]李達. 工業(yè)機器人軌跡規(guī)劃控制系統(tǒng)的研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2011: 2-3.

LI Da. Study on industrial robot control system of trajectory planning[D]. Harbin, China: Harbin Institute of Technology, 2011: 2-3.

[2]張紅強. 工業(yè)機器人時間最優(yōu)軌跡規(guī)劃[D]. 長沙: 湖南大學(xué), 2004: 9-10.

ZHANG Hongqiang. Time-optimal trajectory planning of industrial robot[D]. Changsha, China: Hunan University, 2004: 9-10.

[3]LIN C S, CHANG P R, LUH J Y S. Formulation and optimization of cubic polynomial joint trajectories for industrial robots[J]. IEEE Transactions on Automatic Control, 1983, 28(12): 1066-1074.

[4]譚冠政, 王越超. 工業(yè)機器人時間最優(yōu)軌跡規(guī)劃及軌跡控制的理論與實驗研究[J]. 控制理論與應(yīng)用, 2003, 20(2): 185-192.

TAN Guanzheng, WANG Yuechao. Theoretical and experimental research on time-optimal trajectory planning and control of industrial robots[J]. Control Theory & Applications, 2003, 20(2): 185-192.

[5]BAZAZ S A, TONDU B. Minimum time on-line joint trajectory generator based on low order spline method for industrial manipulators[J]. Robotics and Autonomous Systems, 1999, 29(4): 257-268.

[6]付榮, 居鶴華. 基于粒子群優(yōu)化的時間最優(yōu)機械臂軌跡規(guī)劃算法[J]. 信息與控制, 2011, 40(6): 802-808.

FU Rong, JU Hehua. Time-optimal trajectory planning algorithm for manipulator based on PSO[J]. Information and Control, 2011, 40(6): 802-808.

[7]李東潔, 邱江艷, 尤波. 一種機器人軌跡規(guī)劃的優(yōu)化算法[J]. 電機與控制學(xué)報, 2009, 13(1): 123-127.

LI Dongjie, QIU Jiangyan, YOU Bo. Optimal algorithm for trajectory planning of the robot[J]. Electric Machines and Control, 2009, 13(1): 123-127.

[8]LI Yanliang, SHAO Wei, YOU Long, et al. An improved PSO algorithm and its application to UWB antenna design[J]. IEEE Antennas and Wireless Propagation Letters, 2013, 12: 1236-1239.

[9]GUO Tongying, LI Feng, HUANG Kuan, et al. Application of optimal algorithm on trajectory planning of mechanical arm based on B-Spline curve[J]. Applied Mechanics and Materials, 2013, 376: 253-256.

[10]SASKA M, MACAS M, PREUCIL L, et al. Robot path planning using particle swarm optimization of Ferguson splines[C]//IEEE Conference on Emerging Technologies and Factory Automation. Prague, Czech Republic, 2006: 833-839.

[11]謝曉鋒, 張文俊, 楊之廉. 微粒群算法綜述[J]. 控制與決策, 2003, 18(2): 129-134.

XIE Xiaofeng, ZHANG Wenjun, YANG Zhilian. Overview of particle swarm optimization[J]. Control and Decision, 2003, 18(2): 129-134.

[12]SUPAKAR N, SENTIHIL A. PSO obstacle avoidance algorithm for robot in unknown environment[C]//International Conference on Communication and Computer Vision (ICCCV). Coimbatore, India, 2013: 1-7.

[13]HU Menqi, WU T, WEIR J D. An adaptive particle swarm optimization with multiple adaptive methods[J]. IEEE Transactions on Evolutionary computation, 2013, 17(5): 705-720.

[14]尼庫拉 庫克. 機器人學(xué)導(dǎo)論—分析、控制及應(yīng)用[M]. 孫富春, 朱紀(jì)洪, 劉國棟等, 譯. 第4版. 北京: 電子工業(yè)出版社, 2004: 60-72.

[15]BORYGA M, GRABOS A. Planning of manipulator motion trajectory with higher-degree polynomials use[J]. Mechanism and Machine Theory, 2009, 44(7): 1400-1419.

李小為，女，1989年生，碩士研究生，主要研究方向為機器人、先進控制理論與技術(shù)。

胡立坤，男，1977年生，教授，主要研究方向是非線性系統(tǒng)動力學(xué)與控制、光/風(fēng)/蓄分布式電源轉(zhuǎn)換與控制、工業(yè)測控網(wǎng)絡(luò)應(yīng)用。作為主要參與人參與國家自然科學(xué)基金項目2項，承擔(dān)廣西科技開發(fā)計劃項目2項、廣西自然科學(xué)基金1項和企業(yè)橫向項目5項。獲得軟件著作權(quán)1項，實用新型專利1項，申請發(fā)明專利8項。發(fā)表學(xué)術(shù)論文50余篇，其中被SCI檢索2篇，EI檢索14篇。

王琥，男，1990年生，碩士研究生，主要研究方向為機器人視覺、先進控制理論與技術(shù)。