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

（河南科技大學機電工程學院，河南 洛陽 471003）

機器人的關(guān)節(jié)運動結(jié)果直接影響著作業(yè)效果，對關(guān)節(jié)軌跡規(guī)劃的研究可提高機器人工作效率，減少運動過程中的抖動和沖擊，對于延長機器人使用壽命，提高效益具有重要意義[1?3]。目前軌跡規(guī)劃優(yōu)化方法有很多，可通過改進規(guī)劃算法，使用高階多項式曲線[4]、B樣條曲線等以提高軌跡的平滑性，使機器人關(guān)節(jié)運動更加平穩(wěn)[5]；針對軌跡參數(shù)以時間最優(yōu)、沖擊最優(yōu)和能量最優(yōu)等為目標應用算法尋優(yōu)[6?7]。浦玉學[8]等針對機器人軌跡能耗優(yōu)化問題改進引力搜索算法，提升算法搜索能力，完成低能耗運動軌跡規(guī)劃。王超[9]等提出一種自適應的遺傳算法避免陷入局部極值，以時間最短為目標，完成平滑軌跡規(guī)劃。李俊[10]等通過動態(tài)改進遺傳算法參數(shù)改善局部最優(yōu)問題，結(jié)合樣條函數(shù)生成機器人平穩(wěn)運動軌跡?；谝陨涎芯坷碚摽芍惴ǖ氖諗克俣群腿炙阉髂芰?，避免陷入局部極值，是算法使用的依據(jù)，對軌跡最優(yōu)規(guī)劃有重要影響。

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

1 避障軌跡規(guī)劃

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

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

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

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

圖2 任務(wù)描述

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

1.2 碰撞檢測模型建立

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

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

圖3中球體A中心和六面體B的中心一致，六面體B的最大外圍尺寸即為球體A的直徑2RA，圓柱體C的半徑記為RC。進一步簡化模型，將4個圓柱體C看作4條線段l，其外徑做如下處理：R=RA+RC，R稱為包絡(luò)半徑?？臻g中線段與點的距離決定兩者的位置關(guān)系，據(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個線段模型在三維空間的表達式如式（1）所示。

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

1.3 運動軌跡的數(shù)學描述

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

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

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

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

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

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

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

運動學約束如式（7）所示。

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

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

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

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

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

2.2 改進粒子群算法

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

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

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

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

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

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

圖4 改進PSO 算法流程

3 仿真分析與驗證

3.1 運動學模型建立

以JakaMinCobot機器人為研究對象，采用改進的DH建模方法，建立連桿坐標系如圖5所示。

圖5 連桿坐標系

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

表1 改進DH參數(shù)

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

表2 關(guān)節(jié)運動學約束

正運動學變換矩陣推導如式（13）所示。

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

3.2 模型驗算與工作空間分析

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

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

圖6 運動學模型驗證

同時運用MonteCarlo法模擬機器人工作空間如圖7所示，其中機器人末端在x軸的移動范圍在（?600，600），在y軸的移動范圍在（?500，500），在z軸的移動范圍在（?500，800），由此設(shè)置機器人起點a坐標為（?75，366，60）、終點c坐標為（6，?426，84）以及障礙球心坐標為（250，0，200），其中包絡(luò)半徑設(shè)為R=90 mm。并通過機器人自帶求逆解函數(shù)kine_inverse解出路徑點a、c處相應的關(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)化計算與實驗分析

設(shè)置罰因數(shù)μ=10，算法種群個數(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é)果對比如表3所示，與標準的粒子群算法相比改進算法收斂速度提高25.18%，目標函數(shù)值減少12.64%達到全局最優(yōu)。

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

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

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

圖9 避障軌跡

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

圖10 避障距離變化

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

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

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

圖12 避障運動軌跡

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

4 結(jié)語

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

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

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

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

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