Università degli Studi di Siena. Modellizzazione e Controllo Predittivo Ibrido e Decentralizzato di Una Formazione di Velivoli di Tipo Quadcopter

Transcript

1 Università degli Studi di Siena Facoltà di Ingegneria Corso di Laurea Specialistica in Ingegneria Informatica Modellizzazione e Controllo Predittivo Ibrido e Decentralizzato di Una Formazione di Velivoli di Tipo Quadcopter Tesi di Laurea di Claudio Rocchi Relatore: Prof. Ing. Antonello Giannitrapani Correlatore: Prof. Ing. Alberto Bemporad Anno Accademico 2008/2009 Sessione di Laurea - 8 Febbraio 2010

2 Indice Introduzione 1 1 Sistemi ibridi I modelli ibridi DHA - Discrete Hybrid Automata Sistemi MLD Calcolo proposizionale e programmazione mista intera Sistemi MLD Sistemi PWA Controllo predittivo Controllo predittivo in-linea MPC per sistemi ibridi Convergenza ad anello chiuso Risolutori di problemi misti interi Forma esplicita del controllore Modello di volo Dinamica non lineare del quadcopter Gestione di una formazione di velivoli Progetto delle strategie di controllo MPC lineare per la stabilizzazione i

3 INDICE ii 4.2 Pianificazione della traiettoria per l elusione di ostacoli MPC ibrido Generazione dei vincoli con il metodo dei potenziali Controllo di una formazione di velivoli Struttura gerarchica Schema decentralizzato Schema centralizzato MPC lineare con generazione dei vincoli Risultati delle simulazioni Ostacoli mobili Conclusioni 59 A HYSDEL 66 A.1 Il linguaggio HYSDEL A.1.1 Sezione AUX A.1.2 Sezioni AD e DA A.1.3 Sezione LOGIC A.1.4 Sezione CONTINUOUS A.1.5 Sezione LINEAR A.1.6 Sezione AUTOMATA A.1.7 Sezione OUTPUT A.1.8 Sezione MUST A.2 Listato HYSDEL del velivolo leader Bibliografia 77

4 Elenco delle figure 1.1 Modello di un sistema ibrido Schema di un discrete hybrid automata (DHA) Partizione poliedrica dello spazio degli stati Schema di controllo retroazionato Filosofia ad orizzonte recessivo Il prototipo costruito da Carlo Alberto Pascucci al Dipartimento di Ingegneria dell Informazione di Siena Modello del quadcopter Struttura della formazione Azione integrale sulla z durante il transitorio iniziale Struttura gerarchica Esempio di guscio che avvolge un ostacolo Blocco Simulink del controllo decentralizzato Traiettorie della formazione, strategia ibrida Posizioni e segnali di riferimento del velivolo leader, strategia ibrida Traiettorie della formazione, metodo dei potenziali Posizioni e segnali di riferimento del primo follower, metodo dei potenziali iii

5 Elenco delle tabelle 1.1 Proposizioni logiche e forme equivalenti Trasformazione prop. logiche in disuguaglianze intere Parametri del quadcopter iv

6 Introduzione Negli ultimi anni il controllo di formazioni di velivoli senza pilota (Unmanned Aerial Vehicles - UAVs) è diventato un area di ricerca molto attiva, grazie alla grande varietà di situazioni nelle quali possono essere utilizzati al posto degli esseri umani, sia per applicazioni militari (spionaggio, ricognizione, sorveglianza) che civili (esplorazioni di vasti territori, di ambienti ostili, ecc.). Esistono diverse tipologie di UAVs, ognuna adatta ad un particolare scopo: alianti e aeroplani hanno un alta autonomia e possono essere utilizzati in applicazioni di lungo raggio, ma richiedono ampi spazi di manovra. Gli elicotteri, ed in particolare i quadcopter (elicotteri a quattro rotori), sono meno efficienti dal punto di vista energetico, ma possono operare in spazi limitati, atterrare e decollare verticalmente, e rimanere sospesi in aria sopra l obiettivo. La navigazione in spazi ristretti richiede però una maggiore complessità nel controllare il velivolo [1], soprattutto a causa della dinamica accoppiata e fortemente non lineare, e delle limitazioni sugli attuatori e sugli angoli di pitch/roll. Diverse strategie di controllo sono state proposte in letteratura, oltre al classico PID, per la stabilizzazione dei quadcopter: controllo non lineare [2], LQR [3], visual feedback [1], H [4]. Un altra sfida è rappresentata dalla pianificazione della traiettoria (path planning) per l elusione di ostacoli (obstacle avoidance), per la quale sono state proposte diverse soluzioni: potential fields [5], A con grafi di visibilità [6,7], generazione non lineare di traiettorie (ad esempio il software NTG sviluppato al Caltech [8]), e programmazione lineare mista-intera (MILP) [9]. In partico-

7 Introduzione 2 lare l ultima ha mostrato la grande flessibilità dell ottimizzazione mista-intera in linea nella generazione in tempo reale della traiettoria dei velivoli, come riportato in [10], dove le tecniche di MILP in linea si sono dimostrate molto efficaci nel gestire problemi di risoluzione di conflitti tra diversi velivoli. In questo lavoro di tesi viene utilizzato l approccio proposto nell articolo [11] scritto in collaborazione con il prof. Alberto Bemporad e il collega Carlo Alberto Pascucci, un controllo di tipo predittivo lineare (MPC - Model Predictive Control) a due livelli: viene utilizzato un controllore MPC lineare vincolato per la stabilizzazione del quadcopter e l inseguimento di posizioni, orientazioni e velocità desiderate. Ad un livello gerarchico più alto e con un tasso di campionamento più basso, un controllore ibrido MPC genera on-line la traiettoria da seguire per raggiungere una determinata posizione di target evitando degli ostacoli lungo il percorso. La scelta dell MPC per la stabilizzazione è motivata dai rigidi vincoli hard imposti sulle coppie dei rotori e dai vincoli soft sugli angoli di pitch e roll (e sull altitudine). L MPC è infatti particolarmente adatto per il controllo di sistemi multivariabili regolati da dinamiche vincolate, poichè permette di operare nei pressi dei limiti imposti dai vincoli hard, rispetto ai più classici schemi di controllo [12]. Per gli obiettivi di questo lavoro l approccio di tipo gerarchico è stato utilizzato per realizzare il controllo di una formazione di tre velivoli di tipo quadcopter. E stato poi implementata un altra strategia di controllo di formazione, utilizzando il solo MPC lineare: tramite una funzione Matlab, date le posizioni degli ostacoli e dei velivoli, utilizzando il metodo dei potenziali, vengono generati ad ogni istante di campionamento i limiti sulle uscite controllate dall MPC che insegue una posizione di target. I due metodi verranno confrontati in termini di costi computazionali ed efficienza nell evitare ostacoli, anche mobili. In entrambi i casi il controllo di

8 Introduzione 3 formazione viene effettuato sia in modo centralizzato che decentralizzato, mostrando come quest ultima sia computazionalmente più leggera della prima, permettendo così di effettuare simulazioni con 3D Simulink. La tesi è organizzata come segue: Nel capitolo 1 viene introdotta la teoria dei sistemi ibridi, descrivendo alcuni modelli. Nel capitolo 2 descriveremo il controllo predittivo. Nel capitolo 3 viene descritta la dinamica non lineare del quadcopter e la sua estensione ad una formazione di tre velivoli, nel caso centralizzato e decentralizzato. Nel capitolo 4 presenteremo le strategie di controllo sviluppate per entrambi i casi e riporteremo le simulazioni effettuate mettendo in evidenza i risultati raggiunti. In appendice A si riporta una descrizione del linguaggio HYSDEL (HYbrid Systems DEscription Language), utilizzato per la modellizzazione ibrida dei velivoli.

9 Capitolo 1 Sistemi ibridi L analisi accurata di un qualunque fenomeno o sistema fisico passa attraverso una sua descrizione mediante il linguaggio matematico. L individuazione del sistema fisico presenta larghi margini di arbitrarietà, si intuisce infatti che le interazioni fra le parti di un sistema possono essere un numero molto elevato, e non è detto che sia ottimale cercare di rappresentarle tutte. Cercare di modellare tutte le dinamiche porta a livelli di complessità alti, di difficile gestione. Per descrivere un sistema bisogna, dunque, definire innanzitutto gli aspetti che ci interessano del suo comportamento complessivo e valutare l equilibrio ottimale, in relazione agli scopi dell analisi, tra accuratezza della rappresentazione e semplicità di funzionamento. Negli ultimi anni è cresciuta in molti campi di ricerca, dall informatica, all elettronica, al controllo di processi, la necessità di integrare i classici modelli matematici, in grado di descrivere le dinamiche continue dei sistemi, con componenti logiche, descrivibili con macchine a stati finiti o regole if-then-else. Tali sistemi descritti da interconnessioni di sistemi dinamici e dispositivi logici vengono definiti con il termine sistemi ibridi. In questo capitolo verranno inquadrati teoricamente i sistemi ibridi, illustrandone le tematiche di modellistica. Saranno presentate 4

