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

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. [anonimizat] 2017

DECLARAȚIE DE ASUMARE A [anonimizat], 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” [anonimizat] 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 ([anonimizat].POM.02 – Funcționarea Comisiei de etică universitară), [anonimizat], [anonimizat] (legea 8/1996) și a convențiilor internaționale privind drepturile de autor.

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

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 obstacolelor 27

Figura 4-7 Reprezentare obstacole toolbox RMTool 27

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

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

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 evoluț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, permițâ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 spaț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ă (), în cunoașterea spațiului cosmic, î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 avantaje considerabile pentru pacienți ().

Figura – Robot chirurgical Da Vinci

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 – 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 senzoriale. Arhitectura de control poate fi una centralizată, unde există o unitate centrală (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 traiectoriei reprezintă una din problemele fundamentale ale roboticii și reprezintă un subiect important de cercetare cu aplicabilitate în explorarea planetelor sau a mediilor inaccesibile omului ().

Figura – 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ăștiate î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.

Analiza și descompunerea spațiului de lucru

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 de 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 ⊂ domeniul dreptunghiular de evoluție, iar obstacolele se notează cu . Pentru orice formă politopală S ∈ {E, } se notează cu setul vârfurilor sale și cu setul fațetelor (laturilor) sale, unde |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ă (anti-orară), considerând că fațeta este definită de vârfurile și ș.a.m.d. Spațiul liber reprezintă porțiunea din domeniul E care nu este acoperită de nici un obstacol. () prezintă un mediu de evoluție ce respectă presupunerile de mai sus, ilustrând totodată convențiile de notare ale vârfurilor și fațetelor.

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 formă geometrică(celule). Pentru un domeniu cunoscut E, 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 și o relație de adiacență între celule, 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 – 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 aproximativă dacă uniunea celulelor sale este inclusă în spațiul liber, și aproximează acest spațiu cu o anumită precizie. În continuare voi prezenta trei tipuri de descompuneri exacte (trapezoidală, triunghiulară, politopală) și o descompunere aproximativă (dreptunghiulară) .[3]

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 menț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 faț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 sunt calculate în timpul descompunerii (intersecțiile dintre dreptele verticale și fațetele obstacolelor). Relația de adiacență se determină ușor, având în vedere că două trapeze vecine au în comun un segment al unei drepte verticale.

În cazul în care două sau 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 mică 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 () este prezentat un exemplu de descompunere trapezoidală, în care adiacența între celule este ilustrată prin linii albastre punctate. [3]

Figura – Descompunerea trapezoidală a domeniului din ().

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ă triunghiuri adiacente rezultate au în comun o întreagă fațetă (spre deosebire de cazul 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 drept 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 triunghiurile rezultate 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 dreaptă 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 descompunere în triunghiuri, este dată de o triangularizare cu restricț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 au două vârfuri comune. În () este prezentat un exemplu de descompunere în celule triunghiulare. Liniile punctate reprezintă adiacența între celule. [3]

Figura – Descompunerea în celule triunghiulare a domeniului din ().

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 triunghiurilor). 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 politop determinat nu trebuie să fie intersectat de alte drepte care provin din laturile obstacolelor. În () este prezentat un exemplu de descompunere politopală. Adiacența între celule este ilustrată prin linii punctate.

Figura – Descompunerea politopală a domeniului din ().

Descompunerea dreptunghiulară

Descompunerea dreptunghiulară 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 celule 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 diferite. Algoritmii de partiționare în dreptunghiuri 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 descendenț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 obstacole; (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 dreptunghiulare 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 devenind descendenți ai unui nod în quad-tree). Un dreptunghi nu va fi împărțit dacă este liber, ocupat, sau este mixt dar are o arie mai mică decât o precizie impusă. În ultimul caz (dreptunghi mixt cu arie mică), dreptunghiul este etichetat drept ocupat. Astfel la sfârșitul creării quad-tree-ului, frunzele vor fi etichetate liber sau ocupat, iar frunzele libere vor corespunde 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 (). Această diferență va fi considerată drept ocupată în urma abstracției corespunzând descompunerii, deoarece nu există celule care să aparțină suprafeței respective.

Figura – Descompunerea dreptunghiulară a mediului din ().

Î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 mare 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. Descompunerile 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 predefinite, însă acestea nu sunt obstacole, 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 permite 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 urmate 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.

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 două drepte, precum în ( a), (ii) puncte egal apropiate de două vârfuri: mediatoarea segmentului ce unește cele două vârfuri ( b), puncte egal depărtate de un vârf și un segment de dreaptă: parabolă (cu focarul în vârful dat) ( c).

Figura – Cazuri întâlnite în construcția unui GVD pentru un domeniu cu obstacole poligonale

Ramurile GVD-ului (seturile de puncte echidistante 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 GVD până în apropierea punctului final, iar apoi părăsește GVD-ul și se îndreaptă spre F.

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ă segmentul 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 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 soluție a problemei de planificare ( returnează o traiectorie ce unește nodul sursă cu cel destinație, fără a lovi obstacolele). O astfel de traiectorie este prezentată în () prin linia frântă de culoare roșie.

Figura – Graf de vizibilitate și o soluție pentru problema de planificare

Algoritmi de planificare a traiectoriei

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 deciziile ș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 robotului, 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. Distanța euclidiană a fost calculată folosind formula (3-1).

(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. Deoarece 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 apropiate, traiectoriile se intersectează, numărul roboților este 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 = {

While (Regiuni )

Roboți =

While (Roboți and Regiuni )

Alege Roboți, Regiuni

astfel încât distanța dintre și să fie minimă

Generează traiectoria dintre și

Roboți = Roboți – {}

Regiuni = Regiuni – {}

EndWhile

Mișcă roboții către regiunile alocate

EndWhile

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 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 grafului 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 este 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 urmă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ă este format dintr-o multitudine de rutine de procesare de imagine și rutine de control a roboților Khepera III, care permit detectarea automată a regiunilor de interes, a pozițiilor și orientărilor roboților, precum și comanda mișcării unui robot Khepera III, care să urmărească o traiectorie construită dintr-o multitudine de puncte. Acest algoritm poate fi descris prin ().

Figura – Schema pentru implementarea în timp real

Arhitectura din () 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 culoarea roșie (detecția pieselor, alocarea resurselor și găsirea traiectoriilor fără coliziune) trebuie să fie apelate de fiecare dată când un robot ajunge la depozit. Rutinele evidențiate prin culoarea albastru corespund metodelor de urmărire a traiectoriei coordonate de unitatea centrală. [1]

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 de 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ă pentru a defini obstacolele, pentru a reprezenta traiectoria obținută în urma 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, grafuri 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ă selecteze 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 alegerea 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 prezenta 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ă () î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 punctele intermediare pentru metoda de calcul ().

Figura – Fereastra principală a simulatorului RMTool

Figura – 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 (). 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.

Figura – Panoul pentru controlul mișcării

Definirea mediului, planificarea traiectoriei și controlul mișcării se realizează din panoul pentru simulare () format din trei butoane. Prin apăsarea butonului environement se deschide o casetă text în care trebuie să introducem numărul de obstacole dorit (). 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 () ș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 () și se deschide o nouă fereastră ce ne oferă instrucțiuni pentru introducerea punctului de start și punctului final în care dorim să ajungă robotul ().

Figura – Panoul pentru simulare

Figura – Alegerea numărului de obstacole

Figura – Instrucțiuni pentru definirea obstacolelor

Figura – Reprezentare obstacole toolbox RMTool

Figura – 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 (). 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 – Reprezentare traiectorie care unește punctul de start cu cel final

Figura – Exemplu simulare RMTool

Pentru planificarea online a traiectoriei roboților mobili am creat o interfață grafică (GUI) folosind Matlab (). 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.

Figura – 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, path 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 porneș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 roboți.

Rezultate experimentale

Platforma experimentală

Pentru a implementa experimentele în timp real am folosit platforma experimentală din [5], ilustrată în (). 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 mat. 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 – Platforma experimentală

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 vedere 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 (). 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 realizează 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 wide-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ă rula. 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 () 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.

Figura – 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 infraroș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 – Robotul Khepera III

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 motrice, pe care le vom nota cu (viteza roț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ții mobili nu au control complet independent al celor 3 parametri 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 cinematic următor, unde reprezintă viteza liniară de înaintare, iar viteza unghiulară în jurul axei verticale:

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

(5-2)

(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 referință și sistemul de axe curent atașat corpului robotului: planului I i se atașează sistemul de referință fix XI, OI, YI, corpului robotului i se atașează sistemul de axe XR, OR, YR. în punctul P, iar corespunzător poziției inițiale a robotului se consideră notația XRI, ORI, YRI.

Figura – 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ția robotului printr-un vector format din 3 elemente:

(5-4)

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:

(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ătoarea ecuație din manualul utilizatorului [6]:

(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:

(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 întrerupte 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ă.

Planificarea traiectoriei unei 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 Khepera III. Criteriul de cost ales a fost distanța euclidiană minimă dintre centrul roboților și centrul de greutate al regiunilor de vizitat.

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 (), considerând mediul cu trei regiuni de vizitat, sunt prezentate traiectoriile date ca referință cu culoarea albastru închis și roșu, iar traiectoriile reale urmate de roboți sunt reprezentate prin culoarea albastru deschis pentru unul din roboți, respectiv galben pentru celălalt robot. În () sunt prezente în mediu patru regiuni de vizitat, semnificațiile traectoriilor fiind aceleași ca și în figura anterioară.

În () 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 alocat să viziteze acea regiune.

Figura – Traiectoriile urmate de roboți pentru trei regiuni de vizitat

Figura – Traiectoriile urmate de roboți pentru patru regiuni de vizitat

Figura – Traiectoriile urmate de roboți pentru 5 regiuni de vizitat

În continuare, sunt prezentate rezultatele în urma implementării în timp 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 () 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 – 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 () 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 deschis pentru unul din roboți, respectiv galben pentru celălalt robot.

Figura – Traiectoriile urmate de roboți pentru mediul format din patru obstacole

În () 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 () sunt prezente în mediu cinci obstacole, semnificațiile traiectoriilor fiind aceleași ca și în ().

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

Figura – Traiectoriile urmate de roboți pentru mediul format din cinci obstacole

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 resursele î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 regiunilor. 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 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. Soluția propusă 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 mobili 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 astfel î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.

Soluțiile propuse în această lucrare rezolvă cazurile generale 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 vizitat 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, astfel încât aceștia să nu depindă de unitatea centrală, ci să ia singuri decizii în privința regiunilor de vizitat.

Bibliografie

[1] Vladimir Tiganas, Marius Kloetzer, and Adrian Burlacu-article-Multi-Robot Based Implementation for 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 Siegwart 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 mobile 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.

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

Anexe

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(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];

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

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(:,idxRegion(i)));

if (i>1) && ((coef_raports(trajectory{i},trajectory{i-1})) || angle_condition)

%diferenta pe x,y dintre centrele 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

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,idxRegion(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,:),trajectoryi(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

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

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

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

[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 which 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] = infoRobots_v2(photo, index, robotColor)

% Parameters:

% photo – matrix which contains image data;

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

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

% 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 coordinates 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 distance %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);

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 coefficients

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.

%

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

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

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 rectangle 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 adjacent 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,varargin)

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

% varargin – might be cell decomposition, adjacency, middle points (depending on number of arguments);

% Output:

% env_h – figure handle;

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

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, mid_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 :

% 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

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

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