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

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

0 引言

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

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

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

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

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

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

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

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

本文對機(jī)器人末端軌跡優(yōu)化的目標(biāo)是在運(yùn)動(dòng)學(xué)約束的條件下，對每個(gè)關(guān)節(jié)進(jìn)行時(shí)間尋優(yōu)，使得總運(yùn)動(dòng)時(shí)間最短。

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

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

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

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

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

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

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

3.1.1 模擬退火算法擾動(dòng)機(jī)制

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

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

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

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

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

于是改進(jìn)速度位置公式：

3.1.2 免疫算法濃度免疫機(jī)制

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

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

第i個(gè)抗體粒子的濃度公式為：

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

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

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

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

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

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

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

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

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

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

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

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

4 仿真實(shí)驗(yàn)及分析

4.1 實(shí)驗(yàn)設(shè)計(jì)

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

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

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

表2 插值點(diǎn)坐標(biāo)

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

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

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

4.2 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

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

通過3-5-3多項(xiàng)式對關(guān)節(jié)角進(jìn)行插值，得關(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進(jìn)行多次仿真優(yōu)化后的軌跡運(yùn)動(dòng)時(shí)間對比，HIPSO下的總時(shí)間由未優(yōu)化的12s減少到優(yōu)化后的4.5s左右，相較未優(yōu)化前時(shí)間減少將近60%，相較常規(guī)粒子群算法時(shí)間減少近15%，較大提高了機(jī)器人的工作效率。

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

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

圖8 箱體打磨機(jī)器人

圖9 打磨機(jī)器人控制柜

5 結(jié)語

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