ITCS20110024A1 - Dispositivo e metodo per la stima dello stato di un veicolo in navigazione - Google Patents

Description

Titolo Dell’Invenzione

DISPOSITIVO E METODO PER LA STIMA DELLO STATO DI UN VEICOLO IN NAVIGAZIONE.

Campo Tecnico Associato All’invenzione

L’invenzione riguarda un dispositivo e un metodo per il calcolo delle stime dello stato di un veicolo in navigazione che sorvola un certo terreno.

L’applicazione principale, ma non esclusiva, considerata qui per il detto dispositivo e metodo è a bordo di veicoli “lander”, ovvero veicoli utilizzati per atterrare su superfici di corpi celesti, come Marte e la Luna, per scopi di esplorazione dello spazio. I veicoli considerati devono realizzare una manovra di discesa frenata che porta all’atterraggio. Questa manovra richiede inoltre, in particolare, di identificare al momento il punto di atterraggio, a priori non definito molto accuratamente, che non presenti ostacoli o pericoli per il veicolo stesso come per es. rocce o grossi massi o pendenze eccessive.

Per realizzare una manovra di questo tipo, questi veicoli devono essere in grado di stimare il proprio stato autonomamente e in tempo reale.

Lo stato del veicolo comprende le seguenti componenti: posizione, velocità, assetto e velocità angolare.

I Dispositivi di stima dello stato allo stato dell’arte, in questo contesto, sono tipicamente composti dai seguenti componenti:

• Una fotocamera che produce in uscita una sequenza di immagini prese in momenti successivi, che mostrano il terreno sorvolato

• Una unità di misura inerziale (Inertial Measurement Unit, IMU), che produce le misure di velocità angolare e di accelerazione lineare, in un sistema di riferimento solidale con il velivolo • Un dispositivo di elaborazione immagini (Image Processing) che effettua ripetutamente le seguente operazioni:

- riceve in ingresso le immagini acquisite

- identifica sulle immagini alcuni punti che hanno la caratteristica di essere facilmente “tracciabili” nella sequenza di immagini

- effettua l’operazione di “tracciamento” di detti punti – in altre parole, ne determina l’apparente movimento

- fornisce in uscita una serie di “tracce”, che descrivono questo apparente movimento, in termini di sequenze di coordinate riga e colonna della matrice di pixel della camera (la definizione rigorosa dei dati in uscita di questo dispositivo viene fornita nella sezione 1.1.). • Un dispositivo detto “Filtro di Navigazione”, che produce le stime dello stato in funzione delle misure della IMU e dei dati in uscita del dispositivo di elaborazione immagini

Lo schema rappresentato in Figura 1 riassume quanto detto sopra.

I Dispositivi di Navigazione allo stato dell’arte tipicamente implementano, all’interno del Filtro di Navigazione, algoritmi di filtraggio ricorsivi, basati sull’esecuzione ripetuta di due passi di base; questi passi sono, essenzialmente, la propagazione dello stato stimato, e l’aggiornamento dello stato stimato in funzione degli ultimi dati in ingresso ricevuti. Tali algoritmi tipicamente presentano vari svantaggi (per es. la possibilità di divergenza dell’errore di stima, e un alto costo computazionale).

L’invenzione riguarda un Dispositivo di Navigazione innovativo, che ha lo stesso schema a blocchi (ad alto livello) dei dispositivi di questo tipo allo stato dell’arte, ma implementa -all’interno del dispositivo “Filtro di Navigazione” - un metodo di calcolo diverso, e più performante.

In particolare, al posto di aggiornare iterativamente le stime dello stato, viene utilizzata un’espressione in forma chiusa, non ricorsiva, per calcolare esplicitamente le stime dello stato in modo efficiente, robusto ed accurato, utilizzando i dati in ingresso forniti dalla IMU e dal dispositivo di elaborazione delle immagini. Questo dispositivo non ha gli svantaggi presenti invece nei dispositivi allo stato dell’arte, e inoltre consente un significativo aumento delle prestazioni - in particolare, si ha un aumento significativo dell’accuratezza di stima, robustezza e un’importante riduzione del costo computazionale.

“Prior art” rilevante

I Filtri di Navigazione utilizzati all’interno dei Dispositivi di Navigazione allo stato dell’arte (nel contesto considerato) sono generalmente basati su un approccio che segue il concetto del Filtro di Kalman o di una delle sue varianti (come per es. il Filtro di Kalman Esteso). Il punto di partenza per definire gli algoritmi implementati su questi dispositivi sono tipicamente le seguenti due equazioni:

• L’equazione di osservazione (o di misura), utilizzata per calcolare i valori misurati partendo dallo stato attuale del sistema (veicolo),

• L’equazione di transizione (o propagazione) dello stato, utilizzata per calcolare lo stato successivo del veicolo partendo dallo stato attuale e dagli input al sistema.

E’ importante tenere conto del fatto che, assieme allo stato stimato, gli algoritmi spesso calcolano anche l’incertezza associata allo stato stimato, tipicamente espressa tramite la matrice di covarianza dell’errore di stima.

Il nucleo del processo di stima tipicamente consiste nell’esecuzione ripetuta di un ciclo con i seguenti due passi di base:

• Predizione: qui lo stato stimato (e l’errore associato) vengono propagati utilizzando l’equazione di transizione

• Aggiornamento: qui la stima dello stato ottenuta nel passo precedente viene aggiornata tenendo conto delle ultime misure; questo viene tipicamente fatto sommando, alla stima dello stato, una “correzione” che si ottiene moltiplicando l’”innovazione” (ovvero la differenza tra le misure effettive, e quelle che si sarebbero ottenute se lo stato stimato fosse stato quello “vero”) con un’opportuna matrice di “guadagno”. Un modo per ottenere questa matrice può essere la minimizzazione del valore atteso dell’errore quadratico della stima.

A livello intuitivo, è possibile dire che i filtri utilizzati nei dispositivi allo stato dell’arte utilizzano un modo “indiretto” per ottenere lo stato del veicolo partendo dalle misure: in sostanza, invece di utilizzare un’espressione in forma chiusa che ricava direttamente lo stato del veicolo in funzione dell’insieme delle misure, gli algoritmi utilizzati migliorano iterativamente la stima dello stato, in funzione delle ultime misure ricevute.

D’altra parte, il Dispositivo (di Navigazione) che è oggetto di questa invenzione utilizza, all’interno del Filtro di Navigazione, un’espressione in forma chiusa per ricavare direttamente lo stato del veicolo.

DOCUMENTI SPECIFICI RIGUARDANTI LA “PRIOR ART”

• [D1] X. Sembély et al, “Mars Sample Return: Optimizing the Descent and Soft Landing for an Autonomous Martian Lander”

• [D2] “Navigation for Planetary Approach and Landing Final Report”, European Space Agency, Contract Reference: 15618/01/NL/FM, May 2006.

• [D3] WO 2008/024772 A1 (Univ Florida [US]; Kaiser Michael Kent [US], Gans Nicholas Raphael [US]) 28 February 2008 (2008-02-28)

• [D4] Li et al: "Vision-aided inertial navigation for pinpoint planetary landing", Aerospace Science and Technology, Elsevier Masson, FR, vol 11. no.6, 19 July 2007 (2007-07-19), pages 499-506, XP022158291, ISSN: 1270-9638, DOI: DOI: 10.1016/J.AST.2007.04.006 VALUTAZIONE DELLA “PRIOR ART”

Tipicamente, alcuni dei svantaggi dell’utilizzo dei dispositivi allo stato dell’arte (come per es. i dispositivi descritti in [D1], [D2], [D3] e [D4]) sono i seguenti:

• Notevole importanza delle stime iniziali / preliminari (in alcuni casi, è fondamentale inviare all’inizio questi dati al filtro per avere un funzionamento corretto);

• Rischio di divergenza / instabilità del filtro, oppure di un errore di stima elevato;

• Potenziale necessità di una lunga campagna di verifica per valutare l’effetto degli errori dei dati in ingresso sugli errori dei dati in uscita;

• Limitata capacità di stima della direzione verticale locale, ovvero del vettore gravità (nel sistema di riferimento utilizzato per esprimere le stime dello stato);

• Potenziale necessità di un lungo processo di calibrazione dei parametri del filtro;

• Potenziale necessità di una IMU con un’elevata accuratezza;

• Elevato costo computazionale per l’esecuzione in tempo reale (in altre parole, l’hardware di calcolo del Filtro di Navigazione deve essere in grado di effettuare un numero elevato di operazioni al secondo).

PROBLEMA TECNICO DA RISOLVERE, EFFETTI VANTAGGIOSI DELL’INVENZIONE

L’invenzione considerata ha come oggetto un dispositivo innovativo per ottenere le stime dello stato del veicolo in tempo reale.

Questo dispositivo non ha gli svantaggi presenti nei dispositivi allo stato dell’arte (elencati sopra). In particolare:

• Non è necessario avere valori di inizializzazione;

• Il Filtro di Navigazione non può divergere;

• E’ molto semplice stimare gli errori sui dati in uscita in funzione degli errori sui dati in ingresso; • La direzione verticale locale può essere stimata facilmente (insieme alle stime dello stato); • Non ci sono parametri del filtro significativi oppure altri valori da calibrare tramite un lungo processo;

• L’algoritmo del Filtro di Navigazione è robusto rispetto agli errori nelle misure della IMU;

• Il costo computazionale dell’algoritmo implementato nel filtro è ridotto - la riduzione prevista rispetto alle soluzioni allo stato del’arte è tipicamente di almeno 10 volte.

Inoltre c’è anche un incremento significativo dell’accuratezza della stima dello stato, tipicamente di almeno 10 volte.

Utilizzando all’interno del dispositivo questo innovativo Filtro di Navigazione, sarà possibile ottenere un notevole incremento nelle prestazioni generali della funzione di stima dello stato, rispetto allo stato dell’arte attuale. Questo incremento di prestazioni avrà ripercussioni positive anche su altre funzioni e sistemi del lander. Questo potrebbe portare anche a riduzioni di costo e di budget ingegneristici a livello di sistema - per es., riduzione dei carichi computazionali, incremento dell’affidabilità della missione a causa di un incremento della robustezza, e riduzione della massa del propellente (per es. a causa dell’incremento dell’accuratezza della stima dello stato, con conseguente riduzione del bisogno di fare manovre correttive).

DESCRIZIONE DI UN MODO PER REALIZZARE L’INVENZIONE

ASPETTI INNOVATIVI PRINCIPALI DELL’INVENZIONE

