基于改進(jìn)PSO算法的時間最優(yōu)機(jī)械臂軌跡規(guī)劃

李 虎，劉泓濱

(昆明理工大學(xué)機(jī)電工程學(xué)院，昆明 650500)

0 引言

隨著社會現(xiàn)代化和工業(yè)自動化的發(fā)展，6自由度機(jī)械臂憑借著在工作空間中快速、穩(wěn)定和高效的工作特點，被大量地使用在工業(yè)自動化生產(chǎn)過程中，例如焊接、搬運(yùn)、噴漆、碼垛和機(jī)械加工等方面[1-2]。實際應(yīng)用表明，為了提高工業(yè)生產(chǎn)和制造效率，對機(jī)械臂的運(yùn)行軌跡進(jìn)行合理規(guī)劃是至關(guān)重要的。目前,已有大批的國內(nèi)外專家和學(xué)者在圍繞機(jī)械臂軌跡規(guī)劃中的時間最優(yōu)、能量最優(yōu)、沖擊最優(yōu)以及綜合最優(yōu)等方面展開了大量的深入研究與探討，而其中6自由度機(jī)械臂時間最優(yōu)軌跡規(guī)劃問題則是深入研究與探討中的熱門話題。

時間最優(yōu)軌跡是指6自由度機(jī)械臂在關(guān)節(jié)空間下利用最短時間完成相應(yīng)路徑的軌跡。對6自由度機(jī)械臂軌跡規(guī)劃的智能算法有很多種，如PSO算法、遺傳算法、模擬退火算法和螢火蟲算法等其他智能算法，但是由于粒子群算法具有結(jié)構(gòu)簡單、參數(shù)少和快速搜索能力等優(yōu)點被廣大學(xué)者采用在機(jī)械臂的軌跡規(guī)劃中[3]。馮斌等[4]采用標(biāo)準(zhǔn)的粒子群算法和高次多項式插值的方式對機(jī)械臂軌跡規(guī)劃，保證了機(jī)械臂的各個關(guān)節(jié)可以平穩(wěn)運(yùn)行，但是標(biāo)準(zhǔn)粒子群優(yōu)化機(jī)械臂軌跡容易陷入局部最優(yōu)使其精度不佳。肖仁等[5]利用量子粒子群算法和多項式插值構(gòu)造待優(yōu)化函數(shù)，有效地解決了插值軌跡階次高和凸包性等問題。韓順杰等[6]提出一種正弦函數(shù)動態(tài)調(diào)整學(xué)習(xí)因子的改進(jìn)粒子群算法并結(jié)合3-4-3分段多項式對工業(yè)機(jī)器人的運(yùn)行軌跡進(jìn)行優(yōu)化，避免了粒子群初期容易陷入局部極值。郭彤穎等[7]采用粒子群算法和B樣條曲線對機(jī)械臂進(jìn)行軌跡規(guī)劃，有效地提高了機(jī)械臂的工作效率。陳波等[8]提出一種雙種群粒子群算法對機(jī)械臂軌跡進(jìn)行時間最優(yōu)規(guī)劃，每個路徑點之間通過3次多項式進(jìn)行連接，該方法在機(jī)械臂時間最優(yōu)軌跡規(guī)劃中起到了重要作用。田恒等[9]對傳統(tǒng)的粒子群算法的學(xué)習(xí)因子進(jìn)行了改進(jìn)，提出綜合考慮學(xué)習(xí)因子的影響并且動態(tài)分配學(xué)習(xí)因子的大小，該方法具有更快的收斂速度。黃超等[10]提出改進(jìn)粒子群算法中慣性因子，有效地增強(qiáng)了粒子群算法的搜索精度。以上研究在機(jī)械臂軌跡規(guī)劃中取得了大量的成果，但是大多存在階次高、計算復(fù)雜和優(yōu)化算法易早熟等問題。

綜上所述，本文以UR106自由度機(jī)械臂為研究對象，提出了一種以時間最優(yōu)為目標(biāo)，采用改進(jìn)PSO算法對6自由度機(jī)械臂軌跡進(jìn)行優(yōu)化的方法。首先，在關(guān)節(jié)空間下利用機(jī)械臂正逆運(yùn)動學(xué)原理獲取其軌跡插值點；然后，為了使機(jī)械臂能夠快速平穩(wěn)地到達(dá)目標(biāo)位置，采用3-5-3多項式對其軌跡進(jìn)行插值；最后，使用改進(jìn)PSO算法對多項式插值構(gòu)造的軌跡進(jìn)行優(yōu)化，實現(xiàn)6自由度機(jī)械臂時間最優(yōu)的軌跡規(guī)劃并在MATLAB軟件驗證該方法的有效性。

