Proiect de diplomă [629904]

Universitatea “Politehnica” din București
Facultatea de Electronică, Telecomunicații și Tehnologia Informației

Proiect de diplomă
Dezvolta rea unui sistem stabil de echilibrare și control pentru roboț i

prezentat ca cerință parțială pentru obținerea tit lului de
Inginer în domeniul Electronică și Telecomunicații
programul de studii de licență Rețele și Software de Telecomunicații

Conducător științif ic Absolvent: [anonimizat].Univ.Dr.Ing Corneliu BURILEANU Valentin Gabriel BADEA

2016

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Copyright © 2016 , Valentin Gabriel Badea

Toate drepturile rezervate .

Autorul acordă UPB dreptul de a reproduce și de a distribui public copii pe h ârtie sau
electronice ale acestei lucrări, în formă integrală sau parțială.

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Introducere ………………………….. ………………………….. ………………………….. ………………………….. ………………… 15
1. Blocurile funcționale ale sistemului ………………………….. ………………………….. ………………………….. ….. 17
1.1. Infineon XMC4700 Relax Kit ………………………….. ………………………….. ………………………….. ………. 17
1.2. Senzorul MEMS (MPU 9150) ………………………….. ………………………….. ………………………….. ………. 18
1.3. Determinarea unghiului ………………………….. ………………………….. ………………………….. ……………… 19
1.4. Proiectarea f iltrului Kalman discretizat ………………………….. ………………………….. …………………….. 20
1.5. Modularea PWM ………………………….. ………………………….. ………………………….. ……………………….. 27
1.6. Puntea H ………………………….. ………………………….. ………………………….. ………………………….. ………. 28
1.7. Motorul brushed DC ………………………….. ………………………….. ………………………….. …………………… 29
1.8. Bateria ………………………….. ………………………….. ………………………….. ………………………….. …………. 29
2. Comunicații Seriale. Inter -IC (I2C) ………………………….. ………………………….. ………………………….. ….. 31
2.1. Caracteristicile de bază ale interfeței I2C ………………………….. ………………………….. ………………….. 31
2.2. Avantajele interfeței I2C ………………………….. ………………………….. ………………………….. ……………… 31
2.3. Protocolul I2C ………………………….. ………………………….. ………………………….. ………………………….. . 32
2.4. Descrierea pachetului transmis prin protocolul I2C ………………………….. ………………………….. ……. 33
3. Modelarea sistemului ………………………….. ………………………….. ………………………….. ………………………. 35
3.1. Modelul liniar al unui motor de curent continuu (DC) ………………………….. ………………………….. … 35
3.2. Modelul robotului ………………………….. ………………………….. ………………………….. ………………………. 37
4. Implementarea controlului și optimizarea PID ………………………….. ………………………….. ……………… 43
4.1. Analiza stării inițiale ………………………….. ………………………….. ………………………….. ………………….. 43
4.2. Controlerul PID ………………………….. ………………………….. ………………………….. …………………………. 44
4.3. Metoda Ziegler -Nichols ………………………….. ………………………….. ………………………….. ………………. 45
4.4. Reglare PID bazată pe funcția de transfer ………………………….. ………………………….. …………………. 46
4.5. Optimizare PID prin metoda evoluție i diferențiale ………………………….. ………………………….. ……… 48
5. Rezultate ………………………….. ………………………….. ………………………….. ………………………….. …………….. 53
Concluzii ………………………….. ………………………….. ………………………….. ………………………….. ……………………. 61
Bibliografie ………………………….. ………………………….. ………………………….. ………………………….. ………………… 63
Anexe ………………………….. ………………………….. ………………………….. ………………………….. ………………………… 65

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Cuprins Figuri
Fig. 0.1 Forțele ce acționează asupra unui pendul inversat cu roți ………………………….. …………………………. 16
Fig. 1.1 Schema bloc a sistemului ………………………….. ………………………….. ………………………….. ……………… 17
Fig. 1.2 Schema al goritmului predictor -corector ………………………….. ………………………….. ……………………… 23
Fig. 1.3 Captură osciloscop D ~ 60% ………………………….. ………………………….. ………………………….. ………… 27
Fig. 1.4 Schema conexiunii PWM – punte H ………………………….. ………………………….. ………………………….. .. 28
Fig. 1.5 Schema de funcționare generală a punții H ………………………….. ………………………….. …………………. 28
Fig. 2.1 Captură cu analizorul de semnale a bus -ului I2C ………………………….. ………………………….. ………… 34
Fig. 2.2 Ca ptură osciloscop a bus -ului I2C ………………………….. ………………………….. ………………………….. … 34
Fig. 2.3 Utilizarea validă a i nterfeței I2C ………………………….. ………………………….. ………………………….. …… 32
Fig. 2.4 Secvențele de S TART și STOP ………………………….. ………………………….. ………………………….. ………. 33
Fig. 3.1 Diagrama unui motor de curent continuu ………………………….. ………………………….. ……………………. 35
Fig. 3.2 Forțele implicate în modelarea roților ………………………….. ………………………….. ……………………….. 37
Fig. 3.3 Diagrama de corp liber a șasiului ………………………….. ………………………….. ………………………….. …. 39
Fig. 4.1 Răspunsul sistemului la impuls în buclă deschisă ………………………….. ………………………….. ………… 43
Fig. 4.2 Graficul Pol -Zero ………………………….. ………………………….. ………………………….. ………………………… 43
Fig. 4.3 Schema algoritmului PID ………………………….. ………………………….. ………………………….. …………….. 44
Fig. 4.4 Influența K i asupra sistemului ………………………….. ………………………….. ………………………….. ………. 45
Fig. 4.5 Răspunsul la impuls cu algoritmul PID aplicat ………………………….. ………………………….. ……………. 46
Fig. 4.6 Răspunsul algoritmului PID la perturbații ………………………….. ………………………….. ………………….. 47
Fig. 4.7 Inițializare a populație ………………………….. ………………………….. ………………………….. …………………. 49
Fig. 4.8 Generarea perturbației x r1 – xr2 în mod aleator ………………………….. ………………………….. …………….. 49
Fig. 4.9 Mutația diferenței vectorilor aleși aleat or cu un al treilea, la fel ales aleator ………………………….. . 50
Fig. 4.10 Selecția. Deoarece u 0 este mai performant decât 0 îl înlocuiește în generațiile viitoare …………… 50
Fig. 4.11 Un nou proces este evaluat ………………………….. ………………………….. ………………………….. …………. 51
Fig. 4.12 Elementul 1 este mai bun decât elementul cu care concurează, u 1 și nu va fi înlocuit ………………. 51
Fig. 5.1 Măsurători zgomotoase din cauza oscilației foarte mari ………………………….. ………………………….. . 54
Fig. 5.2 Varianța pentru poziția centrului de greutate ridicat ………………………….. ………………………….. ……. 55
Fig. 5.3 Spațiul 3D cu zgomotul eliminat ………………………….. ………………………….. ………………………….. ……. 55
Fig. 5.4 Performanța celor 300 de configurații individuale testate ………………………….. …………………………. 56
Fig. 5.5 Direcția de convergență a testelor ………………………….. ………………………….. ………………………….. …. 57
Fig. 5.6 Descoperirea punctelor cu o performanță îmbunătățită în spațiul 3D ………………………….. …………. 57
Fig. 5.7 Graficul performanței în funcție de K D și K P ………………………….. ………………………….. ……………….. 58
Fig. 5.8 Varianța unghiului ………………………….. ………………………….. ………………………….. ………………………. 59
Fig. 5.9 V arianța unghiului, zoom -in ………………………….. ………………………….. ………………………….. …………. 59

Cuprins Tabele
Tab. 4.1 Coefici enții de tunare Ziegler -Nichols ………………………….. ………………………….. ………………………… 45
Tab. 5.1 Cele mai bune configurații ………………………….. ………………………….. ………………………….. ……………. 58

Facultatea de Electronică,
Telecomunicații și Tehnologia
Informației

Lista abrevierilor

𝛼 – Accelerația unghiulară a axului
(rad/ s2)
a – Acceleraț ie (m/s)
𝐵 – Matricea de controlul de intrare
𝜃,𝜃𝑝 – Unghiul de înclinare al
robotului(rad )
𝜃̇𝑤 – Viteza unghiulară a roții (rad/s)
𝜃̇𝑏 – Bias (eroarea sistematică ) [1]
Kp – Constanta proporțională
Ki – Constanta integrală
Kd – Constanta derivativă
𝑘𝑓 – Constanta de frecare (Nms/rad)
𝑘𝑚 – Constanta de cuplu (Nm/A)
𝑘𝑒 – Constanta de tensiune inversă
(Vs/rad)
𝐾𝑘 – Câștigul filtrului Kalman
CL, CR – Cuplul aplicat de la motor la roți
𝜏𝑚 – Cuplul motorului (Nm)
𝜏𝑒 – Cuplul aplicat (Nm)
𝐷 – Factorul de umplere (%)
e(t) – Funcția de eroare
𝐹 – Matricea modelului de tranziție al
stărilor
g – Accelerația gravitațională (N/s2)
𝐺𝑝 – Matricea de rotaț ie
HL, HR, PL, PR – Forțele de reacție dintre
șasiu și roți
HfL, HfR – Forțele de frecare ale roților
𝐻 – Matricea m odelului de observaț ie
I – Curentul prin armături (A)
𝐼 𝑤 – Momentul de inerție al roților 𝐼𝑝 – Momentul de inerție al șasiului
IR – Inerția rotorului (kg/m2)
l – Distanța dintre centrul de greutate
al robotului și axul r oților
L – Inductanța rotorului (H)
𝑀𝑤 – Masa ambelor roți ale robotului
𝑀 𝑝 – Masa șasiului robotului
𝑃 – Matricea de covarianță a erorii
𝑃𝑘|𝑘−1 – Matricea de covarianță a erorii a
priori
𝑃𝑘−1|𝑘−1 –Matricea de covarian ță a erorii
precedente
R – Rezistența terminalelor (Ohm)
𝑄𝑘 – Matricea de covarianță a
zgomotului de proces
𝑆𝑘 – Matricea de covarianță a inovaț iei
𝑉𝑎 – Tensiunea aplicată (V)
𝑉𝑒 – Tensiunea inversă (V)
𝜗𝑘 – Zgomotul măsur ătorii
𝑥̂ 𝑘|𝑘−1 – Starea a priori
𝑥̂ 𝑘|𝑘 – Starea a posteriori
𝑥̂ 𝑘−1|𝑘−1 – Starea precedentă
𝑦̃𝑘 – Inovaț ia sau reziduu
𝜔 – Viteza unghiulară a axului (rad/s)
𝑢𝑘 – Intrare de control
𝜔̇̇𝑘 – Zgomotul de proces
𝑥̂𝑘 – Starea la momentul 𝑘
ZMP – Punctul de moment zero
𝑧𝑘 – Măsurătoare discret ă

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

14

Facultatea de Electronică,
Telecomunicații și Tehnologia Introducere
Informației

15

Introducere

Încă din Antichitate, oamenii au visat la obiecte care să le facă viața mai ușoară, să muncească în
locul lor și care să se supună înt ru totul comenzilor. Așa s -a născut dorința de a crea și de a imagina
soluții care să îi ajute. Din dorința confortului și nu numai, oamenii au început încet -încet să
contureze primii roboți. În secolul al XV -lea, Leonardo da Vinci deschide calea roboticii schițând
primul umanoid [2]. Între secolele XVIII -XIX, revoluția industrială dezvoltă nenumărate
automatizări care ajută la evoluția omului și a tehnologiilor. Primul robot autonom apare la
începutul secolului al XX -lea în Japo nia [3]. În anii 1950 apar primele idei de a crea roboți pentru a
fi trimiși pe Lună, iar până în 1969 aceștia îl însoțesc pe Neil Armstrong la prima as elenizare din
istoria omenirii. După anii 1990 piața roboților crește expon ențial, ajungând în prezent la o
multitudine de modele, care deservesc diferite scopuri.

Roboții cu 2 roți au apărut din nevoia omului de a reduce cât mai mult componentele și de a
optimiza produ cția, obținând astfe l aceleași performanțe ca un robot cu 4 roți. Mai mult, aceștia pot
permite manevrabilitate suplimentară , navigare ușoară pe dife rite terenuri, schimbarea direcției cu
un unghi redus și chiar rularea pe trot uare ș i peste borduri. Aceste capabilități au atins potențialul de
a rezolva o serie de p rovocări în industrie și societate. De exemplu, un scaun cu rotile motorizat
care utilizează această tehnologie ar da operatorului o mai mare manevrabilitate și acces mai uș or la
locuri le care pot prezenta dificultate crescută pentru oamenii cu afecț iuni m otorii. Se pot produce
cărucio are folosind această tehnologie care pot permit e oamenilor să călătorească pe distanțe scurte,
într-o zonă mică sau în fabrici, spre deosebire de utilizarea unor autovehicule, care sunt mai
poluant e și mai greu de administrat . De asemenea, un robot cu 2 roți poate fi privit ca o platformă
care este capabilă să transporte diferite obiecte nu numai omul .

În funcție de dimensiunea și caracteristicile sale electrice/mecanice, robotul poate deservi un larg
spectru de aplicații de l a cele industriale de stabilizare până la folosirea lui ca mijloc de transport
individual. În lucrarea de față , dimensiunile robotului sunt mici raportate la necesitățile comerciale ,
dar poate fi privit ca un punct de plecare către aplicații mult mai compl icate și ca o machetă de
observa ție pentru fenomenele fizice apărute.
Un sistem d isponibil comercial, “SEGWAY HT” a fost inventat de Dean Kamen [4], care deține
mai mult de 150 de brevete legate de dispozitive medicale, sisteme de control al e climei, precum ș i
un design de elicopter.”SEGWAY HT” este capabil să echilibreze un om care stă pe o platformă în
timp ce traversează terenul cu el. Această inovație utilizează , pentru a menține poziția verticală ,
cinci gi roscoape și o cole cție de alți senzori de înclinație . Doar trei giroscoa pe sunt necesare pentru
întreg sistem ul, senz orii suplimentari au fost incluș i ca o măsură de siguranță. Unicitatea acestor
produse a atras un int eres din partea fanilor de roboț i.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