L’aspetto innovativo chiave dell’invenzione è il metodo di calcolo che viene utilizzato all’interno del Filtro di Navigazione per calcolare le stime dello stato.

E’ possibile dire che, a differenza dei sistemi allo stato dell’arte, dove un metodo di stima generale (per es. il Filtro di Kalman) viene adattato per il particolare caso considerato, qui il metodo di stima è stato costruito “partendo da zero”, altamente ottimizzato per il problema considerato; il metodo di stima utilizza solamente espressioni “in forma chiusa”, non-iterative, efficienti e robuste.

Intuitivamente, paragonato il problema di stima dello stato considerato qui con il problema della ricerca dei zeri di un polinomio cubico, l’approccio allo stato dell’arte corrisponde all’utilizzo di un metodo iterativo numerico, mentre l’approccio considerato con il metodo proposto corrisponde all’utilizzo di una soluzione “analitica” esplicita (per es. il metodo di Cardano).

DESCRIZIONE DETTAGLIATA DELL’INVENZIONE

Questa sezione contiene tutte le informazioni necessarie per realizzare l’invenzione.

La sezione è strutturata nel modo seguente:

• Concetti e assunzioni di base: qui vengono fornite le definizioni dei concetti principali e le assunzioni utilizzati nella descrizione;

• Descrizione della funzione del Filtro: qui la funzione del Filtro di Navigazione viene definita in modo rigoroso, in termini di ingressi (input) ricevuti e uscite (output) prodotti;

• Definizione del sistema di riferimento per le stime dello stato: qui viene fornita la definizione del sistema di riferimento nel quale le stime dello stato (prodotte dal Filtro) vengono espresse; • Descrizione dell’algoritmo: qui l’algoritmo implementato nel Filtro (QuickNav) viene descritto, in termini delle operazioni che realizza; questa parte è suddivisa in:

- Passo di pre-elaborazione: qui vengono descritte alcune elaborazioni iniziali;

- Passo centrale: qui viene descritto il modo nel quale le operazioni chiave vengono realizzate;

- Passo di post-elaborazione: qui viene descritto il modo nel quale vengono calcolati i dati finali in uscita;

• Informazioni aggiuntive: qui vengono fornite alcune ulteriori informazioni riguardanti l’algoritmo.

1. CONCETTI DI BASE E ASSUNZIONI

Per semplificare la notazione e le descrizioni, un asse dei tempi “continuo” (invece che “campionato”) verrà considerato. L’intervallo temporale per il quale il moto del veicolo (e i dati in ingresso e uscita) viene considerato viene indicato con I:

I = [t0,t1] (1)

Durante questo intervallo temporale, il veicolo realizzerà una certa traiettoria in una regione dello spazio, sopra una parte del terreno.

Si assumerà che in questa regione dello spazio, il vettore di accelerazione di gravità sia costante.

La sua direzione definisce il concetto di “verticale”.

Qui un generico sistema di riferimento (Reference Frame, RF) verrà chiamato “Fisso” se è fisso rispetto al terreno, durante l’intervallo temporale I.

Inoltre, si dirà che un RF possiede un “Asse Z Verticale” se il suo asse z è parallelo al vettore gravità, ma ha la direzione opposta (e quindi la gravità ha componenti x e y nulle, e una componente z negativa).

Qui si assume anche l’esistenza di un sistema di riferimento denominato “Body Reference Frame” (BRF), fisso rispetto al veicolo; anche la IMU e la fotocamera sono fissi rispetto al veicolo.

Un vettore espresso nel BRF verrà indicato con un apice “B”.

Quando non c’è rischio di ambiguità, i vettori espressi in un RF “Fisso” non avranno apice. Definito un certo RF “Fisso”, esso può essere utilizzato per esprimere lo stato dinamico del veicolo.

Per ogni istante t in I, lo stato dinamico del veicolo è definito dalle seguenti componenti:

• r(t): posizione del veicolo: coordinate dell’origine del BRF, espresse nel sistema di riferimento “Fisso” scelto;

• v(t): velocità del veicolo;

• A(t): assetto del veicolo: per semplicità di notazione (ma senza perdere in generalità), si assumerà che l’assetto venga rappresentato tramite una matrice di coseni direttori. Di conseguenza, la matrice di assetto A contiene le componenti dei tre versori di BRF, nel sistema di riferimento “Fisso” scelto;

• ω(t): velocità angolare del veicolo;

L’accelerazione del veicolo, a(t), può essere suddivisa in due componenti:

• Accelerazione di gravità, g

• Accelerazione “misurabile” a bordo dalla IMU, am(t) = a(t) - g

Le misure effettuate dalla IMU sono le seguenti:

• L’accelerazione misurabile, espressa nel BRF: a<B>

m(t )

• La velocità angolare, espressa nel BRF: ω<B>(t).

Per indicare un metodo generico per propagare la matrice di assetto dall’istante temporale t=t' a t=t'', utilizzando ω(t) in t ∈ [t',t''] ⊂ I, verrà utilizzata la seguente espressione:

t' '

A(t'')<=>∫[ω(t)<×>]dt A( t' ) (2) t '

Utilizzando queste definizioni, si ottengono anche le seguenti relazioni, per [t’,t’’] ⊂ I:

t' '

v(t'')<=>v(t')<+>∫am(t)dt<+>g(t''<−>t' )

t '

t' ' t

1 2

<r>(t'')<= r>(t')<+ v>(t')(t''<−>t')<+>∫ ∫<a m>(<τ>)d<τ>dt<+>2<g>(t''<−>t' )

t ' t '

(3)

Un punto generale sulla superficie del corpo celeste considerato avrà il pedice “s”.

1.1 IL CONCETTO DI “TRACCIA” E CONCETTI ASSOCIATI

La figura 2 mostra il concetto di tracce di punti caratteristici del terreno; punti “particolari” vengono riconosciuti in immagini diverse (prese in instanti diversi) dal dispositivo di Elaborazione Immagini; questi punti vengono inseguiti, tracciati nel loro moto apparente sul piano dell’immagine. Questi punti tracciati, assieme ai dati della IMU, vengono utilizzati dal Filtro di Navigazione (vedi figura 2).

Il concetto di “traccia” (di punti caratteristici del terreno) può essere definito in modo accurato nel seguente modo:

• Durante il periodo temporale I, il dispositivo di Elaborazione Immagini ha effettuato, ripetutamente, i seguenti due tipi di operazioni:

a) Estrazione (o identificazione) dei punti caratteristici da tracciare;

b) Tracciamento dei punti caratteristici ;

• La prima operazione consiste nell’identificazione, sulle immagini acquisite, di certi particolari punti del terreno (non precedentemente noti) che apparentemente hanno la caratteristica di essere facilmente tracciabili da un’immagine a quella successiva. L’insieme di tutti i punti (rgi) estratti durante l’intervallo temporale I verrà indicato con Rg:

Rg= (rg1, … rgN) (4) • In generale, l’identificazione di un punto rgiè avvenuto in certo istante in I; questo istante, indicato con tAi(per l’i-esimo punto), indica l’”inizio” della traccia i-esima;

• In un certo istante tBi> tAi, il tracciamento del punto i-estimo finisce (per es. perché è uscito dall’area visibile); questo istante indica la “fine” della traccia i-esima;

• La figura 3 mostra un esempio della disposizione temporale delle tracce:

• L’intervallo temporale (per una traccia generica i-esima) [tAi,tBi] verrà chiamato “Intervallo di Traccia” (questi intervalli sono, in generale, diversi per tracce diverse);

• Durante questo intervallo temporale, il blocco di Elaborazione Immagini “insegue” il punto rgifornendo, per ogni t in [tAi, tBi], la sua posizione apparente nell’immagine, in termini di coordinate (riga e colonna) del pixel (per semplicità di notazione, qui è stato usato un asse temporale “continuo”, mentre in pratica sarà ovviamente “campionato” a una certa frequenza):

pi(t) = (rowi(t),columni(t)), ∀t∈[tAi,tBi] (5)

• La funzione pi(t) viene chiamata “traccia i-esima”; l’insieme di tutte le N tracce viene indicato con P:

P = (p1(⋅),p2(⋅), …pN(⋅)) (6)

dove la notazione pi(⋅) indica una funzione del tempo (che ha come dominio [tAi,tBi]).

Di conseguenza, si può dire che esistono tre “livelli di gerarchia” coinvolti nel concetto di “traccia”:

• L’oggetto a livello più alto è l’insieme delle tracce P, che contiene N elementi (ciascun elemento è una traccia separata);

• A sua volta, ogni traccia è una funzione del tempo; di conseguenza può fornire un valore di pi(t ) per ogni t nel suo dominio, [tAi,tBi] (ovviamente, in pratica è impossibile produrre un numero infinito di valori; di conseguenza ogni traccia in pratica conterrà una sequenza di valori dei vettori pi, associati a istanti temporali spaziati uniformemente nell’intervallo [tAi,tBi] – per es. con un periodo di campionamento di 0.1 sec);

• Il livello più basso sono i vettori p, composti dai valori riga e colonna.

