杜 娟,宋易奇,陳 峙,晉美娟,董超杰

(1.太原科技大學(xué)機(jī)械工程學(xué)院,太原 030024;2.山西平陽重工機(jī)械有限責(zé)任公司,臨汾 043000)

0 引言

隨著制造業(yè)的優(yōu)化升級(jí),智能制造場景依托工廠或車間,通過工業(yè)機(jī)器人與核心制造環(huán)節(jié)的深度融合,改變了傳統(tǒng)的工作模式。機(jī)器人在降低生產(chǎn)成本,提高工作效率等方面優(yōu)勢明顯,因此在工業(yè)生產(chǎn)中扮演著尤為重要的角色,機(jī)器人的軌跡規(guī)劃也日益成為研究的熱點(diǎn)問題[1]。機(jī)器人在三維空間中的基本軌跡規(guī)劃又分為關(guān)節(jié)空間規(guī)劃和笛卡爾空間規(guī)劃[2]。在對關(guān)節(jié)空間內(nèi)軌跡進(jìn)行規(guī)劃時(shí),較小的機(jī)械沖擊和振動(dòng),對延長機(jī)器人的使用期限和提高工作效率都具有重要意義。

目前,國內(nèi)外學(xué)者對機(jī)器人工作中的軌跡規(guī)劃研究也十分全面,主要分為時(shí)間最優(yōu)、能耗最優(yōu)、沖擊最優(yōu)和混合最優(yōu)軌跡規(guī)劃[3-5]。周晟[6]針對機(jī)械臂的軌跡時(shí)間優(yōu)化問題,提出一種引入自適應(yīng)動(dòng)態(tài)旋轉(zhuǎn)角步長調(diào)整機(jī)制和多算子協(xié)同變異機(jī)制的改進(jìn)量子遺傳算法,并驗(yàn)證了算法的有效性。BORYGA等[7]分別使用5次、7次、9次的高次多項(xiàng)式對關(guān)節(jié)進(jìn)行軌跡規(guī)劃,在保證其關(guān)節(jié)角位移、角速度和角加速度連續(xù)的情況下,使起始位置和結(jié)束位置的角加加速度為0,減少了運(yùn)動(dòng)沖擊。孔慶博等[8]提出了一種5次多項(xiàng)式插值與B樣條插值相融合的方法以消除突變現(xiàn)象,軌跡曲線優(yōu)化后變得更加光滑平穩(wěn),突變現(xiàn)象也得到較好地消除,但增加了計(jì)算量。梅江平等[9]分別采用五次非均勻有理B樣條曲線和3-4-5多段多項(xiàng)式曲線進(jìn)行軌跡規(guī)劃,結(jié)果表明基于五次非均勻有理B樣條運(yùn)動(dòng)規(guī)律的規(guī)劃方法關(guān)節(jié)加速度的最大減少量達(dá)到20%,優(yōu)勢明顯。王君等[10]提出一種改進(jìn)差分進(jìn)化算法,并采用5次非均勻有理B樣條曲線進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃,結(jié)果顯示該算法具備的較快搜索速度。黃超等[11]通過非線性動(dòng)態(tài)改變粒子群算法的慣性權(quán)重,并對3-5-3多項(xiàng)式進(jìn)行時(shí)間優(yōu)化,縮短了約26%的工作時(shí)間,驗(yàn)證了該算法的可行性。王玉寶等[12]通過引入動(dòng)態(tài)調(diào)節(jié)因子的方法改進(jìn)粒子群算法,并對軌跡時(shí)間優(yōu)化,改善了局部收斂的問題,提高了搜索效率。

本文采用改進(jìn)三次多項(xiàng)式插值與IPSO算法相結(jié)合的方法,對機(jī)器人工作各關(guān)節(jié)進(jìn)行軌跡規(guī)劃。利用改進(jìn)三次樣條曲線的平滑連續(xù)性,得到機(jī)器人各關(guān)節(jié)的運(yùn)動(dòng)軌跡,通過改進(jìn)型粒子群算法的尋優(yōu)能力實(shí)現(xiàn)對時(shí)間的優(yōu)化。仿真結(jié)果表明,該軌跡優(yōu)化方法在保證了工作平穩(wěn)性的同時(shí),極大地提高機(jī)器人的工作效率。

