Sunteți pe pagina 1din 79

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 18

1.5.2. Algoritmi neconvenționali 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.


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

0.2 Grafic Gantt al proiectului 3


[4]
1.1. Sistem de referință raportat la Pământ 4
[4]
1.2. Sistem de referință local 4
[1]
1.3. Sistemul de referință global și cel atașat vehiculului. 5
[4]
1.4. Sistem de referință vehicul (dronă) – direcții de mișcare 6
[24]
1.5. Articulații cardanice 6
[1]
1.6. Platformă stabilă (cu articulații cardanice) 7
[1]
1.7. Algoritmul navigației inerțiale al unei platforme stabile 7
[1]
1.8. Algoritmul navigației inerțiale al unui sistem strapdown 8
[1]
1.9. Schemă sistem “strapdown” cu explicitarea transformării axelor 9
[22]
1.10. Componente MEMS 10
[21]
1.11. Plăcuță MEMS 10
[4]
1.12. Structura unui accelerometru clasic 11
[1]
1.13. Componența accelerometrului de tip SAW 12
[4]
1.14. Variantă a accelerometrului cu două grinzi 12
[1]
1.15. Giroscop mecanic convențional 13
[1]
1.16. Efectul Sagnac 14
[4]
1.17. Descriere RLG 15
[4]
1.18. Componente FOG 15
[1]
1.19. Accelerația Coriolis 16
[5]
1.20. Structura 17

1.21. Legătura senzori - 17


Irina Diana Vidrașcu LISTA FIGURILOR

1.22. Schemă principiu PID 19

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


cu Ik=indice de performanță [6]
2.1. Structură montaj 22

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


miniaturizate [7]
2.3. Detaliu structură accelerometru MEMS [8] 23

2.4. Accelerometrul ADXL345 Breakout 23


[9]
2.5. Schemă conectare la microcontroler 24
[16]
2.6. Axele de rotație ITG-3200 Breakout 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
(sony.ro)
2.18. Cameră video FCB-EX480CP 33

2.19. Camera în ansamblul dezvoltat 33


[14]
2.20. Schemă filtru complementar
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
Irina Diana Vidrașcu LISTA FIGURILOR

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
Irina Diana Vidrașcu LISTA FIGURILOR

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

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ă.
1
Irina Diana Vidrașcu INTRODUCERE

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.

Condiţii

IMU

Giroscop Poziţie
Procesor
Viteză unghiulară
Accelerometru

Model gravitaţional

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.

2
Irina Diana Vidrașcu INTRODUCERE

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

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

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


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

 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].

Zb Zg

INS
yb

xg
yg
INS
xb

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.

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

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

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

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

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

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.

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

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.

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

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

Structuri
miniaturizate

Micro- Componente
senzori MEMS microelectronice

Micro-
actuatoare

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.
10
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

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;
11
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

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

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

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ă
13
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

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

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

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].
15
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

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.

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

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.

Software

Microcontroler/ PC Elemente de
excuție
(Servomotoare)

Senzori

Figura 1.21 Legătura senzori -

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

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

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

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.

r(t) (t) y(t)


I

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.
19
Irina Diana Vidrașcu CAPITOLUL I – CONSIDERAȚII TEORETICE

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.

20
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.
21
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

Î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.
22
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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

23
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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;

24
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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

25
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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_CFG_2 = 1<<2;
char DLPF_FS_SEL_0 = 1<<3;
char DLPF_FS_SEL_1 = 1<<4;
 adresa dispozitiviului I2C
char itgAddress = 0x69;

26
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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
27
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

28
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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:

ARDUINO

Achiziție Filtrare Calcul Transmisie


date date comandă comandă
servomotoare

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.

29
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

30
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

31
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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)

32
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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

33
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

ACCELEROMETRU
Filtru Trece – Jos
(FTJ)  Unghi de
rotație

Filtru Trece – Sus


