Menü
Aktuelles
Von:
Filter

Particle Filter

Hier noch eine Grafik um die Roboter-Karte besser beurteilen zu können. Meines Erachtens dürfte die Kartenerstellung noch ein bisschen präziser werden. Die Winkel und Längen sind teilweise ganz weit weg von der Realität - Evtl. doch noch ein Bug drin ...

map1.png

Attachment: https://forum.ardumower.de/data/media/kunena/attachments/905/map1.png/
 
Zuletzt bearbeitet von einem Moderator:
Eine Idee zur Vereinfachung:
Nicht für jedes Partikel einen neuen Kurs und danach die neue Position berechnen.
Stattdessen:
Positionsdifferenz vom Roboter x,y alt - x,y neu ist bekannt, daher neue Position von den Partikel ist einfach die Differenz x,y mit ein bisschen Rauschen.
x_neu = x_alt + x_dif*rauschen. Für y analog. Damit hat man Weg und Orientierung mit etwas Rauschen erledigt.

Im Beispielcode wird auch die Orientierung des Roboters über den Partikelfilter berechnet. Ist das sinnvoll, oder nimmt man besser gleich die Richtung aus der IMU.

Ich will das mal mit 50 Partikel im Roboter testen, wenn es funktioniert aber mehr Partikel notwendig sind wird wohl ein 2. Kontroller notwendig werden. Ein Atmega1284 hätte 16kB Ram, genug Platz für Karte und Partikelfilter.

LG Werner
 
Nun, typischerweise will man aber das Rauschen statistisch korrekt umsetzen, d.h. jede Messung/jedes System hat typischerweise einen Meßfehler nach einer Normalverteilung (Gauß). Die Normalverteilung kann man modellieren mit Mittelwert und Standardabweichung. Man arbeitet also mit Mittelwert (mu) und Abweichung (sigma).

1. Distanzfehler: Sigma_distanz
2. Winkelfehler: Sigma_winkel

Damit werden dann die Partikel bewegt:
Distanz_mit_fehler = gauss(Distanz_gemessen, Sigma_distanz)
Winkel_mit_fehler = gauss(Winkel_gemessen, Sigma_winkel)

x_neu = x_alt + Distanz_mit_fehler * cos(Winkel_mit_fehler)
y_neu = y_alt + Distanz_mit_fehler * sin(Winkel_mit_fehler)

Dadurch sind die neuen Positionen wieder Gauß-verteilt.

So wird es typischerweise gemacht, ob man das wirklich braucht ist die zweite Frage :) - Dein Ansatz ist natürlich einfacher :)

Btw: Ich habe mal gerade den Indego eine Weile fahren lassen, dann vorn einem Magneten befestigt - Bosch löst es wie ich es oben vorgeschlagen hatte: beim nächsten Kalibrieren wird der falsche Kompaß zunächst ignoriert, er fährt erstmal mit dem alten Kurs weiter (ihm fällt wohl auf dass der Kompaß irgendwie nicht stimmt). Beim wiederholten Kalibrieren kann er schließlich nicht anders (der Gyro-Kurs ist zu alt). Ab nun sind seine Bahnen alle gedreht :) - Nehme ich den Magneten wieder weg, übernimmt er auch den ursprünglichen Kurs wieder beim Übernächsten Kalibrieren und die Bahnen sind wieder alle zurück gedreht.
 
Ich will ja den Filter vereinfachen, die Berechnungen sollen nur so genau wie nötig sein.
So ein Atmega is ja nicht gerade ein Rechenkünstler ;)

Die erste Testfahrt mit Partikelfilter war nicht erfolgreich, die Position geht nicht über 250cm hinaus.
Über das Resampling bin ich mir noch im unklaren, füge die neuen Partikel im Umkreis von 2m zur Position ein, keine Ahnung ob das ein guter Ansatz ist.
War jetzt nur mal ein erster Schnellschuss.

Gut zu wissen wie der Bosch mäht, dann kann man es ja nachmachen.

LG Werner
 
Ich glaube mit Trial-and-Error in einem realen Roboter kommen wir nicht schnell genug voran - vielleicht sollten wir die Simulation weiter ausbauen und damit weiter testen? Oder wir schicken die Daten des realen Roboters zum PC (WLAN/Bluetooth?) und lassen dort den Filter erstmal laufen? Gerade reale Daten wären wohl erstmal sinnvoll. Am Rechner könnte man dann mit diesen Daten verschiedenes durchspielen? Was wäre einfacher?
 
