99

ROBOTOK IRNYTSA
8. DECENTRALIZLT HROMHURKOS
KASZKD-SZABLYOZS
Ebben a fejezetben bemutatjuk, hogyan lehet alkalmazni a klasszikus lineris
szablyozstechnika eredmnyeit kis teher s viszonylag lass mkds robotok
esetn. A mdszer a nemlineris hatsokat zavar jelnek tekinti, amelyek hatst a
szablyozsnak kell lecskkenteni vagy eliminlni. Gyors mkds s jelents teher
esetn azonban tapasztalat szerint a pontossg leromlik, ezrt az irnytst korszer
irnytsi mdszerekkel kell felvltani.

8.1 Az egyenram motor dinamikus modellje

Tekintsk az lland mgnessel gerjesztett, homogn mgneses trben forg motort
(8.1 bra).

ir

Rr

Lr

q
L

8.1. bra. Az egyenram motor szerkezeti vzlata

Jellje Rr s Lr a forgrsz ellenllst s induktivitst, i r a rotorramot s

u a motor kapocsfeszltsgt. A Lenz-trvny szerint a forgs hatsra a mgneses
trben a motor kapocsfeszltsgvel szembehat c1& bels feszltsg indukldik,
ahol & a motor szgsebessge. Msrszt a BiotSavart-trvny szerint a rotorban

100

Lantos: ROBOTOK IRNYTSA

foly ram hatsra az rammal arnyos er keletkezik, amely a motor tengelyn

m = c 2 i r nyomatkot hoz ltre. Itt c1 , c 2 a motorra jellemz llandk. A forg-

rsznek a motor tengelyre vett tehetetlensgi nyomatka legyen r . A motor a L

tehetetlensgi nyomatk terhet (angolul load) tttelen keresztl hajtja. Az tttel a szgsebessget reduklja. Jellje az tttel teherrel megegyez oldaln a szgsebessget q& , akkor

= & / q&.

(8.1)

Ha a motor vesztesgmentes, akkor az tttel kt oldaln a teljestmnyek megegyeznek. Mivel az N teljestmny a nyomatk s a szgsebessg szorzata, ezrt

N m = N m & = q& = m .

(8.2)

Vegyk szre, hogy mg az tttel a motoroldali szgsebessget tipikusan cskkenti a teher oldalon, addig a motoroldali nyomatkot megnveli a teher oldalon.
Ttelezzk fel, hogy a vesztesg a viszkzus srldsbl szrmazik s arnyos a
szgsebessggel. Akkor a motor oldaln f m & a viszkzus srldsbl (csapgysrldsbl) ered srldnyomatk, a teher oldaln pedig f tt q& a srldnyomatk
(ha a srldst az tttelben a teher oldalra redukltuk). Tegyk fel tovbb, hogy a
teher vltozik, s a vltoz teher hatsa a zavar jellel vehet figyelembe.
Mivel jl ismert, hogy a tehetetlensgi nyomatk megszorozva a szggyorsulssal egyenl a meghajtnyomatkkal, a rotor ellenllsn s induktivitsn es feszltsg sszege pedig egyenl a kapocsfeszltsg s a bels feszltsg klnbsgvel, ezrt az egyenram motor dinamikus viselkedse a kvetkez egyenletekkel
rhat le:

L q&& + f tt q& + + r && + f & = c 2 i r ,

Rr i r + Lr

(8.3)

d ir
= u c1& .
dt

Osszuk el (8.3) els egyenlett -vel, s trjnk t a motoroldali mennyisgekre

( q& = & / , q&& = && / ), akkor

( r +

f
L

&& + ( f r + tt ) & = c 2 i r

)
.

2
2

(8.4)

Vezessk be a

:= r + L / 2

f := f r + f tt / 2

(8.5)

8. DECENTRALIZLT HROMHURKOS KASZKD-SZABLYOZS

101

jellseket (redukljuk a tehetetlensgi nyomatkokat s a srldsokat a motor oldalra), akkor az egyenram motor dinamikus modellje a kvetkez alakra hozhat
( u a bemenet, a kimenet s zavar jellemz):

&& + f& = c 2 i r

(8.6)

d ir
= u c1& .
Rr i r + Lr
dt

Laplace-transzformlva s bevezetve az R r + sLr opertoros impedancit megkapjuk az egyenram motor hatsvzlatt (8.2 bra).

1 /
u+

1 / Rr
1+ s Lr / Rr

ir

c2

1
s

&

1
s

f
c1
8.2. bra. Az egyenram motor hatsvzlata

Vezessk be az

~
x = ( , & , i r ) T llapotvektort, legyen y = a kimenjel, s

lljon a bemenet a hasznos u motor kapocsfeszltsgbl s a zavar jelbl, azaz

legyen u~ = (u , ) T , akkor (8.6) a kvetkez alakra hozhat:

~
dx
=
dt

y = [1

c2 ~
f

0
x+

c
R

1 r
0

Lr
Lr
~
~
0 0 ] x = C x.
0

0
0
1
Lr

1 ~

u = A~
x + Bu~,

Az egyenram motor karakterisztikus egyenlete

(8.7)

102

Lantos: ROBOTOK IRNYTSA

det ( sI A) =

s
0

1
s + ( f / )

c2 /

c1 / Lr

s + ( R r / Lr )

0
,

ahonnan a determinnst kifejtve kapjuk, hogy az egyenram motor harmadfok

karakterisztikus polinomja
det ( sI A) = s [( s +
= s[ s 2 +

) (s +

Rr
cc
)+ 1 2 ]=
Lr
Lr

f Lr + R r
f R r + c1c 2
s+
],
Lr
Lr

(8.8)

= s ( s s1 )( s s 2 )
a sajtrtkek (plusok) pedig
s1, 2 =

( f Lr + Rr ) ( f Lr + R r ) 2 4( f R r + c1c 2 ) Lr
2 Lr
(Tm + Te ) (Tm + Te ) 2 4(1 +

2Tm Te

c1c 2
)Tm Te
Rr f

(8.9)

s 3 = 0.
A plusok megadsakor bevezettk a Tm = / f mechanikai s Te = Lr / R r villamos idllandkat.
Vegyk szre, hogy a motor c1 , c 2 konstrukcis adatainak rtktl fggen a
sajtrtkek (plusok) konjuglt komplexek is lehetnek.
c2 /
jellst, akkor a motorfeszltsg bemenet s
det ( sI A)
szgelforduls kimenet kztti tviteli fggvny:
Vezessk be az a :=

8. DECENTRALIZLT HROMHURKOS KASZKD-SZABLYOZS

, u = [1 0 0] ( sI

= [1 0 0]

103

0
1

A) 0 =
1 / Lr
a 0
0 =
1 / Lr

0
a
= [ a ] 0 =

Lr
1 / Lr
c2

Lr
W
, u = det ( sI A) .

(8.10)

Innen kvetkezik, hogy & szgsebessg kimenet s u kapocsfeszltsg bemenet

esetn az egyenram motor tviteli fggvnye
c2

Lr
A
(8.11)
W& =
, u ( s s )(s s ) = 1 + 2Ts + T 2 s 2 ,
1
2
amely 1 esetn kttrols tagra, < 1 esetn pedig kttrols lengtagra vezet.
A gyakorlatban a motor konstrukcis adataitl fggen mindkt eset lehetsges.

8.2 Hromhurkos kaszkd-szablyozs tervezse

A hromhurkos kaszkd-szablyozs bels ramszablyozsbl ( i ), kzps sebessg-szablyozsbl (ms nven fordulatszm-szablyozsbl, n ) s kls pozciszablyozsbl (ms nven szgelforduls-szablyozsbl, q ) ll, lsd 8.3. bra.
Nem foglalkozunk a szablyozs teljestmny-elektronikai megvalstsval, de feltesszk, hogy az ramszablyozs impulzusszlessg-modulcival (PWM, pulse
width modulation) van megvalstva, amelynek chopper-frekvencija legalbb
10kHz. Ezrt megengedhet a PWM hatsnak elhanyagolsa, ha az ramszablyozsi krt nem gyorstjuk fel jobban, mint 0.5 ms.
Mindhrom hurokban analg szablyozkat feltteleznk, br a legtbb gyakorlati megvalstsban a pozci-szablyozs mintavteles. A vizsglatoknl felhasznljuk a lineris szablyozstechnika eredmnyeit. Mivel a mveleti erstk feszltsgjeleket dolgoznak fel, az rzkelk is feszltsg kimenjelet szolgltatnak,

104

Lantos: ROBOTOK IRNYTSA

~
amelyeket hullmmal jellnk: i = i r , n~ = n, q~ = q. A PID pozciszablyoz mintavteles megvalstsnak elvt ksbbi pontban trgyaljuk..

q~a +

PID

n~a +

PI(n)

~
ia +

PI(i)

&
DC + ROBOT ir

8.3. bra. Hromhurkos kaszkd-szablyozs felptse

8.2.1 A bels ramszablyozsi hurok megtervezse

