politecnico di torino - areeweb.polito.it · il filtro di kalman porta il nome del suo inventore,...
TRANSCRIPT
0
POLITECNICO DI TORINO
Corso di Laurea Magistrale in Ingegneria Informatica
(Computer Engineering)
Tesina di Intelligenza Artificiale
Sensor fusion mediante filtro Bayesiano (Kalman Filter)
Professore: Studente:
Elio Piccolo Valeria Mangiapane
2016
2
1. Introduzione
1.1 La robotica
La robotica è la scienza che ci permette di percepire e manipolare il mondo fisico
attraverso dispositivi meccanici controllati dal computer. Esempi di sistemi robotici di
successo includono piattaforme mobili per l'esplorazione planetaria, bracci robotici nelle
linee di montaggio, auto che viaggiano in modo autonomo sulle autostrade, braccia che
assistono i chirurghi.
Questi sistemi di robotica hanno in comune il fatto di trovarsi nel mondo fisico, di percepire
l’ambiente tramite sensori, e di manipolare il loro ambiente attraverso pezzi meccanici che
si muovono.
Mentre gran parte della robotica è ancora nella sua infanzia, l'idea di dispositivi per la
manipolazione "intelligente" ha un enorme potenziale per cambiare la società. Non
sarebbe bello se tutte le nostre macchine fossero in grado di guidarsi in maniera
autonoma e sicura, rendendo gli incidenti stradali un ricordo del passato?
Non sarebbe bello se i robot, e non le persone, fossero in grado di ripulire i disastri
nucleari come Chernobyl?
Inoltre, una migliore comprensione robotica alla fine porterà a una migliore comprensione
del mondo in cui viviamo.
La caratteristica più sorprendente dei nuovi sistemi robot è che essi operano in ambienti
sempre meno strutturati, ambienti che sono intrinsecamente imprevedibili. Una linea di
assemblaggio è ordini di grandezza più prevedibile e controllabile di una casa privata. Di
conseguenza, la robotica si sta muovendo in aree in cui l’introduzione del sensore diventa
sempre più importante, e in cui il software del robot deve essere sufficientemente robusto
per far fronte a una serie di situazioni, spesso troppe per poterle anticipare tutte.
1. Ambiente: il mondo fisico è intrinsecamente imprevedibile. Mentre il grado di incertezza
in ambienti ben strutturati, quali le linee di montaggio, è di piccole dimensioni, ambienti,
come ad esempio autostrade e case private, sono altamente dinamici ed imprevedibili.
2. Sensore: i sensori sono intrinsecamente limitati in ciò che possono percepire.
Limitazioni che derivano da due fattori principali. Innanzitutto, la portata e la risoluzione del
sensore sono soggetti alle leggi fisiche. Ad esempio, le macchine fotografiche non
possono vedere attraverso i muri, ed anche all'interno dell’intervallo percettivo la
risoluzione spaziale delle immagini della telecamera è limitata . In secondo luogo, i sensori
3
sono soggetti a rumore, che perturba le misure dei sensori in modo imprevedibile e quindi
limita le informazioni che possono essere estratte dalle misure dei sensori.
3. Robot: l’attuazione del robot comprende motori che sono, almeno in una certa misura,
imprevedibili, a causa di effetti come il rumore di controllo, l'usura e la lacerazione . Alcuni
attuatori, come quelli dei bracci robotici industriali, sono abbastanza accurati. Altri, quelli a
basso costo che si usano per i robot mobili, possono essere estremamente imprecisi.
4. Modelli: i modelli sono intrinsecamente imprecisi. I modelli sono astrazioni del mondo
reale. Come tali, essi modellano solo in parte i processi fisici sottostanti il robot e il suo
ambiente. Gli errori di modello sono una fonte di incertezza che è stata largamente
ignorata nel campo della robotica, nonostante il fatto che la maggior parte dei modelli
robotici utilizzati nello stato dell’arte sono piuttosto grezzi.
1.2 Sensor fusion
Un animale è in grado di riconoscere l’ambiente che lo circonda grazie ai segnali che
provengono da diversi sensori. La natura ha trovato un modo per integrare queste
informazioni che provengono da più fonti. Anche in caso di mancanza di un sensore, i
sistemi sono in grado di compensare l’assenza di tali informazioni riutilizzando i dati
ottenuti da sensori con campo di applicazione sovrapposto. Gli umani ad esempio
combinano i segnali che ricevono dai cinque sensi (vista, udito, tatto, gusto, olfatto) con la
conoscenza dell’ambiente per creare ed aggiornare un modello dinamico del mondo.
Basandosi su queste informazioni l’uomo è in grado di interagire con il mondo e di
prendere decisioni per il presente e per le azioni future. Questa abilità di sensor fusion al
giorno d’oggi viene applicata anche in aree tecniche quali la robotica.
I robot sono spesso equipaggiati con diversi sensori che ci permettono di fare differenti
osservazioni. Spesso è utile che queste osservazioni vengano integrate tra loro per avere
una conoscenza più robusta e completa dell’ambiente circostante ma anche della
macchina stessa. Ciò permette ovviamente di intraprendere, ad esempio, strategie di
controllo più mirate al miglioramento delle prestazioni. Questo processo prende il nome di
sensor fusion ed è attualmente uno dei temi caldi nel mondo della robotica, in quanto trova
una larga applicazione in diverse aree della disciplina. I sistemi di sensor fusion sono
spesso combinazioni complesse di dispositivi e di algoritmi per la fusione stessa ed il
processamento dei dati che si tirano fuori.
4
Figura 1: Diagramma a blocchi della sensor fusion e dell'integrazione multi-sensore
I sistemi che utilizzano tecniche di sensor fusion si aspettano di ricavare molti più benefici
di quelli che si hanno utilizzando sensori singoli. La misura di un sensore presenta un
errore generalmente dovuto a diverse problematiche ovvero:
Mancanza del sensore: la rottura di un componente del sensore comporta una perdita di
percezione sull’oggetto desiderato;
Limitata copertura spaziale: di solito un singolo sensore copre solo una limitata regione.
Ad esempio una lettura dal termometro di una caldaia fornisce solo una stima della
temperatura registrata in prossimità del termometro e può fallire nella stima della
temperatura media dell'acqua all’interno della caldaia;
Limitata copertura temporale: alcuni sensori hanno bisogno di un particolare momento
di set-up per eseguire e trasmettere una misurazione, limitando così la frequenza
massima delle misurazioni;
Imprecisione: le misure dei singoli sensori sono limitati alla precisione dell'elemento
sensibile impiegato.
5
2. Robotica probabilistica
2.1 Perché probabilistica?
Quando si parla di robotica bisogna tener conto della presenza di una forte componente di
incertezza. Questa deriva essenzialmente da quattro fattori quali:
1. L’ambiente è stocastico, impredicibile;
2. Le azioni compiute dai robot sono stocastiche;
3. I sensori sono limitati e rumorosi;
4. I modelli sono inaccurati ed incompleti.
L’idea di base è quella di rappresentare esplicitamente l’ incertezza utilizzando la teoria
del calcolo della probabilità. La percezione corrisponde alla stima dello stato mentre
l’azione equivale all’ottimizzazione.
I robot probabilistici presentano diversi vantaggi in quanto possono compensare modelli
inaccurati e sensori non proprio perfetti e risultano robusti nelle applicazioni del mondo
reale. Non mancano comunque le insidie dal momento che risultano computazionalmente
impegnativi e non mancano ovviamente delle approssimazioni.
INCERTEZZA -> non conoscere in maniera chiara lo stato distingue la conoscenza
deduttiva dalla credenza induttiva.
Le fonti di incertezza sono:
Ignoranza;
Complessità;
Casualità fisica;
Indeterminatezza.
6
2.2 Teoria di Bayes
La probabilità condizionata ha diverse applicazioni:
Probabilità totale
Regola di Bayes o probabilità a posteriori
ALGORITMO:
7
La regola di Bayes ci permette di calcolare probabilità che sarebbero difficili da ricavare
altrimenti. Ma Bayes ci aiuta anche a stimare lo stato di sistemi dinamici grazie ai
cosiddetti filtri Bayesiani (strumenti probabilistici).
3. Filtri Bayesiani
3.1 La struttura
Dati:
Il flusso di osservazioni z e l’ingresso u
Il modello del sensore P(z|x);
Il modello dell’azione P(x|u, x’);
La probabilità a priori dello stato del sistema P(x).
Ciò che si vuole ottenere è la stima dello stato X del sistema dinamico. La probabilità a
posteriori dello stato viene anche chiamato BELIEF:
Adesso calcoliamo questa probabilità a posteriori
8
Da cui si ricava una formula importantissima
Questa formula è il fondamento di diversi metodi, tra cui il filtro di Kalman.
3.2 Filtro di Kalman
3.2.1 La storia
Il filtro di Kalman porta il nome del suo inventore, l'ingegnere elettrico e matematico Rudolf
Emil Kalman che pubblicò un primo approccio a questa tecnica nel 1960, sebbene in
precedenza altri matematici svilupparono algoritmi simili.
Durante la sua visita al NASA Ames Reserch Center di Kalman, Schmidt vide la possibilità
di applicare l'idea di Kalman sul programma Apollo che in quegli anni si stava sviluppando.
Di fatto, il filtro di Kalman venne incorporato sul computer di navigazione di ogni navicella
del programma Apollo.
Dopo questa prima applicazione, il filtro di Kalman ebbe un ruolo vitale in campo militare,
nell'implementazione dei sistemi di navigazione dei missili a testata nucleare dei
sottomarini balistici di flotta US Navy, dei sistemi di guida e navigazione dei missili
Tomahawk e Air Launched Cruis Missile. In ambito civile è stato ed è tuttora utilizzato per
il controllo di guida e navigazione dei Space Shuttle NASA e nel attitude control e di
navigazione della stazione orbitante ISS.
Per questa trattazione, Rudolf Emil Kalman è stato insignito della medaglia National Medal
of Science direttamente dal presidente americano in carica Barak Obama nel 2008.
9
3.2.2 Di cosa si tratta
Il filtro di Kalman può essere considerato come computazione ricorsiva ottimale dell’ al-
goritmo dei minimi quadrati. Si tratta di un sottoinsieme dei filtri di Bayes in cui vengono
imposte due ipotesi ovvero quella di una distribuzione gaussiana e quella che dice che lo
stato attuale è linearmente dipendente dallo stato precedente. In altre parole, il filtro di
Kalman è essenzialmente un insieme di equazioni matematiche che implementano uno
stimatore di tipo predittore - correttore che è ottimale dal momento che minimizza la
covarianza dell'errore stimata quando la condizione di linearità gaussiana del sistema è
soddisfatta. Se l'assunzione gaussiana è rilassata, il filtro di Kalman è ancora il migliore
(minimo errore di varianza) filtro nella classe di filtri lineari.
Dal momento in cui è stato sviluppato, il filtro Kalman è stato oggetto di approfondite
ricerche ed applicazioni, soprattutto nel campo della navigazione. La semplice e robusta
natura di questo algoritmo ricorsivo l’ha reso particolarmente attraente. Inoltre, anche se è
raro che esistano le condizioni ottimali in applicazioni reali, a dispetto di questo, il filtro
funziona abbastanza bene solitamente e ciò lo rende ancora più affascinante.
3.2.2 Trattazione matematica
Il filtro di Kalman non è propriamente un filtro ma è piuttosto un algoritmo ricorsivo che
permette di stimare lo stato di un sistema dinamico lineare, e nella versione più estesa
anche non lineare, a partire da misure incomplete e perturbate da rumore.
La fusione dei dati provenienti da sensori diversi ci permette di migliorare la stima delle
variabili di stato del sistema dinamico.
Dato un sistema come quello in figura 2:
Figura 2: Schema a blocchi del sistema
Le equazioni che lo caratterizzano sono:
�̇� = 𝐴𝑥 + 𝐵𝑢 + 𝑤
10
𝑦 = 𝐶𝑥 + 𝑛 dove:
𝑥 = 𝑛𝑥1 vettore di stato;
𝑦 = 𝑚𝑥1 vettore di misura;
𝑤 = 𝑛𝑥1 rumore di processo (disturbo del sistema);
𝑛 = 𝑚𝑥1 rumore di misura;
𝑤 e 𝑛 sono rumori gaussiani bianchi.
La sfida è dunque quella di trovare una stima 𝑥 del vettore di stato 𝑥 a partire da misure
incomplete e rumorose.
L’equazione del filtro di Kalman è la seguente:
�̇� = 𝐴𝑥 + 𝐵𝑢 + 𝐿(𝑦 − 𝐶𝑥)
L è una matrice 𝑛𝑥𝑚 e rappresenta il guadagno del filtro. Esso garantisce che la stima
dell’ errore converga a zero
𝑒 = 𝑥 − 𝑥
lim𝑡→ ∞
𝑒(𝑡) → 0
Si noti che 𝑤 e 𝑛 non sono inclusi in quanto i valori istantanei sono sconosciuti.
Figura 3: Kalman filtering
11
3.2.3 L’algoritmo
L'algoritmo, che consiste nello stimare un processo rumoroso in base ad uno stato
passato ed ottenere un feedback sotto forma di misura affetta da rumore, è costituito da
una sequenza di operazioni da eseguirsi ricorsivamente.
Queste operazioni si dividono in due gruppi: equazioni di time update (previsione) ed
equazioni di measurement update (correzione).
Nel dettaglio, i passi svolti dalle equazioni di time update sono:
Stima a priori di stato: lo stato attuale viene stimato sulla base dello stato all'istante
precedente e degli ingressi.
Calcolo a priori della covarianza di errore di stima.
I passi considerati nelle equazioni di measurement update sono:
Calcolo del guadagno di Kalman.
Calcolo a posteriori dello stato attuale sulla base della stima a priori e della misura.
Calcolo della covarianza a posteriori.
In figura 4 viene mostrato il susseguirsi delle fasi.
Figura 4: Susseguirsi delle fasi
I pedici indicano:
_ apri = valore calcolato a priori della nuova misura.
_ pos = posteriori.
_ measure = misurata.
12
Q = matrice di covarianza relativa a 𝑤.
R = matrice di covarianza relativa a 𝑛.
La scelta di Q ed R impone una spiegazione più accurata: queste due varianze riguardano
la conoscenza del processo che si sta analizzando e del sistema di misura. La regolazione
di questi parametri può incidere sull' efficienza del filtro e tipicamente è effettuata
avvalendosi di altri filtri di Kalman o comunque all'esterno dell'algoritmo che stima lo stato
del processo.
Si può comunque dimostrare che, scelti opportunamente i valori di Q ed R nell'ambito in
cui questi rimangano costanti, sia la covarianza di stima P che il guadagno di Kalman K si
stabilizzano in un certo numero di cicli per poi rimanere costanti.
3.2.4 Implementazione in Matlab
Per testare il funzionamento dell'algoritmo si è effettuata una semplice implementazione in
linguaggio Matlab che a partire dalle misure ricavate da 3 diversi sensori mostra come sia
possibile migliorare la stima dello stato del sistema.
Figura 5: Sensor Fusion con filtro di Kalman
Nel nostro sistema la variabile di stato è la temperatura dell’aria. Assumiamo che la
variazione della temperatura, ovvero la derivata prima del vettore di stato, possa essere
modellata come un processo gaussiano. Per quanto riguarda le misure dei sensori si ha:
𝑦1 = 𝑥 + 𝑛1
𝑦2 = 𝑥 + 𝑛2
𝑦3 = 𝑥 + 𝑛3
Concentriamoci adesso sulle matrici:
A = 1;
13
B = 0;
𝐶 = [ 11 1
];
Q = 0.005;
𝑅 = [0.64 0 0
0 0.64 00 0 0.64
];
La matrice L risulta pari al prodotto 𝑆 𝐶𝑇 𝑅−1 con S matrice positiva che si ricava dalla
equazione di Riccati.
Lanciando il file ‘fusion.m’ si ottengono tre grafici che riportiamo qui per comodità.
In questo primo plot riportiamo in nero il segnale di uno dei sensori, che risulta disturbato,
e in rosso la stima dello stato, la quale risulta ripulita dal rumore nonostante sia ricavata
dal primo segnale.
Qui è possibile notare che il filtro di kalman riduce notevolmente il rumore, ma non elimina
l’effetto dovuto al bias.
14
Infine, in quest’ ultimo grafico è possibile osservare come la sensor fusion, implementata
mediante il filtro di Kalman, abbia migliorato significativamente la stima dello stato,
riducendo l’ effetto di bias e il rumore dei sensori.
4. Conclusioni
Il filtro di Kalman è il filtro Bayesiano più utilizzato per svariati motivi. Innanzitutto si tratta
di un concetto molto semplice ed intuitivo e ciò permette di ottenere risultati con una
buona efficienza computazionale. Tuttavia, detto questo, la sua implementazione richiede
una certa arte affinché sia corretta e molto dipende dal sistema che si ha sotto mano.
Ci sono anche molte varianti per il filtro di Kalman (e più in generale, per i filtri bayesiani)
con i rispettivi vantaggi e svantaggi. Non c'è da meravigliarsi che il campo sia così ben
esplorato e ancora in fase di ulteriori sviluppi.