Sviluppo di un Flight Management System per mini RUAV

(1)

D

IPARTIMENTO

D

I

NGEGNERIA

C

IVILE

E

D

I

NDUSTRIALE

C

ORSO

D

I

L

AUREA

M

AGISTRALE

I

N

I

NGEGNERIA

A

EROSPAZIALE

M

ASTER

T

HESIS

SVILUPPO DI UN FLIGHT MANAGEMENT

SYSTEM PER MINI RUAV

Relatori:

Autore:

Prof. Ing. Eugenio Denti Salvatore Giuseppe Schifano

Ing. Francesco Schettini Prof. Ing. Roberto Galatolo Prof. Ing. Gianpietro Di Rito

(2)

I

Sommario

L’Università di Pisa sta realizzando una ricerca finalizzata allo sviluppo di mini RUAV (Rotary Unmanned Aerial Vehicles) partendo da piccoli elicotteri commerciali radiocomandati. Questi sistemi dovranno essere in grado di realizzare intere missioni, incluse le fasi di atterraggio e decollo in modo automatico.

Il velivolo in studio è un elicottero di piccola scala con un rotore del diametro di circa 1 metro e peso di circa 2,5 kg. L’elicottero è stato acquistato dal DICI (Dipartimento di Ingegneria Civile ed Industriale), sezione aerospaziale, dell’Università di Pisa ed è stato equipaggiato con un sistema di navigazione ed un sistema di telematica.

Scopo della tesi è lo sviluppo di un FMS (Flight Management System) che permetta, all’elicottero in studio, di seguire un percorso prestabilito, definito per punti mediante opportuna interfaccia grafica implementata sulla stazione di controllo di terra

La prima parte della tesi è stata dedicata allo sviluppo delle leggi di guida utilizzando inizialmente modelli semplificati della dinamica del velivolo. Successivamente il FMS è stato applicato al modello non lineare dell’elicottero sviluppato durante lavori precedenti ed è stato validato su un percorso indicato da un fissato numero di waypoints.

Nella seconda parte della tesi il FMS è stato testato su percorsi più complessi, generati da un algoritmo realizzato opportunamente che permette di definire percorsi curvilinei sulla base di un certo numero di waypoints principali assegnati dall’utente. Sono state quindi effettuate simulazioni con diverse tipologie di percorso e sono stati analizzati i risultati ottenuti.

(3)

II

Indice

Introduzione ... 1 1-Descrizione UAV ... 3 1.1 Leggi di guida ... 3 1.2 T-REX 500 ESP ... 4

1.3 Modello non lineare ... 5

1.4 Sistema di controllo del T-REX 500 ESP ... 7

2-FMS ... 9

2.1 Sistema di guida Pure Pursuit ... 9

2.2 Sistema di guida LOS ... 10

2.3 Switch (selezionatore dei waypoints) ... 19

2.4 Validazione degli algoritmi... 20

2.5 FMS in 3 dimensioni ... 22

2.5.1 Analisi dei blocchi ... 24

2.6 Verifica degli algoritmi in 3D ... 29

3- Integrazione del FMS con il modello non lineare del T-REX ... 33

3.1 Analisi dei risultati ... 37

3.1.1 Caso 2D ... 37

3.1.2 Caso 3D ... 44

4-Applicazione del FMS su percorsi curvilinei ... 49

4.1 Generazione del percorso ... 49

4.2 Spline cubica di Hermite ... 49

4.3 Risultati ... 53

(4)

III

4.5 Simulazioni in 3D ... 59 5- Conclusioni e sviluppi futuri ... 63 Bibliografia ... 65

(5)

1

Introduzione

Il termine UAVs (Unmanned Aerial Vehicle) si riferisce ad un velivolo senza pilota umano a bordo e senza passeggeri. Questi velivoli sono più comunemente conosciuti con il nome di droni. In questo campo c’è un interesse sempre crescente riguardante lo sviluppo di elicotteri autonomi senza equipaggio. L’elicottero ha caratteristiche uniche che riguardano la capacità di decollare e atterrare verticalmente, capacità di stare sospeso in un punto (condizione di hovering), alta manovrabilità, ecc..

Queste caratteristiche permettono l’impiego di questi velivoli sia in campo civile che militare.

Presso l’Università di Pisa (Dipartimento di ingegneria Civile ed industriale- Sezione Aerospaziale) è disponibile un elicottero radiocomandato denominato T-REX 500 ESP. Durante dei lavori precedenti è stato sviluppato un modello non lineare della dinamica dell’elicottero mediante l’uso di Matlab e Simulink. I parametri sono stati identificati utilizzando dati provenienti da prove di volo e successivamente è stato sviluppato il sistema di controllo sempre mediante l’uso di Matlab e Simulink.

Lo scopo della tesi è lo sviluppo di un FMS (Flight Management System) che consenta all’elicottero di realizzare in modo autonomo una traiettoria prestabilita.

Durante la prima parte di studio è stata effettuata una ricerca riguardante lo stato dell’arte. Tenendo in considerazione le caratteristiche e le dimensioni dell’elicottero è stata effettuata una ricerca su leggi di guida comunemente utilizzate su droni ma anche su mezzi radiocomandati di superficie.

Nel Capitolo 1 è stata riportata una breve descrizione del velivolo, del modello non lineare della dinamica e del sistema di controllo.

Nel Capitolo 2 vengono presentati due algoritmi sviluppati per le leggi di guida del velivolo, i quali sono stati realizzati separatamente e validati su percorsi casuali determinati da un certo numero di waypoints collegati tra di loro. Durante questa fase è stato considerato un modello ideale trascurando la dinamica dell’elicottero assimilandolo come un punto materiale.

(6)

2

Nel Capitolo 3, il FMS vengono descritti i test effettuati con il modello non lineare del T-REX 500 ESP su percorsi determinati da waypoints fissati e collegati tra di loro mediante delle linee rette.

Nel Capitolo 4 viene descritto un algoritmo sviluppato per generare percorsi curvilinei più complessi. La traiettoria che collega i vari waypoints è stata generata per mezzo di interpolazione polinomiale. Il tratto che collega un waypoint all’altro è stato discretizzato a sua volta in più parti generando così dei waypoints secondari che, interpolati tra di loro, forniscono una traiettoria più accurata.

Sono stati quindi effettuate simulazioni su diverse tipologie di percorso e ne sono stati analizzati i risultati

Nel Capitolo 5 sono evidenziate le principali conclusioni del lavoro e i possibili sviluppi futuri del sistema di guida.

(7)

3

Capitolo 1

1-Descrizione UAV

Un UAV, noto anche con il nome di drone, è un velivolo senza un pilota umano e senza passeggeri. Rispetto ai velivoli tradizionali con piloti a bordo gli UAV presentano molti vantaggi in termini di peso e di costi. Non necessitano di spazi a bordo per l’alloggiamento dei piloti e non necessitano quindi di spazi per la strumentazione e il controllo viene effettuato per mezzo di telecomandi.

La manovrabilità degli aerei convenzionali è legata ai limiti fisiologici dei piloti in termini di accelerazioni misurate in “g”. La rimozione del pilota dall’aereo elimina tutti i requisiti tipici richiesti dalla presenza del pilota a bordo, portando quindi l’interfaccia uomo-macchina a terra.

Gli UAV sono molto più piccoli rispetto ad un aereo tradizionale ma forniscono prestazioni equivalenti.

In principio venivano utilizzati solo per scopi militari poi il loro impego venne esteso anche all’uso civile.

Ci sono molte tipologie di UAV che sono classificati secondo la loro autonomia oraria, autonomia chilometrica, peso ecc..

Gli UAV ad ala rotante prendono il nome di RUAV (Rotary Unmannned Aerial Vehicle). A differenza dei velivoli ad ala fissa, i RUAV hanno alcune particolari caratteristiche che sono preferibili in certe circostanze.

Questi velivoli sono in grado di effettuare atterraggi e decolli verticali fornendo un grande vantaggio in termini di riduzione di spazio.

Sono anche in grado di operare a velocità relativamente basse e di stare in aria in un punto fisso (hovering).

1.1 Leggi di guida

In letteratura sono presenti diverse tipologie di leggi di guida le quali vengono utilizzate sia per veicoli di terra che per velivoli. Un metodo di guida che viene molto impiegato sia in campo aeronautico che per velivoli di terra si basa sull’ MPC (Model Predictive Control). L’idea che ne sta alla base è quella di calcolare l’azione di controllo (ovvero l’ingresso)

