robotokdinamikusmodellje.pdf

51

ROBOTOK DINAMIKUS MODELLJE

4. A ROBOT DIFFERENCIÁLIS MOZGÁSA Ebben a fejezetben egyik célunk a nemlineáris geometriai modell lokális linearizálása és az inverz geometriai feladat lecserélése inverz sebesség feladatra, és a feladat megoldása redundáns robotok és hiányzó szabadságfokok esetén. A lokális linearizáció annak meghatározását jelenti, hogy a robot csuklóváltozóinak sebessége hogyan befolyásolja a megfogó sebességét és szögsebességét. Másik célunk előre-tartó rekurzív összefüggések levezetése az egyes szegmensek szögsebességének, szöggyorsulásának, sebességének és gyorsulásának számítására. Ezek az összefüg-gések a dinamikus modell numerikus számításának alapját fogják képezni. Mindkét feladathoz fontos annak tisztázása, hogyan kell a sebességet és gyorsulást számítani álló (inercia) és mozgó koordináta-rendszerekben. Nem szabad ugyanis elfelejteni, hogy a Newton-Euler törvények inerciarendszerekben érvényesek.

4.1 Differenciálás mozgó koordináta-rendszerekben Legyen 0K egy álló derékszögű koordináta-rendszer, vagy egy olyan inercia derék-szögű koordináta-rendszer, amelyben 0K origója (lényegében) áll vagy konstans sebességgel mozog, de az 000 ,, kji tengelyirányú egységvektorok deriváltja az idő szerint nulla, például a tengelyek iránya a csillagokhoz képest áll, ezért állandónak tekinthető. Legyen továbbá K egy mozgó koordináta-rendszer. Fejezzük ki K egy-ségvektorait 0K bázisában (lásd 4.1. ábra):

.kajaiak,kajaiaj,kajaiai

033023013

032022012

031021011

++=++=++=

(4.1)

4.1. ábra. 0K rögzített és K mozgó koordináta-rendszer

p

P

0i0j

0k

0KK

r

i

j

k

ρ

0i0j

0k

0K

j

52 Lantos: ROBOTOK DINAMIKUS MODELLJE Képezzük K egységvektorainak idő szerinti deriváltjait, és használjuk ki, hogy

0K egységvektorai időben állandóak:

.kajaiak,kajaiaj,kajaiai

033023013

032022012

031021011

′+′+′=′′+′+′=′

′+′+′=′

(4.2)

Mivel KKAA0

:= és TKK AAA ==−

0,1 , ezért

k.ajaiakk,ajaiajk,ajaiai

3332310

2322210

1312110

++=++=++=

(4.3)

( ) ( )( )

( ) ( )( ) .kaaaaaa

jaaaaaaiaaaaaakajaiaa

kajaiaakajaiaai

313321231113

313221221112313121211111

33323131

2322212113121111

′+′+′++′+′+′+′+′+′=

=++′++++′+++′=′

(4.4)

Vegyük észre, hogy az kji ,, vektorok előtt álló tényezők a következő szorzat

oszlopvektorának komponensei:

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

′′′

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

′′′

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

′′′

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−

31

21

111

31

21

11

31

21

11

332313

322212

312111

aaa

Aaaa

Aaaa

aaaaaaaaa

T . (4.5)

Másrészt alkalmazható a Rodriguez formula kdkj,dji,di +++ számára. Figye-lembe véve, hogy ϕd kicsi, ezért például i esetén írható, hogy

itdiidi ×+=+ ϕ . (4.6)

Definiáljuk az ω szögsebességet a következő módon:

,: 000 kωjωiωkωjωiωtd

dtzyxzyx °+°+°=++==

ϕω (4.7)

akkor

4. A ROBOT DIFFERENCIÁLIS MOZGÁSA 53

,kk

j,ji,i

×=′×=′×=′

ωωω

(4.8)

AAω ′=× −1][ és .][ 10 −′=× AAω (4.9)

Tekintsük most a P pontot, akkor r pozíciója, r ′ sebessége és r ′′ gyorsulása rendre

000000 kpjpipkρjρiρkrjrir zyxzyxzyx +++++=++

000

000

kpjpip

kρjρiρkρjρiρkrjrir

zyx

zyxzyxzyx

′+′+′+

+′+′+′+′+′+′=′+′+′

.kpjpipkρjρiρ

kρjρiρkρjρiρkrjrir

zyxzyx

zyxzyxzyx

000

000 )(2′′+′′+′′+′′+′′+′′+

+′′+′′+′′+′′+′′+′′=′′+′′+′′

Vezessük be a következő jelöléseket: ,Tzyx )r,r,(rr = ,)r,r,r(r T

zyx ′′′=′ K , ),p,p,p(p zyx ′′′′′′=′′ akkor

,prAAAr

,pAAr,pAr

′′+′′+′′+′′=′′′+′+′=′

+=

ρρρρ

ρ

2 (4.10)

ahol r és p koordinátái 0K -ban, ρ koordinátái pedig K -ban vannak kifejezve.

Mivel ,AAA)(A)A(AAAIAA 111111 0 −−−−−− ′−=′⇒=′+′⇒= ezért

,AA

AAAAAAAAAAAA′′+××−=

=′′+′′−=′′+′′=′′=′×−

−−−−−−

1

111111

]][[

)()(][

ωω

ω (4.11)

,kωjωiω

kωjωiωkωjωiωkωjωiω

zyx

zyxzyxzyx

′+′+′=

×+′+′+′=′+′+′+′+′+′= ωωε : (4.12)

ahol ε a szöggyorsulás. A P pont r ′ sebessége és r ′′ gyorsulása kifejezhető a mozgó K koordináta-rendszerben is:

,pApAAArA ′+×+′=′+′+′=′ −−−− 1111 ρωρρρ (4.13)

.pA

pAAAAArA′′+××+×+′×+′′=

=′′+′′+′+′′=′′−

−−−−

1

1111

)(2

2

ρωωρερωρ

ρρρ (4.14)

54 Lantos: ROBOTOK DINAMIKUS MODELLJE

Legyen pAvK ′= −1: , pAaK ′′= −1: , ωω =:K és εε =:K rendre K origójának sebessége, gyorsulása, szögsebessége és szöggyorsulása, akkor a P pont ρv sebes-

ségére és ρa gyorsulására teljesül

ρρω ′+×+= KKρ vv , (4.15)

( ) ρωρρωωρε ′×+′′+××+×+= KKKKKρ aa 2 . (4.16)

Speciálisan, ha P egy merev test pontja, akkor 0=′′=′ ρρ , Kωωρ = , Kεε ρ = , és az összefüggés egyszerűbb alakot ölt.

4.2 A geometriai modell lokális linearizálása A robot geometriai modellje egy nemlineáris függvénykapcsolat a csuklóváltozók és a megfogó pozíciója és orientációja között. A kapcsolatot lokálisan, az aktuális q konfigurációban linearizálva eljuthatunk a robot )(qJ Jacobi mátrixához. A linearizálás differenciáláshoz kötődik, ami viszont felfogható a szuperpozíció elve alkalmazásának is. Fagyasszuk be minden csuklóváltozó aktuális értékét iq kivéte-lével, és adjunk iq -nek idq változást. Akkor az i , 1+i , ,K m szegmensek egyetlen merev testet alkotnak, amely a 1−it csuklótengely típusától függően elmoz-dul a tengely mentén idq -vel )(T , vagy elfordul a tengely körül idq -vel )(R . Utóbbi esetben mK origójából merőlegest bocsájthatunk 1−it -re, vehetjük a talppont és mK origója közötti távolságot, és ezzel a sugárral fog mK origója körmozgást végezni, amelynek szögsebessége iq& , az origó sebességének iránya pedig merőleges a 1−it és mip ,1− által meghatározott síkra. Vezessünk be egy iκ maszk változót, amelyre 1=iκ rotációs csukló esetén )(R , és 0=iκ transzlációs csukló esetén

)(T . Jelölje mK origójának sebességét és szögsebességét rendre mm v és m

mω , akkor a szuperpozíció elve szerint

∑=

−=m

iii

mm

m qdv1

1 & , (4.17)

∑=

−=m

iii

mm

m qt1

1 &ω . (4.18)

Az 1−imd parciális sebesség és 1−i

mt parciális szögsebesség rendre a következő:


)1( ,1111

,11 miiiiimiim pttAd −−−

−−− ×+−= κκ , (4.19)

11

,11 −−−− = iimii

m tAt κ . (4.20)

Azért, hogy az eredményt egyszerűbbé tegyük, hagyjuk el miT ,1− -ben az indexeket:

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡=− 1010001 Tm,i

pApnmlT . (4.21)

Ha zi−1 a csuklótengely, mint az eredeti Denavit–Hartenberg alak esetén, akkor

11 −− = ii kt , és ezért TAA =−1 miatt a következő eredményekhez jutunk:

Rotációs csukló )(R esetén:

,nkntnt

,mkmtmt

,lkltlt

ziiz,im

ziiy,im

ziix,im

=⋅=⋅=

=⋅=⋅=

=⋅=⋅=

−−−

−−−

−−−

111

111

111

(4.22)

( ) ( )

( ) ( )( ) ( ) .pnpnpknptnd

,pmpmpkmptmd

,plplpp

lppp

l

pklptld

xyyxiiz,im

xyyxiiy,im

xyyxx

yT

z

y

xT

iix,im

+−=×⋅=×⋅=

+−=×⋅=×⋅=

+−=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛ −=

=×⋅=×⋅=

−−−

−−−

−−−

111

111

111

0000001010

(4.23)

Transzlációs csukló )(T esetén:

,tim 01 =− (4.24)

.nkntnd

,mkmtmd

,lkltld

ziiz,im

ziiy,im

ziix,im

=⋅=⋅=

=⋅=⋅=

=⋅=⋅=

−−−

−−−

−−−

111

111

111

(4.25)

Bevezetve a

[ ]

⎥⎦

⎤⎢⎣

⎡ −×= −−

− 00)1( 11

1 Tiiii

itκtκ

∆ , (4.26a)


[ ]⎥⎦

⎤⎢⎣

⎡ ×= −−

− 0011

1 Ti

mi

m

im dt∆ (4.26b)

jelöléseket, írható hogy

11111

−−−−− == i

mm,im,ii

i

m,i ∆TT∆q

T∂

∂, (4.27)

.T∆T∆ m,iim,iim

11111 −−−−− = (4.28)

Az eredmények tömör alakra hozhatók, ha bevezetjük a robot mm J Jacobi mát-

rixát:

⎥⎥⎦

⎤

⎢⎢⎣

⎡=

−

−

110

110

mmmm

mmmm

mm

t...ttd...ddJ , (4.29)

qJvm

m

mm

mm

&=⎟⎟⎠

⎞⎜⎜⎝

⎛

ω. (4.30)

Mivel a rögzített 0K keretben

mm

m,m vAv 0=° , (4.31)

mm

m,m A ωω 0=° , (4.32)

ezért

mm

m,

m,m J

AA

J ⎥⎦

⎤⎢⎣

⎡=°

0

0

00

, (4.33)

qJv

mm

m &°=⎟⎟⎠

⎞⎜⎜⎝

⎛°°ω

. (4.34)

Következmény: Olyan esetekben, amikor az inverz geometriai feladat nem oldható meg analitikusan, meghatározhatjuk a (4.30) vagy (4.34) egyenlet )(tq& megoldását a q konfiguráció környezetében, és ebből numerikus integrálással )(tq értékét. Ha tehát a megfogó sebességét és szögsebességét írjuk elő a pozíció és orientáció he-lyett, akkor q& meghatározható a Jacobi mátrix #

mJ pszeudoinverze segítségével:

⎟⎟⎠

⎞⎜⎜⎝

⎛°°

°=m

m#m

vJq

ω& . (4.35)


Ha m=6 és mJ nemszinguláris, akkor .JJ m#m

1−= Azon q robot konfigurációkat,

amelyek esetén mJ képtere nem a teljes 6R tér )6)(range(dim <mJ , szinguláris konfigurációknak nevezzük. Szinguláris konfigurációkban vannak olyan irányok, amelyek mentén vagy amelyek körül a robot nem mozgatható.

4.2.1 Redundáns szabadságfokok esete

Ha 6>m , akkor a robot redundáns szabadságfokokkal rendelkezik, és célszerű a végtelen sok megoldás közül olyat kiválasztani, amelyre min→q& , mert ehhez tar-tozik a legrövidebb tranziens. Az optimális megoldás ekkor

( ) ⎟⎟⎠

⎞⎜⎜⎝

⎛=

−

m

mTmm

Tm

vJJJq

ω1

& . (4.36)

Az összefüggést a következő matematikai probléma keretében vezetjük le. Séma:

min,,,,, 2 →=∈∈<× xyAxRyRxmnA nmmn

Lagrange multiplikátor szabály:

>=< xxx ,2 konvex funkcionál 0=− yAx lineáris korlátozás

>−<+>=< yAxxxF ,, λ

optx xF →=′ 0 Alkalmazás:

yAAAx

yAAyAA

AxAxF

yxAxxF

TTopt

TT

TTx

T

1

1

)(

)(221

2102

,,,

−

−

=

−=⇒=⎟⎠⎞

⎜⎝⎛−

−=⇒=+=′

><−><+>=<

λλ

λλ

λλ

Vegyük észre, hogy TAA már egy nn × méretű kvadratikus mátrix, és ennek a közönséges inverzére van szükség.


4.2.2 Hiányzó szabadságfokok esete Ha 6<m , akkor az összes sebesség és szögsebesség előírást nem lehet egyidejűleg kielégíteni, mert kevesebb változó van, mint feltétel. Ekkor két lehetőség kínálko-zik:

i) Kiválaszthatunk mv és mω komponensei közül pontosan annyit, amennyi q&dim . Az ezekhez tartozó egyenleteket betartjuk, a többit figyelmen kívül hagy-

juk. Például ha 2dim =q& és xp , yp csak 1q , 2q -től függ, akkor választható vx és vy , és a hozzájuk tartozó két egyenletből meghatározható 1q& és 2q& .

ii) Választhatunk olyan q& csuklósebességet, amely szimultán minimalizálja az egyenletek hibáját LS értelemben, amikor is a kompromisszum megoldás