Strettamente associato a pi(t) è il concetto del versore KI(t ) (nb: la lettera "I" di "KI(t ) " è u riferimento all'indice "i") Similmente a pi(t), KI(t ) indica la direzione apparente del punto tracciato i-esimo, ma nello spazio tridimensionale di un RF “Fisso”, e non nel piano bidimensionale dell’immagine:

r gi − r( t )

KI(t)= , ∀t∈[ tAi, tBi] (7) |r gi − r( t) |

(nell’espressione sopra, è stato assunto che la posizione della camera può essere approssimata con la posizione dell’origine del BRF). La figura 4 mostra la relazione tra pi(t) e KI(t ) .

E’ possibile trasformare facilmente pi(t) in KI(t ) , se l’assetto del velivolo è noto, come descritto nella sezione “Passo di pre-elaborazione”. La figura 5 mostra un esempio di come il versore KI(t ) “insegue” il punto sulla superficie rgimentre il velivolo si sposta da r(tAi) a r(tBi).

2. DESCRIZIONE DELLA FUNZIONE DEL DISPOSITIVO

Tenendo conto dei concetti introdotti finora, è possibile descrivere accuratamente la funzione del filtro di navigazione.

(NB: qui il concetto di “profilo” di una variabile indica l’insieme di tutti i valori assunti dalla variabile durante un certo intervallo temporale; questo viene indicato inserendo la variabile tra le parentesi tonde - per es. la traiettoria dei veicolo durante l’intervallo temporale I è indicata con (r(t))I).

Il filtro riceve i seguenti dati in ingresso:

• Il profilo delle misure IMU nell’intervallo temporale I, ovvero (a<B>

m( t))<B>

Ie (ωm( t))I

• L’insieme della N tracce ottenute durante l’intervallo temporale I, ovvero P = (p1(⋅),p2(⋅), … pN(⋅))

Utilizzando questi dati, il filtro deve produrre le stime del profilo dello stato del veicolo durante l’intervallo temporale I, ovvero:

• (r(t))I

• (v(t))I

• (A(t))I

• (ω(t))I.

Ovviamente, è fondamentale specificare il sistema di riferimento nel quale queste stime verranno espresse; questo verrà fatto nella sezione successiva.

3. DEFINIZIONE DEL SISTEMA DI RIFERIMENTO PER LE STIME DELLO STATO

Chiaramente, il sistema di riferimento nel quale le stime dello stato verranno espresse dovrà essere scelto in modo adeguato. Un criterio importante è che l’ RF scelto deve essere tale da consentire un efficiente utilizzo delle stime prodotte (da parte di altri dispositivi/algoritmi utilizzati a bordo del velivolo). Per soddisfare questa condizione, l’ RF scelto dovrà essere “Fisso”.

Inoltre, è fondamentale conoscere la direzione del vettore gravità g nell’ RF scelto. In alternativa, l’ RF scelto può semplicemente avere un Asse Z Verticale per definizione, in modo da avere g = (0,0,-|g|)<T>(qui l’apice T indica la trasposta). Questa è l’assunzione considerata nel seguito.

Di conseguenza, l’ RF scelto per le stime dello stato dovrà essere definito rispetto ad alcune caratteristiche “locali” della traiettoria del veicolo o dei punti osservati.

Per definire questo RF, è utile introdurre un RF “Ausiliario”, ARF, definito nel seguente modo: • E’ un RF “Fisso”,

• L’origine del RF coincide con la posizione del veicolo per t = t0

• Gli assi del RF coincidono con gli assi del BRF per t = t0

In alter parole, per t = t0ARF coincide con BRF.

ARF ha le utili proprietà che, in esso, r(t0) = 0 e A(t0) = I (qui I indica la matrice identità 3x3), per definizione. ARF ha anche lo svantaggio di non avere, in generale, un Asse Z Verticale (a meno che il veicolo non abbia l’asse z “body” orientato verticalmente per t = t0).

D’altra parte, se si riescono a calcolare le componenti del vettore gravità g nel ARF, sarà semplice “ruotare” ARF per allineare l’asse z con la gravità. L’ RF risultante verrà chiamato RF “Finale” (FRF) e verrà utilizzato per esprimere le stime dello stato del veicolo calcolate dal Filtro di Navigazione.

I Sistemi di riferimento RF coinvolti sono schematicamente raffigurati nella figura 6 (per semplicità, nella figura è stato considerato il caso 2D)

Il FRF, centrato (come anche ARF) nella posizione del veicolo per t = t0, ha l’Asse Z Verticale, ed inoltre è “Fisso”. Di conseguenza, si tratta di un RF adeguato per esprimere le stime dello stato.

4. DESCRIZIONE DELL’ALGORITMO

L’algoritmo per il filtro di navigazione può essere visto, ad alto livello, come una sequenza di tre passi:

1) Passo di pre-elaborazione, necessario per alcuni calcoli iniziali da fare sui dati in ingresso; 2) Passo centrale: qui vengono realizzate le operazioni principali del filtro; questo essenzialmente consiste nella stima (nell’ ARF) della velocità dei veicolo (per t = t1) e della accelerazione di gravità, g. Questo viene fatto elaborando i dati IMU nell’ ARF e l’insieme di tracce elaborate; questi dati vengono forniti dal passo di pre-elaborazione;

3) Passo di post-elaborazione, qui vengono utilizzati alcuni degli output del passo di preelaborazione, e poi combinati con gli output del “passo centrale”, per ottenere il profilo dello stato stimato nell’ FRF.

4.1 PASSO DI PRE-ELABORAZIONE

Con questo passo vengono preparati i dati in ingresso per esprimerli nella forma corretta – e in particolare per esprimerli nel sistema di riferimento “Ausiliario” (ARF).

4.1.1 CALCOLO DELL’ASSETTO, VELOCITA’ ANGOLARE E ACCELERAZIONE MISURATA NELL’ ARF

La prima operazione effettuata ha come obiettivo il calcolo del profilo dell’assetto e della velocità angolare nell’ ARF, ovvero:

(A<A>(t))I, (ω<A>(t))I(8) partendo dal profilo delle misure di velocità angolare, ovvero (ω<B>(t))I. Si tratta di un’operazione piuttosto semplice, per il fatto che l’assetto iniziale in ARF è noto per definizione:

A<A>(t0) = I (9) A questo punto, per calcolare il profilo dell’assetto e della velocità angolare per t>t0, è possibile utilizzare le seguenti espressioni:

ω<A>(tn) = A<A>(tn) ω<B>(tn)

A<A>(tn+1) = [ω<A>(tn)×] A<A>(tn) ∆t

(10)

dove l’asse temporale è stato discretizzato con un passo ∆t; per n ≥ 0:

tn= t0+ n∆t (11) Una volta ottenuti i profili (A<A>(t))Ie (ω<A>(t))I, anche l’accelerazione misurata viene convertita dal BRF all’ARF:

a<A>

m(t) =A<A>(t) a<B>

m( t ) (12) 4.1.2 ELABORAZIONE DELLE TRACCE PER CALCOLARE I VERSORI ASSOCIATI

L’obiettivo di questo calcolo è l’elaborazione dell’insieme P delle N tracce, P = (p1(⋅),p2(⋅), …pN(⋅)) (ottenuto dal dispositivo di elaborazione immagini), dove ogni traccia è una funzione del tempo pi(t) = (rowi(t), columni(t)), per ottenere un insieme TP di N tracce “trasformate”:

TP =(K1(⋅),K2(⋅),... KN( ⋅ )) (13) dove ogni traccia “trasformata” KI(⋅ ) è semplicemente il versore che indica la direzione verso il punto tracciato i-esimo, durante l’”Intervallo di Traccia” [tAi,tBi], nell’ ARF.

Di conseguenza, P può essere trasformato in TP calcolando – per ogni traccia 1<i<N, e per ogni istante t∈[tAi,tBi] – il valore di KI(t ) utilizzando il valore di pi(t).

Il calcolo di KI(t ) utilizzando pi(t) può essere effettuato nel modo seguente:

1) Si calcola il di valore di KI(t ) nell’ RF della fotocamera (CRF); questo valore verrà indicato con KRFCI(t ) . Si assume di avere un CRF con l’asse z passante attraverso l’asse ottico della fotocamera, e l’origine sul piano focale della fotocamera. Di conseguenza, KRFCI(t ) può essere calcolato utilizzando un modello della fotocamera, che associa le coordinate dei pixel a direzioni nello spazio. Per esempio, se si considera un semplice modello “pinhole”, allora KRFCI(t ) può essere calcolato semplicemente con:

K ( ) =<1>(<~>(column ( )),<~>(row ( )), − T

RFCI t x i t y i t 1 ) (14)<~>x2(columni(t))+<~>y 2(row i( t)) 1

dove :

~<column>FOV FOV y x(column)=(-1+2 ) tan(x ) , ~ y(row)=(1- 2<row>) tan() (15)

N column 2 N row 2 e Nrow, Ncolumn, FOVx, FOVysono, rispettivamente, il numero delle righe e delle colonne dell’immagine, e il campo di vista lungo gli assi x e y della fotocamera.

2) Dato che la posizione e l’orientazione della camera rispetto al veicolo sono note, è possibile trasformare il versore KRFCI(t ) in KRFBI(t ) .

3) Infine, usando l’informazione dell’assetto del veicolo in ARF, KRFBI(t ) viene trasformato in KI(t ) .

Per riassumere, le uscite di questo passo di pre-elaborazione sono le seguenti:

<A A>• I profili (A (t))I, (ω (t))Ie (a<A>

m( t))I;

• Le tracce TP =(K1(⋅),K2(⋅),... KN( ⋅ )) ; ogni traccia contiene il profilo (ovvero i valori nel tempo) del versore KI(t ) , che “punta” nella direzione del punto i-esimo “inseguito” sulla superficie r<A>Questi vettori sono espressi nel ARF.

4.2 PASSO CENTRALE

I dati del passo di pre-elaborazione utilizzati qui sono i seguenti:

• Il profilo di accelerazione misurata, (a<A>

m( t))I;

• L’insieme TP, contenente le tracce dei punti, espressi come versori nell’ ARF.

Inoltre, viene utilizzato anche il valore della norma dell’accelerazione di gravità (nella regione dello spazio considerato), gS.; si considera questo dato come noto.

Utilizzando queste informazioni, verranno calcolati i seguenti valori:

<A>• Il vettore di accelerazione della gravita, nell’ ARF: g

• La velocità del veicolo per t = t1, in ARF: v<A>(t1)

D’ora in poi, nel resto della descrizione di questa parte dell’algoritmo, l’apice “A” verrà omesso, dato che tutti i vettori verranno espressi nell’ ARF.

La stima di g e v(t1) inizia scegliendo una delle N tracce nell’insieme TP. L’indice della generica traccia scelta verrà qui indicato con i*∈[1,…N]. Utilizzando la traccia i*-esima come punto di partenza, si calcolerà una singola stima di g e v(t1).

(NB: è importante notare come, durante il processo di calcolo di g e v(t1), verranno utilizzate anche altre tracce in TP , assieme alla traccia i*. E’ possibile tuttavia vedere la traccia i*-esima come quella principale, ovvero la “pivot”).

La traccia i*-esima, KI(t ) , inizia all’istante tAi*e finisce a tBi*. L’istante temporale centrato tra queste due verrà indicato con tMi*:

t Mi * =tAi* t Bi * (16)

2

La differenza tra questi due istanti temporali verrà indicata con T:

Ti*= tBi*- tMi*= tMi*- tAi*. (17) In generale, è possibile considerare anche un algoritmo dove l’istante temporale tMi*non è esattamente centrato tra tAi*e tBi*; questo può comportare un piccolo incremento nell’accuratezza; per ottenere una descrizione più compatta, questo caso più generale non verrà considerato qui.

E’ importante tenere conto del fatto che, a questo stadio, le coordinate del punto del terreno rgi*non sono note al Filtro di Navigazione.

A questo punto si introduce la seguente notazione semplificativa, per le posizioni e le velocità agli istanti temporali tAi*, tMi*e tBi*:

rA= r(tAi*), rM= r(tMi*), rB= r(tBi*), vA= v(tA), vM= v(tM) (18)

Inoltre si definiscono le differenze delle posizioni:

DELTA_R = ∆rA= rM- rA, ∆rB= rB- rM(19)

Queste differenze sono chiaramente legate ai valori di vA, vB, g e dell’accelerazione misurata amattraverso le seguenti espressioni (negli integrali, il pedice “i*” è stato omesso dal limite temporale, per semplificare la notazione):

t M t

<∆rA = vA>T<+>∫ ∫<a m>(<τ>)d<τ>dt<+ 1>2

2<g>T

t A t A

t B t

<∆rB = vB>T<+ m>(<τ>)d<τ>dt<1>

2 T 2

t ∫ M t ∫<a g>

M

(20)

Inoltre, tenendo conto che:

t M

vM<=>vA<+>∫am(t )dt<+>g T (21) t A

si ha :

∆r 2

A= vAT h<II>

AM+<1>

2 g T

∆r 3

B= vATi*+h I

AMTi*+h II

MB 2

2 g T

(22)

dove ∆rAe ∆rBsono stati espressi in funzione di vA, g e dei seguenti valori che indicano gli integrali dell’accelerazione misurata:

t M t M t t B t

h I

AM<=>a m(t) dt ,<h>II II

<AM = τ>dt<, hMB =>

t ∫ A ∫ ∫<a>m<(τ )>d ∫ ∫<a>m<(τ )d τ dt (23)>t A t A t M t M

E’ importante tenere conto del fatto che h<I>

AM, h<I>

A<I>

Me h<II>

MBpossono essere calcolati dal filtro, dato che il profilo di am(t) è noto.

A questo punto, è utile suddividere le differenze delle posizioni in una parte “posizione” e una parte “lunghezza”:

DELTA_R = ∆rA= lAUA , ∆rB= lBUB (24)

dove UA e UB sono due versori.

I valori di UA e UB possono essere calcolati dal filtro utilizzando le tracce disponibili. La procedura per farlo verrà adesso descritta, nel caso di un vesore U generico, che indica lo spostamento tra due istanti temporali T1 e T2, ovvero per il quale si ha che r(T2) - r(T1) = lGU, dove lGè uno scalare. Per UA e UB la procedura è analoga.

Considerando il piano definito dai punti r1= r(T1), r2= r(T2) e rgi*, si ha che i seguenti versori sono tutti paralleli a questo piano:

• U (il versore da r1a r2);

• KI 1 (il versore da r1a rgi*);

• KI 2 (il versore da r2a rgi*)

Il triangolo di detti versori con i vertici r1, r2e rgi*è mostrato nella figura 7.

Di conseguenza, il versore:

KI1 × KI 2KCI= (25)

| KI1 × KI2 |

è ortogonale a U , ovvero KCI ⋅U = 0 (qui “⋅” indica il prodotto scalare).

Se si assume che, invece di avere una singola traccia da t = T1 a t = T2, che insegue un punto del terreno rgi*, ce ne sono più di una, è possibile definire più triangoli analoghi a questo. Di conseguenza, è possibile pure definire più versori KCI , tutti con la proprietà di ortogonalità rispetto a U .

Per esempio, considerando tre tracce, che seguono rispettivamente i punti del terreno rgi,rgi’e rgi’’, si ottiene la situazione mostrata nella figura 8.

Da qui è possibile facilmente vedere che b<ˆ>⋅U = 0 , b<ˆ>'⋅U = 0 e b<ˆ>''⋅U = 0. Di conseguenza, è possibile calcolare U cercando il vettore ortogonale a b<ˆ>, b<ˆ>' e b<ˆ>' ' (se non ci fossero errori nei dati in ingresso, sarebbe sufficiente fare il prodotto vettoriale tra due vettori b<ˆ>di questo tipo). Di conseguenza, è possibile costruire facilmente un metodo per stimare U (anche in presenza di errori) partendo dalla traccia “pivot” KI(t ) (che inizia a T1 e finisce a T2). Utilizzando questa

traccia, si ottiene un triangolo e un vettore b<ˆ>. Poi, si aggiungono altre tracce da TP =(K1(⋅),K2(⋅),... KN( ⋅ )) , per ottenere più triangoli e versori b<ˆ>.

Dato che il versore U indica la “direzione del moto” tra t = T1 e t = T2, ovvero:

r(T2) − r( T1 )UA= (26) |r(T2) − r( T1) |

le tracce di TP che possono essere aggiunte sono solamente quelle che iniziano prima di T1 e finiscono dopo T2. Questo è dovuto al fatto che, per definire un triangolo (e il versore corrispondente b<ˆ>) partendo da una traccia KI(t ) , è necessario poter valutare questa traccia per gli istanti temporali t = T1 e t = T2. La figura 9 mostra un esempio di soluzione per quanto riguarda l’identificazione di tali tracce “ammissibili”.

Il metodo complessivo è di conseguenza il seguente:

1) Partendo dall’insieme TP, se ne identifica un sottoinsieme Q*=(K1(⋅),K2(⋅),... KM( ⋅ )) , composto dalle tracce che iniziano prima di T1 e finiscono dopo T2 (è possibile modificare facilmente l’algoritmo per tenere conto anche del caso quando il numero di tracce identificate in questo modo, M, è troppo basso per una stima accurata – per esempio, permettendo una modifica di T1 e T2;).

2) Per ogni traccia KI(⋅ ) di Q* (con I∈[1,...M]), ne vengono valutati i valori per t = T1*e t = T2:

KI1 = KI( T1 ) , KI2 =KI( T2 ) (27)

3) Poi, si costruisce il versore ortogonale ad entrambi:

KKI1 × KI 2CI= (28)

| KI1 × KI2 |

4) A questo punto, si ha un insieme di M versori, E =(KC1,KC2,... KCM ) . Se non ci fossero errori nella determinazione delle tracce, tutti questi versori sarebbero ortogonali a U . In pratica, in presenza di errori, è utile trovare U cercando il versore “più ortogonale” a tutti gli (KCI ) . Qui, questo viene fatto minimizzando la funzione:

M

J(u)= ∑ I = 1( KCI ⋅ u)<2>(29)

con il vincolo:

| u | = 1 (30) Per minimizzare la funzione, è necessario trovare i punti, sulla sfera unitaria, per i quali il gradiente di J(u), ∇J(u), è perpendicolare al piano tangente alla sfera. Di conseguenza ∇J(u) dovrà essere parallelo a u, ovvero:

∇J(u) = λ u (31) dove λ è uno scalare.

L’espressione per il gradiente di J è :

∇J(u) = W u (32) dove:

�w1⋅w1 w1⋅w2 w1 ⋅ w 3�

W = 2� �

�w2⋅w1 w2⋅w2 w2 ⋅ w 3� (33) ��w3⋅w1 w3⋅w2 w3 ⋅ w 3��

dove w1, w2and w3sono tre vettori con M componenti, ottenuti, rispettivamente, dalle componenti x, y e z dei vettori KCI :

w1=(KC1x,KC2x,... KCMx) T

w2=(KC1y,KC2y,... KCMy) T

w3=(KC1z,KC2z,... KCM T

z)

(34)

Il vettore U dovrà quindi soddisfare la condizione:

WU = λU (35) per un certo λ.

Di conseguenza, U viene trovato tra gli autovettori di W.

Per calcolare UA , si utilizza KIA (il versore da rAa rgi*) al posto di K 1 , e KIB (il versore da rBa rgi*) al posto di K 2.

Per calcolare UB , si utilizza KIM al posto di K 1 , e KIB (il versore da rBa rgi*) al posto di K 2. Questa stima ai minimi quadrati di U può essere espansa (per es. usando la nota tecnica del Random Sample Consensus, o RANSAC) per rendere il metodo di stima più robusto a tracce “outlier” (ovvero tracce “spurie”).

E’ importante tenere conto del fatto che la scelta dell’insieme Q* dipende dai valori di T1 e T2 i quali, a loro volta, dipendono dalla scelta della traccia “pivot” i*.

A questo punto, assumendo di aver calcolato le stime di UA e UB , si considerano due triangoli TRA e TRB (in generale non coplanari), con, rispettivamente, i seguenti vertici:

• rgi*, rMe rA

• rgi*, rBe rM

I due triangoli TRA e TRB ed alcuni vettori associati, sono mostrati nella figura 10:

I due triangoli hanno un lato comune, lungo l* (il valore di l* non è noto a questo stadio).

I valori di ALPHA_A, ALPHA_B, BETA_A e BETA_B si ottengono calcolando gli angoli tra alcuni versori noti:

ALPHA_A = arccos( UA ⋅ KIA ), ALPHA_B = arccos( -UB ⋅ KIB ) BETA_A = arccos( KIA⋅ KIM ), BETA_B = arccos( KIB ⋅ KIM ),

(36)

Utilizzando il teorema dei seni, si hanno le seguenti espressioni:

sin(ALPHA_ A ) = sin(BETA_ A ) , sin(ALPHA_ B ) =sin(BETA_ B ) (37) l * lAl * lB

Queste espressioni vengono utilizzate per eliminare l* e ottenere un espressione per il rapporto tra lBe lA. Qui questo rapporto viene indicato con RHO:

RHO=<l>B =sin(ALPHA_A)sin(BETA_ B ) (38) l A sin(ALPHA_B)sin(BETA_ A )

A questo punto, si riorganizzano le equazioni (22) (sottraendo la prima dalla seconda) per eliminare vA, e poi si usano le espressioni (24) (per le differenze di posizione), e (38), (per il rapporto tra lBe lA). L’espressione che si ottiene è:

UBl − =h I II II 2ARHO UAlA AMTi* hMB−hAM g Ti* (39) Questa espressione può essere semplificata introducendo le seguenti grandezze:

s0=UBRHO − UA

s I II II

1=hAMTi*+hMB − hAM

s2=T 2

i*

(40)

dove s0e s1sono vettori, e s2è uno scalare. E’ importante tenere conto del fatto che s0, s1e s2possono essere tutti calcolati dal filtro utilizzando i dati in ingresso disponibili.

L’equazione risultante, che lega lAcon g, è la seguente:

s0lA= s1+ s2g (41) Questo è un’equazione vettoriale (corrispondente a 3 equazioni scalari), con 4 incognite scalari (il valore di lA,e le tre componenti del vettore di gravità g, nell’ ARF). Aggiungendo a questa la condizione |g| = gS(dove gSè stato assunto noto), si ottiene un sistema di 4 equazioni scalari e 4 incognite scalari. E’ possibile ridurlo a una singola equazione quadratica con lAcome incognita, scrivendo la condizione che riguarda la gravità come g⋅g = gs<2>e poi sostituendo g, con la (41). L’equazione risultante per calcolare lAè:

