148

Lucrarea 12

Realizarea hrii unui teren i generarea unei traiectorii optime printre obstacole

Coninut 1. Scopu l lucr r i i ..................................................................................... 148 2. Desfurarea lucr r i i ........................................................................ 148 3. Seciunea practic a lucrrii .......................................................................... 151

A.1. APLICAIA 1. Testarea aplicaiilor demonstrative .............................. 151 A.2. APLICAIA 2. Rezolvarea unei probleme n care robotul nu are loc suficient s treac printre dou obstacole ........................................................ 152 3.1. Comanda fizic a robotului NXC ......................................................... 153 A.3. APLICAIA 3. Comanda direct a robotului utiliznd mediul Matlab .. 154 A.4. APLICAIA 2. Comanda robotului prin transmiterea datelor din mediul Matlab ctre programul care ruleaz pe robot .................................................. 155

1. Scopul lucrrii O categorie important de aplicaii n robotic necesit rezolvarea problemei

de gsire a drumului optim ntre dou puncte. Problema const n gsirea drumului de la un punct de start la un punct final, pe un teren n care sunt prezente obstacole. Drumul trebuie s fie gsit pe o hart bidimensional, n care obstacolele sunt de form poligonal, cu coordonatele vrfurilor cunoscute.

Lucrarea are ca scop furnizarea traseului optim pe care robotul trebuie s-l parcurg printre obstacole fixe pentru a ajunge la punctul final. Harta terenului se construiete n mediul Matlab, iar identificarea drumului optim de parcurgere a cii pn la punctual final se realizeaz folosind algoritmul Dijkstra.

2. Desfurarea lucrrii

Modul de comand cel mai flexibil i performant este cel dat de rularea concomitent a dou programe, unul n MATLAB, care ruleaz pe PC, iar cellalt n NXC, care ruleaz pe procesorul robotului. Astfel se beneficiaz de avantajul dezvoltrii i procesrii ntr-un mediu puternic, pe un procesor performant, urmnd ca n ececutarea sarcinii s se utilizeze capacitatea senzorial i de control a robotului. n mediul MATLAB s-a construit harta terenului, s-au introdus coordonatele obstacolelor i s-a detrminat ruta optim de parcurgere a unui traseu dat prin dou puncte de coordonate.

Robotul mobil se gsete ntr-o poziie iniial (de start) i trebuie s ajung ntr-o poziie final (destinaie), evitnd coliziunile cu obstacolele existente n teren.

Realizarea hrii unui teren i generarea traiectoriei optime printre obstacole

149

Obstacolele sunt reprezentate prin suprafee poligonale convexe. Se vor furniza coordonatele vrfurilor acestora. Planul de lucru se consider situat n cadranul I al sistemului de coordonate cartezian.

Punctul de start i punctul destinaie sunt incluse ntr-o matrice ale crei linii sunt coordonatele vrfurilor poligoanelor. Pentru a separa poligoanele, ntre acestea se folosete valoarea infinit: "inf". Prima linie a matricei reprezint punctul de start, iar ultima linie punctul destinaie. n figura 12.1 este ilustrat un exemplul de teren obinut prin introducerea punctelor i a poligoanelor ntr-un program demonstrativ.

Fig 12.2 Exemplu de teren cu obstacole, punct iniial i punct destinaie

Pentru calculul centrul poligonului, se utilizeaz formule:

( )( )[ ] ( )( )[ ]

=

+++

=

+++ +=+=1

0111

1

0111 6

1,

61 n

iiiiiiiy

n

iiiiiiix yxyxyyA

CyxyxxxA

C

unde A este aria poligonului: ( )

=

+=

1

012

1 n

iiiii yxyxA

n funcie de centrele poligoanelor se genereaz contururile acestora. Se consider c robotul a intrat n coliziune cu un obstacol dac poziia sa devine interioar marginilor de siguran ale obstacolului. Prin urmare, dac robotul se deplaseaz de-a lungul unui obstacol delimitat de marginile de siguran, nu intr n coliziune cu acesta.

