基于改進(jìn)粒子群算法的避障軌跡規(guī)劃*

馬東陽 庫祥臣 米 顯 楊星濤 趙歡樂

（河南科技大學(xué)機(jī)電工程學(xué)院，河南 洛陽 471003）

機(jī)器人的關(guān)節(jié)運(yùn)動(dòng)結(jié)果直接影響著作業(yè)效果，對(duì)關(guān)節(jié)軌跡規(guī)劃的研究可提高機(jī)器人工作效率，減少運(yùn)動(dòng)過程中的抖動(dòng)和沖擊，對(duì)于延長機(jī)器人使用壽命，提高效益具有重要意義[1?3]。目前軌跡規(guī)劃優(yōu)化方法有很多，可通過改進(jìn)規(guī)劃算法，使用高階多項(xiàng)式曲線[4]、B樣條曲線等以提高軌跡的平滑性，使機(jī)器人關(guān)節(jié)運(yùn)動(dòng)更加平穩(wěn)[5]；針對(duì)軌跡參數(shù)以時(shí)間最優(yōu)、沖擊最優(yōu)和能量最優(yōu)等為目標(biāo)應(yīng)用算法尋優(yōu)[6?7]。浦玉學(xué)[8]等針對(duì)機(jī)器人軌跡能耗優(yōu)化問題改進(jìn)引力搜索算法，提升算法搜索能力，完成低能耗運(yùn)動(dòng)軌跡規(guī)劃。王超[9]等提出一種自適應(yīng)的遺傳算法避免陷入局部極值，以時(shí)間最短為目標(biāo)，完成平滑軌跡規(guī)劃。李俊[10]等通過動(dòng)態(tài)改進(jìn)遺傳算法參數(shù)改善局部最優(yōu)問題，結(jié)合樣條函數(shù)生成機(jī)器人平穩(wěn)運(yùn)動(dòng)軌跡。基于以上研究理論可知算法的收斂速度和全局搜索能力，避免陷入局部極值，是算法使用的依據(jù)，對(duì)軌跡最優(yōu)規(guī)劃有重要影響。

本文以機(jī)器人避開障礙物，關(guān)節(jié)轉(zhuǎn)角增量最小和用時(shí)最短為目標(biāo)，使用多項(xiàng)式函數(shù)完成軌跡描述。提出指數(shù)曲線遞減和動(dòng)態(tài)調(diào)整策略的改進(jìn)粒子群算法，通過多項(xiàng)式參數(shù)尋優(yōu)，完成軌跡優(yōu)化，并進(jìn)行實(shí)驗(yàn)和仿真驗(yàn)證。

1 避障軌跡規(guī)劃

通過參數(shù)尋優(yōu)的方法完成避障軌跡規(guī)劃總體流程如圖1所示。

圖1 規(guī)劃方法流程

1.1 機(jī)器人避障任務(wù)描述

機(jī)器人分揀任務(wù)如圖2所示，由機(jī)器人完成傳送帶上位置a處零件的抓取，然后根據(jù)測(cè)量裝置b處的信息將零件分揀至不同的物料盒c中，路徑點(diǎn)a記為起點(diǎn)，路徑點(diǎn)b記為測(cè)量作業(yè)點(diǎn)，路徑點(diǎn)c記為目標(biāo)點(diǎn)。

圖2 任務(wù)描述

在測(cè)量操作過程中為避免機(jī)器人本體與測(cè)量裝置及其他裝置的碰撞，傳統(tǒng)上是由工程師根據(jù)經(jīng)驗(yàn)，采用拖拽示教的方法，確定運(yùn)動(dòng)軌跡。本文研究以機(jī)器人各關(guān)節(jié)轉(zhuǎn)角增量最小，運(yùn)動(dòng)時(shí)間最短為優(yōu)化目標(biāo)，在任務(wù)空間中，障礙物信息已知，通過參數(shù)尋優(yōu)完成避障軌跡規(guī)劃，進(jìn)而提高作業(yè)效率。

1.2 碰撞檢測(cè)模型建立

如圖3所示，基于包圍球理論[11]，根據(jù)機(jī)器人本體特征將其簡化為4個(gè)圓柱體C的組合，并將障礙物六面體B簡化為球體A。

圖3 包絡(luò)圓柱理論模型

圖3中球體A中心和六面體B的中心一致，六面體B的最大外圍尺寸即為球體A的直徑2RA，圓柱體C的半徑記為RC。進(jìn)一步簡化模型，將4個(gè)圓柱體C看作4條線段l，其外徑做如下處理：R=RA+RC，R稱為包絡(luò)半徑。空間中線段與點(diǎn)的距離決定兩者的位置關(guān)系，據(jù)此建立碰撞檢測(cè)模型。為確定空間中線段的位置，根據(jù)機(jī)器人關(guān)節(jié)變化矩陣求得關(guān)，關(guān)節(jié)5的位節(jié)2的位，關(guān)節(jié)3的位，關(guān)節(jié)4的位，關(guān)節(jié)6的位，則3個(gè)線段模型在三維空間的表達(dá)式如式（1）所示。

