LR Mate 200iC típusú FANUC robot direkt és inverz kinematikai feladatának vizsgálata

Átírás

1 MISKOLCI EGYETEM GÉPÉSZMÉRNÖKI ÉS INFORMATIKAI KAR TUDOMÁNYOS DIÁKKÖRI DOLGOZAT LR Mte ic típu FANUC robot diret é inverz inemtii feldtán vizgált Olz Attil IV. éve mehtronii mérnö hllgtó Konzulen: Dr. Szbó Tmá egyetemi doen Robert Boh Mehtronii Tnzé Miol,

2 Trtlomjegyzé. BEVEZETŐ.... A FANUC LR MATE IC BEMUTATÁSA A Fnu LR Mte ic tuljdonági é jellemzői...4. A ROBOT DIREKT KINEMATIKAI ELEMZÉSE A nyitott lán robot helyzeténe leírá Denvitrtenberg prmétereel A robot ebeég é gyoruláállpot A ROBOT INVERZ KINEMATIKAI FELADATA Az inverz helyzet meghtározá geometrii ton Inverz orientáió meghtározá z Eulerzöge egítégével PÉLDA A ROBOT KINEMATIKAI VIZSGÁLATÁRA Sebeége é gyoruláo izámítá Elforduláo, ebeége é gyoruláo irjzolá ÖSSZEFOGLALÁS... IRODALOMJEGYZÉK...