16

La baza ideii de echilibru al acestui robot se află conceptul pendulului inversat [5](Fig 0.1) .
Problema pen dulului inversat este frecvent întâlnită î n domeniul ingineriei care se ocupă cu
controlul sisteme lor. Unicitatea și multitudinea de t ehnologii derivate din aces t sistem instabil a
captat atenția multor cercetă tori și pasionați de tehnologie din întreaga lume. În ultimii ani,
cercetătorii au aplicat ideea unui model de pendul inversat mobil în diverse pro bleme, cum ar fi
proiectarea mișc ării de mers pe jos pentru roboți i umanoizi, scaune cu roți robotizate și sisteme de
transport personale. Teoria pendulului inversat care este folosit ă în acest proiect poate fi descrisă pe
scurt astfel :
Un pendul inversat va ieși din poziția de echilibru și va imprima sistemului o forță opusă căderii.
Pentru echilibrare este nevoie de o forță egală în modul, pe aceeași direcție, dar de sens contrar
forței imprimate de pendul pentru a -l redresa.
Lucrarea "Comportamentul unui pendul i nversat pe roți pentru transportarea unui obiect ”, [6] arată
interacțiunea forțelor între diverse obiecte și robot prin luarea în considerare a efectelor de
stabilitate datorate acestor forțe. Această cercetare subliniază posibilitatea transportului p rin
cooperare între doi roboți similari și între un robot și un om. Creșterea rapidă a populației în vârstă
în țări precum Japon ia a determinat cercetă torii să dezvolte scaune cu rotile robotizate , ajutând
infirmi i să se deplaseze . Sistemul de control al unui pendul inversat se aplică atunci când sunt
necesare manevre de urcare a bordurilor sau asigurarea unui control mai bun în curbe.

La un nivel mai înalt , T. Sugihara [7] a folosit modelul mișcă rii de deplasare uman ca pendu l
inversat în proiectarea unei metode de generare a rutinei de mers pentru un robot umanoid. Aceasta
controlează centrul de greutate prin manipulare indirectă a punctului de moment zero (ZMP).
Răspunsul î n timp real al metodei of eră roboți lor umanoizi o mobilitate ridicată .
Fig. 0.1 Forțele ce acționează asupra unui pendul
inversat cu roți

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

17

1. Blocurile funcționale ale sistemului

Sistemul este alcătuit din mai multe blocuri funcționale, fiecare având un aport important la forma
finală . Puterea de calcul necesară pentru a funcț iona la o frecvență foarte ridicată este justifica tă
datorită caracteristicii sistemului de lucru în timp real. Astfel, pentru a putea contracara perturbările
din mediu și pentru a stabiliza robotul, este nevoie de un microprocesor care să efectueze calculele
necesare și să comande motoarele într-un timp cât mai scurt . S-a ales în acest proiect
microcontrolerul Infineon XMC4700.

Fig. 1.1 Schema bloc a sistemului
1.1. Infineon XMC4700 Relax Kit

XMC4700 integrează un procesor RISC ARM Cortex M4 , 32 bit, funcțion ând la o frecvență de
144Mhz, are memoria flash de 2048kb, memoria RAM de 352kB și nu are memorie cache [8] . Este
un model construit pe baza arhitecturii Harvard. Harta memoriei suportată de acest procesor este de
4GB. Capabil să ruleze programe de o complexitate medie spre ridicată, poate face față cu succes
operațiilor din mediul industrial, automatizărilor, aplicațiilor de putere, cât și uzului general. În
cazul de față, va fi folosit la o aplicație exp erimentală cu motoare DC ș i un senzor . Cea mai
complexă operație realizată de acest microcontroler este calculul unghiului robotului în timp real.
La acest rezultat, se aplică un filtru Kalman, deoarece datele oferite de senzor au zgomot, fiind
necesară o filtrare pentru a le p utea folosi eficient.
Limbajul de programare folosit în proiect este C . Dimensiunea codului ajunge la 45kB în modul
Debug și scade la 32kB pentru modul Release. Diferența apare datorită informațiilor în plus
necesare în modul Debug (breaking points, variab ile, regiștrii). Întregul soft rul ează o iterație de
control în 10 ms, fiind optimizat cât se poate de mult.
Ca mediu de programare a fost folosit DAVEv4 [9]. Acesta prezintă o utilitate crescută, deoa rece
are module software de bază care fac accesul la funcționalitatea perifericelor microcontrol erului
pentru a putea dezvolta aplicaț ii mai complexe într-un mod facil . Prin urmare, deș i rigiditatea
modului de programare este un dezavantaj, timpul de dezvol tare scade.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

18

1.2. Senzorul MEM S (MPU 9150)

MEMS (eng. Microelectromechanical systems ) este o tehnologie care stă la baza structurilor
electronice de mărimi reduse de ordinul nanometrilor. În cazul actual, regăsim acest concept în
senzorul pentru detectarea unghiului robotului . Princip iul de bază al funcționării accelerometru lui
MEMS este deplasarea unei mase mici (de referință ) gravat ă în suprafața de siliciu a circuitului
integrat și suspendat ă de mici grinzi . În concordanță cu a doua lege a lui Newton (𝐹 = 𝑚𝑎), când
o accelerație este aplicată dispozitivului forța care apare dislocă masa de referință . Grinzile
acționează ca un arc, iar lichidul ( sau aer) prins în interiorul IC acționează ca un amortizor,
rezultând într -un sistem fizic de ordin 2. Aceasta este sursa de informație oferită de accelerometru
care are o lățime de bandă operațională limitată și răspunsul în frec vență neuniform [10].
Valoare a de ieșire a accelerometru lui este un scalar corespunzător cu mărimea vectorului de
accelerație. C ea ma i comună formă de accelerare și cea la care suntem expuși constant, este
accelerația rezultat ă din atracția gravitațională a Pământului. Aceasta este o valoare comună , de
referință , la care se măsoară toate celelalte accelerații g.
Un senzor comun de inerție este accelerometru l, un senzor dinamic capabil de o gamă largă de
detectare. Accelerometre le disponibile pot măsura accelerația în una, două sau trei axe ortogonale.
Ele sunt utilizate în mod obișnuit într -unul din cele trei moduri:

 Pentru măsura rea vitezei și a poziției;
 Ca un senzor de înclinare sau orientare în 2 s au 3 dimensiuni, având ca punct de referință
acceleraț ia gravitațională (1 g = 9.8m / s2);
 Ca un senzor pentru vibrații sau șocuri .

Senzorul InvenSense MPU 9150 are încorporat un gi roscop și un accelerometru. Acesta a fost
proiectat pentru aplicații de putere joas ă, fiind ideal pentru telefoane și aparate mobile, având în
general un consum foarte mic .
Senzorul comunică cu microcontrolerul prin protocolul I2C. Datele de la accelero metru și giroscop
sunt salvate în 12 regiștrii ai senzorului. Pentru a accesa aceste date, microcontrolerul citește în
rafală câte 6 regiștrii de 2 ori.
Dorindu -se o eficiență sporită, se folosește pinul INT de la senzor. Acesta va fi HIGH în momentul
în care senzorul actualizează regiștrii cu valori noi. Prin această metodă se poate implementa o
întrerupere la microcontroler , citind datele senzorului numai cân d aces tea au fost reactualizate,
evitându -se astfel operaț iile redundante.
Frecvența de eșantiona re a accelerometrului este de 1KHz, iar a giroscop ului de 8KHz. Ca soluție
de mijloc, microcontrolerul va citi date le de la senzor cu o frecvență de 1KHz. Ca alternativă se
poate folosi un timer intern pentru a rula funcția de citire a senzorului la un int erval prestabilit, dar
nu mai mic de 1ms. Protocolul I2C prezintă un avantaj față de versiunea de comunicare analog
deoarece are eficiența crescută ș i erorile care pot apărea sunt nesemnificative. Prezentarea
protocolului se poate găsi în capitolul 2.3.

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

19

1.3. Determinarea unghiului

Informația citită de la senzorul MEMS prin I2C reprezintă coordonatele pe axa X, Y, Z a
accelerometrului și a giroscopului [11]. Având în vedere accelerația gravitațională g și accelerația a
pe care accele rometrul o măsoară, putem defini matricea de rotație a robotului în raport cu gravitația
Pământului astfel:
𝐺𝑝=[𝐺𝑝𝑥
𝐺𝑝𝑦
𝐺𝑝𝑧]=𝑅(𝑔−𝑎) (1.1)
Pentru a rezolva această e cuație, trebuie să presupunem că accelerometrul are accelerația inițială
nulă 𝒂=0 și că forța gravitațională a Pământului este perp endiculară pe axa Z, rezultând:
𝐺𝑝=𝑅∗𝑔=𝑅(0
0
1) (1.2)
Plecând de la această ecuație , putem scrie matrice le de rotație pentru axa transversală, longitudinală
și verticală:
𝑅𝑥(𝜙)=( 10 0
0cos𝜙sin𝜙
0−sin𝜙cos𝜙 )
(1.3)
𝑅𝑦(𝜃)=( cos𝜃0−sin𝜃
010
sin𝜃0 cos𝜃 )
(1.4)
𝑅𝑧(𝜓)=( cos𝜓sin𝜓0
−sin𝜓cos𝜓0
001 )
(1.5)
Pentru a determina unghiul avem nevoie de ecuația R xyz , astfel:
𝑹𝑥𝑦𝑧( 0
0
1 )=𝑹𝑥(𝜙)𝑹𝑦(𝜃)𝑹𝑧(𝜓)(0
0
1 )
(1.6)
=( cos𝜃cos𝜓 cos𝜃sin𝜓 −sin𝜃
cos𝜓sin𝜃sin𝜙 −cos𝜙sin𝜓cos𝜙cos𝜓+ sin𝜃sin𝜙sin𝜓 cos𝜃cos𝜙
cos𝜙cos𝜓sin𝜃+ sin𝜙 sin𝜓cos𝜙sin𝜃sin𝜓−cos𝜓sin𝜙 cos𝜃cos𝜙 )(0
0
1)

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

20

=( −sin𝜃
cos𝜃sin𝜙
cos𝜃cos𝜙 ) (1.7)
Putem rescrie această relație raportând axa transversală și cea longitudinală la valoarea normată a
accelerometrului:

𝐺𝑝
‖𝐺𝑝‖=( −sin𝜃
cos𝜃sin𝜙
cos𝜃cos𝜙 ) ⇒1
√𝐺𝑝𝑥 2+𝐺𝑝𝑦 2+𝐺𝑝𝑧 2[𝐺𝑝𝑥
𝐺𝑝𝑦
𝐺𝑝𝑧]=( −sin𝜃
cos𝜃sin𝜙
cos𝜃cos𝜙 ) (1.8)
Dacă rezolvăm ecuația (1.8) pentru axa transversală și cea longitudinală obținem:
tan𝜙𝑥𝑦𝑧=𝐺𝑝𝑦
𝐺𝑝𝑧 (1.9)
tan𝜃𝑥𝑦𝑧=−𝐺𝑝𝑥
𝐺𝑝𝑦sin𝜙+𝐺𝑝𝑧cos𝜙=−𝐺𝑝𝑥
√𝐺𝑝𝑦 2+𝐺𝑝𝑧 2 (1.10)
unde G px Gpy și Gpz sunt valorile citite de la accelerometru.
Pentru a determina în final valoarea pe axa transversală, cea care reprezintă unghiul de înclinare a l
robotului, vom apela la funcțiile din librăria MATH.h (atanf, atan2f).
float roll = atan2f(accY, accZ) * RAD_TO_DEG;
Unghiul calculat va fi parametrul de intrare al algoritmul ui PID, după ce va trece prin filtrul
Kalm an.

1.4. Proiectarea f iltrului Kalman discretizat

Filtrul Kalman e ste alcătuit dintr -un set de ecuații matematice pentru algoritmul de calcul optim
recursiv care asigură soluția prin metoda celor mai mici pătrate [12]. Ace stea încorporează toate
informațiile care pot fi furnizate și procesează toate măsurătorile disponibile , indiferent de precizia
lor, pentru a estima valoarea curentă a variabilelor de interes . Sunt luate î n considerare cunoștințe le
anterioare ale dinamicii de sist em și de măsurare, d escrierea statistică a zgomotelor din sistem, e rori
de măsurare, imprecizia în dinamica modelului, precum și orice informații disponibile cu privire la
starea inițială a variabile i de interes.

Dacă modelul sistemul ui de interes este li niar și valorile zgomotul ui nu sunt corelat e în timp, atunci
filtrul Ka lman este optim din orice punct de vedere. Filtrul Kalman nu necesită ca toate datele
anter ioare să fie păstrate în memorie și reprocesat e de fiecare dată când se ia o nouă măsurătoare.
Prin acest comportament, filtrul Kalman poate fi implementat î n software pe orice microcontroler.

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

21

A. Ecuația de stare
Filtrul Kalman poate fi aplicat pentru a estima stările sistemului atunci când acesta este modelat în
mod adecvat sub forma unei ecuații diferențiale stocastice liniare.

Ecuația ( 1.1) reprezintă ecuația de stare a sistemului la momentul 𝑘 împreună cu zgomot alb 𝜔̇̇𝑘.

𝑥̂𝑘=𝑭𝑥̂𝑘−1+𝑩𝑢𝑘+𝜔̇̇𝑘 (1.11)
Particularizâ nd la sistemul studiat, 𝑥̂𝑘 este matricea stărilor ș i este egală cu :

𝑥̂𝑘=[𝜃
𝜃̇𝑏]
𝑘 (1.12)

unde 𝜃 este unghiul de la ieșirea filtrului ș i 𝜃̇𝑏 este eroar ea sistematică a măsuratorilor de la
giroscop ș i accelerometru. Putem deduce valoarea reală scăzând eroarea din valoarea măsurată .

