Zavod za robotiku i automatizaciju proizvodnih sustava

Katedra za strojarsku automatiku

SEMINARSKI RAD IZ KOLEGIJA:

TEORIJA SUSTAVA
OPCA

Upravljanje mobilnim robotom s prednjim

zakretnim kotacima

Nositelj kolegija:

Student:

Prof. dr. sc. Branko Novakovic

Sven Savic

Zagreb, 2008

Sadr
zaj
1 Uvod

2 Struktura mobilnog robota

2.1

Kinematicke jednadzbe . . . . . . . . . . . . . . . . . . . . . . . . . .

2.2

Ulancana forma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 Regulacija mobilnog robota

u okolini s preprekama

3.1

Metoda potencijalnih polja . . . . . . . . . . . . . . . . . . . . . . . .

3.2

Pracenje referentne trajektorije . . . . . . . . . . . . . . . . . . . . . 11

3.3

Simulacijski rezultati . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.3.1

Normiranje referentnih velicina . . . . . . . . . . . . . . . . . 17

4 Matlab kodovi
4.1

21

Metoda potencijalnih polja . . . . . . . . . . . . . . . . . . . . . . . . 21

4.1.1

Trajektorija bez lokalnih minimuma . . . . . . . . . . . . . . . 21

4.1.2

Trajektorija s lokalnim minimumom . . . . . . . . . . . . . . . 23

4.2

Priblizna linearizacija . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.3

Priblizna linearizacija - normirane velicine . . . . . . . . . . . . . . . 28

Bibliografija

34

Uvod

Kod upravljanja mobilnim robotom uporabom potencijalnih polja, koriste se tri

stupnja upravljanja. Prvo se generira trajektorija koja izbjegava repulzivne potencijale i tezi dolasku ka atraktivnom potencijalu. Zatim se ta putanja rasclanjuje
i aproksimira ostvarivim segmentima koji zadovoljavaju ogranicenja sustava. I na
kraju se uz pomoc direktne linearizacije podesavaju upravljacke velicine, koje rezultiraju pracenjem ostvarive putanje.
Da bi se moglo upravljati mobilnim robotom bez mrtvog vremena, u kojem robot
proracunava trajektoriju i upravaljcke velicine, kinematicki model robota potrebno
je prevesti u lancanu formu.

Struktura mobilnog robota

U ovom seminaru koristen je model robota baziran na RC monster truck modelu

Tamiya Twin Detonator, ali je modificiran tako da ima obe zakretne osovine upravljive servo motorima (slike 1 i 2).
Model je pogonjen pomocu dva DC motora, ali preko zajednickog kontrolera (Hmost) sto nam daje samo jednu upravljacku velicinu.

Slika 1: 3D model mobilnog robota.

Slika 2: Tlocrt mobilnog robota.

Kinematicke velicine mobilnog robota prikazane su na slici 3 prema [6]. Model

mobilnog robota sa slike 3 mozemo pojednostaviti, tako da ne gledamo posebno
zakret svakog kotaca iako postoji razlika izmedu kutova. Zbog razloga sto su oba
kotaca preko osovine spojena na motor koji daje kutni zakret, mozemo par kotaca
zamijeniti s jednim kotacem, te model promatrati kao motocikl sa oba zakretna
kotaca, prikazan na slici 4, prema [4].

2.1

Kinemati
cke jednad
zbe

Ako uzmemo x y ravninu kao ravninu po kojoj se robot krece, mozemo zapisati
dvije jednadzbe pozicije mobilnog robota prema [6, 7]
x = v1 cos( + 1 )

(1)

y = v1 sin( + 1 )

(2)

gdje v1 oznacava linearnu brzinu mobilnog robota u x-y koordinatnoj ravnini, je

kutni zakret mobilnog robota u odnosu na x koordinatnu os, a 1 je zakret osovine
3

Slika 3: Prikaz kinematickih velicina.

Slika 4: Pojednostavljeni prikaz robota.

kotaca s obzirom na uzduznu os mobilnog robota. Slijedecu jednadzbu dobivamo iz

zakreta mobilnog robota u x-y koordinatnoj ravnini.
v1 sin(1 2 )
=
l
cos 2

(3)

oznacava kutnu brzinu kojom se robot zakrece u x-y koordinatnoj ravnini, l je

meduosovinski razmak izrazen u metrima, 1 i 2 su zakreti osovine kotaca. 1 je
kut izmedu kotaca prednje osovine i uzduzne osi robota i pozitivan kut je u smjeru
kazaljke na satu, a 2 je kut izmedu kotaca straznje osovine i uzduzne osi robota i
pozitivan kut je u smjeru suprotnom od smjera kazaljke na satu. Nadalje imamo jos
dvije upravljacke velicine koje predstavljaju kutne brzine zakreta prednje i zadnje
osovine mobilnog robota.
1 = v2

(4)

2 = v3

(5)

Time smo dobili sustav pet diferencijalnih jednadzbi sa tri upravljacke velicine

v1 v3 . Cime
je potpuno definirana kinematika mobilnog robota.
Radi nemogucnosti dobivanja jedinstvenog rjesenja pri zakretu obje osovine (za razlicite kombinacije kutove 1 i 2 moguce je dobiti jednake rezultate), pri rjesavanju
standardne linearizacije, model je pojednostavljen na nacin da je zakocena zadnja
osovina 1 = 0.
Nakon pojednostavljenja, dobivamo slijedece jednadzbe za zadani model koji odgovara kinematickom modelu automobila pa se jos naziva i car-like robot. Konfiguracija robota je predstavljena pozicijom i orijentacijom tijela robota u ravnini i

kutom zakreta kotaca.

2.2

x = v1 cos

