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 ruote. 2.1 Modello non lineare parallela alle due 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 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 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 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 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 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 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 10 7 ) 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;