Verfolgung bewegter Objekte mit Partikelfiltern - SIM.tud · Kalmanfilter Markovfilter Monte Carlo...

Verfolgung bewegter Objekte mit Partikelfiltern

Betreuer: Martin FriedmanFachgebiet Simulation und Systemoptimierung (SIM)Fachbereich 20 (Informatik)Technische Universität Darmstadt

Christian Hett

24.1.2005

108-xxxx108-xxxx

24.Januar 2005 Christian Hett 2

Gliederung

� Problemstellung

� Andere, bereits bekannte Verfahren� Kalmanfilter

� Markovfilter

� Monte Carlo Methode

� Rao-Blackwellised Partikel Filter

� Algorithmus

� Effizienzsteigerungen

� Obligatorische Robocup-Videos


Ziel

� Mobile Roboter benötigen genaues Wissen über Position von sich und Objekten sowie deren Bewegung

� Odometrie- und Sensordaten sind immer mit Unsicherheit behaftet

� Selbstlokalisierung

� Ballverfolgung


Probleme im RoboCup

� sehr dynamisches Spiel

� viele Interaktionen/plötzliche Änderungen:� Ball � Gegenspieler

� Ball � Bande

� Ball aufnehmen und schießen

� Umsetzen durch Schiedsrichter (Strafe)

� stark verrauschte Odometriedaten

� Miserable Eingabedaten:� CMOS-Chip 176x144 Pixel

� Schwierige Abstandbestimmung zu Ball


Vorhandene Verfahren

� Kalmanfilter

� Markovfilter

� Monte Carlo Methode


Kalmanfilter

� Funktion:� Zustandsvariable mit Varianzen

� Schätzen des nächsten Zustands plus Fehler

� Korrektur des Zustands durch Messung.

� Eigenschaften:� � Tradeoff zw. Stabilität/schnellem Anpassen

� � Basiert auf Linearität und Gaussverteilung

� � Uni-modal: repräsentiert nur eine Schätzung über Zustand

� ☺ Sehr effizient


Markov-Filter� Funktion:

� Zustandsraum wird diskretisiert(z.B. Würfel mit Gitter versehen)

� Jeder Teilwürfel hat Wahrschein-lichkeit, dass Roboter in diesem Zustand ist. (Bel-Funktion)

� Vorhersage nächster Zustand� Bewerten mit Sensordaten

� Eigenschaften:� ☺ Multimodal (repräsentiert alle Schätzungen über

Zustand (diskretisiert) mit Wahrscheinlichkeits-verteilung)

� � Nur für endliche Zustandsräume� � Sehr ineffizient, wächst exponentiell mit

Zustandsraum


Monte-Carlo Partikelfilter

� Funktion:� Partikelfilter� Partikel=Zustand+Wahrscheinlichkeit� Immer N (z.B. 300) Partikel aktiv� Jeder Partikel repräsentiert eine Schätzung über

Zustand + Wahrscheinlichkeit für diesen Zustand� Schlechte werden aussortiert

� Eigenschaften:� ☺ Nähert Markov-Filter an� ☺ Multi-modal� � Weniger effizient als Kalman


Monte-Carlo/Partikelfilter

� A Teilmenge des Zustandsraums

� | …unter der Annahme dass,…

� p Zufallsverteilung wie bei Markov

� Partikelmenge

