51

ROBOTOK DINAMIKUS MODELLJE

4. A ROBOT DIFFERENCILIS MOZGSA
Ebben a fejezetben egyik clunk a nemlineris geometriai modell loklis
linearizlsa s az inverz geometriai feladat lecserlse inverz sebessg feladatra, s
a feladat megoldsa redundns robotok s hinyz szabadsgfokok esetn. A loklis
linearizci annak meghatrozst jelenti, hogy a robot csuklvltozinak sebessge
hogyan befolysolja a megfog sebessgt s szgsebessgt. Msik clunk elretart rekurzv sszefggsek levezetse az egyes szegmensek szgsebessgnek,
szggyorsulsnak, sebessgnek s gyorsulsnak szmtsra. Ezek az sszefggsek a dinamikus modell numerikus szmtsnak alapjt fogjk kpezni. Mindkt
feladathoz fontos annak tisztzsa, hogyan kell a sebessget s gyorsulst szmtani
ll (inercia) s mozg koordinta-rendszerekben. Nem szabad ugyanis elfelejteni,
hogy a Newton-Euler trvnyek inerciarendszerekben rvnyesek.

4.1 Differencils mozg koordinta-rendszerekben

Legyen K 0 egy ll derkszg koordinta-rendszer, vagy egy olyan inercia derkszg koordinta-rendszer, amelyben K 0 origja (lnyegben) ll vagy konstans
sebessggel mozog, de az i 0 , j 0 , k 0 tengelyirny egysgvektorok derivltja az id
szerint nulla, pldul a tengelyek irnya a csillagokhoz kpest ll, ezrt llandnak
tekinthet. Legyen tovbb K egy mozg koordinta-rendszer. Fejezzk ki K egysgvektorait K 0 bzisban (lsd 4.1. bra):
i = a11i 0 + a 21 j 0 + a 31 k 0 ,
j = a12 i 0 + a 22 j 0 + a 32 k 0 ,

(4.1)

k = a13 i 0 + a 23 j 0 + a 33 k 0 .

k0

r
p
i0

K0

j0

j
K

4.1. bra. K 0 rgztett s K mozg koordinta-rendszer

52

Lantos: ROBOTOK DINAMIKUS MODELLJE

Kpezzk K egysgvektorainak id szerinti derivltjait, s hasznljuk ki, hogy

K 0 egysgvektorai idben llandak:
i 0 + a 21 j 0 + a 31
k0 ,
i = a11
i 0 + a 22 j 0 + a 32
k0 ,
j = a12
i 0 + a 23
j 0 + a 33
k0.
k = a13

(4.2)

Mivel A := AK 0 K s A 1 = AK , K 0 = AT , ezrt
i 0 = a11i + a12 j + a13 k,
j 0 = a 21i + a 22 j + a 23 k,

(4.3)

k 0 = a 31i + a 32 j + a 33 k.

(a11i + a12 j + a13 k ) + a 21 (a 21i + a 22 j + a 23 k ) +

i = a11
(a 31i + a 32 j + a 33 k ) =
+ a 31

+ a 21 a 21
+ a 31 a 31
)i + (a12 a11
+ a 22 a 21
+ a 32 a 31
)j +
= (a11 a11
+ a 23 a 21
+ a 33 a 31
)k .
+(a13 a11

(4.4)

Vegyk szre, hogy az i, j , k vektorok eltt ll tnyezk a kvetkez szorzat

oszlopvektornak komponensei:
a11
a
12
a13

a 21
a 22
a 23

a 31 a11
a11
a11

T
1
= A a 21
= A a 21
.
a 32 a 21
a
a

a 33 a 31
31
31

(4.5)

Msrszt alkalmazhat a Rodriguez formula i + d i, j + d j, k + d k szmra. Figyelembe vve, hogy d kicsi, ezrt pldul i esetn rhat, hogy
i + d i = i + d t i .

(4.6)

Definiljuk az szgsebessget a kvetkez mdon:

:=
akkor

t d
= x i + y j + z k = x i 0 + y j 0 + z k 0 ,
dt

(4.7)

4. A ROBOT DIFFERENCILIS MOZGSA

53

i = i,
j = j,

(4.8)

k = k,
[] = A 1 A s [ 0 ] = AA 1 .

(4.9)

Tekintsk most a P pontot, akkor r pozcija, r sebessge s r gyorsulsa

rendre
rx i0 + ry j 0 + rz k 0 = x i + y j + z k + p x i 0 + p y j 0 + p z k 0
rx i 0 + ry j 0 + rz k 0 = x i + y j + z k + x i + y j + z k +
+ p x i 0 + p y j 0 + p z k 0
rxi 0 + ry j 0 + rzk 0 = x i + y j + z k + 2 ( x i + y j + z k ) +
+ x i + y j + z k + p x i 0 + p y j 0 + p z k 0 .

Vezessk be a kvetkez jellseket:

r = (rx ,ry ,rz )T ,

r = (rx ,ry ,rz )T ,

K,

p = (p x , p y , p z ), akkor

r = A + p ,
r = A + A + p ,
r = A + 2 A + Ar + p ,

(4.10)

ahol r s p koordinti K 0 -ban, koordinti pedig K -ban vannak kifejezve.

Mivel AA 1 = I AA 1 + A(A 1 ) = 0 (A 1 ) = A 1 AA 1 , ezrt
[ ] = ( A 1 A) = ( A 1 ) A + A 1 A = A 1 AA 1 A + A 1 A =
= [ ][ ] + A 1 A,

:= xi + y j + z k + xi + y j + z k = xi + y j + z k +
= xi + y j + z k ,

(4.11)

(4.12)

ahol a szggyorsuls. A P pont r sebessge s r gyorsulsa kifejezhet a

mozg K koordinta-rendszerben is:
A 1 r = + A 1A + A 1 p = + + A 1 p ,
A 1 r = + 2 A 1A + A 1A + A 1 p =
= + 2 + + ( ) + A 1 p .

(4.13)
(4.14)

54

Lantos: ROBOTOK DINAMIKUS MODELLJE

Legyen v K := A 1 p , a K := A 1 p , K := s K := rendre K origjnak

sebessge, gyorsulsa, szgsebessge s szggyorsulsa, akkor a P pont v sebessgre s a gyorsulsra teljesl

v = vK + K + ,

(4.15)

a = a K + K + K ( K ) + + 2 K .

(4.16)

Specilisan, ha P egy merev test pontja, akkor = = 0 , = K , = K ,

s az sszefggs egyszerbb alakot lt.

4.2 A geometriai modell loklis linearizlsa

A robot geometriai modellje egy nemlineris fggvnykapcsolat a csuklvltozk s
a megfog pozcija s orientcija kztt. A kapcsolatot loklisan, az aktulis q
konfigurciban linearizlva eljuthatunk a robot J (q ) Jacobi mtrixhoz. A
linearizls differencilshoz ktdik, ami viszont felfoghat a szuperpozci elve
alkalmazsnak is. Fagyasszuk be minden csuklvltoz aktulis rtkt q i kivtelvel, s adjunk q i -nek dq i vltozst. Akkor az i , i + 1 , K , m szegmensek

egyetlen merev testet alkotnak, amely a t i 1 csukltengely tpustl fggen elmozdul a tengely mentn dq i -vel (T ) , vagy elfordul a tengely krl dq i -vel ( R) .
Utbbi esetben K m origjbl merlegest bocsjthatunk t i 1 -re, vehetjk a talppont
s K m origja kztti tvolsgot, s ezzel a sugrral fog K m origja krmozgst
vgezni, amelynek szgsebessge q& i , az orig sebessgnek irnya pedig merleges
a t i 1 s p i 1,m ltal meghatrozott skra. Vezessnk be egy i maszk vltozt,
amelyre i = 1 rotcis csukl esetn ( R) , s i = 0 transzlcis csukl esetn
(T ) . Jellje K m origjnak sebessgt s szgsebessgt rendre
akkor a szuperpozci elve szerint
m

v m s

m ,

v m = m d i 1 q& i ,

(4.17)

i =1

m = m t i 1 q& i .

(4.18)

i =1

Az

d i 1 parcilis sebessg s m t i 1 parcilis szgsebessg rendre a kvetkez:

4. A ROBOT DIFFERENCILIS MOZGSA

m

55

d i 1 = Ai11, m {(1 i )t i 1 + i t i 1 p i 1,m } ,

(4.19)

t i 1 = Ai11, m i t i 1 .

(4.20)

Azrt, hogy az eredmnyt egyszerbb tegyk, hagyjuk el Ti 1,m -ben az indexeket:

l m n
Ti 1,m =
0 0 0

p A
=
1 0 T

p
.
1

(4.21)

Ha zi 1 a csukltengely, mint az eredeti DenavitHartenberg alak esetn, akkor

t i 1 = k i 1 , s ezrt A 1 = AT miatt a kvetkez eredmnyekhez jutunk:
Rotcis csukl (R) esetn:

t i 1,x = l t i 1 = l k i 1 = l z ,

t i 1, y = m t i 1 = m k i 1 = m z ,

t i 1,z = n t i 1 = n k i 1 = n z ,

(4.22)

d i 1,x = l (t i 1 p ) = l (k i 1 p ) =
py
0 1 0 p x

= l 1 0 0 p y = l p x = l x p y + l y p x ,
0
0 0 0 p

d i 1, y = m (t i 1 p ) = m (k i 1 p ) = m x p y + m y p x ,

d i 1,z = n (t i 1 p ) = n (k i 1 p ) = n x p y + n y p x .

(4.23)

Transzlcis csukl (T ) esetn:

m

t i 1 = 0,

d i 1,x = l t i 1 = l k i 1 = l z ,

d i 1, y = m t i 1 = m k i 1 = m z ,

d i 1,z = n t i 1 = n k i 1 = n z .

(4.24)

(4.25)

Bevezetve a
[ t ] (1 i ) t i 1
i 1 = i iT1
,
0
0

(4.26a)

56
m

Lantos: ROBOTOK DINAMIKUS MODELLJE

mt
i 1 = i T1
0

d i 1

(4.26b)

jellseket, rhat hogy

Ti 1,m
q i
m

(4.27)

i 1 = Ti 11,m i 1 Ti 1,m .

(4.28)

= i 1Ti 1,m = Ti 1,m i 1 ,

Az eredmnyek tmr alakra hozhatk, ha bevezetjk a robot

rixt:
m

m d m d ... m d
J m = m 0 m 1 m m 1 ,
t 0 t1 ... t m 1
m vm m

&
m = J m q .
m

J m Jacobi mt-

(4.29)
(4.30)

Mivel a rgztett K 0 keretben

vm = A0 ,m m vm ,

(4.31)

m = A0 ,m m m ,

(4.32)

ezrt
0 m
A0 ,m
J ,
J m =
A0 ,m m
0
vm

= J m q& .
m

(4.33)
(4.34)

Kvetkezmny: Olyan esetekben, amikor az inverz geometriai feladat nem oldhat

meg analitikusan, meghatrozhatjuk a (4.30) vagy (4.34) egyenlet q& (t ) megoldst
a q konfigurci krnyezetben, s ebbl numerikus integrlssal q(t ) rtkt. Ha
teht a megfog sebessgt s szgsebessgt rjuk el a pozci s orientci helyett, akkor q& meghatrozhat a Jacobi mtrix J m# pszeudoinverze segtsgvel:
v
q& = J m# m .
m

(4.35)

4. A ROBOT DIFFERENCILIS MOZGSA

57

Ha m=6 s J m nemszingulris, akkor J m# = J m1. Azon q robot konfigurcikat,

amelyek esetn J m kptere nem a teljes R 6 tr (dim range ( J m ) < 6) , szingulris
konfigurciknak nevezzk. Szingulris konfigurcikban vannak olyan irnyok,
amelyek mentn vagy amelyek krl a robot nem mozgathat.

4.2.1 Redundns szabadsgfokok esete

Ha m > 6 , akkor a robot redundns szabadsgfokokkal rendelkezik, s clszer a
vgtelen sok megolds kzl olyat kivlasztani, amelyre q& min , mert ehhez tartozik a legrvidebb tranziens. Az optimlis megolds ekkor

q& = J mT J m J mT

v
m .
m

(4.36)

Az sszefggst a kvetkez matematikai problma keretben vezetjk le.

Sma:
Anm ,

n < m, x R m , y R n , Ax = y ,

min

Lagrange multipliktor szably:

x

=< x, x > konvex funkcionl

Ax y = 0 lineris korltozs
F =< x, x > + < , Ax y >
Fx = 0 x opt

Alkalmazs:
F =< x, x > + < AT , x > < , y >
Fx = 2 x + AT = 0 x =

1 T
A
2

A AT = y = 2( AAT ) 1 y
2

x opt = AT ( AAT ) 1 y

Vegyk szre, hogy AAT mr egy n n mret kvadratikus mtrix, s ennek a

kznsges inverzre van szksg.

58

Lantos: ROBOTOK DINAMIKUS MODELLJE

4.2.2 Hinyz szabadsgfokok esete

Ha m < 6 , akkor az sszes sebessg s szgsebessg elrst nem lehet egyidejleg
kielgteni, mert kevesebb vltoz van, mint felttel. Ekkor kt lehetsg knlkozik:
i) Kivlaszthatunk v m s m komponensei kzl pontosan annyit, amennyi
dim q& . Az ezekhez tartoz egyenleteket betartjuk, a tbbit figyelmen kvl hagyjuk. Pldul ha dim q& = 2 s p x , p y csak q1 , q 2 -tl fgg, akkor vlaszthat v x
s v y , s a hozzjuk tartoz kt egyenletbl meghatrozhat q&1 s q& 2 .
ii) Vlaszthatunk olyan q& csuklsebessget, amely szimultn minimalizlja az
egyenletek hibjt LS rtelemben, amikor is a kompromisszum megolds

