Tomislav Trošelj

0036403021

Fakultet elektrotehnike i računarstvaZavod za automatiku i računalno inženjerstvo

12.0

1.20

09.

Mobilna robotika

Umjeravanje odometrijskog sustava

Vježba broj 1.

1.1.Uvod

U vježbi se upoznajemo sa simulatorom mobilnog robota u Matlabu.Sljedeće s čim ćemo se upoznati su problemi sustavskih pogrešaka kod odometrijskog sustava mobilnog robota, te određivanje parametara za kompenzaciju sustavske pogreške odometrijskog sustava lokalizacije mobilnog robota.

1.2.Teoretski prikaz i razrada problema

Odometrijski sustav mobilnog robota označava sustav estimacije položaja mobilnog robota na temelju prethodno poznatog položaja.

Položaj mobilnog robota sastoji se od dva dijela: pozicije mobilnog robota izražene u (x, y) koordinatama kartezijevog koordinatnog

sustava orijentacije ( ) mjerene od pozitivne x-poluosi kartezijskog koordinatnog

sustava.Trenutni položaj mobilnog robota s tri kotača ( dva pogonska te jedan za stabilnost) estimira se prema kinematičkom modelu mobilnog robota predstavljen jednadžbama (Sl. 1.1.):

( 1-1 )

( 1-2 )

( 1-3 )

( 1-4 )

( 1-5 )

gdje je položaj mobilnog robota u k–tom trenutku; vL(k) i vR(k) obodne brzine lijevog i desnog kotača; i kutne brzine lijevog i desnog kotača; vt(k) translacijska brzina robota; T vrijeme uzorkovanja; b duljina osovine kotača; R radijus kotača.

1-1

Sl.1.1. Kinematički model mobilnog robotaNavedeni kinematički model koristi obodne brzine vrtnje pogonskih kotača mjerene enkoderima.

Enkoderi su u osnovi mjerni članovi kutne brzine vrtnje te obodnu brzinu vrtnje mogu mjeriti samo uz poznavanje radijusa kotača R. Također se rotacijska brzina mobilnog robota određuje prema poznatoj duljini osovine b. Nepoznavanje navedenih parametara ima za rezultat pogrešku u estimaciji položaja.

Ovakva vrsta pogreške se naziva sustavskom i mogu se kompenzirati jer imaju stalnu vrijednost za razliku od nesustavskih ( čine ih utjecaji neravne podloge, proklizavanje kotača, šum u mjerenju brzine vrtnje ) koje se ne mogu uzeti u obzir jer nemaju stalnu vrijednost. Sustavske pogreške najčešće se kompenziraju korištenjem kalibracije te uvrštavanjem kalibriranih parametara u prošireni model procesa.

U slučaju kinematičkog modela položaja mobilnog robota proširuju se izrazi ( 1-1 ) i ( 1-2) te oni postaju :

( 1-6 )

( 1-7 )

gdje parametri k1 i k2 kompenziraju utjecaj nepoznatih vrijednosti radijusa kotača, a k3 utjecaj nepoznate vrijednosti duljine osovine kotača. Vrijednosti parametara određuju se off-line kalibracijom korištenjem prikupljenih podataka iz dvaju eksperimenata. Eksperimenti su odabrani na taj način da dovedu do izražaja samo jedan dio parametara kalibracije kako bi se smanjila dimenzionalnost problema optimiranja.

Prvi eksperiment jest pravocrtno gibanje, a drugi jest zakret na mjestu.

U eksperimentu pravocrtnog gibanja najviše dolaze do izražaja parametri k1 i k2. Eksperiment se radi tako da se reference obodnih brzina oba pogonska kotača postave na istu vrijednost i mobilni robot mora prijeći udaljenost dovoljnu da do izražaja dođe sustavska pogreška uslijed koje mobilni robot skreće uvijek na istu stranu.