|s0|<2>lA<2>- 2(s0⋅ s1)lA+ |s1|<2>- gSs2<2>= 0 (42) Questa equazione ha due soluzioni per lA; quella corretta può essere identificata con semplici controlli dei valori delle stime ottenute (per es. tipicamente con la soluzione “sbagliata” si ottiene un vettore gravità che punta verso l’alto).

Una volta ottenuto lA, si calcola g, utilizzando la (41). Successivamente si calcola lB, utilizzando il valore di RHO (calcolato con la (38)). Da lAe lB, i vettori DELTA_R = ∆rAe ∆rBsi ottengono dalla (24).

A questo punto, utilizzando la (22), si calcola anche vA. Infine, il valore della velocità per t = t1si ottiene semplicemente tramite integrazione:

t 1

v(t1)<=>vA<+>∫am(t)dt<+>(t1<−>t A ) g (43) t A

Come indicato all’inizio della descrizione del passo centrale dell’algoritmo, questi valori di v(t1) e g sono stati ottenuti utilizzando come “punto di partenza” (o “pivot”) una traccia generica i*. Se non ci fossero errori nei dati in ingresso, sarebbe sufficiente scegliere in modo arbitrario un valore per i* nell’insieme [1,...N], partire dalla traccia corrispondente, e poi seguire il metodo descritto qui per ottenere i valori corretti di v(t1) e g.

In pratica, i dati in ingresso avranno degli errori; di conseguenza, per ridurre l’errore di stima di v(t1) e g, si può utilizzare una procedura di calcolo della media, per esempio:

1) Ciascuna delle N tracce in TP=(K1(⋅),K2(⋅),... KN( ⋅ )) viene presa come “pivot”, i*, per generare una singola coppia di stime della velocità (a t1) e della gravità, indicata con (vi*(t1), gi*) ;

2) Partendo dalle N stime di v(t1) e g ottenute in questo modo, se ne calcola la media, per ridurre gli errori di stima.

Invece di una semplice operazione di media, è possibile anche considerare altri metodi (e.g. il mediano, o RANSAC) per ottenere una stima più robusta rispetto agli “outliers”.

Inoltre, un’altra variante consiste nella realizzazione di una media pesata delle singole stime, calcolando le medie in funzione delle varianze delle singole stime (i valori delle varianze potrebbero a sua volta essere stimate propagando i valori assunti per le incertezze in ingresso attraverso i calcoli), minimizzando la stima complessiva dell’errore.

4.3 PASSO DI POST-ELABORAZIONE

Questo passo riceve i seguenti dati:

• Dal passo di pre-elaborazione: profili di assetto, velocità angolare e accelerazione misurata nell’ ARF, ovvero (A<A>(t))<A>I<A>, (ω (t))Iand (am( t))I

<A A>• Dal passo centrale: le stime di v (t1) e g

L’obiettivo di questo passo è il calcolo del profilo dello stato (composto dai profili di asseto, velocità angolare, posizione e velocità lineare) nell’ FRF.

La prima operazione effettuata qui ha come obiettivo il calcolo del profilo di velocità e posizione in FRF.

<A>Il profilo di velocità è calcolato semplicemente integrando la stima di v (t1) utilizzando (a<A>m( t))Ie g:

t

<∀t∈I,>v<A>(t)<=>v<A>(t1)<+>aA(τ)d τ (t t A

∫ m<+ −>1) g (44) t 1

Dato che r<A>(t0) = 0 per definizione, il profilo della posizione viene calcolato tramite semplice integrazione della velocità:

t

<∀t∈I,>r<A>(t)<=>v<A>(τ ) d τ (45) t ∫ 0

A questo punto sono disponibili tutte le componenti del profilo dello stato in ARF (ovvero (r<A>(t))I, (v<A>(t))I, (A<A>(t))I, (ω<A>(t))I).

Per convertirle in FRF, si applica una matrice di rotazione che descrive la trasformazione ARF→FRF; questa trasformazione tiene conto del fatto che FRF ha l’Asse Z Verticale, mentre ARF no.

Di conseguenza, la matrice di rotazione è quella che descrive la rotazione attorno a un asse di un certo angolo, dove:

• L’asse è il vettore ortogonale sia al vettore di gravità che all’asse Z dell’ARF.

• L’angolo ϕ è l’angolo tra il vettore di gravità e l’azze Z del ARF - di conseguenza, viene ottenuto dalla componente z del vettore gravità nel ARF, g<A>

z:

A

ϕ=arccos( − gz ) (46) g S

In questo modo, intuitivamente, la rotazione “raddrizzerà” tutte le componenti del profilo dello stato in ARF, portandole in FRF.

5. INFORMAZIONI AGGIUNTIVE

5.1 FUNZIONAMENTO TEMPORALE DEL FILTRO

Nella descrizione fino a questo punto, è stata descritta la modalità “batch” del Filtro di Navigazione. In questa modalità, l’algoritmo opera nel seguente modo:

• riceve, come ingressi, i dati ottenuti durante l’intero intervallo temporale [t0,t1]

• poi, produce, in uscita, le stime dello stato, relative all’intero intervallo temporale [t0,t1]

In pratica, il Filtro tipicamente opererà in una modalità “incrementale”, dove:

• L’algoritmo verrà chiamato a una frequenza che può essere relativamente alta (per es.10 Hz), e

• Per ogni chiamata, l’algoritmo:

- Riceverà gli ingressi ottenuti tra la chiamata precedente e quella attuale, e:

- Dovrà calcolare la stima dello stato “corrente”

Per ottenere un funzionamento nella modalità “incrementale”, si fanno le seguenti modifiche: • Tra chiamate successive dell’algoritmo, i valori dei dati in ingresso vengono memorizzati (assieme ad altri dati rilevanti, come per es. le stime di v(t1) e g) in una memoria

• Ad ogni chiamata, l’algoritmo inserisce nella memoria i nuovi dati in ingresso, e poi processa tutti i dati contenuti in essa che sono rilevanti per la stima (per es. da un certo istante temporale in poi)

5.2 CASI DI VELOCITA’ NON OSSERVABILI

In generale, un qualsiasi Filtro di Navigazione - che utilizza solamente dati IMU e tracce di punti sconosciuti sulla superficie - non potrà stimare la velocità (e quindi nemmeno le differenze di posizione) se l’accelerazione è nulla; è possibile dimostrarlo tenendo conto del fatto che in una tale situazione esistono più profili di velocità e posizione che producono lo stesso profilo di dati in ingresso; di conseguenza, il Filtro di Navigazione “non può decidere” la traiettoria corretta. Nel caso del Filtro di Navigazione considerato qui, se l’accelerazione è ≈0 durante l’intervallo di traccia [tAi*,tBi*] (corrispondente alla traccia “pivot” i*-esima), si avrà che:

UB ≈ UA , RHO≈ 1 (47)

e di conseguenza, dalla (40),

s0≈ 0 (48)

e quindi le stime di lAottenute con (42) - e anche le risultanti stime di vi*(t1) and gi*- non saranno affidabili. In tali situazioni, è possibile fare le seguenti operazioni:

• Nel processo di accumulazione di stime singole di v(t1) and g, i valori ottenuti con le tracce “pivot”, che hanno come intervallo di traccia periodi temporali con accelerazione bassa, vengono esclusi dal calcolo

• In alternativa, è possibile combinare le stime basate sulla IMU e sulle tracce con informazioni aggiuntive relative al modello del veicolo. Per esempio, nel caso di un atterraggio su Marte, se il Filtro di Navigazione inizia ad operare quando il veicolo è ancora attaccato al paracadute, si ha che una situazione con un’accelerazione bassa può verificarsi quando il veicolo si avvicina alla velocità terminale (prima dello sgancio del paracadute). Tuttavia, in una situazione di questo tipo, le equazioni della dinamica possono diventare più semplici (per es. il moto potrebbe essere quasi verticale e uniforme), e quindi la conoscenza del modello può essere utilizzata per stimare dei limiti sul modulo della velocità (per esempio, in un caso molto semplice, il modulo della velocità corrisponderebbe alla velocità terminale)

E’ importante tenere conto del fatto che la direzione del movimento può essere osservata anche con un’accelerazione nulla (utilizzando il metodo per la stima di uˆA, del “Passo centrale”).

5.3 FUNZIONALITÀ AGGIUNTIVE

Il Filtro di Navigazione considerato finora, può essere utilizzato anche per implementare alcune funzionalità aggiuntive che sono qui di seguito descritte.

5.3.1 RICOSTRUZIONE DELLA FORMA DEL TERRENO

Questa funzionalità può essere aggiunta facilmente, realizzando un’operazione di triangolazione una volta che i valori di rAe rMsono stati stimati. E’ possibile prendere l’insieme delle tracce Q*=(K1(⋅),K2(⋅),... KM( ⋅ )) utilizzate per la stima di UA , e l’insieme dei vettori risultanti (KJ1 ) and (KJ2 ) . A questo punto, per ogni J ∈{1,... M } , la posizione del punto sulla superficie rg*Jpuò essere ottenuta cercando l’intersezione (oppure il punto di minima distanza) tra:

• La semiretta che parte da rAe va nella direzione KJ 1

• La semiretta che parte da rMe va nella direzione KJ 2

5.3.2 AGGIUNTA DI SENSORI ADDIZIONALI

La combinazione di sensori considerata - IMU e fotocamera - può essere estesa aggiungendo altri sensori per aumentare ulteriormente la robustezza e l’accuratezza del processo di stima dello stato. Il Filtro di Navigazione può essere infatti adattato per ricevere dati da sensori addizionali quali:

• Sensori di stelle: questi possono essere utilizzati per fornire direttamente le misure dell’assetto del veicolo (per es. per un atterraggio lunare);

• Altimetro: l’aggiunta di un altimetro può permettere un incremento dell’accuratezza delle stime dello stato, fornendo un metodo alternativo per stimare una delle principali grandezze coinvolte, ovvero la lunghezza del vettore di “differenza di posizione”, DELTA_R = ∆rA(considerato nel “Passo centrale” dell’algoritmo del filtro). Essenzialmente, processando i dati dell’altimetro, si misura la lunghezza della proiezione del vettore DELTA_R lungo la direzione nˆaltimeter(questo vettore indica le direzione di puntamento dell’altimetro); indicando questa lunghezza con laltimeter, si ha che per calcolare il valore di lAè possibile utilizzare la seguente espressione (dove nˆaltimetere K 1 sono entrambi calcolati nel sistema di riferimento ARF):