Ich bin noch dabei das Prinzip richtig zu verstehen....

In einen Codeschnippsel von dir war die Wahrscheinlichkeit für ausserhalb Karte und nicht am Perimeter =0, nur am Perimeter =1.
Das ist für die Schleifenfahrt sinnvoll, aber wenn man von der Schleife weg ist, müsste es genau umgekehrt sein.
Dann ist aber praktisch fast jede Position sinnvoll.

Andere Annahme: Roboter nähert sicher der Schleife, lt. Karte ist er 50cm entfernt. Jetzt müssten doch alle Partikel die 50cm von der Schleife entfernt sind eine hohe Wahrscheinlichkeit bekommen, andere eine geringere. Für jedes Partikel alle möglichen Abstände zu berechnen ist auf so kleinen Kontrollern nicht machbar.

Wenn man nur Wahrscheinlichkeiten von 0 und 1 hat, benötigt man eine große Partikelwolke um die Umgebung erfassen zu können, und die muss dann auch 2m oder mehr Durchmesser haben.

Oder bin bin beim Ansatz jetzt total daneben :blink:
 
Werner schrieb:
Ich bin noch dabei das Prinzip richtig zu verstehen....

1) In einen Codeschnippsel von dir war die Wahrscheinlichkeit für ausserhalb Karte und nicht am Perimeter =0, nur am Perimeter =1.
Das ist für die Schleifenfahrt sinnvoll, aber wenn man von der Schleife weg ist, müsste es genau umgekehrt sein.
Dann ist aber praktisch fast jede Position sinnvoll.

2) Andere Annahme: Roboter nähert sicher der Schleife, lt. Karte ist er 50cm entfernt. Jetzt müssten doch alle Partikel die 50cm von der Schleife entfernt sind eine hohe Wahrscheinlichkeit bekommen, andere eine geringere. Für jedes Partikel alle möglichen Abstände zu berechnen ist auf so kleinen Kontrollern nicht machbar.

3) Wenn man nur Wahrscheinlichkeiten von 0 und 1 hat, benötigt man eine große Partikelwolke um die Umgebung erfassen zu können, und die muss dann auch 2m oder mehr Durchmesser haben.

Oder bin bin beim Ansatz jetzt total daneben :blink:
Zum Vergrößern anklicken....

Ja, ist schon richtig gedacht.

1) Im aktuellen Roboter Code benutze ich das als Wahrscheinlichkeitsfuntkion:

Code: 
//  computes the probability of a particle
float MapClass::measurementProb(int particleIdx, bool isOnPerimeter){
  float prob = 1.0;
  robot_state_t particle = particles[particleIdx];
  if (!isXYOnMapMeter(particle.x, particle.y)) return 0;
  uint8_t data = getMapDataMeter(particle.x, particle.y);
  if ((Robot.state == STAT_CREATE_MAP) || (Robot.state == STAT_TRACK)){
    // tracking perimeter
    if (data != MAP_DATA_PERI) prob = 0;
  } else {
    // mowing
    if (isOnPerimeter){
      if (data != MAP_DATA_PERI) prob = 0;
    } else {
      if (data == MAP_DATA_PERI) prob = 0;
      if (data == MAP_DATA_OUTSIDE) prob = 0;
    }
  }
  return prob;
}


Ich unterscheide also das Abfahren und das Umherfahren.

2) Du könntest den Spieß umdrehen: die Karte gibt für jedes Pixel die Entfernung zur Schleife an :) Jetzt kannst Du mit einer einfachen Array-Abfrage sofort wissen wie weit ein Partikel auf der Karte von der Schleife entfernt ist. Natürlich brauchst auch noch du die gemessene Feldstärke, sonst kannst Du ja keine Wahrscheinlichkeit für einen Partikel weiter weg von der Schleife berechnen.

3) Der Durchmesser ergibt sich quasi von selbst. Wenn viele Partikel über längere Zeit überleben, wird der Durchmesser automatisch größer, und ja dann sind generell viele Partikel sinnvoll.
 
Zuletzt bearbeitet von einem Moderator:
Ich hab den Filter mit 100 Partikel im Roboter getestet. Nach 10min war das Ergebnis die aktuelle Position. Odometrie, GPS Rohdaten und Partikelfilter lagen alle innerhalb +/- 2m.
Aber später war die Position weg (>10m Abweichnung), und sie wurde nicht wieder errechnet.

