upravljanje robotom

Zavod za robotiku i automatizaciju proizvodnih sustava

Katedra za strojarsku automatiku

SEMINARSKI RAD IZ KOLEGIJA:

OPCA TEORIJA SUSTAVA

Upravljanje mobilnim robotom s prednjimzakretnim kotacima

Nositelj kolegija: Student:

Prof. dr. sc. Branko Novakovic Sven Savic

Zagreb, 2008

Sadrzaj

1 Uvod 2

2 Struktura mobilnog robota 3

2.1 Kinematicke jednadzbe . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2 Ulancana forma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Regulacija mobilnog robota

u okolini s preprekama 8

3.1 Metoda potencijalnih polja . . . . . . . . . . . . . . . . . . . . . . . . 8

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

3.3 Simulacijski rezultati . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

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

4 Matlab kodovi 21

4.1 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

1 Uvod

Kod upravljanja mobilnim robotom uporabom potencijalnih polja, koriste se tri

stupnja upravljanja. Prvo se generira trajektorija koja izbjegava repulzivne poten-

cijale 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 rezul-

tiraju 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.

2

2 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 uprav-

ljive servo motorima (slike 1 i 2).

Model je pogonjen pomocu dva DC motora, ali preko zajednickog kontrolera (H-

most) 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 Kinematicke jednadzbe

Ako uzmemo x y ravninu kao ravninu po kojoj se robot krece, mozemo zapisatidvije 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.

=v1l

sin(1 2)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 ra-

zlicite 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 odgo-

vara kinematickom modelu automobila pa se jos naziva i car-like robot. Konfi-

guracija robota je predstavljena pozicijom i orijentacijom tijela robota u ravnini i

4

kutom zakreta kotaca.

x = v1 cos (6)

y = v1 sin (7)

=v1 tan

l(8)

= v2 (9)

2.2 Ulancana forma

Postojanje kanonske forme kinematickog modela nonholonomic robota je neophodna

za sistematican razvoj open-loop i closed-loop strategija upravljanja. Najvise upotre-

bljavana kanonska struktura je ulancana forma. Za upravljacki sustav s dva ulaza,

ulancana forma prikazana je jednadzbom (10) prema [6, 1].

x1 = u1

x2 = u2

x3 = x2u1 (10)

...

xn = xn1u1

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 (15) sadrzi tri upravljackevelicine v1 v3, te ukoliko ga zelimo zapisati u ulancanoj formi koja sadrzi samodvije upravljacke velicine, potrebno je promatrati pojednostavljeni model prikazan

jednadzbama (6 9). Ulancanu formu oblika (10), dobivamo na nacin da pret-postavimo da su x1 = x i x4 = y iz cega slijedi

x1 = 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 (13)

5

Izraz za x3 dobivamo na slijedeci nacin

x4 = y (14)

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

x3u1 = v1 sin (15)

uz transformaciju (13) dobivamo slijedeci izraz

x3u1 = u1sin

cos (16)

koji nakon sredivanja dobiva oblik

x3 = tan (17)

Raspisemo li totalni diferencijal dobivenog izraza (17), slijedi

x3 = 1cos2

(18)

uvrstimo li izraz za x3 prema jednadzbi (10), slijedi

x2u1 = 1cos2

(19)

nakon sredivanja dobivamo izraz

x2 = 1u1 cos2

(20)

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

x2 =v1 tan

l 1v1 cos

1cos2

(21)

iz kojeg nakon sredivanja slijedi izraz za x2

x2 =tan

l cos3 (22)

Raspisemo li totalni diferencijal izraza (22) dobivamo izraz

x2 =1

l

1cos2 cos3 + 3 cos2 sin tan cos6

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

u2 =1

l

v2 1cos2 cos3

+ 1l

3 v1 cos2 sin cos cos tan l tan cos6

(24)6

daljnjim sredivanjem, slijedi

u2 =v2

l cos2 cos3 +3

l2v1 tan tan2

cos3 (25)

nakon sto izraz svedemo na zajednicki nazivnik, dobivamo

u2 =l v2 cos + 3 u1 tan sin2

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 (29)

x2 =tan

l cos2 (30)x3 = tan (31)

x4 = y (32)

i ulazne transformacije (33) - (34)

v1 =u1

cos (33)

v2 = u2 l cos2 cos3 3 u1 sin sin2

l cos2 (34)

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

robota = pi/2 kpi, k N . Promjene koordinata (29) - (32) moguce je general-izirati 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.

7

