Menü
Aktuelles
Von:
Filter

Particle Filter

N

nero76

Moderator
Ja, hier z.B. gibt es Grundlagen zur "Lokalisierung" mittels Particle Filter:
https://de.wikipedia.org/wiki/Lokalisierung_(Robotik) http://www.allpsych.uni-giessen.de/dirk/projects/particle.pdf https://www12.informatik.uni-erlangen.de/edu/robertino/lokalisierung.pdf http://patrec.cs.tu-dortmund.de/pubs/theses/da_doert.pdf


Bitte nicht von den ganzen Formeln usw. anschrecken lassen. Das Grundprinzip ist einfach (die Formeln kauft man einfach mit ein ;)). Es ist ein statistischer Ansatz, wo man zunächst annimt, dass der Roboter praktisch überall auf der Karte sein kann. Also setzt man zufällig überall einen "Particle" auf die Karte. Während der Roboter fährt, fahren auch die Particles in dieselbe Richtung wie der Roboter (mit etwas Zufall). Nach jedem Schritt wird die aktuelle Roboter-Messung (z.B. Schleife/Perimeter) mit jedem Partikel auf der Karte verglichen. Passt die Messung nicht zu der Karte, wird das Partikel von der Karte entfernt. Nach einiger Zeit bleiben nur die Partikel übrig, welche die aktuelle Position des Roboters auf der Karte beschreiben. Neue Partikel werden immer nahe des Zentrums aller Partikel aufgefüllt.

Crash-Beschreibung einigermaßen Verständlich? :)
 
Ja, hier z.B. gibt es Grundlagen zur "Lokalisierung" mittels Particle Filter:
https://de.wikipedia.org/wiki/Lokalisierung_(Robotik) http://www.allpsych.uni-giessen.de/dirk/projects/particle.pdf https://www12.informatik.uni-erlangen.de/edu/robertino/lokalisierung.pdf http://patrec.cs.tu-dortmund.de/pubs/theses/da_doert.pdf


Bitte nicht von den ganzen Formeln usw. anschrecken lassen. Das Grundprinzip ist einfach (die Formeln kauft man einfach mit ein ;)). Es ist ein statistischer Ansatz, wo man zunächst annimt, dass der Roboter praktisch überall auf der Karte sein kann. Also setzt man zufällig überall einen "Particle" auf die Karte. Während der Roboter fährt, fahren auch die Particles in dieselbe Richtung wie der Roboter (mit etwas Zufall). Nach jedem Schritt wird die aktuelle Roboter-Messung (z.B. Schleife/Perimeter) mit jedem Partikel auf der Karte verglichen. Passt die Messung nicht zu der Karte, wird das Partikel von der Karte entfernt. Nach einiger Zeit bleiben nur die Partikel übrig, welche die aktuelle Position des Roboters auf der Karte beschreiben. Neue Partikel werden immer nahe des Zentrums aller Partikel aufgefüllt.

Crash-Beschreibung einigermaßen Verständlich? :)
 
Hallo Alexander,

das Video über Partikel Filter hat mich ehrlich gesagt nicht überzeugt, da kommt selten die richtige Position raus.

Meine Erkenntnisse über Positionserkennung:
[ul]
[li]die meisten Dokus befassen sich mit Positionserkennung in Räumen, für Rasenmäher mit weiten Strecken und unebenen Boden oft ungeeignet.[/li]
[li]Was wirklich eine Verbesserung brachte war die Ground Plane beim GPS. Trotzdem oft 5m daneben[/li]
[li]Kompass bleibt ein Schätzeisen. Mitten auf der Wiese ok, aber ab 3m Nähe zum Zaun weicht der Winkel schon um 10° ab.[/li]
[li]Gyros helfen doch nur bei schnellen und kleinen Bewegungen, Referenz bleibt der Kompass, Probleme siehe Punkt vorher.[/li]