(8)

4

risolvendo un problema di controllo ad anello aperto. Tale calcolo viene svolto per ogni iterazione all’interno di un intervallo temporale fissato. [8], [9], [10], [11]. Questo sistema ingloba al suo interno, oltre le leggi di guida, anche il sistema di controllo per mezzo di autopiloti. Nel caso in studio il sistema l’utilizzo dell’MPC non è stato valutato idoneo alle specifiche in quanto il sistema di controllo del modello è stato precedentemente sviluppato [2].

Un altro approccio ai sistemi guida che viene soprattutto per robot di terra si basa sulla logica Fuzzy. La logica Fuzzy è una logica in cui si può attribuire a ciascuna proposizione un grado di verità compreso tra 0 e 1. È una logica polivalente, e pertanto un’estensione della logica booleana ed è fortemente legata alla teoria degli insiemi. [12], [13].

Essa è in grado di gestire un insieme di waypoints in cui due o più waypoints successivi possono non essere in una traiettoria direttamente guidabile (ad esempio due waypoints vicini tra loro con direzioni di attraversamento molto diverse) e permette di riconfigurare velocemente l’insieme dei waypoints in risposta a modifiche nello scenario o negli obiettivi della missione.

Molti sistemi di guida realizzati per la navigazione si basano invece sul Pure Pursuit e sul LOS (Line Of Sight) [5], [6], [7]. Questi metodi trovano un’ampia applicazione sia nella navigazione in mare sia nella guida di veicoli di terra. Il Pure Pursuit è un modello semplice che si basa sull’inseguimento di waypoints; una volta raggiunto un waypoints il veicolo insegue quello successivo.

L’idea che sta alla base nel sistema di guida LOS (Line Of Sight) è quella di determinare dei punti intermedi sulla retta congiungente due waypoints. in questo modo si determina una linea guida che il veicolo dovrà seguire per reinserirsi nel percorso.

Sia il Pure Pursuit che il LOS sono impiegati solitamente nella navigazione su un piano. Nel presente lavoro di tesi queste leggi di guida sono state realizzate anche per la navigazione in uno spazio tridimensionale e sono state integrate al modello che descrive la dinamica del velivolo in studio.

1.2 T-REX 500 ESP

Il RUAV oggetto di questa tesi è il T-REX 500 ESP (vedi fig1.1) disponibile presso il DICI sezione aerospaziale dell’Università di Pisa.

(9)

5

Il T-REX 500 ESP è un velivolo ad alta manovrabilità ed è molto popolare fra i velivoli destinati al volo acrobatico.

Figura 1.1: T-REX 500 ESP

L’elicottero è costruito in materiale plastico e fibre di carbonio. La sua massa è pari a 2,14 kg e il rotore principale ha un raggio di 0.485m.

Ulteriori caratteristiche relative al velivolo sono consultabili nei riferimenti [1] [2] [3].

1.3 Modello non lineare

In lavori precedenti [1] è stato sviluppato in dipartimento una simulazione del modello non lineare del RUAV in studio.

Il modello è stato sviluppato mediante l’uso dei software Matlab e Simulink come riportato in fig.1.2

(10)

6

Figura 1.2: Modello della dinamica non lineare

L’analisi e la descrizione dei vari blocchi costituenti il modello esulano dall’obiettivo del presente lavoro, pertanto un’accurata descrizione è consultabile nel rif . [1]

(11)

7

1.4 Sistema di controllo del T-REX 500 ESP

Durante lavori precedenti [2], [3] è stato realizzato il sistema di controllo del sistema del T-REX 500 ESP.

Il sistema di controllo è stato progettato con tecniche classiche con controllori PID, ed è stato quindi necessario linearizzare il modello non lineare in diverse condizioni di trim: hovering, crociera a volo livellato, salita e discesa. Trovate quindi le condizioni di trim è stato realizzato il sistema di controllo ed è stato implementato nel modello non lineare del T-REX. Lo schema del sistema controllato è riportato in figura 1.3:

(12)

8

La tecnica utilizzata per realizzare il sistema di controllo è la Dynamic Inversion. Nella figura 1.4 è riportato il blocco realizzato in Simulink che è stato collegato al modello non lineare che schematizza il funzionamento dell’elicottero.

Figura 1.4: Sistema di controllo del modello

In ingresso al sistema di controllo vi sono il comando relativo all’angolo di imbardata del velivolo (psi_d) e le tre componenti di velocità corrispondenti alle proiezioni della velocità longitudinale e latero-direzionale dell’elicottero sul piano orizzontale (Vlon_d, Vcross_d) e alla proiezione della velocità sull’asse verticale locale (Vz_d).

Lo scopo di questa tesi è appunto quello di creare un FMS che fornisca le tre componenti di velocità e l’angolo di imbardata, in modo da consentire all’elicottero di realizzare un percorso prestabilito.

(13)

9

Capitolo 2

2-FMS

In questo capitolo verrà illustrato l’FMS (Flight Management Systemt) che permetterà al T-REX di seguire un percorso prestabilito passando per dei waypoints fissati arbitrariamente. Le leggi di guida più comunemente utilizzate per la navigazione dei UAV sono il Pure

Pursuit e il LOS. Queste leggi di guida sono comunemente utilizzate per la navigazione in

due dimensioni. In questo capitolo verrà illustrato l’adattamento di queste leggi di guida alla navigazione in 3 dimensioni.

L’implementazione di questi algoritmi è stata effettuata in ambiente Matlab/Simulink. In questo capitolo verranno descritti i due metodi e la loro validazione, che è stata effettuata assimilando l’elicottero ad un punto materiale.

In un primo momento i due algoritmi verranno illustrati considerando il volo sul piano orizzontale.

2.1 Sistema di guida Pure Pursuit

Il sistema di guida Pure Pursuit [5] è un sistema di guida che si basa sull’inseguimento dei waypoints. Il velivolo una volta raggiunto un waypoint insegue il waypoint successivo fino a completamento del percorso indicato. Il metodo è abbastanza intuitivo e l’angolo di rotta (nel caso di navigazione in 2 dimensioni) è calcolato tenendo conto della posizione corrente del velivolo e del waypoint successivo.

Si ha dunque:

tan 𝜂(𝑡) =

𝑦𝑘−𝑦(𝑡)

𝑥_𝑘−𝑥(𝑡) (2.1)

Dove xk e yk sono le coordinate del waypoint da inseguire e x(t) y(t) le coordinate nel piano

orizzontale della posizione corrente del velivolo.

L’angolo di rotta η si calcola con l’arcotangente a 4 quadranti

(14)

10

A differenza della funzione arcotangente, che è definita nell’intervallo [-𝜋 2

,

𝜋 2], l’arcotangente2 indica l’angolo in radianti tra il semiasse positivo delle X (in questo caso coincidente con il Nord) e la retta congiungente l’origine degli assi a un punto nel piano cartesiano di coordinate (x,y) ed è definita nell’intervallo [-π;π] (Figura 2.1).

Figura 2.1 Metodo PP

2.2 Sistema di guida LOS

L’idea che sta alla base nel sistema di guida LOS (Line Of Sight) [5], [6],[7] è quella di determinare dei punti intermedi sulla retta congiungente due waypoints Pk e Pk+1.

Per la determinazione di tali punti intermedi si assegna un cerchio di raggio R fissato attorno al velivolo che interseca la retta congiungente i waypoints in due punti.

Il vettore che collega la posizione corrente del velivolo con il punto intermedio trovato (vedi fig 2.2) prende il nome di setpoint e determina una linea di guida che rappresenta la direzione che il velivolo dovrà seguire per rimettersi nel percorso prestabilito. Il setpoint verrà selezionato in base all’orientazione dei waypoints.

Assumendo che la posizione corrente del velivolo sia data dalle coordinate x(t) y(t) e le coordinate del setpoint siano xlos e ylos , la relazione trigonometrica che collega i due punti è

(15)

11

tan 𝜂(𝑡) =

𝑦𝑙𝑜𝑠−𝑦(𝑡)

𝑥_𝑙𝑜𝑠−𝑥(𝑡) (2.3)

Per determinare quindi l’angolo di rotta η si utilizza anche in questo caso l’arcotangente a 4 quadranti

𝜂(𝑡) = 𝑎𝑡𝑎𝑛2 (𝑦_𝑙𝑜𝑠− 𝑦(𝑡), 𝑥_𝑙𝑜𝑠− 𝑥(𝑡)) (2.4)