10 1.1. I modelli ibridi 5 le diverse rappresentazioni matematiche necessarie per formalizzare modelli matematici di sistemi ibridi e saranno introdotti gli strumenti teorici che verranno utilizzati nei capitoli successivi per la sintesi del controllore di posizione per una squadra di quadcopter che fa obstacle avoidance. 1.1 I modelli ibridi Il concetto di modello di un sistema è tradizionalmente associato a equazioni differenziali ed alle differenze, tipicamente derivate da leggi fisiche che governano le dinamiche del sistema sotto studio, e la teoria e gli strumenti di controllo sono stati sviluppati per questo tipo di sistemi. D altra parte in molte applicazioni il sistema che deve essere controllato è costituito da componenti logiche, come ad esempio interruttori o selettori di velocità. Spesso per questi tipi di sistemi la progettazione del controllo è basata su regole euristiche derivanti dalla conoscenza pratica del sistema da controllare. Recentemente i ricercatori hanno iniziato a trattare i sistemi ibridi (vedi Figura 1.1), ovvero processi che evolvono secondo dinamiche continue e regole logiche, in particolare sistemi costituiti da dinamiche discrete, in genere a più alto livello, come ad esempio dispositivi elettronici, e da parti continue, in genere a più basso livello, come ad esempio motori. L importanza e l utilità dei modelli ibridi è legata al crescente impiego di controllori logici che interagiscono con dinamiche continue, in campi che variano dall automobilistico [13, 14] alle applicazioni domestiche, e alla conseguente necessità di realizzare modelli adeguati che ne permettano lo studio. Inoltre molti sistemi fisici hanno intrinsecamente una natura ibrida, o ben descrivibile come tale, come i circuiti elettrici con diodi o le reti biomolecolari [15]. Diverse classi di modelli ibridi sono state presentate in letteratura: La classe degli automi ibridi discreti, discrete hybrid automata (DHA), introdotta in [16]; questi modelli sono una rappresentazione matematica

11 1.2. DHA - Discrete Hybrid Automata 6 Figura 1.1: Modello di un sistema ibrido astratta che integra una macchina a stati finiti, che descrive la parte logica, con uno switched affine system, che ne rappresenta le dinamiche continue, e due elementi di interconnessione fra le parti; le classi di sistemi ibridi piecewise affine (PWA) e mixed logical dynamical systems (MLD), che maggiormente si prestano ai metodi numerici ricorsivi e specificamente indirizzate al dominio ibrido. Le suddette classi di modelli ibridi sono equivalenti [16, 17], sotto alcune ipotesi, ed è quindi possibile rappresentare lo stesso sistema con modelli di ogni classe. In particolare le classi piecewise affine e mixed logical dynamical systems permettono di effettuare analisi di osservabilità, di sviluppare controllori predittivi ibridi come problemi di minimizzazione mista intera. 1.2 DHA - Discrete Hybrid Automata Sistemi il cui comportamento dinamico è dettato dall interazione di dinamiche continue con dinamiche discrete sono stati studiati nell ambito di teoria dei sistemi fin dagli anni 60. A partire dagli anni 90 assistiamo ad un molti-

12 1.2. DHA - Discrete Hybrid Automata 7 Figura 1.2: Schema di un discrete hybrid automata (DHA) plicarsi di pubblicazioni in merito a questa classe di sistemi per lungo tempo trascurata, ed è poco più di un decennio che sono stati introdotti gli hybrid automata, i quali hanno fornito un concreto approccio formale, utile in primo luogo per l analisi e la verifica dei sistemi ibridi. I discrete hybrid automata (DHA) sono costituiti dalla connessione di una macchina a stati finiti, finite state machine (FSM), che raccoglie le dinamiche discrete del sistema ibrido, con uno switched affine system (SAS), che rappresenta la parte continua delle dinamiche ibride. L interazione fra le due parti è affidata a due elementi di connessione: il generatore di eventi, event generator (EG) e il selettore di modo, mode selector (MS). Il mode selector combina tutte le varie componenti discrete per selezionare il modo operativo, ossia le dinamiche continue, del sistema switched affine. Con riferimento alla figura 1.2, abbiamo: Lo switched affine system, costituito da una collezione di dinamiche continue, tra le quali viene selezionata dal MS quella che rappresenta

13 1.2. DHA - Discrete Hybrid Automata 8 l evoluzione del sistema rispetto al modo attuale: x c (k + 1) = A i(k) x c (k) + B i(k) u c (k) + f i(k) (1.1) dove x c (k) R nc è il vettore degli stati continui, u c (k) R mc è il vettore degli ingressi continui esogeni, A i e B i (i I) sono matrici di opportune dimensioni, i(k) I {1,..., s} è il modo del sistema, cioè il segnale che il SAS riceve in ingresso dal mode selector che seleziona una delle s dinamiche affini del sistema. L event generator, l elemento di interconnessione tra la parte continua e quella logica rappresentata dalla macchina a stati finiti. L EG prende i segnali della parte continua e genera un segnale logico in modo che vengano soddisfatti i vincoli lineari affini; le variabili evento δ e sono generate da condizioni lineari di soglia sugli stati e ingressi continui, e sul tempo: [δe(k) i = 1] [H i x c (k) + K i u c (k) W i ] (1.2) dove δ e {0, 1} ne. Ad esempio: [δ(k) = 1] [x c (k) 0] (1.3) La finite state machine, che rappresenta un processo a dinamiche discrete che evolve secondo una funzione logica di aggiornamento dello stato, attivato da segnali discreti, tra cui quello generato dall EG: x l (k + 1) = f B (x l (k), u l (k), δ e (k)) (1.4) dove x l {0, 1} n b sono le variabili di stato binarie, ul {0, 1} m b gli ingressi esogeni binari e f B è un vettore di funzioni che descrivono un iperpiano lineare.

14 1.3. Sistemi MLD 9 Il mode selector, il blocco di interconnessione tra la FSM e la parte a dinamiche continue; il modo attivo, che determina la dinamica da attivare nel SAS, è selezionato da una funzione booleana dello stato e dell ingresso binari, e della variabile d evento correnti: i(k) = f M (x l (k), u l (k), δ e (k)) (1.5) Per una trattazione più dettagliata in merito ai DHA e le loro proprietà si veda [16]. 1.3 Sistemi MLD I DHA sono un astrazione matematica dei modelli ibridi molto intuitiva e rappresentativa del funzionamento e dell interazione fra la componente a dinamiche discrete e quella a dinamiche continue. Inoltre è il formalismo che viene impiegato in HYSDEL (HYbrid Systems DEscription Language) 1 [16], un linguaggio ad alto livello, che consente di descrivere i DHA e di inserire ulteriori vincoli sulle variabili caratterizzanti il sistema ibrido. Il compilatore di HYSDEL consente di tradurre la descrizione del sistema ibrido in altre classi di sistemi. In particolare il nostro interesse è rivolto verso modelli maggiormente orientati al calcolo numerico e allo sviluppo di strategie di controllo basate su tecniche predittive. Queste richieste sono soddisfatte dai sistemi MLD [18] (Mixed Logical Dynamical) descritti da equazioni dinamiche lineari soggette a disuguaglianze lineari miste intere, cioè disuguaglianze che coinvolgono sia variabili intere che binarie. I sistemi MLD generalizzano un ampio insieme di modelli, tra i quali ci sono i sistemi ibridi, le macchine a stati finiti, alcune classi di sistemi ad eventi discreti, sistemi lineari vincolati, e sistemi non lineari le cui non linearità possono essere descritte da funzioni lineari a tratti. 1 vedi Appendice A

