Progettazione di Sistemi di Controllo

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

2. 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 e SLAM con Wireless Sensors Network Localizzazione 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.

3. 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.

4. 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

5. 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

6. 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.

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

8. 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.

9. 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

10. 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

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

12. 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;

13. Modello dinamico del sistema: Sistema non lineare:

14. Modello dinamico del sistema: Variabili di stato:

15. Modello dinamico del sistema: Variabili di stato:

16. Modello dinamico del sistema: Scelta dei parametri del robot mobile: y ys S fs Xs O x

17. Modello dinamico del sistema: Variabili di ingresso: Accelerazione del robot: Variabili di uscita: Segnali di potenza dei sensori fissi:

18. Caratteristiche cinematiche adottate per il veicolo mobile: • Scelta del movimento: • Spostamento per passi; • Tempo di spostamento legato al tempo di campionamento;

19. Caratteristiche cinematiche adottate per il veicolo mobile: Profilo di accelerazione

20. Caratteristiche cinematiche adottate per il veicolo mobile: Profilo di accelerazione Profilo di velocità

21. Modello dinamico del sistema: Rumore del sistema: Rumore di Processo Modello del rumore Rumore di Misura

22. Modello dinamico del sistema: Equazione di stato:

23. Modello dinamico del sistema: Matrice Q: Matrice f:

24. Modello dinamico del sistema: Equazione di uscita: • Valori numerici: • PTX = 0 • A = 18.2 • np = 2.18 • var(c)= 6.03

25. Discretizzazione del modello:

26. 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.

27. 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.

28. 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

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

30. 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 posteriori Varianza errore di stima a priori

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

32. Il filtro di Kalman S: correlazione tra rumori di processo e di misura La varianza del processo di innovazione e(k): La varianza del rumore bianco : Il guadagno del filtro di Kalman: La matrice F:

33. Il filtro di Kalman

34. 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.

35. Il filtro di Kalman esteso Dato un processo con stato governato da: E equazione di osservazione non lineare: Rumore di processo Rumore di misura

36. Il filtro di Kalman esteso Si possono scrivere le nuove equazioni che linearizzano la stima: Stato e osservazione approssimati Stima a posteriori dello stato al passo k Rumore di processo e di misura Stato e osservazione al passo attuale

37. Il filtro di Kalman esteso N.B. Le matrici sottoindicate dipendono dal passo k

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

39. 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

40. 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.

41. Il filtro di Kalman proposto Sotto queste ipotesi il sistema robot-sensore i-esimo si può rappresentare come: Dalla ICQ invece: Posti : >0

42. 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.

43. Modello del canale • Definisce le caratteristiche del canale • Per la localizzazione i chip radio usano due parametri: LQI RSSI Qualità del segnale Potenza del segnale

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

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

46. Parte Sperimentale … dalla simulazione alla realtà

47. 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

48. 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

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

50. 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