Subsemnata Moroșan Cornelia -Mihaela , legitimat ă cu carte de identitate seria SV [623354]

UNIVERSITATEA TEHNICĂ „GHEORGHE ASACHI” IAȘI
FACULTATEA DE AUTOMATICĂ ȘI CALCULATOARE

Planificarea mișcării unei echipe de roboți
mobili cu roți

PROIECT DE LICENȚĂ

Îndrumător: Student: [anonimizat].dr.ing. Burlacu Adrian Moroșan Cornelia -Mihaela

Iași 2017

2

DECLARA ȚIE DE ASUMARE A AUTENTICITĂȚII
LUCRĂRII DE LICENȚĂ

Subsemnata Moroșan Cornelia -Mihaela , legitimat ă cu carte de identitate seria SV
nr.844689 , CNP [anonimizat], autoarea lucrării Planificarea mișcării unei echipe de
roboți mobili cu roți elaborată în vederea susținerii examenului de finalizare a studiilor de
licență organizat de către Facultatea de Automatică și Calculatoare din cadrul Universit ății
Tehnice „Gheorghe Asachi” din Iași, sesiunea iulie 2017 a anului universitar 2016 -2017 ,
luând în considerare conținutul Art. 34 din Codul de etică universitară al Universității
Tehnice „ Gheorghe Asachi” din Iași (Manualul Procedurilor, UTI.POM.02 – Funcționarea
Comisiei de etică universitară), declar pe proprie răspundere, că această lucrare este rezultatul
propriei activități intelectuale, nu conține porțiuni plagiate, iar sursele bibliografice au fost
folosite cu respectarea legislației române (legea 8/1996) și a convențiilor internaționale
privind drepturile de autor.

Data : Semnătura :

3
Cuprins

Capitolul 1. Introducere ………………………….. ………………………….. …………………………. 6
Capitolul 2. Analiza și descompunerea spațiului de lucru ………………………….. ………. 9
2.1. Descompunere în celule ………………………….. ………………………….. ……………. 9
2.2. Diagrame Voronoi Generalizate ………………………….. ………………………….. 16
2.3. Graf de vizibilitate ………………………….. ………………………….. …………………. 17
Capitolul 3. Algoritmi de planificare a traiectoriei ………………………….. ………………. 19
3.1. Algoritm pentru vizitare regiuni ………………………….. ………………………….. . 19
3.2. Algoritm pentru sample gathering ………………………….. ………………………… 20
Capitolul 4. Toolbox de planificare offline și online a t raiectoriei roboților mobili . 23
Capitolul 5. Rezultate experimentale ………………………….. ………………………….. …….. 30
5.1. Platforma experimentală ………………………….. ………………………….. …………. 30
5.2. Planificarea traiectoriei unei echipe de doi roboți mobili ……………………… 35
Capitolul 6. Concluzii ………………………….. ………………………….. …………………………. 40
Bibliografie ………………………….. ………………………….. ………………………….. ……………… 42
Anexe ………………………….. ………………………….. ………………………….. ……………………… 44

4
Listă de figuri

Figura 1 -1 Robot chirurgical Da Vinci ………………………….. ………………………….. …………………. 6
Figura 1 -2 Robot pentru grădină ………………………….. ………………………….. ………………………….. 7
Figura 1 -3 Robot mobil Rover care evoluează pe teritoriul planetei Marte ………………………… 8
Figura 2 -1 Domeniu de evoluție dreptunghiular ………………………….. ………………………….. …… 10
Figura 2 -2 Descompunerea trapezoidală a domeniului din (Figura 2 -1). ………………………….. 12
Figura 2 -3 Descompunerea în celule triunghiulare a domeniului din (Figura 2 -1). ……………. 13
Figura 2 -4 Descompunerea politopală a domeniului din (Figura 2 -1). ………………………….. … 14
Figura 2 -5 Descompunerea dreptunghiulară a mediului din (Figura 2 -1). ………………………… 15
Figura 2 -6 Cazuri întâlnite în construcția unui GVD pentru un domeniu cu obstacole
poligonale ………………………….. ………………………….. ………………………….. ………………………….. . 17
Figura 2 -7 Graf de vizibilitate și o soluție pentru problema de planificare ……………………….. 18
Figura 3 -1 Schema pentru implementarea în timp real ………………………….. ………………………. 22
Figura 4 -1 Fereastra principală a simulatorului RMTool ………………………….. …………………… 25
Figura 4 -2 Panoul pentru planificarea mișcării ………………………….. ………………………….. …….. 25
Figura 4 -3 Panoul pentru controlul mișcării ………………………….. ………………………….. ………… 26
Figura 4 -4 Panoul pentru simulare ………………………….. ………………………….. ……………………… 26
Figura 4 -5 Alegerea numărului de obstacole ………………………….. ………………………….. ……….. 26
Figura 4 -6 Instrucțiuni pentru definirea obsta colelor ………………………….. ………………………… 27
Figura 4 -7 Reprezentare obstacole toolbox RMTool ………………………….. …………………………. 27
Figura 4 -8 Instrucțiuni pentru alegere punct de start și final pentru robot ………………………… 27

5 Figura 4 -9 Reprezentare traiectorie care unește punctul de start cu cel final …………………….. 28
Figura 4 -10 Exemplu simulare RMTool ………………………….. ………………………….. ……………… 28
Figura 4 -11 Interfata grafică pentru planificarea online a traiectoriei roboților mobile ………. 29
Figura 5 -1 Platforma experimentală ………………………….. ………………………….. …………………… 30
Figura 5 -2 Discul atașat robotului ………………………….. ………………………….. ………………………. 32
Figura 5 -3 Robotul Khepera III ………………………….. ………………………….. …………………………. 32
Figura 5 -4 Model cinematic robot cu două roți ………………………….. ………………………….. ……. 34
Figura 5 -5 Traiectoriile urmate de roboți pentru trei regiuni de vizitat ………………………….. … 36
Figura 5 -6 Traiectoriile urmate de roboți pentru patru regiuni de vizitat ………………………….. 37
Figura 5 -7 Traiectoriile urmate de roboți pentru 5 regiuni de vizitat ………………………….. …… 37
Figura 5 -8 Obstacolele mărite corespunzătoare roboților reduși la un punct și descompunerea
triunghiulară a spațiului liber pentru patru obstacole în mediu ………………………….. …………… 38
Figura 5 -9 Traiectoriile urmate de roboți pentru mediul format din patru obstacole ………….. 38
Figura 5 -10 Obstacolele mărite corespunzătoare roboților reduși la un punct și
descompunerea triunghiulară a spațiului liber pentru patru obstacole în mediu ………………… 39
Figura 5 -11 Traiectoriile urmate de roboți pentru mediul format din cinci obstacole …………. 39

6 Capitolul 1. Introducere

Activitatea umană s -a orientat întotdeauna spre realizarea unei vieți mai bune, spre
eliberarea parțială sau totală de muncile grele și de rutină. Această activitate, de -a lungul
secolelor a încercat să absolve omul de calitatea de rob, ducând la realizarea unui automat,
robotul. Apariția roboților și e voluția lor a fost posibilă grație simbiozei dintre mecanică,
electronică și informatică. Revoluția informatică a marcat saltul de la societatea
industrializată la societatea avansat informatizată generând un val de înnoiri în tehnologie și
în educație, pe rmițând realizarea de roboți.
Roboții oferă beneficii substanțiale muncitorilor, industriilor și implicit țărilor. În
situația folosirii în scopuri pașnice, roboții industriali pot influiența pozitiv calitatea vieții
oamenilor prin înlocuirea acestora în s pații periculoase, cu condiții de mediu dăunătoare
omului, precum și cu condiții necunoscute de exploatare.
Domeniile de aplicare a tehnicii roboților se lărgesc mereu, ei putând fi utilizați în
industrie, transporturi, agricultură (Figura 1-2), în cunoașterea spațiului co smic, în cercetarea
științifică și în medicină. Chirurgia robotică reprezintă cea mai nouă tehnologie a chirurgiei
minim invazive, în care chirurgul nu operează cu propriile mâini, ci manevrează un robot la
distanță, fiind așezat la o consolă amplasată în sala de operații, acest lucru aducând avantaj e
considerabile pentru pacienți ( Figura 1-1).

Figura 1-1 Robot chirurgical Da Vinci

7 Roboții mobili sunt acei roboți care au în dotare un mecanism de propulsie: roți,
șenile sau picioare. Prin urmare, eficiența locomoției bazate pe roți este dependentă de
calitatea mediului. În această lucrare sunt utilizați roboți cu sistem locomotor bazat pe roți cu
acționare diferențială spate.

Figura 1-2 Robot pentru grădină

Sistemele multi -robot cu roboți mobili sunt o temă preferată în domeniul roboticii
datorită numărului mare de aplicații existente, cum ar fi căutarea, recuperarea, explorarea
mediului sau monitorizarea acestuia [12]. Problemele ce pot fi formulate pentru sistemele
multi -robot cu roboți mobili pot fi descrise prin mai multe caracteristici: mediul în care
roboții activează, arhitectura de control a roboților, precum și tipul roboților. Mediul în care
activează roboții poate fi complet cunoscut, atunci când se știu pozițiile și orientările roboților
și localizarea regiunilor de interes sau parțial cunoscut, roboții colectând informații despre
mediu prin dotările lor senz oriale. Arhitectura de control poate fi una centralizată, unde există
o unitate centr ală (master) care ia deciziile în locul roboților (slaves), sau una distribuită,
unde fiecare robot din echipă este autonom cu privire la deciziile sale.
Planificarea trai ectoriei reprezintă una din proble mele fundamentale ale roboticii și
reprezintă un subiect important de cercetare cu aplicabilitate în explorarea planetelor sau a
mediilor inaccesibile omului ( Figura 1-3).

8
Figura 1-3 Robot mobil Rover care evoluează pe teritoriul planetei Marte

Scopul acestei lucrări este de a planifica traiectoriile unei echipe de roboți mobili cu
roți, astfel încât într -o primă fază aceștia să viziteze toate regiunile prezente în mediu, iar apoi
se dorește rezolvarea unei probleme tip colectare de resurse , în care un set de roboți identici
trebuie să adune toate resursele împrăștia te în mediul de lucru.
Lucrarea este structurat ă în 6 capitole, fiecare dintre acestea tratând aspecte relevante
pentru tema considerată. În cele ce urmează în capitolul 2 sunt prezentate principalele metode
de analiză și descompunere a spațiului de lucru și anume descompunere în celule, diagrame
Voronoi generalizate și grafuri de vizibilitate. În capitolul 3 sunt prezentați algoritmii pentru
planificarea traiectoriei: algoritm pentru vizitare regiuni și algoritm pentru sample gathering.
În capitolul 4 este prezentat toolbox -ul pentru planificarea offline și a online a traiectoriei
roboților mobili. În capitolul 5 sunt prezentate rezultatele experimentale obținute în urma
implemetării în timp real, iar în capitolul 6 sunt prezentate concluziile și direcțiile viitoare.

