Universitatea Politehnica Bucureşti Facultatea de...

Universitatea Politehnica Bucureşti

Facultatea de Automatică şi Calculatoare

Departamentul de Automatică şi Ingineria Sistemelor

LUCRARE DE LICENŢĂ

Platformă stabilizată pentru fotografie aeriană

Aplicaţie: UAV

Coordonator științific:

Prof. dr. ing. Sergiu Stelian Iliescu

Consultant:

Sl. dr. ing. Iulia Dumitru

Ing. Cătălin Vasile (ITA)

Absolvent:

Vidraşcu Irina-Diana

București

2013

Irina Diana Vidrașcu CUPRINS

CUPRINS

LISTĂ FIGURI

INTRODUCERE 1

Aspecte generale privind sistemele inerţiale de navigaţie (INS) 1

Structura lucrării de licență 3

CAPITOLUL I – CONSIDERAȚII TEORETICE 4

1.1. Sisteme de referință 4

1.2. Tipuri de sisteme inerțiale 5

1.2.1. Platformă stabilă 6

1.2.2. Sistem tip “strapdown” 7

1.3. Senzori MEMS 9

1.3.1. Accelerometru 11

1.3.2. Giroscop 13

1.4. Microcontrolere ( ) 16

1.5. Algoritmi de reglare 18

1.5.1. Algoritmi convenționali

1.5.2. Algoritmi neconvenționali

18

19

CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE 21

2.1. Structura generală a platformei (montajul) 21

2.2. Senzori 22

2.2.1. Accelerometru ADXL 345 Breakout 23

2.2.2. Giroscop ITG – 3200 Breakout 25

Irina Diana Vidrașcu CUPRINS

2.3. Microcontroler 27

2.4. Motoare 30

2.5. Cameră video 33

2.6. Filtrarea datelor 34

2.7. Soluția de implementare utilizată 37

2.8. Algoritm de reglare folosit 40

CAPITOLUL III – REZULTATE 44

3.1 Rezultatele achiziției de date de la accelerometru 44

3.2 Rezultatele achiziției de date de la giroscop 49

3.3 Scenariu de test 54

CAPITOLUL IV - CONCLUZII 57

4.1. Concluzii 57

4.2. Considerații și perspective 58

BIBLIOGRAFIE 60

ANEXĂ. Cod sursă 62

Irina Diana Vidrașcu LISTA FIGURILOR

LISTA FIGURILOR

Figura Numele figurii Pag.

0.1 Sistem de navigaţie inerţial [4]

2

0.2 Grafic Gantt al proiectului 3

1.1. Sistem de referință raportat la Pământ [4]

4

1.2. Sistem de referință local [4]

4

1.3. Sistemul de referință global și cel atașat vehiculului. [1]

5

1.4. Sistem de referință vehicul (dronă) – direcții de mișcare [4]

6

1.5. Articulații cardanice [24]

6

1.6. Platformă stabilă (cu articulații cardanice) [1]

7

1.7. Algoritmul navigației inerțiale al unei platforme stabile [1]

7

1.8. Algoritmul navigației inerțiale al unui sistem strapdown [1]

8

1.9. Schemă sistem “strapdown” cu explicitarea transformării axelor [1]

9

1.10. Componente MEMS [22]

10

1.11. Plăcuță MEMS [21]

10

1.12. Structura unui accelerometru clasic [4]

11

1.13. Componența accelerometrului de tip SAW [1]

12

1.14. Variantă a accelerometrului cu două grinzi [4]

12

1.15. Giroscop mecanic convențional [1]

13

1.16. Efectul Sagnac [1]

14

1.17. Descriere RLG [4]

15

1.18. Componente FOG [4]

15

1.19. Accelerația Coriolis [1]

16

1.20. Structura [5]

17

1.21. Legătura senzori - 17


1.22. Schemă principiu PID 19

1.23. Exemplu de structură hibridă de optimizare geno-fuzzy/ geno-neuro

cu Ik=indice de performanță [6]

20

2.1. Structură montaj 22

2.2. Dispozitiv MEMS ce oferă o imagine a structurii de elemente

miniaturizate [7]

22

2.3. Detaliu structură accelerometru MEMS [8]

23

2.4. Accelerometrul ADXL345 Breakout

23

2.5. Schemă conectare la microcontroler [9]

24

2.6. Axele de rotație ITG-3200 Breakout [16]

25

2.7. ITG-3200 utilizat 25

2.8. Conectare ITG-3200 Breakout la microcontroler [9]

26

2.9. Vedere de sus (a) Vedere spate (b) Arduino Duemilanove 27

2.10. Mediu de dezvoltare Arduino IDE 28

2.11. Realizare aplicație Arduino IDE 29

2.12. Funcționalități implementate cu ajutorul Arduino IDE 29

2.13. Power HD 1501 MG Analog Servo 30

2.14. Angrenajul de roți dințate al servomotorului [30]

30

2.15. Fire conexiune servomotor 31

2.16. Servomotor control direcție “yaw” 32

2.17. Cuplaj montare servo pentru direcția “pitch”(a) ; direcția “roll” (b) 32

2.18. Cameră video FCB-EX480CP (sony.ro)

33

2.19. Camera în ansamblul dezvoltat 33

2.20. Schemă filtru complementar [14]

34

2.21. Algoritm filtru Kalman 37

2.22. Model 3D al componentei rotative a montajului 37

2.23. Vedere din spate montaj – simulator 3D 38

2.24. Montare la unison a senzorilor 38


2.25. Cabluri tata-tata folosite (a) Rezistență 4.7K (b) 39

2.26. Limitări ale servomotoarelor 40

2.27. Senzori și Arduino (a) Montare pe cadru (b) 40

2.28 Schemă logică a fluxului procesului 41

3.1. Reprezentarea valorilor citite pe axele (XYZ) ale accelerometrului 44

3.2. Mișcare aleatoare axa X 45

3.3. Mișcare aleatoare axa Y 46

3.4. Mișcare aleatoare axa Z 46

3.5. Mișcari progresive spre stânga 47

3.6. Mișcări progresive spre dreapta 47

3.7. Mișcare diagonală în plan orizontal 48

3.8. Mișcare pe direcția verticală 48

3.9. Efect filtru Kalman 49

3.10. Reprezentarea valorilor celor 3 axe (XYZ) ale giroscopului 49

3.11. Mișcare aleatoare pe axa X a giroscopului 50

3.12. Mișcare aleatoare pe axa Y a giroscopului 51

3.13. Mișcare aleatoare pe axa Z a giroscopului 51

3.14. Rotire spre dreapta în plan orizontal 52

3.15. Rotire spre stânga în plan orizontal 52

3.16. Rotire progresivă în jurul axei X 53

3.17. Rotire progresivă în jurul axei Y 53

3.18. Zona activă de test 54


3.19. Evoluția comenziilor celor 3 motoare funcție de timp 55

3.20. Evoluție pe perioadă dublă de timp 55

3.21. Evoluție funcție de timp (urmărire “yaw”) 56

3.22. Schemă bloc de reglare 56

Irina Diana Vidrașcu INTRODUCERE

1

INTRODUCERE

Această lucrare vizează un domeniu de actualitate, căruia i se acordă din ce în ce mai

multă importanţă, şi anume preluarea informaţiilor din mediul extern. Acest aspect este

deosebit de important în tot ceea ce înseamnă tehnologie şi industrie actuală, deoarece orice

proces necesită achiziţia datelor ca prim pas al prelucrării, reglării sau conducerii unui proces

automat.

Contextul actual în care poate fi încadrată tema licenţei este unul mai amplu şi

anume cel al sistemelor inerţiale de navigaţie cărora li se alătură şi aplicaţiile de supraveghere

video de la mare distanţă. Tot acest ansamblu îşi are aplicabilitatea în domenii diverse precum

topografie – geodezie, supravegherea unor zone sensibile (aeroporturi, frontiere), etc. Astfel,

echiparea unor aparate de zbor tip drone UAV (unmanned aircraft vehicle) cu canale video

poate reprezenta o opţiune viabilă în soluţionarea unor misiuni de securitate, monitorizare,

recunoaştere [17].

Progresul continuu al materialelor, senzorilor, al procesării hardware, al sistemelor

de propulsie şi al tehnologiilor software transformă ideea unui UAV performat şi foarte exact

într-una fezabilă.

Obiectivele vizate în cadrul lucrării se axează pe studiul circuitelor

microprogramabile, studiul sistemelor de reglare si dezvoltarea unui montaj practic care să

răspundă cerinţelor formulate. Se va proiecta şi dezvolta un sistem de stabilizare cu trei grade

de libertate de rotaţie (3R) pentru o platformă mobilă în cadrul căreia este amplasată o cameră

foto-video.

Scopul final al aplicaţiei este menţinerea unei direcţii de vizare, menţinerea vizării

spre o ţintă localizată pe sol şi minimizarea efectelor perturbatoare ale mişcărilor aeronavei.

Se va ţine cont, pe întreaga dezvoltare a proiectului, de faptul că platforma urmează să fie

instalată la un moment dat pe un sistem de tip UAV.

Aspecte generale privind sistemele inerţiale de navigaţie (INS)

Un sistem de navigaţie reprezintă o metoda de determinare a poziţiei şi vitezei în

mod automat, putând include şi determinarea altitudinii, acceleraţiei şi a vitezei unghiulare.

Acestea pot fi integrate complet (INS) în cadrul vehiculului sau pot avea componente

exterioare [1].

Principiul de funcţionare al acestor sisteme se bazează pe legile mecanicii clasice

definite de Newton, fiind singurele sisteme de navigaţie care nu folosesc referinţe externe.

Astfel, măsurarea acceleraţiei duce la obţinerea vitezei prin integrare. La rândul ei, viteza

integrată matematic oferă poziţia vehiculului. Dacă presupunem cunoscute poziţia iniţială şi

viteza de deplasare, se poate determina poziţia vehiculului la orice moment de timp t, după

cum urmează:

Cea mai comună referinţă folosită de aceste sisteme este Pământul, considerându-se

un sistem de coordonate pe trei axe la care se va raporta sistemul UAV-ului în cauză.


2

Sistemele de navigaţie inerţială sunt sisteme tridimensionale care indică în timp real

poziţia şi viteza unui vehicul utilizând seturi de date provenite de la senzori (IMU – unităţi

inerţiale de măsurare). Există mai multe categorii de sisteme INS, ele diferenţiindu-se prin

sistemul de referinţă adoptat. Astfel, în cadrul lucrării se va utiliza ca sistem de referinţă local,

vehiculul şi ca sistem de referinţă global, sistemul de navigaţie. Menţionarea sistemului de

referinţă precizează practic faţă de ce sistem se vor raporta măsuratorile făcute cu ajutorul

senzorilor [1].

Principalii senzori folosiţi de aceste sisteme sunt reprezentaţi de accelerometre şi

giroscoape, dar şi magnetometre. Ieşirile senzorilor vor determina poziţia, viteza şi altitudinea

vehiculului. Giroscoapele măsoară viteza unghiulară, iar accelerometrele oferă acceleraţia

datorată tuturor forţelor, cu excepţia forţei gravitaţionale a Pământului.

Figura 0.1 Sistem de navigaţie inerţial

Avantajele cele mai importante ale utilizării acestor sisteme INS sunt achiziţia

permanentă a parametriilor, folosirea unei soluţii de navigaţie cu bandă de frecvenţă înaltă

(minim 50Hz), introducerea unor zgomote de scurtă durată.

Însă, dezavantajul în cazul acestor sisteme îl reprezintă degradarea acurateţei de

navigaţie în timp şi dependenţa preciziei de costul senzorilor folosiţi. Apar cumulări ale

erorilor provenite de la măsurătorile accelerometrelor şi giroscoapelor, erori ce se propagă în

special prin intermediul feedback-ului dat de modelul gravitaţional.

Performanţele obţinute pot varia cu câteva ordine de mărime importante, depinzând

foarte mult de calitatea senzorilor inerţali folosiţi. Spre exemplu, sistemele INS folosite de

liniile aeriene comerciale ajung să coste 100,000 €, în timp ce sistemele folosite de avioane

uşoare, arme ghidate, etc. sunt mult mai ieftine, dar oferă performanţe mai slabe cu

aproximativ două ordine de mărime. Industria automobilă este cea pentru care sunt folosiţi

senzorii inerţiali cu cele mai slabe performanţe (de şase ori mai mici decît în industria

marină), aceştia fiind utilizaţi în mod individual, şi nu integraţi ca unităţi IMU. Pentru a

suplini această deficienţă, sunt utilizate laolaltă mai multe tipuri de sisteme de navigaţie (ex:

GNSS – global navigation satellite system) [4].

Navigaţia inerţială este folosită de o serie variată de aplicaţii din industria

aeronautică, în misiuni spaţiale, în navigaţie marină şi submarină. Progresul rapid al

echipamentelor utilizate în sistemele de navigaţie inerţiale permite, mai nou, folosirea

acestora în aplicaţii ce ţin de domeniul mişcării umane şi animale. Senzorii inerţiali (tip

MEMS - micro-machined electromechanical systems) sunt mult mai mici în dimensiuni şi

greutate, permiţând aplicaţii mai minuţioase.

Procesor

Poziţie

Viteză unghiulară

Model gravitaţional

IMU

Accelerometru

Giroscop

Condiţii


3

Structura lucrării de licenţă

Având în vedere obiectivele menţionate anterior, prin prezenta lucarare a fost

realizată dezvoltarea şi implementarea platformei de fotografiere aeriană, fără a fi montată pe

un sistem tip UAV. De altfel, testele au fost realizate utilizând un stand experimental.

Derularea proiectului a avut necesitat lucru susținut și continuu de-a lungul perioadei

de întocmire, realizare și implementare. Pentru a detalia mai bine activitatea, împărțirea pe

task-uri și perioade precise de timp, s-a elaborat un grafic Gantt în care se pot remarca

principalele etape ale proiectului, precum și subtask-urile incluse.

Figura 0.2 Grafic Gantt al proiectului

Lucrarea de față este structurată pe 5 capitole, acestea acoperind atât consideraţiile

teoretice specifice temei alese, cât și descrierea amănunțită a aplicației şi a montajului

dezvoltat. Împărțirea pe capitole a fost făcută astfel încât se pornește de la scopul și

obiectivele propuse, se prezintă suportul teoretic necesar înțelegerii proiectului și se detaliază

platforma dezvoltată, precum și rezultate obținute în urma efectuării unor teste.

Capitolul întâi conţine partea teoretică a lucrării în cadrul căreia sunt prezentate date

generale despre senzorii MEMS, evoluţia acestora şi tipurile de senzori folosiţi în sistemele

inerţiale de navigaţie. Tot în acest capitol este realizat un sumar al microcontrolerelor utilizate

în aplicaţii asemănătoare sau folosite în domenii conexe UAV-urilor. Ultima secţiune a

capitolului I vizează algoritmii de reglare specifici, variante viabile în cadrul aplicaţiei mele.

Capitolul II detaliază echipamentele folosite în dezvoltarea aplicaţiei prin prezentarea

senzorilor folosiţi, a microcontrolerului, camerei video, motoarelor şi a platformei în

ansamblul ei. Tot aici sunt precizate şi principiile de funcţionare ale sistemului, modalitatea

de realizare a achiziţiei de date, prelucrarea acestora, dar şi proiectarea comenzii şi

implementarea regulatorului ce va asigura performanţele impuse.

Capitolul III prezintă rezultatele obţinute în urma unor teste, rezultate prezentate

comparativ ţinând cont de gradul de prelucrare al datelor achiziţionate (datele brute, datele

filtrate) şi răspunsul sistemului la anumite tipuri de perturbaţii.

În cadrul capitolului IV sunt expuse concluziile acestei lucrări, modul de realizare al

proiectului, precum şi perspectivele pe care tema studiată le oferă.

Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

4

CAPITOLUL I

CONSIDERAŢII TEORETICE

Sistemele inerţiale (“inertial measurement systems”) sunt capabile să detecteze

acceleraţii corespunzătoare axelor sistemului de referinţă faţă de care se raportează. Prin

integrarea acceleraţiei se obţine viteza, iar printr-o a doua integrare se obţine poziţia

vehiculului de-a lungul axei accelerometrului.

Sistemele inerțiale au în componența lor următoarele elemente:

accelerometre și giroscoape, de obicei grupate sub formă de unități IMU;

componente electronice auxiliare;

procesor de navigație ce are drept scop calcularea accelerației și dubla

integrare a acesteia.

Acesta este structura cea mai uzuală folosită de sistemele de navigaţie inerţiale, dar

trebuie ținut cont de sistemul de referință cu care se lucrează, precum și de tipul sistemului

inerțial.

1.1 Sisteme de referință

Pentru a obține informații cât mai exacte este foarte importantă alegerea sistemul de

referință cu care se va lucra. În general, se optează pentru un sistem cartenzian de coordonate

