Lekcija 9: Upravljanje kretanjem mobilnimkretanjem ...jvelagic/laras/dok/Lekcijam9.pdf · 9. Upravljanje mobilnim robotom Zahtjevi za upravljanje kretanjem mobilnim robotom: Poznavanje

Lekcija 9: Upravljanje kretanjem mobilnimkretanjem mobilnim robotomrobotom

Prof dr sc Jasmin VelagićProf.dr.sc. Jasmin VelagićElektrotehnički fakultet Sarajevo

K l ij M h t ikKolegij: Mehatronika

2012/2013

9. Upravljanje mobilnim robotom

Zahtjevi za upravljanje kretanjem mobilnim robotom:Poznavanje kinematičkog odnosno dinamičkog modela 2/37Poznavanje kinematičkog, odnosno dinamičkog modela robota.Model interakcije između kotača i podloge.

2/37

Definiranje referentne trajektorije, odnosno zahtijevanog kretanja.Načini ostvarivanja zahtijevanog kretanja brzinsko i/iliNačini ostvarivanja zahtijevanog kretanja → brzinsko i/ili pozicijsko upravljanje.

Upravljački zakon koji zadovoljava zahtjeve.Up a jač a o oj ado o ja a a tje e

Problemi upravljanja kretanjem mobilnim robotom:Ne postoji izravan način mjerenja pozicije robota.p j j j p jPozicija se mora integrirati u vremenu.Neprecizna (netačna) estimacija pozicije → izazov broj 1 u mobilnoj robotici.

Upravljanje mobilnim robotom

Strategije upravljanja se temelje na:Kinematičkom modelu robota 3/37Kinematičkom modelu robota.

Dinamičkom modelu robota.

3/37

Kada su brzine kretanja mobilnog robota male:Koristi se kinematički model robota.

Primjenjuje se upravljanje brzinom kretanja lijevog i desnog kotača (regulator brzine).

U slučaju većih brzina kretanja i masa robota:Koristi se dinamički model robota.

Primjenjuje se upravljanje momentom.


Upravljanje zasnovano na dinamičkom modelu4/374/37

željena pozicija

Željena Regulator

željena brzina

Regulator Pogon Dinamika Kinematika

napon moment brzina

trajektorija pozicije g

brzine Pogon robota robota

struja pozicija


Upravljanje zasnovano na kinematičkom modelu5/37

Nestaje unutarnja petlja.5/37

željena pozicija

željena brzina napon moment brzina

Željena trajektorija

Regulator pozicije

Regulator brzine Pogon Dinamika

robota Kinematika

robota

pozicija struja

9.1. Upravljanje kinematičkim modelom

Kinematički model mobilnog robota sa diferencijalnim pogonom 6/37p g

Kinematičke jednadžbe:

6/37

Y

xm

ym

θC

v

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎤

⎢⎢⎢⎡

=⎥⎥⎥⎤

⎢⎢⎢⎡

ωθθ

vyx

0 )sin(0 )cos(

&

&

y

D 2r

Ad

⎦⎣⎥⎥⎦⎢

⎢⎣⎥

⎦⎢⎣

ωθ 1 0 &

⎥⎤

⎢⎡⎥⎥⎤

⎢⎢⎡

⎥⎤

⎢⎡ Rvv 2

121

XxO ⎥⎦

⎢⎣⎥⎥

⎦⎢⎢

⎣−

=⎥⎦

⎢⎣ Lv

DD11

22ω

Upravljanje kinematičkim modelomKombiniranjem prethodne dvije kinematičke jednadžbe dobiva se: 7/37

⎤⎡⎥⎥⎥⎤

⎢⎢⎢⎡

⎤⎡⎥⎤

⎢⎡⎥⎤

⎢⎡

⎥⎤

⎢⎡x

)i ()i (2

)cos( 2

)cos(1 10 )cos(

θθ

θθ

θ&

7/37

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥

⎢⎢⎢⎢=⎥

⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣

=⎥⎥⎥

⎦⎢⎢⎢

⎣L

R

L

R

vv

vv

DD

y

112

)sin( 2

)sin(1- 1

22

1 0 0 )sin( θθθ

θ&&

Ulazi kinematičkog modela su linijske brzine lijevog i desnog

⎥⎥⎦⎢

⎢⎣

⎦⎣DD

-

g j j g gkotača.Izlazi kinematičkog modela su koordinate pozicije (x,y) mobilnog robota i orijentacija (ugao θ)mobilnog robota i orijentacija (ugao θ).Neholonomna ograničenja (nemogućnost bočnog kretanja):

0cossin =−+ θθθ &&& dyx

Upravljanje kinematičkim modelomZahtjev koji se postavlja na upravljanje je da pogreška slijeđenja eksponencijalno teži ka nuli kao funkcija vremena,

8/37tj.: 8/37

0=+ Keedtd

(*)

Pogreška pozicije je definirana kao:

dt

Pogreška pozicije je definirana kao:

⎥⎤

⎢⎡

⎥⎤

⎢⎡ xxr

⎥⎦

⎢⎣

−⎥⎦

⎢⎣

=−=yyr

rr xxe

Uvjet (*) je zadovoljen za sljedeći zakon upravljanja:

rr xxxKv &+−= )( K mora biti pozitivno definitnamatrica.

Upravljanje kinematičkim modelomDrugim riječima, potrebno je da vrijedi:

9/370, 21 >kk

9/37

gdje su k1 i k2 elementi matrice K:

⎤⎡ 0k⎥⎦

⎤⎢⎣

⎡=

2

1

00k

kK

Na temelju prethodnog upravljačkog zakona, brzine lijevog i desnog kotača postaju:

RvxxxKvJ &+−==⎥

⎤⎢⎡ − )(1 Matrica Jacobiana J mora

rrLv

xxxKvJ +==⎥⎦

⎢⎣

)(biti nesingularna.

Upravljanje kinematičkim modelomStruktura sistema upravljanja

10/37

⎤⎡

10/37

⎥⎦

⎤⎢⎣

⎡=

L

R

vv

v⎥⎦

⎤⎢⎣

⎡=

r

rr y

xx

xxe −= r

⎥⎦

⎤⎢⎣

⎡=

yx

x

Regulator Mobilni robot

r

Upravljanje kinematičkim modelomRezultati upravljanja

11/37

3

4 11/37

1

2

0

y [m

] stvarnareferentna

-2

-1

4

-3

-6 -4 -2 0 2 4 6-4

x [m]


12/37

4

5 referentnastvarna

12/37

2

3

]

0

1

oord

inat

a [m

]

3

-2

-1

X k

o

-5

-4

-3

0 20 40 60 80 100 120 140-5

Vrijeme [s]


13/37

3

4 referentnastvarna

13/37

2

m]

0

1

koor

dina

ta [m

-1

Y k

-3

-2

0 20 40 60 80 100 120 1403

Vrijeme [s]


14/37

150

200 referentnastvarna

14/37

50

100

]

0

50

enta

cija