(6)

y = v1 sin
v1 tan
=
l
= v2

(7)
(8)
(9)

Ulan
cana forma

Postojanje kanonske forme kinematickog modela nonholonomic robota je neophodna

za sistematican razvoj open-loop i closed-loop strategija upravljanja. Najvise upotrebljavana kanonska struktura je ulancana forma. Za upravljacki sustav s dva ulaza,
ulancana forma prikazana je jednadzbom (10) prema [6, 1].
x 1 = u1
x 2 = u2
x 3 = x2 u1
..
.

(10)

x n = xn1 u1
Mnogi kinematicki problemi mobilnih robota s kotacima mogu se prikazati u toj
formi. Ulancana forma (10) ima temeljnu linearnu strukturu koja se moze pokazati u
slucaju kada je u1 funkcija vremena i vise se ne promatra kao upravljacka varijabla. U
tom slucaju sustav jednadzbi (10) postaje jednovarijabilni vremenski ovisan linearni
sustav. Kinematicki model robota opisan jednadzbama (1 5) sadrzi tri upravljacke
velicine v1 v3 , te ukoliko ga zelimo zapisati u ulancanoj formi koja sadrzi samo
dvije upravljacke velicine, potrebno je promatrati pojednostavljeni model prikazan
jednadzbama (6 9). Ulancanu formu oblika (10), dobivamo na nacin da pretpostavimo da su x1 = x i x4 = y iz cega slijedi
x 1 = x

(11)

Uvrstimo li u jednadzbu (11) izraz (6) slijedi jednadzba

u1 = v1 cos

(12)

iz koje slijedi izraz za transformaciju v1 .

v1 =

u1
cos
5

(13)

Izraz za x3 dobivamo na slijedeci nacin

x 4 = y

(14)

uvrstimo li u gornju jednadzbu izraz (7) i izraz za x 4 prema (10), slijedi

x3 u1 = v1 sin

(15)

uz transformaciju (13) dobivamo slijedeci izraz

x3 u1 = u1

sin
cos

(16)

koji nakon sredivanja dobiva oblik

x3 = tan

(17)

Raspisemo li totalni diferencijal dobivenog izraza (17), slijedi

x 3 =

1
cos2

(18)

uvrstimo li izraz za x 3 prema jednadzbi (10), slijedi

1
cos2

(19)

1
u1 cos2

(20)

x2 u1 =
nakon sredivanja dobivamo izraz
x2 =

Uvrstavanjem jednadzbi (8) i (12) u jednadzbu (20), dobivamo slijedeci izraz

x2 =

v1 tan
1
1

l
v1 cos cos2

(21)

iz kojeg nakon sredivanja slijedi izraz za x2

x2 =

tan
l cos3

Raspisemo li totalni diferencijal izraza (22) dobivamo izraz

1 cos3 + 3 cos2 sin tan

2
1

x 2 = cos

l
cos6
nakon uvrstavanja izraza (9) i (8) u gornju jednadzbu i sredivanja, slijedi

1
sin cos tan
2

tan
1 v2 cos2 1 3 v1 cos

cos
l
u2 =
+

l
cos3
l
cos6

(22)

(23)

(24)

daljnjim sredivanjem, slijedi

u2 =

v2
3 v1 tan tan2
+
l cos2 cos3 l2
cos3

(25)

nakon sto izraz svedemo na zajednicki nazivnik, dobivamo

l v2 cos + 3 u1 tan sin2
u2 =
l2 cos2 cos4

(26)

pomnozimo li jednadzbu s nazivnikom, slijedi

u2 l2 cos2 cos4 = l v2 cos + 3 u1 tan sin2

(27)

nakon sredivanja dobivamo izraz za transformaciju upravljacke velicine v2

v2 = u2 l cos2 cos3

3 u1 sin sin2
l cos2

(28)

Radi preglednosti, navode se promjene koordinata (29) - (32)

x1 = x
tan
l cos2
x3 = tan
x2 =

x4 = y

(29)
(30)
(31)
(32)

i ulazne transformacije (33) - (34)

v1 =

u1
cos

(33)

3 u1 sin sin2
v2 = u2 l cos cos
l cos2
2

(34)

Iz jednadzbi (29) - (32) vidljivo je da transformacija nije definirana za orijentaciju

robota = /2 k, k N . Promjene koordinata (29) - (32) moguce je generalizirati za nonholonomic sustave viseg reda, kao sto su roboti s N prikolica. Opcenito,
x1 i xn koordinate moguce je uvijek odabrati kao x i y koordinate sredista osovine
kotaca zadnje prikolice.

Regulacija mobilnog robota

u okolini s preprekama

Kod upravljanja mobilnim robotom koriste se tri stupnja upravljanja. Prvo se

generira trajektorija koja izbjegava prepreke. Zatim se ta putanja rasclanjuje i
aproksimira ostvarivim segmentima koji zadovoljavaju ogranicenja sustava. I na
kraju se podesavaju upravljacke velicine, koje rezultiraju pracenjem ostvarive putanje.
Da bi se moglo upravljati mobilnim robotom on-line1 , kinematicki model robota
potrebno je prevesti u ulancanu formu. Kao metodu generiranja trajektorije koristi se metoda potencijalnih polja, dok se upravljanje vrsi pribliznom linearizacijom
sistemskih jednadzbi oko zadane trajektorije, prema [2, 3].

3.1

Metoda potencijalnih polja

Kod gibanja robota s n stupnjeva slobode u m dimenzionalnom kartezijevom koordinatnom sustavu s preprekama, planiranje putanje formulirano je na slijedeci nacin,
prema [9]:
zadana je pocetna pozicija S i pozicija cilja G u Rm , potrebno je pronaci putanju u
konfiguracijskom prostoru Rn omogucujuci putanju bez sudara od tocke S do tocke
G.

