Sintesi di un sistema di guida aumentato con funzione di visibilità

Università degli Studi di Pisa

Dipartimento di Ingegneria dell’Informazione Corso di Laurea Magistrale in

Ingegneria Robotica e dell’Automazione

������� �� �� ������� �� �����

��������� ��� �������� �� ����������

���������� Luca Tardioli

���������

Prof. Mario Innocenti ���������������

S O M M A R I O

Lo scopo di questa tesi è quello di gettare delle basi teoriche per la realizzazione di un sistema di guida con ostacoli sfruttando una funzione di visibilità.

La prima parte della tesi propone due modelli di ambiente con ostacoli, uno attraverso LMI, l’altro attraverso funzioni di appartenenza della Logica Fuzzy.

Per mezzo di questi modelli vengono descritte due funzioni di visibilità che hanno lo scopo di attribuire ad ogni punto dello spazio un valore nell’intervallo [0, 1]che indica quanto un target è visibile da una certa posizione.

La funzione di visibilità creata attraverso la Logica Fuzzy verrà poi utilizzata nella seconda parte della tesi in cui viene definito un Gioco Differenziale Pursuit-Evasion in un ambiente con ostacoli.

Risolvendo questo gioco si ottengono due strategie di guida utilizzabili in feed-back senza dover ricorrere ad Algoritmi di Planning o a calcoli computazionali complessi.

Le strategie di guida sono state implementate in ambiente simulativo per verifi-carne l’efficacia.

Infine vengono fatte alcune osservazioni al fine di poter semplificare ulterior-mente la legge trovata per il Pursuer e ottenere una legge di guida che sia, almeno in parte, riconducibile alla famosa PNG.

I N D I C E

1 introduzione 1

2 visibilità in un ambiente con ostacoli 6 2.1 Modellazione dell’ambiente attraverso LMI. 7

2.2 Modellazione dell’ambiente attraverso funzioni di appartenenza. 9 2.3 Funzione di visibilità 17

2.4 Normalizzazione della funzione di visibilità 23 2.5 Discretizzazione della funzione di visibilità 26 3 giochi differenziali 30

3.1 Formulazione del gioco 31 3.2 Rilassamento dei vincoli 33 3.3 Risoluzione del Gioco 34

3.4 Intercettazione nel caso di doppi integratori 40 4 simulazioni 43

4.1 Simulazione 1: Guadagno di Visibilità del Pursuer 43 4.2 Simulazione 2: Comportamento dell’Evader 46 4.3 Simulazione 3: Quantità di Energia del Pursuer 46

4.4 Simulazione 4: Vantaggi del modello con funzioni di

appartenen-za 53

5 conclusioni e lavori futuri 61

5.1 Semplificazione della Legge di Guida 61 5.2 Conclusioni 64

a appendice: calcolo Li(t, tf) 66

b appendice: dalle strategie ottime alla png 69 bibliografia 71

1

I N T R O D U Z I O N E

Il lavoro di questa tesi può essere diviso in due parti: nella prima si parlerà della modellazione di ambienti con ostacoli, logica Fuzzy e funzioni di visibilità men-tre nella seconda si imposterà un problema Pursuit-Evasion utilizzando i Giochi Differenziali.

Lo scopo è quello di trovare una legge di guida che permetta ad un primo sistema, chiamato Pursuer, di intercettarne un secondo, chiamato Evader.

Entrambi i sistemi sono immersi in uno scenario con dei vincoli, degli ostacoli che vanno ad influenzare il loro comportamento.

Spesso per muoversi in un ambiente con ostacoli si parla di Algoritmi di Piani-ficazione [5].

Figura 1: Esempio di Cell Decomposition di un ambiente con ostacoli.

Questi algoritmi permettono, attraverso tecniche continue di pianificazione com-binatoria (Cell Decomposition, Diagrammi di Voronoi...) o tecniche di

pianificazio-introduzione ne discreta basate sul campionamento, di modellare l’ambiente e, utilizzando la teoria dei grafi, trovare un cammino che soddisfi opportune specifiche.

Tuttavia questi algoritmi di pianificazione permettono, come dice la parola stes-sa, di pianificare prima la traiettoria da seguire per poi utilizzare delle leggi di guida più semplici per raggiungere effettivamente l’obiettivo.

Per esempio, modellando un ambiente attraverso la Cell Decomposition, come in Figura 1, si arriva a costruire un grafo in cui ogni nodo rappresenta una certa posizione nello spazio.

Due nodi sono adiacenti se è possibile unire con un segmento le due posizioni senza che questo intersechi alcun ostacolo.

Si capisce che, una volta trovato un cammino su tale grafo, la legge di guida per portare il robot da un nodo ad un altro è estremamente semplice perché si tratta di fargli seguire una traiettoria rettilinea certi che non ci saranno ostacoli lungo il percorso.

Tuttavia, in uno scenario in cui il target da raggiungere è mobile, essendo la sua traiettoria non nota a priori, è pressoché impossibile fare un qualche tipo di pianificazione del moto.

Per questo c’è la necessità di avere una legge di guida che agisca “real-time” sul sistema in base ai movimenti del target e che consideri in qualche modo gli ostacoli presenti nell’ambiente.

Si parlerà quindi di funzione di visibilità la quale introdurrà il concetto di quanto una zona dello spazio sia visibile da un certo punto di vista.

Questo non è un concetto completamente nuovo perché viene trattato in [2], in questa tesi viene ripreso ed in parte modificato.

Una legge di guida per muoversi tra gli ostacoli sarà quindi molto più complessa rispetto a quelle utilizzate nel caso in cui si effettui una pianificazione del moto prima di “avviare” il robot.

Per trovare questa legge si utilizzano i Giochi Differenziali che nella Teoria dei Giochi rappresentano un gruppo di problemi che modellano scenari al cui interno si trovano dei sistemi dinamici.

Il primo a trattare questi problemi fu Isaacs in [4].

Per la risoluzione di questo tipo di giochi si utilizzano gli strumenti forniti dal Calcolo delle Variazioni, gli stessi utilizzati nel Controllo Ottimo.

I Giochi Differenziali infatti possono essere visti come un’estensione del più comune Controllo Ottimo: in quest’ultimo caso lo scopo è quello di trovare, dato un sistema dinamico, il controllo migliore secondo un certo funzionale di costo.

introduzione Nel caso dei Giochi Differenziali i controlli da trovare, che prendono il nome di strategie, riguardano più sistemi, che prendono il nome di giocatori.

Lo scopo è quello di trovare, se esiste, un equilibrio di Nash.

Ciascun giocatore avrà un suo funzionale di costo da minimizzare ma questo dipenderà anche dalle strategie adottate dagli altri giocatori.

Tutti i sistemi presenti infatti interagiscono fra loro in maniera cooperativa se devono raggiungere uno scopo comune o in maniera non cooperativa se hanno obiettivi contrastanti.

Ci sono in letteratura diversi testi ed articoli che trattano lo scenario Pursuit-Evasion come gioco differenziale.

Uno dei primi lavori fu svolto da Ho, Bryson e Baron in [3] i quali, impostando un gioco non cooperativo a somma zero in cui il Pursuer cercava di intercettare un Evader in fuga, arrivano a definire due strategie ottime, una per ciascun giocatore.

Probabilmente la parte più interessante ai fini pratici è quella finale in cui viene mostrato che, sotto certe condizioni, il controllo sul Pursuer trovato attraverso il gioco differenziale coincide con la PNG ossia una delle leggi di guida più usate in ambito missilistico.

Ciò che ha reso questa legge così popolare è stata la sua semplicità, efficacia e facilità di implementazione [6].

Quello che si spera di ottenere alla fine di questa tesi è una legge di guida in un ambiente con ostacoli che sia altrettanto semplice.

Esistono diversi articoli che trattano di scenari Pursuit-Evasion inserendo osta-coli statici o dinamici nell’ambiente, tuttavia le soluzioni proposte vengono tutte calcolate numericamente e richiedono un costo computazionale molto elevato.

Spesso infatti bisogna risolvere dei sistemi di equazioni differenziali a due pun-ti, se si utilizzano le condizioni necessarie sulla variazione prima, o sistemi alle derivate parziali, se vengono utilizzate quelle sufficienti.

Per la risoluzione dei sistemi di equazioni differenziali a due punti esistono algo-ritmi che possono essere divisi in: Algoalgo-ritmi di Shooting e Metodi alle Differenze Finite.

Gli Algoritmi di Shooting attribuiscono un valore alle condizioni iniziali man-canti trasformando il sistema in un problema di Cauchy classico.

Risolvendo il sistema si va a prendere l’errore tra le variabili finali ottenute e quelle date dal problema originario e si cerca, nella successiva iterazione, di modificare le condizioni iniziali per ottenere un errore inferiore.

introduzione I metodi alle differenze finite invece si servono della discretizzazione del sistema di equazioni differenziali dividendo tutto il range di integrazione in intervalli di lunghezza finita.

In entrambi i casi le soluzioni vanno calcolate prima e poi caricate sul robot e questo implica che il controllo è in open-loop e che va bene per quelle specifiche condizioni iniziali.

