Filtro di Kalman Esteso applicato al problema della localizzazione

Filtro di Kalman Esteso applicato al problema della
localizzazione nella robotica mobile
Ing. Elisabetta Fabrizi
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Overview
•
•
Metodi di localizzazione
Filtro di Kalman Esteso
– localizzazione relativa
– richiami sul Filtro di Kalman
– localizzazione assoluta
– confronto tra KF e EKF
Localizzazione per sedia a rotelle
•
Applicazione alla localizzazione
– descrizione del sistema
– schema generale
– descrizione dei sensori
– predizione
• encoders
– correzione
• giroscopio
– matching delle uscite
• sonar
•
•
•
Modellizzazione
– con giroscopio
– modello cinematico con rumore
– modello uscite con rumore
Risultati sperimentali
– senza giroscopio
•
Conclusioni
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
La localizzazione
• Realizza la capacità del robot mobile di conoscere la propria
posizione e il proprio orientamento in un ambiente noto, a partire da
dati sensoriali
•
Es.: per seguire una traiettoria predefinita è necessario che il robot calcoli una
stima della propria configurazione in modo da impostare i comandi di moto
corretti
• A causa del rumore e degli errori (sistematici e non) tipici del
processo di misura, il robot “si perde” rapidamente nell’ambiente.
Bisogna usare tecniche particolari per ottenere
stime corrette (per esempio Kalman Filter)
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Perché il robot “si perde”: un esempio
• Dead reckoning
Supponiamo che il robot sia in una posizione nota e si muova potendo
misurare le velocità lineari e angolari.
Attraverso l’integrazione delle velocità si ottengono la posizione e
l’orientamento in ogni istante. Pero’ un piccolo bias sulla misura della
velocità provoca un errore nella posizione che cresce linearmente nel
tempo e quindi illimitatamente.
Dopo un po’ la stima sulla sua posizione
risulta completamente errata.
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Localizzazione relativa
• Buona stima della posizione iniziale
• Il robot esegue uno spostamento
• Si stima la nuova posizione del robot
Odometria: encoders per misurare la rotazione e la sterzata delle ruote
Navigazione inerziale: giroscopi e accelerometri per misurare le velocità
e le accelerazioni
In entrambi i casi l’errore cresce illimitatamente
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Localizzazione assoluta
• Sistema di riferimento fisso
• Non si ha una stima iniziale
• Stima della posizione assoluta rispetto al riferimento fisso
Map matching: confronto tra mappa locale e mappa globale
Active beacons: triangolazione da beacons in posizione nota
Landmarks naturali e artificiali: triangolazione da punti caratteristici
dell’ambiente
Tipicamente si usano entrambi i tipi di localizzazione
contemporaneamente
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Formalizzazione del problema
Stato del robot: posizione e orientamento
X = (x, y, ϑ )
Modello cinematico:
X k +1 = f (X k , U k , N k )

 Z k = h (X k , Wk , M )
Ipotesi sul rumore: rumore bianco gaussiano e indipendente
N k ~ N(0, Q k )
Wk ~ N(0, R k )
Ad ogni passo si vuole calcolare la stima dello stato
“minimizzando” la varianza dell’errore
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Descrizione della sedia a rotelle a guida assistita
• Sedia a rotelle motorizzata di tipo
convenzionale
• Componenti aggiunte
– Encoder
– Giroscopio
– Sistema sensoriale ad ultrasuoni
– PC portatile con scheda sonora
– Cuscinetti per la misura della
pressione
– Comandi vocali
• Specifiche di progetto: low-cost
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Funzionalità implementate
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Encoders ottici incrementali
• Misurano la velocità di rotazione dei motori
• Sono costituiti da:
– disco di materiale trasparente su cui sono state
stampate (100) strisce nere
– un sensore di tacca a infrarosso che rivela
il passaggio della striscia
– un contatore di tacche
• La risoluzione viene aumentata dal rapporto di riduzione (0.17mm)
• Errori
– errore di quantizzazione a basse velocità
– incapacità di rilevare il verso di rotazione
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Giroscopio piezoelettrico
• Misura la velocità angolare del veicolo
• E’ costituito da:
– prisma triangolare di “Elinvar”
– trasduttori piezoelettrici
posti sulle facce del prisma
A riposo
In rotazione
Le oscillazioni
risultanti rilevate
sono diverse
Elementi
riceventi
• Usa l’effetto dell’accelerazione
A
di Coriolis
B
A
C
B
C
• Errori
– deriva non costante nel
tempo dovuta ad effetti termici
Elemento
oscillante
oscillazione
Le oscillazioni
rilevate si
cancellano una
con l’altra
Elemento
oscillante
oscillazione
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Oscillazioni
di Coriolis
Sensori ad ultrasuoni
• Misurano la distanza dagli ostacoli
• Sono costituiti da
S1
– emettitore di onde acustiche (40 Khz)
– ricevitore di onde acustiche
S4
S3
– misuratore del tempo di volo (TOF)
1
d = v(TOF )
2
• Errori
– variazioni di v dovuti a variazioni di umidità e temperatura
– errori di quantizzazione dovuti alla misura del tempo di volo
– Interazioni con l’ambiente
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
S5
S2
Interazione sonar-ambiente
• L’ampio lobo di radiazione (80°) genera incertezza nel
posizionamento angolare dell’oggetto che ha dato luogo all’eco
• Quando l’onda acustica investe un oggetto
– una parte dell’energia viene riflessa
– una parte viene dispersa
– una parte viene assorbita
Se l’energia riflessa è troppo bassa l’oggetto
non viene rilevato
• Cammini multipli: l’onda riflessa non è visibile dal ricevitore
distanza misurata > distanza reale
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Scheda di acquisizione dati
Giroscopio
Encoders
Motori
Joystick
Scheda
Acquisizione Dati
Chopper
Cuscinetti
Personal
Computer
Microfono
Sound Blaster
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Modello cinematico della carrozzella
• Modello dell’uniciclo tempo-continuo
 x = u cos(ϑ )

 y = u sin(ϑ )