15 1.3. Sistemi MLD Calcolo proposizionale e programmazione mista intera La traduzione di proposizioni logiche in un problema di ottimizzazione a variabili intere si basa sulla possibilità di trasformare una proposizione logica in un sistema di vincoli lineari interi tali che sia mantenuta l equivalenza logica delle espressioni tradotte [18 20]. Il sistema risultante di vincoli deve avere la stessa tabella di verità della proposizione originale, cioè, la verità o la falsità della proposizione è rappresentata dal soddisfacimento o meno del corrispondente insieme di disuguaglianze lineari. Indichiamo con P j la j-esima variabile logica che assume i valori T (true) o F (false) e rappresenta una proposizione atomica descrivente un azione o una decisione. Associamo una variabile intera con ogni tipo di azione (o decisione). Questa variabile, a cui ci si riferisce con il termine variabile binaria, è indicata con δ j e può assumere solo due valori: 1 o 0. La connessione di questa variabile con la variabile P j è definita da 1 se P j = T δ j = 0 se P j = F Le variabili logiche possono essere unite in modo da formare proposizioni più complesse tramite connettivi logici: (and), (or), (not), (implica), (se e solo se), (or esclusivo) (un trattamento più completo del calcolo booleano può essere trovato nei testi di progetto di circuiti digitali, ad esempio [21]). Semplici proposizioni logiche e loro forme equivalenti sono indicate in Tabella 1.1. E noto che tutti i connettivi possono essere definiti in termini di un loro sottoinsieme, ad esempio {, }, il quale è detto essere un insieme completo di connettivi. Le proposizioni logiche elementari possono essere tradotte in disuguaglianze intere, come illustrato in Tabella 1.2, per un elenco più dettagliato vedere [19, 20].

16 1.3. Sistemi MLD 11 Proposizione Forma equivalente P P (P Q) ( P Q) (P Q) Esclusione (P Q) P Q Leggi di De Morgan (P Q) P Q P Q P Q Implicazione Tabella 1.1: Proposizioni logiche e forme equivalenti Proposizione Vincolo P δ 1 = 0 P 1 P 2 δ 1 + δ 2 1 P 1 P 2 δ 1 = 1, δ 2 = 1 P 1 P 2 δ 1 δ 2 0 P 1 P 2 δ 1 δ 2 = 0 Tabella 1.2: Trasformazione prop. logiche in disuguaglianze intere In generale una proposizione logica può essere definita come una funzione logica quando è utilizzata per definire una variabile logica P n come una funzione di P 1,...,P n 1, P n = f(p 1,..., P n 1 ) (1.6) L equazione (1.6) può essere tradotta in forma normale congiuntiva (CNF) k δ i δ i, N j, P j {1,..., n} (1.7) j=1 i P j i N j e successivamente in un insieme di disuguaglianze lineari intere [22]

17 1.3. Sistemi MLD 12 1 i P 1 δ i + i N 1 (1 δ i ). 1 i P k δ i + i N k (1 δ i ). (1.8) dalle quali si ottiene il poliedro Aδ b, δ {0, 1} n. Un metodo alternativo di traduzione delle funzioni logiche in disuguaglianze lineari intere è dato in [23] e si basa su una descrizione geometrica della tabella di verità. In questo caso il politopo P = {δ : Aδ b} è il guscio convesso delle righe della tabella di verità associata alla proposizione logica. Rispetto al poliedro costruito utilizzando la CNF, quello ottenuto tramite guscio convesso è il più piccolo tra tutti. Una variabile binaria può essere definita in funzione di una condizione del tipo [δ = 1] [f(x) 0] (1.9) dove f(x) : R n R è una funzione lineare e x X, dove X è un insieme limitato. Per tradurre la precedente equivalenza in disuguaglianze lineari miste intere è necessario conoscere un limite superiore ed uno inferiore della funzione f(x) M max x X f(x) m min x X f(x). L equivalenza (1.9) si traduce in f(x) M(1 δ) f(x) ɛ + (m ɛ)δ (1.10a) (1.10b) (1.11) dove ɛ è una piccola tolleranza (tipicamente la precisione di macchina). Un altra possibile descrizione di proposizioni logiche è il vincolo logico nella

18 1.3. Sistemi MLD 13 forma di implicazione, LCIF (Logic Constraint Implication Form), ovvero una combinazione logica di semplici vincoli, definito come if antecedente then conseguente1 else conseguente2 dove antecedente è una variabile binaria e conseguente1 \ conseguente2 è un vincolo lineare. Un tale vincolo logico può essere sempre tradotto in un problema misto intero. Consideriamo ad esempio il seguente vincolo LCIF if δ then z = a 1x b 1 else z = a 2x b 2 dove δ {0, 1}, z R, x X. Il vincolo può essere tradotto come (m 2 M 1 )δ + z a 2x b 2 (m 1 M 2 )δ z a 2x + b 2 (m 1 M 2 )(1 δ) + z a 1x b 1 (m 2 M 1 )(1 δ) z a 1x + b 1, (1.12) dove M i sup(a ix b i ), m i inf x X x X (a ix b i ), i = 1, 2 sono i limiti superiori ed inferiori, rispettivamente, di (a ix b i ). Ulteriori esempi di traduzione di proposizioni logiche in problemi misti interi possono essere trovati in [18] Sistemi MLD Le traduzioni illustrate nella sezione precedente sono alla base per la descrizione delle varie parti che compongono un sistema ibrido, riportato in Figura 1.1. Le parti logiche del sistema, automa ed eventuali proposizioni logiche, possono essere tradotte in un insieme di vincoli lineari interi della forma (1.8).

19 1.3. Sistemi MLD 14 La legge di aggiornamento degli stati dell automa può essere infatti descritta da proposizioni logiche comprendenti stati binari, i loro aggiornamenti temporali e segnali binari esterni, sotto l ipotesi che le transizioni da uno stato all altro siano sincronizzate con il tempo di campionamento delle equazioni dinamiche continue: x l (k + 1) = F (x l (k), u l (k)), x l {0, 1} n l, ul {0, 1} m l, dove ul è l insieme di ingressi che comandano la transizione da uno stato all altro dell automa. L interfaccia che collega segnali continui a eventi discreti (A/D) può essere descritta tramite relazioni del tipo (1.9) e tradotta nell insieme di vincoli lineari misti interi (1.11). L interfaccia che collega eventi discreti a variabili continue (D/A) può essere descritta tramite la forma LCIF e viene tradotta nell insieme dei vincoli lineari misti interi del tipo (1.12). Infine, la dinamica continua viene descritta da equazioni alle differenze, del tipo: x(k + 1) = Ax(k) + Bu(k) y(k) = Cx(k) + Du(k). (1.13a) (1.13b) Se da un lato la modellizzazione tempo-discreto non consente di catturare fenomeni che avvengono solo nei sistemi ibridi tempo-continuo, come i comportamenti di Zeno [24], dall altro essa consente di sviluppare schemi numericamente trattabili per risolvere problemi complessi di analisi e sintesi. E da osservare inoltre che la trattazione lineare non è limitativa dato che sistemi non lineari possono essere ricondotti a funzioni lineari a tratti secondo tecniche derivanti dalla letteratura dei circuiti digitali [25, 26]. Raccogliendo le uguaglianze e le disuguaglianze derivanti dalle rappresentazioni delle varie componenti del sistema ibrido si ottiene la forma generale di

20 1.4. Sistemi PWA 15 un sistema MLD [18, 27] x(k + 1) = Ax(k) + B 1 u(k) + B 2 δ(k) + B 3 z(k) y(k) = Cx(k) + D 1 u(k) + D 2 δ(k) + D 3 z(k) E 2 δ(k)+e 3 z(k) E 1 u(k) + E 4 x(k) + E 5 (1.14a) (1.14b) (1.14c) dove x R nc {0, 1} n l è un vettore di stati continui e binari, u R m c {0, 1} m l sono gli ingressi, y R pc {0, 1} p l le uscite, δ {0, 1} r l, z R r c rappresentano rispettivamente variabili ausiliarie binarie e continue, che sono introdotte nella trasformazione delle relazioni logiche in disuguaglianze lineari miste intere, e A, B 1 3, C, D 1 3, E 1 5 sono matrici di dimensioni compatibili. Per le proprietà dei sistemi MLD (ben postezza, stabilità, osservabilità, contollabilità) si rimanda a [18, 28]. 1.4 Sistemi PWA Un altra classe importante di modelli matematici che descrivono sistemi ibridi sono i sistemi PWA (Piecewise Affine) 2 [29]. I sistemi PWA sono definiti partizionando lo spazio degli stati in regioni poliedrali, e associando ad ognuna di esse una diversa funzione affine di aggiornamento dello stato del tipo: x(k + 1) y(k) i(k) = A i(k) x(k) + B i(k) u(k) + f i(k) = C i(k) x(k) + D i(k) u(k) + g i(k) s.t. H i(k) x(k) + J i(k) u(k) K i(k) (1.15) dove C i = {x : H i x K i }, i = 1,, s è la partizione poliedrica dello spazio degli stati (vedi Figura 1.3). 2 Esistono comunque altri modelli ibridi di interesse scientifico come sistemi lineari complementari (LC), sistemi LC estesi (ELC) e sistemi Min-Max-Plus Scaling (MMPS). L equivalenza tra questi sistemi ed i sistemi MLD e PWA è fornita in [17].