1 機(jī)器人運(yùn)動(dòng)學(xué)分析

1.1 機(jī)器人數(shù)學(xué)模型建立

ABB IRB 120型機(jī)器人具有敏捷、緊湊、輕量、控制精準(zhǔn)的特點(diǎn),常用于物料搬運(yùn)和裝配,故選取其完成整個(gè)工作過程,機(jī)器人的結(jié)構(gòu)尺寸圖如圖1所示。機(jī)器人數(shù)學(xué)模型的建立是實(shí)現(xiàn)運(yùn)動(dòng)學(xué)分析和軌跡規(guī)劃的基礎(chǔ)和核心。本文采取經(jīng)典的D-H法[13]建模來描述機(jī)器人相鄰空間連桿的幾何關(guān)系,其可分為連桿本體和連桿連接的描述。一個(gè)連桿的運(yùn)動(dòng)可用連桿長度ai和連桿轉(zhuǎn)角αi來描述,以此來定義相鄰空間兩關(guān)節(jié)軸的相對位置關(guān)系。與連桿描述一致,連桿偏距di和關(guān)節(jié)角θi可定義各連桿之間的所有連接方式。通過以上4個(gè)運(yùn)動(dòng)學(xué)參數(shù)來描述該型碼垛機(jī)器人的運(yùn)動(dòng)關(guān)系,最終可得機(jī)器人連桿坐標(biāo)系如圖2所示,其連桿參數(shù)如表1所示。

表1 機(jī)器人連桿參數(shù)表

圖1 機(jī)器人的結(jié)構(gòu)尺寸圖 圖2 機(jī)器人連桿坐標(biāo)系

1.2 正運(yùn)動(dòng)學(xué)分析

機(jī)器人的正運(yùn)動(dòng)學(xué)問題是給定其各關(guān)節(jié)轉(zhuǎn)角參數(shù)時(shí),推導(dǎo)出末端執(zhí)行器位姿的過程。根據(jù)D-H法建立相鄰空間連桿的變換矩陣,如式(1)所示。

Ai=Rot(z,θi)Trans(0,0,di)Trans(ai,0,0)Rot(x,αi)=

(1)

將表1中的各組參數(shù)帶入,可得到各相鄰空間連桿的變換矩陣A1～A6。

將A1～A6依次變換,最終可得到碼垛機(jī)器人末端執(zhí)行器在基座坐標(biāo)系中的位姿T:

(2)

式中:三維矩陣[noa]和矢量P分別代表末端執(zhí)行器在基座坐標(biāo)系中3個(gè)坐標(biāo)方向上的姿態(tài)和位置。

1.3 逆運(yùn)動(dòng)學(xué)分析

機(jī)器人的逆運(yùn)動(dòng)學(xué)問題是給定末端執(zhí)行器在笛卡爾坐標(biāo)中的位姿矩陣T時(shí),推導(dǎo)出各關(guān)節(jié)向量值的過程[14]。本文選用的機(jī)器人末端后3個(gè)相鄰關(guān)節(jié)軸交于一點(diǎn),滿足Pieper準(zhǔn)則的要求,故可進(jìn)行逆運(yùn)動(dòng)學(xué)封閉解求解。

通過機(jī)器人末端在基座坐標(biāo)系中的位置Pw和機(jī)器人末端的接近矢量a來求解腕心的位置坐標(biāo),將機(jī)器人末端執(zhí)行器逆運(yùn)動(dòng)學(xué)的求解問題轉(zhuǎn)化到腕心。采用位姿分離法,求解機(jī)器人的各關(guān)節(jié)角,在避免了復(fù)雜的求逆運(yùn)算的同時(shí),提高了求解的效率。令d6=0,可得:

Pw=[wx,wy,wz]=[px,py,pz]-d6[ax,ay,az]

(3)

1.4 求各關(guān)節(jié)逆解

