robotica - lucrare 12a
DESCRIPTION
Realizarea hărţii unui teren şi generarea unei traiectorii optime printre obstacoleTRANSCRIPT
-
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