[/ul]
Was ich versucht habe:
[ul]
[li]Umgebung mit Ultraschall scannen und mit der Karte vergleichen. Funktionierte in der Praxis nicht, wahrscheinlich haben die Vibrationen den Ultraschall gestört. Vielleicht geht es mit Laser Scanner. 5m Umkreis würden oft schon genügen.[/li]
[li]Kompass vom GPS: noch schlechter als normaler Kompass.[/li]

[/ul]

Was seit 1 Jahr unfertig herumliegt: Kamera um damit den Roboter zu erkennen und die Position an den Roboter zu senden.

Was dringend notwendig wäre: ein exakter Kompass. Odometrie zur Wegberechnung + exakter Kompass würden viel genauere Daten lieferen.

Was vielleicht möglich wäre: den Abstand zu Schleife über die Signalstärke zu bestimmen, ein weiterer Input zur Sensorfusion.

LG Werner
 
Hallo Werner,

Particle-Filter auf der Schleife funktioniert hier sehr robust. Du musst natürlich die Schleife ein Stück abfahren. Dann weiss der Roboter wieder wo er ist. In dem Video ist das Schleifenabfahren am Ende zu sehen. Ohne Schleife abfahren klappt das nicht gut, da gebe ich Dir Recht, da bräuchte es mehr Sensorinformationen (z.B. Feldstärke). Das tolle am Particle Filter ist seine Flexibilität, du kannst beliebig viele Sensoren hinzunehmen um ihn besser zu machen.

Dass ein guter Gyro als exakte Richtung verwendet werden sieht man ja in dem ersten Video. Der Kompaß dient dann nur noch um den Gyro zeitweise zu kalibrieren. Richtungsinformation und Odometrie ist dann schon mal eine gute Grundlage. Die Schleife dient dann nur noch als Referenz zur Positionserkennung. Inmitten der Schleife muss es auch nicht auf 5cm genau sein, es reicht wenn man grob (1m) abschätzen kann wo man ist und wo noch nicht.

Gruss,
Alexander
 
Hallo Alexander,

das Problem der Positionserkennung besteht aber eher mitten am Grundstück als an der Schleife.

Und Gyro kalibrieren geht schlecht wenn der Kompass falsch anzeigt. Ich hab den CMPS11 3D Kompass, der ist an einigen Stellen im Garten einfach daneben. Einen besseren kenne ich nicht.

Wie macht das eigentlich der Bosch mit der Streifenfahrt?

Finde das toll was du alles mit mathematischen Aufwand herausholst, ich hatte keine Geduld für Kalman und Co ;)
Mein Ansatz ist ein bisschen anders, da ich zuerst mal verlässliche Daten haben will, wenn möglich ohne großen Aufwand mit Formeln.
Umgebung scannen hat im Zimmer gut funktioniert, im Betrieb nicht mehr. Es wäre aber ein weiterer und ich finde brauchbarer Ansatz, aber nur mit optischer Entfernungsmessung.

LG Werner
 
Hallo Werner,