[deg

-100

-50Orij

e

200

-150

0 20 40 60 80 100 120 140-200

Vrijeme [s]


15/37

0.7

0.8 15/37

0 5

0.6

taca

[m]/s

0.4

0.5

na d

esno

g ko

t

0.2

0.3

Lini

jska

brz

in

0 20 40 60 80 100 120 1400

0.1

0 20 40 60 80 100 120 140

Vrijeme [s]


16/37

0.6

0.7 16/37

0.4

0.5

taca

[m/s

]

0.3

0.4

na li

jevo

g ko

0.1

0.2

Lini

jska

brz

in

0 1

0

0 20 40 60 80 100 120 140-0.1

Vrijeme [s]

9.2. Upravljanje dinamičkim modelom

Sistem upravljanja dinamičkim modelom mobilnog robota (Lačević, Velagić, 2005). 17/37( , g , )

d q& v

17/37

dtd

Tr rq

rv

Regulator brzine

Dinamika robota i pogona

τ euu q& Tv

ve qr

Tp

f(e,vr,k)

q Nelinearni

regulator pozicije

)(qSq&

∫q& )(q∫q

Upravljanje dinamičkim modelom

Dinamički model mobilnog robota sa diferencijalnim pogonom. 18/37p g

Dinamičke jednadžbe:

18/37

Y

xm

ym

vLtrLLR

RtrRLR

KAB

KBA

θτθθ

θτθθ&&&&&

&&&&&

−=+

−=+

y

θ

Ad

CLtrLLR

⎟⎟⎞

⎜⎜⎛

++= 021

22

IIrmrAD 2r

d ⎟⎠

⎜⎝

024 D

⎟⎟⎞

⎜⎜⎛

−= 122 IrmrB

X xO

⎟⎟⎠

⎜⎜⎝

24 DB

Upravljanje dinamičkim modelomOpćenito, zadatak regulatora mobilnog robota je da slijedi referentnu trajektoriju qr, koja se mijenja brzinom .rq&

19/37Na temelju dinamičkog modela robota se dizajnira upravljački zakon za pogonske momente mobilnog robota, tako da trajektorija mobilnog robota slijedi glatku zadanu trajektoriju.

19/37

Referentna i stvarna trajektorija opisane su izrazima:

Trrrr yxt ] [)( θ=q

Tyxt ] [)( θ=q

Mobilni robot treba da slijedi referentnu trajektoriju tako da apsolutne vrijednosti pogrešaka koordinata i ugla:

|)()(||| tt

|)()(||||)()(||||)()(|||

tttytyytxtxx

re

re

θθθ =−=−=

budu što je moguće manji, odnosno da teže ka nuli.

|)()(||| ttre θθθ −=

Upravljanje dinamičkim modelomZa potrebe slijeđenja referentne trajektorije često se koristi princip slijeđenja virtualnog (referentnog) robota.

20/37

Y referentni robot

20/37

x1 y1

θr

robot x

yr

θ

y y

θ

Xx xr

Upravljanje dinamičkim modelomPrincip slijeđenja virtualnog (referentnog) robota.

U lokalnom koordinatnom sistemu mobilnog robota 21/37pogreške koordinata iznose:

⎤⎡⎤⎡⎤⎡ θθ 0sincos

21/37

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

θθθθθθ

r

r

yyxx

eee

1000 cos sin

0 sin cos

2

1

odnosno:

⎥⎦⎢⎣⎥⎦⎢⎣⎥⎦⎢⎣ θθ re 1 0 0 3

odnosno:

)( qqTe −= rp

Da bi se dobio dinamički model greške slijeđenja potrebno je derivirati gornji izraz.


Deriviranjem se dobiva: 22/37

rrrr

yyvvxxvv

yyyyxxxxe

θθθθθθθθθθ

θθθθθθ

cos)(sin)sinsin(sin)(cos)coscos(

cos)(sin)(sin)(cos)(1

⋅−+−+⋅−−−=

⋅−+−+⋅−−−=&&

&&&&&&&

22/37

rrrrr

rrrrrr

rrrrrr

vvyyxx

yyvvxxvv

yyvvxxvv




)sinsincos(cos)sin(cos)cos)(sin)((

cos)(sinsinsinsin)(coscoscos

cos)(sin)sinsin(sin)(cos)coscos(

22

22

−++−−+−−=

⋅−+−+⋅−−−=

⋅−+−+⋅−−−=

&

&&

ere vvy θω cos +−=

yyyyxxxxe θθθθθθ sin)(cos)(cos)(sin)( ⋅−−−+⋅−−−−= &&&&&&&

rrrrrr

rrrrrr

rrrr

yyvvxxvv

yyvvxxvv

yyyyxxxxe

θθθθθθθθθθθθ


θθθθθθ

sin)(cossincossincos)(sincossincos

sin)(cos)sinsin(cos)(sin)coscos(

sin)(cos)(cos)(sin)(2

⋅−−−+⋅−−+−=

⋅−−−+⋅−−−−=

+=

&

&&

&&

ere

rrrrr

vxvyyxx

θωθθθθθθθ

sin )sincoscos(sin)sin)(cos)((

+−=−+−+−−=

ωωθθ −=−= rre &&&3


Prema tome, model pogreške slijeđenja glasi: 23/37p g j j g

θω +−= ere vvye1 cos& Dinamika pogreške

23/37

ωωθω

−=+−=

r

ere

evxe

3

2 sin&

& predstavlja nelinearan dinamički sistem

Izborom upravljačkih ulaza dobiva se:Tuu ] [ 21=u

.

312

121

sin eveeuee

r+−=+=

&

&

&

ωω

Zadatak je dizajnirati nelinearni upravljački zakon u=f(e)

23 ue =&

takav da cjelokupni sistem bude stabilan, te da pogreške pozicije i brzine konvergiraju ka nuli.

Upravljanje dinamičkim modelomSinteza regulatora brzine

Prije negoli se pređe na pronalaženje upravljačkog zakona 24/37za poziciju, izvršit će se sinteza regulatora brzine.

Unutarnja regulacijska petlja linijskih i ugaonih brzina.

24/37

PI regulator b i

Dinamika b t +

τ eu u q& Transformacija br ina

v

brzine robota + pogona brzina

.

Regulator brzine ima dva ulaza i dva izlaza.

Ulazi su greške linijskih i ugaonih brzina ev i eω, a izlazi momenti pogona lijevog i desnog kotača τL i τR.


Veza između vektora i brzine robota v data je relacijom:q&25/37

vqq )(S=&25/37

odnosno:

⎤⎡⎥⎤

⎢⎡ −

⎥⎤

⎢⎡ θθx sin cos&

⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥

⎦⎢⎢⎢

⎣

=⎥⎥⎥

⎦⎢⎢⎢

⎣ω

θθ

θ

vy

1 0 cos sin

&

&

Izrazi za linijske i ugaone brzine mobilnog robota su:

θω &

&&

=

+= 22 yxv

θω =


Multivarijabilni PI regulator linijske i ugaone brzine opisan je26/37sljedećom jednadžbom u Laplaceovoj domeni:

⎤⎡⎤⎡⎤⎡ )()()()( sesGsGrrsτ

26/37

⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡==⎥

⎦

⎤⎢⎣

⎡=

)()(

)()()()(

)()()()(

)(2221

1211

sese

sGsGsGsG

Drses

Dr

ss

s vu

L

R

ωττ

Gτ

gdje je ev(s) greška linijske brzine i eω(s) pogreška ugaonebrzine mobilnog robota.gElementi Gij(s) su odabrani na sljedeći način:

11D

)11()()11()(

)11()(,)11(2

)(2

2121

111

KGDKG

sTKsG

sTDKsG

ii

+=+=

)11()(,)11(2

)(2

2221

121 sTKsG

sTDKsG

ii

+−=+=


Regulacijska petlja brzine sa PI regulatorom:27/37

τRev

27/37

G11(s)R G12(s)

Dinamika robota + pogona

u eu τ v

G21(s) G22(s) τL

eω

Parametri PI regulatora su:K1 K2 Ti1 Ti2Parametri PI regulatora su:

Efikasnost projektiranog multivarijabilnog PI brzinskogregulatora testirat će se pomoću skokovitih i sinusoidalnih

100 60 20 20

regulatora testirat će se pomoću skokovitih i sinusoidalnihpobudnih funkcija, koje će respektivno predstavljati linijsku iugaonu brzinu.


Rezultati simulacija 28/37Rezultati simulacija

1.2 1.2

28/37

0 6

0.8

1

)

setpointresponse

0 6

0.8

1

s)