令Si=sinθi,Ci=cosθi,Smn=sin(θm+θn),Cmn=cos(θm+θn)。由式(3)可得:

(4)

ES2+FC2=G

(5)

(6)

由式(4)可得θ1的值:

(7)

由式(5)可得θ2的值:

(8)

由式(6)和式(8)可得θ3的值:

θ3=atan2(a3I-d4H,a3H+d4I)-θ2

(9)

機(jī)器人后3個(gè)相鄰空間連桿的姿態(tài)變換矩陣為:

(10)

由式(10)可得θ5的值:

(11)

當(dāng)S5=0時(shí),θ5=0,由式(11)可得:

θ4+θ6=atan2(-r12,r22)

(12)

此時(shí)機(jī)器人的關(guān)節(jié)4和關(guān)節(jié)6軸線重合,且其處于奇異位形?？上却_定θ4的值,再由式(11)求解處θ6的值。

當(dāng)S5≠0時(shí),由式(10)可得:

(13)

(14)

2 機(jī)器人平滑軌跡規(guī)劃

2.1 三次樣條插值函數(shù)

定義三次樣條插值函數(shù)為區(qū)間[t0,tn]內(nèi)的一個(gè)分段函數(shù)G(t)。G(t)在整個(gè)區(qū)間內(nèi)都是次數(shù)不超過三次的多項(xiàng)式,同時(shí)二階連續(xù)且可導(dǎo),每個(gè)節(jié)點(diǎn)ti處均滿足:Gk(ti-0)=Gk(ti+0),k=0,1,2。

假設(shè)G(ti)=θi,G(t)在ti處的二階導(dǎo)數(shù)值G′′(ti)=Ri,對G′′(ti)=Ri和G′′(ti+1)=Ri+1進(jìn)行線性插值,可得:

(15)

式中:Ti=ti+1-ti,連續(xù)對式(15)進(jìn)行兩次積分可得[15]:

(16)

預(yù)設(shè)邊界條件G′(t0)=0,G′(tn)=0,再利用G′(t)在時(shí)間區(qū)間內(nèi)節(jié)點(diǎn)的連續(xù)性條件G′(ti-0)=G′(ti+0)可得線性方程組如下:

(17)

式中:

2.2 改進(jìn)三次樣條插值函數(shù)

機(jī)器人在傳統(tǒng)三次樣條插值法的軌跡規(guī)劃中,無法保證其在工作起始和結(jié)束時(shí)運(yùn)動(dòng)的平穩(wěn)性,易產(chǎn)生振動(dòng)[16]。為了使軌跡規(guī)劃中機(jī)器人加速度從0開始連續(xù)平滑變化,再到0結(jié)束,且各時(shí)間節(jié)點(diǎn)處的速度和加速度連續(xù)。結(jié)合實(shí)際工作中的需求,對三次樣條插值函數(shù)的起始和結(jié)束分段區(qū)間時(shí)間中點(diǎn)處進(jìn)行修正。

在起始分段區(qū)間的時(shí)間中點(diǎn)處插入修正插值點(diǎn)θ(t1),產(chǎn)生的修正插值函數(shù)需滿足起始時(shí)刻速度,加速度為0,即θ′(t0)=θ′′(t0)=0,在其與下一分段區(qū)間連接節(jié)點(diǎn)處滿足連續(xù)條件:G′(t2-0)=G′(t2+0)。

整理后得到起始分段區(qū)間內(nèi)的修正插值函數(shù):

(18)

在結(jié)束分段區(qū)間的時(shí)間中點(diǎn)處插入修正插值點(diǎn)θ(tn-1),產(chǎn)生的修正插值函數(shù)需滿足終止時(shí)刻速度,加速度為0,即θ′(tn)=θ′′(tn)=0,在其與上一段區(qū)間連接節(jié)點(diǎn)處滿足連續(xù)條件:G′(tn-1-0)=G′(tn-1+0)。

整理后得到結(jié)束分段區(qū)間內(nèi)的修正插值函數(shù):

(19)

