Progettazione di Sistemi di Controllo

Localizzazione e SLAM con Wireless Sensors Network

(WSN)

Desdemona Hoxhaj , Nicola Mazzucato, Maurizio Montis , Marco Sommacal

Localizzazione e SLAM con Wireless Sensors Network Localizzazione SLAM

Argomenti del Progetto:

il nodo mobile posto in un ambiente totalmente sconosciuto e privo di conoscenze sulla propria posizione e orientamento cerca di costruire una mappa verosimile di tale ambiente e determinare la propria posizione all’interno della mappa stessa.

Il nodo mobile, conoscendo in modo esatto la propria posizione e orientamento in ogni punto dell’ambiente cerca di stimare nel modo più preciso possibile il numero e la posizione dei nodi ancora presenti.

Storia dello Slam:

conferenza dell’IEEE chiamata ”Robotica eAutomatica” (San Francisco).

Origini del problema:

1986

i metodi probabilistici vennero per la prima volta applicati alla robotica e all’intelligenza artificiale.

Punto cruciale della teoria: necessità che vi sia una forte correlazione tra le stime della posizione dei diversi oggetti

dell’ambiente

Storia dello Slam:Primi risultati raggiunti

Ayache e Faugeras

Crowley, Chatila e Laumond

Risultato teorico

Smith, Cheesman e Durrant-Whyte

Risultati applicativi

Navigazione mediante visione

Navigazione di robot tramite

sonar e filtro di Kalman

Basi statistiche per la descrizione

dell’ambiente

Storia dello Slam:

Conseguenze

Una soluzione completa richiedeva uno stato composto da

Posizione del veicolo mobile

Posizione dell’ambiente

circostante

Lo stimatore elabora un vettore di grandi dimensioni

Onere di calcolo pari al quadrato dei punti di riferimento fissi

Storia dello Slam:La scelta non teneva conto della proprietà di convergenza

Introduzione di alcune approssimazioni

Il problema dello SLAM si scisse in due problemi distinti:

minimazzare e/o eliminare le correlazioni tra riferimenti

problema di localizzazione del veicolo mobile; problema di mappatura dell’ambiente.

Storia dello Slam:

Ulteriori studi dimostrarono che:

il problema composto verifica la proprietà di convergenza; la correlazione era una parte critica del problema.

Coniatura del termine SLAM

Slam probabilistico:

Problema di simultanea localizzazione e mappatura

richiede la distribuzione di probabilità:

descrive la densità congiunta a posteriori

del veicolo mobile; dei nodi ancora.

Necessito di avere: l’ingresso al passo k; l’osservazione al passo k

Slam probabilistico:Si cerca di determinare una soluzione ricorsiva del problema

Partendo da una stima per la distribuzione al passo k-1

la joint posterior density può essere calcolata tramite il Teorema di Bayes

Condizioni per applicare il Teorema di Bayes:

Slam probabilistico:

modello di osservazione:

modello di transizione:

Condizioni per applicare il Teorema di Bayes:

Probabilità di effettuare un’osservazione zk quando il veicolo mobile e la posizionedel nodo ancora sono conosciute

lo stato di transizione può esseredescritto attraverso un processo di Markov

Proprietà di indipendenza condizionale delle osservazioni

Slam probabilistico:

Time update:

Measurement update:

Forma ricorsiva dell’algoritmo di SLAM

Slam probabilistico:

Dipendenza tra le osservazioni e le posizioni dei nodi fissi e mobile;

Osservazioni sullo SLAM probabilistico:

l’errore tra valori stimati e valori reali delle posizioni dei sensori presenta un andamento comune e omogeneo;

gli errori nelle stime delle posizioni dei sensori risultano fortemente correlati;

Modello dinamico del sistema:

Sistema non lineare:

Modello dinamico del sistema:Variabili di stato:

Modello dinamico del sistema:Variabili di stato:

Modello dinamico del sistema:Scelta dei parametri del robot mobile:

y

xO

ys

Xs

fs

S

Modello dinamico del sistema:Variabili di ingresso:

Variabili di uscita:

Accelerazione del robot:

Segnali di potenza dei sensori fissi:

Caratteristiche cinematiche adottate per il veicolo mobile:

Scelta del movimento: Spostamento per passi; Tempo di spostamento legato al

tempo di campionamento;

Caratteristiche cinematiche adottate per il veicolo mobile:

Profilo di accelerazione

Caratteristiche cinematiche adottate per il veicolo mobile:

