lr mate 200ic típusú fanuc robot direkt és inverz ... · az 1. ábrán látható, hogy a fanuc...
TRANSCRIPT
MISKOLCI EGYETEM GÉPÉSZMÉRNÖKI ÉS INFORMATIKAI KAR
TUDOMÁNYOS DIÁKKÖRI DOLGOZAT
LR Mate 200iC típusú FANUC robot direkt és inverz kinematikai feladatának vizsgálata
Olasz Attila IV. éves mechatronikai mérnök hallgató
Konzulens: Dr. Szabó Tamás egyetemi docens
Robert Bosch Mechatronikai Tanszék
Miskolc, 2010
2
Tartalomjegyzék
1. BEVEZETŐ .................................................................................................................3
2. A FANUC LR MATE 200IC BEMUTATÁSA ...........................................................4
2.1. A Fanuc LR Mate 200iC tulajdonságai és jellemzői.................................................4
3. A ROBOT DIREKT KINEMATIKAI ELEMZÉSE ..................................................6
3.1. A nyitott láncú robot helyzetének leírása Denavit-Hartenberg paraméterekkel ........6
3.2. A robot sebesség- és gyorsulásállapota ...................................................................9
4. A ROBOT INVERZ KINEMATIKAI FELADATA ................................................11
4.1. Az inverz helyzet meghatározása geometriai úton ..................................................12
4.2. Inverz orientáció meghatározása az Euler-szögek segítségével ..............................15
5. PÉLDA A ROBOT KINEMATIKAI VIZSGÁLATÁRA ........................................17
5.1. Sebességek és gyorsulások kiszámítása ..................................................................18
5.2. Elfordulások, sebességek és gyorsulások kirajzolása .............................................18
6. ÖSSZEFOGLALÁS ...................................................................................................22
IRODALOMJEGYZÉK ........................................................................................................23
3
1. BEVEZETŐ
A Robert Bosch Mechatronikai Tanszék rendelkezik egy FANUC LR Mate 200iC típusú ipari robottal. A robot hat szabadságfokú és teljes kinyúlási távolsága 704 mm, ebben a helyzetben a maximális terhelhetősége 5 kg. A robot továbbá rendelkezik háromdimenziós alakfelismerési képességgel. Ez egy kétdimenziós kamera és egy lézerfej segítségével valósul meg.
Ebben a dolgozatban az adott robot direkt kinematikai feladatának vizsgálatával és az inverz kinematikai feladat analitikus megoldásával foglalkozunk. A kinematikai leíráshoz a Denavit-Hartenberg paramétereket alkalmazzuk. A robot hat szabadságfokú mozgását hat csukló valósítja meg, melynek szimbolikus jelölése: RRRRRR (R – Rotation).
A robot utolsó három csuklójának tengelye egy pontban keresztezi egymást, így gömbcsuklót alkotnak. Ez a gömbcsukló tulajdonképpen a zárótag orientációját határozza meg, míg az első három csukló a gömbcsukló helyzetét. A csuklók ezen csoportosítása teszi lehetővé a robot inverz kinematikai feladatának analitikus vizsgálatát. A zárótag helyzetének leírásához felhasználjuk az Euler szögeket is.
A dolgozat lényegi része három pontban kerül kidolgozásra. Az első pontban a Denavit-Hartenberg paraméterek felhasználásával előállítjuk a robot helyzetét meghatározó helyzetmátrixokat. A helyzet ismeretében származtatjuk a sebesség- és gyorsulásállapotokat.
A második pontban Spong, Hutchinson és Vidyasagar szerzők Robot Modeling and Control könyvének feldolgozásával foglalkozunk, a robot inverz kinematikai feladatával. Az első három csukló szögét egyszerű geometriai megfontolások alapján állítjuk elő trigonometrikus függvények segítségével. A vizsgált robotunk és az adott irodalomban bemutatott robot struktúrája nem teljesen egyezik meg, ezért az összefüggéseket ennek megfelelően módosítjuk. Az orientációt meghatározó gömbcsuklót az Euler leírás segítségével elemezzük. A vázolt kinematikai felbontás eredményeként végül hat darab trigonometrikus összefüggést kapunk az egyes csuklók szögelfordulásaira.
Az utolsó pontban a fent ismertetett elmélet alapján bemutatjuk azt a számítógépi programot, amelyet a két feladat alapján dolgoztunk ki. A program a robot egy szerelés során végzett mozgását szimulálja. A zárótag végpontjának pályáját két azonos sugarú félkörívvel és azokat összekötő egyenes szakaszokkal adtuk meg. A mozgás során a zárótag hossztengelye a pálya görbéjére mindig merőleges, és a pálya síkjának normálisával adott szöget zár be. A zárótag végpontjának pályasebességét konstansnak írtuk elő. A program először az inverz kinematikai feladatot oldja meg oly módon, hogy egyenközű időlépéseket feltételezve előállítja a robot helyzetét meghatározó csuklók szögelfordulásait diszkrét időpontokban. Másodszorra a középponti differencia módszerét alkalmazva meghatározzuk a csuklók szögsebességeit és szöggyorsulásait. Ezek ismeretében előállítjuk a robot sebesség- és gyorsulásállapotát a direkt kinematikai feladat összefüggéseinek felhasználásával. Az eredményeket, azaz a robot helyzetét, a csuklók szögelfordulásait, szögsebességeit, és szöggyorsulásait grafikusan szemléltetjük.
4
2. A FANUC LR MATE 200IC BEMUTATÁSA 2.1. A Fanuc LR Mate 200iC tulajdonságai és jellemzői A Fanuc LR Mate 200iC az elődeihez képest sokkal hatékonyabb és magasabb fokú pozícionálási képességgel rendelkezik, ezért számtalan ipari cég alkalmazza. Sebességét hosszabb ideig képes fenntartani, és nagy sebesség mellett - a fejlett szervotechnika miatt - kisebb a vibráció. Az 1. ábrán látható, hogy a Fanuc LR Mate 200iB elődjéhez képest 42%-al keskenyebbek a karjai, asztallap méretű talpazata és hat szabadságfoka miatt a kisebb, nehezebben hozzáférhető helyeken is tud dolgozni. [4]
1. ábra
A 2. ábrán látható a robot munkatere és méretei felülnézetből, valamint oldalnézetből. A J4-es csuklón pneumatikus és elektromos csatlakozók találhatók. A zárt külső megakadályozza, hogy a kábelek és a huzalok összeakadjanak. A Fanuc LR Mate 200iC robotot a következő főbb tevékenységekre alkalmazzák:
gépek ellátása, anyagkezelés, szerelés, mosás, anyag eltávolítás, oktatás.
5
2. ábra
A Fanuc LR Mate 200iC munkatere és méretei
1. táblázat Az LR Mate 200iB és az LR Mate 200iC összehasonlítása
Modell LR Mate
200iB LR Mate
200iC Maximális terhelhetőség a
csuklón 5kg – 68mm 5kg – 163mm
Kinyúlás 700mm 704mm J1 180° /sec 350° /sec J2 180° /sec 350° /sec J3 225° /sec 400° /sec J4 400° /sec 450° /sec J5 330° /sec 450° /sec
Maximális sebesség
J6 480° /sec 720° /sec Ismétlési pontosság +/- 0.04mm +/- 0.02mm Mechanikai tömeg 45kg
27kg
6
3. A ROBOT DIREKT KINEMATIKAI ELEMZÉSE 3.1. A nyitott láncú robot helyzetének leírása Denavit-Hartenberg paraméterekkel
A robotkarok egymáshoz viszonyított helyzetének megadása érdekében minden robotkarhoz rögzítünk egy ún. belső koordinátarendszert. A karok egymáshoz viszonyított helyzetét a 3. ábrán látható koordinátarendszerek közötti transzformációval írjuk le.
3. ábra
A Fanuc robot belső koordinátarendszere
y2 y4
x0
y0
y1
x1
x6
z6
z5 y6
x4 x5
z4 y5
x2 x3
z3
y3
z2
z0
z1
7
A koordináta-rendszerek helyzetének leírására az ún. Denavit-Hartenberg paramétereket használjuk [1], melyek értelmezése a következő:
sk a zk-1 és xk tengelyek metszéspontjának zk-1 koordinátája; Θk a zk-1 tengely körüli forgás előjeles szöge, amely az xk-1 tengelyt
az xk tengelybe viszi; αk az xk tengely körüli forgás előjeles szöge, amely a zk-1 tengelyt a
zk tengelybe viszi; ak a zk-1 és zk tengelyek távolsága (normáltranszverzális hossza).
A Denavit-Hartenberg paraméterek rendre a következő négy egymás utáni merevtestszerű relatív mozgásnak felelnek meg: 1. eltolás a zk-1 tengely irányában (sk értékkel); 2. forgás a zk-1 tengely körül (Θk szöggel); 3. forgás az xk tengely körül (αk értékkel); 4. eltolás az xk tengely irányában (ak értékkel). Ezek a mozgások a fentiek szerint a
ksH ,
kH
Q,
kH
a,
kaH transzformációs
mátrixokkal írhatók le, melyeknek az ugyanebben a sorrendben vett szorzata a
kkkk askkHHHHH
aQ-=
,1 (1)
úúúú
ű
ů
ęęęę
ë
é
úúúú
ű
ů
ęęęę
ë
é-
úúúú
ű
ů
ęęęę
ë
éQQQ-Q
úúúú
ű
ů
ęęęę
ë
é
=-
100001000010
001
100000000001
100001000000
1000100
00100001
,1
k
kk
kkkk
kk
kkk
a
cssccs
sc
sH
aaaa
kifejezéssel írható le. A mátrixszorzásokat elvégezve a következő egyszerűbb alakhoz jutunk, ahol a
cos=c , sin=s rövidítéseket alkalmazzuk: (2) Ezek alapján meghatározzuk a robot egyes koordinátarendszereinek transzformációját megvalósító Denavit-Hartenberg mátrixokat, melyek rendre a következőképpen alakulnak:
úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
úúúú
ű
ů
ęęęę
ë
é
=
-
-
10000
1000 ,1
,1kkk
kkkkkkk
kkkkkkk
kk
kk scssascccscasscsc
rhH
aaaaaa
8
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
010101
01010101010101
01010101010101
01 saa
Haa
aaaa
(3)
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
121212
12121212121212
12121212121212
12 saa
Haa
aaaa
(4)
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
232323
23232323232323
23232323232323
23 saa
Haa
aaaa
(5)
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
343434
34343434343434
34343434343434
34 saa
Haa
aaaa
(6)
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
454545
45454545454545
45454545454545
45 saa
Haa
aaaa
(7)
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
( ) ( )úúúú
ű
ů
ęęęę
ë
éQQ-QQQQQ-Q
=
1000cossin0
sinsincoscoscossincossinsincossincos
565656
56565656565656
56565656565656
56 saa
Haa
aaaa
(8)
A k-adik tag állványhoz viszonyított helyzete a relatív helyzetmátrixok szorzataként nyert
Ő=
--==
k
jjjkkk
HHHHH1
,1,112010... (9)
helyzetmátrixszal adható meg. Ebből nk = behelyettesítésével a zárótag
nH
0
helyzetmátrixát kapjuk, ahol esetünkben 6=n .
9
560556453423120106
4504453423120105
34033423120104
230223120103
120102
HHHHHHHHHHHHHHHHH
HHHHHHHHHHHHH
HHH
==
==
==
==
=
(10)
A zárótag bármely nR helyvektorának az állványhoz kötött koordinátarendszerre vonatkozó koordinátái az nn
RHR00 = összefüggéssel számíthatók.
3.2. A robot sebesség- és gyorsulásállapota
A robot zárótagjának sebesség- és gyorsulásállapotát az nnRHR
00 = összefüggés idő szerinti deriválásával kapjuk:
,00 nn
RHR = ahol (11)
,00 nn
RHV·
= ahol (12)
,00 nn
RHA··
= ahol (13)
Megjegyezzük, hogy nn rv 00
·
= a zárótaghoz kötött koordinátarendszer origójának
sebességkoordinátáit, míg a nnn rva 000
···
== a koordinátarendszer origójának
gyorsuláskoordinátáit jelenti. A n
H0
·
, illetve a n
H0
··
mátrixot a zárótag sebességállapot- és gyorsulásállapot-mátrixának nevezzük. A zárótaghoz kötött pontok esetében állandóR n = (az időnek nem függvénye).
úúúúú
ű
ů
ęęęęę
ë
é
=
··
0000
000
nnn
vhH
úúúú
ű
ů
ęęęę
ë
é
=
1000
000
nnn
rhH
úúúúú
ű
ů
ęęęęę
ë
é
=
····
0000
000
nnn
ahH
10
A kk
H,1-
mátrix felhasználásával igazolható, hogy
kkk
k
kk
k
kk HDHH
,1,1,1
-·-
·
- =Q¶
¶=
Q¶
¶ és
kkkkkHDH
,1,1 -
·
-
·
Q= , (14), (15)
ahol
kD az ún. deriváló mátrix, csukló esetén az alábbi alakú:
(16)
A kQ általános koordináta, a kk w=Q·
koordinátasebesség és a kk e=Q··
koordinátagyorsulás ismeretében bármely közbenső tagok, illetve a zárótag helyzet-, sebességállapot- és gyorsulásállapot-mátrixai az állványtól kiindulva, lépésenként, az alábbi formulákkal számíthatók:
kkkkkkkkkkkkk
kkkkkkk
kkkk
HDHDDHHHH
HDHHH
HHH
,11,01,01,01,00
,11,01,00
,11,00
2-
··
-
··
--
·
-
····
-
·
--
··
--
úűů
ęëé Q+Q÷
řö
çčć Q++=
÷řö
çčć Q+=
=
(17)
úúúú
ű
ů
ęęęę
ë
é -
=
0000000000010010
kD
11
4. A ROBOT INVERZ KINEMATIKAI FELADATA
Az előző fejezetben a robot direkt kinematikai feladatát elemeztük, mely során előállítottuk a zárótag helyzetét és orientációját a kQ függvényében.
Ebben a fejezetben a robot inverz kinematikai feladatával foglalkozunk, azaz a zárótaghoz kötött koordinátarendszer „O” origójának helyzetének és a koordinátarendszer R orientációjának ismeretében keressük az egyes csuklók kQ szögelfordulásait.
Az irodalomban a problémát általános esetben numerikusan szokás kezelni [2]. Az ipari robotok gyakran hat szabadságfokúak, az első három csukló a zárótag helyzetét határozza meg, míg az utolsó három a zárótag orientációját. A robot helyzetét és orientációját egymástól függetlenül vizsgálhatjuk. Azoknál a robotoknál, melyeknek hat csuklója van, és az utolsó három csukló tengelye egy pontban keresztezi egymást - így gömbcsuklót alkotnak - a feladatot felbontjuk inverz kinematikai helyzetre és inverz kinematikai orientációra. [2]. Az általunk vizsgált robot is így van kialakítva.
A 4. ábrán az utolsó három tagot úgy is felfoghatjuk, mint ha egy gömbcsukló lenne, ahol a z3, z4 és z5 tengelyek egy pontban metszik egymást, az cO pontban. Megjegyezzük, hogy a zárótag Euler-szögekkel adott R orientációjának ismeretében a három tengely körüli szögelfordulás egyértelműen meghatározható.
Az alkalmazott módszer lényege, hogy az utolsó három tag mozgása során az előbb említett cO pont helyzete ne változzon, ezáltal az általunk elképzelt gömbcsukló középpontjának pozícióját az első három tag határozza meg.
A 4. ábrán látható, hogy az ötödik koordinátarendszer origója egybeesik az cO ponttal és a helyzetmátrixát az állványhoz képest a 3.1. fejezetben említett
módon adhatjuk meg:
(18) ahol Rh = , azaz
úúú
ű
ů
ęęę
ë
é=
333231
232221
131211
rrrrrrrrr
R . (19)
A 5z és az 6y tengelyek egybeesnek, így a közös irány a h , vagyis az R orientációt leíró mátrix második, azaz yR oszlopával adható meg. A zárótag origójának helyzete előállítható az cO -től mért, 6s nagyságú 6y menti eltolási transzformációval.
úúúú
ű
ů
ęęęę
ë
é
=
1000
06
rhH
12
4. ábra
Kinematikai szétválasztás Az cO középpont helyzete a 4. ábrából leolvasva a következőképpen adható meg: yc RsOO 6-= . (20)
A zárótag „O” középpontjának koordinátáit jelölje xO , yO és zO , míg cO koordinátáit cx , cy és cz . A kettő közötti kapcsolatot a következő kifejezés írja le:
úúú
ű
ů
ęęę
ë
é
---
=úúú
ű
ů
ęęę
ë
é
326
226
126
rsOrsOrsO
zyx
z
y
x
c
c
c
. (21)
Az előző egyenlet segítségével tudjuk meghatározni az első három tag elfordulását. 4.1. Az inverz helyzet meghatározása geometriai úton
Az inverz pozíció meghatározásához a háromszögek geometriai összefüggéseit fogjuk alkalmazni.
Általában az inverz kinematikai feladat bonyolultsága attól függ, hogy mennyi nulla értékű Denavit-Hartenberg paraméter van. Minél több ezek száma, annál könnyebb megoldani az inverz feladatot. A legtöbb manipulátor esetében az sk értéke nulla, az ka értéke pedig vagy nulla vagy ± π/2. A mi esetünkben ezek az értékek a következőképpen alakulnak:
s1 = 0.330 m a1 = 0.075 m α1 = π/2 s2 = 0 m a2 = 0.300 m α2 = 0 s3 = 0 m a3 = 0.075 m α3 = π/2 s4 = 0.320 m a4 = 0 m α4 = -π/2 s5 = 0 m a5 = 0 m α5 = π/2 s6 = 0.140 m a6 = 0 m α6 = π/2
cr0
06r
yRs6
O
cO
13
Látható, hogy az értékek többsége számunkra kedvezőek, így valóban a geometriai megközelítést célszerű alkalmazni. A szögek meghatározásához a trigonometrikus függvényeket hívjuk segítségül. A 1Q kiszámítását a 6. ábra alapján végezzük el, mely a csuklókat és a tagokat az x0 – y0 síkban ábrázolja. A 1Q -re a következő trigonometrikus alakot írhatjuk fel: ( )cc xya ,2tan1 =Q . (22) Az 2tana függvény alkalmas arra, hogy egy síkvektor y és x koordinátáiból kiszámítsuk a vektor irányszögét.
5. ábra Az első három tag az x0 - y0 - z0 térben
6. ábra
Az első három tag felülnézeti képe
1a
2a
cy
0O
1O
2O
4O0y
0xcx
4s
1Q
1Q
2Q
*3Q
30Q
0O
1O2O
3O
4O
0x
0y
0z
cx
cy
cz
r s
1s
4s
3a*3 a
14
A robot első három tagjának egy tetszőleges helyzetét szemlélteti az 5. ábra. Az 1O ,
2O és 4O pontok által meghatározott háromszögre a koszinusztételt alkalmazzuk. Megjegyezzük, hogy a háromszög 2O pontjában lévő szög negatív koszinusza megegyezik a 3Q szög koszinuszával. Így
( )*32
2*3
22
22*3 2
cosaa
aasr --+=Q (23)
Mivel 1
22 ayxr cc -+= és 1szs c -= , az előző egyenlet a következő alakban is felírható:
( ) ( )
Daa
aaszr c =+--+
=Q *32
2*3
22
21
2*3 2
cos (24)
Az 1
22 ayxr cc -+= képletnél azért vontuk le az ma 075.01 = értéket, mert az első csukló után a robot karja ferdén helyezkedik el. Az 5. ábra alapján a Pitagorasz-tételt alkalmazva 2
423
*3 saa += .
Ezek után a *3Q a következő formulával adható meg:
( )DDa ,12tan 2*
3 -±=Q (25)
Azért lehetséges két megoldás, mert a kar állhat felső-könyök helyzetben vagy alsó-könyök helyzetben, attól függően, hogy a célpontot felülről vagy alulról szeretnénk megközelíteni. (7. ábra)
7. ábra
Az 5. ábra alapján a tényleges hármas csukló szögelfordulás paramétere
230
*33
p+Q-Q=Q . (26)
Az előzőhöz hasonlóan a 2Q -t is geometriai úton kaphatjuk meg a 8. ábra alapján.
Felső-könyök helyzet
Alsó-könyök helyzet
4O
15
8. ábra *
2*
2 Q-Q=Q (27)
( ) ( )( ) ( )*
3*32
*3
*31
221
*3
*32
*3
*32
cos,sin2tan,2tan
cos,sin2tan,2tan
Q+Q--+-
=Q+Q-=Q
aaaaayxsza
aaaarsa
ccc
(28)
4.2. Inverz orientáció meghatározása az Euler-szögek segítségével Az inverz orientációs probléma lényege, hogy a zárótag adott orientációja esetén hogyan határozzuk meg az utolsó három csuklóban a szögelfordulásokat. Megjegyezzük, hogy a következő egyenletekben a kifejezések bonyolultsága és hossza miatt az alábbi rövidítéseket alkalmaztuk: iQcos helyett a ic -et ( )ji Q+Qcos helyett a ijc -t iQsin helyett a is -et ( )ji Q+Qsin helyett a ijs -t. Az állvány és a harmadik tag relatív helyzetmátrixát, valamint a harmadik tag és a zárótag relatív helyzetmátrixát megkaphatjuk a megfelelő helyzetmátrixok szorzataként:
23120103HHHH = (29)
56453436HHHH = . (30)
A kijelölt mátrix szorzásokat a MAXIMA 5.20.1 szimbolikus manipulátorral hajtottuk végre [5]. Ebből nyerjük az inverz orientációs feladathoz a
03h és a
36h orientációs mátrixokat:
1O
2O
4O
*3a
2ar
s*3Q
*Q2Q
*2Q
16
( ) ( )( ) ( )
úúú
ű
ů
ęęę
ë
é
-++--+-
=
32322332
23321132321
23321132321
03
0 ccssscscscscscssccsscsccsssccc
h (31)
úúú
ű
ů
ęęę
ë
é
---++-
=
65556
646455446564
466545464654
36
sscscccsscsssccscscsccscssccc
h (32)
A következő egyenlőség feltételezésével határozható meg a további három ismeretlen szög: ( )
060336hhh T= , (33)
ahol úúú
ű
ů
ęęę
ë
é=
333231
232221
131211
06
rrrrrrrrr
h mátrix elemei adottak.
A mátrix egyenletet kifejtve a következő öt egyenlőségekhez jutunk:
( ) ( ) ( )
( ) ( ) ( )
2311123121233165
2311323123233365
12112121221232
233212123321223232325
22111254
12321211212122
2332323232121323212254
scrssrcrcsscrssrcrss
scrssrcrscscrcscscsrccssrc
rcsrsssrccrcsr
scscrssccrcssccsrsc
++-=-
++-=-++-=
=++++-=
-=++=
=++-+-=
(34)
Ezek alapján a keresett három ismeretlen csuklószög meghatározható:
( ) ( )( )( )
( ) ( )( )
( ) ( )( )2311123121233123113231232333
6565666
12321211212122221112
5454444
121121212212322
12112121221232
525555
,2tan,2tan,2tan
,2tan,2tan,2tan
,12tan
,12tan,2tan
scrssrcrscrssrcracsssacsa
srccrcsrrcsrascssacsa
scrssrcrscrssrcra
ccacsa
++-++-==--==Q++-=
===Q
++-++--±=
=-±==Q
(35)
Az utóbbi két képlet csak akkor alkalmazható, ha a 5Q különbözik nullától, ellenkező esetben a 4Q és 6Q összegét tudjuk közvetlenül meghatározni, azaz az egyik felvétele után a másik szög kiadódik.
17
5. PÉLDA A ROBOT KINEMATIKAI VIZSGÁLATÁRA
A 3. és 4. fejezetben ismertetett elmélet alapján számítógépi programot készítettünk SCILAB 5.2.1 rendszer alatt [6]. A program alkalmas a zárótag előírt pályája és orientációja alapján a robot helyzeteinek, sebesség- és gyorsulásállapotainak meghatározására.
A továbbiakban egy olyan mintafeladatot vizsgálunk, amelyben előírjuk a robot zárótagjának pályáját és a zárótag mozgás közbeni orientációját. A pont pályája két azonos sugarú félkörívvel és azokat összekötő párhuzamos egyenesekkel adjuk meg. A geometriai adatokat a 9. ábra szemlélteti.
9. ábra
A 9. ábrán láthatjuk, hogy
· a pálya egy síkbeli alakzat, az x-y síkban helyezkedik el. · a pálya kezdő félkörének középpontja a (0; 0.450; 0.100) pontban található. · a félkörök sugara 0.05 méter nagyságúak. · a szerszám végpontja az x-y síkkal 70°-os szöget zár be (ennek a kiegészítő
szögét kell megadni, azaz 110°-ot). · összesen 120 részből áll: az első félkör 40, az első egyenes szakasz 20, a
második félkör 40, míg a második egyenes szakasz hossza 2p×= rh , amely
20 részből áll.
A pont mozgása során a pályasebességét konstansnak írjuk elő. A csuklók elfordulásait minden időpillanatban kimentjük, majd a középponti differencia módszer alkalmazásával a szögsebességeket és szöggyorsulásokat meghatározzuk.
h
x
y
0.450
18
5.1. Sebességek és gyorsulások kiszámítása
A sebességek és gyorsulások kiszámításánál abból indulunk ki, hogy az egyes csuklókban a szögelfordulások egyenlő időközönként adottak. Továbbá feltételezzük, hogy a diszkrét pontokban adott függvények három egymást követő pontra támaszkodó parabolaívekkel közelíthetők. Ebben az esetben a numerikus eljárásoknál ismert ún. középponti differencia módszert alkalmazhatjuk a szögsebesség és szöggyorsulás közelítő meghatározására [3].
10. ábra Középponti differencia módszer
Az eljárás alapgondolata, hogy elmozdulás-idő függvényt két időlépés
tartományán parabola ívvel közelítjük (10. ábra). Mivel a parabola szelője és a parabola felezőpontjához tartozó érintő párhuzamos, ebből a geometriai megfontolásból felírhatjuk a felezőpontban az idő szerinti első deriváltat, azaz a sebességet:
xyyy ii
i D-
= -+
211' (36)
A gyorsulás hasonló megfontolások alapján a következő alakban írható fel:
( ) ( )
211
211" 2
xyyy
xyyyyy iiiiiii
i D+-
=D
---= -+-+ (37)
5.2. Elfordulások, sebességek és gyorsulások kirajzolása
Az adatok beprogramozása után sikerült kirajzoltatnunk a robot útját, amint a jégkorongpálya mintán halad körbe. A kapott eredményt a 11. és a 12. ábrák szemléltetik. Azt, hogy a robot elfordulásának a szögei a mozgás során mekkorát
y
x
yi-1
yi+1
yi
Δx Δx
19
változnak, és hogy ezek a változások mekkora sebesség és gyorsulás különbségeket eredményeznek, a 13. ábrán látható.
11. ábra
A robot pályája a program által kirajzolva
12. ábra
A robot pályája az x-z síkból nézve
20
13. ábra
A tagok elfordulásának a nagysága, a sebességük és a gyorsulásuk
A 13. ábrán a tagok jelölésére a következő színeket alkalmaztuk: · 1. tag: fekete · 2. tag: piros · 3. tag: zöld · 4. tag: kék · 5. tag: cián · 6. tag: magenta.
Az ábrából kitűnik, hogy a legnagyobb elfordulásokat a 4. tag végzi. A
sebesség ábrán is jól elkülöníthető, hogy a robot mikor rajzolja a félköröket és mikor az egyenes szakaszokat. Az egyenes szakaszok esetén a tagok elfordulása és sebességük alig változik a félkörökéhez képest. A gyorsulás ábrán lokális csúcsok láthatók. Ez azzal magyarázható, hogy a szögelfordulás függvény az eredeti feltételezéseknek megfelelően ugyan folytonos, de a deriváltjai nem, ezért a középponti differencia módszer ezekben a pontokban hibás eredményt szolgáltat.
A gyorsulás ábrán a 4. tag hirtelen nagy gyorsulásnövekedései és –csökkenései miatt kevésbé figyelhető meg a függvények lefutása, ezért a gyorsulás ábrát a [-4; +4]-es tartományba korlátoztuk, melyet a 14. ábra szemléltet.
21
14. ábra A bekorlátozott gyorsulás ábra
Ha a lokális csúcsoktól eltekintünk, akkor a 14. ábrából jól látszik, hogy a legnagyobb gyorsulásokkal az utolsó három tag rendelkezik, ezért várhatóan a legnagyobb inercia erők is itt ébrednek.
22
6. ÖSSZEFOGLALÁS A TDK dolgozatban a LR Mate 200iC ipari robot direkt és inverz kinematikai feladatával foglalkoztunk. Az inverz kinematikai feladat megoldására zárt alakú formulákat dolgoztunk ki, amely gyorsabb megoldással szolgál az általában alkalmazott numerikus módszereknél. A bemutatott elmélet alapján egy számítógépi programot készítettünk a robot mozgásainak leírására. Az elkészített tanulmányt a jövőben dinamikai feladatok irányába kívánjuk továbbfejleszteni.
23
IRODALOMJEGYZÉK [1] Király B.: Ipari robotok kinematikai és dinamikai elemzése, Oktatási
segédlet, Miskolci Egyetem, Miskolc, 1995. 7-20. old. [2] Mark W. Spong, Seth Hutchinson, M. Vidyasagar: Robot Modeling and
Control [3] Páczelt I., Szabó T., Baksa A.: A végeselem-módszer alapjai, 2007.
193. old. [4] LR Mate 200iC Series & R-30iA Mate Controller [5] Maxima 5.20.1 hivatalos weboldala [6] Scilab 5.2.1 hivatalos weboldala