UNIVERSITÀ DI PISA
Facoltà di Ingegneria
Laurea Specialistica in Ingegneria dell’Automazione
Tesi di laurea
Candidato:
Stefano Folcarelli
Relatori:
Prof. Andrea Caiti
Prof. Lorenzo Pollini
Dott. Francesco Di Corato
Guida ottima per un veicolo subacqueo
Sessione di Laurea del 09/05/2014 Anno accademico 2013/2014
Sommario
Lo scopo di questo lavoro è quello di realizzare una legge di guida per il veicolo subacqueo autonomo sperimentale V-Fides.
Dopo un primo lavoro di ricerca al fine di conoscere lo stato dell’arte riguardo le leggi di guida way-point, dato che le soluzioni normalmente in uso non sfruttano le caratteristiche idrodinamiche di V-Fides, perchè progettate per veicoli di tipo siluriforme, si è proceduto alla generazione di una prima legge di guida. Questa utilizza una libreria di manovre generate offline tramite l’algoritmo MPC (Model Predictive Control). Questa prima soluzione presenta limiti quali l’occupazione di memoria e la scarsa ottimalità nella scelta dell’angolo di rollio in curva. Per eliminare questi incovenienti è stata realizzata una seconda legge di guida che sfruttando l’algoritmo MPC ma con una maggior quantità di informazioni è in grado di fornire delle virate qualitativamente migliori, risolevendo però iterativamente l’algoritmo MPC a bordo del veicolo.
Abstract
The purpose of this work is to make a guidance law able for the experimental underwater autonomous vehicle V-Fides.
After a research to know the state of the art about way-point guidance laws, since most of the usual solutions do not use the hydrodynamic characteristic of V-Fides we choose to study a new one that use a library of motion primitives generated offline using MPC algorithm. This first solution has some limits like high memory usage and poor optimality choosing roll angle while turning. To avoid those drawbacks we made a second guidance law, still using MPC algorithm but with more information resulting in better turn, but the MPC need to be solved on board by the vehicle.
Indice
Sommario ... 1 Abstract ... 1 1. Introduzione ... 5 2. Panoramica ... 9 3. Modello ... 11 4. Algoritmo MPC ... 135. Prima legge di guida ... 16
5.1.Principio ... 16
5.1.1. Libreria delle Manovre ... 18
5.1.2. Composizione del percorso ... 31
5.1.3. Analisi ... 44
6. Seconda legge di guida ... 47
6.1. Principio ... 47 6.2. Problematiche da analizzare ... 48 6.3. Virata ... 49 6.3.1. Peso Q ... 51 6.3.2. Gain Scheduling ... 59 6.4. Cambio di profondità ... 63
6.5. Composizione del percorso ... 66
6.7. Esempio ... 68
6.8. Analisi Dati ... 71
6.9. Confronto fra le due leggi di guida ... 74
8. Conclusione ... 75 Bibliografia ... 76 Appendice ... 77 Algoritmo MPC ... 77 Dimostrazione ... 77 Implementazione ... 79
Libreria delle manovre... 83
Implementazione ... 83
Composizione del percorso ... 87
Implementazione ... 87
Seconda legge di guida ... 107
Ringraziamenti
Sono molte le persone che meritano un ringraziamento. Non avrei mai portato a termine un percorso di questo genere se non con il supporto della famiglia, della fidanzata e per ultimo, ma non meno importante, degli amici.
Voglio quindi ringraziare i miei genitori che hanno creduto in me e che con il loro affetto, la pazienza, oltre che con l’aiuto economico, mi hanno permesso di diventare ingegnere.
Un ringraziamento speciale va a mia sorella, sempre pronta e disponibile ad ascoltare i problemi degli altri. Devo chiederle scusa per non esserle stato abbastanza vicino nel periodo del concorso e nella lunga attesa dei risultati.
Ma ora veniamo a lei, ad Alessandra, la ragazza senza la quale non avrei mai terminato la laurea specialistica. Col rischio di essere banale, devo dire che entrando nella mia vita lei ha dato un senso a tutto quanto. Con la sua presenza è riuscita a dare colore e forma ad un modo grigio e piatto. A lei dedico questo mio lavoro perchè è per lei e non per me che ho raggiunto questo traguardo. L’ho fatto nella speranza, di poter costruire insieme a lei un futuro migliore. Il mio desiderio è quello di darle una vita piena, interessante e mai noiosa. Adesso non mi resta che rimboccarmi le maniche è afferrare il futuro che tanto desidero.
Infine voglio ringraziare gli amici, parlo di quelli veri, quelli che ti chiedono: Come và? Come stai? Come procede con gli esami? Novità per la tesi? Se poi nella tua risposta avvertono anche solo un briciolo di tristezza, cercano subito di tirarti su di morale e di farti sentire speciale. Ringrazio quindi tutti i miei amici da quelli che ho conosciuto da piccolo, appena nato in certi casi, a quelli conosciuti all’università.
1. INTRODUZIONE
Il lavoro di questa tesi consiste nella realizzazione di leggi di guida per un Autonumus Unmanned Vehicle (AUV) che opera in ambito subacqueo.
In questi ultimi anni sono stati sviluppati un gran numero di mezzi autonomi in grado di operare in ambiente sottomarino i loro impieghi vanno dal campionamento delle acque, all’effettuazione di misure di tipo batimetrico. La cosa che accomuna la maggiora parte di questi veicoli però è la forma che il più delle volte è a siluro (Figura 1), in altri casi invece è piuttosto tozza (Figura 2).
Il veicolo per il quale è realizzato questo progetto si chiama V-Fides, dal nome dell’omonimo progetto finanziato dalla regione Toscana. Il coordinatore del progetto è WASS con la partecipazione dell’Ateneo di Pisa che è responsabile del sistema di guida navigazione e controllo.
Figura 2 Pluto Plus AUV [14]
Come si può vedere da Figura 3 il mezzo è di forma triangolare e dotato di sette thruster che gli conferiscono una completa capacità di manovra nello spazio. Questo AUV è progettato con lo scopo di effettuare:
- Esplorazione del fondale - Monitoraggio ambientale - Monitoraggio di impianti civile
- Semplici operazioni robotiche (pulizia, micro-carotaggio, campionamento delle acque) L’obiettivo di questo progetto è quello di utilizzare il modello matematico del veicolo al fine di creare una traiettoria che possa fornire a V-Fides la possibilità di muoversi in modo agile. Con movimento agile si intende che data la forma triangolare del mezzo, i dati a disposizione suggeriscono che debba muoversi in modo affine a quello degli aerei, i quali hanno una maggiore agilità lungo il piano verticale rispetto a quello orizzontale. In altre parole, un aereo per virare in modo più efficiente ha bisogno di rollare per sfruttare la (momento di pitch) generata dalle ali, la quale risulta maggiore della (momento di imbardata) generata dal timone di coda. Allo stesso modo si intende far rollare V-Fides in curva dato che la sua forma ricorda più quella dei mezzi utilizzati in ambito aeronautico rispetto a quelli usati in ambito sottomarino. La sua forma triangolare lascia intuire che quindi, a differenza degli AUV subacquei normalmente usati, sia in grado di generare e sfruttare forze idrodinamiche per virare più rapidamente, nel caso in cui rolli in curva.
Per cominciare si è svolta quindi una ricerca per vedere in letteratura quali leggi di guida basate su way-point [4] vengono normalmente utilizzate. Le più semplici prevedono la generazione del percorso attraverso delle Spline che interpolino i vari way-point (Figura 5) oppure l’unione dei vari way-point con dei segmenti e il successivo smussamento degli angoli tramite archi di circonferenza (Figura 6). In questi casi si utilizza qualche conoscenza della dinamica del veicolo, normalmente il raggio di curvatura minimo, che viene cosi inserito come vincolo nelle due leggi precedenti, talvolta legandolo alla velocità di avanzamento del mezzo [8]. Altri approcci utilizzano librerie di primitive [9] che permettono di pianificare un percorso attraverso la concatenazione delle primitive contenute nella libreria. Una macchina a stati finiti chiamata Maneuver Automaton si occupa della composizione del percorso.
In alcuni casi la tecnica appena citata viene usata [10] anche per effettuare manovre complesse e delicate o per evitare ostacoli.
Altre leggi di guida trovate utilizzano algoritmi genetici [11] per la pianificazione del percorso in ambienti caratterizzati da forti correnti. Lo scopo è quello di portare il veicolo a destinazione minimizzando il consumo energetico.
Una soluzione interessante [1], dalla quale è stato preso spunto per la realizzazione della prima legge di guida, utilizza il modello del veicolo per generare off-line, tramite controllo ottimo, una libreria di primitive dette “manovre” da cui vengono poi selezionati i pezzi di traiettoria da utilizzare per comporre on-line il percorso desiderato. Per fare ciò si ricorre alla modellazione tramite elementi finiti con le complessità che ne derivano.
Le leggi di guida citate non permetterebbero però di raggiungere facilmente l’obiettivo prefissato, per questo motivo si è deciso di realizzare delle nuove leggi di guida.
Figura 5 Waypoint guidance tramite segmenti/cerchi [6] Figura 4 Waypoint guidance tramite spline [5]
2. PANORAMICA
La prima legge di guida è ispirata ad [1] e ne alleggerisce la soluzione tramite l’utilizzo di un algoritmo MPC (Model Predictive Control), ereditando però alcuni difetti già accennati nel sommario e che verranno illustrati nella sezione “5.1.3 Analisi”. I problemi della prima legge di guida vengono risolti nella seconda, attraverso l’utilizzo di una maggior quantità di informazioni nell’inizializzazione dell’algoritmo MPC. Con riferimento a Figura 6 questo progetto realizza due diverse implementazioni del blocchetto “Legge di guida”.
Come si può capire dallo schema, e come verrà illustrato meglio in seguito, l’algoritmo MPC essendo interno al blocchetto “Legge di guida”, serve solo a generare la sequenza di controllo che data in ingresso al modello del veicolo, fornisce il riferimento dello stato, , da dare in ingresso all’anello di controllo.
Questo insolito utilizzo dell’algoritmo MPC, ovvero, quello di essere utilizzato per generare traiettorie di riferimento, offre il vantaggio di avere a disposizione anche l’ingresso di controllo utilizzato nel modello. Tale segnale può essere usato come termine feed-forward nell’anello di controllo. Questo permette quindi di usare leggi di controllo ibride, che usano non solo il riferimento dello stato, ma anche quello delle variabili di controllo. In questo modo, come si vedere da Figura 7, il controllore del veicolo, realizzato con PID deve contribuire soltanto a compensare ciò che non è stato considerato nel modello usato dall’algoritmo MPC.
Normalmente questo approccio non viene utilizzato con algoritmo MPC, il quale nasce per applicazioni con una dinamica lenta. Gli attuali progressi negli strumenti di calcolo e nelle architetture embedded offrono però la possibilità di utilizzarlo in un range più ampio di problemi.
3. MODELLO
Di questo mezzo è stato fornito il modello matematico, realizzato seguendo [2]. Non essendo gli autori di questa parte di lavoro, la descrizione fornita corrisponderà al minimo richiesto per poter comprendere a fondo le problematiche da affrontare.
Il vettore di stato è il seguente:
[ ] ( 1 )
Con , ,
Chiaramente il sistema di riferimento ha le velocità in assi Body {B} mentre posizioni ed angoli sono in assi Earth {E}.
Le equazioni del modello sono le seguenti:
{ ̇ ( ) ( ) ( ) ̇ ( ) ( 2 )
con si intede la matrice delle forze/momenti di inerzia è la matrice delle forze centrifughe e di Coriolis contiene la forza di gravità e quella di galleggiamento
è la matrice delle forze idrodinamiche (portanza, resistenza...) è la matrice di rotazione per il cambio di sistema di riferimento
[ ] , -
( 3 )
La variabile di ingresso del sistema è costituita da un vettore di 7 valori compresi fra e il cui modulo è la percentuale di forza usata dall’attuatore i-esimo e il segno indica il verso della forza applicata. Nel modello è quindi presente una funzione di questo tipo:
[ ] ( ) ( 4 )
In forma compatta si può scrivere:
{ ̇ ( ) ( 5 )
Con , - stato del sistema, uscita del sistema, ingresso del sistema.
Il modello matematico è stato fornito sotto forma di implementazione Simulink [7]. Dovendo utilizzare però il sistema all’interno dell’algoritmo MPC è risultato necessario ricopiare il modello matematico in M-file al fine di renderlo più facilmente utilizzabile dalla funzione di ottimo fmincon. Per ulteriori informazioni si rimanda all’appendice (sezione “Algoritmo MPC”). In particolare lo script Crea_Sistema.m calcola la formula (2), le cui equazioni differenziali sono poi state inserite nella funzione sistema.m. A tale funzione viene passato, fra gli altri, il vettore di forze/coppie ricavato dal vettore attraverso la funzione vfides_iallocation_map che implemente la formula (4).
4. ALGORITMO MPC
Trattandosi di un sistema non lineare si parlerà più precisamente di NMPC (Non linear Model Predictive Control). Questa tecnica è basata sul metodo del controllo ottimo. In pratica si risolve un problema di ottimo in ciclo aperto su di un orizzonte finito (detto finestra di predizione) rispettando la dinamica del sistema ed i vincoli sullo stato e sugli ingressi, quindi è necessario un modello del sistema. Basandosi sulle misure e lo stato al tempo il controllore predice, grazie al modello, il comportamento futuro del sistema nella finestra di predizione e determina l’ingresso (all’interno della finestra di controllo) in grado di minimizzare la funzione obiettivo. Per rendere il problema di minimo risolvibile numericamente si utilizza un ingresso discreto con tempo di campionamento . Ciò che ne risulta quindi è un ingresso di controllo costante a tratti. Normalmente della variabile di controllo calcolata si applica solo il valore corrispondente al primo tempo di campionamento, si scarta il resto, e si ripete la procedura di ottimizzazione partendo dal nuovo stato raggiunto. In questo modo la finestra di predizione può avanzare nel tempo.
Fra le numerose versioni di NMPC è stata utilizzata quella che prevede l’utilizzo di “contractive constraint” e l’applicazione dell’intera finestra di controllo calcolata [3]. Questo vincolo deriva da un approccio alla Lyapunov in cui una funzione di Lyapunov scelta a priori viene decrementata in modo discreto (quindi può anche crescere all’interno dell’orizzonte di predizione). In altre parole lo stato alla fine della finestra di predizione deve essere più vicino all’equilibrio di quanto non fosse all’inizio, senza però vincolare il comportamento intermedio.
Lo schema del sistema quindi è il seguente:
L’algoritmo di controllo MPC è in grado di portare il sistema sull’equilibrio nell’origine. Se però il punto di equilibrio è un altro, questo non costituisce un problema. Infatti è sufficiente traslare il sistema nel seguente modo.
Se sono rispettivamente lo stato e l’uscita corrispondenti ad un ingresso , cioè:
̇ ( ) . ( ) ( )/ ( ) . ( ) ( )/
( 6 )
Col cambio di variabili:
̃( ) ( ) ( ) ̃( ) ( ) ( ) ̃( ) ( ) ( )
( 7 )
Si ottiene una nuova rappresentazione:
̃̇ ( ( ) ̃ ̃) . ( ) ( )/ ( ̃ ̃) ( 8 )
Il nuovo sistema ammette il regime stazionario ̃ ̃ ̃ , corrispondente al movimento .
Il tipo di algoritmo MPC usato prevede l’utilizzo della seguente funzione di costo:
∫ ‖ ̃ ( )‖ ∑‖ ̃( | )‖ ∑‖ ̃( | )‖ ( 9 )
In questo modo si pesa l’errore dello stato, il controllo e l’eccessiva variazione di controllo. I vincoli usati sono i seguenti:
{ ̃̇ ( ̃ ( ) ̃ ( )) ̃ ̃( | ) ̃ | ̃( | )| ̃ ‖ ̃ ( )‖ ‖ ̃ ‖ ( 10 )
La prima equazione indica che bisogna rispettare il vincolo imposto dalle equazioni del sistema. La prima disequazione limita i valori massimi di forza degli attuatori, la seconda limita la variazione massima di forza fra un tempo di campionamento ed il successivo mentre la terza ed ultima è il “contractive constraint” che impone allo stato di avere un valore minore alla fine della finestra di predizione rispetto a quello che aveva all’inizio della stessa. Nel caso in cui si utilizzi una finestra di predizione/attuazione molto piccola il contractive constraint rischia di essere violato in presenza di overshoot, motivo per il quale è necessario tarare bene i valori delle varie matrici di peso. Questo ultimo vincolo serve ad assicurare la A.S. (Asintotica Stabilità) del sistema. La dimostrazione di A.S. e l’implementazione dell’algoritmo MPC sono rimandate all’appendice
5. PRIMA LEGGE DI GUIDA
5.1.Principio
La legge di guida sviluppata è di tipo Way-point, quindi assegnati un set di punti viene richiesto al veicolo di passare da questi traguardi entro un certo range seguendo l’ordine fissato dall’utente.
L’approccio usato è quello del “Divide et impera”. Si divide cioè il problema in due fasi, risolvendo separatamente le parti del problema. La prima è quella offline (parte a sinistra del
“libreria”). Quella offline, verrà illustrata successivamente in modo da renderla più comprensibile.
La parte online prevede l’acquisizione dei way-point con successivo congiungimento dei vari punti attraverso segmenti. Si ottiene cosi una spezzata che rappresenta il percorso più breve in grado di passare dai vari traguardi. Nei vari way-point però la spezzata presenta degli “spigoli”, ovvero delle discontinuità nell’angolo di heading. A questo punto risulta necessario quindi smussare gli angoli.
Normalmente questo viene fatto con archi di circonferenza i quali presentano una discontinutà nella velocità angolare, ovvero nel passaggio dal moto rettilineo uniforme della spezzata a quello circolare uniforme dell’arco di circonferenza. In questo progetto si utilizzano invece degli accoppiamenti manovra / trim / manovra calcolati offline nella prima fase tramite algoritmo MPC.
È necessario illustrare il concetto di manovra e quello di trim. La maggior parte dei veicoli creati dall’uomo alterna fasi in cui si applica un algoritmo di controllo (in auto equivale a girare il volante per entrare in curva) a fasi di equilibrio in cui le grandezze di controllo non vengono modificate (in auto si mantiene il volante girato con angolo costante mentre si percorre la curva). Le prime verranno chiamate ora manovre (sono dei transitori), le seconde invece saranno definite come trim (sono dei moti di regime). Per virare quindi è necessario effettuare:
1) una manovra che porti il veicolo da una velocità angolare nulla a quella desiderata 2) il mantenimento del trim (ovvero ) per la durata della curva
3) la successiva manovra di uscita dalla curva per riportare la velocità angolare a zero
In altre parole, per passare dal moto rettilineo uniforme (trim) a quello circolare uniforme (trim) della curva, si hanno dei transitori (manovre) necessarie per passare da un tipo di moto all’altro.
Queste manovre vengono generate tramite la soluzione di un problema di tipo MPC.
La generazione offline di una libreria di manovre permette di relegare ad una fase preliminare (da eseguire solo una volta) la maggior parte degli oneri computazionali.
Andando ad analizzare più nel dettaglio ecco illustrata ora la procedura.
Partendo dalla posizione nell’origine, con veicolo orientato verso Nord si genera una manovra impostando nell’algoritmo MPC lo stato finale desiderato ovvero, il moto circolare uniforme, definito da velocità lineare ed angolare, nonchè angolo di rollio, da mantenere in curva. Ripetendo la procedura per stati finali (di regime) diversi si può ottenere una libreria di manovre più o meno ricca nella quale sono contenuti “pezzi” di traiettoria in grado di portare il veicolo dal moto rettilineo a quello circolare e vice versa. Questa prima parte è chiaramente quella eseguita off-line e si conclude col salvataggio delle “manovre” nella libreria.
La seconda parte, quella online, inizia con l’acquisizione dei way-point. Una volta ricevuto il set di punti, si genera un primo percorso che li unisce con dei segmenti. Gli angoli sui vari way-point vanno a questo punto smussati. Questa operazione come già accennato non viene eseguita con degli archi di circonferenza, si ricorre invece alle “manovre” precedentemente salvate nella libreria. Concoscendo l’angolo di cui deve virare il veicolo per puntare al way-point successivo si possono calcolare i tempi necessari ai vari trim per effettuare la virata e quindi si può risalire allo spazio percorso nel curvare. Unendo manovra di ingresso / trim / manovra di uscita si ottiene cosi un poligono che permette di calcolare i punti di “aggancio” della virata (punti A,D Figura 12). Fra le varie triplette di manovra / trim / manovra, si seleziona quella desiderata. I criteri possibili sono diversi, dal tempo di percorrenza minimo, alla distanza minima dal way-point oppure si può scegliere quella che utilizza meno energia nel controllo.
5.1.1. Libreria delle Manovre
La libreria è composta da manovre necessarie per eseguire virate, e da manovre usate per effettuare cambi di profondità. Queste ultime si dividono in due categorie, quelle che eseguono una rampa e quelle che eseguono una spirale. Verranno ora illustrate le procedure usate, mentre per l’implementazione completa si rimanda all’appendice.
5.1.1.1.Virata
Ricordando che il vettore di stato è:
[ ] ( 11 )
Le velocità lineari ed angolari sono in assi Body, mentre posizione ed assetto in assi Earth.
5.1.1.1.1. Manovra di ingresso in curva
Come già accennato, questo è il transitorio in grado di portare il veicolo da moto rettilineo a moto circolare uniforme. Per questo motivo viene scelta come condizione finale quella di mantenere costante la velocità di surge, nulle quelle di sway ed heave e che permetta di raggiungere la velocità angolare e l’angolo di desiderati:
0 1 ( 12 )
Nulla viene imposto su poichè a regime non manterranno valori costanti. Questo si realizza ponendo a zero il loro peso nel funzionale di costo attraverso la matrice che pesa lo stato, formula (9), e nel calcolo del contractive constraint grazie alla matrice , peso dello stato nel vincolo contractive constraint, formula (10). Quando si parla quindi di condizione di equilibrio si dà per scontato che questa riguarda in realtà il sottosistema che comprende le variabili di stato che a regime mantengono valore costante, nel caso dell’ingresso in curva quindi vengono tagliate fuori .
In base all’angolo di roll ( ) per avere una desiderata viene calcolato il valore di dove con l’apice si indicano gli assi Body mentre con quelli Earth.
Per quanto riguarda invece il vettore di stato iniziale, questo è un vettore tutto nullo esclusa che equivale a dire, veicolo nell’origine, che punta verso Nord con angoli di assetto nulli e velocità di surge .
0 1 ( 13 )
Una volta definiti è possibile tramite il cambio di varibili introdotto nella sezione sul controllo MPC, ̃ , trovare lo stato iniziale del sistema ̃̇ con equilibrio traslato nell’origine. Si calcola poi come l’ingresso tale che ( ) che rappresenta in pratica il vettore delle forze/coppie in grado di mantenere il veicolo alla velocità angolare con assetto costante. Questi sono i dati necessari ad impostare il modello. Per quanto riguarda la funzione di costo, formula (9) è stata utilizzata solo la parte relativa al vettore di stato, annullando il contributo di ̃ ̃. Quindi è stata usata la sola matrice di questo tipo:
con nulli in modo da lasciare libera l’evoluzione di .
La potenza dei thruster è stata ridotta del 15% ( tramite il vincolo ̃ ̃( | ) ̃ , formula (10)) in modo tale da lasciare parte della potenza a disposizione dell’algoritmo di controllo del veicolo reale ed evitare cosi errori di inseguimento troppo grandi nel caso di disturbi esterni o di dinamiche non modellate. Dato che ̃( ) ( ) ( ) si ottiene che:
̃ ̃ , - ( 15 )
Sempre parlando dei thruster è stata fissata ̃ in modo tale che per passare dall’erogazione di massima forza in una direzione, a quella di massima forza in direzione opposta, gli attuatori necessitino di 4 tempi di campionamento ovvero 4 secondi (seconda disequazione formula (10)).
Per quanto riguarda l’ultimo vincolo, il contractive constraint, la matrice è analoga a ovvero con nulli in modo da lasciare libera l’evoluzione di , terza disequazione formula (10).
Per rendere relativamente rapide le computazioni è stato scelto il tempo di campionamento e finestra di predizione e di controllo relativamente .
5.1.1.1.2. Trim
Il mantenimento della curva, il trim, non necessita di molti calcoli nella fase offline, dato che si tratta di mantenere il veicolo in moto circolare uniforme, ovvero con calcolato nella manovra di ingresso in curva. Quindi è sufficiente assicurarsi che l’ appena citato sia minore della erogata dagli attuatori.
5.1.1.1.3. Manovra di uscita dalla curva
Questo è il transitorio in grado di portare il veicolo dal moto circolare (con della manovra di ingresso in curva) al moto rettilino con assetto orizzontale. Il problema MPC è del tutto analogo a quello dell’ingresso in curva basta solamente scambiare con dato che la condizione di regime in uscita dalla curva corrisponde alla condizione iniziale di ingresso in curva (moto rettilineo uniforme) mentre invece la condizione iniziale di uscita dalla curva corrisponde alla condizione di regime dell’ingresso in curva (moto circolare uniforme e relativo angolo di rollio che sono stati mantenuti durante il trim).
5.1.1.1.4. Esempi
Per rendere più comprensibile il tutto è necessario mostrare degli esempi. A seguire è mostrata una curva di con velocità di ⁄ .
Quella in rosso è una virata eseguita con la sola conoscenza del raggio di curvatura del veicolo e che quindi dalla condizione di moto rettilineo passa direttamente a quella di moto circolare, come nel primo approccio illustrato di Figura 5. Quella in blu/verde/celeste è un esempio di virata eseguita assemblando manovra di ingresso in blu, trim in verde e manovra di uscita in celeste, presenti nella libreria e quindi calcolate con algoritmo MPC e modello del veicolo.
La legge di guida qui proposta è tutt’altro che perfetta. Nel trim (verde) il veicolo si muove (come da definizione) di moto circolare uniforme, non si applica quindi ulteriore controllo, ma i thruster vengono utilizzati solo per mantenere il mezzo in traiettoria. É evidente che se fino a metà curva il veicolo manovrasse per aumentare la velocità angolare di virata, e nella seconda metà invece per diminuirla, allora si avrebbe una curva che viene eseguita in tempo/spazio minori. Questo inconveniente (ovvero quello di avere lunghi trim) può essere ridotto ampliando la libreria di manovre e inserendone alcune capaci di portare il veicolo a velocità angolari maggiori. Arricchendo la libreria si potrebbe quindi ridurre il tempo di mantenimento dei trim.
Un secondo inconveniente è dato dal fatto che le due manovre di ingresso e di uscita da una curva comportano una variazione di heading, che nel caso mostrato in Figura 14 è di circa
.
Figura 13 Confronto fra una virata eseguita con arco di cerchio (rosso) ed una realizzata con le manovre
Questo significa che utilizzando quella coppia di manovre non si può eseguire una virata di angolo inferiore ai 36°. In pratica ogni coppia di manovre comporta un angolo minimo di virata. Questo angolo minimo aumenta al crescere della velocità angolare desiderata per il trim. Quindi, più si cerca di far virare velocemente il veicolo e maggiore è l’angolo minimo di virata. Per poter virare liberamente dell’angolo desiderato si può in primo luogo ampliare la libreria ed inserire (come è stato fatto) quindi delle manovre che portino il mezzo ad una velocità angolare di trim inferiore. La virata dell’esempio è eseguita ad una velocità angolare di , dimezzando la velocità angolare a si attengono delle manvore di ingresso/uscita che comportano una variazione di heading di circa , per scendere fino ad un virata di con variazione di heading di . Chiaramente questo non risolve completamente il problema. Per poter virare liberamente dell’angolo desiderato è necessario eseguire, prima della manovra di ingresso in curva, una manovra che corregga la variazione di heading di ingresso/uscita dalla curva come mostrato in Figura 15.
Figura 14 Manovre di ingresso (blu) e di uscita (celeste) da una curva. In evidenza gli heading change delle
In parole semplici, dato che la coppia di manovre di Figura 14 causa un cambio di heading di 36° allora è necessario anteporre una virata di -36° e ne andrebbe calcolata una per ogni coppia di manovre presenti nella libreria. Si ottiene cosi una variazione di heading dovuto alle manovre di . In questo modo però il pacchetto di manovre occuperà più o meno il doppio della memoria. In alternativa, con riferimento a Figura 15, è possibile anteporre le manovre di ingresso/uscita dalla curva facendone lo speculare rispetto al piano ( colori blu e celeste tratteggiati), dato che il veicolo è simmetrico rispetto ad esso. In altre parole per virare a destra prima si usano le manovre di ingresso/uscita dalla curva in direzione opposta (verso sinistra nel caso in esame) in modo da eseguire una pre-virata verso sinistra (linee blue e celesti tratteggiate) per poi eseguire la manovra di ingresso in curva (blu continua) e poi quella di uscita dalla curva (celeste continua). In pratica Figura 15 mostra che la virata realizzata anteponendo le due manovre di ingresso/uscita in direzione opposta (linee tratteggiate) fà si che le quattro manovre, due di ingresso e due di uscita dalla curva non producano heading change offrendo quindi la possibilità di virare dell’angolo desiderato interamente durante il trim come mostrato nel prossimo esempio. La traiettoria ottenuta richiederà uno spazio di percorrenza maggiore e quindi uno spazio minimo maggiore fra i vari way-point, come mostrato in Figura 16.
Figura 15 Manovre di ingresso/uscita dalla curva, con correzione di heading (linee tratteggiate) e senza correzione di heading
In certi casi però questa correzione di heading ha come effetto collaterale quello di poter aiutare a passare più vicino al waypoint come mostrato in Figura 17.
Figura 16 Virate di a confronto: con correzione di heading, senza correzione, arco di cerchio
Questa ultima soluzione proposta, quella della correzione di heading (di seguito chiamata heading fix), è stata utilizzata ma vi è anche un’altra soluzione che verrà illustrata nella sezione sulla composizione del percorso.
5.1.1.2. Cambio di profondità
Nel caso in cui il veicolo debba cambiare profondità si possono seguire due tipi di approcci. Il primo è quello di eseguire una rampa mentre il secondo consiste nel cambiare profondità attraverso un movimento a spirale. Nella sezione inerente alla composizione del percorso verrà spiegato quando si usano traiettorie generate con il primo metodo e quando quelle derivanti dal secondo.
Nel cambio di profondità tramite rampa la prima manovra consente di passare dall’angolo di nullo a quello desiderato, mentre la seconda riporta l’angolo di a valore nullo partendo da quello mantenuto durante la rampa.
Rispetto alle manovre di virata la metodologia rimane la stessa, per cui a seguito verranno indicate solo le differenze, che sono limitate ai valori di .
5.1.1.2.1. Manovra di ingresso rampa
Per eseguire una rampa lo stato finale della manovra di ingresso è:
0 1 ( 16 )
Il vettore di stato iniziale è analogo a quello dell’ingresso in curva, tutto nullo esclusa che equivale a dire, veicolo nell’origine, che punta verso Nord con angoli di assetto nulli.
0 1 ( 17 )
La matrice di peso dello stato nella funzione di costo è:
( ) ( 18 )
con nulli in modo da lasciare libero l’avanzamento del mezzo ed il suo cambio di profondità.
La matrice del contractive constraint è analoga a ovvero con nulli.
5.1.1.2.2. Trim
Il mantenimento della rampa, il trim, non necessita di molti calcoli nella fase offline, dato che si tratta di mantenere il veicolo in moto rettilineo uniforme, ovvero con calcolato nella manovra di ingresso sulla rampa. Quindi è sufficiente assicurarsi che l’ appena citato sia minore della erogata dagli attuatori.
5.1.1.2.3. Manovra di uscita dalla rampa
Anche in questo caso è sufficiente scambiare fra loro i valori attribuiti a nella manovra di ingresso alla rampa, in modo da riportare il veicolo nella condizione iniziale.
5.1.1.2.4. Manovra di ingresso spirale
Questo transitorio deve portare il mezzo in una condizione di regime data dall’insieme delle due precedenti, ovvero si tratta di una virata più una rampa. Il vettore di riferimento dello stato è:
0 1 ( 19 )
Il vettore di stato iniziale è analogo a quelli precedenti, tutto nullo esclusa che equivale a dire, veicolo nell’origine, che punta verso Nord con angoli di assetto nulli.
0 1 ( 20 )
La matrice di peso dello stato nella funzione di costo è:
( ) ( 21 )
con nulli in modo da lasciare liberi sia la posizione del veicolo nello spazio che il suo angolo di heading.
La matrice del contractive constraint è analoga a ovvero con nulli.
5.1.1.2..5. Trim
Il mantenimento della spirale, come gli altri trim, non necessita di molti calcoli nella fase offline, dato che si tratta di mantenere il veicolo in moto rettilineo uniforme lungo l’asse e circolare uniforme nel piano , ovvero con calcolato nella manovra di ingresso nella spirale. Ancora una volta è sufficiente quindi assicurarsi che l’ sia minore della erogata dagli attuatori.
5.1.1.2.6. Manovra di uscita dalla spirale
Questo trasitorio è un po’ differente da quelli precedentemente usati per l’uscita dai vari trim, infatti questo serve a portare il veicolo da un movimento a spirale ad uno circolare, ovvero riporta il veicolo ad un angolo di pitch nullo lasciando inalterata la velocità angolare. In questo modo si entra in una curva (trim) dalla quale si uscirà poi con una delle manovre di uscita dalla virata precedentemente illustrate. Il motivo verrà descritto nella sezione che tratterà la composizione delle traiettorie.
0 1 ( 22 )
Il vettore di stato iniziale coincide con il della manovra di ingresso nella spirale:
0 1 ( 23 )
Le matrici di peso e sono analoghe a quelle di ingresso nella spirale ovvero con nulli.
5.1.1.2.7. Esempi
L’esempio mostrato è quello di una rampa che comporta una aumento di profondità di con velocità di ⁄ .
La manovra di ingresso alla rampa è in blu, quella di uscita è in celeste, mentre la rampa vera e propria è in verde ed ha un angolo di . Le due manovre da sole comportano già una cambio di profondità di circa il che implica come mostrato in Figura 19, che questa coppia di manovre non può essere usata per dislivelli inferiori ai .
Nella libreria generata durante lo sviluppo del progetto la rampa con pendenza minore, che quindi comporta un delle manovre minimo, ha un angolo di ed una sia per rampe in discesa che in salita. Per poter cambiare la profondità in totale libertà è necessario anteporre un cambio di profondità pari a . Questa soluzione non è stata adottata perchè aumenterebbe la dimensione della libreria. A differenza del caso della virata, non si può creare una manovra correttiva che dia semplicemente facendo lo speculare delle manovre di Figura 19 rispetto al piano questo perchè il veicolo non è simmetrico rispetto al piano in questione. Nel caso in cui la variazione di profondità sia inferiore ai non vengono quindi utilizzate manovre, ma si inserisce direttamente un gradino.
Come ultimo esempio ecco quello relativo ad un cambio di profondità/virata eseguito con una spirale. Nello specifico si ha una diminuzione di profondità di ed una virata di , sempre con velocità di surge di ⁄ .
In Figura 20 è visibile l’ingresso nella spirale in blu, la spirale (trim) in verde, l’uscita dalla spirale ed ingresso nella virata in celeste, la virata in rosso (trim) e l’uscita dalla virata in magenta. La spirale ha un angolo di ed una velocità angolare ⁄ . Nel passaggio da spirale a virata si porta a zero l’angolo di e si lascia invariata la . La spirale con angolo di pitch minore, presente nella libreria, richiede un dislivello minimo sia in salita che in discesa, quindi per variazioni di profondità minori viene inserito anche in questo caso un gradino.
Mostrate tutte le possibili combinazioni di manovre verrà ora illustrato l’algoritmo di composizione del percorso.
5.1.2. Composizione del percorso
L’algoritmo di composizione del percorso, necessita di tre punti: uno di partenza ( ), un way-point ( ) ed uno di arrivo ( ). Una volta generato il path fino al superamento del way-point ( ) è possibile aggiungere un nuovo punto ( ), trasformando cosi l’ultimo punto del percorso nel nuovo punto di partenza ( ), il precedente punto di arrivo nel prossimo way-point ( ), ed il nuovo punto ( ) diventerà il nuovo traguardo.
In questo modo è possibile cominciare a generare il percorso prima di averlo completamente pianificato oppure consente di modificarlo fino all’ultimo momento, ovvero fino a quando non si arriva al nuovo punto di partenza .
Ecco a seguire la descrizione dell’algoritmo a partire da quello per eseguire una virata, per passare poi a quello per il cambio di profondità.
5.1.2.1. Curva
Con riferimento a Figura 22, partendo dalla condizione di moto rettilineo ( ) in direzione è necessario individuare la trasizione in grado di portare il veicolo nella condizione di moto rettilineo ( ) in direzione . Questo comporta la conoscenza dell’angolo di variazione di heading , angolo compreso fra i due moti rettilinei. La trasizione realizzata con il minor numero di manovre è quella che prende dalla libreria delle virate la manovra che porta il veicolo dalla condizione di moto rettilineo a quella di moto circolare uniforme che và mantenuta per il tempo , dopo di che inizierà la manovra che porterà il veicolo nella condizione finale .
I punti di cambio fra manovre e trim sono indicati con le lettere mentre invece con le linee continue sono rappresentati i trim ( , moti di regime), mentre le linee tratteggiate indicano le manovre ( , transitori).
Figura 22 Composizione di una virata
Figura 23 Heading change delle manovre di ingresso/uscita dalla curva
Ricordando che le manovre ed comportano una variazione di heading rispettivamente di e ( finale delle manovre di ingresso/uscita dalla curva, generate a partire da veicolo che punta verso Nord) si ottiene che il costing time è quel tempo di mantenimento del moto circolare uniforme , necessario a virare di mantenendo velocità angolare costante . Quindi:
( 24 )
Nel caso in cui si ha tempo negativo. Questo significa che gli heading change delle manovre di ingresso/uscita dalla curva comportano da soli una virata maggiore di quella richiesta, e quindi si ha lo scarto della tripletta manovra / trim / manovra in esame.
Dopo aver calcolato , tempo di mantenimento del trim, è possibile calcolare le distanze ed che identificano i punti rispettivamente di ingresso ed uscita dalla curva.
Definiti i versori come i versori che puntano rispettivamente da a e da a è possibile calcolare i vettori che costituiscono 3 lati del poligono :
Figura 24 Heading change delle manovre di ingresso/uscita dalla curva
̅̅̅̅ ( ) ̅̅̅̅ ( ) . / ̅̅̅̅ . / ( 25 )
Dato che le manovre sono state pre-calcolate offline partendo da veicolo puntato verso Nord, è necessario ruotarle dell’angolo iniziale attraverso la matrice di rotazione ( ). Nella rotazione del trim invece è necessario aggiungere anche la rotazione di , angolo di variazione di heading durante la manovra . Come si può vedere da Figura 24 con si intende il vettore che và dal punto iniziale a quello finale della manovra , mentre è quello relativo alla manovra .
Con si indica la DCM (Direction Cosine Matrix) relativa all’inizio del trim , mentre è la velocità in assi Body mantenuta durante il trim. La è il tensore che lega la velocità angolare, il vettore di rotazione e la sua velocità ( ) ̇ [1]. In generale si ha:
( ) ∫ ( ) ( 26 )
In pratica la parte . / serve a calcolare il vettore distanza complessivamente percorso durante il moto circolare uniforme (trim). Questo permette di calcolare quindi anche la distanza ̅̅̅̅ percorsa durante .
Si può a questo punto scrivere l’equazione di chiusura del poligono:
( 27 )
Ricavando cosi le due incognite ed attraverso la formula:
( ) [ ( ) ( ) ] ( 28 ) Con
È necessario a questo punto scartare tutte le triplette di manovra / trim / manovra che presentano delle coppie , maggiori dello spazio disponibile fra i vari way-point. Questo controllo viene svolto valutando sul solo piano le disequazioni:
‖ ‖ ‖ ‖
( 29 )
Questa procedura viene eseguita per tutti i set di triplette manovra / trim / manovra. Riassumendo le varie triplette devo rispettare i vincoli:
1) | | | | di heading change delle manovre minore dell’heading change richiesto per la virata.
2) ‖ ‖ , ‖ ‖ di virata sufficientemente rapida da “entrare” nello spazio fra la posizione attuale e i due way-point.
3) Percorso che passa entro il range di due volte la lunghezza del veicolo dal way-point. Nel caso in cui ciò non accada allora è sufficiente virare in direzione opposta facendo un giro completo ripetendo cioè la procedura appena illustrata ma con . Il risultato è mostrato in Figura 25.
Quest’ultima soluzione seppur non molto elegante offre la possibilità di avere sempre delle triplette accettabili fra cui scegliere perchè permette di virare dell’angolo desiderato dopo il passaggio dal waypoint, molto utile quindi nel caso di punti molto vicini fra loro.
Oltre a questa seconda serie di triplette di manovre e trim vengono calcolate anche quelle con heading fix (Figura 26), che antepongono cioè le manovre ed in direzione opposta, in modo da avere una variazione di orientazione dovuta alle manovre che risulti complessivamente nulla.
A questo punto è sufficiente prendere la tripletta desiderata fra tutte quelle generate. Il criterio usato è quello del tempo minimo di percorrenza da a . Potrebbero essere usati in alternativa criteri di minima energia o di minima distanza di passaggio dal waypoint.
5.1.2.2. Cambio di profonfidà
Nel caso in cui la posizione attuale ed il way-point successivo si trovino a profondità diverse, allora è necessario, prima dell’eventuale virata, eseguire un cambio di profondità.
Nella libreria generata offline esistono 2 tipi di soluzioni diverse a questo problema. Il primo metodo, quello della rampa, viene utilizzato nel caso in cui fra la posizione attuale ed i way-point vi sia spazio sufficiente per una rampa e l’eventuale virata. Il secondo metodo, quello della
Per primo viene descritto l’algoritmo di inserimento della rampa per il quale Figura 27 è presa a riferimento.
Partendo dalla condizione di moto rettilineo ( ) in direzione ma a profondità è necessario individare la trasizione in grado di portare il veicolo nella condizione di moto rettilineo ( ) sempre in direzione ma a profondità .
Per prima cosa viene calcolato di quanto è necessario salire/scendere di profondità:
( 30 )
Per ogni tripletta di manovra / trim / manovra si calcola la durata del trim di salita (moto rettilineo uniforme con angolo di ):
. /
( 31 )
Con si intendono le variazioni di profondità ottenute eseguendo le manovre di ingresso/uscita dalla rampa.
Con il tempo è possibile calcolare la distanza percorsa durante il trim:
( 32 )
da cui la distanza complessiva percorsa durante il cambio di profondità risulta:
( 33 )
Figura 28 Cambi di profondità delle manovre di ingresso/uscita dalla rampa
dove con e si intendono rispettivamente le distanze percorse durante le manovre di ingresso/uscita dalla rampa, ovvero la loro posizone finale (essendo generate partendo dall’origine).
Fra le varie triplette viene scelta in questo caso, quella che richiede il minor tempo di esecuzione a patto che sul piano si rispetti la condizione:
‖ ‖ ( 34 )
ovvero che fra la posizione attuale ( ) ed il way-point ( ) vi sia spazio sufficiente a garantire l’inserimento della rampa e della eventuale virata successiva.
Nel caso in cui nessuna rampa di quelle in libreria fosse in grado di rispettare tale vincolo, si renderà necessario utilizzare una spirale invece di un rampa più una virata. Quest’ultima infatti consente di superare il way-point, effettuare il cambio di profondità e poi correggere l’heading continuando con una virata. Viene ora illustrato più nel dettaglio l’algoritmo con Figura 29 da prendere come riferimento.
Analogamente al caso della rampa si calcola l’altezza di cui variare la profondità come:
( 35 )
Conoscendo le variazioni di profondità dovute alle 3 manovre si ha che il tempo di mantenimento del moto a spirale è:
. /
( 36 )
Volutamente è stato omesso il termine poichè è sempre nullo (corrisponde all’uscita dalla virata ed ingresso in ).
Dato il tempo si ottiene l’heading change della spirale:
(
)
( 37 )
A questo punto è possibile calcolare l’angolo da coprire durante la virata che risulta essere:
( ) . / ( 38 )
La prima parte ( ) è l’angolo totale di virata, girando in direzione opposta in modo da poter passare direttamente da , la seconda parte ( ) è la somma degli heading change dovuti alle tre manovre più il trim a spirale.
Il tempo di mantenimento della virata risulta essere:
( 39 )
I versori sono tali da puntare rispettivamente da a e da a . A questo punto è possibile calcolare i vettori che costituiscono cinque dei lati del poligono proiettato sul piano :
̅̅̅̅ ( ) ̅̅̅̅ ( ) . / ̅̅̅̅ . / ̅̅̅̅ . / ( ) ̅̅̅̅ . / ( 40 )
Analogamente al caso della virata, di formula (25), le sono rotazioni necessarie a portare i vari vettori nella giusta orientazione, poichè tutti sono calcolati a partire da angolo di yaw nullo, cioè puntando verso Nord. Le invece facendo riferimento a formula (26) servono a calcolare il vettore distanza percorso durante i trim di virata e spirale.
Si può quindi scrivere l’equazione di chiusura del poligono sul piano :
( 41 )
Come nel caso della semplice curva si ricavano le due incognite ed attraverso la formula:
( )
[
( )
( ) ]
5.1.2.3. Esempio
Viene ora mostrato un esempio di percorso costituito da 6 punti, per simulare l’inizio di un percorso di tipo snake, normalmente usato per scansionare il fondale in cerca di reperti o altro.
Con riferimento a Figura 30, il percorso parte dall’origine, quadratino blu, i way-point sono i quadratini verdi, mentre quello rosso è il traguardo. I cerchi color magenta intorno ai way-point rappresentano il range di due volte lunghezza veicolo. Le virate sono tutte di e si alternano virate con heading fix a quelle con loop inverso. Il motivo per cui non vengono usate le curve dirette è che per virate di 90° passerebbero troppo lontane dai way-point. Le virate con loop inverso sono necessarie perchè non vi è spazio sufficiente fra un punto ed il successivo.
Per quanto riguarda Figura 31, si può vedere in corrispondenza della prima virata una picco negativo della velocità di imbardata e subito dopo un suo aumento positivo, indice del fatto che è stata inserita una virata con heading fix. Durante le virate sono facilmente individuabili i trim, ovvero le parti in cui la velocità di imbardata è costante.
Di questo percorso è presente anche un filmato realizzato con MATLAB, per i dettagli si rimanda all’appendice.
5.1.3. Analisi
Per prima cosa è interessante vedere dal grafico di Figura 32, che tutte le manovre presenti nella libreria vengono utilizzate in almeno un percorso.
Il verde indica la virata diretta, il blu quella con heading fix, mentre il rosso quella con loop inverso. In caso contrario sarebbe stato possibile eliminare la manovre superflue.
Dal grafico di Figura 33 invece si può vedere che nell’eseguire una certa virata, il tempo dei vari percorsi presenta delle differenze non eccessive fra manovre con stessa velocità angolare ma diverso angolo di rollio.
Questa legge di guida presenta un utilizzo innovativo dell’algoritmo MPC, qui adottato per generare pezzi di traiettoria, invece del suo più comune impiego negli algoritmi di controllo. Adesso è il caso di analizzare i vantaggi e gli svantaggi di questa legge di guida
Pro:
1) Il percorso contiene un riferimento dell’intero vettore di stato, e delle forze erogate dai thruster. Questo permette di utilizzare una legge di controllo ibrida.
2) I tempi richiesti per generare un percorso sono estremamente bassi. 3) Se necessario si può imporre di virare mantenendo nullo l’angolo di rollio.
Contro:
1) È richiesta una certa quantità di memoria per la libreria di primitive (manovre). L’attuale versione piuttosto scarna e con campioni ogni 1s è di circa 200kb in file .mat. Portando il tempo di campionamento dell’MPC a 0.1s salirebbe a circa 2Mb. Arricchendola con più manovre crescerebbe rapidamente. Al momento vi sono Virate a 2°/s e 4°/s entrambe eseguite con angoli di rollio pari a 0°,5°,10°,15° e Salite/Discese a 5°,10°,15°,20°. Si può intuire quindi che una libreria accurata richiederebbe ben più una decina di Mb. Per ridurre lo spazio occupato in memoria sarebbe opportuno salvare le sole Fi (7 variabili
invece di 12+7), cioè salvare gli ingressi invece dello stato, ed utilizzare il modello del sistema per ricavare lo stato durante la composizione delle manovre, operazione che attualmente viene eseguita nella fase offline.
2) Il veicolo durante le virate rolla di angoli fissi e pre-definiti, scelti senza alcun criterio di ottimalità.
3) Situazioni particolari come ad esempio waypoint troppo vicini, possono essere un problema. Ma dipende in larga parte dalla lenta dinamica del veicolo e quindi non sono risolvibili in modo migliore da altre leggi di guida.
4) Per angoli di virata grandi il veicolo passa troppo lontano dal waypoint, la soluzione usata è quella dell’effetuare un loop in direzione opposta rendendo però notevolmente più lungo il percorso.
L’ingombro di memoria, e la scelta a priori dell’angolo di rollio nella virata sono due problemi che possono essere eliminati sviluppando una nuova legge di guida interamente online, che risolva quindi il problema MPC una volta scelti i way-point. Questo và però a discapito dei tempi di computazione i quali possono essere ampiamente ridotti una volta che il codice del software viene compilato. Viene quindi ora illustrata la nuova idea di legge di guida con ciò che si prefigge ed i vari ostacoli da dover affrontare.
6. SECONDA LEGGE DI GUIDA
6.1. Principio
Questa legge di guida è di tipo Way-point come la precedente, con la differenza sostanziale che viene eseguita interamente una volta acquisito il set di punti, WP di Figura 34.
Figura 34 Schema a blocchi della seconda legge di guida
Questo offre la possibilità di utilizzare, nell’algoritmo MPC, dati quali l’angolo di virata fra due way-point o la differenza di profondità. Ciò che ne risulta è, ad esempio nel caso della curva, un percorso durante il quale il veicolo non mantiene mai una velocità angolare costante, quindi, a differenza di prima, non vi sono moti uniformi (trim). Utilizzando quindi una maggior quantità di informazioni ci si aspetta di ottenere risultati migliori. Come nella precedente legge di guida si utilizzano poi le parti di percorso generate con algoritmo MPC per assemblare la traiettoria. Riassumendo quindi si usa ancora l’algoritmo MPC, ma online, e la manovra che ne viene generata è ad-hoc per i way-point in questione. La composizione del percorso attraverso l’algoritmo di inserimento delle virate e dei cambi di profondità sulla spezzata passante dal set di punti rimane concettualmente analogo a quello della precedente legge di guida.
6.2. Problematiche da analizzare
Per prima cosa è necessario rendersi conto che generare una virata con controllo MPC sfruttando algoritmi realizzati con funzioni MATLAB è una procedura molto dispendiosa dal punto di vista computazionale. La soluzione riguardante la riduzione dei tempi di calcolo viene rimandata ad implementazioni future che utilizzando algoritmi in codice C++, quindi compilati, saranno in grado di ridurre ampiamente il carico computazionale, come si è già visto in alcuni test nei quali è stato utilizzato ACADO toolkit, ancora in versione beta.
Un secondo problema da affrontare è il seguente: si desidera far rollare il veicolo in curva in modo da poter sfruttare anche i thruster che generano e la coppia di pitch generata dalle portanza invece di saturare costantemente i motori che generano la coppia . Questo è dovuto al fatto che dai dati a disposizione si vede che il veicolo ha maggiore agilità sul piano come accade per gli aerei. In realtà i due thruster posteriori che forniscono spinta di sono in grado di fornire una coppia molto grande ed idealmente in grado di rendere la maggiore della generata dai soli thruster. Questo però solo nel caso in cui si utilizzino i motori posteriori per virare (in modo differenziale) e non per spingere in avanti il veicolo e senza considerare le forze idrodinamiche. All’aumentare della velocità di avanzamento viene richiesta ai due thurster di una forza sempre maggiore diminuendo quindi la forza che rimane a disposizione di possibili virate (coppie ) mentre invece la coppia dovuta alle forze idrodinamiche cresce col quadrato della velocità. Per questo motivo si è svolto un semplice calcolo, ovvero, si è controllato quanto vale la e quanto la (dei soli thruster) nel
caso in cui ci siano alte velocità di avanzamento e/o forti correnti contrarie. Il risultato è che ( ) ( ) alla quale andrà aggiunto il contributo delle forze idrodinamiche di circa ( ) con angolo di attacco nullo e velocità ( ⁄ ). Facendo rollare il veicolo in curva si può così aumentare la coppia media di imbardata. Questo lascia immaginare che ad alte velocità si possa trarre vantaggio dal rollare in curva, mentre magari alle basse velocità potrebbe essere sconveniente. Stabilito quindi che far rollare il veicolo in curva può avere senso, rimane il dubbio su come imporre all’algoritmo MPC di generare una virata che abbia tale comportamento. La prima ipotesi è stata quella di usare la matrice di peso sul controllo, in modo tale da pesare molto il contributo dei thruster che generano . Il risultato è che il veicolo tende a rollare ma inibire l’utilizzo di 4 dei 7 motori non permette di generare dei percorsi utilizzabili. L’idea successiva è stata quella di utilizzare un forte peso direttamente sulla velocità angolare ma a fare la differenza in realtà è la riduzione del peso sul e la lasciando cosi il veicolo libero di rollare nella fase di virata senza che questo venga contato troppo dalla funzione di peso.
L’ultima problematica che si intende affrontare più nel dettaglio è legata alla matrice di peso del funzionale di costo usato nel controllo MPC, formula (9). In pratica, eseguire una virata di oppure di significa usare due punti di lavoro diversi fra loro, che quindi in generale richiederanno un controllore differente, ovvero una diversa matrice di peso . La domanda a cui bisogna trovare risposta è quindi la seguente: esiste una matrice di peso “media” ovvero in grado funzionare per tutti i possibili angoli di virata? Ed ancora: modificare la velocità di cosa comporta?
Dai test eseguiti fino ad ora la risposta alla prima domanda sembra essere negativa. Al momento la soluzione più efficace sembra quella del gain scheduling per angoli e per velocità diverse.
6.3. Virata
Come già accennato, una volta noto il set di way-point la virata può essere calcolata con algoritmo MPC. In questo modo la virata è generata ad-hoc in base all’angolo di virata di cui curvare per orientare il percorso verso il punto successivo. Il vettore finale dello stato desiderato è quindi il seguente:
Nulla viene imposto su poichè all’equilibrio non manterranno valori costanti, questo viene fatto ponendo a zero il loro peso nel funzionale di costo attraverso la matrice e nel calcolo del contractive constraint grazie alla matrice .
Per quanto riguarda invece il vettore di stato iniziale, questo è un vettore tutto nullo esclusa che equivale a dire, veicolo nell’origine, che punta verso Nord con angoli di assetto nulli.
0 1 ( 44 )
Una volta definiti è possibile tramite la formula ̃ trovare lo stato iniziale del sistema ̃̇ con equilibrio traslato nell’origine. Si calcola poi come l’ingresso tale che ( ) che rappresenta in pratica il vettore delle forze/coppie in grado di mantenere il veicolo alla velocità di con assetto costante. Questi sono i dati necessari ad impostare il modello. Per quanto riguarda la funzione di costo è stata utilizzata solo la parte relativa al vettore di stato, annullando il contributo di . Quindi è stata usata la sola matrice di questo tipo:
( ) ( 45 )
con nulli in modo da lasciare libera l’evoluzione di mentre con grande e piccolo è stato aumentato il rollio.
La potenza dei thruster è stata ridotta del 15% ( tramite il vincolo ̃ ̃( | ) ̃ , formula (10)) in modo tale da poter lasciare parte della potenza a disposizione dell’algoritmo di controllo del veicolo reale per evitare errori di inseguimento troppo grandi nel caso di disturbi esterni o di dinamiche non modellate. Dato che ̃( ) ( ) ( ) si ottiene che:
̃ ̃ , - ( 46 )
Sempre parlando dei thruster è stata fissata ̃ (seconda disequazione formula (10)) in modo tale che per passare dall’erogazione di massima forza in una direzione, a quella di
Per quanto riguarda l’ultimo vincolo, il contractive constraint di formula(10), la matrice è analoga a ovvero con nulli in modo da lasciare libera l’evoluzione di .
Per rendere relativamente rapide le computazioni è stato scelto il tempo di campionamento e finestra di predizione e di controllo relativamente .
6.3.1. Peso Q
Come appena detto la matrice è del tipo:
( ) ( 47 )
con e in modo da non penalizzare il rollio. Per cercare di capire se una matrice di peso, tarata per virare di un certo angolo, può dare buone performance anche per altri angoli è stato usato il seguente approccio. Sono state tarate due matrici , una per una curva di ( ) ed una per ( ), entrambe con velocità di surge ⁄ . Entrambe le matrici sono state scelte in modo da dare alle variabili di stato un comportamento affine, ovvero, un aumento della fino a circa metà curva in modo tale da non avere overshoot dell’angolo di e aumento dell’angolo di fino a circa metà virata cosi da azzerarsi più o meno contemporaneamente all’angolo di . In questo modo il veicolo si può trovare in assetto orizzontale non appena terminata la curva.
A questo punto sono state eseguite simulazioni per velocità di ⁄ per angoli di virata di ogni uno con undici diverse matrici di peso tali da variare in modo lineare fra e estremi compresi.
La prima cosa che pare evidente è che mentre usare per virate di con ⁄ porta ad avere una risposta del sistema semplicemente più lenta, usare per virate di porta ad un risultato decisamente peggiore. Sempre per la virata di ecco cosa accade per alcune delle matrici intermedie (ottenute per interpolazione lineare di con ).
Il comportamento è instabile. Mentre invece per la virata di anche con le matrici intermedie il comportamento non diventa mai instabile.
Utilizzando velocità di avanzamento diverse dai ⁄ si ottengono comportamenti diversi del sistema che sono talvolta indesiderati come nel seguente esempio.
Sono presenti delle oscillazioni smorzate sull’angolo di rollio che portano ad avere dei picchi nella velocità di .
È necessario mostrare infine i risultati per la virata intermedia di per la quale non sono state tarate le matrici.
Figura 41 Virata di 15° con 𝑄
Concludendo, secondo i dati conviene usare una matrice tarata per una virata di angolo maggiore, la quale comporta nel peggiore dei casi una risposta più lenta del sistema ed un piccolo overshoot.
Nel caso di curva di , cioè con angolo di virata intermedio sembra esistere fra le varie matrici di peso intermedie almeno una matrice Q tale da far assumere al sistema lo stesso comportamento per cui sono state tarate e sulle rispettive virate.
L’utilizzo di una matrice di peso con velocità di diverse da quella per cui è stata tarata può dare comportamenti indesiderati.
Detto ciò la strada migliore da percorrere sembra essere quella del gain scheduling effettuato per un set di angoli di virata e per ogni velocità di avanzamento desiderata.
6.3.2. Gain Scheduling
Il gain scheduling appena accennato è stato realizzato per virate che vanno da a . Il gain scheduling consiste nel tarare diverse matrici di peso, una per ogni diverso punto di lavoro. Dopo questa prima fase è possibile usare tali matrici in un intorno del punto di equilibrio oppure si possono interpolare due matrici successive per ottenere dati intermedi. Sono state eseguite prove con entrambi gli approcci.
Lavorando per il momento su una sola velocità di ⁄ , sono state tarate matrici di peso per angoli di virata che vanno dai ai , con un intervallo di che chiameremo . Sono state eseguite prove sia utilizzando le varie per virate di angoli nell’intervallo * +, sia con matrici ottenute per interpolazione lineare di nell’intervallo * +.
Il risultato ottenuto è il seguente:
Come si può vedere dai grafici, per angoli intermedi le simulazioni sono più lunghe rispetto a quelle per le quali sono state tarate le matrici di peso. Le simulazioni ottenute interpolando le mostrano una seppur piccola maggiore velocità di convergenza dell’angolo di virata, ma in certi casi portano ad oscillazioni troppo ampie sull’angolo di rollio come nel seguente caso.
