SISTEME DE CONDUCERE A ROBOILOR

Lucrarea 3. Control micrii robotului folosind un algoritm simplu

Pentru implemetarea controlului poziiei, se va folosi toolbox-ul Matlab: KiKS is a

Khepera Simulator, care se gsete la locaiile indicate de personalul ndrumtor, sau online
la adresa http://www.tstorm.se/kiks.php.
Scopul lucrrii
Implementarea unui algoritm simplu de control al micrii robotului.
Consideratii teoretice
O modalitate de implementare a unui algoritm simplu de control al micrii robotului
Khepera este implementarea unei serii de condiii simple (de tip if), n funcie de valorile
senzorilor de proximitate.
Pentru aceasta, senzorii se pot grupa astfel (Fig. 1):

Fig. 1. Dispunerea senzorilor i gruparea acestora

Astfel, pseudocodul algoritmului este:

Dac obstacolul este n partea stng, ntoarce spre dreapta
Dac obstacolul este n partea dreapt, ntoarce spre stnga
Dac obstacolul este n fa, ntoarce spre stnga/dreapta (se alege aleatoriu o direcie)
Altfel, mergi n fa.

Se observ totui c un astfel de algoritm este destul de simplist, sunt luate n

considerare numai cazurile simple; totui nu este implementat contextul n care robotul se afl
n col, iar obstacolele sunt n fa i n stnga, sau n fa i n dreapta.
Mod de lucru
Codul Matlab pentru implementarea controlului micrii robotului folosind pseudocodul
anterior este urmtorul:

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

(c) 2000-2004 Theodor Storm <theodor@tstorm.se>

http://www.tstorm.se

% ------------------------------------------------function control(port,baud,time)

if nargin<3 time=200; end;

if nargin<2 baud=9600; end;
if nargin<1 port=-1; end;

ref=kiks_kopen([port,baud,1]);
kSetEncoders(ref,0,0);
reflex = 0;
speed = [5 5];
t0=clock;
loops=0;
while (kiks_ktime(port)<time)
loops=loops+1;
reflex = kProximity(ref);
cruise_spd = 5;
speed = calcSpd(cruise_spd,reflex);
kSetSpeed(ref,speed(1),speed(2));
end;
t=etime(clock,t0);
disp(sprintf('%.1f loops in %.1f seconds = %.1f loops/second
(%.0f%%)\n',loops,t,loops/t,(time/t)*100));
kSetSpeed(ref,0,0);
kiks_kclose(ref);

function out = calcSpd(cruise_spd,reflex)

cmd = cruise_spd;
% se aleg senzorii utilizati
fata=(reflex(4)+reflex(5))/2;
stanga=(reflex(2)+reflex(3))/2;
dreapta=(reflex(6)+reflex(7))/2;
spate = (reflex(1) + reflex(8))/2;

if stanga < 1000

out = speed +[cmd -cmd];
end
if dreapta < 1000
out =speed+ [-cmd cmd];
end
if fata < 1000
out =speed+[-cmd cmd];
end
if spate < 1000
out =speed+ [cmd cmd];
end
if (fata> 1000 && stanga > 1000)
out = speed + [cmd 0];
end
if (fata > 1000 && dreapta > 1000)

out = speed + [0 cmd]

end
end
end

Se observ c:
- la linia 14 se definete vectorul speed, care conine valorile aplicate celor dou
motoare. (speed = [5 5]);
-la linia 20 se seteaz viteza de croazier cruise_spd;
-linia 21: viteza final va fi calculat n funcie de aceast vitez de croazier i de
valorile vectorului reflex;

-la linia 29 este declarat funcia calcSpd, folosit pentru calculul comenzilor aplicate
celor dou motoare, i care primete drept parametri cruise_spd i reflex;
-la linia 30 este declarat comanda cmd ( n cazul de fa aceasta este egal cu viteza
de croazier);
-liniile 32-35 prezint gruparea celor 8 senzori conform Fig. 1:
-media senzorilor s1 i s2 reprezint stanga;
-media senzorilor s3 i s4 reprezint fata;
-media senzorilor s5 i s6 reprezint dreapta;
-media senzorilor s7 i s8 reprezint spate;
Observatie! n vectorul reflex poziia senzorilor este:
reflex = [8 1 2 3 4 5 6 7], astfel:
- reflex(1) = 8 (elementul de pe prima poziie a vectorului) va fi
echivalent senzorului s8;
-reflex(2) = 1 (al doilea element din vector) va fi echivalent senzorului
s1 .a.m.d.
-

Liniile 37-54 implementeaza seria de if-uri care realizeaz controlul micrii

robotului.

Observaie!: Pentru folosirea toolbox-ului, n Matlab se vor executa urmtoarele

comenzi: File>Set Path...>Add with Subfolders. Se va naviga pn cnd se va selecta calea la
care se afl toolbox-ul KiKS, apoi se vor apsa butoanele Save>Close.
1. Se creeaza un nou fiier .m: File> New> Function
-Se copiaz codul funciei de mai sus;
-Se salveaz cu numele control.m : File>Save as> se alege path-ul catre folder-ul kiks2.2.1.
din folderul SCR de pe Desktop >Save;
-Se ruleaz funcia apsnd butonul Run sau tasta F5. Observai comportamentul robotului.
-Apsati butonul halt pentru a opri execuia programului i ncrcai noua hart. n meniul
din stnga, seciunea arena, apsai butonul load i selectai fisierul arena1.kad aflat n
folderul kiks, apoi apsai butonul Open. Ateptai ca noua aren s se ncarce i reluai
execuia programului control.m.
Ce observai ? Ct de buna este aceast modalitate de control din punct de vedere al
preciziei i al eficienei comportamentului robotului ?
2. Modificai programul, introducnd alte condiii simple, sau modificai-le pe cele
existente astfel nct s optimizai controlul micrii robotului.