基于PSO-SA的機器人關節空間軌跡規劃

謝能斌 辛紹杰

(上海電機學院機械學院 上海 201306)

0 引 言

隨著工業4.0的到來，智能制造設備在工業生產和日常生活中正起著越來越重要的作用。機器人作為智能制造設備中的重要組成，廣泛地應用于醫療、汽車制造、焊接、曲面拋光等領域，緩解了勞動力不足的壓力[1]，這要求工業機器人有很高的工作效率和運行穩定性。

對機器人關節空間的軌跡進行合理的規劃，能夠提高機器人的工作效率，減小關節抖動，提高機器人運行的平穩性[2]。

采用傳統的數學插補函數來描述關節位置與時間的函數關系，關節的位置、速度、加速度的連續性可以得到保證，避免了各關節的抖動，但是卻無法優化關節運動時間，造成了效率的低下[3]。于是智能控制算法被運用到傳統的多項式插補函數中優化運行時間，相關專家學者也進行了大量的研究與改進。國內學者采用遺傳算法[4-5]和粒子群算法[6]以及對二者的改進對3-5-3軌跡多項式系數進行優化。文獻[7-8]將差分進化算法和改進的CMA-ES算法應用到了機器人軌跡規劃當中。同時，人工魚群算法[9]和人工蜂群算法[10]也成功地運用到機器人軌跡規劃當中，并且取得了不錯的效果。

粒子群算法很適合求解帶有約束的尋優問題，但算法存在容易早熟無法找到全局最優解的弊端[11]。本文使用模擬退火算法(SA)對粒子群算法(PSO)進行改進，將模擬退火機制引入到PSO中，同時對模擬退火算法中的Metropolis準則進行改進，增強算法的全局搜索能力。采用慣性權重非線性遞減策略對PSO的全局搜索和局部搜索能力進行平衡。引入了動態學習因子，在前期加快算法全局搜索速度，在后期加快局部搜索速度。使用PSO-SA對5-7-5軌跡多項式的系數進行優化，在滿足PUMA_560機器人運動學約束的條件下提高了機器人工作的效率。

1 關節空間軌跡曲線生成

1.1 中間點的生成

工業中，使用機器人將貨物從起始點運送到目標位置是關節空間點到點軌跡規劃問題。本文以6自由度的PUMA_560機器人作為研究對象，對機器人點到點軌跡規劃算法進行驗證。

已知起止點的位置坐標p1和p4分別為(856,-150,-94)和(350,-614,-502)，單位為毫米。使用逆運動學將起止點位置坐標轉換成關節空間下的關節角度。通過以下程序生成兩個中間點所對應的各個關節角度：

mdl_puma560

T1=transl(p1)

T4=transl(p4)

q1=p560.ikine6s(T1)

q4=p560.ikine6s(T4)

q=jtraj(q1,q4,4)

語句mdl_puma560打開MATLAB機器人工具箱Robotic_Toolbox中自帶的PUMA_560機器人模型。語句T1=transl(p1)將位置坐標p1轉換成位姿矩陣T1，語句T4=transl(p4)將位置坐標p4轉換成位姿矩陣T4，其中T1和T4都是4×4的矩陣。語句q1=p560.ikine6s(T1)和q4=p560.ikine6s(T4)通過逆運動學求解起止點在關節空間下所對應的關節角度矩陣q1和q4，其中q1和q4都是1行6列的矩陣。使用q=jtraj(q1,q4,4)語句規劃出兩個中間點在關節空間下所對應的關節角度矩陣，其中q=[q1;q2;q3;q4]是4行6列的矩陣，q2和q3代表關節空間下兩個中間點所對應的關節角度矩陣。

關節空間的路徑點如表1所示，θij表示關節i在第j個路徑點的位置，其中i表示關節的序號，j表示路徑點的序號。

表1 關節空間路徑點表

1.2 插補多項式的構造

機器人的每個關節在關節空間下有4個路徑點，從而形成三段軌跡。當關節在每個路徑點的關節角度已知時，采用5-7-5多項式插補函數對關節的三段軌跡曲線進行規劃。相比于3-5-3多項式插補函數，5-7-5多項式插補函數不僅使軌跡曲線的位置、速度、加速度連續，而且保證了多項式軌跡加加速度的連續性，減小了關節運動過程中的沖擊，提高了機械臂運動時的穩定性。5-7-5三段軌跡曲線表達式如下：

hi1(t)=ai15t5+ai14t4+ai13t3+ai12t2+

ai11t+ai10

(1)

hi2(t)=ai27t7+ai26t6+ai25t5+ai24t4+

ai23t3+ai22t2+ai21t+ai20

(2)

hi3(t)=ai35t5+ai34t4+ai33t3+ai32t2+

ai31t+ai30

(3)

式(1)-式(3)中的hi1(t)、hi2(t)、hi3(t)分別表示關節i的三段軌跡曲線表達式。

