SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio...

33
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)

Transcript of SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio...

Page 1: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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)

Page 2: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 3: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 4: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 5: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 6: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 7: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 8: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

- 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

Page 9: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 10: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 11: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 12: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 13: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 14: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 15: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

). 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:

Page 16: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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 è:

Page 17: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 18: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 19: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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 :

Page 20: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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.

Page 21: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 22: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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:

Page 23: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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).

Page 24: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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;

Page 25: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 26: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 27: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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

Page 28: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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;

Page 29: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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");

Page 30: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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;

Page 31: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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;

Page 32: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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;

Page 33: SISTEMI DI CONTROLLO MULTIVARIABILE - Fabrizio Schianoschianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf · 3.1 Piattaforma Hardware Arduino Duemilanove ... A.1

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