cum ar fi:

sistem de referință inerțial – are originea în centrul Pământului, respectă

legile mișcării definite de Newton și nu accelerează, nici nu se poate roti;

sistem de referință al Pământului – are originea în centrul Pământului, iar

axele sunt fixate în raport cu Pământul așa cum este prezentat în figura 1.1

după [4];

Figura 1.1 Sistem de referință raportat la Pământ

sistem de referință al navigației (local) – are origine locația în care se găsește

sistemul de navigație, iar axele sunt conform cu direcțiile N, E și vertical;

pentru a înțelege mai bine este redat un exemplu prin figura 1.2 după [4];

Figura 1.2 Sistem de referință local


5

sistem de referință al azimutului – este folosit pentru a evita singularitățile în

urma calculelor numerice ce apar la polii sistemului de navigație;

sistem de referință al vehiculului – este un sistem local, atașat vehiculului, ale

cărui axe sunt aliniate cu direcțiile de tracțiune, greutate și portanță (roll, yaw,

pitch) al sistemului de navigație [4].

În prezenta lucrare, se consideră sistem de referință cel al vehiculului, iar sistemul în

care se face navigația este sistemul de referință global. După alegerea referinței, se consideră

că toate valorile măsurate sunt conforme cu axele vehiculului. Trebuie menționat faptul că

ecuația de navigație poate fi rezolvată în oricare dintre sistemele de referință menționate

anterior [1]. Pentru exemplificarea sistemului de referință folosit s-a utilizat figura 1.3 [1].

Figura 1.3 Sistemul de referință global și cel atașat vehiculului

1.2 Tipuri de sisteme inerțiale

La sistemele de navigație inerțiale apare o problemă esențială generată de

accelerometrele din componența sistemului ce nu sunt capabile să facă diferența dintre

accelerația vehiculului și accelerația gravitațională. Astfel, această problemă poate fi

soluționată prin două modalități diferite ce delimitează, practic, și tipurile de sisteme inerțiale.

Astfel, avem sisteme:

tip platformă stabilă(“stable platform systems”) sau cu dublă/triplă articulație

cardanică – accelerometrul este menținut în poziție orizontală pentru a nu

sesiza vectorul ;

tip integrat (“strapdown systems”) – reținerea unghiului dintre axa

accelerometrului și cea a vectorului gravitație, precum și scăderea

componentei vectorului [1].

În continuare voi face referire la aceste două tipuri de sisteme, cu accent pe cel ce

încadrează tehnologia folosită de platforma dezvoltată în cadrul licenței, „strapdown” system.

INS

INS

Zb Zg

yb

yg

xb

xg


6

1.2.1 Platformă stabilă

Acest tip de sistem este cel mai vechi dintre cele două, fiind bazat pe o platformă cu

dublă articulație cardanică. Principalele probleme care se ridică sunt izolarea platformei de

mișcăriile de rotație ale vehiculului (dronei), schimbarea permanentă a vectorului gravitație

odată cu poziția și, în timp, din cauza suprafeței sferice a Pământului și a rotației acestuia, în

jurul propriei axe.

Cele trei axe ale dronei pe care au loc mișcările sunt enumerate mai jos, având și o

reprezentare prin figura 1.4 după [4]:

roll (tracțiune) – axa ce unește punctele extreme de pe lungimea aeronavei;

yaw (greutate) – axa verticală;

pitch (portanță) – axa ce unește linia aripilor aeronavei.

Figura 1.4 Sistem de referință vehicul (dronă) – direcții de mișcare

În cadrul sistemelor cu articulație cardanică, senzorii inerțiali sunt montați pe o

platformă specială ce este izolată de orice mișcare de rotație exterioară, platforma fiind

aliniată astfel sistemul de referință global. Acest lucru este posibil prin folosirea unor cadre

cardanice ce permit trei grade de libertate platformei, așa cum este ilustrat și mai jos [3].

Cadrele cardanice sunt montate unul în interiorul celuilalt, fiecare dintre cele trei

izolând axa respectivă așa cum se poate vedea în figura 1.5 după [24].

Figura 1.5 Articulații cardanice

Cele mai uzuale platforme conțin 6 senzori (3 accelerometre și 3 giroscoape) montați

perpendicular unii pe ceilalți (3 axe, fiecăreia corespunzându-i 2 senzori, unul pentru

măsurarea accelerației, celălalt pentru măsurarea vitezei unghiulare). Giroscoapele detectează

mișcările de rotație ale platformei, trimițând feedback servomotoarelor ce vor acționa

articulațiile cardanice pentru a anula aceste mișcări și a menține platforma aliniată cu sistemul

de referință global. Un exemplu este prezentat în figura 1.6 după [1].


7

Figura 1.6 Platformă stabilă (cu articulații cardanice)

Așa cum se poate vedea în reprezentarea din figura 1.7 după [1], pentru a afla

orientarea vehiculului se folosesc unghiurile dintre axurile cardanice. Pentru a calcula poziția

vehiculului, semnalul accelerometrului este integrat de două ori, cu mențiunea că este

necesară eliminarea componentei accelerației datorată gravitației.

Figura 1.7 Algoritmul navigației inerțiale a unei platforme stabile

Acest tip de sistem inerțial este încă în uz și este folosit pentru aplicații ce necesită un

grad foarte ridicat de acuratețe a datelor de navigație estimate cum ar fi industria navelor și

submarinelor, deoarece minimizează erorile provenite de la mișcările vehiculului gazdă (în

cazul ales, drona este vehiculul gazdă pentru o astfel de platfomă). Mai mult, reduce

încărcarea procesorului (microcontroler-ului folosit), putând fi implementat chiar și calcul

analogic.

1.2.2 Sistem tip “strapdown”

În sistemele de tip “strapdown” senzorii inerțiali sunt fixați pe corpul vehiculului,

ceea ce face ca parametrii măsurați să fie raportați la sistemul de referință al vehiculului

(dronei), și nu la cel global.

Pentru a stabili orientarea dispozitivului, semnalul primit de la giroscop este integrat.

În ceea ce privește stabilirea poziției, semnalul accelerometrelor este transpus în coordonate

globale folosind orientarea deja stabilită de integrarea semnalelor giroscoapelor. Apoi,

algoritmul urmează aceeași dublă procedură de integrare ca și platforma stabilă, obținându-se

în final poziția dispozitivului conform cu figura 1.8 după [1].


8

Figura 1.8 Algoritmul navigației inerțiale al unui sistem strapdown

Acest tip de sistem este unul analitic și folosește implementare numerică. Este

necesară reținerea unghiului dintre axa accelerometrului și cea a vectorului gravitație, precum

și scăderea componentei vectorului . Ieșirile accelerometrelor sunt proporționale cu

cosinusul vectorului gravitație.

Astfel, dacă se consideră:

Unde se cunosc ax, ay, az:

(1.2)

Și se află unghiurile de roll și pitch, respective tracțiune și portanță.

În figura 1.9 după [1], se observă în mod exact axele folosite de cele două

echipamente și modul lor de transformare pentru a obține în final poziția și viteza vehiculului.


9

Figura 1.9 Schemă sistem “strapdown” cu explicitarea transformării axelor

Cele două tipuri de sisteme inerțiale folosesc, în linii mari, aceleași principii de

funcționare, dar diferă prin felul în care sunt montați senzorii și sistemele de referință alese

față de care se raportează măsuratorile parametrilor.

Sistemele tip “strapdown” reduc foarte mult complexitatea mecanică, sunt mult mai

mici în privință dimensiunilor și chiar mai ieftine, lucru datorat costurilor mici necesare

calculului numeric.

1.3 Senzori MEMS

Mărimea, performanțele și costurile senzorilor inerțiali pot varia cu câteva ordine de

mărime, funcție de tehnologia de producere pe care o utilizează. În cadrul proiectului sunt

folosiți senzori inerțiali, astfel încât sunt necesare câteva detalii cu privire la această

tehnologie.

În prezent, accentul în dezvoltarea acestor senzori este pus pe tehnologia MEMS sau

“micro-electromechanical systems”. Această tehnică folosește mecanisme de dimensiuni de

ordinul micronilor, capabile să execute mișcări foarte precise. Adesea aceste mecanisme sunt

acționate fie de forțe electro-magnetice, fie de energie chimică sau luminoasă [27]. MEMS-

urile sunt sisteme integrate foarte mici (de ordinul micronilor sau milimetrilor) având în

componență elemente mecanice și electrice.

Fabricarea lor este asemănatoare cu cea a circuitelor integrate, dar sunt proiectate în

maniera în care pot fi exploatate atât proprietățile electrice, cât și cele mecanice ale

semiconductorului siliciu.

Pe lângă structurile miniaturizate componente, găsi și microsenzori și microactuatori

responsabili pentru conversia unui semnal mecanic într-unul electric (cel mai frecvent).

Tehnologia MEMS își atinge potențialul odată cu îmbinarea componentelor sale

laolaltă cu circuite integrate. În timp ce electronicele sunt fabricate folosind secvenţele

procesului circuitelor integrate (CI) (de exemplu, CMOS, bipolar, etc), componentele

micromecanice sunt fabricate folosind procese "microtehnologice" compatibile care

îndepărtează în mod selectiv părţi din plăcuța de siliciu sau adaugă noi straturi structurale

pentru a forma dispozitive mecanice şi electromecanice.


10

Componentele sistemelor MEMS sunt menționate în figura 1.10 după [22]:

Figura 1.10 Componente MEMS

Perspectiva MEMS prin care microsenzorii, microactuatorii, microelectronicele şi

alte tehnologii, ar putea fi înglobate într-un singur microchip, reprezintă viitorul acestui

domeniu al tehnologiei MEMS. Acest lucru va permite dezvoltarea produselor inteligente

sporind capacitatea de calcul a microelectronicelor. În figura 1.11 după [22] este prezentată o

plăcuță MEMS realizată practic.

Figura 1.11 Plăcuță MEMS

Nanotehnologia este des asimilată cu tehnologia MEMS, dar ea reprezintă abilitatea

de a manipula materia la nivel atomic sau molecular pentru a face ceva util, la scară nano-

dimensională. Avantajele dispozitivelor nano-dimensionale față de MEMS implică beneficii

derivate în special din legile de scalare. Deşi MEMS şi nanotehnologia sunt uneori citate ca

tehnologiile separate şi distincte, în realitate distincţia dintre cele două nu este atât de clară.

De fapt, aceste două tehnologii sunt extrem de dependente una de alta [28].

În prezent, cea mai importantă în aplicaţiile MEMS este abilitatea de a face un

dispozitiv de dimensiuni microscopice, sau de a crea un nou dispozitiv, care nu ar funcţiona

dacă ar avea dimensiuni de câțiva centimetri, dar care funcţionează bine la scară micro [28].

Tehnologia MEMS este deseori utilizată pentru a produce senzori, în special

accelerometre și giroscoape, așa cum este și cazul de față, oferind acestor avantajele unor

costuri scăzute, dimensiuni reduse și toleranță ridicată la șocuri, dar cu performanțe mai slabe.

Micro-

senzori

Componente

microelectronice

Micro-

actuatoare

MEMS

Structuri

miniaturizate


11

1.3.1 Accelerometru

Generic, acceletometrele sunt aparate cu care se măsoară accelerațiile, în special la

vehiculele aeriene sau mai specific un transductor electromecanic de măsurat structurile

vibrante (conform DOOM 2005).

Sistemele inerțiale de navigație se bazează pe măsurarea accelerației în scopul

obținerii vitezei și poziției vehiculului în urma integrării matematice succesive.

Accelerometrele măsoară atât forța inerțială, cât și componenta gravitațională.

Principiul lor de funcționare se bazează pe o structură de masă ce se deplasează liber

în interiorul unei carcase de-a lungul axelor de sensibilitate ale accelerometrului, structură

prinsă cu arcuri. Corpul senzorului (cunoscut în literatura ca “pickoff”) măsoară poziția masei

inerțiale relativ la carcasa accelerometului. Atunci când este aplicată o forță de tracțiune ce

modifică accelerația, masa inerțială va continua deplasarea cu aceeași viteza, în timp ce

carcasa se va deplasa comprimând unul dintre arcuri și diltându-l pe celălalt. Poziția rezultată

a masei este proporțională cu accelerația aplicată carcasei, excepție făcând accelerația

gravitațională ce acționează direct asupra masei inerțiale (și nu prin intermediul arcurilor).

Astfel, se poate trage concluzia că accelerometrele sunt capabile să măsoare accerelația

negravitațională (ceea ce se datoreză unei forțe specifice imprimate dispozitivului).

Figura 1.12 Structura unui accelerometru clasic

Structura prezentată în figura 1.12 după [4] este incompletă deorece masa inerțială

trebuie să aibe suport și pe axele perpendiculare, pe axele de sensibilitate ale

accelerometrului, necesară fiind și amortizarea oscilațiilor masei, însă acest principiu de

funcționare stă la baza tuturor accelerometrelor.

Accelerometrele folosite în sistemele de tip “strapdown” pot fi:

tip pendul (mecanice);

tip rezonant;

tip MEMS.

Accelerometrele tip pendul sunt compuse dintr-o masă suspendată cu ajutorul unor

arcuri, ca în figură. Deplasarea masei este măsurată prin intermediul unui senzor (“pickoff”)

de deplasare care oferă la ieșire un semnal proporțional cu forța F aplicată masei. Pentru a

calcula accelerația propriu-zisă este utilizat al II-lea principiu fundamental al mecanicii,

.

Accelerometrele de tip rezonant pot fi la rândul lor împărțite în funcție de suprafața

rezonantă folosită :

unde acustice de suparafață (SAW – surface acoustic wave);

vibrante;


12

ce utilizează siliconul;

ce utilizează cuarțul.

Spre exemplu, accelerometrul de tip SAW, ilustrat mai jos în figura 1.13 după [1],

conține o grindă încastrată în consolă care rezonează la o anumită frecvență. Un capăt al

grinzii este fixat de carcasă, în timp ce cel de-al doilea capăt are atașată masa inerțială.

Figura 1.13 Componența accelerometrului de tip SAW

Atunci când este aplicată o accelerație pe direcția axelor, grinda începe să se îndoaie.

Comprimarea grinzii reprezintă scăderea frecvenței, în timp de dilatarea (întinderea)

reprezintă creșterea frecvenței. Astfel, măsurând frecvența cu care grinda rezonează, se poate

determina forța ce acționeză de-a lungul axelor de sensibilitate ale acceleronetrului.

Performanțele obținute pot fi îmbunătățite dacă se folosește o pereche de grinzi astfel

încât, în timp ce una se comprimă, cealaltă se dilată. În această situație se pot folosi fie una,

fie două mase inerțiale. Un astfel de accelerometru este prezentat în figura 1.14 după [4].

Figura 1.14 Variantă a accelerometrului cu două grinzi

Accelerometrele de tip MEMS folosesc aceleași principii ca și cele anterioare,

diferită fiind tehnologia de producere. Conțin tot un chip de silicon pe care sunt montate

senzorul și structura sensibilă. Este fabricat în totalitate din silicon și este compus din două

părți: masa inerțială ce este suspendată prin intermediul câte unui arc montat la fiecare capăt

și o pereche de electrozi sensibili care sunt capabili să detecteze mișcarea masei inerțiale

relativ la platforma de silicon. Atunci când chipul este supus unei accelerații, masa inerțială se

deplasează relativ la chip și la restul structurilor fixe din componența accelerometrului.

Amplitudinea mișcării depinde de accelerația imprimată, rigiditatea arcurilor și masă. Atunci

când se detecteză o deviație a masei, se induce o diferență de potențial între aceasta și

structurile sensibile ale chipului.


13

Această structură prezentată mai sus se dublează sau triplează în funcție de numărul

de axe ale accelerometrului, pe fiecare dintre axe fiind posibilă determinarea unei accelerații.

1.3.2 Giroscop

Giroscopul este un aparat care, antrenat de o mișcare de rotație în jurul uneia dintre

axe, se poate deplasa astfel încât să nu modifice direcția axei sale de rotație, iar o a doua

conotație a termenului ar fi de dispozitiv ce asigură stabilitatea și orientarea în spațiu a unui

avion sau submarin (conform DOOM 2005).

Inițial, domeniul giroscopului făcea referire numai la cele ce implicau rotația unei

mase inerțiale, extinzându-se ulterior și incluzând toți senzorii ce măsoară viteze unghiulare și

nu necesită un sistem de referință extern.

Giroscoapele pot fi împărțite în trei categorii principale:

mecanice (care la rândul lor sunt de mai multe tipuri);

optice;

vibrante (MEMS).

Astfel, giroscoapele mecanice sunt de mai multe feluri, dar cel mai des întâlnite sunt

cele ce se bazează pe un rotor aflat în interiorul a două cadre cardanice ce permit rotația pe

toate cele trei axe ca în figura 1.15 după [1]. Are loc conservarea orientării ce se traduce prin