Az ramszablyozs PI szablyozjnak belltst gy hatrozzuk meg, mintha a
c1& bels feszltsg zavar jel lenne, s a PI szablyoznak az lenne a feladata,
hogy az egytrols szakaszt felgyorstsa, lsd 8.4. bra. Ez nylvnvalan egy kzelts, hisz a bels feszltsg hatsa az egyenram motor mkdsi elvnek egyik
alapjt kpezi. Ksbb meg fogjuk vizsglni, hogyan viselkedik a szablyozs e nlkl a kzelts nlkl az gy megtervezett szablyozval.

~
ia +

PI(i)

u+

1 / Rr
1 + sTe

ir

c1&

8.4. bra. Kzelt mdszer az ramszablyozs tervezshez

A kzelts mellett a felnyitott kr tviteli fggvnye

Wi 0 ( s ) =

A pi 1 + sTIi 1 / R r

,
1 + sTe
T Ii
s

(8.12)

ahol Te = Lr / Rr a villamos idlland. Kzenfekv a TIi := Te vlaszts, amely

utn a felnyitott kr egy integrl tag lesz, a zrt kr pedig egytrols tag:
Wi 0 ( s ) =

A pi
T Ii R r

1 Ki
=:
,
s
s

(8.13)

8. DECENTRALIZLT HROMHURKOS KASZKD-SZABLYOZS

Wi ( s ) =

1/
1/
1/
.
=:
=
1 + s / K i 1 + sTi 1 + sa i Te

105
(8.14)

A gyorsts rdekben Ti := a i Te < Te s ezrt a i < 1 , de a gyorstsnak hatrt szab

Rr
.
ai
Tekintsk ezutn az ramszablyozst a megtervezett PI szablyozval, de most
mr kzelts nlkl. Mivel a 8.2. bra szerint

a PWM chopper-frekvencija. A PI szablyoz erstse A pi =

& = c 2

1 + sTm
1/ f
,
ir ir =
1 + sTm
c2 / f

(8.15)

ezrt ha felhasznljuk a (8.8) s (8.9) sszefggseket s & kimenetet vlasztunk,

akkor a bels szablyozsi kr a 8.5. bra szerinti alakra hozhat, ahol a T -tag az
egyenram motor W&u tviteli fggvnye. A gykhelygrbe alakjt a 8.6. bra
mutatja, ha a krersts vltozhatna.

~
ia +

PI(i)

A
1 + 2Ts + T 2 s 2

&

f
(1 + sTm )
c2
8.5. bra. Az ramszablyozs a megtervezett PI szablyozval kzelts nlkl

Szmtva arra, hogy nagy krerstst sikerlt a tervezskor belltani, a zrt

rendszer plusai a ngyszggel megjellt helyekre esnek a gykhelygrbn. Msrszt 1 / Te zrushelye a zrt krnek is, ezrt a kzelbe es zrt rendszer plussal
egymst gyakorlatilag kejt P/Z prt alkot, s ezrt elhanyagolhat. A negatv vals
1
tengelyen a vgtelenhez tart gon lv plus
, a megtervezett gyors plus.
a i Te
Msrszt viszont a felnyitott kr 1 / Tm zrushelye nem zrushelye a zrt rendszernek & kimenet esetn, mert a visszacsatol gban van, ezrt a kzelbe es 1 / Tm
zrt rendszer plus kzelben nem lesz zrushelye a zrt rendszernek, ezrt 1 / Tm
lesz a zrt rendszer dominns plusa & kimenet esetn.

106

Lantos: ROBOTOK IRNYTSA

kiesik

1
Tm

1
aiTe

1
Te

1
T2

1
T1

1
Tm

8.6. bra. A gykhelygrbe alakja a megtervezett PI szablyozval, ha a krersts vltozna

sszefoglalva, a kzps hurok szablyozsnak tervezshez a bels hurok

eredje jl approximlhat a
W&~i ( s )
a

(1 +

Ai

sTm )(1 +

sTi )

Ai :=

c2
.
f

(8.16)

tviteli fggvnnyel.

8.2.2 A kzps sebessg-szablyozsi hurok megtevezse

A kzps fordulatszm-szablyozsi kr tervezsekor kttrols taghoz kell PI
szablyozt tervezni. A felnyitott kr s TI n := Tm vlaszts esetn a zrt rendszer
tviteli fggvnye a kvetkez lesz:
Wn0 (s)

A pn 1 + sTI n
Ai
Kn
,

(
1
TIn
s
s
+ sTi )
(1 + sTm )(1 + sTi )
1/
.
Wn (s)
(1 + sT1 )(1 + sT2 )

(8.17)

A zrt rendszer karakterisztikus egyenlete az 1 + W n 0 ( s ) = 0 felttel alapjn

s 2 Ti + s + K n = 0.

(8.18)

Aperiodikus hatresetre trekedve a fordulatszm- (sebessg-) szablyozsi krnl,

kvetkezik

8. DECENTRALIZLT HROMHURKOS KASZKD-SZABLYOZS

s1, 2 =

1 1 4 K n Ti
2Ti

Kn =

107

1
T1, 2 = 2Ti .
4Ti

8.2.3 A kls pozci-szablyozsi hurok megtervezse

A pozci-szablyozs felnyitott krnek tviteli fggvnye a PID szablyoz tvi1
teli fggvnynek, a bels szablyozsi hurok ered tviteli fggvnynek, az
s
integrl tagnak s az rzkel tviteli tnyezjnek szorzata:
W0 ( s ) =

A p (1 + s 1 )(1 + s 2 )
An
1

=
TI
s
(1 + sT1 )(1 + sT2 ) s

=K

(1 + s 1 )(1 + s 2 )
s 2 (1 + sT1 )(1 + sT2 )

K :=

Ap A
TI

A :=

An

(8.19)

Feltesszk, hogy T1 T2 , s br a kzps szablyozsi hurok tervezsekor T1 = T2

vlasztssal ltnk, kt esetet fogunk vizsglni (ebbl a msodik tartalmazza a
T1 = T2 esetet). Mindkt esetben aperiodikus tranziensre treksznk a zrt rendszerben, ezrt kb. 75 o fzistbblet elrsre fogunk trekedni.
I.

Legyen T1 > 10 T2 .
-40
-20

1
0.1
T1

c
1
T1

10
T1

1
T2

-40

1 = 1 / 1 = 10T1
c = 1 / T1

2 = T1
t 75o

T = 0.1T1

-60

8.7. bra. A felnyitott kr PID szablyozval jelents idlland klnbsgek esetn

Mivel arctan(0.1) 5 o s arctan(10) 85 o , ezrt vlaszthat a kompenzlt felnyitott kr szmra a 8.7. bra szerinti aszimptotikus amplitd-jelleggrbe az ott megadott 1 , 2 , T s c rtkekkel, amely biztostja a kvnt

108

Lantos: ROBOTOK IRNYTSA

t = 180 o + ( c ) 180 o 90 o 90 o + 85 o 5 o 5 o 75 o

(8.20)

fzistbblet. ( A p belltst a msodik esetnl trgyaljuk).

II.

Legyen T1 < 10 T2 .

A kompenzlt felnyitott kr szmra vlaszthat a 8.8. bra szerinti aszimptotikus

amplitd-jelleggrbe s az ott megadott 1 , 2 , T s c rtkek, amely biztostja
a kvnt t 75 o fzistbbletet.
-40
-20

1
0.01
T2

1
T1

c
0.1
T2

1
T2

10
T1

-40

1 = 1 / 1 = 100 T2

2 = T1

c = 0.1 / T2

T = 0.1T1
-60

t 75o

8.8. bra. A felnyitott kr PID szablyozval hasonl nagysgrend idllandk esetn

A PID szablyoz integrlsi s differencilsi ideje meghatrozhat a PID szablyoz kt alakjnak sszevetsvel (8.19) alapjn:

1 + 2 = TI + T TI = 1 + 2 T ,

1 2 = TI (TD + T ) TD = 1 2 T .

(8.21)

TI

A 8.8. bra szerint meghatrozhat a szablyoz erstse is TI ismeretben:

A1 c
=
A1 = c / 1 ,
1 1
K

12

= A1 K =

A112

= c1 =

Ap A
TI

Ap =

c 1TI
A

(8.22)

8. DECENTRALIZLT HROMHURKOS KASZKD-SZABLYOZS

109

A teljes szablyozsi rendszer gykhelygrbjt (a jnak tlhet kzeltsek mellett) a 8.9. bra mutatja.

8.9. bra. A gykhelygrbe alakja a megtervezett PID szablyozval, ha a krersts vltozna

A gykhelygrbbl lthat, hogy a t 75 o fzistbblet miatt a kzel egyforma kt dominns plus mellett a zrt rendszer tartalmazni fog egy egymst csaknem
kiejt alacsonyfrekvencis (teht lassan lecseng) P/Z prt, amely miatt a zrt rendszer tmeneti fggvnynek kb. v 5% tllvse vrhat annak ellenre, hogy a

tranziens aperiodikus. Ez azonban pl. robotoknl elny is lehet, mert ezeknl a rendszereknl az alapjel tipikusan nem ugrsszeren, hanem folytonos fggvny szerint
vltozik.

8.2.4 A pozciszablyoz mintavteles kzeltse

