搬運(yùn)機(jī)器人的軌跡規(guī)劃

20105:1001 2265(2010)05 0083 03搬運(yùn)機(jī)器人的軌跡規(guī)劃李偉光，許陽(yáng)釗(華南理工大學(xué)機(jī)械與汽車工程學(xué)院.廣州510640):用五段三次多項(xiàng)式方法對(duì)一種搬運(yùn)機(jī)器人的運(yùn)行軌跡進(jìn)行規(guī)劃并利用Mathb工具對(duì)該方法進(jìn)行 仿真分析。結(jié)果表明該方法能使運(yùn)動(dòng)軌跡的位直和速度都連續(xù)尤其是加速度也連續(xù)并且能夠使機(jī)器 人按照規(guī)定路徑運(yùn)動(dòng)保證機(jī)器人運(yùn)行平穩(wěn)不抖動(dòng)順利避開(kāi)障礙物滿足設(shè)計(jì)要求。:三次多項(xiàng)式:搬運(yùn)機(jī)器人:軌跡規(guī)劃；關(guān)節(jié)空間:TG65; TP242: AThe Trajectory Pkinnhg of Transport RobotL IW ei-guang, XU Yang-zhao(School ofMechanical & Aukmotive Engineering, South China University of TechnoGuangzhou 510640, China)Abstract:Key words: 83 C 1994-2010 China Academic Journal Electronic Publishing House All rights reserved ki.ne(20105 # C 1994-2010 China Academic Journal Electronic Publishing House All rights reserved ki.ne(20105軌跡規(guī)劃是根據(jù)作業(yè)要求對(duì)末端執(zhí)行器在工作:2009- 11 - 19:李偉光(1958)男華南遑工大學(xué)教授.博士生導(dǎo)師硏究方向?yàn)閿?shù)控技術(shù)、制造業(yè)自動(dòng)化與機(jī)電一體化(EmaiDxuyangzhao2008 yahoo am ciu0機(jī)器人軌跡規(guī)劃的研究始干20世紀(jì)60年代 末軌跡規(guī)劃大致可分為兩種：笛卡爾坐標(biāo)空間的軌 跡規(guī)劃和關(guān)節(jié)空間的軌跡規(guī)劃"I。前者雖然概念直 觀便于理解.可控制機(jī)器人在不同點(diǎn)沿一定的軌跡運(yùn) 行.但此方法涉及大量的笛卡爾坐標(biāo)空間和關(guān)節(jié)空間 的轉(zhuǎn)換和矩陣求逆.計(jì)算量大。而后者由于規(guī)劃函數(shù) 生成的就是關(guān)節(jié)值.可直接用于控制機(jī)器人運(yùn)動(dòng)且關(guān) 節(jié)軌跡易干規(guī)劃有許多不同階次多項(xiàng)式函數(shù)及拋物 線過(guò)渡的線性函數(shù)來(lái)實(shí)現(xiàn)。本文研究的5自由度搬運(yùn)機(jī)器人采用點(diǎn)到點(diǎn)控制 方法要求完成將電徹機(jī)電議成形后高溫的蒜頭狀氣 門(mén)毛坯從電傲機(jī)取出后放入摩撩壓力機(jī)模具內(nèi)完成沖 壓成型的動(dòng)作運(yùn)行過(guò)程要求動(dòng)作連續(xù)平滑且能夠順 利避開(kāi)障礙物。為此必須定義一個(gè)連續(xù)的且一階導(dǎo) 數(shù)和二階導(dǎo)數(shù)均連續(xù)的函數(shù)。本文釆用五段三次多項(xiàng) 式方法進(jìn)行基于關(guān)節(jié)空間的軌跡規(guī)劃。流程中位姿變化的路徑、取向及其速度和加速度變化 過(guò)程的設(shè)定是運(yùn)動(dòng)學(xué)反解的實(shí)際應(yīng)用O1.1機(jī)器人運(yùn)動(dòng)路徑定義為機(jī)器人位形的一個(gè)特定序 列運(yùn)動(dòng)路徑的定義是機(jī)器人軌跡規(guī)劃的前提。5自由 度搬運(yùn)機(jī)器人結(jié)構(gòu)包括腰部石、大臂4、小臂卜手腕4、 手指厶床端執(zhí)行器丿五部分.如圖1所示。腰部厶和手 腕人分別繞關(guān)節(jié)I和關(guān)節(jié)4做回轉(zhuǎn)運(yùn)動(dòng)，大臂4、小臂 卜手指4分別繞關(guān)節(jié)2、關(guān)節(jié)3和關(guān)節(jié)5做俯仰運(yùn)動(dòng)。根據(jù)應(yīng)用要求定義機(jī)器人運(yùn)動(dòng)路徑如圖1所示。 機(jī)器人的運(yùn)動(dòng)過(guò)程為沒(méi)有工作時(shí)處于初始位直S： 工作時(shí)沿著軌跡SA運(yùn)動(dòng)到電黴等待點(diǎn)接著進(jìn)入 如下循環(huán)由AB,到達(dá)電徹位直夾取蒜頭狀氣門(mén)毛 坯，完成夾取后沿退回A點(diǎn),接著由運(yùn)動(dòng) 到摩擦壓力機(jī)模具正上方C.由CT將氣門(mén)毛坯放入 模具內(nèi)，待氣門(mén)壓力成型完成后將氣門(mén)取走并沿DC f4原路退回人處。本文只針對(duì)B段的軌跡規(guī)劃做研究。1.2對(duì)于段的軌跡.不僅指定了起點(diǎn)和 終點(diǎn),還指定了兩個(gè)中間點(diǎn)為此可以通過(guò)匹配前個(gè)運(yùn) 動(dòng)段的末端與下個(gè)運(yùn)動(dòng)段的始端的位置、速度和加速 度來(lái)規(guī)劃出一條平滑連續(xù)的軌跡。利用起點(diǎn)和終點(diǎn)邊 界條件以及中間點(diǎn)的信息，可采用如下七次多項(xiàng)式來(lái) 規(guī)劃軌跡5：h =( +a t +a2 f + / + a4 / +5 / + 兔 / + a： /< 1)求解上式可得機(jī)器人運(yùn)動(dòng)軌跡，但是由于需要大 量計(jì)算且極值難求在工程應(yīng)用中很少使用該方法。 為此,可在軌跡不同的運(yùn)動(dòng)段釆用不同的低次多項(xiàng)式. 然后將它們平滑過(guò)渡地連接在一起以滿足各點(diǎn)的邊界 條件。常用4-3-4軌跡、353軌跡或三次樣條函 數(shù)軌跡等來(lái)代替高次多項(xiàng)式軌跡。由于使用4-3-4 軌跡法和3 - 5 - 3軌跡法所規(guī)劃的機(jī)器人末端執(zhí)行器 軌跡并不能按預(yù)定軌跡行走，難以避免壁碰和路障的 問(wèn)題.不滿足設(shè)計(jì)要求。為了使運(yùn)動(dòng)軌跡的位直、速 度和加速度都連續(xù).并且按照規(guī)定軌跡運(yùn)行本文采用 五段三次多項(xiàng)式進(jìn)行軌跡規(guī)劃。每段關(guān)節(jié)軌跡的三次多項(xiàng)式通式為hj(=(tp fa,(j- 1. 2, 3, 4, 5；(2>式中未知系數(shù)g為第丿段關(guān)節(jié)軌跡的第，個(gè)系數(shù)。在應(yīng)用五段三次多項(xiàng)式插值時(shí)，需要有五段軌跡 和六個(gè)插值點(diǎn)。但是在前面的討論中只有四個(gè)插值 點(diǎn).即B點(diǎn)、A點(diǎn)、C點(diǎn)和Q點(diǎn)。所以，必須選擇另外兩 個(gè)插值點(diǎn)以便有足夠多的邊界條件求解各多頊?zhǔn)较?數(shù)?？梢栽贏點(diǎn)和Q點(diǎn)之間選取這兩個(gè)額外的結(jié)點(diǎn). 設(shè)為E點(diǎn)和尸點(diǎn)。沒(méi)有必要知道這兩個(gè)點(diǎn)的確切位 直我們只要知道時(shí)間間隔(設(shè)為1”，以及必須滿足 這兩點(diǎn)速度和加速度的連續(xù)性條件。因此.這組關(guān)節(jié) 軌跡分段多項(xiàng)式必須滿足的邊界條件是在點(diǎn)、A 點(diǎn)、C點(diǎn)和D點(diǎn)的位直約束.S所有插值點(diǎn)的速度和 加速度的連續(xù)性。五段三次關(guān)節(jié)軌跡的邊界條件示干 圖2中，其中有下劃線的變量為己知量。根據(jù)(2)式可得一階和二階導(dǎo)數(shù)分別為：Vj( t) 一叫;:"=知 +2“ + 3“0,()- 1, 2, 3, 4, 5)cij(t)=2 +6/ (j =1, 2,3,4, 5)(4)給定了初始點(diǎn)和終止點(diǎn)的位賈、速度和加速度，第 一段軌跡和末段軌跡的多項(xiàng)式人仃丿和h,( “就完全確 定了。一且算出這兩個(gè)多項(xiàng)式，就可用位直約束和速 度、加速度連續(xù)條件求出h,(譏爪(”和九(譏 下面 以大臂關(guān)節(jié)為例給出五段三次多項(xiàng)式軌跡的計(jì)算方 法如下根據(jù)工業(yè)實(shí)際要求設(shè)定<0=05. t =15, Q =2s, 6 =3x6=5se由示教可得其他巳知條件如下；6.)=113. 17： b =0.ay =0.6, =99.45：04 =97. 86e.6$ =9425：些=0.a5 =0計(jì)算可得五段三次多項(xiàng)式軌跡方程為片(/) = 1 9752 02395門(mén)0<1(5)/i2(M= 1.7357- 0.7185(1 I)-0.7185(/- lP+0.7265(r- )3(Kt<2)(6 丿筠(0=10252+0. 0239(t 2丿+1.4609" 2)2 0.7974(i 2廣(7)/i4 0) = 1.7125+0. 5534 (t 3b 0. 9314(/- 3)2 +0.3735 (t 3 戶(8)hs (n =1.708 - 0. 189U 4丿 +0. 189(/- 4)2 OO63(八 4)3對(duì)式子(5丿(9)利用式子(3丿和式子(4丿求導(dǎo)可 得相應(yīng)的角速度和角加速度的表達(dá)式o根據(jù)前面求解，利用mallab編寫(xiě)程序可得各關(guān)節(jié) 在一個(gè)運(yùn)動(dòng)周期內(nèi)的角度、角速度和角加速度曲線。 大臂關(guān)節(jié)在MfCfD段的角度、角速度和角加速時(shí)間t(s)從仿真結(jié)果分析可得.大臂關(guān)節(jié)的角度變化平滑 連續(xù).與期望曲線相符。角速度和角加速度也連續(xù)無(wú) 跳變.并且加速度的變化是以線性變化的。此種插值 方法初始點(diǎn)和終止點(diǎn)的速度和加速度可不為零這就 允許再細(xì)分插值點(diǎn)和進(jìn)行過(guò)程中插值。這說(shuō)明了 進(jìn) 行五段三次多項(xiàng)式軌跡規(guī)劃不但可以達(dá)到使機(jī)器人運(yùn) 行平穩(wěn)不抖動(dòng)的要求.而且可以按規(guī)定的路徑運(yùn)動(dòng)。3利用五段三次多項(xiàng)式進(jìn)行軌跡規(guī)劃能生成一條位 2、速度和加速度都連續(xù)的平滑運(yùn)動(dòng)軌跡不但可以達(dá) 到使機(jī)器人運(yùn)行平穩(wěn)不抖動(dòng)的要求而且可以按規(guī)定 的路徑運(yùn)動(dòng)順利避開(kāi)障礙物.滿足設(shè)計(jì)要求。 1 王幼民徐蔚油 機(jī)械晉關(guān)節(jié)空間Bezier曲線軌跡規(guī)劃J.安徽機(jī)電學(xué)院學(xué)報(bào),2000,15(3): 59642 陳偉華張鐵.噴涂機(jī)器人連續(xù)直線軌跡規(guī)劃的研究卩|. 機(jī)械設(shè)計(jì)與制造.2009(8): 178 - 18Q13 豐天友孟正大.趙嬌嬌等.基于焊接機(jī)器人的關(guān)節(jié)空間 軌跡規(guī)劃方法卩電焊機(jī),2009(4):47 - 504) 劉好明.6R關(guān)節(jié)型機(jī)器人軌跡規(guī)劃算法研究及仿直M . 山東:山東理工大學(xué)200&5 王曉朋侯蝮彬王溥.基于VC+的工業(yè)機(jī)器人軌跡規(guī)劃 研究J.工礦自動(dòng)化.2009(5):3437.|61漢SacedB.Niku.普.孫富春等譯機(jī)器人學(xué)導(dǎo)論 分析、玉統(tǒng)及應(yīng)用M.北京：電子工業(yè)出版社.2004.7馬探.六自由度機(jī)械腎軌跡規(guī)劃研究D|.哈爾濱:哈爾濱 工程大學(xué)，2007.(編輯) 85 C 1994-2010 China Academic Journal Electronic Publishing House All rights reserved ki.ne(20105 # C 1994-2010 China Academic Journal Electronic Publishing House All rights reserved ki.ne(20105IU $ + K<JG (S)1 +KpG(上接第82頁(yè))2.4圖5所示為増加前饋控制后伺服系統(tǒng)傳遞函數(shù)框 圖為方便計(jì)算將速度環(huán)部分簡(jiǎn)化為G(S)。系統(tǒng)傳統(tǒng)函數(shù)CR(S)系統(tǒng)穂態(tài)誤差：1 -a $ G (S)EZ八C一*7由式可知若Ia$a (S) =0.即a =時(shí)恒有E(S) =0o可見(jiàn).前饋補(bǔ)償?shù)拇嬖诳?以完全消除系統(tǒng)的穩(wěn)態(tài)誤差，此時(shí)相當(dāng)于在系統(tǒng)中増 加了一個(gè)輸入信號(hào)其產(chǎn)生的誤差信號(hào)與/HS丿產(chǎn)生的 誤差信號(hào)大小相等而方向相反。前饋補(bǔ)償裝置可以使 系統(tǒng)準(zhǔn)確無(wú)誤的復(fù)現(xiàn)控制倍號(hào)解決了-般反饋控制 在提髙控制精度和確保系統(tǒng)穂定性之間的矛盾9。FANUC Oi系統(tǒng)中.在調(diào)整加工精度時(shí)通常將參數(shù) PRM2(X)5#!設(shè)為1.表示前饋有效綜合上述分析可以 消除誤差提髙加工精度。3伺服系統(tǒng)的調(diào)整是一個(gè)十分復(fù)雜的過(guò)程本文結(jié) 合控制理論和傳遞函數(shù)的分析計(jì)算來(lái)深入理解 FANUC系統(tǒng)相應(yīng)伺服參數(shù)的調(diào)整對(duì)機(jī)床動(dòng)、靜態(tài)性能 的影響為調(diào)試人員提供理論依據(jù)和參考以便更好地 優(yōu)化和調(diào)整伺服性能。( I II高平生.伺服乘統(tǒng)位置增益對(duì)數(shù)控機(jī)床性能的晶響J 岳陽(yáng)職業(yè)技術(shù)學(xué)院學(xué)報(bào).2009(9):889212 FANUC伺服系統(tǒng)調(diào)整教材(B-IOO85CM)/O9M .北京 FANUC機(jī)電有限公司.【3張曉東歐洲.數(shù)控悅床何服殺統(tǒng)穩(wěn)定性分析J.機(jī)械設(shè) 計(jì)與制造.2009 (10): 201 - 20A4過(guò)慶琪奚修軍.杜宏棋.班NUC何服杲統(tǒng)的遜論分析和 故障診斷J基礎(chǔ)自動(dòng)化.2000(2):63 - 64|5金海鷹.賴彪.一種提髙伺服系統(tǒng)跟蹤性能的方法研究JJ.今日科苑.2009(18):277.6|柴光遠(yuǎn).趙鵬兵.數(shù)控進(jìn)給伺服茶統(tǒng)的模糊免疫自適應(yīng)PD 控制研究J 組臺(tái)機(jī)床與自動(dòng)化加工技術(shù),2009(8):74 - 77.7何紅欣.數(shù)控機(jī)床伺服性能的調(diào)整J|.制造技術(shù)與機(jī)床. 2006(8): 120- 122(編輯) # C 1994-2010 China Academic Journal Electronic Publishing House All rights reserved ki.ne(