1 UR10機(jī)械臂的運(yùn)動學(xué)模型與仿真

1.1 機(jī)械臂的正運(yùn)動學(xué)分析

UR10協(xié)作機(jī)械臂是一種6自由度串聯(lián)機(jī)械臂，機(jī)械臂的各個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，該機(jī)械臂的額定載量為10 kg，最大工作半徑范圍為1300 mm。該機(jī)械臂的3維模型如圖1所示，連桿坐標(biāo)系如圖2所示。

圖1 UR106自由度機(jī)械臂 圖2 UR10機(jī)械臂連桿坐標(biāo)系

坐標(biāo)系{o}為機(jī)械臂基座參考系，連接基座底面中心，坐標(biāo)系{oi}為第i個連桿的坐標(biāo)系。由連桿坐標(biāo)系創(chuàng)建D-H表，如表1所示。

表1 UR10機(jī)械臂D-H表

其中，ai-1為沿著xi-1軸，從zi-1移動到zi的距離；αi-1為繞著xi-1軸，從zi-1旋轉(zhuǎn)到zi的扭角；di為沿著zi軸，從xi-1移動到xi的位移；θ為繞著zi軸，從xi-1旋轉(zhuǎn)到xi的轉(zhuǎn)角。

根據(jù)D-H表中的參數(shù)建立UR10的正運(yùn)動學(xué)方程，2個相鄰關(guān)節(jié)之間坐標(biāo)系的轉(zhuǎn)換通式為：

(1)

將UR10機(jī)械臂D-H表中的參數(shù)帶入式(1)中可得各關(guān)節(jié)坐標(biāo)系的變換矩陣：

(2)

(3)

(4)

(5)

(6)

(7)

在已知各關(guān)節(jié)的位姿和姿態(tài)時，聯(lián)立式(2)～式(7)可得該機(jī)械臂末端關(guān)節(jié)相對于基坐標(biāo)系{o}的位姿和姿態(tài)：

(8)

式中，n、o和q分別為機(jī)械臂位姿矩陣的分量；p為機(jī)械臂的位置分量。

1.2 UR10機(jī)械臂逆運(yùn)動學(xué)分析與仿真

逆運(yùn)動學(xué)就是通過求解機(jī)械臂末端關(guān)節(jié)相對于基坐標(biāo)系的位姿和姿態(tài)，從而求得各個關(guān)節(jié)的轉(zhuǎn)角。本文使用MATLAB軟件中的Robotics Toolbox工具箱進(jìn)行機(jī)械臂的正逆運(yùn)動學(xué)求解。

在機(jī)械臂的工作空間內(nèi)，選取機(jī)械臂各關(guān)節(jié)角為q=[-π/2,0,π/2,0,0,0]代入式(8)中得到機(jī)械臂末端關(guān)節(jié)相對于基座標(biāo)系的齊次變換矩陣為：

(9)

由式(9)可知機(jī)械臂末端關(guān)節(jié)的位姿和姿態(tài)，在MATLAB機(jī)器人工具箱內(nèi)調(diào)用qi=ikine(r,T)函數(shù)進(jìn)行逆運(yùn)動學(xué)仿真，可以得UR106自由度機(jī)械臂的關(guān)節(jié)角的矢量為qi=[-1.5708,0.0000,1.5708,0.0000,0.0000,0.0000]，由仿真結(jié)果可知將qi轉(zhuǎn)換為角度制，即q=qi。因此驗證了在工具箱中建立的機(jī)械臂模型是正確的。

2 UR10機(jī)械臂軌跡規(guī)劃

2.1 分段多項式插值函數(shù)的構(gòu)造

