基于改進(jìn)粒子群算法的機(jī)械臂軌跡規(guī)劃研究

隋 濤，姜 昊，孔劉君，蔣 強(qiáng)

(沈陽理工大學(xué) 自動(dòng)化與電氣工程學(xué)院，沈陽 110159)

軌跡規(guī)劃是機(jī)器人學(xué)中最重要的問題之一[1]，也是機(jī)器人運(yùn)動(dòng)學(xué)研究的熱點(diǎn)問題。目前軌跡優(yōu)化常采用最短執(zhí)行時(shí)間、最小能量和最小沖擊等標(biāo)準(zhǔn)[2]，其中時(shí)間最優(yōu)軌跡是當(dāng)前學(xué)者們研究最多的方向。

為解決機(jī)械臂時(shí)間最優(yōu)軌跡規(guī)劃的問題，文獻(xiàn)[3]通過五次非均勻B樣條構(gòu)造軌跡函數(shù)，采用改進(jìn)的自適應(yīng)遺傳算法對(duì)運(yùn)行時(shí)間進(jìn)行優(yōu)化，實(shí)現(xiàn)時(shí)間最優(yōu)；文獻(xiàn)[4]在滿足關(guān)節(jié)角速度約束條件下，構(gòu)造4-3-4多項(xiàng)式軌跡，利用改進(jìn)粒子群算法實(shí)現(xiàn)時(shí)間最優(yōu)；文獻(xiàn)[5]在采用三次B樣條插值構(gòu)造軌跡規(guī)劃的基礎(chǔ)上，結(jié)合改進(jìn)的粒子群算法達(dá)到時(shí)間優(yōu)化的目的；文獻(xiàn)[6]采用五次多項(xiàng)式插值法，在滿足最大速度、最大加速度和最大沖擊速度條件下，結(jié)合改進(jìn)的模擬退火算法得到時(shí)間最優(yōu)軌跡；文獻(xiàn)[7]使用粒子群算法，以速度為約束條件，對(duì)構(gòu)造的3-5-3多項(xiàng)式軌跡進(jìn)行優(yōu)化，得到最優(yōu)時(shí)間；文獻(xiàn)[8]采用五次多項(xiàng)式構(gòu)造軌跡插值函數(shù)，結(jié)合改進(jìn)的粒子群算法實(shí)現(xiàn)時(shí)間最優(yōu)。

標(biāo)準(zhǔn)粒子群算法具有規(guī)則簡單、易于實(shí)現(xiàn)等優(yōu)點(diǎn)，但容易陷入局部優(yōu)化的困境。為此，本文利用3-5-3多項(xiàng)式插值法構(gòu)造機(jī)械臂的運(yùn)動(dòng)軌跡，以時(shí)間最優(yōu)為準(zhǔn)則，設(shè)定速度、加速度為約束條件，提出一種改進(jìn)的粒子群算法，通過非線性函數(shù)改變學(xué)習(xí)因子，避免在應(yīng)用標(biāo)準(zhǔn)粒子群算法時(shí)陷入局部優(yōu)化的問題。

1 機(jī)械臂關(guān)節(jié)空間軌跡規(guī)劃

本文采用3-5-3多項(xiàng)式插值法進(jìn)行軌跡規(guī)劃，在機(jī)械臂的工作空間中，選取4個(gè)可到達(dá)的路徑點(diǎn)，包括起始點(diǎn)、終止點(diǎn)、路徑點(diǎn)1和路徑點(diǎn)2，構(gòu)成4個(gè)關(guān)鍵點(diǎn)、3段軌跡，兩點(diǎn)之間分別用3次、5次和3次多項(xiàng)式進(jìn)行插值。設(shè)機(jī)械臂關(guān)節(jié)編號(hào)為i(i＝1，2，3，4，5，6)，4個(gè)路徑點(diǎn)處的關(guān)節(jié)角度為θi0、θi1、θi2、θi3，構(gòu)造某一關(guān)節(jié)的3-5-3插值多項(xiàng)式為