Kako se u ovom eksperimentu mobilni robot giba približno pravocrtno, rotacijska brzina je približno nula i parametar k3 ima minimalan utjecaj. U eksperimentu zakreta na mjestu mobilni robot se mora zakrenuti za 180° oko svoje osi čime do izražaja dolazi parametar k3. Ostali parametri ne utječu jer je translacijska brzina približno jednaka nuli te se pozicija mobilnog robota ne mijenja.

1-2

U oba eksperimenta snimaju se svi relevantni podaci (početni položaj mobilnog robota, obodne brzine pogonskih kotača te vrijeme uzorkovanja) kako bi se krajnji položaj mogao estimirati korištenjem gore opisanog kinematičkog modela i usporediti sa krajnjim položajem koji je mobilni robot dosegao u simulaciji. Korištenjem optimizacijskog „toolboxa“ moguće je zatim odrediti vrijednosti parametara za kalibraciju. Pri tome se kao kriterij optimiranja koristi vrijednost razlike estimirane i stvarne orijentacije mobilnog robota. Prikupljeni podaci prvog i drugog eksperimenta se nezavisno koriste u procesu optimiranja. Prvo se koriste podaci prikupljeni u prvom eksperimentu za određivanje vrijednosti parametara k1 i k2 uz početnu vrijednost parametra k3 jednaku 1.0, a nakon toga se korištenjem dobivenih vrijednosti parametara k1 i k2 određuje vrijednost parametra k3 optimiranjem na temelju podataka prikupljenih u drugom eksperimentu. Postupak se ponavlja u nekoliko iteracija da bi vrijednosti kalibracijskih parametara poprimile stalnu vrijednost. U vježbi je dovoljno izvršiti jednu iteraciju da se odrede vrijednosti parametara.

Za svaki proces optimiranja potrebna je zasebna funkcija kriterija optimiranja koju će pozivati „fsolve“ optimizacijska metoda. Ulazni parametri funkcije kriterija optimiranja su kalibracijski parametri (k1 i k2 u slučaju podataka prvog eksperimenta te k3 u slučaju podataka sakupljenih tijekom drugog eksperimenta). Tijelo funkcije kriterija optimiranja čini učitavanje sakupljenih podataka, izračun krajnjeg položaja na temelju proslijeđenih vrijednosti kalibracijskih parametara i učitanih sakupljenih podataka te izračun rezultata kriterija optimiranja koji je razlika estimirane i stvarne orijentacije mobilnog robota.

Funkcije kriterija optimalnosti implementirane su u „m“-funkciji „calibratekLeftkRight“ (izračuna vrijednosti kalibracijskih parametara k1 i k2) te u „m“-funkciji „calibratekAxle“ (izračun kalibracijskog parametra k3). Funkcije kriterija optimiranja potrebne podatke učitava iz „mat“ datoteka „eksperiment1“ i „eksperiment2“ pa je potrebno u upravljačkim algoritmu osigurati spremanje brzine vrtnje lijevog (varijabla se treba zvati „leftVel“) i desnog (varijabla se treba zvati „rightVel“) pogonskog kotača te koordinate početnog (varijable se trebaju zvati „x_start“, „y_start“ te „theta_start“) i konačnog položaja mobilnog robota (varijable se trebaju zvati „x_exact“, „y_exact“ i „theta_exact“). Vrijeme uzorkovanja je postavljeno u simulatoru (varijabla „sps“, vrijednost je postavljena je na 1). Vrijednosti varijabli brzina kotača potrebno je spremiti u svakom koraku uzorkovanja tako da se u „mat“ datoteci na kraju nalaze vektori duljine jednake broju koraka uzorkovanja. Početni položaj mobilnog robota potrebno je spremiti u prvom koraku simulacije, a stvarni krajnji položaj u zadnjem koraku simulacije. Trajanje simulacije odabrati tako da odmak mobilnog robota zbog sustavske pogreške dođe do izražaja.