Qualora vengano cambiate tali condizioni iniziali è necessario risolvere numeri-camente da capo l’intero problema.

Il controllo che si vuole trovare in questa tesi invece è un controllo da utilizzare in feedback che dipenda solamente dallo stato attuale dei sistemi e che possa essere utilizzato sempre, a prescindere dalle condizioni iniziali.

Per riassumere, i vincoli e gli obiettivi per la legge di guida che si vuole trovare sono:

• Non utilizzare, o utilizzare il meno possibile, algoritmi di pianificazione. • Trovare una strategia di guida analitica anche effettuando semplificazioni

dell’ambiente o approssimazioni.

• Il controllo deve essere utilizzabile in feedback e non deve dipendere dalle condizioni iniziali.

• La guida trovata deve essere il più semplice possibile. La tesi è stata organizzata nella seguente maniera:

• Nel Capitolo 2 vengono proposti due modelli di ambiente uno dei quali uti-lizza la logica Fuzzy per stabilire se un certo vettore posizione appartiene o meno ad un ostacolo. Si passa poi a descrivere la funzione di visibilità già proposta in [2] alla quale vengono applicate alcune modifiche. Infine si parla della normalizzazione della discretizzazione di tale funzione.

• Nel Capitolo 3 si imposta il problema di intercettazione in ambiente con osta-coli e sfruttando un modello di ambiente e la funzione di visibilità descritta in Capitolo 2. Successivamente si passa alla sua risoluzione per arrivare alla fine a due strategie di guida in feedback.

introduzione • Nel Capitolo 4 vengono presentate quattro simulazioni che mostrano il

com-portamento delle leggi appena trovate e i miglioramenti della nuova funzio-ne di visibilità rispetto a quella proposta in [2].

• Nel Capitolo 5, infine, vengono proposte alcune semplificazioni che possono essere fatte al controllo del Pursuer trovato nel Capitolo 3 per rendere la legge di guida ancora più semplice.

2

V I S I B I L I TÀ I N U N A M B I E N T E C O N O S TA C O L I

Prima di poter trovare le strategie ottime di guida in un ambiente con ostacoli bisogna descrivere nel dettaglio la funzione di visibilità.

Lo scopo di questa funzione è quello di stabilire, in qualche modo, quanto un oggetto sia visibile rispetto ad una certa posizione.

In questa tesi si indicherà la posizione nello spazio di un punto materiale attra-verso un vettore r.

Tutto ciò che verrà mostrato in questo capitolo può essere applicato sia ad uno spazio bidimensionale che ad uno tridimensionale, tuttavia spesso le immagini e i grafici riguarderanno esempi di ambienti in 2D poiché risultano più semplici da ca-pire e spesso sarà impossibile mostrare gli stessi grafici per il caso tridimensionale a causa del maggior numero di dimensioni richieste.

Un vettore generico r ∈ Rn _{individua un punto nello spazio che sarà indicato}

con la lettera S e avrà coordinate (rx, ry)se n = 2 o (rx, ry, rz)se n = 3.

Spesso infatti sarà necessario riferirsi ad un segmento che unisce due punti individuati da due vettori rie rj e tale segmento verrà indicato con SiSj.

In questo capitolo per prima cosa verranno proposti due modelli di ambienti: il primo utilizza LMI per modellare gli ostacoli mentre il secondo funzioni di appartenenza Fuzzy.

Successivamente si passerà a descrivere due funzioni di visibilità ognuna legata ad un tipo di modello dell’ambiente.

Si parlerà della normalizzazione di tale funzione resa necessaria da particolari configurazioni che possono venirsi a creare.

Infine si passerà a descriverne la discretizzazione in maniera da renderla utiliz-zabile in un ambiente di simulazione numerico.

2.1 modellazione dell’ambiente attraverso lmi.

r

x

-5

0

5

10

r

y

-5

0

5

10

Figura 2: Esempio di ambiente con un singolo ostacolo. 2.1 modellazione dell’ambiente attraverso lmi.

Il primo modello utilizzato per descrivere l’ambiente consiste nell’utilizzare LMI, una per ciascun ostacolo.

Vengono utilizzati dei sistemi di disequazioni per stabilire se un certo vettore ˜r si trova all’interno di un ostacolo o meno.

L’insieme dei punti dell’i-esimo ostacolo può essere descritto come:

O_i=�r∈ Rn|A_ir� bi

�

(1) dove Ai∈ Rai×n e bi∈ Rai con ai∈ N+₀.

Come esempio si può prendere l’ambiente bidimensionale di Figura 2 con un solo ostacolo rettangolare e descriverlo, attraverso la (1), con un LMI dai seguenti parametri.

2.1 modellazione dell’ambiente attraverso lmi. A1=         1 0 −1 0 0 1 0 −1         b1=         6 2 5 2        

In questa maniera si riesce a modellare perfettamente questo particolare ostacolo senza dover effettuare approssimazioni tuttavia ciò non sempre è possibile.

Infatti attraverso LMI si possono modellare perfettamente dei poligoni conves-si ma quando nell’ambiente conves-si trovano degli ostacoli dalla forma più complessa, come per esempio un vincolo circolare, è necessario effettuare delle approssima-zioni.

Comunque con questo metodo si possono descrivere un gran numero di osta-coli con approssimazioni accettabili tuttavia, qualora il problema richiedesse una modellazione migliore, sarà necessario ricorrere ad espressioni più complesse.

In questa tesi gli ambienti presi in considerazione avranno sempre degli ostacoli modellabili, o ben approssimabili attraverso LMI.

In generale l’insieme dei punti dello spazio Euclideo Rn_{occupati da N ostacoli}

è dato dall’unione di tutti gli insiemi definiti nella (1).

O=

N

�

i=1

O_i

Il vantaggio di questo modello è che per rappresentare un ostacolo vengono utilizzate delle disuguaglianze lineari e risulta piuttosto semplice verificare se un certo vettore posizione ˜r si trovi all’interno di un ostacolo o meno.

Infatti, se si verifica che ˜r soddisfa almeno una delle N LMI, allora questo vettore si troverà all’interno di un ostacolo, altrimenti apparterrà allo spazio libero.

Si indica con Cfree = Rn\O l’insieme dei punti non appartenenti all’insieme

degli ostacoli.

La funzione f: Rn _{→ {0, 1} è una funzione Booleana e indica se un vettore}

posizione appartiene all’insieme Cfreeo meno.

f(r) =      0 r∈ O 1 r�∈ O (2)

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza.

10

5

0

r

x

-5

-10

-10

-5

0

r

y

5

1

0.5

0

10

Figura 3: Modello dell’ambiente di Figura 2 attraverso la (2).

Questa funzione restituirà uno se il vettore r è fuori da tutti gli ostacoli, zero altrimenti.

La Figura 3 mostra nell’ambiente di Figura 2 i valori assunti dalla (2).

2.2 modellazione dell’ambiente attraverso funzioni di apparte-nenza.

Il modello utilizzato nella sezione 2.1 permette di capire se, dato un vettore posi-zione, esso appartenga o meno all’insieme O.

Il risultato che otteniamo può essere espresso come un valore binario: dato un vettore r esso appartiene all’insieme degli ostacoli se almeno una LMI è verificata altrimenti si trova nell’insieme Cfree.

Nel modello proposto in questa sezione non si vuole solo sapere se un certo vettore è interno o meno ad un ostacolo ma anche quanto sia lontano dai bordi.

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. “fuzzy”.

Si andranno quindi a definire N funzioni di appartenenza, una per ciascun ostacolo: ˜fFi: R

n_{→ [0, 1].}

Il pedice F sta ad indicare che è una funzione di appartenenza utilizzata nella logica Fuzzy mentre la i rappresenta l’i-esimo ostacolo a cui si riferisce.

Per cominciare viene preso uno scenario con un singolo ostacolo in cui si ha una sola ˜fF(r).

Questa funzione indica il grado di appartenenza del vettore r all’ostacolo e avrà grado massimo se r ∈ O, grado minimo se sarà sufficientemente lontano dal vincolo mentre assumerà valori compresi tra zero e uno in prossimità dei bordi.

Quello che si vuole è che avvicinando il vettore r al bordo di un ostacolo il suo grado di appartenenza cominci a crescere sempre di più fino a raggiungere l’unità una volta all’interno.

Nella logica Fuzzy esistono una grande quantità di funzioni che hanno questo comportamento.

Per questo modello è stata scelta come funzione di appartenenza quella chiama-ta Generalized Bell-Shaped membership function (gbmf): fgbmf: R→ [0, 1] che ha

l’espressione (3).

fgbmf(x) =

1

1+�x−c_l �2p (3)

Lo scalare c serve a stabilire il centro della campana generalizzata, l invece serve a stabilire la lunghezza della parte piatta di campana.

Il parametro p è un numero intero positivo che serve a stabilire la pendenza della campana.

In Figura 4 viene mostrato il grafico di questa funzione di appartenenza con c= 5, l = 4 e p = 5.