Ansatz war normale Fahrt, Felder ausserhalb =0, innerhalb =1. Keine echte Wahrscheinlichkeitsberechnung, dh es gibt nur 0 oder 1.
Am Start sind Partikel zufällig im gesamten Mähbereich verteilt.
Wenn Partikel rausfallen kommen neue im Umkreis um 200cm um die aktuelle Position nach Odometrie dazu.
Komplette Berechnung 4x in der Sekunde.

Hab mal 700 Partikel getestet, aber das schafft der Atmega nicht mehr neben alle anderen Aufgaben.

Versuche als nächstes den Umkreis für neue Partikel zu erhöhen, vielleicht kann er dann die Position halten, bzw wieder finden.

LG Werner
 
Nun, das deckt sich mit dem Indego und der Simulation. Es gibt (seltene) Situationen wo der Filter die Position verliert. In diesem Fall sinkt die Gesamtwahrscheinlichkeit (über alle Partikel) bzw. der Radius (über alle Positionen) steigt dauerhaft an. Detektiert der Indego (bzw. der Simulator) so eine Situation gibt es nur eins: Partikel auf Neustart oder Schleife ein Stück abfahren (bis Radius wieder sinkt). Ich würde mit dem Abfahren der Schleife anfangen, das muss 100% zuverlässig funktionieren (ansonsten ist der Filter falsch eingestellt). Das Abfahren kann dann jederzeit zum Wiederfinden verwenden.

Ich probiere derzeit die Feldstärke mit zu verwenden - theoretisch sollte das Umherfahren damit robuster werden...

map123.png

Attachment: https://forum.ardumower.de/data/media/kunena/attachments/905/map123.png/
 
Zuletzt bearbeitet von einem Moderator:
Na dann bin ich beruhigt wenn es nicht nur mir so geht ;)
Für den ersten Versuch war es ja doch ein gutes Ergebnis.

Der Filter fängt sich wenn die Positionen nach Odometrie und Filterposition halbwegs zusammen fallen, ab dann funktioniert es.
Fraglich ist ob es dann noch was nutzt, Odometrie war schon vorher und nachher genauer.
Man braucht noch andere Umgebungsdaten damit das funktionieren kann. Denn auf freier Fläche sind alle Partikel gleich wahrscheinlich.
Also für Schleifenfahrt bringt es was, ansonsten weniger.
 
Nach einigen Bugfixes (bessere Kartierung) und weiteren Tests konnte der kleine Roboter die Position im laufenden "Mähen" erstaunlich gut und dauerhaft detektieren.

Neue Erkenntnisse:
- der Filter funktioniert am Besten wenn man das Mähmuster so wählt, dass es nicht zu Mehrdeutigkeiten kommen kann - Konkret: das Mähmuster sollte z.B. nicht parallel zum Rand erfolgen sondern z.B. 45 Grad hierzu (siehe unten). Dadurch sind die Mähbahnen schon nach kurzer Zeit nicht gleich lang und liefern dann brauchbare Informationen für den Filter (rein zufällig?: dieses Verhalten deckt sich mit dem Mähmuster vom Indego)

- exaktes Geradeausfahren ist Voraussetzung damit man den Kursfehler für Partikel herabsetzen kann (habe den Kompaß aufgrund der Fehler in der Wohnung einmal deaktiviert und allein mit Gyro ist der absolute Kurs erstaunlich präzise (Fehler unter 1 Grad - kann der Gyro phasenweise so genau bleiben??))


particles6.png

Attachment: https://forum.ardumower.de/data/media/kunena/attachments/905/particles6.png/
 
Zuletzt bearbeitet von einem Moderator:
@Werner: zu deiner Aussage "läuft bereits mit Odometrie genauso gut" : der Filter korrigiert bei mir ständig die aktuell gemähte Bahn - würde ich ohne den Filter arbeiten (Roboter nur am Rand einer Bahn zurück auf die Schleife setzen), würde die Bahn ja nicht korrigiert und die Position über längere Zeit von der realen Abweichen (ich benutze kein GPS o.ä.)
 
Das bezog sich auf die Fahrt in freier Fläche. Mit meinem Ansatz wie oben beschrieben hilft mitten am Grund der Filter nicht. Erst bei einer Engstelle wurde die richtige Position errechnet. So wie du schreibst, es darf keine Mehrdeutigkeiten geben.
Bei 45 min Fahrt lieferte der Filter nur nach 10 min für 15min gute Ergebnisse, aber nie besser als Odometrie oder GPS.
 
