Handout

Christian Hett
24.1.2005
Handout zum Vortrag:
Verfolgung bewegter Objekte mit Partikelfiltern
von
Christian Hett
Abstrakt
In diesem Vortrag geht es um ein Verfahren, um z.B. im RoboCup die eigene Position und die
Ballverfolgung durchzuführen. Dies wird mit Rao-Blackwellised Partikelfiltern in einem vereinheitlichten Framework realisiert. Partikelfilter haben gegenüber Kalman-Filtern den Vorteil, dass
sie multi-modal sind, d.h. die Simulation in völlig verschiedene Bewegungsarten fortführen können, was bei den vielfältigen Interaktionen im Roboterfußballspiel zu besseren Ergebnissen führt.
Das besondere an dem hier vorgestellten Verfahren ist, dass lineare Anteile an den Bewegungsmodellen über Kalman-Filter effizient berechnet werden, indem jeder Partikel einen Kalmanfilter
enthält. Durch diese Reduktion des Anteils des Zustandsraums, der mit Partikeln simuliert werden muss kommt man mit weniger Partikeln aus, so dass der Algorithmus auf einem AIBO aus
der Sony-RoboCup-League gute Ergebnisse liefern kann.
Inhalt
1. Problemstellung
2. andere, bereits bekannte Verfahren
a. Kalmanfilter
b. Markovfilter
c. Monte Carlo Methode
3. Rao-Blackwellised Partikel-Filter
4. Zustände und Übergangsdiagramm
5. Der Algorithmus
6. Ergebnisse
7. Verbesserungen am Algorithmus
8. Literatur und Referenzen
1. Problemstellung
In diesem Vortrag und dem zugehörigen Paper geht es um die Selbstlokalisation und Ballverfolgung im RoboCup. Im Allgemeinen benötigt ein mobiler Roboter genaues Wissen über seine
eigene Position, die Objekte in der Umwelt und deren Bewegung. Dies ist eine aufwendige Aufgabe, da sowohl die Sensordaten (z.B. Kamera oder Ultraschall), als auch die Odometriedaten
(Auswertung der Bewegungen die der Roboter bisher ausgeführt hat um daraus die neue Position
zu errechen) immer mit Unsicherheiten behaftet sind.
In der RoboCup-Domäne stellen sich besondere Herausforderungen. Es handelt sich um ein sehr
dynamisches Spiel mit vielen Interaktionen
o Der Ball wird von anderen Spielern aufgenommen oder geschossen
o Der Ball prallt an Spielern ab
o Der Ball prallt an der Bande ab
o Der eigene Roboter nimmt den Ball auf oder schießt ihn
o Der Schiedsrichter kann den Roboter zur Strafe neu ins Spielfeld setzen
1/8
Christian Hett
24.1.2005
Diese Interaktionen sind hochgradig nicht linear und stellen Filter, die auf Linearität beruhen, vor
große Probleme.
Außerdem sind die Sensordaten und Odometriedaten im RoboCup außerordentlich verrauscht:
- Der Hauptsensor ist eine CMOS-Kamera mit nur 176x144 Pixeln. Damit lassen sich Abstände zum Ball oder zu Landmarken nur schwer abschätzen. Außerdem müssen die Filter
robust sein gegen Fehlerkennungen in der Bildverarbeitung, sowie Verdeckungen des
Balls durch andere Roboter
- Das sich aus der Laufbewegung auf dem rutschigen Boden ergebende Bewegungsmodell
ist mit großen Unsicherheiten behaftet. Man kann aus den Odometriedaten also nur unsicherer Rückschlüsse auf die Position ziehen
2. Andere, bereits bekannte Verfahren
a) Der Kalmanfilter
Der Kalmanfilter wurde im Referat am 6.12.2004 von Patrick Winkler bereits vorgestellt, da er
hier aber eine entscheidende Rolle spielt, wird er mit seinen Nachteilen nochmals kurz vorgestellt.
Der Kalmanfilter wird im German-Team des RoboCup derzeit auch zur Ballbestimmung eingesetzt und zeigt dort auch die angesprochenen Probleme.
☺ Effizient
Uni-modal
Nur für lineare oder linearisierbare Modelle
Trade/Off zwischen Stabilität/schnellem Anpassen
Die Funktionsweise ist, dass man zu der Zustandsvariable zusätzlich noch die Varianz, also die Unsicherheit mit der diese bekannt ist speichert.
Darauf werden zwei wesentliche Rechenschritte
angewendet:
- Schätzung (kalman_prediction): Mit
Hilfe eines Bewegungsmodells wird der aktuelle Zustand fortgeschrieben, also eine
Schätzung des Folgezustands durchgeführt.
Dazu benötigt man die Steuerungsdaten des Prinzipieller Ablauf des Kalmanfilters
Roboters. Das Modell beschreibt ebenfalls
die damit verbundene Unsicherheit
- Filter-Update (kalman_update): Sobald Sensordaten vorliegen, können diese in die
Zustandsvariable eingerechnet werden. Dazu gibt es wieder ein Modell, welches aus den
Sensordaten die Änderung der Zustandsvariable sowie die damit in dieser Situation verbundene Unsicherheit angibt
Über die Parameter/Funktionen P (Systemrauschen) und Q (Messrauschen) muss man beim Implementieren angeben, wie genau die Schätzung und wie genau die Messung ist. Man hat hier einen Trade-Off zwischen einem stabilen Filter, der sich von Messungenauigkeiten nicht so leicht
aus dem Tritt bringen lässt und einem flexiblen Filter, der auf unvorhergesehene Änderungen
(z.B. Ball wird von einem anderen Roboter geschossen – Dies wird von einem Bewegungsmodell, welches ein lineares Rollen vorhersagt nicht erfasst) schnell reagieren kann. Wählt man einen Kompromiss, wird die Ballerkennung insgesamt nicht sehr leistungsfähig sein.
2/8
Christian Hett
24.1.2005
Der Kalman-Filter ist uni-modal, d.h. er simuliert nur eine Schätzung über den Zustand und kann
sich nicht in mehrere denkbare Zustände verzweigen
Der Kalman-Filter ist eigentlich für lineare Modellfunktionen geeignet, funktioniert aber auch
mit linearisierten Funktionen sehr gut. Dies versagt allerdings, wenn es um extreme NichtLinearitäten wie z.B. das Abprallen eines Balls geht. In diesem Fall funktionieren die dem Kalman-Filter zugrunde liegenden Annahmen nicht mehr (Linearität und Gaussverteilung der Zustandsvariablen).
b) Markovfilter
Der Markovfilter wurde im Referat am 28.11.2004 von Dorian Scholz bereits vorgestellt.
☺ Multi-modal
Ineffizient
Dieser Filter ist multi-modal, da er mit Wahrscheinlichkeitsverteilungen arbeitet, indem er den möglichen Zuständen
Wahrscheinlichkeiten zuordnet. Um dies praktisch realisieren
zu können, kann ein z.B. 3-dimensionaler Zustandsraum (Die
drei Dimensionen sind X- und Y-Position des Roboters, sowie
Winkel der Ausrichtung des Roboters) mit einem Gitter über
einen Würfel diskretisiert werden. Für jeden kleinen Teilwürfel wird dann eine Wahrscheinlichkeit abgespeichert. Dieser
Speicherplatz sowie die Rechenzeit wachsen leider exponentiell mit der Anzahl der Dimensionen des Zustandsraums. Außerdem sind umso mehr Wahrscheinlichkeitswerte zu speichern und mit jedem Simulationsschritt fortzuschreiben, je
feiner man diskretisiert.
c) Monte-Carlo Partikelfilter
Würfel für Zustände eines Roboter
mit Diskretisierung als Gitter.
Bel=Wahrscheinlichkeit
Der Monte-Carlo-Filter wurde im Referat am 28.11.2004 von Alexander Pass bereits vorgestellt.
☺ Multi-modal
Bei hoher Zustandsraum-Dimension große Menge Partikel erforderlich
Bei Partikelfiltern hat man es in der Regel (es gibt viele
Varianten) mit einer festen (z.B. N=300) Anzahl Partikel zu
tun. Diese stellen jeweils einen Systemzustand dar, verknüpft
mit der Wahrscheinlichkeit für diesen Zustand.
Die Partikel werden einzeln simuliert, bei der Simulation geht
die Unsicherheit des jeweiligen Bewegungs- oder
Messmodells als zufällige Abweichung mit ein.
Schlechte, also unwahrscheinliche Partikel (mit niedrigen Spielfeld mit vielen Partikeln für die
Wahrscheinlichkeitswerten) werden aussortiert, gute Partikel Roboterposition. Anfangszustand
verdichten sich zügig an der Position, wo man den Roboter
erwarten kann.
3/8
Christian Hett
24.1.2005
Durch dieses Verfahren wird der Markov-Filter angenähert, allerdings wird nur für die
vielversprechenden Bereiche Rechenzeit verwendet:
p(xt ∈ A|y1:t ) ≈
{
}
1
i:xt(i) ∈ Α
N
In dieser Formel ist
- N die Anzahl der Partikel
- A eine Teilmenge des Zustandsraums
- p( a | b) die Wahrscheinlichkeit für a unter der Vorraussetzung, dass b eingetreten ist
-
1
{...} der Anteil der Partikel, die sich in der Menge befinden (zwischen 0 und 1 wie eiN
ne Zufallsvariable)
Die Verteilung der Partikel auf den Zustandsraum soll also der Wahrscheinlichkeit nach dem
Markov-Modell proportional sein. Bei diesem beschreibt p die Wahrscheinlichkeit für Zustand xt
unter der Annahme, dass bisher die Messungen y1:t durchgeführt wurden
3. Rao-Blackwellised Partikelfilter
Rao-Blackwellised Partikel-Filter kombinieren die Effizienz des Kalmanfilters mit dem Darstellungsreichtum der Partikelfilter, die multi-modal sind und dadurch mehrere denkbare Zustände
gleichzeitig simulieren können.
Dazu erhalten die Partikel je einen Kalman-Filter. Ein Partikel zeichnet sich jetzt also nur noch
durch die Zustandsvariablen aus, die nicht-linear sind. Die linearen Anteile werden durch den
Kalman-Filter simuliert, wobei dieser auf die nicht-linearen Anteile konditioniert wird, d.h. er
simuliert das System unter der Annahme, dass die nicht-linearen Anteile so sind, wie in seinem
Partikel beschrieben.
Dadurch wird es möglich, den Zustandsraum, der mit Partikeln simuliert werden muss zu verkleinern und mit weniger Partikeln effizient zu rechnen.
4. Zustände und Übergangsdiagramm
Für die Simulation mit RB-Filtern bietet es sich an, ein Zustandsdiagramm zu erstellen. Dabei
steht jeder der folgenden Zustände für ein Ballbewegungsmodell. Dies ermöglicht es, den Zustand in die nicht-linearen Anteile der Partikel aufzunehmen und die Kalman-Filter der Partikel
jeweils auf genau das in diesem Partikel simulierte Bewegungsmodell zu tunen.
Die Zustandsübergänge stellen außerdem gerade die extremen Nicht-Linearitäten dar, die es einem gewöhnlichen Kalman-Filter so schwer machen würden.
Die Zustände des Ballbewegungsmodells:
- None: Ball liegt ruhig oder bewegt sich linear ( ideal für Kalman-Filter!)
- Grabbed: Ball befindet sich zwischen den Roboterbeinen und wird vom Roboter bewegt. Diese Situation wird von anderen Algorithmen oft als Spezialfall implementiert
- Kicked: Dieser Zustand wird verwendet, wenn der Ball vom Roboter weggeschossen
wurde.
- Bounced: Der Ball ist an der Bande oder einem anderem Roboter abgeprallt. Für diesen
Fall wird eine Reflektion simuliert. Dabei wird die geschätzte Geschwindigkeit reduziert
und eine starke Unsicherheit in die Schätzung mit eingerechnet.
4/8
Christian Hett
24.1.2005
Aus diesen Zuständen ergibt sich nun folgendes Zustandsübergangsdiagramm:
residuelle
Wahrscheinlichkeit
Roboter verliert Griff
(0,8)
(0,2)
None
Grabbed
Wenn Roboter zugreifen
kann und das tut
(1
=s ,0)
ofo
rt
Bounced
Schuß
mißlingt (0,1)
Deflected
Kicked
Das Diagramm beschreibt, wie man für einen Partikel von einem Ballbewegungsmodell auf das
folgende schließen kann. Die Wahrscheinlichkeiten für die Übergänge entstanden durch Ausprobieren und Optimierungen. Die Wahrscheinlichkeit von 0,1 für den Übergang vom Zustand None zu Deflected wurde eingeführt, da der Ball an einem nicht erkannten Roboter abprallen
könnte.
Der Übergang von None zu Grabbed ist nicht probabilistisch und erfolgt dann, wenn der Roboter nahe genug am Ball ist. Genauso geht man nur dann in den Bounced-Zustand, wenn sich aus
der Ballposition ein Kontakt zur Bande oder einem anderen Roboter ermitteln lässt.
Die Zustände Bounced, Deflected und Kicked kehren aus ihrem Zustand sofort wieder in
den None-Zustand zurück, da sie lediglich Änderungen im Kalman-Filter initiieren.
5. Der Algorithmus
Der im Paper vorgestellte Algorithmus hat folgende Ein- und Ausgaben:
Eingabe:
{
}
-
Die alte Partikelmenge S k −1 = sk −1 , wk −1 1 ≤ i ≤ N
Die Steuerungsparameter uk-1 mit denen das Bewegungsmodell simuliert werden kann
-
Die Beobachtungen: Landmarke
Ausgabe:
-
{
(i )
(i )
z kl oder Ball z kb
Die neue Partikelmenge S k = sk , wk 1 ≤ i ≤ N
(i )
(i )
}
(i )
(i )
(i )
(i )
Dabei ist sk −1 = mk −1 , bk −1 , rk −1 ein Partikel (zum Zeitpunkt k-1) und
wk(i−)1 das dazugehörige
Gewicht, also die Wahrscheinlichkeit (0..1) für diesen Partikel. N ist die Anzahl der Partikel, i die
Nummer eines Partikels.
5/8
Christian Hett
24.1.2005
Zu einem Partikel gehören:
-
mk( i−)1
-
(i )
k −1
r
-
bk = xb , yb , x&b , y&b
Das Ballbewegungsmodell bzw. der Zustand für diesen Partikel:
m∈{None, Grabbed, Kicked, Bounced, Deflected}
= xr , y r , θ r
Die Position des Roboters sowie die Ausrichtung
Zustand des Balls: Position und Beschleunigung sowie die zugehörigen Varianzen.
Unabhängigkeiten im Zustandsraum:
Zur effizienten Berechnung
Landmarkenerkennung
der Folgewahrscheinlichkeiten werden in dieser Abbildung die Abhängigkeiten der
Roboterposition
Zufallsvariablen von einem
Robotersteuerung
Simulationsschritt zum darauf folgenden gezeigt.
akt. Ballbewegungsmodell
Man sieht, dass die (Zufallsvariable) Roboterposition rk
Ballposition und
nur von der vorherigen Ro-geschwindigkeit
boterposition rk-1 und der
Robotersteuerung uk-1 abBallerkennung
hängt.
Die Schätzung für BallbeweZeitlicher Verlauf
gung bk ergibt sich nicht nur
aus der vorherigen Ballposition bk-1, sondern auch aus dem aktuellen Ballbewegungsmodell mk
(da die Bewegung ja vom Bewegungsmodell abhängt). Außerdem hat die aktuelle Roboterposition rk noch einen Einfluss auf die Ballschätzung, da mk=grabbed sein könnte.
Die Wahrscheinlichkeit, eine Landmarke an einer bestimmten Position zu erkennen, ist nur von
der Roboterposition abhängig. Die Ballerkennung hingegen von der Ball- und Roboterposition
Der Algorithmus hat folgenden Aufbau:
Neue Partikelmenge Sk als leere Menge initialisieren
Schleife über die N Elemente der alten Partikelmenge Sk-1
Neue Roboterposition rk schätzen: rk( i ) ≈ p rk | rk(−i )1 , u k −1
Hier wird das Bewegungsmodell auf die Roboterposition des alten Partikels angewendet
und dieser gemäß der Robotersteuerung uk-1 simuliert. Das sich aus dem Modell für diesen Fall ergebene Rauschen wird durch in diesem Ausmaß zufällige Störwerte eingerechnet. (Dies macht Sinn, da ja dies ja mehreren Partikeln widerfährt und somit das
Rauschen für die neuen Partikel eine entsprechende Zufallsverteilung erzeugt)
Neue Partikel für die neue Partikelmenge Sk je nach Fallunterscheidung bestimmen:
zk=Landmarkenerkennung
zk=Ballerkennung
[…]
[…]
Partikelwahrscheinlichkeiten normalisieren
Bei mehr als N Partikel:
Schlechte Partikel aussortieren
(
)
6/8
Christian Hett
24.1.2005
Fall Landmarkenerkennung
Der neue Zustand bzw. das neue Ballbewegungsmodell m wird gemäß dem Zustandsübergangsdiagramm geschätzt:
(
mk( i ) ≈ p mk | mk( i−)1 , rk( i ) , bk(i−)1 , uk −1
)
An dieser Stelle wird z.B. aus Ball- und Roboterposition ermittelt, ob ein Abprallen an der Bande
laut dem Wirklichkeitsbild dieses Partikels stattfinden würde. Erlaubt das Zustandsübergangsdiagramm mehrere Möglichkeiten mit verschiedenen Wahrscheinlichkeiten, so wird dies entsprechen stochastisch entschieden.
Nun kann – da Zustand und Roboterposition für diesen Partikel bekannt sind – der Ballverlauf
linearisiert mit einem Kalmanfilter geschätzt werden. Dabei wird auch entsprechend dem Bewegungsmodell die Varianz der Ballposition für diesen Partikel aktualisiert:
(
bk(i ) := kalman _ prediction rk( i ) , mk(i ) , bk(i−)1
)
Der neue Partikel wird nun noch in die neue Partikelmenge aufgenommen:
{
S k := S k −1 ∪ ≈ bk(i ) , mk(i ) , rk(i )
}
Das dabei benutze Gewicht entspricht der Wahrscheinlichkeit für die von der Kamera stammende
Landmarkenerkennung unter der Annahme, dass sich der Roboter an dieser Position befindet, die
in diesem Partikel gespeichert ist:
(
wk(i ) := p z k | rk( i )
)
Fall Ballerkennung
Bei einer Ballerkennung wird nicht für jeden vorhandenen Partikel ein neuer eingefügt, sondern
mehrere: Einer für jedes Bewegungsmodell.
Dabei wird in jeden Partikel für jedes Ballmodell per Kalman-Filter-Updateschritt die Messung
des Balls in die Ballposition eingerechnet und der der Kalman-Filter so eingestellt (Messmodell,
Messrauschen), dass dies zur Roboterposition rk und dem jeweiligen Bewegungsmodell passt.
(i )
Die Wahrscheinlichkeit wk für einen solchen Partikel ergibt sich aus der Wahrscheinlichkeit
für diese Ballerkennung unter dem aktuellen Modell, der geschätzten Roboterposition und der
alten Ballposition. Dazu kommt noch multiplikativ die Wahrscheinlichkeit für diesen Zustand/Ballbewegungsmodell unter der Annahme des kompletten Zustands und der Bewegungskontrolle.
6. Ergebnisse
Der Algorithmus funktionierte mit von einem AIBO-Roboter gesammelten Daten
sehr gut, allerdings leider nicht in Echtzeit:
Es mussten 300-500 Partikel simuliert werden. Da jeder Partikel einen Kalman-Filter
enthält, war die Rechenlast zu hoch für den
400Mhz Prozessor der AIBOs.
7/8
Christian Hett
24.1.2005
Die hohe Partikelzahl kommt dadurch zustande, dass die Roboterposition und die Ballposition
gekoppelt sind: Dass die Roboterposition die Ballmessung beeinflusst ist klar, da die Balldaten ja
relativ sind. Allerdings gibt es auch in die andere Richtung Einfluss: Die Partikel mit der geringsten Wahrscheinlichkeit werden aussortiert und eine schlechte Ballerkennung sorgt somit auch
dafür, dass Partikel (mit ihren Roboterpositionen) durch gegenüber den Ballmessungen unwahrscheinliche Ballpositionen früher entfernt werden.
Es gibt noch ein prinzipielles Problem: Da die Position des Balls sehr wichtig für die Entscheidungen des Roboters ist und er sich schnell bewegt, wird seine Position etwa 20x pro Sekunden
bestimmt, während die Landmarken nur 2x pro Sekunde ermittelt werden. Dadurch gehen die
Ballbestimmungen zu stark in die Rechnung ein, auch wenn die Roboterposition eigentlich einen
viel höheren Einfluss auf die Ballbestimmung haben müsste als umgekehrt.
Die Lösung für diese Probleme bestand darin, den Zustandsraum und die Partikel in zwei Mengen zu unterteilen: Partikel für die Roboterposition und Partikel für die Ballpositionen. Der Informationsfluss von den Ballpartikeln zu den Roboterpartikeln wird dabei komplett vernachlässigt. (Dies ist in Ordnung, da die stark verrauschten Ballinformationen nicht viel zur Bestimmung/Verifizierung von Roboterpositionen beitragen konnten) Wann immer eine der 10x selteneren Landmarkenerkennungen stattfindet, werden die Roboterpositionen wieder in die BallPartikel eingerechnet, die bis zu diesem Zeitpunkt unabhängig von den Roboterpartikeln geschätzt wurden (dabei wird auch die Roboterbewegung von jedem Ballpartikel weitergeschätzt).
Der modifizierte, effizientere Algorithmus funktionierte mit z.B. 20 Ball-Partikeln sehr gut und
konnte den AIBO besser als frühere Verfahren steuern.
7. Literatur
-
-
Cody Kwok und Dieter Fox. Map-based Multiple Model Tracking of a Moving Object Proceedings of RoboCup Symposium, 2004 (best scientific challenge award)
A. Doucet, J.F.G. de Freitas, K. Murphy, and S. Russell. Rao-Blackwellised particle filtering for
dynamic Bayesian networks. In Proc. of the Conference on Uncertainty in Artificial Intelligence
(UAI), 2000.
S. Funiak and B. Williams. Multi-modal particle filtering for hybrid systems with autonomous
mode transitions. In Proc. of SafeProcess 2003, Washington, USA
Dieter Fox. Adapting the sample size in particle filters through KLD-sampling. International
Journal of Robotics Research (IJRR), 22(12), 2003.
http://www.cs.washington.edu/ai/Mobile_Robotics/projects/mcl/ (Projektseite zum ersten Paper
mit Videos)
Oskar v. Stryk: Vorlesung Mobile und sensorgeführte Robotiksysteme, Teil 8: Navigation
Oskar v. Stryk: Vorlesung Mobile und sensorgeführte Robotiksysteme, Teil 5: MarkovLokalisierung
Wikipedia: Bayestheorem http://de.wikipedia.org/wiki/Bayes-Theorem
Wikipedia: Bedingte Wahrscheinlichkeit
http://de.wikipedia.org/wiki/Bedingte_Wahrscheinlichkeit
Referat von Alexander Pass, 29.11.2004
Referat von Dorian Scholz, 29.11.2004
Referat von Partrick Winkler, 6.12.2004
Röfer, T., Dahm, I., Düffert, U., Hoffmann, J., Jüngel, M., Kallnik, M., Lötzsch, M., Risler, M.,
Stelzer, M., Ziegler, J. GermanTeam 2003. 7th International Workshop on RoboCup 2003
8/8