responsesetpoint

0.2

0.4

0.6

v (m

/s

0.2

0.4

0.6

w (r

ad/s

0 0.5 1 1.5 2-0.2

0

0 0.5 1 1.5 2-0.2

0

t (s) t (s)

Dobro slijeđenje refrentnih trajektorija linijskih i ugaonih brzina.Bolje slijeđenje ugaone brzine.

Upravljanje dinamičkim modelomSinteza regulatora pozicije

Kao regulator pozicije koristi se backstepping algoritam u 29/37sljedećem zapisu [Lačević & Velagić, 2006]:

)(11 xfpeu ⋅−= α

29/37

)(sin)(232

122

212

11

xgeqeveeq

pu r ⋅−+−= −αα

Gdje su p i q pozitivne realne konstante, α > 1 i f(x) i g(x) su funkcije vektora x∈Rm, m∈N, koje zadovoljavaju uvjet: ∃L>0: j , , j j j jf(x), g(x)≥L, ∀ x∈Rm.Ovaj upravljački zakon osigurava stabilnost slijeđenja

i ij (t i d k t [V l ić L č ić &pozicije (teorem i dokaz teorema su u [Velagić, Lačević & Osmić, “Nonlinear motion control of mobile robot dynamic model”, book chapter in Motion planing and control of mobile robot: New Challenges, Vienna, 2008].


Parametri nelinearnog backstepping algoritma su dobiveni 30/37primjenom genetskog algoritma [za više detalja pogledati

prethodno navedeno poglavlje u knjizi]:p = 1 9934 q = 0 0530 f = 9 8615 g = 2 9956

30/37

p 1.9934, q 0.0530, f 9.8615, g 2.9956, Sistem upravljanja pozicijom i brzinom robota u Simulinku:

exe1

ev τRrx τR

Xc

Ycθ

ey

eθ

e2

v

τRr

eθ

xr

yr

θr

τR θXc'

Yc'

θ'

xdθ/ddx/dtdy/dt

v

wθ

e3 ew τLr

R l t

wr

vr

Referent.

τL

xyx'y'

Di iky

v=Tv*q' em=Te*eq Regulator brzine

trajektor. Dinamikarobota

Backsteppingalgoritam


Rezultati simulacija31/37

Performanse regulatora pozicije će se testirati u slučaju slijeđenja trifolijumske trajektorije.

31/37

Upravljanje dinamičkim modelomRezultati simulacija

3 referentna

3

32/37

0

1

2

m)

stvarna

0

1

2

m)

32/37

-2

-1

0

x(m

-3

-2

-1y(m

referentnastvarna

0 5 10 15 20-3

t(s)

0 5 10 15 20

-4

t(s)

18 Odziv x koordinate Odziv y koordinate

10

12

14

16

d)

referentnastvarna

4

6

8

10

thet

a(ra

d

Orijentacija

0 5 10 15 200

2

t(s)


Odzivi linijskih i ugaonih brzina 33/37Odzivi linijskih i ugaonih brzina

4 2.5

33/37

3

3.5

1.5

2

1.5

2

2.5

v(m

/s)

0

0.5

1

w(ra

d/s)

0.5

1

1.5

-1

-0.5

0 5 10 15 200

t(s)0 5 10 15 20

-1.5

t(s)

Linijska brzina Ugaona brzina


Odzivi momenata 34/37Odzivi momenata

3 4

34/37

0

1

2

m)

1

2

3

m)

-2

-1

0

tauR

(Nm

-1

0

1

tauL

(Nm

0 5 10 15 20-4

-3

ti ( )0 5 10 15 20

-3

-2

time(s) time(s)

Moment desnog kotača Moment lijevog kotača

9.3. Navigacija robotaOpisani sistem upravljanja je na niskoj (ili pak srednjoj) razini – upravljanje brzinom i pozicijom.

35/37Referentnu trajektoriju generira visoka razina upravljanja.

visoka razina

35/37

q& Mjerenja

Fuzija podataka

gradnja mape

Planiranje kretanja

qr

razina

dtd Tr

rqrv Mjerenja

senzora

Regulator brzine

Dinamika robota i pogona

τ euu q& Tv

ve Tp

f(e,vr,k)

Regulator q pozicije niska razina

d j i)(qS

q&∫q&

srednja razina

Navigacija robotaNajvažniji problemi u navigaciji su:

Kretanje ka cilju. 36/37j jIzbjegavanje prepreka.

36/37

8

10 8

10

2

4

6

m)

2

4

6

) 6

-4

-2

0 y(m

6

-4

-2

0

y(m

-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

-6

x(m)-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

-6

x(m)

Navigacija robota

6

8

10

6

8

10

37/37

-2

0

2

4

6

y(m

)

-2

0

2

4

6

y(m

)

37/37

-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

-6

-4

( )-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

-6

-4

(x(m) x(m)

10 10

2

4

6

8

m)

2

4

6

8

m)

8

-6

-4

-2

0y(m

8

-6

-4

-2

0y(m

-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

x(m)-10 -8 -6 -4 -2 0 2 4 6 8 10-10

-8

x(m)

Documents

Lekcija 9: Upravljanje kretanjem mobilnimkretanjem ...jvelagic/laras/dok/Lekcijam9.pdf · 9. Upravljanje mobilnim robotom Zahtjevi za upravljanje kretanjem mobilnim robotom: Poznavanje