Könntest Du das Mähverhalten evtl. so abändern dass es keine Mehrdeutigkeiten geben kann? Also möglichst immer "schräg" zur Schleife und möglichst immer quer über die gesamte Fläche so dass er Bahnen immer von Schleifen-Grenze zu Schleife-Grenze mäht? Dann müsste es doch auch auf der Fläche richtig gut laufen.

Engstellen: das ist gut! Genau das braucht man ja um z.B. bei einer engen Passage die Navigation mit Hilfe der Karte zu schaffen. Vermutlich wird man dann auch noch die Schleifen-Feldstärke mit in den Filter einbeziehen (meine Feldstärken hier in der Wohnung schwanken zu stark/zu schnell, evtl. hilft es wenn ich die zuvor filtere)
 
Ich hab derzeit keine besonderen Mähverhalten außer Zufallszahl, Spiralfahrt, und Fahren zu einer bestimmten Position. Über Bahnmähen habe weder nachgedacht noch ein Konzept.
Schleife ist auch noch immer defekt.

Eines kann ich beim Filter noch probieren: alle Partikel entfernen die weiter als 10m von der GPS Position entfernt sind. Dann ist er zumindest in der richtigen Ecke vom Grundstück.
 
Ich denke man kann zusammenfassend festhalten:

1) Genauigkeit: Mit einem Particle Filter wird man niemals dauerhaft besser sein als die Sensoren Daten liefern können, bestenfalls im Durchschnitt etwas besser sein bzw. ggf. kann man sogar Sensoren überflüssig machen (z.B. GPS).

2) Mähmuster: Da man auch mit dem Filter keine cm-Genauigkeit erreicht/die Karte auch nicht cm-genau ist, ist es beim zufälligen Mähen schwierig zu ermitteln ob eine bestimmte Stelle richtig gemäht wurde. Wenn der Roboter jedoch präzise in Bahnen mäht, lässt sich zumindest ermitteln ob eine Fläche richtig gemäht wurde (solange man bis zu den Schleifengrenzen mäht) - einfach weil beim Bahnen mähen keine Stelle mehr übrig bleibt wo man noch nicht gewesen sein konnte. Hierin sehe ich den großen Vorteil.
 
Hallo Alexander,

Ich glaube das muss man noch mehr testen, es kann sein das es hilft, auch wenn es jetzt nicht so danach aussieht. Der Vorteil liegt bei engen Passagen und zerklüfteten Grundstücken. Dazu braucht es aber eine sehr genaue Karte wo möglichst alle Hindernisse erfasst sind. Ebenso wären mehr Sensoren zur Umgebungserkennung von Vorteil damit man die Wahrscheinlichkeiten der Partikel besser berechnen kann, aber dann braucht man bedeutend mehr Rechenleistung und Speicher.

GPS würde ich nicht weglassen, es hilft schnell eine eindeutige eine Position festzulegen (speziell kidnapped Roboter). Und man hat gratis eine genaue Uhrzeit :)

Mähmuster: hab jetzt keinen Indego als Vergleich, aber 20m lange Bahnen gut zu mähen stelle ich mir schwierig vor. Oder mein Grund ist dazu zu uneben das er keine so lange gerade Bahn schafft.
Dazu kommt die Effektivität: je mehr die Bahnen überlappen (müssen), desto geringer ist die effektive Mähleistung.
Hab das gerade bei meiner Spiralfahrt bemerkt: in 4 Minuten schafft er eine Fläche mit einem Durchmesser von 3.5m lückenlos zu mähen, sind 9.6m2. Bei freier Fahrt dagegen (1km/h) wären das 17m2.

Sind andere Mähmuster vielleicht effektiver? zB Quadrate abfahren?


Das Erkennen wo der Mäher schon war funktioniert bei mir schon jahrelang überaschend gut, die als gemäht bezeichneten Flächen passen sehr gut mit der Wirklichkeit zusammen. Man kann damit schon auf 1m genau sagen wo gemäht wurde, dann räumt man dort mit einer Spiralfahrt den Rest auf.

Was man beim Ardumower verbessern könnte:
.) Odometrie ist sehr einfach, diese hat bei Kurven Abweichungen.
.) Man kann die Grenzen dazu verwenden ungültige Positionen auszuschließen. zB eine Insel am Grund, die Position kann nie in der Insel sein, sie muss seitlich davon liegen. Erforder wieder eine genaue Karte.
 
Du musst dich einloggen oder registrieren, um hier zu antworten.
Oben