Lucrare de Licență Strategii de Reglare Optimală Pentru Sisteme Multi-Agent

Universitatea Politehnica București
Facultatea de Automatică și Calculatoare

Departamentul de Automatică și Ingineria Sistemelor
LUCRARE DE LICENȚĂ
Strategii de reglare optimală pentru

sisteme multi-agent
Absolvent
Crișu Alin Gabriel
Coordonator
S.l.dr.ing. Florin Stoican
București, 2016
Cuprins
1 Introducere 5
2 Informații despre drona Crazyflie 2.0 7

2.1 Aspecte hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Aspecte software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3 Modelul dinamic al quadcopter-ului 12
4 Schema de reglare propusă 15

4.1 Stabilizarea UAV-ului folosind un regulator PID . . . . . . . . . . . . . . . 16
4.2 Stabilizarea UAV-ului folosind controller-ul TC(varianta cu PID) . . . . . 17
4.3 Stabilizarea UAV-ului folosind controller-ul TC(varianta cu PD) . . . . . . 21
4.4 Analiză comparativă a celor două variante de control . . . . . . . . . . . . 22
5 Strategii de control 26
5.1 Controlul de traiectorie cu un controller PID . . . . . . . . . . . . . . . . . 26
5.2 Cuplarea controller-ului de stabilizare TC cu controller-ul PID de control
al traiectoriei . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
5.3 Introducerea frecării cu aerul . . . . . . . . . . . . . . . . . . . . . . . . . 33
6 A doua strategie de control 36

6.1 Cuplarea controller-ului de stabilizare TC cu un controller PD de control
al traiectoriei . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
7 Strategia de control optimal 38

7.1 Despre regulatorul de tip MPC . . . . . . . . . . . . . . . . . . . . . . . . 38
7.2 Yalmip . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
7.3 Controller-ul TC de nivel jos bazat pe regulatorul MPC . . . . . . . . . . . 42
7.4 Constrângerile de comandă ale controller-ului MPC . . . . . . . . . . . . . 44
7.5 Cuplarea controller-ului TC bazat pe MPC de nivel jos cu controller-ul
PID de nivel sus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
7.6 Introducerea frecării cu aerul . . . . . . . . . . . . . . . . . . . . . . . . . 49
8 Concluzii 52
9 Anexă 54
A Stabilizarea UAV-ului cu controllerul PID 54

A.1 Funcția de generare a traiectoriei liniare . . . . . . . . . . . . . . . . . . . 54
A.2 Scriptul de inițializare de parametrii . . . . . . . . . . . . . . . . . . . . . 54
A.3 Schema Simulink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
A.4 Controller-ul PID de nivel jos . . . . . . . . . . . . . . . . . . . . . . . . . 56
A.5 Dinamica internă a quadcopterului . . . . . . . . . . . . . . . . . . . . . . 56
B Stabilizarea UAV-ului cu controllerul TC 56

B.1 Schema Simulink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
B.2 Controller-ul TC de nivel jos . . . . . . . . . . . . . . . . . . . . . . . . . 57
1
C Cuplarea controller-ului TC de nivel jos cu controller-ul PID de nivel
sus 58
C.1 Controller-ul PID de nivel sus . . . . . . . . . . . . . . . . . . . . . . . . . 58
C.2 Schema Simulink a cuplării . . . . . . . . . . . . . . . . . . . . . . . . . . 58
C.3 Scriptul de inițializare de parametrii(conține și coeficienții de frecare) . . . 58
D Cuplarea controller-ului TC de nivel jos cu controller-ul PD de nivel

sus 61
D.1 Controller-ul PD de nivel sus . . . . . . . . . . . . . . . . . . . . . . . . . 61
D.2 Codul suplimentar față de cel anterior . . . . . . . . . . . . . . . . . . . . 61
E Cuplarea controller-ului TC bazat pe MPC de nivel jos cu controller-ul

PID de nivel sus 62
E.1 Funcție de extragere a comenzii MPC din variabila de tip container a
Yalmip-ului . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
E.2 Scriptul de inițializare de parametrii + Plotarea graficelor . . . . . . . . . 62
E.3 Controller-ul TC bazat pe MPC de nivel jos . . . . . . . . . . . . . . . . . 67
Bibliografie 67
2
Listă de figuri
2.1 Quadcopter Crazyflie 2.0[9] . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Structura Hardware a quadcopterului[9] . . . . . . . . . . . . . . . . . . . 8
2.3 Modulul radio Crazyradio[9] . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.4 Controlul UAV-ului de pe PC . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.5 Controlul UAV-ului de pe telefon . . . . . . . . . . . . . . . . . . . . . . . 11
3.1 Ilustrarea forțelor, mărimilor și a celor două cadre de referință[3] . . . . . . 13
4.1 Schema clasică de control al quadcopter-ului . . . . . . . . . . . . . . . . . 15
4.2 Mulțimile fezabile de parametrii de reglare ai controller-ului TC bazat pe
factori de corecție PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
factori de corecție PID cu Kp = 25 . . . . . . . . . . . . . . . . . . . . . . 20
factori de corecție PD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.5 Urmărire de unghi cu PID . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.6 Urmărire de unghi cu TC . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.7 Urmărire de unghi cu PID . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.8 Urmărire de unghi cu TC . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
5.1 Interacțiunile dintre stări, derivatele stărilor și comenzile de intrare . . . . 27
5.2 Influențele variației unghiurilor de rotație față de poziționarea în spațiu . . 28
5.3 Monitorizarea comportamentului sistemului cuplat (TC+PID) . . . . . . . 29
5.9 Monitorizarea comportamentului sistemului cuplat(TC+PID) cu frecare . . 34
5.10 Monitorizarea comportamentului sistemului cuplat(TC+PID) cu frecare . . 35
6.2 Monitorizarea comportamentului sistemului cuplat (TC+PD) cu frecare . . 36
7.1 Schema MPC discretă[11] . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
7.2 Monitorizarea comportamentului sistemului cuplat(TC+MPC+PID) . . . 47
7.3 Evoluția constrângerilor comenzilor MPC pentru un orizont de predicție finit 47
7.5 Monitorizarea comportamentului sistemului cuplat(TC+MPC+PID) cu
frecare . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
3
Listă de tabele
3.1 Controlul vitezelor rotoarelor ale UAV-ului[12] . . . . . . . . . . . . . . . . 14
4.1 Parametrii de reglare a controller-ului PID de nivel jos . . . . . . . . . . . 23
4.2 Parametrii de reglare a controller-ului TC bazat pe PID de nivel jos . . . . 24
4.3 Al doilea set de parametrii de reglare a controller-ului PID de nivel jos . . 25
4.4 Al doilea set de parametrii de reglare a controller-ului TC bazat de PID
de nivel jos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
5.1 Primul set de parametrii de reglare corespunzătoare primei strategii de
control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
5.2 Al doilea set de parametrii de reglare corespunzătoare primei strategii de
control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
5.3 Ultimul set de parametrii de reglare corespunzătoare primei strategii de
control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
7.1 Parametrii de reglare a controller-ului PID din cadrul ultimei strategii de
reglare optimale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4
1 Introducere
În ultima perioadă s-a manifestat un interes accentuat în ceea ce privește controlul

și manevrabilitatea unui grup de UAV-uri(Unmanned Aerial Vehicle), sau mai precis
reglarea unor mărimi pentru a se putea realiza urmărirea unei referințe de poziție, de
exemplu, într-un timp cât mai scurt, cu o acuratețe cât mai mare în timp ce generarea
de comenzi să fie cât mai simplă, mai naturală. UAV-urile s-au dovedit a fi utile în
diverse activități precum transporturi de bunuri pe cale aeriană, operațiuni de salvare
ce presupuneau survolarea deasupra unor zone și monitorizarea lor prin intermediul unor
camere , sau pur și simplu în cadrul unor activități de cercetare a zonelor periculoase sau
dăunătoare omului.
Pot spune că tocmai acest gen de activități mi-au stârnit curiozitatea și m-au
făcut să aleg această temă de proiect de licență. Încă de mic mă fascina modul în care
păsările zburau, astfel că mai târziu când mi-a fost introdusă noțiunea de avion am găsit
similarități în ceea ce privește modalitatea de menținere a zborului. Am putut percepe
faptul că aripile jucau un rol important în coordonarea zborului, însă când am aflat de
existența elicopterului și de faptul că acesta nu are nevoie de aripi pentru a menține
zborul, el putând în plus să decoleze de pe loc, atunci am rămas uimit și am început
să-mi pun multe întrebări. Mai târziu am aflat de existența aeronauticii ca știință de
bază a studiului navigației aeriene și atunci am realizat cât de vast, complex și important
a fost, este și va mai fi acest domeniu.
Ideea controlului unor astfel de vehicule aeriene la asemenea proporții și mai ales
știind cât de multă responsabilitate deține un pilot în contextul unui zbor, m-au descurajat
teribil, însă când am văzut pentru prima oară vehicule aeriene miniaturale telecomandate
am fost foarte impresionat. Faptul că acestea nu se adresau strict activităților de natură
militară ci puteau fi achiziționate de către consumatorul de rând m-a făcut să constat
dispariția acelei responsabilități temătoare și deci m-a ambiționat să continui să învăț cât
mai multe despre acest tip de vehicule aeriene, despre dinamicile implicate, dar mai ales
despre controlul acestora. Am decis să aleg această temă pentru a-mi respecta propria
promisiune, o temă în cadrul căruia voi încerca să implementez o strategie de control de
tip autonom, sau mai precis un control capabil să urmărească o referință pe baza unui set
de informații senzoriale ce trebuiesc interpretate, să rejecteze pe cât posibil perturbațiile,
dar mai ales să păstreze stabilitatea sistemului.
Vehiculul aerian miniatural pe care am ocazia să-l controlez este o dronă de tip
quadcopter. Ideea pe care ne bazăm în contextul implementării unui control autonom este
că această dronă este capabilă teoretic să regleze 6 mărimi de stare pe baza informațiilor
senzoriale provenite de la un accelerometru și de la un giroscop folosind 4 motoare dispuse
simetric ce acționează elici răspunzătoare de generarea forței de propulsie. Provocarea însă
o constituie faptul că toate aceste mărimi sunt cuplate între ele și deci orice variație a unei
mărimi le va influența automat pe toate celelalte într-o măsură mai mare sau mai mică.
Pentru a diminua efectul acestei cuplări, am ales o structură de reglare de tip cascadă
cu două controllere care să regleze separat mărimile de poziție ale dronei(coordonatele
x,y,z) și mărimile de orientare ale dronei(unghiurile de rotație ϕ,θ și ψ), obținându-se o
decuplare aparentă a acestor două tipuri de mărimi.
Pentru început, în a doua secțiune se va prezenta quadcopterul din punctul de vedere
hardware(al părților componente), al specificațiilor, al dotărilor și al suportului software
oferit de producători pentru controlul manual sau pentru implementarea unui control
autonom, după care vom modela sistemul pe baza unor relații matematice pentru a obține
5
o dinamică internă a dronei. Pe baza acestei dinamici obținute vom detalia separat câte
o strategie de control, după care o vom analiza din punct de vedere al implementabilității
prin intermediul unor simulări în Simulink/Matlab.
Aceste strategii de control se bazează pe aceeași structură de reglare de tip cascadă,
diferențele făcându-se remarcate la nivel de legi de reglare a cel puțin unuia dintre cele
două controllere de nivel sus sau jos. Menționez că pentru situația controller-ului de nivel
jos se va testa separat funcționalitatea acestuia prin decuplarea de bucla externă, însă
pentru situația controller-ului de nivel sus decuplarea nu va fi posibilă și deci testarea se
va realiza cu o variantă deja aleasă a controller-ului de nivel jos(varianta cu cele mai bune
performanțe înregistrate).
Așadar, inițial se vor analiza separat 3 variante de control al buclei interne de
reglare din punctul de vedere al stabilității și al urmăririi de referință, după care va avea
loc cuplarea buclei interne de reglare cu cele mai bune perfomanțe înregistrate cu cealaltă
buclă de reglare externă, proces ce va presupune găsirea parametrilor de reglare potriviți
pentru o asemenea sarcină. Voi detalia apoi 3 seturi de parametrii de reglare cu scopul de
evidenția influența pe care o are modificarea unor parametrii asupra procesului însine de
reglare. Desigur voi motiva alegerea de creștere sau de scădere a valorilor parametrilor de
reglare față de valorile precedent și voi menționa așteptările pe care le am de la răspunsul
sistemului modificând anumiți parametrii.
Apoi în cadrul strategiei de control curente voi folosi setul cu care am obținut cele mai
bune rezultate pentru a efectua o simulare mai realistă, alegând să nu mai neglijez forțele
de frecare cu aerul. În mod normal ar trebuie să se sesizeze o degradare a performanțelor,
însă în situația în care impactul este mult prea mare voi analiza situația și voi porni o
discuție pe baza graficelor simulării.
În continuare voi prezenta a doua strategie de control, al cărui controller la nivel înalt
va diferi de controller-ul de nivel înalt al strategiei de control precedente. Voi încerca să-mi
dau seama dacă acestă nouă strategie de control este mai viabilă pe baza unor considerente
teoretice, realizând un scenariu de simulare și comentând rezultate obținute.
În ultima parte a acestui proiect voi detalia o a treia strategie de control bazată
pe un controller de nivel jos similar cu cel din prima strategie de control. Spre deosebire
de prima strategie de control aceasta din urmă se bazează pe un regulatorul MPC(Model
Predictive Control) înclus în cadrul controller-ului de nivel jos al cărui lege de reglare
se bazează pe rezolvarea unei probleme de optimizare și deci acest lucru ar putea oferi
strategiei de control un caracter optimal, ea putea fi considerată în consecință singura
strategie de reglare optimală.
Să începem, așadar, printr-o prezentare a quadcopterului pe a cărui dinamică se
bazează toate aceste strategii de control.
6
2 Informații despre drona Crazyflie 2.0
Figura 2.1: Quadcopter Crazyflie 2.0[9]
2.1 Aspecte hardware
Quadcopterele sunt defapt elicoptere cu 4 rotoare ce pot fi programate pentru a

efectua zboruri autonome sau pot fi controlate manual printr-o manetă de control. Ro-
toarele sunt orientate în sus și sunt plasate într-o formă de pătrat și la distanță egală față
de centrul de masă al quadcopter-ului [7]. Prin intermediul profesorului meu coordonator
Florin Stoican căruia îi mulțumesc în primul rând, am putut să adopt ca și suport de
implementare un quadcopter în miniatură. Este vorba o dronă Crazyflie 2.0 ce cântărește
27 grame, are o autonomie de 7 minute iar motoarele ce acționează elici de 7 mm face
posibilă suportarea unei sarcini externe de maxim 15 grame[9].
Acest quadcopter în miniatură este dezvoltat de o echipă de consultanță denumită
Bitcraze. Suedezi la origini, ei au mai lansat o versiune anterioară în anul 2009, Crazyflie
1.0, o versiune care a adoptat arhitectura bazată pe un singur microcontroller. Versiunea
2.0 din 2013 de care dispunem are o arhitectură îmbunătățită, în sensul că acum se ba-
zează pe două microcontrollere interconectate[9]. Motivul acestei schimbări îl presupune
posibilitatea de a adăuga un suport bluetooth de joasă energie(BLE) pentru ca platforma
Crazyflie să permită controlul direct de pe un telefon mobil sau tabletă.
Așadar, pe lângă microcontroller-ul STM32F405, se folosește și microcontroller-ul
nRF51822, acesta fiind compatibil atât cu modulul de comunicație radio Crazyradio cât
și cu microprocesoarele nRF24Lx1 folosite pe Crazyflie 1.0. Acest nou microcontroller
prezintă o arhitectură de tip ARM, destul de puternică pentru a face față comunicării
radio și a administra alimentarea cu energie, însă nu îndeajuns de puternică pentru a
administra singură toată platforma, așadar de aceea s-a ajuns la soluția utilizării a două
microcontrollere interconectate(Figura 2.2).
7
Figura 2.2: Structura Hardware a quadcopterului[9]
După cum se poate observa din figura de mai sus, rolurile fiecărui microcontroller
sunt bine definite, astfel că:
• Microcontroller-ul nRF51 este responsabil de pornire/oprire(logica ON/OFF), ali-

mentarea celorlalte componente ale sistemului precum microcontroller-ul master sau
senzorii de pe IMU(Inertial Measurement Unit), administrarea încărcării bateriei și
măsurări de voltaj, încărcarea bootloader-ului pentru modulul de comunicație radio,
comunicația radio și bluetooth(BLE) și detectarea și verificarea plăcilor de extensie
instalate[9].
• Microcontroller-ul STM32 se ocupă de restul, adică de citirea senzorilor, controlul

zborului, programele suplimentare ale utilizatorului și de telemetrie(i.e. înregistra-
rea datelor colectate și transmiterea lor sigură către echipamentul destinat monito-
rizării). În situația voltajului citit de pe baterie, acesta a fost menționat ulterior ca
sarcină a celuluilalt microcontroller și deci sarcina acestui microcontroller este doar
de a comunica cu nRF51 și să ”paseze” mai departe datele citite[9].
Motoarele
Motoarele folosite pe Crazyflie 2.0 acționează elicele conectate la ele, transformând

astfel mișcarea de rotație rezultantă într-o forță de propulsie Tb [4]. S-a ales varianta
motoarelor de curent continuu deoarece acestea sunt destul de cunoscute pentru raportul
mare de cuplu generat față de inerție, oferind un control precis al vitezei chiar și la o
turație maximă de 58800 rpm. Comanda către aceste motoare este dată prim modularea
8
lățimii pulsurilor sau PWM(Pulse Width Modulation), viteza lor putând fi schimbată fără
vreo variație de voltaj.
Bateria
Bateria este de tip Litiu-Polimer(Li-Po)de 3.7V și 240mAh și este impresionantă

din punctul de vedere al capacității energetice de stocare mari raportate la greutate și
dimensiuni[4]. Bateria este însoțită de un modul ce conține un circuit de protecție(PCM)
pentru a evita apariția avariilor de natură electrică. Bateria se încarcă în aproximativ
40 de minute prin intermediul unui încărcător Li-Po ce folosește un conector micro USB.
Există și posibilitatea înlocuirii bateriei din fabrică cu alta mai durabilă.
Modulul radio
Modulul radio folosit poartă denumirea de Crazyradio și folosește o mufă USB

pentru comunicarea radio. Quadcopterul conține chip-ul nRF51822 pe placă pentru co-
municarea radio cu acest modul, acesta conținând și el la rând lui un chip nRF51822 de
tip semiconductor nordic. Așadar, aceste două chip-uri comunică între ele printr-o bandă
de comunicație de tip ISM la 2.4GHz[4], raza de acțiunea lungindu-se în jurul distanței
de 1 km mulțumită unui amplificator de semnal de 20dB.
Figura 2.3: Modulul radio Crazyradio[9]
În ceea ce privește intercomunicarea dintre cele două microcontrollere, aceasta