Upute za „AMORsim“ simulator nalaze se u pdf obliku u mapi „Manual“. Simulator se pokreće naredbom „simedit“. Nakon pokretanja potrebno je učitati model radnog prostora naredbom „Import bitmap“ te model mobilnog robota naredbom „Add new robot“ (desni klik miša unutar radnog prostora). Model radne prostora za ovu vježbu je spremljen u datoteci „Calibration.bmp“. Potrebno je odabrati model mobilnog robota sa 16 sonara. Upravljački algoritam za eksperimente potrebno je dodati u datoteku „algtempsonars.m“ poštujući komentare. Prilikom prvog pokretanja izvršava se primjer algoritma koji samo dohvaća stvarni i estimirani položaj mobilnog robota, a upravljati se može korištenjem pripadnih grafičkih tipki na zaslonu. U navedenoj datoteci s upravljačkim algoritmom su također označeni dijelovi specifični za naredne laboratorijske vježbe. Simulator omogućuje dohvat stvarnih podataka vezanih za simulirani mobilni robot te dohvat estimiranih odnosno mjerenih. U slučaju realnog mobilnog robota stvarni podaci nisu dostupni pa se ti podaci u sklopu vježbe mogu koristiti samo za ocjenu kvalitete umjeravanja napravljenog korištenjem estimiranih odnosno izmjerenih podataka.

1-3

1.3. Priprema

Napisati upravljački algoritam za provođenje prvog eksperimenta te upravljački algoritam za provođenje drugog eksperimenta. Upravljački algoritmi trebaju biti tako koncipirani da se pomoću njih eksperimenti mogu ponoviti uz korištenje dobivenih vrijednosti kalibracijskih parametara.U prvom eksperimentu mobilni robot bi trebao doći do kraja radnog prostora, a u drugom dostići orijentaciju od 180 stupnjeva.

Napomena za pripremu: Ishodište koordinatnog sustava simulatora mobilnog robota jest donji lijevi kut, a orijentacija mobilnog robota se mjeri u smjeru suprotno od kazaljke na satu od pozitivne x-poluosi tako definiranog koordinatnog sustava. U upravljačkim algoritmima potrebno je koristiti „persistent“ naredbu kako bi se omogućilo da varijabla zadrži vrijednost do sljedećegh izvršavanja algoritma.Za dohvat stvarne kutne brzine pogonskih kotača koristiti naredbu „getvel“ , a za dohvat izmjerene brzine naredbu „getmeasurevel“.Za dohvat stvarnog položaja mobilnog robota koristite naredbe „getpos“ te „gethead“ , a za dohvat estimiranog položaja naredbu „getestimpos“ te „getestimhead“. Za podešavanje brzine vrtnje pogonkih kotača koristiti naredbu „setvel“.

Algoritam 1.eksperimenta:

function [new] = eksp1(simrobot,matrix,step)

% set global variablesglobal k1 k2 k3

% set persistent variablespersistent x_start y_start theta_start ...x y theta x_true y_true theta_true...k leftVel rightVel vl vr

% set algorithm initial parameter valuesif step == 1k1=1; k2=1; k3=1;

% k1=1.0025; k2=0.9975; k3=1.0025; parametri koje smo dobili

% get estimated mobile robot start poseposition = getestimpos(simrobot);x_start = position(1);y_start = position(2);theta_start = getestimhead(simrobot);

% get estimated pose

theta = getestimhead(simrobot);x = position(1);y = position(2);

% get true mobile robot pose

position = getpos(simrobot);x_true = position(1);y_true = position(2);theta_true = gethead(simrobot);

1-4

% set velocityvl=1; vr=1;simrobot=setvel(simrobot,[vl(step) vr(step)]);

disp('Initial parameters set.');

end

% algorithm start

% get estimated mobile robot poseposition = getestimpos(simrobot);x(step,1) = position(1);y(step,1) = position(2);theta(step,1) = getestimhead(simrobot);if theta(step,1)>180

theta(step,1)=theta(step,1)-360;

end

% radi plotanja brzinaif x(step,1)<297

vl=[vl 1];vr=[vr 1];k=step;

else vl=[vl 0];vr=[vr 0];endsimrobot = setvel(simrobot, [vl(step) vr(step)]);

% get true velocityvelocity = getmeasurevel(simrobot);rightVel(step,1) = velocity(2);leftVel(step,1) = velocity(1);