( ) ⎟⎟⎠

⎞⎜⎜⎝

⎛=

−

m

mTmm

Tm

vJJJq

ω1

& . (4.37)

Az összefüggést a következő matematikai probléma keretében vezetjük le. Séma:

min,,,, 2 →−∈∈>× yAxRyRxmnA nmmn

Gradiens egyenlő nulla feltétel:

>−−=< yAxyAxF , konvex funkcionál

optx xF →=′ 0 Alkalmazás:

yAAAx

yAAxAF

yyxyAxAxA

yyyAxAxAxyAxyAxF

TTopt

TTx

TT

1)(

022

,,2,

,,2,,

−=

=−=′

><+><−>=<

>=<+><−>>=<−−=<

Vegyük észre, hogy AAT már egy mm × méretű kvadratikus mátrix, és ennek a közönséges inverzére van szükség.


4.2.3 Dekompozíciós lehetőségek Fontossága miatt részletesebben vizsgáljuk a Jacobi mátrix invertálását abban a spe-ciális 6–DOF esetben, amikor az utolsó 3 tengely rotációs és a tengelyek egy közös

)6( ∗ pontban metszik egymást:

E*,T,

,B,EB, TA

dTTT 6

63

4300 10

0

1000100

00100001

⋅⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⋅⋅= , (4.38)

E,E*, Td

T 66

6

1000100

00100001

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

= . (4.39)

Feltesszük, hogy a végberendezés EB v sebessége és E

Bω szögsebessége a robot

BK báziskeretében adott. Akkor q& a következő lépésekben határozható meg: i) A sebesség és szögsebesség kifejezhető EK -ben:

EB

B,EEE

B,EB

B,EEE AvAv ωω 11 ; −− == . (4.40)

ii) Meghatározható *6K origójának sebessége és szögsebessége:

)( 61

6666

E*,E*,EE

EE

E*,** pAvAv −−×+= ω , (4.41)

EE

E*,** ωAω 66

6 = . (4.42)

iii) Mivel *,,i,di* 65401

6 ==− , ezért A Jacobi mátrix speciális blokk alsó-háromszögmátrix

⎥⎦

⎤⎢⎣

⎡=

CBA

J ** 0

66 . (4.43)