采用5-7-5多項式插補函數進行軌跡規劃時有如下約束條件：已知關節i起止點和兩個中間點的關節位置。保證起止點關節速度、加速度以及加加速度皆為零。兩個中間點處關節的位置、速度、加速度、加加速度、軌跡的四階導數皆連續。這樣在已知關節運動時間的前提下，可以推導出各關節軌跡曲線的系數和各路徑點關節角度的關系。

由式(4)-式(7)中的矩陣組成式(8)的時間系數矩陣A，式(9)中的b是已知的關節位置參數矩陣。通過智能控制算法優化關節運動時間，得到時間系數矩陣A。通過式(10)求得軌跡曲線的系數矩陣a，進而確定5-7-5軌跡多項式。

關節軌跡多項式的生成也為接下來判斷智能控制算法優化的時間間隔是否滿足運動學約束打下了基礎。

(4)

(5)

(6)

(7)

(8)

b=[θ1,0,0,0,θ4,0,0,0,θ2,θ2,θ3,θ3,

0,0,0,0,0,0,0,0]T

(9)

a=A-1·b

(10)

2 基于PSO-SA的時間最優軌跡規劃

2.1 適應度函數

本文主要研究在滿足PUMA_560機器人運動學約束條件下，對關節運動時間間隔進行優化，適應度函數如式(11)所示。

(11)

(12)

2.2 粒子群算法速度及位置更新

粒子群算法源于對自然界生物種群覓食行為的模仿，是一種生物智能算法[12]。對于粒子群優化算法，待優化問題在N維空間的任意一個解被稱為一個粒子。一個種群由m個粒子組成，在N維空間中粒子i的位置記為xi=(xi1,xi2,…,xiN)，粒子i的速度記為vi=(vi1,vi2,…,viN)。粒子與粒子之間通過相互協作尋找待優化問題的全局最優解。在PSO中通過目標函數值來判斷粒子位置的好壞。粒子i的個體最佳位置為pib，粒子群的歷史最佳位置為gb。粒子在N維搜索空間中向更優的位置飛行，粒子i的速度和位置更新如式(13)-式(14)所示。

(13)

(14)

2.3 慣性權重的設置

式(13)中w代表慣性權重的大小，在算法迭代過程中調整w值能平衡PSO的全局與局部搜索能力。采用慣性權重線性遞減的方法使得粒子權重值隨著迭代次數的增加線性遞減，在算法的前期加強全局搜索，在算法后期加強局部搜索。

對于一些復雜的或者非線性的尋優問題，慣性權重線性遞減的策略往往無法很好地平衡PSO的全局搜索和局部搜索能力。本文采用一種慣性權重非線性遞減策略對全局搜索和局部搜索進行平衡。慣性權重非線性遞減策略如式(15)所示，其中iter和itermax分別代表算法當前迭代次數和最大迭代次數。wmax和wmin分別是最大最小權重值，通過以自然常數為底數的冪函數使慣性權重從最大值非線性遞減至最小值。

(15)

2.4 引入動態學習因子

式(13)中的c1和c2分別表示粒子的全局學習因子和局部學習因子，動態學習因子由式(16)和式(17)來更新。動態學習因子的引入使算法早期加快全局搜索，同時使算法后期加快局部搜索。

(16)

(17)

2.5 模擬退火算法基本原理

模擬退火算法在原解xi的鄰域通過擾動隨機產生新解xi_new，算法通過 Metropolis準則判斷是否接受產生的新解[13]。Metropolis準則如式(19)所示，其中T0為模擬退火算法的初始溫度，α為溫度衰減系數，iter為算法的當前迭代次數，p表示接受新解的概率。根據Metropolis準則，在接受使Δf<0的新解的同時，以一定的概率接受使Δf≥0的新解，接受的概率會隨著溫度的降低而減小。

Δf=f(xi_new)-f(xi)

(18)

(19)

2.6 改進Metropolis準則

針對本文關于關節運動時間的優化問題，產生的新解需要滿足機器人運動學約束，需要對Metropolis準則進行改進，增強算法的尋優能力。

改進的Metropolis準則如式(20)所示。

(20)

式中：const=1表示上一代的解xi不滿足關節運動學約束，那么接受產生的新解。如果上一代的解滿足運動學約束條件，那么Δf<0時接受新解，Δf≥0時以一定的概率接受新解。

2.7 PSO-SA的步驟

粒子群模擬退火算法具體步驟如下：

步驟1初始化算法的參數：粒子群算法的最大迭代次數itermax為500，種群規模的大小為20，粒子的飛行速度范圍為-2到2，粒子的位置邊界為0.2到4。將慣性權重w的最大最小值wmax和wmin分別設置為0.9和0.4。令當前迭代次數iter為1。模擬退火的初始溫度T0設置為100 ℃，溫度衰減系數α為0.8。

步驟2初始化粒子的速度和位置，粒子的位置是關節軌跡三段運行時間(ti1,ti2,ti3)。