Dato quindi un cerchio di raggio R con centro coincidente con la posizione corrente del velivolo, le coordinate del setpoint, xlos e ylos , sono date dall’intersezione di tale cerchio con

la retta congiungente due waypoints consecutivi. Si ha pertanto:

(𝑥_𝑙𝑜𝑠− 𝑥(𝑡))2+ (𝑦_𝑙𝑜𝑠− 𝑦(𝑡))2 = 𝑅2 _(2.5) 𝑥_𝑙𝑜𝑠2 + 𝑥(𝑡)2_{− 2𝑥} 𝑙𝑜𝑠𝑥(𝑡) + 𝑦𝑙𝑜𝑠2 + 𝑦(𝑡)2− 2𝑦𝑙𝑜𝑠𝑦(𝑡) = 𝑅2

tan(𝛼

_𝑘

) =

E inserendo la (2.7) nella (2.5) si ha: 𝑥_𝑙𝑜𝑠2 + 𝑥(𝑡)2_{− 2𝑥} 𝑙𝑜𝑠𝑥(𝑡) + [( 𝛥𝑦 𝛥𝑥(𝑥𝑙𝑜𝑠− 𝑥𝑘) + 𝑦𝑘)] 2 + 𝑦(𝑡)2₊ −2 (𝛥𝑦 𝛥𝑥(𝑥𝑙𝑜𝑠− 𝑥𝑘) +) 𝑦(𝑡) = 𝑅 2 (2.8) Che può essere riscritta come un’equazione di secondo grado:

𝑎𝑥_𝑙𝑜𝑠2 + 𝑏𝑥𝑙𝑜𝑠+ 𝑐 = 0 (2.9)

Dove i coefficienti a, b, c sono dati da: 𝑎 = 1 + (𝛥𝑦 𝛥𝑥) 2 (2.10 a) 𝑏 = −2𝑥(𝑡) − 2𝑥_𝑘(𝛥𝑦 𝛥𝑥) 2 + 2𝑦_𝑘(𝛥𝑦 𝛥𝑥) − 2𝑦(𝑡) ( 𝛥𝑦 𝛥𝑥) (2.10 b) 𝑐 = 𝑥(𝑡)2+ 𝑦(𝑡)2+ 𝑥_𝑘2(𝛥𝑦 𝛥𝑥) 2 + 𝑦_𝑘2− 2𝑦𝑘𝑥𝑘( 𝛥𝑦 𝛥𝑥) + 2𝑦(𝑡)𝑥𝑘( 𝛥𝑦 𝛥𝑥) − 2𝑦(𝑡)𝑦𝑘− 𝑅 2 (2.10 c)

Da cui si può ricavare la soluzione:

𝑥

_𝑙𝑜𝑠

=

−𝑏±√𝑏2−4𝑎𝑐 2𝑎 (2.11) e si ha che 𝑥_𝑙𝑜𝑠 = −𝑏 + √𝑏 2_{− 4𝑎𝑐} 2𝑎 𝑠𝑒 𝛥𝑥 > 0 𝑥𝑙𝑜𝑠 = −𝑏 − √𝑏2_{− 4𝑎𝑐} 2𝑎 𝑠𝑒 𝛥𝑥 < 0

(17)

13

Una volta ricavata xlos sfruttando la (2.7) si ricava ylos:

𝑦𝑙𝑜𝑠 = ( 𝛥𝑦

𝛥𝑥) (𝑥𝑙𝑜𝑠− 𝑥𝑘) + 𝑦𝑘 (2.12)

Nel caso in cui

b2-4ac < 0

𝑥_𝑙𝑜𝑠 = 𝑥_𝑘+1 𝑒 𝑦_𝑙𝑜𝑠 = 𝑦_𝑘+1

Si può verificare anche il caso in cui il velivolo stia percorrendo una traiettoria su una linea retta in direzione Est o Ovest. Si verificherà quindi che 𝛥𝑥 è pari a zero e di conseguenza si avrà che:

𝑥_𝑙𝑜𝑠 = 𝑥_𝑘 = 𝑥_𝑘+1 (2.13)

Per ricavare la ylos si utilizza la (2.5). Per cui:

𝑦_𝑙𝑜𝑠 = 𝑦(𝑡) + √𝑅2_{− (𝑥} 𝑙𝑜𝑠− 𝑥(𝑡)) 2 𝑠𝑒 𝛥𝑦 > 0 𝑒 𝑦(𝑡) − 𝑦_𝑘+1 < 0 𝑦𝑙𝑜𝑠 = 𝑦(𝑡) + √𝑅2− (𝑥𝑙𝑜𝑠− 𝑥(𝑡)) 2 𝑠𝑒 𝛥𝑦 < 0 𝑒 𝑦(𝑡) − 𝑦𝑘+1 < 0 𝑦_𝑙𝑜𝑠 = 𝑦(𝑡) − √𝑅2_{− (𝑥} 𝑙𝑜𝑠− 𝑥(𝑡)) 2 𝑠𝑒 𝛥𝑦 < 0 𝑒 𝑦(𝑡) − 𝑦_𝑘+1 > 0 𝑦_𝑙𝑜𝑠 = 𝑦(𝑡) − √𝑅2_{− (𝑥} 𝑙𝑜𝑠− 𝑥(𝑡)) 2 𝑠𝑒 𝛥𝑦 > 0 𝑒 𝑦(𝑡) − 𝑦_𝑘+1 > 0

-

Mappatura da [-π;π] a [0;2 π]

Entrambi gli algoritmi, sia il Pure Pursuit che il LOS, per determinare l’angolo di rotta 𝜂 utilizzano la funzione atan2.L’arcotangente a quattro quadranti è definita nell’intervallo [−𝜋; 𝜋] ; questo significa che nel punto -π/π presenta un punto di discontinuità. Si faccia l’ipotesi che la path costruita sia un cerchio di raggio unitario e che il velivolo debba seguire tale percorso in senso antiorario.

(18)

14 Figura 2.3

Partendo da un angolo di π/2, il velivolo seguirà la traiettoria in senso orario, ma quando il velivolo si troverà all’estremità bassa del cerchio, ovvero nel momento in cui starà passando dal 4° al 3° quadrante, la funzione atan2 passerà dal valore π a –π, il che potrebbe portare il velivolo a ritornare indietro. Quindi affinché il sistema di guida sia funzionale su qualsiasi percorso è necessario eliminare il problema della discontinuità. A tal scopo è stato inserito il blocco:

Figura 2.4 Blocco mappatura

[-π;π]

→

[0;2 π]

Il principio di funzionamento dell’algoritmo dipende dalla conoscenza della posizione del velivolo (e quindi della conoscenza della rotta seguita fino a quel momento) e dalla rotta imposta dal waypoint successivo.

(19)

15

Si faccia l’ipotesi che il velivolo si trovi nel primo quadrante e sta seguendo una rotta dettata dalla posizione del waypoint precedente.

Figura 2.5

Il waypoint successivo determinerà una variazione dell’angolo denominata come Δη. Tale variazione viene calcolata in maniera diversa in base al quadrante in cui si trova il waypoint successivo che determina il segno (e quindi il quadrante successivo) di Δx e Δy . Si ipotizzi che il waypoint successivo sia sempre nel primo quadrante.

Figura 2.6

La variazione dell’angolo di rotta verrà calcolata come segue: Quadrante 1 (quadrante del waypoint successivo)

(20)

16

Qualora il waypoint successivo si dovesse trovare negli altri quadranti si ha:

Quadrante 2 𝛥𝜂 = 𝜂𝑛𝑜𝑤− 𝜂𝑙𝑎𝑠𝑡 Quadrante 3 Se |𝜂_𝑛𝑜𝑤| + 𝜂𝑙𝑎𝑠𝑡 ≤ 𝜋 𝛥𝜂 = 𝜂_𝑛𝑜𝑤− 𝜂_{𝑙𝑎𝑠𝑡} Figura 2.7

In questo caso il velivolo riceverà in ingresso una rotta con angolo 𝜂 negativo e quindi la prua del velivolo virerà in senso antiorario.

Nel caso in cui invece |𝜂_𝑛𝑜𝑤| + 𝜂_{𝑙𝑎𝑠𝑡} ≥ 𝜋

Si ha:

(21)

17

Figura 2.8