conservarea poziției rotorului atunci când se schimbă orientarea.

Practic, când are loc o rotație, rotorul își păstrează orientarea față de sistemul de

referință global, modificându-se de fapt unghurile dintre cardane. Giroscoapele conveționale

măsoară orientarea, în timp ce cele moderne măsoară viteza unghiulară.

Principalul dezavantaj al giroscoapelor mecanice îl reprezintă existența părților

mobile ce cauzează fricțiune ducând la devieri în timp. De asemenea, ele necesită un anumit

timp pentru a fi puse în funcțiune, ceea ce poate fi un inconvenient în multe situații.

Figura 1.15 Giroscop mecanic convențional

Giroscopul optic, a cărui construcție se bazeză pe o observație a lui Geroges Sagnac

din anul 1913, funcționeză pe principiul conform căruia într-un mediu dat, viteza luminii este

constantă.

Efectul Sagnac, reprezentat grafic în figura 1.16 după [1], care stă la baza

funcționării acestui tip de giroscop, constă în apariția unei diferențe de fază între două


14

fascicule coerente de lumină, care se propagă în sensuri opuse printr-un inel de fibră optică

sau de oglinzi aflat în rotație. În figura de mai jos, este schematizat acest efect. Linia punctată

reprezintă traiectoria fasciculului pe direcția de rotație, în timp ce linia normală reprezintă

traiectoria fasciculului în sens contrar rotației. De asemenea, unghiul simbolizează unghiul

de rotație al giroscopului cât timp fasciculul acționează.

Figura 1.16 Efectul Sagnac

Avantajul folosirii giroscopului optic constă în faptul că acest nu are părți mobile și

necesită un timp foarte scurt pentru punerea în funcțiune. Precizia datelor oferite este

dependentă de lungimea de undă a radiației utilizate ce poate suferi constrângeri din cauza

dimensiunilor dispozitivului.

Giroscoapele optice se împart funcție de mediul folosit pentru inelul de propagare:

giroscop laser inelar;

giroscop cu fibră optică.

Giroscopul tip laser inelar (RLG – ring laser gyroscope), ilustrat în figura 1.17 după

[4], are la bază același principiu Sagnac, dar se consideră că fasciculul de lumină este

direcționat cu ajutorul unor oglinzi pentru a forma bucla închisă. Există o cavitate activă a

laserului ce are cel puțin trei canale sub forma unui triunghi echilateral care sunt umplute cu

un amestec de gaz He-Ne. Sursa laser genereză două fascicule coerente ce se propagă în

sensuri opuse. Anodul și catodul sunt folosiți pentru a crea o diferență de potențial a

amestecului gazos, generând câmp electric. Pentru a putea măsura și viteze unghiulare foarte

mici, RLG-urile au implementate și o roată oscilantă („dither wheel”) [19].


15

Figura 1.17 Descriere RLG

Giroscopul cu fibră optică (FOG – fiber optic gyroscope), ilustrat în figura 1.18 după

[4], folosește interferența luminii pentru a măsura viteza unghiulară. Acesta conține o buclă

de fibă optică (cerc). Sursa de lumină genereză un fascicul ce este divizat în două fascicule

coerente de sens opus. Viteza unghiulară este determinată conform principiului Sagnac.

Modulatorul de fază are drept scop introducerea unui defazaj, astfel încât fasciculele ce ajung

la detector, folosind cele două traiectorii, să poată fi distinse.

FOG-urile prezintă avantajul costului și al faptului că sunt robuste, fiind folosite cu

preponderență în sistemele ce nu necesită performanțe ridicate. Creșterea lungimii fibrei

determină creșterea preciziei, dar scăderea semnalului de ieșire al giroscopului.

Figura 1.18 Componente FOG

Giroscopul MEMS folosește tehnica micro-structurilor de silicon. Principiul de

funcționare se bazeză pe efectul Coriolis. Acesta presupune că, dat fiind un sistem de referință

ce se rotește cu viteza unghiulară ω, având în componență o masă inerțială m ce se deplaseză

cu viteza , forța ce apare este definită conform relației:

(1.4)

Giroscopul MEMS conține elemente vibrante ce măsoara efectul Coriolis. Acestea

pot fi corzi, grinzi, roți, inele sau cilindre vibrante. Ele sunt capabile să detecteze accelerația

Coriolis atunci când giroscopul se rotește. Cel mai simplu exemplu în reprezintă o masă

inerțială ce începe să vibreze de-a lungul axei pe care se deplaseză.

Când giroscopul este rotit, se induce o a doua vibrație pe axa perpendiculară cu

elementul, datorată accelerației Coriolis ca în figura 1.19 după [1].


16

Figura 1.19 Accelerația Coriolis

Acuratețea și precizia datelor oferite în prezent de giroscoapele MEMS este

inferioară celor optice, dar ele ofera avantajele oferite de tehnologia MEMS, și anume cost,

dimensiune, putere consumată.

1.4 Microcontrolere ( )

În ceea ce privește partea de procesare ce trebuie realizată în cadrul unui astfel de

proiect, se consideră alegerea unui microcontroler potrivit. Un microcontroler reprezintă un

chip ce integrează un microprocesor împreună cu dispozitivele periferice.

Cele mai importante aspecte sunt dimensiunea redusă, costul mic, consumul redus de

energie și faptul că permite stocarea datelor unui program, precum și rularea programului

local, fără alte componente adiționale. Structurile cele mai importante ale unui sunt

următoarele conform și cu schema 1.20 după [5]: unitatea centrală de procesare (µP core) cu o arhitectura care poate fi pe 8, 16,

32 sau 64 de biți;

memorie de date RAM (volatilă) sau Flash/EEPROM (nevolatilă) pentru date

sau program;

porturi digitale de intrare-ieșire;

interfețe seriale (RS232, SPI, I2C, CAN, RS485);

timere, generatoare de PWM sau watchdog;

convertoare analog-digitale/ digital-analogice;

suport pentru programare și debugging [26].

În alegerea unui microcontroler trebuie avute în vedere următorele aspecte:

limbajul de programare;

tipul de chip folosit;

configurația necesară;

prețul.


17

Figura 1.20 Structura

Limabjul utilizat trebuie să țină cont de competențele programatorului, dar și de

necesitățile pe care le implică proiectul. Astfel, se pot alege microcontrolere al căror mediu de

programare este de nivel mai înalt (C/C++, Basic) sau dacă experiența permite, lucrul cu

medii de programare “low-level” în limbaj de asamblare. Se poate opta, de asemenea, pentru

folosirea unui chip conținând numai microcontroler-ul sau cel al unei plăci “starter board”.

Diferența între cele două este făcută de faptul că micrcontroler-ul necesită componente

auxiliare, în timp ce plăcile “starter board” includ câteva funcționalități de conectare,

alimentare, LCD-uri, controlere de motoare.

În ceea ce privește configurația, alegerea unui microcontroler trebuie aibă în vedere

necesarul de ieșiri/intrări digitale și analogice, putere de alimentare pe care o oferă acesta,

convertoare ADC / DAC (analog-to-digital/ digital-to-analog), tipul de comunicație folosit.

Prețul poate varia foarte mult funcție de criteriile menționate, dar și de performanțele pe care

le dorim. Un factor de luat în seama este timpul de răspuns al microcontroler-ului, foarte

important în aplicații ale industriei aeronautice, ca și cea de față.

Cele mai folosite microcontrolere sunt cele produse de MicroChip (gama PIC),

Atmel (AVR), Zilog, ARM, Propeller.

În reprezentarea figurii 1.21, se poate vedea legătura pe care o asigură

microcontroler-ul într-un sistem de tip UAV. Acesta efectuează întreaga procesare după ce au

fost achiziționate date de la senzori, înglobează partea de reglare a unor variabile și calculează

comenziile optime ce urmeză a fi transmise către elementele de execuție ale sistemului.

De cele mai multe ori, în schema prezentată este inclusă și partea de comunicație

wireless cu dispozitivul, dar această parte nu va fi tratată în prezenta lucrare.

Figura 1.21 Legătura senzori -

Microcontroler/ PC

Elemente de

excuție (Servomotoare)

Senzori

Software


18

În aplicații tip UAV, există câteva aspecte ce trebuie considerate atunci când se

optează pentru un anumit model de microcontroler, și anume existența unor caracteristici

hardware care să simplifice interfațarea sistemului cu senzorii și elementele de execuție. Aici

pot fi incluse convertoarele, timere-le, canale de modulație în durată a impulsurilor (PWM).

Câteva exemple de proiecte ce au ca tematică UAV-urile pot indica tipurile de

microcontrolere indicate: Atmel (gama AT90S2313, AT90S8515) au fost utilizate într-un

proiect similar ca cerințe și anume “Microcontroller Systems for UAV; Autopiloting and

camera trigger system” (Alexandros Skafidas, decembrie 2002), PIC 16F877 în

“Development of Unmanned Aerial Vehicle Manual Control System” (Thae Su Aye, Pan Thu

Tun, Zaw Min Naing, and Yin Mon Myint, 2008), PIC 18F8720 în „A Small Semi-

Autonomous Rotary-Wing Unmanned Air Vehicle (UAV)” (Scott D. Hanford, Lyle N. Long,

and Joseph F. Horn, The Pennsylvania State University, University Park, PA).

1.5 Algoritmi de reglare

Proiectarea unui sistem de reglare necesită un efort ciclic, în care se integreză etapele

de modelare, proiectare, simulare, testare și implementare. Un sistem de reglare automată

(SRA) asigură menținerea variabilei reglate la o valoare constantă, impusă prin referința

sistemului. Proiectarea unui SRA presupune: instalația tehnologică, senzori, elemente de

execuție, obiective, comunicații, arhitecturi și interfețe, perturbații, incertitudini și algoritmi

[5].

În aplicațiile industriale, în cadrul structurilor cu unul sau mai multe grade de

libertate, sunt utilizate legi de reglare convenționale sau neconvenționale. Astfel, în ceea ce

privește industria aeronautică, se folosesc cu preponderență algoritmi convenționali, în primul

rând datorită simplității acestora, ușurinței de implementare și a faptului că foarte multe

dintre performanțele impuse sistemelor de zbor se mulează foarte bine pe ceea ce oferă acești

algoritmi. Dar, se constată, din ce în ce mai mult, o tendință de migrare a structurilor de

conducere convenționale către structuri neconvenționale, mai complexe, ce au un grad ridicat

de abstractizare a proceselor conduse. Algoritmii neconvenționali sunt utilizați, în special, în

sistemele în care apar dificultăți majore în caracterizarea matematică riguroasă [5].

Algoritmii convenționali cei mai folosiți sunt: P, PI, PD și PID, precum și variante de

PID – modificați (PI-D, I-PD), iar dintre cei neconvenționali se remarcă sistemele adaptive și

expert, tehnici fuzzy, rețele neurale și algoritmi genetici.

1.5.1 Algoritmi convenționali

Unele dintre cele mai complexe regulatoare folosite în industrie sunt PID-urile

(proporțional-integral-derivativ), ele asigurând performanțe de reglare superioare. Așa cum îi

spune și denumirea, ele combină efecte proporțional, intergal și derivativ conform cu legea:

unde:


19

Structura, simplitatea, dar și performanțele ce se obțin cu ajutorul acestor regulatoare

determină, și în ziua de astăzi, utilizarea acestora în sisteme simple și complexe. Aceste

performanțe se datorează acordării parametrilor în scopul obținerii unui răspuns rapid, unei

erori staționare egală cu zero și un suprareglaj cât mai mic posibil.

Deși PID-ul nu are decât 3 parametrii, acordarea lui este un proces dificil ce trebuie

să ia în calcul limitările elementelor de execuție.

Foarte multe sisteme de tip UAV folosesc strategii de conducere bazate pe reglarea

în cascadă, dar necesită mulți parametrii și, prin urmare, putere mare de calcul ce este direct

proporțională cu costul. Astfel, se recomandă găsirea unei soluții care să conducă la

performanțele dorite, asigurând totodată și robustețea sistemului [29]. Mai jos este prezentată

schema de principiu a unui sistem de reglate automată cu regulator PID.

Figura 1.22 Schemă principiu PID

1.5.2 Algoritmi neconvenționali

Evoluția sistemelor de conducere de la structuri convenționale și algoritmi proiectați,

pe baza modelelor matematice ale proceselor la sisteme inteligente ce integrează tehnici

euristice, sisteme expert, rețele neurale, tehnici fuzzy și tehnici evoluționiste de procesare a

informației și cunoștiințelor reprezintă o cale naturală ținând seama de stadiul atins în

domeniul micro și nanotehnologiei [5].

Algoritmii neconveționali încep să fie din ce în ce mai mult folosiți în reglarea unor

sisteme de tip UAV. Cele mai multe dintre ele sunt utilizate în cadrul experimentelor, în

cercetare și au drept scop rezolvarea problemelor ce apar în timpul zborului, precum pierderea

bruscă a ariei de control, a puterii motoarelor etc. Noile cercetări relevă faptul că prin

integrarea unor tehnici inteligente de conducere cu cele convenționale se pot obține rezultate

remarcabile și evita multe neajunsuri ale regulatoarelor clasice.

Dintre tipurile de tehnici inteligente amintim:

sisteme adaptive;

sisteme expert;

tehnici fuzzy (ușor de ințeles și presupun implicarea experienței experților în

propunerea soluției de reglare);

rețele neurale;

algoritmi genetici.

D

P

I r(t)

(t) y(t)

-


20

Figura 1.23 Exemplu de structură hibridă

de optimizare geno-fuzzy/ geno-neuro

unde Ik reprezintă indicele de performanță impus. Recentele dezvoltării au arătat că îmbinarea mai multor tehnici dintre cele

menționate anterior poate conduce la rezultate remarcabile, mai ales când este vorba despre

sisteme autonome, așa cum este și un vehicul tip dronă. Astfel, apar arhitecturi de sisteme

hibride, spre exemplu geno-fuzzy, geno-neuro în care una dintre tehnici este folosită pentru

sinteza regulatorului, iar cea de-a doua pentru optimizare.

Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

21

CAPITOLUL II

DETALIEREA

PLATFORMEI DEZVOLTATE

În acest capitol se va face detalierea platformei dezvoltate, a montajului practic

realizat cu ajutorul Institutul de Tehnologii Avansate (ITA), precum și a echipamentelor

indivituale folosite. Astfel, sunt prezentate date legate de senzorii utilizați, de microcontroler,

camera video folosită și motoare.

Montajul de față a fost realizat în urma unor discuții și întâlniri preliminare cu

specialiștii pe parte mecanică de la institutul menționat anterior. Pe lângă limitările mecanice

impuse de tehnologia de realizare utilizată, s-a avut în vedere faptul că ulterior montajul va fi

montat pe un vehicul tip dronă. În acest fel montajul a trebuit încadrat în anumite limite

spațiale și mecanice.

În cerințele de proiectare impuse se consideră că dronă va fi una cu aripi fixe, și nu

rotative. Dronele cu aripi fixe, spre deosebire de cele cu aripi rotative, necesită o zonă mult

mai mare pentru a asigura decolarea și aterizarea în condiții de siguranță, ceea ce reprezintă

un inconvenient. Pe de altă parte, argumentul folosirii acestui tip de dronă este faptul că are

un grad mult mai mare de eficiență în tipul zborului, putând acoperi suprafețe mult mai vaste

de până la 12.000 m2.

2.1 Structura generală a platformei (montajul)

Platforma este rezultatul unui întreg proces de proiectare minuțioasă, la care au

participat mai mulți specialiști din domeniul mecanic și electronic. Soluția mecanică de

realizare a încercat să se supună tuturor limitărilor impuse, dar să permită și o elaborare

realistă și care să poate face posibil lucrul cu aceasta separat de restul componentelor

proiectului (corpul vehiculului).

Platforma dezvoltată are drept suport un cadru metalic. Cadrul a fost realizat astfel

încât ulterior să poată permite montarea pe o dronă.

Se poate considera platforma sub forma unui paralelipiped dreptunghic, cu prelungiri

de susținere pe o axă (comparabile cu două muchii exterioare). De asemenea, acestui

paralelipiped i-au fost eliminate toate suprafețele laterale rămânând doar sub formă de cadru

de suport. Are două puncte de susținere exterioară atunci când este menținut pe orizontală,

susțineri ce au fost fabricate având găuri speciale ce vor permite înglobarea montajului în

corpul vehiculului.

Partea superioară a montajului este compactă deoarece aceasta permite prinderea

unuia dintre servomotoare și a mecanismului de mișcare a camerei foto/video. Este absolut

necesar ca restul planurilor ce compun paralelipipedul să lipsească, permițând camerei

foto/video o rază cât mai mare de vizare.

Platforma include întreg mecanismul ce permite rotația camerei pe cele trei axe (X,Y

și Z), mecanism ce se prezintă sub forma tot a unui cadru metalic rotativ, un suport pe care