9 Capitolul 2. Analiza și descompunerea spațiului de lucru

2.1. Descompunere în celule

Evoluția unui robot mobil într -un anumit domeniu este un proces de natură continuă,
cu o infinitate de stări posibile (configurații ale robotului). Pentru multe sisteme cu numă r
infinit de stări au fost dezvoltate tehnici de reducere a numărului d e stări la un set finit, un
astfel de proces purtând denumirea de abstracție. Pentru un robot mobil ce evoluează într -un
mediu dat, abstracția sa către o reprezentare cu număr finit de stări se face de multe ori prin
folosirea descompunerilor în celule. Modalitățile de descompunere în celule , ce vor fi
prezentate , pot fi aplicabile pentru a construi abstracții finite și pentru alte sisteme cu evoluție
continuă pentru care o partiționare a spațiului liber este de interes.
Pentru explicarea tehnicilor de descompunere în celule se presupune că mediul în care
evoluează un robot mobil este bidimensional, mărginit de un dreptunghi și conține mai multe
obstacole. Fiecare obstacol este presupus a fi un poligon convex, o astfel de formă geometrică
fiind denumită și politop. Robotul mobil este considerat punctiform și „fully -actuated”(se
poate deplasa în orice direcție la orice moment de timp).
Se notează cu E ⊂ 𝑅2 domeniul dreptunghiular de evolu ție, iar obstacolele se notează
cu {𝑂1,𝑂2,…,𝑂𝑛}. Pentru orice formă politopală S ∈ {E, 𝑂1,𝑂2,…,𝑂𝑛} se notează cu 𝑉𝑆=
{𝑣𝑆1,𝑣𝑆2,…,𝑣𝑆|𝑉𝑆|} setul vârfurilor sale și cu 𝐹𝑆={𝑓𝑆1,𝑓𝑆2,…,𝑓𝑆|𝑉𝑆|} setul fațetelor (laturilor)
sale, und e |A| notează cardinalitatea ( numărul de elemente) setului A. Se folosește convenția
de a numerota vârfurile și fațetele oricărei regiune S într -o ordine trigonometrică (ant i-orară),
considerând că fațeta 𝑓𝑆1 este definită de vârfurile 𝑣𝑆1 și 𝑣𝑆2 ș.a.m.d. Spațiu l liber reprezintă
porțiunea din domeniul E care nu este acoperită de nici un obstacol. (Figura 2-1) prezintă un
mediu de evoluție ce r espectă presupunerile de mai sus, ilustrând totodată convențiile de
notare ale vârfurilor și fațetelor.

10 Descompunerea în celule reprezintă o partiționare a spațiului liber din domeniul de
evoluție, neocupat de obstacole, într -un set de zone cu aceeași for mă geometrică(celule).
Pentru un domeniu cunoscut E, 𝑂1,𝑂2,…,𝑂𝑛 ne propunem să obținem un set de celule astfel
încât uniunea acestora să fie egală (sau apropate egală) cu spațiul liber. Celulele rezultate
trebuie să nu se suprapună, intersecția oric ăror două celule trebuie să fie vidă sau să fie un
segment de dreaptă (o fațetă comună ambelor celule). Descompunerea în celule trebuie să
aibă ca rezultat setul celulelor, pe care îl vom nota cu 𝐶={𝑐1,𝑐2,…,𝑐|𝑐|} și o relație de
adiacență între celul e, notată cu A. Astfel, o descompunere în celule poate fi pusă în
corespondență cu un graf adirecțional definit de ( 𝐶,𝐴). Pentru abstractizarea mediului în
celule este folosit algoritmul Dijkstra cu punctul de start, respectiv , cel final corespunzând
celulelor ce conțin poziția inițială și cea finală a robotului. Rezultatul este o secvență de
celule de urmat, iar orice traiectorie ce aparține acestor celule este una validă, deoarece evită
obstacolele.

Figura 2-1 Domeniu de evoluție dreptunghiular

Există două clase de descompuneri în celule: exacte și aproximative. O descompunere
este exactă dacă uniunea celulelor rezultate este egală cu spațiul liber. O descompunere se
numește aproxi mativă dacă uniunea celulelor sale este inclusă în spațiul liber, și aproximează
acest s pațiu cu o anumită precizie . În continuare voi prezenta trei tipuri de descompuneri
exact e (trapezoidală, triunghiulară, politopală) și o descompunere aproximativă
(dreptunghiulară) .[3]

11 Descompunerea trapezoidală

Descompunerea trapezoidală are ca scop acoperirea spațiului liber cu un set de
trapeze, fiecare trapez având laturi paralele verticale (paralele cu axa 𝑂𝑦). Această
descompunere este întâlnită în problemele de robotică cu scop ilustrativ, fiind suficient de
simplu de obținut.
Ideea de bază în cadrul acestei descompuneri este de a extinde o dreaptă verticală din
fiecare vârf al obstacolelor, până când este intersectată o latură. Direcția pentru a extinde o
linie verticală este determinată de poziția relativă a vârfului curent în raport cu alte vârfuri ale
aceluiași obstacol.
Pentu a obține un algoritm de complexitate redusă, vârfurile obstacolelor sunt în
prealabil sortate, pe baza coordonatei axei 𝑂𝑥, iar liniile verticale sunt extinse în ordinea listei
obținute. Un algoritm specific me nține și actualizează un set de fațete , care pot fi intersectate
de linia curentă, reducând astfel numărul de teste necesare pentru a afla intersecțiile dreptei
curente cu f ațete ale obstacolelor sau ale mediului de evoluție. Trapezele descompunerii vor
fi delimitate de liniile dreptelor construite și de fațetele intersectate.
Rezultatul descompunerii trapezoidale constă într -un set de celule trapezoidale ale
căror vârfuri su nt ca lculate în timpul descompunerii (intersecțiile dintre dreptele verticale și
fațetele obstacolelor). Relația de adiacență se de termină ușor, având în vedere că două trapeze
vecine au în comun un segment al unei drepte verticale.
În cazul în care două s au mai multe vârfuri ale obstacolelor au aceeași coordonată pe
axa 𝑂𝑥, algoritmul pentru descompunere trapezoidală trebuie ajustat. O metodă simplă de
realizare a acestei ajustări este de a adăuga o mi că perturbație aleatoare pe componentele
egale ale v ârfurilor, astfel încât acestea nu se vor mai afla pe aceeași dreaptă verticală. De
asemenea, în anumite cazuri particulare, unele trapeze ale descompunerii pot fi degenerate la
triunghiuri. În (Figura 2-2) este prezentat un exemplu de descompunere trapezoidală, în care
adiacența între celule este ilustrată prin linii albastre punctate. [3]

12
Figura 2-2 Descompunerea trapezoidală a domeniului din (Figura 2-1).

Descompunerea triunghiulară

Descompunerea triunghiulară are ca scop partiționarea spațiului liber în triunghiuri.
Aceste triunghiuri au vârfurile în setul de vârfuri existente, prin urmare descompunerea este
echivalentă cu o alocare a câte trei vârfuri din setul amintit fiecărei celule. O consecință a
acestui aspect este faptul că oricare două tr iunghiuri adiacente rezultate au în comun o
întreagă fațetă (spre deosebire de c azul descompunerii trapezoidale, unde celulele adiacente
puteau avea în comun doar un segment al unei laturi).
În geometria computațională există o descompunere în triunghiuri denumită
triangularizare Delaunay. Această triangularizare are d rept intrări un set de puncte ( spre
deosebire de obstacole) și conectează aceste puncte cu scopul de a forma triunghiuri ce
îndeplinesc două proprietăți adiționale: maximizarea unghiului minim al unghiurilor obținute
în triunghiurile din descompunere, și cercul circumscris fiecărui triunghi obținut nu include
un al patrulea punct din setul de puncte dat. O descompunere Delaunay nu poate fi utilizată în
cazul nostru deoarece unele din triunghiur ile rez ultate ar putea tăia obstacolele, fiind cu o
parte în spațiul liber și cu alta în spațiul ocupat.
Pentru a rezolva această situație a fost dezvoltată triangularizarea cu restricții, care are
drept intrări un set de puncte și un set de segmente de dr eaptă definite de aceste puncte.
Triangularizarea rezultată menține segmentele de dreaptă predefinite, astfel încât acestea să
nu fie tăiate de triunghiurile rezultate. Prin urmare , soluția problemei noastre , de

13 descompunere în triunghiuri , este dată de o triangularizare cu re stricții, unde punctele sunt
vârfurile obstacolelor și ale domeniului, iar restricțiile sunt date de laturile obstacolelor și ale
mediului de evoluție.
Algoritmii de construcție ai unei astfel de descompuneri sunt suficient de complecș i:
într-o primă fază, spațiul liber se împarte în așa numite poligoane monotone, care apoi sunt
descompuse în triunghiuri. Apoi, relația de adiacență este ușor de determinat, având în vedere
că două triunghiuri sunt adiacente , dacă și numai dacă , acestea a u două vârfuri comune. În
(Figura 2-3) este prezentat un exemplu de descompunere în celule triunghiulare . Liniile
punctate reprezintă adiacența între c elule. [3]

Figura 2-3 Descompunerea în celule triunghiulare a domeniului din (Figura 2-1).

Descompunerea politopală

În secțiunile anterioare , am arătat că spațiul liber poate fi descompus în celule care au
același număr de vârfuri (patru în cazul trapezelor, trei în cazul triun ghiurilor). Spațiul liber
poate fi descompus și în politoape cu un număr oarecare de vârfuri, după cum va fi
exemplificat în această secțiune.
Ideea de bază în cadrul descompunerii politopale este de a extinde dreptele de suport
ale fațetelor obstacolelor, iar apoi , de a căuta toate politoapele incluse în domeniul E și
delimitate de aceste drepte. Fiecare p olitop determinat nu trebuie să fie intersectat de alte
drepte care provin din laturile obstacolelor. În (Figura 2-4) este prezentat un exemplu de
descompunere politopală. Adiacența între celule este ilustrată prin linii punctate.