In questo caso il velivolo riceverà in ingresso una rotta con angolo 𝜂 positivo e quindi la prua girerà in senso orario.

Quadrante 4

𝛥𝜂 = 𝜂_𝑛𝑜𝑤− 𝜂_{𝑙𝑎𝑠𝑡}

Proseguendo il percorso l’angolo di rotta del velivolo viene aggiornato ciclicamente aggiungendo 𝛥𝜂 fino al completamento del percorso.

In maniera analoga sono stati analizzati anche gli altri casi. L’intero codice è stato sviluppato con Matlab.

La determinazione della posizione corrente del velivolo, necessaria per il calcolo dell’angolo di rotta, è determinata dal sistema di navigazione che usa una piattaforma inerziale ed un ricevitore GPS di cui è fornito il T-REX 500 ESP.

La velocità di percorrenza del percorso è stata impostata pari a 5m/s. Per poter rilevare la posizione dell’elicottero sono state integrate le componenti della velocità nel sistema di riferimento NED che durante il percorso cambiano continuamente in funzione dell’angolo di rotta 𝜂. Le coordinate del velivolo, in uscita del modello, vengono quindi riportate in ingresso e ciclicamente viene calcolato il nuovo angolo di rotta con gli algoritmi suddetti fino al completamento del percorso prestabilito.

(22)

18

Le componenti di velocità nel caso di volo orizzontale sono date da:

Figura 2.9 Scomposizione della velocità in 2D

𝑉_𝑙𝑜𝑛 = 𝑉𝑐𝑜𝑠 𝜂 (2.14a)

𝑉_{𝑐𝑟𝑜𝑠𝑠} = 𝑉 sin 𝜂 (2.14b)

Si faccia riferimento alla seguente figura:

(23)

19

Nel caso di volo in 3 dimensioni come si vedrà in seguito vi saranno 3 componenti di velocità e di conseguenza 3 coordinate.

2.3 Switch (selezionatore dei waypoints)

Per entrambi gli algoritmi è necessario uno switch che sia in grado di selezionare i vari waypoints man mano che il velivolo percorre il percorso prestabilito. Una volta terminati i waypoints il sistema si arresta.

Figura 2.11 Switch (selezionatore di waypoints)

In ingresso allo switch vi sono le coordinate correnti dell’elicottero e la lista dei waypoints primari e secondari che compongono il percorso. In uscita lo switch fornisce le coordinate del waypoint precedente e quelle del waypoint successivo. Il funzionamento dello switch è abbastanza semplice: attorno ad ogni punto di controllo si assegna un cerchio di raggio fissato, quando il velivolo è all’interno di questo cerchio lo switch selezionerà il waypoint successivo. In sostanza si ha: dati 𝛥𝑥 = 𝑥(𝑡) − 𝑥_𝑘 (2.15a) 𝛥𝑦 = 𝑦(𝑡) − 𝑦𝑘 (2.15b) Se 𝛥𝑥2+ 𝛥𝑦2 < 𝑅2 (2.16)

(24)

20 Prec_WP = WPk-1

Next_WP = WPk+1

2.4 Validazione degli algoritmi

Per verificare il funzionamento dei due algoritmi inizialmente è stata effettuata una simulazione che escludeva la dinamica dell’elicottero. In sostanza le componenti di velocità sono state integrate e riportate in ingresso come coordinate correnti. Si riporta lo schema realizzato con Simulink:

(25)

21

Per verificare la funzionalità degli algoritmi è stato generato casualmente un percorso collegando dei waypoints, tramite delle rette. Dopo di che sono stati verificati entrambi gli algoritmi imponendo un raggio di accettazione dello switch pari a 1m.

-

Pure Pursuit

Figura 2.13 Percorso descritto dal punto materiale con Pure Pursuit

Si noti come il punto materiale, rappresentato dalla linea nera, segua il percorso contrassegnato dalla linea rossa sovrapponendosi esattamente ad esso.

Come descritto in precedenza, il LOS prevede la determinazione di un setpoint tramite un cerchio di raggio R attorno al velivolo che interseca la retta congiungente due waypoints. Si ricorda che in questa fase la dinamica del velivolo è stata trascurata e l’elicottero è stato assimilato ad un punto materiale.

In considerazione della distanza che separa i waypoints sono state effettuate diverse simulazioni al variare del raggio R (10m, 20m, 30m), ma a causa della mancanza di effetti dinamici dovuti all’elicottero i risultati ottenuti sono stati perfettamente identici. Si riportano pertanto soltanto i risultati ottenuti imponendo R=20m.

(26)

22

-

LOS, R=20m

Figura 2.14 Percorso descritto dal punto materiale con LOS (R=20m)

Si può notare come il punto materiale, rappresentato dalla linea nera, percorre esattamente la traiettoria con entrambi gli algoritmi.

Come si vedrà in seguito, l’applicazione del FMS al modello non lineare del T-REX, evidenzierà delle differenze in termini di risultati tra i due algoritmi. La scelta dell’utilizzo del sistema di guida sarà dettata dal tipo di navigazione scelto e dalla tipologia di percorso che il velivolo dovrà effettuare.

La descrizione dei blocchi Simulink, che rappresentano gli algoritmi, verrà analizzata dettagliatamente nell’applicazione al caso tridimensionale.

2.5 FMS in 3 dimensioni

Il funzionamento del FMS in 3 dimensioni è simile a quello in 2 ma presenta alcune differenze. Una delle differenze riguarda la scomposizione della velocità.

(27)

23

Figura 2.16 Scomposizione della velocità in 3D Le componenti delle velocità lungo i 3 assi sono date da:

𝑉_𝑥 = 𝑉 𝑐𝑜𝑠𝛾 cos 𝜂 (2.17a) 𝑉𝑦= 𝑉 𝑐𝑜𝑠𝛾 sin 𝜂 (2.17b)

𝑉_𝑧 = 𝑉 𝑠𝑖𝑛𝛾 (2.17c)

Dove 𝛾 rappresenta l’angolo che il vettore velocità forma con il piano xy ed è dato da

𝛾

= 𝑎𝑟𝑐𝑡𝑎𝑛𝑔

(∆𝑧 𝑟) (2.18) Con 𝑟 = √∆𝑥2_{+ ∆𝑦}2 _(2.19) Si ricorda che

∆𝑥 = 𝑥

_𝑘+1

− 𝑥(𝑡)

∆𝑦 = 𝑦

_𝑘+1

− 𝑦(𝑡)

∆𝑧 = 𝑧

_𝑘+1

− 𝑧(𝑡)

Un’altra differenza rispetto al caso bidimensionale riguarda lo switch. Nel caso bidimensionale attorno ad ogni punto di controllo è stato assegnato un cerchio di raggio R. Nel caso

(28)

24

Figura 2.17 Blocco PP

Il sistema Pure Pursuit necessita in ingresso solamente delle coordinate del waypoint successivo e in uscita fornisce gli angoli η e γ necessari per la scomposizione della velocità

(29)

25

All’interno del blocco si trova il seguente schema:

Figura 2.18 Schema PP

LOS in 3D

Figura 2.19 Blocco LOS

Il sistema di guida LOS in ingresso necessita, a differenza del Pure Pursuit, anche delle coordinate del waypoint precedente. In uscita fornisce anch’esso gli angoli γ e η.

A differenza del caso bidimensionale la determinazione delle coordinate del setpoint richiede una coordinata aggiuntiva.

(30)

26

La procedura è analoga al caso bidimensionale ma sono necessarie considerazioni e calcoli aggiuntivi per poter determinare le 3 coordinate del setpoint

All’interno del blocco si trova il seguente schema:

(31)

27

Data quindi una sfera di raggio R, con centro coincidente con la posizione corrente del velivolo, le coordinate del setpoint xlos ylos zlos sono date dall’intersezione di tale sfera con la

retta congiungente i due waypoints consecutivi. Si ha quindi:

(𝑥_𝑙𝑜𝑠− 𝑥(𝑡))2+ (𝑦_𝑙𝑜𝑠− 𝑦(𝑡))2+ (𝑧_𝑙𝑜𝑠− 𝑧(𝑡))2 = 𝑅2 _(2.21)

𝑘

) + 𝑧

