177

SEMINARSKI RAD IZ KOLEGIJA VOENJE TEHNIKIH SUSTAVA
Optimalno pozicioniranje i praenje zadane

trajektorije mobilnim robotom
Igor Cizelj
Zagreb, 2008
Voenje tehnikih sustava
Sadraj
1 Uvod
2 Kinematiki model mobilnog robota
2.1 Kinematiki model mobilnog robota s diferencijalnim pogonom.................................4

2.2 Kinematiki model mobilnog robota sa stranjim pogonom ........................................5
3 Optimalno pozicioniranje primjenom optimalnog upravljanja
3.1 Optimalno upravljanje...7

3.2 Optimalno pozicioniranje mobilnog robota s diferencijalnim pogonom.....................10
3.3 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom ............................14
3.4 Optimalno pozicioniranje mobilnog robota s diferencijalnim pogonom uz zahtjev
minimalnog prijeenog puta..............................................................................................19
4 Praenje zadane trajektorije
22
4.1 Praenje zadane trajektorije mobilnim robotom s diferencijalnim pogonom pomou

regulatora na bazi kuta orijentacije ...............................................................................22
4.2 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom pomou
regulatora na bazi kuta orijentacije i kuta zakreta kotaa .........................................27
4.3 Praenje zadane trajektorije mobilnim robotom s diferencijalnim pogonom uz zahtjev
minimalnog odstupanja..32
4.4 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom uz zahtjev
4.5 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom .39
5 Zakljuak
43
6 Matlab kodovi
44
6.1 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom.............................44

6.2 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom pomou
regulatora na bazi kuta orijentacije i kuta zakreta kotaa .........................................48
6.3 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom uz zahtjev
6.4 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom..54
2
1 Uvod
U skolpu ovog seminara prikazati e se nekoliko naina upravljanja mobilnim robotom.

Seminar e se orijentirati na dvije vrste voenja robota, odnosno na dvije vrste gibanja
optimalno pozicioniranje i praenje zadane trajktorije.
Kod optimalnog pozicioniranja (Slika 1) mobilni robot mora iz poetne konfiguracije
(pozicija i orijentacija) postii eljenu konanu konfiguraciju. U sluaju praenja zadane
trajektorije (Slika 2), mobilni robot mora pratiti trajektoriju (u Kartezijevom
koordinatnom sustavu) startajui iz poetne konfiguracije koja moe biti ili na ili izvan
trajektorije.
Slika 1: Optimalno pozicioniranje
Slika 2: Praenje zadane trajektorije
2 Kinematiki model mobilnog robota
Seminar je koncipiran tako da se svaka metoda upravljanja prvo objasni na jednostavnom

modelu mobilnog robota s diferencijalnim pogonom, a zatim da se ista metoda primjeni
na neto sloenjem modelu mobilnom robotu sa stranjim pogonom (rear-wheel
driving).
2.1 Kinematiki model mobilnog robota s diferencijalnim pogonom

Kinematiki model mobilnog robota s diferencijalnim pogonom identian je
kinematikom modelu monocikla prikazanog na Slici 3.
Slika 3: Monocikl
Ako uzmemo x y ravninu kao ravninu po kojoj se robot kree moemo zapisati
kinematiki model monocikla odnosno mobilnog robtoa s diferencijalnim pogonom
x cos
0

y = sin v1 + 0 v2
0
1
(1.1)
gdje su v1 i v2 linearna brzina mobilnog robota u x y koordinatnoj ravnini, odnosno

kutna brzina zakreta osovine mobilnog robota. x i y koordinata predstavljaju poziciju
robota u x y koordinatnoj ravnini , a je kutni zakret mobilnog robota u odnosu na x

koordinatnu os, odnosno njegova orijentacija.
2.2 Kinematiki model mobilnog robota sa stranjim pogonom

Mobilni robot sa stranjim pogonom (Slika 4.) ima istu kinematiku kao automobil. Radi
jednostavnosti pretpostavlja se da se dva kotaa na svakoj osovini (prednjoj i stranjoj)
mogu opisati jednim kotaem koji se nalazi tono na sredini osovine. Pozicija i
orijentacija mobilnog robota opisana je vektorom q = [ x, y, , ] , gdje su x i y koordinate
zadnjeg kotaa u Kartezijevom koordinatnom sustavu, predstavlja orijentaciju robota u

odnosu na x koordinatnu os, a je kut zakreta osovine prednjeg kotaa s obzirom na
uzdunu os mobilnog robota.
Slika 4: Mobilni robot sa stanjim pogonom
Kinematiki model, tako definiranog mobilnog robota je:
x cos
0

y = sin v + 0 v
tan / l 1 0 2

0
1

(1.2)
gdje su v1 i v2 linearna brzina mobilnog robota u x y koordinatnoj ravnini, odnosno

kutna brzina zakreta prednje osovine mobilnog robota. Model ima problem singularnosti
za vrijednost = / 2 . Za tu vrijednost tan nije definiran. To odgovara situaciji u
kojoj bi automobil bio zaglavljen kada bi prednji kotai bili okomiti na uzdunu os
automobila. U stvarnim izvedbama problem singularnosti je umanjen, zbog ogranienja
kuta . l je meuosovinski razmak, odnosno udaljenost izmeu prednje i stranje osovine
mobilnog robota.
3 Optimalno pozicioniranje primjenom optimalnog upravljanja
3.1 Optimalno upravljanje
Temelji teorije optimalnog upravljanja postavljeni su krajem pedesetih godina radovima

Pontryagina i Bellaman, da bi do kraja esdesetih teorija poprimila svoji konani oblik.
Paralelno s razvojem teorije razvijale su se i razliite numerike metode za rjeavanje
problema optimalnog upravljanja. Zbog matematike kompleksnosti nunih uvjeta
optimalnosti i neophodnosti primjene numerikih metoda teorija optimalnog upravljanja
nije zastupljena u praktinim primjenama s punim opsegom svojih mogunosti.
Iako je teorija optimalnog upravljanja u poetku svog razvoja imala gotovo iskljuivo
primjenu na problemima voenja tehnikih sustava, primjetan je zadnjih godina sve vei
trend njene primjene u ekonomiji menadmentu.
Zbog razloga to rjeenja upravljakih problema nisu jednoznana, treba uvesti
odgovarajue kvantitativne kriterije usporedbe kako bi se ta rjeenja mogla meusobno
usporeivati. Optimalnim rjeenjem se proglaava rjeenje koje na najbolji nain
zadovoljava kvantitativni kriterij usporedbe, koji se jo naziva funkcija cilja. Optimalno
rjeenje problema moe se dobiti preko minimalne (minimum energije, minimum
vremena, minimalno regulacijsko odstupanje, itd.) ili maksimalne vrijednosti kriterije
cilja (maksimalna produktivnost, maksimalna brzina, itd.)
Dakle, sve se svodi na to da se iz niza rjeenja upravljakih problema izdvoji rjeenje
koje zadovljava kriterij optimalnosti.
Matematiki model objekta upravljanja u prostoru stanja glasi
x (t ) = f ( x (t ) , u (t ) , t ) ,
x ( t0 ) = x0
t0 t t f
(1.3)
gdje je x n-dimenzionalni vektor stanja, u je m-dimenzionalni vektor upravljanja, a

f ( x, u ) je n-dimenzionalna kontinuirana nelinearna vektorska funkcija.
Uzmemo li opi kriterij opimalnosti
tf
J = x ( t f ) , t f + F ( x ( t ) , u ( t ) , t ) dt
t0
(1.4)
koji se naziva Bolzin problem, problem optimalnog upravljanja sastoji se u pronalaenju

vektorske funkcije u ( t ) koja minimizira funkciju cilja (1.4), pri emu je povezanost
izmeu x ( t ) i u ( t ) dana u sustavu nelinearnih diferencijalnih jednadbi (1.3). Ako
imamo specificirano konano stanje x f ,tada funkcija cilja (1.3) ne sadri vie funkciju
x ( t f ) , t f te poprima oblik
tf
J = F ( x ( t ) , u ( t ) , t ) dt
(1.5)
H ( x (t ) , u (t ) , (t ) , t ) = F ( x (t ) , u (t ) , t ) + T (t ) f ( x (t ) , u (t ) , t )
(1.6)
t0
Definira se Hamiltonian
gdje se funkcija H ( x ( t ) , u ( t ) , ( t ) , t ) naziva Hamiltonova jednadba, a komponente

