SLAM
Questo documento contiene materiale proveniente da fonti esterne: 1 [ Documento Originale - Autore: Cristian Secchi - Sito Riferimento ]
Dall'inglese Simultaneous Localization and Mapping : Localizzazione e Mappatura Simultanee.
Consiste nell'effettuare una mappatura dell'ambiente circostante al robot sfruttando i dati di localizzazione e, contemporaneamente, utilizzare le informazioni di mapping ottenute per localizzare il robot.
Con questa tecnica la Localizzazione e la Mappatura si influenzano a vicenda.
E' un metodo ad approssimazione incrementale.
Contents |
Il problema
- Per poter seguire una traiettoria desiderata un robot mobile deve conoscere la propria configurazione
- Per poter eseguire una strategia di navigazione non banale, un robot deve conoscere una mappa dell’ambiente circostante
- Ogni controllore in retroazione richiede la conoscenza dello stato del robot (posizione assoluta)
- In generale, per rendere veramente autonomo un robot mobile, è necessario che esso sia in grado di localizzarsi e di costruire una mappa dell’ambiente circostante
Lo SLAM cerca una risposta alla domanda:
E’ possibili usare un veicolo che parte da
- una posizione iniziale ignota, in un
- ambiente ignoto, per
- costruire incrementalmente una mappa dell’ambiente, e
- allo stesso tempo usare la mappa per determinare la posizione del robot?
Origini
Lo SLAM trova le sue origini nella localizzazione odometrica, che è il sistema più semplice per realizzare la localizzazione con un robot mobile.
Vedi voce: Odometria
Il problema della localizzazione
- Data una mappa m di punti di riferimento la cui posizione è nota a priori
- Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)
- Determinare la posizione del veicolo x(k) basandosi sulla misura y(k)
- un filtro è necessario perché le misure sono tipicamente rumorose
- La posizione dei landmark viene utilizzata per ridurre l’incertezza nella posizione del robot
• xk: posizione del veicolo all’stante k
• u : l’ingresso di controllo applicato all’stante k-1 E tenuto costanto nell’intervallo [k-1,k)
• yk: misura del landmark pressa allistante k
• Xk: storia delle posizioni {x1,..,xk}
• Uk: storia degli ingressi di controllo {u1,…,uk}
• m: insieme dei landmark
Il problema del mapping
- Date le posizioni del veicolo X^k
- Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)
- Determinare la mappa basandosi sulla misura y(k)
- un filtro è necessario perché le misure sono tipicamente rumorose
- La posizione del veicolo viene usata per ridurre l’incertezza nel rilevamento dei landmark
• Xk: storia delle posizioni {x1,..,xk}
• mi: posizine del landmark i
• m: insieme dei landmark
Il problema dello SLAM
- Dalla conoscenza delle misure Yk
- Determinare la posizione del veicolo Xk
- Costruire la mappa m delle posizioni dei landmark
• xk: posizione del veicolo all’stante k
• uk: l’ingresso di controllo applicato all’stante k-1 e tenuto costanto nell’intervallo [k-1,k)
• yk: misura del landmark pressa allistante k
• Xk: storia delle posizioni { x1,..,xk}
• Uk: storia degli ingressi di controllo {u1,…,uk}
• Yk: storia delle misure {y1,…,yk}
• m: insieme dei landmark
SLAM
- La localizzazione e il mapping sono problemi correlati
- due quantità incerte devono essere inferite tramite una sola misura
- Una soluzione può essere ottenuta solo se i problemi di localizzazione e mapping sono considerati insieme
SLAM - Setting
- Un veicolo con un modello cinematico noto si muove in un ambiente popolato di landmark fissi (modello del processo)
- Il veicolo è dotato di un sensore che può misurare la posizione relativa tra ciascun landmark e il veicolo stesso (modello dell’osservazione)
Modello del processo
• Supponiamo, per ora, che il modello cinematico del robot sia un sistema lineare descritto da:
dove
• A è la matrice di transizione dello stato
• uv è il vettore degli ingressi
• wv è un vettore aleatorio gaussiano con media nulla e matrice di covarianza Q_v(k) che modella l’incertezza del modello
• L’equazione transizione dello stato per landmark i-esimo è data da:
• E’ possibile raggruppare la dinamica del robot e la dinamica dei landmark in un unico modello del processo
• Lo stato del processo è dato da
• Il modello del processo p descritto da:
dove I_pi e 0_pi sono rispettivamente la matrice identità e la matrice nulla di ordine pi
Modello dell’Osservazione
• Supponiamo, per ora, che i sensori che osservano i landmark siano lineari. Il modello dell’osservazione del i-esimo landmark è dato da:
- dove:
- vi(k) è un vettore aleatorio gaussiano con media nulla e matrice di covarianza Rv(k) che modella l’incertezza della misura
- Ci è la matrice di osservazione (cioè la matrice di uscita) che mette in relazione lo stato del processo con la misura relativa del landmark yi e può essere scritta come:
- Il modello dell’osservazione può essere pertanto riscritto come
misura del landmark relativa alla posizione del veicolo
SLAM
- Determinare la posizione del robot e quella dei landmark equivale a stimare lo stato del processo in base alle misure
- Sia la dinamica del processo che quella delle misure è caratterizzata da un’incertezza gaussiana
- Il Filtro di Kalman risolve il problema! E consente di risolverlo online!
Il Filtro di Kalman
Schema
Filtro di Kalman Esteso (Extended Kalman Filter – EKF)
- Il filtro di Kalman può essere utilizzato solo se il modello del processo e il modello dell’osservazione sono sistemi lineari
- Il modello cinematico di un robot mobile (es.: uniciclo, automobile) sono nonlineari
- Il modello dell’osservazione spesso è nonlineare (es.: utilizzando scanner laser)
- Il Filtro di Kalman Esteso (EKF) permettere di superare l’ipotesi di linearità sul processo e sulla misura
- Il processo e le misure sono in realtà descrivibili da funzioni nonlineari:
- dove w(k) e v(k) sono vettori aleatori di media nulla che descrivono l’incertezza di processo e di misura e le cui matrici di covarianza sono rispettivamente Q(k) e R(k)
- L’idea che sta alla base dell’EKF è quella di approssimare il sistema il cui stato è da stimare linearizzandolo intorno allo stato stimato. Data la stima dello stato all’istante k-1, si linearizza il sistema intorno a quello stato e poi si stima lo stato all’istante k.
- Ad ogni istante si lavora su un sistema lineare e, quindi, è possibile utilizzare il filtro di Kalman per la stima dello stato
- Ad ogni istante, il sistema lineare che si considera è diverso perché si linearizza intorno a diversi punti di equilibrio
Algoritmo EKF
Localizzazione
Modello non lineare!
Un robot di tipo differential drive deve seguire una traiettoria circolare predefinita.
L’inseguimento di traiettoria è implementato tramite IO-SFL ed è quindi necessario una misura della posizione del robot per costruire l’azione di controllo.
Il modello cinematico del robot è affetto da una incertezza di processo.
La posizione del robot è misurata da una telecamera e la misura è affetta da un’incertezza.
Modello del processo
Modello dell’Osservazione
. Tramite l’uscita ho un accesso diretto allo stato.
. L’uscita è però caratterizzata da un’incertezza
Risultati
Il robot si perde e la traiettoria reale (blu) è diversa da quella desiderata (nero).
I cerchi rossi indicano le letture del sensore.
Predizione
- Considero il modello del processo senza rumore:
- Costruisco il Jacobiano di f calcolato in:
- Predico lo stato utilizzando il modello e aggiorno la covarianza:
Aggiornamento
- Costruisco il Jacobiano della funzione di uscita:
- Costruisco l’innovazione e la sua covarianza:
- Costruisco il guadagno di Kalman e aggiorno la stima e la sua covarianza:
Aggiornamento: Risultati
Il robot (blu) segue la traiettoria desiderata (nero) e la stima della posizione del robot (verde) è accurata:
La stima iniziale è molto incerta ma il filtro di Kalman la rende sempre più accurata:
