Embed Size (px)
Citation preview
Controlul unui Pendul Invers cu Roată
de Reacțiune
Autor:
Valentin Preda
Supervizori:
Prof. univ. dr. ing. Dumitru Popescu
As. dr. ing. Valentin Tanasă
Control Avansat și Sisteme în Timp Real – Automatică și Calculatoare - UPB
Iulie 2013
Teză de disertație
Cuprins 1
Cuprins
Cuprins.................................................................................................................................. 1
Introducere ............................................................................................................................ 3
Context și motivație ................................................................................................. 3 1.1
Organizarea tezei ..................................................................................................... 4 1.2
Problematica pendulului invers.............................................................................................. 5
Introducere .............................................................................................................. 5 2.1
Dinamica unui pendulul simplu ............................................................................... 5 2.2
Importanța pendulului simplu în teoria sistemelor automate ..................................... 8 2.3
Dispozitive experimentale pendulare ...................................................................... 10 2.4
Modelarea matematică ......................................................................................................... 12
Conversia unghiurilor ............................................................................................. 12 3.1
Ecuațiile lui Lagrange ............................................................................................. 13 3.2
Ecuațiile de mișcare ................................................................................................ 13 3.3
Simularea în Simulink a sistemului neliniar ............................................................ 16 3.4
Măsurarea cuplului constant ............................................................................ 17 3.5
Măsurarea momentului interțial al pendulului ....................................................... 17 3.6
Măsurarea momentului interțial al roții de reacțiune ........................................... 18 3.7
Relația dintre semnalul de ieșire PWM și tensiunea pe motor ................................. 18 3.8
Determinarea funcției de transfer a motorului ......................................................... 19 3.9
Stabilizarea pendulului invers ............................................................................................... 20
Analiză preliminară a sistemul de control ............................................................... 20 4.1
Controlabilitate ....................................................................................................... 21 4.2
Control cu reacție după stare ................................................................................... 22 4.3
Controller LQR ....................................................................................................... 22 4.4
Simulare Simulink a sistemului de stabilizare ......................................................... 23 4.5
Mecanismul de Swingup....................................................................................................... 25
Control neliniar....................................................................................................... 25 5.1
Puncte de echilibru și stabilitate .............................................................................. 25 5.2
2 Cuprins
Liniarizarea sistemelor neliniare ............................................................................. 28 5.3
Pasivitate ................................................................................................................ 30 5.4
Swingup simplu ...................................................................................................... 31 5.5
Swingup bazat pe control simplu de energie ........................................................... 31 5.6
Swingup bazat pe control de energie ....................................................................... 33 5.7
Controlul hibrid al pendulului .............................................................................................. 35
Stablizare combinată cu swingup ............................................................................ 35 6.1
Implementarea fizică a pendulului invers.............................................................................. 37
Introducere ............................................................................................................. 37 7.1
Construcția cadrului pendulului .............................................................................. 37 7.2
Sistemul de acționare .............................................................................................. 38 7.3
Senzorii .................................................................................................................. 39 7.4
Sistemul de procesare ............................................................................................. 40 7.5
7.5.1 Introducere ....................................................................................................... 40
7.5.2 Întreruperi ........................................................................................................ 40
7.5.3 Scrieri si citiri digitale rapide ........................................................................... 41
7.5.4 Calculul vitezelor unghiulare ........................................................................... 41
7.5.5 Filtrarea semnalelor ale vitezelor unghiulare .................................................... 41
Neliniarități adiționale ale sistemului ...................................................................... 42 7.6
7.6.1 Forțe de frecare ................................................................................................ 42
7.6.2 Neuniformități în distribuția de masă ............................................................... 42
7.6.3 Cuplu auxiliar datorat cablurilor atașate de motor si de encoder ....................... 42
Concluzii și perspective ........................................................................................................ 43
Concluzii .......................................................................................................................... 43
Perpective ......................................................................................................................... 43
Anexe ................................................................................................................................... 44
A.1 Cod sursă software de control Arduino .................................................................. 44
A.2 Cod sursă software de citire date - Matlab ............................................................. 48
Listă de figuri ....................................................................................................................... 51
Bibliografie .......................................................................................................................... 52
Introducere 3
Capitolul 1
Introducere
1.1 Context și motivație
Scopul acestui proiect este de a controla un sistem mecanic complex. Acest sistem este un
pendul invers ce se poate balansa liber în jurul axei proprii. Un motor de curent continuu este
atașat la un capăt antrenând o bara cu două greutăți. Singura modalitate prin care se poate
aplica un cuplu pendului este prin accelerarea acestei greutăți.
Encoderi rotaționali în cuadratură dau informații despre poziția unghiulară a pendulului și
viteza de rotație a motorului. Controllerul este implementat pe o placa Arduino conectată la
acești senzori și la un driver pentru motor.
Motorul și greutatea atașată nu pot dezvoltata suficient cuplu pentru a ridica pendulul direct
în poziție verticală iar din acest motiv energia trebuie acumulată prin mai multe oscilații.
Un model matematic al acestui sistem a fost dedus pe baza legilor fizice și prin diverse teste
și măsurători. Modelul a fost verificat prin simularea unei referințe de tip treaptă și
compararea răspunsului cu instalația reala.
Un sistem de control hibrid a fost implementat ce comută între două stări în funcție de poziția
pendulului: o stare de swingup prin care se aduce pendulul până în poziția verticală și o stare
de stabilizare prin care se menține pendulul în această poziție.
Pentru faza de swingup a fost folosit un algoritm de control al energiei pentru a asigura că
pendulul ajunge în starea verticală cu o viteza unghiulară redusă atât a pendulului cât și a
rotorului.
Legea de control folosită pentru stabilizare este una de reglare după stare, calculată pe baza
modelului matematic în timp continuu. Aceast regulator functionează corect atâta timp cât
perioada de eșantionare este de cel puțin 15 ori mai mare decât frecvența naturală de oscilație
a sistemului.
4 Introducere
1.2 Organizarea tezei
Structura acestei teze urmărește fiecare pas din procesul de dezvoltare a sistemului de control
pentru acest pendul. Punctul de start reprezintă o analiză detaliată a modelului matematic
neliniar al sistemului. Pe baza acestui model matematic se sintetizeaza o lege de control de
tip LQR împreună cu un algoritm de swingup bazat pe un control de energie. Urmează apoi o
descriere a sistemului pe care l-am construit, detaliind diversele componente utilizate precum
și dificultățile întâmpinate în procesul de dezvoltare.
Cu excepția capitolului introductiv teza este organizată în patru capitole după cum urmează:
Capitolul 2
În acest capitol este descrisă problematica pendulului invers. O scurtă descriere matematica
precum și importanța practică sunt prezentate. Sunt ilustrate deasemenea și diversele tipuri de
echipamente de tip pendul invers ce au fost dezvoltate pe parcursul istoriei pentru a studia
această problemă.
Capitolul 3
Acest capitol este dedicat modelării matematice a pendulului invers cu roată de reacțiune.
Plecând de la ecuațiile lui Lagrange, este dedus un set de ecuații diferențiale nelinare ce
aproximează modelul real. Tot în acest capitol sunt prezentate diversele experimente și
măsurători utilizate pentru a determina parametrii acestui model matematic.
Capitolul 4
Capitolul conține un studiu al problemei de stabilizare a pendulului în poziția inversă.
Utilizând un model liniar obținut prin liniarizarea în jurul punctului de echilibru, este
construit un regulator cu reacție după stare ce poate menține pendulul în poziție verticală în
prezența diverselor perturbații.
Capitolul 5
În acest capitol sunt ilustrate concepte de bază din teoria sistemelor neliniare precum: puncte
de echilibru, stabilitate, liniarizare sau pasivitate. Utilizând aceste concepte o lege de reglare
neliniară este proiectate pentru a aduce pendulul în poziție verticală.
Capitolul 6
Tehnicile de stabilizare și swingup descrise în capitolele precendente sunt acum combinate
pentru a genera un algoritm de control ce poate aduce și stabiliza pendulul în poziți e verticală
din orice configurație.
Problematica pendulului invers 5
Capitolul 2
Problematica pendulului invers
2.1 Introducere
Pendulul este unul dintre cele mai importante exemple din controlul sistemelor dinamice și a
fost studiat în profunzime încă din vremea lui Galileo. De fapt, experimentele efectuate de
Galileo, studiind mișcările pendulului au ridicat importante probleme în mecanică, ce au fost
rezolvate odată cu formularea legilor de mișcare ale lui Newton. Experimentele lui Galileo au
subliniat faptul că un pendul ce inițial se ridică la o înălțime aproximativ egală cu cea de la
care a fost eliberat, ajunge într-un final în stare de repaos. Un pendul de masă mai mică va
ajunge în starea de repaos mai repede decat unul de masă mai mare. Tot Galileo a descoperit
și faptul că perioada de oscilație este independentă de masa suspendată și aproape
independentă de amplitudine. Toate aceste proprietăți pot fi deduse din ecuațiile lui Newton
după cum urmează.
2.2 Dinamica unui pendulul simplu
Considerând un pendul simplu ca in Figura 2.2.1, putem ilustra o serie de proprietăți pe bază
ecuațiilor de mișcare. Notăm cu , unghiul făcut de pendul cu verticala, si reprezintă
lungimea respectiv masa pendului iar este accelerația gravitațională ( la
suprafața Pamântului). Ecuațiile de mișcare ale pendului simplu sunt in această situație:
in (2.2.1)
Se poate observa ca ecuația de mai sus este neliniară datorită termenului in . Dacă
aproximăm in cu , această aproximație fiind bună pentru valori mici ale unghiului ,
obținem următorul sistem liniar:
(2.2.2)
6 Problematica pendulului invers
unde am definit prin . Ecuatia (2.2.2) este numita oscillator armonic simplu. Se poate verifica prin ub tituție directă ca ecuația de mai u are următoarea oluție generală:
( ) co ( ) in( ) (2.2.3)
Figura 2.2.1 – Pendulul simplu
Figura 2.2.2 – Potretul fazelor pentru un oscillator armonic simplu
Constantele și sunt determinate din condițiile inițiale, acestea fiind egale cu ( ) și
( ) . Viteză unghiulară a pendului este dată de ecuația (2.2.4):
Problematica pendulului invers 7
( ) co ( ) in( ) (2.2.4)
Printr-un calcul direct se poate demostra urmatoarea relație:
( )
( ) (2.2.5)
unde . Ecuația (2.2.5) reprezintă o elipsă parametrizată după timp. Această
curbă de parametrizare este numită traiectoria oscilatorului armonic simplu (Fig. 2.2.2).
Reuniunea acestor curbe, cate una pentru fiecare pereche de contiții initiale (A și B), este
numită portretul fazelor pentru acel sistemu. Se poate observa ca perioada de oscilație a
oscilatorului armonic simplu ( ) este independent de amplitudine. Este suprinzător faptul că
spre deosebire de ecuațiile pentru oscilatorul arminc simplu, pentru care se poate găsi ușor o
soluție, nu există o soluție de formă închisă pentru ecuația pendulului (2.2.5) similară cu
ecuația (2.2.3). Portretul fazelor pentru pendulul simplu poate fi generat prin simulare
numerica si are forma ca în Fig 2.2.4.
Pentru a înțelege mai bine acest portret al fazelor introducem urmatoarea funcție scalară
( co ( )) (2.2.6)
Figura 2.2.3 – Portretul fazelor al pendulului simplu
Funcția din relația (2.2.6) este defapt proportional cu energia totală a sistemului compusă
din energie cinetică și energie potential. Rata de modificare a funcției este dată de derivata
acesteia
8 Problematica pendulului invers
in( ) (
in( )) (2.2.7)
Funcția pt soluții ale ecuației de mișcare ale pendului simplu. Asta înseamnă ca funcția
este constantă pe traiectoriile soluțiilor. Cu alte cuvinte curbele de nivel ale funcției
corespund cu traiectoriile de mișcare.
2.3 Importanța pendulului simplu în teoria sistemelor automate
Pendulul simplu este un sistem interesant folosit pentru a studia probleme de dinamică și
control. Acest sistem are însă o importanță deosebită și în realitate, multe probleme de
inginerie fiind modelate aproximativ cu un pendul. În această secțiune sunt enumerate o serie
de exemple și aplicații de acest tip.
Figura 2.3.1 – Decolarea în misiunea Apollo 11
În Figura 2.3.1 este ilustrată racheta Saturn V. Control activ permanent este necesar pentru a
menține orientarea dorită în prezența perturbațiilor. Tot în Figura 2.3.1 este ilustrat un model
al unei rachete pentru care orientarea este controlată cu ajutorul unghiului de propulsie .
Dinamica rachetei poate fi aproximată cu cea a unui pendul invers controlat.
În biomecanică, pendulul este adesea folosit pentru a modela mersul biped. În Fig. 2.3.2 este
ilustat robotul umanoid Asimo produs de Honda. În modelarea roboților bipezi, piciorul cu
greutate este adesea asemănat cu un pendul invers în timp ce piciorul liber se comportă ca un
pendul suspendat de șold. Din studiile efectuate asupra dinamicii posturuii și locomoției
umane s-au găsit numeroase asemănări cu dinamica unui pendul. Măsurători ale activității
musuclare în timpul mersului indică faptul ca mușchii piciorului sunt activi in principal la
începutul pasului după care se opresc pentru a permite piciorului să se comporte ca un
Problematica pendulului invers 9
pendul. Aceasta mișcare pendulară este exploată în mod natural pentru a crește eficiența
mersului biped.
Figura 2.3.2 – Robotul umanoid Asimo produs de Honda
Similar, menținerea în poziție verticală necesită controlul echilibrului pentru care se folosec
bucle reacție musculare similare cu cele ale controlului activ al unui pendul.
Transportatorul uman Segway, afișat în Figura 2.3.3, reprezintă o invenție recentă ce s-a
bucurat de succes comercial. Segway-ul este de fapt un pendul invers controlat. Folosind
datele de la senzorii giroscopici montati la baza dispozitivului, un sistem de control
computerizat metinține starea de echilibru vertical în timp ce un om navighează cu aceasta. În
partea dreaptă a Figurii 2.3.3 este prezentat un robot autonom similar cu Segway ce se poate
echilibra și deplasa fara intervenție umană. Acest tip de robot a fost construit de numeroase
laboratoare de cercetare pentru a investiga divese probleme stiintifice din teoria sistemelor,
navigare autonomă, coordonare de grup, etc.
Alte probleme de inginerie în care problema pendulului invers este întălnită e cea de
stabilizare de nave, controlul mișcării lichidelor într-un rezervor.
Studiul acestei probleme reprezintă așadar un punct de start excelent în înțelegerea diverselor
particularități ale controlul dinamicii neliniare.
10 Problematica pendulului invers
Figura 2.3.3 – Segway (stanga) și un robot autonom de auto-echilibrare (dreapta)
2.4 Dispozitive experimentale pendulare
Au fost dezvoltate numeroase dispozitive pentru a ilustra dinamica unui pendul și pentru a
facilita proiectarea și implementarea sistemelor de control. Cel mai vechi dintre aceste
dispozitive îl reprezintă platforma Cart-Pendul (Cart-Pole în engleză) ilustrat in Fig. 2.4.1.
Figura 2.4.1 – Platforma Cart-Pole
O inovație mai recentă îl reprezintă Pendulul Furuta sau Pendulul Rotativ (Fig. 2.4.2). În
cazul acestui robot, mișcarea pendulului este controlată folosind o mișcare de rotație (spre
deosebire de cea liniară a dispozitivul precedent prezentat).
Problematica pendulului invers 11
Figura 2.4.2 – Pendulul Furuta
Pendulul cu roată de reacțiune este cea mai noua și mai simplă dintre diversele experimente
pendulare datorită simetriei roții atașate la capătul pendulului. Datorită acestei simetrii,
ecuațiile de dinamică conțin mai puțin termeni de cuplaj neliniar și deci miscarea acestui
sistem este mai usor de analizat, simulat și controlat.
12 Modelarea matematică
Capitolul 3
Modelarea matematică
Primul pas în proiectarea oricarui sistem de control îl reprez intă dezvoltarea unui model
matematic al sistemului controlat. În această secțiune, sunt deduse modele matematice alte
Pendulului Invers cu Roată de Reacțiune pe baza principiilor fizice. Experimente ulterioare
sunt efectuate pentru a valida aceste modele și pentru a determina parametri. Un model
neliniar este dedus folosind o abordare Langrangiană. Acest model este apoi liniarizat iar
modelul liniar este folosit pentru dezvoltarea unei strategii de control. Modelul neliniar este
ulterior folosit pentru proiectarea unei comenzi nelinare avansate pentru problema de
swingup.
3.1 Conversia unghiurilor
O diagrama schematică a pendului este prezentată în Fig. 3.1.1. Unghiul este unghiul făcut
de pendul cu verticala măsurat în sens trigonometric iar unghiul este unghiul, făcut de bara
inerțială, măsurat la fel.
Figura 3.1.1 – Diagrama schematică a pendului cu roată de reacțiune
Pendulul este dotat cu două encodere optice în cuadratură. Aceste encodere sunt relative, spre
deosebire de absolute și pot măsura doar unghiuri relative. Valorile acestora sunt inițializate
Modelarea matematică 13
la zero la începutul oricărui experiment. Pendulul are atașat la mijloc unul dintre cei doi
encoderi. Acesta întoarce așadar o măsură a unghiului relativ dintre pendul și baza fixă.
Celălalt encoder este atașat printr-un sistem de roți dințate la axul motorului, furnizând astfel
informații despre unghiul relativ dintre pendul și masa inerțială rotită de motor.
3.2 Ecuațiile lui Lagrange
Metoda lui Lagrange se bazează pe definirea unui set de coordonate generalizate, ,
pentru a reprezenta un sistem cu n grade de libertate. Aceste coordonate sunt adesea
coordonate ale unor poziții (distanțe și unghiuri).
În funcție de aceste coordonate generalizate, se calculează energia cinetica, T și energia
potențială, V. În general, energia cinetică este o funcție pozitiv definită a coordonatelor
generale și derivatelor acestora, în timp ce energia potențială este o funcție dependentă numai
de coordonatele generale (nu și a derivatelor).
Într-un sistem multi-corp, energia cinetică și cea potențială pot fi calculate pentru fiecare corp
separat iar ulterior adunate pentru a forma energia întregului sistem. Acesta este un important
avantaj al metodei lui Lagrange și funcționeaza deorecere energia este o funcție scalară și una
vectorială.
Odată ce energiile cinetice și potențiale au fost determinate, Langrangianul,
( ), este apoi calculate ca diferență între energia cinetică și cea potențială.
Lagrangianul este așadar o funcție dependentă de coordonatele generale și derivatele
acestora.
Ecuațiile de mișcare sunt apoi exprimate în funcție de Lagrangian în următoarea formă:
(
)
(3.2.1)
Variabila reprezintă forța generalizată (forță sau cuplu) în direcția . Aceste ecuații se
numesc Ecuațiile lui Lagrange. Pentru clasa de sisteme considerate în această lucrare,
ecuațiile lui Lagrange sunt echivalente cu cele deduse folosind legea a doua a lui Newton.
3.3 Ecuațiile de mișcare
O metoda convenabilă de a dervia ecuațiile de mișcarer ale unui sistem mecanic este folosind
metoda Lagrangiană. Metoda Langragiană ne permite să manipulăm funcții scalare de
energie în loc de forțe vectoriale și accelerații ca în mecanica Newtoniană.
Pendulul cu roată de reacțiune are doua grade de libertate iar unghiurile și (Fig.3.3.1) ale
pendului pot fi considerate coordonate generalizate.
14 Modelarea matematică
Introducem urmatoarele variabile:
= masa pendulului
= masa rotorului
masa totala a pendului și rotorului
momentul inerțial al pendulului în jurul centrului de masă
momentul inerțial al rotorului în jurul centrului de masă
distanța de la pivot la centrul de masă al pivotului
distanța de la pivot la centrul de masă al rotorului
distanța de la pivot la centrul de masă al pendului și rotorului
Energia cinetică T, a sistemului este suma dintre energia cinetică a pendulului și energia
cinetică a rotorului și poate fi scrisă în funcție de acești termeni sub următoarea formă:
(3.3.1)
Considerăm că energia potențială, V, a sistemului este doar cea datorată gravitației. Efectele
elasticității axului motorului sunt neglijabile. Așadar, energia potențială este:
( co ) (3.3.2)
unde am ales să definim energia potențială ca fiind zero atunci când pendulul este in echilibru
stabil în poziția de jos. Este interesant de punctat faptul că energia potențială nu depinde de
poziția rotorului deorece masa este distribuită simetric față de axa de rotație.
Lagrangianul L este dat de relația următoare:
(co ) (3.3.3)
Calculând derivatele parțiale, necesare, ale Lagrangianului obținem:
in
În cazul pendulului invers, cuplul produs de motor rezultă rezultă într-un cuplu aplicat
rotorului și – aplicat pendulului. Acestea sunt doua forțe generalizate ce acționează în
direcțiile și .
Neglijând forțele de frecare, dinamica motorului de curent continuu este dată de urmatoarele
relații:
Modelarea matematică 15
{
(3.3.4)
tensiunea de pe motor
rezistența internă a motorului
curentul prin motor
inductața motorului
tensiunea electromotoare indusă
constanta t.e.m induse
constanta de cuplu a motoruluiRezolvând ecuațiile Langrange și înlocuind dinamica
motorului în expresia cuplului rezultă urmatorul sistem de ecuații diferențiale:
{
in
(3.3.5)
Așadar, ecuațiile de mișcare ale sistemului sunt dependente de cinci parametri: ⁄ : ,
⁄ , ⁄ , ⁄ și ⁄ . A se observa faptul că este frecvența de
oscilații pentru amplitudini mici a sistemului în jurul punctului de echilibru stabil.
Sistemul poate fi liniarizat și descris pe spațiul stărilor utilizând starea: ( ) și
tensiunea de intrare .
(3.3.6)
Unde (
) iar matricea depinde de punctul de liniarizare. Liniarizând in jurul celor
doua puncte de echilbriu obținem:
în jurul lui 0 ( in ): (
) (3.3.7)
16 Modelarea matematică
în jurul lui 0 ( in ( )): (
) (3.3.8)
este substituit cu conform relației:
Diverse măsurători au fost efectuate pentru a afla valoarea parametrilor.
3.4 Simularea în Simulink a sistemului neliniar
Pentru o analiză cât mai bună a sistemului, acesta a fost modelat în Simulink înainte de
proiectarea legilor de control. Modelul este unul neliniar, ce include dinamica moturului,
forțe de frecare, neliniaritate pe intrare precum și perturbații.
Figura 3.4.1 – Modelul Simulink al sistemul în buclă deschisa
Acest model a fost apoi validat cu cel real prin diverse măsuratori în buclă deschisă.
Modelarea matematică 17
Figura 3.4.2 – Modelul neliniar al sistemului
3.5 Măsurarea cuplului constant 𝑚𝑔𝑙
Pentru măsurarea cumplului am umplut o sticlă cu 250ml de apă. De această sticlă am
atașat o sarmă la un capăt. Am pus apoi pendulul în echilibru în poziție orizontală prin
ajustarea lungimii brațului forței exercitat de masa sticlei. Măsurând lungimea acestui braț si
cunoscând masa sticlei cuplul poate fi determinat.
g m m
m
3.6 Măsurarea momentului interțial 𝐽 al pendulului
Momentului inerțial poate fi calculat folosind ecuația pendului, fară forțe de frecare sau
cu motor activ, liniarizată în punctul de echilibru inferior.
(3.6.1)
18 Modelarea matematică
Rezolvând această ecuație diferențială și izolând rezultă:
(
) (3.6.2)
Unde este perioada de oscilație pentru amplitudini mici, măsurata a fi: 1.6s. După înlocuiri
numerice gasim urmatoarea valoare a momentului interțial:
g m
3.7 Măsurarea momentului interțial 𝐽𝑟 al roții de reacțiune
Masa inerțială are forma unei bare având greutătea concentrată în punctele extreme.
(3.7.1)
Interția rotorului din motorul de curent continuu trebuie adaugată pentru a deduce interția
totală.
(3.7.2)
Măsurătorile și datele sunt:
g m g m
Rezultă astfel:
g m
3.8 Relația dintre semnalul de ieșire PWM și tensiunea pe motor
Relațatia dintre semnalul PWM de ieșire și tensiunea de pe motor a fost măsurată. Este
evident faptul ca această dependență este una foarte nelinară. Pentru a atenua această
nelinaritate o funcție de tipul ( ) trebuie derivată.
Punctele din Fig. 3.8.1 reprezintă măsuratori ale tensiunii pe motor pentru diverse semnale
PWM [ ].
Modelarea matematică 19
Figura 3.8.1 – Dependența nelinară între tensiunea de pe motor si comanda aplicată
Se observă că această caracteristică statică conține o porțiune relativ liniară pentru comenzi
cuprinse între valorile 50 și 80.
3.9 Determinarea funcției de transfer a motorului
Pentru a modela dinamica motorului, acesta este acționat cu o treaptă . Se determină
apoi amplificarea și constanta de timp folosind datele măsurate ale vitezei unghiulare.
Figura 3.9.1 - Răspunsul motorului la o treaptă:
În urma măsuratorilor experimentale valoarea funcției de transfer de la comanda PWM u la
turația motorului a fost determinată ca fiind:
(3.9.1)
Acestă dinamică este apoi inclusă în modelul pe spațiul stărilor al sistemului.
-10 -8 -6 -4 -2 0 2 4 6 8 10-150
-100
-50
0
50
100
150
320 340 360 380 400 420 440 4600
50
100
150
200
250
300
wr
K
T
20 Stabilizarea pendulului invers
Capitoul 4
Stabilizarea pendulului invers
4.1 Analiză preliminară a sistemul de control
După ce pendulul a fost adus în poziție verticală folosind mecanismul de swingup, pendulul
ar trebui sa fie în zona liniarizată în jurul poziției . În această secțiune, modelul folosit
va fi așadar cel liniarizat dedus în capitolul de modelare.
În cazul de față, sistemul este în timp continuu iar controllerul implementat trebuie să fie în
timp discret deoarece este bazat pe eșantionari ale ieșirilor sistemului. Sistemul de control
poate fi reprezentat folosind diagrama din Figura … .
Semnalul de intrare este tensiunea v iar ieșirea sistemului este . Eșantionarile ieșirii y sunt
efectuate uniform cu frecvența ⁄ . În bulca de control, controllerul calculează un semnal de
comandă discret [ ] folosind ieșirea discretă până la [ ]. În final un semal continuu ( )
este generat folosind un bloc de tip zero-order-hold aplicat semnalului discret [ ].
Figura 4.1.1 – Diagrama bloc generală a procesul controlat
Controller
Zero-order
hold Eșantionare
Proces
( )
( ) ( )
[ ] [ ]
( )
Stabilizarea pendulului invers 21
După liniarizarea în poziția inversă ecuațiile de mișcare ale pendului sunt date de:
{ ( )
( ) (4.1.1)
Acest model este unul în care dinamica motorului este negijată iar forța de frecare este F.
Pentru a încerca stabilizarea pendului vom folosi o lege de control de tipul:
Neglijând forțele de frecare și introducând această lege în ecuația (4.1) obținem următoarea
expresie pentru sistemul în buclă închisă:
( )
Dacă impunem ca dinamica sistemului în buclă închisă a fie descrisă de polinomul
caracteristic:
( )
atunci valorile coeficienților controllerului sunt:
(4.1.2)
Acest controller nu poate totuși stabiliza pendulul în cazul unor perturbații d ce ar acționa ca
un cuplu auxiliar asupra pendulului în starea de echilibru. Regulatorul ar încerca compensarea
acestui cuplui printr-o accelerație constantă a motorului. Această accelerație nu este însă
sustenabilă în realitate iar motorul intra repede în saturație atingând turația maximă.
4.2 Controlabilitate
Pentru a evita saturația motorului, este necesar ca atât poziția unghiulară a pendulului cât și
turația rotorului să fie simulan controlate. O primă problemă este dacă acest lucru este posibil
folosind doar o singura variabilă de control. Pentru a răspunde la acestă întrebare,
controlabilitatea sistemului trebuie evaluată.
Neglijand forțele de frecare sistemul poate fi descris folosind următoarea reprezentare
folosind vectorul de stare ( ) :
(
) (
) (4.2.1)
22 Stabilizarea pendulului invers
Matricea de controlabilitate în acest caz este:
[ ] *
+ (4.2.2)
Deorece acestă matrice are rangul trei, rezultă ca sistemul este controlabil iar acesta poate fi
adus în orice stare folosind o singură varibilă de intrare.
4.3 Control cu reacție după stare
Deorece poziția pendului este influențată de accelația motorului, este posibil ca viteza
acestuia să se satureze după o vreme. Este așadar necesar să se mențină turația motorului
mică pe lângă stabilizarea pendului în poziție verticală. Pentru a realiza acest lucru, o lege de
control de tipul celei de mai jos a fost utilizată.
( ) (4.3.1)
Introducând această expresie în modelul dat de ecuațiile (4.1) obținem următorul sistem în
buclă închisă:
{ ( )
( ) (4.3.2)
Polinomul caracteristic în acest caz are forma:
( ) ( ) ( )
Polinomul standard pentru un sistem de ordin trei este:
( )(
) ( ) ( )
Prin egalarea coeficienților celor doua polinoame obținem următoarele valori ale
parametrilor:
( )
( )
4.4 Controller LQR
Dacă includem în model și dinamica motorului, matricile sistemului liniarizat în jurul
punctului de echilibru instalbil au urmatoarele valori:
Stabilizarea pendulului invers 23
(
) (
)
Prin identificare experimentală au fost deduse următoarele valori după înlocuiri numerice :
(
) (
)
O altă metodă prin care se pot afla coeficienții ( ) pentru o performață optimă este
de a folosi un controller de tip LQR. Această lege de control este obținută prin minimizarea
unei funcții de cost ponderate folosind matriciele Q, R și N:
( ) ∫ ( )
(4.4.1)
Matricile Q și R au fost alese de forma:
(
)
Prin această dimensionare a matricei Q , o importanță mai mare este acordată variabilelor de
stare și . Coeficientul R reprezintă doar un factor de scalare și a fost setat cu valoarea 100
pentru a evita o tensiune de comanda u prea mare ce ar putea satura. Valoarea lui N a fost
setată pe zero deoarece corelația dintre x și u a fost neglijată.
Pentru sisteme liniare, există o soluție unică pentru această problemă de
optimizare și poate fi exprimată ca o soluție a ecuației Riccati:
cu (4.4.2)
Având soluția numerică:
4.5 Simulare Simulink a sistemului de stabilizare
Pentru a testa legile de control proiectate anterior înainte de aplicarea acestora pe si stemul
real, a fost folosită o simulare Simulink.
24 Stabilizarea pendulului invers
Figura 4.5.1 – Schema de control folosind regulator pe stare
Mecanismul de Swingup 25
Capitoul 5
Mecanismul de Swingup
5.1 Control neliniar
În acest capitol vor fi introduse idei din teoria controlului sistemelor neliniare iar aceste
tehnici vor fi folosite pentru a controla mișcarile unghiulare ample ale pendulului. Problema
de swingup presupune mișcarea pendulului din poziția de echiblibru stabil orientat în jos în
poziția inversată. Deoarece unghiul pendulului trebuie să se schimbe cu 180 de grade,
presupunerea că in( ) poate fi aproximat cu nu mai este una valabilă iar ecuațiile de
dinamică neliniară trebuie folosite într-un mod mai direct. În capitolul urmator, tehnica de
swingup va fi combinată cu cea de stabilizare din capitolul precedent într-o manieră hibridă
numită în engleză: Switching Control. Obiectivul acestei strategii hibride de control este de a
ridica pendulul în poziție inversă și de al stabiliza apoi.
5.2 Puncte de echilibru și stabilitate
Conceptele de funcții de transfer, poli și zerouri nu se extind la sisteme de ecuații diferențiale
neliniare. Au fost dezvoltate așadar, tehnici noi de analiză și proiectare. Conceptul de
Pasivitate, strâns legat de cel de disipare de energie, este un instrument puternic și elegant ce
poate fi folosit pentru a proiecta un controller capabil de a aduce pendulul în poziție verticală.
Fie un sistem neliniar descris de ecuațiile:
( ) ( ) ( )
(5.2.1)
unde funcțiile ( ) și ( ) reprezintă câmpuri vectoriale peste , este vectorul de
stare al sistemului, intrarea sistemului, ieșirea sistemului iar ( ) este funcția de ieșire.
26 Mecanismul de Swingup
În cazul sistemelor liniare o relație intrare/ieșire de tipul ( ) este descrisă printr-o
funcție de transfer. Pentru sistemele neliniare putem considera un “operator” ce transformă
intrarea u în ieșirea y folosind setul de ecuații diferentiale dat de relațiile (5.2.1).
Definiția 5.2.1.
Un punct de echilbiru sau un punct fix al sistemului ( ) este un vector astfel încât
( ) .
Se poate observa că dacă sistemul respectă ( ) atunci funcția ( ) cu poate
fi văzută ca pe o soluție a sistemului ( ). Cu alte cuvinte, dacă sistemul pornește dintr-o
stare de echilibru, acesta va rămâne In echilibru.
Exemplu:
Ecuația pendulului simplu:
in (5.2.2)
poate fi scrisă sub formă pe spațiul stărilor având vectorul de stare:
( ) ( )
Sistemul de ordin doi este echivalent cu doua ecuații diferențiale de ordin întâ i:
in
Iar câmpul vectorial ( ) este dat de:
( ) ( in
) având punctele de echilibru pentru: și cu
Așadar, soluțiile de echilibru sunt cele pentru care viteza inițială este zero iar pendulul este
fie în poziția de jos (pentru n par) sau în poziția de sus, inversată (pentru n impar).
Problema de stabilitate constă în analiza soluțiilor sistemului pentru condiții inițiale departe
de echilibru. Intuitiv, un punct de echilibru este numit stabil dacă pentru condiții inițiale
aproape de echilibru, soluțiile rămân aproape de acest punct. Un astfel de punct de echilibru
stabil este și cel în care pendulul este în poziția de jos. Similar, un punct de echilibru este
instabil dacă traiectoriile sunt divergente în jurul punctului de echilibru, cum este cazul în
care pendulul este în poziția de sus.
Mecanismul de Swingup 27
Definiția 5.2.2. Stabilitate în sens Lyapunov
Punctul de echilibru este stabil (în sens Lyapunov) la dacă pentru orice există
( ) astfel încât
‖ ( )‖ ‖ ( )‖ (5.2.3)
Stabilitatea Lyapunov este o cerință destul de permisivă aplicată punctelor de echilibru. În
mod particular, nu necesită ca traiectoriile ce încep aproape de origine să tindă către aceasta
asimptotic. De asemenea, stabilitatea e definită la un timp . Stabilitate globală este un
concept prin care se garantează că punctul de echilibru nu își pierde stabilitatea. Pentru ca
punctul să deține stabilitate uniformă, din Definiția 5.1 nu trebuie să fie o funcție de
Figura 5.2.1 - Punct de echilibru stabil în sens Lyapunov
Definiția 5.2.3. Stabilitate asimptotică
Punctul de echilibru este asimptotic stabil la dacă:
1. este stabil (5.2.4)
2. ‖ ( )‖ lim ‖ ( )‖
Ca și în definiția precedentă, stabilitatea asimptotică este definită la . Stabilitate
asimptotică globală necesită existentă unui independent de pentru care relația (5.2.4)
rămâne valabilă.
28 Mecanismul de Swingup
Figura 5.2.2 – Stabilitate asimptotică
Definiția 5.2.4. Stabilitate exponențială
Punctul de echilibru este exponențial stabil la dacă există constantele ,
astfel încât:
( ) ‖ ( )‖ ( ) (5.2.5)
Spunem despre un punct de echilibru că este instabil dacă nu este stabil.
Figura 5.2.3 – Instabilitate
5.3 Liniarizarea sistemelor neliniare
Un sistem liniar de tipul este stabil exponențial global dacă toate valorile proprii ale
matricii se regăsesc în semiplanul deschis stâng al planului complex și instabil dacă oricare
valoarea a matricii are partea reală pozitivă.
Mecanismul de Swingup 29
Pentru sisteme neliniare, stabilitatea poate fi dedusă deobicei din aproximația liniară a
sistemului neliniar în jurul unui punct de echilibru. Cu alte cuvinte, examinând sistemul:
unde și
|
(5.3.1)
Teoremă 5.3.1.
Fie sistemul neliniar ( ) cu ( ) și
|
. Atunci:
1. Matricea are toate valorile proprii in semiplanul deschis stâng al planului complex
daca și numai dacă sistemul neliniar este stabil exponențial local.
2. Dacă matricea are una sau mai multe valori proprii cu partea reală pozitivă atunci
punctul de echilibru al sistemul neliniar este instabil
3. Dacă matricea nu are valori proprii cu partea reală pozitivă dar are una sau mai
multe valori proprii pe axa complexă , atunci stabilitatea punctului de echilibru nu
poate fi determinată printr-o aproximație liniară
În ultimul caz, nu putem decide dacă punctul de echilibru este stabil, stabil asimptotic sau
instabil. Acesta nu poate fi însă stabil exponențial.
În cazul pendulului simplu, descris de (5.2.2) obținem următoarele aproximații liniare în punctele de echilibru ( ) și ( ):
*
+ * +
Matricea are valorile proprii confirmând intuiția că pendulul este într-un echilibru
instabil în configurația inversă. Deorece matricea are ambele valori proprii pe axa
imaginară în , stabilitatea pendului în punctul de echilibru din poziția de jos nu poate fi
concluzionată examinând numai aproximația liniară a ecuațiilor neliniare.
Definiția 5.3.1.
Fie ( ): , o funcție continuă, pozitiv definită cu derivate parțiale continue în
vecinătatea orginii din . Atunci V este numită Candidat de Funcție Lyapunov al sistemul
( ).
Derivarea funcției V în direcția traiectoriilor sistemului, sau derivarea în direcția câmpului
vectorial însemnă:
( ) ( )
( )
( ) (5.3.2)
30 Mecanismul de Swingup
Teorema 5.3.2.
Un punct de echilibru este stabil dacă există un cadidat de funcție Lyapunov V astfel încât
este negativ semi-definită pe traiectoriile soluțiilor sistemului.
( ) ( ) . (5.3.3)
Teorema 5.3.3.
Un punct de echilibru este asimptotic stabil dacă există un cadidat de funcție Lyapunov V
astfel încât este negativ definită pe traiectoriile soluțiilor sistemului.
( ) ( ) . (5.3.4)
Inegalitatea (5.3.4) însemnă ca V descrește pe direcția traiectoriilor sistemului, iar deoarece V
acționează ca o normă, însemnă că traiectoriile converg către punctul de echilibru. Acestă
inegalitate poate fi destul de dificil de obținut pentru un sistem oarecare.
Principiul de Invarianță LaSalle poate fi folosit pentru a demonstra stabilitatea asimptotică
chiar și atunci când funcția este doar pozitiv semi-definită.
5.4 Pasivitate
Pentru o analiză intrare/ieșire a sistemul neliniar (5.2.1) este necesar introducerea noțiunii de
pasivitate.
Definiția 5.4.1.
Un sistem H descris de sistemul de ecuații neliniare (5.2.1) este pasiv dacă există (local) o
funcție scalară pozitiv definită S, numită Funcție de Stocare, astfel încât
( ) ( ) ∫ ( ) ( )
(5.4.1)
Spre exemplu, un circuit construit din elemente pasive (resitențe, condensa tori, bobine),
variabila de intrare u o reprezintă intensitatea curentul prin circuit iar variabila de ieșire y este
tensiunea. Produsul reprezintă așadar puterea instantanee iar integrala din aceasta este
energia. Ecuația (5.4.1) descrie faptul că într-un sistem pasiv, schimbarea energiei existente
nu este mai mare decât cea furnizată la intrare.
Un avantaj al sistemelor pasive este că pot fi stabilizate folosind o bulcă închisă cu un
compensator proportițional.
Mecanismul de Swingup 31
Figura 5.4.1 – Stabilizarea sistemelor pasive
Dacă atunci se poate observa ca funcția de stocare S satisface
(5.4.2)
Indiferent de valoarea lui k. Asta înseamnă ca S va descrește continuu atâta timp cât ieșirea y
este diferită de zero. Această “margine de amplificare infinită” este o proprietate importantă a
sistemelor pasive ce poate fi folosită în proiectarea sistemelor de control împreună cu faptul
ca interconexiunile paralele sau in bucla de reacție a sistemlor pasive sunt tot niște sisteme
pasive.
5.5 Swingup simplu
Obiectivul acestui controller este a aduce pendulul în stare verticală într-un timp cât mai
scurt. O soluție este de a oferi cuplu maxim pendulului pentru a-i crește viteza în direcția de
deplasare. Un controller simplu în acest caz ar fi unul de tipul:
pentru
pentru (5.5.1)
Acesta funcționează suficient de bine, dar viteza unghiulară a pendulului este adesea prea
mare c.
5.6 Swingup bazat pe control simplu de energie
Pentru a face ca pendulul să ajungă în poziție verticală cu o viteză unghiulară mică, trebuie
monitorizată energia totală a sistemului la fiecare moment de eșantionare. Aceasta are
valoarea:
( co ) (5.6.1)
32 Mecanismul de Swingup
Obiectivul acestei strategii de control este de a aduce sistemul într-o stare în care energia
totală este egală cu cea din poziția de echilibru instabil:
Un controller de tip bang-bang poate fi apoi utilizat, accelerând pendulul când energia este
prea mică și decelerând când aceasta este prea mare.
Figura 5.6.1 – Energiile pendulului atunci când este eliberat din poziție verticală
Figura 5.6.2 – Schema de control Simulink cu swingup
0 500 1000 1500 2000 2500 3000 35000
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Energie potentiala
Energie totala
Energie cinetica
Mecanismul de Swingup 33
5.7 Swingup bazat pe control de energie
Dacă considerăm din nou, modelul neliniar simplificat al sistemului :
{ in
(5.7.1)
se poate observa că aceasta poate fi privit ca o interconectare în paralel a unui subsistem
pendular cu subsistemul rotorului pentru aceași intrare u.
Figura 5.7.1 – Pendulul invers ca a o interconectare în paralel de subsisteme
Dacă sistemul este descris ca în Figura 5.7.1, obiectivul imediat este cel de a defini funcțiile
de ieșire și astfel încât sistemul în interconexiune paralelă să fie pasiv.
Pentru subsistemul rotorului, se poate alege ca ieșire și ca funcție de stocare:
.
Pentru a alege ieșirea astfel încât subsistemul să fie pasiv, trebuie revăzută ecuația de
energia a pendulului.
( co )
Prin derivarea după timp a acestei ecuații se obține:
in ( in )
(5.7.2)
Subsistemul pendului este deci pasiv de la la cu energia ca și funcție de stocare.
Deorece este de dorit să se manipuleze energia din sistem, funcția de stocare a acestui
subsistem va fi considerată de forma:
( )
(5.7.3)
34 Mecanismul de Swingup
unde reprezintă energia de referință. Cu acestă modificare, urmatoarea relație este
valabilă:
( ) ( ) (5.7.4)
Cu aceste definiții, interconexiunea în paralel a subsistemelor este pasivă, cu ieșirea dată de :
iar funcția de stocare
( )
(5.7.5)
Prin definirea funcției de stocare ca o combinație liniară a funcțiilor de stocare și
ponderate prin coeficienții și , controllerul capătă o flexibilitate sporită. Constantele ,
respectiv pot fi ajustate pentru a influența regimul tranzitoriu al sistemului.
Calculând derivata funcției de stocare S se obține:
( ( ) ) (5.7.6)
Alegând legea de comandă de forma:
( ( ) ) (5.7.7)
se obține următoarea relație:
(5.7.8)
Sistemul este așadar stabil iar pentru orice condiție initială departe de punctul de echilibru :
și (5.7.9)
Controlul hibrid al pendulului 35
Capitoul 6
Controlul hibrid al pendulului
6.1 Stablizare combinată cu swingup
În capitolele precedente au fost proiectate strategiile liniare de stabilizare ce pot fi folosite
odată ce pendulul este adus în jurul poziției verticale. Aceste strategii mențin unghiul
pendulului precum și turația rotorului în jurul valorilor dorite. Au fost deasemenea prezentate
strategii de control neliniar folosite pentru a aduce pendulul în jurul poziției verticale. În
acest capitol cele doua strategii vor fi combinate rezultând într-o tehnică de control hibrid ce
poate aduce și stabilza pendulul în poziție verticală din orice configurație.
Legea de control a energiei are forma:
( ( ) ) (6.1.1)
unde
( co )
Legile de control folosite la stabilizare au forma:
(6.1.2)
Controllererul de energie aduce pendulul până aproape de configurația inversă, dar nu îl poate
menține acolo. Regulatorul LQR stabilizeaza pendulul menținând și turația rotorului aproape
de zero, dar acesta funcționeaza doar local. Este natural atunci, să se folosească regulatorul de
energie până ce pendulul ajunge în configurație inversă iar apoi să se producă o comutație pe
regulatorul cu reacție după stare pentru stabilizare.
36 Controlul hibrid al pendulului
Figura 6.1.1 – Controlul hibrid al pendulului
Pentru a comuta între aceste doua regimuri este utilizată informația legată de poziția
unghiulară a pendulului. Practic, pentru | | este folosit controllerul de swingup, iar
pentru | | , este utlizat algoritmul de stabilizare.
Valoarea unghiului de prag s-a ajustat experimental pentru a minimiza pe cât posibil
șocurile de comutare între cele doua regulatoare.
Stabilizare
Swingup
Zona de stabilizare
25 30 35 40 45 50 550
1
2
3
4
5
6
7
timp (sec)
theta
(ra
d)
25 30 35 40 45 50 55-250
-200
-150
-100
-50
0
50
100
150
200
250
timp (sec)
com
anda P
WM
Figura 6.1.2 – Controlul hibrid al pendulului
Implementarea fizică a pendulului invers 37
Capitolul 7
Implementarea fizică a pendulului invers
7.1 Introducere
In capitolele precedente am prezentat aparatul matematic necesar pentru a înțelege dinamica
sistemului format dintr-un pendul invers cu o roată de reacțiune. Acest capitol este dedicat
prezentării implementării fizice a unui algoritm de control pe un sistem real construit de
mine.
7.2 Construcția cadrului pendulului
Cadrul sistemului a fost construit folosind aluminiu și plexiglas. O tija de aluminiu lungă de
40cm formeaza axa principală a pendului. Aceasta este perforată in centru si fixată printr -un
conector de axul encoderului unghiular. La unul din capete este fixat motorul împreună cu
encoderul de turație iar la celalalt capăt o contragreutate. Contragreutatea este realizată din
plumbi de pescuit perforați ce sunt fixați prin niste șuruburi de bara pendulului. Au fost
folosite două tipuri de greutati de plumb: 70g si 30g. Prin ajustarea acestor bucati de plumb
se poate schimba cu usurinta distribuția de masă a sistemului.
Encoderul din centrul pendulului este fixat in interiorul unei cutii de aluminiu galvanizat. O
gaura laterala in această cutie permite expunerea în exterior doar a axului encoderului.
Cutia de aluminiu împreună cu pendulul atașat este fixat pe un bloc patratic de aluminiu
avand latura de aproximativ 7cm. De acest bloc sunt fixate 2 cleme ce permit atașarea
sistemului pe diverse suprafețe. Tot pe acest bloc a fost atașată o bucată de plexiglass pe care
au fost montate componentele electronice. O altă bucată de plexiglass a fost fixată deasupra
acestor componentelor electronice pentru a le proteja.
38 Implementarea fizică a pendulului
invers
7.3 Sistemul de acționare
Sistemul de acționare este alcătuit dintr-un motor Pololu (50:1 Metal Gearmotor 37Dx54L
mm) de curent continuu având tensiune de alimentare de 12V. Motorul are în componența lui
un reductor 50:1 ce permite dezvoltarea unui cuplu de 12 kg*cm. Curentul prin motorul
blocat este de aproximativ 5A iar in mișcare liberă de aproximativ 300mA la o turație de 200
RPM.
Figura 7.3.1 - Motor Pololu (50:1 Metal Gearmotor 37Dx54L mm)
De axul motorul a fost atașat un conector impreună cu o roată dințată prin care se transmite
mișcarea de rotație către encoderul de viteză. Conectorul facilitează prinderea de axul
motorului a unei tije de aluminiu de care au fost atașate la ambele capete niște greutați de
plumb. Greutațile sunt ușor ajustabile permițând calibrarea distribuției de masă a sistemului.
Motorul este controlat cu ajutorul unui modul ce conține un driver L298N. Acesta este un
driver folosit pentru a controla 2 motoare de curent continuu sau un motor pas cu pas.
Curentul maxim pe fiecare canal este de 2A. Modulul de acționare mai conține pe langa acest
integrat și niște condesatori de filtrare pe alimentare, diode de protecție împotriva curentului
generat de motor in regim de generator. Conectorii atașati de modul permit atașarea cu
ușurința a cablurilor de alimentare și de semnal.
Figura 7.3.2 – Modul Arduino cu driver L298N
Tensiunea aplicată pe motor este controlata printr-un semnal de tip PWM (Pulse Width
Modulation) aplicat pe pinul ENABLE. Prin schimbarea duty-cycle-ului acestui semnal
folosind tehnica PWM se controleaza energia furnizată într-o perioada medie motorului. Doi
Implementarea fizică a pendulului invers 39
pini de direcție permit setaria direcției de rotație a motorului sau punerea acestuia în regim de
franare.
7.4 Senzorii
Pentru a capta poziția unghiulara a pendului și cea a motorului au fost utilizați doi encoderi
rotaționali în cuadratură. Acești encoderi incrementali furnizează semnale ciclice la ieșire
doar atunci cand encoderul este rotit. Acesta este compus din doua piste colorate alternativ ce
au atașate cate o pereche LED – fototranzistor. Canalele de ieșire numite A și B sunt numite
și ieșiri în cudratură deoarece sunt defazate cu 90 de grade.
Diagramele de stări asociate acestor senzori sunt urmatoarele:
Figura 7.4.1 – Formele de undă ale semnalelor de la encoderi
Encoderul atasat motorului este unui de tip HEDL 5540 (Fig. 7.4.2 stânga). Acesta furnizeaza
500 de impulsuri pentru fiecare rotatie completa a axului. Datorită angrenajului cu roți dințate
prin care este conectat motorul la encoder, numărul total de impusuri generate de encoder
pentru o rotație completă a motorul este de 1600.
Figura 7.4.2 – Encoderii incrementali utilizați
Rotație in sensul orar
Faza A B
1 0 0
2 0 1
3 1 1
4 1 0
Rotație in sensul anti-orar
Faza A B
1 1 0
2 1 1
3 0 1
4 1 0
40 Implementarea fizică a pendulului
invers
Encoderul de pe axa principala de rotatie este un EP50S8-1024 5540 (Fig. 7.4.2 dreapta).
Datorită monturii acestuia, este posibil sa susțină greutatea întregului sistem.
7.5 Sistemul de procesare
7.5.1 Introducere
Pe partea de procesare este utilizată o placa Arduino Duemilanove dotată cu un
microcontroller Atmega168. Aceasta platformă a fost aleasă datorită ușurinței de dezvoltare,
a clarității codului sursă, precum și datorită documentației existente pentru aceasta platformă.
Figura 7.5.1 - Arduino Duemilanove
7.5.2 Întreruperi
Întreruperile reprezintă un mecanism prin care un microcontroller sau un microprocesor poate
răspunde la evenimente exterioare independente de fluxul normal de procesare. Odată cu
declanșarea unei întreruperi, contextul aplicației este salvat pe stiva iar procesarea se
continua in rutina de tratare a intreruperilor dupa care se restaureaza contextul si se reia
procesarea din locul in care a fost intrerupta.
Acest sistem de intreruperi permite o procesare eleganta a semnalelor furnizate de cele doua
encodere rotationale. În loc de a monitoriza într-o bucla în permanenta starea pinilor
corespunzători encoderilor (practică numită și polling) se atașează câte o întrerupere pe
momentele de tranziție a semnalelor pentru acei pini.
Placa Arduino este dotată cu doi pini de întrerupere, ce au fost folosiți pentru a capta
momentele de tranziție a impulsurilor generate de encoderi. Datorită limitarii la doi pini de
intreruperi, citirea encoderelor se face la jumătate din rezoluția disponibilă. Astfel, la
momentul unei tranziții pe canalul A al unui encoder, se declansează procedura de întrerupere
în care se citește starea celuilalt canal, utilizând un pin standard de intrare digitală. Odată
citită starea celor doup canale, se poate deduce sensul de rotație și se poate incrementa sau
decrementa un contor al poziției unghiulare.
Implementarea fizică a pendulului invers 41
7.5.3 Scrieri si citiri digitale rapide
Pentru efectuarea scrierilor și citirilor pe pinii digitali a fost utilizată libraria
“digitalWriteFast.h”. Aceasta permite scrieri sau citiri de pini digitali cu o viteza de
aproximativ un ordine de marime mai mare decât procedeul standard utilizând metoda
digitalRead(). Aceasta optimizare este posibilă doar in situația în care pinul de pe care se face
scrierea sau citirea este cunoscut în timpul compilării.
7.5.4 Calculul vitezelor unghiulare
Datorită caracterului discret al semnalelor, viteza unghiulară (sau derivata de ordin unu a
poziției unghiulare) se calculează ca o diferență între două valori unghiulare succesive
împarțite la o constanta de timp.
În cazul unei procesari complexe, datorită diverselor întârzieri ce pot apărea în fluxul de
procesare, acea constantă de timp necesară pentru un calcul corect al vitezei poate avea valori
diferite. Acest fapt se datorează eșantionării neuniforme efectuate asupra semnalului
corespunzător poziției unghiulare.
Deoarece vitezele unghiulare sunt informații vitale pentru algoritmul de calcul, este critic să
se mențină un timp de eșantionare cât mai aproape de o constantă. Acest fapt conduce la
necesatitatea optimizării la maxim a diverselor bucle de calcul precum și la necesitatea
filtrării semnalelor corespunzătoare vitezelor.
7.5.5 Filtrarea semnalelor ale vitezelor unghiulare
Filtre de ordin 1 au fost folosite pentru a filtra vitezele unghiulare. Un asemenea filtru poate
fi descris în timp continuu folosind ecuația:
( )
( ) (7.5.5.1)
unde este constata filtrului. Folosind aproximația lui Euler pentru derivată conform:
(7.5.5.2)
unde este perioada de eșantionare. După calcule simple se poate arăta că:
( )
( )
( )
(7.5.5.3)
Transformând înapoi în domeniul timp și translând cu un pas în urmă obținem:
( )
( )
( )
(7.5.5.4)
42 Implementarea fizică a pendulului
invers
Constatele de timp pentru filtre trebuie acum alese. Dacă constanta de timp a filtrului e prea
mică acesta nu v-a elimina foarte mult zgomot. Daca e setată prea mare, v-a distorsiona și
aplatiza semnalul. Exeperimental, valorile de și s-au dovedit a fi
suficiente pentru a atenua zgomotul într-o manieră bună. Constantele de timp ale sistemelor
corespunzătoare sunt de aproape 20 de ori mai mari, deci “adevăratele semnale” nu sunt
afectate.
7.6 Neliniarități adiționale ale sistemului
Pe lângă modelul neliniar matematic prezentat în capitolele precedent, sistemul fizic este
afectat de numeroase alte neliniarităție pe care le-am ignorat în modelarea matematică. În
această secțiune sunt enumerate o parte dintre aceste particularități neliniare.
7.6.1 Forțe de frecare
O primă clasă de neliniarități sunte cele datorate forțelor de frecare. Datorită forței de frecare
din jurul axei de rotație, pendulul disipă din energia totală iar dinamica sistemului converge
catre punctul de echilibru stabil. Totodată, există frecare vâscoasă și la nivelul rotorului
motorului, aceasta contribuind la forma neliniară a caracteristicii statice. Angrenajul reductor
la care este conectat motorul, marește intervalul de comandă pentru care motorul nu se
învârte (deadzone în limba engleză).
7.6.2 Neuniformități în distribuția de masă
Deoarece greutatea atașata de motor nu este una simetrică față de axa de rotație , momentul
interțial al sistemului este dependent de poziția unghiulară a rotorului. Această dependență
are influență asupra perioadei de oscilație naturale a sistemului.
Imperfecțiunile din timpul construcției cadrului precum și cele datorate poziționării
greutăților contribuie cu unele asimetrii în distribuția de masă a sistemului.
7.6.3 Cuplu auxiliar datorat cablurilor atașate de motor si de encoder
Datorită modalității de alimentare a motorului, cablul de alimentare poate genera un cuplu
sesizabil. Acest fapt poate fi evidențiat din poziția de echilibru instabil prin studierea
efectului unei repoziționari ale cablului asupra dinamicii sistemului.
Concluzii și perspective 43
Capitolul 8
Concluzii și perspective
Concluzii
Scopul acestei lucrări de disertație a fost de a prezenta un sistem complex de control
împreună cu o implementare fizică a acestuia. Sistemul ales a fost un pendul invers cu roată
de reacțiune.
Aparatul matematic utilizat în proiectarea și analiza sistemului a fost prezentat. Acesta
include tehnici de modelare neliniară a sistemelor, regulatoare cu reacție după stare, concepte
de echilibru și stabilitate pentru sisteme neliniare, comandă neliniară și tehnici de linizarizare.
O prezentare detaliată a implementării a fost furnizata. Sistemele de măsurare, acționare și
procesare au fost ilustrate. Au fost amintite deasemenea și diversele problemele ce au fost
întâmpinate în construirea acestui echipament. Neliniaritățile auxiliarea ale sistemului au fost
amintite.
Perpective
O îmbunătățire ce poate fi adusă sistemului este de a utiliza algoritmi de control mai
sofisticați pentru stabilizare de tipul MPC sau .
Performațe mai bune ar putea fi obținute si prin utilizarea unui sistem de procesare mai
puternic. Placa Arduino reduce rezoluția encoderilor incrementali la jumătate deoarece este
dotată doar cu doi pini de întrerupere.
Un motor cu performațe mai bune, precum și cu o roată de reacțiune cu distribuție de masă
simetrică față de axa de rotație ar îmbunătăți considerabil performațele sistemului.
44 Anexe
Anexe
A.1 Cod sursă software de control Arduino
#include <digitalWriteFast.h>
//#define SERIAL_DEBUG
// pini encoderi
#define ENCODER_0_PIN_A 2
#define ENCODER_0_PIN_B 4
#define ENCODER_1_PIN_A 3
#define ENCODER_1_PIN_B 5
// pini motor
#define MOTOR_PIN_A 9
#define MOTOR_PIN_B 8
#define MOTOR_PIN_PWM 10
// pini LED
#define RED_LED_PIN 12
#define YELLOW_LED_PIN 11
// comanda maxima
#define U_MAX 255
// comanda utilizata pt swingup de tip bang-bang
#define U_SWINGUP 150
// energie potentiala in pozitie verticala
#define E0 4232
// energia de referinta
#define Eref 10800
// unghiul de prindere
#define CATCH_ANGLE 30
#define THETA_MAX 400
#define THETA_R_MAX 800
// pozitie unghiulara pendul
volatile int theta = 400;
int old_theta = theta;
// viteza unghiulara pendul
int w = 0;
int old_w = 0;
Anexe 45
double w_f = 0.0;
// pozitie unghiulara rotor
volatile long theta_r = 0;
long old_theta_r = 0;
// turatie rotor
int w_r = 0;
int old_w_r = 0;
double w_r_f = 0.0;
void setup() {
// setari pini encoderi
pinMode(ENCODER_0_PIN_A, INPUT);
pinMode(ENCODER_0_PIN_B, INPUT);
digitalWrite(ENCODER_0_PIN_A, HIGH);
digitalWrite(ENCODER_0_PIN_B, HIGH);
pinMode(ENCODER_1_PIN_A, INPUT);
pinMode(ENCODER_1_PIN_B, INPUT);
digitalWrite(ENCODER_1_PIN_A, HIGH);
digitalWrite(ENCODER_1_PIN_B, HIGH);
// atasare intreruperi
attachInterrupt(0, irsPinAEn0, CHANGE);
attachInterrupt(1, irsPinAEn1, CHANGE);
// setari pini motor
pinMode(MOTOR_PIN_A, OUTPUT);
pinMode(MOTOR_PIN_B, OUTPUT);
pinMode(MOTOR_PIN_PWM, OUTPUT);
// setari pini LED
pinMode(RED_LED_PIN, OUTPUT);
pinMode(YELLOW_LED_PIN, OUTPUT);
#ifdef SERIAL_DEBUG
Serial.begin(115200);
#endif
setMotorSpeed(0);
}
int u_read = 0;
void loop() {
#ifdef SERIAL_DEBUG
/* citiri de date pe seriala
int nrIncomingBytes = Serial.available();
if (nrIncomingBytes > 0) {
u_read = 0;
int negative = 0;
for (int i = 0; i < nrIncomingBytes; i++) {
int val = Serial.read();
if (val == '-') {
negative = 1;
} else {
int digit = val - '0';
46 Anexe
u_read = 10 * u_read + digit;
}
}
if (negative) u_read = -u_read;
}
/**/
#endif
/* W */
w = theta - old_theta;
if (theta > THETA_MAX) theta = theta - 2*THETA_MAX;
if (theta < -THETA_MAX) theta = theta + 2*THETA_MAX;
old_theta = theta;
/* W_R */
w_r = theta_r - old_theta_r;
w_r_f = 0.4444 * w_r_f + 0.5556 * w_r;
//if (theta_r > THETA_R_MAX) theta_r = theta_r - 2*THETA_R_MAX;
//if (theta_r < -THETA_R_MAX) theta_r = theta_r + 2*THETA_R_MAX;
old_theta_r = theta_r;
float u;
if (abs(theta) > CATCH_ANGLE) {
// Swingup
// setare LED
digitalWriteFast(RED_LED_PIN, HIGH);
digitalWriteFast(YELLOW_LED_PIN, LOW);
// energie cinetica
int E_c = w * w;
// energie potentiala
double cos_theta = cos(PI/400 * theta);
double E_p = E0 * (1 + cos_theta);
// energie totala
double E = E_c + E_p;
/* Swingup Bang-bang
if (E < E0) {
if (w >= 0)
u = -U_SWINGUP;
else
u = U_SWINGUP;
} else {
if (w > 0)
u = U_SWINGUP;
else
u = -U_SWINGUP;
}
*/
// comanda neliniara
u = 0.0015*(E-Eref)*w-0.001*w_r_f;
} else {
Anexe 47
// stabilizare
// control LQR
u = 14.5 * theta + 29 * w + 0.7 * w_r_f;
// intensitate LED
float led_power = abs(u);
if (led_power > 255) {
led_power = 255;
}
analogWrite(YELLOW_LED_PIN, led_power);
digitalWriteFast(RED_LED_PIN, LOW);
}
// limitare semnal PWM
if (u > U_MAX) { u = U_MAX; }
if (u < -U_MAX) { u = -U_MAX; }
#ifdef SERIAL_DEBUG
Serial.println(theta);
Serial.println(w);
Serial.println(u);
Serial.println(w_r_f);
#endif
setMotorSpeed(u);
delay(100);
}
// functie auxiliara utilizata pentru a seta turatia motorului
void setMotorSpeed(int val) {
static float old_val;
if (val == old_val) {
return;
} else {
old_val = val;
}
if (val > 0) {
digitalWriteFast(MOTOR_PIN_A, HIGH);
digitalWriteFast(MOTOR_PIN_B, LOW);
analogWrite(MOTOR_PIN_PWM, val);
} else {
digitalWriteFast(MOTOR_PIN_B, HIGH);
digitalWriteFast(MOTOR_PIN_A, LOW);
analogWrite(MOTOR_PIN_PWM, -val);
}
}
/* ISR: schimbare de semnal pin A Encoder 0 */
void irsPinAEn0(){
// citire pini semnal
int drB = digitalReadFast(ENCODER_0_PIN_B);
int drA = digitalReadFast(ENCODER_0_PIN_A);
48 Anexe
// actualizare pozitie
if (drA == HIGH) {
if (drB == LOW) {
theta++;
} else {
theta--;
}
} else {
if (drB == HIGH) {
theta++;
} else {
theta--;
}
}
}
/* ISR: schimbare de semnal pin A Encoder 1 */
void irsPinAEn1(){
// citire pini semnal
int drB = digitalReadFast(ENCODER_1_PIN_B);
int drA = digitalReadFast(ENCODER_1_PIN_A);
// actualizare pozitie
if (drA == HIGH) {
if (drB == LOW) {
theta_r++;
} else {
theta_r--;
}
} else {
if (drB == HIGH) {
theta_r++;
} else {
theta_r--;
}
}
}
A.2 Cod sursă software de citire date - Matlab
%clear all; close all; T = 2; u0 = 60; delete(instrfind({'Port'},{'COM3'})) s = serial('COM3');
WINDOW_SIZE = 256; set(s, 'InputBufferSize', 16); set(s, 'OutputBufferSize', 8); %set(s, 'FlowControl', 'hardware');
Anexe 49
set(s, 'BaudRate', 115200); set(s, 'Parity', 'none'); set(s, 'DataBits', 8); set(s, 'StopBit', 1); set(s, 'Timeout',10);
fopen(s); t=1; disp('Running');
theta_buffer=zeros(1,WINDOW_SIZE); w_buffer=zeros(1,WINDOW_SIZE); wr_buffer=zeros(1,WINDOW_SIZE); u_buffer=zeros(1,WINDOW_SIZE);
fig1 = figure; fig2 = figure; fig3 = figure; fig4 = figure;
u = 0; disp(u); fprintf(s, '%d', u);
while(1) set(0,'CurrentFigure',fig1) tv = (t:t+WINDOW_SIZE-1)/10; % theta theta = fscanf(s,'%f'); theta_buffer=[theta_buffer(2:end) theta]; plot(tv,theta_buffer); axis auto; grid on; title('theta');
% w set(0,'CurrentFigure',fig2) w = fscanf(s,'%f'); w_buffer=[w_buffer(2:end) w]; stairs(tv,w_buffer); axis auto; grid on; title('w');
% u set(0,'CurrentFigure',fig3) u = fscanf(s,'%f'); u_buffer=[u_buffer(2:end) u]; stairs(tv,u_buffer); axis auto; grid on; title('u');
% w_r
50 Anexe
set(0,'CurrentFigure',fig4) w_r = fscanf(s,'%f'); wr_buffer=[wr_buffer(2:end) w_r]; plot(tv,wr_buffer,'r'); axis auto; grid on; title('w_r');
drawnow;
t=t+1; end
fclose(s);
Listă de figuri 51
Listă de figuri
Figura 2.2.1 – Pendulul simplu .............................................................................................. 6
Figura 2.2.2 – Potretul fazelor pentru un oscillator armonic simplu ....................................... 6
Figura 2.2.3 – Portretul fazelor al pendulului simplu ............................................................. 7
Figura 2.3.1 – Decolarea în misiunea Apollo 11 .................................................................... 8
Figura 2.3.2 – Robotul umanoid Asimo produs de Honda ...................................................... 9
Figura 2.3.3 – Segway (stanga) și un robot autonom de auto-echilibrare (dreapta) ................ 10
Figura 2.4.1 – Platforma Cart-Pole ....................................................................................... 10
Figura 2.4.2 – Pendulul Furuta ............................................................................................. 11
Figura 3.1.1 – Diagrama schematică a pendului cu roată de reacțiune ................................... 12
Figura 3.4.1 – Modelul Simulink al sistemul în buclă deschisa ............................................. 16
Figura 3.4.2 – Modelul neliniar al sistemului ........................................................................ 17
Figura 3.8.1 – Dependența nelinară între tensiunea de pe motor si comanda aplicată ............ 19
Figura 3.9.1 - Răspunsul motorului la o treaptă: ....................................................... 19
Figura 4.1.1 – Diagrama bloc generală a procesul controlat .................................................. 20
Figura 4.5.1 – Schema de control folosind regulator pe stare ................................................ 24
Figura 5.2.1 - Punct de echilibru stabil în sens Lyapunov ..................................................... 27
Figura 5.2.2 – Stabilitate asimptotică .................................................................................... 28
Figura 5.2.3 – Instabilitate .................................................................................................... 28
Figura 5.4.1 – Stabilizarea sistemelor pasive ........................................................................ 31
Figura 5.6.1 – Energiile pendulului atunci când este eliberat din poziție verticală ................. 32
Figura 5.6.2 – Schema de control Simulink cu swingup ........................................................ 32
Figura 5.7.1 – Pendulul invers ca a o interconectare în paralel de subsisteme ....................... 33
Figura 6.1.1 – Controlul hibrid al pendulului ........................................................................ 36
Figura 6.1.2 – Controlul hibrid al pendulului ........................................................................ 36
Figura 7.3.1 - Motor Pololu (50:1 Metal Gearmotor 37Dx54L mm) ...................................... 38
Figura 7.3.2 – Modul Arduino cu driver L298N ................................................................... 38
Figura 7.4.1 – Formele de undă ale semnalelor de la encoderi .............................................. 39
Figura 7.4.2 – Encoderii incrementali utilizați ...................................................................... 39
Figura 7.5.1 - Arduino Duemilanove .................................................................................... 40
52 Bibliografie
Bibliografie
[BAS01] Jerome Block, Karl Johan Åström, Mark W. Spong, The Reaction Wheel
Pendulum Morgan & Claypool Publishers, 2007
[CDNOP01] Geoffrey Chauveau, Damien Chazal, Daisuke Nakayama, Erik Olsen,
Staffan Palm, Controlling the Reaction Wheel Pendulum, 2005
[JW01] Karl Henrik Johansson and Bo Wahlberg, Nonlinear Control - Lecture notes
[AF01] Karl Johan Åström, and Furuta, K., Swinging up a pendulum by energy
control, Automatica, vol. 36, pp. 278–285, 2000.
[CH01] Chung, C.C., and Hauser, J., Nonlinear control of a swinging pendulum
Automatica, vol. 31, pp. 851–862, 1995.
[SCL01] Spong, M.W., Corke, P., and Lozano, R., Nonlinear control of the inertia
wheel pendulum, Automatica, 1999