Profilo di accelerazione

Profilo di velocità

Modello dinamico del sistema:

Rumore di Processo

Rumore di Misura

Rumore del sistema:

Modello del rumore

Modello dinamico del sistema:Equazione di stato:

Modello dinamico del sistema:Matrice Q: Matrice f:

Modello dinamico del sistema:Equazione di uscita:

Valori numerici: PTX = 0A = 18.2 np = 2.18 var( )c = 6.03

Discretizzazione del modello:

Il filtro di KalmanCos’è?

Un insieme di equazioni matematiche

Cosa rappresenta?Un metodo computazionale per stimare lo stato di un processo in modo da minimizzare l’errore quadratico medio.

Il filtro di Kalman

Le equazioni del filtro si dividono in:

•predizione: responsabili della previsione dello stato attuale e della covarianza dell’errore e permettono di ottenere una stima a priori dello stato del sistema.

•aggiornamento misura: governano il feedback e vengono impiegate per correggere con una nuova misurazione la stima a priori fatta al passo precedente, così da ottenere una stima a posteriori migliore.

Il filtro di Kalman

Il filtro stima lo stato di un processo x(k) all’istante (k+1):

Ingresso del processo al

k-esimo istanteRumore di processo modellato

come una gaussiana

Il filtro di Kalman

Rumore di misura modellato come una gaussiana

La stima viene fatta attraverso le misurazioni:

Il filtro di Kalman‘’Comportamento della stima’’

Stima a priori Stima a posteriori

Errore di stima a priori Errore di stima a posteriori

Varianza errore di stima a priori

Varianza errore di stima a posteriori

Algoritmo ricorsivo per il calcolo degli stimatori lineari a minima varianza

Il filtro di Kalman

Stime a priori

Stime a posteriori

Condizioni iniziali

S: correlazione tra rumori di processo e di

misura

La varianza del processo di innovazione e(k):

Il filtro di Kalman

La varianza del rumore bianco :

Il guadagno del filtro di Kalman:

La matrice F:

Il filtro di Kalman

Il filtro di Kalman esteso

Quando sostituisce il Kalman ‘’ordinario’’?

Quando il processo da stimare e/o la relazione tra la misura e il processo non è lineare.

Qual’è il suo principio di funzionamento?

Esso esegue una linearizzazione ad ogni istante di campionamento attorno alla migliore stima disponibile in quel momento.

Il filtro di Kalman estesoDato un processo con stato governato da:

E equazione di osservazione non lineare:

Rumore di processo Rumore di misura

Il filtro di Kalman estesoSi possono scrivere le nuove equazioni che linearizzano la

stima:

Stato e osservazione al passo attuale

Stato e osservazione approssimati

Stima a posteriori dello stato al

passo k

Rumore di processo e di

misura

Il filtro di Kalman estesoN.B. Le matrici sottoindicate dipendono dal passo k

Il filtro di Kalman estesoPredizione:

Aggiornamento misura:

Posto come guadagno del filtro di Kalman esteso:

Il filtro di Kalman propostoIl nostro sistema in due stati considera due rumori in ingresso:

Rumore d’uscitaEq. d’uscita del sistema

Rumore sull’accelerazione

Eq. di stato del sistema

Il filtro di Kalman proposto

Condizioni iniziali:

I due rumori e gli errori di stima iniziali devono soddisfare l’equazione ICQ.Se esiste una soluzione per l’eq. Di Riccati allora l’eq. ICQ risulta soddisfatta ed il vettore di stato può essere stimato dalle misure dei segnali RSSI attraverso l’impiego di una versione robusta del filtro di Kalman esteso.

Il filtro di Kalman proposto

Sotto queste ipotesi il sistema robot-sensore i-esimo si può rappresentare come:

Dalla ICQ invece:

Posti : >0

Il filtro di Kalman proposto

Data l’incertezza provocata dalla ICQ, rumore di misura, accelerazione e incertezza delle condizioni iniziali, vengono considerate ingressi deterministici limitati:

(Eq. di misura)

Posti: si osserva

Sotto queste condizioni il sistema soddisfa la ICQ.

Modello del canale

Definisce le caratteristiche del canale Per la localizzazione i chip radio usano due parametri:

LQI

Qualità del segnale Potenza del segnale

RSSI

Power vs Distance P(d)

Distance d

Potenza ricevuta

Potenza nota ad una distanza do

Coefficiente di path loss

