Filtro di Kalman Esteso applicato al problema della ... · Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99 Filtro di Kalman Esteso applicato

Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99

Filtro di Kalman Esteso applicato al problema della localizzazione nella robotica mobile

Ing. Elisabetta Fabrizi


OverviewOverview

• Metodi di localizzazione

– localizzazione relativa

– localizzazione assoluta

• Localizzazione per sedia a rotelle

– descrizione del sistema

– descrizione dei sensori

• encoders

• giroscopio

• sonar

• Modellizzazione

– modello cinematico con rumore

– modello uscite con rumore

• Filtro di Kalman Esteso

– richiami sul Filtro di Kalman

– confronto tra KF e EKF

• Applicazione alla localizzazione

– schema generale

– predizione

– correzione

– matching delle uscite

• Risultati sperimentali

– con giroscopio

– senza giroscopio

• Conclusioni


La localizzazioneLa 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)


Perché il robot “si perde”: un esempioPerché 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.


Localizzazione relativaLocalizzazione 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


Localizzazione assolutaLocalizzazione 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


Formalizzazione del problemaFormalizzazione del problema

Stato del robot: posizione e orientamento

Modello cinematico:

Ipotesi sul rumore: rumore bianco gaussiano e indipendente

Ad ogni passo si vuole calcolare la stima dello stato

“minimizzando” la varianza dell’errore

==+

), W,(Xh Z

)N,U,(X fX

kkk

kkk1k

M

) y, (x,X ϑ=

)RN(0,~W

)QN(0,~N

kk

kk


Descrizione della sedia a rotelle a guida assistita 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


Funzionalità implementateFunzionalità implementate


EncodersEncoders ottici incrementaliottici 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


Giroscopio piezoelettricoGiroscopio piezoelettrico

• Misura la velocità angolare del veicolo

• E’ costituito da:

– prisma triangolare di “Elinvar”

– trasduttori piezoelettrici

posti sulle facce del prisma

• Usa l’effetto dell’accelerazione

di Coriolis

• Errori

– deriva non costante nel

tempo dovuta ad effetti termici

C

A riposo

Elementooscillante

oscillazione

Elementiriceventi

Le oscillazioni rilevate si cancellano una con l’altra

C

In rotazione

oscillazione

Le oscillazioni risultanti rilevate sono diverse

Elementooscillante Oscillazioni

di Coriolis

BA BA


Sensori ad ultrasuoniSensori ad ultrasuoni

• Misurano la distanza dagli ostacoli

• Sono costituiti da

– emettitore di onde acustiche (40 Khz)

– ricevitore di onde acustiche

– misuratore del tempo di volo (TOF)

• 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

)(21

TOFvd =

S1

S5

S2S3

S4


Interazione sonarInterazione sonar--ambienteambiente

• 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


Scheda di acquisizione datiScheda di acquisizione dati

Scheda Acquisizione Dati

Encoders Motori

ChopperJoystick

PersonalComputer

Sound BlasterMicrofono

Cuscinetti

Giroscopio


• Modello dell’uniciclo tempo-continuo

• Modello dell’uniciclo discretizzatonel periodo T

===

ωϑϑϑ

�

�

�

)sin(

)cos(

uy

ux

ModelloModello cinematicocinematico della carrozzella della carrozzella

∆+=

∆+⋅∆+=

∆+⋅∆+=

+

+

+

kkk

kkkkk

kkkkk

Dyy

Dxx

ϑϑϑ

ϑϑ

ϑϑ

1

1

1

)2

sin(

)2

cos(


spostamento ruota sinistra

spostamento ruota destra

S spostamento angolare della ruota relativo all’incremento unitariodell’encoderρ raggio della ruota

spostamento lineare

rotazione

Senza giroscopio

a lunghezza semiasse tra le ruote

CalcoloCalcolo dell’odometriadell’odometria

⋅⋅=∆⋅⋅=∆ρρ

SNRR

SNLL

kk

kk

⋅=∆

∆+∆=∆

T

LRD

gyrok

kkk

ωϑ2

a

LR kkk 2

∆−∆=∆ϑ


Errori sistematici e nonErrori sistematici e non dell’odometriadell’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


Rappresentazione dell’incertezzaRappresentazione 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

+∆+=

+∆+⋅∆+=

+∆+⋅∆+=

+

+

+

ϑϑϑϑ

ϑϑ

ϑϑ

kkkk

yk

kkkkk

xk

kkkkk

n

nDyy

nDxx

1

1

1

)2

sin(

)2

cos(

Posizione di partenza

Percorsostimato

Ellissoidi rappresentanti l’incertezza

�zj �z S gj j+ ⋅�z S gj j− ⋅zj

P


RappresentazioneRappresentazione del’ambientedel’ambiente

• Ambiente tipo ufficio

• Pareti orizzontali-verticali

• Lista di segmenti specificati dalle

coordinate degli estremi

(x1,y1,x2,y2)


• 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

– Parete orizzontale

• A causa degli errori (sistematici e non)

si aggiunge l’incertezza

Modello dell’uscitaModello dell’uscita

))cos(( ϕϑ ++−= kkpk dxxz

))sin(( ϕϑ ++−= kkpk dyyzσd

σw

σw

D

xs xpx

),0(~),( kkkkk RNWWMXhZ +=


Modello complessivo del sistemaModello complessivo del sistema

+∆+=

+∆+⋅∆+=

+∆+⋅∆+=

+

+

+

ϑϑϑϑ

ϑϑ

ϑϑ

kkkk

yk

kkkkk

xk

kkkkk

n

nDyy

nDxx

1

1

1

)2

sin(

)2

cos(

==+

), W,(Xh Z

)N,U,(X fX

kkk

kkk1k

M

z x x d w

z y y d wk p k k k

k p k k k

= − + + += − + + +

( cos( ))

( sin( ))

ϑ ϕϑ ϕ

parete verticale

parete orizzontale

)RN(0,~W

)QN(0,~N

kk

kk


Schema generale di filtraggioSchema generale di filtraggio

SistemaSistema

Errore di stima+

-

kX

kX̂

kU

kZ

PredittorePredittore CorrettoreCorrettore

Filtro


Filtro di Filtro di KalmanKalman

• Si applica al caso di sistemi lineari con rumore additivo gaussiano bianco

• Si dimostra che il filtro converge alla stima ottima, cioè quella che minimizza la varianza dell’errore di stima

≈+=

≈++=+

)R,0(NWWCXZ

)Q,0(NNNBUAXX

kkkkk

kkkkk1k

( )

Tkkkkkk

kkkk

kT

kkk

Tkkk

kkkk

kkkkkkk

KSKPP

CSPK

RCCPS

QAAPP

BUXAX

XCzKXX

−=

=

+=

+=

+=

−+=

−

−−

−

−−

−−−

−−

1|

11|

1|

11|

111|

1|1|

ˆˆ

ˆˆˆ

( )

[ ]1

1

11

1

11

111

11

ˆˆ

ˆˆˆ

−

−−

−−

−−

−−−

−−

=

+=

+=

+=

−+=

CRPK

CPRCIPP

QAAPP

BUXAX

XCzKXX

kk

k|kT

k|kk

kT

kk|k

kkk|k

k|kkkk|kk


Commenti al filtro diCommenti al filtro di KalmanKalman

• 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


FiltroFiltro

Schema a blocchi del KFSchema a blocchi del KF

PredittorePredittore CorrettoreCorrettore

kZ

kUkX̂

QAAPP

BUXAX

Tkkk

kkkk

+=

+=

−−

−−−

11|

111|ˆˆ ( )

[ ]1

1

11

1

11ˆˆˆ

−

−−

−−

−−

=

+=

−+=

CRPK

CPRCIPP

XCzKXX

kk

k|kT

k|kk

k|kkkk|kk

N.B.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


Filtro di Filtro di Kalman Kalman EstesoEsteso

• Si applica al caso di sistemi non lineari con rumore additivo gaussiano bianco

• Si utilizza una linearizzazione del sistema intorno alla predizione

• Si perde la convergenza e l’ottimalità

( )� � ( � )� ( � , )

| |

|

|

|

|

|

X X K z h X

X f X U

K P H S

P F P F Q

S H P H R

P P K S K

k k k k k k k

k k k k

k k k kT

k

k k k k kT

k

k k k k kT

k

k k k k k kT

= + −=

== +

= += −

− −

− − −

−−

− −

−

−

1 1

1 1 1

11

1 1

1

1

==+

), W,(Xh Z

)N,U,(X fX

kkk

kkk1k

M )RN(0,~W

)QN(0,~N

kk

kk

)ˆ( 1| −= kkxfk XJF

)ˆ( 1| −= kkxhk XJH


Commenti al filtro diCommenti al filtro di KalmanKalman EstesoEsteso

• 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 dicovarianza ottenute linearizzando intorno alla stima di predizione che è una variabile aleatoria. Ne segue che K è una variabile aleatoria


Filtro

Schema a blocchi dell’EKFSchema a blocchi dell’EKF

Predittore Correttore

kT

kkkkk

kkkk

QFPFP

UXfX

+=

=

−−

−−

11|

11| ),ˆ(ˆ

[ ]

Tkkkkkk

kTkkkkk

kT

kkkk

kkkkkkk

KSKPP

RHPHS

SHPK

MXhZKXX

−=

+=

=

−+=

−

−

−−

−−

1|

1|

11|

1|1| ),ˆ(ˆˆ

kZ

kUkX̂

)ˆ( 1| −= kkxfk XJF )ˆ( 1| −= kk

xhk XJH


PredittorePredittore

• La predizione fa essenzialmente uso dell’odometria

• 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

∆+=

∆+⋅∆+=

∆+⋅∆+=

+

+

+

kkk

kkkkk

kkkkk

Dyy

Dxx

ϑϑϑ

ϑϑ

ϑϑ

ˆˆ

)2

ˆsin(ˆˆ

)2

ˆcos(ˆˆ

1

1

1


[ ]

)4(

)3(

)2(

)1(),ˆ(ˆˆ

1|

1|

11|

1|1|

Tkkkkkk

kTkkkkk

kT

kkkk

kkkkkkk

KSKPP

RHPHS

SHPK

MXhZKXX

−=

+=

=

−+=

−

−

−−

−−

CorrettoreCorrettore

• (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


Se il veicolo si muove lungo una direzionel’informazione di posizione diventa

imprecisa lungo tale direzione.

Aggiornamentodella

posizioneInizio

Oggettonoto

x

y

0

1.3

t

: incertezza x: incertezza y: incertezza theta

Tkkkk

Tkkkk KSKQFPFP ⋅⋅−+⋅⋅= −1

CovarianzaCovarianza dell’errore di stimadell’errore di stima


• Alcuni errori tipici dei sensori ad ultrasuoni non possono essere modellati con

rumore gaussiano bianco

– pareti non viste

– cammini multipli

– ostacoli non previsti nella mappa

• E’ necessario scartare tali misure perché

l’innovazione apportata potrebbe introdurre

errori molto grandi nella stima

MatchingMatching delle misuredelle misure

Pareti NON VISTE dal sonar

Smax

MATCHING

Letture stimate, al k-esimo istante

Letture reali, al k-esimo istante

Incertezza sulla posizione stimata

jjj zzv ˆ−=

jT

kkj Rhhskkkk

+∇⋅Ρ⋅∇=−− Χ−Χ1/1/

ˆ1/ˆ

21gv

sv j

jj ≤⋅⋅

g e’ il gate di probabilitàinnovazione


EsperimentoEsperimento 1: 1: localizzazione senza giroscopiolocalizzazione 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


Misure non usate perche’ fuori della distanza massima misurabile

Sonar frontale

Sonar destro

Coarianza lungo l’asse x

Covarianza dell’orientamento

Sonar sinistro

+ misure attese-- misure reali

Metàpercorso

Coarianza lungo l’asse y

EsperimentoEsperimento 1: 1: propagazione dll’incertezza usando l’EKFpropagazione dll’incertezza usando l’EKF


Questi ostacoli nonsono nella mappa

+ misure attese-- misure reali

1 2

2

1

Sonar frontale

Sonar destro

Sonar sinistro

EsperimentoEsperimento 2 : matching 2 : matching delle misuredelle misure


EsperimentoEsperimento 3 : 3 : localizzazionelocalizzazione con con giroscopiogiroscopio

Confronto tra gli angoliConfronto tra gli angoli


Esperimento 4:Esperimento 4: cfrcfr algoritmi con e senza giroscopioalgoritmi con e senza giroscopio



Esperimento 4: risultati con e senza giroscopioEsperimento 4: risultati con e senza giroscopio

Senza giroscopioSenza giroscopio

Con giroscopioCon giroscopio

CovarianzaCovarianza


Esperimento 5: Esperimento 5: cfrcfr algoritmi con e senza giroscopioalgoritmi con e senza giroscopio



Esperimento 5: risultati con e senza giroscopioEsperimento 5: risultati con e senza giroscopio

Senza giroscopioSenza giroscopio

Con giroscopioCon giroscopio

Diverge!!!!Diverge!!!!


Esperimento 5:Esperimento 5: matchingmatching letture sonarletture sonar

Nessuna lettura

convalidata

Nessuna lettura

convalidata

Ostacoli nonprevisti

Ostacoli nonprevisti

Durante il primopassaggio questospazio era stato

chiuso

Durante il primopassaggio questospazio era stato

chiuso

Documents

Filtro di Kalman Esteso applicato al problema della ... · Università degli Studi di Roma Tre - Strumentazione e misure per l’automazione A.A.98/99 Filtro di Kalman Esteso applicato