You are on page 1of 56

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
minimalnog odstupanja..36
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
minimalnog odstupanja..50
6.4 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom..54
2

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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.

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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.

Slika 8: Upravljake varijable

16

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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)

Slika 13: Varijable stanja

Slika 14: Varijable stanja

i Lagrangeovi multiplikatori

i Lagrangeovi multiplikatori

Slika 15: Upravljake varijable

Slika 16: Upravljake varijable

20

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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
referentna trajektorija
1

Slika 26: Praenje trajektorije

30

10

Voenje tehnikih sustava

-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

Voenje tehnikih sustava

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)

Voenje tehnikih sustava

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 .
Sljede rezultati simulacije:

1.2

Y [m]

0.8

0.6

0.4

0.2

-0.2
0

referentna trajektorija
feedback
1

X [m]
Slika 28: Praenje trajektorije

33

10

Voenje tehnikih sustava

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

Voenje tehnikih sustava

1.2

Y [m]

0.8

0.6

0.4

0.2

-0.2
0

referentna trajektorija
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

referentna trajektorija
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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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
referentna trajektorija
feedback

0.1
0
0

10

X [m]
Slika 30: Praenje trajektorije

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

Voenje tehnikih sustava

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)

Slika 32: Varijable stanja

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

Voenje tehnikih sustava

4.5 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom

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)

Voenje tehnikih sustava

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
Sljede rezultati simulacije:

40

l
,
R

(1.102)

Voenje tehnikih sustava

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
referentna trajektorija

2
0
-10

-5

10

Slika 34: Praenje referentne trajektorije


41

15

Voenje tehnikih sustava

-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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

%----------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');
%
subplot(2,2,2), plot(xint,Sxint(2,:), 'linewidth',1.5),
ylabel('Y','FontSize',12,'FontName','Times'),
xlabel('vrijeme (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,3), plot(xint,Sxint(3,:), 'linewidth',1.5),
ylabel('\theta','FontSize',12,'FontName','Times'),
xlabel('vrijeme (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,4), plot(xint,Sxint(4,:), 'linewidth',1.5),
ylabel('\Phi','FontSize',12,'FontName','Times'),
xlabel('vrijeme (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
figure(2)
subplot(2,2,1), plot(xint,Sxint(5,:), 'linewidth',1.5),
ylabel('\lambda_1','FontSize',12,'FontName','Times'),
xlabel('vrijem (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,2), plot(xint,Sxint(6,:), 'linewidth',1.5),
ylabel('\lambda_2','FontSize',12,'FontName','Times'),
xlabel('vrijem (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,3), plot(xint,Sxint(7,:), 'linewidth',1.5),
ylabel('\lambda_3','FontSize',12,'FontName','Times'),
xlabel('vrijem (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');

45

Voenje tehnikih sustava

%
subplot(2,2,4), plot(xint,Sxint(8,:), 'linewidth',1.5),
ylabel('\lambda_4','FontSize',12,'FontName','Times'),
xlabel('vrijem (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%=================================================
figure(3)
plot(Sxint(1,:), Sxint(2,:), 'linewidth',1.5),
ylabel('Y','FontSize',12,'FontName','Times'),
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'),
xlabel('vrijeme (sec)','FontSize',12,'FontName','Times');
%
subplot(2,2,2), plot(xint,u2, 'linewidth',1.5),
ylabel('u_2','FontSize',12,'FontName','Times'),
xlabel('vrijeme (sec)','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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

6.2 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom


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

Voenje tehnikih sustava

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;
ThetaRef=unwrap(atan2(dYref,dXref));
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

Voenje tehnikih sustava

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

Voenje tehnikih sustava

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)
subplot(2,2,1), plot(xint,Sxint(1,:), 'linewidth',1.5),
ylabel('X','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,2), plot(xint,Sxint(2,:), 'linewidth',1.5),
ylabel('Y','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,3), plot(xint,Sxint(3,:), 'linewidth',1.5),
ylabel('\theta','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,4), plot(xint,Sxint(4,:), 'linewidth',1.5),
ylabel('\Phi','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
figure(2)
subplot(2,2,1), plot(xint,Sxint(5,:), 'linewidth',1.5),
ylabel('\lambda_1','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,2), plot(xint,Sxint(6,:), 'linewidth',1.5),
ylabel('\lambda_2','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%

51

Voenje tehnikih sustava

subplot(2,2,3), plot(xint,Sxint(7,:), 'linewidth',1.5),


ylabel('\lambda_3','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%
subplot(2,2,4), plot(xint,Sxint(8,:), 'linewidth',1.5),
ylabel('\lambda_4','FontSize',12,'FontName','Times'),
xlabel('time (sec)','FontSize',12,'FontName','Times');
set(findobj(gcf,'tag','legend'),'fontsize',10,'FontName','Times');
%=================================================
figure(3)
subplot(2,1,1), plot(xint,u1, 'linewidth',1.5),
ylabel('u_1 [m/s]','FontSize',12,'FontName','Times'),
xlabel('t [s]','FontSize',12,'FontName','Times');
%
subplot(2,1,2), plot(xint,u2, 'linewidth',1.5),
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

Voenje tehnikih sustava

%-----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

Voenje tehnikih sustava

6.4 Praenje zadane trajektorije mobilnim robotom sa stranjim pogonom


%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;
ThetaRef=unwrap(atan2(dYref,dXref));
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

Voenje tehnikih sustava

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;
ThetaRef=unwrap(atan2(dYref,dXref));
%--------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

Voenje tehnikih sustava

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