q& = J mT J m

v
J mT m .
m

(4.37)

Az sszefggst a kvetkez matematikai problma keretben vezetjk le.

Sma:
Anm ,

n > m, x R m , y R n ,

Ax y

min

Gradiens egyenl nulla felttel:

F =< Ax y , Ax y > konvex funkcionl
Fx = 0 x opt

Alkalmazs:
F =< Ax y, Ax y >=< Ax, Ax > 2 < Ax, y > + < y, y >=
=< AT Ax, x > 2 < AT y, x > + < y , y >
Fx = 2 AT Ax 2 AT y = 0
x opt = ( AT A) 1 AT y
Vegyk szre, hogy AT A mr egy m m mret kvadratikus mtrix, s ennek a
kznsges inverzre van szksg.

4. A ROBOT DIFFERENCILIS MOZGSA

59

4.2.3 Dekompozcis lehetsgek

Fontossga miatt rszletesebben vizsgljuk a Jacobi mtrix invertlst abban a specilis 6DOF esetben, amikor az utols 3 tengely rotcis s a tengelyek egy kzs
(6 ) pontban metszik egymst:

TB,E = TB,0 T0 ,3

T6*,E

1
0

0
1
0
=
0

A
3,6

0 1 d 4 0T

0 0 1

0 0
1 0

0
0

0
T6*,E ,
1

0
0
T6 ,E .
0 1 d6

0 0 1

0 0
1 0

(4.38)

(4.39)

Feltesszk, hogy a vgberendezs B vE sebessge s B E szgsebessge a robot

K B bziskeretben adott. Akkor q& a kvetkez lpsekben hatrozhat meg:
i) A sebessg s szgsebessg kifejezhet K E -ben:
E

1 B
vE = AB,E
vB,E ;

1 B
E = AB,E
E .

(4.40)

ii) Meghatrozhat K 6* origjnak sebessge s szgsebessge:

6*

v 6* = A6*,E { E v E + E E ( A6*,1E p 6*,E )} ,

6*

6* = A6*,E E .

(4.41)
(4.42)

iii) Mivel 6* d i 1 = 0, i = 4 ,5,6* , ezrt A Jacobi mtrix specilis blokk alshromszgmtrix

6*

A 0
J 6* =
.
B C

(4.43)

Legyen qA = (q1,q2 ,q3 )T , qB = (q4 ,q5 ,q6 )T , akkor az inverz sebessg problma kt
rszfeladatra bonthat, amelyben mr csak 3 3 -as mtrixok zrt alakban szmthat inverzei szerepelnek:
q& A = A 1
1

6*

1 6*

q& B = C { B A

v 6* ,

(4.44)

v 6* + 6* } .
6*

(4.45)

60

Lantos: ROBOTOK DINAMIKUS MODELLJE

4.3 A kinematikai mennyisgek rekurzv szmtsa

A robot dinamikus modelljt Hq&& + h = alakra fogjuk hozni, ezrt ehhez hasonlan
paramterezzk a szegmensek kinematikai mennyisgeit is:

i = i q& ,

(4.46a)

vi = i q& ,

(4.46b)

i = i q&& + i ,

(4.46c)

(4.46d)

ai = i q&&+ i ,

ahol i i , i vi , i i , i ai rendre az i szegmenshez rgztett K i keret origjnak szgsebessge, sebessge, szggyorsulsa, gyorsulsa, mind kifejezve a K i keret bzisban. Felhasznlvn az (4.15-16) sszefggseket := p i 1,i vlaszts mellett, s
figyelembe vve, hogy a mozg koordinta-rendszerben rvnyes derivlsi szably
szerint

= (1 i )t i 1 q& i + i t i 1 q& i p i 1,i =: d i 1 q& i ,

= d i 1 q&&i + i t i 1 q& i d i 1,i ,
tovbb hogy az eredmnyek a K i 1 bzisban keletkeznek, amelyeket azutn a K i
bzisba t kell transzformlni, ezrt
i
i

= Ai11,i

i 1

i 1 + i t i 1 q& i ,

(4.47a)

v i = Ai11,i ( i 1 v i 1 + i 1 i 1 p i 1,i ) + i d i 1 q& i ,

i

i = Ai11,i

i 1

i 1 + i t i 1 q&&i + i i i t i 1 q& i ,

(4.47b)
(4.47c)

a i = Ai11,i { i1 a i1 + i1 i1 p i 1,i + i1 i1 ( i1 i1 p i 1,i )} +

+ i d i1 q&&i + i t i1 q& i i d i1 q& i + 2( Ai11,i

i 1

i1 ) i d i1 q& i =

= Ai11,i { i1 a i1 + i1 i1 p i 1,i + i1 i1 ( i1 i1 p i 1,i )} +

(4.47d)

+ 2 i i i d i1 q& i i t i1 q& i i d i1 q& i + i d i1 q&&i .

Itt i d i 1 , i t i 1 (4.21-25) szerint hatrozhat meg m = i vlaszts mellett. A fenti

sszefggsekbl kiolvashatak a i , i , i , i rekurzv szmtsra szolgl kpletek. Azrt, hogy ezeket a kpleteket egyszerbb tegyk, elhagyjuk a mtrixok

4. A ROBOT DIFFERENCILIS MOZGSA

61

vgn ll nulla oszlopokat, vagyis az i -edik lpsben i , i -nek i oszlopa van s

dim q& i = i :

i , j = Ai11,i i 1, j , j = 1, K , i 1 ;
i

i = Ai11,i

i 1

i ,i = i t i 1 ,

(4.48a)

i 1 + i t i 1 q& i ,

(4.48b)

i = Ai11,i i 1 + i i i t i 1 q& i ,

(4.48c)

i, j = Ai11,i ( i 1, j + i 1, j p i 1,i ), j = 1, K , i 1 ;

i,i = i d i 1 ,

i = Ai11,i{i 1 + i 1 pi 1,i + i 1i 1 (i 1i 1 pi 1,i )} +

+ 2 i i i di 1 q&i iti 1 q&i i di 1 q&i .

(4.48d)

(4.48e)

Ha ci jelli az i szegmens tmegkzppontjt a K i keretben kifejezve, akkor

ci = i [ ci ] i ,

(4.49a)

ci = i + i ci + i ( i ci ) ,

(4.49b)

i
i

v ci = ci q& ,

(4.49c)

a ci = ci q&& + ci .

(4.49d)

A kinematikai mennyisgek fenti paramterezse kapcsolatban ll a robot Jacobi

