基于改進粒子群算法的機器人時間最優(yōu)軌跡規(guī)劃

鄒 慧，周 虎，張騏薇，馬玉環(huán)，錢盈錕

（1.東華大學 機械工程學院，上海 201620；2.上海航天設(shè)備制造總廠有限公司，上海 201100）

0 引言

軌跡優(yōu)化[1]是機器人領(lǐng)域的熱點問題，其任務(wù)是根據(jù)給定的目標軌跡點規(guī)劃出通過指定點且滿足速度、加速度等約束條件的最優(yōu)運動軌跡。目前的軌跡優(yōu)化方式主要包括時間優(yōu)化和能量優(yōu)化兩種[2,3]，其中時間優(yōu)化應(yīng)用最廣。

機器人時間優(yōu)化問題中，粒子群算法（PSO）[4]以其結(jié)構(gòu)簡單、參數(shù)可調(diào)的特點在機械臂時間優(yōu)化問題上得到廣泛應(yīng)用。文獻[5]提出自適應(yīng)反函數(shù)PSO算法對機器人雙NURBS軌跡優(yōu)化，但收斂速度較慢且粒子后期易在最優(yōu)解周邊振蕩。文獻[6]基于粒子群算法進行軌跡規(guī)劃，通過時間、速度的雙適應(yīng)度函數(shù)之間切換控制，在滿足運動學約束的同時優(yōu)化了時間，但算法效率不太理想。文獻[7]采用PSO、DE算法得到平滑運動軌跡的同時提高了機器人的工作效率，但無法保證全局收斂。

針對粒子群算法局部搜索能力差、易陷入局部最優(yōu)解的不足，本文通過引入模擬退火算法（SA）的擾動機制及免疫算法（IA）的濃度選擇機制，在選擇粒子群最優(yōu)位置時以一定突跳概率接受劣解以跳出局部極值，并在進行一定次數(shù)迭代后，若發(fā)現(xiàn)群體中最優(yōu)的個體無明顯變優(yōu)時對不同濃度粒子給予抑制或促進。為驗證算法的有效性，以PUMA560機器人為對象仿真并與常規(guī)粒子群算法進行對比，結(jié)果表明，混合免疫粒子群算法（HIPSO）能有效提高機器人工作效率，實現(xiàn)機器人時間最優(yōu)軌跡規(guī)劃。

1 3-5-3多項式插值函數(shù)構(gòu)造

為了實現(xiàn)機器人末端的平滑運動，采用3-5-3多項式插值函數(shù)規(guī)劃軌跡。在起始點和終止點插入2個路徑點，進而將其運動路徑劃分為三段進行插值，通過逆運動學方程逆解算可求解得到各關(guān)節(jié)在4個插值點對應(yīng)的關(guān)節(jié)矢量值。將關(guān)節(jié)角之間的變換看作是關(guān)于時間的函數(shù)，3-5-3多項式插值函數(shù)的通式為：

式(1)～式(3)中l(wèi)j1(t)、lj2(t)、lj3(t)為分別代表多項式的角位移函數(shù)，ajmn為第j個關(guān)節(jié)的第m段插值的第n個系數(shù)，t為插值的時間變量。

3-5-3多項式插值規(guī)劃的前提是插值時間已知，根據(jù)各關(guān)節(jié)在起始點和結(jié)束點速度和加速度為零，且中間兩過渡點的位移、速度、加速度相等的邊界約束條件可推導出表示關(guān)節(jié)插值點與系數(shù)的關(guān)系矩陣A[8]，因為約束條件和約束邊界只與時間t有關(guān)，通過對插值時間尋優(yōu)，當最優(yōu)時間確定后代入關(guān)系式可求解出多項式插值系數(shù)a，從而生成各關(guān)節(jié)時間最優(yōu)運動軌跡。

2 時間最優(yōu)數(shù)學模型

本文對機器人末端軌跡優(yōu)化的目標是在運動學約束的條件下，對每個關(guān)節(jié)進行時間尋優(yōu)，使得總運動時間最短。

第i個關(guān)節(jié)適應(yīng)度函數(shù)為：

分別為第個關(guān)節(jié)各段的插值時間。

對各關(guān)節(jié)的速度、加速度予以約束，使其不得超過最大值。

3 基于混合免疫粒子群算法的軌跡優(yōu)化