Variabile aleatoria gaussiana N(0, σ2)

Potenza trasmessa

Fattore di attenuazione

Fattore di decrescenza

RSSI vs Power

) ))

La potenza del segnale ricevuto viene codificato dal Tmote in un valore di RSSI

)

Calcolo della distanza dal nodo trasmettitore

ParteSperimentale

… dalla simulazione alla realtà

T-mote SkyLe componenti Hardware utilizzate sono i moduli

Tmote Sky

Contengono un chip radio BlueTooth con antenna integrata

Sono alimentati a batteria

Possono essere programmati con TinyOS nel linguaggio NesC

Contengono una UART seriale

SLAM sperimentale Nodi ancora Sono disposti in uno spazio definito Hanno una posizione fissa Numero noto

Nodo mobile Si muove lungo una traiettoria definita Deve ricavare la distanza dai nodi ancora Trasmette le informazioni ricavate alla BaseStation

BaseStation Riceve i dati inviati dal nodo mobile Tramite comunicazione seriale invia i dati al PC

Nodo 1

Nodo 2

Nodo 3

Nodo 4

Mobile

BaseStation

Porta seriale

USB

Schema di comunicazione

Nodo Mobile• Al termine di ogni tratto di traiettoria, nella fase di stop, comunica con i nodi ancora • Interroga in modo sequenziale tutti i nodi ancora tramite un messaggio pkt• Riceve poi un determinato numero di messaggi da ogni nodo ancora• Calcola il valore medio di RSSI dei messaggi ricevuti• Invia il dato ricevuto alla base station (pktAnchor)

pkt

Nodeid id del trasmittente Counter id del riceventeSTART bool abilita la tx

pktAnchor

Nodeid id del nodo Ancora PosIndex posiz del nodo mobilePower potenza media rilevata

Nodo Ancora• Ogni nodo è dotato di un ID unico • Rimangono inattivi fino alla rx di un messaggio pkt:

con nodeid uguale alla ID del nodo mobile con counter uguale alla loro ID con START = TRUE

• In questa condizione iniziano l’invio di pkt al nodo mobile contenenti il proprio nodeid ogni 15ms• Alla ricezione di un messaggio con START = FALSE il nodo mobile interrompe l’invio di pkt

L’accensione di alcuni LED visualizzano l’attività di ricezione (rosso) e trasmissione (verde)

Nodo BaseStation

• Il nodo BaseStation deve essere collegato ad un PC tramite porta USB • Viene abilitata la comunicazione seriale• Attende la ricezione di un messaggio pktAnchor• Quando questo viene ricevuto invia un pacchetto di tipo serialpkt al PC

• Un opportuno programma sul PC riceve i dati dalla seriale e li salva in un file .m con una tabulazione predefinita:

Posindex Power NodeID

MobileBoot.booted( ) Radio ON Timer0.startPeriodic Timer0.Fired Send START nodo 1 Count_media = 0;

GetRSSI Count_media ++ GetRSSI Count_media ++ GetRSSI …… GetRSSI Count_media ++ Count _media = = 40; Send STOP nodo 1 Calcolo RSS medio Send Data to Basestation

Nodo Ancora 1Boot.booted( )Radio ON

Receive.receive Timer2.startPeriodic Timer2.fired Send msg to Mobile Timer2.fired Send msg to Mobile Timer2.fired Send msg to Mobile …… Send msg to Mobile

Receive.receive Timer2.stop

BaseStationBoot.booted( )Serial ONRadio ON

Receive.receive Send SERIAL to PC

Estrapolato di pseudo-codice

E-PUCK

•Cos’ è E-PUCK?

•Caratteristiche meccaniche

•Caratteristiche elettriche

Telaioplastico

2 motoripasso-passo

Diametro di 70mm

Velocitàmax

15cm/s

Ruote di diametro

41mmDistanza

tra le ruote di 53mm

Caratteristiche meccaniche

Unico alimentatore

a 3,3V

Una porta RS232 e unainterfaccia bluetooth

Un processore

di dsPIC

8 sensori di

prossimità

Batteria LiION di capacità

5Wh

Caratteristiche elettriche

Un accelerometro

3 microfoni

1 altoparlante

1 fotocamera

Ipotesi per la sperimentazione

•Perimetro 50x50cm

•Comunicazione tra t-mote solo quando e-puck è fermo

•Tempo di sosta e tempo di percorrenza di un segmento di traiettoria

Realizzazione del movimento1.Programmazione in C