Matricea F este modelul de tranziț ie al stă rilor care este aplicat stă rii anterioare 𝑥̂𝑘−1

𝐹=[1−∆𝑡
01] (1.13)

𝑢𝑘 reprezintă control ul de intrare , în acest caz, este măsurarea giroscop ului în grade pe secundă
(°/s) la momentul 𝑘. Vom rescrie de fapt , ecuația de stare ca:

𝑥̂𝑘=𝑭𝑥̂𝑘−1+𝑩𝜃̇𝑘+𝜔̇̇𝑘 (1.14)

unde matricea B este modelul controlului de intrare, care este definit ca:

𝐵=[∆𝑡
0] (1.15)

Acest lucru are sens, deoarece se va obține unghiul 𝜃 când se multiplică rata 𝜃̇𝑘 cu ∆𝑡=𝑡𝑘−𝑡𝑘−1.
Pentru că nu se poate calcula eroarea sistematică direct , bazată pe rata 𝜃̇𝑘, vom stabili valoarea 0 în
partea de jos a matrice i.

𝜔̇̇𝑘 reprezintă zgomo tul de tip gaussian de medie 0 și covarianță 𝑄 la momentul 𝑘:

𝜔̇̇𝑘~ 𝑁(0,𝑄𝑘) (1.16)

𝑄𝑘 este matricea de covarianță a zgomotului procesului și î n ace st caz matricea de covarianță a
stării estimate a accelerometrului ș i a erorii acestuia. Se va lua î n considerare estimarea erorii
sistematice ș i valoarea accelerometrului ca fiind independente , rezultând că 𝑄𝑘 reprezintă chiar
valorile estimate ale var ianței acceleromet rului și a erorii sistematice (bias) .

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

22

𝑄𝑘=[𝑄𝜃0
0𝑄𝜃̇𝑏] ∆𝑡 (1.17)

După cum se poate vedea , matricea de covarianță 𝑄𝑘 depinde de momentul curent 𝑘, astfel încât
variaț ia accelerometru lui 𝑄𝜃 și variația erorii 𝑄𝜃̇𝑏 sunt multiplicat e cu variația timpului ∆𝑡.
Acest lucru are sens deoarece zgomotul de proces va fi mai mare î ntr-un interval de timp mai lung
de la ultimul moment al ac tualiză rii de stare .

Măsurători le sistemului sunt efectuate la intervale de timp discrete, aceste fiind incluse în vectorul
de măsurare putând fi modelate prin relația :

𝑧𝑘=𝑯𝑥̂𝑘+𝜗𝑘 (1.18)

După cum se observă , măsurare a 𝑧𝑘 este dată de starea actuală 𝑥̂𝑘 înmulțită cu matricea H plus 𝜗𝑘
zgomot ul de măsurare.

Ecuația ( 1.18) precizează că măsurătorile depind de starea sistemului și sunt legate de matricea de
măsurare, cu un adaos de zgomot în măsurăt ori. Matricea H face legătura între valoarea reală și cea
măsurată. Valoarea reală nu poate fi determinată , deoarece există doar măsurătorile din partea
accelerometrului ș i nu pot fi comparate cu valori din alte siste me de măsură . Măsurătorile sunt
adesea obținute la intervale egale de timp , dar acest lucru nu este obligatoriu . Matricea H este egală
cu:

𝐻=[10] (1.19)

B. Zgomotul sistemului și al măsu rătorilor
Zgom otul de măsurare este g aussian ș i distribuit de asemenea cu medie 0 și covarianță 𝑅:

𝜗𝑘~ 𝑁(0,𝑅) (1.20)

𝑅 este o matrice definită pozitiv , ceea c e înseamnă că toate componentele măsurătorilor sunt
corupte cu zgomot și nu există nici o combinație liniară a acestor componente care poate fi lipsit ă de
zgomot. Având în vedere că distribuțiile 𝜔 și 𝜗 sunt presupuse ca fiind gau ssiene, acest lucru
înseamnă că sunt necor elate între ele. Dar cum 𝑅 nu este o matrice de măsurare a zgomot ului, ea
este egală doar cu variația măsurătorii, deoarece covarianța aceleiași variabile este egala cu variaț ia
ei. Putem defini 𝑅 astfel:

𝑅=𝐸[𝜗𝑘𝜗𝑘𝑇]=𝑣𝑎𝑟(𝜗𝑘) (1.21)

Vom asuma că zgomotul de mă surare este același ș i independent de timp :

𝑣𝑎𝑟(𝜗𝑘)=𝑣𝑎𝑟(𝜗) (1.22)
Notă: D acă va riația zgomotului de mă surare este prea mare , atunci filtrul va avea un timp de
răspuns mare ș i nu va acorda o pondere ridicată noilor măsurători, iar dacă este prea mică este
posibil ca valoare măsurată să fie zgomotoasă și ponderea de încredere a valoril or să fie ridicată .

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

23

C. Condiții inițiale

Ecuația diferențială de stare (1.1) are prima iteraț ie cu 𝑥̂0 și pentru orice operațiune specifică a
sistemului , starea inițială are impact î n calcul . Cu toate acestea, deoarece această valoare nu poate fi
cunosc ută cu precizie dinainte, va fi modelată ca un vector alea tor care este distribuit normal.
Astfel, vom descrie 𝑥̂0 cu media 𝑥̂ 0 și covarianț a P0.

𝐸{𝑥̂0}=𝑥̂̂0 (1.23)

𝐸{[𝑥̂0−𝑥̂̂0(𝑥̂0−𝑥̂̂0)𝑇]}=𝑃0 (1.24)

𝑃0 este o matrice sim etrică ce oferă valoarea prezisă a diferenței dintre starea reală și starea
estimat ă. Elementele de pe diagona la acestei matrice asig ură dispe rsia fiecărei variabile de stare î n
raport cu valoarea sa reală.

D. Ecuațiile de filtrare

Filtrul Kalman estimează un proces prin utilizarea unei forme de control cu reacție : filtrul estimează
starea procesului la un anumit m oment de timp și apoi obține răspunsul în forma măsurărilor
(zgomotoase). Astfel, ecuațiile filtrului Kalman se împart în două grupe: ecuații de actualizare în
timp și ecuații de actualizare a măsurării. Ecuațiile de actualizare în timp sunt responsabile d e
determinarea estimaților stării curente și a matricei de covarianță a erorii pentru a obține estimații a
priori pentru următorul moment de timp. Ecuațiile de actualizare a măsurării sunt responsabile d e
reacție – adică de î ncorporarea unei noi măsurări î n estimatul a priori pentru a obține un estimat a
posteriori îmbunătățit . Ecuațiile de actualizare în timp pot fi de asemenea gândite ca ecuații
predictor, în timp ce ecuațiile de actualizare a măsurării pot fi gândite ca ecuații corector. Într-
adevăr algoritmul de estimare final seamănă cu cel al unui algoritm de tip predictor -corector pentru
rezolvarea problemelor n umerice așa cum este arătat în f igura 1.2.

Fig. 1.2 Schema algoritmului predictor -corector

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

24

E. Predicț ia

Primele dou ă ecuații vor încerca să prezică starea curentă și matricea de covarianță a erorii la
momentul 𝑘. În primul rând , filtrul va încerca să estimeze starea actuală, bazată pe toate stările
anter ioare și pe măsurarea giroscopului :

𝑥̂ 𝑘|𝑘−1=𝑭𝑥 𝑘−1|𝑘−1+𝑩𝜃̇𝑘 (1.25)

Acesta este, de asemenea, motivul pentru care se numește o intrare de control, din moment ce am
folosi t un parametru de intrare suplimentar pentru a est ima starea la momentul actual 𝑘, numit ă
starea a priori 𝑥̂ 𝑘|𝑘−1. Se va încerca estimarea a priori a matricei de covarianță a erorii 𝑃𝑘|𝑘−1 bazată
pe matricea anterioară a covarianț ei erorii 𝑃𝑘−1|𝑘−1 , care este definit ă ca:

𝑃𝑘|𝑘−1=𝑭𝑃𝑘−1|𝑘−1𝑭𝑇+𝑄𝑘 (1.26)

Această matrice este utiliz ată pentru a estima cât de multă încredere se acordă valorilor curente ale
stării estimat e. Principiul ecuației de mai sus este , de fapt , destul de ușor de înțeles, aș a cum este
destul de evident, covarianța erorii va creș te, deoarece ultima dată s-a actualizat estimarea stării,
prin urmare, am multiplicat ma tricea de covarianță a erorii cu modelul de tranziție de stare F și
transpusa sa 𝑭𝑇, iar apoi se adaugă zgomotul procesului curent 𝑄𝑘 la momentul 𝑘. Matricea de
covarianță a erorii este î n caz ul actual de formă 2 x 2 :

𝑃=[𝑃00𝑃01
𝑃10𝑃11] (1.27)
F. Corecț ia

Diferența 𝑧𝑘−𝑯𝑥̂ 𝑘|𝑘−1 în ecuația ( 1.28), este numită inovația măsurării sau reziduu l. Reziduul
reflectă diferența dintre măsurarea actuală 𝑧𝑘 și măsurarea prezisă 𝑥̂𝑘|𝑘−1

𝑦̃𝑘=𝑧𝑘−𝑯𝑥̂ 𝑘|𝑘−1 (1.28)
Se va cal cula apoi covarianța reziduului :

𝑆𝑘=𝐻𝑃𝑘|𝑘−1𝐻𝑇+𝑅 (1.29)
𝑆𝑘 încearcă să prezică cât de credibilă este măsurătoare bazată pe matricea de covarianță a priori
𝑃𝑘|𝑘−1 și pe matricea de covarianță a măsură torilor 𝑹. Cu cât este mai mare valoarea zgomotului de
măsurare este mai mare și valoarea lui S, acest lucru înseamnă că nu avem încredere în măsurătorile
de intrare atât de mult.

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

25

G. Câștigul filtrului

Matricea 𝐾𝑘 este numită câștigul sau factorul de amestec , minimizând urma matricei de covarianță a erorii a
posteriori.
𝐾𝑘=𝑃𝑘|𝑘−1𝐻𝑇𝑆𝑘−1 (1.30)

Se poate observa că dacă nu există credibilitate a reziduului foarte mare , credibilitatea inovaț iei
covarianț ei S va fi ridicat ă, iar dacă credibilitatea estimatului stă rii este mare atunci matricei de
covarianță a erorii 𝑷 va fi mic ă, prin urmar e, câștigul Kalman va fi mic.

Dacă se aprofundează se poate observa că transpusa modelului de observare H este folosit ă pentru a
mapa starea de covarianță a erorii matricei P în spațiu observat. Vom compara matricea de
covarianță a erorii prin î nmulțirea cu inversul covarianței de inovare S.

Acest lucru are sens deoarece vom folosi modelul de observare H pentru a extra ge date din
covaria nța erorii de stare și se compară cu estimarea a ctuală a covaria nței de inovare. În acest
moment se poate corecta estimatul a posteriori :

𝑥̂ 𝑘|𝑘=𝑥̂ 𝑘|𝑘−1+𝑲𝒌𝑦̃𝑘 (1.31)

Inovaț ia poate fi pozitivă sau negativă . Pe scurt, ecuația poate fi înțeleasă că va corecta estimatul a
priori 𝑥̂ 𝑘|𝑘−1, care a fost calculat folosind starea anter ioară , și măsurarea giroscopului, cu măsurarea
– în acest caz – a accelerometru lui. Ultimul lucru care se va face este acela de a actualiza matrice a
de covarianță a erorii a posteriori:

𝑃𝑘|𝑘=(𝐼−𝑲𝒌𝑯)𝑃𝑘|𝑘−1 (1.32)
unde I este matricea de identitate și este egală cu 𝐼=[10
01]

Filtrul face î n principiu o autocorectare a matricei de covarianță a er orii în funcție de cât de mult s -a
corectat estimarea. Acest lucru are sens , deoarece s-a corectat starea bazată pe urma matricei de
covarianță a erorii a priori 𝑃𝑘|𝑘−1, dar și covarianța inovă rii 𝑆𝑘.

Performanța filtrului poate varia foarte mult în cazul în care parametrii nu sunt reglați corect. Prin
urmare, această secțiune își propune să ofere o idee despre caracteristicile parametrilor filtrului.

Parametrii reglabili sunt:

1. Matricea inițială de covarianță .
2. Vectorul estimatului de stare .
3. Matricea Q ș i vectorul 𝜔 aleato r corespunzăto r.
4 Matricea R și vectorul 𝜗 aleato r corespunzăto r.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

26

H. Matricea covarianței erorii inițiale

Variația aștept ată a erorii estimatului de stare pentru parametrul coresp unzător este reprezentat ă de
elementele diagonale ale matrice i. Pentru ca filtrul să funcționeze corect, valorile din matricea de
covarianță vor fi definit e astfel încât diferența dintre starea inițială și estimatul stă rii inițiale sunt î n
intervalul care este admisibil conform matricei de covarianță.

Elementul diagonal al matricei de covarianță inițială trebuie să fie suficient de mare pentru a
îndeplini cerința menționată mai sus. În cazul în care estimatul inițial al stă rii este destul de precis ,
atunc i matricea de cov arianță permite doar o mică eroare. Estimatul de stare va avea nevoie de mai
mult timp să conve argă cât timp matricea de covarianță devine din ce î n ce mai mare.
I. Estimatul inițial de stare

O estimare inițială a sistemului trebuie să fie disponibil ă buclei de filtru care urmează să fie
executat ă. Nu există nici o cerință gener ală a estimă rii inițiale pentru a fi c orectă , cu condiția ca
valorile inițiale ale matricei de covarianță să fie suficient de mari pentru ca filtrul să funcționeze
corect.

J. Matricea Q