多項式插值軌跡規(guī)劃的前提是插值時間已知，但傳統(tǒng)方法難以對多項式插值進行優(yōu)化，因此，本文在分段多項式插值的基礎(chǔ)上，在速度和加速度約束下基于改進粒子群算法對關(guān)節(jié)空間的插值時間進行尋優(yōu)搜索。

3.1 改進粒子群算法

粒子群優(yōu)化算法中，把機器人關(guān)節(jié)運動時間看作搜索空間的粒子，每個粒子都有一個位置屬性xi和速度屬性vi，并由適應(yīng)度函數(shù)f(t)來追尋最優(yōu)時間。

3.1.1 模擬退火算法擾動機制

粒子群算法通過位置速度更新公式不斷更新粒子的位置與速度：

其中，pij為單個粒子目前最優(yōu)位置，pgj為當前粒子群最優(yōu)位置，ω為慣性權(quán)重，c1、c2為學習因子，r1、r2為0-1之間的隨機數(shù)。

粒子種群根據(jù)個體最優(yōu)和全局最優(yōu)位置調(diào)整下一步進化方向，但當pgj為局部最優(yōu)時，所有粒子會受其影響而朝著局部最優(yōu)運動，導致粒子群快速趨同，從而出現(xiàn)局部極值或停滯現(xiàn)象。

因此，在PSO算法的速度更新過程中，引入模擬退火算法的擾動機制[9]對當前次優(yōu)粒子賦予一定突跳概率，適應(yīng)度越接近當前最優(yōu)粒子則突跳概率越大，于是替換原最優(yōu)粒子，從而避免陷入局部最優(yōu)。

隨著迭代次數(shù)的增加退火溫度逐漸降低，突跳概率逐漸降低，粒子趨向全局最優(yōu)。根據(jù)Metropolis準則退火溫度T時·pi相對pg的突跳概率為：

于是改進速度位置公式：

3.1.2 免疫算法濃度免疫機制

粒子群算法隨著迭代次數(shù)的增加，收斂速度和精度逐漸降低，因此引入免疫算法濃度機制對群體中無明顯變優(yōu)的最優(yōu)個體進行免疫，其核心思想[10,11]：用抗體粒子的濃度代表粒子群中相似抗體粒子的程度，用抗體粒子的親和度代表在免疫系統(tǒng)中抗體粒子與抗原的相似程度，剔除高濃度低親和力粒子，保留低濃度高親和力粒子，從而改善粒子群尋優(yōu)過程的收斂慢、精度低的問題。

通過設(shè)置精度和迭代次數(shù)，當?shù)竭_指定迭代次數(shù)后或在一定精度范圍內(nèi)無明顯變優(yōu)時，則進行免疫：由Logistic映射新生成Q個抗體粒子，計算個體抗體粒子的濃度和被保留概率，將S個高親和力低濃度的抗體粒子保留下來進入下一次迭代。

第i個抗體粒子的濃度公式為：

第i個抗體粒子的濃度概率選擇公式：

式中f(xi)為第i個粒子的適應(yīng)度值，可以看出抗體濃度越高，被保留的可能性越低，確保了粒子群的多樣性。

3.2 混合免疫粒子群算法對關(guān)節(jié)時間優(yōu)化

混合免疫粒子群算法結(jié)合免疫算法和模擬退火粒子群算法的優(yōu)勢，改善了粒子群算法易陷入局部最優(yōu)的缺陷、加快了收斂速度和精度。將三段插值時間作為粒子的尋優(yōu)位置，算法求解機器人各關(guān)節(jié)時間最優(yōu)解步驟如下：

Step1：初始化參數(shù)：設(shè)置粒子數(shù)量為S=20，粒子維數(shù)D=3、學習因子c1=c2=2，慣性權(quán)重ω=0.5，初始溫度T=100，降溫系數(shù)k=0.90，最大迭代次數(shù)Max=100，免疫間隔迭代次數(shù)DS=8。

Step2：在各關(guān)節(jié)可達范圍內(nèi)由Logistic回歸分析生成S個尋優(yōu)粒子并初始化位置及速度，形成初始抗體粒子種群N0。

Step3：速度、加速度約束：通過初始化粒子的位置得到插值多項式并驗證速度、加速度是否滿足其約束條件，若滿足則獲取當前粒子群體N0中的粒子的適應(yīng)度值，否則將其適應(yīng)度值置為無窮大。

Step4：更新粒子位置和速度：采用Metropolis準則依照概率F(pi)接受劣解替換原最優(yōu)粒子，更新粒子的速度和位置并進行退火操作。

