基于PSO-SA的機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃

謝能斌 辛紹杰

(上海電機(jī)學(xué)院機(jī)械學(xué)院 上海 201306)

0 引 言

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

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

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

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

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

1.1 中間點(diǎn)的生成

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

已知起止點(diǎn)的位置坐標(biāo)p1和p4分別為(856,-150,-94)和(350,-614,-502)，單位為毫米。使用逆運(yùn)動(dòng)學(xué)將起止點(diǎn)位置坐標(biāo)轉(zhuǎn)換成關(guān)節(jié)空間下的關(guān)節(jié)角度。通過以下程序生成兩個(gè)中間點(diǎn)所對應(yīng)的各個(gè)關(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機(jī)器人工具箱Robotic_Toolbox中自帶的PUMA_560機(jī)器人模型。語句T1=transl(p1)將位置坐標(biāo)p1轉(zhuǎn)換成位姿矩陣T1，語句T4=transl(p4)將位置坐標(biāo)p4轉(zhuǎn)換成位姿矩陣T4，其中T1和T4都是4×4的矩陣。語句q1=p560.ikine6s(T1)和q4=p560.ikine6s(T4)通過逆運(yùn)動(dòng)學(xué)求解起止點(diǎn)在關(guān)節(jié)空間下所對應(yīng)的關(guān)節(jié)角度矩陣q1和q4，其中q1和q4都是1行6列的矩陣。使用q=jtraj(q1,q4,4)語句規(guī)劃出兩個(gè)中間點(diǎn)在關(guān)節(jié)空間下所對應(yīng)的關(guān)節(jié)角度矩陣，其中q=[q1;q2;q3;q4]是4行6列的矩陣，q2和q3代表關(guān)節(jié)空間下兩個(gè)中間點(diǎn)所對應(yīng)的關(guān)節(jié)角度矩陣。

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

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

1.2 插補(bǔ)多項(xiàng)式的構(gòu)造

機(jī)器人的每個(gè)關(guān)節(jié)在關(guān)節(jié)空間下有4個(gè)路徑點(diǎn)，從而形成三段軌跡。當(dāng)關(guān)節(jié)在每個(gè)路徑點(diǎn)的關(guān)節(jié)角度已知時(shí)，采用5-7-5多項(xiàng)式插補(bǔ)函數(shù)對關(guān)節(jié)的三段軌跡曲線進(jìn)行規(guī)劃。相比于3-5-3多項(xiàng)式插補(bǔ)函數(shù)，5-7-5多項(xiàng)式插補(bǔ)函數(shù)不僅使軌跡曲線的位置、速度、加速度連續(xù)，而且保證了多項(xiàng)式軌跡加加速度的連續(xù)性，減小了關(guān)節(jié)運(yùn)動(dòng)過程中的沖擊，提高了機(jī)械臂運(yùn)動(dòng)時(shí)的穩(wěn)定性。5-7-5三段軌跡曲線表達(dá)式如下：

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的三段軌跡曲線表達(dá)式。

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

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

關(guān)節(jié)軌跡多項(xiàng)式的生成也為接下來判斷智能控制算法優(yōu)化的時(shí)間間隔是否滿足運(yùn)動(dòng)學(xué)約束打下了基礎(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的時(shí)間最優(yōu)軌跡規(guī)劃

2.1 適應(yīng)度函數(shù)

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

(11)

(12)

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

粒子群算法源于對自然界生物種群覓食行為的模仿，是一種生物智能算法[12]。對于粒子群優(yōu)化算法，待優(yōu)化問題在N維空間的任意一個(gè)解被稱為一個(gè)粒子。一個(gè)種群由m個(gè)粒子組成，在N維空間中粒子i的位置記為xi=(xi1,xi2,…,xiN)，粒子i的速度記為vi=(vi1,vi2,…,viN)。粒子與粒子之間通過相互協(xié)作尋找待優(yōu)化問題的全局最優(yōu)解。在PSO中通過目標(biāo)函數(shù)值來判斷粒子位置的好壞。粒子i的個(gè)體最佳位置為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ù)的增加線性遞減，在算法的前期加強(qiáng)全局搜索，在算法后期加強(qiáng)局部搜索。

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

(15)

2.4 引入動(dòng)態(tài)學(xué)習(xí)因子

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

(16)

(17)

2.5 模擬退火算法基本原理

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

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

(18)

(19)

2.6 改進(jìn)Metropolis準(zhǔn)則

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

改進(jìn)的Metropolis準(zhǔn)則如式(20)所示。

(20)

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

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。令當(dāng)前迭代次數(shù)iter為1。模擬退火的初始溫度T0設(shè)置為100 ℃，溫度衰減系數(shù)α為0.8。

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

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

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

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

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

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

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

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

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

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

3 仿真實(shí)驗(yàn)和結(jié)果分析

3.1 5-7-5軌跡多項(xiàng)式的生成

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

初始化關(guān)節(jié)運(yùn)動(dòng)時(shí)間間隔，令t1、t2、t3都等于3 s。根據(jù)式(8)到式(10)編寫程序，得到5-7-5軌跡多項(xiàng)式。關(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可知，當(dāng)t1、t2、t3取初始時(shí)間值時(shí)，關(guān)節(jié)的運(yùn)動(dòng)滿足速度和加速度的約束，但是還沒有達(dá)到關(guān)節(jié)的最大運(yùn)行速度，關(guān)節(jié)運(yùn)行效率低下。

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

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

表2 優(yōu)化后時(shí)間對比 單位：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é)軌跡多項(xiàng)式曲線可知，關(guān)節(jié)的位置、速度、加速度皆連續(xù)，而且加速度曲線有很好的光順性，有利于關(guān)節(jié)的平穩(wěn)運(yùn)行，減小機(jī)器人關(guān)節(jié)的磨損。

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

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

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

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

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

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

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

4 結(jié) 語

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