𝑘 (2.23b) Segue quindi: 𝑥_𝑙𝑜𝑠2 + 𝑥(𝑡)2− 2𝑥𝑙𝑜𝑠𝑥(𝑡) + [( 𝛥𝑦 𝛥𝑥(𝑥𝑙𝑜𝑠− 𝑥𝑘) + 𝑦𝑘)] 2 + 𝑦(𝑡)2− 2 (𝛥𝑦 𝛥𝑥(𝑥𝑙𝑜𝑠− 𝑥𝑘) + 𝑦𝑘) 𝑦(𝑡) + [( 𝛥𝑧 𝛥𝑥(𝑥𝑙𝑜𝑠 − 𝑥𝑘) + 𝑧𝑘)] 2 + 𝑧(𝑡)2− 2 (𝛥𝑧 𝛥𝑥(𝑥𝑙𝑜𝑠− 𝑥𝑘) + 𝑧𝑘) 𝑧(𝑡) = 𝑅 2 (2.24)

Che può essere riscritta come un’equazione di secondo grado:

𝑎𝑥_𝑙𝑜𝑠2 + 𝑏𝑥_𝑙𝑜𝑠+ 𝑐 = 0 (2.25)

𝛥𝑦 𝛥𝑥

𝛥𝑧 𝛥𝑥

) −

2𝑧(𝑡)𝑦

_𝑘

− 𝑅

2 (2.26c)

Da cui si può ricavare la soluzione:

𝑥

_𝑙𝑜𝑠

=

−𝑏±√𝑏2−4𝑎𝑐 2𝑎 (2.27) E si ha che: 𝑥_𝑙𝑜𝑠 = −𝑏 + √𝑏 2_{− 4𝑎𝑐} 2𝑎 𝑠𝑒 𝛥𝑥 > 0 𝑥_𝑙𝑜𝑠 = −𝑏 − √𝑏 2_{− 4𝑎𝑐} 2𝑎 𝑠𝑒 𝛥𝑥 < 0

Una volta ricavata xlos sfruttando la (2.23a) e la (2.23b) si ricavano ylos e zlos:

𝑦

_𝑙𝑜𝑠

= (

𝛥𝑦

𝛥𝑥

) (𝑥

𝑙𝑜𝑠

− 𝑥

𝑘

) + 𝑦

𝑘 (2.28a)

𝑧

_𝑙𝑜𝑠

= (

𝛥𝑧

𝛥𝑥

) (𝑥

𝑙𝑜𝑠

− 𝑥

𝑘

) + 𝑧

𝑘 (2.28b)

Nel caso in cui

b2_{-4ac < 0}

𝑥_𝑙𝑜𝑠 = 𝑥_𝑘+1 , 𝑦_𝑙𝑜𝑠 = 𝑦_𝑘+1 𝑒 𝑧_𝑙𝑜𝑠 = 𝑧_𝑘+1

Si può verificare che il velivolo stia percorrendo una traiettoria sul piano YZ e di conseguenza si avrà che Δx sia pari a 0 e quindi

(33)

29

In tal caso per determinare le coordinate ylos e zlos si effettua un’operazione analoga al caso

bidimensionale

Si avrà quindi un’equazione di secondo grado in funzione di ylos

𝑑𝑦_𝑙𝑜𝑠2 + 𝑒𝑦𝑙𝑜𝑠+ 𝑓 = 0 (2.29)

Dove i coefficienti d, e, f sono espressi: 𝑑 = 1 + (𝛥𝑧 𝛥𝑦) 2 (2.30a) 𝑒 = −2𝑦(𝑡) − 2𝑦𝑘( 𝛥𝑧 𝛥𝑦) 2 + 2𝑧𝑘( 𝛥𝑧 𝛥𝑦) − 2𝑧(𝑡) ( 𝛥𝑧 𝛥𝑦) (2.30b) 𝑓 = 𝑦(𝑡)2_{+ 𝑧(𝑡)}2_{+ 𝑦} 𝑘2( 𝛥𝑧 𝛥𝑦) 2 + 𝑧_𝑘2− 2𝑧_𝑘𝑦_𝑘(𝛥𝑧 𝛥𝑦) + 2𝑧(𝑡)𝑦𝑘( 𝛥𝑧 𝛥𝑦) − 2𝑧(𝑡)𝑧𝑘− 𝑅 2 (2.30c) La soluzione sarà data da:

𝑦

_𝑙𝑜𝑠

=

−𝑒±√𝑒2−4𝑑𝑓 2𝑑 (2.31) E si ha che 𝑦_𝑙𝑜𝑠 = −𝑒 + √𝑒 2_{− 4𝑑𝑓} 2𝑑 𝑠𝑒 𝛥𝑦 > 0 𝑦𝑙𝑜𝑠 = −𝑒 − √𝑒2_{− 4𝑑𝑓} 2𝑑 𝑠𝑒 𝛥𝑦 < 0 Una volta ricavata ylos si ricava zlos:

𝑧

_𝑙𝑜𝑠

= (

𝛥𝑧

𝛥𝑦

) (𝑦

𝑙𝑜𝑠

− 𝑦

𝑘

) + 𝑧

𝑘 (2.32)

2.6 Verifica degli algoritmi in 3D

Come avvenuto nel caso bidimensionale, anche nel caso tridimensionale, per verificare il funzionamento dei due algoritmi è stato generato un percorso fissando dei waypoints in modo del tutto casuale e sono stati verificati entrambi gli algoritmi imponendo un raggio di accettazione dello switch pari a 1m.

(34)

30

-

Pure Pursuit

Figura 2.21 Percorso descritto dal punto materiale in 3 D con PP

Figura 2.22 Percorso descritto dal punto materiale in 3 D con PP nel piano YX

(35)

31

Anche in questo caso si noti come il punto materiale, rappresentato dalla linea nera, segua il percorso contrassegnato dalla linea rossa sovrapponendosi esattamente ad esso.

Analogamente a quanto fatto nel caso bidimensionale si riportano i risultati ottenuti con il LOS imponendo un raggio pari a 20m.

-

LOS, R=20m

(36)

32

Figura 2.24 Percorso descritto dal punto materiale in 3 D con LOS nel piano YX

Anche con il LOS, il punto materiale, segue fedelmente il percorso contrassegnato dalla linea nera.

Dalle simulazioni effettuate si può constatare come il FMS funzioni nel caso in cui il velivolo è assimilato ad un punto materiale, sia nel caso bidimensionale che nel caso tridimensionale. In mancanza di effetti dinamici non vi sono discrepanze tra il percorso originale e quello descritto dal punto materiale. Apparentemente i due algoritmi sembrano equivalenti in termini di risultati ma, come si vedrà in seguito, la dinamica del velivolo influenzerà notevolmente la scelta del sistema di guida.

(37)

33

Capitolo 3

3- Integrazione del FMS con il modello non

lineare del T-REX

In questo capitolo verrà illustrato il funzionamento del FMS sul sistema non lineare del T-REX. A tal proposito sono state effettuate diverse simulazioni utilizzando entrambi gli algoritmi sviluppati ed è stata effettuata un’analisi per evidenziarne le differenze.

Il sistema non lineare realizzato in Simulink si presenta nella seguente configurazione

(38)

34

All’ingresso del sistema vi sono le tre componenti di velocità, come già illustrato nel paragrafo 1.4, e il comando relativo all’angolo di imbardata ψ, che rappresenta l’angolo tra la proiezione dell’asse longitudinale dell’elicottero sul piano orizzontale e l’asse X della terna verticale locale (nord).

Il FMS fornisce in uscita le tre componenti di velocità e l’angolo di rotta η determinato dalla posizione dei waypoints ed è stato collegato al sistema di controllo del modello non lineare secondo lo schema riportato in figura (per maggiore chiarezza si riporta soltanto il blocco Simulink del FMS e del sistema di controllo)

Figura 3.2 Collegamento fra il FMS e il sistema di controllo del modello non lineare

Affinché la prua dell’elicottero sia sempre allineata con la direzione di percorrenza della traiettoria è stato imposto l’angolo di imbardata ψ uguale all’angolo di rotta η.

A differenza del caso in cui il velivolo era assimilato ad un punto materiale le componenti di velocità si scompongono nel seguente modo

(39)

35

Figura 3.3 Scomposizione della velocità in 3D

𝑉_𝑙𝑜𝑛 = 𝑉 𝑐𝑜𝑠𝛾 (3.1a)

𝑉_{𝑐𝑟𝑜𝑠𝑠} = 0 (3.1b)

𝑉_𝑧 = 𝑉 𝑠𝑖𝑛𝛾 (3.1c)