21 1.4. Sistemi PWA 16 Figura 1.3: Partizione poliedrica dello spazio degli stati Per le proprietà dei sistemi PWA si veda [28], per l equivalenza con i sistemi DHA e MLD si rimanda rispettivamente a [16] e [17].

22 Capitolo 2 Controllo predittivo Nel capitolo 1 abbiamo mostrato come sia possibile descrivere un sistema ibrido in forma DHA, MLD o PWA. In questo capitolo descriveremo la strategia del controllo predittivo, riportandone la formulazione anche per sistemi ibridi. Sarà esaminato anche l aspetto computazionale, mostrando come l azione compiuta dal controllore in linea possa essere tradotta in una forma esplicita computazionalmente più efficiente. 2.1 Controllo predittivo in-linea L idea di base nella teoria del controllo è la progettazione di un dispositivo automatico (il controllore) che consenta di garantire l inseguimento di determinati riferimenti ed il soddisfacimento di determinati vincoli, utilizzando i segnali provenienti dall impianto stesso, in uno schema retroazionato, come descritto in Figura 2.1. Una tecnica molto diffusa per la progettazione di controllori si basa sul concetto di Controllo Predittivo, MPC (Model Predictive Control), tecnica ampiamente utilizzata nell industria di processo per pro- 17

23 2.1. Controllo predittivo in-linea 18 Figura 2.1: Schema di controllo retroazionato blemi di inseguimento di sistemi soggetti a vincoli. Nella sua formulazione più generale, il controllo predittivo consta di tre idee di base: 1. L utilizzo di un modello matematico atto a prevedere le uscite del processo negli istanti di tempo futuri (l orizzonte). Le uscite future, comprese entro l orizzonte e calcolate all istante presente, dipendono dai valori passati degli ingressi e delle uscite e dai segnali di controllo futuri. 2. La presenza di una legge di controllo ottenuta attraverso la minimizzazione di una funzione di costo da parte di segnali di controllo. Tali segnali di controllo sono calcolati ottimizzando un determinato criterio allo scopo di tenere l uscita del processo il più vicino possibile ad una traiettoria di riferimento nota. 3. Una strategia ricorsiva che, ad ogni istante, consiste nello spostamento nel futuro dell orizzonte. Ciò implica che ad ogni istante di tempo solo il primo tra i segnali di controllo calcolati sia effettivamente utilizzato. All istante di tempo successivo infatti si ha un aggiornamento delle uscite e la conseguente ripetizione del calcolo dei segnali di controllo. Il controllo predittivo è un metodo molto generale ben adatto ad essere applicato nel caso di problemi di controllo multivariabile (MIMO: multi inputmulti output) con un elevato grado di interazione fra le variabili di ingresso e quelle di uscita. Inoltre è un metodo che è in grado di gestire vincoli di

24 2.1. Controllo predittivo in-linea 19 disuguaglianza sulle variabili di ingresso ed uscita. La tecnica MPC 1 [30, 31] si basa quindi sull ottimizzazione in linea di un funzionale di costo soggetto a vincoli dettati dalla dinamica del sistema. Essa permette di risolvere i problemi più gravosi dovuti alla presenza di elevati ritardi o di risposte a fase non minima, e, a differenza di altre tecniche, offre la possibilità di includere vincoli sia sugli ingressi che sulle uscite del sistema in fase di progetto del controllore. La sigla MPC è acronimo di Model Predictive Control dove Model: indica il modello (in genere tempo-discreto 2 ) del sistema sul quale implementare il controllo. Predictive: l ottimizzazione è basata sull evoluzione predetta del sistema. Control: vengono considerati i vincoli imposti in fase di dichiarazione e ricavata la legge di controllo in rispetto degli stessi. Questa tecnica di controllo è basata sulla filosofia ad orizzonte recessivo (vedi Figura 2.2), secondo la quale ad ogni istante di campionamento, a partire dallo stato corrente del sistema, si determina, secondo un qualche criterio di ottimalità, una sequenza di comandi da applicarsi su un orizzonte temporale futuro di N passi. La soluzione ottimale viene ottenuta a partire da una descrizione matematica del sistema considerando i vincoli sugli ingressi e sulle uscite e minimizzando un indice di prestazione. Tale indice di prestazione in genere è di tipo lineare o quadratico e per tale motivo ci si riferisce al problema di controllo come ad un problema quadratico (QP) o lineare (LP). Ad esempio, al tempo t, dato lo stato corrente x(t), viene risolto il problema 1 min U 2 U HU + x (t)f U s.t. GU W + Sx(t) (2.1) 1 Per ulteriori informazioni consultare il sito: bemporad 2 Nel caso in cui sia tempo continuo viene effettuato un campionamento interno con intervallo Ts proprio del controllore

25 2.1. Controllo predittivo in-linea 20 Figura 2.2: Filosofia ad orizzonte recessivo dove U [u (0)... u (N 1)] è il vettore di ottimizzazione, H = H 0, e H, F, G, W, S dipendono dai pesi, dai limiti inferiori e superiori sugli ingressi e sulle uscite e dalle matrici dello spazio di stato del sistema. Si ottiene la sequenza ottimale di comandi U = {u (0),..., u (N 1)}, della quale verrà applicato al sistema soltanto il primo campione ( u(t) = u (0) ), scartando tutto il resto. Questa procedura viene ripetuta per tutti gli istanti successivi del controllo.

26 2.1. Controllo predittivo in-linea MPC per sistemi ibridi Sia t l istante di campionamento attuale, x(t) lo stato corrente del sistema, x e, u e una coppia di equilibrio e δ e, z e i corrispondenti equilibri delle variabili ausiliarie, suppondendo che y(t) debba inseguire il riferimento y e il problema di controllo ottimo può essere formulato nel seguente modo: min {v,δ,z} T 1 0 T 1 J({v, δ, z} T 1 0, x(t)) k=0 Q 1 (v(k) u e ) p + Q 2 (δ(k t) δ e ) p + Q 3 (z(k t) z e ) p + Q 4 (x(k t) x e ) p + Q 5 (y(k t) y e ) p (2.2a) x(t t) = x e x(k + 1 t) = Ax(k t) + B 1 v(k) + B 2 δ(k t)+ B 3 z(k t) soggetto a y(k t) = Cx(k t) + D 1 v(k) + D 2 δ(k t)+ D 3 z(k t) E 2 δ(k t) + E 3 z(k t) E 1 v(k)+ (2.2b) E 4 x(k t) + E 5 u min v(t + k) u max, k = 0, 1,..., T 1 x min x(t + k t) x max, k = 1,..., N c dove T e N c T sono, rispettivamente, gli orizzonti di predizione e di vincolo sullo stato, x(k t) è lo stato predetto all istante t + k risultante dall ingresso u(t + k) = v(k) applicato a (1.14) a partire da x(0 t) = x(t), u min, u max e x min, x max sono, rispettivamente, vincoli sugli ingressi e sullo stato. In (2.2a) si ha che Qx p = x Qx quando p = 2, Qx p = Qx ( Qx 1 ) quando p = (p = 1) e Q 1,4 = Q 1,4 0, Q 2,3,5 = Q 2,3,5 0 (p = 2) Q 1 5 non singolari (p =, 1). (2.3)

27 2.1. Controllo predittivo in-linea 22 Se esiste la soluzione ottima {vt (0),..., vt (T 1), δt (0),..., δt (T 1), zv(0),..., zv(t 1)} secondo la filosofia ad orizzonte recessivo si applica al sistema reale soltanto u(t) = vt (0), (2.4) si scarta tutta la sequenza d ingressi vt (1),..., vt (T 1), e si ripete l intera procedura all istante t+1. La legge di controllo (2.2) (2.4) risulta essere un estensione del controllo predittivo ai sistemi ibridi. Le proprietà del controllo predittivo classico possono essere estese, con opportune modifiche, anche al controllo MPC per sistemi ibridi. In [27] sono presentati alcuni esempi di queste estensioni. Come si può notare dalla (2.2b), le equazioni che descrivono il modello MLD del sistema compaiono come vincoli nel problema di controllo ottimo. Dato che nel modello MLD ci sono variabili discrete il problema (2.2) risulta essere un problema misto intero. Se si utilizza la norma Euclidea, p = 2, il problema è risolubile mediante programmazione quadratica mista intera (MIQP), altrimenti, se p = o p = 1, mediante programmazione lineare mista intera (MILP). La soluzione di questi problemi si determina mediante risolutori misti interi (vedi paragrafo 2.1.3) Convergenza ad anello chiuso Il seguente teorema dimostra che l anello di retroazione formato dal sistema e dal controllore MPC ottenuto da (2.2) (2.4) risulta essere stabile [18, 27]. Teorema 1. Siano (x e, u e ) una coppia di equilibrio e (δ e, z e ) la corrispondente coppia di equilibrio delle variabili ausiliarie. Assumiamo che lo stato iniziale x(0) sia tale che esista all istante t = 0 una soluzione ammissibile