este fixată camera, asemănător cu un ax cardanic al unui giroscop, doar că la scară mult mai

mare. Axul cardanic din interiorul platformei se poate roti pe cele trei axe, în urma

comenziilor pe care cele trei servomotoare le impun în procesul de reglare.


22

În figura prezentată mai jos se poate vizualiza întreaga structură descrisă, avându-se

în vedere că dimensiunile aproximative ale montajului sunt 30 x 15 x 20 cm.

Figura 2.1 Structură montaj

De remarcat este modul de proiectare al acesteia, care a implicat modificări la nivelul

structurii de conectare a motoarelor pentru a putea fi adaptate și integrate în cadrul platformei.

2.2 Senzori

În cadrul sistemului dezvoltat, s-au folosit pentru achiziția datelor doi senzori

inerțiali MEMS, aleși în concordanța cu microcontroler-ul, dar și cu cerințele specificate,

aceștia oferind avantajul dimensiunii reduse. În figura 2.2 după [7] s-a dorit evidențierea

structurii miniaturizate de la nivelul componentelor senzoriilor MEMS pentru a înțelege cum

aceștia reușesc să atingă dimensiuni foarte reduse.

Figura 2.2 Dispozitiv MEMS ce oferă o imagine

a structurii de elemente miniaturizate

Astfel, s-a mers pe varianta combinării unui accelerometru și a unui giroscop, ambele

oferind date pe trei direcții.


23

Senzorii pentru care s-a optat sunt un accelerometru ADXL 345 Breakout și un

giroscop ITG-3200 Breakout, ambele produse de Sparkfun. Menționez că aceste dispozitive

mi-au fost puse la dispoziție în urma stabilirii specificațiilor proiectului, acestea fiind

suficiente pentru atingerea obiectivelor propuse, urmând ca ulterior să existe opțiunea

înlocuirii lor cu unele mai performante.

Acești senzori oferă game de valori suficient de largi pentru aplicație și potrivite

pentru tipul de proiect ce implică stabilizare în aer a sistemului.

2.2.1 Accelerometru ADXL 345 Breakout

Așa cum a fost menționat anterior accelerometrul, având ca producător firma

Sparkfun, a fost selecționat pentru realizarea proiectului. ADXL 345 Breakout este un

accelerometru de tip MEMS (cu structura ca ceea din figura 2.3 după [8]), digital, căruia în

poate fi schimbată gama de măsurare (permite măsurători ±2 g, ±4 g, ±8 g, ±16 g), acest lucru

permițând folosirea lui în aplicații din cele mai diverse (navigație, instrumentație industrială,

aplicații medicale).

Figura 2.3 Detaliu structură accelerometru MEMS

ADXL 345 Breakout este un echipament de înaltă precizie și calitate ridicată, dar și

consum mic de putere.

Fiind un accelerometru digital ce măsoară accelerația pe toate cele 3 axe (X, Y, Z),

de mici dimensiuni (3 x 4 x 1 mm), are o rezoluție fixă de 10 biți cu posibilitatea de extindere

până la 13 biți în cazul în care se utilizează gama de măsurare de ±16 g. Un aspect important

care a dus la alegere sa este și folosirea de către acesta a magistralei de comunicație I2C, dar și

SPI. Ieșirea pe care acest dispozitiv o oferă este una pe 2 octeți complementari.

Echipamentul dispune și de funcționalități speciale cum ar fi detecția prezenței sau

lipsa mișcării, prin compararea accelerației citite cu un prag setat de utilizator. De asemenea,

este capabil să detecteze lovituri simple sau duble (“taps”), precum și căderi în gol. Toate

acestea pot fi configurate cu ajutorul pinilor de întrerupere de care dispune senzorul.

Figura 2.4 Accelerometrul ADXL345 Breakout


24

Pini acestui dispozitiv folosiți în proiect sunt:

GND – conexiune la masă;

VCC – conexiune la tensiunea de alimentare (3.3V);

CS – chip select conectat la tensiunea de alimentare (3.3V) realizează

selectarea ca magistrală de comunicație al I2C-ului;

SDA – date seriale - conexiune la pin analog microcontroler;

SCL – ceas de comunicație serială - conexiunea la pin analog microcontroler.

Modalitatea de realizare a conexiunilor accelerometrului cu microcontroler-ul ales

este ilustrată în figura 2.5 după [9].

O observație importantă este remarcarea sensurilor desenate pe chip-ul senzorului

care, la o atentă examinare și testare a plăcuței, a dus la constatarea inversării tuturor axelor.

Astfel, sensurile prezentate reprezintă valori negative și nu pozitive corespunzătoare axelor.

Figura 2.5 Schemă conectare la microcontroler

Un alt aspect îl reprezintă sensibilitatea senzorului care, în acest caz, este de 256

LBS/g pe fiecare dintre cele 3 axe. Asta înseamnă că valoarea de la ieșirea giroscopului va fi

divizată prin această valoare pentru a obține rezultatele scalate.

Indiferent de limbajul de programare folosit, acest senzor are de inițializat câteva

variabile standard:

adresă device,specificată în datasheet #define DEVICE (0x53);

registru pentru power control char POWER_CTL = 0x2D;

precizarea formatului datelor citite char DATA_FORMAT = 0x31;

componentă 0 și 1 pe axa X char DATAX0 = 0x32; char DATAX1 = 0x33;

componentă 0 și 1 pe axa Y char DATAY0 = 0x34;

char DATAY1 = 0x35;

componentă 0 și 1 pe axa Z char DATAZ0 = 0x36;

char DATAZ1 = 0x37;


25

2.2.2 Giroscop ITG – 3200 Breakout

În privința giroscopului, s-a ales pentru acest proiect ITG-3200 Breakout deoarece

acest senzor indică, la ieșirea sa digitală, viteza unghiulară pe cele 3 axe ca în figura 2.6 după

[16]. Este un chip folosit în aplicații multiple de control remote, controlere pentru jocuri,

echipamente de monitorizare sportivă.

Acesta este tot un senzor de tip MEMS (4 x 4 x 0.9 mm) ce folosește 3 convertoare

ADC de 16 biți pentru calcularea ieșirii, iar ca magistrală de comunicație, I2C. El are inclus și

un senzor de temperatură în structura sa. De asemenea, calibrarea este redusă foarte mult prin

stabilitatea sensibilității la temperatură și înclinație.

Sensibilitatea senzorului este de 14.375 LSB per grade/sec, dispunând de o scală de

măsură între -2000 și +2000 de grade/sec. În structura sa este inclus și un filtru trece-jos

configurabil.

Figura 2.6 Axele de rotație ITG-3200 Breakout

În cazul giroscopului axele X, Y, Z corespund cu cele trei direcții de rotație în cazul

unui vehicul aerian (roll, pitch și yaw). În cazul giroscopului sensul pozitiv al axelor s-a

dovedit a fi corect.

Așa cum se poate vedea și în cele ce urmează, chip-ului senzorului i-a fost atașat un

header de pini pentru a putea realiza conexiunile cu microcontroler-ul, metoda utilizată și

pentru ADXL 345.

Figura 2.7 ITG-3200 utilizat

Pentru o bună conectare a senzorului la microcontroler, pini ce trebuie utilizați sunt,

în cazul de față:

VDD – conexiune la tensiune alimentare (3.3V);

VIO – conexiune la tensiune alimentare (3.3V);

GND – conexiune la masă;

SDA – date seriale - conexiune la pin analog microcontroler;

SCL – ceas de comunicație serială - conexiunea la pin analog microcontroler.

Conexiunile utile pentru stabilirea comunicației cu microcontroler-ul sunt

reprezentate în schema 2.8 după [9].


26

Figura 2.8 Conectare ITG-3200 Breakout la microcontroler

Valorile de la ieșire giroscopului sunt date digitale, fără semnificație fizică. Pentru a

obține date care să ajute ulterioarele prelucrări din sistem, le vom scala cu ajutorul factorului

de sensibilitate.

(2.2)

Inițializările necesare ale regiștriilor în cazul giroscopului sunt prezentate mai jos:

verifică identitate device char WHO_AM_I = 0x00;

determină rata de eșantionare char SMPLRT_DIV= 0x15;

selectare scalei maxime de măsurare char DLPF_FS = 0x16;

date axa X pe byte low și high char GYRO_XOUT_H = 0x1D;

char GYRO_XOUT_L = 0x1E;

date axa Y pe byte low și high char GYRO_YOUT_H = 0x1F;

char GYRO_YOUT_L = 0x20;

date axa Z pe byte low și high char GYRO_ZOUT_H = 0x21;

char GYRO_ZOUT_L = 0x22;

configurare frecvență 42 Hz pentru FTJ inclus în giroscop și configurare

frecvență internă 1kHz char DLPF_CFG_0 = 1<<0;

char DLPF_CFG_1 = 1<<1;


char DLPF_FS_SEL_0 = 1<<3;


adresa dispozitiviului I2C

char itgAddress = 0x69;


27

2.3 Microcontroler

Multitudinea de tipuri de microcontrolere face grea alegere unuia care să se

potrivească aplicației. În cazul de față s-a optat pentru unul din gama Arduino, și anume

Arduino Duemilanove. Argumentul principal ar fi ușurința de folosire a protocolului I2C cu

Arduino (dispune de librăria Wire.h). Pe lângă acest lucru, Arduino oferă:

cost și dimensiune reduse;

un software open-source ;

comunicație prin USB;

suficienți pini digitali (14) și analogici (6);

interfață I2C și SPI;

pini PWM;

comunitate largă de utilizatori și aplicații.

Figura 2.9 Vedere de sus (a) Vedere spate (b) Arduino Duemilanove

Modelul utilizat de mine are la bază un microcontroler de la Atmel ATMega 328P.

Tensiunea de alimentare este de 5V obținută de la USB, dar ulterior va fi dată de o baterie,

dorind astfel să fie un montaj stand-alone. Frecvența de funcționare a microcontroler-ului

este de 16 MHz. Arduino este văzut de către PC drept o interfață serială, înseși datele pe care

le obținem putând fi vizualizate prin intermediul software-ului său.

În elaborarea programelor s-a utilizat frecventa de 100 kHz specifică I2C pentru

achiziționarea datelor întrucât această rată de citire este acceptabilă în cazul unor sisteme

inițiale de test, putându-se ulterior trece la valori mai mari ale frecvenței de până la 400 kHz

(fast I2C).

Accesul la librăria Wire.h activează rezistențele interne de pull-up din chip în mod

implicit, astfel atunci când sunt adăugate unele externe, acestea se dezactivează. Aceste

rezistențe au rolul de a defini niveluri logice în circuit și de a elimina eventualele zgomote ce

ar putea afecta acuratețea ieșirii.

Pentru protocolul I2C, microcontroler-ul va fi master, iar dispozitivele (senzorii),

slave. Pinii analogici 4 și 5, corespunzători SDA și SCL, vor fi conectați cu omologii

senzorilor. Această conectare necesită automat folosirea unor rezistențe de pull-up.

Astfel, esențiali în proiectul de față sunt pinii:

GND

VCC

3V3

AN4 și AN5

Pini PWM


28

Așa cum este menționat anterior, mediul de dezvoltare este unul open-source, ușor de

utilizat, cum se poate vedem în cele ce urmează. Pentru realizarea unui nou proiect se

deschide o noua fereastră și, cel mai important, din Tools > Board se selectează

microcontroler-ul folosit. Odată cu conectarea Arduino, se poate vedea portul pe care acesta

în folosește pentru comunicația cu PC-ul.

Figura 2.10 Mediu de dezvoltare Arduino IDE

În scrierea unui program (sketch) există câteva secțiuni care nu trebuie să lipsească.

Este necesară includerea librăriilor ce se vor utiliza, declararea variabilelor auxiliare folosite,

declararea constantelor, dacă există, și două secțiuni:

setup() – se execută o singură dată la deschiderea portului de comunicație,

este folosită pentru inițializări;

loop() – conține programul principal, rulează pe microcontroler în mod

repetat.

Folosirea librăriile predefinite în Arduino IDE reprezintă un avantaj deosebit întrucât

ușurează munca prin clase deja existente în special pentru comunicația prin diverse

dispozitive sau comunicația cu elemente de execuție:

EEPROM.h

Ethernet.h

Math.h

Servo.h

Wifi.h , Wire.h, etc.


29

Figura 2.11 Realizare aplicație Arduino IDE

Pe lângă funcțiile de bază care nu pot lipsi, se pot declara funcții auxiliare. Acestea

se declară ca orice funcție, începând cu header și terminând cu definiția acesteia.

În cazul de față, Arduino IDE este folosit pentru a implementa următoarele

funcționalități ale proiectului:

Figura 2.12 Funcționalități implementate cu ajutorul Arduino IDE

Programul ce se regăsește în anexă reprezintă codul sursă al proiectului ce are mai

multe părți și anume partea de achiziție, filtrarea, calculul comenzii și partea de transmitere

efectivă a comenzii către servomotoarele folosite în proiect.

Toate variabilele sunt declarate la început sketch-ului, iar în setup se face

inițializarea comunicației seriale, inițializarea variabilelor și se definesc regiștrii. În loop se

realizează bucla în care se fac citirile succesive ale datelor de la senzori, filtrarea și comanda.

Există, de asemenea, funcții suplimentare definite pentru funcționalitățile menționate

anterior, iar acest lucru ajută și la modularitatea programului. Împărțirea pe funcții

specializate asigură și un debug mai bun al programului.

ARDUINO

Achiziție date

Filtrare date

Calcul comandă

Transmisie comandă

servomotoare


30

2.4 Motoare

În aplicație s-au selecționat ca elemente de execuție ale procesului 3 servomotoare

astfel încât fiecărei axe să-i corespundă unul. Alegerea a fost bazată pe necesarul de putere,

dar și implicațiile costurilor au fost un factor decisiv.

Modelul ales este cel al producătorului Power HD – 1501 MG, acest model fiind de

fapt un servomotor. Acesta reprezintă un actuator rotativ ce permite controlul precis al

poziției unghiulare și este compus dintr-un motor cuplat la un senzor ce oferă feedback al

poziției prin intermediul unui angrenaj (reductor). În cazul servomotoarelor, de cele mai multe

ori este necesar un modul dedicat, controler servo, pentru obținerea feedback-ului de la acesta.

Aplicațiile servomotoarelor se regăsesc în special în robotică, mașini cu comandă numerică

sau în producția echipamentelor de automatizare [23].

Figura 2.13 Power HD 1501 MG Analog Servo

Power HD 1501 MG Analog Servo este un servomotor analog ce prezintă un

angrenaj compus din roți dințate metalice. S-a folosit acest echipament întrucât acest model

oferă un cuplu foarte mare, întâlnit de obicei la dispozitive de acest gen mult mai scumpe. O

secțiune a angrenajului este reprezentată în figura 2.14 [30].

Figura 2.14 Angrenajul de roți dințate al servomotorului

Acest servomotor are dimensiunile standard ale unui astfel de motor. Pe lângă

angrenajul metalic, dispune și de doi rulmenți care ajută la obținerea cuplului. În kit sunt

incluse și extensiile ce permit fixarea lui în cadrul unui montaj mecanic.

Dimensiunile sale respectă cerințele: 40.7 x 20.5 x 39.5 mm, având o greutate de 60g

per servomotor. Power HD 1501 MG folosește modulația analogică (Pulse Width

Modulation). În cadrul acestei tip de modulații este foarte importantă lățimea impulsului care

în acest caz particular pot varia între 500-2100 . Se ia în considerare și rata de refresh a

unui ciclu PWM, care aici este de 20 ms. Aceste aspecte sunt deosebit de importante, ele fiind

necesare la reconfigurarea librăriei Arduino.


31

Dacă se asigură o tensiune de alimentare de ~ 5V (mai precis 4.8V), viteza

unghiulară garantată de producător este de 0.16 secunde per 60º. Cuplul la aceeași tensiune de

alimentare este 15.5 kg*cm.

Pentru conexiunea servomotoarelor, acestea dispun de 3 fire de culori diferite. În

mod uzual ele reprezintă:

maro = GND;

roșu = VCC (aici 5V);

portocaliu = semnal servo.

2.15 Fire conexiune servomotor

Power HD 1501 MG dispune, teoretic de o gamă de variație a poziției unghiulare de

180º, dar testele efectuate și informațiile furnizate pe pagina de discuții a produsului indică

faptul că acesta nu se poate roti mai mult de 160º. Pentru obținerea gamei maxime de variație

este necesară introducerea unui controler servo care, în cazul lucrării de față, nu a fost inclus.

În privința servomotoarelor, Arduino IDE oferă o librărie special dedicată

servomecanismelor, Servo.h. Pentru acest proiect, ea a fost reconfigurată, stabilind parametrii

conform cu specificațiile tipului de servomotor utilizat. Astfel, s-au redefinit o parte dintre

variabilele librăriei:

#define MIN_PULSE_WIDT 500 //cel mai scurt impuls către servo

#define MAX_PULSE_WIDTH 2100 //cel mai lung impuls către servo

#define DEFAULT_PULSE_WIDTH 1500 //lățime default a impulsului

la conectare servo

#define REFRESH_INTERVAL 20000 //timpul minim de refresh în ms

Montarea servomotoarelor în cadrul platformei a presupus modificări.

Servomotorul corespunzător direcției de “yaw” (stânga-dreapta) este montat în

exteriorul cadrului.


32

Figura 2.16 Servomotor control direcție “yaw”

Celelalte două motoare sunt cuprinse în cadrul interior al montajului, fiecare dintre

ele necesitând modificări la nivelul cuplajului dintre axul servomotorului și partea mecanică

ce trebuie acționată.

Figura 2.17 Cuplaj montare servo pentru direcția “pitch”(a) ; direcția “roll” (b)


33

2.5 Cameră video

În perspectiva procesării de imagine, montajului i-a fost adăugat și o cameră

foto/video. Această este una din portofoliul Sony, FCB-EX480CP și dispune de control

inteligent al lentilelor (“SMART lens control”), de funcția de “Privacy Zone Masking”

(protejează până la 24 de obiecte și suprafețe aflate în raza de acțiune, dar care nu sunt în mod

expres obiecte ale suprevegherii sale). Are inclus și un DSP (Digital Signal Processing) și

funcții de alarmă. Folosește o magistrală serială de viteză foarte mare (maxim 38.4Kb/s) cu

control TTL al nivelului de semnal (protocol VISCA), dar presupune un consum mic de

putere. Dispune de un sistem de back-up chiar și în lipsa bateriei cu scopul de a asigura o

protecție cât mai eficientă a informațiilor stocate.

Prezintă, de asemenea, și o funcție ce încorporează un filtru IR (infrared cut-off

filter), dar este echipat și cu un connector tip switch ce permite controlul camerei de la

distanță.

Figura 2.18 Cameră video FCB-EX480CP

Camera oferă un zoom optic de 18x și 12x digital, cu un număr de aproximativ

440.000 pixeli componenți într-o imagine. Acesta cântărește 230 g, putând opera la

temperaturi între 0 și 50 ºC. Este, de asemenea, capabilă să aplice diverse efecte fotografiilor

efectuare: E-Flip, Nega Art, Black & White, Mirror Image.

Toate aceste atribute fac din Sony FCB-EX480CP o cameră foto/video ideală pentru

aplicații de securitate și monitorizare a traficului terestru și aerian.

Figura 2.19 Camera în ansamblul dezvoltat


34

2.6 Filtrarea datelor

Scopul într-un sistem de vizare este alinierea axelor dispozitivului (vehiculului) cu

axele sistemului de referință pentru menținerea poziției dorite. Folosirea unor senzori în

estimarea deplasării și rotației vehiculului, precum accelerometru și giroscop, poate ridica

probleme și anume, deviații ale accelerometrului care introduc zgomot în sistem și fluctuații

rapide ale valorilor giroscopului ( rata acestuia nu devine 0 nici după ce vehiculul s-a oprit din

rotație) [12].

Astfel, devine esențială introducerea unei modalități de eliminarea (sau reducere) a

zgomotele introduse suplimentar și care duc la perturbarea datelor obținute.

Soluțiile sunt multiple în această direcție, dar dintre cele mai frecvent folosite putem

menționa:

filtru trece-jos (FTJ) – capabil să elimine accelerațiile de scurtă durată,

permițând trecerea doar a celor de durată îndelungată; acest tip de filtru

introduce întârzieri mari datorită medierilor repetate;

filtru complementar – folosește FTJ pentru accelerometru, iar pentru giroscop

combină integrarea numerică cu un filtru trece-sus (FTS);

filtru Kalman – poate predicta starea și orientarea cu un grad mare de

acuratețe, bazându-se pe proprietățile fizice cunoscute ale sistemului în

cauză; practic, acesta combină 2 sau mai multe estimații independente ale

unei variabile pentru a obține o valoare medie.

În aplicație, inițial, s-a apelat la implementarea unui filtru complementar reprezentat

schematic în figura 2.20 după [14], această variantă fiind acceptabilă și oferind performanțe

bune. Pe lângă acest aspect, este mult mai simplificat din punct de vedere al formalismului

matematic, ceea ce îl face și mai ușor de integrat în program.

Figura 2.20 Schemă filtru complementar

GIROSCOP

ACCELEROMETRU Filtru Trece – Jos

(FTJ)

Filtru Trece – Sus (FTS)

Integrare Numerică

Unghi de

rotație

Viteză

unghiulară


35

Integrarea numerică presupune că din viteză (v) integrată obțin poziție, iar din viteză

unghiulară ( ) integrată obțin o rată unghiulară (unghi).

(2.3)

Așa cum este menționt și anterior, FTJ (caracterizat de o constantă de timp ) va

permite numai trecerea accelerațiilor de lungă durată, ignorând fluctuațiile scurte, iar FTS va

face opusul, lăsând să treacă semnale de scurtă durată și filtrându-le pe cele constante pe o

perioadă mai lungă de timp. Practic, se definește o limită între încrederea acordată datelor

citite de accelerometru și cele citite de giroscop.

pentru perioade de timp > media valorilor accelerometrului are o pondere

mai mare;

pentru perioade de timp < integrarea valorilor giroscopului capătă prioritate

și accelerațiile orizontale ce introduc zgomot sunt filtrate [14].

Introducerea filtrului complementar realizează mai mult fuziunea datelor

achiziționate de la accelerometru și giroscop, decât o filtrare foarte eficientă. Performanțele

lui îl fac să fie ușor de implementat și folosit, dar principalul motiv pentru această alegere este

modalitatea prin care datele celor doi senzori sunt relaționate, astfel încât să ofere niște rate de

rotație utile în procesul de reglare.

Mai jos se poate vedea felul în care se realizează această fuziunea a datelor,

formulele matematice bazându-se, în cea mai mare parte, pe schema de principiu a filtrului

prezentată anterior. Implementarea folosită este prezentată ma jos, unde dtime = perioada pe

care se face integrarea:

gyroRateX=-1.0f*dtime*xg;

gyroRateY=dtime*yg;

gyroAngleZ_dt=dtime*zg;

Avantajele pe care le oferă acest filtru sunt:

eliminarea zgomotului, a deviației și dependenței accelerației orizontale;

estimarea rapidă a unghiurilor, întârzieri mai mici decât FTJ;

nu necesită capacitate mare de procesare;

dificultate medie de implementare.

poate predicta starea și orientarea; se bazează pe proprietățile fizice cunoscute

ale sistemului în cauză; practic, acesta combină 2 sau mai multe estimații

independente ale unei variabile pentru a obține o valoare medie; este complex

și bun comparabil cu alte tipuri de filtre deoarece încorporează toate

măsurătorile indiferent de precizia lor [13].

Totuși, pentru realizarea finală a proiectului s-a ales implementarea unui filtru

Kalman. Acesta se bazeză pe reducerea erorii în urmărire și folosește predicția unei valori

viitoare plecând de la o valoare curentă și anterioară. Acestă nouă predicție suferă apoi o

corecție. Noua valoare trebuie să se regăsescă într-un interval definit de valoarea predicată și

cea măsurată.

În acest mod se pot reduce erorile și zgomotele introduse în sistem. Dar, de-a lungul

timpului, există totuși erori cumulative. Pentru sistemul descris aici, se recomandă

combinarea datelor accelerometrului și giroscopului, cele două fiind capabile prin intermediul

filtrului Kalman de a-și compesa erorile.


36

Filtrul Kalman reprezintă o procedură iterativă ce necesită ca intrări două variabile.

La fiecare iterație, filtrul transformă liniarizează aceste variabile, astfel încât ieșirea modelului

să fie cât mai apropiată de viitoarea valoare citită [25].

Pentru situația de fața, modelul filtrului este prezentat mai jos:

(2.4)

În aceste ecuații necunoascutele reprezintă:

= covarianța erorii estimate

câștig Kalman

= valoarea filtrată

= zgomotul senzorului

= valoarea măsurată

Zgomotul procesului este redus gradat în această manieră odată cu atingerea

performanțelor de optim, iar zgomotul senzorului crește până când se atinge un nivel

acceptabil de zgomot.

O schemă de principiu se regăsește în figura 2.21 după [25] în care este exemplificat

fluxul datelor ce folosesc un filtru Kalman. Acest procedeu este unul recursiv.

Pentru elaborarea programului necesar filtrării în Arduino IDE, s-au definit

următoarele variabile, exemplificarea fiind făcută pentru una dintre axele accelerometrului și

anume axa X. Astfel, se pot observa că relațiile (2.4) sunt transpuse sub forma unor ecuații

implementate. Față de varianta matematică, se folosesc și câteva variabile auxiliare pentru a

ușura calculul :

valoarea filtrată double x_acc_f = 1;

eroarea estimată double p_x_acc = 1;

zgomotul procesului double q_x_acc = 0.4;

zgomotul senzorului double r_x_acc = 0.5;

câștigul Kalman double k_x_acc = 1;

Implementarea ecuațiilor este prezentată mai jos sub forma utilizată pentru

implementare: p_x_acc = p_x_acc + q_x_acc;

k_x_acc = p_x_acc / (p_x_acc + r_x_acc);

x_acc_f = x_acc_f + k_x_acc * (xa - x_acc_f);

p_x_acc = (1 - k_x_acc) * p_x_acc;

Valorile alese pentru zgomotul procesului și cel al senzorului se bazeză pe teste

experimentale, precum și pe studiul literaturii de specialitate. Testele realizate cu

implementarea acestui filtru relevă faptul că este mult mai performant decât orice altă

modalitate de filtrare a datelor.


37

Figura 2.21 Algoritm filtru Kalman

Alegerea acestui filtru se dovedește a fi cea mai bună, întrucât este cel mai complex

dintre toate tipurile de filtre pentru că încorporează toate măsurătorile, indiferent de precizia

lor și îmbunătățește considerabil acuratețea sistemului [13].

2.7 Soluția de implementare utilizată

Pentru atingerea obiectivelor propuse în introducere, s-a ales o soluție de

implementare potrivită. Astfel, s-a optat pentru o implementare simplă care să permită

testarea dispozitivului, ulterior montarea pe un cadru al dronei.

În această fază a proiectului, montajul ia în considerație câteva tipuri de mișcări pe

care le efectuează dispozitivul, reducând astfel la scară mai mică probleme ce pot apare.

Punctul principal de atins este testarea funcționalității globale a sistemului, având integrat atât

senzorii, cât și elementele de execuție și unitatea de procesare.

O schema de principiu a felului de realizare și îmbinare a componentelor montajului

este disponibil în cele ce urmează, cu mențiunea că acesta este un model pe 3 axe al

sistemului, realizat cu un simulator 3D, SolidWorks eDrawings.

Figura 2.22 Model 3D al componentei rotative a montajului

În cea de-a doua fotografie se poate vedea și sistemul de coordonate folosit, putând fi

vizualizate cele trei axe (X, Y și Z). Această simulare a fost făcută înaintea proiectării practice

a montajului cu scopul de a stabili niște dimensiuni potrivite ale pieselor componente.

Actualizare

estimare

Măsurări

Câștig Kalman

Estimare inițială

Actualizare

covarianță

k + 1

Estimare

nouă


38

Realizarea acestei simulări a fost elaborată în urma cerințelor impuse și în strânsă

colaboare cu cei de la ITA.

Figura 2.23 Vedere din spate montaj – simulator 3D

În cadrul montajului există câteva aspecte importante de care s-a ținut cont și anume,

faptul că senzorii trebuie montați la unison unul cu celălalt, pentru ca acuratețea mișcăriilor

realizate pe baza datelor achiziționate de la ei să fie cât mai mare.

Pentru o mai bună montare, s-a ales varianta folosirii unui breadboard pe care sunt

montați senzorii, dar care este folosit și pentru realizarea conexiunilor auxiliare ale

microcontroler-ului. Pe plăcuța breadboard-ului au fost scose conexiuni ale pinilor AN4 și

AN5 ale Arduino (SDC și SCL) pentru a-i putea oferi ca intrări ambilor senzori. Dat fiind

faptul ca Arduino dispune de un singur pin de tensiune 3V3, s-a procedat în aceeași manieră

pentru a obține 2 intrari de 3V3.

Figura 2.24 Montare la unison a senzorilor

De asemenea, este de menționat că cei doi senzori folosesc ca magistrală de

comunicație I2C, ceea ce înseamnă că este necesară introducerea unor rezistențe de pull-up. În

cazul de față s-a folosit de rezistențe cu valorea de 4.7 K . Toate conexiunile realizate în

acest sistem sunt făcute prin intermediul unor cabluri. Acestă variantă nu este optimă, dar

poate fi ulterior ușor înlocuită prin realizarea unui circuit printat.


39

Figura 2.25 Cabluri tata-tata folosite (a) Rezistență 4.7K (b)

Frecvența la care se face achiziția de date de la senzori este de 100KHz, suficientă în

acest caz și potrivită cu frecvența la care funcționează . De reținut faptul că se poate folosi

și frecvența de 400KHz (Fast I2C).

În ceea ce privește servomotoarele, acestea necesită o tensiune de alimentare de 5V

pe care Arduino o poate asigura, problema principală fiind curentul consumat de ele în

sarcină. Acest curent nu trebuie să depășească 1A, altfel există riscul deteriorării chip-ului

Arduino. S-a optat pentru scoaterea pinului de 5V în breadboard și conectarea celor 3

servomotoare prin intermediul acestuia. Ca soluție incipientă de obținere a tensiunii s-au

considerat cei 5V pe care portul USB îi oferă, ulterior putând fi pusă la dispoziție o baterie ca

sursă de alimentare standalone.

Conectarea servomotoarelor la Arduino este făcută prin pinii digitali PWM (Pulse

Width Modulation). Fiecăruia dintre ele îi corespunde:

servomotor yaw = pin 9 PWM;

servomotor pitch = pin 10 PWM;

servomotor roll = pin 11 PWM.

Tot referitor la elementele de execuție este important de menționat că mișcările

acestora trebuie să respecte niște limitări mecanice care asigură siguranța și integritatea

montajului.

Aceste servomotoare sunt comandate analog prin menționarea gradelor. Ele permit

comenzi între 0-180º, dar mișcarea efectivă nu descrie un semi-arc de cerc.

Spre exemplu:

motor “yaw” – poate primi comenzi în gama 0 , dar nu descrie decât o

mișcare de aproximativ 160º;

motor “pitch” – poate primi comenzi în gama 0 , dar din cauza

cadrului metalic, trebuie impusă o limitare mecanică reducând gama la

50 pentru nu produce pagube la nivelul camerei video care poate

rămâne fixată în corpul cadrului;

motor “roll” – poate primi comenzi în gama 0 , dar mișcarea pe care o

descrie nu este efectiv o rotire cu un semi-arc de cerc; rotirea descrie practic

un arc de cerc cu un unghi de ≈ 70-80º.

Limitările sunt reprezentate schematic în figura 2.26 pentru a putea fi mai bine

înțelese. Unghiurile menționate sunt cele maxime admisibile pe care le pot descrie cele 3

servomotoare.


40

Figura 2.26 Limitări ale servomotoarelor

O observație de care trebuie ținută seama este acordarea unei atenții permanente

motorului montat pe direcția “roll” deoarece este mai vulnerabil la comenzi bruște. Mișcarea

de rotație este transformată în mișcare de translație printr-o articulație tip bielă-manivelă

foarte sensibilă.

Figura 2.27 Senzori și Arduino (a) Montare pe cadru (b)

Montarea și breadboard-ului cu senzorii pe cadrul metalic este realizată pe un

strat izolator, deoarece tensiunea de alimentare de 5V poate provoca scurtcircuit între

suprafețele metalice ale celor 2 componente.

2.8 Algoritm de reglare folosit

Indiferent de algoritmul de reglare ales, fluxul de date în cadrul procesului este un

aspect neschimbat. Acest flux este important deoarece face mult mai clară maniera în care se

succed etapele reglării. Se poate remarca în schema logică ilustrată prin figura 2.28, felul în

care s-a considerat că are loc reglarea, plecând de la achiziție și ajungând la feedback.

Y

Z

Z

X

O O X

Y

M1

YAW

M2

PITCH

M3

ROLL

1600

1800

00

500

00

1800


41

Figura 2.28 Schemă logică a fluxului procesului

Alegerea unui algoritm de reglare portrivit unei anumite aplicații trebuie să țină

seama de câteva aspecte deosebit de importante cum ar fi:

timpul mort și constanta de timp a sistemului;

eroare staționară admisă ( ); timpul tranzitoriu;

gradul de stabilitate al sistemului;

costul maxim admis.

Date fiind aceste criterii și tipul de proces pentru care se dorește calcularea unui

