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

1

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.