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

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