ntr-o prim etap, se traseaz un segment de dreapt ntre punctul de start i cel de destinaie i se afl locurile n care segmentul se intersecteaz cu obstacolele. Relaiile utilizate pentru a determina dac un segment intersecteaz un poligon sunt urmtoarele:

0111

*

111

421

421

321

321 yyyxxx

yyyxxx && 0

111*

111

432

432

431

431 yyyxxx

yyyxxx (12.3)

unde ( )11, yx , ( )22 , yx reprezint coordonatele limitelor segmentului, n timp ce ( )33, yx , ( )44 , yx sunt coordonatele limitelor poligonului

Lucrarea 12

150

Demonstrativ, n figura 12.4 s-au generat toate variantele de drumuri ntre start i destinaie. Robotul se deplaseaz ntre vrfurile poligoanelor.

Fig. 12.4 Determinarea tuturor variantelor posibile de drumuri

Se observ din imaginea precedent lipsa unui traseu ntre punctele A i B. Acest lucru este realizat pentru a minimiza numrul de rute intermediare inutile, deoarece dac un punct are acces direct la punctul destinaie, nu mai are sens s se creeze restul legturilor cu celelalte puncte.

n figura 12.6 este prezentat drumul optim obinut prin rularea algoritmului Dijkstra, existent n programul: ProcDrum.m

Fig. 12.6 Soluia obinut pentru problema determinrii drumului optim

Pentru exemplul terenului prezentat anterior, traseu parcurs robot marcat cu linie continu este prezentat n figura 12.7. Traseul iniial comandat robotului este reprezentat prin linie punctat.

Realizarea hrii unui teren i generarea traiectoriei optime printre obstacole

151

(a) (b) Fig. 12.7 (a) Diferena dintre traserul comandat (linie continu) i traseul executat (linie

punctat); (b) Distana parcurs de robot n timp

3. Seciunea practic a lucrrii 1) Directorului n care se afl fiierele trebuie s fie adugat n lista cilor

(path) n MATLAB, iar directorul ce conine fiierele surs trebuie s fie directorul curent.

2) ncrcarea coordonatelor - Coord ca variabile.

A.1. APLICAIA 1. Testarea aplicaiilor demonstrative

Se vor rula fiierele cu date: M1.mat i M2.mat ce permit obinerea a dou terenuri iniiale.

Variabila de intrare - Coord va conine: - datele despre obstacole, - punctul de start - punctul destinaie.

Coninutul variabilei Coord: prima coloan reprezint coordonata X a punctelor, cea dea doua

coloan, coordonata Y. primul punct (prima linie) reprezint locul unde este localizat robotul

(punctul sursa). ultimul punct (ultima linie) este poziia unde trebuie s ajung (punctul

destinatie). obstacolele (poligoanele) sunt desprite ntre ele de ctre valoarea: Inf