l<A>= l altimeter (49) K1⋅ nˆ altimeter

• Scanning LIDAR: questo sensore consente la misura delle distanze lungo la direzione di un fascio laser, e questa direzione può essere controllata sui due gradi di libertà (un sensore del tipo “Flash” LIDAR può essere utilizzato in alternativa). In questo modo, si ottiene una scansione del terreno sottostante, espressa rispetto al veicolo. Stimando il moto apparente del terreno scansionato, si può ricostruire il movimento del veicolo. Similmente al caso dell’altimetro, qui lo Scanning LIDAR rappresenta un modo alternativo per stimare ∆rA(d’altra parte, qui possono essere osservate anche tutte e 3 le componenti del vettore ∆rA).

5.3.3 NAVIGAZIONE ASSOLUTA

Questa funzionalità consente la stima del profilo dello stato del veicolo in un sistema di riferimento “planetocentrico” (PRF), ovvero in un sistema di riferimento che viene utilizzato per specificare (in modo non ambiguo) i punti del terreno sul corpo celeste considerato - come per es. tramite la latitudine, longitudine e altezza (ovviamente da definire opportunamente).

Per realizzare questa funzionalità, il Filtro di Navigazione deve avere a disposizione anche un insieme di dati “precaricati” (ovvero messi a disposizione prima dell’inizio del funzionamento del filtro). In questo caso, questi dati corrispondono a una “mappa” del terreno (per es. composta da una combinazione di un Digital Elevation Model e un’ortoimmagine), riferita rispetto al sistema di riferimento PRF.

Senza perdere in generalità, si assume di avere un PRF cartesiano, con l’Asse Z Verticale.

Qui si considerano due varianti del funzionamento del filtro:

A) Variante “senza misure di assetto”: qui si assume di non avere un ingresso aggiuntivo che fornisce (durante l’intervallo temporale di funzionamento del filtro, I) il profilo dell’assetto del velivolo nel PRF (questo profilo potrebbe essere ottenibile, ad esempio, tramite dei sensori di stelle);

B) Variante “con misure di assetto”: qui si assume di avere il suddetto profilo di assetto .

Nel caso della variante A, i passi dell’algoritmo da implementare sono i seguenti (la differenza tra le varianti A e B è che nel caso della variante B si “salta” il secondo passo, essendo il profilo dell’assetto nel PRF già disponibile):

1) Esecuzione dell’algoritmo “standard” (descritto precedentemente);

2) Determinazione del profilo di assetto nel PRF;

3) Calcolo della posizione nel PRF;

4) Calcolo del profilo dello stato del veicolo nel PRF.

Il primo passo ha come obiettivo la stima del profilo dello stato nel FRF.

Nel secondo passo, è necessario determinare la rotazione tra il FRF ed il PRF. Dato che questi due sistemi di riferimento hanno l’asse z comune, è sufficiente determinare l’angolo di rotazione attorno a questo asse, necessario per passare da un RF all’altro.

Questo angolo, indicato qui con α, può essere calcolato partendo dai seguenti valori:

1) La direzione della velocità, nel piano xy del PRF, per un certo tα(anche minore di t0): si tratta di una grandezza tipicamente nota - per es., nel caso di un atterraggio su Marte o sulla Luna, questo valore è ottenibile dai parametri dell’orbita del veicolo (tipicamente noti con un’accuratezza accettabile).

2) La direzione della velocità, nel piano xy del FRF: è calcolabile partendo dall’output del primo passo (ovvero dall’algoritmo “standard”) - se tα< t0, si utilizzano le misure IMU ottenute tra tαe t0

Indicando queste due direzioni delle velocità rispettivamente con θPRFe θFRF, si ha che α è dato semplicemente da:

α = θPRF- θFRF(50)

A questo punto, per determinare la completa trasformazione tra il FRF e il PRF (la quale consentirà la trasformazione del profilo dello stato nel FRF in quello in PRF), è necessario determinare anche la traslazione (espressa come un vettore del PRF) che porta dall’origine del PRF all’origine del FRF. Questo vettore ovviamente coincide con la differenza tra il vettore r(tP), espresso nel PRF, e lo stesso vettore, espresso nel FRF (qui tP∈ I). Dato che i valori di posizione nel FRF sono noti (essendo ottenuti nel primo passo), rimane da calcolare il vettore r(tP) nel PRF. La procedura per farlo è descritta qui di seguito.

Per semplicità di notazione, questo vettore viene in seguito indicato con r.

Confrontando la “mappa” del terreno precaricata con il terreno osservato dalla fotocamera, si identificano alcuni punti sull’immagine per i quali le coordinate nel PRF sono note. Questi punti (nel PRF) verranno indicati con rg1,rg 2,... rgS. Utilizzando la stima dell’assetto nel PRF, è possibile ottenere i versori (nel PRF) associati a ciascuno di questi punti; questi versori verranno indicati qui con n<ˆ>g1,n<ˆ>g 2,... n<ˆ>gS(ciascun versore indica la direzione apparente del corrispondente punto sul terreno).

Ciascuna coppia (rgi, n<ˆ>gi) , per i ∈{1,... S } , definisce una semiretta, i cui punti sono parametrizzabili con li> 0 :

qi(li)=rgi+ n<ˆ>gili(51)

In assenza di errori, tutte queste semirette incrocerebbero a un certo punto r. In altre parole, si avrebbe per ogni i ∈{1,... S } un valore li<*>tale per cui qi( l<*>

i) = r .

Ovviamente, nella pratica è impossibile che queste semirette si incrocino. Per ottenere una stima di r, si segue un approccio ai minimi quadrati, minimizzando il valore di f (r ) definito da: f(<r>) = ∑<2>

i= 1d i(<r>) (52) dovedi(r ) indica la distanza minima tra il punto r e la semiretta i-esima:

di(r)=min|r − q( ) | (53)l> i l i

i0

Il puntoqi( l<r>

i) della semiretta più vicino al punto r è quello per il quale è verificata la condizione:

n<ˆ>gi⋅(r−qi( l<r>

i)) = 0 (54)

e quindil<r>

i=n<ˆT>

gi(r − rgi) ; di conseguenza, si ha che:

di(r)=|r−qi( l<r>

i)|=|r−rgi−n<ˆ>gin<ˆT>

gi(r−rgi)|=|Bi(r − rgi) | (55)

doveBi=I−n<ˆ>gin<ˆ T>

gi. E’ possibile quindi riscrivere l’espressione per d<2>

i( r ) come:

d<2>

i(r)=(Bi(r−rgi))<T>(Bi(r−rgi))=(r−rgi)<T>Ci(r − rgi) (56)

doveCi=B<T>

iBiè una matrice simmetrica. Il gradiente di f(r ) è quindi dato da :

S

<∇>f(<r>)<=>∑<∇>2S

i=1 (d i(<r>))<=>∑i= 12<C>i(<r−r>gi )<=Dr − h>(57) S S

<dove D>=<2>∑ i =1<C i e>h = 2 ∑i=1Ci r gi . Di conseguenza, il valore di r per il quale f(r ) è minimo si ricava tramite l’espressione:

r=D<− 1>h (58) A questo punto, si può calcolare il profilo dello stato nel PRF partendo da quello nel FRF (ottenuto con l’algoritmo “standard”) e operando le seguenti trasformazioni:

• Una rotazione attorno all’asse z, di un angolo α ;

• Una traslazione di r .

Per ridurre ulteriormente l’errore di stima dello stato nel PRF, si può fare la seguente operazione:

1) si calcolano i valori della posizione dei veicolo nel PRF per più istanti temporali di I (invece che solo per tP∈ I).

2) poi, queste stime, indicate qui con r(tP1),r(tP 2),... r( tPH) , vengono elaborate nel modo seguente:

• Per ogni i ∈{1,... H } , si ottiene (dal valore di r(tP i) ), una stima indipendente della traslazione dal PRF al FRF. Calcolando la media delle stime ottenute in questo modo, l’errore complessivo di stima di questa traslazione viene ridotto.

• Analogamente, per un certo istante t* ∈ I , si possono ottenere stime indipendenti di v(t *) (da mediare successivamente) usando coppie di punti (r(tPi), r( tP j)) (dove (i,j) ∈{1,... H } ei ≠ j ) tramite l’espressione :

t Pj t t *

<v>( t*)<1 =>(<r>(tPj)−<r>(tPi)−∫ ∫<a>m(<τ>)d<τ>dt−<1g>(t −t ) 2)+ () ( − *)<(59)>tPj − t 2 Pj Pi mtdt tPjt Pi t Pi t Pi t ∫<a g>

Pi

Claims (10)