Legyen TA qqqq ),,( 321= , T

B qqqq ),,( 654= , akkor az inverz sebesség probléma két részfeladatra bontható, amelyben már csak 33× -as mátrixok zárt alakban számítha-tó inverzei szerepelnek:

**

A vAq 661−=& , (4.44)

66

6611

**

**

B vABCq ω+−= −−& . (4.45)


4.3 A kinematikai mennyiségek rekurzív számítása A robot dinamikus modelljét τ=+ hqH && alakra fogjuk hozni, ezért ehhez hasonlóan paraméterezzük a szegmensek kinematikai mennyiségeit is:

qiii &Γω = , (4.46a)

qΩv iii &= , (4.46b)

iiii q φΓε += && , (4.46c)

,θqΩa iiii += && (4.46d)

ahol iiω , i

iv , iiε , i

ia rendre az i szegmenshez rögzített iK keret origójának szög-sebessége, sebessége, szöggyorsulása, gyorsulása, mind kifejezve a iK keret bázi-sában. Felhasználván az (4.15-16) összefüggéseket iip ,1: −=ρ választás mellett, és figyelembe véve, hogy a mozgó koordináta-rendszerben érvényes deriválási szabály szerint

iiiiiiiiii qdpqtqt &&& 1,111 :)1( −−−− =×+−=′ κκρ , iiiiiii dqtqd ,111 −−− ×+=′′ &&& κρ ,

továbbá hogy az eredmények a 1−iK bázisában keletkeznek, amelyeket azután a iK bázisába át kell transzformálni, ezért

,1111

1 iii

ii

i,iii qtωAω &−−

−−− += (4.47a)

,qdpωvAv iii

i,iii

ii

i,iii &111

11

111 )( −−−

−−

−−− +×+= (4.47b)

,qtωqtεAε iii

ii

iii

ii

i,iii &&& 111

111 −−−

−−− ×++= (4.47c)

.2

)(

)(2

)(

1111

111

11

111

111

1

1111

1111

111

11

111

111

1

iii

iii

iii

iii

ii

i,iii

ii

i,iii

ii

i,i

iii

ii

i,iiii

iii

iii

i,iii

ii

i,iii

ii

i,iii

qdqdqtqdω

pωωpεaA

qdωAqdqtqd

pωωpεaAa

&&&&&

&&&&&

−−−−

−−−

−−

−−−

−−−

−

−−−−

−−−−

−−−

−−

−−−

−−−

−

+×−×+

+××+×+=

=×+×++

+××+×+=

(4.47d)

Itt 1−ii d , 1−i

i t (4.21-25) szerint határozható meg im = választás mellett. A fenti összefüggésekből kiolvashatóak a iiii θΩφΓ ,,, rekurzív számítására szolgáló kép-letek. Azért, hogy ezeket a képleteket egyszerűbbé tegyük, elhagyjuk a mátrixok


végén álló nulla oszlopokat, vagyis az i -edik lépésben ii ΩΓ , -nek i oszlopa van és iqi =&dim :

1,,1,,11

,1, −== −−− ijA jiiiji KΓΓ ; 1, −= i

iii tΓ , (4.48a)

iii

ii

i,iii qtωAω &11

111 −−

−−− += , (4.48b)

iii

ii

ii,ii qtωA &1111 −−

−− ×+= φφ , (4.48c)

1,,1),( 11111 −=×+= −−−

−− ijpΓΩAΩ i,ij,ij,ii,iji, K ; 1−= i

iii, dΩ , (4.48d)

.2

)(

111

111

11

11111

iii

iii

iii

ii

i,iii

ii

i,iiii,ii

qdqtqdω

pωωpθAθ

&&& −−−

−−−

−−

−−−−−

×−×+

+××+×+= φ (4.48e)

Ha ciρ jelöli az i szegmens tömegközéppontját a iK keretben kifejezve, akkor

[ ] iciici ΓρΩΩ ×−= , (4.49a)

)( ciii

ii

ciiici ρωωρφθθ ××+×+= , (4.49b)

qΩv cicii &= , (4.49c)

cicicii θqΩa += && . (4.49d)

A kinematikai mennyiségek fenti paraméterezése kapcsolatban áll a robot Jacobi mátrixával is. A kapcsolat érzékeny arra, hogy a bázisvektorok a 0K (rögzített) vagy mK (mozgó) koordináta-rendszerből valók-e. Ha vessző (’) jelöli a formális (elemenkénti) deriváltat az idő szerint, akkor

,qΓΩ

qΓΩ

qdtJd

qJεa

m

m

m

mmm

m

m &&&&&& ⎥⎦

⎤⎢⎣

⎡′°

′°+⎥

⎦

⎤⎢⎣

⎡°°

=°

+°=⎟⎟⎠

⎞⎜⎜⎝

⎛°°

(4.50)

.qΓ

Ω])qΓ[(ΩqΓΩ

qΓ)qΩ()qΓ(qΩqJ

εa

mm

mm

mm

mm

mm

mm

mm

mm

mm

mm

mm

mm

mm

&&

&&

&

&&&&&

⎥⎥⎦

⎤

⎢⎢⎣

⎡

′×+′

+⎥⎥⎦

⎤

⎢⎢⎣

⎡=

=⎟⎟⎠

⎞⎜⎜⎝

⎛

′×+′

+=⎟⎟⎠

⎞⎜⎜⎝

⎛

(4.51)

Az indexek közül a jobb alsó (a névvel együtt) azonosítja a mennyiséget, a bal felső pedig a bázist, amiben a mennyiség fel lett írva. A parciális sebesség és parciális szögsebesség (4.21-25) szerint számítható. A végberendezés sebessége és szögse-bessége m

m J ismeretében már meghatározható, ugyanis ha


⎥⎦

⎤⎢⎣

⎡=

10,,

, TEmEm

EmpA

T , (4.52)

akkor )( ,, Emm

mm

mTEmE

E pvAv ×+= ω , (4.53a)

mmT

EmEE A ωω ,= , (4.53b)

( )

mm

TEm

TEmEm

TEm

EE J

AApA

J⎥⎥⎦

⎤

⎢⎢⎣

⎡ ×=

,

,,,

0][

. (4.54)

5. FIZIKAI ELVEK A MECHATRONIKÁBAN 63

5. FIZIKAI ELVEK A MECHATRONIKÁBAN Ebben a fejezetben megismerkedünk a merev testek inerciajellemzőivel és a mecha-tronikai rendszerek dinamikus modelljeinek felállításához használható fizikai elvek-kel.

5.1 Merev testek inercia jellemzői A merev test kinetikus energiájának kifejezéséből fogjuk kiolvasni, melyek azok a merev test jellemzők, amelyek energia-megfontolásból fontosak.

Tekintsük a ][][ ×× ρρ T kifejezést:

.

00

0

00

0][][

22

22

22

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+−−−+−−−+

=

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

−=××

yxzyzx

zyzxyx

zxyxzy

xy

xz

yz

xy

xz

yzT

ρρρρρρρρρρρρρρρρρρ

ρρρρρρ

ρρρρρρ

ρρ

(5.1)

Ha ρ az [ ]s szegmens (merev test) egy tetszőleges pontja és dm a hozzá tartozó infinitezimális tömeg, akkor a mxx d)(: ××∫−= ρρK lineáris transzformáció az inercia tenzor, amelynek pozitív definit mátrixa a K tehetetlenségi vagy inercia mátrix. (Az inercia mátrixot 'bold' K -val jelöljük a könyvnek ebben a részében, hogy elkerüljük az ütközést a koordináta-rendszer K jelölésével. Az I vagy J je-lölést nem használjuk, mert ezek az egységmátrixot és a robot Jacobi mátrixát jelö-lik. A könyv más részein a 'bold' K jelölést már elhagyjuk. A koordináta-rendszereket az ábrákon bekarikázott K -val jelöltük).

[ ] [ ] ,d:][ ⎥

⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

=××= ∫zyzxz

yzyxy

xzxyx

s

T

KKKKKKKKK

mρρK (5.2)

ahol ∫ +=

][

22 ,d)(s

zyx mK ρρ (5.3)


∫−=][

,ds

yxxy mK ρρ (5.4)

rendre az x tengelyre és az ],[ yx síkra vonatkozó tehetetlenségi (inercia) nyoma-ték, és K többi elemei hasonló módon vannak definiálva. (Megjegyezzük, hogy az irodalomban szokás a főátlón kívüli elemeket yzxzxy KKK −−− ,, módon is jelölni).

A tömeg és a tömegközéppont (center of mass) értelmezése

∫=][

ds

mm és ∫=][

ds

c mm ρρ . (5.5)

Az cmρ szorzatot szokás első momentumnak is nevezni. Legyen K és K~ két ortonormált koordináta-rendszer. Vizsgáljuk meg, hogyan

függ a tehetetlenségi mátrix a vonatkoztatási pont (a koordináta-rendszer origójá-nak) megválasztásától, lásd 5.1. ábra.

5.1. ábra. Az inerciamátrix transzformációja

Mivel ρρ += p~~ koordináta független alakban és

[ ][ ] ( )[ ] ( )[ ]

[ ][ ] [ ][ ] [ ][ ] [ ][ ],~~~~~~~~

××+××+××+××==×+×+=××

ρρpρρpppρpρpρρ

ezért integrálás után a Huygeno–Steiner képlethez jutunk, amelynek tenzor alakja

[ ][ ] [ ][ ] [ ][ ]××−××−××−= pmmpppm cc~~~~~ ρρρρ KK , (5.6)

mátrix alakja pedig

ρ

K

K~x~

y~

z~ρ~

p~

x

y

z

x

y

p


[ ][ ]

[ ] ( )[ ] ( )[ ][ ].~~

~~

~~

~~~

××−××−

−××−=

pmAmAp

ppmAA

cKKcKK

TKKρKKρ

ρρ

KK (5.7)

Speciálisan, ha a koordináta-rendszer cKK =: a tömegközéppontba van helyez-

ve és KK =:~ egy tetszőleges koordináta-rendszer, akkor 0:d == ∫ mm c ρρ ,

,:p cρ=~

[ ][ ]

[ ] ,2ccc

TK,KcK,K

ccTK,KcK,K

mImAA

mAA

cc

cc

ρρρ

ρρ

o−+=

=××−=

K

KK (5.8)

ami lehetővé teszi a koordináta-rendszer origójához tartozó inerciamátrix )(K ki-számítását, ha ismeretes a tömegközéppont )( cρ és a tömegközépponthoz tartozó inerciamátrix )( cK . Bár a cK keret origója a tömegközéppontban van, az

ccc zyx ,, tengelyek nem szükségképpen a főtengelyek.

5.2. ábra. Összetett rendszer eredő tehetetlenségi mátrixa

Alkalmazzuk ezt az eredményt két merev testből álló összetett rendszerre, lásd

5.2. ábra. Legyenek a két merev test inercia jellemzői 111 ,, ccm Kρ és

222 ,, ccm Kρ , ahol az inercia mátrixok a merev testek tömegközéppontjához rögzí-tett ciK keretben vannak megadva. (A K keret robot esetén lehet például a Denavit-Hartenberg keret, a két merev test a szegmens, valamint a szegmensre sze-relt és a következő szegmenst mozgató motor). A tömegközéppontok legyenek adot-tak K -ban. Feltesszük, hogy a K és cK koordináta-rendszerek párhuzamosak. Al-

1cK

cK

2cK

Kcρ

1cρ

2cρ

11, cm K

cm K,

22 , cm K

1cK

cK

2cK

cρ

1cρ

2cρ

11, cm K

cm K,

22 , cm K


kalmazzuk a (5.8) összefüggést rendre a 1cK , cK , cc ρρ −1 , 11 ccc K,K,KK AA = és

2cK , cK , cc ρρ −2 , 22 ccc K,K,KK AA = választás mellett. Az eredő rendszer jellemezhető az ccm K,, ρ inercia jellemzőkkel:

m m m= +1 2 és ,21

2211

mmmm cc

c ++

=ρρ

ρ (5.9)

( ) ( )[ ]( ) ( )[ ] .ImmAA

ImmAA

ccccccTK,KcK,K

ccccccTK,KcK,Kc

cc

cc

2222222

2111111

22

11

ρρρρρρ

ρρρρρρ

−+−−−+

+−+−−−=

o

o

K

KK (5.10)

5.2. Lagrange, Appell és Newton-Euler egyenletek Tekintsünk egy N tömegpontból álló rendszert. Legyen az l -edik tömegpont tö-mege lm és helyvektora lr . Legyenek az NNN xxxxxx 31323321 ,,,,,, −−K koordiná-ták révén adottak a helyvektorok egy rögzített ortonormált koordináta-rendszerben. Feltesszük, hogy a koordináták k holonóm és stacionárius korlátozásnak tesznek eleget, amelyek

krrf N ,,1,0),,( 1 KK == νν (5.11)

skalár egyenlet alakban adottak. A D'Alambert elv szerint a rendszer dinamikáját

NlRrmF llll ,,1,0 K&& ==+− (5.12)

írja le, ahol lF az lm tömegpontra ható eredő aktív erő és lR a korlátozásoknak megfelelő eredő reakció erő. A rendszer szabadságfoka kNn −= 3 . Ezért definiál-hatunk nqq ,,1 K független skalár paramétereket, az ú.n. általánosított koordinátá-kat, amelyek segítségével mindegyik lr helyvektor felírható

Nlqqrr nll ,,1),,,( 1 KK == (5.13)

alakban. A mozgásegyenletek levezetéséhez alkalmazzuk a virtuális munka elvét. Legyen

x a rendszer pozíciója a t időpillanatban. Ha x′ egy lehetséges másik pozíciója a rendszernek ugyanebben a t időpillanatban, akkor xxx −′=δ elmozdulás hatására a rendszer x -ből x′ -be hozható. A xδ elmozdulást virtuális elmozdulásnak nevez-zük, szemben a dt idő hatására bekövetkező dx elmozdulással, ahol az erők és kor-látozások megváltozhatnak. Adjunk a rendszernek virtuális elmozdulást, és szoroz-zuk meg az egyenleteket a hozzá tartozó lrδ -lel, akkor kapjuk, hogy


NlrRrmF lllll ,,1,0, K&& =>=+−< δ , (5.14)

és összegzés után

0,1

=>+−<∑=

N

llllll rRrmF δ&& . (5.15)

Feltételezzük, hogy a korlátozások ideálisak, ezért

∑=

>=<N

lll rR

10,δ , (5.16)

∑∑∑===

><=><⇔=>−<N

llll

N

lll

N

lllll rrmrFrrmF

111,,0, δδδ &&&& . (5.17)

Következik (5.13)-ból, hogy

i

N

l i

ll q

qr

r δδ ∑= ∂∂

=1

, (5.18)

i

n

i

n

i

N

l i

llli

N

l i

ll q

qr

rmqqr

F δδ∑ ∑ ∑∑= = ==

⎟⎟⎠

⎞⎜⎜⎝

⎛>

∂∂

<=⎟⎟⎠

⎞⎜⎜⎝

⎛>

∂∂

<1 1 11

,, && . (5.19)

Definiáljuk a iq általánosított koordinátához tartozó iQ általánosított erőt

∑=

>∂∂

<=N

l i

lli q

rFQ

1,: (5.20)

alapján. Mivel a iqδ virtuális elmozdulások függetlenek, ezért

∑=

>∂∂

<=N

l i

llli q

rrmQ

1,&& , (5.21)

ami felfogható egy absztrakt fizikai elvnek is.

5.2.1 Lagrange-egyenlet Az N tömegpontból álló rendszer kinetikus energiája

∑=

=N

lll .rm

1

2

21

&K (5.22)

Mivel (5.13) szerint


∑= ∂∂

=n

ii

i

ll q

qr

r1

&& és ,qr

qr

i

l

i

l

∂∂

=∂∂&

& (5.23)

∑ ∑= = ⎪⎭

⎪⎬⎫

⎪⎩

⎪⎨⎧

∂∂∂

+∂∂

=n

i

n

jji

ji

li

i

ll ,qq

qqr

qqr

r1 1

2&&&&&& (5.24)

ezért

∑∑==

>∂∂

<=>∂∂

<=∂∂ N

l i

lll

N

l i

lll

i qr

,rmqr

,rmq 11

&&

&&

K (5.25)

∑ ∑∑= ==

>∂∂

∂<+>

∂∂

<=∂∂ N

l

n

jj

ji

lll

N

l i

lll

iq

qqr

,rmqr

,rmqtd

d

1 1

2

1

&&&&&

K (5.26)

∑ ∑∑= ==

>∂∂

∂<=>

∂∂

<=∂∂ N

l

n

jj

ij

lll

N

l i

lll

iq

qqr

,rmqr

,rmq 1 1

2

1

&&&

&K (5.27)

Felhasználjuk, hogy sima függvény esetén a vegyes másodrendű parciális derivál-takra teljesül

ijji qq

xqq

x∂∂

∂=

∂∂∂ νν

22

,

ezért

i

N

l i

lll

iiQ

qr

,rmqqtd

d=>

∂∂

<=∂∂

−∂∂ ∑

=1

&&&

KK . (5.28)

Az összefüggés érvényes összekapcsolt rendszerek esetén is az integrál tulajdon-sága alapján. A iq általánosított koordinátához tartozó iQ általánosított erő fel-bontható a iτ meghajtó nyomatékra (erőre) és a )(qP potenciális energia hatására, amely utóbbi iq∂−∂P . Ezért bevezethetjük a K kinetikus energiát, a P potenciális energiát, az PKL −= Lagrange függvényt és a iτ meghajtó nyomatékot (erőt), amelynek segítségével a következő összefüggéshez jutunk:

iiiiii qqtqqqt

τ=∂∂

−∂∂

=∂∂

+∂∂

−∂∂ LLPKK

&& dd

dd Lagrange-egyenlet. (5.29)

Tekintsünk egy merev testet a hozzá rögzített koordináta-rendszerrel. Legyen a merev test origójának sebessége v és szögsebessége ω , akkor a merev test tetsző-leges ρ helyvektorú pontja esetén


[ ] [ ] ,2

2

>××<+>×<+><=

>=××<+>×<+><=

>=×+×+<>=<

ω,ωρρωρ,vv,v

ρρ,ωωρv,ωv,v

ρωvρ,ωv,vv

T

ρρ

és ezért a merev test K kinetikus energiájára teljesül

,,,21

21

>×<+><+><= ωρωω vmmv,v cKK (5.30)

ahol cρ a tömegközéppontja és K az inercia mátrixa a merev testnek, amely a me-rev testhez rögzített keret origójához tartozik. Ha a keret origója egybeesik a tömegközépponttal, azaz 0=cρ , akkor a merev test kinetikus energiája

><+><= ωω ,21

21

ccc m,vv KK , (5.31)

ahol cc vv ρω ×+= a tömegközéppont sebessége és cK a tömegközépponthoz tar-tozó inercia mátrix.

5.2.2 Appell-egyenlet A “gyorsulás” energia vagy Gibbs függvény N tömegpont esetén

∑=

=N

lll rm

1

2

21

&&G . (5.32)


,i

l

i

l

qr

qr

∂∂

=∂∂&&

&& (5.33)

és

∑∑==

>∂∂

<=>∂∂

<=∂∂ N

l i

lll

N

l i

lll

i qr

,rmqr

,rmq 11

&&&&

&&&&

&&

G , (5.34)

ezért (5.21) alapján a következő egyszerű alakú Appell-egyenletet kapjuk:

,ii

Qq

=∂∂&&

G (5.35)

vagy


iii qq

τ=∂∂

+∂∂ PG&&

. (5.36)

Tekintsük a ⟩⟨ ρρ ,aa kifejezést, akkor

,)][]([2

]][[22][][

)()(

L+>×××<−

−>××<+>×<+>××<+><=

>=××+×+××+×+<>=<

ωε,

aρ,aρ,εε,a,a

ωρε,aωρεa,aa

T

T

ρρ

ωρρ

ωωερρ

ρωρω

ahol a nem részletezett tagok nem függenek q&& -tól. Ezért vehetjük a következő egy-szerűsített Gibbs függvényt:

( )

( ) >××+×<+

+>×−<+><=

aam

ma,a

c ωωερ

εωωε

,

,221

21 KKG

(5.37)

Speciálisan, ha a merev testhez rögzített koordináta-rendszer origója a tömegközép-pontban van, akkor 0=cρ és

( ) >×−<+><= εωωε ,221

21

cccc m,aa KKG , (5.38)

ahol )( ccc aa ρωωρε ××+×+= a tömegközéppont gyorsulása és cK a tömeg-középponthoz tartozó inercia mátrix.

5.2.3 Newton–Euler-egyenlet Legyen 0K és K rendre egy inercia keret és egy merev testhez rögzített, vele együtt mozgó keret. Legyen p a 0K origójábol a K origójába mutató pozíció vek-tor. Merev test helyett először tömegpontokból álló rendszert tekintünk, lásd 5.3. ábra.

A tömegpontokból álló rendszerre alkalmazható (5.12) és vehető a következő összeg:

∑ ∑∑= ==

+=N

l

N

ll

N

llll RFrm

1 11

&& . (5.39)


5.3. ábra. Rögzített 0K és mozgó K keret

Alkalmazván a Newton és az Euler elvet feltehető, hogy a belső erők összege és

a belső erők bármely pontra vett nyomatékának összege nulla. Legyen a külső erők összege extF . Jelölje m és cr az eredő tömeget és a tömegközéppontot, akkor (5.39)-ből következik a Newton-egyenlet:

.extFrm c =&& (5.40)

Definiálja a perdületet (angular moment)

( )∑=

×−=N

llllK rmpr

1

&Π , (5.41

akkor

( ) ( )

( )

( ) ,rmprrmp

rmprrmp

rmprrmprtd

d

N

llllc

N

llll

N

lll

N

llll

N

llll

K

∑

∑∑

∑∑

=

==

==

×−+×−=

=×−+×−=

=×−+×−=

1

11

11

&&&&

&&&&

&&&&&Π

(5.42)

( ) .rmprrmptd

d N

llllc

K ∑=

×−=×+1

&&&&Π

(5.43)

0K

cr

r

p

ρ

K

0K

cr


Jelölje ext,KN a külső erők eredő nyomatékát:

( ) ,1

ext, ∑=

×−=N

lllK FprN (5.44)

akkor (5.12), a feltételek, valamint (5.43) és (5.44) alapján kapjuk, hogy

.ext,KcK Nrmp

tdd

=×+ &&Π

(5.45)

Merev test esetén

∫ ×=[s]

ρK mdvρΠ , (5.46)

ezért

( ) ( )

( ),v

vvvρωρρρ

ρωρρρωρρ

××−×=

=××+×=×+×=×

felhasználásával írhatjuk, hogy

ωρΠ K+×= vm cK . (5.47)

Felhasználván az integrál tulajdonságait, a mozgó koordináta-rendszerekben érvé-nyes differenciálási szabályt és a (5.45) összefüggést, kapjuk, hogy

( )ωωεΠ

KK ×++×+×= armvvmtd

dcc

K , (5.48)

( )

( ) ,am

mvvamvmvvmvtd

d

c

ccccK

×+×+=

=×+×++×+×=×+

ρωωε

ωωερΠ

KK

KK (5.49)

( ) .extK,c Nam =×+×+ ρωωε KK (5.50)

Speciálisan, ha a K keret origója a tömegközéppontban van, akkor 0=cρ , és a következő Euler-egyenletethez jutunk:

( ) .ext,cKcc N=×+ ωωε KK (5.51)

6. ROBOTOK DINAMIKUS MODELLJE 73

6. ROBOTOK DINAMIKUS MODELLJE Ebben a fejezetben bemutatjuk a robotok dinamikus modelljének meghatározását az Appell-egyenlet és a Lagrange-egyenlet felhasználásával. Az első különösen nume-rikus számításokra, a második pedig szimbólikus számításokra alkalmas. Ezen kívül a második módszer lehetőséget ad a különféle fizikai hatások szeparálására is. A di-namikus modell számítását tipikusan 1 ms gyakorisággal kell elvégezni a korszerű robotirányítási algoritmusokban.

6.1 A robot Appell-egyenleten alapuló dinamikus modellje Korábban bevezettük a “gyorsulás” energiát vagy Gibbs-függvényt. Ha a szegmens-hez rögzített keret origója a tömegközéppontban van, akkor a Gibbs-függvény

( ) >×−<+><= εε ω,ωKKm,aa cccc 221

21G (6.1)

ahol ca a tömegközéppont gyorsulása, ε és ω a szegmens szöggyorsulása és szög-sebessége, m a szegmens tömege és cK a tömegközéppontra vonatkozó inercia mátrix a szegmenshez rögzített koordináta-rendszerben. Feltesszük, hogy a koordi-náta-rendszer tengelyei párhuzamosak a Denavit–Hartenberg keret tengelyeivel, ezért használhatjuk a kinematikai mennyiségek számítására a (4.48a-e) és (4.49a-d) képleteket, továbbá feltehető, hogy a szegmens és a rászerelt motor inercia jellemző-it már összevontuk (5.10) szerint. Ha G a robot Gibbs-függvénye és P a potenciá-lis energiája, akkor az Appell-egyenlet alapján

miqq i

ii,...,2,1, ==

∂∂

+∂∂ τPG&&

(6.2)

ahol τ i az i -edik szegmenst meghajtó általánosított nyomaték. Mivel a robot Gibbs-függvénye a szegmensek Gibbs-függvényeinek összege, elegendő egy szeg-mens hatását tisztázni a dinamikus modellben és a hatásokat összegezni. Azért, hogy egyszerű összefüggést kapjunk, hozzáveszünk nulla oszlopokat csΩ -hez és sΓ -hez, ezáltal a mátrixok m×3 méretűek lesznek. Akkor


,ωKK,q

m,aqa

q,ωKK,

qK

mqa

,am,aqa

q

ss

ss

cssci

sscs

i

cs

i

ss

ss

scsscss

i

scs

si

cscsscs

i

cs

i

s

>×−∂∂

<+>∂∂

=<

>=∂∂

×−<+>∂∂

<+

+>∂∂

<+>∂∂

<=∂∂

ωεε

εωεε

ε

)(

)(221

21

21

21

&&&&

&&&&

&&&&&&

G

(6.3)

,ΩθqΩqq

aics,cscs

ii

cs =+∂∂

=∂∂

)( &&&&&&

(6.4)

,ΓqΓqq

εis,ss

ii

s =+∂∂

=∂∂

)( φ&&&&&&

(6.5)

ahol ics,Ω és siΓ rendre az i -edik oszlopa sΩ -nek és sΓ -nek. Irjuk fel a skalárszorzot mátrixszorzásos alakban, akkor

])([

)(

])()([)(

ss

ss

csscsT

is,

scsTcs,iscs

Tis,scs

Tcs,i

ss

ss

cssscsT

is,cscsT

ics,si

s

ωωKKΓ

mθΩqΓKΓmΩΩ

ωωKqΓKΓθqΩΩmq

×−+

+++=

=×−+++=∂∂

φ

φ

&&

&&&&&&

G

(6.6)

Összegezvén az egyes szegmensek hatását, a robot dinamikus modelljét kapjuk:

τ=++ )()()( qhqq,hqqH gcc &&& (6.7)

,ΓKΓmΩΩHm

sscs

Tsscs

Tcs∑

=

+=1

(6.8)

,ωωKKΓmθΩhm

ss

ss

scsscs

Tsscs

Tcscc ∑

=

×−+=1

])([ φ (6.9)

ahol cch a centripetális és Coriolis hatás és gh a gravitációs hatás.

Ha m egy tömegpont, amelynek helyvektora r a 0K keretben, akkor mgh po-tenciális energiája felírható

,mr

gr,gm T⎟⎟⎠

⎞⎜⎜⎝

⎛−>=<−

1)0( 00 (6.10)


alakban, ahol 0g a gravitációs gyorsulás a 0K keret bázisában kifejezve. Ha az s -edik szegmens tömegközéppontja csρ a sK Denavit–Hartenberg keretben, akkor a robot P potenciális energiája és hatása az Appell egyenletben:

∑=

⎟⎟⎠

⎞⎜⎜⎝

⎛−=

m

ss

css,

T ,mTg1

00 1)0(

ρP (6.11)

.m

qT

Tg

mq

Tg

qm

iss

cs

i

s,ii,

T

m

ss

cs

i

s,T

i

∑

∑

=

−−

=

⎟⎟⎠

⎞⎜⎜⎝

⎛∂

∂−=

=⎟⎟⎠

⎞⎜⎜⎝

⎛∂

∂−=

∂∂

1)0(

1)0(

1100

1

00

ρ

ρP

(6.12)


s,iii

s,i T∆q

T11

1−−

− =∂

∂,

ezért

,1

)0( 11100 ⎟⎟⎠

⎞⎜⎜⎝

⎛=⎟⎟

⎠

⎞⎜⎜⎝

⎛−=

∂∂ ∑

=−−−

i

iTi

csm

iss,isii,

T

i MR

GTmTgq

ρ∆P (6.13)

,∆TgG ii,TT

i 1100 )0( −−−= (6.14)

∑=

=m

issi mM , (6.15)

∑=

−− +=m

iss,icss,isi pAmR )( 11 ρ . (6.16)

Az eredeti Denavit–Hartenberg alakban 11 −− = ii kt a csuklótengely, ezért i) rotációs csukló esetén

,

0000000000010010

1

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=−i∆ (6.17)

,lm

T i,i,ii, ⎥

⎦

⎤⎢⎣

⎡ −= −−

−− 0000001010

110 ∆ (6.18)

( ),,,lg,mgG i,i,Ti 00100100 −− ⋅⋅−= (6.19)


ii) transzlációs csukló esetén

,

0000100000000000

1

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=−i∆ (6.20)

,n

T i,ii, ⎥

⎦

⎤⎢⎣

⎡= −

−− 0000000 10

110 ∆ (6.21)

( ).ng,,,G i,Ti 100000 −⋅−= (6.22)

Felhasználván, hogy

( ) ( )

( ) ,pmmpAmA

pAmpApAAmR

i,i

m

iss

m

isciisi,cssi,si,i

i,icii,ii

m

issi,i,ii,icssi,i,isi

11

1

111

111

−=+=

−

−−+=

−−−

⎟⎟⎠

⎞⎜⎜⎝

⎛+⎪⎭

⎪⎬⎫

⎪⎩

⎪⎨⎧

++=

=++++=

∑∑

∑

ρρ

ρρ

(6.23)

a következő hátratartó rekurziós algoritmust kapjuk iR és iM számítására:

,M,R mm 00 11 == ++ (6.24)

,1 iii mMM += + (6.25)

( ) .pMmRAR i,iiciiii,ii 111 −+− ++= ρ (6.26)

A gravitációs hatás a dinamikus modellben

.MR

Ghi

iTig,i ⎟⎟

⎠

⎞⎜⎜⎝

⎛= (6.27)

6.2 A robot Lagrange-egyenleten alapuló dinamikus mo-dellje Az F1. függelékben megmutattuk, hogy a robot szegmens kinetikus energiája, ha a koordináta-rendszer origója a tömegközéppontban van

><+><= ωω,Km,vv ccc 21

21K , (6.28)


ahol cK a keret origójára (a tömegközéppontra) vonatkozó tehetetlenségi mátrix. Feltesszük, hogy a keret tengelyei párhuzamosak a szegmenshez rögzített Denavit–Hartenberg keret tengelyeivel, ezért alkalmazható (4.46a-b) és (4.49c) a robot ki-netikus energiájának meghatározásakor:

><=

>=+<=

=><+><=

=><+><=

=⎭⎬⎫

⎩⎨⎧ ><+><=

∑

∑

∑

∑

=

=

=

=

q,qqH

q,qΓKΓmΩΩ

q,qΓKΓq,qmΩΩ

qΓ,qΓKmqΩ,qΩ

ω,ωKm,vv

m

sscs

Tsscs

Tcs

m

sscs

Tsscs

Tcs

m

ssscsscscs

m

ss

ss

scsscscs

&&

&&

&&&&

&&&&

)(21

21

21

21

21

21

1

1

1

1K

(6.29)

A robot kinetikus energiája a dinamikus modellben szereplő )(qH mátrix kvadrati-kus alakja a q& csuklósebességekben. Vezessük be a

mmjk qDH(q) ×= )]([ (6.30)

jelölést, akkor ][ jkDH = szimmetrikus és fizikai megfontolások alapján pozitív definit, továbbá

∑∑= =

=m

j

m

kkjjk qqD

1 1.

21

&&K (6.31)

Alkalmazzuk az (5.29) Lagrange-egyenletet:

.dd

iiii qqqt

τ=∂∂

+∂∂

−∂∂ PKK&

(6.32)

Mivel ][ jkDH = szimmetrikus, ezért ijji DD = és

,21

21

111∑∑∑===

=+=∂∂ m

jjij

m

kkik

m

jjji

iqDqDqD

q&&&

&

K (6.33)

.dd

1 11∑∑∑= == ∂

∂+=

∂∂ m

j

m

kkj

k

ijm

jjij

iqq

qD

qDqt

&&&&&

K (6.34)


Másrészt

,22

yxbayxbayxbyxa ++

+=+ (6.35)

ezért a második tag mindig szimmetrizálható:

.21

dd

1 11∑∑∑= ==

⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

+∂

∂+=

∂∂ m

j

m

kkj

j

ik

k

ijm

jjij

iqq

qD

qD

qDqt

&&&&&

K (6.36)

Hasonló módon kapjuk, hogy

∑∑= = ∂

∂=

∂∂ m

j

m

kkj

i

jk

iqq

qD

q 1 1,

21

&&K (6.37)

⎟⎟⎠

⎞⎜⎜⎝

⎛=

∂∂

=i

iTi

ii M

RG

qD P: . (6.38)

Ezért a robot dinamikus modellje Lagrange alakban

,,...,1,1 11

miDqqDqDm

jii

m

kkjijk

m

jjij ==++∑∑∑

= ==

τ&&&& (6.39)

,21

⎟⎟⎠

⎞⎜⎜⎝

⎛−+=

i

jk

j

ik

k

ijijk q

DqD

qD

D∂∂

∂∂

∂∂

(6.40)

ahol

iiD effektív inercia,

ijD csatoló inercia )( ji ≠ ,

ijjD centripetális hatás,

ijkD Coriolis hatás )( kj ≠ ,

iD gravitációs hatás.

A iijkij DDD ,, paraméterek függvényei q -nak, ezért a robot mozgása közben vál-toznak.

Vegyük észre, hogy ha ismert ijD szimbolikus (képletszerű) alakban, akkor

(6.40) alapján parciális deriválással meghatározható ijkD . Hasonló módon, ha is-mert a P potenciális energia szimbolikus alakban, akkor parciális deriválással meghatározható a iD gravitációs hatás is. Ilymódon az energiák képletszerű is-

6. ROBOTOK DINAMIKUS MODELLJE 79 merete esetén a dinamikus modell szimbolikus oprációval (parciális deriválással) képletszerű alakban is meghatározható.

6.3 A SCARA robot dinamikus modellje A 4-DOF RRTR SCARA robot Denavit–Hartenberg parametereit (lásd 6.1. táblá-zat) a 6.1. ábra koordináta-rendszerei alapján értelmeztük.

6.1. ábra. A SCARA robotnál használt keretek

6.1. táblázat. SCARA robot Denavit–Hartenberg parameterei

i qi ϑi di ai αi 1 ϑ1 ϑ1 d1 a1 0° 2 ϑ2 ϑ2 d2 a2 0° 3 d3 0° d3 0 0° 4 ϑ4 ϑ4 0 0 0°

Az egyes szegmensek közötti homogén transzformációk a következők:

,

100010000

1

1111

1111

1,0

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=dSaCSCaSC

T ,

100010000

2

2222

2222

2,1

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=d

SaCSCaSC

T (6.41)

0x 0y

0z1ϑ

1d

2d

2ϑ

1a

2a

1x 1y

1z2x 2y

2z

3d

43 , xx 43 , yy

43 , zz4ϑ

0x 0y

0z1ϑ

1d

2d

2ϑ

1a

2a

1x 1y

1z2x 2y

2z

3d

43 , xx 43 , yy

43 , zz4ϑ


,

1000100

00100001

33,2

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=d

T .

100001000000

44

44

4,3

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=CSSC

T (6.42)

A homogénszformációkat fokozatosan összeszorozzuk a parciális sebességek és szögsebességek és az eredő homogén transzformáció meghatározásához:

,d

CSSC

T ,

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

1000100

0000

3

44

44

42 ,dd

SaCSCaSC

T ,

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+

−

=

100010000

32

222424

222424

41 (6.43)

.dddSaSaCSCaCaSC

T ,

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

++++−

=

100010000

321

12211124124

12211124124

40 (6.44)

Itt 40,Tq a a direkt geometriai feladat megoldása. A qT , a40 inverz geometriai feladat a következő egyenletekre vezet:

a C a C px1 1 2 12+ = , (6.45) a S a S py1 1 2 12+ = , (6.46)

d d d pz1 2 3+ + = , (6.47) C lx124 = , (6.48) S ly124 = . (6.49)

A q1 csuklóváltozó meghatározásához végezzük el a következő átalakításokat:

,,

11122

11122

SapSaCapCa

y

x

−=−=

,22

,2222

21

221111

21

2111

221

2111

2212

22

212

22

aappSpaCpa

SaSpapCaCpapSaCa

yxyx

yyxx

−++=+

+−++−=+

ahol az utolsó egyenlet alakja DSBCA =+ αα , amelynek megoldása


,22

222

BADBAABD

S+

−++= α

αδ

(6.50)

.22

222

BADBABAD

C+

−++= α

αγ

(6.51)

Mivel teljesülnie kell, hogy 122 =+ αα CS , ezért csak két ),( αα CS megoldás van, amelyek a )1,1(),( −=αα γδ és )1,1(),( −=αα γδ értékpárokhoz tartoznak. Mivel

),( αα CS meghatározza a térnegyedet a síkon, ezért mindkét ),( αα CS párhoz egy-egy α megoldás van, amely

( )ααα CS ,2arctan= , (6.52)

feltéve hogy a

0222 ≥−+ DBA (6.53) munkatér feltétel teljesül.Választhatjuk azt a 1q megoldást, amely a megelőző 1q -hez legközelebb van.

Ha már 1q -et meghatároztuk, akkor

,2

1112 a

SapS y −= (6.54)

,2

1112 a

CapC x −= (6.55)

és innen ( ).,2arctan 121221 CSqq =+ (6.56) Az utolsó lépések nyílvánvalóak:

,213 ddpq z −−= (6.57) ( ).,2arctan421 xy llqqq =++ (6.58) A SCARA robot 4

4 J Jacobi mátrixa (4.22-25) segítségével határozható meg:


( ) ( )( ) ( )

.

CaCaCaSaSaSa

J

,d

,CaSa

CaCSaSCaSSaC

d

,CaCaSaSa

CaCaCSaSaSCaCaSSaSaC

d

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡++

=

=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛++−

=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛++

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛++++++−

=

10110000000001000000

0

00

00

4242241

4242241

44

34

42

42

22242224

22242224

14

42241

42241

1221112412211124

1221112412211124

04

(6.59)

Ha szükséges 4J° is, akkor alkalmazható (4.33). De a SCARA robot elég egyszerű (a csuklótengelyek párhuzamosak), ezért közvetlenül is írható, hogy

( )( ) ,

3

21122111

2112211140

4⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+++−−

==°q

qqCaqCaqqSaqSa

tdpd

v ,

&

&&&

&&&

(6.60)

,00

421

4⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

++=°

qqqω

&&&

(6.61)

.

10110000000001000000

12212211

12212211

4

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡+

−−−

=°

CaCaCaSaSaSa

J (6.62)

Ha 000 ,, zyx vvv és 0zω értéke elő van írva, akkor tekinthetők a nekik megfelelő egyenletek ,03 zvq =& (6.63) ,214 0 qqq z &&& −−=ω (6.64)


.0

0

12212211

12212211

2

1⎟⎟⎠

⎞⎜⎜⎝

⎛⎟⎟⎠

⎞⎜⎜⎝

⎛+

−−−=⎟⎟

⎠

⎞⎜⎜⎝

⎛

y

x

vv

CaCaCaSaSaSa

qq&

& (6.65)

A két utolsó egyenlet egyszerűen megoldható, mert

,11

⎟⎟⎠

⎞⎜⎜⎝

⎛−

−−

=⎟⎟⎠

⎞⎜⎜⎝

⎛−

acbd

bcaddcba

(6.66)

ahol

( ) ( )

( ) 0det

22142442421

42422414242241

==−==+−+=−=

SaaSCCSaaSaCaCaCaSaSacbda

(6.67)

definiálja a szinguláris konfigurációkat:

°= 02q vagy °=1802q , (6.68)

összhangban a 6.1. ábrával.

A (4.48a-e) és (4.49a-d) rekurzív algoritmussal a következő kinematikai mennyi-ségeket kapjuk ( iρ jelöli az i szegmens tömegközéppontját):

,0

,0

,0,100

211

11

111

1

111 qa

a y

x

cx

y

c &⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+

−==

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛= ρ

ρθρ

ρΩφΓ

(6.69)

,00

20

00

,0,110000

222

22

212

2221221

2212

2

222212

2221

2

22

qa

qqa

qSaCaa

aCaaSa

y

x

y

x

y

x

c

xx

yy

c

&&&&⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−

−−−=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+++

−−=

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

ρρ

ρρ

ρρ

θ

ρρρρ

Ω

φΓ

(6.70)


,00

20

,10000

,0,011000000

223

32

213

3221321

3212

3

323212

3321

3

33

qa

qqa

qSaCaa

aCaaSa

y

x

y

x

y

x

c

xx

yy

c

&&&&⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−

−−−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+++

−−=

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

ρρ

ρρ

ρρ

θ

ρρρρ

Ω

φΓ

(6.71)

,0

20

20

2

000

,0100

00

,0,101100000000

424

4

414

4

21442

442

244

422442

44221424142

4241424

4442424142

4442424142

4

44

qqqqqqSaCa

qqSaCa

qSaSaCaCa

CaCaCaSaSaSa

y

x

y

x

y

x

y

x

y

x

y

xc

xxx

yyy

c

&&&&&&

&&&

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−−

+⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−+−−−

=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+++

−−−+=

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

ρρ

ρρ

ρρ

ρρ

ρρ

ρρ

θ

ρρρρρρ

Ω

φΓ

(6.72)

A dinamikus modellt a kinematikai mennyiségek ismeretében az Appell egyen-lettel határozzuk meg. Figyelembe véve sΓ speciális alakját, a scs

Ts K ΓΓ szorzattól

származó tagok ijij DH = -ben egyszerűsíthetők:

[ ] .00

********

00 ,,,

,,

, zcszsjzsi

zsjzcs

zsi KK

ΓΓΓ

Γ =⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

(6.73)

Mivel sΓ oszlopai és sω párhuzamosak, és 0=sφ , ezért a második tag cch -ben nulla:


( )[ ] 0=×− sscsscsT

s KK ωωφΓ . (6.74)

Egyszerű számítások után ijD , ijkD és iD következő nemtriviális elemeihez ju-tunk:

,

D***DD**DDD*DDDD

H⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

44

3433

242322

14131211

(6.75)

,

D***DD**DDD*DDD

D⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

144

134133

124123122

114113112

1

0

(6.76)

,

D***DD**DD*DDD

D⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

=

244

234233

224223

214213112

2 00

(6.77)

,

D***D**DD*DDD

D⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−−−

=

344

334

324223

314213133

3

000

(6.78)

,

***D**DD*DDD

D⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−−−−−−

=

0000

334

324224

314214114

4 (6.79)

.

DDDD

D⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

4

3

2

1

(6.80)

A dinamikus modell egyszerű alakot ölt, ha bevezetjük a következő 111 ,, PP K pa-ramétereket és az 71 ,, FF K függvényeket(a c indexet elhagytuk csρ -ben és csK -ben):


,

)(

)(

)(

42

424

22

214

323

232

213

22

222

2212

12

112111

zyx

zyx

zxy

zxy

Kaam

Kaam

Kaam

KamP

+++++

++++++

++++++

++++=

ρρ

ρρ

ρρ

ρρ

,amP y4242 ρ=

,ρamP x4243 = ,ρamρamP yy 3132124 +=

,aamρaamρaamP xx 214321322125 )()( ++++= (6.81) ,ρamP y4146 =

,ρamP x4147 =

,Kρρam

KρρamKρρamP

zyx

zyxzyx

42

424

224

32

32

32322

22

2228

)()(

++++

++++++++=

,KρρmP zyx 42

42449 ++=

,mmP 4310 += ;)( 4311 zgmmP +=

,11 =F

42 SF = , ,43 CF = ,24 SF = (6.82) ,25 CF = ,246 SF = ;247 CF =

,222222 7766554433221111 PFPFPFPFPFPFPFD +−+−+−= ,22 7766554433228112 PFPFPFPFPFPFPFD +−+−+−=

,013 =D ,776633229114 PFPFPFPFPFD +−+−=

,22 33228122 PFPFPFD +−= (6.83) ,023 =D

,33229124 PFPFPFD +−= ,10133 PFD =


,034 =D ;9144 PFD =

,67764554112 PFPFPFPFD −−−−= ,0113 =D

,67762332114 PFPFPFPFD −−−−= ,67764554122 PFPFPFPFD −−−−=

,0123 =D (6.84) ,67762332124 PFPFPFPFD −−−−=

,0133 =D ,0134 =D

,67762332144 PFPFPFPFD −−−−=

,0213 =D ,2332214 PFPFD −−=

,0223 =D ,2332224 PFPFD −−= (6.85)

,0233 =D ,0234 =D

;2332244 PFPFD −−=

,0314 =D ,0324 =D (6.86) ,0334 =D ;0344 =D

,0444 =D (6.87)

,01 =D ,02 =D

,1113 PFD −= (6.88) .04 =D

A szegmens és a rászerelt motor inerciajellemzőit a rotor álló helyzetében össze-vontuk, de nem vettük figyelembe a forgó rotor kinetikus energiáját, amely


222

21

21

iiriiriri qK && νΘϕΘ == , ahol riΘ a rotor tehetetlenségi nyomatéka, iϕ& a rotor

szögsebessége és iii q&& /ϕν = az áttétel. Mivel

iirii

ri qq

Kdtd

&&&

2νΘ=∂∂

, (6.89)

ezért a beavatkozószerv hatása jó közelítéssel 2: iriiiii DD νΘ+= révén vehető figye-lembe. A surlódási veszteség az ii qf & viszkózus surlódással közelíthető, amelyet szintén a iτ meghajtó nyomatéknak kell fedeznie. Ezért néhány paraméter megvál-tozik, és a surlódás miatt további paraméterek lépnek be:

,: 21111 νΘ rPP +=

,: 2331010 νΘ rPP +=

,22212 νΘ rP =

,24413 νΘ rP = (6.90)

,114 fP = ,215 fP = ,316 fP = ;417 fP =

,: 122222 PDD += ,: 134444 PDD += (6.91) .: 13 iiii qPhh &++=

6.4 Kétszabadságfokú robotkar dinamikus modellje Egy kétszabadságfokú robotkar Denavit-Hartenberg paramétereit mutatja a 6.2. táb-lázat. A robotkar a vízszintes 00 , yx síkban mozog, a gravitációs gyorsulás 0y− irányú. A rotációs csuklók tengelyei az 00 , yx síkra merőlegesek és egymással pár-huzamosak.

6.2. táblázat. 2-DOF robotkar Denavit–Hartenberg parameterei

i qi ϑi di ai αi 1 ϑ1 ϑ1 0 1l 0° 2 ϑ2 ϑ2 0 2l 0°


A robotkar szegmenseinek tömegközéppontja 21 , cc ll távolságra van a csuklótenge-lyektől ix irányban. A szegmensek tömege 21 , mm , a tengelyirányú tehetetlenségi nyomaték a tömegközéppontban 21 , zz II . A dinamikus modellt a szimbolikus mód-szerrel vezetjük le. Először meghatározzuk a geometriai modellt, majd a szegmen-sek sebességét előbb az inerciarendszeren, majd a saját koordináta-rendszerben. Ezt követően meghatározzuk a ][ jkDH = általánosított inerciamátrixot és abból a cent-ripetális és Coriolis hatást, végül pedig a gravitációs hatást. Geometriai modell:

,

10000100

00

,

10000100

00

2222

2222

2,11111

1111

1,0

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=SlCSClSC

TSlCSClSC

T

(6.92)

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡++−

=

10000100

00

122111221

122111212

2,0SlSlCSClClSC

T

Sebességek és szögsebességek:

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=⇒

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛==⇒

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−==

0

0

0

0

011111

01,01

1111

111,0

10 lqlvAvqCl

Sl

dtdp

v T Ω&&

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+=⇒⎟⎟

⎠

⎞⎜⎜⎝

⎛

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+==

⎟⎟⎠

⎞⎜⎜⎝

⎛

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+

−−−=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+++−−

==

00

0

00

0

000)()(

2221

21

22

12221

21

20

2,022

2

112212211

12212211

21122111

211221112,0

20

llClSl

qq

llClSl

vAv

qq

ClClClSlSlSl

qqClqClqqSlqSl

dtdp

v

T Ω&

&

&

&&&&

&&&


⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

110000

,100

,00

0,

0

0

212221

21

211 ΓΓΩΩ ccccc llClSl

l (6.93)

Általánosított inerciamátrix:

⎥⎦

⎤⎢⎣

⎡∗

=

=⎥⎦

⎤⎢⎣

⎡+

⎥⎥⎦

⎤

⎢⎢⎣

⎡

++++

+⎥⎦

⎤⎢⎣

⎡+⎥

⎦

⎤⎢⎣

⎡=

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

∗∗∗∗∗∗∗∗

⎥⎦

⎤⎢⎣

⎡+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+⎥

⎦

⎤⎢⎣

⎡ +

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

∗∗∗∗∗∗∗∗

⎥⎦

⎤⎢⎣

⎡+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎦

⎤⎢⎣

⎡=

22

1211

22

2222

22221

222122221

211

1

21

1

22221

21

2

22121

1

111

)()(2

000

000

110000

100100

00

0

000

010000

000100

00000

00000

DDD

IIII

mlllCl

llCllClllIml

ImllCl

Sl

llClSl

Iml

lH

ccc

ccccc

ccc

c

cc

222222

22221212

2122122

212

21111

)()2(

IlmD

IllClmDIICllllmlmD

c

cc

ccc

+=

++=+++++=

(6.94)

Centripetális és Coriolis hatás:

⎥⎦

⎤⎢⎣

⎡∗

−=⎥

⎦

⎤⎢⎣

⎡∗

=00

,0 1122

122

1121 DD

DD

D

22121

22

2

12

2

12122

22121

12

1

12

2

11112

21

21

Sllmq

DqD

qD

D

Sllmq

Dq

DqD

D

c

c

−=⎟⎟⎠

⎞⎜⎜⎝

⎛∂∂

−∂∂

+∂∂

=

−=⎟⎟⎠

⎞⎜⎜⎝

⎛∂∂

−∂∂

+∂∂

=

(6.95)

Potenciális energia hatása:


12222

2

1221121111

1

122112111

)(

)(

CglmqPD

ClClgmCglmqPD

SlSlgmSglmP

c

cc

cc

=∂∂

=

++=∂∂

=

++=

(6.96)

A 2-DOF robotkar dinamikus modellje:

22122121122221212

121121212221211222121211

),()()(

),()()(2)()(

τ

τ

=+−+

=++++

qqDqqDqDqqD

qqDqqDqqqDqqDqqD

&&&&&

&&&&&&& (6.97)

6.5 Identifikációs megfontolások A robot dinamikus modellje felírható jelek és független paraméterek szorzataként is

τα =),,,( zzqqY &&&& alakban, amely a független paraméterek identifikációjának alapját képezi. Az identifikáció önhangoló adaptív irányítás keretében valósítható meg sta-bilitásgaranciák mellett. A zz &&&, jelek helyére a szabályozás megfelelően választott referenciajelének rr qq &&& , deriváltjai kerülnek. A 2-DOF robotkar esetén a független paraméterek, valamint az önhangoló adaptív irányítás alapját képező modell alak a következő. Független paraméterek:

225

12114

22223

2122

2122

212

2111

)(

)(

c

c

c

c

cc

lgmlmlmg

Ilm

llmIIllmlm

=+=

+=

=++++=

ααα

αα

(6.98)

Dinamikus modell függvényei kifejezve a független paraméterekkel:

322

22312

22111 2

ααααα

=+=+=

DCDCD


5122

512411

22122

22112

αα

αα

CDaCCD

SDSD

=+=

−=−=

(6.99)

A dinamikus modell felírása jelek és paraméterek szorzataként:

iikk k j

jijkkik DzqDzD τ=++∑ ∑ ∑ &&&& )( (6.100)

1225

24

2123

1121222

21

1215

114

213

222121221212

111

2

1

5

4

3

2

1

2524232221

1514131211

,0,

,,0

,,

),()2(,

CYY

zzYzqSzCY

YCYCYzY

zqqzzqSzzCYzY

YYYYYYYYYY

==

+=+=

====

++−+==

⎟⎟⎠

⎞⎜⎜⎝

⎛=

⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜

⎝

⎛

⎥⎦

⎤⎢⎣

⎡

&&&&

&&&&

&&

&&&&&&&&&&

&&

ττ

ααααα

(6.101)

7. GÉPJÁRMŰ DINAMIKUS MODELLJE 93

7. GÉPJÁRMŰ DINAMIKUS MODELLJE Tekintsük a vízszintes síkban mozgó gépjármű egyszerűsített vázlatát a 7.1. ábra szerint. A hátsó kerekek meghajtottak és az első kerekek kormányozhatók.

7.1. ábra. Vízszintes síkban mozgó gépjármű egyszerűsített vázlata

A jármű (vehicle) dinamikus modelljének felállításakor az első kerekekre (front

wheels) F betűvel, a hátsó kerekre (rear wheels) R betűvel fogunk hivatkozni. A bal oldali kerekeket (left wheels) L , a jobb oldali kerekeket (right wheels) R jelöli. A dinamikus modell felállításakor szerepet játszik a jármű tömegközéppontjába he-lyezett CoGK (center of gravity) koordináta-rendszer, a kerekekhez rendelt WK (wheel) koordináta-rendszerek és a InK (inertial) koordináta-rendszer.

Az egyszerűsített jármű dinamikus modell feltételezi, hogy a járműnek csak az első kereke kormányzott és a jármű bal oldala és jobb oldala szimmetrikus felépítésű és viselkedésű, ezért a két fél összevonható egyetlen egységgé a dinamikus modell-


ben. Az CoGx tengely legyen a jármű hossztengelye (longitudinal axle), az CoGy tengely mutasson ettől balra és a CoGz tengely mutasson felfelé. A kerekeknek a tá-volságát a tömegközépponttól jelölje rendre Fl és Rl . Az első kerék WK koordiná-ta-rendszerének origója legyen az CoGx tengelyen, az Wx tengely legyen a kerék síkjában és a kerék elfordulási szöge (az Wx és CoGx közötti szög) legyen Wδ . A tömegközéppont szögsebességét jelölje ψ& . A jármű tömegközéppontjának sebesség vektora legyen CoGv , a sebesség vektornak az CoGx tengellyel bezárt szöge β , és a sebesség vektor abszolút értékét jelölje Gv . Az első kerék sebesség vektorát jelölje

WFv és a sebesség vektornak az Wx tengellyel bezárt szögét Fα (ekkor WFv az

CoGx tengellyel FW αδ − szöget zár be). A hátsó kerék sebesség vektorát jelölje

WRv és a sebesség vektornak az CoGx tengellyel bezárt szögét Rα .

A RF ααβ ,, szögek szokásos elnevezése rendre a jármű oldalcsúszási szöge (vehicle body side slip angle), az első gumiabroncs oldalcsúszási szöge (tyre slide slip angle front), a hátsó gumiabroncs oldalcsúszási szöge (tyre slide slip angle rear).

Ha az első kereket a jármű hossztengelyéhez ( CoGx ) képest balfelé kormányozzuk, akkor a CoGv és WFv sebesség vektorok CoGx bal oldalán, a WRv sebesség vektor pedig CoGx jobb oldalán lesznek. A kerék elfordulási szöget és az oldalcsúszási szögeket ekkor pozitív irányúnak tekintjük.

Az első kerékre lFF longitudinális és tFF transzverzális erő hat az első kerék koordináta-rendszerének origójában, és feltesszük, hogy a transzverzális komponens

FFtF cF α= , ahol Fc konstans. Hasonlóan a hátsó kerékre az lRF longitudinális és az tRF transzverzális erő hat a hátsó kerék koordináta-rendszerének origójában, és a transzverzális komponens RRtR cF α= , ahol Rc konstans.

Az első kerék sebessége a jármű tömegközéppontjának sebessége megnövelve a jármű ψ& szögsebességének hatásával, amely azonos az Fl sugáron ψ& szögsebes-séggel körpályán mozgó pont sebességével. Tekintsük az első kerék sebességének

CoGy és CoGx irányú komponenseit, akkor a szögek pozitív irányának fenti értel-mezése mellett

)sin()sin( βψαδ GFFWWF vlv +=− & (7.1a) )cos()cos( βαδ GFWWF vv =− , (7.1b)

ahonnan következik


)cos()sin(

)tan(β

βψαδ

G

GFFW v

vl +=−

&. (7.2)

Hasonlóan a hátsó kerék sebességének CoGy és CoGx irányú komponensei a po-zitív szögirányok figyelembevételével rendre

)sin()sin( βψα GRRWR vlv −= & (7.3a) )cos()cos( βα GRWR vv = , (7.3b)

ahonnan következik

)cos()sin(

)tan(β

βψα

G

GFR v

vl −=

&. (7.4)

Kis FW αδ − , Rα és β szögeket feltételezve FWFW αδαδ −≈− )tan( ,

RR αα ≈)tan( , ββ ≈)sin( és 1)cos( ≈β közelítéssel írható

G

FWF v

l ψβδα

&−−= (7.5)

G

RR v

l ψβα

&+−= . (7.6)

Alkalmazzuk a korábban már megismert ββ ββ SC == )sin(,)cos( egyszerű-

sített jelöléseket, akkor a 7.1. ábra és a deriválási szabály szerint mozgó koordináta-rendszerben meg tudjuk adni a kinematikai, majd a dinamikus modelljét a járműnek.

GCOG vSC

v⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

0β

β

(7.7)


GG

G

G

GGGCOGCOGCOG

vCS

vCvSSvC

vSC

vCS

vSC

vva

ψβ

ψψ

βω

β

β

ββ

ββ

β

β

β

β

β

β

&&

&

&

&

&&&

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−+⎟⎟

⎠

⎞⎜⎜⎝

⎛

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −+

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−+

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=×+=

000

00000000

00

(7.8) Az erők irányának figyelembevételével a haladó mozgás egyenlete a jármű vm tö-megével átosztva, és COGa -nek csak a nem triviális első két komponensét tekintve:

⎟⎟⎠

⎞⎜⎜⎝

⎛+++−

=⎟⎟⎠

⎞⎜⎜⎝

⎛=

tRtFlF

lRtFlF

vy

x

vCOG FCFSF

FSFCFmF

Fm

aww

ww

δδ

δδ11 (7.9)

Közbenső lépések:

⎥⎦

⎤⎢⎣

⎡−

=⎥⎦

⎤⎢⎣

⎡−

−−

=⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡ − −−

ββ

ββ

ββ

ββ

CSSvCv

vacbd

bcaddcba

CvSSvC GG

GG

G 11:11

44444 344444 21

&&

&

&ψ

ψβ β

β

ββ

ββ

ββ

ββ

⎟⎟⎠

⎞⎜⎜⎝

⎛−

⎟⎟⎠

⎞⎜⎜⎝

⎛−⎥⎦

⎤⎢⎣

⎡−

+⎟⎟⎠

⎞⎜⎜⎝

⎛⎥⎦

⎤⎢⎣

⎡−

=⎟⎟⎠

⎞⎜⎜⎝

⎛

10

111G

GG

Gy

x

v

GG

G

G vCS

CSSvCv

vFF

mCSSvCv

vv

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

+−+

++−=

=⎟⎟⎠

⎞⎜⎜⎝

⎛+++−

⎥⎦

⎤⎢⎣

⎡−

−−

−−

][11

11

βββδβδ

βββδβδ

δδ

δδ

ββ

ββ

CFSFCFSFv

SFCFSFCF

m

FCFSFFSFCF

mCSSvCv

v

tRlRtFlFG

tRlRtFlF

v

tRtFlF

lRtFlF

v

GG

G

ww

ww

ww

ww

Vegyük figyelembe, hogy


⎟⎟⎠

⎞⎜⎜⎝

⎛+−==

⎟⎟⎠

⎞⎜⎜⎝

⎛−−==

G

RRRRtR

G

FwFFFtF

vlccF

vlccF

ψβα

ψβδα

&

&

(7.10)

A haladó mozgás differenciálegyenlete:

)cos()sin(

)sin()cos(1

ββψ

β

βδψ

βδβδ

lRG

RR

wG

FwFwlF

vG

Fvl

c

vl

cFm

v

+⎟⎟⎠

⎞⎜⎜⎝

⎛+−+

+−⎟⎟⎠

⎞⎜⎜⎝

⎛−−−−=

&

&&

(7.11)

)cos()()cos()(

)sin()sin(1

βψ

ββδψ

βδ

ββδψβ

G

RRW

G

FWF

lRWlFGv

vl

cv

lc

FFvm

&&

&&

+−+−−−+

+−−+−=

(7.12) A forgatónyomaték: tRRwtFWlFFz FlFFl −++= )90sin()sin( δδτ o A forgó mozgás differenciálegyenlete :

)cos(

)cos()sin(11

βψ

β

δψ

βδδτψ

⎟⎟⎠

⎞⎜⎜⎝

⎛+−−

−⎟⎟⎠

⎞⎜⎜⎝

⎛−−+==

G

RRR

wG

FwFFWlFF

zz

z

vl

cl

vl

clFlII

&

&&&

(7.13)

Állapotegyenlet:

),( uxfx =& nemlineáris, tipikusan 0=lFF T

GT

lRwT

G vyFuvx ),(,),(,),,,( ψδψψβ === & (7.14) Egyszerűsített linearizált modellhez jutunk, ha feltehető, hogy Gv konstans és a szö-gek kicsik. Egyszerűsített linearizált modell állapotegyenlete:


CxyBuAxx =+= ,& , ψδψβ && === yux WT ,,),( (7.15)

BuAx

Ilcvm

c

vIlclc

Ilclc

vmlclc

vmcc

W

zz

FF

Gv

F

Gzz

FFRR

zz

FFRR

Gv

FFRR

Gv

RF

+=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+⎟⎟⎠

⎞⎜⎜⎝

⎛

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+−

−

−−+

−=⎟⎟

⎠

⎞⎜⎜⎝

⎛δ

ψβ

ψβ

&&&

&22

2 1

(7.16) A linearizált modell 0)det( =− AsI karakterisztikus egyenlete:

0)())(()()(

2

22222 =

−++++

++++

zzGv

FFRRGvRFRF

zzGv

RvzzRFvzzF

Ivmlclcvmllcc

sIvm

lmIclmIcs

Az egyszerűsített linearizált modell stabil, ha FFRR lclc > , ami a gyakorlatban

elterjedt járművek esetén rendszerint teljesül. Irodalmi adatok alapján az egyszerűsí-tett linearizált modell g4.0 -nél kisebb laterális járműgyorsulás tartományban kielé-gítő pontosságú.

Az egyszerűsített linearizált modell )0(, WWAstat δψ&= statikus átviteli tényezője a

BAsICsWW

1, )()( −−=δψ& átviteli függvény kiértékelésével képezhető 0=s esetén:

.)())((

)(22

FFRRGvRFRF

RFRFGstat lclcvmllcc

llccvA

−+++

+= (7.17)

A gyakorlatban bevezetik a charv karakterisztikus sebességet, ahol

)()(

:2

2

FFRRv

RfRFchar lclcm

llccv

−

+= (7.18)

( 2charv negatív is lehet, ha 0<− FFRR lclc ), amely szoros kapcsolatban van a stati-

kus átviteli tényezővel:

22 /11

charG

G

RFstat vv

vll

A+

⋅+

= . (7.19)

Tipikus járműveknél charv értéke 68 és 112 km/h között van. A kis karakterisztikus sebesség arra utal, hogy a jármű alulkormányozott.

robotokdinamikusmodellje.pdf

Documents