Il FMS in ingresso necessita delle coordinate correnti dell’elicottero. Tali coordinate sono state ricavate per integrazione delle velocità all’uscita del sistema. All’interno del blocco Inertial &

Scope sono stati inseriti degli integratori come nello schema in figura e le coordinate

dell’elicottero riportate in ingresso al FMS

(40)

36

Per completezza si riporta lo schema Simulink del FMS collegato all’intero modello non lineare

(41)

37

3.1 Analisi dei risultati

In questa sezione verranno analizzati i risultati derivanti da diverse simulazioni effettuate. Nella prima parte sono state effettuate simulazioni relative ad un percorso che si sviluppa nel piano e nella seconda parte quelle relative ad un percorso 3D.

In entrambi i casi sono stati testati entrambi gli algoritmi sviluppati e ne sono state analizzate le differenze. Le simulazioni sono state effettuate tutte a velocità costante di 5m/s dopo un intervallo temporale di 10 secondi in cui il velivolo staziona nella condizione di hovering. (velocità tutte nulle)

3.1.1 Caso 2D

Fissando casualmente dei waypoints su un piano, è stato generato un percorso tramite delle rette congiungenti i punti di controllo.

(42)

38

Figura 3.7 Traiettoria nel piano YX

Sono state quindi effettuate delle simulazioni utilizzando entrambi gli algoritmi sul medesimo percorso. Per entrambi i casi il raggio di accettazione dello switch è stato fissato a 1m.

-

Pure Pursuit

(43)

39

Figura 3.9 Percorso descritto dal velivolo su un piano con PP

Come si può notare il velivolo attraversa tutti i waypoints, ma in presenza di brusche variazioni di direzione l’elicottero va fuori traiettoria per poi reinserirsi nel percorso come mostrato in figura 3.8. In presenza di queste variazioni di direzione del percorso l’elicottero oltre ad uscire fuori traiettoria presenta una variazione di quota con punte massime di 1m. Queste piccole variazioni di quota sono dovute ad una perdita di portanza determinata dalla variazione dell’angolo di rollio φ e dell’angolo di imbardata ψ nei punti in cui la direzione del percorso cambia bruscamente. Questa perdita di portanza genera anche una variazione della componente di velocità lungo l’asse verticale, come si può vedere in figura 3.11.

(44)

40

Figura 3.10 Angoli di Eulero dell’elicottero su percorso piano con PP

Figura 3.11 Componenti di velocità dell’elicottero su percorso piano con PP

Il LOS prevede la determinazione di un setpoint tramite una sfera di raggio R attorno al velivolo che interseca la retta congiungente due waypoints.

In considerazione della distanza che separa i waypoints sono state effettuate simulazioni al variare del raggio R (10m, 20m)

(45)

41

- LOS, R=10m, 20m.

Figura 3.12 Percorso descritto dal velivolo su un piano con LOS nel piano YX

(46)

42

Come si può notare in figura 3.13 anche nel caso dell’utilizzo del LOS, come per il Pure Pursuit, vi sono delle oscillazioni verticali durante i cambi di direzione.

Dai risultati ottenuti si nota come, nel caso specifico, il LOS con un raggio R=20m si comporti meglio rispetto a quello con raggio R=10m; il velivolo segue meglio il percorso originale e le oscillazioni verticali sono ridotte.

Si riportano gli andamenti delle componenti di velocità e degli angoli di Eulero durante il percorso per R=20m

Figura 3.14 Angoli di Eulero dell’elicottero su percorso piano con LOS (R=20m)

(47)

43

Confronto tra Pure Pursuit e LOS in 2D

Successivamente è stato effettuato un confronto tra il Pure Pursuit e il LOS (R=20m)

Figura 3.16 Percorso descritto dal velivolo su un piano con PP e LOS

(48)

44

Dal confronto effettuato si può affermare come, nel caso di navigazione point to point, il LOS sia più efficace del PP. Con il LOS, l’elicottero segue il percorso originale in maniera più fedele rispetto al PP e in più le oscillazioni verticali sono più contenute.

3.1.2 Caso 3D

Analogamente a quanto fatto nel caso bidimensionale sono state effettuate alcune simulazioni su un percorso che si sviluppa nello spazio fissando opportunamente dei waypoints.

-

Pure Pursuit

(49)

45

Figura 3.19 Percorso descritto dal velivolo in 3D con PP nel piano YX

Si riportano gli andamenti degli angoli di Eulero durante il percorso

Figura 3.20 Angoli di Eulero dell’elicottero su percorso 3D con PP

Come nel caso bidimensionale durante i cambiamenti di direzione si hanno delle variazioni dell’angolo di rollio φ che determina una perdita di portanza che causa una perdita di quota dell’elicottero.

(50)

46

-

LOS

Per quanto visto nel caso bidimensionale si riportano soltanto i risultati relativi al caso in cui la determinazione del setpoint è effettuata con una sfera di raggio R=20m.

Figura 3.21 Percorso descritto dal velivolo in 3D con LOS (R=20m)

(51)

47

Figura 3.23 Angoli di Eulero dell’elicottero su percorso 3D con LOS (R=20m)

Anche in questo caso il velivolo va fuori traiettoria durante i bruschi cambiamenti di direzione come mostrato in figura 3.22 e anche in questo caso si hanno delle oscillazioni lungo l’asse verticale.

Confronto tra Pure Pursuit e LOS in 3D

(52)

48

Figura 3.25 Percorso descritto dal velivolo in 3D con PP e LOS nel piano YX

Anche nel tridimensionale, nel caso di navigazione point to point, il LOS risulta più efficace del PP. Il velivolo segue più fedelmente la traiettoria originale e le perdite di quota sono minori. Nel caso quindi di navigazione point to point il LOS è il metodo che fornisce una maggiore accuratezza. A seconda della distanza che separa i waypoint bisogna valutare l’entità del raggio della sfera (o del cerchio, in caso di percorso piano) che determina il setpoint. Per un corretto funzionamento del LOS, in maniera iterativa, è stato determinato che il raggio ideale deve essere minore della metà della minima distanza che separa due waypoints.

(53)

49

Capitolo 4

4-Applicazione del FMS su percorsi curvilinei

4.1 Generazione del percorso

Nel capitolo precedente il FMS è stato integrato con il modello non lineare del T-REX e sono state fatte delle simulazioni su una navigazione point to point. In questo capitolo verrà illustrata la capacità del FMS di guidare il velivolo su percorsi più articolati e curvilinei. Per quanto visto fino ad ora, il velivolo, una volta raggiunto un waypoint, insegue quello successivo fino al completamento del percorso con entrambi gli algoritmi sviluppati. Per far in modo che il velivolo segua un percorso curvilineo sarebbero quindi necessari numerosi waypoints in sequenza con coordinate tali da descrivere il percorso desiderato.

Per semplificare la realizzazione di un percorso è stato realizzato un algoritmo che permette di costruire un percorso fornendo soltanto i principali punti di controllo per i quali si vuole far passare la curva. La generazione del percorso avviene per mezzo di interpolazione polinomiale che permette di collegare i waypoints tra di loro. L’interpolazione polinomiale è una tecnica che implica l’utilizzo di un polinomio di grado n che assume esattamente il valore dato in corrispondenza dei waypoints iniziali e finali.

Nel caso in studio ogni waypoint è espresso da 3 coordinate (xk ,yk ,zk) e per la realizzazione

del percorso in uno spazio tridimensionale sono stati utilizzati 3 polinomi.

4.2 Spline cubica di Hermite

Una spline è una funzione costituita da un insieme di polinomi raccordati tra di loro, il cui scopo è quello di interpolare, in un intervallo definito, un insieme di punti.

La spline cubica di Hermite è probabilmente la più utilizzata per la generazione di path. Ogni tratto della spline è definito da un generico polinomio di grado 3

𝑃(𝑥) = 𝑎 + 𝑏𝑥 + 𝑐𝑥2 + 𝑑𝑥3

Per poter generare una spline su un piano son necessari 2 polinomi parametrizzati in un intervallo t ϵ [0,1]

(54)

50

𝑦(𝑡) = 𝑎𝑦+ 𝑏𝑦𝑡 + 𝑐𝑦𝑡2+ 𝑑𝑦𝑡3 (4.2)

Gli estremi dell’intervallo sono definiti dai punti iniziali e finale tra i quali si vuole generare la spline e per poter determinare la spline è necessario ricavare i coefficienti dei polinomi a, b, c, d.