vektora Lagrangeovi multiplikatori.
Uvoenjem jednadbe (1.6) u (1.5) jednadba (1.5) poprima oblik
tf
J = H ( x ( t ) , u ( t ) , ( t ) , t ) + T ( t ) x ( t ) dt
t0
(1.7)
Ako se uzme prva varijacija funkcije cilja u ovisnosti o varijaciji vektora upravljanja i
vektora stanja, oko optimalnog vektora upravljanja i vektora stanja, dobiva se
tf
J = x
T
x
T
t = t0
t =t f
H
H
+
+ x +
u dt
x
x
t0
(1.8)
iz rubnih uvjeta x ( t0 ) = x0 i x ( t f ) = x f , slijedi x ( t0 ) = 0 i x ( t f ) = 0 , a izraz (1.8)

poprima oblik
tf
H
H
J =
+ x +
u dt
x
x
(1.9)
iz ega slijede uvjeti optimalnosti
x=
H
x
(1.10)
(1.11)
H
=0
u
(1.12)
koji imaju definirano i poetno krajnje stanje

x (t f ) = x f
x ( t0 ) = x0 ,
(1.13)
Ovi uvjeti vrijede za sluaj bez ogranienja na vektor stanja i upravljanja. Sustav
jednadbi (1.10) - (1.12) moe se rijeiti ako je Hamiltonian kvadratina funkcija po
upravljakim varijablama. Ukoliko je Hamiltonian linearna funkcija po upravljakim
varijablama ili ne ovisi eksplicite o njima, te ako jo imamo ogranienja na upravljake
varijable, tada za odreivanje rjeenja koristimo Pontryaginov princip maksimuma:
U sluaju svih singularnih i neregularnih situacija ukljuujui i ogranienja

vektora upravljanja, izbor komponenti vektora upravljanja mora biti takav da
maksimizira (odnosno minimizira) vrijednost Hamiltoniana
H ( x (t ) , u (t ) , (t ) , t ) = F ( x (t ) , u (t ) , t ) + T (t ) f ( x (t ) , u (t ) , t )
(1.14)
gdje i dalje vrijede jednadbe
x=
H
x
(1.15)
3.2 Optimalno pozicioniranje mobilnog robota s diferencijalnim pogonom

Funkcija cilja u ovom sluaju, koristei Lagrangove multiplikatore kao ogranienja
diferencijalnih jednadbi u funkciji cilja, je slijedea
tf
1
J = ( k1v12 + k2 v2 2 )dt
20
Funkcija cilja pomnoena je s
(1.16)
1
, to e olakati daljnje raunanje, a ne utjee na
2
efikasnost metode.
Uvoenjem Hamiltonove jednadbe ista ima oblik
H=
1
1
k1v12 + k2 v2 2 + 1v1 cos + 2 v1 sin + 3v2
2
2
(1.17)
Da bi se dobile upravljake varijable v1 i v2 raspisuje se totalni diferencijal Hamiltoniana i

prema (1.12) dobivamo
H
= k1v1 + 1 cos + 2 sin = 0
v1
H
= k2 v2 + 3 = 0
v2
(1.18)
(1.19)
Iz jednadbi (1.18) i (1.19) dobivaju se upravljake varijable v1 i v2 kako slijedi

v1 =
1
k1
cos
v2 =
2
k1
sin
3
k2
(1.20)
(1.21)
Izrazi (1.20) i (1.21) predstavljaju upravljake varijable u ovisnosti o Lagrangeovim

multiplikatorima te teinskim faktorima k1 i k 2 .
Daljnjim diferenciranjem iz uvjeta optimalnosti (1.11) slijede diferencijalne jednadbe
Lagrangeovih multiplikatora
H
= 1 = 0
x
(1.22)
H
= 2 = 0
y
(1.23)
10
H
= 3 = 1v1 sin 2 v1 cos
(1.24)
Diferencijalne jednadbe (1.22) - (1.24), uz tri diferencijalne jednadbe koje slijede iz

kinematikog modela (1.1)
x = v1 cos
(1.25)
y = v1 sin
(1.26)
= v2
(1.27)
ine sustav od ukupno est diferencijalnih jednadbi. Sustav diferencijalnih jednadbi

(1.22) - (1.27) rjeava se numeriki koritenjem Matlaba. U Matlabu se koristi funkcija
BVP4C, koja rjeava probleme graninih vrijednosti. Kao poetne i krajnje uvjete zadaju
se x i y koordinate, tj. njegova pozicija te orijentacija mobilnog robota s obzirom na os x
os, kut . Teinski faktori postavljeni su k1 = 1 te k 2 = 5 .
Varijabla
Poetna vrijednost
Krajnja vrijednost
Sljede rezultati simulacije optimalnog pozicioniranja mobilnog robota s diferencijalnim

pogonom.
11
5.5
5
4.5
4
3.5
3
2.5
2
1.5
1
1
1.5
2.5
3.5
4.5
Slika 5: Gibanje robota prilikom pozicioniranja
Iz Slike 5 vidljiv je put kojim se je robot gibao prilikom pozicioniranja iz toke (1,1) u
toku (5,5). Poto je robot prvotno bio orjentiran za kut - u odnosu na x os, vidljivo je
da se je veliki dio puta gibao unatrag, te se je prije konanog pozicioniranja okrenuo za
(petlja, gore desno) kako bi bio orjentiran za 0 u odnosu na x os kako je bilo definirano
u krajnjoj vrijednosti za . To se moe zakljuiti i sa Slike 6 jer je vidljivo da je
upravljaka varijabla u1 dijelom negativna to znai da se je robot gibao unatrag.
Slika 6: Upravljake varijable
12
2
1
0
-1
Y
5
0
0
10
vrijeme (sec)
-0.6623
-1
-2
0
10
time (sec)
-0.6623
0
10
vrijeme (sec)
-0.5
-1
-0.6623
-4
0
10
-0.6623
vrijeme (sec)
-2
-3
-1.5
-2
10
time (sec)
-2.5
0
10
time (sec)
Slika 7: Varijable stanja i Lagrangeovi multiplikatori
Slika 6 prikazuje iznose veliina upravljakih varijabli, a Slika 7 prikazuje x i y poziciju

robota u vremenu, kao i njegovu orjentaciju, te iznose Lagrangeovih multiplikatora u
vremenu. Vidljivo je da su 1 i 2 konstantni u vremenu, to odgovara (1.22) i (1.23), jer
je derivacija konstanti jednaka 0.
13
3.3 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom
Cijeli postupak metode optimalnog upravljanja kod optimalnog pozicioniranja objanjen

