Progettazione di Sistemi di Controllo
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
Localizzazione
e SLAM
con Wireless
Sensors
Network
Localizzazionepunto
SLAM
dell’ambiente
cerca
di stimare
nel modo
più preciso possibile il numero e la posizione
dei nodi ancora presenti.
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
Ayache e
Crowley, Chatila
e Durrant-Whyte
Faugeras
e Laumond
Punto cruciale
della teoria:
necessità che vi sia una forte Navigazione di
Basi statistiche
correlazione
tra le stime della robot tramite
Navigazione
per la descrizione
posizione
dei diversi
oggetti sonar e filtro di
mediante
visione
dell’ambiente
dell’ambiente
Kalman
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
 problema di localizzazione del
scisse in due problemi distinti: 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.
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
Slam probabilistico:
Condizioni per applicare il Teorema di Bayes:
 modello di osservazione:
Probabilità di effettuare
un’osservazione zk quando il
veicolo
mobile
la posizione
Proprietà
di eindipendenza
del
nodo ancora
sono
conosciute
condizionale
delle
osservazioni
 modello di transizione:
lo stato di transizione può essere
descritto attraverso un processo
di Markov
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:
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.
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
Condizioni iniziali
Stime a priori
Stime a posteriori
Il filtro di Kalman
La varianza del processo di
innovazione e(k):
Il guadagno del filtro di
Kalman:
S: correlazione tra
rumori di processo e di
misura
La varianza del rumore
bianco :
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
Stima a posteriori Rumore di
Stato e
dello stato al
processo e di
osservazione al osservazione
passo k
misura
passo attuale approssimati
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 :
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
RSSI
Potenza del segnale
Power vs Distance
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)
RSSI vs Power
))
))
La potenza del segnale ricevuto viene codificato dal
Tmote in un valore di RSSI
Calcolo della distanza dal nodo trasmettitore
… dalla simulazione alla realtà
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
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
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
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
E-PUCK
•Cos’ è E-PUCK?
•Caratteristiche meccaniche
•Caratteristiche elettriche
Caratteristiche meccaniche
Ruote di
diametro
41mm
Distanza
tra le ruote
di 53mm
2 motori
passopasso
Velocità
max
15cm/s
Telaio
plastico
Diametro
di 70mm
Caratteristiche elettriche
3
microfoni
1
altoparlante
Unico
alimentatore
a 3,3V
Un
accelerometro
8 sensori
di
prossimità
1
fotocamera
Batteria
LiION di
capacità
5Wh
Un
processore
di dsPIC
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:
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.