式中:h函數(shù)項(xiàng)的下角標(biāo)中1、2、3表示三段軌跡；t是插值時(shí)間；b表示函數(shù)中的待求系數(shù)，其下角標(biāo)對(duì)應(yīng)為關(guān)節(jié)、軌跡和多項(xiàng)式系數(shù)序號(hào)。

約束條件分別為起始點(diǎn)和終止點(diǎn)處的關(guān)節(jié)角度、速度和加速度及路徑點(diǎn)1、2的關(guān)節(jié)角度，路徑點(diǎn)的關(guān)節(jié)角度、速度和加速度保持連續(xù)，根據(jù)滿足的約束條件分別求得3-5-3多項(xiàng)式軌跡的14個(gè)系數(shù)，具體求解過程參見文獻(xiàn)[9]。

2 時(shí)間最優(yōu)軌跡規(guī)劃問題描述

2.1 目標(biāo)函數(shù)的描述

在機(jī)械臂的工作空間中選取4個(gè)可到達(dá)的路徑點(diǎn)構(gòu)成3段軌跡，每段軌跡各自的插值時(shí)間為ti1、ti2、ti3，總的運(yùn)行時(shí)間用T表示，以時(shí)間最短為優(yōu)化目標(biāo)，即優(yōu)化問題的目標(biāo)函數(shù)為

2.2 約束條件的描述

對(duì)于時(shí)間最優(yōu)問題，以三段軌跡的時(shí)間總和為目標(biāo)函數(shù)，約束條件為速度、加速度。

(1)速度約束條件

式中:｜｜表示關(guān)節(jié)運(yùn)動(dòng)時(shí)的實(shí)時(shí)速度絕對(duì)值；為關(guān)節(jié)允許的最大關(guān)節(jié)速度。

(2)加速度約束條件

式中:｜｜表示關(guān)節(jié)運(yùn)動(dòng)時(shí)的實(shí)時(shí)加速度絕對(duì)值；為關(guān)節(jié)允許的最大關(guān)節(jié)加速度。

3 改進(jìn)的粒子群算法

粒子群算法是一種基于種群的搜索算法。將隨機(jī)生成粒子作為初始種群，每個(gè)粒子代表一個(gè)由速度和位置組成的潛在解，每個(gè)粒子根據(jù)目前獲得的記憶更新其速度[10]。

假設(shè)在D維搜索空間中，每個(gè)粒子代表一個(gè)候選解，設(shè)第j個(gè)粒子位置為xj，xj＝(xj1，xj2，…，xjD)，速度為vj，vj＝(vj1，vj2，…，vjD)，粒子從初始到迭代結(jié)束搜索產(chǎn)生的個(gè)體最優(yōu)解為Pj，Pj＝(pj1，pj2，…，pjD)。整個(gè)種群的最優(yōu)解為Pg，隨機(jī)初始化j個(gè)粒子，隨機(jī)初始化粒子的位置和速度，更新粒子j在d維分量(1≤d≤D)中的位置和速度，迭代公式為

式中:k為當(dāng)前的迭代次數(shù)；w為慣性權(quán)重；r1、r2取值為[0，1]的隨機(jī)數(shù)；c1為局部學(xué)習(xí)因子；c2為全局學(xué)習(xí)因子；表示粒子在當(dāng)前迭代次數(shù)下的速度和位置。

標(biāo)準(zhǔn)的粒子群算法容易陷入局部最優(yōu)、迭代不收斂問題。為此，引入線性遞減的慣性權(quán)重w為

式中:wmax為慣性權(quán)重最大值；wmin為慣性權(quán)重最小值；Nmax為允許的最大迭代次數(shù)。

本文將迭代公式中的學(xué)習(xí)因子c1、c2由固定值改為非線性變化的動(dòng)態(tài)函數(shù)，分別為

式中cmax、cmin為學(xué)習(xí)因子的最大值和最小值。

在算法的早期階段，粒子需要有較強(qiáng)的個(gè)體學(xué)習(xí)能力，所以局部學(xué)習(xí)因子c1較大，以加快粒子更新速度，更快找到最優(yōu)位置；在算法迭代后期，需要較強(qiáng)的群體學(xué)習(xí)能力，全局學(xué)習(xí)因子c2增大，局部學(xué)習(xí)因子c1減小，以提高算法精度[11]，故c1為遞減函數(shù)，c2為遞增函數(shù)。