Az analg PID szablyoz jl kzelthet mintavteles PID szablyozval, pldul
az egysgugrs-ekvivelens kzeltst felhasznlva. Ekkor elszr is az analg PID
szablyoz tervezsekor figyelembe kell venni, hogy a mintavteles szablyoz utn
ll DAC talakt is hozzjrul a satatikus erstshez. Legye a DAC talakt feszltsgtartomnya U DA Volt, a bitszma pedig DAbitszam , akkor a statikus tviteli tnyeznek az analg PID szablyoz tervezsekor hasznlt rtke mdosul:

110

Lantos: ROBOTOK IRNYTSA

A :=

u DA
DAbitszam1

An

(8.23)

A mintavteles PID szablyoz vlaszthat

D PID ( z ) =

q 0 + q1 z 1 + q 2 z 2
p 0 + p1 z 1 + p 2 z 2

(8.24)

alaknak, ahol T mintavteli id esetn

T
q 0 = A p 1 + D
Tc

2T
T
q1 = A p 1 + e T / Tc
+ D
T
Tc
I

T
T
q 2 = A p 1 e T / Tc + D
Tc
T I

p0 = 1

p1 = 1 + e T / Tc

(8.25)

p 2 = e T / Tc

Ha e jelli a szablyoz bemenjelt (hibajel) u a szablyoz kimenjelt

(amely a fordulatszm szablyozsi kr n~a alapjele DAC talakts utn), akkor
(8.24) alapjn a mintavteles szablyoz processzornak a kvetkez szmtst kell
megvalstania:
u k :=

1
( p1u k 1 p 2 u k 2 + q 0 e k + q1ek 1 + q 2 ek 2 )
p0

(8.26)

Ne feledjk azonban, hogy szoftver eszkzkkel biztostani kell azt is, hogy pl.
DAbitszam=12 esetn a 2048 mkdsi tartomnyt nem szabad elhagyni. A teltses jelleggrbe megvalstsa teht a szoftver rszt kpezi.

9. KORSZER ROBOTIRNYTSI MDSZEREK

111

9. KORSZER ROBOTIRNYTSI MDSZEREK

Ebben a fejezetben nhny elterjedt korszer robotirnytsi mdszerrel ismerkednk meg, amelyek alkalmasak a robot elvrt pontossgnak biztostsra gyors
mkds s jelents teher esetn is. Az irnytsok alapjt a robot dinamikus modelljnek felhasznlsa kpezi, amelyet valsidben tipikusan T = 1 ms mintavteli
gyakorisggal kell meghatrozni.

9.1 A kiszmtott nyomatkok mdszere

A mdszert szoks nemlineris sztcsatolsnak nevezni a csuklvltozk terben.
Elterjedt angol megnevezse Computed Torque Technique (CTT). Az elnevezs onnan szrmazik, hogy a szablyozsi trvny nyomatk kimenet. A valsgban a
nyomatk megvalsthat az alacsony hierarchia szinten ramszablyozssal, hiszen
DC motorok esetn a nyomatk arnyos a rotorrammal. Felhasznljuk, hogy a
H (q) ltalnostott inercia mtrix pozitv definit, ezrt H 1 (q ) inverze. Az u jel
a szablyoz bels jele, melyet a plyakvets biztostsra s stabilizlsra fogunk
hasznlni.
Robot (szablyozott szakasz): H (q )q&& + h(q, q& ) =

(9.1)

Nemlineris szablyoz: := H (q )u + h(q, q& )

(9.2)

Zrt rendszer:

H (q )q&& + h(q, q& ) = H (q)u + h(q, q& )

q&& = u q&&i = u i , i = 1, K , m

(9.3a)
(9.3b)

Jl lthatan a kompenzlt rendszer sztesett a szabadsgfokkal megegyez szm

sztcsatolt kettsintegrtorokra. Mivel a rendszernek a megtervezett elrt plyn
kell mozognia, tovbb a teher s a paramterek vltozsakor robusztusnak is kell
lennie, ezrt a szablyoz bels jeleit kpezhetjk a kvetkez decentralizlt szablyozkkal:
Decentralizlt szablyozk:
u i = q&&ai + PID = q&&ai + k Pi (q ai q i ) + k Ii (q ai q i ) dt + k Di (q& ai q& i )

(9.4)

Ne feldjk, hogy a plyatervezskor nemcsak a plyt, hanem annak els s msodik derivltjt is megtervezzk. A decentralizlt szablyozkban szksges

112

Lantos: ROBOTOK IRNYTSA

q ai , q& ai , q&&ai teht a plyatervezskor elll, a q i , q& i rtkeket pedig az rzkelk

mrik.

Htra van mg a decentralizlt szablyoz paramtereinek megvlasztsa. Abbl

indulhatunk ki, hogy a
q&&i = u i = q&&ai + k Pi (q ai q i ) + k Ii (q ai q i ) dt + k Di (q& ai q& i )

(9.5)

alakbl trendezssel s derivlssal kvetkezik

(q ai q i ) + k Di (q ai q i ) + k Pi (q ai q i ) + k Ii (q ai q i ) = 0 ,

(9.6)

teht a rendszer az ei = (q ai q i ) hibajelben egy gerjesztetlen lineris rendszer,

amelynek karakterisztikus egyenlete
s 3 + k Di s 2 + k Pi s + K Ii = 0

(9.7)

s amelyet stabill s gyorss kell tenni.

Vlaszthat teht egy (1 + sT ) 3 = 0 mintarendszer, amelynek T idllandjval
a kvnt mkdsi sebessget bellthatjuk. Pldul mivel az els hrom csukl robusztusabb, vlaszthat szmukra T = 50 ms , mg az utols hrom, miniatrebb
csukl szmra a gyorsabb mkdst biztost T = 25 ms . talaktsok utn
(1 + sT ) 3 = 1 + 3sT + 3s 2 T 2 + s 3T 3 = 0 s 3 +

3 2
3
1
s + 2 s + 3 =0,
T
T
T

(9.8)

ahonnan kapjuk, hogy az alkalmas decentralizlt szablyozbellts:

k Di =

1
1
1
, k Pi = 2 , k Ii = 3
T
T
T

(9.9)

Meg kell azonban jegyezni, hogy a centralizlt nemlineris szablyozban hasznlt nemlineris robot dinamikus modell rendszerint eltr a valditl, mivel zem
kzben a megvltozott teher stb. nem mindig ismeretes, ezrt valjban csak a nominlis H (q ), h(q, q& ) ll rendelkezsre, ezrt a zrt rendszer csak kzelten sztcsatolt:
Hq&& + h = H u + h q&& = H 1 (h h) + H 1 H u

(9.10)

9. KORSZER ROBOTIRNYTSI MDSZEREK

113

9.2 Ekvivalens erk s nyomatkok szmtsa

Ha nemcsak a robot mozgst, hanem a trgy s a krnyezet klcsnhatsakor a
kontaktuspontban kifejtend ert s nyomatkot is kell szablyozni, akkor
er/nyomatk rzkelt is clszer alkalmazni, amely tipikusan az utols szegmens
s a szerszm kztt helyezkedik el, teht mshol, mint ahol szablyozni akarunk.
Msrszt a trbeli irnytskor a trben kifejtend ert s nyomatkot tervezi meg az
irnytsi trvny, amelyet azonban a csuklk szintjn a csuklnyomatkokkal lehet
megvalstani. Szksg van a teht az ttrsre kzttk.

9.2.1 Er s nyomatk transzformci er/nyomatk rzkelk esetn

Mrjen az er/nyomatk rzkel a sajt K s koordinta-rendszerben f s er s
N s nyomatk vektorokat, s keressk a velk ekvivalens ert s nyomatkot a szerszmkzppontban (TCP) vagy a kontaktuspontban. Mivel a kt eset hasonl, korltozdjunk a szerszmkzppont esetre (a kontaktuspont felfoghat a trggyal kiegsztett absztrakt szerszm szerszmkzppontjnak).
Helyezznk el egy nulla ert a szerszmkzppontban f s alakjban, s legyen

a helyvektor a K s origjbl a K E vgberendezs origjba, azaz a szerszmkzpppontba, p s , E . Akkor keletkezik egy f s er a szerszmkzppontban, s az
N s nyomatk megn a K s origjnan lv + f s s a K E origjban lv f s
erk ltal kpzett erpr nyomatkval. Az erkar alkalmas -vel

p sE cos( ) = p sE sin(90 o + ) ,

ezrt

az

erpr

nyomatka

irnyhelyesen

f s p sE .
Bzisfggetlen alak:
pE = ps

(9.11)

N E = N s + f s p sE
Bzisfgg alak:
Ks
fs
Ns

Ts, E

K E
T
f E = AsE
fs
NE =

T
AsE

(N s +

f s p sE )

(9.12)

talaktsok utn kapjuk, hogy

T
T
T
T
T
N E = AsE
N s AsE
[ p sE ] f s = AsE
N s + AsE
[ p sE ]T f s = AsE
N s + ([ p sE ] AsE ) f s ,
T

114

Lantos: ROBOTOK IRNYTSA

ahonnan a kvetkez transzformcis szablyhoz jutunk:

T
fE
AsE

=
T
N E ([ p sE ] AsE )

0 fs

T
AsE
N s

(9.13)