regulator (platforma), cel mai bine, din punct de vedere teoretic, ar funcționa PID-ul.

Controlul proporțional-integrator-derivativ (PID) este o metodă folosită la scară largă

pentru a atinge și menține o referință a unui proces de orice fel. Principiul de bază rămâne

neschimbat, indiferent de tipul de proces.

Forma standard a comenzii unui PID este:

SETARE POZIȚIE

INIȚIALĂ

CITIRE VALORI ACCELEROMETRU

CU CALCULAREA RATELOR PE 3

AXE

MEDIE CITIRI GIROSCOP

CALCUL RATE 3 AXE

CALCUL

CALCUL

COMENZII

LIMITARE

MINIMĂ/MAXIMĂ A

COMENZII

PRIMIRE COMENZI

SERVO

ÎNCADRARE

COMANDĂ

SERVO

NU

DA

FEEDBACK


42

,

unde cei 3 coeficienți , , sunt corespunzători fiecărei componente a

regulatorului, iar este eroarea, diferența dintre referința impusă și valoarea măsurată la

ieșirea procesului reglat.

Însă, în situația de față și pentru etapa inițială a testării s-a decis folosirea unui

regulator proporțional P. Deși acesta este cel mai simplu, reflectă suficient de bine

modalitatea de funcționare și felul în care sistemul răspunde unei referințe.

Trebuie ținut cont de faptul că montajul dispune de 3 servomotoare, iar

implementarea unui singur regulator va controla un singur element de execuție. În acest caz,

varianta este de utilizare a trei regulatoare de tip P pentru fiecare servomotor în parte.

Maniera de proiectarea a regulatorului de tip P este simplă, dar ține cont de eroarea

unui anumit parametru. Regulatorul tip P reprezentând de fapt o amplificare a comenzii,

trebuie să se țină seama de limitările servomotoarelor și de limitele ce trebuie evitate a fi

atinse pentru a nu satura elementele de execuție.

Este cunoscut faptul că regulatoarele ce nu au implementat un integrator, utilizarea

unui proporțional conduce la eroare staționară diferită de zero pentru o referință tip treaptă

unitară, dar trebuie ținut cont că odată cu creșterea factorului de amplificare se reduce

semnificativ eroare staționară [5].

Dacă pentru implementarea finală a proiectului (montarea platformei pe vehiculul tip

dronă) se decide folosirea unui PID, trebuie știut faptul că plecând de la compensatoarul PID

se poate ajunge ușor la P implementat de mine în această fază incipientă. Pentru aceasta este

necesară doar o modificarea a constantelor de timp ale regulatorului și anume Ti = și Td = 0.

În anexa în care este listat codul sursă al programului sunt prezentate două metode

prin care s-a încercat stabilizarea platformei în cazul apariției unei perturbații.

Astfel metodele încercate sunt:

a) integrarea dublă a accelerațiilor provenite de la accelerometru și

giroscop pentru obținerea poziției și a unghiului de rotație; în urma implementării, s-a dorit

calculul unei erori de poziție, însă această variantă s-a dovedit a nu fi viabilă întrucât

introduce deviații cumulative, iar datele ajung să nu mai ofere nici un fel de informație utilă

asupra procesului și variabilelor; implementarea acestei variante este prezentată în programul

final (prin urmare și în anexă) sub forma unor comentarii.

b) prin evaluarea erorii provenite de la diferența dintre valorile

achiziționate de la senzori înainte introducerii perturbației și după; aceasta variantă este cea

aleasă pentru a ilustra stabilizarea montajului; în urma calculului erorii, au fost stabilite niște

praguri care daca sunt depășite, introduc amplificarea (regulatorul P) comenzii

servomotoarelor.

(2.5)

Comanda în cazul ales va fi reprezentată de acestă amplificare adăugată unei

comenzi anterior. Ea se prezintă sub forma unui unghi impus servomotorului. Pentru fiecare

dintre cele 3 elemente de execuție s-a ales un factor separat:

(alfa) – direcție “yaw”

(beta) – direcție “pitch”

(gama) – direcție “roll”

Acești factori au niște valori inițiale în limitele impuse și posibile ale

servomotoarelor ( ; ).


43

În cele ce urmează este prezentată secvența de cod prin care se face ajustarea acestor

factori ai regulatoarelor tip P: alfa+=0.3

beta+=0.1

gama+=0.3

În capitolul III este prezentată explicit maniera în care are loc testarea acestui tip de

regulator, studiul de caz pe care se bazează practic rezultatele obținute.

Irina Diana Vidrașcu CAPITOLUL III - REZULTATE

44

CAPITOLUL III

REZULTATE

În acest capitol voi detalia rezultatele obținute în urma testelor efectuate. Dată fiind

împărțirea proiectului pe etape și momentele diferite la care au fost realizate anumite secvențe

de program, rezultatelor le sunt atribuite secțiuni individuale.

Rezultatele sunt prezentate, în cea mai mare parte, sub forma unor grafice Matlab.

S-a dorit ilustrarea unor mișcări simple până la complexe. Sunt reprezenatate rezultate

obținute în urma testelor efectuate cu senzorii în mod individual, dar și montați la unison pe

montaj. Datele pe care se bazeză toate aceste grafice au fost achiziționate prin intermediul

portului serial al PC-ului ce primește date de la microcontroler-ul Arduino.

3.1 Rezultatele achiziției de date de la accelerometru

Accelerometrul achiziționeză datele și le trimite pe portul serial într-o formă brută, ca

ieșirea a convertorului ADC. Pentru a putea folosi ulterior datele și pentru ca acestora să le

poată fi acordată o semnificație fizică, ele sunt scalate în concordanță cu sensibiliatea

senzorului, în acest caz 256 LSB/g (least significat bit/g).

Un prim rezultat obținut de la ADXL 345 Breakout este ilustrat în figura următoare.

Aici este exemplificată funcționalitatea senzorului pe toate axele de coordonate, fără a urmări

un fel precis de mișcare.

Figura 3.1 Reprezentarea valorilor citite pe

axele (XYZ) ale accelerometrului

Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

45

Se poate observa că, deși mișcările sunt unele aleatoare, variația datelor achiziționate

reflectă multe date parazite, afectate de erori și care nu fac decât să se propage în întreaga

implementarea. Din grafic se observă și gama pe care accelerometrul o folosește pentru datele

măsurate, aceasta fiind ± 2g.

Pentru a evita problemele apărute din cauza acestor erori de măsura, s-a apelat la

implementarea folosind filtre.

În etapa inițială s-a încercat implementarea unui filtru complementar însă acest s-a

dovedit a nu fi suficient de performant.

Astfel, opțiunea imediat următoare a fost folosirea filtrului Kalman. S-au studiat mai

multe tipuri de mișcări luând drept exemplu câteva cazuri particulare pentru a demonstra

eficiența acestei metode introduse suplimentar. Au fost efectuate teste pe fiecare plan

individual al accelerometrului. Astfel, s-a considerat o mișcare complet aleatoare pe toate trei

axele de coordonate pentru a vedea care este comportarea senzorului.

Figura 3.2 Mișcare aleatoare axa X

Încă de pe-acum se observă diferențele apărute între valorile achiziționate (scalate în

g) și valorile obținute după aplicarea filtrului. Acest lucru este remarcat cu precădere în cazul

valorilor de vârf (“spike-uri”). În următoare suită de grafice sunt prezentate concomitent

valorile nefiltrate și cele filtrate pentru a observa diferențele existente între cele două maniere

de tratarea a achiziției:

cu linie roșie - date senzor (nemodificate);

cu linie albastră – date filtrate cu filtru Kalman.


46

Figura 3.3 Mișcare aleatoare axa Y

Atunci când senzorul este staționar, valorile afișate nu sunt zero absolut, au un

anumit offset. Se remarcă, de asemenea, că datorită existenței accelerației gravitaționale, axa

pe care se regăsește va indică mereu o valoare aproximativ egală cu 1 g. Se observă în figura

3.4 momentele în care z ≈ 0 ceea ce înseamnă fixarea accelerometrului având axa Z în plan

orizontal.

Figura 3.4 Mișcare aleatoare axa Z


47

Pe baza testelor efectuate s-a ajuns la concluzia că sensurile indicate de producător

pe plăcuța accelerometrului sunt inversate. S-au realizat mișcări progresive spre stânga, în

plan orizontal, în sensul negativ al axei Y. Rezultatul poate fi analizat în figura 3.5:

Figura 3.5 Mișcari progresive spre stânga

Același test s-a efectuat și pentru deplasare pe axa Y, dar spre dreapta, în sensul

pozitiv ale axei.

Figura 3.6 Mișcări progresive spre dreapta

Comparând cele două cazuri, se pot observa diferențele apărute pe axa Y, în cazul

figurii 3.5 (spre stânga) se observă o majoritate a valorilor negative, în timp ce în figura 3.6

(spre dreapta), după filtrare, sunt preponderente valorile pozitive.

De remarcat în cele două figuri sunt axele X și Z. Axa X suferă câteva modificări,

deși nu ar fi trebuit, deoarece ea acționează în același plan cu axa Y. Axa Z prezintă în ambele

situații valori apropiate de 1g, așa cum este și normal datorită accelerației gravitaționale. O a


48

treia situație în plan orizontal este o deplasare a sistemului pe diagonala unui pătrat, situație ce

simulează o mișcare compusă pe axele X și Y.

Figura 3.7 Mișcare diagonală în plan orizontal

Aici se poate observa variația valorile citite pe axele X și Y comparativ cu cazurile

precedente. Graficul reflectă modificarea ambelor axe simultan și dependent una față de

cealaltă.

Pentru a evidenția și schimbarea valorilor axei Z, senzorului i se imprimă o

accelerație pe direcția verticală. În acest caz, variația axei X este normală, în timp ce axa Y ar

trebui să indice o valoare ≈ 1g, ea fiind axa perpendiculară pe planul mișcării. Absența acestui

aspect se justifică prin felul în care mișcările sunt efectuate de către operatorul uman.

Figura 3.8 Mișcare pe direcția verticală


49

Pentru o mai bună ilustrare a efectului filtrării Kalman în achiziționarea datelor de la

senzori, s-a selectat o secvența a unuia dintre grafice. Este evidentă contribuția acestei filtrări,

reprezentative fiind “spike-urile” atenutate.

Figura 3.9 Efect filtru Kalman

3.2 Rezultatele achiziției de date de la giroscop

Achiziția datelor de la giroscop se face în aceeași manieră ca și la accelerometru.

Datele fiind recepționate tot prin intermediul portului serial, vor fi trimise într-o formă

neprelucrată, așa cum sunt în urma trecerii prin convertorul de 16 biți ADC. Cu ajutorul

Arduino IDE, aceste date sunt scalate pentru ca ele să capete o conotație fizică valabilă.

Pentru aceasta, s-au utilizat datele menționate în datasheet-ul produsului, și anume

sensibilitatea giroscopului care în acest caz este de 14.375 LBS/grade/s. Ca și în cazul

accelerometrului primul experiment este ilustrat mai jos, urmărindu-se doar testarea

funcționalității senzorului pe un orizont de timp limitat. Practic, sunt reprezentate concomitent

toate cele 3 axe ale giroscopului.

Figura 3.10 Reprezentarea valorilor celor 3 axe (XYZ) ale giroscopului

Atenuare spike-uri


50

Din figură se poate observa că, deși mișcările sunt unele aleatoare, variația datelor

achiziționate reflectă multe date parazite, există erori care nu fac decât să se propage în

întreaga implementarea. Se remarcă și o gamă de variație a valorilor giroscopului care, pentru

reprezentarea de față, este de ( ) grade/s.

Mare parte dintre erori pot fi evitate dacă se aplează la folosirea unui filtru.

Giroscopul ITG-3200 Breakout dispune totuși de un filtru trce-jos încorporat, însă acesta nu

este suficient pentru a evita fluctuațiile senzorului. În implementarea proiectului s-a introdus o

metodă suplimentară de filtrare, filtrul Kalman. Inițial s-a încercat utilizarea unui filtru

complementar, dar atenuarea pe care acest o oferea nu era suficientă.

Astfel, ca și la accelerometru, testarea giroscopului și a corectitudinii datelor s-a

realizat pe mai multe tipuri de mișcări, prima dintre ele fiind o mișcare aleatoare.

Mai jos sunt ilustrate rezultatele obținute pe fiecare dintre cele trei axe de

coordonate:

Figura 3.11 Mișcare aleatore pe axa X a giroscopului

Se observă reprezentarea simultană a datelor brute și a celor cărora le-a fost aplicat

un filtru. Pentru axa X, datele filtrate urmăresc aliura celor provenite direct de la senzori ceea

ce înseamnă că erorile în acest caz nu sunt foarte mari. În cazul spike-rilor apar totuși

diferențe sesizabile, iar filtrul își face efectul.

Gama de variație a accelerației unghiulare în această situație a crescut, ea putând

varia în cadrul testelor în gama maximă admisibilă definită de producătorul senzorului. Se iau

în considerare și faptul că pentru stabilizare vor fi utilizate accelerații nu foarte mari, însă în

practică accelerațiile pot atinge valori ridicate dacă ne referim la o perturbație a mișcăriilor

aerului (vânt) care are un model neliniar și este foarte dificilă de compensat.

Aceleași concluzii sunt valabile și pe axele Y și Z. Pentru axa Y se observă

diferențele celor două situații, cu date nefiltrate și filtrate.


51

Figura 3.12 Mișcare aleatoare pe axa Y a giroscopului

În cazul axei Z se poate menționa faptul că, pentru această mișcare aleatoare, valorile

filtrului diferă considerabil ceea ce implică precauții și o atenție sporită la mișcăriile pe

orizontală (axa Z) ale giroscopului.

Figura 3.13 Mișcare aleatoare pe axa Z a giroscopului

În cazul unor mișcării mai precise s-a considerat întâi mișcarea pe orizontală, axa Z.

Au fost luate două cazuri separate și anume, mișcarea spre stânga și cea spre dreapta pentru a

evidenția mai exact sensurile și semnele datelor achiziționate.


52

Figura 3.14 Rotire spre dreapta în plan orizontal

În figura 3.14 se evidențiază faptul că la rotirea pe orizontală a giroscopului axele X

și Y rămân în jurul unor valori apropiate de zero, cu mici deviații. Acest lucru se datorează

faptului că aceste 2 axe sunt perpendiculare pe axa Z.

În următoarea reprezentare se execută rotirea în sens contrar, dar în același plan. Aici

este ilustrat mult mai bine că valorile citite pe Z spre stânga sunt negative.

Figura 3.15 Rotire spre stânga în plan orizontal

Ultimele două grafice au reprezentat mișcarea pe axa Z, urmând ca acum să prezint

felul în care achiziția datelor de la giroscop influențează valorile axelor X și Y.


53

Figura 3.16 Rotire progresivă în jurul axei X

Rotirea în jurul axei X indică modificări nu numai la nivelul acesteia, ci și la nivelul

axei Y deoarece cele două se află în același plan, valorile lor compunând mișcarea. Variația

lui Z ar trebui să se apropie de zero, ceea ce se și întâmplă pentru datele filtrate.

Figura 3.17 Rotire progresivă în jurul axei Y

În cazul figurii 3.17 este reprezentată o situație asemănătoare cu cea a rotirii în jurul

axei X.

O observație de luat în seamă este faptul că rotirile în jurul axelor X și Y au fost

realizate fără un suport precis al axei de referință (așa cum este cazul lui Z), astfel putând fi

introduse erori considerabile de poziționare a senzorului. Din această cauză, valorile de la nici


54

una dintre axe nu pot fi considerate zero (normal ar fi ca celelalte două axe care nu sunt

implicate în rotație să aibă valori ≈ 0 grade/s).

3.3 Scenariu de test

Pentru a valida soluția de implementare propusă, se consideră un scenariu de test ce

are drept scop studierea și validarea funcționalitățiilor sistemului.

Cazul particular al scenariului de test a fost ales deoarece montajul prezintă anumite

limitări mecanice și spațiale care nu permit deplasarea acestuia în siguranță. Aceste

contrângeri țin de cadrul fizic al montajului căruia nu-i pot fi impuse orice tipuri de mișcări,

camera video care are și ea niște parametrii pe care trebuie să-i ia în considerare, precum și

servomotoarele care nu sunt capabile să acopere o rotație de 360º pe toate cele trei axe X, Y

și Z.

Astfel, important pentru acest scenariu ales este stabilirea ipotezelor de lucru care

duc la validarea sistemului implementat. Pentru aceasta, trebuie luate în seamă câteva aspecte

ce țin de integrarea acestui subsistem în sistemul final al vehiculului.

Dat fiind faptul că este vorba de o dronă ce se deplaseză în mediul aerian, vizarea pe

care o presupune camera video aflată în cadrul acesteia, unghiul pe care trebuie să-l mențină