14
Figura 2-4 Descompunerea politopală a domeniului din (Figura 2-1).

Descompunerea dreptunghiulară

Descompunerea dreptung hiulară este o descompunere aproximativ ă, în care o parte a
spațiului liber va fi acoperită de dreptunghiuri, rămânând mici porțiuni neconținute în celule.
Aceste porțiuni vor fi localizate în apropierea obstacolelor. Acest aspect va constitui un
avantaj la planificarea unui robot mobil prin ce lule ale descompunerii, deoarece se vor evita
mișcările în imediata apropiere a obstacolelor.
Dreptunghiurile din descompunere au același raport lățime/înălțime ca și domeniul de
evoluție E, însă vor avea arii di ferite. Algoritmii de partiționare în dreptu nghiuri sunt inspirați
de tehnici „quad -tree”, care sunt descompuneri ierarhice folosite pentru organizarea datelor
sau pentru compresia imaginilor. Un quad -tree este un arbore ale cărui noduri fie sunt frunze
(nu au descendenți), fie au câte patru descend enți. Fiecare nod din quad -tree-ul ce va fi creat
va corespunde unui dreptunghi conținut în E, și poate fi etichetat cu una din următoarele trei
variante: (i) ocupat, dacă dreptunghiul este conținut în întregime într -un obstacol sau într -o
uniune de obstac ole; (ii) liber, dacă dreptunghiul se află în spațiul liber; (iii) mixt, dacă unele
porțiuni ale dreptunghiului sunt incluse în obstacole și unele porțiuni sunt în spațiul liber.
Ideea de bază a descompunerii dreptunghiu lare este de a împărți recursiv (începând cu
dreptunghiul E) fiecare dreptunghi mixt în patru dreptunghiuri egale, înjumătățind fiecare
latură (aceste patru dreptunghiuri d evenind descendenți ai unui nod în quad -tree). Un
dreptunghi nu va fi împărțit dacă este liber, ocupat, sau este mixt da r are o arie mai mică

15 decât o precizie impusă. În ultim ul caz (dreptunghi mixt cu arie mică), dreptunghiul este
etich etat drept ocupat. Astfel la sfâ rșitul creării quad -tree-ului, frunzele vor fi etichetate liber
sau ocupat, iar frunzele libere vor corespu nde celulelor din descompunerea dreptunghiulară.
Diferența dintre spațiul liber din E și spațiul acoperit de celule dreptunghiulare este marcată
cu culoare gri în ( Figura 2-5). Această dif erență va fi considerată drept ocupată în urma
abstracției corespunzâ nd descompunerii, deoarece nu există celule care s ă aparțină suprafeței
respective.

Figura 2-5 Descompunerea dreptunghiulară a mediului din (Figura 2-1).

În urma prezentării celor patru tipuri de descompuneri în celule se pot formula căteva
comparații între acestea. În general, descompunerile trapezoidale și triunghiulare tind să aibă
un număr mai mic de celule decât celelalte descompuneri, fapt benefic deoarece
complexitatea abstracției finite este redusă. Descompunerea trapezoidală este uneori
dezavantajoasă deoarece vârfurile obstacolelor nu trebuie să aibă aceeași coordonată pe axa
𝑂𝑥, iar unele trapeze rezultate pot fi foarte înguste. Descompunerile în politoape și în
dreptunghiuri au în general un număr mar e de celule, ceea ce duce la o complexitate
crescută. Descompunerea dreptunghiulară, deși aproximativă și cu un număr crescut de
celule, are avantajul că spațiul de lângă obstacole nu este acoperit de celule, iar celulele mici
tind să apară doar în apropierea obstacolelor. Descompuneri le triunghiulare și politopale au
un avantaj adițional și anume de a permite și descompunerea obstacolelor odată cu spațiul
liber. Acest lucru poate fi folosit în cazurile în care , în spațiul de evoluție există regiuni

16 predefinite, însă acestea nu sunt obs tacole, ci de exeplu , pot fi zone de interes care trebuie
vizitate într -o anumită ordine.

Hărți

O posibilă soluție a problemei de navigare poate fi dată de hărțile de evoluție robotică
(eng. roadmaps). Într -un astfel de caz, trebuie întâi rezolvată partea de planificare și anume ,
găsirea unui traseu S – F). Astfel de hărți robotice trebuie înțelese ca o reprezentare a spațiului
de evoluție , care permi te găsirea unor trasee rapide ( sau simple) între diverse puncte de
interes.
Ideea este similară cu o hartă rutieră: pe o astfel de hartă există trasee (străzi) care
permit multiple conexiuni între puncte de interes (orașe). Dintr -un punct oarecare de start,
trebuie găsită o modalitate de a ajunge la drumurile de pe hartă, după care aceste drumuri
sunt urm ate până în apropierea destinației; apoi, se părăsesc aceste drumuri și se evoluează pe
drumuri locale (care se poate să nu fie reprezentate pe hartă) până la atingerea destinației.
Cele mai utilizate hărți robotice sunt grafurile de vizibiliate și diagramele Voronoi
generalizate. Vom prezenta ambele tipuri de hărți presupunând că obstacolele din domeniul
de evoluție sunt poligoane convexe.

2.2. Diagrame Voronoi Generalizate

Diagramele Voronoi Generalizate (GVD) reprezintă o altă modalitate de construc ție a
unei hărți robotice. Un GVD este alcătuit din setul de puncte aflate la distanță egală de cele
mai apropiate două obstacole. În cazul obstacolelor poligonale, GVD -ul este alcătuit din
segmente de dreaptă și porțiuni de parabolă. Aceasta , deoarece avâ nd obstacole poligonale,
există trei situații de construcție a setului de puncte echidistant față de cele mai apropiate
două obstacole: (i) puncte egal apropiate de două segmente de dreaptă (laturi ale
obstacolelor): bisectoarea unghiului definit de cele d ouă drepte, precum în ( Figura 2-6 a), (ii)
puncte egal apropiate de două vârfuri: mediatoarea segmentului ce unește cele două vârfuri
(Figura 2-6 b), puncte egal depărtate de un vârf și un segment de dreaptă: parabolă (cu
focarul în vârful dat) ( Figura 2-6 c).

17
Figura 2-6 Cazuri întâlnite în construcția unui GVD pentru un domeniu cu obstacole poligonale

Ramurile GVD -ului (seturile de puncte ech idistante față de cele mai apropiate două
obstacole) se intersectează în punctele pentru care distanța la cele mai apropiate trei obstacole
este egală. O dată construit GVD -ul, un robot mobil se îndreaptă spre o ramură apropiată a
GVD -ului, evoluează pe GV D până în apropierea punctului final, iar apoi părăsește GVD -ul
și se îndreaptă spre F.

2.3. Graf de vizibilitate

Grafurile de vizibilitate sunt create unind prin segmente de dreaptă vârfurile
obstacolelor care pot fi „văzute” unul din celălalt (adică segment ul rezultat nu intersectează
un alt obstacol) . Laturile obstacolelor fac parte din graful de vizibilitate.
Numărul de linii rezultat poate fi destul de mare, mai ales în cazul obstacolelor
complexe. De aceea, s -a încercat reducerea acestui număr și exist ă un rezultat care garantează
corectitudinea grafului de vizibilitate , în cazul în care se mențin doar două tipuri de segmente
de dreaptă: (i) segmente de separare, care au proprietatea că dreapta din care fac parte separă
obstacole unite (un obstacol este de o parte a dreptei, celălalt obstacol este de cealaltă parte),
(ii) segmente de suport, pentru care obstacolele ce definesc segmentul se află de aceeași parte
a dreptei din care fac parte segmentul respectiv.
După obținerea grafului curent, trebuie considerate punctele S și F și construite
conexiunile între aceste puncte și segmentele existente. Pentru aceasta se unesc punctele S
(respectiv F) cu vârfuri ale obstacolelor aflate în linie de vedere din S (respectiv F). Graful de

18 vizibilitate astfel obț inut poate fi considerat ca un graf format din noduri și arce, nodurile
fiind vârfurile obstacolelor, împreună cu punctele S și F, iar arcele fiind segmentele ce unesc
aceste vârfuri. Astfel , se poate utiliza un algoritm de căutare pe graf și se obține un set de arce
unind nodul sursă S cu nodul final F. În cazul în care se consideră un cost al fiecărui arc egal
cu lungimea segmentului de dreaptă respectiv, traiectoria obținută va avea lungimea minimă.
Traseul obținut este o sol uție a problemei de planifica re ( returnează o traiectorie ce
unește nodul sursă cu cel destinație, fără a lovi obstacolele). O astfel de traiectorie este
prezentată în ( Figura 2-7) prin linia frântă de culoare roșie.

Figura 2-7 Graf de vizibilitate și o so luție pentru problema de planificare

19 Capitolul 3. Algoritmi de planificare a traiectoriei

3.1. Algoritm pentru vizitare regiuni

În această lucrare este studiată o clasă de alocări robot -regiune pentru o problemă de
vizitare, considerând o echipă de roboți mobili formată din doi roboți mobili Khepera III. Se
folosește o arhitectură de control centralizată în care o unitate centrală ia deciziil e și comandă
roboții. Mediul este complet cunoscut, o imagine a acestuia achiziționându -se cu ajutorul unei
camere tip „eye -in-the-sky”.
Problema de vizitare pentru o echipă de roboți mobili poate fi formulată în felul
următor: fiind dată o echipă de roboți mobili și un mediu care conține regiuni disjuncte
scopul este de a determina traiectoriile pe care roboții să le urmeze astfel încât timpul de
vizitare al tuturor regiunilor să fie minim. Pentru a rezolva această problemă, am luat în
considerare ca și criteriu de cost distanța euclidiană minimă dintre centrul roboților și centrul
de greutate al regiunilor. Soluția propusă este de a mișca echipa de roboți mobili către
regiunile specifice pe traiectorii construite din două puncte: primul este centrul r obotului, iar
al doilea este centrul de greutate al regiunii celei mai apropiate de robotul respectiv.
Alocările robot -regiune sunt făcute considerând distanța euclidiană minimă dintre
centrul robotului și centrele de greutate ale regiunilor de interes. Di stanța euclidiană a fost
calculată folosind formula ( 3-1).
22
1 2 1 2( ) ( ) dist x x y y   
(3-1)
Traiectoriile sunt formate din mișcări de rotație (pentru a orienta robotul înspre
centrul de greutate al regiunii de vizitat) și mișcări rectilinii. Dacă se folosea un singur robot,
toate traiectoriile acestuia ar fi fost formate din două puncte (centrul robotului și centrul de
greutate al regiunii celei mai apropiate), fără a apărea probleme legate de coliziune. Deo arece
folosim mai mult de un robot mobil (folosim o echipă formată din doi roboți mobili), pot
apărea câteva cazuri particulare care pot genera coliziuni între roboți: traiectoriile sunt foarte