mtrixval is. A kapcsolat rzkeny arra, hogy a bzisvektorok a K 0 (rgztett)
vagy K m (mozg) koordinta-rendszerbl valk-e. Ha vessz () jelli a formlis
(elemenknti) derivltat az id szerint, akkor
a m


d J m

= J m q&& +
q& = m q&& + m q&,
dt
m
m
m
m m q& + ( m m q&) ( m m q&)
m am m
=
= J m q&& +

m
m q&

m
m m + [( m m q&) ]
m
= m m q&& +
m
m

m
q&.

(4.50)

(4.51)

Az indexek kzl a jobb als (a nvvel egytt) azonostja a mennyisget, a bal fels
pedig a bzist, amiben a mennyisg fel lett rva. A parcilis sebessg s parcilis
szgsebessg (4.21-25) szerint szmthat. A vgberendezs sebessge s szgsebessge m J m ismeretben mr meghatrozhat, ugyanis ha

62

Lantos: ROBOTOK DINAMIKUS MODELLJE

A
Tm, E = mT, E
0

p m, E
,
1

(4.52)

akkor
E

v E = AmT , E ( m v m + m m p m, E ) ,
E

E =

AT
J E = m, E
0

(4.53a)

AmT , E m m ,

(4.53b)

([ p m,E ] Am, E )T m J
AmT , E

(4.54)

5. FIZIKAI ELVEK A MECHATRONIKBAN

63

5. FIZIKAI ELVEK A MECHATRONIKBAN

Ebben a fejezetben megismerkednk a merev testek inerciajellemzivel s a mechatronikai rendszerek dinamikus modelljeinek fellltshoz hasznlhat fizikai elvekkel.

5.1 Merev testek inercia jellemzi

A merev test kinetikus energijnak kifejezsbl fogjuk kiolvasni, melyek azok a
merev test jellemzk, amelyek energia-megfontolsbl fontosak.
Tekintsk a [ ]T [ ] kifejezst:
0

[ ] [ ] = z
y

0
x

y2 + z2

= xy

x z

y 0

x z
0 y

xy

x2 + z2
yz

z
0

x =

xz

y z .
x2 + y2

(5.1)

Ha az [s] szegmens (merev test) egy tetszleges pontja s dm a hozz tartoz

infinitezimlis tmeg, akkor a K x := ( x) d m lineris transzformci az
inercia tenzor, amelynek pozitv definit mtrixa a K tehetetlensgi vagy inercia
mtrix. (Az inercia mtrixot 'bold' K -val jelljk a knyvnek ebben a rszben,
hogy elkerljk az tkzst a koordinta-rendszer K jellsvel. Az I vagy J jellst nem hasznljuk, mert ezek az egysgmtrixot s a robot Jacobi mtrixt jellik. A knyv ms rszein a 'bold' K jellst mr elhagyjuk. A koordintarendszereket az brkon bekarikzott K -val jelltk).

Kx

K := [ ] [ ] d m = K xy
[s]
K xz

K xy
Ky
K yz

K xz

K yz ,
K z

(5.2)

ahol
K x = ( y2 + z2 ) d m,
[s]

(5.3)

64

Lantos: ROBOTOK DINAMIKUS MODELLJE

K xy = x y d m,

(5.4)

[s]

rendre az x tengelyre s az [ x, y ] skra vonatkoz tehetetlensgi (inercia) nyomatk, s K tbbi elemei hasonl mdon vannak definilva. (Megjegyezzk, hogy az
irodalomban szoks a ftln kvli elemeket K xy , K xz , K yz mdon is jellni).
A tmeg s a tmegkzppont (center of mass) rtelmezse
m = d m s m c =
[s]

dm .

(5.5)

[s]

Az m c szorzatot szoks els momentumnak is nevezni.

~
Legyen K s K kt ortonormlt koordinta-rendszer. Vizsgljuk meg, hogyan
fgg a tehetetlensgi mtrix a vonatkoztatsi pont (a koordinta-rendszer origjnak) megvlasztstl, lsd 5.1. bra.

~
z

y
K

~
p
x

~
x
~
K
~
y

5.1. bra. Az inerciamtrix transzformcija

Mivel ~ = ~
p + koordinta fggetlen alakban s