Slika 5: Atraktivni potencijal.

1

Slika 6: Repulzivni potencijal.

Kod on-line metode upravljanja, upravljacke velicine proracunavaju se tijekom kretanja robota

i to za jedan korak unaprijed

Metoda potencijalnih polja sastoji se od izmjene topologije radnog prostora robota radi planiranja njegove putanje. Dodavanjem atraktivnih dolina (Slika 5) i
repulzivnih vrhova (Slika 6) dobivamo ukupno potencijalno polje (Slika 8).
Radi pojednostavljenja, uzima se 2-dimenzijski kartezijev koordinatni sustav (m=2)

Slika 7: Radni prostor robota.

Slika 8: Ukupno potencijalno polje.

prikazan slikom 7. Cilj se oznacava s jednom tockom u prostoru kao atraktivni kruzni
potencijal pG = (xG , yG ).
Opcenito za tocku p u radnom prostoru robota s koordinatama p = (x, y), atraktivni potencijal iznosi, prema [5]

Ua (p) =


1
(x xG )2 + (y yG )2
2

(35)

gdje su x, y pozicija centra mase mobilnog robota, a xG , yG pozicija cilja u fiksnom

koordinatnom sustavu. Standardni oblik repulzivnog potencijala prikazuje se na
slijedeci nacin



1 
2
2
(x xR,j ) + (y yR,j )
Ur (p) = exp
2

(36)

gdje je xR,j , yR,j pozicija prepreka, j = 0, 1, . . . N gdje je N broj prepreka.

Zeljeni
zakon gibanja mobilnog robota prikazan je na slijedeci nacin

N 
Ua X
Ur
Ur
xd = a1
+
b1
+ b2
x
x
y
j=0

N 
Ua X
Ur
Ur
yd = a1
+
b1
b2
y
y
x
j=0
9

(37)

(38)

gdje je a1 pozitivna konstanta koja oznacava jakost atraktivnog potencijalnog polja,

b1 pozitivna konstanta koja oznacava jakost repulzivnog potencijalnog polja, a b2
pozitivna konstanta koja oznacava brzinu vrtloga oko repulzivnog potencijalnog
polja.
Vektorski prikaz zakona gibanja za slucaj jedne prepreke izgleda
pd = a1 p Ua (p, pA ) + b1 p Ur (p, pR ) b2 Jp Ur (p, pR )

(39)

uz


p = [x y] , p
x y
T

"

T
,

J=

#
(40)

1 0

gdje je p vektor diferencijalnih operatora, a J matrica predznaka.

Metodom potencijalnih polja generirana je trajektorija prikazana slikom 9 gdje
su prepreke prikazana uz pomoc kruznih potencijala radijusa 0.5m razmjestene po
radnom prostoru robota, a tocka cilja se nalazi na poziciji pG (9, 9).

10
9
8
7

Y, m

6
5
4
3
2
1
0
1

4
X, m

10

Slika 9: Trajektorija generirana metodom potencijalnih polja.

Prikazana trajektorija uspjesno izbjegava prepreke i zavrsava u tocki cilja.
Tipicno ogranicenje metode potencijalnih polja je nastajanje prividnog lokalnog minimuma u ukupnom potencijalnom polju, koje moze dovesti do prekida izvrsavanja
10

algoritma planiranja putanje.

Primjer zaglavljivanja prikazan je na slici 10 gdje je radnom polju robota iz prethodnog
primjera dodano jos sest prepreka u L formi. Metoda izbjegavanja stvaranja lokalnog

10
9
8
7

Y, m

6
5
4
3
2
1
0
1

4
X, m

10

Slika 10: Trajektorija generirana metodom potencijalnih polja (primjer lokalnog

minimuma).
minimuma, je metoda vrtloznih polja. Repulzivno djelovanje zamijenjeno je brzinom toka koja tangira konturu (prepreku) tako da je robot prisiljen zaokrenuti oko
prepreke, sto dovodi do konvergencije algoritma planiranja putanje.

3.2

Pra
cenje referentne trajektorije

Odabrana je metoda pracenja trajektorije bazirana na teoriji standardnog linearnog

upravljanja. Temelji se na pribliznoj linearizaciji jednadzbi sustava oko zeljene trajektorije, prema [3]. Za male perturbacije oko nekog referentnog stanja, nelinearne
jednadzbe
x(t)

= f (x(t), u(t))

(41)

moguce je linearizirati, ako je funkcija f (x, u) kontinuirana i diferencijabilna u okolini

zeljene trajektorije po svakoj varijabli x i u. Oznacimo li male promjene stanja i
11

ulaza oko referentnog vektora stanja i ulaza s x(t) i u(t) tada se stanje sustava i
ulaz mogu prikazati jednadzbama
x(t) = xd (t) + x(t)

(42)

u(t) = ud (t) + u(t)

(43)

iz cega slijedi jednadzba stanja nelinearnog sustava

x d + x(t)

= f (xd (t) + x(t), ud (t) + u(t))

(44)

Gore navedeni izraz moguce je prikazati na slijedeci nacin, s obzirom da je funkcija

f kontinuirana i diferencijabilna po svim svojim argumentima
x =

f
f
x +
u + h(x, u)
x
u

(45)

gdje su parcijalne derivacije funkcije referentnog stanja i upravljanja, a h predstavlja

clanove viseg reda. Kod malih odstupanja od referentne vrijednosti, jednadzbu (45)
mozemo prikazati u matricnoj formi
x = A(t)x(t) + B(t)u(t)
gdje su Jacobiani A(t) i B(t) definirani kao