Matricea Q reprezintă covarianța vectorului eroare de sistem , 𝜔. Se presupune că elementele 𝜔 sunt
necorelate. Prin urma re, valoarea fiecărui element Q care nu se află pe diagonală este zero.
Creșterea matricei Q ar indica fie zgomote puternice din cauza părții mecanice , fie incertitudine
crescută în caracterul modelului în sine pen tru a descrie adevărata dinamică cu precizie. Acest lucru
va mări rata de creștere a elementelor 𝑃(𝑡) sau valorile proprii dintre timpii de mă surare și valorile
lor la starea de ech ilibru. Ca rezultat, câștigul de filtrare va crește în general, iar măsurătorile sunt
ponderate în mare măsură, acest lucru este rezonabil, dat fiind faptul că matricea Q dictează că ar
trebui să se acorde mai puțină încredere în ieșirea filtrului.

K. Matricea R

Matricea R reprezintă covarianța matricei de eroare de măsurare , 𝜗. Aceas tă matrice indică cât de
mari sunt așteptate erorile de măsurare să fie. O matrice R cu valori mari ar indica faptul că
măsurătorile sunt supuse u nui zgomot mai puternic și așa ar trebui să aibă o pondere mai mică
acordată de filtru . Filtrul ajunge î n starea de echilibru rapid în cazul în care valorile proprii ale
matricei Q sunt mari în comparație cu valorile proprii ale lui R (raportul Q / R est e mare). Acest
lucru se datorează incertitudinii mari implicate în propagarea de stare în comparație cu precizia
măsurătorii, as tfel încât noua estimare de stare depinde în mare măsură de noua măsurare și nu este
strâns legată de estimările anterioare.

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

27

1.5. Modularea PWM

PWM (eng. Pulse Witdth Modulation) este o modulare care folosește lățimea pulsului unui semnal
dreptunghiular pentru a transmite informația. Este o tehnică folosită pentru a varia în mod controlat
tensiunea dată unui dispozitiv electronic. Ac eastă metodă schimbă foarte rapid tensiunea oferită
dispozitivului respectiv din ON în OFF și invers. Perioada de timp corespunzătoare valorii ON
dintr -un ciclu ON -OFF se numește factor de umplere ( duty cycle ) și reprezintă, î n medie, ce
tensiune va primi dispozitivul electronic. A șa se pot controla circuitele analogice din domeniul
digital. De cele mai multe ori este folosit în controlul aplicațiilor de putere. Este caracterizat de
factorul de umplere și frecvență [13].
PWM este folosit în cazul acesta pentru a seta viteza motorului. E roarea corectată de la ieșirea
blocului PID este factorul de umplere din blocul PWM. Rezultă că factorul de umplere variază în
funcție de postura robotului, compensând astfel mișcă rile care îl scot din echilibru . În Figura 1 .3 se
pot observa semnale PWM cu factor de umplere de 60%. S e poate deduce foarte ușor formula
factorului de umplere 𝐷:

𝐷=𝐿𝑎𝑡𝑖𝑚𝑒𝑎 𝑝𝑢𝑙𝑠𝑢𝑙𝑢𝑖 𝑂𝑁
𝑃𝑒𝑟𝑖𝑜𝑎𝑑𝑎 𝑠𝑒𝑚𝑛𝑎𝑙𝑢𝑙𝑢𝑖 ∗100=𝑡𝑂𝑁
(𝑡𝑂𝑁+𝑡𝑂𝐹𝐹)∗100 (1.33)

Tensiunea medie care ajunge la dispozitiv este dată de relația: 𝐷∗𝑉𝑐𝑐

Modularea folosește
variația factorului de
umplere a unei forme
de undă
dreptunghiular ă
generând la ieșire o
tensiune analogică.

Considerând o formă de undă dreptunghiulară f(t) cu o valoare minimă ymin și o valoare
maximă ymax și factorul de umplere 𝐷, valoarea medie a formei de undă e dată de relația:
𝑦̃=1
𝑇∫𝑓(𝑡)𝑑𝑡𝑇
0 (1.34)
cum f(t) este o formă de undă dreptunghiulară valoarea sa maximă se atinge pentru 0<𝑡<𝐷∗𝑇. Fig. 1.3 Captură osciloscop D ~ 60%

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

28

1.6. Puntea H

Puntea H (eng. H Bridge) este un circuit electronic ce permite aplicarea une i tensiuni pe o sarcină
în orice sens. Aceste circuite sunt adesea folosite în robotică și alte aplicații permițând motoarelor
de curent continuu să ruleze înainte și înapoi. Punțile H sunt disponibile ca circuite integrate sau
pot fi construite din compo nente discrete, tranzistoare bipolare sau MOS. Puntea H are numele
derivat de la modul obișnuit de desenare a circuitului.
Elementele de comutare ( Q1..Q4 ) s unt, de obicei , tranzistori bi polari sau FET , iar în unele ap licații
de înaltă tensiune I GBT . Există , de asemenea, soluții integrate.
Diode le ( D1. ..D4 ) se numesc diode de captură și sunt,
de obicei , de tip Schottky . Vârful punții este conectat
la o sursă de alimen tare ( baterie, de exemplu) , iar
partea de jos este la GND. În general, toate cele patru
elemente de comutare pot fi porni te și oprite în mod
independent , cu toate că e xistă unele restricții evidente. Cu toate că sarcina poate fi, în teorie, orice ,
de departe cererea cea mai răspândită în cazul punții H este pentru motoare de curen t continuu sau
stepper.
Pentru a putea fi compatibil cu motoarele DC, în proiectul prezentat chipul punții H trebuie sa aibă
un curent de ieșire mai mare de 1.1A și o tensiune de alimentare de minim 12V.
În ace astă lucrare s-a folosit o punte H de tip ci rcuit integrat L298 N. Aceasta are o configurație full-
bridge și poate comanda cele 2 motoare în mod independent unul de celă lalt.

Fig. 1.4 Schema conexiunii PWM – punte H
Fig. 1.5 Schema de funcționare generală a punții H

Facultatea de Electronică,
Telecomunicații și Tehnologia Blocurile funcționale ale sistemului
Informației

29

1.7. Motorul brushed DC

Dimensionarea motoarelor DC folosite este făcută cu ajutorul aspectelor expuse anterior; astfel,
pentru o greutate de aproximativ 0.6 kg și la o înălțime de 8 cm față de axul de rotație al motoarelor,
se pot folosi motoare cu reductor pentru a obține o viteză între 300 -500 RPM și un cuplu între 2 -4
Kg/cm.
Întregul sistem este pus în mișcare de două motoar e brushed DC cu reductor 21:1, viteză 3 70 RPM.
Acestea au ș i enc odere care au un rol important î n procesul de optimizare descris î n capitolul 4.2.3.
Mai multe detalii despre motoare sunt pre zentate în secțiunea care prezintă modelul matematic al
motorului în capitolul 3.1 .

1.8. Bateria

Este necesară o tensiune de 12V pentru a alimenta sistemul . O asemenea tensiune poate fi obținut ă
dintr -o baterie LiPo cu 3 celule de 3.7V (3C). Timpul de funcționare este ușor de calculat.
Motoarele consumă în sarcină maximă 1 .1A, iar celelalte componente nu depășesc valoarea de
400mA. Astfel, pentru o autonomie de minim um o oră și jumătate este suficient o baterie cu
2800mAh.
𝑎𝑢𝑡𝑜𝑛𝑜𝑚𝑖𝑒 =𝑐𝑎𝑝𝑎𝑐𝑖𝑡𝑎𝑡𝑒𝑎 𝑏𝑎𝑡𝑒𝑟𝑖𝑒𝑖 (𝑚𝐴ℎ)
𝑐𝑜𝑛𝑠𝑢𝑚𝑢𝑙 𝑠𝑖𝑠𝑡𝑒𝑚𝑢𝑙 𝑢𝑖 (𝑚𝐴)
Bateria are protecție la descărcarea rapidă, deoarece acest incident s -a petrecut și poate avea efecte
iremediabile. Senzorul de tensiune al bateriei este format dintr -un divizor rezistiv care este
interpretat de convertorul ADC al microcontr olerului. Dacă tensiunea bateriei scade sub un prag
considerat critic, tot sistemul se oprește și se aprinde un LED de alarmă.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

30

Facultatea de Electronică,
Telecomunicații și Tehnologia Comunicații S eriale. Inter -IC (I2C)
Informației

31

2. Comun icații Seriale . Inter -IC (I2C)

Interfața I2C este un standard răspândit astăzi în foarte multe module electronice. În continuare se
va prezenta modul său de funcționare, diferitele caracteristici, avantaje și aplicarea sa practică în
proiectul curent.
I2C (eng. Inter -IC) semnifică conectarea circuitelor integrate ca într -o rețea și comunicarea lor pe
magistrala astfel cr eată. Toate circuitele care sunt compatibile au o interfață I2C încorporată, care se
poate conecta la unul sau mai multe dispozitive, identice sau nu. Astfel, apar roluri de master/slave
în funcție de cine inițiază comunicația și generează semnalul de ceas (SCL).

2.1. Caracteristicile de bază ale interfeței I2C

1. Magistrala I2C este compusă din 2 semnale: o linie serială de date (SDA) și o linie serială de ceas
(SCL).
2. Fiecare dispozitiv conectat la magistrală se poate accesa printr -o adresă unică.
3. Relaț iile de master/slave sunt întotdeauna prezente.
4. Poate exista o comunicație cu mai multe dispozitive în rolul de master în același timp, deoarece
este inclusă opțiunea de evitare a coliziunilor și de arbitrare între mai mulți master când aceștia vor
să transmită simultan.
5. Este o conexiune serială, cu preponderență de d imensiune 8 biți, bidirecțională și care poate avea
viteze de la 100kbit/s până la 3.4Mbit/s, iar pentru transmisiuni unidirecționale până la 5 Mbit/s.
6. Numărul maxim de dispozitive ce se pot conecta la magistrală este dat de capacitanța magistralei .

2.2. Avantajele interfeței I2C

1. Este foarte ușor de configurat și se poate folosi într -o gama variată de aplicații.
2. Se poate depana foarte ușor și erorile sunt evidente.
3. Are doar 2 pin i și este eficient pentru producătorii de IC, deoarece ocupă puțină suprafață pe
silicon și nu are un impact mare asupra numărului de pini ca în cazul altor protocoale (SPI).
4. Este o tehnolog ie larg răspândită putând să interconecteze o varietate de disp ozitive: senzori,
ecrane LCD, module de memorie etc.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

32

2.3. Protocolul I2C

Pentru a vehicula informația pe magistrala I2C, avem nevoie de cele 2 conexiuni seriale, de date
(SDA) și de ceas (SCL). Fiecare dispozitiv este recunoscut printr -o adresă unică și p oate fi
transmițător sau receptor. Unele dispozitive pot opera doar în modul receptor, astfel un afișaj LCD
nu va putea transmite date la fel cum poate face o memorie [14].
Rolurile pe care un dispozitiv le poate avea este fie de master, fie de slave. Masterul inițiază
comunicația, generează semnalul de ceas (SCL) și începe transferul de date, fie primește, fie
transmite. În acest moment orice alt dispozitiv legat la magistrală devine slave. Dacă există mai
mult de un master con ectat și doi sau mai mulți master vor să transmită date va apărea situația de
arbitrare, când dispozitivele master vor decide care poate să trimită primul, iar celelalte vor trece în
modul slave deoarece este posibil ca masterul să vrea să comunice cu alt master și astfel acesta
trebuie să fie disponibil.
Pentru a putea transmite informație validă pe magistrală, nivelele HIGH -LOW t rebuie să fie stabile.
Semnalul SDA poate face trecerea de la HIGH -LOW sau LOW -HIGH doar când semnalul de ceas
este LOW.

Fig. 2.1 Utilizarea validă a interfeței I2C; Sursa: I2C Manual

Toate transferurile de date încep cu o secvență de START și se termină cu o secvență de STOP.
Secvența de START reprezintă o tranziție HIGH -LOW a s emnalului SDA cât timp SCL este HIGH.
Secvența de STOP reprezintă o tranziție LOW -HIGH a semnalului SDA cât timp SCL este HIGH.
Secvențele de START și STOP sunt întotdeauna generate de către master. Magistrala este
considerată ocupată după o secvență de START și liberă după o secvență de STOP.

Facultatea de Electronică,
Telecomunicații și Tehnologia Comunicații Seriale. Inter -IC (I2C)
Informației

33

Fig. 2.2 Secvențele de START și STOP; Sursa: I2C Manual
Datele transferate pe magistrală trebuie să aibă lungimea de 8biți și sunt urmați de un semnal de
ceas de confirmare (ACK) pe linia SCL. Datele sunt transmise serial, primul bit fiind cel mai
semnificativ (MSB).
Pentru ca receptorul să confirme primirea (ACK), masterul eliberează linia SDA pentru ca
receptorul să îl fixeze LOW. Dacă SDA rămâne LOW până când pulsul de ceas transmis de master
(al 9 puls după cele 8 pentru byte -ul de date transmis) este terminat, înseamnă că transmisia a av ut
loc cu succes. Dacă semnalul SDA este HIGH cât timp se transmite semnalul de ceas de la master,
înseamnă că apare un NACK și transmisiunea nu a fost efectuată cu succes.
Sunt 5 cazuri când poate apărea NACK:

1. Nu există un receptor pe magistrală, deci nu este nimeni să poată confirma comunicația.
2. Receptorul nu este pregătit să primească sau să transmită, deoarece este ocupat cu alte operațiuni.
3. În timpul comunicației, receptorul nu interpretează corect toate datele.
4. În timpul comunicației, re ceptorul nu poate primi mai mulți biți de informație.
5. În momentul în care un master -receptor transmite terminarea transferului către un slave care a
transmis.
2.4. Descrierea p achetul ui transmis prin protocolul I2C