[~ ][~ ] = [( ~p + ) ][( ~p + ) ] =
p ][ ~
p ] + [ ~
p ][ ] + [ ][ ~
p ] + [ ][ ],
= [~
ezrt integrls utn a HuygenoSteiner kplethez jutunk, amelynek tenzor alakja
K ~ = K m[ ~
p ][ ~
p ] [ ~
p ][m c ] [m c ][ ~
p ] ,

mtrix alakja pedig

(5.6)

5. FIZIKAI ELVEK A MECHATRONIKBAN

K ~ = A ~ K AT~ m[ ~
p ][ ~
p ]

KK

KK

] [

65
(5.7)

[~
p ] (mAK~K c ) (mAK~K c ) [ ~
p ].

Specilisan, ha a koordinta-rendszer K := K c a tmegkzppontba van helyez~

ve s K := K egy tetszleges koordinta-rendszer, akkor m c = d m:= 0 ,
~
p: = ,
c

T
K = AK,K c K c AK,K
m[ c ][ c ] =
c
2

T
= AK,K c K c AK,K
+ m c
c

(5.8)

I m[ c o c ] ,

ami lehetv teszi a koordinta-rendszer origjhoz tartoz inerciamtrix (K ) kiszmtst, ha ismeretes a tmegkzppont ( c ) s a tmegkzpponthoz tartoz
inerciamtrix (K c ) . Br a K c keret origja a tmegkzppontban van, az
x c , y c , z c tengelyek nem szksgkppen a ftengelyek.

K c1
m1 , K c1

c1
K
c

m, K c
Kc

c2

m2 , K c 2 K
c2
5.2. bra. sszetett rendszer ered tehetetlensgi mtrixa

Alkalmazzuk ezt az eredmnyt kt merev testbl ll sszetett rendszerre, lsd

5.2. bra. Legyenek a kt merev test inercia jellemzi m1 , c1 , K c1 s
m 2 , c 2 , K c 2 , ahol az inercia mtrixok a merev testek tmegkzppontjhoz rgztett K ci keretben vannak megadva. (A K keret robot esetn lehet pldul a
Denavit-Hartenberg keret, a kt merev test a szegmens, valamint a szegmensre szerelt s a kvetkez szegmenst mozgat motor). A tmegkzppontok legyenek adottak K -ban. Feltesszk, hogy a K s K c koordinta-rendszerek prhuzamosak. Al-

66

Lantos: ROBOTOK DINAMIKUS MODELLJE

kalmazzuk a (5.8) sszefggst rendre a K c1 , K c , c1 c , AK c ,K c1 = AK,K c1 s

K c 2 , K c , c 2 c , AK c ,K c 2 = AK,K c 2 vlaszts mellett.
Az ered rendszer jellemezhet az m, c , K c inercia jellemzkkel:

m = m1 + m2 s c =

m1 c1 + m 2 c 2
,
m1 + m 2

T
K c = AK,K c1 K c1 AK,K
m1 [( c1 c ) o ( c1 c )] + m1 c1 c
c1

(5.9)
2

I+

T
+ AK,K c 2 K c 2 AK,K
m 2 [( c 2 c ) o ( c 2 c )] + m 2 c 2 c
c2

(5.10)
I.

5.2. Lagrange, Appell s Newton-Euler egyenletek

Tekintsnk egy N tmegpontbl ll rendszert. Legyen az l -edik tmegpont tmege ml s helyvektora rl . Legyenek az x1 , x 2 , x3 , K , x 3 N 2 , x 3 N 1 , x3 N koordintk rvn adottak a helyvektorok egy rgztett ortonormlt koordinta-rendszerben.
Feltesszk, hogy a koordintk k holonm s stacionrius korltozsnak tesznek
eleget, amelyek
f (r1 , K , rN ) = 0, = 1, K , k

(5.11)

skalr egyenlet alakban adottak. A D'Alambert elv szerint a rendszer dinamikjt

Fl ml &r&l + Rl = 0, l = 1, K , N

(5.12)

rja le, ahol Fl az ml tmegpontra hat ered aktv er s Rl a korltozsoknak

megfelel ered reakci er. A rendszer szabadsgfoka n = 3 N k . Ezrt definilhatunk q1 , K , q n fggetlen skalr paramtereket, az .n. ltalnostott koordintkat, amelyek segtsgvel mindegyik rl helyvektor felrhat
rl = rl (q1 , K , q n ), l = 1, K , N

(5.13)

alakban.
A mozgsegyenletek levezetshez alkalmazzuk a virtulis munka elvt. Legyen
x a rendszer pozcija a t idpillanatban. Ha x egy lehetsges msik pozcija a
rendszernek ugyanebben a t idpillanatban, akkor x = x x elmozduls hatsra
a rendszer x -bl x -be hozhat. A x elmozdulst virtulis elmozdulsnak nevezzk, szemben a dt id hatsra bekvetkez dx elmozdulssal, ahol az erk s korltozsok megvltozhatnak. Adjunk a rendszernek virtulis elmozdulst, s szorozzuk meg az egyenleteket a hozz tartoz rl -lel, akkor kapjuk, hogy

5. FIZIKAI ELVEK A MECHATRONIKBAN

67

< Fl ml &r&l + Rl , rl >= 0, l = 1, K , N ,

(5.14)

s sszegzs utn
N

< Fl ml &r&l + Rl , rl > = 0 .

(5.15)

l =1

Felttelezzk, hogy a korltozsok idelisak, ezrt

N

< Rl , rl >= 0 ,

(5.16)

l =1

l =1

l =1

l =1

< Fl ml &r&l , rl > = 0 < Fl , rl > = < ml &r&l , rl > .

(5.17)

Kvetkezik (5.13)-bl, hogy

N

rl =
l =1

< Fl , ql
i =1

l =1

rl
q i ,
q i

n N

r
> q i = < ml &r&l , l > q i .
q i
i =1 l =1

(5.18)

(5.19)

Definiljuk a q i ltalnostott koordinthoz tartoz Qi ltalnostott ert

N

Qi := < Fl ,
l =1

rl
>
q i

(5.20)

alapjn. Mivel a q i virtulis elmozdulsok fggetlenek, ezrt

N

Qi = < ml &r&l ,
l =1

rl
>,
q i

(5.21)

ami felfoghat egy absztrakt fizikai elvnek is.

5.2.1 Lagrange-egyenlet
Az N tmegpontbl ll rendszer kinetikus energija
K=

Mivel (5.13) szerint

1 N
ml r&l2 .
2 l =1

(5.22)

68

Lantos: ROBOTOK DINAMIKUS MODELLJE

n

r&l =
i =1

rl
r&
r
q& i s l = l ,
q i
q& i q i

(5.23)

n
n

2 rl
r
&r&l = l q&&i +
q& i q& j ,

i =1
j =1 q i q j
q i

(5.24)

N
N
r
r&
K
= < ml r&l , l > = < ml r&l , l >
q& i l =1
q i
q i
l =1

(5.25)

N
N
n
r
2 rl
d K
= < ml &r&l , l > + < ml r&l ,
q& j >
d t q& i l =1
q i
l =1
j =1 q i q j

(5.26)

N
N
n
r&
2 rl
K
= < ml r&l , l > = < ml r&l ,
q& j >
q i l =1
q i
l =1
j =1 q j q i

(5.27)

ezrt

Felhasznljuk, hogy sima fggvny esetn a vegyes msodrend parcilis derivltakra teljesl
2 x
2 x
,
=
q i q j q j q i

ezrt
N
r
d K K

= < ml &r&l , l > = Qi .

d t q& i q i l =1
q i

(5.28)

Az sszefggs rvnyes sszekapcsolt rendszerek esetn is az integrl tulajdonsga alapjn. A q i ltalnostott koordinthoz tartoz Qi ltalnostott er felbonthat a i meghajt nyomatkra (erre) s a P(q) potencilis energia hatsra,
amely utbbi P q i . Ezrt bevezethetjk a K kinetikus energit, a P potencilis
energit, az L = K P Lagrange fggvnyt s a i meghajt nyomatkot (ert),
amelynek segtsgvel a kvetkez sszefggshez jutunk:
d K K P d L
L

+
=

= i Lagrange-egyenlet.
dt q& i q i q i dt q& i q i

(5.29)

Tekintsnk egy merev testet a hozz rgztett koordinta-rendszerrel. Legyen a

merev test origjnak sebessge v s szgsebessge , akkor a merev test tetszleges helyvektor pontja esetn

5. FIZIKAI ELVEK A MECHATRONIKBAN

69

< v ,v >= < v + ,v + >=

= < v,v > +2 < v, > + < , >=
= < v,v > +2 < ,v > + < [ ] [ ], >,
T

s ezrt a merev test K kinetikus energijra teljesl

K=

1
1
< v,v > m + < K , > + < m c , v >,
2
2

(5.30)

ahol c a tmegkzppontja s K az inercia mtrixa a merev testnek, amely a merev testhez rgztett keret origjhoz tartozik. Ha a keret origja egybeesik a
tmegkzpponttal, azaz c = 0 , akkor a merev test kinetikus energija
K=

1
1
< v c ,v c > m + < K c , > ,
2
2

(5.31)

ahol v c = v + c a tmegkzppont sebessge s K c a tmegkzpponthoz tartoz inercia mtrix.

5.2.2 Appell-egyenlet
A gyorsuls energia vagy Gibbs fggvny N tmegpont esetn
G=

1 N
ml &r&l 2 .

2 l =1

(5.32)

Mivel (5.24) szerint

&r&l
r
= l ,
q&&i q i

(5.33)

N
&r&
r
G N
= < ml &r&l , l > = < ml &r&l , l > ,
q&&i l =1
q&&i
q i
l =1

(5.34)

ezrt (5.21) alapjn a kvetkez egyszer alak Appell-egyenletet kapjuk:

G
= Qi ,
q&&i
vagy

(5.35)

70

Lantos: ROBOTOK DINAMIKUS MODELLJE

G P
+
=i .
q&&i q i

(5.36)

Tekintsk a a ,a kifejezst, akkor

< a ,a >= < a + + ( ) ,a + + ( ) >=
= < a,a > + < [ ]T [ ] , > +2 < , a > +2 < ,[ ][ ] a >
2 < , ([ ]T [ ] ) > +L ,

ahol a nem rszletezett tagok nem fggenek q&& -tl. Ezrt vehetjk a kvetkez egyszerstett Gibbs fggvnyt:
G=

1
1
< a,a > m + < K 2(K ) , > +
2
2
+ < m c , a + ( a ) >

(5.37)

Specilisan, ha a merev testhez rgztett koordinta-rendszer origja a tmegkzppontban van, akkor c = 0 s

G=

1
1
< a c ,a c > m + < K c 2(K c ) , > ,
2
2

(5.38)

ahol a c = a + c + ( c ) a tmegkzppont gyorsulsa s K c a tmegkzpponthoz tartoz inercia mtrix.

5.2.3 NewtonEuler-egyenlet
Legyen K 0 s K rendre egy inercia keret s egy merev testhez rgztett, vele
egytt mozg keret. Legyen p a K 0 origjbol a K origjba mutat pozci vektor. Merev test helyett elszr tmegpontokbl ll rendszert tekintnk, lsd 5.3.
bra.
A tmegpontokbl ll rendszerre alkalmazhat (5.12) s vehet a kvetkez
sszeg:
N

l =1

l =1

l =1

ml &r&l = Fl + Rl .

(5.39)

5. FIZIKAI ELVEK A MECHATRONIKBAN

71

r
rc

K0
5.3. bra. Rgztett K 0 s mozg K keret

Alkalmazvn a Newton s az Euler elvet feltehet, hogy a bels erk sszege s

a bels erk brmely pontra vett nyomatknak sszege nulla. Legyen a kls erk
sszege Fext . Jellje m s rc az ered tmeget s a tmegkzppontot, akkor
(5.39)-bl kvetkezik a Newton-egyenlet:

m&r&c = Fext .

(5.40)

Definilja a perdletet (angular moment)

N

K = (rl p ) ml r&l ,

(5.41

l =1

akkor
N
N
d K
= (r&l p& ) ml r&l + (rl p ) ml &r&l =
dt
l =1
l =1
N

l =1

l =1

= p& ml r&l + (rl p ) ml &r&l =

(5.42)

= p& mr&c + (rl p ) ml &r&l ,

l =1

N
d K
+ p& mr&c = (rl p ) ml &r&l .
dt
l =1

(5.43)

72

Lantos: ROBOTOK DINAMIKUS MODELLJE

Jellje N K ,ext a kls erk ered nyomatkt:

N

N K ,ext = (rl p ) Fl ,

(5.44)

l =1

akkor (5.12), a felttelek, valamint (5.43) s (5.44) alapjn kapjuk, hogy

d K
+ p& mr&c = N K ,ext .
dt

(5.45)

Merev test esetn

K =

vdm ,

(5.46)

[s]

ezrt

v = (v + ) = v + ( ) =
= v ( ),

felhasznlsval rhatjuk, hogy

K = m c v + K .

(5.47)

Felhasznlvn az integrl tulajdonsgait, a mozg koordinta-rendszerekben rvnyes differencilsi szablyt s a (5.45) sszefggst, kapjuk, hogy

d K
= mv c v + mrc a + K + (K ) ,
dt

(5.48)

d K
+ v mv c = mv c v + m c a + K + (K ) + v mv c =
dt
= K + (K ) + m c a ,

(5.49)

K + (K ) + m c a = N K,ext .

(5.50)

Specilisan, ha a K keret origja a tmegkzppontban van, akkor c = 0 , s a

kvetkez Euler-egyenletethez jutunk:

K c + (K c ) = N K c ,ext .

(5.51)

6. ROBOTOK DINAMIKUS MODELLJE

73

6. ROBOTOK DINAMIKUS MODELLJE

Ebben a fejezetben bemutatjuk a robotok dinamikus modelljnek meghatrozst az
Appell-egyenlet s a Lagrange-egyenlet felhasznlsval. Az els klnsen numerikus szmtsokra, a msodik pedig szimblikus szmtsokra alkalmas. Ezen kvl
a msodik mdszer lehetsget ad a klnfle fizikai hatsok szeparlsra is. A dinamikus modell szmtst tipikusan 1 ms gyakorisggal kell elvgezni a korszer
robotirnytsi algoritmusokban.

6.1 A robot Appell-egyenleten alapul dinamikus modellje

Korbban bevezettk a gyorsuls energit vagy Gibbs-fggvnyt. Ha a szegmenshez rgztett keret origja a tmegkzppontban van, akkor a Gibbs-fggvny

G=

1
1
< a c ,a c > m + < K c 2(K c ) , >
2
2

(6.1)

ahol a c a tmegkzppont gyorsulsa, s a szegmens szggyorsulsa s szgsebessge, m a szegmens tmege s K c a tmegkzppontra vonatkoz inercia
mtrix a szegmenshez rgztett koordinta-rendszerben. Feltesszk, hogy a koordinta-rendszer tengelyei prhuzamosak a DenavitHartenberg keret tengelyeivel,
ezrt hasznlhatjuk a kinematikai mennyisgek szmtsra a (4.48a-e) s (4.49a-d)
kpleteket, tovbb feltehet, hogy a szegmens s a rszerelt motor inercia jellemzit mr sszevontuk (5.10) szerint. Ha G a robot Gibbs-fggvnye s P a potencilis energija, akkor az Appell-egyenlet alapjn

G P
+
= i , i = 1,2,..., m
q&&i q i

(6.2)

ahol i az i -edik szegmenst meghajt ltalnostott nyomatk. Mivel a robot

Gibbs-fggvnye a szegmensek Gibbs-fggvnyeinek sszege, elegend egy szegmens hatst tisztzni a dinamikus modellben s a hatsokat sszegezni. Azrt, hogy
egyszer sszefggst kapjunk, hozzvesznk nulla oszlopokat cs -hez s s -hez,
ezltal a mtrixok 3 m mretek lesznek. Akkor

74

Lantos: ROBOTOK DINAMIKUS MODELLJE

G s 1 a cs
a
1
= <
,a cs >m s + < a cs , cs >m s +
&
&
&
&
q i
2 q i
2
q&&i
+

1
1
s
< K cs s , s > + < K cs s 2( K cs s ) s s , s >=
2
q&&i
2
q&&i

=<

(6.3)

a cs

s
,a cs >m s + < s , K c s ( K cs s ) s s > ,
q&&i
q&&i
a cs

=
( cs q&& + cs ) = cs,i ,
q&&i
q&&i
s

=
( s q&& + s ) = s,i ,
q&&i q&&i

(6.4)
(6.5)

ahol cs ,i s si rendre az i -edik oszlopa s -nek s s -nek. Irjuk fel a

skalrszorzot mtrixszorzsos alakban, akkor
G s
s
= m s Tcs,i ( cs q&& + cs ) + s,Ti [ K cs ( s q&& + s ) ( K cs s ) s s ] =
&
&
q i
= ( Tcs,i cs m s + s,Ti K cs s ) q&& + Tcs,i cs m s +

(6.6)

+ s,Ti [ Kcs s ( K cs s s ) s s ]
sszegezvn az egyes szegmensek hatst, a robot dinamikus modelljt kapjuk:
H (q ) q&& + hcc (q,q& ) + h g (q ) =

(6.7)

H = { Tcs cs m s + sT K cs s },

(6.8)

hcc = { Tcscs m s + sT [ K cs s ( K cs s ) s s ]},

(6.9)

s =1

s =1

ahol hcc a centripetlis s Coriolis hats s h g a gravitcis hats.

Ha m egy tmegpont, amelynek helyvektora r a K 0 keretben, akkor mgh potencilis energija felrhat
r
m < g 0 ,r >= ( g 0T 0) m ,
1

(6.10)

6. ROBOTOK DINAMIKUS MODELLJE

75

alakban, ahol g 0 a gravitcis gyorsuls a K 0 keret bzisban kifejezve. Ha az s edik szegmens tmegkzppontja cs a K s DenavitHartenberg keretben, akkor a
robot P potencilis energija s hatsa az Appell egyenletben:
m

P = ( g 0T 0) T0 , s cs m s ,
1
s =1
m
T0 ,s cs
P

ms =
= ( g 0T 0)
q i 1
q i s =1
m
Ti 1,s cs

ms .
= ( g 0T 0) T0 ,i 1
q i 1
s =i

(6.11)

(6.12)

Mivel (4.27) szerint

Ti 1,s
q i

= i 1Ti 1,s ,

ezrt
m
R

P
= ( g 0T 0) T0 ,i 1 i 1 m s Ti 1,s cs = GiT i ,
q i
1
Mi
s =i

(6.13)

GiT = ( g 0T 0) T0 , i 1 i 1 ,

(6.14)

M i = ms ,

(6.15)

Ri = m s ( Ai 1,s cs + p i 1,s ) .

(6.16)

s =i

s =i

Az eredeti DenavitHartenberg alakban t i 1 = k i 1 a csukltengely, ezrt

i)
rotcis csukl esetn

i 1

0 1
1 0
=
0 0

0 0

m
T0 ,i 1 i 1 = 0 ,i 1
0

0 0
0 0
,
0 0

0 0
l 0 ,i 1 0 0
,
0
0 0

GiT = g 0 m 0 ,i 1 , g 0 l 0 ,i 1 ,0 ,0 ,

(6.17)

(6.18)
(6.19)

76

Lantos: ROBOTOK DINAMIKUS MODELLJE

ii)