A transzformcis szably ltalnosthat tetszleges kt K 1 , K 2 koordintarendszer s a kzttk lv T1, 2 homogn transzformci esetre, ha az er
tmadspontja is megvltozott:
T
f2
A12
=
T
N 2 ([ p12 ] A12 )

0 f1

T
A12
N 1

(9.14)

T
Ha az er tmadspontja nem vltozik meg, akkor nincs erpr, s csak A12
szerepel a blokk-diagonlisban a koordinta-transzformci (bziscsere) miatt.

9.2.2 Ekvivalens csuklnyomatkok szmtsa

Vonjuk ssze a szerszmkzppontban specifiklt ert/nyomatkot egy F , a keresett ekvivalens csuklnyomatkokat egy , valamint a pozcit s orientcit a
Rodriguez-kplet alapjn egy x vektorban:
1

fE
p

6
R , = 2 R m , x = R 6
F =
M
t
NE


m

(9.15)

Legyen a robot Jacobi-mtrixa K 0 -tl K E -ig J , akkor x& = Jq& x = Jq felhasznlsval, ekvivalens munkavgzst felttelezve kapjuk, hogy
< F , x >=< , q >=< F , Jq >=< J T F , q >

(9.16)

ahonnan kvetkezik, hogy az F ltalnostott ervel ekvivalens csuklnyomatk a

kvetkez:
= JTF
(9.17)

9.3 A robot dinamikus modellje Descartes-koordintkban

A robot modellje csuklvltozkban: Hq&& + h =
Specifiklt er/nyomatk a TCP-ben: F
Robot Jacobi-mtrixa a TCP-ig:
J

(9.18a)
(9.18b)
(9.18c)

9. KORSZER ROBOTIRNYTSI MDSZEREK

115

Vgezzk el a kvetkez talaktsokat s vezessk be kzben az vektort:

x& = Jq&
&x& = Jq&& +

dJ
q& = Jq&& + = Jq&& +
dt

dJ

q&& = J 1 &x&
q& = J 1 (&x& )
dt

HJ 1 (&x& ) + h = J T F
J T HJ 1 &x& + J T h J T HJ 1 = F

Bevezetve a
H := J T HJ 1 ,

h := J T h H

(9.19)

jellseket, a robot dinamikus modelljt Descartes-koordintkban a kvetkez

alakban kapjuk:
H &x& + h = F

(9.20)

A robot kinetikus energija most is a H kvadratikus alakja az x& vltozban,

kvetkezskpp fizikai megfontols alapjn H pozitv definit s ezrt ( H ) 1 .

9.4 A szabad mozgs nemlineris sztcsatolsa Descateskoordintkban

Az irnytsi trvnyt Descartes-koordintkban fogalmazzuk meg, realizlni pedig
a csuklnyomatk (rotorram) szablyozsok bevonsval fogjuk. A mdszer hasonlt a kiszmtott nyomatkok mdszerhez csuklvltozkban, de most a nemlineris sztcsatols a trben trtnik. Felhasznljuk, hogy H 1 . A mdszer alapjt
fogja kpezni a hibrid pozci/er szablyozsnak is megfelel korrekcik utn.
Robot (szablyozott szakasz): H &x& + h = F

(9.21)

F := H u + h

(9.22)

H &x& + h = H u + h
&x& = u &x&i = u i , i = 1, K ,6

(9.23a)

Nemlineris szablyoz:
Zrt rendszer:

(9.23b)

116

Lantos: ROBOTOK IRNYTSA

Jl lthatan a kompenzlt rendszer sztesett 6 darab sztcsatolt kettsintegrtorra. A hat irny a bziskoordinta-rendszerben a tengelyek irnyban val halads s
a tengelyek krli forgsok irnya, amely mozgsok a trben sztcsatoltt vltak.
Mivel a rendszernek a megtervezett elrt trbeli plyn kell mozognia, tovbb a
teher s a paramterek vltozsakor robusztusnak is kell lennie, ezrt a szablyoz
bels jeleit kpezhetjk a kvetkez decentralizlt szablyozkkal:
Decentralizlt szablyozk:
u i = &x&ai + PID = &x&ai + k Pi (x ai x i ) + k Ii (x ai xi ) dt + k Di (x& ai x& i )

(9.24)

Ne feldjk, hogy a plyatervezskor Descartes-koordintkban nemcsak a plyt,

hanem annak els s msodik derivltjt is megtervezzk. A decentralizlt szablyozkban szksges x ai , x& ai , &x&ai teht a plyatervezskor elll, az xi , x& i rtkeket
pedig az rzkelk mrik s/vagy szmtjk a geometriai s a sebessg modellek bevonsval.
Htra van mg a decentralizlt szablyoz paramtereinek megvlasztsa. Abbl
indulhatunk ki, hogy a

&x&i = u i = &x&ai + k Pi (x ai x i ) + k Ii (x ai xi ) dt + k Di (x& ai x& i )

(9.25)

alakbl trendezssel s derivlssal kvetkezik

( x ai x i ) + k Di ( x ai x i ) + k Pi ( x ai x i ) + k Ii ( x ai xi ) = 0 ,

(9.26)

teht a rendszer az ei = ( x ai xi ) hibajelben egy gerjesztetlen lineris rendszer,

amelynek karakterisztikus egyenlete
s 3 + k Di s 2 + k Pi s + K Ii = 0

(9.27)

s amelyet stabill s gyorss kell tenni.

Vlaszthat teht egy (1 + sT ) 3 = 0 mintarendszer, amelynek T idllandjval
a kvnt mkdsi sebessget bellthatjuk. Pldul mivel a tengelyek irnyban val halads nagyobb tmegeket mozgat s ebben az rtelemben robusztusabb, vlaszthat szmukra T = 50 ms , mg a tengelyek krli forgatsokra a gyorsabb mkdst biztost T = 25 ms . talaktsok utn
(1 + sT ) 3 = 1 + 3sT + 3s 2 T 2 + s 3T 3 = 0 s 3 +

3 2
3
1
s + 2 s + 3 =0,
T
T
T

(9.28)

9. KORSZER ROBOTIRNYTSI MDSZEREK

117

ahonnan kapjuk, hogy az alkalmas decentralizlt szablyozbellts:

k Di =

1
1
1
, k Pi = 2 , k Ii = 3
T
T
T

(9.29)

Az irnytsi trvnyt a csuklnyomatkok szintjn implementljuk. Ennek sorn

figyelembe vesszk, hogy a szablyozban csak a robot nominlis dinamikus modellje (pldul nvleges teher esetn) ll rendelkezsre.
Implementls:

= J T F = J T ( H u + h ) = H J 1u + h HJ 1
= H J 1 (u ) + h

(9.30)

9.5 Hibrid pozci s er irnyts

Tekintsk azt a tipikus esetet, amikor egy altrben elrt mozgst kell megvalstani
(pldul a tblra egy karaktert kell felrni), mikzben az altrre merleges komplemens trben elrt ert kell kifejteni (pldul a tblra merleges irnyban az
irsra hasznlt krtval elrt ert kell kifejteni annak rdekben, hogy a karakter
rsa ne a levegbe trtnjen a mozgs irnytsi hibja miatt, ugyanakkor az er ne
is legyen nagy, nehogy a krta eltrjn).

9.5.1 ltalnos feladatspecifikcis mtrixok

Feltehetjuk teht, hogy a megfog egy trgyat tart, s pldul a szerszmkzppontban (TCP) egy elrt f a irny s nagysg ert kell kifejteni, mikzben mozgst kell megvalstani az f a -ra merleges altrben. Mivel az f a irny ltalnos
helyzet lehet a K o opercis trben (a vilgkoordinta-rendszerben), hasonlan a
mozgs komplemens tere is, ezrt a mdszert opercis tr mdszernek is nevezik.
Az f a -ra merleges altrben mozgst szablyozunk, mg az f a irnyban ert.
A feladatot formalizlhatjuk a kvetkez mdon. Vlasztunk egy K f koordinta-rendszert, amelynek origja a TCP-ben van, de amelynek z f tengelye prhuzamos f a -val ( z f || f a ) . Vlasszunk egy diagonlis f pozci specifikcis mtrixot,

x
= 0
0

y
0

, = 1 , ha szabad mozgs lehet az x irnyban, stb. (9.31)

x
f

z
0
0

118

Lantos: ROBOTOK IRNYTSA

( z = 0 tipikusan, kivve a teljes szabad mozgs esett, amelyet nem zrunk ki)
Vegyk szre, hogy f egy maszk mtrix, ahol 1 ll, ott mozgst irnytunk. Ha~
sonlan definilhatunk egy f komplemens maszk mtrixot az er irnytsok
szmra:
~

f =I f

(9.32)

A koordinta-rendszerek kztti kapcsolatatot (csak koordinta-transzformcit felttelezve) a kvetkezkpp rhatjuk le:

Af
K f
K o , r f = A f o , o = A f 1 r f

(9.33)