f1
f1
f1
f1

xn
x1 x2
u1
f2 f2 f2
f2
x1 x2

xn
A(t) = .
,
B(t) = u. 1
..
..
..
..
..

.
.
.

fn
fn
fn
fn
xn
x1
x2
u1

(46)

f1
u2
f2
u2

fn
u2

..
.

..
.

f1
un
f2
un

..
.

fn
un

(47)

Izraz (46) predstavlja linearni vremenski-varijabilni sustav, koji priblizno opisuje

dinamiku sustava prikazanog jednadzbom (41) u okolini referentne trajektorije.
Uz pomoc metode potencijalnih polja generirana je trajektorija oblika
xd = xd (t),

yd = yd (t),

t t0

(48)

Iz generirane trajektorije (48) i promjene koordinata (29) - (32) dobivamo jednadzbe

stanja koje generiraju zeljenu trajektoriju u ulancanoj formi
xd1 (t) = xd (t)

(49)

xd2 (t) = [
yd (t)x d (t) y d (t)
xd (t)]/x 3d (t)

(50)

xd3 (t) = yd (t)/x d (t)

(51)

xd4 (t) = yd (t)

(52)
(53)
12

i transformacije ulaza
ud1 (t) = x d (t)
(54)
...
...
ud2 (t) = [ y d (t)x 2d (t) x d (t)y d (t)x d (t) 3
yd (t)x d (t)
xd (t) + 3y d (t)
x2d (t)]/x 4d (t) (55)
U gornjim izrazima pojavljuju se druge i trece derivacije referentnih pozicija xr ef
i yr ef za cije estimacije su koristeni filteri drugog odnosno treceg reda prikazani
slijedecim izrazima
f (x + x) f (x)
x
f
(x
+
x)
2f (x) + f (x x)
f 00 (x)
x2
f 0 (x)

(56)
(57)

Ako nas sustav prikazemo u ulancanoj formi (10). Naznacimo greske stanja i ulaza
kao
xi = xdi xi , i = 1, ..., 4,

uj = udj uj , j = 1, 2

(58)

Slijedi da su nelinearne jednadzbe greske

x 1 = u1

(59)

x 2 = u2

(60)

x 3 = xd2 ud1 x2 u1

(61)

x 4 = xd3 ud1 x3 u1

(62)

Linearizacija oko zeljene trajektorije dovodi do slijedeceg linearnog vremenski zavisnog sustava

0
0
0
1
x + 0
u = A(t)
x = 0
x + B(t)
u
0 u (t)

0
0

xd2 (t) 0
d1
0
0
ud1 (t) 0
xd3 (t) 0

(63)

Gdje je pogreska transformacije upravljackih velicina prikazana kao linearni vremenski zavisan zakon
u1 = k1 x1

(64)

u2 = k2 x2

k3
k4
x3 2 x4
ud1
u d1

13

(65)

Da bi sustav bio stabilan mora biti zadovoljeno k1 > 0, a jednadzba (66) mora biti
Hurwitzov polinomu.
3 + k2 2 + k3 + k4

(66)

Hurwitzov kriterij kazuje da je nuzan i dovoljan uvjet da svi korijeni karakteristicne

jednadzbe imaju realne negativne dijelove, ako su svi koeficijenti ai diferencijalne
jednadzbe i sve dijagonalne subdeterminante Hi vece od nule., prema [8].
Raspisemo li sada subdeterminante za nas slucaj




 k2 k4 0
 k k 

 2 4 
H1 = k2 , H2 = 
 , H3 =  1 k3 0
 1 k3 

 0 0 k4

iz cega slijedi da ce sustav biti stabilan ukoliko odabrani koeficijenti zadovoljavaju

slijedece uvjete
H1 = k2 > 0

(67)

H2 = k2 k3 k4 > 0

(68)

H3 = H2 k4 > 0

(69)

Odabrani parametri sustava prikazani su tablicom 1

Tablica 1: Koeficijenti karakteristicne jednadzbe
Varijable

vrijednost

k1

10

k2

k3

15

k4

17

Za takav odabir konstanti sistemska matrica A, kao sto je prikazano jednadzbom

(70), u slucaju zatvorene petlje ima konstantne svojstvene vrijednosti s negativnim
realnim dijelovima.

k1

0
k2 k3 /ud1 (t) k4 /u2 d1 (t)
Avl (t) =
k x (t) u (t)
0
0

1 d2
d1
k1 xd3 (t)
0
ud1 (t)
0
14

(70)

Za zakon upravljanja prikazan jednadzbama (64) i (65) ne postoji opca analiza

stabilnosti, te se ne moze garantirati asimptotska stabilnost vremenski zavisnog
zatvorenog sustava. Medutim, za specificne vrijednosti ud1 (t) (razlicitih od nule)
i ud2 (t), moguce je koristiti rezultate na sporo promjenjivim linearnim sustavima
kako bi se dokazala asimptotska stabilnost. Polozaj svojstvenih vrijednosti zatvorene
petlje u otvorenoj lijevoj poluravnini moze biti odabran prema opcem principu za
dobivanje brze konvergencije greske pracenja ka nuli uz prihvatljive napore upravljanja.
Da bismo odredili dvije realne negativne svojstvene vrijednosti 1 i 2 i dvije
kompleksne svojstvene vrijednosti sa negativnim realnim dijelom, modul n i koeficijent prigusenja (0 < 1), pojacanja ki trebaju biti odabrana na slijedeci
nacin
k1 = 1 , k2 = 2 + 2n , k3 = 2 n + 2n 2 , k4 = 2 n 2

(71)

Cjelokupna ulazna transformacija prikazana u ulancanoj formi je