transzlcis csukl esetn

i 1

0
0
=
0

0 0 0
0 0 0
,
0 0 1

0 0 0

(6.20)

0 0 0 n 0 ,i 1
T0 ,i 1 i 1 =
,
0
0 0 0

(6.21)

GiT = 0 ,0,0 , g 0 n 0 ,i 1 .

(6.22)

Felhasznlvn, hogy
Ri =

m s (Ai 1,i Ai,s cs + pi 1,i + Ai 1,i pi,s ) + mi (Ai 1,i ci + pi 1,i ) =

m

s = i +1

m
m

= Ai 1,i m s Ai,s cs + p i,s + mi ci + m s p i 1,i ,

s =i +1
s =i

(6.23)

a kvetkez htratart rekurzis algoritmust kapjuk Ri s M i szmtsra:

R m +1 = 0, M m +1 = 0,

(6.24)

M i = M i +1 + m i ,

(6.25)

Ri = Ai 1,i Ri +1 + mi ci + M i p i 1,i .

(6.26)

A gravitcis hats a dinamikus modellben

R
h g,i = GiT i .
Mi

(6.27)

6.2 A robot Lagrange-egyenleten alapul dinamikus modellje

Az F1. fggelkben megmutattuk, hogy a robot szegmens kinetikus energija, ha a
koordinta-rendszer origja a tmegkzppontban van

K=

1
1
< v c ,v c > m + < K c , > ,
2
2

(6.28)

6. ROBOTOK DINAMIKUS MODELLJE

77

ahol K c a keret origjra (a tmegkzppontra) vonatkoz tehetetlensgi mtrix.

Feltesszk, hogy a keret tengelyei prhuzamosak a szegmenshez rgztett Denavit
Hartenberg keret tengelyeivel, ezrt alkalmazhat (4.46a-b) s (4.49c) a robot kinetikus energijnak meghatrozsakor:
m
1
1

K = < v cs ,v cs >m s + < K cs s s , s s > =

2

s =1 2

1 m
< cs q& , cs q& > m s + < K cs s q& , s q& > =
2 s =1

1 m
< Tcs cs m s q&, q& > + < sT K cs s q&, q& > =
2 s =1

1 m
< Tcs cs m s + sT K cs s q&, q& >=
2 s =1

1
< H (q )q& , q& >
2

(6.29)

A robot kinetikus energija a dinamikus modellben szerepl H (q) mtrix kvadratikus alakja a q& csuklsebessgekben. Vezessk be a
H(q) = [ D jk (q )] mm

(6.30)

jellst, akkor H = [ D jk ] szimmetrikus s fizikai megfontolsok alapjn pozitv

definit, tovbb

K=

1 m m
D jk q& j q& k .
2 j =1 k =1

(6.31)

Alkalmazzuk az (5.29) Lagrange-egyenletet:

d K K P

+
=i.
dt q& i q i q i

(6.32)

Mivel H = [ D jk ] szimmetrikus, ezrt D ji = Dij s

m
K 1 m
1 m
= D ji q& j + Dik q& k = Dij q& j ,
q& i 2 j =1
2 k =1
j =1

(6.33)

m
m m D
d K
ij
= Dij q&& j +
q& j q& k .
dt q& i j =1
j =1 k =1 q k

(6.34)

78

Lantos: ROBOTOK DINAMIKUS MODELLJE

Msrszt
ax y + bx y =

a+b
a+b
xy +
x y,
2
2

(6.35)

ezrt a msodik tag mindig szimmetrizlhat:

m
m m
d K
1 Dij Dik
= Dij q&& j +
+

dt q& i j =1
q j
j =1 k =1 2 q k

q& j q& k .

(6.36)

Hasonl mdon kapjuk, hogy

K 1 m m D jk
=
q& j q& k ,
q i 2 j =1 k =1 q i
Di :=

(6.37)

R
P
= GiT i .
q i
Mi

(6.38)

Ezrt a robot dinamikus modellje Lagrange alakban

m

j =1

j =1 k =1

Dij q&& j + Dijk q& j q& k + Di = i , i = 1,...,m,

Dijk =

1 Dij Dik D jk
+

2 q k
q j
q i

(6.39)
(6.40)

ahol
Dii

effektv inercia,

Dij

csatol inercia (i j ) ,

Dijj

centripetlis hats,

Dijk

Coriolis hats ( j k ) ,

Di

gravitcis hats.

A Dij , Dijk , Di paramterek fggvnyei q -nak, ezrt a robot mozgsa kzben vltoznak.
Vegyk szre, hogy ha ismert Dij szimbolikus (kpletszer) alakban, akkor
(6.40) alapjn parcilis derivlssal meghatrozhat Dijk . Hasonl mdon, ha ismert a P potencilis energia szimbolikus alakban, akkor parcilis derivlssal
meghatrozhat a Di gravitcis hats is. Ilymdon az energik kpletszer is-

6. ROBOTOK DINAMIKUS MODELLJE

79

merete esetn a dinamikus modell szimbolikus oprcival (parcilis derivlssal)

kpletszer alakban is meghatrozhat.

6.3 A SCARA robot dinamikus modellje

A 4-DOF RRTR SCARA robot DenavitHartenberg parametereit (lsd 6.1. tblzat) a 6.1. bra koordinta-rendszerei alapjn rtelmeztk.

z3 , z 4
4
x3 , x 4

z2
y2

x2

d3

d2

y3 , y4

z1

d1
y1

x1

a1
a2

z0

1
y0

x0

6.1. bra. A SCARA robotnl hasznlt keretek

6.1. tblzat. SCARA robot DenavitHartenberg parameterei

qi
1
2
d3
4

i
1
2
3
4

i
1
2
0
4

di
d1
d2
d3
0

ai
a1
a2
0
0

i
0
0
0
0

Az egyes szegmensek kztti homogn transzformcik a kvetkezk:

T0,1

C1
S
= 1
0

S1
C1
0
0

0 a1 C1
C 2

S
0 a1 S1
, T1,2 = 2
0
1 d1

0
1
0

S2
C2
0
0

0 a2 C2
0 a 2 S 2
,
d2
1

0
1

(6.41)

80

Lantos: ROBOTOK DINAMIKUS MODELLJE

T 2 ,3

1
0
=
0

0
C 4

S
0
, T3, 4 = 4
0
0 1 d3

0 0 1
0

0 0
1 0

S4
C4
0
0

0 0
0 0
.
1 0

0 1

(6.42)

A homognszformcikat fokozatosan sszeszorozzuk a parcilis sebessgek s

szgsebessgek s az ered homogn transzformci meghatrozshoz:

T2 ,4

C 4
S
= 4
0

S4
C4