{ }Αi:xN

)A|yp(x (i)t:tt ∈≈∈ 1

1

{ }(i)tx

Integrierte Wahrscheinlichkeit für A soll proportional zu Anzahl Partikel in A sein


Rao-Blackwellised particle filters� ☺ Kombinieren Effizienz von Kalmanfiltern

mit Darstellungsreichtum von Partikelfiltern� ☺ Multimodal� Funktion:

� Unterteilung des Zustandsraums in lineare und nicht-lineare Anteile

� Ein Kalman-Filter pro Partikel für lineare Anteile

� Zustand:� Roboterposition (Nicht-linear)� Ballbewegungsmodell (Nicht-linear)� Ballposition (Linear pro Partikel)


Herkömmlicher Rao BlackwellisedPartikelfilter Partikelfilter

2x16=32 Partikel 2 Partikel mit je einem Kalmanfilter

=Roboterpositionschätzung =Ballpositionsschätzung


Ballbewegungsmodelle� Übergänge zwischen Bewegungsmodellen sind extrem

nicht-linearen Probleme für Kalman-Filter� Kalman-Filter wird auf Bewegungsmodell und

Roboterposition konditioniert� Ballbewegungsmodelle:

� None: Ball ruhig oder bewegt sich linear� Grabbed: Ball zwischen Roboterbeinen

(normalerweise Spezialfall im Code)� Kicked: Roboter hat Ball geschossen� Bounced: An Bande oder Roboter abgeprallt.

Reflektion mit starker Unsicherheit der Schätzung + reduzierte Geschwindigkeit

� Deflected: Trajektorie hat sich plötzlich verändert (unerkanntes Hindernis). Schätzung sehr ungenau, muss sich wieder stabilisieren.


Zustandsübergangsdiagramm

None Grabbed

Bounced Deflected Kicked

(1,0)

=sofort

(1,0)

=sofort

(1,0)=sofort

(0,9)

Roboter verliert Griff

(0,1)

(0,8)

(0,2)

Wenn Roboter zugreifen

kann und das tut

wenn Kollision

laut Karte

Roboter schießt

Ball e

rfolgreich

Schuß

mißlingt (0,1)

Ball bleibt zwischen den

Beinen


Algorithmus

� Eingabe� Partikelmenge

� Steuerungsparameter

� Beobachtung: Landmarke oder Ball

� Ausgabe� Neue Partikelmenge

{ }NiwsS ik

ikk ≤≤= −−− 1, )(

1)(11

)(1

)(1

)(1

)(1 ,, i

ki

ki

ki

k rbms −−−− =

Roboterposition

Ballzustand+ Varianzbbbbk yxyxb && ,,,=

{None, Grabbed, Kicked,

Bounced, Deflected}

N=300

lkz b

kz

{ }NiwsS ik

ikk ≤≤= 1, )()(

1−ku


Abhängigkeiten im Zustandsraum

Landmarkenerkennung

RoboterpositionRobotersteuerung

akt. Ballbewegungsmodell

Ballposition und -geschwindigkeit

Ballerkennung

Zeitlicher Verlauf

Für Grabbed

Erkennungen sind relative Angaben

Roboterposition hängt nur von vorheriger und Steuerung ab

Ball wird je nach Ballbewegungs-modell simuliert


Algorithmus Hauptschleife

� Schleife über die N Elemente der alten Partikelmenge Sk-1

� Neue Roboterposition rk schätzen

� Neue Partikel für neue Partikelmenge Sk jenach Fallunterscheidung:

� zk=Landmarkenerkennung

� zk=Ballerkennung

� Mehr als N Partikel:� Schlechte Partikel aussortieren

� Partikelwahrscheinlichkeiten normalisieren

( )1)(1

)( ,| −−≈ ki

kki

k urrpr

Robotersteuerung


Fall Landmarkenerkennung

� Neues Ballbewegungsmodell m schätzen

� Ballposition schätzen (Kalman)

auf Basis von Roboterposition und Ballbewegungsmodell

� Ergibt Partikel für nächste Runde

( )1)(1

)()(1

)( ,,,| −−−≈ ki

ki

ki

kki

k ubrmmpm

( ))(1

)()()( ,,_: ik

ik

ik

ik bmrpredictionkalmanb −=

{ })()()(1 ,,: i

ki

ki

kkk rmbSS ∪= −( ))()( |: i

kki

k rzpw =

Wahrscheinlichkeit für diesen Partikel= Wahrscheinlichkeit, dass zk von dieser Roboterposition aus gesehen werden kann


Fall Ballerkennung

� Mehrere Partikel einfügen durch

Unterschleife über alle 5 Ballbewegungs-modelle:� Jeweils Kalman-Updateschritt mit Messwert für

Position des Balls, konditioniert auf Roboterposition rk und Bewegungsmodell mk

� Wahrscheinlichkeit für Partikel= (Wahrscheinlichkeit der Beobachtung) * (Wahrscheinlichkeit des Ballbewegungsmodells)

� Bei Landmarke war keine Unterschleife, weil Wahrscheinlichkeit dort immer gleich


Ergebnisse

� Funktionierte sehr gut mit gesammelten Daten

� Aber: 300 Partikel= 300 Kalmanfilter

� � Zuviel für den AIBOin Echtzeit


Effizienzsteigerung� Grund für große Partikelzahl: Kopplung von

Ball- und Roboterschätzungen� Roboterposition beeinflusst Ballmessung: klar� Ball beeinflusst Roboter: durch Wahrscheinlichkeit

für Partikel, entfernt zu werden

� Übergewicht von Ballmessungen:� Ballupdates 20x pro Sekunde� Landmarkenupdates 2x pro Sekunde

� Lösung:� Fasse 10 Ballbeobachtungen zusammen pro

Landmarke� Ignoriere Einfluss Ballerkennung auf Position


Idee neuer Algorithmus

� Zustandsraum in Roboter- und Ballposition partitionieren

� 2 Partikelmengen getrennt updaten� Roboterposition mit normalen Partikeln

� Ballposition neu an Roboterposition binden, wenn Landmarke gesehen. Sonst Roboterposition des Ballpartikels fortschreiben.

� Funktionierte mit 20 Samples (für Ball)


Das obligatorische Robocup-Video

22

Videos unterhttp://http://www.cs.washington.eduwww.cs.washington.edu//aiai//MMobile_Roboticsobile_Robotics//projectsprojects//mclmcl//


Literatur�� CodyCody KwokKwok und Dieter Fox. und Dieter Fox. MapMap--based Multiple Model based Multiple Model

Tracking of a Moving Object Tracking of a Moving Object Proceedings of Proceedings of RoboCupRoboCupSymposium, 2004Symposium, 2004 (best scientific challenge award)(best scientific challenge award)

�� A. A. DoucetDoucet, J.F.G. de , J.F.G. de FreitasFreitas, K. Murphy, and S. Russell. , K. Murphy, and S. Russell. RaoRao--BlackwellisedBlackwellised particle filtering for dynamic particle filtering for dynamic Bayesian networksBayesian networks. In . In Proc. of the Conference on Proc. of the Conference on Uncertainty in Artificial Intelligence (UAI)Uncertainty in Artificial Intelligence (UAI), 2000., 2000.

�� S. S. FuniakFuniak and B. Williams.and B. Williams. MultiMulti--modal modal particleparticle filteringfilteringforfor hybrid hybrid systemssystems withwith autonomousautonomous mode mode transitionstransitions. . In In ProcProc. of . of SafeProcessSafeProcess 2003, Washington, 2003, Washington, USAUSA

�� Dieter Fox. Dieter Fox. AdaptingAdapting thethe samplesample sizesize in in particleparticle filtersfiltersthroughthrough KLDKLD--samplingsampling. . International Journal of International Journal of RoboticsRoboticsResearch (IJRR)Research (IJRR), 22(12), 2003., 22(12), 2003.

�� RRööferfer, T., Dahm, I., , T., Dahm, I., DDüüffertffert, U., Hoffmann, J., , U., Hoffmann, J., JJüüngelngel, M., , M., KallnikKallnik, M., , M., LLöötzschtzsch, M., , M., RislerRisler, M., , M., StelzerStelzer, M., Ziegler, J. , M., Ziegler, J. GermanTeamGermanTeam 20032003. . 7th International Workshop on 7th International Workshop on RoboCupRoboCup 20032003


Literatur und Referenzen�� VorgefVorgefüührte Videos: hrte Videos:

http://http://www.cs.washington.eduwww.cs.washington.edu//aiai//Mobile_RoboticsMobile_Robotics//projectsprojects//mclmcl// (Projektseite zum ersten Paper)(Projektseite zum ersten Paper)

�� Oskar v. Oskar v. StrykStryk: : Vorlesung Mobile und sensorgefVorlesung Mobile und sensorgefüührte hrte Robotiksysteme, Teil 8: NavigationRobotiksysteme, Teil 8: Navigation

�� Oskar v. Oskar v. StrykStryk: : Vorlesung Mobile und sensorgefVorlesung Mobile und sensorgefüührte hrte Robotiksysteme, Teil 5: Robotiksysteme, Teil 5: MarkovMarkov--LokalisierungLokalisierung

�� WikipediaWikipedia: : BayestheoremBayestheoremhttp://de.wikipedia.org/wiki/Bayeshttp://de.wikipedia.org/wiki/Bayes--TheoremTheorem

�� WikipediaWikipedia: Bedingte Wahrscheinlichkeit: Bedingte Wahrscheinlichkeithttp://de.wikipedia.org/wiki/Bedingte_Wahrscheinlichkeithttp://de.wikipedia.org/wiki/Bedingte_Wahrscheinlichkeit

�� Referat von Alexander Pass, 29.11.2004Referat von Alexander Pass, 29.11.2004�� Referat von Referat von DorianDorian Scholz, 29.11.2004Scholz, 29.11.2004�� Referat von Partrick Winkler, 6.12.2004Referat von Partrick Winkler, 6.12.2004


Vielen Dank fürs Zuhören

Verfolgung bewegter Objekte mit Partikelfiltern - SIM.tud · Kalmanfilter Markovfilter Monte Carlo...

Documents

Transcript of Verfolgung bewegter Objekte mit Partikelfiltern - SIM.tud · Kalmanfilter Markovfilter Monte Carlo...