% get true mobile robot poseposition = getpos(simrobot);x_true(step,1) = position(1);y_true(step,1) = position(2);theta_true(step,1) = gethead(simrobot);

% algorithm end% save/plot needed variables at simulation endif step == 600

% position plotfigure(2)plot(x_true, y_true), hold on, plot(x, y, 'g')

xlabel('x position coordinate'), ylabel('y position coordinate')grid on, legend('true position', 'estimated position')title('MOBILE ROBOT POSITION')

% orientation plotfigure(3)plot(theta_true), hold on, plot(theta, 'g')xlabel('time step'), ylabel('orientation');grid on, legend('true orientation', 'estimated orientation')

title('MOBILE ROBOT ORIENTATION')1-5

% velocity plotfigure(4)subplot(2,1,1);plot(leftVel(1:k)),hold on,plot(vl(1:k),'g'); grid on;

ylabel('brzina lijevog kotaca'),legend('mjerena','stvarna');subplot(2,1,2);plot(rightVel(1:k)),hold on,plot(vr(1:k),'g'); grid on;ylabel('brzina desnog kotaca'),legend('mjerena','stvarna');

xlabel('step');

% save variables save results.mat x_start y_start theta_start x y theta;

% konacni polozaj mobilnog robotax_exact=x_true(end);y_exact=y_true(end);theta_exact=theta_true(end);

% save variables for kalibrationsave eksperiment1.mat x_start y_start theta_start ...x_exact y_exact theta_exact rightVel leftVel

disp('Needed variables saved');

end

% continue simulationnew = simrobot;

1-6

Algoritam 2.eksperimenta:

function [new] = eksp2(simrobot,matrix,step)

% set global variablesglobal k1 k2 k3

% set persistent variables

persistent x_start y_start theta_start ...

x y theta x_true y_true theta_true...

k leftVel rightVel vl vr

% set algorithm initial parameter values

if step == 1

k1=1; k2=1; k3=1;

% get estimated mobile robot start poseposition = getestimpos(simrobot);x_start = position(1);y_start = position(2);theta_start = getestimhead(simrobot);

% get estimated posetheta = getestimhead(simrobot);x = position(1);y = position(2);

% get true mobile robot poseposition = getpos(simrobot);x_true = position(1);y_true = position(2);theta_true = gethead(simrobot);

% set velocityvl=-0.05; vr=0.05;simrobot=setvel(simrobot,[vl(step) vr(step)]);

disp('Initial parameters set.');

end

% algorithm start

% get estimated mobile robot poseposition = getestimpos(simrobot);x(step,1) = position(1);y(step,1) = position(2);theta(step,1) = getestimhead(simrobot);

if theta(step,1)>180

theta(step,1)=theta(step,1)-360;

1-7

end

% check orientation

theta_true(step,1) = gethead(simrobot);

if theta_true<180

vl=[vl -0.05];vr=[vr 0.05];

k=step; % iskljucivo radi plotanja brzina dok se giba

else vl=[vl 0];vr=[vr 0];endsimrobot = setvel(simrobot, [vl(step) vr(step)]);

% get true velocityvelocity = getmeasurevel(simrobot);rightVel(step,1) = velocity(2);leftVel(step,1) = velocity(1);

% get true mobile robot poseposition = getpos(simrobot);x_true(step,1) = position(1);y_true(step,1) = position(2);theta_true(step,1) = gethead(simrobot);

% algorithm end

% save/plot needed variables at simulation endif step == 300

% position plotfigure(2)plot(x_true, y_true), hold on, plot(x, y, 'g')xlabel(' x position coordinate'), ylabel(' y position coordinate')grid on, legend('true position', 'estimated position')title('MOBILE ROBOT POSITION')% orientation plotfigure(3)plot(theta_true), hold on, plot(theta, 'g')xlabel('time step'), ylabel('orientation');grid on, legend('true orientation', 'estimated orientation')

title('MOBILE ROBOT ORIENTATION')

% velocity plotfigure(4)subplot(2,1,1);plot(leftVel(1:k)),hold on,plot(vl(1:k),'g'); grid on;