Si faccia l’ipotesi di voler generare una spline fra due punti Pi e Pi+1 rispettivamentedi

coordinate [Pi,x, Pi,y] e [Pi+1,x, Pi+1,y]

Figura 4.1

È necessario quindi forzare il passaggio della curva agli estremi dell’intervallo:

𝑥(0) = 𝑎𝑥 = 𝑃𝑖,𝑥 (4.3)

𝑥(1) = 𝑎𝑥+ 𝑏𝑥+ 𝑐𝑥+ 𝑑𝑥= 𝑃𝑖+1,𝑥 (4.4)

𝑦(0) = 𝑎𝑦 = 𝑃𝑖,𝑦 (4.5)

𝑦(1) = 𝑎𝑦 + 𝑏𝑦+ 𝑐𝑦+ 𝑑𝑦 = 𝑃𝑖+1,𝑦 (4.6)

Queste equazioni però non sono sufficienti per risolvere il problema. Per poter determinare tutti i coefficienti dei polinomi si devono imporre dei vincoli relativi alla derivata prima del polinomio. In sostanza si deve definire la tangente alla curva con la quale deve passare attraverso i waypoints imponendo una continuità alla derivata prima nei punti.

Per determinare le tangenti esistono diversi metodi. Ogni metodo fornisce una curvatura diversa ma in tutti i casi il passaggio attraverso i waypoints è garantito. Si riportano di seguito 4 metodi utilizzati più comunemente in letteratura per determinare le tangenti.

(55)

_𝑖+1,𝑥′

=

‖𝑃𝑖+2−𝑃𝑖+1‖ (‖𝑃𝑖+2_‖𝑃−𝑃𝑖+1‖𝑢𝑖,𝑥+‖𝑃𝑖+1−𝑃𝑖‖𝑢𝑖+1,𝑥) 𝑖+2−𝑃𝑖‖ (4.17)

(56)

52

Una volta ricavati 𝑃_𝑖,𝑥′ e 𝑃_𝑖+1,𝑥′ si ha

𝑥(0) = 𝑎_𝑥 = 𝑃_𝑖,𝑥 (4.18)

𝑥(1) = 𝑎_𝑥+ 𝑏_𝑥+ 𝑐_𝑥+ 𝑑_𝑥= 𝑃_𝑖+1,𝑥 (4.19)

𝑥′_{(0) = 𝑏}

𝑥 = 𝑃𝑖,𝑥′ (4.20)

𝑥′(1) = 𝑏_𝑥+ 2𝑐_𝑥+ 3𝑑_𝑥 = 𝑃_𝑖+1,𝑥′ (4.21)

Analogamente per il polinomio y(t):

𝑦(0) = 𝑎𝑦 = 𝑃𝑖,𝑦 (4.22)

𝑦(1) = 𝑎_𝑦 + 𝑏_𝑦+ 𝑐_𝑦+ 𝑑_𝑦 = 𝑃_𝑖+1,𝑦 (4.23)

𝑦′_{(0) = 𝑏}

𝑦 = 𝑃𝑖,𝑦′ (4.24)

𝑦′(1) = 𝑏𝑦 + 2𝑐𝑦+ 3𝑑𝑦 = 𝑃𝑖+1,𝑦′ (4.25)

I due problemi sono disaccoppiati ma possono comunque essere rappresentati in forma matriciale come segue

[ 1 0 0 0 1 1 1 1 0 1 0 0 0 1 2 3 ] [ 𝑎_𝑥 𝑎_𝑦 𝑏𝑥 𝑏𝑦 𝑐𝑥 𝑐𝑦 𝑑_𝑥 𝑑_𝑦] = [ 𝑃𝑖,𝑥 𝑃𝑖,𝑦 𝑃_𝑖+1,𝑥 𝑃_𝑖+1,𝑦 𝑃_𝑖,𝑥′ 𝑃_𝑖,𝑦′ 𝑃_𝑖+1,𝑥′ 𝑃_𝑖+1,𝑦′ _] dove A=[ 1 0 0 0 1 1 1 1 0 1 0 0 0 1 2 3

] è detta matrice di Vandermonde

X= [ 𝑎_𝑥 𝑎_𝑦 𝑏𝑥 𝑏𝑦 𝑐𝑥 𝑐𝑦 𝑑_𝑥 𝑑_𝑦_]

è la matrice delle incognite

Y= [ 𝑃𝑖,𝑥 𝑃𝑖,𝑦 𝑃𝑖+1,𝑥 𝑃𝑖+1,𝑦 𝑃_𝑖,𝑥′ 𝑃_𝑖,𝑦′ 𝑃_𝑖+1,𝑥′ 𝑃_𝑖+1,𝑦′ _]

è la matrice dei termini noti

Le tangenti nel punto iniziale e finale del percorso sono state imposte arbitrariamente pari a 0.

(57)

53

4.3 Risultati

La procedura descritta è stata realizzata mediante l’utilizzo di Matlab. Per validare l’algoritmo sono stati assegnati casualmente 14 waypoints e il tratto che va tra un waypoint e l’altro (parametrizzato in t ϵ[0,1]) è stato discretizzato in 10 parti. Quindi, oltre ai 14 waypoints principali sono stati generati 140 waypoints secondari che hanno permesso di generare la spline come riportato nella figura seguente

Figura 4.2: Traiettoria con i 4 metodi in 2D

Il processo analogo è stato effettuato per la realizzazione di un percorso in 3 dimensioni. Partendo da 14 waypoints principali scelti casualmente, discretizzando in 10 parti ogni intervallo tra un waypoint e l’altro, sono state ricavate le seguenti spline:

(58)

54

Figura 4.3 Traiettoria con i 4 metodi in 3D

Analizzando i 4 metodi per generare il percorso si nota come il metodo 1 fornisca una traiettoria molto segmentata mentre gli altri 3 metodi creano un collegamento con i vari waypoints più curvilineo, il che rende la traiettoria più eseguibile rispetto a quella generata con il primo metodo.

Come si vedrà in seguito, dalle simulazioni effettuate, una maggiore discretizzazione, e quindi la determinazione di un maggior numero waypoints secondari, influenzerà la percorrenza del tracciato da parte dell’elicottero.

4.4 Simulazioni su un percorso in 2D

Per la realizzazione dei percorsi, nelle simulazioni effettuate, è stato scelto in modo del tutto arbitrario il Metodo 3. Una prima simulazione è stata effettuata generando un percorso a spirale che si sviluppa nel piano con una discretizzazione di ogni tratto in 10 parti.

(59)

55

Figura 4.4 Traiettoria piana a spirale

Dopo di che sono state effettuate delle simulazioni utilizzando entrambi gli algoritmi

-

Pure Pursuit (10 WP secondari)

(60)

56

Figura 4.6 Percorso a spirale piano descritto dal velivolo con PP nel piano YX (10 WP secondari)

Analizzando i risultati si nota come nel piano YX il velivolo segue la traiettoria a spirale molto accuratamente. La realizzazione di percorsi che presentano variazioni di direzione meno brusche, come in questo caso, fanno si che le oscillazioni verticali diminuiscano sensibilmente rispetto al caso di navigazione point to point. Dopo una brusca oscillazione iniziale il velivolo prosegue il suo percorso con delle piccole oscillazioni in corrispondenza dei waypoints secondari. In considerazione di questa analisi è stata effettuata un ulteriore simulazione discretizzando ogni tratto in 100 parti.

-

Pure Pursuit (100 WP secondari)

(61)

57

Figura 4.8 Percorso a spirale piano descritto dal velivolo con PP nel piano YX (100WP secondari)

Confrontando le figure 4.5 e 4.7 si vede chiaramente come una maggiore discretizzazione determina una drastica diminuzione di oscillazioni lungo l’asse verticale. Osservando invece il percorso effettuato dal velivolo sul piano YX non si notano particolari discrepanze dovute ad una maggiore discretizzazione.

Si riportano gli andamenti delle velocità e degli angoli di Eulero

(62)

58

Figura 4.10 Angoli di Eulero dell’elicottero su percorso a spirale con PP (100WP secondari))

La velocità longitudinale del velivolo, dopo una breve oscillazione iniziale si mantiene costante lungo tutto il percorso come anche l’angolo di rollio φ che dopo qualche oscillazione iniziale si stabilizza. La mancanza quindi di brusche variazioni di direzione favorisce la stabilità dell’elicottero che non deve cambiare di molto l’assetto per seguire il percorso.

- LOS