Az eljrst megismtelhetjk a tengelykrli forgatsok altere s az arra merleges nyomatkszablyozsok komplemens altere tekintetben, bevezetve a a specifiklt nyomatkot, a K koordintatendszert, a diagonlis orientci specifik~
cis mtrixot, annak komplemenst, a = I diagonlis nyomatk specifikcis mtrixot, valamint a K s K o kztti koordinta-transzformcit ler A
orientcis mtrixot.
Az irnytsi feladat lersra bevezetjk az albbi ltalnos feladatspecifikcis
mtrixokat:
A 1 A
S= f f f
0

,
1
A A
0

~
~ A f 1 f A f
S =
0

A A
1

0
~

(9.34)

Ezek segtsgvel a kvetkez szablyozstechnikai feladatok egyszeren elvgezhetk:

1. x a x s Fa F pozci/orientci s er/nyomatk hibk mrse K o -ban

2. A hibk ttranszformlsa K f , K -ba
3. Az agyes alterekben nem szablyozhat hibk elhagysa (nullzsa)
4. A megmaradt hibk visszatranszformlsa K o -ba
~
A ngy lps egyszeren megvalsthat S ( x a x) s S ( Fa F ) segtsgvel.

9. KORSZER ROBOTIRNYTSI MDSZEREK

119

9.5.2 A hibrid pozci/er irnytsi trvny

Az irnytsi trvnyt magas szint centralizlt nemlineris s alacsony szint decentralizlt lineris rszekre bontjuk a fizikai elv figyelembevtelvel. A mozgs illetve az er szablyozsnak megvalstsi korltait az egyes alterekben az S il~
letve az S ltalnos feladatspecifikcis mtrixokkal vesszk figyelembe.
A decentralizlt szablyozsok nem figyelik a fizikai korltokat, gy mkdnek,
mintha a teljes mozgs s er irnyts korltozsok nlkl megvalsthat lenne.
Az er irnytst aktv rszre s csillaptsra bontjuk. Az aktv rsz decentralizlt
szablyozja PI elv, mivel a gyakori oszcillci miatt elnytelen lenne az oszcillcikat kiemel D hats bevonsa. Az oszcillcik hatsnak cskkentsre sebessgfgg csillaptst alkalmazunk. Mivel a kinetikus energia H kvadratikus
alakja, ezrt a sebessgfgg csillapts slyozsban rszt vesz H is.
Nemlineris centralizlt szablyozsok:
F = Fmozgs + Fccgf + Faktv

Fmozgs = H Su mozgs

(9.35)

Fccgf = h
~
~
Faktv = S u aktv
+ H S u csillapt
s

Decentralizlt szablyozsok:

u mozgs
= &x&a + PID = &x&a + k Pi (x a x i ) + k Ii (x a x i ) dt + k Di (x& a x& i )

u aktv
= Fa + PI = Fa + k PF ( Fa F ) + K IF ( Fa F )dt

(9.36)

&
u csillapt
s = K VF x

Implementci:
~

= J T F = H J 1{Su mozgs
+ S u csillapt
s } + J S u aktv + hccgf

(9.37)

A nyomatkot most is a decentralizlt ramszablyozsok valstjk meg az

~
als hierarchiaszinten, amelyek -val arnyos ia alapjelet kapnak.

120

Lantos: ROBOTOK IRNYTSA

9.6 Robotok nhangol adaptv irnytsa

A robotok dinamikus modelljben szerepl inercia paramterek (tmegek, tmegkzppontok, tehetetlensgi nyomatkok) rendszerint nem ismertek, ezrt a dinamikus
modellen alapul korszer robotirnytsi algoritmusok megvalstsakor szksg
van ezek megbzhat becslsre. Egy lehetsges megolds a robotok nhangol
adaptv irnytsa (selftuning adaptive control), lsd 9.1. bra.

Design

Estimation

Controller
parameters
uc
Controller

Process

9.1. bra. nhangol adaptv irnyts (STAC) elvi smja

Rendszerint vlaszthatk olyan fggetlen 1 , 2 , K paramterek, amelyek a robot dinamikus modelljben linerisan fordulnak el. Pldul a ktszabadsgfok robotkar esetn (az ott szerepl jellsekkel) paramterknt vlaszthat

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 .

(9.38a)

Ezek a paramterek linerisan fordulnak el a robotkar modelljnek Lagrange-fle

alakjban:

9. KORSZER ROBOTIRNYTSI MDSZEREK

121

D11 = 1 + 2C 2 2 ,
D12 = 3 + C 2 2 = D21 ,
D 22 = 3 ,
D111 = 0,
D112 = S 2 2 = D121 = D122 ,

(9.38b)

D 211 = S 2 2 ,
D 212 = D 221 = D 222 = 0,
D1 = C1 4 + C12 5 ,
D 2 = C12 5 .

Itt az elterjedt rvidtett jellseket alkalmaztuk, pldul C12 = cos(q1 + q 2 ) , ahol

q i az i -edik csuklvltoz. rjuk fel a dinamikus modellt

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

k

(9.39)

alakban, ahol z k ksbb kerl megvlasztsra. A fenti paramtervlasztssal a dinamikus modell a kvetkez alak lesz:

1 = D11 &z&1 + D12 &z&2 + ( D111 q&1 + D121 q& 2 ) z&1 + ( D112 q&1 + D122 q& 2 ) z& 2 + D1 =
= ( 1 + 2C 2 2 ) &z&1 + ( 3 + C 2 2 )&z&2 + ( S 2 2 )(q&1 z& 2 + z&1 q& 2 ) +
+( S 2 2 )q& 2 z& 2 + (C1 4 + C12 5 ),

(9.40a)

2 = D 21 &z&1 + D 22 &z&2 + ( D211 q&1 + D 221 q& 2 ) z&1 + ( D 212 q&1 + D222 q& 2 ) z& 2 + D2 =
(9.40b)
= ( 3 + C 2 2 )&z&1 + 3 &z&2 + ( S 2 2 )q&1 z&1 + C12 5 .
Az gy kapott dinamikus modellt felrhatjuk paramterektl fggetlen Yij jelekbl
ll Y mtrix s j paramterekbl ll paramtervektor szorzataknt:

Y11 Y12
Y
21 Y22

ahol

Y13
Y23

Y14
Y24

1

2
Y15 1
= ,
Y25 3 2
4

5

(9.41)

122

Lantos: ROBOTOK IRNYTSA

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 ,

(9.42)

Y21 = 0,
Y22 = C 2 &z&1 + S 2 q&1 z&1 ,
Y23 = &z&1 + &z&2 ,
Y24 = 0,
Y25 = C12 .

A robot dinamikus modelljt az irnytsi trvny s az adaptcis trvny levezetsekor hromfle alakban is fogjuk hasznlni:
H (q )&z& + h(q, q& , z& ) = ,

(9.43a)

H (q )&z& + C (q, q& ) z& + D (q ) = , ahol C = [C ik ], C ik = Dijk (q)q& j ,

(9.43b)

Y (q, q& , z&, &z&) = .

(9.43c)

Vlasszuk meg a konstans mtrixot gy, hogy det( sI + ) = 0 s i gykeire

teljesljn Re s i < 0 . Jellje q d (t ), q& d (t ), q&&d (t ) a csuklvltoz elrt (desired)
alapjel idfggvnyt s derivltjait a robot megtervezett plyja esetn. Akkor ha
az e = q d q hibajel kielgti az e& + e = 0 differencilegyenletet, akkor e(t ) 0
exponencilisan megvlasztsval befolysolhat sebessggel. Az alapjel idfggvny helyett bevezetjk annak a hibaintegrl -val slyozott rtkvel korriglt rtkt, amelyet referenciajelnek fogunk nevezni:
q r := q d + (q d q)dt ,
q& r = q& d + (q d q ),
q&&r = q&&d + (q& d q& ).

(9.44)

Vegyk szre, hogy az

s := q& r q& = q& d q& + (q d q) = e& + e

(9.45)

vlasztssal elegend biztostani az irnytsi trvnnyel, hogy s = 0 llandsuljon,

mert akkor e(t ) 0 exponencilis sebessggel, ami azt jelenti, hogy eltnik a

9. KORSZER ROBOTIRNYTSI MDSZEREK

123

q d (t ) s a q(t ) kztti hiba egy tranziens utn. A kvetkez irnytsi trvnnyel,

amely felfoghat a kiszmtott nyomatkok mdszere (nemlineris sztcsatols a
csuklk terben), a cssz szablyozs (sliding control) s a PID szablyozs tvzetnek, egy tranziens utn s (t ) 0 biztosthat. Megjegyezzk, hogy az irnytsi
trvny ugyan kzvetlenl csak PD komponenst mutat a szablyozban, de ez csak
ltszlagos, mert az I komponens megtallhat q r -ben. Az irnytsi trvny nyomatk kimenet, amelyet az als irnytsi hierarchia szinten gyors tranziens ramszablyozsokkal lehet megvalstani (DC motorok esetn az ram arnyos a nyomatkkal). Az irnytsi trvny felhasznlja a robot paramterek aktulis becslst.
Irnytsi trvny:

:= H (q)q&&r + C (q, q& )q& r + D (q) + K p (q r q) + K D s =

= Y (q, q& , q& r , q&&r ) + K p (q r q ) + K D s

(9.46)

( K p > 0, K D > 0)
A zrt rendszer az irnytsi trvnnyel
H (q )q&& + C (q, q& )q& + D(q ) = H (q )q&&r + C (q, q& )q& r + D (q ) + K p (q r q) + K D s ,