este realizată printr-un emițător/receptor asincron universal, controller-ul nRF51 purtând
rolul de slave iar controller-ul STM32 de master. Acesta din urmă este răspunzător de
controlul și de stabilitatea sistemului, preluând informații citite de la senzori și adoptând
decizii. Operează cu date pe 32 de biți, are 196kB de memorie RAM și o frecvență de
operare mărită față de Crazyflie 1.0 la 168MHz.
Microcontroller-ul STM32 comunică printr-o interfață I 2 C cu un bloc de măsurare
inerțială(IMU) sau MTD(Motion Tracking Device) denumit MPU-9250. El reprezintă
defapt un modul de tip multi-chip organizat pe două matrițe integrate în același pachet.
O matriță găzduiește un giroscop și un accelerometru, fiecare pe 3 axe, iar cealaltă matriță
gazduiește un magnetometru pe 3 axe[5].
9
Accelerometrul
Accelerometrul folosit de către Crazyflie 2.0 este de tip sistem micro-electro-mecanic(MEMS)

ce măsoară accelerațiile în 3 dimensiuni[4], sau altfel spus măsoară mișcarea de translație
a quadcopterului. Accelerometrul are câte o greutate pentru fiecare axă și măsoară acce-
lerația prin intermediul unor senzori capacitivi ce detecteză fiecare variație a greutăților.
Acest accelerometru are integrat un convertor analog-digital(CAN) integrat pe 16 biți,
se poate testa singur, dezvoltă un curent nominal de 450 µA și poate funcționa doar în
gama -40◦ →+85◦ Celsius.
Giroscopul
Rolul giroscopului este de a menține orientarea obiectului. Asemenea accelerome-

trului, un giroscop de tipul MEMS este folosit în cadrul acestui dispozitiv de urmărire de
mișcare al dronei, el măsurând mișcarea de rotație prin intermediul vibrației unei greu-
tăți de probă, sesizând astfel efectul Coriolis cauzat de viteza unghiulară[4]. Altfel spus,
giroscopul este folosit pentru a măsura rata de rotire pe axa X,Y și Z. Acest giroscop
are integrat un convertor analog-digital(CAN) pe 16 biți și un filtru trece-jos digital pro-
gramabil, se poate testa singur, dezvoltă un curent nominal de 3.2mA și poate funcționa
doar în gama -40◦ →+85◦ Celsius.
Magnetometrul
Magnetometrul folosit este și el de tipul MEMS și măsoară direcția și intensitatea

câmpului magnetic pe fiecare dintre cele 3 axe. Magnetometrul folosit poartă denumirea
de HMC5883L și utilizează tehnologia anizotropă magnetorezistivă(AMR). Practic, acest
dispozitiv este folosit pentru detecția senzitivității și liniarității axelor interne. Magne-
tometrul acestui quadcopter are o rezoluție de ieșire de 16 biți, se poate testa singur,
dezvoltă un curent nominal de 280µA și poate măsura doar în gama ±4800µT.
Aceste 3 componente oferă blocului de măsurare inerțială MPU-9250 9 grade liber-
tate, însă modulul MPU-9250 în particular dispune de un al zecelea grad de libertate prin
introducerea posibilității de interfațare cu un senzor digital neinerțial printr-un port auxi-
liar de comunicare utilizând protocolul I 2 C. Accelerometrul, giroscopul și magnetometrul
reprezintă așadar senzori digitali inerțiali, senzori ce se pot dovedi utili în cadrul reglării
unui sistem liniar. Particularizând aceste beneficii la quadcopterul nostru, sau mai pre-
cis la sistemul nostru puternic neliniar(voi detalia acest aspect în secțiunea următoare),
putem constata că aceste unelte pot fi folosite în contextul liniarizării sistemului nostru,
însă probabilitatea ca senzorii să acumuleze erori de citire în timp este destul de mare.
Putem intui faptul că în condițiile implementării oricăror strategii de reglare bazate
pe preluarea informațiilor de la acești senzori digitali inerțiali, se va ajunge la un moment
dat la o funcționare defectuasă. Din cauza acumulărilor de erori, datele măsurate nu-și vor
mai demonstra veridicitatea, cu toate că în spate procesul de reglare realizează aceleași
transformări, aceleași calcule.
În contextul acestei probleme, ne este oferită ca alternativă acel port auxiliar de
comunicație ce poate fi folosit de exemplu de un senzor de presiune, el neacumulând erori
de calcul în timp. Acel senzor de presiune poate fi folosit ca un indicator al altitudinii
dacă plecăm de la premisa că valoare presiunii atmosferice este dependentă de altitudine.
Dezavantajul unei astfel de variante l-ar putea constitui cu siguranță costul unui senzor de
presiune destul de sensibil astfel încât să sesizeze diferențe de presiune în raza de acțiune
a semnalului radio sau bluetooth.
10
2.2 Aspecte software
Pentru programarea quadcopterului Crazyflie 2.0, firma producătoare Bitcraze

oferă o platformă interesantă de tip open-source(i.e. oferită spre folosire sub o licență
liberă ce oferă acces utilizatorilor la codul sursă), permițănd utilizatorilor să reprogrameze
acest quadcopter. Mai mult, dacă se va opta pentru o reproiectare a dispozitivului atunci
accesul la scheme și codul sursă al firmware-ului sunt și ele de tip open-source[4].
Pentru programarea UAV- ului se oferă ca variantă utilizarea limbajul dinamic
de programare Python. Python este un limbaj elaborat în C și este folosit pentru pro-
gramarea aplicațiilor web în mare parte însă aplicabilitatea sa se extinde până la astfel
de aplicații. El este preferat datorită manierei simple în care se poate scrie un cod de
program, dispunând în același timp de o varietate impresionantă de biblioteci de metode.
În primă instanță, pentru controlul manual al dronei, producătorii oferă două po-
sibilități: controlul dronei prin intermediul unei aplicații instalate pe telefon sau tabletă
sau controlul dronei prin intermediul unei mașini de calcul. Varianta aplicației de pe
un dispozitiv mobil este cea mai rapidă, ea presupunând instalarea aplicației, conectarea
modulului Crazyradio prin USB la dispozitiv, iar apoi folosind ecranul tactil se putea
comanda în două grupe de câte două mărimi , unghiurile de rotație și propulsia dorită.
Pentru situația în care se dispunea de o manetă de control cu minim 4 axe de control
analogice se putea adopta metoda controlului dronei folosind un sistem de calcul. Echipa
Bitcraze a pus la dispoziția utilizatorilor o mașină virtuală ce conținea toate configurațiile
necesare conectării cu Crazyflie, utilizatorul rămând a emula și a rula acea mașină virtuală
și selectând în cadrul aplicației adresa radio la care să se stabilească conexiunea cu drona
prin intermediul modului Crazyradio conectat prin USB. La stabilirea conexiunii, vor fi
afișate o multitudine de informații de la senzori, printre care și nivelul de încărcare a
bateriei, calitatea semnalului, turația fiecărui motor etc. toate prezentate într-o manieră
prietenoasă cu utilizatorul.
Figura 2.5: Controlul UAV-ului de pe tele-

Figura 2.4: Controlul UAV-ului de pe PC fon
11
3 Modelul dinamic al quadcopter-ului
UAV-urile sunt controlate prin modificarea vitezelor unghiulare cu ajutorul motoa-

relor electrice. Provocarea controlului unui UAV o reprezintă faptul că acesta are 6 grade
libertate (3 de translație și 3 de rotație) cu 4 intrări independente. Dinamicile rezultate
sunt neliniare, iar frecarea este mult mai mică față de vehiculele de teren (se va considera
o amortizare a ei) .
Modelul dinamic de bază al UAV- ului reprezintă punctul de plecare al studiului
nostru, urmat apoi de legile de reglare corespunzătoare celor două nivele de control.
Metodele de control necesită informații precise despre poziție și orientare preluate de la
un giroscop, un accelerometru și multe alte dispozitive de măsură precum un modul GPS,
senzori laser etc.
Vom începe mai întâi cu aspectele de bază pentru modelarea, simularea și controlul
unui UAV după care vom prezenta mai multe strategii de control,toate acestea având
obiective comune precum urmărire de traiectorie și stabilitate.
Vom pleca de la un model matematic ce va descrie dinamica internă a dronei, relațiile
obținându-se din ecuațiile Newton-Euler și Euler-Lagrange, însă mai întâi trebuiesc făcute
câteva precizări și notații și stabilite câteva convenții. Pentru început trebuie menționat
faptul că exprimarea coordonatelor (atât liniare cât și unghiulare) se poate face în două
sisteme de referință : cadrul inerțial unde axele X și Y sunt definite de sol, iar axa Z
este dată de sensul opus indicat de greutate și cadrul de corp (sau bodyframe) definit
de orientarea quadcopter-ului unde brațele lui indică axele X și Y, iar sensul forței de
propulsie (sau thrust) indică axa Z. Conform unei convenții [2] am ales ca pozițiile liniare
să fie exprimate în cadrul inerțial, iar pozițiile unghiulare față de bodyframe. Voi specifica
mai jos, totuși, notațiile tuturor pozițiilor în ambele sisteme de referință.
Așadar, în cadrul inerțial voi nota cu ξ vectorul pozițiilor liniare x,y,z și cu η
vectorul pozițiilor unghiulare ϕ, θ, ψ. Unghiul ϕ de tangaj determină rotația în jurul axei
X , unghiul θ de ruliu determină rotația în jurul axei Y și unghiul ψ de girație reprezintă
rotația în jurul axei Z [7].
   
x ϕ [ ]
ξ
ξ = y  , η = θ , q= (3.1)
η
z ψ
În ceea ce privește bodyframe-ul, îi vom considera originea în centrul de masă
al dronei. Deci, în cadrul body vom considera vectorul vitezelor liniare exprimat față
de vectorul pozițiilor liniare în cadrul inerțial, el fiind notat cu Vb și vectorul vitezelor
unghiulare față de vectorul pozițiilor unghiulare în cadrul inerțial, el fiind notat cu ν.
   
Vx,b p
Vb = Vy,b  , ν = q  (3.2)
Vz,b r
Există o metodă de trecere a pozițiilor liniare de la un sistem de coordonate la altul
cu ajutorul unei matrici de rotație R. Ea este o matrice ortogonală (RT = R−1 ) și se
obține prin folosirea conversiei la unghiuri Euler, având forma :
 
C(ψ)C(θ) C(ψ)S(θ)S(ϕ) − S(ψ)C(ϕ) C(ψ)S(θ)C(ϕ) + S(ψ)S(ϕ)
R =  S(ψ)C(θ) S(ψ)S(θ)S(ϕ) + C(ψ)C(ϕ) S(ψ)S(θ)C(ϕ) − C(ψ)S(ϕ) , (3.3)
−S(θ) C(θ)S(ϕ) C(θ)C(ϕ)
12
Figura 3.1: Ilustrarea forțelor, mărimilor și a celor două cadre de referință[3]
unde S(x) = sin(x) și C(x) = cos(x).  

ẋ
Practic, pentru un vector de viteze liniare v̄i = ẏ  definit în cadrul inerțial, vectorul

ż
−1
vitezelor liniare definit în bodyframe ar fi dat de relația v̄i = Rv̄b și viceversa
  v̄b = R v̄i .
p
O transformare asemănătoare a vectorului vitezelor unghiulare ν = q  face posibilă

r
trecerea lor din cadrul inerțial în bodyframe, această matrice de transformare fiind notată
cu Wη și viceversa din bodyframe în cadrul inerțial cu Wη−1 . Evident matricea Wη este
inversabilă însă nu ortogonală ca matricea de rotație R. Relațiile acestei transformări sunt
[7] :
    
p 1 0 −S(θ) ϕ̇
  
ν = Wη η̇ , q = 0 C(ϕ) C(θ)S(ϕ)   θ̇  , (3.4)
r 0 −S(ϕ) C(θ)C(ϕ) ψ̇
    
ϕ̇ 1 S(ϕ)T (θ) C(ϕ)T (θ) p
η̇ = Wη−1 ν,  θ̇  , = 0 C(ϕ) −S(ϕ)  q  , (3.5)
ψ̇ 0 −S(ϕ)/C(θ) C(ϕ)/C(θ) r
unde T (x) = tg(x) .
Vom aduce în discuție și inerția UAV-ului, chiar dacă masa acestuia este una foarte
de mică(de aproximativ 27g). Considerăm drona a fi un corp rigid [2]. Momentan vom
preciza doar că din cauza structurii simetrice a dronei în care axele bodyframe-ului coincid
cu axele principale de inerție ale corpului, matricea de inerție I este o matrice diagonală
cu Ixx = Iyy [2] :
 
Ixx 0 0
I =  0 Iyy 0  (3.6)
0 0 Izz
Altfel spus cele 4 brațe ale UAV-ului sunt aliniate cu axele X și Y ale bodyframe-ului.
În ceea ce privește viteza unghiulară ωi a unuia din cele i = {1, 2, 3, 4} rotoare,
aceasta crează o forță fi de propulsie(thrust) pe direcția axei rotorului, adică perpendi-
culare pe axele X și Y ale bodyframe-ului. Această forță este proporțională cu pătratul
vitezei unghiulare [2] și împreună cu accelerația rotorului generează un cuplu τMi în ju-
rul axei rotorului, cuplu ce afectează accelerația UAV-ului pe unul dintre cele 3 unghiuri
ϕ, θ, ψ. Relațiile sunt :
fi = kωi2 , τMi = bωi2 + IM ω̇i (3.7)
13
unde k este un factor de ridicare, b este o constantă de frecare(drag), iar IM este inerția
unui moment M. De cele mai multe ori valoare lui ω̇i este mult prea mică și deci este omis
al doilea termen din relația cuplului. Dispunem de 4 astfel de rotoare și deci trebuie să
luăm în calcul forța de propulsie rezultantă T (o forță ce afectează accelerația pe axa Z și
ține drona în aer) și cuplurile generate τϕ , τθ , τψ în direcția fiecărui unghi de rotație. Vom
nota cu Tt thrust-ul total pe toate cele 3 axe si τt cuplul total :
 
∑ 4 ∑ 4 0
T = fi = k ωi , T t = 0  ,
2  (3.8)
i=1 i=1 T
   
τϕ lk(−ω22 + ω42 )
τt =  τθ  =  lk(−ω12 + ω32 ) , (3.9)
τψ b(ω1 − ω2 + ω3 − ω4 )
2 2 2 2
unde l reprezintă distanța de la rotor la centrul de masă UAV, k este factorul de

ridicare, iar b este constanta de frecare.
Așadar putem observa din formula cuplurilor, vitezele unghiulare ale căror rotoare
trebuiesc modificate pentru a realiza cele 3 rotații ale dronei : descreșterea vitezei rotorului
nr.2 și creșterea vitezei rotorului nr.4 pentru o rotație pe axa X de tangaj, descreșterea
vitezei rotorului nr.1 și creșterea vitezei rotorului nr.3 pentru o rotație pe axa Y de ruliu,
creșterea vitezei rotorului nr.1 cu descreșterea vitezei rotorului nr.2 cu creșterea vitezei
rotorului nr.3 și descreșterea vitezei rotorului nr.4 pentru o rotație pe axa Z de girație.
Din relația forței de propulsie(thrust-ul) putem observa că aceasta acționează doar pe
direcția axei Z , în sensul opus al gravitației. Pentru o mai bună înțelegere a grupării
celor 4 rotoare puteți consulta tabelul de mai jos :
Rotor 1 Rotor 2 Rotor 3 Rotor 4

Propulsie ↑ ↑ ↑ ↑
Ruliu ↑ - ↓ -
Tangaj - ↑ - ↓
Girație ↓ ↑ ↓ ↑
Tabela 3.1: Controlul vitezelor rotoarelor ale UAV-ului[12]
Revenind la modelul matematic de proces , dedus din ecuațiile Newton-Euler și Euler-

Lagrange și ținând cont de aspectele de mai sus, putem formula ecuațiile diferențiale ce
caracterizează dinamica internă a quadcopterului [7] :
        
ẍ 0 C(ψ)S(θ)C(ϕ) + S(ψ)S(ϕ) Ax 0 0 ẋ
T 1
ÿ  = −g 0 + S(ψ)S(θ)C(ϕ) − C(ψ)S(ϕ) −  0 Ay 0  ẏ  , (3.10)
m m
z̈ 1 C(θ)C(ϕ) 0 0 Az ż
   −1   Iyy −Izz 
ṗ τϕ Ixx qr
−1 
q̇  =  τθ Iyy  IzzI−I
xx

−  Iyy pr  ,
xx
(3.11)
−1 Ixx −Iyy
ṙ τψ Izz pq
Izz
    
p 1 0 −S(θ) ϕ̇
q  = 0 C(ϕ) C(θ)S(ϕ)   θ̇  , (3.12)
r 0 −S(ϕ) C(θ)C(ϕ) ψ̇
14
unde Ax , Ay , Az sunt coeficienți ai forțelor de drag (de frecare) pentru vitezele de pe
T
fiecare direcție a cadrului inerțial, matricea corespunzătoare coeficientului m reprezintă
rezultatul înmulțirii matricii de rotație R cu Thrust-ul Tt ce conține doar propulsia T pe
axa Z , iar p,q și r reprezintă vitezele unghiulare exprimate în bodyframe.
Mai există câteva efecte aerodinamice ce ar putea fi incluse în model precum depen-
dența unghiului de atac față de propulsie, fluturarea de elice sau întreruperi ale fluxului
de aer. Influența efectelor aerodinamice sunt complicate, iar efectele sunt dificil de mode-
lat. De asemenea, unele efecte au un impact important doar pentru viteze mari. Totuși,
aceste efecte le vom exclude din modelul nostru.
Pentru controlul quadcopter-ului se propune descompunerea problemei de reglare
într-o buclă externă de reglare a poziției liniare (x,y,z) și o buclă internă de reglare a po-
ziției unghiulare (ϕ, θ, ψ) [2]. Dinamicile unui corp rigid sunt caracterizate de neliniarități
puternice și de aceea atenția trebuie focusată pe planificarea de traiectorie și optimizare
de problemă.
4 Schema de reglare propusă
Această schemă de reglare este des întâlnită în controlul UAV-urilor (Fig.4.1) și

presupune o structură de reglare automată în cascadă, reușind să exploateze decuplarea
dintre dinamicile de poziție și cele de rotație a quadcopterului.
Mai precis, se va considera un controller de poziție C(ξ, ξref ) al buclei externe
ce compară o referință externă dată ξref cu pozițiile reale ξ și generează unghiurile de
referință ηref pentru controller-ul buclei interne și propulsia(thrust-ul) T pentru a modela
direct dinamica quadcopterului. Controller-ul de rotație C(η, ηref ) aparține buclei interne
și primește așadar unghiurile de referință de la controller-ul buclei externe și le compară
cu unghiurile reale η pentru a genera mai departe cuplurile unghiurilor ce vor modela
dinamica quadcopter-ului.[2]
pozitiile reale propulsia T propulsia T pozitiile reale
cuplurile unghiurilor unghiurile reale

x_y_z_de_referinta pozitiile de referinta unghiurile de referinta
From Dinamica interna

Controller unghiuri
Workspace unghiurile de referinta
cuplurile unghurilor
unghiurile reale
Controller cupluri
Figura 4.1: Schema clasică de control al quadcopter-ului
În cadrul acestei scheme clasice de control, controller-ul de poziție este de obicei