(FTS)

Integrare
Numerică

Viteză
GIROSCOP unghiulară

Figura 2.20 Schemă filtru complementar

34
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

35
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

36
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

Estimare inițială Măsurări

Câștig Kalman Actualizare Actualizare


estimare covarianță

k+1
Estimare
nouă

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.

37
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

38
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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.

39
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

M1 M2 M3
YAW PITCH ROLL

Y X Z

O
X
O 1800
Z Y
00 1800
1600

00
500

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.

40
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

SETARE POZIȚIE
INIȚIALĂ

CITIRE VALORI ACCELEROMETRU


CU CALCULAREA RATELOR PE 3
AXE

MEDIE CITIRI GIROSCOP


CALCUL RATE 3 AXE

CALCUL 

CALCUL
COMENZII

ÎNCADRARE NU LIMITARE
COMANDĂ MINIMĂ/MAXIMĂ A
SERVO COMENZII

DA

PRIMIRE COMENZI
SERVO
FEEDBACK

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:

41
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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

42
Irina Diana Vidrașcu CAPITOLUL II – DETALIEREA PLATFORMEI DEZVOLTATE

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

43
Irina Diana Vidrașcu CAPITOLUL III - REZULTATE

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

44
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

45
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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

46
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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

47
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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ă

48
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

Atenuare spike-uri

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

49
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

50
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.
51
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

52
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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
53
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

10 cm

10 cm

Punct vizat (referința


sistemului)

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
54
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

55
Irina Diana Vidrașcu CAPITOLUL III – REZULTATE

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.

Referință poziție + Achiziție și Filtru Proporțional Model


servomotoare - calculul erorii Kalman (P) Sistem

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.

56
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII

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.

57
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII

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.

58
Irina Diana Vidrașcu CAPITOLUL IV - CONCLUZII

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.

59
Irina Diana Vidrașcu BIBLIOGRAFIE

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

60
Irina Diana Vidrașcu BIBLIOGRAFIE

[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

61
Irina Diana Vidrașcu ANEXĂ

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;

62
Irina Diana Vidrașcu ANEXĂ

char DLPF_CFG_0 = 1<<0;


char DLPF_CFG_1 = 1<<1;
char DLPF_CFG_2 = 1<<2;
char DLPF_FS_SEL_0 = 1<<3;
char DLPF_FS_SEL_1 = 1<<4;

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

63
Irina Diana Vidrașcu ANEXĂ

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);
64
Irina Diana Vidrașcu ANEXĂ

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);
65
Irina Diana Vidrașcu ANEXĂ

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(x_acc_f);
Serial.print('\t');
Serial.print(y_acc_f);
Serial.print('\t');
Serial.print(z_gyro_f);
Serial.println('\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];
66
Irina Diana Vidrașcu ANEXĂ

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.beginTransmission(DEVICE);
Wire.write(address); // sends address to read from
Wire.endTransmission();

Wire.beginTransmission(DEVICE);
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++;
}
Wire.endTransmission();
}

void itgWrite(char address, char registerAddress, char data)


{

Wire.beginTransmission(address);
67
Irina Diana Vidrașcu ANEXĂ

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
Wire.endTransmission();
}

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
Wire.endTransmission();

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

}
Wire.endTransmission();
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();
68
Irina Diana Vidrașcu ANEXĂ

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;

69
Irina Diana Vidrașcu ANEXĂ

//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(x_acc_f);
Serial.print('\t');

Serial.print(ya);
Serial.print('\t');
Serial.print("yf");
Serial.print(y_acc_f);
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);
70
Irina Diana Vidrașcu ANEXĂ

Serial.print('\t');
Serial.print(y_gyro_f);
Serial.print('\t');

Serial.println(zg);
Serial.print('\t');
Serial.print(z_gyro_f);
Serial.println('\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);
71
Irina Diana Vidrașcu ANEXĂ

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');

72