Step5：判斷若達到最高迭代次數(shù)，則輸出最優(yōu)解，終止迭代。

Step6：保持粒子多樣性：每迭代指定免疫次數(shù)后檢查最優(yōu)粒子，若無明顯變優(yōu)則由Logistic映射產(chǎn)生Q個新抗體粒子開始進行免疫：計算生成S+Q個新抗體粒子的概率，并依照抗體概率濃度選擇公式選擇個抗體粒子形成抗體種群Ni并轉(zhuǎn)入Step3。

混合免疫粒子群優(yōu)化算法流程如圖1所示。

圖1 混合免疫粒子群算法流程

4 仿真實驗及分析

4.1 實驗設(shè)計

本文以典型的關(guān)節(jié)空間機器人PUMA560為研究對象驗證算法的可行性。其D-H參數(shù)如表1所示。

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

為避免在求解過程中出現(xiàn)奇異位形及保證機械臂的運動可達性，采用正向運動學求解，在已知起點和終止點的基礎(chǔ)上插入兩個路徑點，得到機器人運動軌跡的四個插值點如表2所示。

表2 插值點坐標

根據(jù)逆運動學解得各關(guān)節(jié)的起始點、路徑點和結(jié)束點的角度值，其中前三個關(guān)節(jié)角度值如表3所示。

表3 關(guān)節(jié)序號

根據(jù)混合免疫粒子群算法，在速度的約束下對機器人的前三個關(guān)節(jié)進行優(yōu)化，求解最優(yōu)時間。

4.2 實驗驗證

通過MATLAB對各關(guān)節(jié)的收斂時間進行仿真，以機械臂的前三個關(guān)節(jié)在速度為90°/s、加速度為70°/s2時為例，使用常規(guī)粒子群算法及混合免疫粒子群算法分別對運動時間優(yōu)化，生成在速度和加速度約束下的最短插值時間迭代圖，獲得混合免疫粒子群算法優(yōu)化關(guān)節(jié)位置、速度、加速度曲線以實現(xiàn)對機器人時間最優(yōu)軌跡規(guī)劃。

由圖2～圖4所示混合免疫粒子群算法在30次迭代后開始收斂，與常規(guī)粒子群算法相比插值時間的收斂速度明顯提升，且插值時間更短。100次迭代后得到插值時間分別為t1=1.5672s、t2=1.0436s、t3=1.8327s，取優(yōu)化后的每段插值時間的最大值t3作為各段插值時間。

圖2 關(guān)節(jié)1的時間收斂圖

圖3 關(guān)節(jié)2的時間收斂圖

圖4 關(guān)節(jié)3的時間收斂圖

通過3-5-3多項式對關(guān)節(jié)角進行插值，得關(guān)節(jié)位置、速度、加速度曲線如圖5-7所示，可知各關(guān)節(jié)角變化平滑，且關(guān)節(jié)速度、加速度滿足最大約束無突變。

圖5 關(guān)節(jié)位置曲線

圖6 關(guān)節(jié)速度曲線

圖7 關(guān)節(jié)加速度曲線

表4為兩種算法對關(guān)節(jié)1進行多次仿真優(yōu)化后的軌跡運動時間對比，HIPSO下的總時間由未優(yōu)化的12s減少到優(yōu)化后的4.5s左右，相較未優(yōu)化前時間減少將近60%，相較常規(guī)粒子群算法時間減少近15%，較大提高了機器人的工作效率。

表4 算法優(yōu)化結(jié)果

將改進算法運用到圖9所示實驗室的箱體打磨六自由度機器人的控制器中，其能夠在速度約束下完成預(yù)期的運動軌跡并花費的時間更短，驗證了算法能夠有效減少六自由度機器人運行時間，提高加工效率。

圖8 箱體打磨機器人

圖9 打磨機器人控制柜

5 結(jié)語

本文針對六自由度工業(yè)機器人空間關(guān)節(jié)時間優(yōu)化問題進行了研究，提出混合免疫粒子群算法的3-5-3多項式插值軌跡優(yōu)化方法。通過仿真實驗進行了驗證，結(jié)果表明優(yōu)化后的運動軌跡不僅符合運動約束，且各曲線在路徑范圍內(nèi)平滑過渡，使機器人在保證運行平穩(wěn)的前提下提高了關(guān)節(jié)運動的效率，有助于提升機器人的性能。