ϑ = ω

• Modello dell’uniciclo discretizzato
nel periodo T
∆ϑk

=
+
∆
⋅
+
ϑ
x
x
D
cos(
)
k
k
k
 k +1
2

∆ϑk

=
+
∆
⋅
+
ϑ
y
y
D
sin(
)
 k +1
k
k
k
2

ϑk +1 = ϑk + ∆ϑk

Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Calcolo dell’odometria
 ∆Lk = NLk ⋅ S ⋅ ρ

∆Rk = NRk ⋅ S ⋅ ρ
spostamento ruota sinistra
spostamento ruota destra
S
spostamento angolare della ruota relativo all’incremento unitario
dell’encoder
raggio della ruota
ρ
∆Rk + ∆Lk

∆Dk =

2
∆ϑk = ω gyro ⋅ T
Senza giroscopio
∆Rk − ∆Lk
∆ϑ k =
2a
spostamento lineare
rotazione
a lunghezza semiasse tra le ruote
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Errori sistematici e non dell’odometria
• Sistematici
Si accumulano costantemente
– Diverso diametro delle ruote
– Parametri reali diversi da quelli nominali
– Disallineamento delle ruote
– Risoluzione finita degli encoders
– Deriva di velocità nel giroscopio
• Non Sistematici
Non si sa quando si verificano
– Cammino su superfici non perfettamente piane
– Slittamento delle ruote dovuto a:
•
•
•
•
•
•
superfici non aderenti
sovraccelerazioni
sterzate veloci
forze esterne
forze interne (castors)
assenza di punti di contatto tra ruota e pavimento
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Rappresentazione dell’incertezza
Per rappresentare l’incertezza dovuta agli errori di misura si introduce il
rumore additivo gaussiano bianco Nk~N(0,Qk) nelle equazioni cinematiche
∆ϑk

x
+
=
+
∆
⋅
+
ϑ
cos(
)
x
x
D
n
k
k
k
k
 k +1
2

