謝能斌 辛紹杰

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

0 引 言

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

對機器人關(guān)節(jié)空間的軌跡進行合理的規(guī)劃，能夠提高機器人的工作效率，減小關(guān)節(jié)抖動，提高機器人運行的平穩(wěn)性[2]。

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

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

1 關(guān)節(jié)空間軌跡曲線生成

1.1 中間點的生成

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

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

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轉(zhuǎn)換成位姿矩陣T1，語句T4=transl(p4)將位置坐標p4轉(zhuǎn)換成位姿矩陣T4，其中T1和T4都是4×4的矩陣。語句q1=p560.ikine6s(T1)和q4=p560.ikine6s(T4)通過逆運動學求解起止點在關(guān)節(jié)空間下所對應的關(guān)節(jié)角度矩陣q1和q4，其中q1和q4都是1行6列的矩陣。使用q=jtraj(q1,q4,4)語句規(guī)劃出兩個中間點在關(guān)節(jié)空間下所對應的關(guān)節(jié)角度矩陣，其中q=[q1;q2;q3;q4]是4行6列的矩陣，q2和q3代表關(guān)節(jié)空間下兩個中間點所對應的關(guān)節(jié)角度矩陣。

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

表1 關(guān)節(jié)空間路徑點表

1.2 插補多項式的構(gòu)造

機器人的每個關(guān)節(jié)在關(guān)節(jié)空間下有4個路徑點，從而形成三段軌跡。當關(guān)節(jié)在每個路徑點的關(guān)節(jié)角度已知時，采用5-7-5多項式插補函數(shù)對關(guān)節(jié)的三段軌跡曲線進行規(guī)劃。相比于3-5-3多項式插補函數(shù)，5-7-5多項式插補函數(shù)不僅使軌跡曲線的位置、速度、加速度連續(xù)，而且保證了多項式軌跡加加速度的連續(xù)性，減小了關(guān)節(jié)運動過程中的沖擊，提高了機械臂運動時的穩(wěn)定性。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)分別表示關(guān)節(jié)i的三段軌跡曲線表達式。

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

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

關(guān)節(jié)軌跡多項式的生成也為接下來判斷智能控制算法優(yōu)化的時間間隔是否滿足運動學約束打下了基礎(chǔ)。

(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的時間最優(yōu)軌跡規(guī)劃

2.1 適應度函數(shù)

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

(11)

(12)

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

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

(13)

(14)

2.3 慣性權(quán)重的設(shè)置

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

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

(15)

2.4 引入動態(tài)學習因子

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

(16)

(17)

2.5 模擬退火算法基本原理

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

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

(18)

(19)

2.6 改進Metropolis準則

針對本文關(guān)于關(guān)節(jié)運動時間的優(yōu)化問題，產(chǎn)生的新解需要滿足機器人運動學約束，需要對Metropolis準則進行改進，增強算法的尋優(yōu)能力。

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

(20)

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

2.7 PSO-SA的步驟

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真實驗和結(jié)果分析

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

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

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

圖1 關(guān)節(jié)1原始軌跡

圖2 關(guān)節(jié)2原始軌跡

圖3 關(guān)節(jié)3原始軌跡

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

3.2 基于PSO-SA的時間優(yōu)化

使用PSO-SA對關(guān)節(jié)的運動時間進行優(yōu)化，根據(jù)PSO-SA的執(zhí)行步驟編寫程序，得到優(yōu)化后各關(guān)節(jié)的運動時間間隔如表2所示。從表2可知，經(jīng)過優(yōu)化后，關(guān)節(jié)1到關(guān)節(jié)3運行時間分別從9 s減少到了4.883 1 s、2.374 8 s、1.176 9 s，大大地提高了關(guān)節(jié)的運行效率。

表2 優(yōu)化后時間對比 單位：s

圖4 關(guān)節(jié)1優(yōu)化軌跡

圖5 關(guān)節(jié)2優(yōu)化軌跡

圖6 關(guān)節(jié)3優(yōu)化軌跡

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

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

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

3.3 傳統(tǒng)PSO和PSO-SA優(yōu)化時間對比

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

圖8 PSO和PSO-SA優(yōu)化時間對比

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

表3 傳統(tǒng)PSO和PSO-SA在優(yōu)化時間上的對比 單位：s

4 結(jié) 語

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