Könnte man deine Kompaß-"Ausreißer" evtl. erkennen? Z.B. fährt der Roboter ja mit dem Gyro ja sehr exakt geradeaus (für 3 Minuten unter 1 Grad wenn man einen richtig guten Gyro nimmt wie den MPU9150 (mit Lage-Ausgang (Yaw/Pitch/Roll) durch automatischer d.h. integrierter Sensor-Fusion von Beschleunigung und Gyro). Wenn nun der Kompaß plötzlich Ausreißer aufweist (z.B. zum Gyro-Kurs), könnte man von einer "schlechten" Position zum Einlesen ausgehen und diese Position meiden. Man hat ja 3 Minuten Zeit eine Position zu wählen. Bei Dir würde der Kompaß also mitten in der Fahrt, aber nicht am Rand eingelesen. Also, 3 Minuten sind um, Roboter detektiert Ausreißer und fährt zunächst erstmal weiter seine Bahn bis Kompaß nicht allzu weit vom Gyro abweicht. Dann Kompaß einlesen.

Bosch macht es sehr wahrscheinlich so wie beschrieben. Jedenfalls verhält sich der Roboter so und die notwenidgen Sensoren sind auch verbaut (die neueste Generation benutzt übrings kein GPS mehr, vermutl. hat es nicht viel zur Positionserkennung beigetragen, ob Bosch die Schleifen-Feldstärke auswertet weiss ich (noch) nicht, vermutl. aber nicht).

Gruss,
Alexander
 
Es sind keine Ausreißer, je näher er dem Zaun oder einer Wand kommt desto mehr weicht der Kompaßwert ab. Ich erkenne das auch an der Kurskurrektur, dass der Roboter auf einmal eine Kurve fährt um den vermeintlich geraden Kurs zu folgen.

Hält der Gyro 3 Minuten lang den Kurs? Hatte bisher immer von bis zu 1° Abweichung je Sekunde gelesen.

Kein GPS im Bosch mehr? Dafür haben die GSM, man kann zwar Handys orten, das geht aber nicht sehr genau.
 
Ja, das meine ich doch: Du kannst dann deutlich eine Abweichung vom Gyro-Kurs detektieren.

Der MPU9150 Gyro hat einen Fehler von nur 0,005 Grad pro Sekunde, also nach 180 Sekunden nur 0,9 Grad. Der L3G4200D Gyro ist da deutlich schlechter (Faktor 6 schlechter). Es ist gar nicht so einfach einen guten Gyro für diese Anwendung zu finden :)
 
Danke, den kannte ich nicht. Anscheinend ist der 9150 das Auslaufmodell, und der 9250 hat eine höhere Abweichung wenn ich das Datenblatt richtig verstanden habe. Beim Chinesen um 11€ zu haben.

Über die integrierte Sensorfusion stand nichts genaueres wahrscheinlich muss man sich erst registrieren um mehr Info zu erhalten.


Jetzt hast mich wieder auf eine neue Idee gebracht, als wenn ich nicht schon genug Baustellen hätte ;)

LG Werner
 
Ja, der Nachfolger ist leider wieder schlechter :-/ (um Kosten zu sparen ist der Chip kleiner, aber dadurch beeinflussen/stören sich die integrierten Teile im Chip wieder mehr...)

Die integrierte Sensorfusion ist einfach anzuwenden:

1. Bei Invensense anmelden
2. Unter Software/Developer, den "MotionDriver" herunterladen, welcher den Treiber (C Code und darin enthalten das Binary für die MPU9150 Fusion), ein PDF und eine Beispielanwendung enhält. Der Treiber lädt die Fusion-Software automatisch bei jedem Start in den MPU9150 Speicher.
3. Das Beispiel als Grundlage verwenden.

Alternativ: diese Arduino-Libary verwenden (enthält auch das Fusion-Binary sowie ein Arduino Demo-Programm): https://github.com/Ardumower/ardumower/tree/develop/tests/mpu9150test
Viel Spaß beim Ausprobieren ;-)

Gruss,
Alexander
 
Funktionsfähiger Particle Filter Prototyp im Roboter ist gerade fertig geworden - In diesem Beispiel schätzt der Filter die Position des Roboters auf der Schleife für einige Durchläufe. Der Filter arbeitet tolerant (Schlupf wird toleriert, d.h. die Karte sieht jedes mal ein wenig anders aus, man kann den Roboter auch "kidnappen", also seine Position komplett verändern - werde ich später noch zeigen...).

https://www.youtube.com/watch?v=ySj2lEhs7L0
 
Die Karte ist noch nicht im Roboter implementiert, oder?
Wieviele Partikel benötigt man für eine gute Berechnung?
Annahme Auflösung der Karte mit 50x50cm, genügt 1 Partikel pro Feld?

Fragen über Fragen, ich weiss. Meine Karte ist jetzt teilweise selbstlernend, 1 Partikel pro Feld könnte ich gerade noch reinquetschen wenn nur die Position des Partikels bekannt sein muss und die Richtung und neue Position laufend neu berechnet werden.