20 apropiate, traiectoriile se intersectează, numărul roboților e ste mai mare decât al regiunilor de
vizitat sau distanța dintre centrele roboților și centrul de greutate al aceleași regiuni este
egală.
Algoritmul de alocare al regiunilor este descris prin următorul pseudocod:

Regiuni = {R1,R2,…}∈ℝ2
While (Regiuni ≠ ∅)
Roboți = {𝐀𝟏,𝐀𝟐}∈ℝ2
While (Roboți ≠ ∅ and Regiuni ≠ ∅)
Alege Ai ∈ Roboți , Rj ∈ Regiuni
astfel încât distanța dintre Ai și Rj să fie minimă
Generează traiectoria dintre Ai și Rj
Roboți = Roboți – {Ai}
Regiuni = Regiuni – {Rj}
EndWhile
Mișcă roboții către r egiunile alocate
EndWhile

3.2. Algoritm pentru sample gathering

Pentru a defini problema , considerăm un sistem multi -robot alcătuit din roboți
identici , capabili să efectueze mișcări elementare. Mediul este cunoscut prin procesare de
imagine, iar o unitate centrală coordonează mișcarea roboților. În mediu se găsesc piese, iar
scopul este de a le transporta la depozit , într-un timp minim. Pe parcursul mișc ării, robotul
trebuie să evite obstacolele, precum și alți roboți din mediu. Se presupune că un robot poate
transporta la un moment dat o singură piesă. Informațiile referitoare la mediu sunt extrase

21 prin procesare de imagine (cameră deasupra platformei). Mediul este văzut ca un graf, iar
nodurile reprezintă regiunile care pot fi vizitate de roboți. [1]
O problemă importantă este cea de a găsi o alocare optimă robot -piesă astfel încât
transportarea pieselor la depozit să se realizeaze într -un timp minim. În această lucrare este
prezentată o soluție pentru această problemă bazându -ne pe existența unei unități centrale
care comunică în permanență cu roboții. Soluția analitică presupune alocarea optimă a
roboților liberi , la piesele disponibile , în nodurile graf ului folosind o matrice binară.
Problema implementării multi -robot este rezolvată printr -o problemă de programare liniară.
În continuare voi prezenta o metodă pentru alocarea optimă a roboților liberi către
piesele libere din nodurile grafului. Un robot e ste liber dacă nu este alocat să transporte o
piesă, sau nu transportă o piesă la depozit. În același timp o piesă este considerată liberă dacă
nu este niciun robot alocat să o transporte.
Algoritmul pentru implementare sample gathering este descris prin u rmătorul
pseudocod:

Detecție de obstacole
Obținerea grafului pe baza descompunerii în celule
While (există piese în afara depozitului)
Identifică poziția și orientarea roboților
Se alocă roboții liberi să transporte piesele libere
Se găsesc traiectorii fără coliziune care trebuie urmate
While (niciun robot nu ajunge la depozit)
Identifică poziția și orientarea roboților
Se comandă roboții să urmeze traiectoriile generate
Endwhile
Endwhile

Algoritmul care rulează pe unitatea centrală e ste format dintr -o multitudine de rutine
de procesare de imagine și rutine de control a roboților Khepera III, care permit detectarea

22 automată a regiunilor de interes, a pozițiilor și orientărilor roboților, precum și comanda
mișcării unui robot Khepera II I, care să urmărească o traiectorie construită dintr -o
multitudine de puncte. Acest algoritm poate fi descris prin ( Figura 3-1).

Figura 3-1 Schema pentru implementarea în timp real

Arhitectura din ( Figura 3-1) ilustrează pașii principali ai algoritmului descris mai sus ,
evidențiind anumite rutine care trebuie să fie apelate la un anumit moment de timp. De
exemplu detecția obstacolelor și abstractizarea mediului sunt folosite doar la începutul
experimentului. Rutinele evidențiate prin culoare a roșie (detecția pieselor, alocarea resurselor
și găsirea traiectoriilor fără coliziune) trebuie să fie apelate de fiecare da tă când un robot
ajunge la depoz it. Rutinele evidențiate prin culoarea albastru corespund metodelor de
urmărire a traiectoriei coo rdonate de unitatea centrală. [1]

23 Capitolul 4. Toolbox de planificare offline și online a
traiectoriei roboților mobili

RMTool este un toolbox Matlab pentru roboți mobili proiectat și implementat de
Ramon Gonzales, Marius Kloetzer și Cristian Mahulea. Acest toolbox este open -source și ne
oferă posibilitatea de a testa diferite aspecte legate de modelarea, planificarea traiectoriei și
controlul mișcării. Rezultatele acestor teste pot fi vizualizate foarte ușor în interfața grafică ,
deoarece aceasta este formată dintr -o singură fereastră ce conține butoane, imagini și figuri
intuitive.
Toolbox -ul RMTool are o interfață grafică ce permite utilizatorului să introducă ușor
parametrii de intrare necesari pentru realizarea simulării ( Figura 4-1). Interfața are multe
ferestre grafice , unde sunt afișate rezultatele simulării. Mediul în care se mișcă roboții poate
fi definit de utilizator sau poate fi importat dintr -un fișier de tipul *.mat. În cadrul interfeței
mai avem o serie de obiecte d e tip casetă text și meniuri pop -up, unde utilizatorul poate
specifica diferiți parametrii pentru planificarea traiectoriei și controlul mișcării. Această
interfață grafică include 5 panouri de control: meniu, zona de desenare (traiectorie/spațiu de
lucru) , panou pentru planificarea mișcării, panou pentru controlul mișcării și panou pentru
simulare.
Meniul afișează 5 opțiuni: fișier, setări, simulare, ajutor și simulare în mod online.
Butonul fișier permite urmatoarele două comenzi: deschide, pentru a putea încarca date pe
care le avem deja și salvează pentru a stoca datele curente într -un fișier de tipul *.mat.
Butonul setări permite utilizatorului să introducă diferiți parametri de configuare, cum ar fi:
limitele mediului, poziția inițială și cea finală a robotului, precum și parametrii pentru
regulatorul PI.
Zona de desenare (traiectorie/spațiu de lucru) este compusă din patru axe, care sunt
folosite pentru a reprezenta evoluția diferitelor variabile de control și mediul. Axa principală
este folosită pent ru a defini obstacolele, pentru a reprezenta traiectoria obținută în urma

24 algoritmilor de planificare a mișcării și pentru a afișa rezultatele simulării pentru controlul
mișcării. Celelalte trei axe sunt folosite pentru a afișa orientarea, viteza liniară ș i unghiul de
bracaj al robotului.
Panoul pentru planificarea mișcării este format din două butoane de tip meniu pop -up
și patru butoane radio. Primul meniu pop -up permite selectarea metodei folosite pentru
planificarea mișcării: descompunere în celule, g rafuri de vizibilitate și diagrame Voronoi
generalizate. Pentru descompunerea în celule se poate alege tipul de descompunere folosind
butoanele radio. Al doilea meniu, de tip pop -up, permite selectarea punctelor intermediare
pentru metoda de calcul: puncte de mijloc (fără optimizare), norma 1,2 sau infinit (prin
rezolvarea unei probleme de optimizare).
Panoul pentru controlul mișcării este format din șapte casete de text, un meniu pop -up
și două butoane radio. Casetele de text permit utilizatorului să sele cteze diferiți parametri
pentru robot și pentru controlul mișcării: viteza maximă liniară și unghiulară, unghiul maxim
de bracaj, distanța dintre roți, raza roții, perioada de eșantionare, distanța la care să privească
înainte. Meniul pop -up permite aleger ea regulatorului: Pure Pursuit sau regulator de tip PI.
Tipul robotului poate fi selectat cu ajutorul butoanelor radio: robot de tip mașină sau robot de
tip differencial -drive.
Panoul pentru simulare este alcătuit din trei butoane care sunt folosite pentru pornirea
simulării: mediu, planificarea traiectoriei și controlul mișcării. Prin apăsarea butonului mediu
se pornește o rutină prin care utilizatorul poate defini obstacolele. Butonul pentru planificarea
traiectoriei pornește automat metoda de planificare a mișcării și afișeaza traiectoria care ar
trebui urmărită de robot. Butonul pentru controlul mișcării simulează traiectoria urmărită,
utilizând modelul robotului și parametrii de control, din panoul pentru controlul mișcării. [2]
În continuare voi prezent a modul în care se realizează o simulare în toolbox -ul
RMTool. Acesta se lansează în execuție scriind în linia de comandă a programului Matlab
rmt. La pornire se deschide o fereastră grafică (Figura 4-1) în care vom introduce ulterior
parametrii necesari simulării. Pentru început este necesar să selectăm din panoul pentru
planificarea mișcării tipul de descompunere a spațiului liber și punct ele intermediare pentru
metoda de calcul ( Figura 4-2).

25
Figura 4-1 Fereastra principală a simulator ului RMTool

Figura 4-2 Panoul pentru planificarea mișcării

Apoi în panoul pentru controlul mișcării se introduc diferiți parametrii pentru robot și
pentru controlul mișcării: viteza maximă liniară și unghiulară, unghiul maxim de bracaj,
distanța dintre roți, raza roții, perioada de eșantionare, distanța la care să privească înainte
(Figura 4-3). Tot din acest panou se va selecta tipul de robot folosit pentru simulare: car -like
sau robot de tip differencial drive, precum și tipul regulatorului Pure Pursuit sau PI.

26
Figura 4-3 Panoul pentru controlul mișcării

Definirea mediului, planificarea traiectoriei și controlul mișcării se realizează din
panoul pentru simulare (Figura 4-4) format din trei butoane. Prin apăsarea butonului
environement se deschide o casetă text în care trebuie să introd ucem numărul de obstacole
dorit (Figura 4-5). După ce am ales numărul de obstacole se deschide o fereastră care ne oferă
câteva instrucțiuni pentru definirea obstacolelor cu ajutorul mouse -ului (Figura 4-6) și anume
click -stânga pentru definirea colțurilor obstacolelor și click -dreapta pentru închiderea
conturului obstacolului. După ce am definit obstacolele acestea sunt reprezentate în zona de
desenare a toolbox -ului ( Figura 4-7) și se deschide o nouă fereastră ce ne oferă instrucțiuni
pentru introducerea punctului de start și punctul ui final în care dorim să ajungă robotul
(Figura 4-8).

