Grundlagen der Grundlagen der Positionsbestimmung mit Positionsbestimmung mit

PartikelfilternPartikelfiltern

Vertiefer-Seminar Geoinformation WS 02/03 07.11.2002 Silvia Becker

2

InhaltInhalt

- Prinzip des Partikelfilters am Bsp. mobiler Roboter - Bayes-Filter - Algorithmus - Verteilungen

- Vergleich zur Grid-based Localization

- Rao-Blackwellization

- Anwendungen

- Einleitung

- Fazit

3

- Monte Carlo Localization, Survival of the fittest, Sequential Sampling-Importance-Resampling

Ziel: Schätzung der aktuellen Position aufgrund von (Roboter-)Bewegung, Sensormessungen und vorheriger Position

- Algorithmus für Lokalisation

EinleitungEinleitung

4

Bsp.: Lokalisation mobiler Bsp.: Lokalisation mobiler RoboterRoboter

Globale Lokalisation mit 10000 Partikeln

5

Bsp.: Lokalisation mobiler Bsp.: Lokalisation mobiler RoboterRoboter

Nach 1 m Bewegung

6

Bsp.: Lokalisation mobiler Bsp.: Lokalisation mobiler RoboterRoboter

Nach weiteren 2 mBewegung

7

Bayes-Filter IBayes-Filter I- auch Markov Localization

- Abschätzung von Zustand ´x´eines dynamischen Systems aufgrund von Sensormessungen

3-dim Positionsvektor (x,y,o)

mobiler Roboter + Umgebungz.B. Entfernungsmessungen,

Kamerabilder

- Voraussetzung: Markov-Umgebung, d.h. frühere und spätere Daten sind gegenseitig voneinander unabhängig, falls aktueller Zustand bekannt

8

Bayes-Filter IIBayes-Filter II

Nutzung einer Wahrscheinlichkeitsverteilung über Menge aller potentiellen Positionen

- Idee: Schätzung der a-posteriori-Wahrscheinlichkeitsdichte über dem Zustandsraum aufgrund bestimmter Daten

ttt dxpxBel ...0

Zustands-Vektor

Daten von Zeitpunkt 0 bis t

´belief´

9

00211 ,,...,,,, yuuyuyxpxBel tttttt

- Aufteilung in ´Perceptual data y´ Wahrnehmung ´Odometry data u´ Bewegung

Bayes-Filter IIIBayes-Filter III

01

0101

,,...,...,,...,,

yuypyuxpyuxyp

xBeltt

tttttt

Bayes- Regel Ap

BpBApABp ii

i

10

1...01

0101

,,...,,...,,

ttt

tttttt duyp

yuxpyuxypxBel

Markov

1...01

01

,,...,

ttt

ttttt duyp

yuxpxypxBel

Bayes-Filter IVBayes-Filter IV

10110111...01

,...,,...,,, tttttt

ttt

tt dxyuxpyuxxpduypxyp

iii dBBpBApApWahrscheinlichkeitSatz der totalen

11

10110111...01

,...,,...,,, tttttt

ttt

ttt dxyuxpyuxxp

duypxyp

xBel

11111...01

,, ttttt

ttt

ttt dxxBeluxxp

duypxyp

xBel

Markov Definition 1txBel

Bayes-Filter VBayes-Filter V

1111, ttttttt dxxBeluxxpxyp

Rekursive`update`-Gleichung bildet Grundlage für späteren Algorithmus

12

Implementierung als Implementierung als PartikelfilterPartikelfilter

Falls kein Anfangsposition vorhanden gleichmäßigeVerteilung der Partikel über dem Zustandsraum mit gleicher Gewichtung m

1

Sample(Partikel)

Gewicht(´Importance Factor´)

miii pxxBel ,...,1, 1;0 ii ppmit

Idee: Repräsentiere durch eine Menge von m gewichteten Samples (Partikel)!

xBel

13

Algorithmus IAlgorithmus IAlgorithmus implementiert Berechnung der `update`-Gleichung von rechts nach links

1

1

111, t

tzurZeitvermutungPositions

t

ModelMotion

ttt

ModelPerceptual

ttt dxxBeluxxpxypxBel

Perceptual Model: Wahrscheinlichkeit der aktuellen Sensoreingaben an Position txMotion Model: Wahrscheinlichkeit von Position nach ausgeführter Bewegung an Position 1tx1tu

tx

14

Algorithmus II Algorithmus II

Sampling

1111, ttttt dxxBeluxxp

handlungs-basierter Teil

Initialisierung

Partikelmenge aufgrund vonAnfangswissen

mm pxpxS ,,...,, 110

repräsentiert 0xBel

Importance sampling

wahrnehmungs-basierter Teil

tt xyp

Resampling

neue Partikelmenge gemäß Wahrscheinlichkeit

repräsentiert txBel

tS

Resampling

neue Partikelmenge gemäß Wahrscheinlichkeit

repräsentiert txBel

tS

Importance sampling

wahrnehmungs-basierter Teil

tt xyp

Sampling

1111, ttttt dxxBeluxxp

handlungs-basierter Teil

15

111,: tttt xBeluxxpqDaher: ´proposal distribution q´ verwenden

diese approximiert den a-posteriori-Wert

Aber: zu schwierig!