式中：di為線段i到球心的距離，i=1,2,3。若di>R,則線段與球不接觸無碰撞，否則接觸即發(fā)生碰撞。

1.3 運(yùn)動(dòng)軌跡的數(shù)學(xué)描述

本文在關(guān)節(jié)空間采用多項(xiàng)式函數(shù)進(jìn)行軌跡規(guī)劃，通過尋找一個(gè)最優(yōu)作業(yè)點(diǎn)，連接前后兩段軌跡，作業(yè)點(diǎn)關(guān)節(jié)信息與多項(xiàng)式參數(shù)相關(guān)，通過改變參數(shù)來改變作業(yè)點(diǎn)信息完成避障。作業(yè)點(diǎn)和起點(diǎn)之間與該作業(yè)點(diǎn)和目標(biāo)點(diǎn)之間的軌跡使用五次多項(xiàng)式描述。對(duì)于單關(guān)節(jié)在路徑點(diǎn)n之后的軌跡曲線數(shù)學(xué)描述如式（3）所示。

路徑點(diǎn)n和n+1處的關(guān)節(jié)角度，角速度和角加速度分別表示為θn、，關(guān)節(jié)從路徑點(diǎn)n經(jīng)過時(shí)間tn到達(dá)路徑點(diǎn)n+1，則θn(t)的6個(gè)約束條件如式（4）所示。

求解θn(t)可得各系數(shù)，經(jīng)過整理，該段軌跡函數(shù)如式（5）所示。

根據(jù)式（5）可知只要輸入θn、θn+1、和tn即可求出該段的軌跡函數(shù)θn(t)。分別計(jì)算起點(diǎn)到作業(yè)點(diǎn)和作業(yè)點(diǎn)到目標(biāo)點(diǎn)的兩段軌跡函數(shù)θ1(t)，θ2(t)根據(jù)已知條件：起點(diǎn)的機(jī)器人各關(guān)節(jié)角度θ0=[q01,q02,q03,q04,q05,q06]，終點(diǎn)的各關(guān)節(jié)角度θf=[qf1,qf2,qf3,qf4,,qf5,qf6]，起點(diǎn)和終點(diǎn)的各關(guān)節(jié)角速度，角加速度，在測(cè)量作業(yè)點(diǎn)處，則需要確定的參數(shù)分別是作業(yè)點(diǎn)處各關(guān)節(jié)的角度θz=[qz1,qz2,qz3,qz4,qz5,qz6]和兩段軌跡時(shí)間t1、t2。

2 參數(shù)尋優(yōu)模型

2.1 優(yōu)化模型建立

本文以各關(guān)節(jié)角度增量最小和關(guān)節(jié)運(yùn)動(dòng)的時(shí)間最短為優(yōu)化目標(biāo)建立目標(biāo)函數(shù)如式（6）所示。

運(yùn)動(dòng)學(xué)約束如式（7）所示。

式中：θjmax、分別表示關(guān)節(jié)j的最大轉(zhuǎn)角、速度和加速度值。fθ是各關(guān)節(jié)角度增量之和，計(jì)算方法如式（8）所示。

ft是各段運(yùn)動(dòng)時(shí)間之和，ft=t1+t2。η1、η2是轉(zhuǎn)角增量權(quán)重和時(shí)間權(quán)重。在優(yōu)先保證時(shí)間優(yōu)先的情況下，設(shè)置η1<η2。

在目標(biāo)函數(shù)中還需加上機(jī)器人本體與障礙物不發(fā)生碰撞的約束條件，本文使用懲罰函數(shù)處理碰撞約束，構(gòu)造新的目標(biāo)函數(shù)如式（9）所示。

式中：μ為罰因數(shù);fob為碰撞檢測(cè)項(xiàng)，將軌跡函數(shù)插入n個(gè)采樣點(diǎn)，在每個(gè)采樣點(diǎn)處分別計(jì)算距離d并進(jìn)行碰撞判斷，將結(jié)果記為fobi規(guī)定不碰撞為1，反之為0。將采樣點(diǎn)處的計(jì)算結(jié)果累乘如式（10）所示。

根據(jù)以上分析，參數(shù)尋優(yōu)的避障軌跡規(guī)劃問題即在滿足碰撞檢測(cè)條件下關(guān)節(jié)轉(zhuǎn)角增量最小，時(shí)間最短的多項(xiàng)式結(jié)構(gòu)參數(shù)p=(θzj,t1,t2),j=1,2,···,6的尋優(yōu)問題。

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