Figura 4-4 Panoul pentru simulare

Figura 4-5 Alegerea numărului de obstacole

27
Figura 4-6 Instrucțiuni pentru definirea obstacolelor

Figura 4-7 Repreze ntare obstacole toolbox RMTool

Figura 4-8 Instrucțiuni pentru alegere punct de start și final pentru robot

Prin apăsarea butonul path planning se realizează descompunerea spațiului liber în
funcție de setările făcute în panoul pentru planificarea mișcării și se afișează o traiectorie ce
unește punctul de start cu cel final, traiectorie care ar trebui urmărită de robot ( Figura 4-9).
Apoi apăsând butonul motion control se afișează traiectoria urmată de robot precum și încă
trei grafice ce reprezintă orientarea, viteza liniară și unghiul de bracaj ( Figura 4-10).

28
Figura 4-9 Reprezentare traiectorie care unește punctul de start cu cel final

Figura 4-10 Exemplu simulare RMTool

Pentru planificarea online a traiectoriei roboților mobili am creat o interfață grafică
(GUI) folosind Matlab ( Figura 4-11). Interfața este formată din trei panouri de control și două
ferestre grafice și ne permite să vizualizăm ușor rezultatele experimentale obținute.

29
Figura 4-11 Interfata grafică pentru planificarea online a traiectoriei roboților mobile

Primul panou de control (Connect) este format din două butoane: webcam
initialization și connect khepera. Primul buton este folosit pentru inițializarea camerei situate
deasupra platformei, iar al doilea este folosit pentru conectarea prin Bluetooth la cei doi
roboți.
Al doilea panou de control este folosit pentru controlul aplicației de sample gathering
și este format din patru butoane: environment, triangular decomposition, pat h planning și
motion control. Prin apăsarea butonului environment se pornește o rutină prin care sunt
definite obstacolele și sunt identificați roboții din mediu. Butonul triangular decomposition
realizează descompunerea spațiului liber în triunghiuri și m ărește obstacolele, acestea
corespunzând reducerii robotului la un punct. Butonul path planning ne permite să alegem
punctul final în care dorim să ajungă cei doi roboți și afișează traiectoria care ar trebui
urmărită de aceștia. Butonul motion control po rnește rutinele de urmărire a traiectoriei și
actualizează în timp real traiectoria urmată de cei doi roboți.
Panoul de control (Visit Regions) este folosit pentru controlul aplicației de vizitare de
regiuni și este format din două butoane: environment și motion control. Prin apăsarea
butonului environment se pornește o rutină prin care sunt definite regiunile ce trebuiesc
vizitate de cei doi roboți. Butonul motion control pornește rutinele pentru vizitarea de regiuni
și actualizează traiectoria urmată de r oboți.

30 Capitolul 5. Rezultate experimentale

5.1. Platforma experimentală

Pentru a implementa experimentele în timp real am folosit platforma experimentală
din [5], ilustrată în ( Figura 5-1). Dimensiunile platformei sunt de aproximativ 4 m x 2.5 m.
Din punct de vedere structural, suprafața pe care rulează roboții mobili este construită din
plăci aglomerate de lemn (PAL), acoperite cu folie autocolantă de culoare negru m at.
Platforma este suficient de rezistentă pentru a suporta greutatea unui operator uman, care
trebuie, de exemplu să plaseze regiuni de interes pe aceasta. Balustradele care mărginesc
platforma au rolul de a opri un robot mobil care, datorită unei erori, pierde conexiunea cu
calculatorul și continuă să ruleze nesupravegheat. Din punct de vedere al prelucrării de
imagini culoarea negru mat este necesară pentru a permite o robustețe sporită cu privire la
variațiile luminozității din încăpere.

Figura 5-1 Platforma experimentală

31 Regiunile de interes (sau obstacolele, în cazul în care aceste regiuni trebuie evitate)
din domeniul de evoluție corespund unor hârtii colorate ce pot avea diverse forme. Din punct
de ve dere al procesării de imagini, fiecare regiune de interes trebuie să fie un poligon convex
de culoare roșie, verde sau albastră. Dimensiunea sau numărul de laturi ale unei regiuni nu
sunt limitate, aspect vizibil din diversitatea regiunilor din ( Figura 5-1). Setul de culori
posibile ale regiunilor poate fi extins, însă setul menționat oferă o robustețe sporită în
recunoașterea corectă a culorii, indiferent de poziția relativă a regiunii în domeniul de lucru
sau de luminozitatea mediului înconjurător.
Pentru a putea detecta la orice moment de timp regiunile de interes din mediu,
pozițiile și orientările celor doi roboți, achiziția de imagine se rea lizează cu ajutorul unei
camere de tip „eye -in-the-sky” plasată deasupra platformei experimentale. Camera video care
supraveghează platforma este o cameră de tip web „Microsoft LifeCam Studio” [7], cu
rezoluție HD maximă de 2560 x 2048 pixeli și lentile wi de-angle. Pentru experimentele în
timp real am folosit o rezoluție de 1280 x 720 pixeli, rezoluție ce ne oferă suficientă
informație necesară pentru această problemă, precum și un timp rezonabil necesar pentru ca
rutinele de procesare de imagine să poată r ula. Plasarea camerei la o înălțime de aproximativ
3 m a dus la obținerea unui domeniu de lucru robotic de aproximativ 3.75 m x 2.1 m (apropiat
de suprafața totală a platformei, excluzând marginile).
Echipa de roboți mobili folosită pentru implemetarea în timp real a aplicației este
formată din doi roboți mobili Khepera III [6] cărora le -am aplicat discuri albe pe partea
superioară. Aceste discuri albe ( Figura 5-2) au o formă specifică: un șablon alb/negru format
din trei dreptunghiuri ce reprezintă un cod binar, poziționat în dreptul centrului robotului și
folosit pentru a identifica tipul robotului și parametrii necesari pentru conectarea la unitatea
centrală, un punct negru folosit pentru a afla orientarea robotului și un contur negru folosit
pentru identificarea separată a roboților , când aceștia sunt foarte apropiați unul față de
celălalt.

32
Figura 5-2 Discul atașat robotului

Khepera III este un robot mobil diferențial cu două roți, de dimensiuni mici (5.5 cm).
Sunt extrem de utilizați în cercetare , deoarece performanțele lor , la o scară mică , sunt
comparabile cu cele pe care le pot avea roboții de dimensiuni mai mari , dar cu aceleași
caracteristici. Acesta se bazează pe un sistem de operare Linux și poate fi conectat la distanță
prin Bluetooth, cablu și Wi -fi. Robotul dispune de 9 senzori infr aroșu pentru detecție de
obstacole și 5 senzori Ultrasonic pentru detecția obstacolelor la distanțe mari. În plus, mai are
5 senzori infraroșu dispuși astfel încât să poată urmări traiectorii liniare. Pentru experimentele
făcute , nu am folosit senzorii sau extensiile roboților, deoarece camera video oferă suficientă
informație unității centrale. Comanda vitezelor este asigurată de două motoare de curent
continuu.

Figura 5-3 Robotul Khepera III

33 Roboții au o structură diferențială a roților, fapt care le permite să facă mișcări de
rotație în loc, mișcări rectilinii sau traiectorii curbate. Intrările de control pentru un astfel de
robot sunt vitezele celor două roți m otrice, pe care le vom nota cu 𝑣𝑠(viteza r oții stângi) ș i
𝑣𝑑(viteza roții drepte). Acestea sunt considerate a fi vitezele punctelor de contact ale roților în
raport cu solul (nu vitezele de rotație ale roților, relația cu acestea fiind o proporționalitate
dependentă).
Considerăm robotul differential -wheel ca un corp rigid cu roți deplasâ ndu-se pe un
teren plat. Sunt necesari 3 parametri pentru a poziționa corpul robotului în planul respectiv:
doi pentru a determina poziția acestuia în raport cu originea sistemului de axe atașat planului
de mișcare, iar al treilea pentru determinarea orientării acestui corp în raport cu axa
absciselor. Prin corpul robotului , întelegem doar corpul rigid al acestuia, ignorând cuplele
cinematice dintre acest corp și roți.
Un vehicul aflat pe un teren plat are 3 grade de libertate: 2(x, y) ce descriu poziția și
unul referitor la orientarea θ(rotația în jurul axei verticale). În general roboți i mobili nu au
control complet independent al celor 3 paramet ri ce definesc situația lui și trebuie să
efectueze manevre complexe pentru a atinge o anumită situare .
Există două moduri de abordare a modelului pentru robotul differencial drive, în
funcție de ceea ce primește la intrare. Într-o privire idealistă a problemei considerând robotul
punctiform și mișcarea determinat ă în mod natural de o viteză de translație și o viteză de
rotație și nu de vitezele pe roți, se poate vorbi de un model cinematic uniciclu [8]. La fel ca la
roboții cu o singură roată și în această versiune descrierea lor se face pe baza modelului
cinemati c următor, unde 𝑣 reprezintă viteza liniară de înaintare, iar ω viteza unghiulară în
jurul axei verticale:
cos
sinxv
yv




(5-1)
În acest caz, modelul oferă doar o aproximare neconformă cu realitatea a felului cum
robotul se mișcă, utilizând doar legea de bază 𝑣=𝑑𝑡 (unde 𝑣 este viteza liniară, 𝑑 distanța
parcursă și 𝑡 intervalul de timp) și de aceea este necesară o transformare a datelor de intrare
din 𝑣 și ω în 𝑣𝑟𝑖𝑔ℎ𝑡 și 𝑣𝑙𝑒𝑓𝑡. Robotul este abstractizat ca un disc cu două roți, ce au razele 𝑟 și
distanța între ele 𝑙. În relația (5-2) avem relația dintre cele două tipuri de intrări, iar în relația
(5-3) este exprimat modelul de tip diferențial.

34
2
2
2
2right
leftvlvr
vlvr
  (5-2)
/ 2( )cos
/ 2( )sin
/ ( )right left
right left
right leftx r v v
y r v v
r l v v