1. RIVENDICAZIONI 1. Dispositivo per la stima dello stato di un veicolo in navigazione, costituito da un’unità di misura inerziale (IMU), che rileva le velocità angolari e le accelerazioni di detto veicolo, da un mezzo di acquisizione di immagini (CAM) e da un dispositivo (PROC) di elaborazione delle immagini fotografiche del terreno prese in momenti successivi, identificando una serie di punti caratteristici del terreno, che vengono tracciati sulle diverse immagini, e da un filtro di navigazione (NF) che attraverso un algoritmo opportuno calcola lo stato del veicolo in termini di velocità, posizione, assetto e velocità angolare, in un sistema di riferimento fisso rispetto al terreno, caratterizzato dal fatto: - che il filtro di navigazione per ottenere un versore (U), che indica la direzione dello spostamento del veicolo tra due istanti temporali (T1) e (T2), realizza i seguenti passaggi di calcolo: A) calcola prima un insieme di vettori, (E), i quali in assenza di errori di misura sarebbero tutti ortogonali al detto versore (U), realizzando le seguenti operazioni: A.1) integra le misure di velocità angolare dell'unità di misura inerziale in modo da ottenere il profilo dell'assetto, ovvero l'andamento temporale dell'assetto durante un certo intervallo di tempo, rispetto a un sistema di riferimento fisso rispetto al terreno A.2) utilizza il detto profilo d'assetto per processare l'uscita del dispositivo di elaborazione di immagini, ovvero l'insieme di tracce di pixel dove ogni traccia di pixel descrive il moto apparente di un punto caratteristico del terreno all'interno della sequenza di immagini, con lo scopo di ottenere un insieme (TP) di tracce di versori, dove ogni traccia di versore è una funzione del tempo e descrive il moto dei detto punto caratteristico in un sistema di riferimento fisso rispetto al tempo A.3) l'insieme (E) viene costruito realizzando i seguenti calcoli su ogni membro (KI) di un sottoinsieme di (TP): A.3.1) i valori di (KI) a (T1) e (T2), indicati rispettivamente con (KI1) e (KI2), vengono estratti, A.3.2) il prodotto vettoriale normalizzato di (KI1) e (KI2), indicato con (KCI), viene calcolato, A.3.3) (KCI) viene inserito nell'insieme (E) B) determina, tramite un procedimento ai minimi quadrati, il vettore più vicino alla ortogonalità al detto insieme di vettori (E) e che in tal modo meglio approssima il versore (U) reale ; - che per ottenere il rapporto, (RHO), tra le lunghezze degli spostamenti del veicolo, rispettivamente, da (TA) a (TM), e da (TM) a (TB), dove (TA), (TM) e (TB) sono tre instanti temporali consecutivi, il filtro di navigazione realizza i seguenti calcoli: A) calcola, utilizzando la procedura descritta sopra, i vettori (UA) e (UB), che indicano le direzioni dei spostamenti dei veicoli rispettivamente tra gli istanti temporali (TA) e (TM), e (TM) e (TB) B) per ciascuna traccia di versore (KI) che appartiene al detto insieme (TP), due triangoli (TRA) e (TRB) sono definiti quando possibile, tali per cui: i vertici di (TRA) sono: la posizione del veicolo a (TA), la posizione del veicolo a (TB), e il punto del terreno tracciato da (KI) e i vertici di (TRB) sono: la posizione del veicolo a (TB), la posizione del veicolo a (TC), e il detto punto del terreno tracciato; C) gli angoli rilevanti dei triangoli (TRA) e (TRB) sono calcolati effettuando le seguenti operazioni: a) tre versori, (KIA), (KIM) e (KIB), appartenenti alla detta traccia (KI), e associati rispettivamente agli istanti temporali (TA), (TM) e (TB), sono estratti dalla detta traccia b) viene calcolato l'angolo tra i vettori (KIA) e (UA), il quale è anche l'angolo (ALPHA) del triangolo (TRA) c) viene calcolato l'angolo tra i vettori (KIA) e (KIM), il quale è anche l'angolo (BETA) del triangolo (TRA) d) viene calcolato l'angolo tra l'opposto del vettore (UB) e il vettore (KIB), il quale è anche l'angolo (ALPHA) del triangolo (TRB) e) viene calcolato l'angolo tra i vettori (KIB) e (KIM), il quale è anche l'angolo (BETA) del triangolo (TRB) D) (RHO) viene calcolato moltiplicando il seno dell'angolo (ALPHA) di (TRA) con il seno dell'angolo (BETA) di (TRB), e poi dividendo il prodotto con il prodotto del seno dell'angolo (ALPHA) di (TRB) con il seno dell'angolo (BETA) di (TRA); è possibile combinare stime multiple di (RHO) - che per calcolare la lunghezza dello spostamento del veicolo tra due istanti temporali, il filtro di navigazione elabora i detti versori (U), e il detto rapporto (RHO), e le misure dell’unità di misura inerziale, opportunamente processate, assieme al valore del vettore accelerazione di gravità oppure al valore della norma di tale vettore . 2. Metodo per la stima dello stato di un veicolo in navigazione a partire dalle misurazioni di un’unità di misura inerziale che rileva le velocità angolari e le accelerazioni di detto veicolo, dalla elaborazione delle immagini fotografiche del terreno prese in momenti successivi, identificando su una serie di punti caratteristici del terreno, che vengono tracciati sulle diverse immagini, e che attraverso un algoritmo opportuno calcola lo stato del veicolo in termini di velocità, posizione, assetto e velocità angolare, in un sistema di riferimento fisso rispetto al terreno, caratterizzato dal fatto: - che per ottenere il versore (U), che indica la direzione dello spostamento del veicolo tra due istanti temporali, si calcola prima un insieme di vettori, (E), i quali in assenza di errori di misura sarebbero tutti ortogonali al detto versore (U), e successivamente si determina, tramite un procedimento ai minimi quadrati, il vettore più vicino alla ortogonalità al detto insieme di vettori (E) e che in tal modo meglio approssima il versore (U) reale; - che per ottenere il rapporto, (RHO), tra le lunghezze degli spostamenti del veicolo, rispettivamente, da (TA) a (TM), e da (TM) a (TB), dove (TA), (TM) e (TB) sono tre instanti temporali consecutivi, si calcolano prima gli angoli di due triangoli, aventi rispettivamente i seguenti vertici: a) la posizione del veicolo a (TA), la posizione del veicolo a (TM), e un punto del terreno tracciato sulle immagini, e b) la posizione del veicolo a (TM), la posizione del veicolo a (TB), e il detto punto del terreno tracciato, e successivamente si applica il teorema geometrico dei seni; - che per calcolare la lunghezza dello spostamento del veicolo tra due istanti temporali, si elaborano i detti versori (U), il detto rapporto (RHO), e le misure dell’unità di misura inerziale, opportunamente processate, assieme al valore del vettore accelerazione di gravità oppure al valore della norma di tale vettore. 3. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che per calcolare il profilo di detto stato, ovvero l’andamento temporale dello stato durante un intervallo temporale, viene utilizzato anche un sistema di riferimento ausiliario, il quale coincide, all’istante iniziale di detto intervallo temporale, con il sistema di riferimento fisso rispetto al veicolo. 4. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che il risultato dell’elaborazione delle immagini, composto da un insieme di tracce di pixel, dove ciascuna traccia di pixel descrive il movimento apparente di un punto caratteristico del terreno nella sequenza delle immagini, viene ulteriormente elaborato per ottenere un insieme (TP) di tracce di versori, dove ciascuna traccia di versore, (KI), è una funzione del tempo e descrive il movimento del suddetto punto caratteristico in un sistema di riferimento fisso rispetto al terreno . 5. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che per calcolare il versore, (U), che indica la direzione dello spostamento del veicolo tra due istanti temporali, vengono effettuate le seguenti operazioni: - viene processato l'insieme di tracce (TP), ottenuto elaborando le immagini e le misure dell’unità di misura inerziale, per definire prima un insieme di piani nello spazio, tutti contenenti, in assenza di errori di misura, il detto versore (U), e poi per definire un insieme di vettori, (E), unitari e ortogonali al detto insieme di piani; - si calcola il versore più ortogonale al detto insieme di vettori (E), minimizzando analiticamente la somma dei quadrati dei prodotti scalari con i vettori del detto insieme. 6. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato dal fatto che per calcolare il rapporto, (RHO), tra le lunghezze degli spostamenti del veicolo rispettivamente da (TA) a (TM), e da (TM) a (TB), dove (TA), (TM) e (TB) sono tre instanti temporali per i quali esiste almeno una traccia di pixel (KI) che inizia a (TA) o prima, e finisce a (TB) o dopo, vengono effettuate le seguenti operazioni: - si calcolano le direzioni (UA) e (UB) degli spostamenti del veicolo rispettivamente tra (TA) e (TM), e tra (TM) e (TB); - si definiscono due triangoli, (TRA) e (TRB), aventi rispettivamente i seguenti vertici: a) la posizione del veicolo a (TA), la posizione del veicolo a (TM), e il punto del terreno tracciato dalla detta traccia di pixel (KI), e b) la posizione del veicolo a (TM), la posizione del veicolo a (TB), e il punto del terreno tracciato dalla detta traccia di pixel (KI); - l'insieme delle tracce (TP), ovvero il risultato dell’elaborazione delle immagini e delle misure dell’unità di misura inerziale, viene usato assieme ai vettori (UA) e (UB), per calcolare gli angoli dei detti due triangoli (TRA) e (TRB); - si calcola il detto rapporto, (RHO), tra le lunghezze degli spostamenti, utilizzando i detti angoli dei triangoli (TRA) e (TRB), e applicando il teorema geometrico dei seni; è possibile combinare stime multiple di (RHO). 7. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che per calcolare la velocità del veicolo in un certo istante temporale (TA), e il vettore accelerazione di gravità, vengono effettuate le seguenti operazioni: - si utilizzano altri due istanti temporali, (TM) e (TB), per calcolare i detti versori (UA) e (UB), e il rapporto (RHO); - si utilizzano i valori calcolati al passo precedente, assieme alle misure dell’unità di misura inerziale, opportunamente processate, per calcolare il vettore (DELTA_R) che indica lo spostamento dei veicolo da (TA) a (TM); - se il vettore gravità non è noto, ma è nota solamente la sua norma, allora partendo dal detto vettore(DELTA_R), e dal valore della norma del vettore gravità, e da altri valori ottenuti precedentemente, si determina il vettore gravità; - partendo dal detto vettore gravità, e da altri valori ottenuti precedentemente, si calcola la velocità nell’istante temporale (TA). 8. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che, utilizzando anche le misure di un altimetro, allora si calcola lo spostamento tra due istanti temporali, (DELTA_R), effettuando le seguenti operazioni: - si calcola la direzione, (U), del detto vettore spostamento; - si utilizzano le misure dell’altimetro per calcolare la proiezione del detto vettore spostamento lungo la direzione di misura dell’altimetro; - si utilizzano i valori della detta direzione (U), della detta direzione di misura dell’altimetro, e della detta proiezione, per calcolare la lunghezza del detto spostamento (DELTA_R). 9. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che, ricevendo anche le misure di un sensore LIDAR, a scansione o flash, allora si calcola lo spostamento, (DELTA_R), tra due istanti temporali, effettuando le seguenti operazioni: - si calcola la direzione, (U), del detto vettore spostamento, utilizzando il procedimento descritto nella rivendicazione 5; - utilizzando la detta direzione, si determina la lunghezza del detto spostamento massimizzando la sovrapposizione tra i punti del terreno misurati con le scansioni LIDAR ottenute nei detti due istanti temporali. 10. Metodo per la stima dello stato di un veicolo in navigazione secondo la rivendicazione 2, caratterizzato da fatto che utilizzando come dati in ingresso anche una mappa del terreno osservato, che include anche un’ortoimmagine, ovvero una matrice di pixel, riferita a un sistema di riferimento planetocentrico, che indica l’aspetto del terreno osservato, allora si effettuano le seguenti operazioni per calcolare il profilo dello stato del veicolo nel detto sistema di riferimento: - viene fatto un confronto tra la detta ortoimmagine e le immagini osservate dal veicolo, per ottenere un insieme di punti del terreno, dove per ogni punto di tale insieme sono disponibili sia le coordinate planetocentriche, che le coordinate che indicano la posizione apparente del punto su una o più immagini acquisite; - per ciascuna detta immagine, si effettua una elaborazione delle coordinate sull’immagine di detti punti, per ottenere un insieme di versori che indicano le direzioni in cui si trovano detti punti, rispetto al veicolo, nel detto sistema di riferimento planetocentrico ; - si calcola la posizione del veicolo nel detto sistema di riferimento planetocentrico, definendo una semiretta per ogni coppia di detti punto e versore, e poi definendo una funzione che, per ogni punto dello spazio tridimensionale, valuta la somma delle distanze al quadrato, tra detto punto dello spazio e le semirette precedentemente definite, e infine minimizza la detta funzione calcolando, tramite un procedimento analitico, il punto per il quale il gradiente di detta funzione è nullo. 1. Device to estimate the state of a moving vehicle, composed of an inertial measurement unit (IMU), measuring the angular velocities and the accelerations of said vehicle, by a camera (CAM) and by a device (PROC) for the processing of the terrain photographic images taken at successive instants, identifying a set of characteristic points of the terrain, which are tracked on the different images, and by a navigation filter (NF) which using a suitable algorithm calculates the state of the vehicle in terms of velocity, position, attitude and angular velocity, in a reference frame fixed with respect to the ground, characterized in that: - the navigation filter, in order to get a unit vector (U), indicating the direction of the displacement of the vehicle between two time instants (T1) and (T2), performs the following two computing steps: A) calculates at first a set of vectors, (E), which in case of no measurement errors would be all orthogonal to the said unit vector (U), by performing the following operations: A.1) integrates the angular velocity measurements of the inertial measurement unit in order to obtain the profile of the attitude, i.e. the temporal behaviour of the attitude during a time interval, with respect to a reference frame fixed with respect to the ground A.2) utilizes the said attitude profile in order to process the output of the image processing device, i.e. the set of pixel tracks where each pixel track describes the apparent motion of a characteristic point of the terrain in the sequence of the images, in order to obtain a set (TP) of tracks of unit vectors, where each unit vector track is a function of time and describes the motion of said characteristic point in a reference frame fixed with respect to the ground A.3) the set (E) is constructed by performing the following computations on each member (KI) of a subset of (TP): A.3.1) the values of (KI) at (T1) and (T2), indicated respectively with (KI1) and (KI2), are extracted, A.3.2) the normalized vector cross product of (KI1) and (KI2), indicated with (KCI), is computed, A.3.3) (KCI) is inserted in the set (E) B) determines, through the method of the least squares, the vector closer to the perpendicular to the said set of vectors (E) and that in this way better approximates the actual unit vector (U); - to obtain the ratio, (RHO), between the lengths of the displacements of the vehicle, respectively in the timeframe from (TA) to (TM) and from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants, the navigation filter performs the following computations: A) computes, by using the procedure described above, the vectors (UA) and (UB), that indicate the directions of the displacements of the vehicle respectively between time instants (TA) and (TM), and (TM) and (TB) B) for each unit vector track (KI) that is a member of the said set (TP), two triangles, (TRA) and (TRB), are defined when possible, such that the vertices of (TRA) are: the position of the vehicle at time (TA), the position of the vehicle at time (TM), and the point on the terrain tracked by (KI) and the vertices of (TRB) are: the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the said tracked point of the terrain, C) the relevant angles of the triangles (TRA) and (TRB) are computed by performing the following operations: a) three unit vectors, (KIA), (KIM) and (KIB), belonging to the said pixel track (KI), and associated respectively to the time instants (TA), (TM) and (TB), are extracted from the said pixel track b) the angle between vectors (KIA) and (UA), which is also the angle (ALPHA) of the triangle (TRA), is computed, c) the angle between vectors (KIA) and (KIM), which is also the angle (BETA) of the triangle (TRA), is computed, d) the angle between the opposite of the vector (UB), and the vector (KIB), is computed, which is also the angle (ALPHA) of the triangle (TRB), is computed, e) the angle between vectors (KIB) and (KIM), which is also the angle (BETA) of the triangle (TRB), is computed, D) (RHO) is then computed by multiplying the sine of the angle (ALPHA) of (TRA) with the sine of the angle (BETA) of (TRB), and then dividing that product with the product of the sine of the angle (ALPHA) of (TRB) with the sine of the angle (BETA) of (TRA); multiple estimates of (RHO) can be combined - to calculate the length of the displacement of the vehicle between two time instants, the navigation filter processes the said unit vectors (U), the said ratio (RHO), and the measurements of the inertial measurement unit, conveniently treated, together with the value of the gravity acceleration vector or the value of the norm of such vector.
2. 2. Method to estimate the state of a moving vehicle, on the base of the measurements of an inertial measurement unit, sensing the angular velocities and the accelerations of said vehicle, and of the processing of the photographic images of the terrain taken at successive instants, identifying a series of characteristic points of the terrain, which are tracked on the different images, and that by a suitable algorithm calculates the state of the vehicle in terms of velocity, position, attitude and angular velocity, in a reference frame fixed with respect to the ground and characterized in that: - in order to get the unit vector (U), indicating the direction of the displacement of the vehicle between two time instants, calculates at first a set of vectors, (E), which in case of no measurement errors would be orthogonal to the said unit vector (U), and then determines, through the method of the least squares, the vector closer to the perpendicular to the said set of vectors (E) and that in this way better approximates the actual unit vector (U); - to obtain the ratio, (RHO), between the lengths of the displacements of the vehicle, respectively in the timeframe from (TA) to (TM) and from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants, at first the angles of two triangles are calculated, having respectively the following vertices: a) the position of the vehicle at time (TA), the position of the vehicle at time (TM), and one of the points on the terrain tracked in the images, and b) the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the said tracked point of the terrain, and then the triangle law of sines is applied; - to calculate the length of the displacement of the vehicle between two time instants, the said unit vectors (U), the said ratio (RHO), and the measurements of the inertial measurement unit, conveniently treated, are processed together with the value of the gravity acceleration vector or the value of the norm of such vector.
3. 3. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that to calculate the profile of said state, i.e. the temporal behaviour of the state during a time interval, an auxiliary reference system is also used, that coincides, at the initial time of said time interval, with the reference system fixed with respect to the vehicle.
4. 4. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that the result of the processing of the images, composed of a set of pixel tracks, where each pixel track describes the apparent motion of a characteristic point of the terrain in the sequence of the images, is further processed in order to obtain a set (TP) of tracks of unit vectors, where each unit vector track, (KI), is a function of time and describes the motion of said characteristic point in a reference frame fixed with respect to the ground.
5. 5. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the unit vector, (U), indicating the direction of the displacement of the vehicle between two time instants, the following operations are performed: - the set of tracks, (TP), obtained by processing the images and the measurements of the inertial measurement unit is processed to define at first a set of planes in space, all containing, in case of no measurement errors, the said unit vector (U), and then to define a set of unit vectors, (E), orthogonal to the said set of planes. - the unit vector closer to the perpendicular to the said set of vectors (E) is calculated, analytically minimizing the sum of the squares of the scalar products with the vectors of said set.
6. 6. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the ratio, (RHO), between the lengths of the displacements of the vehicle respectively in the timeframe from (TA) to (TM) and in the timeframe from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants for which at least one pixel track exists (KI) that starts at (TA) or before, and ends at (TB) of after, the following operations are performed: - the directions (UA) and (UB) of the displacements of the vehicle respectively between time (TA) and (TM), and between (TM) and (TB) are calculated; - two triangles, (TRA) and (TRB), are defined having respectively the following vertices: a) the position of the vehicle at time (TA), the position of the vehicle at time (TM), and the point of the terrain tracked in the said pixel track (KI), and b) the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the point of the terrain tracked in the said pixel track (KI); - the set of tracks, (TP), obtained by processing the images and the measurements of the inertial measurement unit, is used together with the vectors (UA) and (UB), to calculate the angles of said triangles (TRA) and (TRB) - the said ratio, (RHO), between the lengths of the displacements, is calculated, by using the said angles of the triangles (TRA) and (TRA), and applying the triangle law of sines; multiple estimates of (RHO) can be combined
7. 7. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the velocity of the vehicle in a given time instant (TA), and the gravity acceleration vector, the following operations are performed: - other two time instants, (TM) and (TB), are used to calculate the said unit vectors (UA) and (UB) and the ratio (RHO); - the values calculated at the previous step are used, together with the measurements of the inertial measurement unit, conveniently processed, to calculate the vector (DELTA_R) representing the displacement of the vehicle from (TA) to (TM); - if the gravity acceleration vector is not known, but only its norm is known, then from the said vector (DELTA_R), and from the value of the gravity acceleration norm, and from other values previously obtained, the gravity acceleration vector is determined; - from said gravity acceleration vector, and from others values previously obtained, the velocity at the time instant (TA) is calculated.
8. 8. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, using also the measurements of an altimeter, then the displacement, (DELTA_R), between two time instants, is calculated by performing the following operations: - the direction, (U), of the said displacement vector is calculated; - the measurements of the altimeter are used to calculate the projection of the said displacement vector along the direction of the measurement of the altimeter; - the values of the said direction (U), of the said direction of measurement of the altimeter, and of the said projection are used to calculate the length of the said displacement (DELTA_R).
9. 9. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, having available also the measurements of a LIDAR sensor, of the scanning type or flash type, then the displacement, (DELTA_R), between two time instants is calculated by performing the following operations: - the direction, (U), of the said displacement vector is calculated using the method described in Claim 5; - using said direction, the length of said displacement is determined by maximizing the overlapping between the points of the terrain measured with the LIDAR scans in the said two time instants.
10. 10. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, using as input data also a map of the observed terrain, including also an orthoimage, i.e. a pixel matrix, referred to a planeto-centric reference system, showing the aspect of the observed terrain, then the following operations are performed to calculate the profile of the state of the vehicle in the said reference system: - a comparison is done between the said orthoimage and the images observed by the vehicle, to obtain a set of points of the terrain, where for each point of said set, both the planetocentric coordinates and the coordinates giving the apparent position of the point on one or more of the acquired images are available; - for each of said images, a processing of the coordinates of said points on the image is performed, to obtain a set of unit vectors which give the directions of the position of said points with respect to the vehicle, in the said planetocentric reference frame; - the position of the vehicle in the said planetocentric reference system is calculated, defining a half line for each pair of said point and unit vector, and then defining a function that, for each point in the three-dimensional space, calculates the sum of the squared distances between said point in space and the half lines previously defined, and finally minimizes the said function calculating, with an analytical method, the point for which the gradient of said function is null.