u = ud + u
gdje je ud referentna velicina, a u odstupanje od referentne vrijednosti. Da bi se
izracunala stvarna upravljacka velicina v za car-like robota, potrebno je koristiti
ulaznu transformaciju prema jednadzbama (33) i (34). Kao rezultat dobivamo brzinu gibanja robota i kutnu brzinu zakreta robota u x-y ravnini kao nelinearne zakone
upravljanja. Za odabir druge upravljacke velicine (jednadzba 65) zahtjeva se da je
ud1 6= 0. Vidljivo je da postavljanjem svojstvenih vrijednosti na konstantnim pozicijama ce zahtijevati veca pojacanja kako se varijabla x1 priblizava nuli. To ogranicenje
moguce je zaobici tako da se svojstvene vrijednosti postavljaju kao funkcije ulaza
ud1 . Na nacin
u2 = 3|ud1 |
x2 32 ud1 x3 3 |ud1 |
x4

(72)

gdje je > 0. Uz pomoc ovakve input scaling procedure, druga upravljacka velicina
priblizava se nuli kako se zeljena trajektorija xd1 zaustavlja. Za upravljacke zakone
dane jednadzbama (65) i (72) moguce je izvesti Lyapunovljevu analizu asimptotske
stabilnosti.

15

3.3

Simulacijski rezultati

Primjenom metode potencijalnih polja generirano je nekoliko razlicitih konfiguracija

radnog prostora robota, te robot treba iz zadane pocetne pozicije do tocke cilja
izbjegavajuci pritom prepreke. Za ne podesene parametre jakosti atraktivnog i repulzivnog potencijala (a1 , b1 i b2 ) iz jednadzbe (39), mobilni robot ne uspijeva zaobici
prepreku koja mu se nalazi na putanji prema tocki cilja (slika 11). Nakon nekoliko

10
Referentna trajektorija
Trajektorija robota
Prepreka

9
8
7

Y, m

6
5
4
3
2
1
0
1
1

4
5
X, m

10

Slika 11: Pracenje trajektorije standardnom linearizacijom - ne podesena jakost

potencijala
iteracija podesavanja parametara, dobiveni su slijedece vrijednosti prikazane tablicom 2
Tablica 2: Vrijednosti parametara jakosti potencijala
Varijable

vrijednost

a1

b1

15

b2

cime je dobivena trajektorija koja uspjesno izbjegava prepreke.

16

Slika 12 prikazuje malo odstupanje stvarne trajektorije od referentne. U pocetnom

trenutku orijentacija robota je u smjeru osi x, te se ne poklapa s vektorom smjera
referentne trajektorije. Kada se putanja mobilnog robota poklopi s referentnom
trajektorijom, nema vecih odstupanja, te se moze konstatirati da robot dobro prati
zeljenu trajektoriju.

10
9
8
7

Y, m

6
5
4
3
2
1

Referentna trajektorija
Trajektorija robota
Prepreka

0
1
1

4
5
X, m

10

Slika 12: Pracenje trajektorije standardnom linearizacijom - konfiguracija 1

3.3.1

Normiranje referentnih veli

cina

Da bi smo mogli primjeniti gore navedenu metodu vodenja mobilnog robota u realnim primjenama, potrebno je normirati vrijednosti referentnih brzina dobivenih
metodom potencijalnih polja na iznose ostvarive ugradenim aktuatorima. U ovom
trenutku nije toliko bitan gornji iznos ostvarivih brzina nego nacin na koji se one
ogranicavaju.
Ako se u jednadzbu 39 uvrste ogranicenja oblika Vmax tanh(U ) vidimo da ce vrijed-

17

nosti U biti definirane samo unutar intervala Vmax

pd = Vmaxa tanh(a1 p Ua (p, pA )) + Vmaxb1 tanh(b1 p Ur (p, pR ))+
+ Vmaxb2 tanh(b2 Jp Ur (p, pR ))

(73)

Postavljanjem vrijednosti koeficijenata brzina polja na iznose prikazane tablicom 3

Tablica 3: Koeficijenti brzina polja
Varijable

vrijednost

Vmaxa

0.7

Vmaxb1

0.7

Vmaxb2

0.6

dobivamo slijedece upravljacke modele.

10
9

Referentna trajektorija
Trajektorija robota
Prepreka

8
7

Y, m

6
5
4
3
2
1
0
1
1

4
5
X, m

Slika 13: Pracenje trajektorije

18

10

Slika 13 prikazuje referentnu trajektoriju i putanju mobilnog robota nakon normiranja vrijednosti maksimalnih iznosa brzina atraktivnog i repulzivnih potencijala, a
time i ukupnu linearnu brzinu gibanja. Vidljivo je da se normiranjem ne gubi tocnost
pracenja trajektorije, sto je bilo i za ocekivati. No smanjenjem brzine produljit ce
se vrijeme trajanja simulacije.
Na slici 14 prikazane su normirane brzine i ubrzanja. Vidljivo je da su maksimalni
iznosi brzina oko 2ms1 sto je lako ostvarivo s prosjecnim elektromotorom, ali su
ubrzanja u kratkom pocetnom periodu oko 13ms2 sto bi vec moglo biti problem
ostvariti. Proracunskom modelu je potrebno oko 20 sekundi da dostigne tocku cilja.

dYref, m/s

ddXref, m/s2

1.5

0.5
0

10

20
t, s

30

40

0.5

15

15

10

10

ddYref, m/s2

dXref, m/s

5
0
5

10

20
t, s

30

40

10

20
t, s

30

40

10

20
t, s

30

40

5
0
5

Slika 14: Brzine i ubrzanja mobilnog robota

19

