Progettazione di Sistemi di Controllo Localizzazione e SLAM con Wireless Sensors Network (WSN) Desdemona Hoxhaj , Nicola Mazzucato, Maurizio Montis , Marco Sommacal

Argomenti del Progetto: 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. Localizzazione Localizzazione e SLAM con Wireless Sensors Network SLAM 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.

Storia dello Slam: Origini del problema: 1986 conferenza dell’IEEE chiamata ”Robotica e Automatica” (San Francisco). i metodi probabilistici vennero per la prima volta applicati alla robotica e all’intelligenza artificiale.

Storia dello Slam: Primi risultati raggiunti Risultato teorico Risultati applicativi Smith, Cheesman e Durrant-Whyte Ayache e Faugeras Crowley, Chatila e Laumond Punto cruciale della teoria: necessità che vi sia una forte correlazione tra le stime della posizione dei diversi oggetti dell’ambiente Basi statistiche per la descrizione dell’ambiente Navigazione di robot tramite sonar e filtro di Kalman Navigazione mediante visione

Storia dello Slam: Conseguenze Posizione del veicolo mobile Una soluzione completa richiedeva uno stato composto da 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 minimazzare e/o eliminare le correlazioni tra riferimenti Il problema dello SLAM si scisse in due problemi distinti: 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

Problema di simultanea localizzazione e mappatura 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.

Si cerca di determinare una soluzione ricorsiva del problema 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 Necessito di avere: l’ingresso al passo k; l’osservazione al passo k

Proprietà di indipendenza condizionale delle osservazioni Slam probabilistico: Condizioni per applicare il Teorema di Bayes: Condizioni per applicare il Teorema di Bayes: modello di osservazione: Probabilità di effettuare un’osservazione zk quando il veicolo mobile e la posizione del nodo ancora sono conosciute Proprietà di indipendenza condizionale delle osservazioni modello di transizione: lo stato di transizione può essere descritto attraverso un processo di Markov

Forma ricorsiva dell’algoritmo di SLAM Slam probabilistico: Forma ricorsiva dell’algoritmo di SLAM Time update: Measurement update:

Slam probabilistico: Osservazioni sullo SLAM probabilistico: Dipendenza tra le osservazioni e le posizioni dei nodi fissi e mobile; 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 ys S fs Xs O x

Modello dinamico del sistema: Variabili di ingresso: Accelerazione del robot: Variabili di uscita: 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 del sistema: Rumore di Processo Modello del rumore Rumore di Misura

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 = 0 A = 18.2 np = 2.18 var(c) = 6.03

Discretizzazione del modello:

Un insieme di equazioni matematiche Il filtro di Kalman Cos’è? 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.

Le equazioni del filtro si dividono in: 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 istante Rumore di processo modellato come una gaussiana

Il filtro di Kalman La stima viene fatta attraverso le misurazioni: Rumore di misura modellato come una gaussiana

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

Il filtro di Kalman Algoritmo ricorsivo per il calcolo degli stimatori lineari a minima varianza Stime a priori Condizioni iniziali Stime a posteriori

S: correlazione tra rumori di processo e di misura Il filtro di Kalman La varianza del processo di innovazione e(k): La varianza del rumore bianco : Il guadagno del filtro di Kalman: S: correlazione tra rumori di processo e di misura 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 esteso Dato un processo con stato governato da: E equazione di osservazione non lineare: Rumore di processo Rumore di misura

Il filtro di Kalman esteso Si 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 esteso N.B. Le matrici sottoindicate dipendono dal passo k

Il filtro di Kalman esteso Predizione: Aggiornamento misura: Posto come guadagno del filtro di Kalman esteso:

Il filtro di Kalman proposto Il nostro sistema in due stati considera due rumori in ingresso: Rumore d’uscita Eq. d’uscita del sistema Eq. di stato del sistema Rumore sull’accelerazione

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 LQI RSSI Qualità del segnale Potenza del segnale Definisce le caratteristiche del canale Per la localizzazione i chip radio usano due parametri: LQI RSSI Qualità del segnale Potenza del segnale

Power vs Distance ( ) Potenza ricevuta Potenza trasmessa P(d) Distance d ( ) Potenza trasmessa Fattore di attenuazione Fattore di decrescenza Potenza ricevuta Potenza nota ad una distanza do Coefficiente di path loss Variabile aleatoria gaussiana N(0, σ2)

) ) RSSI vs Power ) Calcolo della distanza dal nodo trasmettitore ) La potenza del segnale ricevuto viene codificato dal Tmote in un valore di RSSI Calcolo della distanza dal nodo trasmettitore

… dalla simulazione alla realtà Parte Sperimentale … dalla simulazione alla realtà

Le componenti Hardware utilizzate sono i moduli T-mote Sky Le 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

Schema di comunicazione Nodo 2 Nodo 3 Nodo 1 Nodo 4 Porta seriale USB ( ) ( ) Base Station Mobile

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 pktAnchor Nodeid id del trasmittente Counter id del ricevente START bool abilita la tx Nodeid id del nodo Ancora PosIndex posiz del nodo mobile Power 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

Estrapolato di pseudo-codice BaseStation Boot.booted( ) Serial ON Radio ON Receive.receive Send SERIAL to PC Mobile Boot.booted( ) Radio ON Timer0.startPeriodic Timer0.Fired Send START nodo 1 Count_media = 0; GetRSSI Count_media ++ …… Count _media = = 40; Send STOP nodo 1 Calcolo RSS medio Send Data to Basestation Nodo Ancora 1 Boot.booted( ) Radio ON Receive.receive Timer2.startPeriodic Timer2.fired Send msg to Mobile …… Timer2.stop

E-PUCK Cos’ è E-PUCK? Caratteristiche meccaniche Caratteristiche elettriche

Caratteristiche meccaniche Ruote di diametro 41mm 2 motori passo-passo Distanza tra le ruote di 53mm Velocità max 15cm/s Telaio plastico Diametro di 70mm

Caratteristiche elettriche 3 microfoni Un accelerometro 8 sensori di prossimità 1 altoparlante 1 fotocamera Batteria LiION di capacità 5Wh Un processore di dsPIC Unico alimentatore a 3,3V Una porta RS232 e una interfaccia bluetooth

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 movimento 1.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.