基于改進(jìn)的粒子群算法時(shí)間最優(yōu)軌跡規(guī)劃的步驟如下。

步驟1:參數(shù)初始化。初始化種群位置為0.1~4上的隨機(jī)數(shù)值，構(gòu)成初始種群，并初始化粒子的速度。

步驟2:將初始化后得到的隨機(jī)粒子帶入到式(1)~(3)中，解出各關(guān)節(jié)軌跡函數(shù)的系數(shù)，并得到關(guān)節(jié)的每段軌跡方程。

步驟3:對(duì)各關(guān)節(jié)的每段軌跡方程求導(dǎo)得到各關(guān)節(jié)每段軌跡的關(guān)節(jié)角速度和關(guān)節(jié)角加速度方程，判斷每段方程中的速度和加速度是否同時(shí)滿足速度和加速度約束條件，若有任何一段不滿足，則設(shè)此粒子的適應(yīng)度值為無窮大，方便后續(xù)剔除該粒子。

步驟4:計(jì)算各粒子適應(yīng)度，挑選出具有個(gè)體和全局最佳適應(yīng)度值以及對(duì)應(yīng)最佳位置的粒子。

步驟5:更新慣性權(quán)重與學(xué)習(xí)因子，利用迭代公式更新粒子的位置和速度，隨著粒子位置的不斷更新，計(jì)算粒子適應(yīng)度，通過迭代，得到全局最優(yōu)解，若滿足最大迭代次數(shù)，則適應(yīng)度值最小的粒子為最優(yōu)解，算法結(jié)束，否則回到步驟2。

時(shí)間最優(yōu)軌跡規(guī)劃流程如圖1所示。

圖1 時(shí)間最優(yōu)軌跡規(guī)劃程序流程圖

4 仿真分析

4.1 機(jī)械臂建模

本文以KUKA KR3 R540為研究對(duì)象，該機(jī)械臂D-H參數(shù)如表1所示。

表1 D-H參數(shù)表

利用Matlab中的機(jī)器人工具箱對(duì)該機(jī)械臂采用標(biāo)準(zhǔn)D-H參數(shù)法進(jìn)行建模，連桿坐標(biāo)系建立方式參考文獻(xiàn)[12]，本文的基坐標(biāo)系O0建立在地面上，O1~O6為各關(guān)節(jié)對(duì)應(yīng)的連桿坐標(biāo)系。O4、O5坐標(biāo)系建立在同一原點(diǎn)處，O6坐標(biāo)系建立在末端法蘭盤上。機(jī)械臂D-H連桿坐標(biāo)系如圖2所示，始關(guān)節(jié)角度為(0，-90，0，0，0，0)，機(jī)械臂仿真圖及路徑點(diǎn)選取如圖3所示。

圖2 機(jī)械臂D-H連桿坐標(biāo)系

圖3 機(jī)械臂仿真圖及路徑點(diǎn)選取

在機(jī)械臂工作空間中選取4個(gè)能到達(dá)的路徑點(diǎn)作為插值點(diǎn)，將選取的4個(gè)插值點(diǎn)從笛卡爾空間變換到關(guān)節(jié)空間，路徑點(diǎn)處關(guān)節(jié)空間角度如表2所示。

表2 路徑點(diǎn)處關(guān)節(jié)空間角度

4.2 時(shí)間最優(yōu)軌跡規(guī)劃仿真

對(duì)于傳統(tǒng)的3-5-3構(gòu)造插值多項(xiàng)式的方法，需輸入指定的預(yù)設(shè)時(shí)間以求取多項(xiàng)式的系數(shù)，本文預(yù)設(shè)的3段插值時(shí)間均為4 s。

初始化種群規(guī)模為50的粒子群，迭代次數(shù)Nmax＝200；wmax＝0.9、wmin＝0.4；參考文獻(xiàn)[9]，選取cmax＝2.5、cmin＝0.5，使c1、c2在[0.5，2.5]范圍變化。

