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 mobilnim robotom
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 mobilnim robotom
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]
Upravljanje kinematičkim modelomRezultati upravljanja
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]
Upravljanje kinematičkim modelomRezultati upravljanja
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]
Upravljanje kinematičkim modelomRezultati upravljanja
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]
Upravljanje kinematičkim modelomRezultati upravljanja
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]
Upravljanje kinematičkim modelomRezultati upravljanja
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.
Upravljanje dinamičkim modelomPrincip slijeđenja virtualnog (referentnog) robota.
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
Upravljanje dinamičkim modelomPrincip slijeđenja virtualnog (referentnog) robota.
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.
Upravljanje dinamičkim modelomSinteza regulatora brzine
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
θω =
Upravljanje dinamičkim modelomSinteza regulatora brzine
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
+−=+=
Upravljanje dinamičkim modelomSinteza regulatora brzine
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.
Upravljanje dinamičkim modelomSinteza regulatora brzine
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].
Upravljanje dinamičkim modelomSinteza regulatora pozicije
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
Upravljanje dinamičkim modelomSinteza regulatora pozicije
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)
Upravljanje dinamičkim modelomRezultati simulacija
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
Upravljanje dinamičkim modelomRezultati simulacija
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)
Top Related