2.Connessione dell’e-puck via bluetooth al computer

3. Scrittura nella memoria dell’e-puck del codice costruito

Realizzazione del movimento

•1 ciclo for per ogni segmento della traiettoria•7 secondi di sosta ogni passo•1s di tempo per compiere 10cm

Algoritmi e Simulazioni: Assunzioni a priori: La comunicazione sul veicolo mobile è abilitata solo quando questo è fermo. La traiettoria del veicolo è prefissata e immutabile. I sensori hanno antenne isotropiche

Metodologia delle prove sperimentali: Percorso limitato entro un quadrato di 50 cm di lato. (Per tempo richiesto dai sensori e per irregolarità del piano). Operazioni non in tempo reale di modo che fosse scindibile la parte Matlab e la parte di raccolta dati. Il veicolo mobile si muove di 10cm in una sola direzione, in un secondo poi elabora le misure in 7 secondi salvandole su un file di testo.

Algoritmi e Simulazioni:Localizzazione:

Si vuole individuare la posizione dei nodi conoscendo in modo esatto la posizione del nodo mobile.

Run.m: Algoritmo semplicistico, non efficace.

Localizzazione.m: Algoritmo molto efficace, preciso, almeno in via teorica(Provato anche con dati sperimantali).

Algoritmi e Simulazioni:

L’algoritmo proposto nel file Run.m:

Questo algoritmo utilizza la triangolazione, in particolare calcola i punti di intersezione delle circonferenze determinate dalle misure.Servono quindi almeno tre misure ricavate lungo un percorso non allineato.

Algoritmi e Simulazioni:Localizzazione su traiettoria allineata:

X

Algoritmi e Simulazioni:Localizzazione efficiente su percorso non allineato:

Algoritmi e Simulazioni:Qualora non ci fosse rumore esisterebbe un solo punto comune a tutte le circonferenze. Tuttavia a causa del rumore ciò non accade e si trova quindi una nuvola di punti prossimi alla posizione vera del nodo.

Per ottenere valori più corretti si ripete la misura e poi si applica il filtro di Kalman per determinare la distanza “vera”.

In uscita viene presentato il grafo del movimento del veicolo con il plot dei punti di intersezione. Per avere il valore corretto si dovrebbe determinare il valor medio.

Algoritmi e Simulazioni:

Algoritmi e Simulazioni:

L’algoritmo proposto nel file Localizzazione.m:

Questo algoritmo utilizza lo stimatore ai minimi quadrati presentato nella tesi di L.Parolini del 2007. In questo caso per avere una prima stima sono sufficienti 2 misure. Per far sì che il sistema tenga conto della bontà delle misure sì è introdotto un fattore di peso, ricavato dalle statistiche delle misure.

Algoritmi e Simulazioni:

Per ricavare la matrice di peso (matrice di varianza di w) si è usata la seguente formula:

Con i e j indici della matrice.Da questa matrice si può ricavare la seguente stima della posizione del nodo m-esimo:

Algoritmi e Simulazioni:Dove la matrice A e i vettori b e T sono:

Algoritmi e Simulazioni:

Con questo algoritmo si ottengo risultati molto soddisfacenti con piccoli errori, dell’ordine di qualche millimetro.

Inoltre se si osserva la matrice di varianza essa al termine del movimento del veicolo mobile, raggiunge valori infinitesimi.

Di seguito viene presentato un esempio con 10 nodi con disposizione dei nodi fissi predeterminata.

Algoritmi e Simulazioni:

Qui è presentato l’andamento del veicoli e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Questo algoritmo è stato anche testato sui dati di laboratorio. Ottenendo risultati solo in parte sorprendenti.

Infatti si ottengono risultati non altrettanto positivi, seppure appena accettabili poiché sono dell’ordine del 20% sulla dimensione totale del piano di lavoro.

Algoritmi e Simulazioni:

Questo perché si sono fatte delle supposizioni ad esempio:

• le antenne si sono supposte isotropiche;

• per costruire la matrice di peso si usa il modello statistico dell’Rssi che contiene delle approssimazioni;

• è possibile migliorare le prestazioni affinando i parametri alla situazione presente in laboratorio.

Algoritmi e Simulazioni:

Qui è presentato l’andamento del veicolo e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y del 2° nodo, quello presente nella posizione 0,15-0,25 .

Algoritmi e Simulazioni:Simultaneous Localization and Mapping:Si vuole individuare la posizione dei nodi con una conoscenza iniziale parziale o nulla della posizione dei vari elementi.

