Vai alla Home Page About me Courseware Federica Living Library Federica Federica Podstudio Virtual Campus 3D Le Miniguide all'orientamento Gli eBook di Federica La Corte in Rete
 
 
Il Corso Le lezioni del Corso La Cattedra
 
Materiali di approfondimento Risorse Web Il Podcast di questa lezione

Alberto Finzi » 12.FastSLAM


SLAM con particle filters

SLAM con filtro particellare.
Problema della dimensione: molte particelle e l’algoritmo scala in modo esponenziale.
Si può utilizzare però la fattorizzazione dei filtri Rao-Blackwellized: se un oracolo ci dice il path, la stima della locazione delle features è indipendente.
FastSLAM: Particelle per la posa, Gaussiane per le features.
Mantiene più ipotesi sulle associazioni di dati (più robusto); non linearità; full SLAM & online SLAM trattati in modo uniforme.

Filtri Particellari

Rappresentano belief con insiemi di campioni.
Stima di processi non-Gaussiani, nonlineari.

Principio di Sampling Importance Resampling (SIR):

  • estrai la nuova generazione di particelle;
  • assegna un peso di importanza ad ogni particle;
  • resampling.

Applicazioni tipiche sono: tracking, localization, etc.

Localizzazione vs. SLAM

Un filtro particellare può essere usato per risolvere entrambi i problemi

Localizzazione: spazio di stato < x, y, θ>

SLAM: spazio di stato < x, y, θ, map>

  • per mappe di landmark = < l1, l2, …, lm>
  • per mappe a grid = < c11, c12, …, c1n, c21, …, cnm>

Problema: il numero di particelle necessario per rappresentare il posterior cresce esponenzialmente con la dimensione dello spazio di stato.

Dipendenze

Esiste una dipendenza tra le dimensioni dello spazio di stato?
Se è così, possiamo usare le dependenze per risolvere il problema con più efficienza?

Nel contesto dello SLAM:

  • la mappa dipende dalle pose del robot;
  • noi sappiamo come costruire una mappa data la posizione nota del sensore.

Posteriore Fattorizzato (Landmark)

Aiuta a risolvere il problema?

Aiuta a risolvere il problema?


Mapping con Landmarks

La conoscenza del percorso reale rende le posizioni dei landmark cond. indipendenti.

La conoscenza del percorso reale rende le posizioni dei landmark cond. indipendenti.


Posterior Fattorizzato


Rao-Blackwellization

p(x_{1:t}, l_{1:m}|z_{1:t}, u_{0:t-1})=

p(x_{1:t}|z_{1:t}, u_{0:t-1})\cdot \prod_{i=1}^M p(l_i|x_{1:t}, z_{1:t})

Questa fattorizzazione anche chiamata Rao-Blackwellization.

Dato che il secondo termine può essere calcolato efficientemente il particle filtering diventa possibile.

FastSLAM

Rao-Blackwellized particle filtering basato su landmarks [Montemerlo et al., 2002].

Ogni landmark rappresentato da un 2×2 Extended Kalman Filter (EKF).

Ogni particella deve mantenere M EKFs.


Fast SLAM

Ogni particella mantiene M EKF

Per M volte:

  • Retrieval: prendi una posa xk,t-1;
  • Predizione: campiona una posa da p(xk,t | xk,t-1, ut);
  • Update di misura: per ogni misura i, trova la corrispondenza j, e aggiorna il corrispondente EKF;
  • Peso: pesa la nuova particella wk.

Ricampionamento: ricampiona le M particelle.


Action Update

Per ogni posa, campionamento dal modello probabilistico di moto:

s_t^{[m]}\sim p (s_t|u_t, s_{t-1}^{[m]})

Esempio, dal modello in velocità:

v_t\sim N(v;v_t,\alpha_1v_1+\alpha_2)

\omega_t'\sim N(\omega;\omega_t,\alpha3\omega_t+\alpha_4)

Particella aggiunta in un insieme di particelle.