je na prethodnom modelu a ovdje e se navesti samo konane jednadbe te rjeenja
simulacije.
Funkcija cilja:
tf
1
k1v12 + k2 v2 2 )dt
(
20
(1.28)
1
1
1
k1v12 + k2 v2 2 + 1v1 cos + 2 v1 sin + 3 tan v1 + 4 v2
2
2
l
(1.29)
J=
Hamiltonova jednadba:
H=
Upravljaka varijabla v1 :
H
1
= k1v1 + 1 cos + 2 sin + 3 tan = 0
l
v1
v1 = 1 cos 2 sin 3 tan

k1
k1
k1
(1.30)
Upravljaka varijabla v2 :
H
= k2 v2 + 4 = 0
v2
1
v2 = 4
k2
(1.31)
Diferencijalne jednadbe:
x = v1 cos
(1.32)
y = v1 sin
1
l
= tan v1
(1.33)
(1.34)
= v2
H
= 1 = 0
x
14
(1.35)
(1.36)
H
= 2 = 0
y
(1.37)
H
= 3 = 1v1 sin 2v1 cos
(1.38)
1 + cos ( 2 )
H
1
= 4 = 3v1
= 3v1
2
l cos
2l
(1.39)
Teinski faktori su postavljeni na k1 = 1, k2 = 10, duljina robota je 0.1m, a rubni uvjeti su

sljedei
Varijabla
Poetna vrijednost
Krajnja vrijednost
Dakle, mobilni robot sa stranjim pogonom treba iz toke (6,6), u kojoj je njegova
uzduna os odklonjena za kut
u odnosu na os x, doi u toku (1,2) s istom
orijentacijom. Prednji kotai su i u poetnoj i krajnjoj toki paralelni s uzdunom osi

robota.
15
Sljede rezultati simulacije:

8
2
0
Slika 7: Gibanje robota prilikom pozicioniranja
Iz Slike 7 vidljivo je da robot prvo dio puta vozi unatrag (poto je poetna orijentacija
robota
) , a zatim se pozicionira vonjom unaprijed. Oito je da to nije optimalno
gibanje s obzirom na prijeeni put (realno gledajui neoptrebna vonja unatrag

negativna upravljaka varijabla u1 na Slici 8), ali je optimalno gibanje za zadanu funkciju
cilja koja trai da se minimiziraju upravljake varijable.
16
6
4
2
0
0
2
0
vrijeme (sec)
0.2
0.1
vrijeme (sec)
-1
-2
0
-0.1
-3
0
-0.2
0
vrijeme (sec)
vrijeme (sec)
Slika 9: Varijable stanja
1.6393
0.5664
0.5664
1.6393
1.6393
1.6393
0
0.5664
0.5664
0
vrijem (sec)
vrijem (sec)
-2
-4
0
-2
0
vrijem (sec)
vrijem (sec)
Slika 10: Lagrangeovi multiplikatori
17
5.5
5
4.5
4
3.5
3
2.5
2
1.5
1
1
1.5
2.5
3.5
4.5
x ( 0 ) = 1, y ( 0 ) = 1, ( 0 ) = / 2
x (T ) = 5, y ( T ) = 5, (T ) = 0
5
4.5
4
3.5
3
2.5
2
1.5
1
0.5
1.5
2.5
3.5
4.5
x ( 0 ) = 1, y ( 0 ) = 1, ( 0 ) = / 5
x (T ) = 5, y (T ) = 5, (T ) = 0
Na ovoj stranici su dani rezultati dviju simulacija optimalnog pozicioniranja mobilnog
robota sa stranjim pogonom kod kojih su rubni uvjeti identini, a poetni se uvjeti
razlikuju samo u orijentaciji. Vidljivo je da se trajektorije gibanja koje se postiu
razlikuju ovisno o poetnim i rubnim uvjetima.
18
3.4 Optimalno pozicioniranje mobilnog robota s diferencijalnim pogonom uz

zahtjev minimalnog prijeenog puta
Kako je vidljivo iz prethodne simulacije, funkcija cilja (1.28), odnosno (1.16) ne

osigurava da mobilni robot prilikom pozicioniranja prijee minimalni put. Da bi se
ispunio taj zahtjev potrebno je proiriti funkciju cilja zahtjevom da se u svakom trenutku
minimizira odstupanje od krajnje pozicije. Nova funkcija cilja je
tf
J=
1
k1v12 + k2 v2 2 + k3 ( x x f
20
+ k3 ( y y f
)dt
(1.40)
pri emu su xf i yf koordinate krajnje pozicije. Time se u sustavu diferencijalnih jednadbi
(1.22) - (1.27) mijenjaju iznosi Lagrangeovih multiplikatora 1 i 2 . Novi sustav

diferencijalnih jednadbi je sljedei
1 = k3 ( x x f )
(1.41)
2 = k3 ( y y f )
(1.42)
3 = 1v1 sin 2v1 cos
(1.43)
uz identine diferencijalne jednadbe kinematikog modela
x = v1 cos
(1.44)
y = v1 sin
(1.45)
= v2
(1.46)
Teniski faktori su postavljeni na k1 = 1, k2 = 5 i k3 = 1, a poetni i rubni uvjeti su

sljedei:
Varijabla
Poetna vrijednost
Krajnja vrijednost
19
Sljede rezulati simulacija, na lijevoj strani rezulati simulacije u kojoj je koritena funkcija
cilja (1.16), te uz to odgovarajui sustav diferencijalnih jednadbi (1.22) - (1.27), a na
desnoj strani rezultati simulacije u kojoj je koritena funkcija cilja (1.40), te sustav
diferencijalnih jednadbi (1.41) - (1.46) ime bi se trebao osigurati optimalni put prilikom
pozicioniranja.
6
6.5
6
5.5
5.5
4.5
4.5
4
4
3.5
3.5
3
2.5
1
1.5
2.5
3.5
4.5
2.5
0.5
1.5
2.5
Slika 11: Pozicioniranje
1.0679
1.0679
1.0679
10
time (sec)
10
-1.5
0
vrijeme (sec)
0.8855
0.8855
0.8855
2
0
10
-4
0
0
0
10
vrijeme (sec)
5
10
time (sec)
10
vrijeme (sec)
-2
0
10
time (sec)
0.5
-2
10
0
0
0.8855
0
10
vrijeme (sec)
0.8855
time (sec)
vrijeme (sec)
2
0
10
4
3
-1
1
0
vrijeme (sec)
4.5
-0.5
0.5
3.5
Slika 12: Pozicioniranje
0
0
-5
time (sec)
10
-10
0
10
time (sec)
i Lagrangeovi multiplikatori
i Lagrangeovi multiplikatori
20
Usporedbom rezultata simulacije sa Slike 11 i sa Slike 12 oito je da nova funkcija cilja

(1.40) osigurava optimalni put prilikom pozicioniranje. Nema vie, realno gledajui,
nepotrebnih petlji prilikom gibanja robota. Sa Slike 14 vidljivo je da je gibanje robota u x
i u y smijeru glae te da Lagrangeovi multiplikatori 1 i 2 vie nisu konstante. Valja
napomenuti da su u oba sluaja funkcije upravljakih varijabli identine, ali razliitih
iznosa - (1.20) i (1.21).
21
4 Praenje zadane trajektorije
4.1 Praenje zadane trajektorije mobilnim robotom s diferencijalnim

pogonom pomou regulatora na bazi kuta orijentacije
Ve poznati kinematiki model diferencijalnog robota:
x = u1 cos
(1.47)
y = u1 sin
(1.48)
= u2
(1.49)
moemo matrino prikazati na sljedei nain
x cos

y = sin
0

0
u
0 1 ,
u
1 2
(1.50)
odnosno
X = G ( X )u
(1.51)
Uz neku referentnu trajektoriju X d ( t ) mogue je definirati upravljaku varijablu

u pomou pseudoinverza
u = G T ( X ) G ( X ) G T ( X ) X d ,
(1.52)
odnosno
u = G# ( X ) X d
(1.53)
U sluaju mobilnog robota s diferencijalnim pogonom pseudoinverz je sljedei

cos
G# =
0
sin
0
0
1
te je stoga upravljake varijable prema (1.53) mogue izraziti matrino kako sljedi
22
(1.54)
u1 cos
u = 0
2
sin
0

xd
0
yd
1
d

(1.55)
Iz (1.55) mogue je izraziti zasebno svaku upravljaku varijablu
u1 = xd cos + yd sin
(1.56)
u2 = d
(1.57)
Iz (1.47) i (1.48) mogue je postaviti omjer
yd
xd
sin d
= tan d
cos d
(1.58)
a iz (1.58) slijedi da je
d = atan
yd
(1.59)
xd
2
yd
1
J
Uz uvjet optimalnosti J = k atan , te ako vrijedi da je =
dobiva
2
x
d
se izraz
yd
= k atan
xd
(1.60)
Na temelju (1.56), (1.57) i (1.60) moe se definirati konaan oblik upravljakih varijabli
u1 = k p xd cos + yd sin
(1.61)
y
d
u2 = k atan
xd
(1.62)
Kao referentna trajektorija odabrana je Gaussova krivulja koja odgovara situaciji u kojoj
mobilni robot zaobilazi neku prepreku. Trajektoriju zadajemo pomou pozicije robota u
23
Kartezijevom koordinatnom sustavu xd = xd ( t ) , yd = yd ( t ) , t > t0 . Matematika

forumulacija Gaussove krivulje glasi
xd ( t ) = t
2
x (t ) x
yd ( t ) = e ( d 0 )
i kao takva koristiti e se kroz seminar. Slijede rezultati simulacije:

12
10
2
X
Xref
0
0
5
t,s
10
Slika 17: Praenje trajketorije u smjeru osi x

1.2
0.8
0.6
0.4
0.2
Y
Y
ref
-0.2
0
5
t,s
Slika 18: Praenje trajketorije u smjeru osi y
24
10
1
0.8
0.6
0.4
0.2
0
-0.2
-0.4
-0.6
ref
-0.8
-1
0
5
t,s
10
Slika 19: Praenje referentne orijentacije

1.2
0.8
0.6
0.4
0.2
-0.2
0
feedback
referentna trajektorija
2
Slika 20: Praenje trajektorije

25
10
12
Slike 17 i 18 prikazuju referentnu krivulju i gibanje mobilnog robota u smjeru osi x

odnsno u smjeru osi y, a Slika 19 prikazuje kako mobilni robot prati referentni kut
zakreta, odnosno orijentaciju.
Na Slici 20 vidljivo je praenje referentne linije mobilnim robotom u Kartezijevom
koordinatnom sustavu. Greke praenja, odnosno x xd , te y yd vidljive su na
sljedeoj slici
0.03
X-Xref
0.02
0.01
0
-0.01
0
5
t,s
10
5
t,s
10
Y-Yref
0.05
-0.05
0
Slika 21: Greke praenja u x i y smjeru

Iz Slike 21 vidljivo je da simulacija rezultira vrlo dobrim praenjem zadane trajektorije,
te da je greka jednaka 0 sve dok Gaussova krivulja ne poinje rasti. Ako se pogleda
tijeme Gaussove krivulje na Slici 20 vidljivo je da se krivulja i putanja gibanja robota
sijeku u jednoj toci. U toj toki greka je takoer jednaka 0 to odgovara minimumu oko
pete sekunde na plavoj krivulji odnosno infleksiji oko 5 sekunde na crvenoj krivulji na
Slici 21.
U simulaciji pojaanja k p i k postavljena su na 1 odnosno 20, a poetni uvjeti jednaki su
x ( 0 ) = 0 , y ( 0 ) = 0 te ( 0 ) = 0 .
26
4.2 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom

pomou regulatora na bazi kuta orijentacije i kuta zakreta kotaa
U ovom poglavlju navesti e se samo pseudoinverz, kako su definirane upravljake
varijable te rezultati simulacije, a postupak je identian onome u prethodnom poglavlju.
Kinematiki model jednak je onomu navedenom u drugom poglavlju (1.2) iz ega slijedi
njegov matrini zapis u obliku X = G ( X ) u :
x cos
sin
y =
tan
l
0

0
0
u1

0 u 2
(1.63)
Pomou simbolike matematike u Matlabu iz (1.63) dobiva se pseudoinverz G # ( X )
l 2 cos
cos 2 l 2 + sin 2 l 2 + tan 2
l 2 sin
cos 2 l 2 + sin 2 l 2 + tan 2
0
(1.64)
l tan
cos l + sin 2 l 2 + tan 2
0
2
Iz izraza (1.53) i (1.64) sljede upravljake varijable u obliku:
l 2 cos
l 2 sin
l tan
u1 = k f
x
+
y
+
d
d
d
2
2
2
2
2
cos 2 l 2 + sin 2 l 2 + tan 2
cos 2 l 2 + sin 2 l 2 + tan 2
cos l + sin l + tan
(1.65)
u2 =
(1.66)
Potrebno je kao i u prethodnom primjeru izvriti transformaciju kako upravljaka

varijabla u2 ne bi ovisila o derivaciji varijable stanja . Drugim rijeima potrebno je na
drugaiji nain izraziti .

Kao to je izveden izraz (1.60) moe se pokazati da vrijedi izraz
= k ( d )
27
(1.67)
Iz izraza x = cos u1 te izraza =
tan
u1 nakon transformacije dobiva se potrebni
l
izraz
d = atan
cos d d l
(1.68)
xd
Uvrtavanjem (1.68) u (1.67) dobiva se konani oblik , odnosno uprvljake varijable u2
cos
l
d
d
u 2 =k atan
xd
(1.69)
y
d
pri emu vrijedi izraz d = atan , odnosno d = k atan .
xd
xd
yd
Time su definirane obje upravljake varijable. Pojaanja sustava koja se koriste

postavljan su na k f = 100 i kt = 100 , a duljina mobilnog robota je l=0.02. Simulacija je
pokazala da to je manji robot to je bolje praenje zadane trajektorije. Stoga je duljina

mobilnog robota smanjena na l=0.002 i time je smanjeno trajno regulacijsko odstupanje
kod praenja vidljivo na sljedeoj slici poglavito u smjeru Y osi.
0.1
X-Xref
0.05
-0.05
0
5
t,s
10
5
t,s
10
0.15
Y-Yref
0.1
0.05
0
-0.05
0
Slika 22: Pogreke praenja kod mobilnog robota duljine l=0.02
28
Slijede rezultati simulacije praenja Gaussove krivulje mobilnim robotom sa stranjim

pogonom. Duljina mobilnog robota je l=0.02, pojaanja upravljakih varijabli su
k f = 100 i kt = 100 , a poetni uvjeti slijedei: x ( 0 ) = 0, y ( 0 ) = 0, ( 0 ) = 0 i ( 0 ) = 0 .

10
9
8
7
6
5
4
3
2
X
Xref
1
0
0
5
t,s
10
Slika 23: Praenje trajketorije u smjeru osi x

1.2
0.8
0.6
0.4
0.2
-0.2
0
Y
Yref
1
5
t,s
Slika 24: Praenje trajketorije u smjeru osi y
29
10
1
0.8
0.6
0.4
0.2
0
-0.2
-0.4
-0.6
-0.8
-1
0
ref
1
5
t,s
10
Slika 25: Praenje referentne orijentacije

1.2
0.8
0.6
0.4
0.2
-0.2
0
feedback
1
30
10
-3
15
x 10
X-Xref
10
5
0
-5
0
5
t,s
10
5
t,s
10
0.03
Y-Yref
0.02
0.01
0
-0.01
0
Slika 27: Greke praenja u x i y smjeru
Iz priloenih slika vidljivo je da je praenje referentne trajektorije uspjeno, no uz

postojanje trajnog regulacijskog odstupanja u smjeru osi Y nakon to mobilni robot proe
karakteristino zvono (Slika 26) u Gaussovoj krivulji. To odstupanje se smanjuje kako se
smanjuje dimenzija, odnosno duljina mobilnog robota (usporedba Slika 22 i Slika 27).
Ova metoda ima ogranienje da je potrebno unaprijed poznavati krivulju koja e se pratiti
kao i potrebu da se poetni uvjeti definiraju tako da mobilni robot bude pozicioniran i
orijentiran na samom poetku krivulje.
31
4.3 Praenje zadane trajektorije mobilnim robotom s diferencijalnim

pogonom uz zahtjev minimalnog odstupanja
Ovdje e se rijeiti problem da mobilni robot pratiti zadanu trajektoriju pomou

optimalnog upravljanja. Funkcija cilja koja e se primijeniti vrlo je slina funkciji cilja
(1.40). Ako se razmotre ta dva sluaja da mobilni robot optimalno prati zadanu
trajektoriju, te drugi, da se optimalno pozicionira, moe se vidjeti da su ta dva problema
vrlo slina. Naime, kod optimalnog pozicioniranja uz zahtjev optimalnog puta zahtjeva se
da mobilni robot u svakom trenutku naini pomak prema krajnjoj poziciji koji e
osigurati da odstupanje trenutne pozicije od krajnje pozicije bude minimalno. Identina je
ideja i kod praenja zadane trajektorije, pri emu se zahtjeva da odstupanje trenutne
pozicije od trenutne referentne pozicije bude minimalno. Dakle, dio izraza iz funkcije
cilja (1.40), k3 ( x x f
k3 ( x xref
+ k3 ( y y f
+ k3 ( y yref
, zamjenjuje se izrazom
) , pri emu su x
2
ref
i yref koordinate referentne pozicije
mobilnog robota u Kartezijevom koordinantnom sustavu.

Sljedi funkcija cilja za praenje zadane trajektorije mobilnim robotom s diferencijalnim
pogonom uz zahtjev minimalnog odstupanja:
tf
1
J = k1v12 + k2 v2 2 + k3 ( x xref
20
+ k3 ( y yref
)dt
(1.70)
Sustav diferencijalnih jednadbi je:
1 = k3 ( x xref )
(1.71)
2 = k3 ( y yref )
(1.72)
3 = 1v1 sin 2v1 cos
(1.73)
x = v1 cos
(1.74)
y = v1 sin
(1.75)
= v2
32
(1.76)
Funkcije upravljakih varijabli jednake su onima prije koritenim (1.20) - (1.21).

Prije no to se pokau rezultati simulacije potrebno je definirati krivulju, odnosno
referentnu trajektoriju koju e robot pratiti. I u ovom sluaju mobilni robot treba pratiti
Gaussovu krivulju.
Poetni i rubni uvjeti su postavljeni u skladu s nunim uvjetima optimalnog upravljanja
kada je specificirano poetno i nesprecificirano konano stanje. Poetni i rubni uvjeti su
x ( 0 ) = 0, y ( 0 ) = 0, ( 0 ) = 0, 1 ( 0 ) = 0, 2 ( 0 ) = 0, 3 ( 0 ) = 0
(1.77)
a pojaanja sustava su nakon nekoliko podeavanja postavljena na
k1 = 0.9, k2 = 0.9, k3 = 15 .
1.2
Y [m]
0.8
0.6
0.4
0.2
-0.2
0
feedback
1
X [m]
33
10
10
1
0.5
0.5
0
0
10
-0.5
-0.5
0
time (sec)
-1
0
10
time (sec)
0.5
-1
time (sec)
10
10
1
-0.5
time (sec)
-1
0
-2
0
time (sec)
10
-0.5
0
10
time (sec)
Slika 29: Varijable stanja i Lagrangeovi multiplikatori
Na Slici 28 vidljivo je da mobilni robot (plava krivulja) prati referentnu krivulju (crvena
krivulja), ali s odreenom grekom. Dakle, poto je u funkciji cilja ogranienje i na
upravljake varijable kao i zahtjev za minimalnim odstupanjem, rezultat simulacije je
svojevrstan kompromis na zahtjeve. Sa Slike 29 vidljivo je da poetni i rubni uvjeti
odgovaraju onima zadanim (1.77). Vidljivo je da je greka praenja vea nego u
poglevlju 4.1.
34
1.2
Y [m]
0.8
0.6
0.4
0.2
-0.2
0
feedback
1
10
X [m]
k1 = k2 = 20, k3 = 100, k3 / k1 = 5
1.2
Y [m]
0.8
0.6
0.4
0.2
-0.2
0
feedback
1
10
X [m]
k1 = k2 = 1, k3 = 20, k3 / k1 = 20
Na ovoj stranici dani su rezultati dviju simulacija praenja zadane trajektorije mobilnim
robotom s diferencijalnim pogonom uz zahtjev minimalnog odstupanja. Identini su
poetni uvjeti, ali razlika je u omjeru koeficijenata k3 / k1 . U prvom primjeru k3 je 5 puta
vei nego k1 a u drugom primjeru je k3 20 puta vei od k1. Ako se pogleda funkcija cilja
(1.70) to bi znailo da je u prvom sluaju manji naglasak na minimiziranje odstupanja
nego u drugom primjeru to je i podkrepljeno rezultatima simulacije jer je manje
odstupanje u drugom primjeru nego u prvom.
35
4.4 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom uz

zahtjev minimalnog odstupanja
U ovom poglavlju postupak iz prethodnog poglavlja primijeniti e se na mobilni robot sa

stanjim pogonom. Funkcija cilja je identina kao i u prethodnom primjeru
tf
J=
1
k1v12 + k2 v2 2 + k3 ( x xref
20
+ k3 ( y yref
)dt
(1.78)
Upravljake funkcije su
v1 =
1
k1
cos
sin
k1
3
k1
tan
1
l
(1.79)
1
4
k2
(1.80)
x = v1 cos
(1.81)
v2 =
Sustav diferencijalnih jednadbi je
y = v1 sin
1
l
= tan v1
(1.82)
(1.83)
= v2
(1.84)
1 = k3 ( x xref )
(1.85)
2 = k3 ( y yref )
(1.86)
3 = 1v1 sin 2v1 cos
(1.87)
4 = 3v1
1 + cos ( 2 )
2 l
(1.88)
Poetni i rubni uvjeti su postavljeni u skladu s nunim uvjetima optimalnog upravljanja

kada je specificirano poetno i nesprecificirano konano stanje. Poetni i rubni uvjeti su
36
x ( 0 ) = 0, y ( 0 ) = 0, ( 0 ) = 0, ( 0 ) = 0, 1 ( 0 ) = 0, 2 ( 0 ) = 0, 3 ( 0 ) = 0, 4 ( 0 ) = 0 (1.89)
a pojaanja sustava su nakon nekoliko podeavanja postavljena na
k1 = 10, k2 = 10, k3 = 10 , a duljina robota je l=0.02.
1
0.9
0.8
0.7
Y [m]
0.6
0.5
0.4
0.3
0.2
feedback
0.1
0
0
10
X [m]
Na Slici 30 je vidljivo praenje Gaussove krivulje mobilnim robotom sa stranjim

pogonom u Kartezijevom koordinatnom sustavu. Poetna konfiguracija mobilnog robota
odgovara poziciji krivulje u poetnom trenutku.
Slijede Lagrangeovi multiplikatori kao i varijable stanja u odnosu na vrijeme simulacije.
37
2
-5
0
-2
-10
0
-4
0
10
time (sec)
0.02
0.2
0.1
-0.02
-0.04
0
10
time (sec)
0
-0.1
-0.2
0
10
time (sec)
10
time (sec)
Slika 31: Lagrangeovi multiplikatori

10
0.8
0.6
5
0.4
0.2
0
0
0
0
10
time (sec)
10
time (sec)
-3
0.4
5
0
0.2
0
-5
-0.2
-0.4
0
x 10
-10
0
10
time (sec)
10
time (sec)
Ako se usporede Slika 28 i Slika 30 vidljivo je da je kod praenja referentne trajektorije

mobilnim robotom sa stranjim pogonom pogreka praenja vea u odnosu na mobilni
robot s diferencijalnim pogonom.
38
Pretpostavimo li da je zadana referentna trajektorija kao pozicija stranjeg kotaa

mobilnog robota u Kartezijevom koordinatnom sustavu
xd = xd ( t ) , yd = yd ( t ) , t t 0 .
(1.90)
Kinematiki model mobilnog robota sa stranjim pogonom poprima slijedei oblik
x d = vd 1 cos d
(1.91)
y d = vd 1 sin d
(1.92)
1
l
d = tan d vd 1
(1.93)
d = vd 2
(1.94)
Potrebno je rijeiti gornji sustav jednadbi kako bi se dobile upravljake varijable

vd 1 i vd 2 .
Iz (1.91) i (1.92) sljedi da je
2
vd 1 ( t ) = x d ( t ) + y d ( t )
(1.95)
gdje predzak ovisi o izboru da li e se praenje trajektorije izvriti s gibanjem unaprijed

ili unatrag.
Ako se jednadba (1.92) podijeli s (1.91) i uzme li se u obzir predznak linearne brzine
moe se izraziti eljena orijentacija mobilnog robota kao
yd (t ) xd (t )
d ( t ) = atan
,
vd 1 ( t ) vd 1 ( t )
(1.96)
Deriviranjem jednadbi (1.91) i (1.92), i kombiniranjem rezultata kako bi se eliminirala
varijabla v d 1 dobije se
y d (t ) x d (t ) x d (t ) y d (t )
d (t ) =
vd21 ( t )
Uvrtavanjem gornjeg izraza u (1.93) dobije se eljeni kut zakreta kotaa
39
(1.97)
l y d ( t ) x d ( t ) x d ( t ) y d ( t )
d ( t ) = atan
3
vd 1 ( t )
iji su rezultati u intervalu
(1.98)
( / 2, / 2 ) .
Na kraju, deriviranjem (1.98) i uvrtavanjem rezultata u (1.94) dobiva se druga

upravljaka varijabla
2

y d x d x d y d vd 1 3 y d x d x d y d x d x d y d y d
vd 2 ( t ) = l vd 1
2
vd61 + l 2 y d x d x d y d
(1.99)
Izrazi (1.95) - (1.99) ovise samo o vrijednstima referentne trajektorije (1.90) i o njezinim
derivacijama, i to do derivacije treeg reda. Dakle, da bi se zagarantirala funkcionalnost
ove metode potrebno je da referentna trajektorija bude tri puta derivabilna.
Kao primjer razmotriti e se krunica radijusa R kao referentna trajektorija
xd ( t ) = R sin t ,
yd ( t ) = R(1 cos t )
(1.100)
Dakle, nakon to je zadana referentna trajektorija, u Matlabu se moe izvriti simulacija.

Iz prethodnih izraza, kada se mobilni robot treba gibati ravno, upravljake veliine su
vd 1 ( t ) = R , vd 2 ( t ) = 0
(1.101)
i iz tih ogranienja slijede poetni uvjeti sustava koji osiguravaju praenje zadane
trajektorije.
xd ( t ) = 0, yd ( t ) = 0, d ( t ) = 0, d ( t ) = atan
40
l
,
R
(1.102)
20
20
15
10
10
0
X
Xref
-10
0
5
t,s
Y
Yref
5
0
0
10
5
t,s
10
10
8
6
4
ref
0
0
5
t,s
10
Slika 33: Praenje trajektorije u x smjeru, y smjeru i praenje referentne orijentacije

20
18
16
14
12
10
8
6
4
feedback
2
0
-10
-5
10
Slika 34: Praenje referentne trajektorije

41
15
-5
x 10
X-Xref
2
0
-2
-4
0
5
t,s
10
5
t,s
10
-5
x 10
Y-Yref
2
0
-2
-4
0
Slika 35: Greka odstupanja prilikom praenja trajektorije
Vidljivo je da je praenje zadane trajektorije vrlo dobro. Metoda se moe koristiti kada je
trajektorija unaprijed poznata i na osnovu nje je potrebno izvriti poetno pozicioniranje
robota.
42
5 Zakljuak
Ovim seminarom su se htjele obuhvatiti neke od metoda upravljanja mobilnim robotima,

orijentirajui se na optimalno pozicioniranje i na praenje zadane trajektorije.
Kod optimalnog pozicioniranja valja jo jednom istaknuti vanost funkcije cilja u samoj
metodi. Usporeujui optimalno pozicioniranje mobilnog robota s diferencijalnim
pogonom i optimalno poziciniranje mobilnog robota s diferencijalnim pogonom uz
zahtjev minimalnog prijeenog puta vidljivo je da se je koritenjem funkcije cilja (1.40)
mobilni robot pravilno pozicionirao a uz to imao i optimalni prijeeni put za razliku od
pozicioniranja kada je koritena funkcija cilja (1.16).
Na 18. stranici seminara prikazani su rezultati simulacije pozicioniranja mobilnog robota
sa stranjim pogonom kada su krajnja pozicija i orijentacija robota identine ali postoji
razlika u poetnoj orijentaciji, i time je potvrena ispravnost i tonost metode.
Na 35. stranici seminara usporeeni su utjecaji koeficijenata, odnosno pojaanja u samoj
funkciji cilja. Vidljivo je da je mogue utjecati na tonost i efikasnost metode koristei
razliita pojaanja, odnosno razliite omjere pojaanja u funkciji cilja.
Za kraj e se spomenuti poticaji i ideje autora koje su vodile i na kraju rezultirale ovim
seminarom. U dosadanjem tijeku studija uvelike sam se bavio mobilnim robotima. Do
sada sam napisao nekoliko seminara na temu mobilni roboti, te napravio nekoliko
fizikih modela mobilnih robota u kojima su koritene razliite metode upravljanja.
Bavio sam se problemom praenja linije mobilnim robotom, kao i problemom
izbjegavanja prepreka. Tema zavrnog rada mi je bila upravljanje mobilnom robotom u
interakciji s predmetom rada, gdje se je rjeavao problem guranja kutije mobilnom
robotm. Nedavno mi je fakultet omoguio i aktivno koritenje mobilnog robota Pioneer.
Cilj mi je neku od metoda spomenutih u ovom seminaru implementirati u Pioneer-a ili u
LEGO Mindstorm Set kojim se takoer bavim. Dakle, zbog svega navedenog htio sam se
bolje upoznati i istraiti metode upravljanja mobilnim robotom.
43
6 Matlab kodovi
Uz seminar su priloeni kodovi koji su koriteni za pozicioniranje odnosno za praenje
zadane trajektorije kod mobilnog robota sa stranjim pogonom :
6.1 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom

%Matlab datoteka ch3ex1
clc;
clear;
%----------Globalne varijable--------------------------------------global x_0 y_0 theta_0 phi_0 x_f y_f theta_f phi_f k1 k2 k3 l
k1=1;
k2=10;
k3=1;
l=0.10;
%---------Pocetni i rubni uvjeti---------------------------------x_0 = 6;
y_0 = 6;
theta_0 = -pi/2;
phi_0 = 0;
x_f = 1;
y_f = 2;
theta_f = -pi/2;
phi_f = 0;
%----------Vrijeme simulacije-----------------------------------T=6.0;
%----------Rjeavanje diff jednadbi---------------------------solinit = bvpinit(linspace(0,T,1000),@guess);
sol = bvp4c(@odes,@bcs,solinit);
xint = linspace(0,T,1000);
Sxint = deval(sol,xint);
%---------Funkcije za ispis rjeenja----------------------------Theta = Sxint(3,:); Phi = Sxint(4,:); Lambda1 = Sxint(5,:); Lambda2 = Sxint(6,:); Lambda3 = Sxint(7,:);
Lambda4 = Sxint(8,:);
u1 = -Lambda1.*cos(Theta)/k1 - Lambda2.*sin(Theta)/k1 - Lambda3.*tan(Phi)/(l*k1);
u2 = -Lambda4/k2;
44
%----------Ispis rjeenja----------------------------------figure(1)
subplot(2,2,1), plot(xint,Sxint(1,:), 'linewidth',1.5),
ylabel('X','FontSize',12,'FontName','Times'),
xlabel('vrijeme (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
ylabel('Y','FontSize',12,'FontName','Times'),
%
ylabel('\theta','FontSize',12,'FontName','Times'),
%
ylabel('\Phi','FontSize',12,'FontName','Times'),
%
figure(2)
ylabel('\lambda_1','FontSize',12,'FontName','Times'),
xlabel('vrijem (sec)','FontSize',12,'FontName','Times');
%
%
45
%
%=================================================
figure(3)
plot(Sxint(1,:), Sxint(2,:), 'linewidth',1.5),
xlabel('X','FontSize',12,'FontName','Times');
%=================================================
figure(4)
subplot(2,2,1), plot(xint,u1, 'linewidth',1.5),
ylabel('u_1','FontSize',12,'FontName','Times'),
%
ylabel('u_2','FontSize',12,'FontName','Times'),
%Matlab datoteka odes

function dydx = odes(x,y)
%----------Globalne varijable--------------------------------------global k1 k2 k3 x_f y_f l
%----------------------------------------------------------------------X = y(1); Y = y(2); Theta = y(3); Phi = y(4);
Lambda1 = y(5); Lambda2 = y(6); Lambda3 = y(7); Lambda4 = y(8);
%--------Upravljake varijable------------------------------------u1 = -(1/k1)*Lambda1*cos(Theta) - (1/k1)*Lambda2*sin(Theta) - (1/(k1*l))*Lambda3*tan(Phi);
u2 = -(1/k2)*Lambda4;
%---------Diff jednadbe-------------------------------------------f1 = u1*cos(Theta);
f2 = u1*sin(Theta);
f3 = (u1/l)*tan(Phi);
f4 = u2;
46
f5 = 0;
f6 = 0;
f7 = Lambda1*u1*sin(Theta) - Lambda2*u1*cos(Theta);
f8 = -Lambda3*u1/((1/2)*(1+cos(2*Phi)));
dydx = [f1; f2; f3; f4; f5; f6; f7; f8];
%Matlab datoteka guess

function v = guess(x)
global x_0 y_0 theta_0 phi_0 x_f y_f theta_f phi_f
v = [x_0; y_0; theta_0; phi_0; x_f; y_f; theta_f; phi_f];
%Matlab datoteka bcs

function res = bcs(ya,yb)
global x_0 y_0 theta_0 phi_0 x_f y_f theta_f phi_f
res = [ya(1)-x_0; ya(2)-y_0; ya(3)-theta_0; ya(4)-phi_0; yb(1)-x_f; yb(2)-y_f; yb(3)-theta_f; yb(4)-phi_f];
47

pomou regulatora na bazi kuta orijentacije i kuta zakreta kotaa
%Matlab datoteka Main

clear;
clc;
%-----------Globalne varijable-----------------------------T=10;
DT=0.01;
global kp kf Vx Ymax sigg Xxc
Vx=1;
Ymax=1;
sigg=2.0;
Xxc=Vx*T/2;
%-----------Potni uvjeti----------------------------------x10=0;
x20=0;
x30=0;
x40=0;
%----------Vrijeme simulacije---------------------------tT=0:DT:T;
%---------Referentna trajektorija------------------------xxc=Xxc*ones(1,length(tT));
Xref=Vx*tT;
Yref=Ymax*exp(-sigg*(Xref-xxc).*(Xref-xxc));
dXref=Vx;
dYref=-2*Vx*sigg*(Xref-xxc).*Yref;
ThetaRef=unwrap(atan2(dYref,dXref));
%---------Rjeavanje diff jednadbi--------------------options=odeset('RelTol',1e-8,'AbsTol',1e-8);
[t,y]=ode45('MobRobot',tT,[x10 x20 x30 x40],options);
%--------Ispis rjeenja--------------------------------------figure(1)
subplot(2,2,1),
plot(tT,y(:,1),'b',tT,Xref,':r','linewidth',1.5), legend('X','X_{ref}',4), xlabel('t,s')
subplot(2,2,2),
48
plot(tT,y(:,2),'b',tT,Yref,':r','linewidth',1.5), legend('Y','Y_{ref}',4), xlabel('t,s')

subplot(2,2,3),
plot(tT,y(:,3),'b',tT,ThetaRef,':r','linewidth',1.5), legend('\theta','\theta_{ref}',4), xlabel('t,s')
subplot(2,2,4),
plot(y(:,1),y(:,2),'b',Xref,Yref,':r','linewidth',1.5), legend('feedback','referentna trajektorija',4)
%===========================================
figure(2)
subplot(2,1,1), plot(tT,y(:,1)-Xref','b','linewidth',1.5), xlabel('t,s'), ylabel('X-X_{ref}')
subplot(2,1,2), plot(tT,y(:,2)-Yref','r','linewidth',1.5), xlabel('t,s'), ylabel('Y-Y_{ref}')
%Matlab datoteka MobRobot

function dy = MobRobot(t,y)
dy=zeros(4,1);
%--------Globalne varijable--------------------------global kp kd Vx Ymax sigg Xxc l
kf=100;
kt=100;
l=0.002;
%------------ Referentna trajektorija ------------------Xref=Vx*t;
Yref=Ymax*exp(-sigg*(Xref-Xxc)*(Xref-Xxc));
dXref=Vx;
dYref=-2*Vx*sigg*(Xref-Xxc)*Yref;
dThetaRef=kf*(ThetaRef-y(3));
PhiRef=unwrap(atan2(cos(ThetaRef)*dThetaRef*l,dXref));
dPhiRef=kt*(PhiRef-y(4));
%-------Upravljake varijable---------------------------u1=(1/((l^2)*((cos(y(3))^2+(sin(y(3)))^2)+(tan(y(4)))^2)))*((l^2)*cos(y(3))*dXref+(l^2)*sin(y(3))*dYref+
l*tan(y(4))*dPhiRef);
u2=dPhiRef;
%---------Diff jednadbe--------------------------------dy(1)=u1*cos(y(3));
dy(2)=u1*sin(y(3));
dy(3)=(u1/l)*tan(y(4));
dy(4)=u2;
49
6.3 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom uz

zahtjev minimalnog odstupanja
% Matlab datoteka ch3ex1

clc;
clear;
%-------------Globalne varijable----------------------global x_0 y_0 theta_0 phi_0 lambda_10 lambda_20 lambda_30 lambda_40 k1 k2 k3 Vx Ymax sigg Xxc l
T=10.0;
k1=10;
k2=10;
k3=10;
l=0.02;
Vx=1;
Ymax=1;
sigg=1.0;
Xxc=Vx*T/2;
%-----------Poetni uvjeti-------------------------------x_0 = 0;
y_0 = 0;
theta_0 = 0;
phi_0 = 0;
lambda_10 = 0;
lambda_20 = 0;
lambda_30 = 0;
lambda_40 = 0;
%------------Referentna trajektorija---------------------t = linspace(0,T,1000);
xxc=Xxc*ones(1,length(t));
Xref = Vx*t;
Yref = Ymax*exp(-sigg*(Xref-xxc).*(Xref-xxc));
%---------Rjeavanje diff jednadbi--------------------solinit = bvpinit(linspace(0,T,100),@guess);
sol = bvp4c(@odes,@bcs,solinit);
xint = linspace(0,T,1000);
Sxint = deval(sol,xint);
50
Theta = Sxint(3,:); Phi = Sxint(4,:); Lambda1 = Sxint(5,:); Lambda2 = Sxint(6,:); Lambda3 = Sxint(7,:);
Lambda4 = Sxint(8,:);
u1 = -Lambda1.*cos(Theta)/k1 - Lambda2.*sin(Theta)/k1 - Lambda3.*tan(Phi)/(l*k1);
u2 = -Lambda4/k2;
%----------------------Ispis rjeenja-------------------------------figure(1)
ylabel('X','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
%
%
ylabel('\theta','FontSize',12,'FontName','Times'),
%
ylabel('\Phi','FontSize',12,'FontName','Times'),
%
figure(2)
%
%
51

%
%=================================================
figure(3)
ylabel('u_1 [m/s]','FontSize',12,'FontName','Times'),
xlabel('t [s]','FontSize',12,'FontName','Times');
%
ylabel('u_2 [rad/s]','FontSize',12,'FontName','Times'),
xlabel('t [s]','FontSize',12,'FontName','Times');
%=================================================
figure(4)
plot(Xref, Yref, 'r:', Sxint(1,:), Sxint(2,:), 'linewidth',1.5), legend('referentna trajektorija','feedback',4)
ylabel('Y [m]','FontSize',12,'FontName','Times'),
xlabel('X [m]','FontSize',12,'FontName','Times');
%Matlab datoteka odes

function dydx = odes(x,y)
global k1 k2 k3 Vx Ymax sigg Xxc l
X = y(1); Y = y(2); Theta = y(3); Phi = y(4);
Lambda1 = y(5); Lambda2 = y(6); Lambda3 = y(7); Lambda4 = y(8);
%------Referentna krivulja-----------Xref = Vx*x;
Yref = Ymax*exp(-sigg*(Xref-Xxc)*(Xref-Xxc));
%------Upravljake varijable---------u1 = -(1/k1)*Lambda1*cos(Theta) - (1/k1)*Lambda2*sin(Theta) - (1/(k1*l))*Lambda3*tan(Phi);
u2 = -(1/k2)*Lambda4;
52
%-----Diff jednadbe sustava----------------f1 = u1*cos(Theta);

f2 = u1*sin(Theta);
f3 = (u1/l)*tan(Phi);
f4 = u2;
f5 = -k3*(X-Xref);
f6 = -k3*(Y-Yref);
f7 = Lambda1*u1*sin(Theta) - Lambda2*u1*cos(Theta);
f8 = -Lambda3*u1/(l*cos(Phi)*cos(Phi));
dydx = [f1; f2; f3; f4; f5; f6; f7; f8];
%Matlab datoteka guess

function v = guess(x)
global x_0 y_0 theta_0 phi_0 lambda_10 lambda_20 lambda_30 lambda_40
v = [x_0; y_0; theta_0; phi_0; lambda_10; lambda_20; lambda_30; lambda_40];
%Matlab datoteka bsc

function res = bcs(ya,yb)
global x_0 y_0 theta_0 phi_0 lambda_10 lambda_20 lambda_30 lambda_40
res = [ya(1)-x_0; ya(2)-y_0; ya(3)-theta_0; ya(4)-phi_0; yb(5)-lambda_10; yb(6)-lambda_20; yb(7)lambda_30; yb(8)-lambda_30 ];
53

%Matlab datoteka Main
clear;
clc;
%------------Globalne varijable-----------------------------T=10;
DT=0.01;
global l R w
l=0.2;
R=10;
w=1.0;
%-----------Poetni uvjeti----------------------------------x10=0;
x20=0;
x30=0;
x40=atan2(l,R);
%-----------Vrijeme simulacije---------------------------tT=0:DT:T;
%-----------Krunica---------------------------------------Xref = R*sin(w*tT);
dXref = R*cos(w*tT)*w;
ddXref = -R*sin(w*tT)*w^2;
Yref = R*(1-cos(w*tT));
dYref = R*sin(w*tT);
ddYref = -R*cos(w*tT)*w^2;
PhiRef=unwrap(atan2(((dXref.*ddYref-ddXref.*dYref)*l),u1.^3));
%---------------Rjesavanje diff jednadzbi--------------options=odeset('RelTol',1e-8,'AbsTol',1e-8);
[t,y]=ode45('MobRobot',tT,[x10 x20 x30 x40],options);
%-------------Ispis rjeenja-----------------------------figure(1)
subplot(2,2,1),
plot(tT,y(:,1),'b',tT,Xref,':r','linewidth',1.5), legend('X','X_{ref}',4), xlabel('t,s')
subplot(2,2,2),
plot(tT,y(:,2),'b',tT,Yref,':r','linewidth',1.5), legend('Y','Y_{ref}',4), xlabel('t,s')
54
subplot(2,2,3),
plot(tT,y(:,3),'b',tT,ThetaRef,':r','linewidth',1.5), legend('\theta','\theta_{ref}',4), xlabel('t,s')
figure(2)
% subplot(2,2,4),
plot(y(:,1),y(:,2),'b',Xref,Yref,':r','linewidth',1.5), legend('feedback','referentna trajektorija',4)
figure(3)
subplot(2,1,1), plot(tT,y(:,1)-Xref','b','linewidth',1.5), xlabel('t,s'), ylabel('X-X_{ref}')
subplot(2,1,2), plot(tT,y(:,2)-Yref','r','linewidth',1.5), xlabel('t,s'), ylabel('Y-Y_{ref}')
%Matlab datoteka MobRobot

function dy = MobRobot(t,y)
dy=zeros(4,1);
global l R w
%---------Referentna trajektorija-----------Xref = R*sin(w*t);
dXref = R*cos(w*t)*w;
ddXref = -R*sin(w*t)*w^2;
dddXref = -R*cos(w*t)*w^3;
Yref = R*(1-cos(w*t));
dYref = R*sin(w*t);
ddYref = -R*cos(w*t)*w^2;
dddYref = -R*sin(w*t)*w^3;
%--------Upravljake veliine---------------u1=sqrt(dXref.^2+dYref.^2);
u2=l*u1* ( (dddYref*dXref - dddXref*dYref)*u1^2 - 3*(ddYref*dXref - ddXref*dYref)...
*(dXref*ddXref+dYref*ddYref))/ ( u1^6 + l^2*(ddYref*dXref - ddXref*dYref))^2;
%------Kinematike jednadbe--------------dy(1)=u1*cos(y(3));
dy(2)=u1*sin(y(3));
dy(3)=(u1/l)*tan(y(4));
dy(4)=u2;
55
Literatura
[1]
Shuzhi Same Ge, Frank L. Lewis, Autonomous Mobile Robots, (Taylor & Francis
Group, 2006)
[2]
George A. Bekey, Autonomous Robots, (The MIT Press, 2005).
[3]
Teo Geluk, Control of first order nonholonomic systems, (Technische Universiteit

Eindhoven).
[4]
A. De Luca, G. Oriolo, C. Samson, Feedback control of nonholonomic car-like

robot, (Universita di Roma La Sapienza).
[5]
R. Siegwart, Illah R. Nourbakhsh, Introduction to Autonomous Mobile Robots,

(The MIT Press, 2004).
[6]
A. De Luca, G. Oriolo, Local Incremental Planning for Nonholonomic Mobile

Robots, (Universita di Roma La Sapienza).
[7]
Branko Novakovi, Metode voenja tehnikih sistema, (kolska knjiga, Zagreb,

1990).
56

177

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

177

Hochgeladen von

Copyright:

Verfügbare Formate

SEMINARSKI RAD IZ KOLEGIJA VOENJE TEHNIKIH SUSTAVA

Optimalno pozicioniranje i praenje zadane

Voenje tehnikih sustava

2 Kinematiki model mobilnog robota

2.1 Kinematiki model mobilnog robota s diferencijalnim pogonom.................................4

3 Optimalno pozicioniranje primjenom optimalnog upravljanja

3.1 Optimalno upravljanje...7

4 Praenje zadane trajektorije

4.1 Praenje zadane trajektorije mobilnim robotom s diferencijalnim pogonom pomou

6.1 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom.............................44

Voenje tehnikih sustava

U skolpu ovog seminara prikazati e se nekoliko naina upravljanja mobilnim robotom.

Slika 1: Optimalno pozicioniranje

Slika 2: Praenje zadane trajektorije

Voenje tehnikih sustava

2 Kinematiki model mobilnog robota

Seminar je koncipiran tako da se svaka metoda upravljanja prvo objasni na jednostavnom

2.1 Kinematiki model mobilnog robota s diferencijalnim pogonom

gdje su v1 i v2 linearna brzina mobilnog robota u x y koordinatnoj ravnini, odnosno

Voenje tehnikih sustava

robota u x y koordinatnoj ravnini , a je kutni zakret mobilnog robota u odnosu na x

2.2 Kinematiki model mobilnog robota sa stranjim pogonom

zadnjeg kotaa u Kartezijevom koordinatnom sustavu, predstavlja orijentaciju robota u

Slika 4: Mobilni robot sa stanjim pogonom

Voenje tehnikih sustava

Kinematiki model, tako definiranog mobilnog robota je:

gdje su v1 i v2 linearna brzina mobilnog robota u x y koordinatnoj ravnini, odnosno

Voenje tehnikih sustava

3 Optimalno pozicioniranje primjenom optimalnog upravljanja

3.1 Optimalno upravljanje

Temelji teorije optimalnog upravljanja postavljeni su krajem pedesetih godina radovima

gdje je x n-dimenzionalni vektor stanja, u je m-dimenzionalni vektor upravljanja, a

Uzmemo li opi kriterij opimalnosti

Voenje tehnikih sustava

koji se naziva Bolzin problem, problem optimalnog upravljanja sastoji se u pronalaenju

gdje se funkcija H ( x ( t ) , u ( t ) , ( t ) , t ) naziva Hamiltonova jednadba, a komponente

iz rubnih uvjeta x ( t0 ) = x0 i x ( t f ) = x f , slijedi x ( t0 ) = 0 i x ( t f ) = 0 , a izraz (1.8)

iz ega slijede uvjeti optimalnosti

Voenje tehnikih sustava

koji imaju definirano i poetno krajnje stanje

U sluaju svih singularnih i neregularnih situacija ukljuujui i ogranienja

gdje i dalje vrijede jednadbe

Voenje tehnikih sustava

3.2 Optimalno pozicioniranje mobilnog robota s diferencijalnim pogonom

Da bi se dobile upravljake varijable v1 i v2 raspisuje se totalni diferencijal Hamiltoniana i

Iz jednadbi (1.18) i (1.19) dobivaju se upravljake varijable v1 i v2 kako slijedi

Izrazi (1.20) i (1.21) predstavljaju upravljake varijable u ovisnosti o Lagrangeovim

Voenje tehnikih sustava

Diferencijalne jednadbe (1.22) - (1.24), uz tri diferencijalne jednadbe koje slijede iz

ine sustav od ukupno est diferencijalnih jednadbi. Sustav diferencijalnih jednadbi

Sljede rezultati simulacije optimalnog pozicioniranja mobilnog robota s diferencijalnim

Voenje tehnikih sustava

Slika 5: Gibanje robota prilikom pozicioniranja

Slika 6: Upravljake varijable

Voenje tehnikih sustava

Slika 7: Varijable stanja i Lagrangeovi multiplikatori

Slika 6 prikazuje iznose veliina upravljakih varijabli, a Slika 7 prikazuje x i y poziciju

Voenje tehnikih sustava

3.3 Optimalno pozicioniranje mobilnog robota sa stranjim pogonom

Cijeli postupak metode optimalnog upravljanja kod optimalnog pozicioniranja objanjen

v1 = 1 cos 2 sin 3 tan

Voenje tehnikih sustava

Teinski faktori su postavljeni na k1 = 1, k2 = 10, duljina robota je 0.1m, a rubni uvjeti su

u odnosu na os x, doi u toku (1,2) s istom