camera față de un anumit punct de referință este invers proporțional cu distanța dronei față de

Pământ. Mai precis, s-a ales studiul mișcării la variații mici tocmai în prespectiva faptului că

unghiurile pe care va trebui să le comprenseze regulatorul vor avea valori din ce în ce mai

mici. Desigur, va necesita testare și la implemetarea finală, dar aceasta nu face studiul lucrării

de față.

Scenariul de test presupune limitarea mișcărilor pe care le poate efectua montajul

astfel încât să se rezume la un perimetru precis. Perimetrul în care s-a decis validarea

implementării este reprezentat de un spațiu pătrat de 10 x 10 cm, pe direcția orizontală. Acest

perimetru se consideră zona activă în care regulatorul își poate atinge performanțele maxime.

Figura 3.18 Zona activă de test

În cele ce urmează, sunt prezentate rezultatele obținute în urma testelor aplicate

servomotoarelor. Aceste rezultate urmăresc verificarea funcționalității elemetelor de execuție,

dar și a răspunsului corect al sistemului funcție de modificarea valorilor accelerometrului și

giroscopului.

În figura următoare este prezentată maniera în care se modifică comanda sistemului

pe toate cele 3 servomotoare. Se observă că aceasta crește sau scade cu un factor de

10 cm

10 cm

Punct vizat (referința

sistemului)


55

proporționaliate foarte mic. În plus, pe acest grafic, se observă o urmărire parțială a referinței

impuse și anume poziția mediană a tutoror servomotoarelor tradusă prin:

yaw = 90º;

pitch = 90º;

roll = 90º.

Figura 3.19 Evoluția comenziilor celor 3 motoare funcție de timp

S-au mai considerat încă două situații posibile având aceiași parametrii. Astfel,

aceste sunt ilustrate mai jos:

Figura 3.20 Evoluție pe perioadă dublă de timp

În figura 3.20, ca și în 3.19 se observă o saturare a comenzii servomotorului direcției

“yaw”. Acest lucru este o consecință a faptului că accelerometrului nu-i pot fi eliminate toate

erorile și se întâmplă ca în unele situații acesta să ajungă să indice același semn pentru 2

accelerații de sensuri diferite.


56

Figura 3.21 Evoluție funcție de timp (urmărire “yaw”)

Principiul schemei de conducere este redat prin intermediul reprezentării realizate

în figura următoare. S-au folosit elemente tip accelerometru și giroscop cu 3 axe, filtrul

Kalman predefinit, precum și restul componentelor pentru închiderea buclei de reglare.

Figura 3.22 Schemă bloc de reglare

Având drept scop reglare pe direcția orizontală, parametrii cu care se lucrează sunt

axele X și Y ale accelerometrului și axa Z a giroscopului. Astfel, aceștia reprezintă mărimile

funcție de care se stabilește o eroare și se calculează noua comandă pentru fiecare servomotor

în parte.

Se constată, în urma testelor, că există situații în care comanda a cel puțin unui motor

se poate satura din cauza măsurătorilor eronate ale senzorilor, dar și a simplității regulatorului

care acumulează erori, astfel încât eroare staționară crește progrseiv.

Referință poziție

servomotoare

Achiziție și

calculul erorii

Filtru

Kalman

Proporțional

(P)

Model

Sistem

+

-

Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII

57

CAPITOLUL IV

CONCLUZII

4.1 Concluzii

Lucrarea de față urmărește stabilizarea unei platforme de fotografiere aeriană în

cazul aparițiilor unor perturbații, ținându-se cont că aceasta va fi montată pe un vehicul aerian

tip dronă.

Scopul lucrării este de a menține direcția de vizarea reprezentate de un punct fixat,

problemă esențială în cadrul oricărui sistem de zbor autonom tip UAV.

Pentru realizarea unui proiect trebuie fixate obiectivele și metodologia de

implementare. Obiectivele planificate nu sunt totdeauna ușor de realizat și pot fi întâlnite

multe obstacole, unele previzibile, altele nu. Practica a dovedit că fără o planificare riguroasă

proiectele complexe nu pot fi realizate, implementarea lor necesitând o bună pregatire încă de

la început.

Existența multiplelor tipuri de UAV face ca performanțele dorite să difere în funcție

de aplicație, de mediul în care sunt folosite, precum și de componentele utilizate. Însă, toate

au un numitor comun și anume siguranța și securiatate sistemului de zbor construit.

Componentele electronice folosite în industria aviatică trebuie să atingă un anumit grad de

siguranță întrucât orice funcționare anormală a unei dintre acestea poate duce la catastrofe ce

pot implica sau nu pagube.

Pentru realizarea lucrării au fost consultate multiple surse al literaturii de specialitate

(articole, studii de caz, cărți) pentru a-mi putea crea o imagine de ansamblu asupra problemei

urmărite și a unei posibile rezolvări. Astfel, s-au analizat diferite variante de implementare

pentru sistemul propus, alegând în final una în concordanță cu obiectivele, echipamentelor,

timpul și costurile implicate.

Elaborarea proiectului a necesitat mai multe etape:

analiza variantelor de implementare și a echipamentelor necesare;

dezvoltarea mecanică a sistemului în colaborare cu ITA;

realizarea achiziției de date;

implementarea algoritmului de reglare;

testarea și validarea implementării.

În vederea implementării practice, au fost alese echipamentele potrivite pentru timpul

de aplicație ținând cont de raportul cost-performanță.

Sistemul realizat este constituit dintr-o platformă demonstrativă pe care sunt atașate

următoarele subsisteme: subsistemul de achiziție a datelor, de procesare a datelor și

subsistemul de execuție.

Alegerea senzorilor a reprezentat un punct esențial deoarece ei sunt cei care oferă un

feedback în cadrul sistemului. S-a optat pentru un accelerometru și un giroscop ce oferă ieșiri

pe toate cele 3 axe (ADXL 345 Breakout și ITG-3200 Breakout). Pe lângă aceștia, modelul

microcontroler-ului ales trebuie să ofere suportul de procesare necesar unui astfel de sistem.

În acest caz, s-a optat pentru Arduino Duemilanove, acesta fiind un microcontroler compatibil

cu senzorii aleși și având o manieră de programare facilă prin software-ul oferit, Arduino

IDE.


58

Un alt aspect a fost alegerea elementelor de execuție, a servomotoarelor. Acestea

sunt 3 servomotoare analogice ce îndeplinesc bine cerințele montajului realizat. Pentru

proiectarea montajului a fost necesară modificarea modalității de conectarea a unora dintre

servomotoare.

Soluția de conectarea a tuturor echipamentelor utilizate a vizat funcționalitatea,

precum și acuratețea datelor achiziționate astfel încât atât microcontroler-ul, cât și senzorii

împreună cu breadboard-ul suport au fost montați la unison pe cadrul metalic.

Achiziția senzorilor este aspectul primordial atins. Accelerometrul și giroscopul

recepționează date care ulterior au fost preluate și scalate pentru a le da o semnificație fizică și

a le putea folosi în procesul de reglare a poziției camerei. Pentru a obține date cât mai corecte,

s-a urmărit implemetarea unui filtru. Într-o primă etapă s-a utilizat un filtru complementar,

mai simplu, dar cu o performanță scăzută din punct de vedere al filtrarii. Astfel, în final s-a

pentru implementarea unui filtru Kalman pentru fiecare dintre valorile achiziționate de la

senzori.

În urma testelor efectuate folosind servomotoarele, s-au impus limitele maxime

adimisibile pentru aceste elemente în scopul protejării integrității acestora și a montajului.

Pentru efectuarea reglării în scopul menținerii unei referințe de poziție impusă s-a

folosit un algoritm de reglare bazat pe calculul erorii poziției ca variantă inițială. Întrucât acest

regulator introducea erori necontrolabile s-a apelat la implementarea unui regulator tip

porporțional bazat pe calcul diferenței dintre valorile senzorilor. Pragurile pentru care este

aplicată amplificarea au fost determinat experimental.

Implementarea a fost realizată în Arduino IDE, incluzând în program achiziția

datelor, scalarea, afișarea lor, filtrarea, conexiunea cu servomotoarele, calculul și transmiterea

comenzilor elementelor de execuție. Au fost realizate funcții specializate pentru fiecare dintre

aceste funcționalități.

De asemenea, pentru ilustrarea rezultatelor și pentru analiza lor au fost preluate

datele citite prin intermediul portului serial și au fost prelucrate folosind Matlab. Astfel, se pot

realiza comparații și studii pe diverse tipuri de date.

Validarea și testarea funcționalității sistemului s-a efectuat utilizând un scenariu de

test, într-un perimetru delimitat, considerând diferite tipuri de mișcări tip perturbație ce

acționează asupra sistemului. S-a constat că sistemul răspunde corect, dar introduce erori

staționare.

4.2 Consideraţii şi perspective

Aplicațiile ce folosesc sisteme UAV trebuie să asigure un grad ridicat de siguranță și

performanță astfel încât criteriile de calitate specifice vehiculelor aeriene ce dispun de un

operator uman au fost extinse și pentru cele fără pilot.

Utilitatea acestor sisteme este indiscutabilă, dar trebuie avute în vedere atingerea

unor parametrii calitativi care să permită folosirea lor în medii în care există activitate umană

fără a o pune în pericol. Astfel, trebuie considerate toate aspectele ce ar putea duce la

îmbunătățirea performanțelor și calitații.

Pentru proiectul de față se pot avea în vedere câteva direcții de perfecționare și

anume folosirea unor senzori cu un grad mai mare de acuratețe a datelor, modificarea

parametriilor filtrului, înlocuirea microcontroler-ului cu unul cu putere mai mare de procesare,

precum și utilizarea unor motoare de putere mai ridicată.

Sistemul pot fi îmbunătățit prin introducerea unui controller pentru servomotoare în

scopul primirii de feedback din partea acestora. Totodată se pot obține performanțe ridicate în

cazul implementarii unui regulator de tip PID sau fuzzy.


59

Este posibilă și folosirea unui magnetometru și a unui barometru în scopul validării

gradului de acuratețe al accelerometrului și giroscopului. Un aspect inclus în ansamblul final

al proiectului este utilizarea camerei foto/video pentru procesarea de imagini și în scopul

recunoașterii unor ținte fixate.

Toate îmbunătățirile care se poate aduce acestui sistem trebuie să vizeze scopul final

ce presupune montarea cadrului pe un vehicul tip dronă. Așa cum în prezenta lucrare a fost

considerat un scenariu de test, acesta trebuie extins către cazul general pornind de la

principiile enunțate și demonstrate.

Complexitatea și posibilitatea dezvoltării ulterioare a proiectului au constituit

principala motivație a alegerii temei. Această lucrare reprezintă doar un prim pas în ansamblul

final al proiectului, aprofundarea fiind necesară pentru dezvoltările ulterioare.

Irina Diana Vidrașcu BIBLIOGRAFIE

60

BIBLIOGRAFIE

[1] Woodman J. Oliver. August 2007. An introduction to inertial navigation. Tehnical

Report nr.696, p. 5-21. University of Cambridge

[2] Basic principles of inertial navigation. Seminar on inertial navigation. Tampere

University of Technology. Disponibil pe :

http://aerostudents.com/files/avionics/InertialNavigationSystems.pdf, accesat la

15.05.2013

[3] Inertial Navigation. Courses ELEC 4600, Departament of electronics, Carleton

University. Disponibil pe:

http://www.doe.carleton.ca/courses/ELEC4600/Nav_Inertial_Navigation_bw.ppt

accesat la data 01.05.2013

[4] Groves D. Paul. 2008. Principles of GNSS, inertial and multisensor integrated

navigation systems. p. 3-159. Artech House

[5] Dumitrache I. 2005. Ingineria reglării automate. Editura Politehnica Press

[6] Pătrașcu M, Hanchevici A.B. 2011. Sisteme avansate de conducere. Îndrumar de

laborator. Editura Politehnica Press

[7] Varesano F. Aprilie 2011. Using Adruino for tangible human computer interaction.

Departement of informatics, Univerity of Torino

[8] Atif M, Serdaroglu S. 2011. A measurement system for human movement analysis.

Chalmers University of Technology, Sweden

[9] ADXL345Breakout Tutorial. Disponibil pe: https://www.sparkfun.com/tutorials/240

[10] PID Control: A brief introduction and guide using Arduino. Septembrie 2011. Jacobs

School of Engineering. Disponibil pe

http://www.maelabs.ucsd.edu/mae156alib/control/PID-Control-Ardunio.pdf

[11] ITG-3200Breakout Tutorial. Disponibil pe: https://www.sparkfun.com/tutorials/265

[12] Baker W. Ianuarie 2013. Kalman sense. Digital processing of gyroscope and

accelereometer measurements

[13] Wimmer C. Aprilie 2006. Position measurement in inertial systems. p. 4-12

[14] Colton S. Iunie 2007. The balance filter. A simple solution for integrating

accelerometer and gyroscope measurements for a balancing platform. Disponibil pe:

http://web.mit.edu/scolton/www/filter.pdf

[15] ADXL345 Breakout Datasheet. Disponibil pe:

https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf

http://aerostudents.com/files/avionics/InertialNavigationSystems.pdf

http://www.maelabs.ucsd.edu/mae156alib/control/PID-Control-Ardunio.pdf

https://www.sparkfun.com/tutorials/265

http://web.mit.edu/scolton/www/filter.pdf

https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf

Irina Diana Vidrașcu BIBLIOGRAFIE

61

[16] ITG-3200 Breakout Datasheet. Disponibil pe:

https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf

[17] Ettinger S., Nechybal P., Ifju G., Waszak M. Vision-Guided Flight Stability and

Control for Micro Air Vehicles. Departament of electrical and computer engineering,

Departament of aerospace engineering, mechanics and engineering, University of

Florida. Dynamics and control branch, NASA Langley Research Center

[18] Kim J.H., Wishart S., Sukkarieh S. 2006. Real-time navigation, guidance and control

of a UAV using low-cost sensors. Australian Centre for Field Robotics, University of

Sydney

[19] Nasiri S. A critical review of MEMS gyroscopes technology and commercialization

status. InvenSense. California

[20] Specificații și cerințe – temă proiect licență date de ITA (Institutul de Tehnologii

Avansate)

[21] MEMSIC Inc. Inertial Technology and Product. 2012.

Memsic Inertial Products and Tech

[22] What is MEMS Technology? Disponibil pe: https://www.memsnet.org/about/what-

is.html accesat la data 25.05.2013

[23] Servomotor Disponibil pe : http://en.wikipedia.org/wiki/Servomotor accesat la data

20.05.2013

[24] Strickland J. What is a gimbal - and what does it have to do with NASA? Disponibil

pe: http://science.howstuffworks.com/gimbal1.htm accesat la data 20.05.2013

[25] Basta O.P. Mai 2012. Quad Copter Flight. California State University, Northridge

[26] Laborator microcontrolere. Platformă cursuri. Disponibil pe:

http://andrei.clubcisco.ro/cursuri/3pm/lab1.pdf accesat la data 10.06.2013

[27] Budiu M. Decembrie 2000. Discuri micro-electro-mecanice

[28] Manole C., Ionescu D. Micromotoare realizate în tehnologie MEMS

[29] Kada B., Ghazzawi Y. 2011. Robust PID Controller Design for an UAV. World

Congress on Engineering and Computer Science, Vol II, San Francisco, USA

[30] Power HD High-Torque Servo 1501MGSpecifications. Disponibil pe:

http://www.pololu.com/catalog/product/1057 accesat la data 01.05.2013

https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf

https://www.memsnet.org/about/what-is.html%20accesat%20la%20data%2025.05.2013

https://www.memsnet.org/about/what-is.html%20accesat%20la%20data%2025.05.2013

http://en.wikipedia.org/wiki/Servomotor%20accesat%20la%20data%2020.05.2013

http://en.wikipedia.org/wiki/Servomotor%20accesat%20la%20data%2020.05.2013

http://science.howstuffworks.com/gimbal1.htm

http://www.pololu.com/catalog/product/1057

http://www.pololu.com/catalog/product/1057

Irina Diana Vidrașcu ANEXĂ

62

ANEXĂ

În cele ce urmeză este listat scriptul codului folosit pentru relizarea proiectului,

program elaborat în Arduino IDE.

Introducerea codului are alăturată și comentarii făcute la momentul întocmării pentru

a facilita ulteriorele revizuiri ale acestuia și a face înțelegerea lui mai ușoară.

#include <Servo.h>

#include <Wire.h>

// Cabling for i2c using Sparkfun breakout with an Arduino Uno /

Duemilanove:

// Arduino <-> ADXL 345 Breakout board

// Gnd - GND

// 3.3v - VCC

// 3.3v - CS

// Analog 4 - SDA

// Analog 5 - SLC

// Arduino <-> ITG-3200 Breakout board

// Gnd - GND

// 3.3v - VCC

// 3.3v - CS

// 20 - SDA