Tuttavia, per motivi che saranno chiari più avanti, risulterà più utile avere il complementare di questa funzione ossia il grado di non appartenenza all’ostacolo.

Nella logica Fuzzy esistono vari tipi di complemento, in questo caso verrà utilizzato quello standard in cui f(x) = 1 − f(x).

In Figura 5 viene rappresentata la stessa funzione di Figura 4 e il suo comple-mentare.

Questa funzione di appartenenza è utile solo nel caso monodimensionale. É stato necessario quindi modificarla per adattarla ai casi bidimensionali e tridimen-sionali.

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza.

x

f

Figura 4: Funzione di appartenenza a campana di gauss generalizzata.

x

f

(x

)

f

(x)

¯

f

(x)

Figura 5: Funzione di appartenenza a campana di gauss generalizzata e il suo complementare.

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. ˜fF(r) = � 1+ ||r − c||2p_2p ˜L−1 �−1 (4) Nell’equazione (4) il termine ||r − c||2p_˜L−1 sta ad indicare la norma 2p pesata in

˜L−1_: ||r− c||2p_˜L−1 = 2p � � � ��n i=1 � (ri− ci) l−1ii �2p

Il vettore r ∈ Rn _{come al solito individua una posizione nello spazio (n={2,3}),}

le componenti del vettore c ∈ Rn_{invece individuano il centro dell’ostacolo lungo}

ogni asse.

Infine la matrice ˜L è diagonale definita positiva e l’elemento i-esimo di tale diagonale è legato alla lunghezza del tratto per cui la funzione ha valore massimo e costante (questa caratteristica è influenzata in parte dalla pendenza p come sarà mostrato più avanti).

Per definire ˜L basta un vettore l ∈ Rn_{: ˜L = diag(l).}

É facile notare che nel caso n = 1 si ritorna alla formula monodimensionale (3). ˜L è una matrice diagonale i cui autovalori corrispondono agli elementi di tale diagonale.

Affinché questa matrice sia singolare vuol dire che almeno uno di questi ele-menti deve essere nullo ma ciò è impossibile perché ciascuna componente della diagonale è legata alla lunghezza lungo un asse dell’ostacolo e nella realtà ogni ti-po di ostacolo ha una lunghezza maggiore di zero in ogni dimensione dello spazio, per quanto piccola essa sia.

L’esistenza dell’inversa nella formula (4) è garantita dal fatto che la norma re-stituisce sempre un valore maggiore o uguale a zero che sommato a uno risulterà sempre un valore diverso da zero consentendo l’operazione di inversione.

Come detto prima si preferisce avere il complemento di questa funzione di appartenenza:

fF(r) = 1 − ˜fF(r) (5)

In Figura 6 viene modellato l’ostacolo dell’ambiente di Figura 2 attraverso la (5). In questo caso è stato scelto c = [2 1.5]T_{, l = [4 3.5]}T _{e p = 10.}

Con la formula (5) non si possono però rappresentare ostacoli ruotati come quello in Figura 7.

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. 10 0

r

r

Figura 6: Modello dell’ambiente di Figura 2 attraverso la funzione (5).

r

x

-5

0

5

10

r

y

-5

0

5

10

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. 10 5 0

r

r

Figura 8: Modello dell’ambiente di Figura 7 attraverso la funzione (5). Per ruotare un ostacolo bisogna modificare la formula (4):

˜fF(r) = � 1+ ||R(r − c)||2p_2p ˜L−1 �−1 (6) R è una matrice di rotazione che dipenderà da un solo parametro θ nel caso bidimensionale e da tre parametri nel caso tridimensionale necessari per la para-metrizzazione dell’orientamento di un oggetto (per esempio utilizzando gli angoli di Eulero ZYZ).

La funzione di appartenenza (5) di uno spazio bidimensionale con l’ostacolo posizionato come in Figura 7 viene mostrata in Figura 8 e attribuisce ad ogni punto del piano un valore di appartenenza a Cfree.

Finora si è visto il caso più semplice ossia un ambiente con un solo ostacolo ma come trattare il caso in cui siano presenti più ostacoli?

Come è stato detto, ogni vincolo che si trova nell’ambiente ha una sua funzione che indica il grado di non appartenenza di un vettore r a quell’ostacolo specifico. Quello che si vuole ottenere alla fine è una funzione m: Rn_{→ [0, 1] che, preso un}

generico punto nello spazio, indichi il suo grado di non appartenenza all’insieme di tutti gli ostacoli O.

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. Per risolvere questo problema si utilizzano funzioni fornite dalla Logica Fuzzy: le norme triangolari (T-norm) per le intersezioni e le conorme triangolari (S-norm) per le unioni.