28 2.1. Controllo predittivo in-linea 23 del problema (2.2). Allora per tutte le matrici Q 1 5 che soddisfano (2.3) la legge MPC (2.2) (2.4) stabilizza il sistema in quanto lim t = x e lim t = u e lim 2(δ(t) δ e ) p t = 0 lim 3(z(t) z e ) p t = 0 lim 5(y(t) y e ) p t = 0 e sono soddisfatti i vincoli (1.14c) ed i vincoli dell ingresso e dello stato u min u(t) u max, x min x(t) x max. Dimostrazione. Si tratta di dimostrare che una funzione di Lyapunov è decrescente ed inferiormente limitata. Sia U t la sequenza di controllo ottimale {v t (0),..., v t (T 1)}, sia V (t) J(U t, x(t)) il corrispondente valore ottenuto dall indice di prestazione, e sia U 1 la sequenza {vt (1),..., vt (T 2), u e }. Allora, U 1 è ammissibile all istante t+1, assieme con i vettori δ(k t + 1) = δ(k + 1 t), z(k t + 1) = z(k + 1 t), k = 0,..., T 2, δ(t 1 t+1) = δ e, z(t 1 t+1) = z e, dato che x(t 1 t+1) = x(t t) = x e. Risulta che, V (t + 1) J(U 1, x(t + 1)) = V (t) Q 4 (x(t) x e ) p + Q 1 (u(t) u e ) p Q 2 (δ(t) δ e ) p + Q 3 (z(t) z e ) p Q 5 (y(t) y e ) p (2.5) e quindi che V (t) è decrescente. Dato che V (t) è inferiormente limitato da 0, allora esiste V = lim t V (t), che implica V (t + 1) V (t) 0. Inoltre,

29 2.1. Controllo predittivo in-linea 24 ogni termine della somma Q 4 (x(t) x e ) p + Q 1 (u(t) u e ) p + Q 2 (δ(t) δ e ) p + Q 3 (z(t) z e ) p + Q 5 (y(t) y e ) p (2.6) V (t) V (t + 1) converge a zero, e questo prova il teorema. Osservazione 1. Si può osservare che se Q 2 (o Q 3, Q 5 ) è non singolare, si garantisce la convergenza di δ(t) (o z(t), y(t)). Osservazione 2. Il teorema (1) assicura stabilità assunto che il problema (2.2) sia ammissibile all istante t = 0. L insieme delle condizioni iniziali x(0) per le quali il problema (2.2) è ammissibile verrà caratterizzato nel paragrafo 2.2.

30 2.1. Controllo predittivo in-linea Risolutori di problemi misti interi Rimane da chiarire come possa essere determinata una soluzione ottima del problema (2.2). Richiamiamo brevemente la forma generale di un problema misto intero min q l(q c, q d ) s.t. G c q c + G d q d S + F x(t) (2.7) dove l(q c, q d ) è il funzionale di costo, q c, q d rappresentano, rispettivamente, le componenti continue e discrete del vettore di ottimizzazione q. x(t) è lo stato del sistema all istante t che assumiamo come noto; mentre G c,g d,s,f sono matrici di dimensioni compatibili. Se il funzionale di costo è del tipo l(q c, q d ) = f T c q c + f T d q d il problema è detto lineare misto intero (MILP), mentre se è del tipo l(q) = 1 2 qt Hq + c T q si parla di problema quadratico misto intero (MIQP). A parte particolari strutture, i problemi misti interi sono classificati come problemi N P -hard per i quali non è possibile trovare un algoritmo che li risolva in tempo polinomiale rispetto alla dimensione del problema 3. Sebbene i problemi misti interi abbiano questa particolare natura, esistono diversi metodi per risolvere problemi misti interi: Metodi basati su tagli, in cui nuovi vincoli sono aggiunti al problema misto intero in modo da ridurre il dominio ammissibile finchè la soluzione intera non viene trovata. Metodi di decomposizione, dove la struttura del problema viene investigata tramite metodi di rilassamento e dualità. 3 Tale risultato, sebbene fortemente sospettato, non è ancora formalmente dimostrato.

31 2.2. Forma esplicita del controllore 26 Metodi basati su logica, dove sono utilizzate tecniche basate su inferenze e programmazione logica per esprimere le variabili binarie. Metodi Branch and Cut e Branch and Bound, dove le combinazioni binarie sono esplorate attraverso un albero binario. Tra quelli precedentemente indicati i metodi Branch and Bound risultano essere i più efficienti, come indicato in [32], e sono tipicamente usati nei risolutori. Alcuni esempi di risolutori possono essere [32 34] per risolvere problemi MIQP, e [33 35] per risolvere problemi MILP. In questo lavoro è stato sviluppato il controllo ottimo (2.2) in norma Euclidea, in particolare per risolvere il problema MIQP abbiamo utilizzato il risolutore CPLEX [33]. Un algoritmo Branch and Bound per problemi MILP/MIQP risolve e genera nuovi problemi LP/QP in accordo a tecniche di esplorazione di un albero di ricerca, dove i nodi dell albero corrispondono a sottoproblemi LP/QP. Inizialmente viene risolto il problema radice in cui sono rilassate tutte le variabili binarie, δ {0, 1} δ [0, 1], ed il valore del funzionale di costo rappresenta un limite inferiore al valore ottimo. L algoritmo procede generando i nodi figli fissando di volta in volta una variabile discreta ad uno dei possibili valori (operazione di branch). Nel passaggio da un problema ai sottoproblemi figli il valore dell ottimo non può diminuire. Ne segue che se ad una iterazione il valore del problema LP/QP è peggiore del valore di una soluzione ammissibile (che costituisce un limite superiore all ottimo), nessuna eventuale soluzione mista intera che può essere trovata nel sottoalbero avente come radice il nodo corrente potrà essere migliore della soluzione ammissibile, pertanto il sottoalbero non verrà esplorato (operazione di bounding). 2.2 Forma esplicita del controllore Nella sezione abbiamo mostrato come il controllo predittivo, noto lo stato del sistema x(t), richieda la risoluzione di un problema di programma-

32 2.2. Forma esplicita del controllore 27 zione MILP o MIQP ad ogni istante di campionamento t. Da un punto di vista computazionale questa tecnica di controllo risulta adatta per sistemi con tempi di campionamento sufficientemente lunghi, ad esempio dell ordine di decine di secondi, nei quali in genere è presente un hardware adatto a supportare la pesantezza computazionale richiesta. Nel paragrafo assumiamo che le posizioni degli ostacoli non sono conosciute in anticipo, e che quindi non sarebbe possibile effettuare una pianificazione off-line della traiettoria; d altronde il controllore MPC ibrido ha un tempo di campionamento dell ordine del secondo, e quindi si suppone che per velivoli reali, sui quali sono presenti dispositivi embedded, la pianificazione della traiettoria on-line sia supportabile dall hardware. Altro discorso si deve fare per il controllore di stabilità, l MPC lineare; esso ha un tempo di campionamento basso, dell ordine delle decine di millisecondi, e molto probabilmente il carico computazionale che richiede è troppo pesante per un dispositivo embedded; quindi per implementare il controllo di stabilità su velivoli reali si potrebbe fare ricorso ad una legge di controllo che dipende esplicitamente dallo stato e che debba essere calcolata ad ogni iterazione. L idea alla base della forma esplicita, come indicato in [36], è quella di gestire il vettore di stato x(t), che appare nel lato destro dei vincoli, come un vettore di parametri. Se nel funzionale di costo si adotta la norma- il problema di ottimizzazione diventa un problema MILP multiparametrico (mp-milp), altrimenti se si adotta la norma-2 si parla di problema di ottimizzazione MIQP multiparametrico. Risolvere un problema mp-milp/miqp consiste nell esprimere la soluzione del problema MILP/MIQP come funzione dei parametri. Una volta che la soluzione del problema multiparametrico è stata trovata, Ut = f(x(t)), il controllore predittivo è disponibile esplicitamente. La legge di controllo MPC può dunque essere espressa esplicitamente come un insieme di regioni nello spazio degli stati (funzione piecewise affine); ad ogni regione è associata una legge affine di retroazione dello stato. E neces-