ahonnan kvetkezik
Hq&& = H q&&r + C q& r + D Cq& D + (Cq& r Cq& r ) =
142
4 43
4
0

= H q&&r + (C C )q& r + ( D D) + C (q& r q& ).

123

(9.47)

Megmutatjuk, hogy a zrt rendszer Ljapunov-rtelemben stabil. Ljapunovfggvnyknt vlasszuk a q r -hez kpest rtelmezett hiba "kinetikus s potencilis
1
energijt" annak analgijra, hogy a robot kinetikus energija < Hq& , q& > , egy k
2
1 2
rgllandj rg potencilis energija pedig kx :
2
1
1
1
(9.48)
V := < H (q ) s, s > + < K p (q r q), q r q > + < ( ), > .
2
2
2
A Ljapunov fggvny > 0 slyoz mtrixszal bnteti az paramterbecslsi hibt is. A Ljapunov fggvny derivltja

124

Lantos: ROBOTOK IRNYTSA

1
dV
= < Hs&, s > + < H& s, s > + < K p (q& r q& ), q r q > +
2
dt
.
&
&

+ < ( ), >

(9.49)

Mivel Hs& = Hq&&r Hq&& , ezrt (9.47) felhasznlsval kapjuk, hogy

Hs& = ( H H )q&&r + (C C )q& r + ( D D ) K p (q r q ) K D s Cs ,

(9.50)

ahonnan kvetkezik
dV
= < ( H H )q&&r + (C C )q& r + ( D D ) K p (q r q ) K D s Cs, s > +
144444424444443
dt
Y ( )

1 &
< Hs, s > + < K p (q& r q& ), q r q > + < (& & ), >=
2
1
= < K D s, s > + < ( H& 2C ) s, s >+ < , Y T s + (& & ) > .
2 1442443
+

(9.51)

Felhasznltuk, hogy H& 2C antiszimmetrikus mtrix, ezrt kvadratikus alakja tetszleges vltoz, gy s esetn is azonosan nulla. Ezrt vlaszthat
Y T s + (& & ) = 0 , ahonnan = const valdi robot paramterek esetn & = 0
miatt a kvetkez adaptcis trvnyt kapjuk a robotparamterek hangolsra:
Adaptcis trvny:
d
= 1Y T (q, q& , q& r , q&&r ) s .
dt

(9.52)

dV
= < K D s, s >
dt

(9.53)

Mivel ekkor

negatv, ha s 0 , ezrt V addig cskken, amg s = 0 be nem kvetkezik, de ekkor

a hiba exponencilisan nullhoz tart.
Megjegyzs:
i) Minden alkalommal, amikor a robot megfog egy trgyat (CLOSE), vagy elenged
egy trgyat (OPEN), a robot valdi paramtervektora j konstans rtket vesz
fel a kvetkez mozgsperidusra, s az nhangol adaptv irnytsnak egy rvid tranziens utn meg kell tallnia a j becslst.

10. MOBILIS ROBOTOK IRNYTSA

125

ii) Br K p nincs hatssal a stabilitsra (kiesik dV / dt -ben), az tranziensre hatssal van. ltalban a nagy hibk miatt a rotorram korltozsok megszlalhatnak, ha mg semmifle informcink sincs a robot paramtereirl. Kezdetben
ezrt clszer kikapcsolni K p -t a hiba hatsnak cskkentse rdekben a
beavatkoz jelben, s K p = 0 mellett el-identifiklni a robot szegmensek ismeretlen inercia paramtereit (nulla vagy nvleges teher esetn).
iii) Ksbb, amikor mr csak a vltoz teher miatti kisebb paramtervltozsokra
kell reaglni s ezrt a hiba is kisebb, K p -t clszer bekapcsolni a paramterhangols tranziens idejnek cskkentse rdekben.

126

Lantos: ROBOTOK IRNYTSA

10. MOBILIS ROBOTOK IRNYTSA

Egy kerekeken gurul, kormnyozhat mobilis robot (mobilis platform) vzlatos
felptst mutatja a 10.1. bra.

yb
v

xb

10.1. bra. Mobilis robot vzlatos felptse

A mobilis robot tmegkzppontjnak koordintit jellje ( x, y ) a bzis koordinta-rendszerben, orientcijt pedig a bzis koordinta-rendszer x b tengelyhez
kpest jellje . Legyen a mobilis robot lineris sebessge v , szgsebessge pedig
. A mobilis robot kinematikai modellje
x& = v cos
x& C