∆ϑk

) + nky
 y k +1 = y k + ∆Dk ⋅ sin(ϑk +
2

ϑk +1 = ϑk + ∆ϑk + nkϑ

Posizione di
partenza
Percorso
stimato
P
zj − Sj ⋅ g
zj
zj
zj + Sj ⋅ g
Ellissoidi
rappresentanti
l’incertezza
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Rappresentazione del’ambiente
• Ambiente tipo ufficio
• Pareti orizzontali-verticali
• Lista di segmenti specificati dalle
coordinate degli estremi
(x1,y1,x2,y2)
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Modello dell’uscita
• Le uscite sono rappresentate dalle misure di distanza fornite dai
sensori ad ultrasuoni
• Non si ha una unica espressione in forma chiusa perché la misura
dipende dalla parete di incidenza
– Parete verticale
z k = x p − ( xk + d cos(ϑk + ϕ ))
– Parete orizzontale
z k = y p − ( y k + d sin(ϑk + ϕ ))
x
xs
xp
σd
• A causa degli errori (sistematici e non)
si aggiunge l’incertezza
D
Z k = h( X k , M ) + Wk
Wk ~ N (0, Rk )
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
σw
σw
Modello complessivo del sistema
X k +1 = f (X k , U k , N k )

 Z k = h (X k , Wk , M )
N k ~ N(0, Q k )
∆ϑk

x
x
x
D
n
cos(
)
=
+
∆
⋅
+
+
ϑ
k
k
k
k
 k +1
2

∆ϑk

y
y
D
sin(
) + nky
ϑk +
 k +1 = k + ∆ k ⋅
2

ϑk +1 = ϑk + ∆ϑk + nkϑ

 z k = x p − ( x k + d cos(ϑ k + ϕ )) + wk

 z k = y p − ( y k + d sin(ϑ k + ϕ )) + wk
parete verticale
parete orizzontale
Wk ~ N(0, R k )
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Schema generale di filtraggio
Xk
Uk
Sistema
Sistema
Zk
Filtro
Predittore
Predittore
+
Errore di stima
Correttore
Correttore
X̂k
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Filtro di Kalman
• Si applica al caso di sistemi lineari con rumore additivo gaussiano
bianco
N k ≈ N (0, Q k )
X k +1 = AX k + BU k + N k

Z k = CX k + Wk
Wk ≈ N (0, R k )
• Si dimostra che il filtro converge alla stima ottima, cioè quella che
minimizza la varianza dell’errore di stima
Xˆ k = Xˆ k |k −1 + K k (z k − CXˆ k |k −1 )
Xˆ k |k −1 = AXˆ k −1 + BU k −1
Xˆ k = Xˆ k|k −1 + K k (z k − CXˆ k|k −1 )
Pk |k −1 = APk −1 A + Q
Xˆ k|k −1 = AXˆ k −1 + BU k −1
S k = CPk |k −1C + Rk
Pk|k −1 = APk −1 AT + Qk
T
T
K k = Pk |k −1CS k
−1
Pk = Pk |k −1 − K k S k K kT
[
−1
Pk = Pk|k −1 I + C R CPk|k −1
T
]
K k = Pk CR −1
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
−1
Commenti al filtro di Kalman
•
Filtro lineare ottimo ricorsivo
•
Minimizza la varianza dell’errore di stima. Le ipotesi che assicurano l’ottimalità
sono:
– linearità del sistema
– rumore additivo gaussiano bianco
•
Tutte le variabili aleatorie in gioco sono gaussiane perché trasformazioni lineari
di v.a. gaussiane
•
Il filtro è esso stesso un sistema lineare nello stato e nelle misure
•
Nessun filtro non lineare può fare meglio!!!!!
•
La ricorsività permette di processare ad ogni passo soltanto le misure relative a
quel passo, ottenendo pero’ lo stesso risultato che si otterrebbe processando
tutte le misure contemporaneamente: ciò significa che ad ogni passo viene
estratta tutta l’informazione contenuta nelle misure
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Schema a blocchi del KF
Filtro
Predittore
Uk
Xˆ k |k −1 = AXˆ k −1 + BU k −1
Pk |k −1 = APk −1 AT + Q
Zk
Correttore
Xˆ k = Xˆ k|k −1 + K k (z k − CXˆ k|k −1 )
[
Pk = Pk|k −1 I + C T R −1CPk|k −1
]
−1
K k = Pk CR −1
N.B. Se C=I K=PR-1 si nota che K è direttamente proporzionale all’incertezza
della stima odometrica e inversamente proporzionale all’incertezza delle misure
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
X̂k
Filtro di Kalman Esteso
• Si applica al caso di sistemi non lineari con rumore additivo gaussiano
bianco
N k ~ N(0, Q k )
X k +1 = f (X k , U k , N k )

Wk ~ N(0, R k )
 Z k = h (X k , Wk , M )
• Si utilizza una linearizzazione del sistema intorno alla predizione
X k = X k |k −1 + K k (z k − h( X k |k −1 ))
X k | k −1 = f ( X k −1 , U k −1 )
K k = Pk |k −1 H kT S k −1
Pk |k −1 = Fk Pk −1Fk T + Qk
S k = H k Pk | k −1 H k T + Rk
Pk = Pk | k −1 − K k S k K kT
Fk = J xf ( Xˆ k |k −1 )
H k = J hx ( Xˆ k |k −1 )
• Si perde la convergenza e l’ottimalità
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Commenti al filtro di Kalman Esteso
• Filtro ricorsivo
• Non si può assicurare l’ottimalità in generale
• Le variabili aleatorie in gioco non sono più gaussiane
• Il filtro è lineare nelle misure ma non nello stato
• Un filtro non lineare può fare meglio?
• La matrice di guadagno K è ottenuta componendo le matrici di
covarianza ottenute linearizzando intorno alla stima di predizione che è
una variabile aleatoria. Ne segue che K è una variabile aleatoria
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Schema a blocchi dell’EKF
Filtro
Predittore
Uk
Correttore
[
Xˆ k = Xˆ k |k −1 + K k Z k − h( Xˆ k |k −1 , M )
Xˆ k|k −1 = f ( Xˆ k −1 ,U k )
K k = Pk |k −1 H k Sk
Pk|k −1 = Fk Pk −1Fk + Qk
Sk = H k Pk |k −1 H kT + Rk
T
T
−1
Pk = Pk |k −1 − K k S k K kT
Zk
Fk = J xf ( Xˆ k |k −1 )
H k = J hx ( Xˆ k |k −1 )
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
]
X̂k
Predittore
• La predizione fa essenzialmente uso dell’odometria
∆ϑ k

ˆ
 xˆ k + 1 = xˆ k + ∆ D k ⋅ cos( ϑ k + 2 )

∆ϑk

ˆ
)
 yˆ k + 1 = yˆ k + ∆ D k ⋅ sin( ϑ k +
2

ϑˆ k + 1 = ϑˆ k + ∆ ϑ k

• La covarianza Pk|k-1 esprime l’incertezza della stima odometrica ed è
composta da:
– propagazione dell’incertezza della stima precedente
– incertezza introdotta dal rumore
• Nota che Pk|k-1>Pk sempre
l’ellissoide si espande
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Correttore
[
]
Xˆ k = Xˆ k |k −1 + K k Z k − h( Xˆ k |k −1 , M ) (1)
K k = Pk |k −1 H k Sk
T
−1
(2)
Sk = H k Pk |k −1 H kT + Rk
(3)
Pk = Pk |k −1 − K k S k K kT
(4)
• (1) rappresenta la stima finale: il termine tra parentesi quadre viene detto
innovazione e rappresenta l’informazione apportata dal processo di misura
• (2) guadagno di Kalman
• (3) covarianza dell’innovazione: compone due tipi di incertezze
– incertezza sulla stima odometrica
– incertezza sulle misure
• (4) la covarianza dell’errore di stima diminuisce dopo la correzione
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Covarianza dell’errore di stima
Oggetto
noto
Se il veicolo si muove lungo una direzione
l’informazione di posizione diventa
imprecisa lungo tale direzione.
Aggiornamento
della
posizione
Inizio
Pk = Fk ⋅ Pk −1 ⋅ Fk + Qk − K k ⋅ S k ⋅ K k
T
T
1.3
: incertezza x
: incertezza y
: incertezza theta
y
0
x
t
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Matching delle misure
•
Alcuni errori tipici dei sensori ad ultrasuoni non possono essere modellati con
rumore gaussiano bianco
Pareti NON VISTE dal sonar
– pareti non viste
– cammini multipli
– ostacoli non previsti nella mappa
•
E’ necessario scartare tali misure perché
Smax
l’innovazione apportata potrebbe introdurre
errori molto grandi nella stima
Letture reali, al
k-esimo istante
Letture stimate, al
k-esimo istante
MATCHING
Incertezza
sulla posizione
stimata
innovazione
v j = z j − zˆ j
s j = ∇h Χˆ
vj ⋅
⋅ Ρk / k −1 ⋅ ∇h Χˆ
T
k / k −1
k / k −1
1
⋅vj ≤ g2
sj
g e’ il gate di probabilità
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
+ Rj
Esperimento 1: localizzazione senza giroscopio
•
La sedia a rotelle è stata guidata con il
joystick perr circa 24 m. evitando slittamenti
e brusche sterzate.
•
La localizzazione è stata effettuata a partire
dalle misure degli encoders e dei sonar con
due metodi diversirosso:
– EKF estimate
– Blu: Stima odometrica
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 1: propagazione dll’incertezza usando l’EKF
Misure non usate perche’ fuori
della distanza massima misurabile
+ misure attese
-- misure reali
Sonar frontale
Sonar destro
Coarianza lungo l’asse x
Sonar sinistro
Coarianza lungo l’asse y
Metà
percorso
Covarianza dell’orientamento
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 2 : matching delle misure
Questi ostacoli non
sono nella mappa
2
1
Sonar frontale
Sonar destro
2
1
Sonar sinistro
+ misure attese
-- misure reali
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 3 : localizzazione con giroscopio
Confronto
Confrontotra
tragli
gliangoli
angoli
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 4: cfr algoritmi con e senza giroscopio
Confronto
Confrontotra
tragli
gliangoli
angoli
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 4: risultati con e senza giroscopio
Covarianza
Covarianza
Senza
Senzagiroscopio
giroscopio
Con
Congiroscopio
giroscopio
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 5: cfr algoritmi con e senza giroscopio
Confronto
Confrontotra
tragli
gliangoli
angoli
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 5: risultati con e senza giroscopio
Senza
Senzagiroscopio
giroscopio
Diverge!!!!
Diverge!!!!
Con
Congiroscopio
giroscopio
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99
Esperimento 5: matching letture sonar
Nessuna
Nessuna
lettura
lettura
convalidata
convalidata
Ostacoli non
Ostacoli non
previsti
previsti
Durante il primo
Durante il primo
passaggio questo
passaggio questo
spazio era stato
spazio era stato
chiuso
chiuso
Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99