LG Werner
 
Doch, die Karte ist schon drin im Roboter - ich benutze derzeit 50x50 "Pixel" für die Karte. Je nach Größe des Rasens steigt/sinkt dann die Auflösung der Karte. Zunächst lasse ich den Roboter die Schleife abfahren, erhalte eine grobe Karte und korrigiere diese automatisch (so dass Start und Ende zusammenfallen). Diese wird dann nicht mehr verändert.

Die Partikel haben mit der Karte zunächst erstmal nichts zu tun. Die Partikel schätzen die Position - aber nicht pro "Kartenpixel", sondern alle Partikel sind in einer Liste gespeichert und zu jedem Partikel wird gespeichert:

Position (Meter): x, y (Fließkkomma)
Orientierung: Winkel (Fließkomma)

Wenn man noch nicht weiss wo der Roboter sein kann, werden zunächst alle Partikel zufällig initialisiert, aber so dass die Partikel (grob gesehen) auf der Schleife in der Karte liegen.

Also z.B.:

Partikel 1: x=0.50m, y=0.60m
Partikel 2: x=0.49m, y=0.60m
Partikel 3: x=0.45m, y=0.32m
usw.

Dann fährt der Roboter und bei jeder kleinen Bewegung (z.B. 1cm) fahren die Partikel mit. Die Partikel fahren also im cm-Bereich mit. Nach einer Sekunde vergleiche ich alle Partikel mit der aktuellen Messung : liegt Partikel noch auf Schleife ja/nein? Falls nicht wird er verworfen und bei den Partikeln welche gut abschneiden wieder aufgefüllt. Dadurch dass nur die "richtigen" Partikel überleben, sammeln sich immer mehr Partikel an der echten Position an. Dadurch wird diese Schätzung dort immer genauer (viele Partikel in einem kleinen Umkreis).

Ich arbeite derzeit mit 1000 Partikeln. Weitere Tests folgen noch. Im Simulator lief es auch mit 100 Partikeln.


Gruss,
Alexander
 
Nachtrag: Jede Sekunde werden alle Partikel also nach Plausibiltät überprüft, Partikel verworfen und gegen neue ersetzt. Wenn man nun die Durchschnittsposition aller Partikel errechnet (Zentrum, der rote Kreis in dem Video), erhält man die geschätzte Position - diese geschätzte Position ist dann die neue Roboter-Position. Mit dieser geschätzten Roboter-Position fährt der Roboter dann weiter auf der Karte.

Also:

1. Roboter Position X,Y,Phi wird fortlaufend (relative Bewegung) durch Odometrie,Yaw aktualisiert (20 Hz)
2. Alle Partikel X,Y,Phi werden fortlaufend (relative Bewegung+Zufall) durch Odometrie,Yaw aktualisiert (20 Hz)
3. Nach Ablauf von einer Sekunde wird die (absolute) Position anhand aller Partikel geschätzt - diese Position ersetzt die bisherige Roboter Position X,Y
 
Muss man die Orientierung speichern, oder kann man diese nicht laufend neu berechnen? Etwa aktueller Kurs mit Zufallswert von +/- 10° und neue Position merken.
Dann wäre der Speicherbedarf schon mal kleiner, 100 Partikel = 200 Bytes. (ich verwende Festkomma Arithmetik, diese ist 5x schneller als Fließkomma)
 
Stimmt, die Orientierung wollen wir ja nicht schätzen, die braucht also der Partikel nicht speichern (das wäre ein anderes Anwendungs-Szenario wo man z.B. aufgrund von gemessenenen Entfernungen (Positionen) die Orientierung schätzen möchte).

Da ist bestimmt noch einiges an Optimierungs-Potential.

Muss das erstmal weiter testen und die Grenzen ausfindig machen ... ;)
 