Nakon nekoliko podesavanja koeficijenata, dobivamo ubrzanja od oko 6ms2 ali

se i vrijeme simulacije produzilo na preko 40 sekundi, sto je vidljivo slikom 15.

0.6

0.5

0.5

0.4

dYref, m/s

dXref, m/s

0.2
0

20

40

0.2

60

20

2
0
2

20

40

60

40

60

t, s

ddYref, m/s2

ddXref, m/s2

t, s

40

2
0
2

60

20

t, s

t, s

Slika 15: Brzine i ubrzanja mobilnog robota

Vrijednosti koeficijenata brzina polja prikazani su tablicom 4 Prepravljeni matlab
Tablica 4: Koeficijenti brzina polja
Varijable

vrijednost

Vmaxa

0.3

Vmaxb1

0.3

Vmaxb2

0.2

kod nalazi se u prilogu.

20

Matlab kodovi

U prilogu su sadrzani matlab kodovi koji se odnose na obradeni zadatak, te su

pojedini odlomci referencirani u tekstu. Kodovi koji se odnose na isti problem ali
uz neke manje izmjene, biti ce navedene samo izmjene i linija na koju se ta izmjena
odnosi.

4.1

Metoda potencijalnih polja

U ovom odjeljku nalaze se matlab kodovi za generiranje trajektorije metodom umjetnih potencijalnih polja.
4.1.1

Trajektorija bez lokalnih minimuma

Glavna datoteka u kojoj se inicijaliziraju varijable i nakon provodenja numerickog

rjesavanja diferencijalne jednadzbe, ispis rezultata obrade.
clear;
clc;
axis(equal);

% set equal scale on axes per pixel

T=40.0; %vrijeme simulacije

DT=0.01; %korak integracije
%--------------------------------------------------------------%
%Globalne varijable
global b xa ya x0 y0 j1 j2 j3
b=2;
R=0.5;
xa=9;
ya=9;
j1=1;
j2=5;
j3=2;
tT = 0:DT:T;
%--------------------------------------------------------------%
%pozicije prepreka
x0 = [2 3 4 5

5.5 4];

y0 = [2 3 1 2.5 5.5 3.5 3.5 4];

%po
cetni uvjeti
21

xx0 = [0 0];
%--------------------------------------------------------------%
% numerika
options = odeset(RelTol,1e-5,AbsTol,1e-5);
[t,y] = ode45(mobilac,tT,xx0,options);
%--------------------------------------------------------------%
%ispis
figure(1)
hold on
plot(y(:,1),y(:,2), r);xlabel(X);ylabel(Y)
for z=1:length(x0)
[xx1, yy1] = circle (x0(z), y0(z), R);
plot(xx1, yy1, r)
end
Datoteka u kojoj su definirane diferencijalne jednadzbe koja se poziva iz glavne
datoteke iz funkcije ode45.
function dy = MobRobot(t,y)
dy = zeros(2,1);

% a column vector

global b xa ya x0 y0 j1 j2 j3
%--------------------------------------------------------------%
x1=y(1); y1=y(2);
ra_2=(x1-xa)^2+(y1-ya)^2;
for i=1:length(x0)
r0_2(i)=(x1-x0(i))^2 + (y1-y0(i))^2;
end
%atraktivni potencijal
dVa_dX = tanh(5*(x1-xa));
dVa_dY = tanh(5*(y1-ya));
%ukupni repulzivni potencijal
dVr_dX = 0;
dVr_dY = 0;
for i=1:length(x0)
dVr_dX = (dVr_dX - b*(x1-x0(i))*exp(-b*r0_2(i)));

22

dVr_dY = (dVr_dY - b*(y1-y0(i))*exp(-b*r0_2(i)));

end
%ukupni potencijal
dy(1) = -j1*dVa_dX - j2*dVr_dX + j3*dVr_dY;
dy(2) = -j1*dVa_dY - j2*dVr_dY - j3*dVr_dX;
%--------------------------------------------------------------%

4.1.2

Trajektorija s lokalnim minimumom

Datoteka koja sadrzi funkciju s diferencijalnim jednadzbama ostaje u nepromijenjenom obliku prema odjeljku [4.1.1], dok se u glavnoj datoteci mijenja vektor
prepreka na slijedeci nacin
%...
%pozicije prepreka
x0 = [2 3 4 5

5.5 4 8 8 8 8 7 6];

y0 = [2 3 1 2.5 5.5 3.5 3.5 4 5 6 7 8 8 8];

%...
oznaka %... pokazuje da se prije i poslije navedenog koda nalazi nepromijenjeni
ostatak koda datoteke.

4.2

Pribli
zna linearizacija

clear;
clc;
axis(equal);

% set equal scale on axes per pixel

T=15.0; %vrijeme simulacije

DT=0.01; %korak integracije
%--------------------------------------------------------------%
%Globalne varijable
global A w k1 k2 k3 k4 b j1 j2 j3 x0 y0 xa ya KK eps
A = 1;
w = pi;
23

L = 0.29;
%
lambda1=5;
lambda2=5;
omegan=5;
zeta=1;
k1=lambda1;
k2=lambda2+2*zeta*omegan;
k3=omegan^2+2*zeta*omegan*lambda2;
k4=omegan^2*lambda2;
KK=50;
%std_control 1 - ne pode
seni parametri
j1=6;
j2=15;
j3=5;

b=2;
% eps=0.0001;
eps=0.000001;
R=0.5;
%--------------------------------------------------------------%
tT = 0:DT:T;
%std_control 2
% x0 = [4 6 7];
% y0 = [5 4 6.5];
% xa=9;
% ya=9;
% x0 = [2 5 7];
% y0 = [2 3 2.9];

24