Valorile se introduc n mm (dac se dorete alt unitate de msur se modific constantele latimeRobot i departare (localizate n fiierul ProcDrum). Atenie: Se va exclude situaia n care punctele introduse la editarea fiierului (Coord), ar conduce la suprapuneri de poligoane.

Lucrarea 12

152

Date de intrare ale terenului cu obstacole, existente n fiierul M1.mat

n figura 12.8 este ilustrat exemplul de teren generat folosind datele de intrare alturate, n care ostacolele sunt reprezentate vizual sub form de poligoane.

Fig 12.8 Teren cu obstacole, punct de start i punct destinaie

Punct de start:

2200 400

Separarea

pligoanelor:

Inf Inf

Punctele unui

poligon:

2000 800

2400 900

2500 1100

1700 1200

Inf Inf

3100 400

4000 700

3400 1100

2900 1000

2600 500

Inf Inf

3900 1100

4000 1500

3500 1300

Inf Inf

3000 1400

3700 1700

3300 2100

2500 1600

Inf Inf

1800 1400

1800 1800

1400 2300

1300 1900

1000 1800

1500 1600

Inf Inf

2400 1900

2800 2300

1900 2300

2100 2000

Inf Inf

3700 2300

3900 2600

3300 2700

Inf Inf

2800 2800

3100 3200

2500 3200

Inf Inf

1600 2600

2000 2700

2100 2900

1300 3100

Inf Inf

Punctul destinatie: 2800 3700

*Determinarea drumului optim se realizeaz prin rularea programului: ProcDrum.m

3) Modalitatea de introducerea a altor puncte surs i destinaie i de adaugare/modificare/eliminare de obstacole, se realizeaz prin creerea nc de la nceput a variabilei Coord, care trebuie s respecte tipul de formatare enunat anterior (Interpretarea variabilei Coord).

A.2. APLICAIA 2. Rezolvarea unei probleme n care robotul nu are loc suficient s treac printre dou obstacole

Se consider cazul n care robotul nu are loc suficient s treac printre dou obstacole. Pentru a aceasta se vor generara contururile poligoanelor (la o distan ce reprezint ~ 2/3 sau din limea robotului). Distana fa de un obstacol se poate modifica n fiierul ProcDrum.m n funcie de limea robotului.

Astfel, ntre cele dou obstacole nu va mai fi generat niciun drum posibil, deoarece cele dou contururi se suprapun. n figura 12.10 este ilustrat cazul analizat

Realizarea hrii unui teren i generarea traiectoriei optime printre obstacole

153

robotul nu poate trece printre dou obstacole existente n teren. Terenul are urmtoarele date de intrare: Date: 766 1000 Inf Inf 565 580 770 415 875 550 671 720 Inf Inf 53 980 425 980 425 1230 53 1230 Inf Inf 395 1545 585 1545 585 1790 395 1790 Inf Inf 725 1400 890 1400 890 1550 725 1550 Inf Inf 600 2000 Fig. 12.10 Robotul mobil nu poate trece printre dou obstacole existente n teren

Drumul optim generat prin rularea aplicaiei este ilustrat n figura 12.12.

Fig. 12.12 Determinarea drumului optim n final, din tot acest proces rezult matricea (matrFin) cu coordonatele

punctelor pe care trebuie s le parcurg robotul.

Exemplu: matrFin = [0,33;33,66;0,33;66,66;33,99], n care prima linie reprezint punctul de start, iar ultima linie - punctul destinaie.

Dac se dorete micarea robotului dup un anumit traseu, se poate utiliza doar aceast matrice, introducnd punctele dorite pentru poligoane, punctele de start i destinaie.

3.1. Comanda fizic a robotului NXC Calculele i efectuarea micrii ntre punctele furnizate de programul din MAtlab sunt realizate de ctre robot prin rularea unui program n limbajul NXC.

Lucrarea 12

154

A.3. APLICAIA 3. Comanda direct a robotului utiliznd mediul Matlab

ntre comenzile directe disponibile n mediul Matlab nu exist (momentan) o funcie pentru resetarea pozitiei unghiurilor motoarelor (funcia disponibil de pentru resetare permite resetarea unui singur contor). Pentru aceasta este necesar utilizarea unui program ce ruleaz pe robot (de exemplu rulat n NXC) ce realizeaz doar oprirea motoarelor. Prin accesarea automat a programului din MATLAB se va putea realiza resetarea tuturor unghiurilor motoarelor. Se va descrca acest progam, deja existent, cu numele STOP, pe robot. Programul este inclus n directorul n care se gsete fiierul cu instruciuni.

// STOP.nxc task main () { Off(OUT_ABC); ResetAllTachoCounts(OUT_A); ResetAllTachoCounts(OUT_B); ResetAllTachoCounts(OUT_C); Wait(10); }

