148

Lucrarea 12
Realizarea hrii unui teren i generarea unei
traiectorii optime printre obstacole
Coninut
1. Scopul l ucr r i i ..................................................................................... 148
2. Desf ur ar ea l ucr 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
0
1 1 1
1
0
1 1 1
6
1
,
6
1
n
i
i i i i i i y
n
i
i i i i i i x
y x y x y y
A
C y x y x x x
A
C

unde A este aria poligonului:
( )

=
+
=
1
0
1
2
1
n
i
i i i i
y x y x A

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:
0
1 1 1
*
1 1 1
4 2 1
4 2 1
3 2 1
3 2 1

y y y
x x x
y y y
x x x
&&
0
1 1 1
*
1 1 1
4 3 2
4 3 2
4 3 1
4 3 1

y y y
x x x
y y y
x x x
(12.3)
unde ( )
1 1
, y x , ( )
2 2
, y x reprezint coordonatele limitelor segmentului, n timp ce
( )
3 3
, y x , ( )
4 4
, y x 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