% xa=9;
% ya=9;
x0 = [3 4

6.6 8.3];

y0 = [3 1.7 5.1 9];

xa=9;
ya=9;
xx0 = zeros(10, 1);
%--------------------------------------------------------------%
% numerika
options = odeset(RelTol,1e-5,AbsTol,1e-5);
[t,y] = ode45(MobRobot,tT,xx0,options);
%--------------------------------------------------------------%
X1_tilda = y(1) - y(7);
X4_tilda = y(2) - y(10);

figure(1)
hold on
plot(y(:,1),y(:,2),b, linewidth,1.2), xlabel(X, m),...
ylabel(Y, m), axis([-1 10

-1

10])

xlabel(X)
ylabel(Y)
plot(y(:,7),y(:,10), g)
for z=1:length(x0)
[xx1, yy1] = circle (x0(z), y0(z), R);
plot(xx1, yy1, r),
end
legend(Referentna trajektorija, Trajektorija robota,...
Prepreka,4)

25

function dy = MobRobot(t,y)
dy = zeros(10,1);

% a column vector

global A w k1 k2 k3 k4 b j1 j2 j3 x0 y0 xa ya KK eps
%----- Referentna trajektorija generirana PFM ----------------%
x1=y(1); y1=y(2);
ra_2=(x1-xa)^2+(y1-ya)^2;
for i=1:length(x0)
r0_2(i)=(x1-x0(i))^2 + (y1-y0(i))^2;
end
dVa_dX = tanh((x1-xa));
dVa_dY = tanh((y1-ya));
dVr_dX = 0;
dVr_dY = 0;
for i=1:length(x0)
dVr_dX = (dVr_dX - b*(x1-x0(i))*exp(-b*r0_2(i)));
dVr_dY = (dVr_dY - b*(y1-y0(i))*exp(-b*r0_2(i)));
end

dXref = -j1*dVa_dX - j2*dVr_dX + j3*dVr_dY;

dYref = -j1*dVa_dY - j2*dVr_dY - j3*dVr_dX;
%--------------------------------------------------------------%
%---Estimacija druge derivacije--------------------------------%
ddXref = -KK*(y(3) - dXref);
ddYref = -KK*(y(4) - dYref);
%--------------------------------------------------------------%
%---Estimacija trece derivacije--------------------------------%
26

dddXref = -KK*(y(5) - ddXref);

dddYref = -KK*(y(6) - ddYref);
%--------------------------------------------------------------%
%---Referentni sustav------------------------------------------%
U1_ref = dXref;
U2_ref = (dddYref*dXref^2-dddXref*dYref*dXref...
-3*ddYref*dXref*ddXref+3*dYref*ddXref^2)/(eps+dXref^4);
X1_ref = y(1);
X2_ref = (ddYref*dXref-dYref*ddXref)/(eps+abs(dXref)^3)...
*sign(dXref);
X3_ref = dYref/(eps+abs(dXref))*sign(dXref);
X4_ref = y(2);
%--------------------------------------------------------------%
%---Regulacijska pogreska--------------------------------------%
X1_tilda = X1_ref - y(7);
X2_tilda = X2_ref - y(8);
X3_tilda = X3_ref - y(9);
X4_tilda = X4_ref - y(10);
U1_refapp = (abs(U1_ref)+eps);
U1_tilda = -k1*X1_tilda;
U2_tilda = -k2*X2_tilda-(k3*X3_tilda/U1_refapp)*sign(U1_ref)...
-(k4*X4_tilda/(eps+U1_ref^2));
U1 = U1_ref - U1_tilda;
U2 = U2_ref - U2_tilda;
%--------------------------------------------------------------%
%---Diferencijalne jednad
zbe-----------------------------------%
dy(1) = dXref;

27

dy(2) = dYref;
dy(3) = ddXref;
dy(4) = ddYref;
dy(5) = dddXref;
dy(6) = dddYref;
dy(7) = U1;
dy(8) = U2;
dy(9) = y(8)*U1;
dy(10) = y(9)*U1;
% --------------------------------------------------------------%

4.3

Pribli
zna linearizacija - normirane veli
cine

clear;
clc;
axis(equal);

% set equal scale on axes per pixel

T=50.0; %vrijeme simulacije

DT=0.01; %korak integracije
%--------------------------------------------------------------%
%Globalne varijable
global A w k1 k2 k3 k4 b j1 j2 j3 x0 y0 xa ya KK eps Vmaxa Vmaxr1 ...
Vmaxr2
A = 1;
w = pi;
L = 0.29;
lambda1=12;
lambda2=12;
omegan=5;

28

zeta=1;
k1=lambda1;
k2=lambda2+2*zeta*omegan;
k3=omegan^2+2*zeta*omegan*lambda2;
k4=omegan^2*lambda2;
KK=50;
Vmaxa=0.3;
Vmaxr1=0.3;
Vmaxr2=0.2;
j1=2;
j2=3;
j3=4;
b=2;
eps=0.01;
R=0.5;
%--------------------------------------------------------------%
tT = 0:DT:T;
x0 = [3 4

6.6 8.3];

y0 = [3 1.7 5.1 9];

xa=9;
ya=9;
xx0 = zeros(10, 1);
%--------------------------------------------------------------%
% numerika
options = odeset(RelTol,1e-5,AbsTol,1e-5);
[t,y] = ode45(MobRobot,tT,xx0,options);
%--------------------------------------------------------------%

29

figure(1)
hold on
plot(y(:,1),y(:,2),b, linewidth,1.2), xlabel(X, m), ...
ylabel(Y, m), axis([-1 11

-1

11])