FastSLAM – Action Update


FastSLAM – Sensor Update

Corrispondenze note n1, … , nt.

Se feature n osservata, allora update

p(\theta_n_{t}|s^t,z^t, u^t, n^t)\stackrel{Markov}{=}\eta p(z_t|\theta_{n_t},s_t,n_t)p(\theta_{n_t}|s^{t-1}, z^{t-1}, u^{t-1}, n^{t-1})

Se feature n non osservata, stessa probabilità

p(\theta_{n\ne n_t}|s^t,z^t,u^t,n^t)=p(\theta_{n\ne n_t}|s^{t-1}, z^{t-1}, u^{t-1}, n^{t-1})

osservata, Aggiornamento come EKF.

osservata, Aggiornamento come EKF.


FastSLAM – Sensor Update


FastSLAM – Resampling

Peso delle particelle e ricampionamento: le particelle non sono distribuite secondo il posterior, non è inclusa la misura.

Occorre il peso (considerando le corrispondenze):

w_t^{[m]}=\frac{\text{target di riferimento}}{\text{proposal distribution}}=\frac{p(s^{t,[m]}|z^t,u^t,n^t)}{p(s^{t,[m]}|z^{t-1}, u^t, n^{t-1})}

w_t^{[m]}\stackrel{Markov}{=}\frac{p(z_t|s^{t,[m]},z^{t-1}, u^t,n^t)p(s^{t,[m]}|z^{t,[m]},u^t,n^{t-1})}{p(s^{t,[m]}|z^{t-1},u^t,n^{t-1})}}

Innovazione: valore osservato – valore stimato.

Innovazione: valore osservato – valore stimato.


FastSLAM – Sensor Update

Peso per la verosomiglianza.

Peso per la verosomiglianza.


FastSLAM Complessità

Update particelle del robot basate su controllo ut-1 → Constant time per particella
Incorporare osservazioni zt nei filtri di Kalman → Log time per particella
Resampling del particle set → \frac{\text{Log time per particella}}{O(N\bullet \log (M))}

\text{Log time per particella}

N = Numero di particelle
M = Numero di features

FastSLAM Complessità

I passi di update e di peso sono lineari nel numero delle particelle.

Il passo di resampling se naive allora lineare nel numero di feature.

Se ogni particella organizzata come albero binario di stimatori di feature allora log N.

Fino a 1000000 landmark.


Problema di Data Association

Quali osservazioni appartengono a quale landmark?

Un SLAM robusto deve considerare possibli associazioni di dati.

Le possibili associazioni di dati dipendono anche dalla posa del robot.


Data Association multi-ipotesi

Se la data association è basata su particelle.

Ogni particella ha la sua data association.

Prima si calcola la posa, quindi l’associazione.

Errori di posa del robot sono fattorizzati fuori dalle decisioni di associazioni data.


Associazione dati per particella

Due opzioni per l’associazione per-particle:

  • prendi la più probabile;
  • prendi un’associazione random pesata con il likelihood di osservazione.

Se la probabilità è bassa, genera un nuovo landmark


FastSLAM Riassunto

FastSLAM fattorizza il posterior SLAM in problemi di stima a bassa dimensione.
Scala problemi con più di 1 milione di feature.
FastSLAM fattorizza l’incertezza della posa del robot separandola dal problema del data association.
Robusto a significative ambiguità nell’associazione dati.
Permette di ritardare decisioni di data association finchè non si ottiene evidenza non ambigua.
Complessità di O(N logM).

  • Contenuti protetti da Creative Commons
  • Feed RSS
  • Condividi su FriendFeed
  • Condividi su Facebook
  • Segnala su Twitter
  • Condividi su LinkedIn
Progetto "Campus Virtuale" dell'Università degli Studi di Napoli Federico II, realizzato con il cofinanziamento dell'Unione europea. Asse V - Società dell'informazione - Obiettivo Operativo 5.1 e-Government ed e-Inclusion