ylabel('brzina lijevog kotaca'),legend('mjerena','stvarna');subplot(2,1,2);plot(rightVel(1:k)),hold on,plot(vr(1:k),'g'); grid on;ylabel('brzina desnog kotaca'),legend('mjerena','stvarna');

xlabel('step');

% save variables save results.mat x_start y_start theta_start x y theta;

1-8

% konacni polozaj mobilnog robotax_exact=x_true(end);y_exact=y_true(end);theta_exact=theta_true(end);

% save variables for kalibrationsave eksperiment2.mat x_start y_start theta_start ...x_exact y_exact theta_exact rightVel leftVeldisp('Needed variables saved');end% continue simulationnew = simrobot;

1-9

1.4. Prikaz dobivenih rezultata

Prije pokretanja upravljačkih algoritama pokrećem m-funkciju „parametri(3)“.

Slijede prikazi traženih eksperimenata.

1.Eksperiment – pravocrtno gibanje ( parametri k1, k2 i k3 su postavljeni na vrijednost 1 )

Sl. 1.2. Prikaz eksperimenta za pravocrtno gibanje

Mobilni robot upravljan je prije navedenim prvim algoritmom.

U eksperimentu pravocrtnog gibanja najviše dolaze do izražaja parametri k1 i k2. Eksperiment se radi tako da se reference obodnih brzina oba pogonska kotača postave na istu vrijednost i mobilni robot mora prijeći udaljenost dovoljnu da do izražaja dođe sustavska pogreška uslijed koje mobilni robot skreće uvijek na istu stranu.

Kako se u ovom eksperimentu mobilni robot giba približno pravocrtno, rotacijska brzina je približno nula i parametar k3 ima minimalan utjecaj.

Snimaju se svi relevantni podaci (početni položaj mobilnog robota, obodne brzine pogonskih kotača te vrijeme uzorkovanja).

1-10

0 50 100 150 200 250 30015

20

25

30

35

40

45

50

55

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.3. Prikaz odziva pozicije eksperimenta za pravocrtno gibanje

0 100 200 300 400 500 600-50

0

50

100

150

200

250

300

350

400

time step

orie

ntat

ion

MOBILE ROBOT ORIENTATION

true orientation

estimated orientation

Sl. 1.4. Prikaz odziva orijentacije eksperimenta za pravocrtno gibanje

1-11

14 16 18 20 22 24 26 28 300.94

0.96

0.98

1

1.02

brzi

na li

jevo

g ko

taca

mjerena

stvarna

64 66 68 70 72 74 76 78 80

0.96

0.98

1

1.02

1.04

brzi

na d

esno

g ko

taca

step

mjerena

stvarna

Sl. 1.5. Prikaz brzina kotača kod eksperimenta za pravocrtno gibanje

Vidi se prema slici 1.3. da estimirana pozicija loše prati simuliranu i da robot skreće u desno.

To se događa jer su brzine postavljene na 1, a polumjer lijevog kotača je veći od polumjera desnog.

Nakon eksperimenta pomoću kojeg smo skupili početni položaj, obodne brzine pogonskih kotača te vrijeme uzorkovanja koristimo funkciju fsolve za pronalazak parametara kalibracije k1 i k2 .

k = fsolve('calibratekLeftRight', [1 1]) ;

Funkcija fsolve vratila je vrijednosti k1 = 1.0025, k2 = 0.9975 i k3 = 0.

1-12

2.Eksperiment – zakret na mjestu ( parametri k1, k2 i k3 su postavljeni na vrijednost 1 )

U eksperimentu zakreta na mjestu mobilni robot se mora zakrenuti za 180° oko svoje osi čime do izražaja dolazi parametar k3. Ostali parametri ne utječu jer je translacijska brzina približno jednaka nuli te se pozicija mobilnog robota ne mijenja.

12.922 12.924 12.926 12.928 12.93 12.932 12.934 12.936 12.938 12.94 12.94252.655

52.66

52.665

52.67

52.675

52.68

52.685

52.69

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.6. Prikaz odziva pozicije kod zakreta na mjestu

0 50 100 150 200 250 300-200

-150

-100

-50

0

50

100

150

200

time step

orie

ntat

ion

MOBILE ROBOT ORIENTATION

true orientation

estimated orientation

1-13

Sl. 1.7. Prikaz orijentacije kod zakreta na mjestu

0 20 40 60 80 100 120 140 160 180 200-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0br

zina

lije

vog

kota

ca

mjerena

stvarna

0 20 40 60 80 100 120 140 160 180 2000

0.01

0.02

0.03

0.04

0.05

0.06

brzi

na d

esno

g ko

taca

step

mjerena

stvarna

Sl. 1.8. Prikaz brzina kotača za slučaj zakreta na mjestu

Kao što je bilo i za očekivati niti u ovom slučaju estimacija ne prati dobro simulaciju.Nakon što obavimo drugi eksperiment koristimo funkciju fsolve za dobivanje parametara k3.

k3 = fsolve('calibratekAxle', 1);

Parametar k3 iznosi 1.0025

Sljedeće što smo radili je provjera ispravnosti izračunatih vrijednosti kalibracijskih parametara ponavljanjem prvog i drugog eksperimenta uz korištenje proširenog kinematičkog modela mobilnog robota.

1-14

1.Eksperiment – provjera uz kalibrirane parametare ( k1 = 1.0025, k2 = 0.9975 i k3 = 1.0025 )

Sl. 1.9. Prikaz prvog eksperimenta sa kalibriranim parametrima

0 50 100 150 200 250 30015

20

25

30

35

40

45

50

55

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.10. Prikaz pozicija uz kalibrirane parametre za pravocrtno gibanje

1-15

0 100 200 300 400 500 600-50

0

50

100

150

200

250

300

350

400

time step

orie

ntat

ion

MOBILE ROBOT ORIENTATION

true orientation

estimated orientation

Sl. 1.11. Prikaz orijentacija uz kalibrirane parametre za pravocrtno gibanje

0 100 200 300 400 500 6000

0.5

1

1.5

brzi

na li

jevo

g ko

taca

mjerena

stvarna

0 100 200 300 400 500 6000

0.5

1

1.5

brzi

na d

esno

g ko

taca

step

mjerena

stvarna

Sl. 1.12. Prikaz brzina kotača uz kalibrirane parametre

Vidimo da u slučaju ponovljenog eksperimenta sa kalibriranim parametrima estimacija puno bolje prati stvarnu (simuliranu). Ipak postoje mala odstupanja koja su posljedica postojanja šuma prilikom mjerenja.

1-16

2.Eksperiment – provjera uz kalibrirane parametare ( k1 = 1.0025, k2 = 0.9975 i k3 = 1.0025 )

5.62 5.625 5.63 5.635 5.64 5.645 5.65 5.655 5.66 5.66554.785

54.79

54.795

54.8

54.805

54.81

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.13. Prikaz pozicije ponovljenog eksperimenta kod zakreta na mjestu

0 50 100 150 200 250 300-200

-150

-100

-50

0

50

100

150

200

time step

orie

ntat

ion

MOBILE ROBOT ORIENTATION

true orientation

estimated orientation

Sl. 1.14. Prikaz orijentacije sa kalibiranim parametrima za zakret na mjestu

1-17

0 20 40 60 80 100 120 140 160 180 200-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0

brzi

na li

jevo

g ko

taca

mjerena

stvarna

0 20 40 60 80 100 120 140 160 180 2000

0.01

0.02

0.03

0.04

0.05

0.06

brzi

na d

esno

g ko

taca

step

mjerena

stvarna

Sl. 1.15. Prikaz brzina sa uključenim kalibriranim parametrima za zakret na mjestu

I u ovom primjeru vidi se znatno poboljšanje u odnosu na eksperiment sa nekalibriranim parametrima.Za sada ovakvi rezulatati me zadovoljavaju s obzirom na osobni delay.