y& = v sin y& = S
& 0
& =
{

0
v
0 z& = G ( z )u .

1 {

(10.1)

z&

Feltesszk, hogy az irnyts alacsonyabb hierarchiaszintjn sebessg szablyozsok vannak megvalstva, ezrt az irnyts vizsglt szintjn v s tekinthetk
vezrl (beavatkoz) jelnek, amelyek alapjelknt (referencia jelknt) szolglnak az
alacsonyabb hierarchiaszint sebessg szablyozsai szmra.
Vizsgljuk meg, vajon lehet-e a z = 0, u = 0 egyenslyi pont krnyezetben lineris llapot-visszacsatolssal stabilizlni a mobilis robotot, s ha nem, akkor mi javasolhat. Ha a rendszert a z& = f ( z , u ) alakban kpzeljk el, akkor a z& = Az + Bu
linearizlt rendszer a (0,0) egyenslyi pont krnykn a kvetkez mdon hatrozhat meg:

10. MOBILIS ROBOTOK IRNYTSA

127

0 0 0
C u1
0 0 S u1

f ( z , u ) = S u1 f z = 0 0 C u1
= 0 0 0 =: A,
u
0 0
0 (0,0) 0 0 0
2
C
f u = S
0

0
1 0

0
= 0 0 =: B,
1 (0,0) 0 1

1 0
z& = 0 0 u .
0 1
Vilgos, hogy a linearizlt rendszer irnythatsgi mtrixa
1 0 0 0 0 0
M c = 0 0 0 0 0 0 rank M c = 2 < 3 = dim z ,
0 1 0 0 0 0
ezrt a mobilis robot mg a (0,0) egyenslyi helyzet krnyezetben sem stabilizlhat llapot-visszacsatolssal. Ezrt nemlineris u = k (z ) , vagy nemlineris s idben vltoz u = k ( z , t ) visszacsatols javasolhat irnytsi stratgiaknt az egyenslyi pont krli helyzetszablyozsra (position control) vagy egy elrt plya kvet szablyozsra (tracking control). Mint ltni fogjuk, nem biztos, hogy a helyzetszablyozs a kvet szablyozs specilis eseteknt foghat fel, mert bizonyos stabilitsi kvetelmnyek az ll (nyugalmi) helyzetet kizrhatjk.

10.1 Mobilis robot irnytsa statikus visszacsatolssal

Legyen adott egy referencia robot plyja az

x& r = v r Cr
y& r = v r S r

(10.2)

&r = r
alakban, s legyen a szablyozsi hiba defincija
~
~
x = x xr , ~
y = y y r , = r .

(10.3)

128

Lantos: ROBOTOK IRNYTSA

Vezessk be a kvetkez (globlis) hibatranszformcit:

S 0 ~
x
x
~
e1 C
~

~
e 2 := S C 0 y = R( ) y ,
~
~
e 0
0 1

3
~
e1
x

~
T
y = R ( ) e 2 ,
e
~
3

(10.4)

ahol R( ) = Rot T ( z , ) , ami jl kihasznlhat lesz a ksbbiekben, mert R 1 = R T

s R& R 1 = [k ] . Hatrozzuk meg a transzformlt hiba llapotegyenlett:
&
~
x
~
e&1
e1
vC vr Cr
x
~

1
~
&
&
&
e&2 = R( ) y + R( ) y = R( ) R ( ) e2 + R( ) vS vr Sr =
~&
~
e&
e

r

3
3

e1 C S 0 vC vr Cr

= [k ] e2 + S C 0 vS vr Sr =
e 0
0 1 r
3
e1 C (vC vr Cr ) + S (vS vr S r )

= [k ] e2 + S (vC vr Cr ) + C (vS vr Sr ) =

e
r

3
0 0 e1 v vr Ce3

=
0 0 e2 + vr Se3 .
0
0 0 e3 r

(10.5)

Hajtsunk vgre bemenjel transzformcit is:

u1 := v v r C e3 ,
u 2 := r .

(10.6)

A transzformlt hiba llapotegyenlete a kvetkez alakban adhat meg:

e&1 0

e& 2 =
e& 0
3

0
0

0 e1 0
1 0

0 e 2 + S e3 v r + 0 0 1 .
u
0 1 2
0 e3 0

(10.7)

10. MOBILIS ROBOTOK IRNYTSA

129

Linearizls az e=0, u=0 egyenslyi helyzet krnykn

Feltesszk, hogy a referencia robot v r s r sebessge konstans. A (10.7) llapotegyenletet e& = f (e, u ) alakban elkpzelve a linearizls a kvetkez eredmnyt adja
az (e, u ) = (0,0) egyenslyi helyzet krnykn:

0
f e (e, u ) =
0

0
0

r
0

0 0 0
0
0

0 + 0 0 C e3 v r A := r
0
0 0 0
0

0
v r ,
0

1 0
f u (e, u ) = 0 0 =: B .
0 1

(10.8)

(10.9)

Ekkor
0
2
A = r
0

r
0
0

0 0
v r r
0 0

r
0
0

0 r2

v r = 0
0 0

0
r2
0

r vr

0
0

(10.10)

felhasznlsval a linearizlt rendszer M c = [ B AB A 2 B ] irnythatsgi mtrixa

1 0 0

M c = 0 0 r
0 1 0

0
vr

r2
0

r vr

0 ,
0

(10.11)

ezrt

rank M c = 3 = dim e , feltve, hogy r vagy v r nemnulla.

(10.12)

Ha (10.12) teljesl (ami csak kvet szablyozs esetn lehetsges, ll helyzetben,

pl. dokkolskor nem), akkor lehetsges lineris llapot-visszacsatolssal stabilizlni
a mobilis robotot konstans vagy lassan vltoz referencia robot sebessg esetn.
Lineris llapot-visszacsatols

u1 := k1 e1 ,
u 2 := k 2 sign (v r ) e 2 k 3 e3 .

(10.13)

130

Lantos: ROBOTOK IRNYTSA

A vlasztott llapot-visszacsatolssal a zrt rendszer llapotegyenlete

e&1 = r e 2 k1e1 , e& 2 = r e1 + v r e3 , e&3 = k 2 sign (v r ) e 2 k 3 e3 miatt a kvetkez
lesz:

k1
e& = r
0

r
0

0
~
v r e =: A e ,
k 2 sign (v r ) k 3

(10.14)

ahonnan kvetkezik

s + k1
~
sI A = r
0

0
s
v r ,
k 2 sign (v r ) s + k 3

~
det( sI A) = ( s + k1 ){s( s + k 3 ) + k 2 v r } + r { r ( s + k 3 )} .

(10.15)

Vlasszuk a kvetkez paramtereket az llapot-visszacsatolsban:

k 3 := k1 := 2a; k 2 =:

a 2 r2
,
vr

(10.16)

akkor a zrt rendszer karakterisztikus egyenlete a kvetkez lesz:

~
det( sI A) = ( s + k1 ){s 2 + k1 s + k 2 v r + r2 } = ( s + 2a )( s 2 + 2a + a 2 ) .

(10.17)

A zrt rendszernek ezrt egy dominns konjuglt komplex plusprja (csillaptsa s csillaptatlan sajtfrekvencija 0 = a , vals rsze 0 ) s egy ennl
gyorsabb vals plusa lesz (2 0 ) . Vegyk azonban szre, hogy ebben a formban az irnyts a helyzetszablyozs cljra nem alkalmazhat, mivel ekkor
v r = r = 0 miatt a linearizlt rendszer nem irnythat.
Sebessg sklzs
A fenti problmn a lineris rendszerek elmletre alapozva kvn segteni a sebessg sklzs. Ez egyrszt robusztusabb teszi az irnytst lassan vltoz r , v r
esetn, msrszt v r = r = 0 esetn is hasznlhat beavatkozst eredmnyez:

10. MOBILIS ROBOTOK IRNYTSA

131

a := r2 + bv r2 , b > 0,
k1 := 2 r2 + bv r2 ,
k 2 :=

a 2 r2 r2 + bv r2 r2
=
= b vr ,
vr
vr

(10.18)

k 3 := 2 r2 + bv r2 = k1 .
Vegyk szre, hogy az irnyts definilt minden v r , r esetn, specilisan

v r = r = 0 esetn k1 = k 2 = k 3 = 0 , s ezrt u1 = u 2 = 0 , teht v = = 0 , ami ll

helyzetben az elvrt beavatkozs.

10.2 Mobilis robot

visszacsatolssal

irnytsa

nemlineris

llapot-

A fenti mdszerek a linearizlt modellen alapultak, ezrt csak loklis stabilitst

eredmnyeznek, s nyitva hagyjk, hogyan fog viselkedni a rendszer nagy hiba esetn. Vizsgljuk ezrt meg, nem lehet-e az llapot-visszacsatols alakjt lnyegben
megtartva, de nemlineriss tve s kicsit korriglva rajta, a globlis stabilitsrl valamit mondani. Erre pozitv vlasz adhat a Barbalat-lemmra alapozva bizonyos
felttelek teljeslse esetn, amelyek azonban kizrjk a helyzetszablyozst.
Nemlineris visszacsatols

u1 := k1 (v r , r )e1 ,
u 2 := k 4 v r

S e3
e3

e 2 k 3 (v r , r )e 3 ,

(10.19)

ahol k1 (), k 3 () folytonos s szigoran pozitv fggvnyek, k 4 > 0 konstans. Mint

azt ksbb ltni fogjuk, a mdszer csak
lim v r (t ) 0 s lim r (t ) 0

(10.20)

egyttes teljeslse esetn hasznlhat (teht helyzetszablyozs cljra nem).

Legyen a Ljapunov-fggvny
V ( e) =

k4 2
1
(e1 + e 22 ) + e32 ,
2
2

akkor a transzformlt hiba (10.7) nemlineris llapotegyenlete alapjn

(10.21)

132

Lantos: ROBOTOK IRNYTSA

dV
= k 4 e1 ( e 2 + u1 ) + k 4 e 2 ( e1 + v r S e3 ) + e3 u 2 =
dt
= k 4 e1 ( e 2 k1e1 ) + k 4 e 2 ( e1 + v r S e3 ) + e3 {k 4 v r

S e3
e3

e 2 k 3 e3 } = (10.22)

= k1 k 4 e12 k 3 e32 0.

A Barbalat-lemma felhasznlsval megmutathat, hogy

(v r2 + r2 )ei 0 , ha t ,

(10.23)

de ebbl csak akkor kvetkezik, hogy az ei (t ) hiba tart nullhoz, ha teljesl

(10.20).

10.3 Mobilis robot irnytsa idben vltoz dinamikus

visszacsatolssal
A mobilis robotok irnytsra eddig bemutatott mdszerek vagy nem tudjk a globlis stabilitst biztostani, vagy pedig nem hasznlhatk egyszerre kvet s helyzetszablyozsra. Ltezik azonban olyan mdszer, amely idben vltoz dinamikus
llapot-visszacsatolssal a problmt meg tudja oldani.
Induljunk ki a 10.1. brn bemutatott mobilis robot (10.1-3) egyenletekkel adott
kinematikai modelljbl, a referencia robot plyjbl s a szablyozsi hiba defincijbl, de vezessnk be egy alkalmasabb hibatranszformcit, amely ugyan bonyolultabb, de elnysebb irnytstechnikai tulajdonsg transzformlt modellt
~
x, ~
y , ) T ( w, z1 , z 2 ) T = ( w, z T ) T hibatranszformci a
eredmnyez. Legyen az ( ~
kvetkez:
~
w C + 2 S

0
z1 =
z
C
2

~
S 2C
0
S

x
x
0 ~
~
~
~
1 y =: A y .
~
~
0

Mivel
S

det A = 2 s adj A = C
0

ezrt az inverz hibatranszformci

~
S 2C

~
C + 2 S ,

2
0

0
0

(10.24)

10. MOBILIS ROBOTOK IRNYTSA

A 1

1
2 S
1
= C
2

0
0
1

133

( z1 S + 2C )
2

1
( z1 C 2 S ) .
2

(10.25)

Legyen v1 := v s v 2 := . Alkalmazzuk a kvetkez v = (v1 , v 2 ) T u = (u1 , u 2 ) T

bemenjel transzformcit:
u1 = v 2 v 2 r ,
u 2 = v1 + ( ~
x S + ~
y C )v 2 v1r C~ ,

(10.26)

v 2r
1
0

, ahol T 1 =
u = T 1 v
.
~
~

~
v
C
x
S
y
C

+
1

1r

Az inverz bemenjel transzformci, amely szksges lesz a realizcihoz, mivel az

irnytst u -ban fogjuk megtervezni, a kvetkez:
v 2r
xS ~
y C
~
, ahol T =
v = Tu + T

v1r C~

1
.
0

(10.27)

Vegyk szre, hogy w R 1 s z , u , v R 2 .

Hatrozzuk meg a mobilis robot transzformlt llapotegyenlett:
~
~
w = ( C + 2S ) ~
x + ( S 2C ) ~
y,
~
z1 = ,
z =C ~
x+S ~
y,
2

(10.28a)

~&
~
~
w& = ( C + S & + 2C & ) ~
x + ( C + 2 S ) ~
x& + 2S +
~&
~
~
+( S C & + 2S & ) ~
y + ( S 2C ) ~
y& ,
~&
z&1 = ,
z& = S & ~
x +C ~
x& + C & ~
y+S ~
y& ,

(10.28b)

~
x& = x& x& r = v1C v1r Cr ,
~
y& = y& y& r = v1 S v1r S r ,
~&
= & &r = v 2 v 2 r .

(10.28c)

134

Lantos: ROBOTOK IRNYTSA

(10.28a-c) felhasznlsval kapjuk, hogy

~&
z&1 = = v 2 v 2 r = u1 ,

z& 2 = S v 2 ~
x + C (v1C v1r Cr ) + C v 2 ~
y + S (v1 S v1r Sr ) =
~
~
= v + ( S x + C y )v v (C C + S S ) =

1r

= v1 + ( S ~
x + C ~
y )v 2 v1r C~ = u 2 ,

~
w& = (v 2 v 2 r )(C ~
x + S ~
y ) + v 2 ( S ~
x C ~
y ) + 2v 2 (C ~
x + S ~
y) +
~
~
+ ( C + 2 S )(v1C v1r Cr ) + ( S 2C )(v1 S v1r S r ) =
= [(v 2 v 2 r ) + 2v 2 ](C ~
x + S ~
y)
~
~
~
{v1 + ( S x + C y )v 2 v1r (C Cr + S S r )}
2v1r ( S Cr C S r )
~
w& = (v 2 v 2 r )(C ~
x + S ~
y ) {v1 + ( S ~
x + C ~
y )v 2 v1r C~ } +
x+S ~
y) v S ~} =
+2{v (C ~
2r

1r

= u1 z 2 u 2 z1 + 2(v 2 r z 2 v1r S z1 ).
Vezessk be a kvetkez jellseket:
0 1
J :=
antiszimmetrikus, kvadratikus alakja < J , > 0, ,
1 0
f ( z ) := v 2 r z 2 v1r S z1 ,

(10.29a)
(10.29b)

akkor a transzformlt dinamikus modell a kvetkez egyszer alakot lti:

w& =< J u , z > + f ( z ),
z& = u .

(10.30)

Irnytsi algoritmus
Mrjk z hibjt egy ksbb megvlasztsra kerl s idben vltoz z d R 2 -hz
~
kpest: z := z d z . A transzformlt beavatkoz jelet idben vltoz, nemlineris,
dinamikus llapot-visszacsatolssal lltjuk el, amelynek paramterei ki kell, hogy
elgtsk a k1 , k 2 , 0 , 1 > 0 s min{k1 , k 2 } > 1 feltteleket ksbb indokoland
stabilitsi megfontolsokbl. Az idben vltozst

d (t ) := 0 exp ( 1t )
fogja okozni az irnytsi algoritmusban. Vegyk szre, hogy

(10.31a)

10. MOBILIS ROBOTOK IRNYTSA

&d (t ) = 0 1 exp( 1t )

135

&d (t )
= 1 konstans.
d (t )

(10.31b)

A szablyozt a kvetkez komponensek alkotjk:

u := u a k 2 z ,
u a :=

k1w + f

d2

J z d + 1 z d ,

k w+ f

&
z&d := d z d + 1 2 + w1 J z d ,
d
d

(10.32)

z d (0) := d2 (0),
&
k w+ f
1 := k 2 + d + w 1 2 .

A dinamikus rszt a szablyozban z d kpviseli, llapotainak szma kett s idben vltoz. Problmt okozhatna d (t ) 0 miatt, ha az u (t ) irnytsban az
albbi kritikus jelek nem maradnnak korltosak, ezrt a stabilits mellett ezek korltossgt is bizonytani kell:
k1 w + f

d2

J zd ,

w ( k1 w + f )

d2

zd ,

w 2 ( k1 w + f )

d2

J zd .

(10.33)

A hrom kritikus jel kzl az els u a -ban, a msodik 1 -ben s a harmadik w 1

rvn z& -ban fordul el.
d

A dinamikus rsz vlasztst a kvetkez motivlja:

k w+ f
&
d
< z d , z d >= 2 < z d , z& d >= 2 < z d , d z d + 1 2
+ w 1 J z d >=

d
dt

d
&

= 2 d < z d , z d >,

mivel w, f , 1 R 1 s J kvadratikus alakja minden vltozban, gy z d -ben is

azonosan nulla. Ezrt rhat, hogy

&
d < zd , zd >
2d d
,
= 2 d dt =
d
d
< zd , zd >
ahonnan integrls utn kapjuk, hogy

136

Lantos: ROBOTOK IRNYTSA

ln < z d (t ), z d (t ) > ln < z d (0), z d (0) >= 2 ln [ d (t )] 2 ln[ d (0)] ,

innen pedig kvetkezik

z d (t )

= d2 (t ) s z d (0)

= d2 (0) .

(10.34)

~
Mivel f = 2(v 2 r z 2 v r1 S z1 ) s z = z d z , ezrt (10.34) felhasznlsval a kvetkez becsls adhat:

~
f 2{ v 2 r z 2 + v1r z1 } 4 v r z 4 v r ( z d + z ) .

(10.35)

A becslst a ksbbi vizsglatoknl fogjuk felhasznlni. Tekintsk ezutn a zrt

szablyozsi krt, teht a transzformlt modellt a szablyozsi algoritmussal, s
hasznljuk ki (10.34)-et s a J mtrix tulajdonsgait, gy az antiszimmetrit s azt,
hogy J T = J , J T J = I s J 2 = I .
Zrt szablyozsi kr llapotegyenlete
~
~
w& =< Ju , z > + f =< J [u a k 2 ( z d z )], z d z > + f =
~
~
=< J u a , z d z > + f =< Jz , u a > < Jz d , u a > + f =
k w+ f
~
=< J z , u a > < Jz d , 1 2 J z d + 1 z d >=

k w+ f
~
=< J z , u a > 1 2 < J T J z d , z d > + f =

~
=< J z , u a > k1 w,

k w+ f

&
~& &
z = z d z& = d z d + 1 2 + w 1 J z d u =

d
d

k w + f
k w+ f
&
~
= d zd + 1 2
+ w 1 J z d 1 2 J z d + 1 z d k 2 ( z d z ) =

d
d

k w + f

~
= k 2 z + wJ 1 2 J z d + 1 z d =
d

~
= k 2 z + w J u a .
~
A z llapotvltozra ttrve a zrt rendszer llapotegyenlete a kvetkez egyszer
alakot lti:

10. MOBILIS ROBOTOK IRNYTSA

~
w& = k1 w+ < J z , u a >,
~&
~
z = k 2 z + wJ u a .

137
(10.36)

Ezutn mr elvgezhetjk a stabilitsvizsglatot Ljapunov direkt mdszervel.

Ehhez egy alkalmas V Ljapunov-fggvnyt vlasztunk s megmutatjuk, hogy bizonyos felttelek betartsa mellett a nemlineris rendszer exponencilisan stabilis lesz.
Stabilitsvizsglat
V :=

1 2 1 ~ ~
w + < z, z > ,
2
2

dV
~ ~&
~
~
~
= ww& + < z , z >= w(k1 w+ < J z , u a >)+ < z ,k 2 z + wJ u a >=
dt
~ ~
= k1 w 2 k 2 < z , z >
min{k1 , k 2 }2 V ,

dV
2 min {k1 , k 2 } dt ln V (t ) ln V (0) 2 min {k1 , k 2 } t ,
V
V (t ) V (0) exp(2 min {k1 , k 2 }t ) .
~
Legyen := ( w, z T ) T R 3 , akkor V =

(10.37)

/ 2 , s ezrt gykt vonva

(t ) (0) exp( min {k1 , k 2 }t ).

(10.38)

Kvetkezmny:
~
w(t ), z (t ) L ( L a lnyegben korltos fggvnyek tere).
i)
~
z = z d z s z d (t ) = d (t ) L z d , z L .
ii)
~
iii)
z (t ), w(t ) L A 1 (t ) L ~
x (t ), ~
y (t ), (t ) L