為了確保機(jī)械臂在工業(yè)生產(chǎn)中運(yùn)行的平穩(wěn)性和準(zhǔn)確性，其位置、速度和加速度都是連續(xù)可控的，通常使用插值的方式對機(jī)械臂的運(yùn)行軌跡進(jìn)行插值，常用的插值多項式函數(shù)有3次和5次多項式。使用3次多項式對軌跡進(jìn)行插值是一種基本的插值方法，但是采用該多項式函數(shù)插值只能保證機(jī)械臂各關(guān)節(jié)的位置和速度連續(xù)，無法使各關(guān)節(jié)的加速度連續(xù)可控。5次多項式插值是機(jī)械臂軌跡規(guī)劃中最常用的插值方法，它可以保證機(jī)械臂各關(guān)節(jié)的位置、速度和加速度連續(xù)，但是每一段軌跡都使用5次多項式插值其計算量大，且階次過高會導(dǎo)致凸包性過差等問題。因此本文采用3-5-3分段多項式對機(jī)械臂的運(yùn)行軌跡進(jìn)行插值，在一定的約束條件下，該多項式既可以保證各關(guān)節(jié)位置、速度和加速度的連續(xù)，也可以減小計算量和避免凸包性過差等問題。

3-5-3分段多項式函數(shù)的表達(dá)式為：

(10)

式中，qij為機(jī)械臂第i個關(guān)節(jié)第j段的多項式軌跡；aijk為機(jī)械臂第i關(guān)節(jié)第j段插值多項式函數(shù)的第k個未知系數(shù)；t1、t2、t3分別為機(jī)械臂每個關(guān)節(jié)第1段、第2段和第3段的運(yùn)動時間。

機(jī)械臂各個關(guān)節(jié)通過起始點β1，路徑點β2、β3和終止點β4時，β1和β4處的速度和加速度均為0，其他插值點重合處的速度和加速度應(yīng)相等。因此利用上述的約束條件可以求得3-5-3分段多項式的各個系數(shù)[11]。

(11)

b=[0 0 0 0 0 0β40 0β10 0β3β2]

(12)

a=A-1b=[A1A2A3]T

(13)

式中，

2.2 基于改進(jìn)粒子群算法的軌跡優(yōu)化

以時間最少為目標(biāo)，利用改進(jìn)PSO算法對6自由度機(jī)械臂的運(yùn)動軌跡進(jìn)行規(guī)劃，選取每個關(guān)節(jié)完成每段軌跡所用的時間t1，t2，t3構(gòu)成一個三維的粒子群，其適應(yīng)度函數(shù)表達(dá)式為：

F=t1+t2+t3

(14)

根據(jù)機(jī)械臂的實際工作環(huán)境以及機(jī)械臂自身運(yùn)動學(xué)和動力的物理極限，可以確定機(jī)械臂各關(guān)節(jié)的物理約束條件為：

(15)

式中，Qimax、Vimax和Aimax分別為機(jī)械臂各關(guān)節(jié)運(yùn)動過程中允許的最大位置、速度和加速度。

PSO算法是一種模擬鳥群捕食活動的智能算法，它憑借著參數(shù)少、收斂迅速、結(jié)構(gòu)簡單和易控制等特點，在處理優(yōu)化問題中更加快速便捷[12]，但是傳統(tǒng)的PSO算法容易受到待優(yōu)化函數(shù)的影響提前收斂到局部最優(yōu)。在每次迭代過程中，粒子群中的所有粒子都必須從當(dāng)前的位置根據(jù)其更新的速度向新的位置移動。

其粒子的速度和位置更新公式為[13]：

(16)

(17)

標(biāo)準(zhǔn)粒子群算法中的參數(shù)是固定不變的。ω描述的是粒子的慣性，在進(jìn)化前期ω應(yīng)該更大一些，保證每個粒子能夠獨立飛行充分搜索空間，后期應(yīng)該小一點，多向其他粒子學(xué)習(xí)。c1和c2分別描述的是個體和全局的最大飛行步長，前期c1盡可能的大一些，后期c2應(yīng)盡可能的大一些，這樣就可以平衡粒子的全局搜索能力和局部搜索能力。3個參數(shù)共同影響了粒子的飛行方向，假設(shè)其他粒子找到了更好的位置，但是當(dāng)前粒子的慣性太大不能很快地飛向更優(yōu)的位置，這就有可能導(dǎo)致算法易早熟或者是算法后期收斂緩慢等問題。

針對PSO易早熟且后期收斂速度慢等問題，本文對粒子群算法進(jìn)行以下幾點改進(jìn)：