Il LOS, su un percorso curvilineo discretizzato, è di difficile applicazione. In un percorso discretizzato i waypoints secondari sono molto ravvicinati tra di loro. Per avere un corretto funzionamento del LOS, il raggio della sfera che si deve imporre, al fine di individuare il setpoint, deve essere minore della minima distanza che separa due waypoints secondari. Se la discretizzazione è molto fitta la determinazione di tale raggio diventa quindi molto difficile e l’utilizzo del Pure Pursuit risulta essere più indicato. A titolo di esempio è stato applicato il LOS ad un percorso a spirale discretizzato in 100 parti per tratto imponendo un raggio R pari a 0,1m

(63)

59

Figura 4.11 Percorso a spirale piano descritto dal velivolo con LOS (R=0,1m ,100 WP secondari)

Come si vede dalla figura 4.11, a causa dei waypoints secondari molto ravvicinati tra di loro, il LOS si comporta in maniera analoga al Pure Pursuit. In considerazione di quanto visto l’utilizzo del LOS, nel caso di percorso discretizzato curvilineo, è poco consigliato, in quanto richiede una precisa imposizione del raggio per la determinazione del setpoint. Inoltre il sistema, per quanto visto nel capitolo 2, dovrebbe effettuare maggiori operazioni di calcolo, il tutto ottenendo sempre lo stesso risultato del Pure Pursuit.

4.5 Simulazioni in 3D

Un’ultima simulazione per testare il funzionamento del FMS è stata effettuata su un percorso in tre dimensioni. Nella fattispecie sono stati fissati alcuni waypoints principali in modo da generare una traiettoria che riproduce la fase di salita dell’elicottero e che, una volta in quota, descriva un percorso circolare alla quota prestabilita.

(64)

60

Figura 4.12 Percorso tridimensionale

Per quanto visto nel paragrafo precedente, sono state fatte simulazioni utilizzando soltanto il Pure Pursuit. Ogni tratto che separa i waypoints principali è stato discretizzato prima in 10 parti e poi in 100.

Figura 4.13 Percorso tridimensionale descritto dal velivolo con PP (10 WP secondari, 100 WP secondari)

(65)

61

Figura 4.13 Percorso tridimensionale descritto dal velivolo con PP (10 WP secondari, 100 WP secondari) nel piano XZ

Apparentemente, causa anche della larga scala dei grafici, sembra che non ci sia alcuna differenza tra una discretizzazione in 10 parti e una in 100. In entrambi i casi l’elicottero segue il percorso originale fino al suo completamento. Analizzando però gli andamenti degli angoli di Eulero del velivolo si notano alcune differenze.

Figura 4.14 Angoli di Eulero dell’elicottero su percorso tridimensionale con PP (10WP secondari))

(66)

62

Figura 4.15 Angoli di Eulero dell’elicottero su percorso tridimensionale con PP (100WP secondari)

Nel caso di discretizzazione del tratto in 10 parti si notano alcune oscillazioni soprattutto nell’andamento dell’angolo di rollio φ che si attenuano aumentando la discretizzazione in 100 parti. Queste oscillazioni sono molto piccole, ma gli effetti reali sull’elicottero potrebbero determinare delle vibrazioni indesiderate sul velivolo.

Si può concludere quindi che una discretizzazione maggiore del percorso porta ad una maggiore stabilità del velivolo. La discretizzazione deve essere comunque effettuata in relazione della distanza che separa i waypoints principali; qualora quindi la distanza fosse superiore all’ultimo caso simulato si può ricorrere ad una discretizzazione ancora superiore a 100 aumentandone la stabilità.

(67)

63

5- Conclusioni e sviluppi futuri

La ricerca bibliografica effettuata nella prima parte della tesi ha permesso l’identificazione delle leggi di guida più idonee al caso in studio. La scelta è ricaduta sul Pure Pursuit e sul LOS che sono le leggi di guida più comunemente utilizzate soprattutto su veicoli di superficie. Durante la prima parte del lavoro sono stati realizzati gli algoritmi utilizzando Matlab e Simulink. In seguito sono stati validati mediante opportune semplificazioni del modello dinamico dell’elicottero che è stato assimilato ad un punto materiale ed in mancanza di effetti dinamici i due algoritmi hanno fornito risultati analoghi.

Successivamente il FMS è stato applicato al sistema non lineare del T_REX e sono state rilevate alcune differenze tra il Pure Pursuit e il LOS. In una navigazione point to point il LOS si è rivelato più efficace del Pure Pursuit; oltre a seguire più fedelmente la traiettoria rispetto al Pure Pursuit le perdite di quota in corrispondenza dei waypoints sono più contenute.

Nella seconda parte della tesi, il FMS è stato testato su percorsi curvilinei generati da un algoritmo realizzato opportunamente. Mediante interpolazione polinomiale è stato possibile generare percorsi più complessi passanti per i waypoints principali che hanno identificato un certo numero di waypoints secondari.

Dalle simulazioni effettuate è stato rilevato come un percorso più curvilineo ed una maggiore discretizzazione tra waypoints principali, porti ad una drastica diminuzione di oscillazioni verticali.

Inoltre, è stato constatato come il LOS sia di difficile applicazione e che in caso di percorso curvilineo discretizzato è più conveniente l’impego del Pure Pursuit.

Il FMS fornisce buoni risultati su percorsi prestabiliti ma non considera la presenza di ostacoli sul percorso. Uno sviluppo futuro potrebbe riguardare quindi un’integrazione del FMS ad un sistema Sense & Avoid che permetterebbe al velivolo di rilevare e aggirare eventuali ostacoli presenti sul percorso.

Le leggi di guida sono state sviluppate sulla base di una velocità di percorrenza del percorso costante quindi, ulteriori miglioramenti, potrebbero riguardare la realizzazione di un sistema che tiene conto di possibili variazioni di velocità da parte del velivolo che permetterebbe la percorrenza di percorsi che possibilmente a velocità costante non sarebbero possibili.

(68)

64

Il sistema di guida, inoltre, potrebbe essere migliorato sviluppando un algoritmo real-time che sia in grado di fornire un percorso alternativo qualora il percorso originale non sia eseguibile per via delle caratteristiche aeromeccaniche del velivolo che ne impedirebbero la percorrenza.

(69)

65

Bibliografia

[1] Pietro Rosata. “Identification of a small scale helicopter dynamic model”.Master Thesis. Università di Pisa, 2015.

[2] Vito Grasso. “Dynamic inversion control for a small-scale helicopter”. Master Thesis. Università di Pisa, 2016.

[3] Giulia Gori. “Development and validation of flight control laws for small-scale helicopter”. Master Thesis. Università di Pisa, 2016.

[4] Carlo Casarosa. “Meccanica del volo” Pisa University Press

[5] Tor Marius Jensen. “Waypoint-Following guidance based on feasibility algorithms”. Master of science in engineering cybernetics. Norwegian University 2011

[6] Anastasios M.Lekkas. “Guidance and path-planning system for autonomous vehicles”. Master Thesis. Norwegian University 2014

[7] Morten Breivik. “Nonlinear maneuvering control of underactuated ships” Master Thesis.Norwegian University 2003

[8] A. Voelker, K. Kouramas “Three-layer Aircraft Control: Path-planning,Guidance, Model Predictive Controller as Autopilot” Imperial College, Centre for Process Systems Engineering, London

[9] Hongbo Zhou, Hailong Pei and Yunji Zhao “Trajectory tracking control of a small unmanned helicopter using MPC and Backstepping” 2011 American Control Conference on O'Farrell Street, San Francisco, CA, USA

[10] C. L. Castillo, W. Moreno, K. P. Valavanis “Unmanned Helicopter Waypoint Trajectory Tracking Using Model Predictive Control” Computer Science and Engineering Unmanned Systems Laboratory USF, Tampa, FL 33620, USA

[11] Ionela Prodan, Sorin Olaru, Cristina Stoica “Predictive Control for Autonomous Aerial Vehicles Trajectory Tracking” SUPELEC Systems Sciences (E3S) -Automatic Control Department, 3 rue Joliot Curie, 91192, Gif sur Yvette, France [12] Lorenzo Cancemi. “Tecniche di guida innovative in scenari complessi” Master

(70)

66

[13] Gabriel Ferri. “Sistema di guida Fuzzy per veicoli non pilotati in presenza di ostacoli: teoria ed esperimenti” Master Thesis. Università di Pisa, 2006