步驟3將m組解代入式(10)，求出系數矩陣a。將系數矩陣a代入式(1)-式(3),求出關節軌跡多項式。對關節5-7-5軌跡多項式求導得到關節速度多項式，對軌跡多項式求二階導得到加速度多項式，根據式(12)判斷解是否滿足運動學約束。

步驟4計算每個粒子的適應度值，如果粒子位置不滿足式(12)的約束，那么粒子的適應度值設為12。如果粒子滿足式(12)的約束，那么按照式(11)計算粒子的適應度值。

步驟5將m個粒子的初始位置設為每個粒子的個體最佳位置pib。比較m個粒子的目標函數值的大小，目標函數值最小的粒子位置設為粒子群的歷史最佳位置gb。

步驟6根據式(15)更新慣性權重值w，根據式(16)和式(17)更新全局學習因子c1和局部學習因子c2。

步驟7根據式(13)和式(14)分別更新m個粒子的速度和位置，并判斷每個粒子當前位置是否滿足運動學約束。

步驟8按照式(20)改進的Metropolis準則判斷每個粒子是否接受新產生的位置。

步驟9對比當前代種群每個粒子的目標函數值和該粒子的個體最佳位置pib的目標函數值，粒子當前位置的目標函數值更優，則將pib更新為粒子的當前位置。以步驟5的方式更新粒子群的歷史最佳位置gb。記錄每個粒子的當前位置。

步驟10按照公式T=α×T更新模擬退火溫度，使溫度下降。

步驟11判斷迭代次數iter是否小于最大迭代次數itermax，如果iter

3 仿真實驗和結果分析

3.1 5-7-5軌跡多項式的生成

本文以PUMA_560機器人作為研究對象，對機器人前3個關節進行仿真實驗。機器人關節空間下起止點和兩個中間點的關節角度如表1所示。

初始化關節運動時間間隔，令t1、t2、t3都等于3 s。根據式(8)到式(10)編寫程序，得到5-7-5軌跡多項式。關節1到關節3的原始位置、速度、加速度軌跡曲線如圖1至圖3所示。

圖1 關節1原始軌跡

圖2 關節2原始軌跡

圖3 關節3原始軌跡

由圖1至圖3可知，當t1、t2、t3取初始時間值時，關節的運動滿足速度和加速度的約束，但是還沒有達到關節的最大運行速度，關節運行效率低下。

3.2 基于PSO-SA的時間優化

使用PSO-SA對關節的運動時間進行優化，根據PSO-SA的執行步驟編寫程序，得到優化后各關節的運動時間間隔如表2所示。從表2可知，經過優化后，關節1到關節3運行時間分別從9 s減少到了4.883 1 s、2.374 8 s、1.176 9 s，大大地提高了關節的運行效率。

表2 優化后時間對比 單位：s

圖4 關節1優化軌跡

圖5 關節2優化軌跡

圖6 關節3優化軌跡

由圖4-圖6所示的關節軌跡多項式曲線可知，關節的位置、速度、加速度皆連續，而且加速度曲線有很好的光順性，有利于關節的平穩運行，減小機器人關節的磨損。

圖7為機械臂末端在笛卡爾空間的軌跡示意圖，軌跡通過了起止點和兩個中間點，同時軌跡曲線具有很好的光順性，驗證了使用5-7-5插補多項式規劃機器人軌跡的正確性和有效性。

圖7 機械臂末端笛卡爾空間軌跡示意圖

3.3 傳統PSO和PSO-SA優化時間對比

以關節1作為研究對象，分別使用傳統PSO和PSO-SA對關節運動時間進行優化。由圖8可知PSO-SA比傳統PSO收斂速度更快，能在搜索空間搜尋到更短的軌跡時間，說明PSO-SA具有更強的全局搜索能力。

圖8 PSO和PSO-SA優化時間對比

分別使用PSO-SA和PSO對關節1運動時間進行5次尋優，由表3可知PSO-SA的優化時間明顯短于PSO。同時，PSO的5次優化時間不斷變化，而PSO-SA的優化時間非常穩定，具有很強的穩定性。

表3 傳統PSO和PSO-SA在優化時間上的對比 單位：s

4 結 語

本文針對工業機器人點到點軌跡規劃問題，提出了一種基于PSO-SA的時間最優機器人關節空間軌跡規劃方法。使用模擬退火算法優化粒子群算法，增強了PSO的全局搜索能力。使用慣性權重非線性遞減策略和動態學習因子平衡了算法的全局搜索和局部搜索能力。實驗表明PSO-SA相比于傳統PSO能夠得到更短的軌跡時間，具有更強的全局搜索能力，同時算法的穩定性更好。PSO-SA在滿足運動學約束的前提下優化了關節的運動時間，提高了機械臂運行效率。5-7-5多項式插補函數規劃關節軌跡曲線，保證了軌跡曲線位置、速度、加速度的連續性，增強了關節運動的穩定性，避免了抖動，延長了機器臂的使用壽命。