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 dellAllocazione 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. Lobiettivo 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 nellIDE Arduino, per una controprova realistica di quanto ottenuto.

2. Sistema Fisico
Come gi evidenziato nellintroduzione, il comportamento del nostro robot pu essere esemplificato con quello di un pendolo inverso, schematizzato nella figura seguente:

Con: coordinata libera di spostamento dellasta dalla verticale, positiva se oraria; lo scostamento del carrello dalla posizione iniziale.

Come si pu notare, il sistema composto da unasta di massa trascurabile e lunghezza , incernierata alla base a un carrello di massa e libero di scorrere su un piano di appoggio. In cima allasta 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 dellespressione dellenergia potenziale U e dellenergia 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:

Lequazione espressa in forma matriciale la seguente:

Moltiplicando lequazione a sinistra entrambi i membri per linverso 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 opensource 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 2050KW e 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'Atmega328 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 tempoprogrammabile 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 unuscita 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 nellintervallo 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 allArduino): con queste pile si sono alimentati i due servomotori; - 1 Batteria da 9V (con relativo connettore ed interruttore): con essa si fornita lalimentazione allArduino, 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 questordine: - PID (Proporzionale - Integrale Derivativo) - Allocazione dei Poli - LQR (Linear-Quadratic-Regulator) Prima di scendere nel dettaglio delle varie tecniche di controllo bene introdurre lambiente 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 digitando il comando eig( in MATLAB si ha:

presenta un polo instabile, infatti

Quindi si giunge alla conclusione, logica, che il punto di equilibrio theta=0 un punto di equilibrio instabile. Inoltre lautovalore nullo rappresenta la possibilit di avere una posizione di equilibrio per ogni . Invece per testare losservabilit 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. Unalternativa 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, unalternativa 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 nellAppendice.

4.2 PID
Questo tipo di regolatore prevede un controllo proporzionale allerrore rispetto ad un riferimento, derivativo ed integrale. Leffetto del controllo proporzionale quello di contrastare lerrore, quello del derivativo quello di anticipare la risposta de l sistema, quello dellintegrale di annullare lerrore 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 landamento dellangolo per una simulazione di 100s con La cosa che inizialmente ci ha lasciati un po perplessi landamento oscillante dellangolo. Questa cosa pu essere spiegata, a nostro parere, analizzando la funzione di trasferimento a ciclo chiuso del sistema complessivo. Per arrivare

allespressione 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 dellanello chiuso data da:

Analizzando la struttura di questultima, si nota che, comunque si scelgano i coefficienti del regolatore, c sempre un polo nellorigine che non possibile spostare e quindi a questo dovuto il carattere oscillante, precedentemente indicato dellandamento della variabile . Un ulteriore concetto da sottolineare la scelta dellalgoritmo realmente implementato sul microcontrollore Arduino. Come si legge nella maggior parte dei libri di controlli automatici la ritmica di funzionamento dellalgoritmo 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 nellAppendice, precisamente al paragrafo A.2.

4.3 Allocazione dei poli

Questa tecnica prevede lallocazione 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 dellAllocazione 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 landamento dellangolo

. :

Il secondo grafico descrive landamento della variabile di controllo :

Come si pu notare dal primo grafico, langolo partendo da una condizione iniziale di 2 si porta in circa 11s a 0 (valore di riferimento). In seguito allistante 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 lelemento relativo alla variabile di stato , penalizzer il transitorio delluscita relativa a questa variabile. - R rappresenta la matrice di peso dellenergia spesa dallazione d i controllo. In particolare, nel progetto in esame la R risulter 1x1 in quanto lazione 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 allandamento 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 langolo 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 unamplificazione con un picco di -1,5. La variabile di controllo mostrata nel secondo grafico ha un andamento che rispecchia lazione 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 sullandamento 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 allandamento 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 (dellordine 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 nellintorno 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 nellintorno 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 lazione 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 allasintotica stabilit. Il vantaggio di aver usato questa tecnica lutilizzo di un solo sensore per la retroazione delluscita. Lallocazione dei poli ha prodotto risultati piuttosto soddisfa centi. 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 dellallocazioni dei poli, abbiamo stato richiesto lutilizzo 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 lallocazione 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 lalgoritmo 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

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 dellautogetup()*/

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;}