0
C 24 S 24 0 a 2 C 2

S
0
C 24 0 a 2 S 2
, T1,4 = 24
,
0
0
1 d3
0
1 d2 + d3

0
0 1
0
0
1
0
C124 S124 0 a1 C1 + a 2 C12
S
C124 0 a1 S1 + a 2 S12
.
T0 ,4 = 124
0
0
1 d1 + d 2 + d 3

0
0
1

0
0
0

(6.43)

(6.44)

Itt q a T0 ,4 a direkt geometriai feladat megoldsa. A T0 , 4 a q inverz geometriai

feladat a kvetkez egyenletekre vezet:

a1 C1 + a2 C12 = px ,
a1 S1 + a2 S12 = p y ,
d1 + d2 + d3 = pz ,
C124 = lx ,
S124 = l y .

(6.45)
(6.46)
(6.47)
(6.48)
(6.49)

A q1 csuklvltoz meghatrozshoz vgezzk el a kvetkez talaktsokat:

a 2 C12 = p x a1 C1 ,
a 2 S12 = p y a1 S1 ,
2
2
a 22 C12
+ a 22 S12
= p x2 2a1 p x C1 + a12 C12 + p 2y 2a1 p y S1 + a12 S12 ,

2a1 p x C1 + 2a1 p y S1 = p x2 + p 2y + a12 a 22 ,

ahol az utols egyenlet alakja AC + B S = D , amelynek megoldsa

6. ROBOTOK DINAMIKUS MODELLJE

S =
C =

81

DB + A A2 + B 2 D 2
A2 + B 2
D A + B A2 + B 2 D 2
A2 + B 2

(6.50)

(6.51)

Mivel teljeslnie kell, hogy S2 + C2 = 1 , ezrt csak kt ( S ,C ) megolds van,

amelyek a ( , ) = (1, 1) s ( , ) = (1,1) rtkprokhoz tartoznak. Mivel
( S ,C ) meghatrozza a trnegyedet a skon, ezrt mindkt ( S ,C ) prhoz egyegy megolds van, amely

= arctan2(S , C ) ,

(6.52)

A2 + B 2 D 2 0

(6.53)

feltve hogy a

munkatr felttel teljesl.Vlaszthatjuk azt a q1 megoldst, amely a megelz q1 hez legkzelebb van.
Ha mr q1 -et meghatroztuk, akkor
S12 =

C12
s innen

p y a1 S1

(6.54)

,
a2
p a1 C1
= x
,
a2

(6.55)

q1 + q 2 = arctan2(S12 ,C12 ).

(6.56)

Az utols lpsek nylvnvalak:

q3 = p z d1 d 2 ,

q1 + q 2 + q 4 = arctan2 l y ,l x .
A SCARA robot 4 J 4 Jacobi mtrixa (4.22-25) segtsgvel hatrozhat meg:

(6.57)
(6.58)

82

Lantos: ROBOTOK DINAMIKUS MODELLJE

C124 (a1 S1 + a 2 S12 ) + S124 (a1 C1 + a 2 C12 ) a1 S 24 + a 2 S 4

d 0 = S124 (a1 S1 + a 2 S12 ) + C124 (a1 C1 + a 2 C12 ) = a1 C 24 + a 2 C 4 ,

0
0

+
C
a
S
S
a
C
a
S

24 2 2
24 2 2 2 4

4
d 1 = S 24 a 2 S 2 + C 24 a 2 C 2 = a 2 C 4 ,

0
0

(6.59)

d 3 = 0,

a1 S 24 + a 2 S 4
a C + a C
2 4
1 24

0
4
J4 =
0

a2 S 4
a2 C 4
0
0
0
1

0 0
0 0
1 0
.
0 0
0 0

0 1

Ha szksges J 4 is, akkor alkalmazhat (4.33). De a SCARA robot elg egyszer

(a csukltengelyek prhuzamosak), ezrt kzvetlenl is rhat, hogy
v 4 =

d p 0 ,4
dt

a1 S1 q&1 a 2 S12 (q&1 + q& 2 )

= a1 C1 q&1 + a 2 C12 (q&1 + q& 2 ) ,

q& 3

4 =
0
,
q& + q& + q&
2
4
1
a1 S 1 a 2 S12 a 2 S 12
a C +a C
a 2 C12
2 12
1 1

0
0
J 4 =
0
0

0
0

1
1

(6.60)

(6.61)
0 0
0 0
1 0
.
0 0
0 0

0 1

(6.62)

Ha v x0 ,v y 0 ,v z 0 s z 0 rtke el van rva, akkor tekinthetk a nekik megfelel

egyenletek
q& 3 = v z 0 ,
q& 4 = z 0 q&1 q& 2 ,

(6.63)
(6.64)

6. ROBOTOK DINAMIKUS MODELLJE

q&1 a1 S1 a 2 S12
=
q& 2 a1 C1 + a 2 C12

a 2 S12 v x 0
.

a 2 C12 v y 0

83
(6.65)

A kt utols egyenlet egyszeren megoldhat, mert

a b

c d

d b
1

,
ad bc c a

(6.66)

ahol
det = a d bc = (a1 S 24 + a 2 S 4 )a 2 C 4 (a1 C 24 + a 2 C 4 )a 2 S 4 =
= a1 a 2 (S 24 C 4 C 24 S 4 ) = a1 a 2 S 2 = 0

(6.67)

definilja a szingulris konfigurcikat:

q 2 = 0 vagy q 2 = 180 ,

(6.68)

sszhangban a 6.1. brval.

A (4.48a-e) s (4.49a-d) rekurzv algoritmussal a kvetkez kinematikai mennyisgeket kapjuk ( i jelli az i szegmens tmegkzppontjt):
0
y1
a1 x1

1 = 0 , 1 = 0, c1 = a1 + x1 , c1 = y1 q&12 ,
1

0
0

(6.69)

0 0

2 = 0 0 , 2 = 0,
1 1

y2
a1 S 2 y 2

c 2 = a 2 + a1 C 2 + x 2 a 2 + x 2

0
0

a 2 a1 C 2 x 2
a2 x2
a2 x2

2
c 2 = a1 S 2 y 2
q&1 + 2 y 2 q&1 q& 2 + y 2 q& 2 ,

0
0
0

(6.70)

84

Lantos: ROBOTOK DINAMIKUS MODELLJE

0 0 0
3 = 0 0 0, 3 = 0,
1 1 0

c3

c3

a1 S 2 y 3
= a 2 + a1 C 2 + x3

y3
a 2 + x3
0

0
0,
1

(6.71)

a 2 a1 C 2 x3
a 2 x3
a 2 x3

2
=
a1 S 2 y 3
q&1 + 2 y 3 q&1 q& 2 + y 3 q& 2 ,

0
0
0

0 0 0 0

4 = 0 0 0 0, 4 = 0,
1 1 0 1

a 2 S 4 + a1 S 24 y 4
c 4 = a C + a C +
x4
1 24
2 4

c4

a2 S 4 y4
a2 C 4 + x4
0

0 y4
0 x 4 ,
1
0

a 2 C 4 a1 C 24 x 4
a C x4
x4

2 2 4
2

= a S +a S
&1 + a 2 S 4 y 4 q& 2 + y 4 q& 42 +
y 4 q
2 4
1 24

0
0
0

(6.72)

a2 C 4 x4
x4
x4

+ 2 a 2 S 4 y 4 q&1 q& 2 + 2 y 4 q&1 q& 4 + 2 y 4 q& 2 q& 4 ,

0
0
0

A dinamikus modellt a kinematikai mennyisgek ismeretben az Appell egyenlettel hatrozzuk meg. Figyelembe vve s specilis alakjt, a sT K cs s szorzattl
szrmaz tagok H ij = Dij -ben egyszersthetk:

[0

0 si , z

* *
* 0

* 0
* *
* * K cs , z sj , z

= si , z sj , z K cs , z .

(6.73)

Mivel s oszlopai s s prhuzamosak, s s = 0 , ezrt a msodik tag hcc -ben

nulla:

6. ROBOTOK DINAMIKUS MODELLJE

85

s T [K cs s (K cs s ) s ] = 0 .

(6.74)

Egyszer szmtsok utn Dij , Dijk s Di kvetkez nemtrivilis elemeihez jutunk:

D11
*
H =
*

D12

D13

D 22
*
*

D23
D33
*

0 D112
* D
122
D1 =
*
*

*
*

D14
D 24
,
D34

D 44

D114
D123 D124
,
D133 D134

*
D144
D112 0 D 213 D 214
*
0 D 223 D 224
D2 =
,
*
* D 233 D 234

*
*
D 244
*
D133 D 213 0 D314
*
D 223 0 D324
D3 =
,
*
0 D334
*

*
* D344
*
D114 D 214 D314 0
*
D 224 D324 0
4

,
D =
*
*
D334 0

*
*
0
*
D1
D
D = 2 .
D3

D4

(6.75)

D113

(6.76)

(6.77)

(6.78)

(6.79)

(6.80)

A dinamikus modell egyszer alakot lt, ha bevezetjk a kvetkez P1 , K, P11 pa-

ramtereket s az F1 , K, F7 fggvnyeket(a c indexet elhagytuk cs -ben s K cs ben):

86

Lantos: ROBOTOK DINAMIKUS MODELLJE

P1 = m1 { y21 + (a1 + x1 ) 2 } + K z1 +
+ m 2 {a12 + y22 + (a 2 + x 2 ) 2 } + K z 2 +
+ m3 {a12 + (a 2 + x3 ) 2 + y23 } + K z 3 +
+ m 4 {a12 + a 22 + x24 + y24 } + K z 4 ,

P2 = m 4 a 2 y4 ,
P3 = m 4 a 2 x4 ,
P4 = m 2 a1 y 2 + m3 a1 y 3 ,
P5 = m 2 a1 (a 2 + x 2 ) + m3 a1 (a 2 + x3 ) + m 4 a1 a 2 ,

(6.81)

P6 = m 4 a1 y4 ,
P7 = m 4 a1 x4 ,
P8 = m 2 {(a 2 + x 2 ) 2 + 2y 2 } + K z 2 + m3 {(a 2 + x3 ) 2 + 2y 3 } + K z 3 +
+ m 4 {a 22 + x24 + 2y 4 } + K z 4 ,
P9 = m 4 { x24 + 2y 4 } + K z 4 ,

P10 = m3 + m 4 ,
P11 = (m3 + m 4 ) g z ;
F1 = 1,
F2 = S 4 ,
F3 = C 4 ,
F4 = S 2 ,

(6.82)

F5 = C 2 ,
F6 = S 24 ,
F7 = C 24 ;
D11 = F1 P1 2 F2 P2 + 2 F3 P3 2 F4 P4 + 2 F5 P5 2 F6 P6 + 2 F7 P7 ,
D12 = F1 P8 2 F2 P2 + 2 F3 P3 F4 P4 + F5 P5 F6 P6 + F7 P7 ,
D13 = 0,
D14 = F1 P9 F2 P2 + F3 P3 F6 P6 + F7 P7 ,
D 22 = F1 P8 2 F2 P2 + 2 F3 P3 ,
D 23 = 0,
D 24 = F1 P9 F2 P2 + F3 P3 ,
D33 = F1 P10 ,

(6.83)

6. ROBOTOK DINAMIKUS MODELLJE

87

D34 = 0,
D 44 = F1 P9 ;
D112 = F4 P5 F5 P4 F6 P7 F7 P6 ,
D113 = 0,
D114 = F2 P3 F3 P2 F6 P7 F7 P6 ,
D122 = F4 P5 F5 P4 F6 P7 F7 P6 ,
D123 = 0,

(6.84)

D124 = F2 P3 F3 P2 F6 P7 F7 P6 ,
D133 = 0,
D134 = 0,
D144 = F2 P3 F3 P2 F6 P7 F7 P6 ,
D 213 = 0,
D 214 = F2 P3 F3 P2 ,
D 223 = 0,
D 224 = F2 P3 F3 P2 ,

(6.85)

D 233 = 0,
D 234 = 0,
D 244 = F2 P3 F3 P2 ;
D314 = 0,
D324 = 0,

(6.86)

D334 = 0,
D344 = 0;
D 444 = 0,

(6.87)

D1 = 0,
D 2 = 0,
D3 = F1 P11 ,

(6.88)

D 4 = 0.

A szegmens s a rszerelt motor inerciajellemzit a rotor ll helyzetben sszevontuk, de nem vettk figyelembe a forg rotor kinetikus energijt, amely

88

Lantos: ROBOTOK DINAMIKUS MODELLJE

1
1
K ri = ri & i2 = ri i2 q& i2 , ahol ri a rotor tehetetlensgi nyomatka, & i a rotor
2
2
szgsebessge s i = & i / q& i az tttel. Mivel
d K ri
= ri i2 q&&i ,
&
dt q i

(6.89)

ezrt a beavatkozszerv hatsa j kzeltssel Dii := Dii + ri i2 rvn vehet figyelembe. A surldsi vesztesg az f i q& i viszkzus surldssal kzelthet, amelyet
szintn a i meghajt nyomatknak kell fedeznie. Ezrt nhny paramter megvltozik, s a surlds miatt tovbbi paramterek lpnek be:
P1 := P1 + r1 12 ,
P10 := P10 + r 3 32 ,
P12 = r 2 22 ,
P13 = r 4 42 ,

(6.90)

P14 = f 1 ,
P15 = f 2 ,
P16 = f 3 ,
P17 = f 4 ;
D 22 := D22 + P12 ,
D 44 := D 44 + P13 ,
hi := hi + P13+i q& i .

(6.91)

6.4 Ktszabadsgfok robotkar dinamikus modellje

Egy ktszabadsgfok robotkar Denavit-Hartenberg paramtereit mutatja a 6.2. tblzat. A robotkar a vzszintes x 0 , y 0 skban mozog, a gravitcis gyorsuls y 0
irny. A rotcis csuklk tengelyei az x 0 , y 0 skra merlegesek s egymssal prhuzamosak.
6.2. tblzat. 2-DOF robotkar DenavitHartenberg parameterei

i
1
2

qi
1
2

i
1
2

di

ai

l1

l2

i
0
0

6. ROBOTOK DINAMIKUS MODELLJE

89

A robotkar szegmenseinek tmegkzppontja l c1 , l c 2 tvolsgra van a csukltengelyektl x i irnyban. A szegmensek tmege m1 , m 2 , a tengelyirny tehetetlensgi
nyomatk a tmegkzppontban I z1 , I z 2 . A dinamikus modellt a szimbolikus mdszerrel vezetjk le. Elszr meghatrozzuk a geometriai modellt, majd a szegmensek sebessgt elbb az inerciarendszeren, majd a sajt koordinta-rendszerben. Ezt
kveten meghatrozzuk a H = [ D jk ] ltalnostott inerciamtrixot s abbl a centripetlis s Coriolis hatst, vgl pedig a gravitcis hatst.
Geometriai modell:

T0,1

T0, 2

C1
S
= 1
0

0
C12
S
= 21
0

S1
C1
0
0
S12
C12
0
0

0 l1C1
0 l1 S1
,
1
0

0
1

T1, 2

C 2
S
= 2
0

S2
C2
0
0

0 l2C2
0 l 2 S 2
,
1
0

0
1

(6.92)

0 l1C1 + l 2 C12
0 l1 S1 + l 2 S12

1
0

0
1

Sebessgek s szgsebessgek:

0
0
l1 S1

T 0
1
v1 =
= l1C1 q&1 v1 = A0,1 v1 = l1 q&1 1 = l1
dt
0
0
0

dp 0,1

l1 S1 q&1 l 2 S12 (q&1 + q& 2 ) l1 S1 l 2 S12 l 2 S12

q&
v2 =
l 2 C12 1
= l1C1 q&1 + l 2 C12 (q&1 + q& 2 ) = l1C1 + l 2 C12
q&
dt

0
0
0 2

0
0
l1 S 2
l1 S 2
0
q&
2
v 2 = A0T, 2 v 2 = l1C 2 + l 2 l 2 1 2 = l1C 2 + l 2 l 2
q&

0
0 2
0
0

dp 0, 2

90

Lantos: ROBOTOK DINAMIKUS MODELLJE

1c

0
= l c1 ,
0

2c

l1 S 2
= l1C 2 + l c 2

0
l c 2 ,
0

0
1 = 0 ,
1

0 0
2 = 0 0
1 1

(6.93)

ltalnostott inerciamtrix:
0 l c1
H=
0 0

0
0
l c1
0
0
l1C 2 + l c 2
lc2

l1 S 2
0

l 2 0
I1
= c1
m1 +
0
0 0
D12
D
= 11

D22

0
0
0 0 1

0 m1 +
0
0
0
0

I 1
0
1

0
l1 S 2
0
0 0
l1C 2 + l c 2 l c 2 m 2 +

0
0 0

0
0
0 l12 + 2l1l c 2 C 2 + l c22
+
0 (l1C 2 + l c 2 )l c 2

0
0 +
0
0 0
1
0 0 =

1
I 1 1 1

I 2
(l1C 2 + l c 2 )l c 2
m2 +
2
lc2
I 2

I2
=
I 2

D11 = m1l c21 + m 2 (l12 + l c22 + 2l1l c 2 C 2 ) + I 1 + I 2

D12 = m 2 (l1C 2 + l c 2 )l c 2 + I 2
D22 =

m 2 l c22

(6.94)

+ I2

Centripetlis s Coriolis hats:

0 D112
D1 =
,
D122
D112 =
D122

D112
D2 =

1 D11 D12 D12

q1
q1
2 q 2

D
1 D
= 12 + 12
q 2
2 q 2

Potencilis energia hatsa:

0
0

= m 2 l1l c 2 S 2

D22
= m 2 l1l c 2 S 2

q1

(6.95)

6. ROBOTOK DINAMIKUS MODELLJE

91

P = m1 gl c1 S1 + m 2 g (l1 S1 + l c 2 S12 )
D1 =

P
= m1 gl c1C1 + m 2 g (l1C1 + l c 2 C12 )
q1

D2 =

P
= m 2 gl c 2 C12
q 2

(6.96)

A 2-DOF robotkar dinamikus modellje:

D11 (q 2 )q&&1 + D12 (q 2 )q&&2 + 2 D112 (q 2 )q&1 q& 2 + D122 (q 2 )q&12 + D1 (q1 , q 2 ) = 1
D12 (q 2 )q&&1 + D 22 q&&2 D112 (q 2 )q&12 + D 2 (q1 , q 2 ) = 2

(6.97)

6.5 Identifikcis megfontolsok

A robot dinamikus modellje felrhat jelek s fggetlen paramterek szorzataknt is
Y (q, q& , z&, &z&) = alakban, amely a fggetlen paramterek identifikcijnak alapjt
kpezi. Az identifikci nhangol adaptv irnyts keretben valsthat meg stabilitsgarancik mellett. A z&, &z& jelek helyre a szablyozs megfelelen vlasztott
referenciajelnek q& r , q&&r derivltjai kerlnek. A 2-DOF robotkar esetn a fggetlen
paramterek, valamint az nhangol adaptv irnyts alapjt kpez modell alak a
kvetkez.
Fggetlen paramterek:

1 = m1l c21 + m 2 (l12 + l c22 ) + I 1 + I 2

2 = m 2 l1l c 2
3 = m 2 l c22 + I 2
4 = g (m1l c1 + m 2 l1 )
5 = gm 2 l c 2
Dinamikus modell fggvnyei kifejezve a fggetlen paramterekkel:
D11 = 1 + 2C 2 2
D12 = 3 + C 2 2
D 22 = 3

(6.98)

92

Lantos: ROBOTOK DINAMIKUS MODELLJE

D112 = S 2 2
D122 = S 2 2

(6.99)

D1 = C1 4 + C12 a 5
D2 = C12 5
A dinamikus modell felrsa jelek s paramterek szorzataknt:

Dik &z&k + ( Dijk q& j )z& k + Di = i

k

Y11 Y12
Y
21 Y22

Y13
Y23

(6.100)

Y14
Y24

2
Y15

3 = 1

Y25
4 2

Y11 = &z&1 ,
Y12 = C 2 (2&z&1 + &z&2 ) S 2 (q&1 z& 2 + z&1 q& 2 + q& 2 z& 2 ),
Y13 = &z&2 ,
Y14 = C1 ,
Y15 = C12
Y21 = 0,
Y22 = C 2 &z&1 + S 2 q&1 z&1 ,
Y23 = &z&1 + &z&2 ,
Y24 = 0,
Y25 = C12

(6.101)

7. GPJRM DINAMIKUS MODELLJE

93

7. GPJRM DINAMIKUS MODELLJE

Tekintsk a vzszintes skban mozg gpjrm egyszerstett vzlatt a 7.1. bra
szerint. A hts kerekek meghajtottak s az els kerekek kormnyozhatk.

7.1. bra. Vzszintes skban mozg gpjrm egyszerstett vzlata

A jrm (vehicle) dinamikus modelljnek fellltsakor az els kerekekre (front

wheels) F betvel, a hts kerekre (rear wheels) R betvel fogunk hivatkozni. A
bal oldali kerekeket (left wheels) L , a jobb oldali kerekeket (right wheels) R jelli.
A dinamikus modell fellltsakor szerepet jtszik a jrm tmegkzppontjba helyezett K CoG (center of gravity) koordinta-rendszer, a kerekekhez rendelt KW
(wheel) koordinta-rendszerek s a K In (inertial) koordinta-rendszer.
Az egyszerstett jrm dinamikus modell felttelezi, hogy a jrmnek csak az
els kereke kormnyzott s a jrm bal oldala s jobb oldala szimmetrikus felpts
s viselkeds, ezrt a kt fl sszevonhat egyetlen egysgg a dinamikus modell-

94

Lantos: ROBOTOK DINAMIKUS MODELLJE

ben. Az xCoG tengely legyen a jrm hossztengelye (longitudinal axle), az y CoG

tengely mutasson ettl balra s a z CoG tengely mutasson felfel. A kerekeknek a tvolsgt a tmegkzpponttl jellje rendre l F s l R . Az els kerk K W koordinta-rendszernek origja legyen az xCoG tengelyen, az xW tengely legyen a kerk
skjban s a kerk elfordulsi szge (az xW s xCoG kztti szg) legyen W . A
tmegkzppont szgsebessgt jellje & . A jrm tmegkzppontjnak sebessg
vektora legyen v CoG , a sebessg vektornak az xCoG tengellyel bezrt szge , s a
sebessg vektor abszolt rtkt jellje vG . Az els kerk sebessg vektort jellje
vWF s a sebessg vektornak az xW tengellyel bezrt szgt F (ekkor vWF az
xCoG tengellyel W F szget zr be). A hts kerk sebessg vektort jellje
vWR s a sebessg vektornak az xCoG tengellyel bezrt szgt R .
A , F , R szgek szoksos elnevezse rendre a jrm oldalcsszsi szge
(vehicle body side slip angle), az els gumiabroncs oldalcsszsi szge (tyre slide
slip angle front), a hts gumiabroncs oldalcsszsi szge (tyre slide slip angle
rear).
Ha az els kereket a jrm hossztengelyhez ( xCoG ) kpest balfel kormnyozzuk,
akkor a vCoG s vWF sebessg vektorok xCoG bal oldaln, a vWR sebessg vektor
pedig xCoG jobb oldaln lesznek. A kerk elfordulsi szget s az oldalcsszsi
szgeket ekkor pozitv irnynak tekintjk.
Az els kerkre FlF longitudinlis s FtF transzverzlis er hat az els kerk
koordinta-rendszernek origjban, s feltesszk, hogy a transzverzlis komponens
FtF = c F F , ahol c F konstans. Hasonlan a hts kerkre az FlR longitudinlis s
az FtR transzverzlis er hat a hts kerk koordinta-rendszernek origjban, s a
transzverzlis komponens FtR = c R R , ahol c R konstans.
Az els kerk sebessge a jrm tmegkzppontjnak sebessge megnvelve a
jrm & szgsebessgnek hatsval, amely azonos az l F sugron & szgsebessggel krplyn mozg pont sebessgvel. Tekintsk az els kerk sebessgnek
y CoG s xCoG irny komponenseit, akkor a szgek pozitv irnynak fenti rtelmezse mellett

vWF sin( W F ) = l F& + vG sin( )

vWF cos( W F ) = vG cos( ) ,
ahonnan kvetkezik

(7.1a)
(7.1b)

7. GPJRM DINAMIKUS MODELLJE

tan( W F ) =

l F& + vG sin( )
.
vG cos( )

95
(7.2)

Hasonlan a hts kerk sebessgnek y CoG s xCoG irny komponensei a pozitv szgirnyok figyelembevtelvel rendre

vWR sin( R ) = l R& v G sin( )

vWR cos( R ) = v G cos( ) ,

(7.3a)
(7.3b)

ahonnan kvetkezik

tan( R ) =

l F& v G sin( )
.
v G cos( )

(7.4)

Kis W F , R s szgeket felttelezve tan( W F ) W F ,

tan( R ) R , sin( ) s cos( ) 1 kzeltssel rhat

F = W
R = +

l F&
vG

l R&
.
vG

(7.5)
(7.6)

Alkalmazzuk a korbban mr megismert cos( ) = C , sin( ) = S egyszerstett jellseket, akkor a 7.1. bra s a derivlsi szably szerint mozg koordintarendszerben meg tudjuk adni a kinematikai, majd a dinamikus modelljt a jrmnek.

vCOG

= S
0

vG

(7.7)

96

Lantos: ROBOTOK DINAMIKUS MODELLJE

a COG

= v&COG + v COG
v&G + C

C vG S
S

v&G

v G C & + C &v G
= S

0
0 0
C

= S
0

&
v G + &

&
0
0

0 C

0 S
0 0

vG =

(7.8)
Az erk irnynak figyelembevtelvel a halad mozgs egyenlete a jrm mv tmegvel tosztva, s a COG -nek csak a nem trivilis els kt komponenst tekintve:

a COG =

1
mv

Fx 1 FlF C w FtF S w + FlR

=
F y m FlF S + FtF C + FtR
v
w
w

(7.9)

Kzbens lpsek:

C
S

vG S
vG C

v&G 1 vG C
=

&
vG S

a b
=:

c d

vG S 1
C mv

d b 1 v G C
1

=
ad bc c a v G S

Fx 1 v G C v G S S
+
C v G&
Fy v S
1G4
4444C2 44
4443
0
&
1

1 v G C

vG S

v G S 1 FlF C w FtF S w + FlR

=

C mv FlF S w + FtF C w + FtR

FlF C w FtF S w + FlR C + FtR S

1
1
=
[F S

F
C
F
S
F
C
]
+

+
mv
lF w
tF w
lR
tR
v
G

Vegyk figyelembe, hogy

vG S
C

7. GPJRM DINAMIKUS MODELLJE

l &
FtF = cF F = cF w F
vG

l &
FtR = cR R = cR + R
vG

97

(7.10)

A halad mozgs differencilegyenlete:

v&G =

l &
1
{FlF cos( w ) c F w F sin( w ) +
mv
vG

l &
+ c R + R sin( ) + FlR cos( )}
vG

& = & +

(7.11)

1
{FlF sin( W ) FlR sin( ) +
mv vG
l &
l &
+ c F ( W F ) cos( W ) + c R ( + R ) cos( )}
vG
vG
(7.12)

A forgatnyomatk: z = l F {FlF sin( W ) + FtF sin(90 o + w )} l R FtR

A forg mozgs differencilegyenlete :

&& =

l &
1
1
z = {l F FlF sin( W ) + l F c F w F cos( w )
Iz
Iz
vG

l &
l R c R + R cos( )}
vG

(7.13)

llapotegyenlet:

x& = f ( x , u ) nemlineris, tipikusan FlF = 0

x = (vG , , ,& ) T , u = ( w , FlR ) T , y = (vG , ) T

(7.14)

Egyszerstett linearizlt modellhez jutunk, ha feltehet, hogy vG konstans s a szgek kicsik.

Egyszerstett linearizlt modell llapotegyenlete:

98

Lantos: ROBOTOK DINAMIKUS MODELLJE

x& = Ax + Bu , y = Cx , x = ( ,& ) T , u = W , y = &

cF + cR

&

mv vG
=
&& c l c l
R R
F F

I zz

(7.15)

cRlR cF lF

cF
1

+ mv v G W = Ax + Bu
2
c F l F & c F l F

I zz
I zz vG

(7.16)

mv v G2
c R l R2 +

A linearizlt modell det(sI A) = 0 karakterisztikus egyenlete:

s2 +

(c + c R )(l F + l R ) 2 + m v v G2 (c R l R c F l F )
c F ( I zz + m v l F2 ) + c R ( I zz + m v l R2 )
=0
s+ F
m v v G I zz
m v v G2 I zz

Az egyszerstett linearizlt modell stabil, ha c R l R > c F l F , ami a gyakorlatban

elterjedt jrmvek esetn rendszerint teljesl. Irodalmi adatok alapjn az egyszerstett linearizlt modell 0.4 g -nl kisebb laterlis jrmgyorsuls tartomnyban kielgt pontossg.
Az egyszerstett linearizlt modell Astat = W& , W (0) statikus tviteli tnyezje a

W& , W ( s ) = C ( sI A) 1 B tviteli fggvny kirtkelsvel kpezhet s = 0 esetn:

Astat = v G

c F c R (l F + l R )
(c F + c R )(l F + l R ) 2 + mv vG2 (c R l R c F l F )

(7.17)

A gyakorlatban bevezetik a v char karakterisztikus sebessget, ahol

2
v char
:=

c F c R (l f + l R ) 2
m v (c R l R c F l F )

(7.18)

2
( v char
negatv is lehet, ha c R l R c F l F < 0 ), amely szoros kapcsolatban van a statikus tviteli tnyezvel:
vG
1
Astat =

.
(7.19)
2
2
l F + l R 1 + v G / v char

Tipikus jrmveknl v char rtke 68 s 112 km/h kztt van. A kis karakterisztikus
sebessg arra utal, hogy a jrm alulkormnyozott.