設(shè)定機(jī)械臂各關(guān)節(jié)速度約束條件為?θmax＝[-100，100]，加速度約束條件設(shè)為＝50。

利用粒子群算法求解時(shí)間最優(yōu)，分為3個(gè)策略進(jìn)行比較。策略1:標(biāo)準(zhǔn)粒子群，w＝0.9，c1＝c2＝2；策略2:慣性權(quán)重根據(jù)式(7)變化，c1＝c2＝2；策略3:慣性權(quán)重根據(jù)式(7)變化，學(xué)習(xí)因子根據(jù)式(8)、式(9)變化。

以關(guān)節(jié)1為例，分別在策略1、策略2和策略3下進(jìn)行適應(yīng)度仿真，結(jié)果對(duì)比如圖4所示。

圖4 3種策略的適應(yīng)度曲線對(duì)比圖

根據(jù)仿真結(jié)果的對(duì)比可以明顯看出，策略3相比策略1提高了收斂速度與精度，與策略2相比提高了收斂速度，驗(yàn)證了改進(jìn)算法的有效性。

策略3優(yōu)化下的各關(guān)節(jié)運(yùn)行時(shí)間如表3所示。

表3 關(guān)節(jié)優(yōu)化結(jié)果 s

在執(zhí)行工作任務(wù)時(shí)，機(jī)械臂的所有關(guān)節(jié)在同一時(shí)間內(nèi)運(yùn)動(dòng)，所以選取的是優(yōu)化后各關(guān)節(jié)在每段軌跡中的最大值。最終的插值時(shí)間組合為(2.906 6，2.445 0，2.446 8)，總時(shí)間為7.798 4 s。

利用3-5-3多項(xiàng)式插值進(jìn)行規(guī)劃，優(yōu)化前6個(gè)關(guān)節(jié)角度變化曲線如圖5所示。

圖5 優(yōu)化前6個(gè)關(guān)節(jié)角度變化曲線

按照策略3優(yōu)化后6個(gè)關(guān)節(jié)角度變化曲線如圖6所示。

圖6 優(yōu)化后6個(gè)關(guān)節(jié)角度變化曲線

優(yōu)化前6個(gè)關(guān)節(jié)角速度變化曲線如圖7所示。

圖7 優(yōu)化前6個(gè)關(guān)節(jié)角速度變化曲線

策略3優(yōu)化下各關(guān)節(jié)的角速度更快一些，優(yōu)化后6個(gè)關(guān)節(jié)角速度變化曲線如圖8所示。

圖8 優(yōu)化后6個(gè)關(guān)節(jié)角速度變化曲線

優(yōu)化前6個(gè)關(guān)節(jié)角加速度變化曲線如圖9所示。

圖9 優(yōu)化前6個(gè)關(guān)節(jié)角加速度變化曲線

策略3優(yōu)化下各關(guān)節(jié)角加速度更快一些，優(yōu)化后6個(gè)關(guān)節(jié)角加速度變化曲線如圖10所示。

圖10 優(yōu)化后6個(gè)關(guān)節(jié)角加速度變化曲線

根據(jù)上述優(yōu)化前后的仿真圖對(duì)比，優(yōu)化后的機(jī)械臂關(guān)節(jié)可以得到平滑、連續(xù)且時(shí)間最優(yōu)的軌跡曲線，優(yōu)化后比優(yōu)化前各關(guān)節(jié)的速度和加速度峰值明顯提高，表明提高了機(jī)械臂的工作效率。

5 結(jié)論

針對(duì)機(jī)械臂關(guān)節(jié)空間時(shí)間最優(yōu)軌跡規(guī)劃問題，提出一種改進(jìn)的粒子群算法，通過對(duì)學(xué)習(xí)因子非線性變化及線性慣性權(quán)重合理匹配，實(shí)現(xiàn)機(jī)械臂軌跡時(shí)間優(yōu)化。通過Matlab仿真實(shí)驗(yàn)可以看出:改進(jìn)粒子群算法將機(jī)械臂的運(yùn)行時(shí)間從預(yù)設(shè)12 s縮短至7.798 4 s，并可以使各關(guān)節(jié)的角度、角速度、角加速度軌跡連續(xù)平滑。