Das tolle am Particle Filter ist dass er mit Wahrscheinlichkeiten arbeitet (also in einem eingestelltem Bereich tolerant ist) und seine Vielseitigkeit (man kann praktisch mit einem Algorithmus alles erschlagen) : möchte man z.B. einen weiteren Sensor hinzufügen (GPS, Beacon, Perimeter-Feldstärke), so ändert man einfach die Wahrscheinlichkeitsfunktion passend ab, welche die Wahrscheinlichkeit eines Partikel zun den aktuellen Messungen berechnet. Meine derzeitige (noch viel zu simple) Wahrscheinlichkeitsfunktion sieht z.B. so aus:


Code: 
//  computes the probability of a particle
float MapClass::measurementProb(int particleIdx, float measurement){
  robot_state_t particle = particles[particleIdx]; // get particle data (X,Y)
  if (!isXYOnMapMeter(particle.x, particle.y)) return 0; // a particle that is not on map has probability 0
  uint8_t data = getMapDataMeter(particle.x, particle.y); // get map state of particle's position on map
  if (data != MAP_DATA_PERI) prob = 0;  // a particle that is not on perimeter has probability 0
  return 1.0; // otherwise particle has probability 1
}


Nimmt man z.B. GPS noch hinein, könnte man falsche Particles schneller verschwinden lassen.

Ich würde ggf. einkalkulieren, dass man einen weiteren Mikrocontroller (oder größeren) benötigt, welcher dann diese Aufgaben (Karte, Particle Filter) erledigt.
 
Wie setzt du denn die neuen Partikel rein? Im Umkreis der errechneten Position oder neben für gut befundenen Partikel?

Oder könnte man in zB 2-5m Umkreis der GPS Position neue Partikel erstellen wenn andere rausfallen?

Habe sicher schon 4 Jahre lang immer wieder darüber gelesen, aber erst bei deinem Video ist der Geistesblitz und ein paar Ideen zur Umsetzung gekommen :)
Von großem Vorteil sehe ich das man diese Lösung ohne großen Rechenaufwand umsetzen kann.

Speicherplatz für 100 Partikel sollte zum schaffen sein, meine Karte ist 46x86 Bytes, A-Stern läuft mit halber Auflösung, und ein paar weitere Bytes kann man immer durch Optimierung der Variablen herausholen.

An größeren Controllern gibt es ja schon eine riesige Auswahl, jetzt sogar mit Atom Prozessor .
Beim Atmega kann man aber eigentlich zusätzliches RAM anschließen, habe es mir aber noch nicht im Detail angesehen. Speichern kann man diese dann über ein I2C EEPROM.

LG Werner
 
Ich habe den Code nach diesem Online-Kurs umgesetzt: https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373 Professor Sebastian Thrun erklärt die Grundlegen des Particle Filters (Kapitel 3) schrittweise und sehr gut verständlich (und hat auch beim Google Car mitgewirkt). https://www.udacity.com/course/viewer#!/c-cs373/l-48704330
Ich muss mir den Teil des "Resamplings" nochmal genau ansehen - es funktioniert jedenfalls :) https://www.udacity.com/course/viewer#!/c-cs373/l-48704330/e-48748082/m-48740082
So wie ich es sehe werden nur Kopien von "guten" Partikeln erzeugt. Die Verteilung nach dem Resampling ist identisch zur Gewichtung (Wahrscheinlichkeit) der Partikel. Also Partikel mit hoher Wahrscheinlichkeit werden öfter kopiert, dadurch fallen die mit geringer Wahrscheinlichkeit unterm Tisch.

Hier findet man übring meinen Simulator-Code incl. Particle Filter (etwas ältere Version): https://github.com/Ardumower/ardumower/tree/develop/sim
Der Filter funktioniert (in gewissen Grenzen) auch beim Umherfahren (Hier noch nicht ganz richtig eingestellt ... die Bedingungen mit dem kleinen Roboter sind aber auch nicht einfach):

https://youtu.be/kaCt7vCkF-g
 
Du musst dich einloggen oder registrieren, um hier zu antworten.
Oben