răspunzător de oferirea propulsiei și referințelor de orientare a dronei, acestea permițând
mișcări simple precum deplasarea în linie dreaptă, mișcare circulară în jurul unui centru
15
fixat sau menținerea unei altitudini dorite [2]. Controller-ul de orientare generează cu-
plurile necesare dinamicii interne a UAV-ului pentu a realiza mișcări de rotație simultane
sau consecutive în jurul oricăror cele 3 axe (X,Y,Z).
Fiind o schemă de reglare în cascadă nu trebuie sub nicio formă neglijat faptul că
bucla internă de reglare trebuie să fie de minim 3 ÷ 4 ori mai rapidă decât bucla externă,
acest aspect urmând a fi tratat mai târziu cand vom aplica criteriul analitic de stabilitate
Hurwitz.
În secțiunile următoare vom detalia cele două controllere și vom simula un scenariu
de evoluție.
4.1 Stabilizarea UAV-ului folosind un regulator PID
Pentru a stabiliza quadcopterul este necesară utilizarea unui regulator PID, sta-
bilizarea realizându-se în nivelul inferior de control a dronei. Avantajele unui controller
PID sunt constituite de structura și implementarea simplă a sa. Forma generală a unui
PID [7]:
e(t) = xref (t) − x(t),
∫ t
d e(t)
u(t) = KP e(t) + KI e(τ )dτ + KD , (4.1)
0 dt
unde u(t) reprezintă comanda de control , e(t) reprezintă eroarea staționară, xref (t)
reprezintă starea dorită, de referință, x(t) reprezintă starea curentă și KP , KI , KD repre-
zintă parametrii elementelor proporțional, integral și derivativ din regulatorul PID.
Într-un UAV, sunt 6 stări(poziții ξ și unghiuri η), și doar 4 semnale de control
și anume vitezele unghiulare al celor 4 rotoare ωi . Interacțiunile dintre stări, propulsia
totală T și cuplurile τ create de rotoare sunt vizibile în dinamica internă a dronei definită
la ecuațiile (3.10),(3.11),(3.12).
Prin urmare ecuațiile regulatorului PID sunt [7]:
∫ t
τϕ = (Kϕ,D (ϕref˙ (t) − ϕ̇(t)) + Kϕ,I (ϕref (τ ) − ϕ(τ ))dτ + Kϕ,P (ϕref (t) − ϕ(t)))Ixx
o
∫ t
τθ = (Kθ,D (θref˙ (t) − θ̇(t)) + Kθ,I (θref (τ ) − θ(τ ))dθ + Kθ,P (θref (t) − θ(t)))Iyy (4.2)
o
∫ t
τψ = (Kψ,D (ψref˙ (t) − ψ̇(t)) + Kψ,I (ψref (τ ) − ψ(τ ))dτ + Kψ,P (ψref (t) − ψ(t)))Izz
o
unde g este accelerația gravitațională, Kx,P , Kx,I , Kx,D reprezintă valorile parametri-
lor controllerelor PID pentru elemente proporțional,integral și derivative corespunzătoare
unghiurilor ϕ, θ și ψ (altfel spus avem 3 regulatoare pentru unghiurile ϕ, θ, ψ ). Mai avem
masa m și momentul de inerție al UAV-ului I.
Dacă în plus s-ar dori și un control asupra înălțimii la care s-ar poziționa quadcopterul
atunci se poate introduce un al patrulea controller de tip PID pentru reglarea propulsiei
16
T astfel încât să se mențină altitudinea(drona să rămână în hovering).
∫ t
m
T = (g + Kz,D (zref˙ (t) − ż(t)) + Kz,I (zref (τ ) − z(τ ))dτ + Kz,P (zref (t) − z(t))) ,
o C(ϕ)C(θ)
Utilitatea acestui controller ar fi dovedită în situația în care s-ar dori implementarea

acestei variante de stabilizare a dronei fără intervenția controller-ului de nivel înalt de
reglare a altitudinii sau a valorii pe axa Z. Este evident că pentru o eventuală testare a
acestui controller de stabilizare ce dorește să urmărească anumite unghiuri de referințe
prin generarea de cupluri este necesară atingerea unei înălțimi nenule. Vom detalia în
secțiunile următoare un controller de nivel înalt aflat în buclă deschisă ce realizează pur
și simplu conexiunile și nu efectueză niciun fel de reglaj.
În ceea ce privește vitezele unghiulare corecte ale rotoarelor ωi , acestea pot fi calculate
din ecuațiile (3.8) și (3.9) cu valorile obținute anterior la (4.2) :
T τθ τψ
ω12 = − − ,
4k 2kl 4b
T τϕ τψ
ω22 = − + , (4.3)
4k 2kl 4b
T τθ τψ
ω32 = + − ,
4k 2kl 4b
T τϕ τψ
ω42 = + + ,
4k 2kl 4b
4.2 Stabilizarea UAV-ului folosind controller-ul TC(varianta cu

PID)
A doua variantă de control pentru un sistem multi-agent presupune utilizarea