Slijedeći zadatak jest grafički prikazati stvarni te estimirani položaj mobilnog robota prije i poslije umjeravanja odometrijskog sustava.

U nastavku je priložen kod, a zatim i grafički prikaz.

function [new] = lab1_algoritam(simrobot,matrix,step)

% set global variablesglobal k1 k2 k3

% set persistent variablespersistent x_start y_start theta_start x y theta x_true y_true theta_true sensorPosition sensorAxis map pC stupac_s redak_s

%z = [1 1 1];

k1=1.0025; k2=0.9975; k3=1.0025;

% set algorithm initial parameter values

if step == 1% set calibration parameters - experiment value is 1

% get estimated mobile robot start pose

position = getestimpos(simrobot);x_start = position(1);

1-18

y_start = position(2);theta_start = getestimhead(simrobot);

1-19

% get estimated posetheta = getestimhead(simrobot);x = position(1);y = position(2);

% get true mobile robot poseposition = getpos(simrobot);x_true = position(1);y_true = position(2);theta_true = gethead(simrobot);

end% algorithm start

% get estimated mobile robot poseposition = getestimpos(simrobot);x(step,1) = position(1);y(step,1) = position(2);theta(step,1) = getestimhead(simrobot);

% get true mobile robot poseposition = getpos(simrobot);x_true(step,1) = position(1);y_true(step,1) = position(2);theta_true(step,1) = gethead(simrobot);

% algorithm end

% save/plot needed variables at simulation endif step == 1000

% position plotfigure(2)clfplot(x_true, y_true), hold on, plot(x, y, 'g')plot(x_true(end),y_true(end),'ob'), plot(x(end),y(end),'og');

xlabel(' x position coordinate'), ylabel(' y position coordinate')

grid, legend('true position', 'estimated position')title('MOBILE ROBOT POSITION') % orientation plotfigure(3)clfplot(theta_true), hold on, plot(theta, 'g')xlabel('time step'), ylabel('orientation');grid, legend('true orientation', 'estimated orientation')title('MOBILE ROBOT ORIENTATION')

% save variablessave results.mat x_start y_start theta_start x y theta;disp('Needed variables saved');

end

% continue simulation

1-20

new = simrobot;

1-21

Sl. 1.16. Prikaz eksperimenta za usporedbu ( prije umjeravanja odom.sustava )

0 20 40 60 80 100 120 140 160 180 20040

45

50

55

60

65

70

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.17. Prikaz odziva stvarnog i estimiranog položaja prije umjeravanja

1-22

Sl. 1.18. Prikaz eksperimenta za usporedbu ( nakon umjeravanja odom.sustava )

0 20 40 60 80 100 120 140 160 18035

40

45

50

55

60

65

70

x position coordinate

y p

ositi

on c

oord

inat

e

MOBILE ROBOT POSITION

true position

estimated position

Sl. 1.19. Prikaz odziva stvarnog i estimiranog položaja nakon umjeravanja

Još jednom smo se uvjerili da su praćenja nakon umjeravanja mnogo bolja iako ne prate očekivanja.

1-23

1.5. Zaključak

Ovom vježbom ušli smo barem malo u problematiku kretanja mobilnih robota.Kroz dobivene rezultate vidjeli smo da je vrlo bitno čak i do treće decimale dobiti dobre kalibracijske parametre koji nam kompenziraju veličine koje su nam nepoznate.

Kroz rad smo dolazili do raznih saznanja i nesaznanja kao što su nelogičnosti kod dobivanja neidentičkih parametara u prvoj iteraciji sa jednom te istom funkcijom. Moje objašnjenje kolegici Poropat je da se do toga dolazi zbog nesavršenosti numeričkih metoda kojima se Matlab služi. Ispravite me ako sam u krivu.

Isto tako vidjelo se da rezultati kod funkcije „calibratekLeftkRight“ ovise o početnim uvjetima što zapravo ne bi trebali.

Za kraju zahvalio bi se i na Vašem sufliranju i otkrivanju nekih grešaka u kodu što nam je uvelike olakšalo daljnje napredovanje kroz vježbu.

1-24