粒子群優(yōu)化算法（particle swarm optimization，PSO）算法結(jié)構(gòu)簡單，通過追隨當(dāng)下最優(yōu)解來搜尋全局最優(yōu)[12]。標(biāo)準(zhǔn)PSO算法由于參數(shù)固定，在迭代求解過程中若粒子個(gè)體認(rèn)知權(quán)重大于群體社會(huì)性權(quán)重，則可能使算法后期無法收斂，反之則可能出現(xiàn)收斂速度快陷入局部極值無法尋到全局最優(yōu)，本文通過改進(jìn)PSO來優(yōu)化這一情況。

粒子更新公式如式（11）所示。

其中：pi(k)是個(gè)體粒子i的適應(yīng)度最優(yōu)值，g(k)是粒子種群的適應(yīng)度最優(yōu)值，r1、r2是[0,1]內(nèi)的隨機(jī)值。速度更新式（11）當(dāng)中：ω(k)?vi(k)是粒子自身慣性值，ω值越大算法早期全局搜索能力越強(qiáng)，后期較小的 ω可加快算法收斂速度；c1r1(pi(k)?xi(k))是粒子的個(gè)體認(rèn)知能力，表示粒子有逼近自身記憶中最優(yōu)值的趨勢(shì)；c2r2(g(k)?xi(k))是粒子趨近整個(gè)種群或者鄰域內(nèi)最優(yōu)值的趨勢(shì)。c1、c2是加速權(quán)值，算法早期應(yīng)使c1>c2，粒子個(gè)體認(rèn)知能力的意義要大于群體決策，后期為快速收斂于全局最優(yōu)則應(yīng)使c1

本文將通過改進(jìn)粒子群參數(shù) ω和c1、c2來改進(jìn)PSO算法。其中根據(jù)式（12）給予慣性權(quán)值指數(shù)曲線遞減策略，給與加速常數(shù)正弦函數(shù)動(dòng)態(tài)調(diào)整。

式中：ωmin為初期慣性權(quán)值；ωmax為最大慣性權(quán)值；r3是在[0,1]之間的隨機(jī)數(shù)；k為當(dāng)下迭代次數(shù)；kmax最大迭代次數(shù)。

根據(jù)上述目標(biāo)函數(shù)模型中作業(yè)點(diǎn)處各關(guān)節(jié)的角度和兩段軌跡的時(shí)間值構(gòu)成種群中每個(gè)粒子，其編碼方式為：pi=(θzj,t1,t2),j=1,2,···,6。初始化加速權(quán)值c1為0.8，c2為0.6，慣性權(quán)值為1，粒子的初始速度和初始位置為0，終止條件是迭代次數(shù)達(dá)到設(shè)置代數(shù)，同時(shí)滿足碰撞檢測(cè)約束條件，改進(jìn)PSO算法的流程如圖4所示。

圖4 改進(jìn)PSO 算法流程

3 仿真分析與驗(yàn)證

3.1 運(yùn)動(dòng)學(xué)模型建立

以JakaMinCobot機(jī)器人為研究對(duì)象，采用改進(jìn)的DH建模方法，建立連桿坐標(biāo)系如圖5所示。

圖5 連桿坐標(biāo)系

根據(jù)連桿坐標(biāo)系得到機(jī)器人各關(guān)節(jié)的改進(jìn)的DH參數(shù)如表1所示。

表1 改進(jìn)DH參數(shù)

其中各關(guān)節(jié)的約束如表2所示。

表2 關(guān)節(jié)運(yùn)動(dòng)學(xué)約束

正運(yùn)動(dòng)學(xué)變換矩陣推導(dǎo)如式（13）所示。

將DH參數(shù)代入式（13）即可得到機(jī)器人各關(guān)節(jié)相對(duì)基坐標(biāo)系的齊次變換矩陣。

3.2 模型驗(yàn)算與工作空間分析

設(shè)機(jī)器人各關(guān)節(jié)初始角度q0=[0,0,90°,0,90°,0]，根據(jù)運(yùn)動(dòng)學(xué)公式計(jì)算可得關(guān)節(jié)6的坐標(biāo)變換矩陣：

同時(shí)使用Matlab中的Robotic構(gòu)建機(jī)器人模型使用Fkine函數(shù)計(jì)算結(jié)果與相同，如圖6所示機(jī)器人在此位置姿態(tài)與機(jī)器人實(shí)物對(duì)比一致，驗(yàn)證運(yùn)動(dòng)學(xué)模型建立準(zhǔn)確。

圖6 運(yùn)動(dòng)學(xué)模型驗(yàn)證

