工業(yè)機(jī)器人沖擊最優(yōu)的軌跡規(guī)劃算法

工業(yè)機(jī)器人沖擊最優(yōu)的軌跡規(guī)劃算法楊錦濤;姜文剛;林永才【摘要】軌跡規(guī)劃是工業(yè)機(jī)器人控制的一個(gè)重要組成部分,它對機(jī)器人性能的提高有著關(guān)鍵作用,為了解決目前笛卡爾空間軌跡規(guī)劃加速度不連續(xù),導(dǎo)致機(jī)器人運(yùn)行過程中沖擊過大而產(chǎn)生振動(dòng)、機(jī)械磨損、使用壽命縮短等問題,研究沖擊最優(yōu)的軌跡規(guī)劃算法?用S形速度曲線代替梯形速度曲線，對B樣條進(jìn)行插補(bǔ)，插補(bǔ)后得到笛卡爾空間的位置、速度、加速度參數(shù),反推到關(guān)節(jié)空間進(jìn)行仿真驗(yàn)證,仿真結(jié)果表明,利用S形速度曲線插補(bǔ)時(shí)各關(guān)節(jié)加速度連續(xù),沖擊有了明顯減小.期刊名稱】《科學(xué)技術(shù)與工程》年(卷),期】2014(014)028【總頁數(shù)】6頁(P64-69)【關(guān)鍵詞】工業(yè)機(jī)器人;沖擊最優(yōu);軌跡規(guī)劃;S形曲線【作者】楊錦濤;姜文剛;林永才【作者單位】江蘇科技大學(xué)電子信息學(xué)院,鎮(zhèn)江212000;江蘇科技大學(xué)電子信息學(xué)院,鎮(zhèn)江212000;蘇州時(shí)運(yùn)機(jī)器人有限公司,蘇州215600【正文語種】中文【中圖分類】TP242.2隨著工業(yè)機(jī)器人的發(fā)展越來越迅速，應(yīng)用領(lǐng)域越來越廣泛，對工業(yè)機(jī)器人的性能要求也越來越高。軌跡規(guī)劃是工業(yè)機(jī)器人控制的一個(gè)重要組成部分，它對機(jī)器人控制性能的提高有著關(guān)鍵作用，軌跡規(guī)劃的一個(gè)重點(diǎn)就是使機(jī)器人能夠按照期望軌跡平穩(wěn)快速地運(yùn)行，盡量避免位移、速度、加速度的突變，減少振動(dòng)和沖擊［1］。軌跡規(guī)劃可以在關(guān)節(jié)空間進(jìn)行也可以在笛卡爾空間進(jìn)行。在關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃，其優(yōu)點(diǎn)是容易滿足各關(guān)節(jié)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)約束，計(jì)算簡單，不會(huì)發(fā)生機(jī)構(gòu)的奇異現(xiàn)象［1，2］，缺點(diǎn)是各關(guān)節(jié)軌跡與末端執(zhí)行器的軌跡是非線性的，所以關(guān)節(jié)空間的軌跡規(guī)劃不容易預(yù)見末端執(zhí)行器的運(yùn)動(dòng)［3］，只有那些無路徑要求的作業(yè)才能直接在關(guān)節(jié)空間進(jìn)行規(guī)劃，對于那些有路徑嚴(yán)格要求的作業(yè)，如連續(xù)噴涂，弧焊，切割等，就必須在笛卡爾空間進(jìn)行軌跡規(guī)劃［1，3—5］。為了解決目前笛卡爾空間軌跡規(guī)劃中存在的沖擊過大的問題，利用S形速度曲線在笛卡爾空間對工業(yè)機(jī)器人進(jìn)行軌跡規(guī)劃，并進(jìn)行仿真驗(yàn)證。1笛卡爾空間軌跡規(guī)劃的研究現(xiàn)狀目前在笛卡爾空間進(jìn)行軌跡規(guī)劃的研究并不多，但是在笛卡爾空間進(jìn)行軌跡規(guī)劃又是一些作業(yè)任務(wù)所必須的，所以對笛卡爾空間軌跡規(guī)劃進(jìn)行深入研究也是必要的、具有實(shí)際意義的。文獻(xiàn)［5］和文獻(xiàn)［6］在笛卡爾空間進(jìn)行了軌跡規(guī)劃，運(yùn)用B樣條擬合運(yùn)動(dòng)路徑，但使用的是梯形速度函數(shù)。文獻(xiàn)［7］詳細(xì)論述了梯形速度函數(shù)在電機(jī)運(yùn)行中的缺陷，會(huì)引起機(jī)械的振動(dòng)，顫抖和過沖等。同樣，在機(jī)器人的軌跡規(guī)劃中，梯形速度函數(shù)的加速度不連續(xù)，會(huì)產(chǎn)生沖擊，使機(jī)器人運(yùn)行時(shí)產(chǎn)生振動(dòng)乃至偏離預(yù)定軌跡，增加機(jī)械磨損，降低機(jī)器人使用壽命。文獻(xiàn)［8—10］在關(guān)節(jié)空間的軌跡規(guī)劃中，都論述了沖擊對機(jī)器人運(yùn)行的危害，并進(jìn)行了優(yōu)化，在笛卡爾空間規(guī)劃時(shí)同樣也需要預(yù)先的考慮減小關(guān)節(jié)沖擊的問題。雖然文獻(xiàn)［5］進(jìn)行了二次插補(bǔ)以減小沖擊，但他是通過增加插補(bǔ)點(diǎn)個(gè)數(shù)，減小采樣步長，降低機(jī)器人運(yùn)行速度的方法來減小沖擊的，實(shí)際上，可以在笛卡爾空間直接對速度進(jìn)行局部優(yōu)化，以預(yù)見關(guān)節(jié)空間的沖擊約束，沒有必要降低整個(gè)運(yùn)行速度。其次文獻(xiàn)［5］為了簡化計(jì)算，假設(shè)機(jī)器人末端在第一段樣條曲線時(shí)勻加速，也就是以一個(gè)均勻遞增的步長插補(bǔ)，在最后一段樣條曲線勻減速，也就是以一個(gè)均勻遞減的步長插補(bǔ)，中間段以等步長插補(bǔ)，這樣的規(guī)劃方法只適用于第一段樣條曲線和最后一段長度一樣的情況，當(dāng)最后一段比第一段長時(shí)，會(huì)導(dǎo)致還未到達(dá)終點(diǎn)，插補(bǔ)速度已經(jīng)為零，當(dāng)最后一段比第一段短時(shí)，會(huì)導(dǎo)致到達(dá)終點(diǎn)時(shí)，速度還未減到零。并且有可能第一段曲線過長，導(dǎo)致插補(bǔ)步長越來越大，超出電機(jī)所能達(dá)到的最大速度，導(dǎo)致機(jī)器人偏離軌跡?，F(xiàn)用S形速度函數(shù)代替梯形速度函數(shù)，在笛卡爾空間進(jìn)行軌跡規(guī)劃，首先采用三次B樣條擬合期望運(yùn)動(dòng)路徑，然后計(jì)算出路徑的總長度，根據(jù)機(jī)器人末端運(yùn)行的最大速度、最大加速度、最大加加速度參數(shù)，精確計(jì)算出按照S形速度函數(shù)運(yùn)行的七個(gè)時(shí)間節(jié)點(diǎn)，計(jì)算出不同時(shí)間段對應(yīng)的速度函數(shù)，積分求出運(yùn)行距離函數(shù)，最后根據(jù)采樣時(shí)間算出采樣步長，這樣的步長計(jì)算方法更加合理精確，不會(huì)產(chǎn)生停止時(shí)機(jī)器人末端不在預(yù)期終止點(diǎn)的問題，用得到的步長對B樣條進(jìn)行運(yùn)動(dòng)狀態(tài)插補(bǔ)，得出機(jī)器人末端的位置，由速度、加速度函數(shù)得到機(jī)器人末端的速度、加速度，把笛卡爾空間的各個(gè)參數(shù)逆推到關(guān)節(jié)空間進(jìn)行驗(yàn)證，使機(jī)器人各關(guān)節(jié)平緩啟動(dòng)，速度平穩(wěn)過渡，并平緩?fù)Ｖ褂诮K止點(diǎn)，減小各個(gè)關(guān)節(jié)的沖擊，達(dá)到能夠平穩(wěn)地沿著預(yù)定軌跡運(yùn)行的目的。2三次均勻B樣條的軌跡規(guī)劃擬合曲線的樣條函數(shù)有很多，選擇B樣條的原因是B樣條有很強(qiáng)的靈活性，可以局部修改，可以在曲線中間構(gòu)造出一段直線等等。三次均勻B樣條相對簡單，計(jì)算量小，編程簡單，且在很大程度上能符合生產(chǎn)需求，因此選用三次均勻B樣條來擬合運(yùn)動(dòng)路徑。2.1三次均勻B樣條曲線方程假設(shè)有控制點(diǎn)Bi，則每四個(gè)控制點(diǎn)確定一段曲線Pi(u)，第i段曲線為式(1)可簡化為式(2)中u為參變量，為了方便推導(dǎo)與編程，將B樣條曲線參數(shù)u做歸一化處理，將局部參變量uE[01]拼接為全局參變量uE[0n-1],則在新的參變量u下在實(shí)際的機(jī)器人操作中，通常是已知型值點(diǎn)，需要通過型值點(diǎn)計(jì)算出控制點(diǎn)，才能得到B樣條曲線。2.2反算控制點(diǎn)B樣條是由控制點(diǎn)決定的，但在實(shí)際應(yīng)用中通常是已知曲線上通過示教或其他方式得到的幾個(gè)型值點(diǎn)，這就需要由型值點(diǎn)反算出控制點(diǎn)，由n個(gè)型值點(diǎn)得出n+2個(gè)控制點(diǎn)，型值點(diǎn)Pi為式(1)中當(dāng)u=0時(shí)的值，即：這樣的方程共有n個(gè)，而控制點(diǎn)有n+2個(gè)，需要兩個(gè)補(bǔ)充條件，這里補(bǔ)充條件選為則由式(4)和式(5)可以求出所有控制點(diǎn)。3S形速度曲線梯形速度曲線在軌跡規(guī)劃中的應(yīng)用非常廣泛，但是為了使加速度連續(xù)，減小各個(gè)關(guān)節(jié)的沖擊，要對梯形速度曲線進(jìn)行優(yōu)化，變化為S形速度曲線，如圖1所示。圖1S形速度曲線與其加速度曲線Fig.1S-curveanditsaccelerationcurve優(yōu)化后的S形速度函數(shù)分為七個(gè)階段:第一個(gè)階段加速度均勻增加，到本階段結(jié)束時(shí)加速度達(dá)到最大值;第二階段，加速度保持不變，使速度繼續(xù)增加，即勻加速狀態(tài);在第三個(gè)階段，加速度均勻減小，當(dāng)加速度減為零時(shí)，速度正好達(dá)到最大值;第四階段，加速度為零，此時(shí)速度不變，即勻速運(yùn)動(dòng)階段;第五到第七階段為第一到第三階段的反向狀態(tài)，當(dāng)機(jī)器人速度減為零時(shí)，正好停止在終止點(diǎn)。圖1中Si表示七個(gè)不同階段速度對時(shí)間t的積分，即這七階段各自面積。在運(yùn)用S形速度曲線對路徑進(jìn)行插補(bǔ)時(shí)，要求出各個(gè)狀態(tài)轉(zhuǎn)換的時(shí)間節(jié)點(diǎn)，保證狀態(tài)轉(zhuǎn)換的時(shí)間點(diǎn)精確，最后才能停止在終止點(diǎn)。求速度函數(shù)狀態(tài)間的時(shí)間節(jié)點(diǎn)需要先知道機(jī)器人的一些參數(shù):Vmax(末端最大運(yùn)行速度)、amax(末端最大運(yùn)行加速度)、Jmax(末端運(yùn)行最大加加速度)、L(路徑長度)，其中Vmax、amax、Jmax是機(jī)器人自身的相關(guān)參數(shù)，是在機(jī)器人試運(yùn)行過程中根據(jù)測試得出的一個(gè)相對值，L的值要根據(jù)規(guī)劃路徑的不同預(yù)先求出。之后就可以求出時(shí)間參數(shù)t1、t2、t3、t4t5、t6、t7:由amax二Jmaxt1得t1 max=amaxt2得t2;由t3=t2+t1得t3;由得t4;由t5=t4+t1得t5;由t6=t5+(t2-t1)得t6;由t7=t6+t1得t7。于是就可以得到速度曲線函數(shù)如式(6)最后可以求出以優(yōu)化的速度函數(shù)運(yùn)行到時(shí)刻t時(shí)的運(yùn)行軌跡長度S(t)。為了表達(dá)和編程方便，定義中間變量Si(t)(i=1,2，…，7)，代表機(jī)器人末端在七個(gè)不同階段中運(yùn)行的長度機(jī)器人末端運(yùn)行路徑長度就可以表示為上述算法在路徑較長，可以達(dá)到最大速度時(shí)適用，當(dāng)路徑長度比較小，在機(jī)器人未達(dá)到最大速度時(shí)就需要降速，否則到達(dá)終止點(diǎn)時(shí)速度就降不到零，即當(dāng)圖1中的S4=0,且Lv2S(t3)時(shí)，速度就不能達(dá)到最大再減速，要減小最大速度的值，假設(shè)達(dá)到速度Vnewm就要減速，此時(shí)路徑長度滿足式(8):將S(t3)中的t3替換為t1+t2,Vmax替換為Vnewm，將t2替換為Vmax/amax,這樣式中就只有Vnewm—個(gè)未知量，化簡后得式(9)。便可得式(10)。式(10)中t1=amax/Jmax，當(dāng)式(10)中符號取負(fù)時(shí)Vnewm為負(fù)，不符實(shí)際舍去，所以符號只能取正，繼而用Vnewm代替原來的Vmax，就可以按照原來的方法求解時(shí)間節(jié)點(diǎn)和路程函數(shù)。如果L的長度更小，當(dāng)圖1中的S2=S4=S6=0,且LVS1+S3+S7時(shí)，就需要減小最大加速度amax，設(shè)減小后的amax為anewm，則有式(11)。式(11)中Vmax二t1anewm,t1替換為anewm/Jmax，因?yàn)镾2=0有t2=t1,根據(jù)加速度圖形可知此時(shí)t3=2t1,這樣式(11)中就只有anew—個(gè)未知量，簡化后得式(12)。求出anewm之后，t1,t3=2t1,t4=t3,t5=t4+t1,t6=t5,t7=t6+t1o可以得到時(shí)間節(jié)點(diǎn)，路程函數(shù)表達(dá)式仍為式(7)，速度函數(shù)仍為式(6)。4插補(bǔ)算法插補(bǔ)運(yùn)算就是選取合適的速度函數(shù)，末端執(zhí)行器按照這個(gè)速度函數(shù)運(yùn)行，計(jì)算出在采樣時(shí)刻ti時(shí)機(jī)器人的運(yùn)行狀態(tài)，包括位置、速度、加速度信息。4.1三次B樣條中插補(bǔ)點(diǎn)位置對三次B樣條插補(bǔ)時(shí)，需要先求三次B樣條的長度L，對式(2)進(jìn)行曲線長度積分，有第i段B樣條曲線長度為式(13)。然而式(13)中的定積分不存在解析解，需要用數(shù)值積分的方法近似地求定積分，由于柯特斯積分公式積分收斂速度快、誤差小等特點(diǎn)，本文采用復(fù)合柯特斯公式對式(14)求定積分n階復(fù)合柯特斯公式如式(15):式(15)中，xk二分別為［ab］區(qū)間4n等分后小區(qū)間的端點(diǎn)，且k=0,1，…，n。當(dāng)給定積分誤差「使得2n階的復(fù)合柯特斯積分與n階的復(fù)合柯特斯積分之差小于限定誤差量時(shí)，有式(16):可以用2n階復(fù)合柯特斯積分近似求取每段B樣條曲線長度Li，即式(17):每段的長度加起來即可得總的曲線長度L，進(jìn)而可以求出優(yōu)化速度曲線中的參數(shù)，再有采樣時(shí)間序列ti，則可以得到按照速度曲線的采樣步長：本文采用預(yù)估-矯正的方法求出采樣步長對應(yīng)的B樣條曲線參變量u由過去三個(gè)時(shí)刻的采樣點(diǎn)預(yù)估下一時(shí)刻的采樣點(diǎn):初始的三個(gè)采樣點(diǎn)可以用泰勒級數(shù)展開近似:，其中分別為P(u)的三個(gè)分量關(guān)于參變量u的導(dǎo)數(shù)在u=0處的值，分別為P(u)的三個(gè)分量關(guān)于參變量u的導(dǎo)數(shù)在處的值。將代入式(20)?？傻孟鄳?yīng)的預(yù)估位置點(diǎn)可得步長為:預(yù)估所得步長AL與期望步長AS是有偏差的，偏差指數(shù)為當(dāng)S在允許的誤差范圍內(nèi)時(shí)，可以選擇下一時(shí)刻的參變量為下一時(shí)刻的位置點(diǎn)為當(dāng)S不在允許的誤差范圍內(nèi)時(shí)，用式(23)進(jìn)行迭代校正，經(jīng)過大量數(shù)據(jù)驗(yàn)證表明，迭代一次即可將誤差縮小到10-5量級，滿足精確度要求。將uj+1代入式(20)即得第j+1個(gè)位置點(diǎn)P(uj+1)。4.2插補(bǔ)點(diǎn)的速度機(jī)器人末端的運(yùn)動(dòng)速度分為線速度和角速度，角速度與機(jī)器人的姿態(tài)變化有關(guān)，可以運(yùn)用B樣條對關(guān)鍵姿態(tài)點(diǎn)進(jìn)行擬合插補(bǔ)，假設(shè)工具頭方向不變，暫不考慮角速度的變化。由上一節(jié)得出的參變量uj可以計(jì)算出對應(yīng)這一點(diǎn)的線速度矢量的方向，即求B樣條上一點(diǎn)P(uj)的切線方向?yàn)槭?24)中，當(dāng)Bi分別取x,y,z坐標(biāo)時(shí)，P'(u)有對應(yīng)的x,y,z三個(gè)量x'(u),y'(u),z'(u),得切線方向向量為：(x'(u),y'(u),z'(u))方向與參數(shù)u增大時(shí)點(diǎn)P移動(dòng)的方向一致。由式(6)可得在t時(shí)刻線速度為大小為V(t)，由采樣時(shí)間t得到對應(yīng)的參變量u，則方向?yàn)镻'(u)，可得線速度矢量v為5仿真驗(yàn)證利用robotictoolboxformatlab中具有代表性的六軸工業(yè)機(jī)器人puma560進(jìn)行仿真，在笛卡爾空間對一段螺旋線進(jìn)行插補(bǔ)，螺旋線半徑為0.6m,螺距為0.1m,參數(shù)方程為式(26)。用三次均勻B樣條擬合螺旋線，設(shè)定機(jī)器人末端運(yùn)動(dòng)參數(shù)Vmax=0.5m/s，amax=0.4m/s2，Jmax=1m/s3，分別基于梯形速度和S形速度對三次B樣條進(jìn)行插補(bǔ),得到的插補(bǔ)點(diǎn)的位置、速度、加速度參數(shù)通過逆運(yùn)動(dòng)學(xué),反推到關(guān)節(jié)空間進(jìn)行對比。圖2~5中，虛線為未優(yōu)化的各參數(shù)曲線，實(shí)線為用S形曲線優(yōu)化后的各參數(shù)曲線。圖2各關(guān)節(jié)角度變化曲線對比Fig.2Comparisonofanglechangesineachjoint6結(jié)論圖3各關(guān)節(jié)角速度曲線對比Fig.3Comparisonofangularvelocityineachjoint圖4各關(guān)節(jié)角加速度曲線對比Fig.4Comparisonofangularaccelerationineachjoint圖5各關(guān)節(jié)加加速度曲線對比Fig.5ComparisonofJerkineachjoint由圖5的對比可以明顯的看出根據(jù)未優(yōu)化的速度函數(shù)得到的Jerk(加加速度)函數(shù)在速度的拐點(diǎn)處有很大的沖擊產(chǎn)生,實(shí)際上從圖4可以看出,它的加速度在起始和終止位置也不為零,意味著在其實(shí)和終止位置也會(huì)有過大沖擊,這些沖擊導(dǎo)致電機(jī)驟然加速或驟然減速,使機(jī)器人產(chǎn)生振動(dòng),這些振動(dòng)會(huì)逐漸累積,加劇機(jī)械磨損,當(dāng)振動(dòng)頻率與機(jī)械固有頻率一致時(shí),會(huì)發(fā)生共振,導(dǎo)致根本無法完成預(yù)定軌跡的運(yùn)動(dòng)，需要頻繁對機(jī)器人進(jìn)行復(fù)位調(diào)整。優(yōu)化前機(jī)器人運(yùn)行時(shí)間為3.25s，優(yōu)化后的運(yùn)行時(shí)間為3.65s，優(yōu)化后，機(jī)器人的運(yùn)行時(shí)間稍微增加，但各關(guān)節(jié)加速度函數(shù)是連續(xù)的,也就是電機(jī)的加減速過程是連續(xù)的,大大減小了沖擊,避免了機(jī)械運(yùn)動(dòng)過程的沖擊，減小機(jī)器人運(yùn)行過程中的抖動(dòng)，保證跟蹤路徑的精度，也能夠降低機(jī)械磨損，延長機(jī)器人的使用壽命。優(yōu)化結(jié)果適用于曲率較小的路徑規(guī)劃，當(dāng)曲率較大時(shí)，依然會(huì)有較大的沖擊，需要預(yù)先判斷不同曲率時(shí)的最大過彎速度，提前進(jìn)行減速，過彎之后繼續(xù)加速，可以在本文的基礎(chǔ)上繼續(xù)進(jìn)行優(yōu)化。參考文獻(xiàn)【相關(guān)文獻(xiàn)】1CraigJJ.機(jī)器人學(xué)導(dǎo)論?(第3版).員超，等譯?北京:機(jī)械工業(yè)出版社，2006:218—292CraigJJ.Roboticsintroduction(3rded).Trans，YunChao，etal.eds.Beijing:ChinaMachinePress，2006:218—2922阮啟剛，黃磊.6R機(jī)器人軌跡規(guī)劃及仿真?機(jī)械制造與自動(dòng)化，2011;40(1):168—170RuanQigang，HuangLei.Trajectoryplanningandsimulationof6Rrobot.MachineBuilding＆Automation，2011;40(1):168—1703桑紅波.ABB-IRB2400/10型機(jī)器人空間軌跡規(guī)劃的研究?長春:吉林大學(xué)，2007SangHongbo.ResearchofABB-IRB2400/10robot'strajectoryplanning.Changchun:JilinUniversity，20074AlessandroG，VanniZ.Atechniquefortime-jerkoptimalplanningofrobottrajectories.RoboticsandComputer-integratedManufacturing，2008;24(3):415—4265陳偉華，張鐵?三次均勻有理B樣條曲線插補(bǔ)算法的研究?機(jī)械設(shè)計(jì)與制造，2010;(8):3—5ChenWeihua，ZhangTie.ThestudyofcubicuniformrationalB-splineinterpolationalgorithm.MachineryDesign＆Manufacture，2010;(8):3—56李林峰，馬蕾?三次均勻B樣條在工業(yè)機(jī)器人軌跡規(guī)劃中的應(yīng)用研究?科學(xué)技術(shù)與工程，2013;13(13):3621—3625LiLinfeng，MaLei.AresearchonthecubicuniformB-splinecurveanditsapplicationontrajectoryplanningalgorithmofindustryrobot.ScienceTechnologyandEn