aceleiași scheme clasice de reglare, însă cu o altă formă a regulatorului din bucla internă.
Mai precis, această variantă alternativă de controller folosește aceleași legi de reglare PID
și implicit beneficiază de aceleași avantaje de implementare simplă, însă le folosește pentru
a obține alte relații de dependență mai complexe.
Acest controller de cupluri(TC - Torque Controller) reprezintă o aplicație specială
de liniarizare a sisteme neliniare prin reacție negativă. A devenit populară în teoria
sistemelor moderne prin oferirea unor performanțe excelente de urmărire de traiectorii
prin compensarea neliniarităților[6]. Această liniarizare, ca bază a acestei strategii de
control, poate exploata capacitățile fizice al quadcopterului și deci va reuși să urmărească
traiectorii mai complexe în 3 dimensiuni. O bună cunoaștere a dinamicilor UAV- urilor
poate ajuta controller-ul să transforme sistemele neliniare în sisteme liniare echivalente
fără alte simplificări de transformări exacte și intrări de control adecvate.
Pentru a vedea concret care sunt diferențele dintre aceste două strategii de control
vom realiza o analiză comparativă a performanțelor celor două tipuri de controller de
cupluri pentru a ne da seama ce variantă ar fi mai potrivită pentru implementare pe
drona Crazyflie, ținând cont de caracterul multivariabil puternic neliniar al acestuia. Mai
întâi însă, vom detalia această variantă de TC.
17
Generarea cuplurilor de către TC se bazează pe relația[8]:
τη = M (η)τη′ + V (η, η̇) (4.4)
unde τη′ se consideră a fi un factor de corecție :
∫ t
′ ¨ ˙ − ηref˙ (t)) (4.5)
τη = ηref (t) − Kp (η(t) − ηref (t)) − Ki (η(τ ) − ηref (τ ))dτ − Kd (η(t)
0
Așadar am rămas cu necunoscutele M (η) și V (η, η̇).

Reluând ecuația (3.4) și derivând-o, vom obține :
ν̇ = Ẇη η̇ + Wη η̈ (4.6)
unde Ẇη se obține derivând matricea Wη . Matricea Ẇη obținută are forma:
 
1 0 −C(θ)θ̇
0 −S(θ)θ̇ S(θ)S(ϕ)θ̇ + C(θ)C(ϕ)ϕ̇ 
0 −C(ϕ)ϕ̇ −S(θ)C(ϕ)θ̇ − C(θ)S(ϕ)ϕ̇
Rescriind ecuația (3.12) obținem :
ν̇ = I −1 (τη − ε(ν)) (4.7)
unde ν am considerat în secțiunea 2 a modelului dinamic a quadcopterului ca fiind
vectorul vitezelor unghiulare exprimat în cadrul body, I este matricea de inerție, ε(ω)
reprezintă termenul redus la :  
(Iyy − Izz )qr
(Izz − Ixx )rp
(Ixx − Iyy )pq
iar τη cuplurile generate de forma τη = [τϕ τθ τψ ]T
În acest moment dacă egalăm ecuația (4.6) și (4.7) vom obține Ẇη η̇ + Wη η̈ =
I −1 (τη − ε(ν)). Înmulțind această relație la stânga cu I și trecând termenul ε(ν) în partea
stângă obținem :
IWη η̈ + (I Ẇη η̇ + ε(Wη η̇)) = τη (4.8)
Această ecuație (4.8) reprezintă dinamica de rotație a UAV-ului sau simplificată :
M (η)η̈ + V (η, η̇) = τη . Identificând termenii M (η) și V (η, η̇) din ce am obținut, putem
rescrie relația pe baza căreia sunt generate cuplurile de controller nostru TC [8]:
τη = M (η)τη′ + V (η, η̇) ⇔ τη = IWη τη′ + I Ẇη η̇ + ε(Wη η̇) (4.9)

Având acum relația ce definește cuplurile τη , trebuie să găsim parametrii corespunză-
tori factorului de corecție pentru a asigura stabilitatea sistemului, în cazul nostru factorul
de corecție având forma unui regulator PID.
Reamintim că am plecat de la formula de generare a cuplurilor τη = M (η)τη′ +V (η, η̇)
și am ajuns la forma τη = M (η)η̈ + V (η, η̇). Egalând aceste două ecuații vom simplifica
neliniaritățile și vom putea obține un model stabil:
M (η)τη′ = M (η)η̈ ⇒ M (η)η̈ − M (η)τη′ = 0 ⇒ M (η)(η̈ − τη′ ) = 0

Căutăm să rezolvăm η̈ − τη′ = 0, iar din ecuația (4.5) obținem o dinamică de buclă închisă
liniarizată prin reacție:
∫ t
¨ ¨
(η(t)−ηref (t))+Kp (η(t)−ηref (t))+Ki ˙
(η(τ )−ηref (τ ))dτ +Kd (η(t)− ηref˙ (t)) = 0 (4.10)
0
18
Înlocuind η − ηref cu ε vom obține:
∫ t
¨ + Kp ε(t) + Ki
ε(t) ˙ =0
ε(τ )dτ + Kd ε(t)
0
Dacă luăm în considerare cazul real în care intervine o perturbație d(t) atunci
această ecuație nu va mai nulă și deci va intra în acțiune factorul de corecție τη′ :
∫ t
¨ + Kp ε(t) + Ki
ε(t) ˙ = d(t)
0
Aplicând transformata Laplace obținem dinamica de buclă închisă ce poate fi sta-

bilizată cu parametrii Kp ,Ki și Kd :
ε(s)
s2 ε(s) + Kp ε(s) + Ki + Kd ε(s)s = D(s)
s
⇒ funcția de transfer corespunzătoare sistemului automat este :
ε(s) 1
H(s) = = 3 (4.11)
D(s) s + Kd s2 + Kp s + Ki
În alegerea acestor parametrii trebuie să ne asigurăm că polii acestei funcții de
transfer au parte reală negativă pentru a se asigura stabilitatea sistemului. Așadar, pentru
a defini constrângerile necesare asigurării stabilității, vom aplica în continuare criteriul
de stabilitate Hurwitz.
Menționăm că pentru a respecta structura schemei de reglare automată de tip
cascadă, parametrii de reglare trebuie aleși astfel încât bucla aceasta(internă) să fie de cel
puțin 3 ÷ 4 ori mai rapidă decât bucla externă. Acești timpi se reflectă în soluțiile acestui
polinom și anume cu cât partea reală dorită negativă este mai mică, cu atât timpii vor fi
mai mici.
Criteriul general de stabilitate menționează faptul că pentru ca un sistem automat
să fie stabil, este necesar și suficient ca toți polii funcției de transfer să fie localizați în
semiplanul stâng al planului complex [1].
Criteriul de stabilitate Hurwitz(denumit și criteriul coeficienților) [1] este un criteriu
analitic de stabilitate ce se bazează pe relația care trebuie să existe între coeficienții unei
ecuații diferențiale pentru ca rădăcinile sale să se localizeze în semiplanul stâng din cadrul
reprezentării în planul complex. În ceea ce privește condiția de aplicare a acestui criteriu,
toți coeficienții trebuie să fie pozitivi.
În funcție de gradul polinomului se determină numărul de determinanți Hurwitz
ce trebuiesc construiți. Pentru ca un sistem să fie stabil, este necesar și suficient ca acei
coeficienți corespunzători polilor sistemului să contribuie la construcția determinanților
Hurwitz strict pozitivi. În situația polinomului de la ecuația (4.11), calculăm determi-
nanții Hurwitz și obținem:
H1 = K d ⇒ K d > 0
[ ]
Kd Ki
H2 = > 0 ⇒ Kd Kp − Ki > 0
1 Kp
Avem 2 cazuri :
1. Kd ∈ (0, 1) ⇒ Kd Kp < Ki ⇒ caz nefavorabil pentru îndeplinirea condiției propuse
2. Kd ∈ [1; ∞) ⇒ Kp > Ki
19
 
Kd Ki 0
H3 =  1 Kp 0  > 0 ⇒ Kd Kp Ki − Ki2 > 0 ⇒ Kd Kp > Ki
0 Kd Ki


1.Kd > 1
Avem așadar constrângerile : 2.Kp > Ki


3.Kd Kp > Ki
Aceste constrângeri de alegere a parametrilor de reglare Kp , Kd și Ki ar fi asigurat
așadar, stabilitatea sistemului nostru. Pentru a ilustra mai bine această interpretare a
constrângerilor, puteți vedea în Figura 4.2 o reprezentare în 3 dimensiuni a mulțimilor
fezabile de parametrii de reglare. Fiecare axă va reprezenta câte un parametru de re-
glare, iar mulțimile de parametrii ce vor respecta aceste constrângeri vor avea următoare
reprezentare:
Figura 4.2: Mulțimile fezabile de parametrii de reglare ai controller-ului TC bazat pe

factori de corecție PID
Dacă spre exemplu, s-ar fi dorit respectarea unei constrângeri suplimentare de

forma Kp = 25, atunci una din cele 3 mulțimi s-ar fi concentrat într-un singur punct, iar
în consecință s-ar fi ilustrat combinațiile fezabile rămase de parametrii de reglare de tip
Ki și Kd ce ar fi definit suprafața unei figuri geometrice în două dimensiuni:

factori de corecție PID cu Kp = 25
20
4.3 Stabilizarea UAV-ului folosind controller-ul TC(varianta cu
PD)
O altă variantă de controller TC funcționează după aceleași principii, oferă aceeași

expresie a cuplurilor generate τη = M (η)τη′ + V (η, η̇), însă diferența constă în faptul că de
data aceasta se bazează pe un controller PD pentru a obține acele relații de dependență
mai complexe . Practic, calculele rămân aceleași, se obține din nou echivalența sistemului
neliniar cu unul liniar, însă dinamica de buclă închisă liniarizată prin reacție nu va mai
conține componenta integrală :
¨ − ηref¨(t)) + Kp (η(t) − ηref (t)) + Kd (η(t)

(η(t) ˙ − ηref˙ (t)) = 0 (4.12)
Înlocuind η − ηref cu ε vom obține:
¨ + Kp ε(t) + Kd ε(t)
ε(t) ˙ =0
Dacă luăm în considerare cazul real în care intervine o perturbație d(t) atunci
această ecuație nu va mai nulă și deci va intra în acțiune factorul de corecție τη′ :
¨ + Kp ε(t) + Kd ε(t)
ε(t) ˙ = d(t)
Aplicând transformata Laplace obținem dinamica de buclă închisă ce poate fi sta-

bilizată cu parametrii Kp și Kd :
s2 ε(s) + Kp ε(s) + Kd ε(s)s = D(s)
⇒ funcția de transfer corespunzătoare sistemului automat este :
ε(s) 1 1
H(s) = = 3 = (4.13)
D(s) s + Kd s 2 + Kp s s(s2 + Kd s + Kp )
Din forma acestui polinom se poate observa că un pol este nul (s1 = 0) și deci sistemul
nostru va manifesta în cel mai bun caz un comportament marginal stabil. Pentru a verifica
acest aspect vom aplica în continuare criteriul de stabilitate Hurwitz pentru polinomul
s2 + Kd s + Kp și obținem:
H1 = K d ⇒ K d > 0
[ ]
Kd 0
H2 = > 0 ⇒ Kd Kp > 0 ⇒ Kp > 0
1 Kp
{
1.Kd > 0
Avem așadar constrângerile :
2.Kp > 0
Aceste constrângeri de alegere a parametrilor de reglare Kp și Kd ar fi asigurat
așadar, stabilitatea marginală a sistemului nostru. Pentru a ilustra mai bine această in-
terpretare a constrângerilor, puteți vedea în Figura 4.4 o reprezentare în două dimensiuni
a mulțimilor fezabile de parametrii de reglare. Ambele axe va reprezenta câte un para-
metru de reglare, iar mulțimile de parametrii ce vor respecta aceste constrângeri vor avea
următoare reprezentare:
21
factori de corecție PD
Dacă s-ar fi considerat același gen de exemplu în care s-ar fi dorit respectarea unei
constrângeri suplimentare de fixare a unui parametru, atunci una din cele două mulțimi
s-ar fi concentrat într-un singur punct, iar în consecință s-ar fi ilustrat valorile fezabile
rămase de parametrii de reglare de tip Kd ce ar fi definit un segment de dreaptă într-o
singură dimensiune.
4.4 Analiză comparativă a celor două variante de control
În continuare vom realiza o analiză comparativă a variantelor de control cu PID

și cu TC bazat pe PID din punctul de vedre al stabilității și al urmăririi de referință în
contextul implementării și a obținerii unor performanțe din partea dronei Crazyflie. Ne
vom folosi de simulările realizate în Simulink pentru a ilustra diverse aspecte de interes,
simulări în cadrul cărora nu avem cuplat tot sistemul ci stimulăm separat doar regulatorul
buclei interne cu trepte de referință. Trebuie reamintit că pentru fiecare grad de libertate
dispunem de un regulator PID și deci vom avea un total de 18 parametrii de reglare,
parametrii de la care mi-am luat libertatea de a-i modifica cu scopul obținerii unor soluții
favorabile. Așadar fiecărei variante de control îi va corespunde câte un set parametrii
de reglare pentru fiecare controller PID, parametrii fiind menționați în cadrul fiecărei
ilustrări pentru a se evita ambiguitățile.
În ceea ce privește stabilitatea sistemului, spre deosebire de varianta de control
ce presupunea folosirea unui PID pentru generarea de cupluri către dinamica internă
a quadcopterului, varianta cu TC garanta faptul că prin ecuațiile matematice pe care
se bazează, efectul neliniarităților sistemului reglat se anulau cu efectul neliniarităților
introduse de regulatorul în sine(acesta folosind un model neliniar), obținându-se așadar
un sistem echivalent liniar. Acesta este motivul pentru care am tratat aspectul stabilității
în cazul TC și am obținut în final anumite restricții de alegere a parametrilor de reglare
aferenți.
22
Pentru varianta reglării de cupluri doar cu un PID, ne bazăm pur și simplu pe faptul
că factorii de corecție corespunzători elementului proporțional, integrativ și proporțional
vor reuși să anuleze efectul neliniarităților, parametrii de reglare fiind obținuți printr-o
metodă empirică. De aceea ne așteptăm ca performanțele obținute din cadrul variantei
de control cu TC să fie mai bune.
Din moment ce am specificat că vom analiza momentan cele două strategii din
punctul de vedere al stabilității, am ales să stimulez ambele variante de controllere cu o
”treaptă” de valoare 0 și am obținut următoarele rezultate:
→ varianta controlului cu PID

Parametrii de reglare x y z ϕ θ ψ
Kp 10 10 10 10 10 10
Ki 0 0 0 0 0 0
Kd 5 5 5 5 5 5
Tabela 4.1: Parametrii de reglare a controller-ului PID de nivel jos
Orientarea UAV-ului în spațiu

15
φ
θ
ψ
10 "Treapta" lui φ incepe la t=3, a lui θ la t=4 si a lui ψ la t=5
°(grade) 0
−5
−10
−15
0 2 4 6 8 10 12 14 16 18 20
sec
Figura 4.5: Urmărire de unghi cu PID
→ varianta controlului cu TC
În ceea ce privește stabilitatea am reușit să configurez ambele controllere să func-
ționeze cu aceeași parametrii de reglare. Din graficele obținute se poate observa cum
ambele pot asigura stabilitatea atâta timp cât referința este nulă, iar cum era de așteptat
performanțele TC sunt mai bune decât cele ale PID-ului. Față de TC, regulatorul PID
manifestă inițial o perioadă scurtă de variație rapidă de rotație pe fiecare axă, însă această
perioadă scurtă de timp este suficient de mare pentru a face ca sistemul să se stabilizeze
cu aproximativ 2 secunde mai târziu față de TC.
23
Kp 10 10 10 10 10 10
Ki 0 0 0 0 0 0
Kd 5 5 5 5 5 5
Tabela 4.2: Parametrii de reglare a controller-ului TC bazat pe PID de nivel jos

12
φ
θ
10 ψ
"Treapta" lui φ incepe la t=3, a lui θ la t=4 si a lui ψ la t=5
6
°(grade)
−2
−4
0 2 4 6 8 10 12 14 16 18 20
sec
Figura 4.6: Urmărire de unghi cu TC
Așadar controller-ul TC s-a dovedit a stabiliza mai repede sistemul cu aproximativ

2 secunde, el suferind totuși câteva variații mai lente de rotație pe fiecare axă.
Al doilea aspect vizat de comparația noastră îl reprezintă urmărirea de referință,

așadar am atribuit o valoare nenulă treptei și am obținut următoarele rezultate :
→ varianta controlului cu PID : aici rezultatele au fost foarte dezamăgitoare în sensul

că nu am găsit parametrii de reglare astfel încât aceste variante de controllere să urmă-
rească măcar una din cele 3 grade de libertate, așadar pentru reprezentarea din Figura
4.7 am folosit aceiași parametrii de reglare ca ai variantei cu TC. Regulatorul încearcă
să urmărească referința,totuși, însă se poate observa cât de mare este subreglajul și deci
în contextul implementării acestei legi de reglare pe dronă, împreună cu constrângerile și
limitările ei chiar nu ar reprezenta o variantă viabilă.
24
Kp 10 10 10 10 10 10
Ki 0 0 0 0 0 0
Kd 0 0 0 5 5 5
Tabela 4.3: Al doilea set de parametrii de reglare a controller-ului PID de nivel jos
14
x 10
0.5
φ
θ
0 ψ
"Treapta" lui φ incepe la t=7
-0.5
-1
°(grade)
-1.5
-2
-2.5
-3
-3.5
0 2 4 6 8 10 12 14 16 18 20
sec
Figura 4.7: Urmărire de unghi cu PID
→ varianta controlului cu TC : aici rezultatele au fost mai acceptabile în sensul că

am găsit destul de ușor parametrii de reglare astfel încât aceste variante de controllere să
urmărească toate cele 3 grade de libertate. Se poate observa efectul cuplării dintre cele
3 grade de libertate(Figura 4.8) în sensul influențării unei variații de unghi de rotație pe
o axă față de unghiul de rotație de pe altă axă însă și așa se reușește să se urmărească
referința . Mai precis, se observă influențe puternice între ϕ și ψ, iar dacă θ suferea o
variație de unghi de rotație mai mare de 7 ÷ 8 ◦ , atunci orice tendință de urmărire de
referință din partea celorlalte două controllere ar fi fost impiedicată.
Kp 10 10 10 10 10 10
Ki 0 0 0 0 0 0
Kd 0 0 0 5 5 5
Tabela 4.4: Al doilea set de parametrii de reglare a controller-ului TC bazat de PID de

nivel jos
25
140
φ
θ
120 ψ
"Treapta" lui φ incepe la t=7, a lui θ la t=9 si a lui ψ la t=12
100
80
°(grade)
60
40
20
−20
0 2 4 6 8 10 12 14 16 18 20
sec
Figura 4.8: Urmărire de unghi cu TC
Referința de tip treaptă a lui ϕ a fost setată să intervină la t = 7 sec cu o valoare a

treptei de 90◦ , a lui θ la t = 9 sec cu 8◦ și a lui ψ la t = 12 sec cu 90◦ .
Este evident că în urma comparației, strategia de control ce presupunea implemen-
tarea unui controller TC pentru generarea de cupluri direct către dinaminca internă a
quadcopterului ar fi mult mai avantajoasă dacă ar fi implementată din simplul motiv că
a oferit performanțe mult mai bune și a asigurat mai rapid stabilitatea.
5 Strategii de control
5.1 Controlul de traiectorie cu un controller PID
Pentru controlul de traiectorie se propune utilizarea unui regulator PID, controlul

de traiectorie realizându-se în nivelul superior de control a dronei. Practic, scopul con-
trolului de traiectorie reprezintă mișcarea UAV-ul din locația curentă într-o locație dorită
prin controlarea vitezelor rotoarelor. Găsirea traiectoriei optimale pentru o dronă este o
sarcină dificilă mulțumită dinamicilor complexe [7] însă o metodă de control simplă va fi
capabilă să controleze quadcopterul într-o manieră adecvată.
Controlul dat de intrările ωi definește propulsia totală T și cuplurile τϕ , τθ și τψ .
Cuplurile afectează accelerațiile unghiulare în funcție de pozițiile și vitezele unghiulare.
26
Unghiurile η pot fi integrate din vitezele unghiulare η̇, iar acestea din urmă pot fi integrate
din accelerațiile unghiulare η̈. Accelerațiile liniare ξ¨ depind de propulsia totală T , de
unghiurile η și de vitezele liniare ξ.˙ Pozițiile liniare ξ sunt integrate din accelerațiile
liniare ξ¨ prin vitezele liniare ξ˙ .
T ξ¨ ξ˙ ξ
τ η̈ η̇ η
Figura 5.1: Interacțiunile dintre stări, derivatele stărilor și comenzile de intrare
Această metodă propune generarea unor accelerații liniare care să urmărească traiec-
toria dorită conform pozițiilor x,y și z din fiecare moment. Din ecuațiile dinamicii interne
(3.10),(3.11) și (3.12) putem obține următoarele 3 ecuații [7] :
dx S(ψ) − dy C(ψ)
ϕ = arcsin( ),
d2x + d2y + (dz + g)2
dx C(ψ) + dy S(ψ)
θ = arctg( ), (5.1)
dz + g
T = m(dx (S(θ)C(ψ)C(ϕ)+S(ψ)S(ϕ)+dy (S(θ)S(ψ)C(ϕ)−C(ψ)S(ϕ))+(dz +g)C(θ)C(ϕ),
unde
∫ t
dx = ẍref − (Kx,D (ẋ(t) − xref˙ (t)) + Kx,I (x(τ ) − xref (τ ))dτ + Kx,P (x(t) − xref (t))),
o
∫ t
dy = ÿref − (Ky,D (ẏ(t) − yref˙ (t)) + Ky,I (y(τ ) − yref (τ ))dτ + Ky,P (y(t) − yref (t))), (5.2)
o
∫ t
dz = z̈ref − (Kz,D (ż(t) − zref˙ (t)) + Kz,I (z(τ ) − zref (τ ))dτ + Kz,P (z(t) − zref (t)))
o
Practic atunci când se cunosc unghiurile ϕ și θ (unghiul ψ se consideră a fi știut de

la referință ), vitezele unghiulare și accelerațiile unghiulare pot fi calculate prin simple
derivări. Cu vitezele și accelerațiile unghiulare putem afla cuplurile τ prin rezolvarea
ecuației (3.11) , iar când cuplurile vor fi determinate de regulatorul de nivel inferior (de
stabilizare), atunci comenzile de intrare ωi pot fi calculate din (4.3).
27
5.2 Cuplarea controller-ului de stabilizare TC cu controller-ul
PID de control al traiectoriei
Scopul următoarele 4 grafice din Figura 5.2 (grafice ce alcătuiesc un ansamblu de

monitorizare aproape complet a reglării sistemului nostru) este să reflecte influența pe
care o are variația unei mărimi de orientare(ϕ, θ sau ψ) asupra unei mărimi de poziție(x,
y sau z).
Pentru a obține aceste reprezentări grafice și din necesitatea de a cupla cele două
bucle de reglare, s-a implementat un controller în buclă deschisă(No Feedback) al cărui
rol este doar de realiza legăturile, fără a regla vreun parametru de poziție. Practic se
păstrează relațiile pentru propulsia T și pentru unghiurile de referință ηref , însă relațiile
pentru dx, dy și dz se vor schimba în :
dx = ẍref ,
dy = ÿref , (5.3)
dz = z̈ref
Controller-ul de nivel jos are în continuare aceeași referință de tip treaptă ca mai sus.
Poziționarea UAV-ului în spațiu x 10

10
Variația propulsiei T în timp
5000 1
Propulsia T
0 0.5
x
y
cm
-5000 0
N
z
xref
-10000 yref -0.5
zref
-15000 -1
0 5 10 15 20 0 5 10 15 20
sec sec
Orientarea UAV-ului în spațiu Cuplurile generate pentru urmărirea referinței

150 100
φ
θ τφ
100 ψ 50 τθ
φref τψ
(grade)
N/m2
50 θref 0
ψref
°
0 -50
-50 -100
0 5 10 15 20 0 5 10 15 20
sec sec
Figura 5.2: Influențele variației unghiurilor de rotație față de poziționarea în spațiu
Acum că am observat această influență a variației mărimilor de orientare față de

mărimile de poziție, vom dori să cuplăm cele două controllere pentru a corecta în plus
28
și deviațiile mărimilor de poziție față de referința externă dată. În continuare voi fo-
losi controller-ul de nivel sus de tip PID, voi prezenta 3 seturi similare de parametrii
de reglare în cadrul cuplării celor două bucle de reglare și voi detalia concret diferențele
apărute. Menționez că în elaborarea acestor 3 seturi de parametrii de reglare am respec-
tat constrângerile de stabilitate Hurwitz impuse parametrilor de reglare corespunzătoare
regulatorului TC de nivel jos, parametrii de reglare au fost aleși respectând structura de
reglare de tip ”cascadă”, adică o buclă de reglare să fie mai rapidă decât cealaltă, iar
traiectoria de referință este una liniară.
Referința dată este xref = 0 cm, yref = 5 cm, zref = 3 cm și ψref = 0 ◦
Primul set de parametrii x y z ϕ θ ψ

Kp 5 5 5 10 10 10
Ki 2 2 2 0 0 0
Kd 0.5 0.5 0.5 5 5 5
Tabela 5.1: Primul set de parametrii de reglare corespunzătoare primei strategii de control
Poziționarea UAV-ului în spațiu Variația factorului de corecție a poziției pe fiecare axă

10
12 x dx
y dy
10
5 dz
z
8
xref
adim
6
cm
yref 0
4 zref
2 -5
0
-2 -10
0 5 10 15 20 0 5 10 15 20
sec sec

3 0.05
φ τφ
2.5
θ τθ
2 ψ τψ
φref
(grade)
1.5
N/m2
0
θref
1
°
ψref
0.5
-0.5 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec
Figura 5.3: Monitorizarea comportamentului sistemului cuplat (TC+PID)
29
Variația propulsiei în timp
Propulsia T
5.5
N
4.5
3.5
0 5 10 15 20
sec
Față de ultimul set am modificat doar parametrii de reglare ai poziției, micșorând

amplificarea Kp la 5 și introducând valori nenule pentru componenta integrală și deriva-
tivă. M-am bazat pe componenta integrală în ceea ce privește urmărirea de referință și
rejecția de perturbații și pe componenta derivativă pentru a reduce suprareglajul intro-
dus de integrală. Am ținut cont de sensibilitatea accentuată la perturbații a componentei
derivative și de aceea am ales ca valoarea acesteia să fie mai mică decât cea a integralei
cu cel puțin o zecimală.
Se observă cum stabilizarea pe x, împreună cu stabilizarea rotației ψ necesită puțin
mai mult timp față de celelalte, însă interesant este că în cazul rotației ψ referința însine
se stabilizează greu în timp ce cuplul aferent nu pare a varia foarte mult.
Păstrând acceași referință, vom detalia în cele ce urmează efectele celui de-al doilea
și celui de-al treilea set de parametrii de reglare.
Propulsia T
5.5
5
N
4.5
3.5
0 5 10 15 20
sec
30
15 10
x dx
y dy
z 5 dz
10
xref
adim
yref
cm
0
5 zref
-5
0
-10
0 5 10 15 20 0 5 10 15 20
sec sec

3 0.05
φ τφ
2.5
θ τθ
2 ψ
φref τψ
(grade)
1.5
N/m2
θref 0
1 ψref
°
0.5
-0.5 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec
Al doilea set de parametrii x y z ϕ θ ψ

Kp 5 5 5 20 20 20
Ki 7 7 7 0 0 0
Kd 10 10 10 5 5 5
Tabela 5.2: Al doilea set de parametrii de reglare corespunzătoare primei strategii de

control
După cum se poate vedea în acest tabel și în Figura 5.5 și 5.6, am încercat să
măresc amplificarea controller-ului TC și valoarea componentei integrale și derivative a
controller-ului de nivel sus.
De obicei o amplificare mai mare (nu cu mult mai mare deoarece poate conduce
sistemul într-o stare de instabilitate) micșorează eroarea staționară, iar împreună cu o
componentă integrală mai puternică evoluția dinamicii sistemului către referință ar fi mai
accelerată. Evident m-am gândit să măresc și componenta derivativă pentru a reduce pe
cât posibil suprareglajul indus de celelalte două însă din păcate s-a accentuat semnificativ
sensibilitatea față de perturbații.
Nu am obținut așadar performanțe mai bune față de setul precedent, iar dacă ne
31
gândim, de exemplu la o referință nenulă pentru toate cele 3 mărimi de poziție și pentru
ψ atunci pentru acești parametrii de reglare sistemul va intra cu siguranță în instabilitate.
Urmează acum ultimul set de parametrii:
Ultimul set de parametrii x y z ϕ θ ψ

Kp 25 25 25 225 225 225
Ki 1 1 1 0 0 0
Kd 10 10 10 30 30 30
Tabela 5.3: Ultimul set de parametrii de reglare corespunzătoare primei strategii de con-
trol

200 10
dx
x
dy
150 y 5 dz
z
xref
adim
cm
100 0
yref
zref
50 -5
0 -10
0 5 10 15 20 0 5 10 15 20
sec sec

0.05
φ τφ
150
θ
τθ
ψ
τψ
φref
(grade)
100
N/m2
θref 0
°
ψref
50
0 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec
Acest ultim set de parametrii de reglare prezintă valorile aflate în vecinătatea celor
optime, acestea determinând sistemul să facă tot posibilul pentru a urmări referințele
date. Am ales să cresc amplificările pentru ambele controllere(o diferență sesizabil mai
mare față de seturile anterioare observându-se la amplificarea controller-ului TC), să reduc
parametrii componentei integrale corespunzătoare reglării de poziție la valoarea unitară,
să mențin parametrii componentei integrale corespunzătoare reglării de orientare nuli, să
cresc valorile componentei derivative a controller-ului de nivel jos de aproximativ 6 ori și
32
să păstrez parametrii de reglare a componentei derivative a controller-ului PID la valoarea
10.
Menționez că am obținut acest set după multe încercări practice, însă m-am bazat
pe faptul că o amplificare destul de mare poate reduce substanțial eroarea staționară, ea
putând fi anulată de o componentă integrală mică aici, iar orice potențial suprareglaj sau
subreglaj introdus de aceste două să fie redus prin intermediul componentei derivative.
Evident și sensibilitatea la perturbații a crescut însă s-au obținut totuși rezultate bune
datorită echivalenței realizate de controller-ul TC a sistemului nostru neliniar cu unul
sistem liniarizat.
Se poate observa în Figura 5.8 că inclusiv propulsia T face eforturi mult mai relaxate
pentru a stabiliza quadcopterul. Pentru a dovedi capacitățile acestui set am complicat
referința, introducând o referință distinctă pentru cele 4 mărimi. Așadar noua referință
dată este xref = 15 cm, yref = 110 cm, zref = 100 cm și ψref = 90 ◦
Propulsia T
5.5
5
N
4.5
3.5
0 5 10 15 20
sec
5.3 Introducerea frecării cu aerul
Până acum orice fel de forțe de rezistență sau de frecare au fost neglijate și, deci,
voi folosi acest ultim set de parametrii de reglare pentru scenariul în care quadcopterul
întâmpină rezistență rezultată din frecarea cu aerul. Practic am introdus valori nenule
corespunzătoare coeficienților de frecare(de drag) Ax ,Ay și Az .
Ne așteptăm să obținem performanțe mai puțin bune, însă acceptabile în sensul că
această simulare este mai realistă, iar dacă se dovedește a urmări, totuși, referința dată
atunci vom putea fi mai toleranți asupra unor timpi mai mari de stabilizare sau asupra
vreunui suprareglaj mărit spre exemplu.
33
250 10
x dx
200 y dy
5 dz
z
150 xref
adim
cm
yref 0
100
zref
-5
50
0 -10
0 5 10 15 20 0 5 10 15 20
sec sec
x 10
4
4 0.05
τφ
φ
3 θ τθ
ψ τψ
(grade)
2 φref
N/m2 0
θref
°
1 ψref
0
-0.05
0 5 10 15 20 0 5 10 15 20
sec sec

100
Propulsia T
80
60
N
40
20
0 5 10 15 20
sec
Figura 5.9: Monitorizarea comportamentului sistemului cuplat(TC+PID) cu frecare
Din Figura 5.9 se poate observa cum referința folosită până acum este mult prea dură
pentru strategia de control curentă. Mai precis se poate observa cum variația mărimilor
y și z se influențează reciproc, dar mai ales că influențează destul de intens urmărirea
referinței de către mărimea ψ , reușind chiar să împiedice mărimea x să-și urmărească
referința.
Variind referința dată, am constatat că doar mărimile x și ψ au fost cele mai
afectate de frecarea cu aerul, o relaxare a referințelor acestora rezolvând problema noastră.
Menționez că am păstrat același set de parametrii de reglare.
Așadar referința dată este xref = 12 cm, yref = 110 cm, zref = 100 cm și ψref =
◦
45
34
200 10
x dx
y dy
150 z 5 dz
xref
adim
cm
100 yref 0
zref
50 -5
0 -10
0 5 10 15 20 0 5 10 15 20
sec sec

0.05
150 τφ
φ
τθ
θ
τψ
ψ
(grade)
100
φref N/m2
0
θref
°
50 ψref
0 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec

8
Propulsia T
7
6
N
0 5 10 15 20
sec
Figura 5.10: Monitorizarea comportamentului sistemului cuplat(TC+PID) cu frecare
Rezultatele obținute pentru această nouă referință sunt neașteptat de bune în sensul
că sistemul evoluează rapid către referință, fără erori staționare și fără un suprareglaj sau
subreglaj semnificativ. Variația factorilor de corecție a poziției prezintă un mic subreglaj
la începutul simulării din cauza referinței mari însă reușește să stabilizeze sistemul. În
cadrul evoluției în timp a cuplurilor se pot observa oscilațiile întreținute corespunzătoare
celor 3 cupluri, acesta reprezentând pe de o parte apropierea de limita de stabilitate a
sistemului nostru, iar pe de altă parte cuplarea celor 3 mărimi. Deși referința este dată
decât pentru ψ, mărimile ϕ și θ prezintă și ele oscilații întreținute, mai mici într-adevăr,
însă scopul lor este doar de a stabiliza sistemul.
35
6 A doua strategie de control
6.1 Cuplarea controller-ului de stabilizare TC cu un controller

PD de control al traiectoriei
O altă strategie de reglare presupune cuplarea buclei interne reglate de controller-

ul TC cu un regulator PD modificat al buclei externe. Acesta din urmă reprezintă o
formă simplificată a controller-ului precedent, eliminându-se astfel componenta integrală
din pricina deviațiilor posibile ale unghiurilor, acestea generând mici erori ce se acumulau
în timp mulțumită caracterului de sumă al integralei[7].
Asemenea primei variante, valorile dorite ale coeficienților dx ,dy ,dz sunt calculate
țînând cont de deviațiile dintre valoriel reale și cele dorite al pozițiilor ξ, vitezelor ξ˙ și ale
¨
accelerațiilor ξ.
dx = Kx,P (xref (t) − x(t)) + Kx,D (xref˙ (t) − ẋ(t)) + Kx,DD (xref¨ (t) − ẍ(t))
dy = Ky,P (yref (t) − y(t)) + Ky,D (yref˙ (t) − ẏ(t)) + Ky,DD (yref¨(t) − ÿ(t)) (6.1)
dz = Kz,P (zref (t) − z(t)) + Kz,D (zref˙ (t) − ż(t)) + Kz,DD (zref¨(t) − z̈(t))
Cu acești coeficienți se calculează apoi unghiurile de referință ηref , împreună cu
propulsia T folosind relațiile (5.1). În continuare se pot observa în Figurile 6.1 și 6.2
rezultatele simulărilor în care se cuplează controller-ul TC cu această variantă de PD
modificat.

8
Propulsia T
7
6
N
0 5 10 15 20
sec
Figura 6.2: Monitorizarea comportamentului sistemului cuplat (TC+PD) cu frecare
Rezultatele obținute nu au fost bune cu referința de până acum, însă într-adevăr

nici referința păstrată în situațiile anterioare nu este una relaxată. După cum se poate
observa în Figura 24 nici măcar după o relaxare a referinței(setarea referinței nule pentru
axa X și Y) sistemul nu poate urmări referințele de altitudine și girație.După mai multe
încercări repetate, nici măcar traiectoria ce presupunea urmărirea referinței al unui singur
grad de libertate nu a putut fi realizată.
36
200 10
x dx
y dy
150 5 dz
z
xref
adim
cm
100 yref 0
zref
50 -5
0 -10
0 5 10 15 20 0 5 10 15 20
sec sec

0.05
1500 φ τφ
θ τθ
ψ τψ
φref
(grade)
1000
θref N/m2 0
°
ψref
500
0 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec
Această nouă strategie de control constituită dintr-un controller PD al buclei externe

și un controller TC al buclei interne s-a dovedit a fi o strategie de control neviabilă în
vederea implementării pe dronă. Aceasta reușește doar să stabilizeze sistemul pentru o
referință nulă și să urmărească referința corespunzătoare unghiului de rotație ψ, această
reglare din urmă fiind realizată oricum la nivelului controller-ului TC de nivel jos, ase-
menea situațiilor anterioare. Micile variații al evoluției cuplurilor în timp se datorează
faptului că se ia considerare în continuare rezistența rezultată din forțele de frecare cu
aerul. Menționez că simularea scenariului în care se neglijează frecările cu aerul nu au
avut nicio influență benefică urmăririi referinței de poziție.

6
Propulsia T
5.5
5
N
4.5
3.5
0 5 10 15 20
sec
37
1 10
x dx
y dy
0.5 z 5 dz
xref
yref
adim
cm
0 0
zref
-0.5 -5
-1 -10
0 5 10 15 20 0 5 10 15 20
sec sec

0.05
150 φ τφ
θ
τθ
ψ
τψ
100 φref
(grade)
N/m2
θref 0
°
ψref
50
0 -0.05
0 5 10 15 20 0 5 10 15 20
sec sec
7 Strategia de control optimal
7.1 Despre regulatorul de tip MPC
Controlul bazat pe predicția de model(MPC sau Model Predictive Control) repre-

zintă o metodă de control de proces avansată ce se bazează pe modelul dinamic al proce-
sului, cel mai adesea pe modelele liniare empirice obținute prin identificarea de sistem[11].
Aici, totuși, modelul quadcopterului va avea o dinamică neliniară. Modelele MPC sunt
folosite pentru a reprezenta comportamentul sistemelor dinamice complexe, atunci când
un regulator PID nu poate face față unor întărzieri mult prea mari sau pentru sisteme de
ordin superior(mai mari de 2).
38
Un MPC va ține cont de două tipuri de variabile: cele dependente de sistemul dina-
mic(măsurători sau constrângeri de proces) și cele independente(referințe sau ieșiri)[11].
El va predicta variația variabilelor dependente în funcție de variația variabilelor indepen-
dente, acestea din urmă afectând deci sistemul dinamic.
Principalul avantaj al reglării MPC este reprezentat de posibilitatea optimizării unei
perioade de timp, sau mai precis, optimizarea unui orizont temporal predictiv finit cu
implementarea soluției corespunzătoare momentului de timp curent. Practic, are grijă ca
prima schimbare a variabilelor independente să fie implementată, repetând apoi calculele
atunci când este necesară următoarea schimbare. Spre deosebire de alte regulatoare PID
sau LQR, MPC-ul are abilitatea de a anticipa evenimente viitoare de timp și să preia
controlul corespunzător dinamicii predictate[11].
MPC-ul se bazează pe o problemă de optimizare iterativă, de orizont temporal finit
al unui model de proces. La un moment de timp t, starea procesului este eșantionată și
se calculează în timp real(online) o strategie de control de minimizare a costului până la
următorul moment de eșantionare t + Te .
Doar primul pas va fi implementat, urmând ca la următorul moment de eșantionare
să se verifice dacă s-a schimbat starea sistemului. În caz afirmativ, atunci se reiau calculele
pentru această nouă stare și se obține o nouă strategie de control pentru noul traseu de
stări predictat.
Figura 7.1: Schema MPC discretă[11]
Alt avantaj important îl constituie faptul că este printre puținele metode de control
ce poate face față stărilor generice și a constrângerilor de control[10]. Mai precis, el poate
include modele generice(liniare sau neliniare) și constrângeri în problema de control bazată
pe optimizare. În plus, merită menționată capacitatea MPC-ului de a redefini funcția de
cost și constrângerile pentru a ține cont de schimbările de sistem sau de mediu.
Principalul dezavantaj al strategiei de orizont culisant, așa cum mai este cunoscut
MPC-ul[11], este reprezentat de cerința computațională mare datorată regulii ce dictează
faptul că un algoritm de optimizare trebuie să ruleze și să ajungă să își termine execuția
la fiecare actualizare a blocului controller(acesta fiind sincronizat de obicei cu ceasul de
eșantionare).
39
Principiile unui control MPC
MPC-ul este un algoritm de control multivariabil ce folosește: modelul dinamic

intern al procesului, istoricul comenzilor trecute, istoricul constrângerilor problemei de
optimizare și o funcție de cost J aplicată orizontului de predicție ce înaintează(glisează)
constant în timp[11].
Practic, problema de optimizare constă în minimizarea acestei funcții de cost J ce
se adaptează orizontului temporal de predicție finit și are următoarea formă[10]:
Np −1
∑
J = arg min Vf (x(k + Np ) + Vn (x(k + s), u(k + s))
u
 s=0
(7.1)
 x(k + s + 1) = f (x(k + s), u(k + s)), s = 0, ...Np − 1
cu constrângerile h(x(k + s), u(k + s)) ≤ 0, s = 0, ...Np − 1

hf (x(k + Np )) ≤ 0
Funcția de cost constă dintr-o funcție de cost terminală Vf (·) : IRn → IR și o funcție
de cost intermediară Vn (·) : IRn → IR.
Tipic, funcția obiectiv al unui MPC penalizează deviațiile stărilor și comenzilor
de la valorile lor de referință, în timp ce constrângerile sunt tratate explicit. Fiecare
optimizare a orizontului predictat generează o traiectorie de control optimal ce ține cont
de dinamicile sistemului(cu proprietatea generică f(0,0) = 0), de constrângerile stărilor și
comenzilor h(·) și de constrângerea terminală hf (·).
În ceea ce privește forma funcției de cost aceasta poate fi pătratică,
Vn (x(k + s), u(k + s)) = xT (k + s)Qx(k + s) + uT (k + s)Ru(k + s) (7.2)
liniară,
Vn (x(k + s), u(k + s)) = f T x(k + s) + g T u(k + s) (7.3)
sau neliniară, însă pentru implementare vom considera mai târziu forma pătratică[10].
Pașii unui MPC
Așadar, presupunându-se cunoscută secvența mărimilor de referință rk+j , obiectivul

acestei conduceri preventive este să se asigure similaritatea ieșirii viitoare a procesului
yk+j cu referința rk+j . Acestea fiind spuse putem afirma că pașii algoritmului de control
MPC sunt[11]:
1. selectarea valorii viitoare a referinței rk+j
2. cu modelul de predicție se generează ieșirea predictorului ŷk+j , urmând a se calcula
eroarea predictată êk+j = rk+j − ŷk+j . Pentru un timp dorit d (j>d), yk+j depinde de
valorile viitoare ale lui uk+j .
3. se minimizează funcția obiectiv selectată și se determină valorile viitoare ale co-
menzii uk+j .
4. din secvența obținută se memorează și se implementează primul element în proces.
Vectorul de elemente se actualizează pentru a fi folosit la calculul comenzii de la pasul
următor.
Ideal ar fi ca variația comenzii să fie constantă (∆uk = ∆uk+1 = ... = ∆uk+j−d ).
40
MPC-ul neliniar
Majoritatea proceselor nu sunt liniare , dar pot fi adesea considerate a fi liniare în

jurul unui punct static de funcționare(sau unei plaje mici de operare). Abordarea de tip
MPC liniar e folosită în majoritatea aplicațiilor cu un mecanism de feedback al MPC-ului
de compensare a erorilor de predicție datorate diferențelor dintre procesul real și modelul
procesului.
Când nici aceste modele liniare în jurul unui punct static de funcționare nu sunt
suficient de precise, atunci se propun alte abordări. Una ar fi să se transforme variabilele
de proces înainte și/sau după aplicarea modelului MPC pentru a reduce neliniaritatea.
Interesant aici este că procesul este controlat cu un MPC neliniar ce folosește evident un
model neliniar direct în reglare. Modelul neliniar se poate prezenta într-o formă empirică
ce poate fi liniarizat, formându-se un filtru Kalman(asta doar dacă se dorește să se specifice
un model pentru un alt MPC liniar de exemplu).
Acest NMPC, o variantă a MPC-ului, necesită la fel ca originalul o soluție iterativă
pentru rezolvarea problemelor de control optimal pe un orizont temporal de predicție
finit, însă trebuie avut în vedere dezavantajul faptului că pentru NMPC problemele sunt
neconvexe și deci poate fi compromisă soluția numerică, dar mai ales stabilitatea[11].
Ca și soluție la un asemenea inconvenient, se exploatează de obicei faptul că proble-
mele de control optimal consecutive sunt asemănătoare. Acest lucru permite inițializarea
eficientă a unei proceduri de găsire a soluției printr-o ”deplasare” convenabil aleasă față
de soluția optimală calculată anterior, salvând astfel timp de calcul important.
MPC-ul robust
Variantele robuste de MPC sunt capabile să asigure îndeplinirea constrângerilor de

stare pentru un set limitat de perturbații. Avem 4 variante[11]:
→ MPC Min-Max: aici optimizarea este realizată ținând cont de toate evoluțiile
posibile ale perturbației. Această variantă este recomandată pentru problemele liniare de
control robust, necesitând totuși un cost computațional mărit.
→ MPC de tip relaxare de constrângeri: așa cum se poate deduce din denumire,
constrângerile de stare sunt ”lărgite” prin anumite margini date astfel încât urmărirea
traiectoriei să fie garantată indiferent de evoluția perturbației.
→ MPC de tip tub: folosește un model nominal de sistem și un controller de feedback
pentru a asigura convergența stărilor curente către starea nominală. Pentru a se determina
abaterea cauzată de constrângerile de stare se folosește un set de deviații posibile de stare
ce pot fi introduse de perturbații prin controller-ul de feedback.
→ MPC multi-stagii: folosește o formulare de tip arborescentă prin aproximarea spa-
țiilor de incertitudine cu un set de eșantionări, iar varianta este non-conservativă deoarece
ține cont de faptul că informațiile de măsurare sunt valabile la fiecare perioadă de timp
în cadrul predicției, iar deciziile la fiecare perioadă de timp pot diferi și se pot comporta
ca o consecință a combaterii efectelor incertitudinii. Dezavantajul acestei variante este ca
dimensiunea problemei crește exponențial cu numărul de incertitudini și cu dimensiunea
orizontului de predicție.
41
7.2 Yalmip
Yalmip este un limbaj de modelare dedicat modelării avansate și găsirii de soluții

pentru probleme de optimizare convexe și neconvexe(MPC și NMPC). Este implementat
ca un toolbox în Matlab.
Printre avantajele limbajul Yalmip putem menționa de dezvoltarea sa algoritmică
rapidă cu o interfață ușor de folosit sau de faptul că implementează trucuri de modelare
obținându-se o eficiență mare de modelare.
Ar mai fi de menționat faptul că suportă un număr mare de clase de probleme de op-
timizare: liniare(Liniar Programming), pătratice(Quadratic Programming), semidefinite,
geometrice, polinomiale locale și globale, multiparametrice sau programare robustă.
De asemenea Yalmip permite focusarea atenției utilizatorului pe modelarea de
nivel înalt, în timp ce cea de nivel jos este administrată de Yalmip prin intermediul
unor aplicații externe responsabile de rezolvarea problemelor de optimizare(solvere). El
reprezintă unealta pe care ne vom baza pentru a rezolva problemele de optimizare în
simulările următoare.
7.3 Controller-ul TC de nivel jos bazat pe regulatorul MPC
Spre deosebire de celelalte, această strategie de control cu MPC se poate implementa

doar asupra unui model liniar sau liniarizat în jurul unui punct static de funcționare.
Reamintim că modelul sistemului nostru este puternic neliniar și deci singura posibilitate
de implementare a acestuia este în interiorul controller-ul TC de nivel jos, mulțumită
obținerii echivalenței sistemului nostru neliniar cu un sistem liniarizat. Practic vom dori
să reformulăm relațiile de bază ale TC, astfel încât să nu se mai bazeze pe un controller
PID ci pe un MPC.
Se va folosi în continuare relația de bază a controller-ului TC (4.4), forma factorului
de corecție τη′ (4.5) și dinamica de rotație a UAV-ului simplificată (4.8). Obiectivul va
rămâne același și anume de rezolvare a ecuației
η̈ − τη′ = 0
iar împreună cu notația η − ηref se va folosi în continuare dinamica de buclă închisă (4.8):
∫ t
¨
ε(t) + Kp ε(t) + Ki ˙ =0
0
În acest moment pentru schimba strategia de control trebuie efectuată o substituție, și

anume că în afară de primul termen al sumei vom considera că restul sunt generate sub
forma unei simple comenzi de la regulatorul MPC. Așadar, din punct de vedere relațional
trebuie să considerăm:
ε̈ + µM P C = 0 (7.4)
unde µM P C reprezintă notația comenzii MPC.
Ținând cont de dimensiunea lui ηref obținut ca o scădere a doi vectori ce conțineau
unghiurile de rotație reale și unghiurile de rotație de referință, și de faptul că o lege de
42
reglare de tip MPC operează în timp discret, vom dori să obținem reprezentarea în timp
discret a dinamicelor sistemului de forma:
{
x(k + 1) = Ax(k) + Bu(k)
y(k) = Cx(k) + Du(k)
unde un pas de timp corespunde perioadei de eșantionare Te , iar fiecare pas reactualizează
orizontul temporal finit de predicție. Pentru acest lucru ne propunem să exprimăm
ϕ̇
 θ̇ 
  [ ]
 
ψ̇  ε̇
starea ca x(k) =   sau x(k) = și evident u(k) să reprezinte diferența dintre
 ϕ̈  µ̇
 
 θ̈ 
ψ̈
unghiurile de rotație reale și unghiurile de rotație de referință u(k) = ε(k)
Dacă din { relația (7.4) folosim ε̈(t) = −µ̇M P C și dacă notăm µ̇ = −µM P C atunci
ε̈ = −µ̇(t)
obținem x(k) =
ε̇ = µ(t)
Aceste două relații scrise în formă matriceală ⇒
[ ] [ ][ ] [ ]

 ε̇ 0 1 ε(t) 0

 = + µM P C

 −1
 µ̇ 0 0 µ
(7.5)

 [ ]

 [ ] ε(t) [ ]


 ε(t) = 1 0 + 0 µM P C
µ
Identificând matricile A, B, C și D constatăm că am obținut reprezentarea pe stare în
timp continuu : [ ] [ ]

 ε̇ ε(t)

 =A + BµM P C


 µ̇ µ

 [ ]

 ε(t)


ε(t) = C + DµM P C
µ
Această reprezentare pe stare este exprimată în timp continuu și deci în continuare
vom trece în domeniul de timp discret prin metoda de discretizare Euler:
[ ] [ ]
ε̇ ε̇
(k + 1) − (k) [ ]
µ̇ µ̇ ε(t)
=A + BµM P C
Te µ
⇒
[ ] [ ] [ ]
ε̇ ε̇ ε(t)
(k + 1) − (k) = Te A (k) + Te BµM P C
µ̇ µ̇ µ
Trecând starea aferentă momentul de timp curent k în partea dreaptă , vom obține
[ ] [ ]
ε̇ ε(t)
(k + 1) = (I6 + Te A) + Te BµM P C
µ̇ µ
unde I6 reprezintă matricea unitate iar Te perioada de eșantionare
43
Identificând noile matricile Ad și Bd (Cd și Dd rămânând aceleași) constatăm că am
obținut reprezentarea pe stare în timp discret căutată :
[ ] [ ]

 ε ε(t)(k)

 (k + 1) = Ad (k) + Bd µM P C

 µ µ

(7.6)

 [ ]

 ε(t)


ε(k) = Cd + Dd µM P C
µ
7.4 Constrângerile de comandă ale controller-ului MPC
Așadar folosindu-ne de aceste matrici ale reprezentării pe stare în timp discret se va

genera comanda MPC folosită mai departe de controller-ul TC pentru a genera cuplurile
necesare urmăririi unghiurilor de referință. Trebuie reamintit faptul că un algoritm de
reglare MPC rezolvă practic o problemă de optimizare la fiecare orizont de predicție
temporal finit pe baza unor constrângeri. Este evident faptul că în contextul controlului
unui quadcopter trebuie ținut cont de constrângerile elementului nostru de execuție, adică
de constrângerile celor 4 motoare de curent continuu. Aceste constrângeri fizice vor trebui
respectate, însă diferă de cele ale MPC-ului, el fiind nevoit să-și calculeze și să-și impună
propriile constrângeri.
Există relații indirecte de dependență între aceste constrângeri, relații pe care
le vom detalia după ce determinăm constrângerile de cuplu obținute în funcție de cele
fizice. Am menționat în capitolul 2 că turația maximă a celor 4 motoare este de 58000
rotații pe minut. Folosindu-ne de relația (3.9) putem determina constrângerile de cuplu τη
aferente turațiilor motoarelor, acestea încadrându-se așadar, într-o plajă de valori minime
și maxime, negative și pozitive:
 2
    2

lkωϕmin τϕ lkωϕmax
 lkωθmin
2  <  τθ  <  lkωθmax
2 
2 2
2bωψmin τψ 2bωψmax
Pentru obținerea cuplurilor am ținut cont de valoare distanței de la centrul UAV-ului
la oricare din cele 4 elice l = 0.225 cm, de factorul de drag kt = k = 2.98·10−6 și de factorul
de propulsie b = 1.14 · 10−7 . Am observat că pentru cuplurile corespunzătoare unghiurilor
de rotații ϕ și θ am obținut aceleași valori minimale și maximale de ∓2318, 2 N m
, iar pentru
cuplul corespunzător unghiului de rotație ψ am obținut o valoare maximală/minimală de
±788, 29623 N m
. Așadar pot rescrie relațiile :
     
−lkωϕmax
2
τϕ 2
lkωϕmax
 −lkωθmax
2  <  τθ  <  lkωθmax
2 
−2bωψmax
2
τψ 2
2bωψmax
Înlocuind în această formă valorile menționate mai sus vom obține următoarele con-
44
strângeri de cuplu :
     
−2318, 2 N
m
τ ϕ 2318, 2 N
m
 −2318, 2 N  <  τθ  <  2318, 2 N  (7.7)
m m
−788, 29632 N
m
τ ψ 788, 29632 N
m
Aceste constrângeri de cuplu τη trebuiesc adaptate pentru τη′ , iar apoi acestea din
urmă vor trebui și ele adaptate la comenzile µM P C . Conform relației (4.4) și (4.5) avem:

 ′
τη = M (η)τη + V (η, η̇)

 ∫t
τη′ = ηref¨(t) − Kp (η(t) − ηref (t)) − Ki 0 (η(τ ) − ηref (τ ))dτ − Kd (η(t)
˙ − ηref˙ (t))
Pentru varianta de control cu MPC integrat în controller-ul TC avem :


 ′ ¨
τη = ηref (t) − µM P C
(7.8)


τη = M (η)(ηref¨(t) − µM P C ) + V (η, η̇
Din contrângerile obținute la ecuația (7.7) de forma τmin < τη < τmax sau
−τmax < τη < τmax împreună cu relațiile de la (7.8) vom obține :
−τmax < M (η)(η̈ref − µM P C ) + V (η, η̇) < τmax
Prelucrând această relație vom dori să obținem constrângerile ce trebuiesc respectate
de regulatorul MPC, sau mai precis plaja de valori pentru comenzile MPC µM P C .
−τmax − M (η)η̈ref − V (η, η̇) < M (η)µM P C + V (η, η̇) < τmax − M (η)η̈ref − V (η, η̇)
Știind că matricea M (η) este nesingulară, vom înmulți la stânga acestă relație cu
opusul inversei acesteia și vom obține:
−M −1 (η)τmax + η̈ref + M −1 (η)V (η, η̇) < µM P C < M −1 (η)τmax + η̈ref + M −1 (η)V (η, η̇)
(7.9)
Această relație va defini așadar constrângerile comenzii MPC, acestea trebuind a fi
determinate la fiecare pas al orizontului de predicție pentru ca acele constrângeri fizice de
turație să fie respectate.
Problema în această situație o constituie faptul că matricile obținute din relațiile
de bază ale controller-ului TC și anume M (η), V (η, η̇), dar și accelerațiile unghiulare
de referință η̈ref variază și ele în timp. Așadar în funcție de aceste variații ale acestor 3
componente, constrângerile MPC trebuiesc recalculate la fiecare pas din cadrul orizontului
temporal de predicție finit.
La asemenea cerințe efortul computațional este destul mare, suficient cât să re-
prezinte o piedică de netolerat. Pentru a soluționa un astfel de incovenient se propune
considerarea unei ipoteze simplificatoare ce se referă la faptul că variațiile acestor 3 mă-
rimi la fiecare pas de predicție nu sunt atât de mari încât să necesite schimbări drastice
a constrângerilor MPC. Desigur, la reactualizarea orizontului de predicție se recalculează
oricum constrângerile MPC după valorile din acel moment de timp ale acelor 3 compo-
nente.
Trebuie menționat faptul că pentru o astfel de ipoteză simplificatoare este o necesitate
să considerăm un interval temporal de predicție mic și o perioadă de eșantionare la fel de
mică.
45
Parametrii de reglare x y z
Kp 25 25 25
Ki 1 1 1
Kd 10 10 10
Tabela 7.1: Parametrii de reglare a controller-ului PID din cadrul ultimei strategii de
reglare optimale
7.5 Cuplarea controller-ului TC bazat pe MPC de nivel jos cu

controller-ul PID de nivel sus
Având în vedere faptul că MPC-ul se bazează pe rezolvarea unei probleme de

optimizare la fiecare orizont de predicție temporal finit, atunci putem afirma că tocmai
acest lucru poate oferi acestei strategii de control un caracter optimal. Avem certitudinea
că se respectă constrângerile de viteză, mulțumită propriilor constrângeri MPC ce se
adaptează și care sunt implementate în contextul găsirii soluții optimale a problemei de
optimizare.
În continuare vom dori să cuplăm acest ultim regulator de nivel jos cu regulatorul
PID de nivel sus, iar motivul alegerii controller-ului de tip PID este influențată de perfor-
manțele deosebite obținute de strategia de control constituită din cuplarea un regulator
PID de nivel sus și un regulator TC bazat pe PID de nivel jos. Deoarece acest controller
funcționează după anumite legi de reglare distincte față de PID-ul de până acum, pentru
următoarele simulări vom specifica doar parametrii de reglare ai controller-ului PID de
nivel sus și alt gen de parametrii sau mărimi de interes pentru controller-ul TC bazat pe
MPC.
Respectând necesitățile impuse de considerarea ipotezei simplificatoare, am ales o
perioadă de eșantionare Te = 0.01 secunde și un orizont de predicție N = 5. În plus
am ales să ne folosim de o funcție de cost intermediară și terminală de tip pătratică,
cu matricea de cost aplicată ieșirii Qy egală cu matricea unitate și matricea de cost a
comenzii R nulă, iar constrângerile fiind construite și adaptate treptat la fiecare pas de
eșantionare.
După cum se poate vedea în Figura 7.2 referința dată este xref = 30 cm, yref = 5
cm, zref = 40 cm și ψref = 30◦
În urma multor încercări practice am constatat că această strategie de reglare
optimală nu reușește să urmărească o referință atât de dură ca aceea pe care o putea urmări
cealaltă strategie de control constituită din cuplarea regulatorului TC cu un regulator PID,
însă această referință ușor mai relaxată reușește a fi urmărită prin intermediul acestei
strategii de control. De apreciat este că această strategie reușește să urmărească simultan
câte o referință pentru fiecare parametru fără întârzieri, suprareglaje sau interinfluențe
sesizabile. În ceea ce privește stabilizarea sistemului pentru o referință nulă, aceasta este
realizată cu ușurință.
În cele ce urmează vom ilustra evoluția contrângerilor comenzilor MPC pentru a
realiza dacă ipoteza noastră simplificatoare a avut vreun impact major asupra metodei
de reglare a MPC-ului.
46
Vizualizarea traiectoriilor
Poziționarea UAV-ului în spațiu
Urmarirea referintei
80 x
Traiectoria de referinta
y 60
60 z
xref 40
cm
40 yref
cm
20
zref
20
0
0 10
40
0 20
-20 0
0 5 10 15 20 cm -10 -20 cm
sec
Orientarea UAV-ului în spațiu Variația propulsiei T în timp

80 10
Propulsia T
φ
60 θ
ψ 5
(grade)
φref
40
N
θref
°
ψref 0
20
0 -5
0 5 10 15 20 0 5 10 15 20
sec sec
Figura 7.2: Monitorizarea comportamentului sistemului cuplat(TC+MPC+PID)
5
Evoluția constrângerilor comenzilor MPC pentru un orizont de predicție finit
x 10
u_φ max 5
u_φ min -5
0 2 4 6 8 10 12 14 16 18 20
5 Eșantioane de timp
x 10
5
u_θ max
u_θ min
-5
0 2 4 6 8 10 12 14 16 18 20
x 10
1
u_ψ max
0.5
0
-0.5
u_ψ min 47
-1
0 2 4 6 8 10 12 14 16 18 20
După cum se poate observa, în cadrul constrângerilor fiecărei comenzi MPC cores-
punzătoare unghiului de rotație η se poate observa o variație inițială, mai mică sau mai
mare în funcție de caz, însă relevant din aceste grafice este faptul că încă din timpul pri-
mului eșantion de timp aceste constrângeri se stabilizează și rămân constante până finalul
simulării. Acesta reprezintă un argument solid pentru care ipoteza noastră simplificatoare
se dovedește a fi utilă, reducând considerabil efortul de calcul.
Puteți observa mai jos aceeași reprezentare restrânsă la o perioadă de timp mai
mică, evidențiind astfel procesul de cuantizare.
Evoluția constrângerilor comenzilor MPC pentru o perioadă mai mică decât orizontul de predicție finit
5
x 10
u_φ max
4
2
0
u_φ min -2
-4
6.55 6.6 6.65 6.7
x 10
u_θ max
4
2
0
u_θ min -2
-4
6.55 6.6 6.65 6.7
Eșantioane de timp
4
x 10
u_ψ max
0
u_ψ min
-5
6.55 6.6 6.65 6.7

Eșantioane de timp
Figura 7.4: Evoluția constrângerilor comenzilor MPC pentru un orizont de predicție finit
48
7.6 Introducerea frecării cu aerul
Până acum orice fel de forțe de rezistență sau de frecare au fost neglijate în cadrul
acestei strategii de reglare optimale și, deci, voi simula în cele ce urmează scenariul în care
quadcopterul întâmpină rezistență rezultată din frecarea cu aerul. Practic am introdus
valori nenule corespunzătoare coeficienților de frecare(de drag) Ax ,Ay și Az .
Ne așteptăm să obținem performanțe mai puțin bune, însă acceptabile în sensul că
această simulare este mai realistă, iar dacă se dovedește a urmări, totuși, referința dată
atunci vom putea fi mai toleranți asupra unor timpi mai mari de stabilizare sau asupra
vreunui suprareglaj mărit spre exemplu.
Poziționarea UAV-ului în spațiu Vizualizarea traiectoriilor
x Urmarirea referintei
80
y Traiectoria de referinta
60
60 z
xref 40
40 yref cm
cm
20
20 zref
0
0 2
40
-11 0 20
x 10 0
-20
0 5 10 15 20 cm -2 -20 cm
sec
Orientarea UAV-ului în spațiu Variația propulsiei T în timp

4 10
φ Propulsia T
3 θ
ψ 5
2 φref
(grade)
θref
1
°
ψref 0
0
-1 -5
0 5 10 15 20 0 5 10 15 20
sec sec
Figura 7.5: Monitorizarea comportamentului sistemului cuplat(TC+MPC+PID) cu fre-

care
Referința din secțiunea anterioară s-a dovedit a fi mult prea dură pentru acest
scenariu și deci am fost nevoit să o fac mai lejeră. Mai precis am decis să setez o referință
nulă pentru mărimea de poziție y și cea de orientare ψ. Așadar referința dată este xref =
30 cm, yref = 0 cm, zref = 40 cm și ψref = 0◦ .
Spre deosebire de strategia de control constituită dintr-un controller TC bazat pe
PID cuplat cu un controller PID, se observă un impact mai pronunțat în sensul că a fost
necesară renunțarea la referințele nenule pentru două mărimi din 4 din cauza cuplării
puternice dintre acestea. Practic se încerca urmărirea referințelor anulate aici, însă influ-
49
ențau atât de mult unghiul de rotație θ sau mai ales se influențau reciproc atât de mult
încăt nu mai erau capabile nici măcar să se stabilizeze.
Cu toate că acestă nouă referință este urmărită cu succes, se poate observa în cadrul
vizualizării traiectoriilor cuplarea mărimilor x și z cu y, astfel că în starea inițială există
o decalare inexplicabilă pe axa Y, însă din fericire sistemul va converge în scurt timp la
traiectoria de referință. În cele ce urmează vom ilustra încă o dată evoluția contrângerilor
comenzilor MPC pentru a realiza dacă ipoteza noastră simplificatoare a avut vreun impact
major asupra metodei de reglare a MPC-ului.
Evoluția constrângerilor comenzilor MPC pentru un orizont de predicție finit

5
x 10
u_φ max 5
u_φ min -5
0 2 4 6 8 10 12 14 16 18 20
x 10
u_θ max 5
u_θ min -5
0 2 4 6 8 10 12 14 16 18 20
5
Eșantioane de timp
x 10
u_ψ max 1
0.5
0
-0.5
u_ψ min -1
0 2 4 6 8 10 12 14 16 18 20
Eșantioane de timp
După cum se poate observa și în acest caz, în cadrul constrângerilor fiecărei comenzi
MPC corespunzătoare unghiului de rotație η se poate observa o variație inițială, mai mică
sau mai mare în funcție de caz, însă relevant din aceste grafice este faptul că încă din
timpul primului eșantion de timp aceste constrângeri se stabilizează și rămân constante
până finalul simulării. Acesta reprezintă un argument solid pentru care ipoteza noastră
simplificatoare se dovedește a fi utilă, reducând considerabil efortul de calcul.
Puteți observa mai jos aceeași reprezentare restrânsă la o perioadă de timp mai
mică, evidențiind astfel procesul de cuantizare.
50
Evoluția constrângerilor comenzilor MPC pentru o perioadă mai mică decât orizontul de predicție finit
5
x 10
u_φ max 4
2
0
-2
u_φ min -4
6.55 6.6 6.65 6.7
x 10
u_θ max
4
2
0
-2
u_θ min
-4
6.55 6.6 6.65 6.7
4
Eșantioane de timp
x 10
u_ψ max
5
-5
u_ψ min
6.55 6.6 6.65 6.7
Eșantioane de timp
Putem afirma că prin această ultimă strategie de reglare am obținut performanțe

de reglare acceptabile, performanțe ce conferă acestei strategii dovada clară a fiabilității.
Este adevărat că s-au obținut performanțe mai bune cu prima strategie de control, ea
reușind să urmărească o referință mult mai dură, însă trebuie să ținem cont că în contextul
implementării acestei strategii de control pe microcontroller-ul quadcopter-ului, cerința
computațională reprezintă un aspect de neglijat. Poate prima strategie s-a manifestat
bine, însă dacă efortul de calcul necesar depășește capacitățile microcontrollerului sau
dacă se lansează comenzi ce depășesc capacitatea motoarelor de curent continuu atunci
putem spune că această strategie va reprezenta un eșec la final. Pentru acestă ultimă
problemă există într-adevăr posibilitatea introducerii unor blocuri de saturare a unor
comenzi pentru a respecta constrângerile fizice ale quadcopterului însă această soluție nu
îmi oferă garanția stabilității.
Strategia de reglare optimală, pe de altă parte este singura ce ține cont implicit de
constrângerile fizice ale UAV-ului, ea putând garanta stabilitatea sistemului atâta timp
cât este dată o referință către care poate converge. De asemenea este singura strategie
de control despre care putem afirma că oferă o comandă optimală rezolvând în principiu
o problemă de optimizare. În plus ea a reușit să urmărească simultan două referințe,
spre deosebire de a doua strategie de control ce a reușit să urmărească o singură referință
printr-o comandă ce nu era nici măcar calculată la nivelul controller-ului ce diferenția
această strategie de control de cea precedentă.
În concluzie putem spune că acestă strategie de reglare optimală reprezintă cea mai
51
fiabilă și realistă variantă strategie de control din cele analizate ce se poate implementa
pe quadcopterul Crazyflie 2.0. Evident se poate încerca și prima strategie de control,
obținându-se poate rezultate mai bune însă în mod cert vom fi conștienți că în orice
moment comportamentul dronei va fi imprevizibil.
8 Concluzii
În cadrul acestui proiect am studiat 3 strategii de reglare, fiecare beneficiind de

regulatorul de buclă internă cu cele mai bune performanțe de stabilizare și de urmărire de
referință în urma comparării a 3 variante de controllere de nivel jos al structurii noatre de
reglare de tip cascadă. Dintre aceste 3 strategii de reglare, una s-a dovedit a nu fi fiabilă
deoarece a reușit să urmărească o singură referință din cele 4 intrări de referință(x,y,z,ψ)
printr-o comandă ce nu era nici măcar calculată la nivelul controller-ului ce diferenția
această strategie de control de următoarea . A reușit totuși să stabilizeze sistemul fără
nicio problemă în situația unei referințe nule. Am preferat să ofer ca referință doar 4
din cele 6 mărimi de reglare pentru a nu complica scenariul simulării, el putând aluneca
rapid într-o stare de instabilitate din cauza cuplării acestor mărimi, în ciuda încercărilor
de decuplare aparentă abordate în cadrul acestui proiect de licență.
Din simularea celei de-a doua strategii de control s-au obținut performanțe remar-
cabile de urmărire de referință și de stabilizare însă cu prețul unui cost computațional
ridicat și cu neglijarea constrângerilor fizice a motoarelor de curent continuu a quad-
copterului, adică a elementului de execuție. În ceea ce privește costul computațional
necesar ridicat, acesta reprezintă un aspect discutabil în sensul că argumentul contra-
riu nu poate fi oferit decât în timp, după ce a fost implementată această strategie de
control pe quadcopter și după ce a fost realizată o analiză a comportamentului acesteia
în diverse scenarii, oferindu-se un ansamblu de referințe mai lejere sau mai puțin lejere
și analizându-se comportamenul quadcopterului la o variație de referințe. Așadar nu se
poate spune cu certitudine dacă microntroller-ul din quadcopter STM32 va face față ce-
rinței computaționale a acestei strategii, însă va exista mereu riscul alunecării sistemului
într-o stare de instabilitate.
În ceea ce privește neglijarea constrângerilor fizice, o posibilă soluție ar putea
fi constituită din introducerea unor blocuri de saturare a comenzile date de regulator
astfel încât să nu se suprasolicite elementele de execuție ale quadcopterului, însă încă
odată stabilitatea nu poate fi garantată deoarece saturarea unei comenzi poate determina
imposibilitatea de stabilizare și implicit de urmărire de referință. Din acest punct de
vedere se remarcă faptul că acestă strategie de reglare se bazează pe controllere ce operează
în timp continuu și deci reacția unei reglări poate fi atât de rapidă sau oscilantă încât
sistemul ar putea fi condus inevitabil într-o stare de instabilitate.
Ultima strategie de control este o alternativă bazată pe optimalitate și îndeplinirea
de constrângeri. Această strategie de control a obținut performanțe acceptabile în cadrul
simulărilor noastre, însa nu au fost la fel de bune ca la strategia precedentă. Trebuie
apreciat faptul că spre deosebire de prima strategie de control care a reușit să urmărească
o singură referință printr-o comandă ce nu era nici măcar calculată la nivelul controller-
ului ce diferenția această strategie de control de celelalte, acestă strategie optimă de
control urmărește simultan cu succes două referințe distincte. De asemenea este singura
52
ce ține cont implicit de constrângerile fizice ale UAV-ului, ea putând garanta stabilitatea
sistemului atâta timp cât este dată o referință către care poate converge. De asemenea
este singura strategie de control despre care putem afirma că oferă o comandă optimală
rezolvând o problemă de optimizare la fiecare orizont de predicție.
Un alt aspect important al acestei strategii de reglare este că regulatorul MPC
pe care se bazează controller-ul de nivel jos operează doar în timp discret, permițând
utlizatorului să aleagă o perioadă de eșantionare convenabilă. Spre deosebire de strategia
de control precedentă reacția unei reglări poate fi controlată tocmai prin această perioadă
de eșantionare, influențele unei variații rapide a comenzii putând fi acum controlate, iar
impactul asupra sistemului minimizându-se.
Putem afirma că acestă strategie de control optimală reprezintă cea mai fiabilă
și realistă variantă strategie de control din cele analizate ce se poate implementa pe
quadcopterul Crazyflie 2.0. Desigur există și alte strategii de control, poate chiar strategii
la fel de realiste precum strategia de control optimal prin intermediul căruia se pot obține
performanțe superioare celor obținute cu ce-a dea doua strategie de control, însă cu această
ocazie aș dori să menționez și propriile direcții viitoare.
Aș fi dorit ca în cadrul acestei strategii de control optimale să introduc constrângeri
ale comenzilor mai detaliate fără să mă folosesc de ipoteza simplificatoare cu privire la
variația constrângerilor comenzilor elaborate astfel încât conform relațiilor de dependență
să se respecte constrângerile fizice ale quadcopterului. Poate chiar și introducerea unor
constrângeri de stare sau de variație a stării ar fi ajutat la obținerea unor performanțe mai
bune, însă pentru toate acestea ar fi trebuit neapărat să măresc perioada de eșantionare
împreună cu orizontul temporal de predicție deoarece putem intui că evoluția efortului de
calcul necesar ar fi avut o caracteristică exponențială.
Altă direcție viitoare este constituită de propria dorință de a reveni la o schemă de
reglare clasică folosind un regulator de tip MPC, însă o mare problemă ar fi constituită
de cuplarea puternică a parametrilor de stare și neliniaritatea rezultantă sistemului. Rea-
mintesc că MPC-ul l-am putut aplica în cadrul strategiei de control optimal mulțumită
echivalenței realizate a sistemului nostru neliniar cu unul liniarizat.
Și nu în ultimul rând aș fi dorit să realizez mai multe experimente practice pentru
a găsi strategia de control potrivită, cu parametrii de reglare aferenți care să fie capa-
bilă să urmărească referințe ce pot defini forme geometrice în spațiu. Au existat câteva
tentative de schimbare a acestei traiectorii, însa strategiile de control discutate în cadrul
acestui proiect nu au putut face față unor referințe ce își variau în timp coordonatele. Cel
mult reușeau să pornescă din punctul inițial și să ajunga în poziția finală însă traiectoria
parcursă nu se putea spune că definea vreo formă geometrică regulată.
Vă mulțumesc pentru atenție și vă invit a studia anexa acestui proiect pentru a vă
lămuri mai mult în privința implementării acestor variante și strategii de control pentru
drona Crazyflie 2.0.
53
9 Anexă
A Stabilizarea UAV-ului cu controllerul PID
A.1 Funcția de generare a traiectoriei liniare
1 f u n c t i o n [ x , y , z , p s i ]= gendata ( xf , yf , z f , p s i _ f , t f , xi , yi , z i , p s i _ i , t i , t )
2
3
4 x=x i +(t−t i ) * ( xf−x i ) / ( t f −t i ) ;
5 y=y i +(t−t i ) * ( yf−y i ) / ( t f −t i ) ;
6 z=z i +(t−t i ) * ( z f −z i ) / ( t f −t i ) ;
7 p s i=p s i _ i +(t−t i ) * ( p s i _ f −p s i _ i ) / ( t f −t i ) ;
A.2 Scriptul de inițializare de parametrii
1 clc ; clear all ; close all

2
3 m=0. 4 6 8 ; % masa
4 g=9 . 8 1 ; % a c c g r a v i t a t i o n a l a
5
6 %i n e r t i i l e
7 I_xx=4. 8 5 6 e −3;
8 I_yy=4. 8 5 6 e −3;
9 I_zz=8. 8 0 1 e −3;
10
11 %c o e f i c i e n t i f o r t e de drag ( f r e c a r e ) ( i i n e g l i j e z i n i t i a l )
12 Ax = 0 ;
13 Ay = 0 ;
14 Az = 0 ;
15
16 %d i s t a n t a de l a c e n t r u l UAV−u l u i l a o r i c e e l i c e d i n c e l e 4 r o t o a r e
17 L=0. 2 2 5 ;
18
19 %f a c t o r de drag
20 K_T=2. 9 8 e −6;
21
22 %f a c t o r de t h r u s t
23 b=1. 1 4 e −7;
24
25 %f a z a i n i t i a l a
26 p h i 0 =0;
27 t h e t a 0 =0;
28 p s i 0 =0;
29
30 %v i t e z a u n g h i u l a r a maxima admisa pt c e l e 4 r o t o a r e
31 omega_i =58800;
32
33 time=l i n s p a c e ( 0 , 2 0 , 1 0 0 0 ) ;
34 %param g e n d a t a _ l i n i a r : xf , yf , z f , p s i _ f , t f , xi , yi , z i , p s i _ i , t i , t
35 [ x , y , z , p s i ]= g e n d a t a _ l i n i a r ( 0 , 0 , 1 0 0 , 9 0 , 2 0 , 0 , 0 , 0 , 0 , 0 , time ) ;
36
37 %p a r a m e t r i i r e g u l a t o r
38 k_x_deriv =5;
39 k_y_deriv =5;
54
40 k_z_deriv =5;
41 k_phi_deriv =5;
42 k_theta_deriv =5;
43 k_psi_deriv =5;
44 k_x_propor =10;
45 k_y_propor =10;
46 k_z_propor =10;
47 k_phi_propor =10;
48 k_theta_propor =10;
49 k_psi_propor =10;
50 k_x_integ =0;
51 k_y_integ =0;
52 k_z_integ =0;
53 k_phi_integ =0;
54 k_theta_integ =0;
55 k_psi_integ =0;
56
57 %a f i s a r e a r a d a c i n i l o r p o l i n o a m e l o r c o n s t i t u i t e d i n c o e f i c i e n t i i
58 %p a r a m e t r i l o r de r e g l a r e de i n t e r e s d o r e s c s a vad daca i n
59 %urma a l e g e r i i unor p a r a m e t r i i de r e g l a r e p o l i i s i s t e m u l u i
60 %s u n t s t a b i l i a d i c a daca au p a r t e a r e a l a n e g a t i v a
61 r o o t s ( [ 1 k_phi_deriv k_phi_propor k_phi_integ ] )
62 r o o t s ( [ 1 k_x_deriv k_x_propor k_x_integ ] )
63
64 %c r e a r e v a r i a b i l e p e n t r u i n t r a r i l e s i m u l a r i l o r n o a s t r e
65 x _ r e f . t i m e=time ;
66 x _ r e f . s i g n a l s . v a l u e s=x ’ ;
67 x_ref.signals.dimensions =[1];
68
69 y _ r e f . t i m e=time ;
70 y _ r e f . s i g n a l s . v a l u e s=y ’ ;
71 y_ref.signals.dimensions =[1];
72
73 z _ r e f . t i m e=time ;
74 z _ r e f . s i g n a l s . v a l u e s=z ’ ;
75 z_ref.signals.dimensions =[1];
76
77 p s i _ r e f . t i m e=time ;
78 p s i _ r e f . s i g n a l s . v a l u e s=p s i ’ ;
79 psi_ref.signals.dimensions =[1];
A.3 Schema Simulink
55
A.4 Controller-ul PID de nivel jos
A.5 Dinamica internă a quadcopterului
B Stabilizarea UAV-ului cu controllerul TC

B.1 Schema Simulink
56
B.2 Controller-ul TC de nivel jos
57
C Cuplarea controller-ului TC de nivel jos cu controller-
ul PID de nivel sus
C.1 Controller-ul PID de nivel sus
C.2 Schema Simulink a cuplării
C.3 Scriptul de inițializare de parametrii(conține și coeficienții

de frecare)

2
3 sat_max =100;
4 sat_min=−100;
5 m=0. 4 6 8 ; % masa
58
7
9 I_xx=4. 8 5 6 e −3;
10 I_yy=4. 8 5 6 e −3;
11 I_zz=8. 8 0 1 e −3;
12
14 Ax=0 . 2 5 ;
15 Ay=0 . 2 5 ;
16 Az=0 . 2 5 ;
17
19 L=0. 2 2 5 ;
20
22 K_T=2. 9 8 e −6;
23
25 b=1. 1 4 e −7;
26
28 p h i 0 =0;
29 t h e t a 0 =0;
30 p s i 0 =0;
31
33 omega_i =58800;
34
35 time=l i n s p a c e ( 0 , 2 0 , 1 0 0 0 ) ;
36 %param gendata : xf , yf , z f , p s i _ f , t f , xi , yi , z i , p s i _ i , t i , t
37 [ x , y , z , p s i ]= gendata ( 1 2 , 1 1 0 , 1 0 0 , 9 0 , 2 0 , 0 , 0 , 0 , 0 , 0 , time ) ;
38
40 k_x_deriv =10;
41 k_y_deriv =10;
42 k_z_deriv =10;
43 k_phi_deriv =30;
45 k_psi_deriv =30;
46 k_x_propor =25;
47 k_y_propor =25;
48 k_z_propor =25;
52 k_x_dd = 0 . 6 ;
53 k_y_dd = 0 . 2 ;
54 k_z_dd = 0 . 6 ;
55 k_phi_integ =0;
57 k_psi_integ =0;
58
59 roots ([1 k_phi_deriv k_phi_propor k_phi_integ ] )
60 roots ([1 k_theta_deriv k_theta_propor k_theta_integ ] )
61 roots ([1 k_x_deriv k_x_propor k_x_integ ] )
62 roots ([1 k_z_deriv k_z_propor k_z_integ ] )
63
59
67
71
75
79
80 %d e s c h i d e r e model S i m u l i n k
81
82 open ( ’ sim_quadcopter_23_06_2016 ’ )
83 set_param ( ’ sim_quadcopter_23_06_2016 ’ , ’ StopTime ’ , . . .
84 num2str ( 2 0 ) ) % s e t a r e timp de s i m u l a r e
85 set_param ( ’ sim_quadcopter_23_06_2016 ’ , ’ A l g e b r a i c L o o p S o l v e r ’ , ’ L i n e S e a r c h ’ )
86 load_system ( ’ sim_quadcopter_23_06_2016 ’ ) % s e i n c a r c a modelul s i m u l i n k ;
87 sim ( ’ sim_quadcopter_23_06_2016 ’ ) % s e r u l e a z a modelul s i m u l i n k
88
89 % Grafice
90
91 c l o s e a l l ; h o l d on ; g r i d on ;
92 c o l o r = ’ rgb ’ ;
93 f o r i =1:3
94 plot ( simout.time , s i m o u t . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i ) ) ;
95 end
96 p l o t ( x _ r e f . t i m e , x _ r e f . s i g n a l s . v a l u e s , c o l o r ( 1 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
97 p l o t ( y _ r e f . t i m e , y _ r e f . s i g n a l s . v a l u e s , c o l o r ( 2 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
98 p l o t ( z _ r e f . t i m e , z _ r e f . s i g n a l s . v a l u e s , c o l o r ( 3 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
99 legend ( ’x ’ , ’y ’ , ’ z ’ , ’ xref ’ ) ;
100
101 % close all ;

102 f i g u r e ; h o l d on ; g r i d on ;
103 plot3 ( simout.signals.values (: ,1) , simout.signals.values (: ,2) , . . .
104 simout.signals.values (: ,3))
105 view ( 3 )
106 %
108 c o l o r = ’ rgb ’ ;
109 f o r i =4:6
110 plot ( simout.time , s i m o u t . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i −3));
111 end
112 f o r i =1:3
113 plot ( simout2.time , s i m o u t 2 . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i ) , . . .
114 ’ L i n e S t y l e ’ , ’−− ’ ) ;
115 end
116 t i t l e ( ’ unghiuri s i r e f e r i n t e l e lor ’ ) ;
117 l e g e n d ( ’ \ p h i ’ , ’ \ t h e t a ’ , ’ \ p s i ’ , ’ phi_ { r e f } ’ , ’ theta_ { r e f } ’ , ’ psi_ { r e f } ’ ) ;
118
119 % thrust
121 plot ( simout4.time , s i m o u t 4 . s i g n a l s . v a l u e s )
122 a x i s ( [ 0 20 3 . 4 1 0 ] )
60
D Cuplarea controller-ului TC de nivel jos cu controller-
ul PD de nivel sus
D.1 Controller-ul PD de nivel sus
D.2 Codul suplimentar față de cel anterior
3 k_x_deriv =10;
4 k_y_deriv =10;
5 k_z_deriv =10;
6 k_phi_deriv =30;
8 k_psi_deriv =30;
9 k_x_propor =25;
10 k_y_propor =25;
11 k_z_propor =25;
15 k_x_dd = 0 . 6 ;
16 k_y_dd = 0 . 2 ;
17 k_z_dd = 0 . 6 ;
18 k_phi_integ =0;
20 k_psi_integ =0;
61
E Cuplarea controller-ului TC bazat pe MPC de nivel
jos cu controller-ul PID de nivel sus
E.1 Funcție de extragere a comenzii MPC din variabila de tip
container a Yalmip-ului
1 f u n c t i o n [ y]= quadMpcController ( t , u )
2
3
4
5 global controller
6
7 [ y , e r r o r c o d e ]= c o n t r o l l e r {u } ; % don ’ t f o r g e t : we u s e c u r l y%
8 %b r a c k e t s and not p a r a n t h e s e s
9 i f isnan (y)
10 y=z e r o s ( l e n g t h ( y ) , 1 ) ;
11 yalmiperror ( errorcode )
12 end
E.2 Scriptul de inițializare de parametrii + Plotarea graficelor

2
3 sat_max =100;
4 sat_min=−100;
5 sampleTime=0 . 0 1 ;
6 m=0. 4 6 8 ; % masa
8
10 I_xx=4. 8 5 6 e −3;
11 I_yy=4. 8 5 6 e −3;
12 I_zz=8. 8 0 1 e −3;
13
15 Ax=0;
16 Ay=0;
17 Az=0;
18
20 L=0. 2 2 5 ;
21
23 K_T=2. 9 8 e −6;
24
26 b=1. 1 4 e −7;
27
29 p h i 0 =00;
30 t h e t a 0 =0;
31 p s i 0 =0;
32
62
34 omega_i =58800;
35
36
37 time=l i n s p a c e ( 0 , 2 0 , 1 0 0 0 ) ;
38 %param gendata : xf , yf , z f , p s i _ f , t f , xi , yi , z i , p s i _ i , t i , t
39 [ x , y , z , p s i ]= gendata ( 3 0 , 5 , 4 0 , 3 0 , 2 0 , 0 , 0 , 0 , 0 , 0 , time ) ;
40
42 k_x_deriv =10;
43 k_y_deriv =10;
44 k_z_deriv =10;
45 k_x_propor =25;
46 k_y_propor =25;
47 k_z_propor =25;
48 k_x_integ =1;
49 k_y_integ =1;
50 k_z_integ =1;
51
52 %a f i s a r e a r a d a c i n i l o r p o l i n o a m e l o r c o n s t i t u i t e d i n
53 %c o e f i c i e n t i i p a r a m e t r i l o r de r e g l a r e de i n t e r e s
54 %d o r e s c s a vad daca i n urma a l e g e r i i unor p a r a m e t r i i
55 % de r e g l a r e p o l i i s i s t e m u l u i s u n t s t a b i l i
56 %a d i c a daca au p a r t e a r e a l a n e g a t i v a
57 r o o t s ( [ 1 k_phi_deriv k_phi_propor k_phi_integ ] )
58 r o o t s ( [ 1 k_theta_deriv k_theta_propor k_theta_integ ] )
59 r o o t s ( [ 1 k_x_deriv k_x_propor k_x_integ ] )
60 r o o t s ( [ 1 k_z_deriv k_z_propor k_z_integ ] )
61
62 %c r e a r e v a r i a b i l e p e n t r u i n t r a r i l e s i m u l a r i l o r n o a s t r e
66
70
74
78
79 %p a r t e a d e d i c a t a f o r m u l a r i i de problema de o p t i m i z a r e
80 %f o l o s i n d u −ne de m a t r i c i l e
81 %c e d e f i n e s c r e p r e z e n t a r e pe s t a r e a s i s t e m u l u i
82 global controller
83
84 A = [ z e r o s ( 3 ) eye ( 3 ) ; z e r o s ( 3 ) z e r o s ( 3 ) ] ;
85 B = [ z e r o s ( 3 ) −eye ( 3 ) ] ’ ;
86 C = [ eye ( 3 ) z e r o s ( 3 ) ] ;
87 Ad = eye (6)+ sampleTime *A;
88 Bd = sampleTime *B ;
89 Cd = C ;
90
91 %s c o p : s a s e c o n t r o l e z e i e s i r e a s i s t e m u l u i a s t f e l i n c a t s a m a n i f e s t e
92 %s t a b i l i t a t e s i s a s a t i s f a c a c o n s t r a n g e r i
93 %s o l u t i e : s a s e r e z o l v e o problema MPC cu un c o s t p a t r a t i c
63
94 % tinandu−s e c o n t
95 %de dinamica s i s t e m u l u i s i de c o n s t r a n g e r i
96
97 %f o r m u l a r e a p r o b l e m e i
98 %s a l v a r e a d i m e n s i u n i i s i s t e m u l u i
99 [ dx , du ] = s i z e (Bd ) ;
100 dy = s i z e (Cd , 1 ) ;
101
102 %m a t r i c i de p o n d e r a r e
103 Q = eye ( dx ) ; % m a t r i c e a de c o s t a s t a r i i x
104 Qy = eye ( dy ) ; % m a t r i c e a de c o s t a i e s i r i i y
105 P = 10*Q; % m a t r i c e a de c o s t t e r m i n a l
106 R = 0; % c o s t u l comenzii u
107
108 Npred = 5 ; % o r i z o n t u l de p r e d i c t i e
109
110 %In c o n t i n u a r e d e f i n i m v a r i a b i l e l e de o p t i m i z a r e s i formulam problema de
111 %o p t i m i z a r e i n s i n t a x a s p e c i f i c a Yalmip
112 u = sdpvar ( repmat ( du , 1 , Npred ) , o n e s ( 1 , Npred ) ) ;
113 umin = sdpvar ( du , 1 ) ;
114 umax = sdpvar ( du , 1 ) ;
115 u i n i t = sdpvar ( du , 1 ) ;
116 uprev = sdpvar ( du , 1 ) ;
117 x = sdpvar ( repmat ( dx , 1 , Npred +1) , o n e s ( 1 , Npred + 1 ) ) ;
118 y = sdpvar ( repmat ( dy , 1 , Npred +1) , o n e s ( 1 , Npred + 1 ) ) ;
119
120 %i n i t i a l i z a r e a c o n s t r a n g e r i s i o b i e c t i v e
121 c o n s t r a i n t s = [ y{1}==Cd*x { 1 } ] ;
122 objective = 0;
123
124 %s c r i e c o n s t r a n g e r i l e s i o b i e c t i v e l e p e s t e o r i z o n t u l de p r e d i c t i e
125 f o r k = 1 : Npred
126 i f ( k==1)
127 uprev = u i n i t ;
128 else
129 uprev = u{k −1};
130
131 end
132 constraints = [ constraints , . . .
133 x{k+1} == Ad*x{k}+Bd*u{k} , . . . %dinamica
134 umin <= u{k} <= umax , . . . %c o n s t r a n g e r i l e de comanda
135 y{k+1} == Cd*x{k} ] ;
136 o b j e c t i v e = o b j e c t i v e+y{k } ’ *Qy*y{k}+(u{k}−uprev ) ’ *R* ( u{k}−uprev ) ;
137 %f u n c t i a de c o s t p a t r a t i c a i n t e r m e d i a r a
138 end
139 o b j e c t i v e = o b j e c t i v e+y{ Npred +1} ’*Qy*y{ Npred +1};
140 %f u n c t i a de c o s t t e r m i n a l a
141
142 %o p t i o n s = s d p s e t t i n g s ( ’ v e r b o s e ’ , 1 , ’ s o l v e r ’ , ’ + c p l e x ’ ) ; %impune ca s o l v e r
143 %c p l e x , nu e s t r i c t n e c e s a r ; i m p o rt a n t e s t e semnul ”+” c e t r a s m i t l u i
144 %yalmip
145 %ca problema e n e l i n i a r a
146 options =[];
147
148 %c a l c u l a r e c o n t r o l l e r MPC
149 %−−pr i m u l s i a l d o i l e a argument r e p r e z i n t a c o n s t r a n g e r i l e s i o b i e c t i v u l
150 %−−a l t r e i l e a argument r e p r e z i n t a s e t a r i l e o p t i o n s sau s d p s e t t i n g s
151 %−−a l p a t r u l e a argument r e p r e z i n t a v a r i a b i l e l e de i n t r a r e sau p a r a m e t r i i
152 %−−a l c i n c i l e a argument r e p r e z i n t a v a r i a b i l e l e de i e s i r e sau v a r i a b i l e l e
153 % de d e c i z i e
64
154
155 p a r a m e t e r s = { [ x { 1 } ; umin ; umax ; u i n i t ] } ;
156 output = {u { 1 } } ;
157 c o n t r o l l e r = o p t i m i z e r ( c o n s t r a i n t s , o b j e c t i v e , o p t i o n s , parameters , output ) ;
158 %d e f i n e s t e a i c i p a r a m e t r i i s i i n t r a r i l e
159
160 %r u l a r e model s i m u l i n k
161 open ( ’ untitledCrisuMPC ’ )
162 % s e t a r e timp de s i m u l a r e
163 set_param ( ’ untitledCrisuMPC ’ , ’ StopTime ’ , num2str ( 2 0 ) )
164 set_param ( ’ untitledCrisuMPC ’ , ’ A l g e b r a i c L o o p S o l v e r ’ , ’ L i n e S e a r c h ’ )
165 load_system ( ’ untitledCrisuMPC ’ ) % s e i n c a r c a modelul s i m u l i n k ;
166 sim ( ’ untitledCrisuMPC ’ ) % s e r u l e a z a modelul s i m u l i n k
167
168 %r e p r e z e n t a r e a g r a f i c a a d a t e l o r de i n t e r e s
169 subplot (2 ,2 ,1)
170 %c l o s e a l l ;
171 h o l d on ; g r i d on ;
172 c o l o r = ’ rgb ’ ;
173 f o r i =1:3
174 plot ( simout.time , s i m o u t . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i ) ) ;
175 end
176 p l o t ( x _ r e f . t i m e , x _ r e f . s i g n a l s . v a l u e s , c o l o r ( 1 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
177 p l o t ( y _ r e f . t i m e , y _ r e f . s i g n a l s . v a l u e s , c o l o r ( 2 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
178 p l o t ( z _ r e f . t i m e , z _ r e f . s i g n a l s . v a l u e s , c o l o r ( 3 ) , ’ L i n e S t y l e ’ , ’−− ’ ) ;
179 l e g e n d ( ’ x ’ , ’ y ’ , ’ z ’ , ’x_{ r e f } ’ , ’y_{ r e f } ’ , ’ z_{ r e f } ’ ) ;
180 a x i s ( [ 0 20 −20 90 ] )
181 y l a b e l ( ’cm ’ ) ;
182 xlabel ( ’ sec ’ ) ;
183
184 subplot (2 ,2 ,2);
186 plot3 ( simout.signals.values (: ,1) , simout.signals.values (: ,2) , . . .
187 simout.signals.values (: ,3))
188 plot3 ( x_ref.signals.values , y_ref.signals.values , z_ref.signals.values , . . .
189 ’ Color ’ , ’ r ’ )
190 view ( 3 )
191 l e g e n d ( ’ Urmarirea r e f e r i n t e i ’ , ’ T r a i e c t o r i a de r e f e r i n t a ’ )
192 y l a b e l ( ’cm ’ ) ;
193 x l a b e l ( ’cm ’ ) ;
194 z l a b e l ( ’cm ’ ) ;
195
196 subplot (2 ,2 ,3)

198 c o l o r = ’ rgb ’ ;
199 f o r i =4:6
200 plot ( simout.time , s i m o u t . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i −3));
201 end
202 f o r i =1:3
203 plot ( simout2.time , s i m o u t 2 . s i g n a l s . v a l u e s ( : , i ) , c o l o r ( i ) , . . .
204 ’ L i n e S t y l e ’ , ’−− ’ ) ;
205 end
206 l e g e n d ( ’ \ p h i ’ , ’ \ t h e t a ’ , ’ \ p s i ’ , ’ \ phi_ { r e f } ’ , ’ \ theta_ { r e f } ’ , ’ \ psi_ { r e f } ’ ) ;
207 a x i s ( [ 0 20 −1 8 0 ] )
208 y l a b e l ( ’ ^{\ c i r c } ( g r a d e ) ’ ) ;
209 xlabel ( ’ sec ’ ) ;
210
211 subplot (2 ,2 ,4)
213 plot ( simout4.time , s i m o u t 4 . s i g n a l s . v a l u e s )
65
214 legend ( ’ Propulsia T’ )
215 ylabel ( ’N ’ ) ;
216 xlabel ( ’ sec ’ ) ;
217 axis ([0 20 −5 1 0 ] )
218
219 %p l o t a r e a v a r i a t i e i c o n s t r a n g e r i l o r de comanda pe t o t o r i z o n t u l
220 % de p r e d i c t i e
221 figure
222 f o r i =1:3
223 subplot (3 ,1 , i )
225 plot ( simout5.time , s i m o u t 5 . s i g n a l s . v a l u e s ( : , i ) , ’b ’ )
226 plot ( simout6.time , s i m o u t 6 . s i g n a l s . v a l u e s ( : , i ) , ’ r ’ )
227 a x i s ( [ s i m o u t 5 . t i m e ( 1 ) s i m o u t 5 . t i m e (end) 1 . 2 5 *min . . .
228 ( s i m o u t 5 . s i g n a l s . v a l u e s ( end −100 :end , i ) ) 1 . 2 5 * . . .
229 max( s i m o u t 6 . s i g n a l s . v a l u e s ( end −100 :end , i ) ) ] )
230 end
231
232 %p l o t a r e a v a r i a t i e i c o n s t r a n g e r i l o r de comanda pe t o t o r i z o n t u l
233 % de p r e d i c t i e
234 figure
235 i n t e r v a l =[6 . 5 6 . 7 5 ] ;
236 i=f i n d ( s i m o u t 5 . t i m e >i n t e r v a l ( 1 ) , 1 , ’ f i r s t ’ ) ;
237 j=f i n d ( s i m o u t 5 . t i m e <i n t e r v a l (end) , 1 , ’ l a s t ’ ) ;
238 i n t e r v a l=i : j ;
239 i n t S a m p l e=s i m o u t 5 . t i m e ( i n t e r v a l ( 1 ) ) : sampleTime : s i m o u t 5 . t i m e ( i n t e r v a l (end) ) ;
240 f o r i =1:3
241 subplot (3 ,1 , i )
243 plot ( simout5.time ( i n t e r v a l ) , s i m o u t 5 . s i g n a l s . v a l u e s ( interval , i ) , ’b ’ )
244 plot ( simout6.time ( i n t e r v a l ) , s i m o u t 6 . s i g n a l s . v a l u e s ( interval , i ) , ’ r ’ )
245 a x i s ( [ s i m o u t 5 . t i m e ( i n t e r v a l ( 1 ) ) s i m o u t 5 . t i m e ( i n t e r v a l (end) ) . . .
246 min ( s i m o u t 5 . s i g n a l s . v a l u e s ( i n t e r v a l , i ) ) . . .
247 max( s i m o u t 6 . s i g n a l s . v a l u e s ( i n t e r v a l , i ) ) ] )
248 stem ( intSample , i n t e r p 1 ( i n t e r v a l , . . .
249 simout5.signals.values ( interval , i ) , . . .
250 intSample , ’ p c h i p ’ ) , ’ b ’ ) ;
251 stem ( intSample , i n t e r p 1 ( i n t e r v a l , . . .
252 simout6.signals.values ( interval , i ) , . . .
253 intSample , ’ p c h i p ’ ) , ’ r ’ ) ;
254 end
66
phi
wx
theta
[A] ddot_phi
psi
wy ddot_theta um1
phi_dot From
ddot_psi
theta_dot [B] m1
wz
m2
psi_dot From1
4 du/dt m3
phi w
[C] m4
Derivative phi
m5 um2
theta From2
5 du/dt wx_dot simout5
m6
psi
theta
m7
Derivative1 phi_dot To Workspace
m8
theta_dot wy_dot
6 du/dt
m9
psi_dot
psi
v1
Derivative2 I_xx
wz_dot v2 um3
I_yy
v3
I_zz
I_xx w_dot
Constant1 u_mpc_min
I_yy
Constant
I_zz ddot_phi
ddot_theta uM1
Constant2
ddot_psi
m1 m1
phi
m2 m2
theta m3 m3
m4 m4
psi
m5 m5 uM2
I_xx
m6 m6 simout6
I_yy m7 m7
To Workspace1
m8 m8
I_zz
m9 m9
M_eta v1
v2 uM3
phi v3
theta v1
psi u_mpc_max
phi_dot
theta_dot v2
psi_dot
I_xx
I_yy v3
I_zz
V_eta_dot_eta
phi
theta tau_phi 1
psi tau_phi
Saturation
tau_phi_prim
1 du/dt du/dt tau_theta_prim
phi_ref
tau_psi_prim
dot phi_ref ddot phi_ref 2
wx_dot
tau_theta
wy_dot tau_theta Saturation1
wz_dot
wx
wy 3
tau_psi
wz
Add Add1 Saturation2
I_xx
I_yy tau_psi
2 du/dt du/dt I_xx I_zz
theta_ref
67
dot theta_ref ddot theta_ref Constant4
I_yy main
Constant3
I_zz
Add3
Add2
Constant5
3 du/dt du/dt
psi_ref
dot psi_ref ddot psi_ref [A]
Goto
[B]
Add5 Goto1
Add4
[C]
Goto2
Pulse
Generator
Inputs outputs
0
Triggered
Clock
scheduling controller
du/dt
Derivative3
E.3 Controller-ul TC bazat pe MPC de nivel jos
du/dt
Derivative4
du/dt
Derivative5
Transport
Delay
Bibliografie
[1] Edmund X DeJesus and Charles Kaufman. Routh-hurwitz criterion in the examina-
tion of eigenvalues of a system of nonlinear ordinary differential equations. Physical
Review A, 35(12):5288, 1987.
[2] Simone Formentin and Marco Lovera. Flatness-based control of a quadrotor helicop-
ter via feedforward linearization. In CDC-ECE, pages 6171–6176, 2011.
[3] Research Gate. Quad-rotor Body and Earth Inertial Frame

. https://www.researchgate.net/figure/256093576_fig6_
Fig-6-Quad-rotor-Body-and-Earth-Inertial-Frame, 2016. [Online; acce-
sat 29-Iunie-2016].
[4] Meghana Gopabhat Madhusudhan. Control of Crazyflie nano quadcopter using Si-
mulink. PhD thesis, CALIFORNIA STATE UNIVERSITY, LONG BEACH, 2016.
[5] InvenSense, Datasheet. MPU-9250 Product Spe-

cification, 1 2014. rev. 1.0. [Online]. Available:
http://store.invensense.com/datasheets/invensense/MPU9250REV1.0.pdf.
[6] Jong Tai Jang, Hyeon Cheol Gong, and Joon Lyou. Computed torque control of
an aerospace craft using nonlinear inverse model and rotation matrix. In Control,
Automation and Systems (ICCAS), 2015 15th International Conference on, pages
1743–1746. IEEE, 2015.
[7] Teppo Luukkonen. Modelling and control of quadcopter. Independent research project
in applied mathematics, Espoo, 2011.
[8] Ionela Prodan, Sorin Olaru, Ricardo Bencatel, Joao Borges De Sousa, Cristina Stoica,
and Silviu-Iulian Niculescu. Receding horizon flight control for trajectory tracking of
autonomous aerial vehicles. Control Engineering Practice, 21(10):1334–1349, 2013.
[9] Bitcraze Team. Crazyflie 2.0. https://www.bitcraze.io/crazyflie-2/, 2016. [On-

line; accesat 26-Iunie-2016].
[10] C. WEN, X. Ma, and B. Erik Ydstie. Analytical expression of explicit MPC solution
via lattice piecewise-affine function. Automatica, 45(4):910–917, 2009. 00049.
[11] Wikipedia. Model predictive control. https://en.wikipedia.org/wiki/Model_

predictive_control, 2016. [Online; accesat 26-Iunie-2016].
[12] Zongyu Zuo. Trajectory tracking control design with command-filtered compensation
for a quadrotor. Control Theory & Applications, IET, 4(11):2343–2355, 2010.
68

Lucrare de Licență Strategii de Reglare Optimală Pentru Sisteme Multi-Agent

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Lucrare de Licență Strategii de Reglare Optimală Pentru Sisteme Multi-Agent

Încărcat de

Drepturi de autor:

Formate disponibile

Universitatea Politehnica București

Facultatea de Automatică și Calculatoare

Strategii de reglare optimală pentru

2 Informații despre drona Crazyflie 2.0 7

3 Modelul dinamic al quadcopter-ului 12

4 Schema de reglare propusă 15

6 A doua strategie de control 36

7 Strategia de control optimal 38

A Stabilizarea UAV-ului cu controllerul PID 54

B Stabilizarea UAV-ului cu controllerul TC 56

D Cuplarea controller-ului TC de nivel jos cu controller-ul PD de nivel

E Cuplarea controller-ului TC bazat pe MPC de nivel jos cu controller-ul

În ultima perioadă s-a manifestat un interes accentuat în ceea ce privește controlul

Figura 2.1: Quadcopter Crazyflie 2.0[9]

2.1 Aspecte hardware

Quadcopterele sunt defapt elicoptere cu 4 rotoare ce pot fi programate pentru a

• Microcontroller-ul nRF51 este responsabil de pornire/oprire(logica ON/OFF), ali-

• Microcontroller-ul STM32 se ocupă de restul, adică de citirea senzorilor, controlul

Motoarele folosite pe Crazyflie 2.0 acționează elicele conectate la ele, transformând

Bateria este de tip Litiu-Polimer(Li-Po)de 3.7V și 240mAh și este impresionantă

Modulul radio folosit poartă denumirea de Crazyradio și folosește o mufă USB

Figura 2.3: Modulul radio Crazyradio[9]

În ceea ce privește intercomunicarea dintre cele două microcontrollere, aceasta

Accelerometrul folosit de către Crazyflie 2.0 este de tip sistem micro-electro-mecanic(MEMS)

Rolul giroscopului este de a menține orientarea obiectului. Asemenea accelerome-

Magnetometrul folosit este și el de tipul MEMS și măsoară direcția și intensitatea

Pentru programarea quadcopterului Crazyflie 2.0, firma producătoare Bitcraze

Figura 2.5: Controlul UAV-ului de pe tele-

UAV-urile sunt controlate prin modificarea vitezelor unghiulare cu ajutorul motoa-

unde S(x) = sin(x) și C(x) = cos(x).  

unde l reprezintă distanța de la rotor la centrul de masă UAV, k este factorul de

Rotor 1 Rotor 2 Rotor 3 Rotor 4

Tabela 3.1: Controlul vitezelor rotoarelor ale UAV-ului[12]

Revenind la modelul matematic de proces , dedus din ecuațiile Newton-Euler și Euler-

4 Schema de reglare propusă

Această schemă de reglare este des întâlnită în controlul UAV-urilor (Fig.4.1) și

pozitiile reale propulsia T propulsia T pozitiile reale

cuplurile unghiurilor unghiurile reale

From Dinamica interna

Figura 4.1: Schema clasică de control al quadcopter-ului

În cadrul acestei scheme clasice de control, controller-ul de poziție este de obicei

4.1 Stabilizarea UAV-ului folosind un regulator PID

Utilitatea acestui controller ar fi dovedită în situația în care s-ar dori implementarea

4.2 Stabilizarea UAV-ului folosind controller-ul TC(varianta cu

A doua variantă de control pentru un sistem multi-agent presupune utilizarea

Așadar am rămas cu necunoscutele M (η) și V (η, η̇).

τη = M (η)τη′ + V (η, η̇) ⇔ τη = IWη τη′ + I Ẇη η̇ + ε(Wη η̇) (4.9)

M (η)τη′ = M (η)η̈ ⇒ M (η)η̈ − M (η)τη′ = 0 ⇒ M (η)(η̈ − τη′ ) = 0

Aplicând transformata Laplace obținem dinamica de buclă închisă ce poate fi sta-

Figura 4.2: Mulțimile fezabile de parametrii de reglare ai controller-ului TC bazat pe

Dacă spre exemplu, s-ar fi dorit respectarea unei constrângeri suplimentare de

Figura 4.3: Mulțimile fezabile de parametrii de reglare ai controller-ului TC bazat pe

O altă variantă de controller TC funcționează după aceleași principii, oferă aceeași

¨ − ηref¨(t)) + Kp (η(t) − ηref (t)) + Kd (η(t)

Aplicând transformata Laplace obținem dinamica de buclă închisă ce poate fi sta-

s2 ε(s) + Kp ε(s) + Kd ε(s)s = D(s)

⇒ funcția de transfer corespunzătoare sistemului automat este :

4.4 Analiză comparativă a celor două variante de control

În continuare vom realiza o analiză comparativă a variantelor de control cu PID

→ varianta controlului cu PID

Tabela 4.1: Parametrii de reglare a controller-ului PID de nivel jos

Orientarea UAV-ului în spațiu

Figura 4.5: Urmărire de unghi cu PID