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

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

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

李小為，胡立坤，王琥

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

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

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

DOI:10.3969/j.issn.1673-4785.201404035

中圖分類號：TP183 文獻標志碼：A

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

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

作者簡介：

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

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

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

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

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

(1)

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

(2)

(3)

(4)

式中：

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

粒子群算法是一種基于群體智能的全局進化優(yōu)化算法，它源于對鳥類捕食行為的模擬，將每個個體看作D維搜索空間中一個沒有體積和質量的微粒[11-12]，且在搜索空間中以一定的速度飛行，通過群體中粒子之間的合作和競爭來尋找最優(yōu)解。假設一個包含m個微粒的微粒群在其D維搜索空間中飛行，第i個粒子在D維空間的位置為xi=(xi1,xi2,…,xiD)，飛行速度為vi=(vi1,vi2,…,viD)，每個粒子都具有一個由被優(yōu)化的目標函數(shù)決定的適應值，微粒i所經(jīng)歷的最好位置pi=(pi1,pi2,…,piD)，也就是微粒i所經(jī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)的復雜性和困難性。優(yōu)化目標是使各個關節(jié)在約束的速度范圍內以最短的時間運行。其適應度函數(shù)為

(7)

(8)

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

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

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

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

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

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

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

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

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

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

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

3機器人建模與PSO仿真

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

圖1　機械臂的D-H坐標系 Fig. 1　The D-H coordinates of robot manipulators

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

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

表 2　笛卡爾空間的路徑表

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

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

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

+t 11, ?t 12, *t 13 圖2　關節(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

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

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

4實驗結果與分析

由于機器人各個關節(jié)是在同一時間內運動，所以每段插值時間取各關節(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分別是各關節(jié)的位置、速度、加速度的實驗曲線。從圖中可以看出關節(jié)位置，速度，加速度均滿足14個運動學約束條件。圖4中，各個關節(jié)速度的最大值和最小值都在第2段插值時出現(xiàn)，第2關節(jié)和第3關節(jié)的速度在第2段時趨近于-Vmax，第5關節(jié)的速度在第2段時趨近于Vmax。

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

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

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

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

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

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

5結束語

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

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

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

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

參考文獻:

[1]李達. 工業(yè)機器人軌跡規(guī)劃控制系統(tǒng)的研究[D]. 哈爾濱: 哈爾濱工業(yè)大學, 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]. 長沙: 湖南大學, 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]. 控制理論與應用, 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]. 電機與控制學報, 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]尼庫拉 庫克. 機器人學導論—分析、控制及應用[M]. 孫富春, 朱紀洪, 劉國棟等, 譯. 第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年生，碩士研究生，主要研究方向為機器人、先進控制理論與技術。

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

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