sistemi di controllo multivariabile
Post on 15-Feb-2015
142 Views
Preview:
TRANSCRIPT
SISTEMI DI CONTROLLO MULTIVARIABILE
Self-Balancing of a two-wheeled robot
Ch.mo Prof. Francesco Amato
Mario Boccaccino (M58/8)
Antonio Russo (M58/2)
Fabrizio Schiano (M58/5)
INDICE
1
1. Introduzione .............................................................................................................................................................................................. 3
2
2. Sistema Fisico ............................................................................................................................................................................................ 3
2.1 Modello non lineare ................................................................................................................................................................................ 4
2.2 Modello Linearizzato ............................................................................................................................................................................... 5
3
3. Componenti Hardware del sistema ......................................................................................................................................................... 5
3.1 Piattaforma Hardware Arduino Duemilanove ....................................................................................................................................... 6
3.1.1 Arduino duemilanove ........................................................................................................................................................................... 6
3.1.2 Alimentazione ....................................................................................................................................................................................... 7
3.1.3 Memoria ............................................................................................................................................................................................... 7
3.1.4 Input e Output ...................................................................................................................................................................................... 7
3.1.5 Comunicazione ..................................................................................................................................................................................... 8
3.1.6 Programmazione .................................................................................................................................................................................. 8
3.1.7 USB Protezione da sovracorrente ........................................................................................................................................................ 9
3.1.8 Caratteristiche fisiche........................................................................................................................................................................... 9
3.1.9 Tabella di sintesi ................................................................................................................................................................................... 9
3.2 Sensori...................................................................................................................................................................................................... 9
3.2.2 Sensore a Ultrasuoni .......................................................................................................................................................................... 10
3.3 Attuatori ................................................................................................................................................................................................. 11
3.4 Altri componenti .................................................................................................................................................................................... 13
4
4. Algoritmi di Controllo ............................................................................................................................................................................. 14
4.1 Programmi in MATLAB/SIMULINK ........................................................................................................................................................ 14
4.2 PID .......................................................................................................................................................................................................... 17
4.3 Allocazione dei poli................................................................................................................................................................................ 19
4.3.1 Risultati dell’Allocazione dei poli ....................................................................................................................................................... 19
4.4 Linear-Quadratic-Regulator (LQR) ........................................................................................................................................................ 20
4.4.1 Risultati LQR........................................................................................................................................................................................ 21
5
5 Conclusioni ............................................................................................................................................................................................... 24
Appendice
A.1 Codice principale MATLAB .................................................................................................................................................................... 25
A.2 Codice Arduino PID ............................................................................................................................................................................... 26
A.3 Codice Arduino Allocazione dei poli ..................................................................................................................................................... 28
A.4 Codice Arduino LQR .............................................................................................................................................................................. 31
1. Introduzione In questo progetto è stato realizzato il self-balancing per un robot a due ruote
dotato di una piattaforma hardware per il physical computing (Arduino).
Come si può intuire, il modello a cui si è fatto riferimento è quello del pendolo
inverso.
L’obiettivo è quello di bilanciare il robot con diversi algoritmi di controllo, studiati al
corso di Sistemi di Controllo Multivariabile ed evidenziarne le differenze. In
particolare sono stati presi in considerazione i seguenti algoritmi di controllo:
- PID (Proporzionale-Integrale-Derivativo)
- Allocazione dei poli (Poles Placement)
- Linear Quadratic Regulator (LQR)
Tutti i risultati sono stati analizzati in ambiente MATLAB/SIMULINK e trasferiti
successivamente nell’IDE Arduino, per una controprova realistica di quanto
ottenuto.
2. Sistema Fisico Come già evidenziato nell’introduzione, il comportamento del nostro robot può
essere esemplificato con quello di un pendolo inverso, schematizzato nella figura
seguente:
Con:
- coordinata libera di spostamento dell’asta dalla verticale, positiva se oraria;
- lo scostamento del carrello dalla posizione iniziale.
Come si può notare, il sistema è composto da un’asta di massa trascurabile e
lunghezza , incernierata alla base a un carrello di massa e libero di scorrere su
un piano di appoggio. In cima all’asta vi è posizionata una massa . Sul carrello
sono montati due servomotori in grado di generare una forza parallela alle due
ruote.
2.1 Modello non lineare
Per poter progettare il sistema di controllo, è necessario conoscere le equazioni
della dinamica del sistema; tali equazioni possono essere ricavate tramite il metodo
Lagrange. Si procede quindi alla scrittura dell’espressione dell’energia potenziale U
e dell’energia cinetica T del sistema:
Per ottenere le equazioni della dinamica si sfrutta la formula di Lagrange per
entrambi i gradi di libertà:
dove rappresenta il coefficiente di attrito.
Dalle due equazioni ottenute è possibile ottenere un modello agli stati del sistema,
dove gli stati sono quattro e sono costituiti dalle coordinate libere e dalle loro
derivate temporali prime, alle quali viene assegnato un proprio simbolo per
maggiore chiarezza della notazione:
2.2 Modello Linearizzato
Le equazioni ottenute nel paragrafo precedente non sono ancora quelle utili ai fini
del progetto del controllore, in quanto sono non lineari. Occorre quindi linearizzare
le equazioni rispetto ad una condizione di equilibrio, e si capisce facilmente che tale
condizione si realizza per:
L’equazione espressa in forma matriciale è la seguente:
Moltiplicando l’equazione a sinistra entrambi i membri per l’inverso di V si ottiene il
sistema nella forma consueta .
3. Componenti Hardware del sistema In questo paragrafo, come da titolo, verranno analizzati i componenti hardware
utilizzati per realizzare quanto esposto nei paragrafi precedenti.
3.1 Piattaforma Hardware Arduino Duemilanove
Arduino è un open-source di prototipazione elettronica basato su una piattaforma
flessibile, con hardware e software facili da usare. É destinato ad artisti, designer,
hobbisti e a chiunque sia interessato a creare oggetti o ambienti interattivi.
Arduino può percepire l'ambiente, ricevere input da una varietà di sensori e può
influenzare l'ambiente circostante con luci di controllo, motori ed altri attuatori.
Il microcontrollore della scheda è programmato usando il linguaggio di
programmazione Arduino (basato su cablaggio) e l'ambiente di sviluppo Arduino
(basato su Processing). I progetti di Arduino possono essere stand-alone o
comunicare con il software in esecuzione su un computer (ad esempio Flash,
Processing, MaxMSP). Le tavole possono essere costruite a mano o acquistate già
preassemblate e il software può essere scaricato gratuitamente.
I riferimenti di progetti hardware (i file CAD) sono disponibili sotto una licenza open-
source e liberi di adattarli alle proprie esigenze. Arduino ha ricevuto una Menzione
d'Onore nella Comunità sezione digitale del Prix Ars Electronica 2006.
3.1.1 Arduino Duemilanove
Il cervello del sistema di controllo é senza dubbio il microcontrollore, capace di
ricevere segnali in ingresso provenienti da sensori, elaborarli e trasmettere il giusto
comando all'attuatore, nel nostro caso ai due motoriduttori.
Nella realizzazione del robot si è fatto uso della piattaforma ARDUINO,
precedentemente descritta, che implementa il linguaggio Process/Wiring
(Processo/Cablaggio). Più nello specifico é stata scelta la versione Duemilanove,
illustrata nella figura seguente, dotata di un microcontrollore ATMega-328.
La scheda Arduino Duemilanove contiene tutto il necessario per sostenere il
microcontrollore, ed è sufficiente connettersi a un computer con un cavo USB, ad un
alimentatore o ad una batteria per iniziare la trasmissione.
3.1.2 Alimentazione
Arduino Duemilanove può essere alimentato attraverso la connessione USB, o con
un alimentatore esterno. Il consiglio dato dal manuale è quello di farlo operare con
un alimentatore esterno da 6V a 20V. Il pin 5V può fornire meno di 5V e di
conseguenza la scheda potrebbe risultare instabile. Se invece si utilizzano più di 12V,
il regolatore di tensione potrebbe surriscaldarsi e danneggiare la scheda. L'intervallo
consigliato è 7-12V.
I pin di alimentazione sono i seguenti:
- VIN. Il pin con cui é possibile alimentare la scheda Arduino con una tensione
superiore ai 5V. É possibile fornire tensione attraverso connessione
USB o un'altra fonte di alimentazione regolata in tensione.
- 5V. Il Pin di alimentazione regolata, utilizzata per alimentare il
microcontrollore e gli altri componenti sulla scheda. La tensione di 5V può
essere ricavata dal pin VIN tramite un regolatore a bordo, o essere fornita
tramite USB o un altro 5V controllato.
- 3V3. É generata dal board su chip FTDI e ha un assorbimento di corrente
massimo pari a 50 mA.
- GND. Ground pin.
3.1.3 Memoria
L'Atmega-328 ha 32 KB, di cui 2 KB utilizzati per il bootloader, 2 KB di SRAM e 1 KB di
EEPROM.
3.1.4 Input e Output
Ciascuno dei 14 pin digitali sull'Arduino Duemilanove può essere usato come input o
output, utilizzando funzioni attraverso il software speci_co (pinMode(),
digitalWrite() e digitalRead() ). I pin operano a 5 Volt e ognuno di questi può fornire
o ricevere un massimo di 40mA; presentano una resistenza di pull-up interna di 20-
50KWe inoltre alcuni pin hanno funzioni specializzate:
- Pin: 0(RX) e 1(TX). Sono utilizzati per ricevere e trasmettere dati seriali in
formato TTL. Sono collegati ai pin 0 (RX) e 1 (TX) corrispondenti della serie
FTDI USB-to-chip TTL.
- Interrupt esterni: 2 e 3. Questi pin possono essere configurati per attivare un
interrupt, sia su un valore basso di salita o di discesa, sia su una variazione del
valore di ingresso.
- PWM: (pin 3, 5, 6, 9, 10 e 11). Sono pin utilizzati come uscite PWM.
- LED: (pin 13). Quando il valore del pin è alto il LED corrispondente è acceso,
viceversa, è spento.
- Reset. É un pin che permette di resettare il microcontrollore. La stessa
funzione di reset risulta possibile anche attraverso un pulsante situato sulla
scheda.
Inoltre Arduino duemilanove dispone di 6 ingressi analogici, ciascuno dei quali
fornisce 10 bit di risoluzione (ossia 1024 diversi valori).
3.1.5 Comunicazione
L' Arduino Duemilanove ha un certo numero di strutture per la comunicazione con
un computer, con un altro Arduino, o con altri microcontrollori. L'Atmega-328
fornisce una comunicazione seriale UART TTL (5V) che é disponibile sui pin digitali 0
(RX) e 1 (TX). Il software predisposto alla comunicazione include un monitor Arduino
seriale che consente di inviare alla scheda Arduino semplici dati testuali. I led RX e
TX sulla scheda lampeggiano quando i dati vengono trasmessi sia tramite il chip
FTDI, sia tramite una connessione USB con il computer; ciò non accade per la
comunicazione seriale. L'Atmega-328 dispone anche di un supporto I2C (TWI) e di
una comunicazione SPI. Il software include una libreria Arduino Wire per
semplificare l'utilizzo del I2C bus.
3.1.6 Programmazione
La scheda Duemilanove può essere programmata con il software Arduino, di cui si
approfondiranno le funzionalità nell'ultimo paragrafo di questo capitolo. L'Atmega-
328 su Arduino Duemilanove si precarica con un bootloader che permette di caricare
il nuovo codice senza l'uso di un programmatore hardware esterno.
La comunicazione con la scheda avviene utilizzando l'originale protocollo STK500
che ha come riferimento i file di intestazione C.
3.1.7 USB Protezione da sovracorrente
L'Arduino Duemilanove ha un polyfuse azzerabile (componente di memoria tempo-
programmabile usato in circuiti a semiconduttori per la memorizzazione dei dati
unici), che protegge le porte USB del computer da cortocircuiti e sovracorrenti.
Sebbene la maggior parte dei computer fornisce una propria protezione interna, il
fusibile fornisce un ulteriore livello di protezione. Se più di 500mA vengono applicati
alla porta USB, il fusibile automaticamente interrompe la connessione fino a quando
il corto o sovraccarico viene rimosso.
3.1.8 Caratteristiche fisiche
La lunghezza e la larghezza massima del PCB duemilanove sono 2,7 * 2:1 pollici, e,
con il connettore USB e il jack di alimentazione, si estende oltre la dimensione
precedente. Tre fori per le viti consentono alla scheda di essere fissata su di una
superficie.
3.1.9 Tabella di sintesi
La sintesi di quanto suddetto è mostrata nella tabella seguente:
3.2 Sensori
3.2.1 Sensori a Infrarossi (IR)
su questo robot sono una coppia di sensori ad infrarossi (IR) del tipo Sharp 2D120X ,
riportati nella figura seguente:
Sono stati scelti questi sensori per il buon rapporto qualità/prezzo, in quanto con
soli 13€ si è riusciti ad avere un sensore molto preciso in un range di misura (4-30
cm) che include ampiamente quello di interesse.
Questo sensore è molto popolare nelle applicazioni di robotica amatoriale, è
costruito dalla Sharp e produce un’uscita analogica che varia da 3.1V a 4cm fino a
0.3V a 30cm. La formula per tradurre il valore del sensore (SensorValue) nella
corrispondente distanza (Distance) è la seguente (la formula è valida solo per un
SensorValue compreso tra 80 e 530):
Nella tabella seguente sono riportate le caratteristiche principali del sensore in
esame:
3.2.2 Sensore a Ultrasuoni
Oltre ai due sensori ad infrarossi, per la retroazione di posizione del nostro robot
abbiamo scelto un sensore ad ultrasuoni del tipo Devantech SRF05 come quello in
figura:
L'SRF05 è un sensore ad ultrasuoni che unisce delle ottime prestazioni ad un costo
veramente conveniente. Questo sensore e' dotato di un microcontrollore che
assolve tutte le funzioni di calcolo ed elaborazione, è sufficiente inviare un impulso e
leggere l'eco di ritorno per stabilire, con facilità, la distanza dell'ostacolo o
dell'oggetto che si trova davanti. Riesce ad individuare ostacoli ed oggetti, anche di
piccole dimensioni, da 1cm fino a 4mt. Come tutti i sensori di questo tipo, e a
differenza dei sensori IR, è insensibile alla luce ambientale, quindi e' ottimo per
essere usato anche all'esterno.
Nella tabella seguente sono riportate le caratteristiche principali del sensore in
esame:
3.3 Attuatori
Per quanto riguarda gli attuatori, sono stati scelti due servo standard Zebra ZS-S1113
come quelli riportati nella figura seguente:
Come la maggior parte dei servomotori standard la rotazione di questi piccoli motori
è limitata ad angoli tra 0° e 180°. Per la nostra applicazione questa limitazione è
logicamente inaccettabile dovendo dedicare i servo alla movimentazione delle ruote
del robot. Per ovviare a questo problema i servo sono stati modificati sia
meccanicamente che elettricamente, in questo modo si sono ottenuti due servo a
rotazione continua.
I test effettuati in laboratorio da alcuni colleghi dimostrano che questi servomotori
sopportano alti assorbimenti senza rischio di surriscaldamento fino a .
Alimentati a questo voltaggio producono una coppia di .
Le caratteristiche più importanti, che ci hanno portato a scegliere questi servo,
possono essere così riassunte:
Dimensioni: 39,8mm x 19,8mm x 36,5mm
Peso: 48,5 g
Coppia 3,0kg - 3,8kg * cm a 4,8 - 6 Volt
Velocità sec/60 a (4,8 - 6 volt) 0,21 - 0,18 (circa 55 rpm a 6V).
I servo vengono collegati ad Arduino nel modo seguente:
Per quanto riguarda la modalità di controllo di questi servo, se essi non avessero
subito le modifiche di cui si è parlato precedentemente si potrebbero controllare in
posizione, ovvero con delle semplici istruzioni gli si può far descrivere un qualunque
angolo tra 0° e 180° . Invece in seguito alle modifiche da noi apportate il servo è
controllabile solo in velocità e non più in posizione. In particolare, il comando per
controllare il servo è il seguente:
servo.write(value)
- servo sta ad indicare un oggetto della classe Servo, che deve essere
preventivamente definito;
- value un valore compreso nell’intervallo A 0 corrisponde la massima
velocità in un verso e a 180 la velocità massima nel verso opposto
(supponendo che la funzione di trasferimento del motore sia lineare, si
intuisce che scrivendo servo.write(90) il servo si ferma).
In sintesi il comportamento del servo, può essere descritto dalla seguente
caratteristica ingresso/uscita:
3.4 Altri componenti
Oltre a quelli già elencati, sono stati utilizzati anche altri componenti, di seguito ne è
riportato un elenco:
- 4 Batterie AA da 1.5V (con relativo case portapile e connettore per collegare
le pile all’Arduino): con queste pile si sono alimentati i due servomotori;
- 1 Batteria da 9V (con relativo connettore ed interruttore): con essa si è fornita
l’alimentazione all’Arduino, tramite il pin (di cui al paragrafo 3.1.2);
- Piattaforma in PVC;
- Elementi vari per il fissaggio sulla piattaforma in PVC dei vari componenti;
- Cavi vari di collegamento, breadboard da 400 punti, 1 interruttore
- Ruote in plastica di raggio 4 cm
3.5 Immagini del sistema fisico in esame
Di seguito sono riportate alcune immagini significative del robot in esame:
4. Algoritmi di Controllo In questo paragrafo verranno analizzati gli algoritmi di controllo che sono stati
utilizzati per implementare quanto detto nei paragrafi precedenti. Gli algoritmi
utilizzati sono 3 e verranno esaminati in quest’ordine:
- PID (Proporzionale - Integrale – Derivativo)
- Allocazione dei Poli
- LQR (Linear-Quadratic-Regulator)
Prima di scendere nel dettaglio delle varie tecniche di controllo è bene introdurre
l’ambiente di sviluppo utilizzato e i programmi di base che ricorrono in ogni
algoritmo, per evitare di ripeterli nella trattazione di ognuno di essi.
4.1 Programmi in MATLAB/SIMULINK
Per la simulazione del processo finora descritto è stato utilizzato il software
MATLAB/SIMULINK. Abbiamo definito le matrici del sistema tramite il codice
MATLAB riportato in Appendice.
Di seguito è riportato lo schema SIMULINK del sistema a ciclo chiuso:
Si noti che la retroazione realizzata è una retroazione full information ovvero una
retroazione dello stato. Questo schema è quindi adatto alla sintesi del controllore
con tecnica Poles Placement ed LQR. Per quanto riguarda la sintesi del controllore
PID, basta sostituire la suddetta retroazione dello stato con una di uscita (variabile
). Di seguito sono riportati due schemi che esemplificano rispettivamente la
retroazione di uscita e la retroazione di stato (full information):
Innanzitutto analizziamo le caratteristiche principali del sistema in esame con
retroazione di stato, in particolare di seguito è riportata la quadrupla
(Am,Bm,Cm,Dm), che rappresenta il sistema motore + pendolo:
Innanzitutto si deve notare che la matrice presenta un polo instabile, infatti
digitando il comando eig( in MATLAB si ha:
Quindi si giunge alla conclusione, logica, che il punto di equilibrio theta=0°è un
punto di equilibrio instabile. Inoltre l’autovalore nullo rappresenta la possibilità di
avere una posizione di equilibrio per ogni .
Invece per testare l’osservabilità del sistema abbiamo utilizzato la cosiddetta
matrice di osservabilità definita come segue:
analizzandone il rango si trova che questa matrice è a rango pieno e quindi il nostro
sistema è osservabile. Un’alternativa a questa operazione sarebbe quella di
utilizzare direttamente il comando MATLAB:
In maniera speculare, per testare la controllabilità del sistema bisogna analizzare la
cosiddetta matrice di controllabilità, definita come segue:
analizzandone il rango si trova che questa matrice è a rango pieno e quindi il nostro
sistema è controllabile. Analogamente al caso precedente, un’alternativa a questa
operazione sarebbe quella di utilizzare direttamente il comando MATLAB
ctrb(Am,Bm).
Il codice MATLAB utilizzato per il funzionamento degli algoritmi seguenti è riportato
nell’Appendice.
4.2 PID
Questo tipo di regolatore prevede un controllo proporzionale all’errore rispetto ad
un riferimento, derivativo ed integrale. L’effetto del controllo proporzionale è quello
di contrastare l’errore, quello del derivativo è quello di anticipare la risposta del
sistema, quello dell’integrale di annullare l’errore a regime. Questo si traduce nel
calcolare i coefficienti del regolatore in modo tale che il sistema controllato abbia
tutti i poli a parte reale minore di zero; lavorando sulle funzioni di trasferimento
occorre quindi aggiungere al sistema non controllato la funzione di trasferimento del
regolatore PID e studiare la stabilità del sistema in anello chiuso. La funzione di
trasferimento del regolatore PID è:
La scelta dei coefficienti del regolatore, come spesso avviene in questi casi, è stata
fatta per lo più basandosi su basi sperimentali. Seguendo questa strada si è arrivati a
determinare i seguenti coefficienti:
Come già accennato precedentemente questo tipo di controllore richiede soltanto
una retroazione di uscita (e non di stato come quella richiesta dalle due tecniche
successive), quindi si è modificato opportunamente lo schema SIMULINK riportato
precedentemente. Si sono quindi inseriti i valori di e trovati
precedentemente e si è arrivati al comportamento descritto dalla figura seguente:
Questo grafico descrive l’andamento dell’angolo per una simulazione di 100s con
La cosa che inizialmente ci ha lasciati un po’ perplessi è l’andamento
oscillante dell’angolo. Questa cosa può essere spiegata, a nostro parere, analizzando
la funzione di trasferimento a ciclo chiuso del sistema complessivo. Per arrivare
all’espressione della fdt a ciclo chiuso, bisogna passare prima per la struttura della
fdt del solo processo, essa può essere indicata nel modo seguente:
Quindi la funzione di trasferimento dell’anello chiuso è data da:
Analizzando la struttura di quest’ultima, si nota che, comunque si scelgano i
coefficienti del regolatore, c’è sempre un polo nell’origine che non è possibile
spostare e quindi a questo è dovuto il carattere oscillante, precedentemente
indicato dell’andamento della variabile .
Un ulteriore concetto da sottolineare è la scelta dell’algoritmo realmente
implementato sul microcontrollore Arduino. Come si legge nella maggior parte dei
libri di controlli automatici la ritmica di funzionamento dell’algoritmo è a tempo
costante. Al contrario, in questo progetto è stata adottata una tecnica leggermente
diversa. Infatti invece di avere un tempo costante e scartare il tempo avanzato
aspettando il prossimo turno, è stato semplicemente cronometrato il tempo
necessario per ogni loop e le azioni integrale e derivativa sono state rese
proporzionali a questo intervallo di tempo. Quindi, con questa tecnica, se
un'acquisizione è durata più di un'altra, avrà più peso nella correzione generata dal
controllore. Per capire meglio quanto appena esposto bisogna rifarsi al codice
presente nell’Appendice, precisamente al paragrafo A.2.
4.3 Allocazione dei poli
Questa tecnica prevede l’allocazione dei poli a ciclo chiuso mediante una
retroazione lineare di tutto lo stato, per questo, come già accennato
precedentemente, si dice che questo è un tipo di controllo full-information ed il
controllore risultante sarà del tipo:
Dove è un vettore riga di dimensione 1x4 essendo 4 gli stati presi in
considerazione nel modello matematico.
Da notare che per sistemi multivariabile la soluzione analitica del problema suddetto
non esiste. In alternativa si può allocare, tramite LMI, i poli del sistema a ciclo chiuso
in una zona del piano complesso, come ad esempio a sinistra della retta Re{s}=-α :
MATLAB offre inoltre la funzione k=place(A, B, p) che cerca di posizionare, entro
certe tolleranze, i poli di (A-Bk) in corrispondenza di p.
Dove è un vettore riga di dimensione 1x4 essendo gli stati 4.
4.3.1 Risultati dell’Allocazione dei poli
Di seguito sono riportati graficamente i risultati ottenuti per i seguenti valori del
vettore
ottenuti allocando i poli in: .
Il primo grafico descrive l’andamento dell’angolo :
Il secondo grafico descrive l’andamento della variabile di controllo :
Come si può notare dal primo grafico, l’angolo partendo da una condizione iniziale
di 2° si porta in circa 11s a 0° (valore di riferimento). In seguito all’istante il
robot è sottoposto a un disturbo di 1°, al quale riesce a resistere, senza andare
quindi in instabilità e in circa 11s si riporta in 0°.
Dal 2° grafico si nota che la variabile di controllo ha un andamento abbastanza
coerente con quanto appena descritto riguardo il movimento del robot.
4.4 Linear-Quadratic-Regulator (LQR)
La tecnica LQR consiste nel trovare una legge di controllo in forma di retroazione di
stato tale da minimizzare un indice di costo nella forma:
Con :
Q ed R sono due matrici che hanno i seguenti significati:
- Q rappresenta la matrice di peso dello stato, e spesso viene assunta come
matrice diagonale per semplificare i calcoli. Manipolando accuratamente i
valori di questa matrice si può conferire alle uscite relative alle diverse
variabili di stato delle caratteristiche particolari. In particolare se aumento
l’elemento relativo alla variabile di stato , penalizzerò il transitorio
dell’uscita relativa a questa variabile.
- R rappresenta la matrice di peso dell’energia spesa dall’azione di controllo. In
particolare, nel progetto in esame la R risulterà 1x1 in quanto l’azione di
controllo coincide con la sola spinta data dai 2 servo (il lettore non deve
essere tratto in inganno dal fatto che i servo sono 2, in quanto è da notare che
la direzione della forza generata dai 2 servo è la stessa e per questo il risultato
sopracitato per quanto riguarda la R).
4.4.1 Risultati LQR
Di seguito sono proposti i risultati ottenuti mediante la tecnica LQR; più
precisamente sono stati analizzati due comportamenti della variabile θ in relazione
alla scelta delle matrici di peso Q e R.
Il primo comportamento è legato alla scelta seguente delle matrici di peso Q ed R:
=
I valori di utili alla realizzazione della legge di controllo ottimo risultano essere:
Di seguito sono mostrati rispettivamente i grafici relativi all’andamento della
variabile θ e quello della variabile di controllo con i valori di espressi
precedentemente.
Variabile θ:
Variabile di controllo:
Da quanto mostrato dai grafici è possibile notare come l‘angolo θ partendo da una
posizione di circa 2° in meno di 5’’ si stabilizza portandosi cosi nella posizione di
equilibrio (0°). Dopo circa 40’’ il robot viene sottoposto ad un disturbo di circa 1° che
non viene reiettato, anzi si ha un’amplificazione con un picco di -1,5°.
La variabile di controllo mostrata nel secondo grafico ha un andamento che
rispecchia l’azione da far eseguire ai motori per riportare la variabile θ nella
posizione di equilibrio.
Nella seconda prova della tecnica LQR si può notare, facilmente, come una scelta
diversa della matrice di peso Q incida molto sull’andamento delle varibili e .
Il secondo comportamento è legato alla scelta della matrice di peso R uguale al caso
precedente, e ad una matrice Q siffatta:
I valori di utili alla realizzazione della legge di controllo ottimo risultano essere:
Di seguito sono mostrati rispettivamente i grafici relativi all’andamento della
variabile θ e alla variabile di controllo con i valori di espressi precedentemente:
Variabile θ:
Variabile u:
I grafici mostrati evidenziano come le variabili θ e u, ottenute scegliendo la matrice
di peso Q con valori molto grandi (dell’ordine di 107) relativi alle variabili θ e ,
assumano un andamento molto diverso da quanto visto nel primo caso.
La variabile θ partendo da una posizione di 2° dopo circa 5’’ riesce a portarsi
nell’intorno del punto di equilibrio oscillando continuamente senza mai assestarsi. Il
disturbo (circa 5°) a cui viene sottoposto il robot viene reiettato abbastanza bene
tant’è che dopo pochissimi secondi (1s ca.) la variabile ritorna ad oscillare
nell’intorno del punto di equilibrio. Questo comportamento è legato alla scelta della
matrice di peso Q , in quanto avendo scelto il corrispettivo valore molto grande, è
stato fortemente penalizzato il transitorio relativo a .
La variabile assume un andamento molto inconsueto portandosi sempre in
saturazione. Questo si può spiegare considerando il fatto che la variabile non si
assesta mai, quindi l’azione di controllo è continua; inoltre avendo progettato il
controllore in modo da reiettare i disturbi nel modo più efficace possibile, la
variabile di controllo viene sfruttata al massimo raggiungendo così la saturazione.
C’è da considerare però che nel robot realizzato questo effetto si nota poco a causa
della mancata modellazione del tempo di azionamento dei motori: nel grafico gli
azionamenti risultano essere impulsivi, ma fisicamente non è possibile realizzare ciò
perché per ottenere un azionamento è necessario un tempo minimo in cui il
servomotore deve essere controllato (questo tempo minimo è dipendente dal tipo
di servo impiegato).
5 Conclusioni In conclusione, mettendo a confronto le tre tecniche utilizzate per raggiungere il
nostro obiettivo di auto-bilanciamento del robot, abbiamo evinto i seguenti risultati:
Con il controllo PID, non si è riusciti a portare il sistema all’asintotica stabilità.
Il vantaggio di aver usato questa tecnica è l’utilizzo di un solo sensore per la
retroazione dell’uscita.
L’allocazione dei poli ha prodotto risultati piuttosto soddisfacenti. Innanzitutto
però c’è stato bisogno di due sensori, ed inoltre c’è da considerare che non è
stato semplice scegliere i poli del sistema a ciclo chiuso che realizzassero le
performance migliori.
Nella tecnica LQR, come nel caso dell’allocazioni dei poli, abbiamo è stato
richiesto l’utilizzo di due sensori. Al contrario però è stato più semplice
tradurre le specifiche imposte. Effettuando un trade-off tra le soluzioni LQR
sarebbe possibile quindi mediare tra le due e conferire al nostro robot sia un
breve transitorio che una buona reiezione ai disturbi.
Appendice
A.1 Codice principale MATLAB
Il codice riportato di seguito è quello principale utilizzato per determinare le varie
matrici del sistema e il sistema a ciclo chiuso con i controllori realizzati,
rispettivamente, con tecnica LQR e con l’allocazione dei poli:
function [sys,A,B,C,D,k,kd,Am,Bm]=pendolo1()
m_tot = 0.533; %massa totale m_pndl = 0.15; %massa totale - mase(motori+ruote) l = 0.08; %lunghezza pendolo 30cm g = 9.81; %accelerazione di gravità c = 0.35; %forza d'attrito gomma-legno
%Scrittura delle matrici V = [1 0 0 0; 0 1 0 0; 0 0 m_pndl*l^2 m_pndl*l; 0 0 m_pndl*l m_tot];
A_ = [0 0 1 0; 0 0 0 1; m_pndl*g*l 0 0 0; 0 0 0 -c]; B_ = [0;
0; 0; 1]; %Come descritto al paragrafo 2.2 si trovano le matrici A,B,C
A = (V^-1)*A_; B = (V^-1)*B_; C = [1 0 0 0; 0 1 0 0]; D = [0; 0]; %Funzione di trasferimento del sistema sist = ss(A,B,C,D); %Funzione di trasferimento del motore (guadagno) motore = 2*pi*0.04*55.5555/(90*60); %Funzione di trasferimento del sistema complessivo (motore*sistema) sys=motore*sist; [Am,Bm,Cm,Dm]=ssdata(sys); %Provare se il sistema è osservabile disp('osservabilità') N=[Cm' Am'*Cm' (Am')^2*Cm' (Am')^3*Cm']; rank(N); %Allocazione dei poli P=[-30 -15 -2 -0.01]; k=place(Am,Bm,P); %LQR Q=diag([20000000 1 20000000 1]); %La matrice Q scelta in questo modo è quella %che penalizza il transitorio delle variabili con peso maggiore. %Per avere una minore penalizzazione delle stesse, %si può scegliere una matrice del genere: %Q=diag([100 1 100 1]); R=6; kd=lqr(sys,Q,R); %Autovalori del sistema a ciclo chiuso, con controllore realizzato con %l'allocazione dei poli eig(Am-Bm*k) %Autovalori del sistema a ciclo chiuso, con controllore realizzato con %l'allocazione dei poli eig(Am-Bm*kd) end
A.2 Codice Arduino PID
Di seguito è riportato il codice utilizzato per implementare l’algoritmo PID esposto al
paragrafo 4.2, direttamente sul microcontrollore Arduino:
#include <Servo.h> //Libreria utilizzata per pilotare i servomotori
Servo left; //Definizione di un oggetto servo
Servo right; //Definizione di un oggetto servo
float time = 0;
#define servoLeftPin 9
#define servoRightPin 10
#define servoZero 90 // calibrazione per servomotori
#define IRFront 0
#define IRBack 1
int frontSense = 0; // sensore ir anteriore
int backSense = 0; // sensore ir posteriore
int orientation = 0;
int previousOrientation = 0;
float P = 0;
float I = 0;
float D = 0;
int fall = 0; //variabile utilizzata per determinare se il robot è caduto //Coefficienti del regolatore//////////
float kP = 12;
float kI =430;
float kD =20; ///////////////////////
void setup() {
pinMode(ledPin, OUTPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}
void getOrientation() {
frontSense = 0;
backSense = 0;
orientation = 0;
for (int i = 0; i < 10; i++) {
frontSense = analogRead(IRFront) + frontSense;
backSense = analogRead(IRBack) + backSense;
if (i == 9) {
frontSense = frontSense / 10;
backSense = backSense / 10;
orientation = frontSense - backSense;
Serial.println(orientation);
}
}
} /*Implementazione dell’autogetup()*/
void equilibrio(int pid) {
left.write(servoZero);
right.write(servoZero);
delay(100);
left.write(90 + pid);
right.write(90 -pid);
}
void loop() {
getOrientation();
float previousTime = time;
time = millis();
float interval = time - previousTime;
P = orientation * kP;
I = I + interval*kI*P;
D = kD*(orientation - previousOrientation) / interval;
float PID = P + I + D;
if(PID > 90) PID = 90; //stop increase or decrease of the value
if(PID < -90) PID = -90; //costrizione del valore tra 90 e -90
if(orientation > 300) fall = fall + 1;
if(orientation < -300) fall = fall - 1;
if(PID <= 1 && PID > 0) PID = 0;
if(PID >= -1 && PID < 0) PID = 0;
while(orientation < 200 && orientation > -200){
I=0;
float PID = P + I + D;
equilibrio(PID);
getOrientation();
}
left.write(90 + PID);
right.write(90 - PID);
if (fall == 25 || fall == -25) {
left.write(servoZero);
right.write(servoZero);
delay(250);
fall = 0;
I=0;
}
previousOrientation = orientation;
}
A.3 Codice Arduino Allocazione dei poli
#include <Servo.h>\\ Libreria utilizzata per pilotare servi
#include <math.h>
Servo left;
Servo right;
float time = 0;
#define servoLeftPin 9
#define servoRightPin 10
#define servoZero 90 // Calibrazione per servi
#define IRFront 0
#define IRBack 1
#define trigger 2
#define echo 3
int frontSense = 0; //forward ir sensor - sensore ir anteriore
int backSense = 0; //backward ir sensor - sensore ir posteriore
float orientation = 0;
float previousOrientation = 0;
float previousmisura = 0;
float misura=0;
float distance=0;
int P = 0;
void setup() {
pinMode(trigger, OUTPUT);
pinMode(echo, INPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}
void getOrientation() {
frontSense = 0;
backSense = 0;
orientation = 0;
for (int i = 0; i < 10; i++) {
frontSense = analogRead(IRFront) + frontSense;
backSense = analogRead(IRBack) + backSense;
if (i == 9) {
frontSense = frontSense / 10;
backSense = backSense / 10;
float frontdist = 2076.0/(frontSense-11.0);
float backdist = 2076.0/(backSense-11.0);
orientation = atan((frontdist-backdist)/24); //misurare la distanza tra gli sharp
}
}
}
void getPosition(){
digitalWrite(trigger, HIGH);
delayMicroseconds(10);
digitalWrite(trigger, LOW);
long pulse_lenght = pulseIn(echo,HIGH);
delay(50);
distance = ((pulse_lenght/58)-36);
Serial.println("Distance");
Serial.println(distance);
}
void loop() {
getOrientation();
getPosition();
misura=distance*cos(orientation);
Serial.println(misura);
float previousTime = time;
time = millis();
float interval = time - previousTime;
float tetapunto = (orientation - previousOrientation)/interval;
float vel = (misura - previousmisura)/interval;
float k1= -8427.6 * orientation;
float k2= -10.9 * misura;
float k3= -644.6 * tetapunto;
float k4= -1229.0 *vel;
P = k1+k2+k3+k4;
if(P > 90) P = 90; //stop increase or decrease of the value
if(P < -90) P = -90; //costrizione del valore tra 90 e -90
left.write(90 + P);
right.write(90 - P);
previousOrientation = orientation;
previousmisura = misura;}
A.4 Codice Arduino LQR
#include <Servo.h>\\ Libreria utilizzata per pilotare servi
#include <math.h>
Servo left;
Servo right;
float time = 0;
#define servoLeftPin 9
#define servoRightPin 10
#define servoZero 90 // Calibrazione per servi
#define IRFront 0
#define IRBack 1
#define trigger 2
#define echo 3
int frontSense = 0; //forward ir sensor - sensore ir anteriore
int backSense = 0; //backward ir sensor - sensore ir posteriore
float orientation = 0;
float previousOrientation = 0;
float previousmisura = 0;
float misura=0;
float distance=0;
int P = 0;
void setup() {
pinMode(trigger, OUTPUT);
pinMode(echo, INPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}
void getOrientation() {
frontSense = 0;
backSense = 0;
orientation = 0;
for (int i = 0; i < 10; i++) {
frontSense = analogRead(IRFront) + frontSense;
backSense = analogRead(IRBack) + backSense;
if (i == 9) {
frontSense = frontSense / 10;
backSense = backSense / 10;
float frontdist = 2076.0/(frontSense-11.0);
float backdist = 2076.0/(backSense-11.0);
orientation = atan((frontdist-backdist)/24); //misurare la distanza tra gli sharp
}
}
}
void getPosition(){
digitalWrite(trigger, HIGH);
delayMicroseconds(10);
digitalWrite(trigger, LOW);
long pulse_lenght = pulseIn(echo,HIGH);
delay(50);
distance = ((pulse_lenght/58)-36);
Serial.println("Distance");
Serial.println(distance);
}
void loop() {
getOrientation();
getPosition();
misura=distance*cos(orientation);
Serial.println(misura);
float previousTime = time;
time = millis();
float interval = time - previousTime;
float tetapunto = (orientation - previousOrientation)/interval;
float vel = (misura - previousmisura)/interval;
float k1= -8427.6 * orientation;
float k2= -10.9 * misura;
float k3= -644.6 * tetapunto;
float k4= -1229.0 *vel;
P = k1+k2+k3+k4;
if(P > 90) P = 90; //stop increase or decrease of the value
if(P < -90) P = -90; //costrizione del valore tra 90 e -90
left.write(90 + P);
right.write(90 - P);
previousOrientation = orientation;
previousmisura = misura;}
top related