La presentazione è in caricamento. Aspetta per favore

La presentazione è in caricamento. Aspetta per favore

Progettazione di Sistemi di Controllo Desdemona Hoxhaj, Nicola Mazzucato, Maurizio Montis, Marco Sommacal.

Presentazioni simili


Presentazione sul tema: "Progettazione di Sistemi di Controllo Desdemona Hoxhaj, Nicola Mazzucato, Maurizio Montis, Marco Sommacal."— Transcript della presentazione:

1 Progettazione di Sistemi di Controllo Desdemona Hoxhaj, Nicola Mazzucato, Maurizio Montis, Marco Sommacal

2 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 allinterno della mappa stessa. Il nodo mobile, conoscendo in modo esatto la propria posizione e orientamento in ogni punto dellambiente cerca di stimare nel modo più preciso possibile il numero e la posizione dei nodi ancora presenti.

3 Storia dello Slam: conferenza dellIEEE chiamata Robotica e Automatica (San Francisco). Origini del problema: 1986 i metodi probabilistici vennero per la prima volta applicati alla robotica e allintelligenza artificiale.

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

5 Storia dello Slam: Conseguenze Una soluzione completa richiedeva uno stato composto da Posizione del veicolo mobile Posizione dellambiente 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 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 dellambiente. problema di localizzazione del veicolo mobile; problema di mappatura dellambiente.

7 Storia dello Slam: Ulteriori studi dimostrarono che: il problema composto verifica la proprietà di convergenza; la correlazione era una parte critica del problema. 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. del veicolo mobile; dei nodi ancora.

9 Necessito di avere: lingresso al passo k; losservazione al passo k Necessito di avere: lingresso al passo k; losservazione 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

10 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 unosservazione z k quando il veicolo mobile e la posizione del nodo ancora sono conosciute lo stato di transizione può essere descritto attraverso un processo di Markov Proprietà di indipendenza condizionale delle osservazioni

11 Slam probabilistico: Time update: Measurement update: Forma ricorsiva dellalgoritmo di SLAM

12 Slam probabilistico: Dipendenza tra le osservazioni e le posizioni dei nodi fissi e mobile; Osservazioni sullo SLAM probabilistico: lerrore 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 x O ysys XsXs s S

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

22 Modello dinamico del sistema: Equazione di stato:

23 Modello dinamico del sistema: Matrice :

24 Modello dinamico del sistema: Equazione di uscita: Valori numerici: P TX = 0 A = 18.2 n p = 2.18 var = 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 lerrore quadratico medio.

27 Il filtro di Kalman Le equazioni del filtro si dividono in: predizione: responsabili della previsione dello stato attuale e della covarianza dellerrore 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) allistante (k+1): Ingresso del processo al k-esimo istante Rumore di processo modellato come una gaussiana

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

30 Il filtro di Kalman Comportamento della stima Stima a prioriStima a posteriori Errore di stima a prioriErrore di stima a posteriori Varianza errore di stima a priori Varianza errore di stima a posteriori

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

32 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:

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 processoRumore di misura

36 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

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 duscita Eq. duscita del sistema Rumore sullaccelerazione Eq. di stato del sistema

40 Il filtro di Kalman proposto Condizioni iniziali: I due rumori e gli errori di stima iniziali devono soddisfare lequazione ICQ. Se esiste una soluzione per leq. Di Riccati allora leq. ICQ risulta soddisfatta ed il vettore di stato può essere stimato dalle misure dei segnali RSSI attraverso limpiego 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 :

42 Il filtro di Kalman proposto Data lincertezza 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 Qualità del segnale Potenza del segnale RSSI

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

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 … 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 Nodo 1 Nodo 2 Nodo 3 Nodo 4 Mobile Base Station Porta seriale USB Schema di comunicazione

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

51 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 linvio di pkt al nodo mobile contenenti il proprio nodeid ogni 15ms Alla ricezione di un messaggio con START = FALSE il nodo mobile interrompe linvio di pkt Laccensione di alcuni LED visualizzano lattività di ricezione (rosso) e trasmissione (verde)