Pachetul transmis începe cu o secvență de START și adresa slave la care se dorește acces. Adresa
slave are 7 biți și este urmată de încă un bit care indică dacă se citește sau se scrie. Dacă acest bit
este 0, semnifică scrierea, iar dacă este 1, citirea. La final se transmite secvența de STOP. D acă se
vrea o comunicare prelungită, se poate omite secvența de STOP și se transmite încă o secvență de
START (restart). Se poate folosi și adresarea slave pe 10 biți, dar această caracteristică este rar
întâlnită. În Anexa 3 se pot observa operațiile de s criere/citire care pot fi efectuate și structura lor.
În proiectul prezentat s -a folosit o interfață I2C Master la viteza de 100 KHz, cu adresare pe 7 biți și
o magistrală bidirecțională cu un singur master (XMC4700) și un singur slave (M PU9150). Pentru a
putea accesa datele de la senzor, se activează chipul scoțându -l din sleep cu un prim transfer de
scriere a registru lui de power management. S enzorul este activat și începe să înregistreze valori în
regiștrii pentru accelerometru și giroscop. După această inițializare se fac 2 citiri în rafală a câte 6
octeți cu o frecvență de 100 Hz (Anexa 1 -2). Astfel, protocolul I2C este folosit pentru ca
microcontrolerul să primească direct date reale care nu mai au nevoie de o interpretare sau
transformare ulterioară, cum este în cazul senzorilor analogici care au nevoie și de un convertor
analog -digital.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

34

Fig. 2.3 Captură cu analizorul de semnale a bus -ului I2C

Fig. 2.4 Captură osciloscop a bus -ului I2C

Facultatea de Electronică,
Telecomunicații și Tehnologia Modelarea sistemului
Informației

35

3. Modelarea sistemului

Dinamica robotului trebuie să fie descrisă pri ntr-un model matematic, facilitând dezvoltarea unui
sistem de control eficient pentru echilibrare. În acest capitol, sunt prezentate ecuația de mi șcare
pentru un pendul inversat cu două roți și pen tru un motor de curent continuu .

3.1. Modelul liniar al unui motor de curent continuu (DC)

Robotul este alimentat de două motoare de curent continuu Polulu. În această secțiune se va analiza
ecuația în spaț iul stă rilor al motorului . Acest model este apoi utilizat în modelul dinamic de
echilibrare oferind o relație între tensiunea de intrare la motoare și cuplul de control necesar.

Fig. 3.1 Diagrama unui motor de curent continuu
Figura 3.1 exemplifică un model eficient , liniar pentru un m otor de curent continuu. Când o
tensiune se aplică la bornele motorului, un curent 𝑖 este generat între armă turile acestuia . Motorul
produce un c uplu 𝜏𝑚, care este propor țional cu curentul. Aceast ă relație poate fi exprimată ca :

𝜏𝑚=𝑘𝑚𝑖 (3.1)

O pereche rezistor -bobină în serie cu o tensiune, 𝑉𝑒𝑚𝑓, poate fi folosit pentru a modela circuit ul
electric al motorului. Tensiune 𝑉𝑒𝑚𝑓 este produsă deoarece bobina motorului se deplasează printr –
un câmp magnetic. Tensiunea produsă poate fi aproximată ca o funcție liniară a vitezei arborelui,
care poate fi scrisă ca :

𝑉𝑒=𝑘𝑒𝜔 (3.2)

La acest punct, o ecuație diferențială liniară pentru circuitul electric al motorului de cu rent continuu
poate fi scrisă prin utilizarea l egii tensiune Kirchhoff. L egea prevede că suma tutu ror tensiunilor din
circuit trebuie să fie egală cu zero. Pentru motorul de curent continuu, acest lucru poate fi scris ca :

𝑉𝑎−𝑅𝑖−𝐿𝑑𝑖
𝑑𝑡=0 (3.3)

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

36

În derivarea ecuației de mișc are a motorului , frecarea pe axul motorului este aproximată ca o funcție
liniară a vi tezei arborelui . Legea mișcării lui Newton afirmă că suma tuturor cup lurile produse pe
arborele este liniar legată de accelerarea arborelui prin sarcină ine rțială a armă turii. Declarați a
anterioară poate fi scrisă ca :

∑𝑀=𝜏𝑚−𝑘𝑓𝜔−𝜏𝑎=𝐼𝑅𝜔 (3.4)

Substituind ecuația ( 3.1) și ( 3.2) în ecuațiile ( 3.3) și ( 3.4) și rearanj ând în funcție de de rivata î n
raport cu timpul , sunt evidențiate următo arele două ecuații fundamentale care regleme ntează
mișcarea motorului:

𝑑𝑖
𝑑𝑡=𝑅
𝐿𝑖+𝑘𝑒
𝐿𝜔+𝑉𝑎
𝐿 (3.5)

𝑑𝜔
𝑑𝑡=𝑘𝑚
𝐼𝑅𝑖+−𝑘𝑒
𝐼𝑅𝜔−𝜏𝑎
𝐼𝑅 (3.6)

Ambele ecuații sunt fun cții liniare ale curentului și vitezei și aces tea includ derivata de ordin 1 î n
timp. Un model simp lificat al motor ului de curent continuu este suficient pentru echilibrarea
robot ului. Din acest motiv, inductanța motor ului și frecarea motor ului este considerat ă neglijabilă
și este ap roximată cu zero. Prin urmare, (3.5) și (3.6) poate fi aproximat cu :

𝑖=−𝑘𝑒
𝑅𝜔+𝑉𝑎
𝑅 (3.7)

𝑑𝜔
𝑑𝑡=𝑘𝑚
𝐼𝑅𝑖−𝜏𝑎
𝐼𝑅 (3.8)

Înlocuind ecuația ( 3.7) în ecuația ( 3.8):

𝑑𝜔
𝑑𝑡=−𝑘𝑚𝑘𝑒
𝐼𝑅𝑅𝜔+𝑉𝑎
𝐼𝑅𝑅−𝜏𝑎
𝐼𝑅 (3.9)

Având în vedere că inductanța motorului este neglijată, curentul prin înfăș urare nu est e luat în
considerare în ecuația de mișcare a motoru lui. Curentul va ajunge apoi î n stare a constantă imediat,
în comparație cu viteza arborelui, care are nevoie de timp pentru a accelera de la o anumită viteză
inițială până la o viteză finală după o mod ificare a tensiunii de intrare. Dinamica motorului poate fi
reprezenta tă cu un model de spațiu -stare , acesta este un sistem de ecuații diferențiale de ordinul
întâi cu funcție de parametri, θ și ω, care reprezintă în mod unic funcționarea acestuia. La intrare a
motorului este apoi apli cată tensiunea și cuplul adecvat .

[𝜃̇
𝜔̇]=[01
0−𝑘𝑚𝑘𝑒
𝐼𝑅𝑅][𝜃
𝜔]+[00
𝑘𝑚
𝐼𝑅𝑅−1
𝐼𝑅][𝑉𝑎
𝜏𝑎] (3.10)

𝑦=[10][𝜃
𝜔]+[00][𝑉𝑎
𝜏𝑎] (3.11)

Facultatea de Electronică,
Telecomunicații și Tehnologia Modelarea sistemului
Informației

37

3.2. Modelul robotului

Pendulul inversat cu două roți, are un comportament similar cu un pendul pe o platformă mobilă .
Dinamica pendulului și roțile sunt analizate separat la început, dar acest lucru va conduce la două
ecuații de mișcare, care descriu complet comport amentul robotului .

Deoarece comportamentu l robotului poate fi influențat de tulburări precum cuplul motor, modelul
matematic va t rebui să ia în considerare astfel de forțe. În primul rând, se obțin ecuațiile de mișcare
asociate cu roțile din stânga și din dreapta. Următoarea figură prezintă diagrama de corp liber a
ambelor roți. Având în vedere că ecuația pentru ro ata din stânga și dreapta sunt complet analoage,
numai ecuația roții din dreapta este dat ă.

Fig. 3.2 Forțele implicate în modelarea roț ilor

Folosind legea mișcării a lui Newton, suma forțelor pe direcția x este :

∑𝐹𝑥=𝑀𝑎

𝑀𝑤𝑥̂̈=𝐻𝑓𝑅−𝐻𝑅 (3.12)

Suma forț elor în jurul centrului roții:
∑𝑀0=𝐼𝛼

𝐼𝑤𝜃̈𝑤=𝐶𝑅−𝐻𝑓𝑅𝑟 (3.13)

Din dinamica motoarelor de curent continuu, cuplul motorului poate fi exprimat ca:
𝜏𝑚=𝐼𝑅𝑑𝜔
𝑑𝑡+𝜏𝑎 (3.14)

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

38

Prin rearanjarea ecu ației și substituind parametrii , cupl ul de ieșire la roți este:

𝐶=𝐼𝑅𝑑𝜔
𝑑𝑡=−𝑘𝑚𝑘𝑒
𝑅𝜃̇𝑤+𝑘𝑚
𝑅𝑉𝑎 (3.15)

Prin urmare, ecuația ( 3.13) devine :
𝐼𝑤𝜃̈𝑤=−𝑘𝑚𝑘𝑒
𝑅𝜃̇𝑤+𝑘𝑚
𝑅𝑉𝑎−𝐻𝑓𝑅𝑟 (3.16)
Rezultă că :

𝐻𝑓𝑅=−𝑘𝑚𝑘𝑒
𝑅𝑟𝜃̇𝑤+𝑘𝑚
𝑅𝑟𝑉𝑎−𝐼𝑤
𝑟𝜃̈𝑤 (3.17)

Ecuația ( 3.15) este substituit ă în (3.12), pentru a obține ecuația roților .

Pentru roata din stânga :
𝑀𝑤𝑥̂̈=−𝑘𝑚𝑘𝑒
𝑅𝑟𝜃̇𝑤+𝑘𝑚
𝑅𝑟𝑉𝑎−𝐼𝑤
𝑟𝜃̈𝑤−𝐻𝐿 (3.18)

Pentru roata din dreapta :
𝑀𝑤𝑥̂̈=−𝑘𝑚𝑘𝑒
𝑅𝑟𝜃̇𝑤+𝑘𝑚
𝑅𝑟𝑉𝑎−𝐼𝑤
𝑟𝜃̈𝑤−𝐻𝑅 (3.19)

Deoarece mișcarea liniară acționează în centrul roții, rotația unghiulară poate să fie transformată în
mișcare l iniară prin simpla transformare:

𝜃̈𝑤𝑟=𝑥̂̈⇒𝜃̈𝑤=𝑥̈
𝑟

𝜃̇𝑤𝑟=𝑥̂̈⇒𝜃̇𝑤=𝑥̈
𝑟

Prin transformarea liniară, ecuația ( 3.18) și ( 3.19) devine:

Pentru roa ta din stânga:

𝑀𝑤𝑥̂̈=−𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇+𝑘𝑚
𝑅𝑟𝑉𝑎−𝐼𝑤
𝑟2𝑥̂̈−𝐻𝐿 (3.20)

Pentru roata din dreapta:
𝑀𝑤𝑥̂̈=−𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇+𝑘𝑚
𝑅𝑟𝑉𝑎−𝐼𝑤
𝑟2𝑥̂̈−𝐻𝑅 (3.21)

Facultatea de Electronică,
Telecomunicații și Tehnologia Modelarea sistemului
Informației

39

Sumând ecu ațiile (3.20) și ( 3.21) :

2(𝑀𝑤+𝐼𝑤
𝑟2)𝑥̂̈=−2𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇+2𝑘𝑚
𝑅𝑟𝑉𝑎−(𝐻𝐿+𝐻𝑅) (3.22)

Șasiul robotului poate fi modelat ca un pendul in vers. F igura 3.3 prezintă d iagrama de corp liber a
șasiului.

Fig. 3.3 Diagrama de corp liber a șasiului

Din nou, prin utilizarea legii lui Newton, suma forțelor în direcție orizontală :

∑𝐹𝑥=𝑀𝑝𝑥̂̈

(𝐻𝐿+𝐻𝑅)−𝑀𝑝𝑙𝜃̈𝑝cos𝜃𝑝+𝑀𝑝𝑙𝜃̇𝑝2sin𝜃𝑝=𝑀𝑝𝑥̂̈ (3.23)

prin urmare :
(𝐻𝐿+𝐻𝑅)=𝑀𝑝𝑥̂̈+𝑀𝑝𝑙𝜃̈𝑝cos𝜃𝑝−𝑀𝑝𝑙𝜃̇𝑝2sin𝜃𝑝 (3.24)

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

40

Suma forțelor perpendicular e pendulul ui:

∑𝐹𝑥𝑝=𝑀𝑝𝑥̂̈cos𝜃𝑝

(𝐻𝐿+𝐻𝑅)cos𝜃𝑝+(𝑃𝐿+𝑃𝑅)sin𝜃𝑝−𝑀𝑝𝑔sin𝜃𝑝−𝑀𝑝𝑙𝜃̈𝑝=𝑀𝑝𝑥̂̈cos𝜃𝑝 (3.25)

Suma momentelor în jurul centrului de masă al pendulului :

∑𝑀0=𝐼𝛼

−(𝐻𝐿+𝐻𝑅)𝑙cos𝜃𝑝−(𝑃𝐿+𝑃𝑅)𝑙sin𝜃𝑝−(𝐶𝐿+𝐶𝑅)=𝐼𝑝𝜃̈𝑝 (3.26)

Cuplul aplicat pe pendulul de la motor așa cum es te definit în ecuația ( 3.15) și după transformare
liniară este :

𝐶𝐿+𝐶𝑅=−2𝑘𝑚𝑘𝑒
𝑅𝑥̇
𝑟+2𝑘𝑚
𝑅𝑉𝑎

Substituind această relație în ecuația ( 3.26) rezultă :

−(𝐻𝐿+𝐻𝑅)𝑙cos𝜃𝑝−(𝑃𝐿+𝑃𝑅)𝑙sin𝜃𝑝−(−2𝑘𝑚𝑘𝑒
𝑅𝑥̇
𝑟+2𝑘𝑚
𝑅𝑉𝑎)=𝐼𝑝𝜃̈𝑝
prin urmare:
−(𝐻𝐿+𝐻𝑅)𝑙cos𝜃𝑝−(𝑃𝐿+𝑃𝑅)𝑙sin𝜃𝑝=(−2𝑘𝑚𝑘𝑒
𝑅𝑥̇
𝑟+2𝑘𝑚
𝑅𝑉𝑎)+𝐼𝑝𝜃̈𝑝 (3.27)
Înmulțind ecuația ( 3.25) cu –𝑙:

⌊−(𝐻𝐿+𝐻𝑅)𝑙cos𝜃𝑝−(𝑃𝐿+𝑃𝑅)𝑙sin𝜃𝑝⌋+𝑀𝑝𝑔𝑙sin𝜃𝑝+𝑀𝑝𝑙2𝜃̈𝑝=−𝑀𝑝𝑙𝑥̂̈cos𝜃𝑝 (3.28)

și se înlocuiește ecuația (3.27) în ecuația (3.28):

𝐼𝑝𝜃̈𝑝 −2𝑘𝑚𝑘𝑒
𝑅𝑥̇
𝑟+2𝑘𝑚
𝑅𝑉𝑎+𝑀𝑝𝑔𝑙sin𝜃𝑝+𝑀𝑝𝑙2𝜃̈𝑝=−𝑀𝑝𝑙𝑥̂̈cos𝜃𝑝 (3.29)

Pentru a elimina (𝐻𝐿+𝐻𝑅) din dinamica motorului, ecuația (3 .24) este substituit ă în ecuația (3.22) :

2(𝑀𝑤+𝐼𝑤
𝑟2)𝑥̂̈=−2𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇+2𝑘𝑚
𝑅𝑟𝑉𝑎−𝑀𝑝𝑥̂̈−𝑀𝑝𝑙𝜃̈𝑝cos𝜃𝑝+𝑀𝑝𝑙𝜃̇𝑝2sin𝜃𝑝 (3.30)

Rearanjarea ( 3.29) și ( 3.30) prezintă e cuațiile neliniare ale mișcării sistem ului:

(𝐼𝑝+𝑀𝑝𝑙2)𝜃̈𝑝 −2𝑘𝑚𝑘𝑒
𝑅𝑟𝑥̂̇+2𝑘𝑚
𝑅𝑉𝑎+𝑀𝑝𝑔𝑙sin𝜃𝑝=−𝑀𝑝𝑙𝑥̂̈cos𝜃𝑝 (3.31)

Facultatea de Electronică,
Telecomunicații și Tehnologia Modelarea sistemului
Informației

41

2𝑘𝑚
𝑅𝑟𝑉𝑎=2(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2)𝑥̂̈+2𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇+𝑀𝑝𝑙𝜃̈𝑝cos𝜃𝑝−𝑀𝑝𝑙𝜃̇𝑝2sin𝜃𝑝 (3.32)

Cele două ecuații de mai sus pot fi liniarizate dacă asumăm că θ = π + φ , unde φ reprezintă un mic
unghi față de verticală. Această simplificare a fost folosit ă pentru a permite un model liniar care
urmează să fie obținut. Prin urmare:
cos𝜃𝑝=−1,sin𝜃𝑝=−𝜙, (𝑑𝜃𝑝
𝑑𝑡)2
=0
Ecuaț ia liniarizată de mișcare este:

(𝐼𝑝+𝑀𝑝𝑙2)𝜙̈− 2𝑘𝑚𝑘𝑒
𝑅𝑟𝑥̂̇+2𝑘𝑚
𝑅𝑉𝑎−𝑀𝑝𝑔𝑙𝜙=𝑀𝑝𝑙𝑥̂̈ (3.33)

2𝑘𝑚
𝑅𝑟𝑉𝑎=2(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2)𝑥̂̈+2𝑘𝑚𝑘𝑒
𝑅𝑟2𝑥̂̇−𝑀𝑝𝑙𝜙̈ (3.34)

În scop ul de a obține reprezentarea spațiului stă rilor a sistemului, ecuațiile ( 3.33) și ( 3.34)
sunt rearanjate :

𝜙̈=𝑀𝑝𝑙
(𝐼𝑝+𝑀𝑝𝑙2)𝑥̂̈+2𝑘𝑚𝑘𝑒
𝑅𝑟(𝐼𝑝+𝑀𝑝𝑙2)𝑥̂̇−2𝑘𝑚
𝑅(𝐼𝑝+𝑀𝑝𝑙2)𝑉𝑎+𝑀𝑝𝑔𝑙
𝐼𝑝+𝑀𝑝𝑙2𝜙 (3.35)

𝑥̂̈=2𝑘𝑚
2𝑅𝑟(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2)𝑉𝑎−2𝑘𝑚𝑘𝑒
2𝑅𝑟2(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2)𝑥̂̇+𝑀𝑝𝑙
(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2)𝜙̈ (3.36)

Înlocuind ecuația ( 3.35) în ecuația ( 3.34), înlocuind ecuația ( 3.36) în ecuația ( 3.33), și după o serie
de m anipulă ri a ecuației algebrice , spațiul stărilor pentru sistem este :

[𝑥̂̇
𝑥̂̈
𝜙̇
𝜙̈]=
[ 0 1 00
02𝑘𝑚𝑘𝑒(𝑀𝑝𝑙𝑟−𝐼𝑝−𝑀𝑝𝑙2)
𝑅𝑟2𝛼𝑀𝑝2𝑔𝑙2
𝛼0
0 0 01
02𝑘𝑚𝑘𝑒(𝑟𝛽−𝑀𝑝𝑙)
𝑅𝑟2𝛼𝑀𝑝𝑔𝑙𝛽
𝛼0]
[𝑥̂
𝑥̂̇
𝜙
𝜙̇]+
[ 0
2𝑘𝑚(𝐼𝑝+𝑀𝑝𝑙2−𝑀𝑝𝑙𝑟)
𝑅𝑟𝛼
0
2𝑘𝑚(𝑀𝑝𝑙−𝑟𝛽)
𝑅𝑟𝛼 ]
𝑉𝑎 (3.37)

Unde :
𝛽=2(𝑀𝑤+𝐼𝑤
𝑟2+𝑀𝑝
2); 𝛼=[𝐼𝑝𝛽+2𝑀𝑝𝑙2(𝑀𝑤+𝐼𝑤
𝑟2)] (3.38)

În modelul de mai sus, se presupune că roțile vehic ulului vor rămâne întotdeauna în contact cu solul
și că roțile nu alun ecă. Având în vedere că se cunosc ecuaț iile spațiului stă rilor putem decide să
implementăm o metodă de control. S-a ales pentru aceasta algori tmul PID.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

42

Facultatea de Electronică,
Telecomunicații și Tehnologia Implementarea controlului și optimizarea PID
Informației

43

4. Implementarea c ontrol ului și optimiz area PID

4.1. Analiza stării iniț iale
Deoarece sistemul este în mod inerent instabil, un impuls aplicat în buclă deschisă va determina ca
unghiul de înclinare și poziția robotului să crească la infinit . Acest lucru duce la că derea robot ului,
unghiul de î nclinare peste care s -a decis că nu se mai poate reechilibra fiind de 20 de grade pe
fiecare parte. Figura 4.1 prezintă simularea pentru un impuls aplicat sistemul ui liber.

Fig. 4.1 Răspunsul sistemului la impuls în buclă deschis ă
Trasarea planului poli -zero-uri al sistemului verifică dacă acesta este instabil. D eoarece există un
pol pe planul din partea dreaptă a graficului , instabilitatea se confirmă . În mod ideal , toți polii ar
trebui să fie în planul din stânga pentru a fi stabil . Polii sunt prezenț i la valorile 0, 9.1605, -9.5141,
-1.5739 .

Fig. 4.2 Graficul Pol -Zero

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

44

4.2. Controlerul PID

Un control er proporțional -integral -derivat (PID eng. Proportional -Integral -Derivative ) este un
mecanism cu buclă de control utilizat în mod obișnuit în sistemele indu striale de contro l. Un
Controller PID calculează în mod continuu o valoare de eroare ca diferența dintre o valoare de
referință dorită și o variab ilă de proces măsurată. Controle rul încearcă să minimizeze eroarea în
timp prin ajustarea unei variabile de control, cum ar fi în cazul de față unghiul de î nclinare al
sistemului, la o nouă valoare determinată printr -o sumă ponderată:
𝑢(𝑡)=𝐾𝑝𝑒(𝑡)+𝐾𝑖∫𝑒(𝜏)𝑑𝜏+𝐾𝑑𝑑𝑒(𝑡)
𝑑𝑡𝑡
0 (4.1)
unde 𝐾𝑝, 𝐾𝑖,𝐾𝑑 sunt valori pozitive și reprezintă coeficienții proporț ional -integral -derivativ.

Fig. 4.3 Schema algoritmului PID [15]
Fiecare coeficient contribuie la performanț ă astfel :
 Proportional . Acest mod determin ă viteza de reacție pentru buclă . Cu cât este mai mare
câștigul controlerului, cu atât mai repede va răspunde bucla. Cu toate acestea, câștiguri mai
mari ale regulatorului duce la depășire și la oscilații a le răspunsului. Pentru fiecare buclă,
există o valoare maximă a câștigului controlerului, cunoscut sub numele de câștig final,
pentru care bucla este stabilă.
 Integral . Contrar cre dinței populare, modul integral nu produce un răspuns mai rapid.
Singura con tribuție a modului integral este de a se asigura că bucla se poate stabiliza doar la
valoarea de referință . Ea realizează acest lucru pr in ajustarea valorii prejudecată de ieșire a
controler ului. Prejudecata trebuie schimbată la o rată în conformitate cu ca racteristicile de
răspuns ale procesului. Schimbarea prejudecăț ii prea rapid, creează mai multe oscilații și, în
cazuri extreme, o buclă instabilă.
 Derivat . În unele aplicații, derivata reduce depăș irea nivelului dorit și oscilațiile. La rândul
său, acest lucru permite să se utilizeze o valoare mai mare pentru câștigul controlerului ,
deoarece mărește viteza de ră spuns.

Facultatea de Electronică,
Telecomunicații și Tehnologia Implementarea controlului și optimizarea PID
Informației

45

4.3. Metoda Ziegler -Nichols

Pentru a putea regla algoritmul PID eficient în cazul robotului, se pot aborda mai multe strategii.
Cea mai simplă , dar și cea mai costisitoare din punct de vedere al timpului, este metoda try -and-fail
[16]. Prin această metodă se incrementează toți coeficienții PID (K P KI KD) până când sistemul
devine stabil. Sunt greu de găsit valorile potrivite într -un interval de timp eficient, astfel încât s -a
folosit o strategie bazată pe comportamentul modelului.
Metoda aplicată inițial este Ziegler –Nichols, unde în calculul coeficienților PID este nevoie de
frecvența de oscilare a robotului (P u) și de valoarea K p maximă (K u) la care acesta intră în oscilație
[17].
Având aceste valori ca punct de start pentru parametrii PI D, se poate începe optimizarea sistemului ,
observând comportamentul său și reglând ulterior parametr ii.
În punctul acesta există o configuraț ie
care poate reprezenta un început î n
optimizarea echilib rului. Deoarece nu s-
au obț inut rezultate favorabile s -a decis
implementarea unui algorit m genetic de
mutare a configurațiilor până se ajunge
la o performan ță satisfăcă toare.
Algoritmul este prezentat î n capitolul
4.5.
Ieșirea din blocul PID reprezintă
parametrul de intrare al blocului PWM,
cel care se ocupă de controlul fizic al
motoarelor.

Tipul de
control KP KI KD
P 0.5 K u – –
PI 0.45 K u 1.2 K p / Pu –
PID 0.6 K u 2 K p / Pu Kp * Pu/8
Tab. 4.1 Coeficienții de tunare Ziegler -Nichols [17] Fig. 4.4 Influența K i asupra sistemului

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

46

4.4. Reglare PID bazată pe funcț ia de transfer

Pentru a putea controla acest si stem instabil s e va folosi funcț ia de transfer calculată din ecuaț iile de
stare . Cu această funcție ș i cu funcț ia de transfer a control erului PID putem simula comportamentul
sistemul ui la diferite valori ale coeficienților PID. Configurația ideală și cea optima nu se pot aplica
în realitate deoarece rezultatele obținute cu aceste configurații nu sunt satisfăcă toare. Ac est lucru se

datorează dificultăț ii de modelare a robotului. Formele neregulate ale ș asiului , erorile de cuplu ale
motoarelor, timpi i de răspuns, inerția și indicele de frecare al forț ei de frecare , toate influențează
negativ modelu l matematic și îl fac mai puțin credibil î n cazul actual. Astfel , singura metodă de
îmbunătățire a performanței rămâne metoda evoluției diferențiale [18] care rulează în software -ul de
pe microcontroler și lucr ează direct cu ră spunsul rob otului pentru fiecare configurație PID. În acest
caz nu mai există limitări date de factorii enumeraț i mai sus.

Fig. 4.5 Răspunsul la impul s cu algoritmul PID aplicat

Facultatea de Electronică,
Telecomunicații și Tehnologia Implementarea controlului și optimizarea PID
Informației

47

Deoarece valorile P WM care sunt folosite de la ieșirea contro lerului PID sunt discrete în timp, vom
folosi funcția de transfer discretizată cu o frecvență de eș antionare de 100 H z. Astfel, cel mai bun
compromis între overshoot ș i timpul de stabil izare este reprezentat în figura 4.6.
Fig. 4.6 Răspuns ul algoritmului PID la perturbaț ii

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

48

4.5. Optimizare P ID prin metoda evoluție i diferenț iale

În termeni simpl i, optimizarea este încercarea de a maximiza proprietă țile unui sistem în timp ce
simultan , se minimizează caracteristicil e sale nedorite . Care sunt aceste proprietăți și cât de
eficient pot fi ele îmb unătăț ite ține doar de sistemul studiat.

Deoarece schimbarea semnul ui unei funcții analizate transformă maxime le în minime , nu există
nicio generalitate pierdută prin limitarea următoarelor discuții numai la minimizarea valorilor
funcț iei. Se va observa că la aplicarea acestei metode este necesară doar că utarea minim ului.
Pentru a determina ce soluție de optimizare este potrivită sistem ului, se va analiza caracteristicile
acestuia.

 Parametri i. Sunt variabile le funcției continue, discrete sau ele nu ap arțin unui set finit? Î n
plus, sunt toate variabilele de același tip ?
 Dependența î ntre parametri i. Sunt parametrii funcției ce urmează a fi optimizată