3 . BEVEZETŐ A Robert Boh Mehtronii Tnzé rendelezi egy FANUC LR Mte ic típu ipri robottl. A robot ht zbdágfo é telje inylái távolág 74 mm, ebben helyzetben mximáli terhelhetőége 5 g. A robot továbbá rendelezi háromdimenzió lfelimeréi épeéggel. Ez egy étdimenzió mer é egy lézerfej egítégével vlóul meg. Ebben dolgoztbn z dott robot diret inemtii feldtán vizgáltávl é z inverz inemtii feldt nlitiu megoldáávl fogllozun. A inemtii leírához Denvitrtenberg prmétereet llmzzu. A robot ht zbdágfo mozgáát ht uló vlóítj meg, melyne zimboliu jelölée: RRRRRR (R Rottion. A robot utoló három ulóján tengelye egy pontbn ereztezi egymát, így gömbulót lotn. Ez gömbuló tuljdonéppen zárótg orientáióját htározz meg, míg z elő három uló gömbuló helyzetét. A uló ezen oportoítá tezi lehetővé robot inverz inemtii feldtán nlitiu vizgáltát. A zárótg helyzeténe leíráához felhználju z Euler zögeet i. A dolgozt lényegi réze három pontbn erül idolgozár. Az elő pontbn Denvitrtenberg prmétere felhználáávl előállítju robot helyzetét meghtározó helyzetmátrixot. A helyzet imeretében zármzttju ebeég é gyoruláállpotot. A máodi pontbn Spong, uthinon é Vidygr zerző Robot Modeling nd Control önyvéne feldolgozáávl fogllozun, robot inverz inemtii feldtávl. Az elő három uló zögét egyzer geometrii megfontoláo lpján állítju elő trigonometriu függvénye egítégével. A vizgált robotun é z dott irodlombn bemuttott robot trutráj nem teljeen egyezi meg, ezért z özefüggéeet enne megfelelően módoítju. Az orientáiót meghtározó gömbulót z Euler leírá egítégével elemezzü. A vázolt inemtii felbontá eredményeént végül ht drb trigonometriu özefüggét pun z egye uló zögelforduláir. Az utoló pontbn fent imertetett elmélet lpján bemuttju zt zámítógépi progrmot, melyet ét feldt lpján dolgoztun i. A progrm robot egy zerelé orán végzett mozgáát zimulálj. A zárótg végpontján pályáját ét zono ugr félörívvel é zot özeötő egyene zzol dtu meg. A mozgá orán zárótg hoztengelye pály görbéjére mindig merőlege, é pály íján normáliávl dott zöget zár be. A zárótg végpontján pályebeégét ontnn írtu elő. A progrm előzör z inverz inemtii feldtot oldj meg oly módon, hogy egyenöz időlépéeet feltételezve előállítj robot helyzetét meghtározó uló zögelforduláit dizrét időpontobn. Máodzorr özépponti differeni módzerét llmzv meghtározzu uló zögebeégeit é zöggyoruláit. Eze imeretében előállítju robot ebeég é gyoruláállpotát diret inemtii feldt özefüggéeine felhználáávl. Az eredményeet, zz robot helyzetét, uló zögelforduláit, zögebeégeit, é zöggyoruláit grfiun zemléltetjü.

4 . A FANUC LR MATE IC BEMUTATÁSA.. A Fnu LR Mte ic tuljdonági é jellemzői A Fnu LR Mte ic z elődeihez épet ol htéonybb é mgbb fo pozíionálái épeéggel rendelezi, ezért zámtln ipri ég llmzz. Sebeégét hozbb ideig épe fenntrtni, é ngy ebeég mellett fejlett zervotehni mitt iebb vibráió. Az. ábrán láthtó, hogy Fnu LR Mte ib elődjéhez épet 4%l eenyebbe rji, ztllp méret tlpzt é ht zbdágfo mitt iebb, nehezebben hozzáférhető helyeen i tud dolgozni. [4]. ábr A. ábrán láthtó robot muntere é méretei felülnézetből, vlmint oldlnézetből. A J4e ulón pneumtiu é eletromo tlozó tlálhtó. A zárt ülő megdályozz, hogy ábele é huzlo özedjn. A Fnu LR Mte ic robotot övetező főbb tevéenyégere llmzzá: gépe ellátá, nygezelé, zerelé, moá, nyg eltávolítá, ottá. 4

5 . ábr A Fnu LR Mte ic muntere é méretei. táblázt Az LR Mte ib é z LR Mte ic özehonlítá Modell LR Mte LR Mte Mximáli terhelhetőég 5g 68mm 5g 6mm ulón Kinylá 7mm 74mm J 8 /e 5 /e J 8 /e 5 /e Mximáli J 5 /e 4 /e ebeég J4 4 /e /e J5 /e /e J6 48 /e 7 /e Imétléi pontoág +/.4mm +/.mm Mehnii tömeg g 7g 5

6 . A ROBOT DIREKT KINEMATIKAI ELEMZÉSE.. A nyitott lán robot helyzeténe leírá Denvitrtenberg prmétereel A robotro egymához vizonyított helyzeténe megdá érdeében minden robotrhoz rögzítün egy n. belő oordinátrendzert. A ro egymához vizonyított helyzetét. ábrán láthtó oordinátrendzere özötti trnzformáióvl írju le. x x z y 4 x 4 x 5 z 6 y z y z 4 y 5 x 6 z 5 y 6 y z x z y x. ábr A Fnu robot belő oordinátrendzere 6

7 7 A oordinátrendzere helyzeténe leíráár z n. Denvitrtenberg prmétereet hználju [], melye értelmezée övetező: z é x tengelye metzépontján z oordinátáj; Θ z tengely örüli forgá előjele zöge, mely z x tengelyt z x tengelybe vizi; α z x tengely örüli forgá előjele zöge, mely z tengelyt z tengelybe vizi; z é z tengelye távolág (normáltrnzverzáli hoz. A Denvitrtenberg prmétere rendre övetező négy egymá utáni merevtetzer reltív mozgán felelne meg:. eltolá z tengely irányábn ( értéel;. forgá z tengely örül (Θ zöggel;. forgá z x tengely örül (α értéel; 4. eltolá z x tengely irányábn ( értéel. Eze mozgáo fentie zerint,,, trnzformáió mátrixol írhtó le, melyene z ugynebben orrendben vett zorzt, ( ë é ë é ë é ë é, ifejezéel írhtó le. A mátrixzorzáot elvégezve övetező egyzerbb lhoz jutun, hol o, in rövidítéeet llmzzu: ( Eze lpján meghtározzu robot egye oordinátrendzereine trnzformáióját megvlóító Denvitrtenberg mátrixot, melye rendre övetezőéppen luln: ë é ë é,, r h

8 éo in ë ( in( o( in( in( o( ( o( o( o( in( in( in( o( éo in ë ( in( o( in( in( o( ( o( o( o( in( in( in( o( éo in ë ( in( o( in( in( o( ( o( o( o( in( in( in( o( éo in ë ( 4 in( 4 o( 4 in( 4 in( 4 4 o( 4 ( 4 o( 4 o( 4 o( 4 in( 4 4 in( 4 in( o( éo in ë ( in( o( in( in( o( ( o( o( o( in( in( in( o( éo in ë ( 56 in( 56 o( 56 in( 56 in( o( 56 ( 56 o( 56 o( 56 o( 56 in( in( 56 in( o( ( (4 (5 (6 (7 (8 A di tg állványhoz vizonyított helyzete reltív helyzetmátrixo zorztént nyert helyzetmátrixzl dhtó meg. Ebből helyzetmátrixát pju, hol eetünben n Ő, j, j (9 j n behelyetteítéével zárótg n 8

9 ( A zárótg bármely R n helyvetorán z állványhoz ötött oordinátrendzerre vontozó oordinátái z n özefüggéel zámíthtó. R R n.. A robot ebeég é gyoruláállpot A robot zárótgján ebeég é gyoruláállpotát z özefüggé idő zerinti deriváláávl pju: R R n n é h r R R n n, n n hol n ( ë é R h v, hol n n n ( n ë V n é R h, hol n n n ( n ë A n Megjegyezzü, hogy vn r n ebeégoordinátáit, míg zárótghoz ötött oordinátrendzer origóján n v n r n oordinátrendzer origóján n gyoruláoordinátáit jelenti. A, illetve mátrixot zárótg n ebeégállpot é gyoruláállpotmátrixán nevezzü. A zárótghoz ötött ponto eetében R n állndó (z időne nem függvénye. 9

10 A, mátrix felhználáávl igzolhtó, hogy D,,, é D,,, (4, (5 hol D z n. deriváló mátrix, uló eetén z lábbi l: (6 A áltláno oordinát, w oordinátebeég é e oordinátgyorulá imeretében bármely özbenő tgo, illetve zárótg helyzet, ebeégállpot é gyoruláállpotmátrixi z állványtól iindulv, lépéenént, z lábbi formulál zámíthtó: D D D D,,,,,,,,,, ë é + ř ö ç č ć + + ř ö ç č ć + (7 ë é D

11 4. A ROBOT INVERZ KINEMATIKAI FELADATA Az előző fejezetben robot diret inemtii feldtát elemeztü, mely orán előállítottu zárótg helyzetét é orientáióját függvényében. Ebben fejezetben robot inverz inemtii feldtávl fogllozun, zz zárótghoz ötött oordinátrendzer O origóján helyzeténe é oordinátrendzer R orientáióján imeretében ereü z egye uló zögelforduláit. Az irodlombn problémát áltláno eetben numeriun zoá ezelni []. Az ipri roboto gyrn ht zbdágfo, z elő három uló zárótg helyzetét htározz meg, míg z utoló három zárótg orientáióját. A robot helyzetét é orientáióját egymától függetlenül vizgálhtju. Azonál robotonál, melyene ht ulój vn, é z utoló három uló tengelye egy pontbn ereztezi egymát így gömbulót lotn feldtot felbontju inverz inemtii helyzetre é inverz inemtii orientáiór. []. Az áltlun vizgált robot i így vn ilítv. A 4. ábrán z utoló három tgot gy i felfoghtju, mint h egy gömbuló lenne, hol z, z 4 é z 5 tengelye egy pontbn metzi egymát, z O pontbn. Megjegyezzü, hogy zárótg Eulerzögeel dott R orientáióján imeretében három tengely örüli zögelfordulá egyértelmen meghtározhtó. Az llmzott módzer lényege, hogy z utoló három tg mozgá orán z előbb említett O pont helyzete ne változzon, ezáltl z áltlun elépzelt gömbuló özéppontján pozíióját z elő három tg htározz meg. A 4. ábrán láthtó, hogy z ötödi oordinátrendzer origój egybeei z O ponttl é helyzetmátrixát z állványhoz épet.. fejezetben említett módon dhtju meg: 6 é ë h r (8 hol h R, zz ér r r R r r r. (9 ë r r r A z 5 é z y 6 tengelye egybeene, így özö irány h, vgyi z R orientáiót leíró mátrix máodi, zz R y ozlopávl dhtó meg. A zárótg origóján helyzete előállíthtó z O től mért, 6 ngyág y 6 menti eltolái trnzformáióvl.

12 O 6 R y r O r 6 Az 4. ábr Kinemtii zétválztá O özéppont helyzete 4. ábrából leolvv övetezőéppen dhtó meg: O O 6 R y. ( O x, O y é O z, míg z. A ettő özötti poltot övetező ifejezé írj le: A zárótg O özéppontján oordinátáit jelölje oordinátáit x, y é éx y ë z éo O ë O x y z r 6 r 6 r 6 O. ( Az előző egyenlet egítégével tudju meghtározni z elő három tg elforduláát. 4.. Az inverz helyzet meghtározá geometrii ton Az inverz pozíió meghtározáához háromzöge geometrii özefüggéeit fogju llmzni. Áltlábn z inverz inemtii feldt bonyolultág ttól függ, hogy mennyi null érté Denvitrtenberg prméter vn. Minél több eze zám, nnál önnyebb megoldni z inverz feldtot. A legtöbb mnipulátor eetében z értée null, z értée pedig vgy null vgy ± π/. A mi eetünben eze z értée övetezőéppen luln:. m.75 m α π/ m. m α m.75 m α π/ 4. m 4 m α 4 π/ 5 m 5 m α 5 π/ 6.4 m 6 m α 6 π/

13 Láthtó, hogy z értée többége zámunr edvezőe, így vlóbn geometrii megözelítét élzer llmzni. A zöge meghtározáához trigonometriu függvényeet hívju egítégül. A izámítáát 6. ábr lpján végezzü el, mely ulót é tgot z x y íbn ábrázolj. A re övetező trigonometriu lot írhtju fel: ( tn y, x. ( Az tn függvény llm rr, hogy egy ívetor y é x oordinátáiból izámítu vetor irányzögét. z O 4 z O O O 4 * * r O y y x x 5. ábr Az elő három tg z x y z térben y O 4 y 4 O O O x 6. ábr Az elő három tg felülnézeti épe x

14 A robot elő három tgján egy tetzőlege helyzetét zemlélteti z 5. ábr. Az O, O é O 4 ponto áltl meghtározott háromzögre ozinuztételt llmzzu. Megjegyezzü, hogy háromzög O pontjábn lévő zög negtív ozinuz megegyezi zög ozinuzávl. Így o * ( * r + ( * Mivel r x + y é z, z előző egyenlet övetező lbn i felírhtó: * ( z + ( * r + o D (4 * Az r x + y épletnél zért vontu le z. 75m értéet, mert z elő uló után robot rj ferdén helyezedi el. Az 5. ábr lpján Pitgorztételt llmzv Eze után +. * 4 * övetező formulávl dhtó meg: ( ± D D * tn, (5 Azért lehetége ét megoldá, mert r állht felőönyö helyzetben vgy lóönyö helyzetben, ttól függően, hogy élpontot felülről vgy lulról zeretnén megözelíteni. (7. ábr Felőönyö helyzet O 4 Alóönyö helyzet 7. ábr Az 5. ábr lpján ténylege hárm uló zögelfordulá prmétere * p +. (6 Az előzőhöz honlón t i geometrii ton phtju meg 8. ábr lpján. 4

15 O 4 * O * O * * r 8. ábr tn tn (7 * * * * * * (, r tn ( in, + o * * * * ( z, x + y tn ( in, + o (8 4.. Inverz orientáió meghtározá z Eulerzöge egítégével Az inverz orientáió problém lényege, hogy zárótg dott orientáiój eetén hogyn htározzu meg z utoló három ulóbn zögelforduláot. Megjegyezzü, hogy övetező egyenleteben ifejezée bonyolultág é hoz mitt z lábbi rövidítéeet llmztu: o i helyett i et o ( i + j helyett ij t in i helyett i et in ( i + j helyett ij t. Az állvány é hrmdi tg reltív helyzetmátrixát, vlmint hrmdi tg é zárótg reltív helyzetmátrixát megphtju megfelelő helyzetmátrixo zorztént: (9. ( A ijelölt mátrix zorzáot MAXIMA 5.. zimboliu mnipulátorrl hjtottu végre [5]. Ebből nyerjü z inverz orientáió feldthoz h é h orientáió mátrixot: 6 5

16 ( ( + ( ( + é h ( ë + é h ( ë A övetező egyenlőég feltételezéével htározhtó meg további három imeretlen zög: T h h h, ( 6 ( 6 hol ér r r h r r r mátrix elemei dott. 6 ë r r r A mátrix egyenletet ifejtve övetező öt egyenlőégehez jutun: r 5 5 r r 6 6 r ( + r ( + r ( + ( + r ( + + r ( + r r r r + r + r + r + r + r + r + r + r Eze lpján ereett három imeretlen ulózög meghtározhtó: tn tn tn tn tn tn ( 5, 5 tn ( ± 5, 5 ( ± ( r + r + r,r + r + r ( 4, 4 tn (, ( r r, r + r + r ( 6, 6 tn ( 56, 56 ( r + r + r,r + r + r (4 (5 Az utóbbi ét éplet or llmzhtó, h 5 ülönbözi nullától, ellenező eetben 4 é 6 özegét tudju özvetlenül meghtározni, zz z egyi felvétele után mái zög idódi. 6

17 5. PÉLDA A ROBOT KINEMATIKAI VIZSGÁLATÁRA A. é 4. fejezetben imertetett elmélet lpján zámítógépi progrmot ézítettün SCILAB 5.. rendzer ltt [6]. A progrm llm zárótg előírt pályáj é orientáiój lpján robot helyzeteine, ebeég é gyoruláállpotin meghtározáár. A továbbibn egy olyn mintfeldtot vizgálun, melyben előírju robot zárótgján pályáját é zárótg mozgá özbeni orientáióját. A pont pályáj ét zono ugr félörívvel é zot özeötő párhuzmo egyeneeel dju meg. A geometrii dtot 9. ábr zemlélteti. y. h 9. ábr x A 9. ábrán láthtju, hogy pály egy íbeli lzt, z xy íbn helyezedi el. pály ezdő félöréne özéppontj (;.;. pontbn tlálhtó. félörö ugr.5 méter ngyág. zerzám végpontj z xy íl 7 o zöget zár be (enne iegézítő zögét ell megdni, zz ot. özeen rézből áll: z elő félör 4, z elő egyene zz, p máodi félör 4, míg máodi egyene zz hoz h r, mely rézből áll. A pont mozgá orán pályebeégét ontnn írju elő. A uló elforduláit minden időpillntbn imentjü, mjd özépponti differeni módzer llmzáávl zögebeégeet é zöggyoruláot meghtározzu. 7

18 5.. Sebeége é gyoruláo izámítá A ebeége é gyoruláo izámítáánál bból indulun i, hogy z egye ulóbn zögelforduláo egyenlő időözönént dott. Továbbá feltételezzü, hogy dizrét pontobn dott függvénye három egymát övető pontr támzodó prbolíveel özelíthető. Ebben z eetben numeriu eljáráonál imert n. özépponti differeni módzert llmzhtju zögebeég é zöggyorulá özelítő meghtározáár []. y y i+ y i y i Δx Δx x. ábr Középponti differeni módzer Az eljárá lpgondolt, hogy elmozduláidő függvényt ét időlépé trtományán prbol ívvel özelítjü (. ábr. Mivel prbol zelője é prbol felezőpontjához trtozó érintő párhuzmo, ebből geometrii megfontolából felírhtju felezőpontbn z idő zerinti elő deriváltt, zz ebeéget: ' yi+ yi yi (6 Dx A gyorulá honló megfontoláo lpján övetező lbn írhtó fel: y ( y y ( y y " i+ i i i yi+ i Dx y Dx i + y i (7 5.. Elforduláo, ebeége é gyoruláo irjzolá Az dto beprogrmozá után ierült irjzolttnun robot tját, mint jégorongpály mintán hld örbe. A pott eredményt. é. ábrá zemlélteti. Azt, hogy robot elforduláán zögei mozgá orán meorát 8

19 változn, é hogy eze változáo meor ebeég é gyorulá ülönbégeet eredményezne,. ábrán láthtó.. ábr A robot pályáj progrm áltl irjzolv. ábr A robot pályáj z xz íból nézve 9

20 . ábr A tgo elforduláán ngyág, ebeégü é gyoruláu A. ábrán tgo jelöléére övetező zíneet llmztu:. tg: feete. tg: piro. tg: zöld 4. tg: é 5. tg: ián 6. tg: mgent. Az ábrából itni, hogy legngyobb elforduláot 4. tg végzi. A ebeég ábrán i jól elülöníthető, hogy robot mior rjzolj félöröet é mior z egyene zzot. Az egyene zzo eetén tgo elfordulá é ebeégü lig változi félöröéhez épet. A gyorulá ábrán loáli o láthtó. Ez zzl mgyrázhtó, hogy zögelfordulá függvény z eredeti feltételezéene megfelelően ugyn folytono, de deriváltji nem, ezért özépponti differeni módzer ezeben pontobn hibá eredményt zolgáltt. A gyorulá ábrán 4. tg hirtelen ngy gyorulánöveedéei é öenéei mitt evébé figyelhető meg függvénye lefutá, ezért gyorulá ábrát [4; +4]e trtományb orlátoztu, melyet 4. ábr zemléltet.

21 4. ábr A beorlátozott gyorulá ábr loáli otól elteintün, or 4. ábrából jól látzi, hogy legngyobb gyoruláol z utoló három tg rendelezi, ezért várhtón legngyobb ineri erő i itt ébredne.

22 6. ÖSSZEFOGLALÁS A TDK dolgoztbn LR Mte ic ipri robot diret é inverz inemtii feldtávl foglloztun. Az inverz inemtii feldt megoldáár zárt l formulát dolgoztun i, mely gyorbb megoldál zolgál z áltlábn llmzott numeriu módzerenél. A bemuttott elmélet lpján egy zámítógépi progrmot ézítettün robot mozgáin leíráár. Az elézített tnulmányt jövőben dinmii feldto irányáb ívánju továbbfejlezteni.

23 IRODALOMJEGYZÉK [] Király B.: Ipri roboto inemtii é dinmii elemzée, Ottái egédlet, Mioli Egyetem, Miol, old. [] Mr W. Spong, Seth uthinon, M. Vidygr: Robot Modeling nd Control [] Pázelt I., Szbó T., B A.: A végeelemmódzer lpji, old. [4] LR Mte ic Serie & RiA Mte Controller [5] Mxim 5.. hivtlo weboldl [6] Silb 5.. hivtlo weboldl