• (A ∩ B)(x) = T(A(x), B(x)) • (A ∪ B)(x) = S(A(x), B(x)

A(x)e B(x) rappresentano il grado di appartenenza di x agli insiemi Fuzzy A e Brispettivamente.

Esistono diversi tipi di T-norm e S-norm, quelli utilizzati per questo modello sono i seguenti:

• T(A(x), B(x)) = A(x) · B(x)

• S(A(x), B(x)) = A(x) + B(x) − A(x) · B(x)

Si ricorda inoltre che S(A(x), B(x)) = 1 − T(1 − A(x), 1 − B(x)) e che come ope-razione complemento si utilizza A(x) = 1 − A(x).

Si supponga che A(x) sia il grado di appartenenza di x ad un ostacolo e B(x) il grado di appartenenza dello stesso x ad un secondo ostacolo.

Quello che si vuole sapere è il grado di appartenenza di x all’intero insieme degli ostacoli O e quindi si cerca il valore dell’unione delle due funzioni di appartenenza calcolabile attraverso la T-norm: T(A(x), B(x)) = A(x) · B(x).

Come è stato detto più volte, in realtà si è interessati all’insieme complementare ossia al grado di non appartenenza a O.

Quindi si ha bisogno di conoscere (A ∪ B)(x) che in base alle leggi di De Morgan per la logica Fuzzy: (A ∪ B)(x) = A(x) ∩ B(x).

Secondo la scelta della T-norm che è stata fatta si ottiene che il grado di non appartenenza all’insieme O è dato da A ∪ B(x) = A(x) · B(x).

Quindi se si utilizzano ˜fF1(r) e ˜fF2(r) come funzioni di appartenenza ad un primo e ad un secondo ostacolo rispettivamente, seguendo i ragionamenti appena fatti si ottiene che il grado di non appartenenza a O è dato da una funzione:

m(r) = ˜fF1(r)· ˜fF2(r) (7)

Utilizzando l’operazione di complemento della formula (5), la (7) diventa:

2.2 modellazione dell’ambiente attraverso funzioni di appartenenza. A questo punto è facile generalizzare per un ambiente con N ostacoli:

m(r) =

N

�

i=1

fFi(r) (8)

La funzione m: Rn_{→ [0, 1] prende in ingresso un vettore posizione dello spazio}

e come uscita restituisce circa 0 se questo si trova all’interno di un ostacolo, circa 1 se è sufficientemente lontano da tutti gli ostacoli e valori compresi (0, 1) se si trova in prossimità di almeno un ostacolo.

Per definire un ambiente con N ostacoli si avrà bisogno di diversi parametri. • Una matrice C ∈ RN×n in cui l’i-esima riga corrisponde alle coordinate del

centro dell’i-esimo ostacolo.

• Una matrice L ∈ RN×nin cui l’i-esima riga corrisponde alla diagonale della matrice ˜Lidell’i-esimo ostacolo.

• Uno scalare p ∈ N0 per il tipo di norma da usare nella (6).

• Nel caso bidimensionale un vettore Θ ∈ RN _{la cui componente i-esima}

in-dichi la rotazione dell’i-esimo ostacolo. Nel caso tridimensionale sarà ne-cessario avere tre vettori Ψ, Φ, Θ ∈ RN _{il cui i-esimo elemento indichi}

l’orientazione dell’i-esimo ostacolo secondo una qualche parametrizzazione. Prima di passare a definire la funzione di visibilità e cosa rappresenta bisogna chiarire ancora un paio di dettagli su questo modello.

Per prima cosa il parametro l nella funzione fF(r).

Per capire meglio cosa rappresenta questo parametro si utilizzano i grafici della funzione monodimensionale con c = 0, l = 5 e p variabile.

Dall’immagine di Figura 9 si vede come la zona in cui la fgbmf(x) è circa nulla

dipende non solo dal parametro l, che è fisso, ma anche dalla pendenza p.

Il parametro l indica la distanza tra il centro e il valore in cui la funzione fgbmf(x) = 0.5.

Per modellare l’ambiente si vuole che se r ∈ Oiil valore della funzione fFi(r)≈ 0 ∀ i = {1, 2, ..., N}.

Quando si modella l’ambiente è necessario scegliere l in maniera da tener conto del tratto in cui la funzione non è nulla e che deve risultare fuori dall’ostacolo.

Se il vettore di ∈ Rn indica la lunghezza dei lati dell’i-esimo ostacolo, li può

essere scelto come li= 1₂di+ �1n. Con 1n si intende un vettore di dimensione n

2.3 funzione di visibilità

x

f

(x

)

p

= 2

p

= 8

p

= 30

Figura 9: Funzione complementare a (3) al variare di p.

Scegliendo � in maniera opportuna si può fare in modo che l’i-esimo ostacolo rientri completamente nella zona in cui fFi(r)è circa nulla.

Infine bisogna dire che con questo tipo di modello si possono rappresentare solo ostacoli rettangolari, nel caso bidimensionale, o a forma di parallelepipedo, nel caso tridimensionale.

Questo vuol dire che ostacoli con forma diversa rispetto a quelle appena nomi-nate devono essere approssimate come viene mostrato in Figura 10.

In questo caso ci sono delle zone del piano che nel modello approssimato risultano appartenere all’insieme O ma che in realtà appartengono a Cfree.

2.3 funzione di visibilità

In questo capitolo verranno inseriti nell’ambiente con ostacoli due sistemi model-lati attraverso due punti materiali e verrà definita una funzione di visibilità.

Questa funzione ha lo scopo di indicare se tra i due sistemi esiste o meno una line-of-sight (LOS) ossia se il segmento congiungente i due punti si trova interamente in Cfree.

Presi due vettori posizione rp∈ Rne re∈ Rnche individuano due punti nello

2.3 funzione di visibilità

r

r

Obstacle approximation

Real obstacle

Figura 10: Approssimazione di un ostacolo triangolare con uno rettangolare. bidimensionale con un solo ostacolo.

Nella configurazione di Figura 11a, poiché il segmento che congiunge i due pun-ti non interseca l’ostacolo, si può affermare che i due sistemi si vedono (esistenza della LOS). Al contrario, nella configurazione di Figura 11b, i due sistemi non si vedono a causa del vincolo interposto fra di loro.

Per dare una definizione più rigorosa si può affermare che: dati due punti Sp e

Se esiste una LOS se e solo se SpSe∩ O = ∅.

Questa ovviamente è una definizione booleana perché la LOS tra i due punti o esiste o non esiste.

Tuttavia, riferendosi anche al lavoro fatto da [2] si vuole utilizzare una funzione di visibilità che fornisca dei valori compresi tra [0, 1] in modo da stabilire quanto la visibilità di un certo sistema nell’ambiente sia buona.

Nella realtà infatti gli oggetti (robot, veicoli...) non sono dei punti materiali adi-mensionali ma hanno una loro grandezza. Questo vuol dire che un sistema può essere solo parzialmente nascosto da un ostacolo e quindi la sua visibilità non sarà ottima e nemmeno nulla ma assumerà un valore intermedio.

Questa è una delle possibili interpretazioni della funzione di visibilità proposta in [2] e che viene ripresa ed in parte modificata in questa tesi.

Un’altra possibile interpretazione è quella in cui un primo sistema, posizionato in Sp, non conosca perfettamente la posizione di un secondo sistema, posizionato

2.3 funzione di visibilità rx -5 0 5 10 ry -5 0 5 10 Sp Se

(a) In questa configurazione la LOS esiste.

rx -5 0 5 10 ry -5 0 5 10 Sp Se

(b) In questa configurazione la LOS non esiste.

Figura 11: Esempio di esistenza/non esistenza della LOS.

in Se, (per esempio a causa del rumore sui suoi sensori) e quindi attribuisce ad

ogni punto dello spazio una certa probabilità di “presenza” del secondo sistema. Secondo questa interpretazione la funzione di visibilità avrà valore unitario se si vedono tutti i punti dello spazio in cui la probabilità di “presenza” del secondo modello è diversa da zero mentre sarà nulla se nessuno di questi punti è visibile da Sp.

L’espressione di questa funzione p: Rn_{× R}n_{→ [0, 1] è la seguente:}

p(rp,re) =

�

Rn

fe(re,r)fLOS(rp,r)dr (9)

La funzione fe: Rn× Rn→ R+₀ prende in ingresso un vettore posizione re ed

uno generico r.

Una possibile scelta di fe(re,r) può essere una distribuzione gaussiana con

media in re. fe(re,r) = � 1 (2π)ndet(Σ)exp � −1 2�r − re� 2 Σ−1 � (10) La Σ ∈ Rn×n _{è una matrice definita positiva e quindi invertibile. Con �r −}

re�Σ−1 si indica la norma due pesata sulla matrice Σ−1secondo la formula (11).

�r − re�Σ−1 = �

2.3 funzione di visibilità

r

r

S

S

Figura 12: Esempio di esistenza/non esistenza di LOS in un intorno circolare di Se.

Lo scopo di questa funzione è quello di pesare un vettore posizione generico r con la posizione del vettore re.

Il valore di questa funzione sarà massimo quando il vettore r coinciderà con re

mentre mano a mano che la distanza con quest’ultimo aumenta il valore della (10) sarà sempre più vicino a zero.

La funzione fLOS: Rn× Rn→ [0, 1] è una funzione che serve a stabilire se esiste

una LOS tra un vettore posizione generico r ed il vettore rp.

Anche di questa funzione possono esserci infinite varianti comunque in questa tesi vengono proposte due versioni: la prima è quella riportata in [2] ed è legata al modello di ambiente descritto nella sezione 2.1 mentre la seconda è legata al modello riportato nella sezione 2.2.

Il primo tipo di fLOS: Rn× Rn→ {0, 1} è una funzione booleana che restituisce

uno se esiste la LOS tra r e rp zero altrimenti.

fLOS(rp,r) =      0 se ∃ ˜S ∈ SpS: ˜S∈ O 1 altrimenti (12)

Per capire meglio come funziona questa funzione si fa riferimento alla Figura 12 dove, per semplicità, vengono mostrati solo alcuni di questi segmenti presi in

2.3 funzione di visibilità

r

r

O

O

O

O

Figura 13: Ambiente generico con quattro ostacoli.

un intorno circolare di Se e vengono riportati in rosso quelli che attraversano

l’ostacolo ed in verde quelli che invece hanno tutti i punti nell’insieme Cfree.

Per i vettori di ingresso r i cui relativi segmenti sono rossi il valore della fLOS(rp,r)

è nullo mentre per quelli a cui corrispondono segmenti verdi la stessa funzione assume valore unitario.

Ognuno di questi segmenti viene pesato in maniera diversa attraverso la (10). Fissato rpil segmento che lo unisce ad un punto molto vicino a reavrà un peso

più grande rispetto a quello che si trova ad una distanza maggiore.

Si prenda un ambiente generico come quello mostrato in Figura 13 e lo si modelli attraverso LMI ottenendo le matrici definite in (13).

2.3 funzione di visibilità 40 30 20 10

r

r

p

(r

,

r

)

r

Figura 14: Funzione di visibilità dell’ambiente di Figura 13 utilizzando la f_LOS(rp,r) descritta in (12). A1 =         1 −1 −1 1 −1 −1 1 1         b1=         −2 10 −4 20         A2 =         1 0 −1 0 0 1 0 −1         b2 =         29 −7 −1 6         A3 =         1 −1 −1 1 −1 −1 1 1         b3=         24 −14 −28 38         A4 =         1 0 −1 0 0 1 0 −1         b4 =         31 −17 18 −14         (13)

La Figura 14 mostra i valori assunti dalla funzione di visibilità per l’ambiente di Figura 13 al variare di rpe con re= [15 10]T.

Dal grafico tridimensionale si vede che il valore di questa funzione è massimo quando si ha piena visibilità di re e del suo intorno mentre è nullo all’interno

degli ostacoli o in una posizione in cui re risulta ben nascosto da uno di essi.

Questa funzione è discontinua: si vede che in prossimità di alcuni bordi degli ostacoli la funzione di visibilità salta da un valore unitario ad uno nullo.

2.4 normalizzazione della funzione di visibilità La discontinuità è legata alla funzione fLOS(rp,r) ed un modo per eliminarla

è modificare questa funzione utilizzando una procedura simile a quella mostrata nella sezione 2.2 facendo assumere a fLOS(rp,r)dei valori compresi tra [0, 1].

Preso quindi il solito segmento che unisce rp ad un punto individuato da un

vettore generico r bisogna dare a tale segmento un grado di non appartenenza all’insieme degli ostacoli.

Ogni vettore ˜r che individua un punto del segmento SpSavrà un certo grado di

appartenenza dato dalla funzione m(˜r) di (8).

Come valore di fLOS(rp,r) si sceglie quello del punto del segmento per cui (8)

assume il valore più piccolo.

fLOS(rp,r) = min ˜r∈SpS

m(˜r) (14)

Facendo riferimento all’ambiente di Figura 13 e modellando gli ostacoli come funzioni di appartenenza i parametri necessari per la sua descrizione sono: le matrici C e L, il vettore Θ, stando su uno spazio bidimensionale, e gli scalari p e �. Il loro significato è stato spiegato nella sezione 2.2.

C=         3 9 18 −3.5 26 7 24 16         L=         2√2 4√2 11 5₂ 5 2 √ 2 5₂√2 14 4         Θ=         −π₄ 0 π 4 0         p= 10 �= 1

In Figura 15 è riportato il grafico tridimensionale della funzione di visibilità appena descritta e si vede che in questo caso sono state eliminate le discontinuità che si vedevano in Figura 14.

Infine in Figura 16 vengono riportate le curve di livello della funzione di visibi-lità.

2.4 normalizzazione della funzione di visibilità

Per come è stata definita la funzione di visibilità questa assume valore quasi unita-rio quando reed un suo intorno è pienamente visibile dalla posizione rp. Tuttavia

2.4 normalizzazione della funzione di visibilità 40 30 20

r

p

(r

,

r

)

r

r

Figura 15: Funzione di visibilità dell’ambiente di Figura 13 utilizzando la fLOS(rp,r) descritta in (14).

r

x

r

y

p(r, r

)

r

2.4 normalizzazione della funzione di visibilità esistono ambienti simili in cui, a parità di configurazione dei vettori rp ed re, la

funzione di visibilità assume valori diversi come nel caso di Figura 17.

Due ambienti sono simili se differiscono tra loro solo per un fattore di scala o per una diversa orientazione complessiva cioè tutti gli ostacoli vengono ruotati di uno stesso angolo attorno ad un asse comune.

rx 5 10 15 20 25 30 ry 0 5 10 15 Sp Se

(a) Configurazione in cui la funzione di visibilità non assume valore unitario.

rx 20 30 40 50 60 70 80 90 ry 0 10 20 30 40 Sp Se

(b) Configurazione in cui la funzione di visibilità assume valore unitario.

Figura 17: Esempio di ambienti simili.

Si vede che in Figura 17b tutto l’intorno di Se è visibile cosa che non accade

in Figura 17a.

Il cerchio nero indica l’intorno di Se al di fuori del quale la funzione peso

fe(r, re)ha valori molto vicini allo zero.

Ciò non è dovuto ad una diversa configurazione dei punti Spe Se ma

semplice-mente al fatto che l’ambiente di Figura 17b è lo stesso di Figura 17a ma con una scala diversa.

Questo può dare fastidio perché per una stessa configurazione dei due vettori presi in considerazione e degli ostacoli, cambiando solo un fattore di scala, si hanno valori di visibilità differenti.

Un’altra configurazione da vedere è quella mostrata in Figura 18.

In questo caso dalla posizione rp che individua il punto Sp si ha una visione

perfetta di Se ma la visibilità non è unitaria a causa del fatto che parte dell’intorno

sta all’interno di un ostacolo.

La (9) viene modificata attraverso una funzione n(re)che ha lo scopo di

norma-lizzare la funzione di visibilità.

n(re) =

�

Rnfe

(re,r)m(r)dr (15)

2.5 discretizzazione della funzione di visibilità

r

r

S

S

Figura 18: Configurazione in cui Sp vede benissimo Se ma la visibilità non è vicina all’unità. p(rp,re) = 1 n(re) � Rn fe(re,r)fLOS(rp,r)dr (16)

L’esistenza dell’inversa di n(re) è garantita dal fatto che nell’ambiente ci sarà

sempre almeno un vettore r ∈ Cfree per cui m(r) �= 0 e quindi, visto che fe(re,r) è

sempre strettamente maggiore di zero, l’integrale (15) assumerà sempre un valore non nullo. Non ha senso infatti, considerare ambienti in cui C_free=∅.

2.5 discretizzazione della funzione di visibilità

Poiché la funzione di visibilità descritta nella sezione 2.3 verrà utilizzata in simu-lazione numeriche, è necessario avere una sua espressione discreta per renderla utilizzabile.

Per prima cosa bisogna definire un dominio di integrazione che ovviamente non può essere tutto Rn _{ma bisogna limitarlo.}

Si fa notare che la funzione (10) è quasi nulla quando r si trova sufficientemente lontano da re.

La grandezza d questa distanza dipende dalla scelta della varianza come mo-strano i grafici di Figura 19 in cui sono rappresentate gaussiane a media nulla.

2.5 discretizzazione della funzione di visibilità

x

f

(x

)

Figura 19: Distribuzioni gaussiane con differenti varianze.

Per esempio, facendo riferimento sempre alla Figura 19, per la distribuzione gaussiana in blu si può scegliere un dominio più piccolo rispetto a quella di colore giallo.

Quando si passa al caso bidimensionale e tridimensionale le varianze lungo gli assi possono essere scelte diverse tra loro e quindi anche gli intervalli presi su ciascuno di questi assi saranno diversi.

Per semplicità in questa tesi le varianze saranno sempre le stesse lungo tutti gli assi.

Per effettuare una discretizzazione va scelto un dominio di integrazione che deve essere simmetrico rispetto a re. In genere si sceglierà un dominio B quadrato

nel caso bidimensionale, o cubico nel caso tridimensionale, con baricentro in re.

Una volta definito un dominio limitato bisogna discretizzare l’integrale creando una griglia in B e trasformare le funzioni continue (10) e (14) in funzioni discrete. Ovviamente la qualità dell’approssimazione dipenderà dalla grandezza dell’in-tervallo scelto che verrà indicata con la lettera s.

In Figura 20 viene mostrata la discretizzazione della stessa funzione gaussiana con due diverse s.

Chiaramente con un valore di s più piccolo si ottiene una migliore approssima-zione ma un maggior carico computazionale.

La scelta di s dipenderà anche dal valore della varianza poiché con una campana più larga si possono scegliere degli intervalli più grandi e mantenere comunque

2.5 discretizzazione della funzione di visibilità x -4 -2 0 2 4 6 8 f (x ) 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Discretef unction P iecewisef unction Continuousf unction

(a) Lunghezza dell’intervallo s = 0.5.

x -4 -2 0 2 4 6 8 f (x ) 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Discretef unction P iecewisef unction Continuousf unction (b) Lunghezza dell’intervallo s = 1.

Figura 20: Esempi di discretizzazione al variare di s. una buona approssimazione come mostra la Figura 21.

x -4 -2 0 2 4 6 8 f (x ) 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Discretef unction P iecewisef unction Continuousf unction

(a) Lunghezza dell’intervallo s = 1 e σ2_{= 1.}

x -5 0 5 10 f (x ) 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 Discretef unction P iecewisef unction Continuousf unction (b) Lunghezza dell’intervallo s = 1 e σ2_{= 5.}

Figura 21: Esempi di discretizzazione al variare di σ2_.

Per calcolare l’integrale della curva non si fa altro che sommare l’area dei singoli rettangoli, il cui contorno superiore nei grafici è stato tracciato dalle linee rosse.

Infatti il valore della funzione viene considerato costante per tutta la lunghezza dell’intervallo.

Nel caso bidimensionale gli intervalli saranno dei quadrati mentre in quello tridimensionale dei cubi.

Tutti i punti all’interno di un quadrato o di un cubo della griglia avranno lo stes-so valore e nel castes-so bidimensionale verranno utilizzati gli indici i e j per riferirsi all’i-esimo e al j-esimo elemento della griglia lungo l’asse rxe ryrispettivamente.

2.5 discretizzazione della funzione di visibilità

pD(rp,re) =

�

i,j∈B

fe(ri,j,re)fDLOS(rp,ri,j)ΔrxΔry (17)

I parametri Δrx e Δry indicano la grandezza degli intervalli lungo i due assi. Alla (17) va aggiunta la normalizzazione descritta nella sezione 2.4.

Quindi il dominio di integrazione non sarà tutto Rn _{ma solo un intorno di}

re mentre la discretizzazione delle altre funzioni e dell’integrale avviene nella

maniera appena descritta.

Infine per poter utilizzare la (14) è necessario discretizzare anche i segmenti: invece di calcolare il minimo di m(r) su tutti gli infiniti punti di un dato segmento si considerano solo un numero finito essi.

Quindi dato un segmento SpS, individuato dai vettori rpe r, questo verrà diviso

in Nspunti dove il punto i-esimo Siviene individuato dal vettore ri.

Si definisce con Γ_SpS l’insieme di tutti questi punti:

Γ_SpS= {Si i= 1,.., Ns}

La formula (14) diventerà:

fDLOS(rp,ri,j) = min Si∈ΓSpS

3

G I O C H I D I F F E R E N Z I A L I

In questo capitolo viene impostato e risolto un Gioco Differenziale per lo scenario Pursuit-Evasion. Prima di cominciare però bisogna chiarire alcuni aspetti generali.

Nei Giochi Differenziali si hanno dei funzionali di costo da minimizzare cioè bisogna definire un J: V → R, dove V è uno spazio di funzioni ossia uno spazio vettoriale a infinite dimensioni.

Jquindi prende in ingresso funzioni e restituisce uno scalare. Una generica funzione x(t) può essere definita come:

x: [t0, tf]→ Rn (18)

Tale funzione prende in ingresso uno scalare nell’intervallo [t0, tf]e restituisce

un vettore.

In questa tesi per definire una funzione si usa la notazione (18) mentre con x_{∈ R}n_{si fa riferimento alle dimensioni dell’insieme o del sottospazio immagine}

di tale funzione.

In generale le uscite di una funzione possono essere limitate e non comprendere tutto un determinato spazio.

Per esempio u: [t0, tf]→ U dove U è un insieme incluso in Rm che può essere

definito come:

U= {u_{∈ R}m | u� u} con u vettore noto.

In questo caso specifico vengono presi solo le funzioni le cui componenti nel sottospazio Immagine risultano minori o uguali delle rispettive componenti del vettore u.

3.1 formulazione del gioco Un altro modo per definire un funzionale che dipende da più funzioni, per esempio J(x(t), u(t)), è il seguente:

J: Rn× U → R

In questo gioco saranno presenti due giocatori, un Pursuer e un Evader, per riferirsi a grandezze che riguardano il primo si utilizzerà il pedice p, per quelle che riguardano il secondo il pedice e, se invece si vuol far riferimento ad entrambi verrà utilizzato il pedice generico i.

3.1 formulazione del gioco

Il tipo di gioco che si vuole impostare è un gioco a tempo finito ossia l’istante in cui il gioco termina è noto e pari a tf, quindi l’intera sua evoluzione è compresa

nell’intervallo [t0, tf].

Si definisce Ui con i ∈ {p, e} l’insieme dei controlli utilizzabili, tuttavia per

questo Gioco non si vogliono porre particolari vincoli e quindi Ui≡ Rmi ∀ i.

Il Pursuer e l’Evader sono descritti attraverso una dinamica lineare tempo inva-riante.

˙xi(t) = Fixi(t) + Giui(t), xi(t0) = xi,0, i∈ {p, e} (19)

Le matrici Fi ∈ R2n×2n sono le matrici dinamiche dei sistemi mentre Gi ∈ R2n×mi quelle di ingresso.

Le funzioni ui: [t0, tf] → Rmi sono gli ingressi e indicano i controlli sulle

accelerazioni dei sistemi.

Le traiettorie dello stato sono rappresentate da xi: [t0, tf]→ R2n.

Il vettore di stato xi∈ R2n è composto da un vettore posizione ri(t)∈ Rn e da

uno velocità vi(t)∈ Rne viene riportato nella (20).

xi(t) =

�

rT_i(t), vT_i(t)�T _{∈ R}2n, i∈ {p, e} (20) Le equazioni dei due sistemi dinamici (19) possono essere scritte insieme come un unico sistema:

3.1 formulazione del gioco con x(t) = [xT

p(t), xTe(t)]T, u(t) = [uTp(t), uTe(t)]T e le matrici F e G definite come

nella (21). F=   Fp 02n×2n 0_2n×2n Fe  , G =   Gp 02n×me 0_2n×mp Ge   (21)

A questo punto si costruisce un gioco non a somma zero in cui il Pursuer e l’Evader hanno due funzionali ciascuno da minimizzare riportati nella (22).

J_p(up,ue) = 1 2�xp(tf) − xe(tf)� 2 Qf + �tf t0 � −kp,pp(rp(t), re(t)) + 1 2�up� 2 Rp � dt (22) J_e(up,ue) = − 1 2�xp(tf) − xe(tf)� 2 Qf + �tf t0 � kp,ep(rp(t), re(t)) + 1 2�ue� 2 Re � dt Nei funzionali la Qf ∈ R2n×2n è una matrice semi-definita positiva, Ri ∈ Rmi×mi sono definite positive mentre k

p,i� 0.

Guardando questi funzionali si capisce che il Pursuer vuole cercare di minimiz-zare la distanza dall’Evader al tempo tf, massimizzare la funzione di visibilità e

minimizzare l’energia del controllo.

L’Evader invece vuole massimizzare la distanza dal Pursuer al tempo finale, minimizzare la visibilità e l’energia del controllo.

Si vogliono trovare due strategie di controllo in feedback (u∗

p,u∗e) che siano un

equilibrio di Nash come espresso nella (23) e nella (24).

J_p(u∗_p,u∗_e)� Jp(up,u∗e), ∀up∈ Rmp (23)

J_e(u∗_p,u∗_e)� Je(u∗p,ue), ∀ue ∈ Rme (24)

Il Pursuer cercherà di evitare gli ostacoli per massimizzare la visibilità mentre l’Evader per minimizzarla cercherà di nascondersi.

Tuttavia bisogna dire che il funzionale Je(up,ue), per come è definito, può

portare l’Evader ad entrare all’interno di un ostacolo poiché i vettori r ∈ O hanno visibilità nulla.

3.2 rilassamento dei vincoli Per evitare che ciò accada vengono introdotti i vincoli (25) grazie ai quali i vettori posizione dei due sistemi devono restare in Cfree.

Questo vuol dire che le traiettorie sono vincolate a restare all’interno dello spazio libero.

r∗_p(t)_{∈ Cfree ∀t ∈ [t}0, tf]

r∗_e(t)_{∈ Cfree ∀t ∈ [t}0, tf]

(25)

3.2 rilassamento dei vincoli

Risolvere il problema presentato nella sezione 3.1 con i vincoli (25) è possibile solo numericamente.

Per poter trovare una soluzione analitica al problema è necessario impostare i vincoli (25) come soft-constraints modificando i funzionali di costo in questo modo: J_p(up,ue) = 1 2�xp(tf) − xe(tf)� 2 Qf + �tf t0 � hp(rp(t), re(t)) + 1 2�up(t)� 2 Rp � dt (26) J_e(up,ue) = −1 2�xp(tf) − xe(tf)� 2 Qf + �tf t0 � he(rp(t), re(t)) + 1 2�ue(t)� 2 Re � dt (27) In (26) e (27) sono state introdotte delle funzioni hi(rp,re)definite come:

hp(rp,re)� −kp,pp(rp,re) − km,pm(rp)

he(rp,re)� kp,ep(rp,re) − km,em(re)

con km,i� 0.

Introducendo la funzione di appartenenza all’insieme degli ostacoli nei funzio-nali di costo e richiedendo la sua massimizzazione, il Pursuer e l’Evader vengono forzati a mantenere le loro traiettorie in Cfree.

3.3 risoluzione del gioco 3.3 risoluzione del gioco

Per risolvere il il gioco si utilizza il teorema in [1] che qui viene riportato adattato per le diverse notazioni utilizzate in questa tesi.

Teorema: Condizione Necessarie per un equilibrio di Nash.

Dato un gioco differenziale tempo continuo e definito nell’intervallo [t0, tf]con

Ngiocatori descritti da equazioni di stato

˙x = f(t, x(t), u1(t), u2(t), . . . , uN(t)), x(t0) = x0

e dati N funzionali di costo

J_i(u1(t), . . . , uN(t)) = qi(x(tf)) + �tf t0 gi(t, x(t), u1(t), . . . , uN(t))dt, i ∈ {1, . . . , N} Siano: • f continua e differenziabile su Rn_{× U} 1× · · · × UN ∀t ∈ [t0, tf], • gicontinua e differenziabile su Rn× U1× · · · × UN ∀t ∈ [t0, tf], i ∈ N+0. Allora, se u∗

i(t), i ∈ {1, . . . , N} è continua e differenziabile e rappresenta un

equilibrio di Nash e x∗_{(t), t ∈ [t}

0, tf]è la traiettoria delle variabili di stato

corri-spondente, esistono N funzioni λi: [t0, tf]→ Rn, i ∈ {1, . . . , N} tale per cui sono

soddisfatte le seguenti relazioni:

˙x∗_{(t) = f(t, x}∗_{(t), u}∗ 1(t), . . . , u∗N(t)), x∗(t0) = x0 u∗_i(t) =arg min ui∈Ui H_i(t, λi(t), x∗(t), u∗1(t), . . . , u∗_i−(t), ui(t), u∗i+1(t), . . . , u∗N(t)) (28) ˙λT i(t) = − ∂ ∂xHi(t, λi(t), x ∗_{(t), u}∗ 1(t), . . . , u∗N(t))

3.3 risoluzione del gioco λT_i(tf) = ∂ ∂xqi(x ∗_(t f)), i∈ {1, . . . , N} dove H_i(t, λi(t), x(t), u1(t), . . . , uN(t))� gi(t, x(t), u1(t), . . . , uN(t))+ λT_i(t)f(t, x(t), u₁(t), u₂(t), . . . , u_N(t)) Per prima cosa in (29) si definiscono gli Hamiltoniani sia per il Pursuer che per l’Evader (i ∈ {p, e}). H_i(λi,x, up,ue) = hi(rp,re) + 1 2�ui� 2 Ri+ λ T i [Fx + Gu] (29)

Vengono introdotti i moltiplicatori di Lagrange λi: [t0, tf]→ R4n.

Si ricorda che per questo problema Ui≡ Rmi e quindi, non essendoci vincoli sui

controlli, per risolvere la (28) basta calcolare la derivata dell’Hamiltoniano i-esimo rispetto all’ingresso ui.

Le equazioni da risolvere che rispondono alle condizioni necessarie per avere delle strategie ottime sono scritte in maniera più semplice nel seguente sistema:

             ˙x = Fx(t) + Gu(t), x(t0) = x0 � ∂ ∂uiHi(t, λi(t), x(t), up(t), ue(t)) �T = 0N ˙λi(t) = � ∂ ∂xiHi(t, λi(t), x(t), up(t), ue(t)) �T , λi(tf) = � _∂ ∂xqi(x(tf)) �T con i ∈ {p, e}.

Dalla (21), gli Hamiltoniani (29) possono essere riscritti come:

H_i(λi,x, up,ue) = hi(rp(t), re(t)) + 1 2�ui� 2 Ri + λT_i,p[Fpxp+ Gpup] + λTi,e[Fexe+ Geue] , i∈ {p, e}

I moltiplicatori di Lagrange di entrambi i giocatori possono essere suddivisi in due sotto-vettori, ciascuno di dimensione 2n.

λp(t) =  λp,p(t) λp,e(t)   , λe(t) =  λe,p(t) λe,e(t)   , λi,j(t)∈ R2n

3.3 risoluzione del gioco qp(x(tf))� 1 2�xp(tf) − xe(tf)� 2 Qf qe(x(tf))� −qp(x(tf))

Grazie a questi termini è possibile ricavarsi le condizioni finali sulle variabili aggiunte. λp(tf) = � ∂qp(x(tf)) ∂x(tf) �T =   Qf(xp(tf) − xe(tf)) −Q_f(xp(tf) − xe(tf))   (30) λe(tf) = � ∂qe(x(tf)) ∂x(t_f) �T =  −Qf(xp(tf) − xe(tf)) Qf(xp(tf) − xe(tf))   (31)

Dalle condizioni necessarie le strategie ottime risultano:

u∗_p=arg min

up∈Rmp

H_p, u∗_e =arg min

ue∈Rme

H_e (32)

Poiché non ci sono vincoli sui controlli le (32) diventano: � ∂Hp ∂up �T = 0mp, � ∂He ∂ue �T = 0me (33)

Queste equazioni rappresentano le condizioni necessarie sulla variazione prima ma bisogna verificare anche quelle sulla variazione seconda.

� ∂2H_p ∂u2 p �T � 0, � ∂2H_e ∂u2 e �T � 0

Questo vuol dire che le matrici Hessiane relative alle derivate seconde degli Ha-miltoniani rispetto agli ingressi upe ue devono essere semi-definite positive.

Dalla (33) si ottengono le strategie di guida in funzione dei moltiplicatori di Lagrange.

up(t) = −R−1p GTpλp,p(t) (34)

ue(t) = −R−1e GTeλe,e(t) (35)

É facile a questo punto vedere che le condizioni necessarie del secondo ordine sono rispettate essendo le matrici Rpe Re definite positive.

3.3 risoluzione del gioco � ∂2H_p ∂u2 p �T = Rp� 0, � ∂2_H e ∂u2 e �T = Re � 0

Ora bisogna calcolare l’evoluzione dei moltiplicatori di Lagrange risolvendo il sistema di equazioni differenziali generato dalle seguenti formule:

˙λp(t) = − � ∂Hp ∂x �T , ˙λe(t) = − � ∂He ∂x �T (36) Dalla (36) si può scrivere il seguente sistema:

 ˙λp,p ˙λp,e   =   −FTp 02n×2n 02n×2n −FTe    λp,p λp,e   −    �_∂h p ∂xp �T �_∂h p ∂xe �T    (37)  ˙λe,p ˙λe,e   =   −FTp 02n×2n 0_2n×2n −FT_e    λe,p λe,e   −    � ∂he ∂xp �T � ∂he ∂xe �T    (38)

Dagli ingressi trovati in (34) e (35) si scopre di essere interessati solo ad alcune delle componenti delle variabili aggiunte e cioè ai vettori λp,pe λe,e.

Quindi si è interessati alle prime 2n equazioni differenziali della (37) ed alle ultime 2n della (38).

Per semplificare la notazione si definisce:

li(rp,re)� � ∂hi(rp,re) ∂xi �T = � ∂hi(rp,re) ∂ri ,0_1×n �T

Il sistema di equazioni da risolvere, utilizzando la (34) e la (35), è il seguente:                    ˙λp,p(t) = −FTpλp,p(t) − lp(rp,re), λp,p(tf) = Qf(xp(tf) − xe(tf))

˙λe,e(t) = −FTeλe,e(t) − le(rp,re), λe,e(tf) = Qf(xp(tf) − xe(tf))

˙xp(t) = Fpx(t) − GpR−1p GpTλp,p(t), xp(t0) = xp0 ˙xe(t) = Fex(t) − GeR−1e GeTλe,e(t), xp(t0) = xe0

(39)

Questo è un sistema di equazioni differenziali di dimensione 8n non linea-re, omogeneo a due punti, visto che le condizioni al contorno sono date da 4n equazioni al tempo tf e 4n equazioni al tempo t0.

3.3 risoluzione del gioco Trovare la soluzione ad un sistema del genere è possibile solo numericamente, inoltre la soluzione deve essere per forza calcolata “offline” e quindi sostanzial-mente non si ottiene nessuna legge di guida in feedback che possa essere utilizzata real-time.

Per poter andare avanti nei calcoli e trovare una soluzione da utilizzare in feed-back, si considera la parte non lineare, data dalla funzione li(rp,re), come un

ingresso.

Così facendo il sistema (39) diventa lineare non omogeneo di cui si può calcolare la soluzione in forma analitica.

Si passa ora a scrivere la soluzione delle (37) e (38) in funzione di λp,p(tf) e

λe,e(tf).

λi,i(t) = ΦTi(tf− t)λi,i(tf) +

�tf

t

ΦT_i(τ − t)li(τ)dτ, i ∈ {p, e} (40)

Sempre per semplificare la notazione si definisce Φi(t)� eFit e la funzione

fi(t, tf)�

�tf

t

ΦT_i(τ − t)li(τ)dτ, i ∈ {p, e}

Si sostituiscono le condizioni al contorno (30) e (31) nella (40) ottenendo:

λi,i(t) = ΦTi(tf− t)Qf(xp(tf) − xe(tf)) + fi(t, tf), i∈ {p, e} (41)

A questo punto resta da trovare gli stati dei due giocatori al tempo tf e quindi si

utilizzano le equazioni della dinamica dei due sistemi (19) per scrivere xi(tf) in

funzione di xi(t)integrando all’indietro.

xi(tf) = Φi(tf− t)xi(t) +

�tf

t

Φi(tf− τ)Giui(τ)dτ (42)

Sostituendo la (34), (35), e (41) nella (42), si arriva a scrivere la differenza tra gli stati finali dei due sistemi come:

xp(tf) − xe(tf) = � I+ (Mp(t, tf) − Me(t, tf))Qf �−1 � Φp(tf− t)xp(t) − Φe(tf− t)xe(t) − Lp(t, tf) + Le(t, tf) � (43) dove i ∈ {p, e}.

3.3 risoluzione del gioco Mi(t, tf)� �tf t Φi(tf− τ)GiR−1i GTiΦTi(tf− τ)dτ Li(t, tf)� �tf t Φi(tf− τ)GiRi−1GTifi(τ, tf)dτ

Infine sostituendo (43) nella (41) e inserendo il risultato nella (34) e nella (35) si ottengono le strategie ottime per i due giocatori.

u∗_p(t) = − R−1_p GT_pΦT_p(tf− t)Qf � I+�Mp(t, tf) − Me(t, tf) � Qf �−1� Φp(tf− t)xp(t)+ − Φe(tf− t)xe(t) − Lp(t, tf) + Le(t, tf) � − R−1_p GT_pfp(t, tf) (44) u∗_e(t) = − R−1_e GT_eΦT_e(tf− t)Qf � I+�Mp(t, tf) − Me(t, tf) � Qf �−1� Φp(tf− t)xp(t)+ − Φe(tf− t)xe(t) − Lp(t, tf) + Le(t, tf) � − R−1_e GT_efe(t, tf) (45)

Le strategie (44) e (45) hanno al loro interno l’inversione di una matrice e quindi bisogna definire delle condizioni di esistenza di tale inversa.

det (I + (Mp(t, tf) − Me(t, tf)) Qf)�= 0 (46)

Questo determinante è sicuramente diverso da zero se:

Mp(t, tf) − Me(t, tf)� 0 (47)

La matrice Mi(t, tf)può essere vista come un Gramiano di controllabilità pesato

attraverso la matrice R−1 i .

La condizione (47) indica che il Pursuer deve essere più controllabile dell’Evader. Se i sistemi hanno la stessa dinamica questa condizione implica che chi insegue deve avere più energia da spendere rispetto a chi scappa.

Si vede che nelle strategie di ciascun giocatore compaiono i gradienti delle hi

del giocatore avversario e questo è dovuto al fatto che entrambi hanno tutte le informazioni del gioco.

Per esempio il Pursuer sa che l’Evader tenterà di nascondersi e quindi agirà di conseguenza, un discorso analogo può essere fatto per l’Evader.

3.4 intercettazione nel caso di doppi integratori 3.4 intercettazione nel caso di doppi integratori

In questa sezione verranno applicate la (44) e la (45) nel caso in cui i sistemi dinamici sono dei doppi integratori.

Le loro matrici dinamiche e di ingresso sono:

Fp= Fe =  0n×n In×n 0_n×n 0_n×n   , G_p= Ge =  0n×n I_n×n   (48)

In questo caso i controlli, rappresentando le accelerazioni lungo ciascun asse, risultano up,ue ∈ Rn.

La matrice di transizione invece è data da:

Φp(t) = Φe(t) =  In×n tIn×n 0n×n In×n   (49)

Per quanto riguarda il costo terminale dei funzionali visti nella sezione 3.1, in questo caso si vuole minimizzare/massimizzare solo la distanza relativa dei due giocatori al tempo finale e non la velocità.

Per questo motivo la matrice Qf viene scelta semi-definita positiva:

Qf = qf  In×n 0n×n 0n×n 0n×n   , qf > 0

La (50) mostra le matrici peso sui controlli.

Rp= c−1p In×n, Re = c−1e In×n, cp, ce > 0 (50)

Sostituendo la (48), (49) e (50), nelle funzioni Mi(t, tf), fi(t, tf) e Li(t, tf)sia per

il pursuer che per l’evader si ottengono i seguenti risultati:

Mi(t, tf) = ci  (tf−t) 3 3 In×n (tf−t)2 2 In×n (tf−t)2 2 In×n (tf− t)In×n   fi(t, tf) = �tf t   [∂hi(rp,re)/∂ri]T (τ − t) [∂hi(rp,re)/∂ri]T   dτ (51)

3.4 intercettazione nel caso di doppi integratori Li(t, tf) =ci   (tf−t)2 2 �tf t (τ − t) [∂hi(rp,re)/∂ri] T_dτ (tf− t) �tf t (τ − t) [∂hi(rp,re)/∂ri] T_dτ   − ci  (tf−t) 3 6 �tf t [∂hi(rp,re)/∂ri] T_dτ (tf−t)2 2 �tf t [∂hi(rp,re)/∂ri]Tdτ   + ci   �tf t (tf−τ)3 6 [∂hi(rp,re)/∂ri] T_dτ �tf t (tf−τ)2 2 [∂hi(rp,re)/∂ri] T_dτ   (52)

A questo punto però sorge un problema: il calcolo di (51) e di (52) richiede la conoscenza dell’evoluzione del gradiente nel futuro.

Poiché non si possono conoscere le traiettorie e quindi i gradienti dei due gioca-tori nel futuro è necessario attuare la seguente approssimazione: ad ogni istante t si considera il gradiente di hi(rp,re)costante per tutto l’intervallo [t, tf].

Facendo questa approssimazione è possibile portare il gradiente fuori dagli integrali e risolvere la (51) e la (52). fi(t, tf) =  (tf− t) [∂hi(rp,re)/∂ri]T (tf−t)2 2 [∂hi(rp,re)/∂ri] T   Li(t, tf) = ci  (tf−t) 4 8 [∂hi(rp,re)/∂ri] T (tf−t)3 6 [∂hi(rp,re)/∂ri] T  

A questo punto si hanno tutti gli elementi necessari da inserire nella (44) e nel-la (45) per ottenere i controlli di guida per i due doppi integratori.

up(t) =uPNG(t) + cpt5go 8 � 1 qf + t3_go 3 (cp− ce) � � cp � ∂hp(rp,re) ∂rp �T − ce � ∂he(rp,re) ∂re �T� −cpt 2 go 2 � ∂hp(rp,re) ∂rp �T ue(t) = ce cp uPNG(t) + cet5go 8 � 1 qf+ t3_go 3 (cp− ce) � · � cp � ∂hp(rp,re) ∂rp �T − ce � ∂he(rp,re) ∂re �T� −cet 2 go 2 � ∂he(rp,re) ∂re �T

3.4 intercettazione nel caso di doppi integratori Il termine uPNG(t)ha l’espressione: uPNG(t) = − cptgo 1 qf+ t3_go 3 (cp− ce) · [rp(t) − re(t) + (vp(t) − ve(t))tgo]

mentre con tgo� tf− t si indica il time-to-go.

La condizione di esistenza dell’inversa vista in (46) in questo caso diventa: 1 qf +t 3 go 3 (cp− ce)�= 0 (53)

Scegliendo cp � ce, il Pursuer ha più energia dell’Evader e la (53) è verificata

essendo qf> 0e la sua inversa esisterà sempre.

Se invece cp < ce per tf è sufficientemente grande esisterà un istante in cui la

condizione (53) non è verificata e quindi le strategie diventano indefinite.

Questo accade perché, essendo (cp− ce) < 0, esisterà, per tf sufficientemente

grandi, un ˜t < tf per cui (tf−˜t)

3

3 (cp− ce) = −q1f rendendo non vera e quindi non invertibile la (53) per quel preciso istante ˜t.

Poiché si vuole raggiungere in ogni caso l’intercettazione al tempo tf, si sceglie

qf infinitamente grande (qf → ∞) ottenendo così le seguenti strategie:

up(t) = − 3cp (cp− ce)t2go � rp(t) − re(t) + (vp(t) − ve(t))tgo � + 3cpt 2 go 8(cp− ce) � cp � ∂hp(rp,re) ∂rp �T − ce � ∂he(rp,re) ∂re �T� −cpt 2 go 2 � ∂hp(rp,re) ∂rp �T (54) ue(t) = − 3ce (cp− ce)t2go � rp(t) − re(t) + (vp(t) − ve(t))tgo � + 3cet 2 go 8(cp− ce) � cp � ∂hp(rp,re) ∂rp �T − ce � ∂he(rp,re) ∂re �T� −cet 2 go 2 � ∂he(rp,re) ∂re �T (55)

4

S I M U L A Z I O N I

In questo capitolo verranno svolte delle simulazioni per testare le leggi di control-lo (54) e (55) trovate alla fine del precedente capitocontrol-lo.

Vengono riportati in apposite tabelle tutti i parametri utilizzati per le simula-zioni. Il significato di questi parametri è stato ampiamente descritto in questa tesi eccetto uno che riguarda il calcolo del gradiente delle funzioni hi(rp,re) per

i∈ {p, e}.

Infatti, poiché le simulazioni vengono effettuate per via numerica, i gradienti delle funzioni sono calcolati in maniera discreta. Il rapporto incrementale delle componenti del gradiente non viene quindi calcolato facendo tendere il termine incrementale a zero ma fissandolo ad un valore piccolo che verrà indicato con la lettera greca δ.

In queste simulazioni si ritrova tutto ciò che è stato spiegato finora, dalla mo-dellazione degli ostacoli negli ambienti, ai parametri scelti per la discretizzazione delle varie funzioni, alle leggi di guida in feedback trovate risolvendo il gioco differenziale impostato nel Capitolo 3.

4.1 simulazione 1: guadagno di visibilità del pursuer

In questa simulazione viene considerato un ambiente con un solo ostacolo e si mostra il comportamento del Pursuer al variare del suo guadagno di visibilità kp,p.

Si vuole mostrare come variano le traiettorie scegliendo parametri diversi di visibilità in un ambiente molto semplice.

I parametri di simulazione sono riportati in Tabella 1.

I guadagni di visibilità del Pursuer sono stati scelti appositamente più grandi di quelli dell’Evader proprio per poter analizzare meglio l’influenza di kp,p sulle

4.1 simulazione 1: guadagno di visibilità del pursuer traiettorie del Pursuer.

La Figura 22 mostra le traiettorie seguite dal Pursuer e dall’Evader. Come prima cosa si nota che l’ostacolo viene evitato con tutti i guadagni di visibilità.

Inoltre maggiore è il guadagno e più le traiettorie si tengono lontane dall’osta-colo.

In Figura 23 si vede anche l’evoluzione della visibilità al variare del guadagno kp,p, infatti più grande è questo parametro e più velocemente il Pursuer cerca di

massimizzarlo.

parametro valore

Tempo finale tf= 2

Energia Pursuer cp= 12

Energia Evader ce = 0.5

Guadagno di visibilità Pursuer kp,p∈ {40, 60, 80, 100, 120}

Guadagno di visibilità Evader kp,e= 5

Guadagno per la Collision

Avoidance Pursuer km,p = 80

Guadagno per la Collision

Avoidance Evader km,e= 80

Stato iniziale Pursuer xp(t0) = [4 6 0 0]T

Stato iniziale Evader xe(t0) = [10 16 0 0]T

Varianza della funzione fe(r, re) Σ=diag([2 2])

Dominio di Integrazione B B=square(10)

Passo di Integrazione s= 0.5

Numero di punti per segmento Ns = 25

Lunghezza passo per calcolo gradiente δ= 0.1 Tabella 1: Parametri della simulazione 1.

4.1 simulazione 1: guadagno di visibilità del pursuer -5 0 5 10 15 20

r

r

Figura 22: Traiettorie del Pursuer e dell’Evader al variare del guadagno di visibilità del Pursuer. 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Time t 0.5 0.6 0.7 0.8 0.9 1 V is ib il it y fu n ct io n p( rp (t ), re (t )) kp,p= 40 kp,p= 60 kp,p= 80 kp,p= 100 kp,p= 120