independenț i (funcții separabile) sau minimul unuia sau mai mulți parametri i depind de
valoarea unuia s au a mai mul tor parametri i ?
 Dimensionalitate, D . Cât de multe variabile definesc funcția ?
 Are funcția doar un minim local (Uni -modal) sau mai multe minime locale(multi -modal)?
 Dependență de timp . Minimul local este staț ionar sau non -staționar (dinamic ) ?
 Zgomot . Se evaluează un vector. Sunt aceleași rezultat e de fiecare dată (fără zgomot) sau
fluctuează ?
 Diferențiabilitate . Este funcția derivabilă î n toate punctele de interes ?
Odată evaluate aceste caracteristici se poate lua hotărâ rea asu pra metod ei de optimizare. Funcț ia
sistemului studiat are parametrii discre ți în timp, cu o dependență între param etrii (performanț a
depinde de tens iunea de alimentare care descrește î n timp , dar și de ceilalț i parametrii care au fost
optimizaț i deja ), multi -modală , staț ionar ă în timp, cu rezultate zgomotoase și derivabilă î n toate
punctele de interes. Pentru a putea optimiza această funcț ie se poate apela la algoritmi genetici și
strategii evoluti ve. Metoda evoluției diferențiale are la bază aceste 2 strategii.
Price și Storn au dezvoltat algoritmul DE pentru a fi o funcție de o ptimizare fiabilă și versatilă care
este, de asemenea, ușor de utilizat. Prima publicație scrisă a DE a apărut ca un raport tehnic în 1995
[18]. De atunci, DE a dovedit ce poate în competiț ii precum Concursul Internațion al IEEE pe
optimizarea evolutivă (ICEO), în 1996 – 1997 și în lumea reală pe o varietate largă de aplicații. La
fel ca aproape t oți algoritmii evolutivi, DE este un optimizator de populaț ie care atacă problema
punc tului de pornire prin puncte inițiale alese în mod aleator . Limitele parametrilor presetați
definesc domeniul î n care vectorii 𝑁𝑝 din aceas tă populație inițială sunt aleș i (Fig. 4.7). Fiecare
vector este indexat cu un număr de la 0 l a 𝑁𝑝−1. Ca ș i alte metode bazate pe generarea de
populații , DE genereaz ă noi puncte care sunt perturbaț ii ale punctelor existente, dar aceste abateri
nu sunt nici reflecții, cum ar fi cele din metodele Nelder -Mead [19], nici mostre dintr -o funcție de
densitate de probabilitate pr edefinite, cum ar fi cele din strategiile evolutive [20]. In schimb, DE
modifică vectori i cu diferența scalată a doi vectori de populație selectaț i aleatoriu (F ig. 4. 8). Pentru
a produce vectorul proces, 𝑢0, DE adaugă diferența vectorul ui scalat , aleator la un al treilea vector
de populație de asemenea selectat în mod aleator (F ig. 4. 9). În etapa de selecție, vectorul proces
concurează împotriva vectorului populație d e același indice, care în acest caz este numărul 0.

Facultatea de Electronică,
Telecomunicații și Tehnologia Implementarea controlului și optimizarea PID
Informației

49

Figura 4.10 ilustrează etapa selec ției în care vectorul cu valori inferioare ale funcției este marcat
ca membru al generației următoare. Cifrele indică faptul că în figurile 4.11 – 4.12 procedura se
repetă până când toți vectorii de populație 𝑁𝑝 au concurat cu un vector de proces generat aleator.
Odată ce ultimul vector proces a fost testat, supr aviețuitorii concursurilor 𝑁𝑝 vor deveni părinți în
perechi pentru următoarea generație în ciclul evolutiv.

Fig. 4.7 Inițializarea populație [21]

Fig. 4.8 Generarea perturbației x r1 – xr2 în mod aleator

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

50

Fig. 4.9 Mutația diferenței vectorilor aleș i aleator cu un al treilea , la fel ales aleator

Fig. 4.10 Selecția. Deoarece u 0 este mai performant decât 0 îl înlocuieș te în generaț iile viitoare

Facultatea de Electronică,
Telecomunicații și Tehnologia Implementarea controlului și optimizarea PID
Informației

51

Fig. 4.11 Un nou proces este evaluat

Fig. 4.12 Elementu l 1 este mai bun decât elementul cu care concurează, u 1 și nu va fi înlocuit

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

52

Particularizâ nd cu sistemul studiat , există 3 variabile care vor fi optimizate, dependente una de
cealaltă , având valori discrete.
Algoritmul folosește spaț iul tridimensional a l variabilelor pentru a putea găsi o tendință de plasare a
punctelor performant e, pentru a echilibra sistemul și astfel , spațiul acestor soluț ii este explorat chiar
de că tre robot . Acest lucru se obț ine plecâ nd de la valorile KP, KD, KI determinate prin me toda try
and fail, combinată cu Ziegler -Nichols. Punctul de star t al algoritmu lui necesită o definire a unui
spațiu delimita t superior ș i inferior pentru fiecare parametru ce va fi optimizat din punct de vedere
al pe rformanței .
Prima generație va căuta soluții în spațiu delimitat, apoi va încerca să depășească spațiul dacă se
consid eră că punct ul minim global nu va fi gă sit în acea zonă . Astfel, algoritmul în funcție de
numărul de generaț ii va tinde către punctul de minim. Experimentele au fost făcute cu 30 de
generaț ii, fiecar e cu cate 10 indivizi. Un test în care se eval uează perfo rmanța durează 3 secunde ,
astfel un întreg set de evaluare și optimizarea durează 20 de minute.
Ecuația care evaluează pe rformanț a este de forma:
𝑦=𝑣𝑎𝑟(𝜃)+𝑚∗𝑎𝑣𝑔(𝑒𝑛𝑐𝑜𝑑𝑒𝑟) (4.2)
unde 𝑚 este un factor de normalizare, egal cu 0.001 și 𝑎𝑣𝑔() este media distanței înregistrată de
cele două encodere .
În evaluarea performanței are impact varia nța unghiului de înclinare, cât și distanț a parcursă de
robot până la echilibr u. Aceș ti pa rametrii trebuie minimizaț i simultan pentru ca algoritmul să se
dovedească util.
Utilizâ nd acest algoritm de evoluț ie și evaluând performanța configuraț iei, s-a ajuns la rezultate
satisfăcă toare. Acestea sunt analizate pe larg î n capitolul 5.

Facultatea de Electronică,
Telecomunicații și Tehnologia Rezultate
Informației

53

5. Rezultate

Pentru a cuantiza randamentul robotului s -a ales ca indice de performanță varianța unghiului de
înclinare și distanța parcursă până la echilibru. În acest proiect s -a folos it tehnologia Bluetooth
pentru a constru i canalul de comunicații dintre robot și P C și pentru a putea extrage info rmații
referitoare la performanț a acestuia. Se putea folosi și o soluț ie cu cablu (USB), dar se putea ca firul
să influenț eze s tabilitatea sistemului, astfel încâ t s-a luat decizia de a se folosi tehnologia Bluetooth .

Cu ro botul asamblat ș i cu software -ul dezvoltate corespunzător , se începe tunarea coeficienț ilor PID
prin m etoda try and fail. KD și KI sunt egalați cu 0 ș i KP este incrementat până când acesta începe să
oscileze. Apoi , se incrementează KD până când oscilaț iile dispar. Se incrementează din nou KP și
ulterior KD până câ nd robotul nu mai oscilează puternic . Se repetă această procedură până în
momentul î n care o incrementare KP sau KD nu mai produce s chimbări î n comportamentul
sistemului. Cu aceste valori maxime , se începe calibrarea KI pentru a încerca să se gă sească poziț ia
de echilibru. R obotul nu cade , dar este instabil și oscilează puternic. Deoarece această metodă are
nevoie de un timp îndelungat de implementare până la succes, s-a încercat folosirea metodei
Ziegler -Nichols în care s -a măsurat empiric timpul de oscilație ș i valoarea KP la care începe să
oscileze. Folosi nd tabelul 4.1 s-au determinat valori pentru PID care reu șesc să mențină echilib rul
un timp mai î ndelungat , dar performanța atinsă nu este mulț umitoare.

Existâ nd un comportament pseudo -stabil în care robo tul nu mai cade, dar nici nu stă î n echilibru , s-a
putut trece la faza de optimi zare î n care scopul pr incipal a fost reducerea oscilaț iilor. Algoritmul de
optimizare DE [18] se potriveș te cu sistemul și a fos t implementat pentru a îmbunătăț ii performanț a.
Astfel, s-a reușit în primul râ nd automatizarea pr ocedurii de testare a configurației PID, reducâ nd
substanț ial timpul de tunare.
Din acest moment varianța unghiului de înclinare a fost introdusă ca un factor de performanță.
Vizualizâ nd grafic acest param etru s -a observat că algorit mul DE nu poate gă si un model de
optimizare din cauza zgomotului dat de oscilaț iile rapide cu care sistemul se confruntă (Fig 5.1) .
S-a constatat că poziț ia componente lor pe șasiul robotului influențează performanț a acestuia . O
asemenea component ă este senzorul de î nclinare . Iniț ial, acesta a fost amplasat cât mai sus față de
axul roților, dar înregistra târziu căderile și oscilaț iile mari. Astfel, senzorul a fost plasat cât mai
aproape de roți să aibă un ră spuns mai rapid la mișcările bruș te de dezechilib ru.
O altă problemă de construcție a apărut î n cazul bateriei, c are este cea mai grea componentă.
Aceasta a fost inițial plasată câ t mai j os, deoarece s -a considerat minimizarea distanț ei centrului de
greutate față de roți pentru o inerț ie cât mai mică .

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

54

Fig. 5.1 Măsurători zgomotoase din cauza oscilației foarte mari
În figura 5.2 ș i figura 5.3 se poate observa că varianța unghiului în cercurile pline ș i verzi este de
maxim 5 grade. Acest test a fost făcut în momentul î n care b ateria era plasată câ t mai jos. Conform
testelor efectuate cu bateria cât mai jos și cu bateria cât mai sus a reieșit faptul că centrul de greutate
poziționat cât mai sus ajută la filtrarea oscilațiile rapide care î l scoteau din echilibru. Astfel, cu o
inerție mai mare, sistemului i se oferă un timp de răspuns mai î ndelungat pentru reechilibrare.
Rezultatele s -au îmbunătă țit considerabil ș i comportamentul robotului s -a stabilizat vizibil (Fig.
5.4). Din acest punct, cu ajustările de șasiu fă cute s -au testat cât mai multe puncte din spaț iul
tridimensional al variabilelor KP KD KI.
Teoretic, algoritmul funcționează corect , caută minimul varianței și încearcă să mărească
coeficienț ii PID. Comportamentul robotului avea momente care implica u mișcare, dar și momente
staționare , deci nu era constant. Din acest motiv algor itmul DE nu optimiza în aceleași condiții.
Astfel, covarian ța unghiului poate fi minimă și în momentul în care robotul se deplasează într -o
direcț ie, cu viteza constantă ș i un unghi minim de înclinaț ie. Teoretic algoritmul percepe acest
comportament acceptabil și îl optimizează până în punctul î n care robotul are doar acest
comportament , deoarece miș carea cu unghi constant are varianță minimă față de varianța în
momentul de staționaritate î n echilibru.
Pentru a evita această situație s -a introdus în formula performanței și media unghiului. Abordarea
aceasta reduce numărul de teste fals -pozitive , dar nu î n totalitate . Deoarece acest fenomen apare și
când media unghiului este mică , nu se elimină problema din cauza proporț ionalității celor două
mărimi, dar dim inuează probabilitatea de apariț ie. Considerâ nd că media ung hiului nu este un
param etru adițional sufic ient pentru estimarea performanței s -a luat decizia înlocuirii acesteia cu

Facultatea de Electronică,
Telecomunicații și Tehnologia Rezultate
Informației

55

două encodere pentru cele două motoare. A șa se poate măsura distanța parcursă de robot și se poate
încerca minimizarea acesteia cu ajutoru l algoritmului DE.

Fig. 5.2 Varianța pentru poziț ia centrului de greutate ridicat

Fig. 5.3 Spațiul 3D cu zgomotul eliminat

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

56

Fig. 5.4 Performanța celor 300 de configurații individuale testate
În figura 5.4 se poate observa un test automat cu 300 de puncte în care encoderele îș i fac simțită
prezența. Varianța scade brusc de la 5 grade cum era î n testele anterio are la valori subunitare. Se
poate obser va că algoritmul de optimizare începe să conve argă într -o direcție. Punctele pline verzi
indică o performanță acceptab ilă, dar punctele pătrate albastre indică o stabilitate crescută. Se
observă că în acest test nu s unt foarte multe pătrate. De aceea, se rulează testul din nou schimbând
limitele intervalului iniț ial de optimizare ale algoritmului DE sperând că se va obține o performanță
îmbunătățită .

Facultatea de Electronică,
Telecomunicații și Tehnologia Rezultate
Informației

57

Fig. 5.5 Direcția de convergență a testelor
În figura 5.5 se observă cum , alegâ nd un interval de explorare mai apropiat de starea optimă ș i un
pas de explorare mai mic, algoritmul converge mai repede și direcția soluțiilor este evidentă .

Fig. 5.6 Desco perirea punctelor cu o performanță îmbunătățită în spaț iul 3D
Figurile 5.6 și 5.7 arată cum performanța î ncepe sa crească datorită numă rului mare de puncte
pline verzi. Î n acest moment pe grafice apar e un numă r satisfăcător d e configuraț ii perfor mante

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

58

(pătrate albastre). Având această dreaptă a soluțiilor trasată, se poate analiza varianț a unghiului
acestui test și se poate determina care a fost configuraț ia cu cel mai stabil rezultat. Aceste figuri
arată dependenț a celor 3 va riabile, lucru care a fost luat în considerare la început î n alegerea
algori tmului de optimizare. Se dovedește că acesta s -a potrivit siste mului și că are capacitatea de a
explora spațiul soluțiilor , găsind punctul cu cel mai bun randament.