52 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 PowerNodeID

53 Mobile Boot.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 1 Boot.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 BaseStation Boot.booted( ) Serial ON Radio ON Receive.receive Send SERIAL to PC Estrapolato di pseudo-codice

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

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

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

57 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

58 Realizzazione del movimento 1.Programmazione in C 2.Connessione delle-puck via bluetooth al computer 3. Scrittura nella memoria delle-puck del codice costruito

59 Realizzazione del movimento 1 ciclo for per ogni segmento della traiettoria 7 secondi di sosta ogni passo 1s di tempo per compiere 10cm

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

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

62 Algoritmi e Simulazioni: Lalgoritmo 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.

63 Algoritmi e Simulazioni: Localizzazione su traiettoria allineata:

64 Algoritmi e Simulazioni: Localizzazione efficiente su percorso non allineato:

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

66 Algoritmi e Simulazioni:

67 Lalgoritmo proposto nel file Localizzazione.m : Questo algoritmo utilizza lo stimatore ai minimi quadrati presentato nella tesi di L.Parolini del 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.

68 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:

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

70 Algoritmi e Simulazioni: Con questo algoritmo si ottengo risultati molto soddisfacenti con piccoli errori, dellordine 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.

71 Algoritmi e Simulazioni: Qui è presentato landamento del veicoli e delle stime.

72 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

73 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 dellordine del 20% sulla dimensione totale del piano di lavoro.

74 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 dellRssi che contiene delle approssimazioni; è possibile migliorare le prestazioni affinando i parametri alla situazione presente in laboratorio.

75 Algoritmi e Simulazioni: Qui è presentato landamento del veicolo e delle stime.

76 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y del 2° nodo, quello presente nella posizione 0,15-0,25.

77 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 luso esclusivo del filtro di Kalman esteso. Slam.m: Algoritmo migliore e preciso, almeno in via teorica, con luso contemporaneo dell Ekf e dei Least Squares (Provato anche con dati sperimentali).

78 Algoritmi e Simulazioni: Lalgoritmo 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 dellRssi effettivamente rilevato, che interviene sulla equazione di aggiornamento di Kalman.

79 Algoritmi e Simulazioni: In particolare si è modificato come segue laggiornamento: 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.

80 Algoritmi e Simulazioni: Landamento dei vari γ è di tipo cubico al variare dellRssi entro una certa banda di incertezza, allesterno 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.

81 Algoritmi e Simulazioni: Qui è presentato landamento dei veicoli e delle stime.

82 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore del veicolo mobile.

83 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

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

85 Algoritmi e Simulazioni: Lalgoritmo proposto nel file Slam.m (2° versione) : Questo algoritmo utilizza insieme al filtro di Kalman lo stimatore ai minimi quadrati utilizzato precedentemente. Tuttavia lintegrazione è 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.

86 Algoritmi e Simulazioni: La parte di minimi quadrati quindi rimane invariata, semplificando lalgoritmo. Daltra parte ciò introduce delle inevitabili imprecisioni che nelle simulazioni non sono significative. Lapplicazione del filtro di Kalman esteso resta pressoché invariata, si usa un solo parametro γ, anchesso varia con legge cubica entro un certo rangedegli Rssi.

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

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

89 Algoritmi e Simulazioni: Qui è presentato landamento dei veicoli e delle stime.

90 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y del nodo mobile.

91 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

92 Algoritmi e Simulazioni: Qui è presentato landamento dei veicoli e delle stime.

93 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y del nodo mobile.

94 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

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

96 Algoritmi e Simulazioni: Qui è presentato landamento dei veicoli e delle stime.

97 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y del nodo mobile.

98 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

99 Algoritmi e Simulazioni: Qui è presentato landamento dellerrore lungo lasse x e asse y di un nodo generico.

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


Scaricare ppt "Progettazione di Sistemi di Controllo Desdemona Hoxhaj, Nicola Mazzucato, Maurizio Montis, Marco Sommacal."

Presentazioni simili


Annunci Google