iv)

(10.34), (10.35), (10.38) s d (t ) = 0 exp( 1t ) miatt min{k1 , k 2 } > 1

esetn lteznek alkalmasan megvlasztott 0 , 1 , 2 , 3 > 0 konstansok, hogy
teljesl
k1 w + f

d2

J z d 0 exp{[min{k1 , k 2 } 1 ] t} ,

w ( k1 w + f )

d2

z d 2 exp{[2 min{k1 , k 2 } 1 ] t} ,

138

Lantos: ROBOTOK IRNYTSA

w 2 ( k1 w + f )

d2

J z d 3 exp{[3 min{k1 , k 2 } 1 ] t} ,

kvetkezskpp u a (t ), z& d (t ), 1 (t ), u (t ) L .
Feltehet, hogy v r (t ) L , ezrt a fentiek alapjn s a T mtrix alakja miatt
v (t ) L is teljesl, amibl kvetkezik, hogy x& (t ), y& (t ), & (t ) L .
~
vi) z z + z d (0) exp( min{k1 , k 2 }t ) + 0 exp( 1t )
miatt
ltezik
~
0 > 0 , hogy ~x (t ) , ~y (t ) , (t ) 0 exp( 0 t ) , ahol 0 kezdeti llapot fgg

v)

s 0 nem fgg a kezdeti llapottl.

sszefoglalva megllapthat, hogy a rendszer exponencilisan stabilis, s az
sszes jel a rendszerben s a szablyozban korltos marad. Az irnytsi trvny
mind helyzetszablyozs, mind kvet szablyozs esetn alkalmazhat. A mdszer
tovbbfejleszthet, kis mdostssal a dinamikus modellre alapozva is megadhat
hasonl elv globlisan stabilis irnyts.