在起始和結(jié)束分段區(qū)間預(yù)設(shè)邊界條件G′(t0)=0,G′(tn)=0的同時(shí),加入邊界條件G′′(t0)=0,G′′(tn)=0,可得線性方程組:

(20)

式中:

2.3 構(gòu)造改進(jìn)三次樣條軌跡曲線

選取一些插值點(diǎn),如表2所示。用這些插值點(diǎn)描繪出選用傳統(tǒng)三次樣條和改進(jìn)三次樣條曲線時(shí)機(jī)器人關(guān)節(jié)位置、速度和加速度關(guān)于時(shí)間的變化對比圖像,如圖3所示。

表2 曲線的插值點(diǎn) (rad)

(a) 三次樣條軌跡 (b) 改進(jìn)三次樣條軌跡

通過對比示例關(guān)節(jié)在分別采用兩種插值曲線運(yùn)動(dòng)時(shí)其隨時(shí)間變化的圖像看出,在保證各時(shí)間節(jié)點(diǎn)處的速度和加速度連續(xù)性的同時(shí),利用改進(jìn)三次樣條插值后使軌跡規(guī)劃中機(jī)器人起始和結(jié)束時(shí)刻的加速度從0開始連續(xù)變化。

3 改進(jìn)型粒子群算法(IPSO)優(yōu)化

3.1 適應(yīng)度函數(shù)和約束條件

在有效減少機(jī)器人工作起始和結(jié)束時(shí)產(chǎn)生的振動(dòng)后,為了提高其工作效率,引入IPSO算法對機(jī)器人工作時(shí)間進(jìn)行優(yōu)化,因此其適應(yīng)度函數(shù)為:

(21)

式中:f(t)為機(jī)器人運(yùn)行的總時(shí)間,Ti=ti+1-ti是相鄰節(jié)點(diǎn)的時(shí)間間隔。

約束條件是對機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力性能的物理限制。定義機(jī)器人在工作過程中受到的約束條件為:①位置約束:qi(t)≤qmax;②速度約束:vi(t)≤vmax;③加速度約束:ai(t)≤amax。其中qmax、vmax和amax分別表示保持機(jī)器人運(yùn)行平穩(wěn)的最大位置、速度和加速度值。

3.2 粒子群算法(PSO)

粒子群算法是一種智能優(yōu)化算法。其基本思想是受到鳥類群體覓食行為時(shí),個(gè)體之間協(xié)作和信息共享的啟發(fā)。

在D維搜索空間中存在一個(gè)由m個(gè)粒子組成的種群,且每個(gè)粒子的位置表示一個(gè)潛在最優(yōu)解。種群初始化后,粒子在該搜索空間中每一次迭代并更新其位置、速度和適應(yīng)度,都會(huì)產(chǎn)生一個(gè)粒子最優(yōu)位置piD的適應(yīng)值,將其與全局經(jīng)歷過的最優(yōu)位置pgD的適應(yīng)值進(jìn)行比較,確定全局最優(yōu)解[18]。粒子的速度、位置的更新公式分別為:

(22)

(23)

3.3 改進(jìn)型粒子群算法(IPSO)

標(biāo)準(zhǔn)的粒子群算法運(yùn)算簡單、高效且尋優(yōu)速率快,但是易陷入局部最優(yōu)[20]。主要是因?yàn)槠錂?quán)重因子為固定值,為此本文引入一種動(dòng)態(tài)變化的方式給c1和c2賦值。預(yù)先設(shè)定動(dòng)態(tài)學(xué)習(xí)因子的最大值相等,c1值隨著搜索次數(shù)的增加逐漸減小,c2值隨著搜索次數(shù)的增加逐漸增大。使其在前期具備較強(qiáng)的全局搜索能力且保持了種群的多樣性,而后期則具備較強(qiáng)的局部搜索的能力,快速收斂到局部最優(yōu),不僅提高了搜索精度,又能優(yōu)化結(jié)果易陷入局部最優(yōu)的不足。其變化公式為:

(24)

c2=2-c1

(25)

式中:c1為全局學(xué)習(xí)因子,c2為局部學(xué)習(xí)因子,k為當(dāng)前迭代次數(shù),Nmax為最大迭代次數(shù)。