// 21 - SLC

#define Rad2Deg 57.2957795 //1 rad=57.2957795 deg

#define Deg2Rad 0.0174532925 // 1 deg=0.0174532925 rad

//accel

#define DEVICE (0x53) //adresa accel

//gyro

char itgAddress = 0x69;//adresa gyro

byte _buff[6];

char POWER_CTL = 0x2D; //Power Control Register

char DATA_FORMAT = 0x31;

char DATAX0 = 0x32; //X-Axis Data 0

char DATAX1 = 0x33; //X-Axis Data 1

char DATAY0 = 0x34; //Y-Axis Data 0

char DATAY1 = 0x35; //Y-Axis Data 1

char DATAZ0 = 0x36; //Z-Axis Data 0

char DATAZ1 = 0x37; //Z-Axis Data 1

//gyro

char WHO_AM_I = 0x00;

char SMPLRT_DIV= 0x15;

char DLPF_FS = 0x16;

char GYRO_XOUT_H = 0x1D;

char GYRO_XOUT_L = 0x1E;

char GYRO_YOUT_H = 0x1F;

char GYRO_YOUT_L = 0x20;

char GYRO_ZOUT_H = 0x21;

char GYRO_ZOUT_L = 0x22;


63






float xa, ya, za; //acce;

float xg, yg, zg;//gyro

Servo servoY;

Servo servoP;

Servo servoR;

float actualY, actualP, actualR; //pozitie actuala servo Y, P, R

float lastY, lastP, lastR; // ultima pozitie a servo

float errorY, errorP, errorR; // diferenta actual-last pozitie servo

float setY, setP, setR; // referinta a celor 3 servo

float integralY, integralP, integralR; //param. PID

float driveY, driveP, driveR;//comanda pe cele 3 servo

float IntThresh; //ce e?

float kP=-0.1591;

float kI=-0.0157;

float kD=0.0029; //de test

//Kp: 19.56 Ki: 0.71 Kd: 134.26

//calcul pozitie

float accel[3]={0,0,0};

float velocity[3]={0,0,0};

float displacement[3]={0,0,0};

float time;

//lucrez doar cu primele doua valori ale vectorilor

float avgAccel[3];

float newAccel[3];

float newSpeed[3];

float avgSpeed[3];

float newDisplacement[3];// iau numai [0]=xa, [1]=ya

float er[3];//[0]=xa, [1]=ya

float newGyro; //zg

float newTime;

float deltaTime;

float alfa=1; //proportinal factor scalare motor Y

float beta=1;

float gama=1;

//filtru accel

double x_acc_f=1; //val filrata

double p_x_acc=1; //eroare estimata

double q_x_acc=0.4; //zgomot proces

double r_x_acc=0.5; // zgomot senzor

double k_x_acc=1; //castig kalman


64

double y_acc_f=1; //val filrata

double p_y_acc=1; //eroare estimata

double q_y_acc=0.4; //zgomot proces

double r_y_acc=0.5; // zgomot senzor

double k_y_acc=1; //castig kalman

double z_acc_f=1; //val filrata

double p_z_acc=1; //eroare estimata

double q_z_acc=0.4; //zgomot proces

double r_z_acc=0.5; // zgomot senzor

double k_z_acc=1; //castig kalman

//filtru gyro

double x_gyro_f=1; //val filrata

double p_x_gyro=1; //eroare estimata

double q_x_gyro=0.4; //zgomot proces

double r_x_gyro=0.5; // zgomot senzor

double k_x_gyro=1; //castig kalman

double y_gyro_f=1; //val filrata

double p_y_gyro=1; //eroare estimata

double q_y_gyro=0.4; //zgomot proces

double r_y_gyro=0.5; // zgomot senzor

double k_y_gyro=1; //castig kalman

double z_gyro_f=1; //val filrata

double p_z_gyro=1; //eroare estimata

double q_z_gyro=0.4; //zgomot proces

double r_z_gyro=0.5; // zgomot senzor

double k_z_gyro=1; //castig kalman

float

v[12]={0.05,0.06,0.07,0.10,0.11,0.12,0.13,0.27,0.28,0.29,0.14,0.08};

int i=1;

void setup()

{

Wire.begin();

Serial.begin(115200);

Serial.print("init");

delay(300);//incalzire

//put the ADXL345 into +/- 4G range by writing the value 0x01 to

the DATA_FORMAT register.

writeTo(DATA_FORMAT, 0x00);

//put the ADXL345 into Measurement Mode by writing 0x08 to the

POWER_CTL register.

writeTo(POWER_CTL, 0x08);

char id=0;

id = itgRead(itgAddress, 0x00);

Serial.print("ID: ");

Serial.println(id, HEX);

servoY.attach(9);


65

servoP.attach(10);

servoR.attach(11);

servoY.write(90);

servoP.write(90);

servoR.write(90);

time = millis();

}

void loop()

{

readAccel();

readGyro();

if((i!=1)&&(i!=2)&&(i!=3)){

readAccel();

readGyro();

//printValoriAccGyro();

//printUtil();

//calcul comanda dpls stg-dr

//boolean ok=true;

//for(int i=0;i<12;i++){

// if(v[i]==y_acc_f) ok=false;

//}

if((alfa>=0)&&(alfa<=2)){

if(y_acc_f<0.0){ alfa-=0.3;

}else if((y_acc_f>0.04)){

alfa+=0.3;

}

servoY.write(alfa*90);

}

if((beta>=0.6)&&(beta<=2)){

if(x_acc_f<0.0){

beta+=0.1;

}else if(x_acc_f>0.06){

if(beta>=0.7)beta-=0.1;//limitare motor P

}

servoP.write(beta*90);

}

if((gama>=0)&&(gama<=2)){

if(z_gyro_f<0.0){

gama-=0.3;

}else if(z_gyro_f>11.0){

gama+=0.3;

}

servoR.write(gama*90);

}

Serial.print(y_acc_f);

Serial.print('\t');

Serial.print(alfa);

Serial.print('\t');

Serial.print(alfa*90);

Serial.print('\t');

Serial.print(x_acc_f);


66

Serial.print('\t');

Serial.print(beta);

Serial.print('\t');

Serial.print(beta*90);

Serial.print('\t');

Serial.print(z_gyro_f);

Serial.print('\t');

Serial.print(gama);

Serial.print('\t');

Serial.print(gama*90);

Serial.println('\t');

}

i=i+1;

//Serial.println(i);

delay(100);

}

void printUtil(void){


Serial.print('\t');


Serial.print('\t');



}

void printEr(void){

for(int i=0;i<2;i++){

// Serial.print("viteza ");

// Serial.print(newSpeed[i]);

// Serial.print('\t');

//

Serial.print("poz: ");

Serial.print(newDisplacement[i]);

Serial.print('\t');

Serial.print("Er: ");

Serial.print(er[i]);

Serial.print('\t');

}

Serial.println("");

//Serial.print("v_u ");

//Serial.print();

}

void readAccel() {//g

uint8_t howManyBytesToRead = 6;

readFrom( DATAX0, howManyBytesToRead, _buff); //read the

acceleration data from the ADXL345

// each axis reading comes in 10 bit resolution, ie 2 bytes.

Least Significat Byte first!!

// thus we are converting both bytes in to one int

int x = (((int)_buff[1]) << 8) | _buff[0]; //shift

int y = (((int)_buff[3]) << 8) | _buff[2];


67

int z = (((int)_buff[5]) << 8) | _buff[4];

xa = x * 0.0039f;

ya = y * 0.0039f;

za = z * 0.0039f;

p_x_acc = p_x_acc + q_x_acc;

k_x_acc = p_x_acc / (p_x_acc + r_x_acc);

x_acc_f = x_acc_f + k_x_acc * (xa - x_acc_f);

p_x_acc = (1 - k_x_acc) * p_x_acc;

p_y_acc = p_y_acc + q_y_acc;

k_y_acc = p_y_acc / (p_y_acc + r_y_acc);

y_acc_f = y_acc_f + k_y_acc * (ya - y_acc_f);

p_y_acc = (1 - k_y_acc) * p_y_acc;

p_z_acc = p_z_acc + q_z_acc;

k_z_acc = p_z_acc / (p_z_acc + r_z_acc);

z_acc_f = z_acc_f + k_z_acc * (za - z_acc_f);

p_z_acc = (1 - k_z_acc) * p_z_acc;

//float x_angle = asin(xg) * 57.2957795;

// daca vreau sa transform din G in grade

delay(100);

}

void writeTo(byte address, byte val) {

Wire.beginTransmission(DEVICE);

Wire.write(address); // send register address

Wire.write(val); // send value to write

Wire.endTransmission();

}

// Reads num bytes starting from address register on device in to

_buff array

void readFrom(byte address, int num, byte _buff[]) {


Wire.write(address); // sends address to read from



Wire.requestFrom(DEVICE, num); // request 6 bytes from device

int i = 0;

while(Wire.available()) // device may send less than

requested (abnormal)

{

_buff[i] = Wire.read(); // receive a byte

i++;

}


}

void itgWrite(char address, char registerAddress, char data)

{

Wire.beginTransmission(address);


68

Wire.write(registerAddress);//tell the I2C address which

register we are writing to

Wire.write(data); //Send the value to write to the specified

register


}

unsigned char itgRead(char address, char registerAddress)

{

//this variable will hold the contents read from the i2c device.

unsigned char data=0;

Wire.beginTransmission(address); //send the register address to

be read.

Wire.write(registerAddress);//send the Register Address


//ask the I2C device for data

Wire.beginTransmission(address);

Wire.requestFrom(address, 1);

if(Wire.available()){ //wait for a response from the I2C device

data = Wire.read(); //save the data sent from the I2C

device

}


return data; //return the data read during the operation

}

int readX(void)

{

int data=0;

data = itgRead(itgAddress, GYRO_XOUT_H)<<8;

data |= itgRead(itgAddress, GYRO_XOUT_L);

return data;

}

int readY(void)

{

int data=0;

data = itgRead(itgAddress, GYRO_YOUT_H)<<8;

data |= itgRead(itgAddress, GYRO_YOUT_L);

return data;

}

int readZ(void)

{

int data=0;

data = itgRead(itgAddress, GYRO_ZOUT_H)<<8;

data |= itgRead(itgAddress, GYRO_ZOUT_L);

return data;

}

void readGyro(){//grade/secunda

int xRate, yRate, zRate;

xRate=readX();

yRate=readY();


69

zRate=readZ();

xg=xRate/14.375f;

yg=yRate/14.375f;

zg=zRate/14.375f;

p_x_gyro = p_x_gyro + q_x_gyro;

k_x_gyro = p_x_gyro / (p_x_gyro + r_x_gyro);

x_gyro_f = x_gyro_f + k_x_gyro * (xg - x_gyro_f);

p_x_gyro = (1 - k_x_gyro) * p_x_gyro;

p_y_gyro = p_y_gyro + q_y_gyro;

k_y_gyro = p_y_gyro / (p_y_gyro + r_y_gyro);

y_gyro_f = y_gyro_f + k_y_gyro * (yg - y_gyro_f);

p_y_gyro = (1 - k_y_gyro) * p_y_gyro;

p_z_gyro = p_z_gyro + q_z_gyro;

k_z_gyro = p_z_gyro / (p_z_gyro + r_z_gyro);

z_gyro_f = z_gyro_f + k_z_gyro * (zg - z_gyro_f);

p_z_gyro = (1 - k_z_gyro) * p_z_gyro;

delay(100);

}

void alg_PID(void){

actualY = analogRead(9);

actualP = analogRead(10);

actualR = analogRead(11);

errorY=setY-actualY;

errorP=setP-actualP;

errorR=setR-actualR;

//?????

if (abs(errorY) < IntThresh){ // prevent integral 'windup'

integralY = integralY + errorY; // accumulate the error

integral

}

else {

integralY=0; // zero it if out of bounds

}

if (abs(errorP) < IntThresh){

integralP = integralP + errorP;

}

else {

integralP=0;

}

if (abs(errorR) < IntThresh){

integralR = integralR + errorR;

}

else {

integralR=0;

}

driveY=errorY*kP+integralY*kI+(lastY-actualY)*kD;

driveP=errorP*kP+integralP*kI+(lastP-actualP)*kD;

driveR=errorR*kP+integralR*kI+(lastR-actualR)*kD;


70

//Drive = Drive*ScaleFactor; // scale Drive to be in the range

0-255

//cine e scale?

// if (Drive < 0){ // Check which direction to go.

// digitalWrite (Direction,LOW); // change direction as

needed

// }

// else { // depending on the sign of Error

// digitalWrite (Direction,HIGH);

// }

if ((abs(driveY)>255)) {

driveY=255;

}

if ((abs(driveP)>255)) {

driveP=255;

}

if ((abs(driveR)>255)) {

driveR=255;

}

servoY.write(driveY);

servoP.write(driveP);

servoR.write(driveR);

//analogWrite (Motor,Drive); // send PWM command to motor board

lastY=actualY;

lastP=actualP;

lastR=actualR;

}

void printValoriAccGyro(void){

//accel

Serial.print("Acc: ");

Serial.print(xa);

Serial.print('\t');


Serial.print('\t');

Serial.print(ya);

Serial.print('\t');

Serial.print("yf");


Serial.print('\t');

Serial.print(za);

Serial.print('\t');

Serial.print(z_acc_f);

Serial.print('\t');

//gyro

Serial.print("Gyro: ");

Serial.print(xg);

Serial.print('\t');

Serial.print(x_gyro_f);

Serial.print('\t');

Serial.print(yg);


71

Serial.print('\t');

Serial.print(y_gyro_f);

Serial.print('\t');

Serial.println(zg);

Serial.print('\t');



}

void ramasite(void){

newTime = millis();

deltaTime = newTime - time;

//studiu de caz al miscarii functie de y_acc_f (orizontal)

//newAccel[0]=x_acc_f; //xa

//newAccel[1]=y_acc_f; //ya

//newGyro=z_gyro_f;

// if(y_acc_f>0.03){

// alfa=alfa-0.3;

// }else if(y_acc_f<0){

// alfa=alfa+0.3;

// }

// servoY.write(alfa*90);

//newAccel[2]=z_acc_f;

//calcul pozitie pe y

//calcul pozitie, varianta initiala

// for(int i=0; i<2; i++){

//

// newSpeed[i] =(newAccel[i] * deltaTime);

// newDisplacement[i] = newSpeed[i] * deltaTime;

// er[i]=newDisplacement[i]-displacement[i];

//

// displacement[i] = newDisplacement[i];

//

// time = newTime;

//

//

//

// }

// printEr();

//if(zg<0) servo

//alg_PID();

//sensurile nu sunt foarte exacte, nu merge foarte bine

}

void filter(){

gyroRateX=-1.0*dtime*xg;// calcul e(t)=integrala(viteza

unghiulara)

gyroRateY=dtime*yg;

gyroAngleZ_dt=dtime*zg;

//gyroAngleZ +=-1.0*gyroAngleZ_dt*(1/(cos(Deg2Rad*Roll)))

// if(GyroAngleZ<0) GyroAngleZ+=360;

// if(GyroAngleZ>=360) GyroAngleZ-=360;

Serial.print("GyroRateX: ");

Serial.println(gyroRateX);


72

Serial.print(" GyroRate: ");

Serial.println(gyroRateY);

Serial.print(" Unghi: ");

Serial.println(gyroAngleZ_dt);

}

Cod Matlab :

function afisare_a fid=fopen('misc_vert.txt','r'); %read data from file A=textscan(fid,'%f%f%f%f%f%f');

x_a=A{1}; x_a_f=A{2}; y_a=A{3}; y_a_f=A{4}; z_a=A{5}; z_a_f=A{6}; acc=[x_a,x_a_f,y_a,y_a_f,z_a, z_a_f];

fclose(fid); t=0:1:214; %t=t(1:116); figure(1); title('Accelerometru deplasare spre stanga'); subplot(3,1,1); plot(t,x_a,'r'); xlabel('timp [s]'); ylabel('Acceleratie_x [g]'); hold on plot(t,x_a_f,'b'); hold on legend('x','x(filtrat)','Location','EastOutside'); title('Accelerometru pe x functie de timp'); figure(2); subplot(3,1,2); plot(t,y_a,'r'); xlabel('timp [s]'); ylabel('Acceleratie_y [g]'); hold on plot(t,y_a_f,'b'); hold on

legend('y','y(filtrat)','Location','EastOutside'); title('Accelerometru pe y functie de timp');

figure(3); subplot(3,1,3); plot(t,z_a,'r'); xlabel('timp [s]'); ylabel('Acceleratie_z [g]'); hold on plot(t,z_a_f,'b'); hold on legend('z','z(filtrat)','Location','EastOutside'); title('Accelerometru pe z functie de timp');

Universitatea Politehnica Bucureşti Facultatea de...

Documents

Transcript of Universitatea Politehnica Bucureşti Facultatea de...