同時(shí)運(yùn)用MonteCarlo法模擬機(jī)器人工作空間如圖7所示，其中機(jī)器人末端在x軸的移動(dòng)范圍在（?600，600），在y軸的移動(dòng)范圍在（?500，500），在z軸的移動(dòng)范圍在（?500，800），由此設(shè)置機(jī)器人起點(diǎn)a坐標(biāo)為（?75，366，60）、終點(diǎn)c坐標(biāo)為（6，?426，84）以及障礙球心坐標(biāo)為（250，0，200），其中包絡(luò)半徑設(shè)為R=90 mm。并通過機(jī)器人自帶求逆解函數(shù)kine_inverse解出路徑點(diǎn)a、c處相應(yīng)的關(guān)節(jié)角度，a=[?1.571,0.785,1.571,1.047,0.524,0.785]，c=[?4.712,1.047,0.785,0,1.047,?1.047]。

圖7 工作空間模擬

3.3 優(yōu)化計(jì)算與實(shí)驗(yàn)分析

設(shè)置罰因數(shù)μ=10，算法種群個(gè)數(shù)為30，權(quán)重因子η1=0.6,η2=0.8，最大迭代次數(shù)200。如圖8所示在經(jīng)過迭代100次之后，算法收斂，得出軌跡函數(shù)待求參數(shù)值p=[?3.141，0.698，0.785，0.523，1.571,1.463，5.58，3.36]，優(yōu)化結(jié)果對(duì)比如表3所示，與標(biāo)準(zhǔn)的粒子群算法相比改進(jìn)算法收斂速度提高25.18%，目標(biāo)函數(shù)值減少12.64%達(dá)到全局最優(yōu)。

表3 優(yōu)化結(jié)果對(duì)比

圖8 算法迭代結(jié)果

將優(yōu)化參數(shù)代入式（5）即可求出兩段運(yùn)動(dòng)軌跡函數(shù)，同時(shí)運(yùn)用Matlab仿真可得到機(jī)器人在笛卡爾空間的避障運(yùn)動(dòng)軌跡如圖9所示。

圖9 避障軌跡

如圖10所示，利用運(yùn)動(dòng)學(xué)模型求得l1、l2、l3在運(yùn)動(dòng)過程中到障礙物中心的距離d變化曲線，距離的最小值為107.8 mm均大于障礙物模型的包絡(luò)半徑90 mm，滿足避障約束條件。

圖10 避障距離變化

利用Matlab對(duì)五次，三次多項(xiàng)式仿真對(duì)比。以關(guān)節(jié)1～3為例繪制角位移，角速度和角加速度的仿真曲線如圖11所示，五次多項(xiàng)式可將起點(diǎn)、終點(diǎn)和作業(yè)點(diǎn)處角加速度設(shè)為零，且加速度變化更加平滑，可減輕機(jī)器人的振動(dòng)和沖擊，使作業(yè)過程更加平穩(wěn)，三次多項(xiàng)式在路徑點(diǎn)處加速度有突變，五次多項(xiàng)式則可更好地滿足應(yīng)用要求。

圖11 軌跡規(guī)劃仿真對(duì)比

對(duì)于機(jī)器人提供的動(dòng)態(tài)庫函數(shù)和API接口，利用C++編程二次開發(fā)，將上述優(yōu)化的得到的避障各路徑點(diǎn)參數(shù)代入機(jī)器人運(yùn)動(dòng)控制函數(shù)，并采集機(jī)器人實(shí)時(shí)運(yùn)動(dòng)原始數(shù)據(jù)，使用Matlab繪制曲線，圖12中可知機(jī)器人避障運(yùn)動(dòng)軌跡平滑，如圖13～15所示關(guān)節(jié)轉(zhuǎn)角，速度，加速度峰值均在運(yùn)動(dòng)學(xué)約束范圍內(nèi)。

圖12 避障運(yùn)動(dòng)軌跡

圖13 關(guān)節(jié)轉(zhuǎn)角

4 結(jié)語

本文研究了任務(wù)空間中通過參數(shù)尋優(yōu)的方法完成機(jī)器人全局避障軌跡規(guī)劃得出以下結(jié)論：

（1）采用五次多項(xiàng)式作為軌跡函數(shù)，建立機(jī)器人碰撞檢測(cè)模型，并引入罰函數(shù)來處理避障約束條件，結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)模型，完成了機(jī)器人避障規(guī)劃，仿真驗(yàn)證了五次多項(xiàng)式由于路徑點(diǎn)加速度可控更加滿足應(yīng)用需求，該方法流程可進(jìn)一步推廣至多自由度串聯(lián)機(jī)器人的避障研究當(dāng)中。

圖14 關(guān)節(jié)速度

圖15 關(guān)節(jié)加速度

（2）運(yùn)用指數(shù)曲線遞減和動(dòng)態(tài)調(diào)整策略改進(jìn)粒子群算法，加快粒子群收斂速度，提高算法的全局搜索能力，為機(jī)器人在線避障軌跡規(guī)劃奠定了基礎(chǔ)。