與此同時(shí),為了進(jìn)一步強(qiáng)化粒子的全局搜索能力,使其可以搜索更多的解空間。本文對慣性權(quán)重ω采用下式進(jìn)行修正:

(26)

式中:ω0是初始值。

改進(jìn)型粒子群算法不僅采用了動(dòng)態(tài)變化學(xué)習(xí)因子策略,還對慣性權(quán)重進(jìn)行了修正。將改進(jìn)三次多項(xiàng)式插值與IPSO算法有效結(jié)合,實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)時(shí)間的最優(yōu)求解。

4 仿真實(shí)驗(yàn)與結(jié)果

4.1 仿真分析

為驗(yàn)證本文采用的基于IPSO的改進(jìn)三次樣條曲線時(shí)間最優(yōu)軌跡規(guī)劃在機(jī)器人中應(yīng)用的有效性。預(yù)設(shè)相鄰路徑點(diǎn)之間時(shí)間的迭代初始值為2 s,工作時(shí)間總計(jì)14 s。通過逆運(yùn)動(dòng)學(xué)分析將路徑點(diǎn)位置信息轉(zhuǎn)化為改進(jìn)三次樣條曲線的各關(guān)節(jié)角度值,如表3所示。

表3 路徑點(diǎn)對應(yīng)的各關(guān)節(jié)位置 (rad)

初始化IPSO算法參數(shù):群體個(gè)數(shù)m=50,最大迭代次數(shù)Nmax=300,慣性權(quán)重初始值ω0=0.729。各關(guān)節(jié)運(yùn)動(dòng)特性約束值如表4所示。

表4 運(yùn)動(dòng)學(xué)約束

4.2 仿真結(jié)果

根據(jù)上述優(yōu)化條件和參數(shù),利用IPSO算法對改進(jìn)三次樣條曲線進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃仿真。經(jīng)過優(yōu)化計(jì)算后,得到機(jī)器人工作的時(shí)間最優(yōu)解如表5所示。機(jī)器人各關(guān)節(jié)在工作過程中位移、速度和加速度關(guān)于時(shí)間的變化圖像如圖4所示。與傳統(tǒng)機(jī)器人運(yùn)動(dòng)軌跡相比,經(jīng)過IPSO算法優(yōu)化之后的工作時(shí)間大幅度減少,最大可降低4.795 s,平均工作時(shí)間減少了4.440 s,平均工作時(shí)間縮短了30%左右,實(shí)現(xiàn)了機(jī)器人的時(shí)間最優(yōu)軌跡規(guī)劃。經(jīng)過該方法規(guī)劃后的軌跡無論在位置、速度、加速度都有明顯的優(yōu)勢。

(a) 關(guān)節(jié)1 (b) 關(guān)節(jié)2

(c) 關(guān)節(jié)3 (d) 關(guān)節(jié)4

(e) 關(guān)節(jié)5 (f) 關(guān)節(jié)6

表5 時(shí)間優(yōu)化結(jié)果 (s)

5 結(jié)論

針對機(jī)器人的運(yùn)動(dòng)軌跡優(yōu)化問題,利用改進(jìn)三次樣條曲線和IPSO算法結(jié)合的方法對機(jī)器人進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃。通過對三次樣條曲線的改進(jìn),有效減少了傳統(tǒng)三次樣條曲線在機(jī)器人工作起始和結(jié)束時(shí)因加速度發(fā)生突變而產(chǎn)生振動(dòng)的問題,再結(jié)合IPSO算法對各段改進(jìn)三次樣條曲線的工作時(shí)間進(jìn)行優(yōu)化。經(jīng)過MATLAB仿真實(shí)驗(yàn),其結(jié)果驗(yàn)證了本文設(shè)計(jì)的基于IPSO算法的改進(jìn)三次樣條曲線時(shí)間最優(yōu)軌跡規(guī)劃在機(jī)器人中應(yīng)用的可行性和有效性,在保證了機(jī)器人工作平穩(wěn)性的前提下,大幅縮短了工作時(shí)間,提高了機(jī)器人的工作效率。