xlabel(X)
ylabel(Y)
plot(y(:,7),y(:,10), g)
for z=1:length(x0)
[xx1, yy1] = circle (x0(z), y0(z), R);
plot(xx1, yy1, r),
end
legend(Referentna trajektorija, Trajektorija robota, Prepreka,4)
figure(2)
subplot(2,2,1), plot(t, y(:,3), b), xlabel(t, s), ...
ylabel(dXref, m/s),
subplot(2,2,2), plot(t, y(:,4), b), xlabel(t, s), ...
ylabel(dYref, m/s),
subplot(2,2,3), plot(t, y(:,5), b), xlabel(t, s), ...
ylabel(ddXref, m/s^2),
subplot(2,2,4), plot(t, y(:,6), b), xlabel(t, s), ...
ylabel(ddYref, m/s^2),
function dy = MobRobot(t,y)
dy = zeros(10,1);

% a column vector

global A w k1 k2 k3 k4 b j1 j2 j3 x0 y0 xa ya KK eps Vmaxa Vmaxr1 ...

Vmaxr2
%----- Referentna trajektorija generirana PFM ----------------%
x1=y(1); y1=y(2);
ra_2=(x1-xa)^2+(y1-ya)^2;

30

for i=1:length(x0)
r0_2(i)=(x1-x0(i))^2 + (y1-y0(i))^2;
end
dVa_dX = (x1-xa);
dVa_dY = (y1-ya);
dVr_dX = 0;
dVr_dY = 0;
for i=1:length(x0)
dVr_dX = (dVr_dX - b*(x1-x0(i))*exp(-b*r0_2(i)));
dVr_dY = (dVr_dY - b*(y1-y0(i))*exp(-b*r0_2(i)));
end
dXref = Vmaxa*tanh(-j1*dVa_dX) - Vmaxr1*tanh(j2*dVr_dX) + ...
Vmaxr2*tanh(j3*dVr_dY);
dYref = Vmaxa*tanh(-j1*dVa_dY) - Vmaxr1*tanh(j2*dVr_dY) - ...
Vmaxr2*tanh(j3*dVr_dX);
%--------------------------------------------------------------%
%---Estimacija druge derivacije--------------------------------%
ddXref = -KK*(y(3) - dXref);
ddYref = -KK*(y(4) - dYref);
%--------------------------------------------------------------%
%---Estimacija trece derivacije--------------------------------%
dddXref = -KK*(y(5) - ddXref);
dddYref = -KK*(y(6) - ddYref);
%--------------------------------------------------------------%
%---Referentni sustav------------------------------------------%
U1_ref = dXref;
U2_ref = (dddYref*dXref^2-dddXref*dYref*dXref...

31

-3*ddYref*dXref*ddXref+3*dYref*ddXref^2)/(eps+dXref^4);
X1_ref = y(1);
X2_ref = (ddYref*dXref-dYref*ddXref)/(eps+abs(dXref)^3)*tanh(9*dXref);
X3_ref = dYref/(eps+abs(dXref))*tanh(9*dXref);
X4_ref = y(2);
%--------------------------------------------------------------%
%---Regulacijska pogreska--------------------------------------%
X1_tilda = X1_ref - y(7);
X2_tilda = X2_ref - y(8);
X3_tilda = X3_ref - y(9);
X4_tilda = X4_ref - y(10);
U1_refapp = (abs(U1_ref)+eps);
U1_tilda = -k1*X1_tilda;
U2_tilda = -k2*X2_tilda-(k3*X3_tilda/U1_refapp)*tanh(9*U1_ref)...
-(k4*X4_tilda/(eps+U1_ref^2));
U1 = U1_ref - U1_tilda;
U2 = U2_ref - U2_tilda;
%--------------------------------------------------------------%
%---Diferencijalne jednad
zbe-----------------------------------%
dy(1) = dXref;
dy(2) = dYref;
dy(3) = ddXref;
dy(4) = ddYref;
dy(5) = dddXref;
dy(6) = dddYref;
dy(7) = U1;
dy(8) = U2;

32

dy(9) = y(8)*U1;
dy(10) = y(9)*U1;
% --------------------------------------------------------------%

33

Literatura
[1] J.J. Risler A. Bellaiche, F. Jean. Robot Motion Planning and Control, volume
229, chapter Geometry of nonholonomic systems, pages 5591. Springer, Berlin
/ Heidelberg, 1998.
[2] B.Novakovic. Metode vodenja tehnickih sistema: primjena u robotici, fleksibilnim

sistemima i procesima. Skolska

knjiga, Zagreb, 1990.
[3] B.Novakovic. Regulacijski sistemi. Sveucilisna naklada d.o.o., Zagreb, 1990.
[4] Jean-Claude Habumuremyi. Models of a wheeled robot named robudem and
design of a state feedback controller for its posture tracking. ISMCR05, 08(10),
November 2005.
[5] D. Majetic B. Novakovic J. Kasac, D. Brezak. Mobile robot path planing using
gauss potential functions and neural network. pages 287298, 2002.
[6] A. De Luca, G. Oriolo, and C. Samson. Robot Motion Planning and Control,
volume 229, chapter Feedback control of a nonholonomic car-like robot, pages
171253. Springer, Berlin / Heidelberg, 1998.
[7] Illah R. Nourbakhsh R.Siegwart. Introduction to Autonomous Mobile Robots.
The MIT Press, Massachusetts Institute of Technology Cambridge, 2004.

[8] T.Surina.
Automatska regulacija. Skolska
knjiga, Zagreb, 1991.
[9] J. Borenstein Y. Koren. Potential field methods and their inherent limitations
for mobile robot navigation. In Proc. of the IEEE Int. Conf. on Robotics and
Automation, pages 13981404, 1991.

34