(5-3)
Pentru a specifica situarea instantanee a robotului în mediul în care se deplasează,
stabilim o relație de legătură între sistemul de refe rință și sistemul de axe curent atașat
corpului robotului: planului I i se ataș ează sistemul de referință fix XI, O I, Y I, corpului
robotulu i i se atașează sistemul de axe XR, O R, Y R. în punctul P , iar corespunzăto r poziției
inițiale a robotului se consi deră notația XRI, ORI, YRI.

Figura 5-4 Model cinematic robot cu două roți

Poziția punctului P în planul I este reprezentată prin coordonatele x și y, iar unghiul
dintre planul I și corpul robotul este reprezentat prin
 . Putem descrie poziți a robotului
printr -un vector format din 3 elemente:
Ix
y



(5-4)

35 Pentru a descrie mișcarea robotului e necesar să mapăm mișcarea de -a lungul axelor
planului I în mișcarea de -a lungul axelor corpului robotului . Maparea este realizată folosind o
matrice ortogonală de rotație:
cos sin 0
( ) sin cos 0
0 0 1R
  


(5-5)
Viteza care este trimisă către fiecare roată , este o valoare care reprezintă numărul de
pulsații ale encoder -ului. Motoarele roților sunt controlate de un regulator PID , care asigură
că viteza este menținută constant. Pentru a converti viteza motorului în timp real, folosim
următoare a ecuație din manualul utilizatorului [6]:
MotorSpeed mmRealSpeedKspeed s
(5-6)
unde Kspeed pentru configurația predefinintă este 218,72 pentru unul din roboți, sau 144,01
pentru celălalt robot. MotorSpeed este calculată în [6] ca valoarea maximă a unui timer
înmulțită cu 256 (0xFFFF8256=16776960), împărțită apoi cu valoarea curentă a timer -ului:
16776960MotorSpeedTimervalue
(5-7)
Pentru a mișca robotul către o poziție dată, sistemul achiziționează imagini iterativ,
sunt găsite pozițiile inițiale și orientările roboților, comanda dată fiind vitezele roților.
Iterațiile sunt în trerupte atunci când punctul de referință este suficient de apropiat de punctul
țintă, cu o eroare setată de 2 pixeli. Mișcările roboților vor fi rectilinii și rotații în loc pentru
a orienta robotul către punctul său țintă.

5.2. Planificarea traiectoriei une i echipe de doi roboți mobili

În această secțiune , sunt prezentate rezultatele experimentale pentru vizitarea de
regiuni în urma implementării în timp real. Experimentele au fost realizate într -un mediu care
conține trei, patru sau cinci regiuni de interes cu o echipă de roboți mobili formată din doi
roboți Kheper a III. C riteriul de cost ales a fost distanța euclidiană mini mă dintre centrul
roboților și centrul de greutate al regiunilor de vizitat.

36 S-au considerat traiectoriile formate din două puncte, fără a lua în considerare cazurile
particulare de alocare, în care puteau apărea coliziuni între roboți. În ( Figura 5-5),
considerând mediul cu trei regiuni de vizitat, sunt prezentate traiectoriile date ca referință cu
culoarea albastru închis și roșu , iar tra iectoriile reale urmate de roboți sunt reprezentate prin
culoarea albastru deschis pentru unul din roboți, respectiv galben pentru celălalt robot. În
(Figura 5-6) sunt prezente în mediu patru regiuni de vizitat, semnificațiile traectoriilor fiind
aceleași ca și în figura anterioară.
În (Figura 5-7) este prezentat un experiment cu cinci regiuni de vizitat. Atunci când
rămâne o singură regiune de vizitat, se elimină din setul roboților robotul care nu mai are nici
un task de î ndeplinit, iar robotul care este cel mai apropiat de regiunea rămasă , va fi al ocat să
viziteze acea regiune.

Figura 5-5 Traiectoriile urmate de roboți pentru trei regiuni de vizitat

37
Figura 5-6 Traiectoriile urmate de roboți pentru patru regiuni de vizitat

Figura 5-7 Traiectoriile urmate de roboți pentru 5 regiuni de vizitat

În continuare , sunt prezentate rezultatele în urma implementării în t imp real a
aplicației de evitare a obstacolelor. Experimentele au fost realizate într -un mediu care conține
patru și cinci obstacole cu o echipă de roboți mobili formată din doi roboți Khepera III. În

38 (Figura 5-8) este prezentată descompunerea triunghiulară a spațiului liber și obstacolele
mărite, corespunzând roboților reduși la un punct pentru mediu format din patru obstacole .

Figura 5-8 Obstacolele mărite corespunzătoare roboților reduși la un punct și descompunerea
triunghiulară a spațiului liber pentru patru obstacole în mediu

În (Figura 5-9) considerând mediul cu 4 obstacole , sunt prezentate traiectoriile date ca
referință cu culoarea verde și roșu, iar traiectoriile reale urmate de roboți sunt reprezentate
prin culoarea albastru de schis pentru unul din roboți, respectiv galben pentru celălalt robot.

Figura 5-9 Traiectoriile urmate de roboți pentru mediul format din patru obstacole

39 În (Figura 5-10) este prezentată descompunerea triunghiulară a spațiului liber .
Obstacolele sunt mărite, corespunzând roboților reduși la un punct pentru mediu format din
cinci obstacole. În (Figura 5-11) sunt prezente în mediu cinci obstacole , semnificațiile
traiectoriilor fiind aceleași ca și în (Figura 5-9).

Figura 5-10 Obstacolele mărite corespunzătoare roboților reduși la un punct și descompunerea
triunghiulară a spațiului liber pentru patru o bstacole în mediu

Figura 5-11 Traiectoriile urmate de roboți pentru mediul format din cinci obstacole

40 Capitolul 6. Concluzii

În cadrul lucrării s -au prezentat două metode de planificare a traiectoriei pentru roboți
mobili cu roți astfel încât într -o primă fază aceștia să viziteze toate regiunile prezente în
mediu, iar apoi s -a dorit rezolvarea unei probleme tip colectare de resurse, în care un set de
roboți identici trebuie să adune toate r esursele împrăștiate în mediul de lucru .
Problema de vizitare pentru o echipă de roboți mobili poate fi formulată în felul
următor: fiind dată o echipă de roboți mobili și un mediu care conține regiuni disjuncte
scopul este de a determina traiectoriile pe care roboții să le urmeze astfel încât timpul de
vizitare al tuturor regiunilor să fie minim. Această problemă a fost rezolvată luând în
considerare ca și criteriu de cost distanța euclidiană minimă dintre centrul roboților și centrul
de greutate al regiu nilor. Soluția propusă a fost de a mișca echipa de roboți mobili către
regiunile specifice pe traiectorii construite din două puncte: primul este centrul robotului, iar
al doilea este centrul de greutate al regiunii celei mai apropiate de robotul respectiv .
A doua problemă prezentată în această lucrare a fost cea de sample gathering. Pentru
a defini problema, considerăm un sistem multi -robot alcătuit din roboți identici, capabili să
efectueze mișcări elementare. Mediul este cunoscut prin procesare de imagi ne, iar o unitate
centrală coordonează mișcarea roboților. În mediu se găsesc piese, iar scopul este de a le
transporta la depozit, într -un timp minim. Pe parcursul mișcării robotul trebuie să evite
obstacolele precum și alți roboți din mediu. Soluția prop usă a fost de a aloca roboții liberi la
piesele disponibile în nodurile grafului folosind o matrice binară. Problema implementării
multi -robot a fost rezolvată printr -o problemă de programare liniară.
Pentru planificarea offline a traiectoriei roboților m obili am folosit toolbox -ul
RMTool la care am adăugat o legătură către o interfață grafica GUI realizată în Matlab cu
ajutorul căreia am realizat planificarea traiectoriilor roboților mobili în mod online. În
toolbox -ul RMTool ar trebui aduse modificări as tfel încât să se poată realiza planificarea
mișcării pentru o echipă de roboți mobili formată din doi sau mai mulți roboți, acesta
generând momentan planificarea traiectoriei pentru un singur robot.

41 Soluțiile propuse în această lucrare rezolvă cazurile g enerale pentru planificarea
traiectoriilor unei echipe de roboți mobili pentru o problemă de explorare. Pentru problema
de vizitare de regiuni dacă traiectoriile nu se intersectează sau nu sunt foarte apropiate vom
obține o soluție optimă.
Ca și direcții viitoare pentru planificarea traiectoriei putem rezolva câteva cazuri
particulare care pot genera coliziuni între roboți: traiectoriile sunt foarte apropiate,
traiectoriile se intersectează. numărul roboților este mai mare decât cel al regiunilor de vizit at
sau distanța dintre centrele roboților și centrul de greutate al aceleași regiuni este egală.
Deoarece o imagine a mediului în care evoluează roboții este achiziționată de o cameră web
plasată deasupra platformei apar sensibilități de luminozitate, deci mediul ar trebui să fie egal
iluminat pentru a nu detecta regiuni care de fapt nu se regăsesc în mediu. Un alt aspect de
implementat în viitor ar fi acela de a face roboții autonomi, ast fel încât aceștia să nu depindă
de unitatea centrală, ci să ia singur i decizii în privința regiunilor de vizitat.

42 Bibliografie

[1] Vladimir Tiganas, Marius Kloetzer, and Adrian Burlacu -article -Multi -Robot Based
Implementation fo r a Sample Gathering Problem, 2013
[2] Ramon Gonzales, Cristian Mahulea, Marius Kloetzer -article -A Matlab -based Interactive
Simulator for Teaching Mobile Robotics
[3] Marius Kloetzer -carte -Strategii de planificare automată a roboților mobile, 2012
[4] Roland Siegwar t and Illah R. Nourbakhsh -book -Introduction to Autonomous Mobile
Robots , 2004
[5] M. Kloetzer, S. Magdici and A. Burlacu, “Experimental Platform and Matlab Toolbox for
Planning Mobile Robots,” 16th International Conference on System Theory, Control and
Computing (ICSTCC), 2012, Sinaia , Romania.
[6] K-Team, “Khepera III robot”, available at http://www.k -team.com/mobilerobotics –
products/ khepera -iii.
[7] Microsoft, “LifeCam Studio”, available at http://www.microsoft.com/hardware/en -us/
p/lifecam -studio
[8] Magnus Egerstedt,Control of mobile robots,https://www.coursera.org/course/ conrob
Coursera, 2014
[9] Pertanika J. –article – A review on Robot Motion Planning Approaches,2012
[10] M. Kloetzer, C. Mahulea, A. Burlacu, and F. Ostafi, “Sample gathering solutions with
numerical evaluations for m obile robots,” Autonomous Robots, 2015, under review.
[11] A. Burlacu, M. Kloetzer, and D. Panescu, “Optimal multi -agent planning solution for a
sample gathering problem,” in IEEE Int. Conference on Automation, Quality and Testing,
Robotics, 2014, pp. 1 –5.