Slam.m: Algoritmo poco efficace con l’uso esclusivo del filtro di Kalman esteso.

Slam.m: Algoritmo migliore e preciso, almeno in via teorica, con l’uso contemporaneo dell’ Ekf e dei Least Squares (Provato anche con dati sperimentali).

Algoritmi e Simulazioni:L’algoritmo proposto nel file Slam.m:

Questo algoritmo utilizza solamente il filtro di Kalman esteso.

Inizialmente si è agito col filtro di Kalman così come definito, senza introdurre alcun parametro.

A causa però di alcuni problemi riscontrati si è poi optato per introdurre un nuovo parametro di pesatura delle misure in base al valore dell’Rssi effettivamente rilevato, che interviene sulla equazione di aggiornamento di Kalman.

Algoritmi e Simulazioni:In particolare si è modificato come segue l’aggiornamento:

Questa soluzione si è dimostrata poco efficiente, poiché un errore sulla posizione del veicolo mobile crea un impedimento alla stima, seppur imprecisa, dei nodi fissi. Allora si introduce un nuovo parametro che si attiva in queste circostanze.

Algoritmi e Simulazioni:

L’andamento dei vari γ è di tipo cubico al variare dell’Rssi entro una certa banda di incertezza, all’esterno della quale vale o 0.5 o 0.Con questi accorgimenti si presentava ancora un errore, infatti in caso di conoscenza nulla, non si riesce ad aggiornare la stima delle posizioni dei nodi fissi quindi si deve inserire una routine che si attiva in tali situazioni portando la stima del nodo nei pressi del veicolo fisso.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dei veicoli e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore del veicolo mobile.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Introducendo queste routine si ottengono risultanti positivi ma non soddisfacenti poiché quando il nodo fisso è entro una certa zona di incertezza dalla posizione vera, essa viene sì aggiornata ma in modo troppo esiguo.

Questo comportamento non pienamente sufficiente è da imputarsi alla divergenza del filtro di Kalman.

Algoritmi e Simulazioni:

L’algoritmo proposto nel file Slam.m (2° versione):

Questo algoritmo utilizza insieme al filtro di Kalman lo stimatore ai minimi quadrati utilizzato precedentemente.

Tuttavia l’integrazione è a solo una via. In particolare non viene tenuto conto della bontà della stima della posizione del nodo mobile nel calcolo della posizione dei nodi fissi.

Algoritmi e Simulazioni:

La parte di minimi quadrati quindi rimane invariata, semplificando l’algoritmo.

D’altra parte ciò introduce delle inevitabili imprecisioni che nelle simulazioni non sono significative.

L’applicazione del filtro di Kalman esteso resta pressoché invariata, si usa un solo parametro γ, anch’esso varia con legge cubica entro un certo rangedegli Rssi.

Algoritmi e Simulazioni:

Note:

Se la varianza della stima è troppo elevata questa viene scartata.

Se la condizione iniziale del nodo mobile è troppo lontana dalla “vera” si crea un offset che il sistema non è in grado di annullare.

Algoritmi e Simulazioni:

Con conoscenza approssimata della posizione dei nodi si ottengono risultati molto buoni, con errori del 5% almeno nelle simulazioni.

Se invece la conoscenza iniziale è nulla allora gli errori crescono tanto quanto è sbagliata la condizione iniziale del nodo mobile.

Di seguito viene presentato un paio di esempi con 5 nodi con conoscenza a priori approssimata e nulla.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dei veicoli e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y del nodo mobile.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dei veicoli e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y del nodo mobile.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Questo algoritmo è stato anche testato sui dati di laboratorio. Ottenendo risultati meno soddisfacenti.

Infatti si ottengono del 20% sulla dimensione totale del piano di lavoro.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dei veicoli e delle stime.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y del nodo mobile.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Qui è presentato l’andamento dell’errore lungo l’asse x e asse y di un nodo generico.

Algoritmi e Simulazioni:

Questo perché si sono fatte delle supposizioni ad esempio:

le antenne si sono supposte isotropiche;

per costruire la matrice di peso nel least squares si usa il modello statistico dell’Rssi che contiene delle approssimazioni;

è possibile migliorare le prestazioni tenendo anche conto della precisione della stima sul veicolo mobile nel calcolo dei nodi fissi;

è possibile migliorare le prestazioni affinando i parametri alla situazione presente in laboratorio.