Se va deschide fiierul ComandaDMAT.m

Un parametru important este variabila beta. Aceasta reprezint unghiul iniial. Acest unghi este doar pozitiv (0,360)grade. De exemplu:

Testele de comand a robotului se vor realiza pe o structur de tip tribot. Distana dintre roi (ecartament) este setat n program la valoarea de 115 mm. Valoare ecartamentului se poate modifica n fiierul ValInt.m, avnd numele rrr.

Pentru a obine o precizie ct mai bun (o deviaie prea mare poate duce la euarea parcurgerii traseului) trebuie ajustat puterea motoarelor i parcurgerea traseului de ctre robot se va face la o vitez relativ mic. Este acceptabil o deviaie de dou grade la fiecare micare. Pentru testarea acestui fapt vor fi modificate dou paragrafe cu codul:

abs(SS.TachoLimit); abs(SS.RotationCount);

Se va elimina caracterul ; pentru afiarea la consol a datelor. Se permite analiza citirilor i implicit a diferenei ntre comanda dat i rotirea real a roilor robotului. In functie de aceste diferente se seteaz puterile putereSens i putereDistanta. (Exemplu de puteri care au dat erori mici: putereSens = 8; putereDistanta = 16; )

Observatie: Realizarea comenzilor sa facut cu ajutorul librariei RWTH Mindstorms NXT Toolbox. Adresa paginii pentru instalarea librariei este urmtoarea: http://www.mindstorms.rwthaachen.de/trac/wiki/Download

Realizarea hrii unui teren i generarea traiectoriei optime printre obstacole

155

Dup instalare se pot efectua setrile necesare conexiunii Bluetooth. Dup activarea robotului NXT, se va executa urmtorul cod:

handle = COM_OpenNXT('bluetooth.ini', 'check');

Se atepta pn la afiarea unui mesaj i se va ignora mesajul de Warning, dac apare. Dac apare un mesaj de EROARE, atunci se vor verifica setarile, i se vor reporni pe rnd robotul NXT i mediul MATLAB.

Se va introduce comanda: COM_SetDefaultNXT(handle); Astfel, este realizat conexiunea i poate fi executat fiierul pentru comanda propriuzis a robotului (ComandaDMAT.m)

A.4. APLICAIA 4. Comanda robotului prin transmiterea datelor din mediul Matlab ctre programul care ruleaz pe robot

A doua variant de comand const n transmiterea datelor din mediul Matlab ctre programul care ruleaz pe robot, program scris n limbajul NXC. Comanda propriuzis va fi realizat de ctre programul scris n limbajul NXC (numit go.nxc).

Primul pas este adaugarea fisierului suplimentar NXT_MessageWrite.m n directorul unde se gsesc celelalte fiiere ale librariei pentru MATLAB (RWTHMindstormsNXT). Acesta este un fiier suplimentar ce nu a fost inclus n libraria standard.

Se va stabili modul de comunicare ntre MATLAB i robot.

handle = COM_OpenNXT('bluetooth.ini', 'check'); SetDefaultNXT(handle); )

Se va rula programul de pe robot (go.nxc), completat cu comenzile de micare utile. Acesta poate fi executat din interfaa robotului sau din mediul MATLAB folosind urmtoarea comand:

NXT_StartProgram('go');

Rularea comenzii TransMess, care va transmite coordonatele programului go.nxc. Fiierul TransMess are pe ultima linie comanda PrimMess, comanda ce lanseaz fiierul PrimMess. Acesta ateapt primirea datelor napoi de la robot (programul go.nxc) ctre MATLAB.

Dup executarea comenzilor de ctre programul ce ruleaz pe robot (go.nxc), acesta va furniza mediului Matlab date precum numrul de grade efectuate de robot la fiecare micare. Acestea vor fi primite de PrimMess.

Fig 12.13 Rularea fiierelor TransMess i PrimMess

156