43 [12] M. Meghjani and G. Dudek “Combining multi -robot exploration and rendezvous”,
Canadian Conference on Computer and Robot Vision, 2011, p. 80 – 85.

44 Anex e

mainVizitareRegiuni.m

clc
clear all
close all

% Initializing webcam if not already initialized
video = imaqfind();
if isempty(video)
video = initializeWebcam();
end

% Define thresholds for the used colors
defineColors;

% Capturing a frame
clear photo;
trigger(video);
photo = getdata(video);

% Identify robots in the environment
[r_c, h_c, robotNo] = infoRobots_v2(photo,0:7,Colors(1));
global Robot;
if ~isempty(Robot)
serialobj = [Robot.serial];
[Robot.free] = deal(1);
[Robot.carry] = deal(0);
[Robot.dir] = deal(1);
end
if isempty(Robot) | ~isequal([Robot.no],robotNo) |
find(strcm p(serialobj.status, 'closed' )==1)
if ~isempty(Robot)

45 fclose([Robot.serial]);
end
if ~kConnect_v2(robotNo); % connect robots
return
end
end

global videoimg;
global videofig;
videoimg = {};
videofig = {};

% Plot captured frame
init_fig=figure( 'units','normalized' ,'outerposition' ,[0 0 1 1]);
image(photo); title( 'Original Image' ); hold on;
% Determine environment bounds
x_max=size(photo,2);
y_max=size(photo,1);
env_bounds=[0,x_max,0,y_max];

% Plot robots
plot(h_c(1,:),h_c(2,:), 'pw','Markersize' ,10);
initial_h_c=h_c;

% Detect regions in the environment
[Reg, bckg_img] = detectRegions(photo, Colors, 1);
for i=1:length(Reg)
plot(Reg(i).centroids(1),Reg(i).centroids(2), 'b*');
end

regCentroids=[];
for j=1:length(Reg)
regCentroids=[regCentroids;Reg(j).centroids];
end

regions=round(regCentroids)';
reg_backup=regions;

46 while ~isempty(regions)

if size(regions,2)== 1
[minDist,idxRegion]=reg_to_rob(h_c,regions);
[minD,idx]=min(minDist);
trajectory={};

Robot=Robot(idx);;

trajectory{1}=[[h_c(1,idx);h_c(2,idx)] ,
[regions(1,idxRegion(idx));regions(2,idxRegion(idx))]];
figure(init_fig);
trajectoryi=trajectory{1};
plot(trajectoryi(1,:),trajectoryi(2,:), '.-','MarkerSize' ,14,'Linewidth' ,2);
h=gcf();
goal=regions(:,1);
[trajectory, r_c, h_c] =
followTrajectory_v4(video,Colors(1),trajectory,2,init_fig,0,goal);
return;
else
for i=1:size(h_c,2)
[minDist,idxRegion]=reg_to_rob(h_c,regions);
trajectory{i}=[[h_c(1,i);h_c(2,i)] ,
[regions(1,idxRegion(i));regions(2,idxRegion(i))]];

[dist1,dist2,angle_condition]=angle_between_paths(h_c,reg_backup(:,idxRegio
n(i)));
if (i>1) && ((coef_raports(trajectory{i},trajectory{i -1})) ||
angle_condition)
%diferenta pe x,y dintre centr ele robotilor
dif_x=abs(h_c(1,1) -h_c(1,2));
dif_y=abs(h_c(2,1) -h_c(2,2));
if dif_y<100
if env_bounds(4) -h_c(2,i)>=env_bounds(4)/2
trajectory{i}=[[h_c(1,i);h_c(2,i)] ,[r_c(1,i -1);r_c(2,i –
1)+170],[regions(1,idxRegion(i));regions(2,idxRegion(i))]];
else
trajectory{i}=[[h_c(1,i);h_c(2,i)] ,[r_c(1,i -1);r_c(2,i -1)-
170],[regions(1,idxRegion(i));regions(2,idxRegion(i))]];
end

47 end
if dif_x<100
if env_bounds(2) -h_c(1,i)>=env_bounds(2)/2
trajectory{i}=[[h_c(1,i);h_c(2,i)] ,[r_c(1,i -1)+170;r_c(2,i –
1)],[regions(1,idxR egion(i));regions(2,idxRegion(i))]];
else
trajectory{i}=[[h_c(1,i);h_c(2,i)] ,[r_c(1,i -1)-170;r_c(2,i –
1)],[regions(1,idxRegion(i));regions(2,idxRegion(i))]];
end
end
end

figure(init_fig);
trajectoryi=trajectory{i};

plot(trajectoryi(1,:),traj ectoryi(2,:), '.','MarkerSize' ,14,'Linewidth' ,2);
h=gcf();
goal=regions(:,idxRegion(i));
regions(:,idxRegion(i))=[];
end

[trajectory, r_c, h_c] =
followTrajectory_v4(video,Colors(1),trajectory,2,init_fig,0,goal);

end
end

mainSampleGathering.m

clc
clear all
close all

% Initializing webcam if not already initialized
video = imaqfind();
if isempty(video)
video = initializeWebcam();
end

% Define thresholds for the used colors

48 defineColors;

% Capturing a frame
clear photo;
trigger(video);
photo = getdata(video);

% Identify robots in the environment
[r_c, h_c, robotNo] = infoRobots_v2(photo,0:7,Colors(1));
global Robot;
if ~isempty(Robot)
serialobj = [Robot.serial];
[Robot.free] = deal(1);
[Robot.carry] = deal(0);
[Robot.dir] = deal(1);
end
if isempty(Robot) | ~isequal([Robot.no],robotNo) |
find(strcmp(serialobj.status, 'closed' )==1)
if ~isempty(Robot)
fclose([Robot.serial]);
end
if ~kConnect_v2(robotNo); % connect robots
return
end
end

global videoimg;
global videofig;
videoimg = {};
videofig = {};

% Plot captured frame
init_fig=figure( 'units','normalized' ,'outerposition' ,[0 0 1 1]);
image(photo); title( 'Original Image' ); hold on;

% Determine environment bounds
x_max=size(photo,2);
y_max=size(photo,1);
env_bounds=[0,x_max,0,y_max];

49 % Plot robots
plot(h_c(1,:),h_c(2,:), 'pw','Markersize' ,10);
initial_h_c=h_c;

% Detect regions in the environment
[Reg, bckg_img] = detectRegions(photo, Colors, 1);
for i=1:length(Reg)
plot(Reg(i).vert(1,:),Reg(i).vert(2,:), '.m','MarkerSize' ,14);
end

% radius = Robot(1).radius; % robots' radius
epsilon = Robot(1).epsilon; % distance between the center and the reference
of the robots

% Increase obstacles
[obstacles_incr, env_bounds_new] = increaseObstacles({Reg.vert},
epsilon*11/4, 8, env_bounds);

% Compute triangular decomposition
[C,adj,mid_X,mid_Y] =
triangularDecomposition(obstacles_incr,env_bounds_new);
dec_fig =
plotDecomposition(obstacles_incr,env_bounds,C,adj);set(gca, 'ydir','reverse'
);

% Modify costs based on approximate travelling distance (last argument can
be 1,2,3)
cost = modifyCost(C,adj,mid_X,mid_Y,1);

% Detect samples in the free space
samples = detectSamples(photo, Colors(1));

if size(samples,2) < size(h_c,2)
% Selecting target for each robot
fprintf( 'Select target for e ach robot. \n');
title('Click on target points' );
goal=zeros(2,size(h_c,2));
for i=1:size(h_c,2);
[goal(1,i),goal(2,i)]=ginput(1);
end

50 plot(goal(1,i),goal(2,i), 'ow','Markersize' ,8);
title(' ');
else
goal = samples(:,1:size(h_c,2));
plot(goal(1,:),goal(2,:), '.w','MarkerSize' ,20);
end