(1)采用混沌序列初始化代替PSO中的隨機(jī)初始化粒子的種群。PSO中隨機(jī)初始化易導(dǎo)致種群初始化分布不均從而影響算法的優(yōu)化?；煦缧蛄谐跏蓟N群時具有遍歷性、隨機(jī)性和均勻性的特點，是在規(guī)定的范圍內(nèi)根據(jù)本身規(guī)律不重疊地遍歷整個搜索空間。因此，混沌序列初始化的種群可以有效地提高PSO算法的求解的精度和收斂速度。本文采用典型的混沌序列非線性映射[14]，其表達(dá)式為：

(18)

(2)提出一種自適應(yīng)調(diào)整慣性權(quán)重的方法。常用的線性遞減的慣性權(quán)重對POS跳出局部最優(yōu)有一定作用，但是無論粒子的適應(yīng)度是否變得更好，慣性權(quán)重都會根據(jù)迭代次數(shù)以線性速度遞減。所以依靠線性遞減慣性因子的粒子群算法有時也會陷入局部最優(yōu)。自適應(yīng)調(diào)整慣性權(quán)重綜合考慮了粒子的迭代次數(shù)和粒子的目標(biāo)值，初期使用較大的ω增強(qiáng)粒子的全局搜索能力，當(dāng)有粒子飛行至最優(yōu)點附近時，則減小ω增加粒子的局部搜索能力，這樣對PSO優(yōu)化的結(jié)果有較大提升。自適應(yīng)調(diào)整慣性權(quán)重的表達(dá)式為：

(19)

式中，ωmax、ωmin為慣性因子的最大值和最小值；Tmax為最大迭代次數(shù)；f為當(dāng)前粒子的目標(biāo)值；favg為平均目標(biāo)值。

(3)將局部和全局的學(xué)習(xí)因子c1和c2聯(lián)合起來。當(dāng)增大c1時，會增強(qiáng)粒子的個體認(rèn)知能力，但是收斂速度緩慢；當(dāng)增大c2時，會增強(qiáng)粒子的社會認(rèn)知能力，收斂速度迅速，但是極易使算法早熟。為了保證PSO算法的搜索能力和協(xié)調(diào)粒子的個體認(rèn)知和社會認(rèn)知能力，采用動態(tài)調(diào)整學(xué)習(xí)因子的方法，將學(xué)習(xí)因子的取值與粒子當(dāng)前的迭代次數(shù)聯(lián)系起來。在搜索初期粒子個體的搜索意義大于全局的搜索意義，所以c1>c2；在搜索后期為了更加快速地獲得全局最優(yōu)解，應(yīng)增加粒子與種群之間的交流，所以c1

(20)

式中，c1max、c1min、c2max和c2min為c1、c2的最大值和最小值。

改進(jìn)PSO算法對6自由度機(jī)械臂軌跡優(yōu)化的具體步驟如下：

步驟1：機(jī)械臂每個關(guān)節(jié)3段軌跡的運(yùn)動時間(t1、t2、t3)構(gòu)成一個三維的搜索空間，采用混沌初始化產(chǎn)生N個粒子(共有3N個粒子)構(gòu)成種群，并初始化種群中各粒子的位置和速度。

步驟2：將初始化產(chǎn)生的N個三維時間組合帶入式(11)～式(13)，求得各分段函數(shù)的系數(shù)。

步驟3：判斷機(jī)械臂各關(guān)節(jié)3段運(yùn)動軌跡的速度和加速度是否滿足最大約束條件，若3段都滿足約束條件，則將3段軌跡的運(yùn)行時間帶入式(14)計算其適應(yīng)度值，若3段中任意1段的速度或加速度不滿足約束，則賦予一個很大的值作為該粒子的適應(yīng)度值，以便在下一次迭代中淘汰該粒子。

步驟4：經(jīng)過k次迭代后，通過比較個體極值和全局極值的適應(yīng)度值，選出適應(yīng)度最佳的粒子，即為最優(yōu)極值。

步驟5：當(dāng)機(jī)械臂所有關(guān)節(jié)的軌跡都完成優(yōu)化時，每段時間取各關(guān)節(jié)完成該段軌跡所需要的最長時間，進(jìn)而確保機(jī)械臂的所有關(guān)節(jié)都能完成軌跡的運(yùn)行。

6自由度機(jī)械臂軌跡優(yōu)化的具體流程如圖3所示。

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