Optimal wäre: Partikel direkt vom a-posteriori-Wert erzeugen

11...0

111

,,

ttt

ttttttt udyp

xBelxuxpxypxBel

Proposal DistributionProposal Distribution

Gewichte (´importance factors´) ergeben sich durch Quotienten

11...0

1111111 ,

,,

ttt

tttttttttt udyp

xBelxuxpxypxBeluxxp tt xyp

16

- Hohes Sensorrauschen bewirkt großen Fehler, aber auch: je genauer Sensor, desto größer der Fehler!

- Filter arbeitet schlecht, wenn´proposal distribution´ zu wenig Partikel in Regionen platziert, in denen groß ist txBel

Fehlerquellen IFehlerquellen I

bester Arbeits-bereich 10-20%

MCL mit 1000 Partikeln

17

Fehlerquellen IIFehlerquellen II

- Sensoren liefern schlechte Informationen flach zu große Streuung der Partikel beim Resampling

tt xyp

- Sensoren haben geringes Rauschen, spitz zu wenig Streuung der Partikel

tt xyp

18

bester Arbeits-bereich 10-20%

MCL mit 1000 PartikelnIdee: high error model, d.h. Sensorrauschen überschätzen

Aber: präzise Informationen bleiben unberücksichtigt, Ergebnis ist nicht mehr a-posteriori-Wert

Fehlerquellen III Fehlerquellen III

19

Alternative Verteilung Alternative Verteilung

Aber: unabhängig von und 1txBel1tu

Idee: Verteilung nutzen, die proportional tt xyp

t

tt

yxyp

q

: tttt dxxypy mit

111,: tttt xBeluxxpq

d.h. Partikel gemäß neuester Messungen erzeugenty

20

Gemischte Verteilung IGemischte Verteilung I

qq 1 10 mit Misch-verhältnis

Besser:

q versagt, wenn zu spitz bzw. zu flach wird xyp

q versagt, wenn Sensoren sich irren, da nur neueste Sensormessungen betrachtet werden

Beide Verteilungen nicht optimal:

21

Gemischte Verteilung IIGemischte Verteilung II

gemischte Verteilung mitund 1000 Partikeln

1,0

MCL

Hybrid MCL

´proposal distribution´ undgemischte Verteilung im Vergleich mit 50 Partikeln

22

- 3-dim-Gitter über den Zustandsraum des Roboters legen

- basiert auch auf Markov Localization bzw. Bayes-Filter

Grid-based LocalizationGrid-based Localization

- gehört zu den bisher besten bekannten Algorithmen

- Wahrscheinlich- keit für jede Zelle berechnen

xBel

23

Vergleich Grid-based-Vergleich Grid-based-PartikelfilterPartikelfilter

Genauigkeit der Grid-basedLocalization in Abhängigkeitvon der Gitterauflösung

Genauigkeit vom Partikel-filter in Abhängigkeit von Partikelanzahl

24

Grid-basedPartikelfilter

4cm-Gitter-Auflösung(nicht realisierbar)

Optimale Partikelanzahlzw. 1000 und 5000

20cm-Auflösung benötigt10mal mehr Speicher

5000 Partikel

120 SekundenIn weniger 3 Sekundenglobale Lokalisation

Keine Echtzeit-Lösung´update´ in Echtzeit

klare Überlegenheit von Partikelfiltern!

Partikelfilter vs Grid-basedPartikelfilter vs Grid-based

25

Je mehr Dimensionen, desto mehr Partikel und Rechenaufwand

Problem: Je höher Dimension von Zustandsvektor ´x´, desto schlechter arbeitet Partikelfilter

Dimension von ´x´ möglichst niedrig halten!

Wichtig, um Echtzeit-Lösung zu erhalten

Rao-Blackwellization IRao-Blackwellization I

Rao-Blackwellization

26

Rao-Blackwellization IIRao-Blackwellization II

Idee: Kalman-Filter und Partikelfilter kombinieren

Aufteilung des Zustandsvektors

kf

t

pft

t xx

x

Positions-ableitungen

Anwendung des Kalman-Filters auf linearen Teil,

Partikelfilter auf nicht-linearen Teil

3-dim Positions-koordinaten

27

Rao-Blackwellization IIIRao-Blackwellization III

Integrated Navigation System

------ Partikelfilter mit 60000 Partikeln

Rao-Blackwellizationmit 4000 Partikeln

tpf

t px

t

tkft a

vx;

28

AnwendungenAnwendungen

- Positioning Schätzung der eigenen Position, z.B. Car Positioning durch Map Matching,Radio Frequency

- Navigation zusätzlich Geschwindigkeit, Beschleunigung, Winkelverhältnisse etc.

- Target Tracking Position eines anderen Objektes relativ zur eigenen Position, z.B. Car Collision Avoidance, Air Traffic Control

Bereiche:

29

wahre Position geschätzte Position

Car PositioningCar Positioning

30

- Möglichkeit, beliebige Verteilungen zu verwenden ( nicht nur Normalverteilung )

- Verwendung nicht-linearer Modelle

- einfacher zu implementieren

- weniger Speicherplatz

Problem: noch keine Ergebnisse im Bereich der Fußgängernavigation

- bietet Echtzeit-Lösung

FazitFazit

31