3 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 ko-

risti 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 koordi-

natnom 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. Slika 6: Repulzivni potencijal.

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

i to za jedan korak unaprijed

8

Metoda potencijalnih polja sastoji se od izmjene topologije radnog prostora rob-

ota 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), atrak-

tivni potencijal iznosi, prema [5]

Ua(p) =1

2

[(x xG)2 + (y yG)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

Ur(p) = exp

( 1

2

[(x xR,j)2 + (y yR,j)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

xd = a1Uax

+Nj=0

(b1Ur

x+ b2

Ury

)(37)

yd = a1Uay

+Nj=0

(b1Ur

y b2Ur

x

)(38)

9

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 = a1pUa(p, pA) + b1pUr(p, pR) b2JpUr(p, pR) (39)

uz

p = [x y]T , p [

x

y

]T, J =

[0 1

1 0

](40)

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).

0 2 4 6 8 101

0

1

2

3

4

5

6

7

8

9

10

X, m

Y, m

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

imuma 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

0 2 4 6 8 101

0

1

2

3

4

5

6

7

8

9

10

X, m

Y, m

Slika 10: Trajektorija generirana metodom potencijalnih polja (primjer lokalnog

minimuma).

minimuma, je metoda vrtloznih polja. Repulzivno djelovanje zamijenjeno je brzi-

nom toka koja tangira konturu (prepreku) tako da je robot prisiljen zaokrenuti oko

prepreke, sto dovodi do konvergencije algoritma planiranja putanje.

3.2 Pracenje referentne trajektorije

Odabrana je metoda pracenja trajektorije bazirana na teoriji standardnog linearnog

upravljanja. Temelji se na pribliznoj linearizaciji jednadzbi sustava oko zeljene tra-

jektorije, 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

xd + 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

xx+

f

uu+ h(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) (46)

gdje su Jacobiani A(t) i B(t) definirani kao

A(t) =

f1x1

f1x2

f1xn

f2x1

f2x2

f2xn

......

. . ....

fnx1

fnx2

fnxn

, B(t) =

f1u1

f1u2

f1un

f2u1

f2u2

f2un

......

. . ....

fnu1

fnu2

fnun

(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)xd(t) yd(t)xd(t)]/x3d(t) (50)xd3(t) = yd(t)/xd(t) (51)

xd4(t) = yd(t) (52)

(53)

12

i transformacije ulaza

ud1(t) = xd(t) (54)

ud2(t) = [...y d(t)x

2d(t)

...x d(t)yd(t)xd(t) 3yd(t)xd(t)xd(t) + 3yd(t)x2d(t)]/x4d(t) (55)

U gornjim izrazima pojavljuju se druge i trece derivacije referentnih pozicija xref

i yref za cije estimacije su koristeni filteri drugog odnosno treceg reda prikazani

slijedecim izrazima

f (x) f(x+ x) f(x)x

(56)

f (x) f(x+ x) 2f(x) + f(xx)x2

(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

x1 = u1 (59)

x2 = u2 (60)

x3 = xd2ud1 x2u1 (61)x4 = xd3ud1 x3u1 (62)

Linearizacija oko zeljene trajektorije dovodi do slijedeceg linearnog vremenski zav-

isnog sustava

x =

0 0 0 0

0 0 0 0

0 ud1(t) 0 0

0 0 ud1(t) 0

x+

1 0

0 1

xd2(t) 0

xd3(t) 0

u = A(t)x+B(t)u (63)

Gdje je pogreska transformacije upravljackih velicina prikazana kao linearni vremen-

ski zavisan zakon

u1 = k1x1 (64)

u2 = k2x2 k3ud1

x3 k4u2d1

x4 (65)

13

Da bi sustav bio stabilan mora biti zadovoljeno k1 > 0, a jednadzba (66) mora biti

Hurwitzov polinomu.

3 + k22 + 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

H1 = k2, H2 =

k2 k41 k3 , H3 =

k2 k4 0

1 k3 0

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 5

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.

Avl(t) =

k1 0 0 0

0 k2 k3/ud1(t) k4/u2d1(t)k1xd2(t) ud1(t) 0 0k1xd3(t) 0 ud1(t) 0

(70)

14

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

janja.

Da bismo odredili dvije realne negativne svojstvene vrijednosti 1 i 2 i dvijekompleksne svojstvene vrijednosti sa negativnim realnim dijelom, modul n i ko-

eficijent prigusenja (0 < 1), pojacanja ki trebaju biti odabrana na slijedecinacin

k1 = 1, k2 = 2 + 2n, k3 = 2n + 2n2, k4 =

2n2 (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 brz-

inu 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 pozici-jama 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 32ud1x3 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 re-

pulzivnog 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

1 0 1 2 3 4 5 6 7 8 9 101

0

1

2

3

4

5

6

7

8

9

10

X, m

Y, m

Referentna trajektorijaTrajektorija robotaPrepreka

Slika 11: Pracenje trajektorije standardnom linearizacijom - ne podesena jakost

potencijala

iteracija podesavanja parametara, dobiveni su slijedece vrijednosti prikazane tabli-

com 2

Tablica 2: Vrijednosti parametara jakosti potencijala


a1 6

b1 15

b2 5

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.

1 0 1 2 3 4 5 6 7 8 9 101

0

1

2

3

4

5

6

7

8

9

10

X, m

Y, m


Slika 12: Pracenje trajektorije standardnom linearizacijom - konfiguracija 1

3.3.1 Normiranje referentnih velicina

Da bi smo mogli primjeniti gore navedenu metodu vodenja mobilnog robota u re-

alnim 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(a1pUa(p, pA)) + Vmaxb1 tanh(b1pUr(p, pR))++ Vmaxb2 tanh(b2JpUr(p, pR)) (73)

Postavljanjem vrijednosti koeficijenata brzina polja na iznose prikazane tablicom 3

Tablica 3: Koeficijenti brzina polja


Vmaxa 0.7

Vmaxb1 0.7

Vmaxb2 0.6

dobivamo slijedece upravljacke modele.

1 0 1 2 3 4 5 6 7 8 9 101

0

1

2

3

4

5

6

7

8

9

10

X, m

Y, m


Slika 13: Pracenje trajektorije

18

Slika 13 prikazuje referentnu trajektoriju i putanju mobilnog robota nakon normi-

ranja 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.

0 10 20 30 401

0

1

2

t, s

dXre

f, m

/s

0 10 20 30 400.5

0

0.5

1

1.5

t, s

dYre

f, m

/s

0 10 20 30 405

0

5

10

15

t, s

ddXr

ef, m

/s2

0 10 20 30 405

0

5

10

15

t, s

ddYr

ef, m

/s2

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 20 40 600.5

0

0.5

1

t, s

dXre

f, m

/s

0 20 40 600.2

0

0.2

0.4

0.6

t, sdY

ref,

m/s

0 20 40 602

0

2

4

6

t, s

ddXr

ef, m

/s2

0 20 40 602

0

2

4

6

t, s

ddYr

ef, m

/s2

Slika 15: Brzine i ubrzanja mobilnog robota

Vrijednosti koeficijenata brzina polja prikazani su tablicom 4 Prepravljeni matlab

Tablica 4: Koeficijenti brzina polja


Vmaxa 0.3

Vmaxb1 0.3

Vmaxb2 0.2

kod nalazi se u prilogu.

20

4 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 um-

jetnih 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 6 7 5.5 4];

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

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

jenjenom obliku prema odjeljku [4.1.1], dok se u glavnoj datoteci mijenja vektor

prepreka na slijedeci nacin

%...

%pozicije prepreka

x0 = [2 3 4 5 6 7 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 Priblizna linearizacija

clear;

clc;




%--------------------------------------------------------------%

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


[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)


plot(xx1, yy1, r),

end

legend(Referentna trajektorija, Trajektorija robota,...

Prepreka,4)

25



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)



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);




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;


%--------------------------------------------------------------%

%---Diferencijalne jednadzbe-----------------------------------%

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 Priblizna linearizacija - normirane velicine

clear;

clc;




%--------------------------------------------------------------%

%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


[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)


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),


ylabel(dYref, m/s),


ylabel(ddXref, m/s^2),


ylabel(ddYref, m/s^2),



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)



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





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));



%--------------------------------------------------------------%

%---Diferencijalne jednadzbe-----------------------------------%

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

UvodStruktura mobilnog robotaKinematicke jednadbeUlancana forma

Regulacija mobilnog robota u okolini s preprekamaMetoda potencijalnih poljaPracenje referentne trajektorijeSimulacijski rezultatiNormiranje referentnih velicina

Matlab kodoviMetoda potencijalnih poljaTrajektorija bez lokalnih minimumaTrajektorija s lokalnim minimumom

Priblina linearizacijaPriblina linearizacija - normirane velicine

Bibliografija

Documents

upravljanje robotom