為了驗證上述方法的正確性和有效性，本文以UR106自由度機(jī)械臂為實驗對象，在MATLAB軟件中采用3-5-3分段多項式構(gòu)造關(guān)節(jié)軌跡，再使用改進(jìn)PSO算法以時間最優(yōu)為目的對關(guān)節(jié)軌跡進(jìn)行優(yōu)化。機(jī)械臂各關(guān)節(jié)插值點的位置信息如表2所示。

表2 各關(guān)節(jié)位置信息

在仿真實驗中，根據(jù)改進(jìn)PSO算法對6自由度機(jī)械臂軌跡進(jìn)行優(yōu)化，對所需要的參數(shù)進(jìn)行設(shè)定，初始粒子數(shù)N為50；初始位置為0.1～4 s之間的隨機(jī)數(shù)；總迭代次數(shù)Tmax為60；c1，c2的最大值和最小值分別為2和0.5；慣性權(quán)重ω的最大值和最小值為0.9和0.4；控制映射參數(shù)μ為3；混沌序列的初值x0為0.6。優(yōu)化后可得各關(guān)節(jié)適應(yīng)度值變化曲線如圖4所示。

圖4 改進(jìn)PSO算法的適應(yīng)度變化曲線

為了確保6自由度機(jī)械臂各關(guān)節(jié)能夠快速平穩(wěn)地運(yùn)動到指定的位置，在起始位置和終止位置之間插值兩個路徑位置，并使用3-5-3分段多項式連接每段軌跡且起始點和終止點的速度和加速度均為0，每段軌跡重合處的速度、加速度相等。由仿真實驗結(jié)果可知，機(jī)械臂各關(guān)節(jié)的位置、速度和加速度變化曲線如圖5～圖10所示，優(yōu)化后機(jī)械臂各關(guān)節(jié)的時間分量如表3所示。

圖5 優(yōu)化前各關(guān)節(jié)位置曲線 圖6 優(yōu)化前各關(guān)節(jié)速度曲線

圖7 優(yōu)化前各關(guān)節(jié)加速度曲線 圖8 優(yōu)化后各關(guān)節(jié)位置曲線

圖9 優(yōu)化后各關(guān)節(jié)速度曲線 圖10 優(yōu)化后各關(guān)節(jié)加速度曲線

表3 優(yōu)化后時間分量

采用改進(jìn)PSO算法優(yōu)化6自由度機(jī)械臂的軌跡后，機(jī)械臂各關(guān)節(jié)軌跡有了明顯的改變。由圖5～圖10可知：與優(yōu)化前機(jī)械臂各關(guān)節(jié)軌跡相比，機(jī)械臂各關(guān)節(jié)的位置軌跡和加速度更加平滑，速度更加快速，完成運(yùn)動軌跡所用的時間也有了明顯的減少。由表3機(jī)械臂各關(guān)節(jié)優(yōu)化后的時間分量可知：為了確保機(jī)械臂各關(guān)節(jié)都有充足的時間完成相應(yīng)的軌跡，取每個關(guān)節(jié)完成相同段軌跡所用的時間中的最大值為該段機(jī)械臂的運(yùn)行時間，機(jī)械臂完成3段軌跡所用的時間從3 s減小到2.1 s，有效地提高了機(jī)械臂的運(yùn)行速度和工作效率。6自由度機(jī)械臂在3維空間內(nèi)運(yùn)動軌跡如圖11所示。

圖11 機(jī)械臂運(yùn)動軌跡

4 結(jié)論

本文提出一種改進(jìn)PSO算法對6自由度機(jī)械臂軌跡進(jìn)行優(yōu)化。為了解決機(jī)械臂各關(guān)節(jié)在運(yùn)行過程不穩(wěn)定，位置、速度和加速度出現(xiàn)突變等問題，采用改進(jìn)PSO算法對3-5-3分段多項式插值的每段軌跡進(jìn)行優(yōu)化。在滿足機(jī)械臂運(yùn)動學(xué)約束的前提下，機(jī)械臂完成整段軌跡所用的時間從3 s減少到2.1 s，整體時間相對于PSO算法優(yōu)化前縮短了近30%，不僅縮短了機(jī)械臂的運(yùn)行時間還提高了機(jī)械臂的工作效率。因此驗證了基于改進(jìn)PSO算法的時間最優(yōu)機(jī)械臂軌跡規(guī)劃的方法是有效的。