33 2.2. Forma esplicita del controllore 28 sario sottolineare che la forma piecewise affine ed il controllore MPC-ibrido predittivo sono uguali, nel senso che entrambi producono la stessa azione di controllo, ed inoltre condividono le stesse proprietà di ottimalità e stabilità. La sola differenza sta nell implentazione del controllore: in forma esplicita il calcolo in linea si riduce ad una valutazione di una funzione PWA. Risulta quindi evidente come la forma esplicita sia interessante da un punto di vista implementativo. Oltre alla semplicità indotta dal metodo che la rende adatta per sistemi con tempo di campionamento veloci e con hardware economico, permette di comprendere la struttura matematica della legge di controllo, altrimenti nascosta dal formalismo di ottimizzazione. Oltretutto il fatto di poter rappresentare l anello chiuso in forma PWA consente di riutilizzare gli strumenti per l analisi di prestazione basati sull analisi di raggiungibilità.

34 Capitolo 3 Modello di volo Tradizionalmente la configurazione del quadcopter non è stata utilizzata molto nell industria aerospaziale, soprattutto perchè la maggior parte dei carichi possono essere sollevati utilizzando solo uno o due rotori; aumentarne il numero non è ritenuto necessario anche perchè aumenterebbe il peso e la complessità del veicolo. Tuttavia, i quadcopter (o quadrotor) stanno emergendo come piattaforma più popolare tra gli UAVs (Unmanned Aerial Vehicles). Essi consistono di quattro rotori in totale, con due coppie di eliche a passo fisso (ovvero aventi attacco rigido con il mozzo), controrotanti, disposte ai quattro angoli del velivolo, un esempio del quale è mostrato in Figura 3.1. Grazie alle loro particolari capacità, l uso di veicoli autonomi come i quadcopter è stato previsto per una varietà di applicazioni, sia individualmente che in formazione, come sorveglianza, ricerca e salvataggio, reti mobili di sensori [37]. Il particolare interesse delle comunità di ricerca nei confronti dei quadrotor è giustificato da alcuni vantaggi che presentano rispetto ai comuni UAVs a decollo e atterraggio verticale (VTOL - verticale take off and landing), come gli elicotteri: 29

35 30 Figura 3.1: Il prototipo costruito da Carlo Alberto Pascucci al Dipartimento di Ingegneria dell Informazione di Siena Semplicità del sistema di controllo. In tutte le altre configurazioni di UAVs il controllo dell assetto di volo è effettuato variando l angolo di pitch dell elica, mentre la velocità rotazionale dei rotori rimane costante. La variazione dell angolo di pitch richiede complessi sistemi meccanici che aumentano il peso del velivolo e richiedono una frequente manutenzione. Ma se il controllo dell assetto può essere effettuato con la sola variazione della velocità dei motori, allora si può risparmiare sui costi, sul peso e sul volume del veicolo, permettendone la costruzione anche di dimensioni ridotte. I quadcopter non richiedono quindi per l attuazione dei rotori complessi sistemi meccanici di controllo; utilizzano quattro rotori a passo fisso e le variazioni della velocità dei motori per il controllo dell assetto, rendendone così più semplice sia il design che la manutenzione. Inoltre la semplicità

36 31 del sistema di controllo li rende facili da automatizzare. Maggiore capacità di carico. La spinta sviluppata da un rotore aumenta con il suo diametro [38, 39]. Così, aumentando il diametro è possibile aumentare la spinta e quindi il carico che può essere sollevato. Tuttavia, c è un limite a quanto il diametro può essere aumentato a causa di fattori aerodinamici. Si potrebbe aumentare la spinta aumentando il numero di eliche per rotore, ma anche questo ha un limite, imposto dalla complessità meccanica crescente. Quindi se la spinta deve essere aumentata, è necessario aggiungere altri rotori; anche in questo caso c è un limite in quanto aggiungere troppi rotori aumenta in maniera eccessiva il peso a vuoto del velivolo, diminuendone la capacità di carico. L uso di quattro rotori permette di avere singoli rotori di diametro più piccolo di un equivalente rotore principale di un elicottero, relativamente alle dimensioni dell airframe (la struttura meccanica del velivolo che non include il sistema di propulsione). I rotori individuali, quindi, accumulano meno energia cinetica durante il volo, e questo rende più sicuri i quadcopter per interazioni a distanza ravvicinata. Infine, i rotori possono essere racchiusi all interno di una struttura che ne consenta la protezione in caso di collisione, permettendo voli al chiuso e in ambienti densi di ostacoli con un basso livello di rischio di danneggiare il velivolo, gli operatori e ciò che lo circonda. Effetti giroscopici ridotti. Gli effetti giroscopici si possono osservare in ogni corpo rotante, inclusi i rotori di un elicottero; tali effetti però sono differenti a seconda del tipo di rotore considerato [40]. Un rotore completamente articolato [41] tenderà a comportarsi come un giroscopio ideale, cioè il vettore del momento angolare tenderà a mantenere la stessa orientazione quando l elicottero cambia

37 3.1. Dinamica non lineare del quadcopter 32 l assetto di volo. Mentre un rotore perfettamente rigido [41] introdurrà un momento giroscopico nell airframe quando c è un cambio nell assetto di volo. Tutti gli altri tipi di rotori avranno comportamenti intermedi. In ogni caso, gli effetti giroscopici dipenderanno dalla velocità e dalla direzione di rotazione del rotore. In un quadrotor, due rotori girano in senso orario e gli altri due in senso antiorario. Se le velocità sono identiche per i quattro rotori gli effetti giroscopici sono minimi (quasi nulli), comunque molto più piccoli di quelli osservati negli elicotteri. In questo capitolo verrà descritto il modello matematico utilizzato per la dinamica di volo del nostro quadcopter, e come tale modello è stato esteso ad una schema di formazione di tipo centralizzato e decentralizzato. 3.1 Dinamica non lineare del quadcopter Il quadcopter è un sistema sottoattuato poichè ha sei gradi di libertà ma è controllato utilizzando solo quattro attuatori (vedi Figura 3.2). Indichiamo con x, y, z la posizione del veicolo e con θ, φ, ψ le rotazioni intorno agli assi cartesiani, relativi al frame mondo I. In particolare, x e y sono le coordinate del piano orizzontale, z denota la posizione verticale, ψ è l angolo di yaw (imbardata - rotazione intorno all asse z), θ è l angolo di pitch (beccheggio - rotazione intorno all asse x), e φ è l angolo di roll (rollio - rotazione intorno all asse y). Non avendo a disposizione un velivolo reale da cui ricavare la dinamica per il controllo di stabilità, il modello adottato in questo lavoro di tesi è basato principalmente su quello utilizzato in [11], nel quale il controllo viene eseguito agendo sulle coppie dei motori e sulla forza totale di spinta. Come riportato in Figura 3.2, i quattro motori M 1, M 2, M 3, M 4 generano quattro forze di spinta f 1, f 2, f 3, f 4, e quattro coppie τ 1, τ 2, τ 3, τ 4, rispettivamente, che sono

38 3.1. Dinamica non lineare del quadcopter 33 Figura 3.2: Modello del quadcopter regolate manipolando le tensioni applicate ai motori. Vengono così generate tre coppie τ θ, τ φ, τ ψ intorno ai corrispondenti assi τ θ = (f 2 f 4 )l (3.1a) τ φ = (f 3 f 1 )l (3.1b) 4 τ ψ = (3.1c) i=1 dove l è la distanza tra il centro di ogni elica e il centro di gravità del velivolo; viene generata anche una forza totale u = f 1 + f 2 + f 3 + f 4, che permette il cambiamento delle coordinate di posizione e orientazione nello spazio tridimensionale. Considerando τ = [τ θ τ φ τ ψ ] (vettore delle coppie complessive), η = [θ φ ψ] (vettore degli spostamenti angolari), e J (matrice delle inerzie), la dinamica rotazionale del quadcopter è descritta da τ = J η + J η 1 2 η ( η J η) (3.2) Effettuando delle semplificazioni riportate in [2], si può riscrivere η = τ (3.3) τ i