Fig. 5.7 Graficul performanței în funcție de K D și K P
În figura 5.7 putem distinge 2 drepte. Prima , cea descrisă de cercurile pline verzi care are o grosime
mult mai mare față de cea de -a doua, cea a pă tratelor pline alba stre. Acest lucru este normal
deoarece punctele alba stre sunt cazurile particulare în care robotul stă în echilibru ș i ajunge la acest
echil ibru parcurgând o distanță minimă . Alegând o configurație de pe linia alb astră, analizând
varianț a din aceste pu ncte se poate alege o configuraț ie KP KD KI care este suficient de performantă
pentru a satisface cerinț ele sistemului. În tabelul de mai jos sunt prezentat e top 10 configuraț ii
performante. Se pot observa intervalele 𝐾𝑃∈[16,18], 𝐾𝑖∈[1.4,1.9], 𝐾𝐷∈[38,40].
Varianța Distanța Performanță KP KI KD
1 0.538 294 0.979 16.864 1.624 38.539
2 0.706 185 0.983 16.864 1.684 38.539
3 0.528 321 1.010 17.932 1.474 39.378
4 0.575 294 1.016 17.035 1.642 39.467
5 0.585 290 1.020 16.074 1.832 38.328
6 0.658 246 1.027 16.998 1.657 39.569
7 0.647 255 1.029 15.775 1.954 38.431
8 0.609 284 1.035 18.106 1.497 40.174
9 0.541 341 1.052 17.641 1.622 40.096
10 0.608 310 1.073 16.837 1.632 39.739
Tab. 5.1 Cele ma i bune configurații

Facultatea de Electronică,
Telecomunicații și Tehnologia Rezultate
Informației

59

Privit stric din punctele de vedere al variaț iei unghiului , se poate observa (Fig 5.8) că robotul a
căzut doar o singură dată. Dacă se elimină această cădere, adică variaț ia foarte mare a unghiului
(Fig 5.9) se observă că media varianț ei este mai mică de 1.5 grade.

Fig. 5.8 Varianța unghiului

Fig. 5.9 Varianța ung hiului, zoom -in

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

60

Facultatea de Electronică,
Telecomunicații și Tehnologia Concluzii
Informației

61

Concluzii

Am dezvoltat pe baza microcontrolerului ARM M4 un robot funcțional care se auto -echilibrează în
timp real. Acest a este independent, toate operațiile fiind desfășurate intern. Am propus o soluț ie de
echilibrare prin estimarea unghiului bazată pe gravitația Pămân tului. Senzorul MEMS determină
unghiul de înclinare și îndeplinește cerinț ele de performanță necesare echilibră rii.
Am implementat o interfață de comunicare între microprocesor ș i senzor prin protocol ul
I2C. Controlul î n timp real are nev oie de date de la senzor cu rată fixă, constantă î n timp. I2C s -a
dovedit a fi soluția potrivită punând la dispoziț ia sist emului operaț ii de citire în rafală, pachete de
dimensiuni mici veh iculate cu viteză suficientă pentru un sistem mecanic, dar ș i stabilitate pe
parcursul transmisiei, zgomotul fiind neglijabi l. Am analizat un model matematic al ansamblului
care a ajutat la investigarea detalia tă asupra robotului. Cu modelul elaborat am simulat stabilitatea
acestuia și am propus o metodă de control optim.
Am implementa t algoritmul PID simulat anterior în formă paralelă pe microcontro lerul ARM. Am
iterat mai multe metode de op timizare ai param etrilor de cont rol. Prima ș i cea mai simplă a fost try
and fail. Această metodă este caracterizată de incrementarea parametrilor de control până se ajunge
la performanța do rită. Fiind o metodă costisitoare din punct de vede re al timpului, am aplicat
metoda Ziegler -Nichols care se bazează pe param etrii calculați prin comportamentul robotului .
Algoritmul evoluțiilor diferențiale s-a dovedit a fi cel mai avantajos , deoarece opt imizează
parametrii de control în timp prin încercări succesive rulate pe sistem. Am i ntrodus indicatori de
performanță ca: varianța și media unghiului alături de distanța parcursă de robot până la echilibru
într-o fereastră de timp bine determinată .
În urma optimiză rii folosite prin metoda propusă în lucrare, performanțele s -au îmbunătăț it de la
aproape 10 grade la valori subunitare ale varianț ei un ghiului , fapt ce determină un comp ortament
stabil. S -a observat că funcționarea poate fi afec tată de mai mul ți factori. Suprafața de rularea,
gradul de descărcare al bateriei, perturbațiile externe și timpul de rulare au fost testate, iar sistemul a
funcționat conform.
Complexitatea este mai ridicată în comparație cu un ansamblu pe 4 roți care e stabil intrinsec . E
nevoie de senzori care furnizează cu o rată destul de ridicată accelerația pentru a putea prezice
unghiul robotului și a aplica un algoritm de stabilizare în timp util , menținând echilibrul. Lipsa de
conexiune de depanare și rularea în timp real compli că investigarea unor eventuale probleme care
sunt greu de reprodus din cauza naturii instabile a sistemului .
Provocarea acestui proiect a reprezentat integrarea tuturor blocurilor funcționale pentru a putea
opera în timp real.
Lucrarea s -a bucurat de apre ciere la Sesiunea de Comunicări Științifice din cadrul Universității
Politehnica București, fiind premiată cu locul doi.
În final, se demonstrează că roboții cu două roți pot furniza performanțe similare ca cei pe patru
roți.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

62

Facultatea de Electronică,
Telecomunicații și Tehnologia Bibliografie
Informației

63

Bibliogra fie

[1] V. Croitoru și D. Geleriu, Telecomunicații. Acronime. Termeni. Definiții, vol. I, București:
Editura Academiei Române, 2013, p. 305.
[2] M. E. Roshein, Leonardo's Lost Robots, New York: Springer, 2006, p. 69.
[3] S. Narumi, „Japan’s First Robot,” 30 iulie 2012. Disponibil la :
http://www.nippon.com/en/views/b00906/.
[4] S. Kemper, Code name Ginger : the story behind segway and Dean Kamen's quest to invent a
new world, Boston: Harvard Business School Press., 2003.
[5] Y. Takahashi, N. Ishikawa și T. Hagiwara, „Inverse pendulum controlled two wheel drive
system,” 2001.
[6] N. Shiroma, O. Matsumoto, S. Kajita și K. Tani, „Cooperative Behaviour of a Wheeled
Inverted Pendulum for Object Transportation,” în IEEE International Conference o n Intelligent
Robots and Systems , Tokyo, 1996.
[7] T. Sugihara, T. Nakamura și I. Hirochika, „Realtime Humanoid Motion Generation through
ZMP Manipulation based on Inverted Pendulum Control,” Washington D.C, 2002.
[8] Infineon, „ Reference Manual XMC4700,” v1.2, 2016.
[9] Infineon, „DAVE V4 User Manual,” 2016.
[10] M. Elwenspoek și R. Wiegerink, Mechanical Microsensors, New York: Springer, 1993, pp.
132-145.
[11] M. Pedley, Tilt Sensing Using a Three -Axis Accelerometer, Rev. 6 ed., Freescale, 2013.
[12] R. Kalman, „A New Approach to Linear Filtering and Prediction Problems,” Journal of Basic
Engineering, pp. 35 -45, 1960.
[13] V. Croitoru, Comunicații Analog Digitale – Laborator, București: ETTI, 2015.
[14] NXP, „Offic ial I2C Specification,” Rev 6, 2014.
[15] Nick, „Mr. Digital,” 27 Iunie 2011. Disponibil la :
https://nicisdigital.wordpress.com/2011/06/27/proportional -integral -derivative -pid-controller/.

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

64

[Accesat 10 Martie 2016].
[16] K. Astrom ș i T. Hagglund, PID Controllers:Theory, Design And Tunning, Washington DC:
Instrument Society of America: Research Triangle Park,, 1995.
[17] J. G. Ziegler și N. B. Nichols, Optimum settings for automatic controllers, New York: ASME,
1942.
[18] K. Pri ce și R. Storn, Differential Evolution; A practical Approach to Global Optimization, New
Tork: Springer, 1995.
[19] J. Nelder și R. Mead, A simple method for function minimalization, Computer Journal, 1965,
p. 303.
[20] H. Mühlenbein și D. Schlierkamp -Vosen, „Evolutionary Computation,” în Predictive models
for the breeder genetic algorithm , 1993, pp. 25 -49.
[21] K. Price și R. Storn, Differential Evolution; A practical Approach to Global Optimization, New
Tork: Springer, 1995.
[22] K. Lundberg, „w eb.mit.edu,” 21 Ianuarie 2012. Disponibil la :
http://web.mit.edu/klund/www/papers/UNP_pendulum.pdf. [Accesat 3 April 2016].

Facultatea de Electronică,
Telecomunicații și Tehnologia Anexa 1
Informației

65

Anexa 1

Fig. 1 Diagrama bloc a rutinei de întrerupere pentru I2C RX
START
Se primesc date
acum ?
1.Citește valoarea de la
pointerul de recepție
2.Numără câtă octeți au
fost primiți
3.Activează intreruperea
de primire
4.Seteaza fanionul cu
“recep ție în curs ”
FINALSTART procedur ă de
intrerupere a recep ției
FIFO
configura t?Citește un
octet din
FIFO
Trimite ACKCitește un
octet din
registru
RBUF
Trimite ACK
S-a primit toat ă
informa ția?S-a primit toat ă
informa ția?
Este FIFO
gol?Setare fl ag
“STAND -BY”
Execut ă
funcția de
callbackExist ă funcție de
callback ?DA
NUDANU
NU DA
NU
DANUDA NU
DA

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

66

Anexa 2

Fig. 2 Diagrama bloc a rutinei de întrerupere pentru I2C TX
STARTSTART
întrerupere
Este transmisia
activă?
1. Obține datele
2. Num ără octeții de
date
3. Activeaz ă
întreruperea de
transmisie
4. Seteaz ă fanion ul
“transmisie activ ă”
FinalAu fost transmi și toți
octeții ?
Este FIFO
configur ată?
Încarc ă octeții în
FIFO p ână când
aceasta devine
plină Încarc ă un
octet în
registru l
RBUFSeteaz ă
fanionul din
“transmisie
activă” în
“stand by”
Exista func ție de
callback ?
Execut ă
funcția de
callbackDA
NUDA
NU
DANU DA
NU

Facultatea de Electronică,
Telecomunicații și Tehnologia Anexa 3
Informației

67

Anexa 3

Fig. 3 Operații folosite în protocolul I2C

Facultatea de Electronică, Dezvoltarea unui sistem
Telecomunicații și Tehnologia de ec hilibrare și control pentru roboți
Informației

68

Anexa 4
//Codul funcțiilor de transfer I2C

const uint8_t Address = 0xD0 ;
volatile uint8_t tx_completion_0 = 0, rx_completion_0 = 0;
//funcția de READ din MPU –-INPUT: //adresa registrului, numărul de octeți //care
trebuie citiți și pointerul //vectorului în care se salvează //octeții primiț i
void MPU_Read ( uint8_t *mem_address , uint8_t number , uint8_t *receive ) {
I2C_MASTER_Transmit (&I2C_MASTER_0 , true , Address , mem_address , 1, false );

while (tx_completion_0 == 0);
tx_completion_0 = 0;

I2C_MASTER_Receive (&I2C_MASTER_0 , true , Address , receive , number , true ,
true );
while (rx_completion_0 == 0);
rx_completion_0 = 0;
}
//funcția de WRITE î n MPU–INPUT: //adresa registrului ș i valoarea ce //trebuie
scrisă în registru
void MPU_Write (uint8_t *mem_address , uint8_t *payload ) {

I2C_MASTER_Transmit (&I2C_MASTER_0 , true , Address , mem_address , 1, false );

while (tx_completion_0 == 0);
tx_completion_0 = 0;

I2C_MASTER_Transmit (&I2C_MASTER_0 , false , Address , payload , 1, true );

while (tx_completion_0 == 0);
tx_completion_0 = 0;
}

Apelarea funcțiilor I2C și calcularea unghiului :
MPU_Read(&GYROX, 6, received_Gyro);

gyroX = (rece ived_Gyro[0] << 8) + received_Gyro[1];
gyroY = (received_Gyro[2] << 8) + received_Gyro[3];
gyroZ = (received_Gyro[4] << 8) + received_Gyro[5];

MPU_Read(&ACCX, 6, received_Acc);

accX = (received_Acc[0] << 8) + received_Acc[1];
accY = (received_Acc[2] << 8 ) + received_Acc[3];
accZ = (received_Acc[4] << 8) + received_Acc[5];

float dt = 0.01;
float roll = atan2f(accY, accZ) * RAD_TO_DEG;
float pitch = atanf( -accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_D

Facultatea de Electronică,
Telecomunicații și Tehnologia Anexa 5
Informației

69

Anexa 5

DIAGRAMA GANTT

ID Activitate Start Final Duraratafeb. 2016 mar. 2016 apr. 2016 mai. 2016 iun. 2016
31.1 7.2 14.2 21.2 28.2 6.3 13.3 20.3 27.3 3.4 10.4 17.4 24.4 1.5 8.5 15.5 22.5 29.5 5.6 12.6 19.626.6
1 6w 25.03.2016 15.02.2016 Documentare pentru partea teoretică
2 3w 31.03.2016 11.03.2016 Documentare pentru partea practică
4 3w 21.04.2016 01.04.2016Asamblare si testare initiala a robotului
prin try and fail
5 2w 19.04.2016 06.04.2016Determinarea modelului si diverse
simulari MATLAB
8 3w 4d 20.06.2016 25.05.2016 Testare si optimizare prin algoritm DE6 3w 09.05.2016 19.04.2016Testare si tunare cu algorimtul Ziegler –
Nichlos
7 3w 20.05.2016 02.05.2016Documentare optimizare algoritm de
optimizare DE3 13w 1d 01.07.2016 01.04.2016 Scris la lucrare