Filtro di Kalman Esteso applicato al problema della localizzazione nella robotica mobile Ing. Elisabetta Fabrizi
Overview 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 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
erché il robot si perde : un esempio Dead reconing 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. ero 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 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 assoluta Sistema di riferimento fisso Nonsi 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 Landmars naturali e artificiali: triangolazione da punti caratteristici dell ambiente Tipicamente si usano entrambi i tipi di localizzazione contemporaneamente
Formalizzazione del problema Stato del robot: posizione e orientamento Modello cinematico: X (x, y, X Z f Ipotesi sul rumore: rumore bianco gaussiano e indipendente (X h (X, U, N, W, M N W ~ ~ N(0,Q N(0,R Ad ogni passo si vuole calcolare la stima dello stato minimizzando la varianza dell errore
Descrizione della sedia a rotelle a guida assistita Sedia a rotelle motorizzata di tipo convenzionale Componenti aggiunte Encoder Giroscopio Sistema sensoriale ad ultrasuoni C portatile con scheda sonora Cuscinetti per la misura della pressione Comandi vocali Specifiche di progetto: low-cost
Funzionalità implementate
Encoders ottici incrementali Misurano la velocità di rotazione dei motori Sono costituiti da: disco di materiale trasparente su cui sono state stampate (00 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.7mm Errori errore di quantizzazione a basse velocità incapacità di rilevare il verso di rotazione
Giroscopio piezoelettrico Misura la velocità angolare del veicolo E costituito da: prisma triangolare di Elinvar A riposo In rotazione trasduttori piezoelettrici posti sulle facce del prisma Elementi riceventi Le oscillazioni risultanti rilevate sono diverse Usa l effetto dell accelerazione di Coriolis A C B A C B 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 Oscillazioni di Coriolis
Sensori 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 S4 S3 S S5 S2 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
Interazione sonar-ambiente 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
Scheda di acquisizione dati Giroscopio Encoders Motori Joystic Scheda Acquisizione Dati Chopper Cuscinetti ersonal Computer Microfono Sound Blaster
Modello dell uniciclo tempo-continuo Modello dell uniciclo discretizzato nel periodo T ω sin( cos( u y u x Modello Modello cinematico cinematico della carrozzella della carrozzella D y y D x x 2 sin( 2 cos(
Calcolo dell odometria L R NL NR S ρ S ρ spostamento ruota sinistra spostamento ruota destra S spostamento angolare della ruota relativo all incremento unitario dell encoder ρ raggio della ruota D R ω gyro L 2 T spostamento lineare rotazione Senza giroscopio R L 2a a lunghezza semiasse tra le ruote
Errori sistematici e non dell odometria Sistematici Si accumulano costantemente Diverso diametro delle ruote arametri 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 incertezza er rappresentare l incertezza dovuta agli errori di misura si introduce il rumore additivo gaussiano bianco N ~N(0,Q nelle equazioni cinematiche x y x y D D osizione di partenza cos( sin( n 2 2 ercorso stimato n n x y Ellissoidi rappresentanti l incertezza z S g j j z j z S g j j z j
Rappresentazione del ambiente Ambiente tipo ufficio areti orizzontali-verticali Lista di segmenti specificati dalle coordinate degli estremi (x,y,x2,y2
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 arete verticale z x ( x d cos( ϕ p arete orizzontale z ( y d sin( ϕ y p x x s x p A causa degli errori (sistematici e non σ d si aggiunge l incertezza D Z h( X, M W W ~ N(0, R σ w σ w
Modello complessivo del sistema Modello complessivo del sistema y x n n D y y n D x x 2 sin( 2 cos( W,, h (X Z N, U, (X f X M z x x d w z y y d w p p ( cos( ( sin( ϕ ϕ parete verticale parete orizzontale N(0,R ~ W N(0,Q ~ N
Schema generale di filtraggio U X Sistema Z Errore di stima redittore Filtro Correttore - X
Filtro di Filtro di Kalman Kalman 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 N(0, W W CX Z N(0,Q N N BU AX X ( T T T K S K CS K R C C S Q A A BU AX X CX z K X X ( [ ] CR K C R C I Q A A BU AX X CX z K X X T T
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
Schema a blocchi del KF Filtro redittore Correttore U Z X AX A A BU T Q X K X CR ( K z CX T [ I C R C ] X N.B. Se CI KR - si nota che K è direttamente proporzionale all incertezza della stima odometrica e inversamente proporzionale all incertezza delle misure
Filtro di Kalman Esteso Si applica al caso di sistemi non lineari con rumore additivo gaussiano bianco X f (X, U, N N ~ N(0,Q Z h (X, W, M W ~ N(0,R Si utilizza una linearizzazione del sistema intorno alla predizione ( X ( X K z h X X ( f X, U T K HS T F F Q T S H H R T K S K Si perde la convergenza e l ottimalità F H X x J f ( X x J h (
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
Filtro Schema a blocchi dell EKF Schema a blocchi dell EKF redittore Correttore T Q F F U X f X, ( [ ] T T T K S K R H H S S H K M X h Z K X X, ( Z U X ( x f X J F ( x h X J H
redittore redittore La predizione fa essenzialmente uso dell odometria La covarianza - esprime l incertezza della stima odometrica ed è composta da: propagazione dell incertezza della stima precedente incertezza introdotta dal rumore Nota che - > sempre l ellissoide si espande D y y D x x 2 sin( 2 cos(
[ ] (4 (3 (2 (, ( T T T K S K R H H S S H K M X h Z K X X Correttore Correttore ( 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
Covarianza dell errore di stima Se il veicolo si muove lungo una direzione l informazione di posizione diventa imprecisa lungo tale direzione. Oggetto noto Aggiornamento della posizione Inizio F F T Q K S K T.3 y 0 x t : incertezza x : incertezza y : incertezza theta
Matching delle misure Alcuni errori tipici dei sensori ad ultrasuoni non possono essere modellati con rumore gaussiano bianco areti NON VISTE dal sonar 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 Letture stimate, al -esimo istante Incertezza sulla posizione stimata MATCHING Letture reali, al -esimo istante innovazione v s v j j j z j h s j v z j Ρ / h T Χ Χ / / j g 2 g e il gate di probabilità Smax R j
Esperimento : localizzazione senza giroscopio La sedia a rotelle è stata guidata con il joystic 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
Esperimento : propagazione dll incertezza usando l EKF Misure non usate perche fuori della distanza massima misurabile Sonar frontale misure attese -- misure reali Sonar destro Coarianza lungo l asse x Sonar sinistro Coarianza lungo l asse y Metà percorso Covarianza dell orientamento
Esperimento 2 : matching delle misure Questi ostacoli non sono nella mappa Sonar frontale 2 Sonar destro 2 misure attese -- misure reali Sonar sinistro
Esperimento 3 : localizzazione con giroscopio Confronto tra tra gli gli angoli angoli
Esperimento 4: cfr algoritmi con e senza giroscopio Confronto tra tra gli gli angoli angoli
Esperimento 4: risultati con e senza giroscopio Senza Senza giroscopio Covarianza Con Con giroscopio
Esperimento 5: cfr algoritmi con e senza giroscopio Confronto tra tra gli gli angoli angoli
Esperimento 5: risultati con e senza giroscopio Senza Senza giroscopio Diverge!!!! Con Con giroscopio
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