39 3.1. Dinamica non lineare del quadcopter 34 dove τ = [ τ θ τ φ τ ψ ] è considerato come un nuovo ingresso di comando. Attraverso trasformazioni rotazionali tra il frame I e il frame posizionato nel centro di massa del quadcopter, otteniamo le equazioni dinamiche mẍ = u sin θ βẋ mÿ = u cos θ sin φ βẏ m z = u cos θ cos φ mg βż θ = τ θ φ = τ φ ψ = τ ψ (3.4a) (3.4b) (3.4c) (3.4d) (3.4e) (3.4f) dove m denota la massa del velivolo, g l accelerazione di gravità, e il fattore di smorzamento β è stato aggiunto per tenere conto degli effetti realistici di attrito. Poichè il controllo sulle coppie non è attuabile nella realtà, le equazioni (3.4) sono state riscritte secondo le dinamiche analizzate in [42, 43]: ẍ = ( U 1 sin θ βẋ) 1 m (3.5a) ÿ = (U 1 cos θ sin φ βẏ) 1 m z = g + (U1 cos θ cos φ βż) 1 m θ = U 2 I xx φ = U 3 I yy (3.5b) (3.5c) (3.5d) (3.5e) ψ = U 4 (3.5f) I zz dove I xx, I yy, I zz sono i momenti d inerzia calcolati rispetto agli assi x, y, z del body frame.

40 3.1. Dinamica non lineare del quadcopter 35 La forza totale e le coppie sono date da U 1 = u U 2 = τ θ U 3 = τ φ U 4 = l( f 1 + f 2 f 3 + f 4 ) (3.6a) (3.6b) (3.6c) (3.6d) Infine sono state utilizzate le equazioni riportate in [3], dalle quali si ricavano le spinte dei motori (espresse in Newton) a partire dalle rispettive tensioni (in Volt): f 1 = 9.81(22.5V m1 9.7) 1000 f 2 = 9.81(22.5V m2 9.7) 1000 f 3 = 9.81(22.5V m3 9.7) 1000 f 4 = 9.81(22.5V m4 9.7) 1000 (3.7a) (3.7b) (3.7c) (3.7d) Combinando le equazioni (3.6) con le (3.7) è così possibile effettuare il controllo di stabilità direttamente sulle tensioni del quadcopter, V m1, V m2, V m3, V m4. Il modello non lineare risultante ha dodici stati (sei posizioni e sei velocità) e quattro ingressi (le tensioni dei motori) fortemente accoppiati tramite le relazioni non lineari in (3.5). In tabella 3.1 sono riportati i parametri utilizzati per il quadcopter.

41 3.2. Gestione di una formazione di velivoli 36 Parametro Valore m kg l m β 0.2 Ns/m I xx Nms 2 I yy Nms 2 I zz Nms 2 Tabella 3.1: Parametri del quadcopter 3.2 Gestione di una formazione di velivoli Nel paragrafo precedente abbiamo illustrato la dinamica di volo per un singolo quadcopter; andremo ora a descrivere come tale modello non lineare è stato utilizzato per descrivere una formazione di tre velivoli e come tale formazione viene gestita. Generalmente il controllo di una formazione di robot si divide in tre categorie: behavior based, struttura virtuale e inseguimento del leader. Nell approccio behavior based [44 46], diversi comportamenti desiderati (ad esempio evitare le collisioni, mantenere la formazione, inseguire un target) sono assegnati ad ogni robot. L azione finale del robot deriva dal pesare la relativa importanza di ogni comportamento. La formulazione teorica e l analisi matematica di questo approccio è difficile e di conseguenza non è facile garantire la convergenza della formazione ad una configurazione desiderata. Il metodo della struttura virtuale [47] considera la formazione di robot come una singola rigida struttura virtuale e in questo modo il comportamento del sistema robotico è assimilabile a quello di un oggetto fisico. Le traiettorie desiderate non sono assegnate ad ogni singolo robot ma all intera formazione nella sua totalità. In questo caso il comportamento della formazione è preve-

42 3.2. Gestione di una formazione di velivoli 37 dibile e di conseguenza il suo controllo è semplice. Tuttavia è necessario che i robot comunichino tra di loro in maniera costante. Nell approccio leader-follower, un elemento della formazione, designato come il leader, si muove lungo una traiettoria predefinita mentre gli altri robot, gli inseguitori (followers), mantengono una posizione desiderata (distanza e orientazione) rispetto al leader [48, 49]. In questo caso la formazione non tollera errori del leader e mostra scarse capacità di reiettare i disturbi. Nonostante questi difetti l approccio leader-follower è particolarmente apprezzato per la sua semplicità e scalabilità. Nell ambito dell approccio leader-follower, la gestione di una formazione di velivoli può essere centralizzata o decentralizzata [50]. Nel primo caso, un supervisore della formazione gestisce la topologia dei canali usati per lo scambio di informazioni tra i velivoli. Il supervisore può essere uno dei velivoli della formazione oppure può essere ground-based. In entrambi casi, lo schema centralizzato presenta alcuni svantaggi, come l alto numero di comunicazioni necessarie tra il supervisore e la formazione, che non sempre possono essere garantite, e inoltre il ritardo che si può avere nel correggere errori di formazione a causa di ritardi nello scambio di informazioni. D altra parte, utilizzare una gestione ground-based permette di avere capacità di decisione migliori rispetto a quelle raggingibili da un computer a bordo dei velivoli. Inoltre, se necessario, si potrebbe aggiungere l intervento umano a quello del sistema automatico. In uno schema decentralizzato, ogni velivolo possiede una certa capacità di decisione, mentre l intera formazione deve essere in grado di effettuare riconfigurazioni, prendere decisioni, e raggiungere gli obiettivi della missione. In questo caso i problemi derivano dall insorgere di conflitti tra le decisioni prese dai singoli velivoli. Diversi sono i vantaggi della gestione decentralizzata: le informazioni vengono scambiate solo tra i velivoli della formazione, ad eccezione di possibili aggiornamenti riguardo la missione comunicati dalla

43 3.2. Gestione di una formazione di velivoli 38 Figura 3.3: Struttura della formazione stazione a terra; è possibile utilizzare dispositivi di comunicazione a basso consumo come sensori ottici grazie alle distanze ridotte tra i velivoli; infine, i tempi di reazione sono minimi. Il controllo di formazione sviluppato in questo lavoro di tesi si può ricondurre ad un approccio di tipo leader-follower, per il quale sono stati utilizzati entrambi gli schemi appena descritti, con delle variazioni. La squadra è composta da tre quadcopter disposti in forma triangolare (vedi Figura 3.3). Per quanto riguarda l approccio centralizzato, la formazione dei tre velivoli è stata modellizzata come un unico elemento, creando un macromodello con una dinamica non lineare derivata dalla (3.5): il modello risultante ha dodici ingressi (le tensioni dei motori dei velivoli) e trentasei stati; il leader insegue una posizione di target mentre i followers mantengono una determinata distanza da esso. Un unico controllore agisce da supervisore, regola la stabilità dei singoli velivoli, li mantiene in formazione e permette l elusione di ostacoli manipolando le tensioni dei motori in base alle uscite del sistema. Nello schema decentralizzato la formazione è invece costituita da tre dina-

44 3.2. Gestione di una formazione di velivoli 39 miche come la (3.5), indipendenti tra di loro; il leader insegue una posizione di target e gli altri due quadcopter si mantengono ad una posizione desiderata rispetto ad esso. In questo caso abbiamo tre controllori, uno per ogni dinamica, che singolarmente regolano la stabilità dei quadcopter, permettono l elusione degli ostacoli e che nell insieme agiscono per mantenerli in formazione; inoltre, il controllore di ogni follower deve intervenire anche per evitare collisioni con gli altri velivoli.

45 Capitolo 4 Progetto delle strategie di controllo Nei precedenti capitoli abbiamo descritto quali sono gli strumenti teorici richiesti per lo sviluppo delle strategie di controllo necessarie agli scopi di queso lavoro di tesi, e su quale modello matematico si basa il loro design. In questo capitolo illustreremo nel dettaglio il progetto dei controllori e i parametri utilizzati, partendo dal singolo quadcopter per poi estenderci al controllo di formazione centralizzato e decentralizzato. Infine, verrano riportati i risultati ottenuti con le simulazioni effettuate per confrontare le metodologie sviluppate. 4.1 MPC lineare per la stabilizzazione Allo scopo di progettare un controllore MPC lineare per il quadcopter, abbiamo linearizzato il modello non lineare (3.5) intorno ad un punto di equilibrio di hovering (volo sostentato a velocità nulla e quota costante). Il risultante sistema lineare in spazio di stato a tempo continuo viene convertito in tempo 40