% Finding trajectory for each robot
trajectory=cell(1,size(h_c,2));
travel_dist=[];
path_cells=cell(1,size(h_c,2));
cost_path=[];
max=0;
color='rgbcym' ;
for i=1:size(h_c,2)
[trajectoryi, travel_disti, path_cellsi, cost_pathi] =
findTrajectory(C,cost,mid_X,mid_Y, …
h_c(1,i),h_c(2,i),goal(1,i),goal(2,i),obstacles_incr);
fprintf( '\nDistance to be travelled by robot %d:
%g\n',Robot(i).no,travel_disti);
figure(init_fig);
plot(trajectoryi(1,:),trajectoryi(2,:), '.-
','Color',color(i), 'MarkerSize' ,14,'Linewidth' ,2);
if (size(trajectoryi,2)>max)
max=size(trajectoryi,2);
end
trajectory{i}=trajectoryi;
travel_dist=[travel_dist; travel_disti];
path_cells{i}=path_cellsi;
cost_path= [cost_path; cost_pathi];
end

% Threshold for reaching target
eps = 10; % pixels

disp ('press any key to move…' );
pause

% Move robots along the trajectories

51 [trajectory, r_c, h_c] =
followTrajectory_v4(video,Colors(1),trajectory,eps,init_fig,0,goal) ;

[cdata, ~] = getframe(init_fig);
videoimg{end+1} = photo;
videofig{end+1} = cdata;

initializeWebcam.m

% ––––––––––––––––-
% Initialize the webcam and its parameters.
% Syntax:
% vid = initializeWebcam()
% Parameters: –
% Output:
% vid – video object;
% ––––––––––––––––-

defineColors.m

%–––––––––––––––––––––––––
––––––––
% This script defines each color wh ich is used with minimum and maximum
threshold for RGB colors.
% Syntax: defineColors
%–––––––––––––––––––––––––
––––––––

infoRobots_v2.m

% ––––––––––––––––––––––––-
––––––––––––
% Provides information on the position of the robots' center and reference
point.
%
% Syntax:
% [rob_center, hole_center, robot_no] = infoR obots_v2(photo, index,
robotColor)
% Parameters:
% photo – matrix which contains image data;

52 % index – 1 X m vector which contains robots' number, where m is the
number of robots to be searched for;
% robotColor – the robot's color from the structure where colors are
defined;
% Output :
% rob_center – 2 X n matrix which contains the coordinates of the
robots center, where n is the
% number of found robots;
% hole_center – 2 X n matrix which contains the coor dinates of the
robots reference point (hole),
% where n is the number of found robots;
% robot_no – 1 X n vector which contains the number of each found
robot.
% ––––––––––––––––––––––- ––
––––––––––––

kConnect_v2.m

% ––––––––––––––––––––––––-
––––––
% Function for connecting mobile robots. It loads the Robot structure
defined by the script
% defineRobP aram.m and it connects to the robots specified by their
numbers. Returns true
% if all robots are successfully connected and false otherwise.
%
% Syntax:
% ok = kConnect_v2(index)
% Parameters:
% robotNo – vector with numbers corresponding to each robot waiting
to be connected.
% Output :
% ok – boolean value; true if all robots are successully connected,
false otherwise.
% ––––––––––––––––––––––––-
––––––

detectRegions.m

%–––––––––––––––––––––––––
–––––––––
% Detect all regions from the environment except robots.

53 % Syntax:
% [Reg, bckg_img] = detectRegions(init_img, Colors, ind_robotColor)
% Parameters:
% init_img – initial image;
% Colors – the strucure which defines each color used with minimum
and maximum threshold;
% ind_robotColor – robot's color index;
% Output:
% Reg(i).vert – matrix with 2 rows; contains vertexes coordinate s of
region i;
% Reg(i).color – color of region i;
% bckg_img – initial image without background;
%–––––––––––––––––––––––––
–––––––––

reg_to_rob.m

%%function that calculate min distance between every region and robots
function [min_distance,indexOfMin]=reg_to_rob(h_c,regions)
distance=[];
for i=1:size(regions,2)
for j=1:size(h_c,2)
distance(j,i)=sqrt((h_c(1,j) -regions(1,i))^2+(h_c(2,j) –
regions(2,i))^2);
end
end
for i=1:size(h_c)
[min_distance(i),indexOfMin(i)]=min(distance(i,:));
fprintf( 'robot number %d to region number %d with di stance
%d\n',i,indexOfMin(i),min_distance(i));
end
end

angle_between_paths.m

%%function that calculates the angle between h_c and the closest region

function [dist1,dist2,angle_condition]= angle_between_paths(h_c,region)
for i=1:size(h_c,2) -1
xb=h_c(1,i);
xa=region(1,1);

54 xc=h_c(1,i+1);
yb=h_c(2,i);
ya=region(2,1);
yc=h_c(2,i+1);
dist1=sqrt((xb -xa)^2+(yb -ya)^2);
dist2=sqrt((xc -xa)^2+(yc -ya)^2);
alfa=acos(((xb -xa)*(xc-xa)+(yb-ya)*(yc-ya))/(dist1*dist2));
end
if alfa<(pi/6)
angle_condition=true;
else
angle_condition=false;

end

coef_raports.m

%%function that calculates the raports of the path ecuations coeffi cients
function ret_val=coef_raports(trajectory1,trajectory2)
a1=trajectory1(2,2) -trajectory1(2,1)
b1=trajectory1(1,2) -trajectory1(1,1)
c1=-trajectory1(1,1)*a1+trajectory1(2,1)*(trajectory1(1,1) –
trajectory1(1,2))
a2=trajectory2(2,2) -trajectory2(2,1)
b2=trajectory2(1,2) -trajectory2(1,1)
c2=-trajectory2(1,1)*a1+trajectory2(2,1)*(trajectory2(1,1) –
trajectory2(1,2))
eps1=1.5;

if abs(a1/a2 -b1/b2)<eps
ret_val=true;
else
ret_val=false;
end
end

moveRobots_v2

% ––––––––––––––––––––––––-
––––––
% Set the speed of the two motors for each robot and send it to a serial
port object.

55 %
% Syntax:
% moveRobots_v2(index, speed_motor_left, speed_motor_right)
% Parameters:
% index – array containing robots' index;
% speed_motor_left – array containing the speeds for the left motor
of each robot;
% speed_motor_right – array containing the speeds for the right motor
of each robot;
% Output: –
% Obs: The command is sent asynchronous.
% ––––––––––––––––––––––––-
––––––

increaseObstacles.m

% ––––––––––––––––––––––––-
–––––––––––––
% Increase convex obstacles for a robot with radius "robot_radius".
% Syntax:
% [obst_new, env_bounds_new] = increaseObstacles(obstacles,
robot_radius, nr_rob_pol_vert, env_bounds)
% Parameters:
% obstacles – the regions which we want to increase;
% robot_radius – if the robot rotates around a desired reference
point, this new radius should include
% all po ssible rotations;
% nr_rob_pol_vert – number of vertices of regular convex polygon
which approximates the robot;
% env_bounds – defines the rectangle with environment bounds, having
the form [x_min, x_max, y_min, y_max];
% Output:
% obst _new – the regions with increased dimensions;
% env_bounds_new – new environment bounds, in the same form;
% Obs: The robot is assumed non -rotating, and it is reduced to its center.
% –––––––––––––––––––––– –––
–––––––––––––

56 triangularDecomposition.m

% ––––––––––––––––––––––––-
–––––––––––––
% Decompose the environment into triangles (without the obstacles).
% Syntax:
% [C,adj,varargout]=triangularDecomposition(objects,env_bounds)
% Parameters:
% objects – cell where each element is a matrix with 2 rows, giving
the vertices of current obstacle;
% env_bounds – defines the rectang le with environment bounds, having
the form [x_min, x_max, y_min, y_max];
% Output:
% C – triangle vertices in each element; (cell with each element a
matrix with 2 rows and 3 columns);
% adj – adjacency matrix, with adj(i,j)=1 if cell i is adj acent to j
(adj(i,i)=1 and adj is symmetric);
% varargout – middle points between adjacent cells;
% ––––––––––––––––––––––––-
–––––––––––––

plotDecomposition.m

% ––––– ––––––––––––––––––––
–––––––––––––
% Plot the obstacles, environment bounds, cell decomposition, adjacency,
middle points.
% Syntax:
% env_h = plotDecomposition(objects,env_bounds,vararg in)
% Parameters:
% objects – cell where each element is a matrix with 2 rows, giving
the vertices of current obstacle;
% env_bounds – defines the rectangle with environment bounds, having
the form [x_min, x_max, y_min, y_max];
% varargi n – might be cell decomposition, adjacency, middle points
(depending on number of arguments);
% Output:
% env_h – figure handle;
% ––––––––––––––––––––––––-
–––––––––––––

57 modifyCost.m

% ––––––––––––––––––––––––-
–––––––––––––––––––
% Modify costs based on approximate travelling distance.
% Syntax:
% cost = modifyCost(C,adj,mid_X,mid_ Y,flag)
% Parameters:
% C – triangle vertices in each element; (cell with each element a
matrix with 2 rows and 3 columns);
% adj – adjacency matrix, with adj(i,j)=1 if cell i is adjacent to j
(adj(i,i)=1 and adj is symmetric);
% mid_X, m id_Y – middle points between adjacent cells;
% flag = 1 – cost for going from cell i to cell j equals with
distance between centroids of the two cells;
% = 2 – cost for going from cell i to cell j equals with
distance between centroid of i and middle point of line
% segment shared by i and j;
% otherwise – cost for going from cell i to cell j equals with
the average distance from possible entry points in i
% (from other cell than j) to exit point from i
(midpoint of line segment shared by i and j);
% Output :
% cost – the distance between adjacent cells (estimated according
considered flag);
% ––––––––––––––––––––––––-
––––– ––––––––––––––

detectSamples.m

% ––––––––––––––––––––––––-
–––––––––
% Detects the coordinates of the samples placed in the environment.
% Syntax:
% sample = detectSamples(photo, sampleColor)
% Parameters:
% photo – matrix which contains image data;
% sampleColor – the sample's color from the structure where colors
are defined;
% Output :

58 % sample – 2 by n matrix which contains the coordinates for each
sample, where n is the
% number of samples found.
% ––––––––––––––––––––––––-
–––––––––

findTrajectory.m

% –––––––––––––––- –––––––––
––––––––––––––
% Given a start and a final point, it finds the trajectory.
% Syntax:
% [trajectory, distance, path, cost_path] =
findTrajectory(C,adj,mid_X,mid_Y,x_st,y_st,x_fin,y_fin,varargin)
% Parameters:
% C – cells from decomposition;
% adj – adjacency (costs/weights) matrix;
% mid_X, mid_Y – middle points of line segments shared by adjacent
cells;
% x_st, y_st – coordinates of start points;
% x_fin, y_fin – coordinates of final points;
% Output:
% trajectory – matrix with 2 rows; each column defines a point;
% distance – the travelled distance (length of trajectory);
% path – the sequence of cells to be followed;
% Obs. The trajectory is obtained by linking these successive points (it is
an angular trajectory formed by
% connected line segments).
% ––––––––––––––––––––––––-
––––––––––––– –

followTrajectory_v4.m

% ––––––––––––––––––––––––-
––––––––––––––
% Move robots along the trajectories, with the option of returning when
first trajectory is finished thus the index
% of the robot which finished its path is returned along with the
unfinished trajectories. A suplimentary parameter
% is introduced for picking samples. Robots will align to the direction of
the next node in their trajectories after

59 % picking samples in order to not drop any. Collision between robots are
avoided. The movement stops if the distance
% between two robots is below a specified threshold. The aolgorithm goes
into deadlock and waitsfor the user to
% resolve it. Also the Robot structure is d efined as global.
%
% Syntax:
% [trajectory, r_c, h_c, varargout] = followTrajectory_v3(vid,
khepera, robotColor, trajectory, eps, …
% init_fig, return_first,
pick_sample)
% Parameters:
% vid – video object;
% khepera – array containing serial objects that enable communication
via Bluetooth;
% robotColor – the color of robot from the structure where colors are
defined;
% trajectory – 1 by n cell structure containing trajectories for the
n robots
% eps – threshold for reaching target;
% init_fig – original image used for plotting robots' path;
% return_first – flag for return when one of the trajectories is
finished;
% pick_sample – node in the trajectory where the sample is placed.
% Output:
% trajectory – 1 by n cell structure containing trajectories for the
n robots;
% if return_first=0 a structure with empty cells is
returned;
% r_c – robots' current position;
% h_c – robots' reference;
% varargout{1} – index of the robot which finished its trajectory
(only when return_first).

% ––––––––––––––––––––––––-
––––––––––––––

60