46 4.1. MPC lineare per la stabilizzazione 41 discreto con tempo di campionamento T s ξ L (k + 1) = Aξ L (k) + Bu L (k) y L (k) = ξ L (k) (4.1) dove ξ L (k) = [θ, φ, ψ, x, y, z, θ, φ, ψ, ẋ, ẏ, ż, z I ] R 13 è il vettore degli stati, u L (k) = [v m1, v m2, v m3, v m4 ] R 4 è il vettore degli ingressi, y L (k) R 13 è il vettore delle uscite(che assumiamo completamente misurate o stimate), e A, B, C, D sono matrici di dimensioni opportune ottenute dal processo di linearizzazione. Lo stato aggiuntivo, z I = (z z d ) è incluso per fornire azione integrale sull altezza z, in maniera da garantire un inseguimento senza offset della posizione desiderata z d in regime stazionario. L azione integrale serve principalmente a bilanciare l effetto della forza di gravità che agisce contro la spinta sviluppata dalla forza totale u (vedi Figura 4.1) Figura 4.1: Azione integrale sulla z durante il transitorio iniziale Per il design del controllore è stata utilizzata la formulazione MPC lineare del Model Predictive Control Toolbox per Matlab [31], dove l azione di controllo

47 4.1. MPC lineare per la stabilizzazione 42 MPC all istante k si ottiene risolvendo il problema di ottimizzazione min u L (k k). u L (m 1+k k) ɛ N L 1( n y i=0 w y j [y Lj(k + i + 1 k) r Lj (k + i + 1)] 2 + j=1 n u w u j=1 j u Lj (k + i k) 2 + ρ ɛ ɛ 2 ) (4.2a) s.t. u min Lj y min Lj u Lj (k + i k) u max Lj, j = 1,..., n u ɛv y,min Lj y Lj (k + i + 1 k) y max Lj j = 1,..., n y + ɛv y,max Lj (4.2b) u(k + h k) = 0, h = N Lu,..., N L ɛ 0 i = 0,..., N L 1, rispetto alla sequenza degli incrementi degli ingressi { u(k k),..., u(n Lu 1 + k k)} e alla variabile slack ɛ. In (4.2a) il pedice () j denota il j-esima componente di un vettore, (k + i k) denota il valore predetto al tempo k + i basato sull informazione disponibile al tempo k, r L (k) è il campione attuale del riferimento per l uscita, V y,min, V y,max sono vettori costanti con valori non negativi che rappresentano l intenzione nel rilassare il corrispondente vincolo sull uscita (più grande è V, più rilassato è il vincolo), n y = 13 è il numero delle uscite, n u = 4 è il numero degli ingressi. Il controllore MPC lineare pone u(k) = u(k 1) + u (k k), dove u (k k) è il primo elemento della sequenza ottimale.

48 4.2. Pianificazione della traiettoria per l elusione di ostacoli 43 Figura 4.2: Struttura gerarchica 4.2 Pianificazione della traiettoria per l elusione di ostacoli MPC ibrido Come mostrato in Figura 4.2, il progetto dell MPC lineare viene esteso con una struttura gerarchica, dove al livello più alto un controllore ibrido MPC impone i riferimenti r L sulle posizioni, sugli angoli, e sulle velocità allo scopo di svolgere il compito principale, ossia raggiungere una data posizione di target (x t, y t, z t ) mentre si evitano degli ostacoli 1. Si assume che le posizioni del target e degli ostacoli possano variare nel tempo e che non siano conosciute 1 La formulazione utilizzata in questo lavoro può essere estesa per includere l angolo di yaw ψ come una variabile controllata aggiuntiva. Si noti che ψ è dinamicamente disaccoppiata in regime stazionario da (x, y, z).

49 4.2. Pianificazione della traiettoria per l elusione di ostacoli 44 in anticipo, dunque non sarebbe possibile effettuare la pianificazione off-line (ottimale). Quindi, utilizziamo il controllo predittivo per generare i set point (x d, y d, z d ) per il controllore MPC lineare in tempo reale. L approccio proposto consiste nel costruire un modello ibrido astratto del velivolo controllato e degli ostacoli presenti, e poi usare una strategia ibrida MPC per la generazione on-line della posizione desiderata (x d, y d, z d ) (gli altri riferimenti sulle uscite nel vettore r L sono poste a zero). La dinamica dell anello chiuso costituita dal quadcopter e il controllore MPC lineare può essere approssimata come un sistema dinamico lineare diagonale 3x3, i cui ingressi sono (x d, y d, z d ) e le uscite (x, y, z). Di conseguenza, la dinamica è formulata in tempo discreto come x(k + 1) = α 1x x(k) + β 1x (x d (k) + x d (k)) y(k + 1) = α 1y y(k) + β 1y (y d (k) + y d (k)) z(k + 1) = α 1z z(k) + β 1z (z d (k) + z d (k)) (4.3) dove x d (k) è l incremento della posizione x desiderata, imposta all istante kt hyb, e T hyb > T s è il tempo di campionamento del controllore ibrido. Gli incrementi degli ingressi x d (k), y d (k), z d (k) sono limitati inferiormente e superiormente da una quantità [ ] ] [ ] [ xd (k) y d (k) z d (k) 1 (4.4) Il vincolo (4.4) rappresenta una manopola di sintonia per il controllore MPC ibrido, poichè permette di controllare direttamente la velocità di manovra [ del ] quadcopter imponendo vincoli sulle derivate dei riferimenti: ẋd (t) ẏ d (t) T hyb. ż d (t) Gli ostacoli sono modellati come insiemi poliedrali. Per minimizzare la complessità, l i-esimo ostacolo, i = 1,..., M, è modellato come il tetraedro [ ] x(k) xi (k) A obs k i y(k) y i (k) B obs (4.5) z(k) z i (k)

50 4.2. Pianificazione della traiettoria per l elusione di ostacoli 45 [ xyz ] dove A obs B obs è la rappresentazione di un iperpiano fisso di un ] tetraedro di riferimento, k i è un fattore di scalamento fissato, e [ xi (k) y i (k) z i (k) è un punto di riferimento dell ostacolo. Abbiamo scelto A obs, B obs in maniera tale che il corrispondente poliedro sia il guscio convesso dei vettori [ ] ] ] ] [ 00 xi ],,,, cosa che rende il vettore di riferimento y i il ver- z i 0 [ 5/2 0 0 [ 0 5/2 0 [ 5/6 5/6 5/2 tice con le più piccole coordinate. L equazione (4.5) può essere riscritta come [ ] x(k) A obs k i y(k) C obs (k) (4.6) z(k) [ ] xi (k) dove C obs (k) = B obs + A obs k i y i (k) R 4 è una quantità che può variare in z i (k) tempo reale, modellata in predizione con una dinamica costante C obs (k + 1) = C obs (k). (4.7) Infine, per rappresentare il vincolo di elusione ostacoli, definiamo le seguenti variabili binarie δ ij {0, 1}, i = 1,..., M, j = 1,..., 4 [ ] x(k) [δ ij (k) = 1] [A j obs k i y(k) C j obs (k)] (4.8) z(k) dove j indica la j-esima riga (componente) di una matrice (vettore). I seguenti vincoli logici 4 δ ij (k) = TRUE, i = 1,..., M (4.9) j=1 impongono che almeno una disuguaglianza lineare in ](4.6) debba essere violata, forzando così la posizione del quadcopter a rimanere al di fuori di ogni ostacolo. [ x(k) y(k) z(k) Con questi soli vincoli il nostro velivolo si comporta come un punto materiale senza dimensioni, seguendo traiettorie che sfiorano gli ostacoli; per tenere conto delle reali dimensioni, abbiamo dovuto creare una zona di sicurezza : ogni ostacolo è contenuto da un guscio costituito da un altro tetraedro di

51 4.2. Pianificazione della traiettoria per l elusione di ostacoli 46 Figura 4.3: Esempio di guscio che avvolge un ostacolo dimensioni maggiori (vedi Figura 4.3). Per esprimere le condizioni di obstacle avoidance di questi tetraedri sono state utilizzate le stesse variabili binarie definite in (4.8); utilizzando però gli stessi vincoli logici (4.9) sugli ostacoli e sui loro gusci si presentano problemi di non risolubilità del problema di ottimizzazione. D altronde, non è necessario imporre dei vincoli così stringenti per i gusci, in quanto essi possono essere colpiti e attraversati dai velivoli. Quindi, per ogni guscio, è stata definita una variabile ausiliaria continua 1 se 4 j=1 γ i (k) = δ ij(k) = 1 i = M/2,..., M (4.10) 0 altrimenti il cui riferimento è tenuto costantemente a zero; pesando opportunamente tali variabili, si ottiene una condizione per la quale i velivoli attraversano le zone di sicurezza senza colpire o sfiorare gli ostacoli reali. Il tempo di campionamento T hyb deve essere scelto abbastanza grande in modo da poter trascurare dinamiche transitorie veloci, in maniera tale da poter disaccoppiare il progetto dei due controllori della struttura gerarchica. D altra parte, il vincolo di obstacle avoidance (4.9) è imposto solo a istanti di tempo mutlipli di T hyb, e quindi un tempo di campionamento eccessivamente