Was bei mir auf'm BPI M2 Zero noch läuft:
- Schaffe eine xy-Rasterkarte (statisch, dynamisch oder besser noch dynamisch mit Kacheln), also ein 2D-Array, bei dem jedes Element ein Stückchen Boden (z.B 3x3cm) beschreibt.
- Sammele die Daten eines Rundumscans, trage den ersten Rundumscan stumpf in die Rasterkarte ausgehend von Deiner virtuellen Pose (x,y-Koordinate + Drehwinkel) ein. Das Eintragen erfolgt mithilfe der Bresenham-Linienbildung, d.h. die freien Rasterpunkte bis zum Hindernis werden dekrementiert, der Rasterpunkt am Hindernis wird inkrementiert. Negatives Vorzeichen heißt also, da darf man fahren, positives Vorzeichen deutet auf ein Hindernis.
- Bei allen folgenden gesammelten Rundumscans wird zuerst getestet, anschließend eingetragen. Bilde dazu per Zufallsgenerator einige zig Variationen über die aktuelle Pose (mit Odometrie: letzte Pose+gemessene Odometrieänderung / ohne Odometrie muss der Variationsbereich die mögliche Posenänderung seit dem letzten Scan umfassen). Der Test selbst wird wieder mithilfe des Bresenhams ausgeführt: Für jede angenommene Pose wird per Bresenham jede Scanlinie auf Übereinstimmung mit der Rasterkarte geprüft (Vergleich der Vorzeichen), das ergibt im einfachsten Fall einen Zähler, der geteilt durch die Anzahl aller im Test durchgesteppten Bresenham-Linienpunkten die Güte Deines Matchings für die getestete Pose angibt. Nach Durchlauf aller angenommen Posen ergibt sich so eine Gewinnerpose, die a) als tatsächlich Pose für den folgenden Ablauf behalten wird (Localization) und b) anhand derer nun die letzten Scanwerte wirklich in die Rasterkarte eingetragen werden (Mapping).
Anmerkungen:
- Man kann das mit mehreren Maps parallel machen. Es hilft beim closed-loop-Problem, wenn mehrere Möglichkeiten längere Zeit am Leben gehalten werden.
- Man kann bei der Gewichtung im Test die tatsächlichen relativen Fehler des Sensors einbeziehen.
- Man muss nicht jeden Rundumscan des Sensors bei voller Auflösung verwenden. Wenn's zeitlich eng wird, dünnt man aus.
- Die Tests der einzelnen Posen sind als Hauptrechenlast optimal für multithreading geeignet.
Was beim Pathfinder (AStar&Co suchen den kürzesten Weg) erschwerend hinzukommt: Man hat zwar mit so einer Rastermap die Info, auf welchen Rasterfeldern Hindernisse liegen, wenn die Abmessungen des Robbis die Größe eines Rasterfeldes überschreitet, knallt man trotzdem unweigerlich beim Umfahren einer Ecke davor (dem Algorithmus fehlt die Info, welche Abmessungen das Fahrzeug hat). Dieses Problem kann man umgehen, indem man die Werte aus dem einen Grid (ich nenne es LidarGrid) auf ein zweites Grid (ErodedGrid) überträgt. Nur beim Vorzeichenwechsel eines Punktes im Lidargrid in-/dekrementiert man die Punkte einer an die Fahrzeuggröße angepasste umliegende Maske (z.B. 7x7 Rasterpunkte) im ErodedGrid (Man muss dieses Feld nicht jedes mal komplett neu erzeugen). Tatsächlich ist es dem Pathfinder in der folgenden Rechnung auf dem ErodedGrid egal, ob der Robbi oder das Hindernis eine Ausdehnung hat. Er findet dann freie Felder erst etwas weiter wech vom Hindernis. Im Zweifelsfall reicht dem Pathfinder ja ein 1 Pixel breiter Durchgang, um einen Weg durch einen Engpass zu finden.
Das Problem mit dem maulwurfsblinden Lidar kenne ich auch. Naturgemäß montiert man das "teuer Teil" für eine freie Rundumsicht am höchsten Punkt. Da hilft dann zusätzliche Sensorik (z.B. zwei Sharp Sensoren mit gekreuztem Sichtfeld), die unter der Lidarebene wache schiebt.
Ein anderer Ansatz: Eine redundante Odometrie, also zwei zusätzliche Messräder (mit gelagertem Arm, Dreipunktauflage beachten!) neben die Antriebsräder zu packen, funktioniert in der Praxis auch ganz gut, ist aber mechanisch aufwendig und macht das Fahrzeug insgesamt breiter.
Egal, was man da nimmt, man kann diese Infos in ein drittes Grid (BumperGrid, das Lidar wird solche Hindernisse ja nicht erkennen) eintragen und ebenfalls mit dem ErodedGrid verknüpfen.
Ich hab auch noch ein viertes Grid (ShortTimeGrid) im Angebot, in das nur die letzten zwei, drei Scans des Lidars eingetragen werden. Man kann sich vorstellen: Ein 200 mal als "frei" detektiertes Feld muss im LidarGrid bei dieser ganzen In-/Dekrementiersache auch wieder 200 mal als besetzt detektiert werden, bevor der Pathfinder schnallt, dass es zur Zeit hier nicht langgeht, weil spontan die Katze im Weg sitzt. Auch das ShorttimeGrid wird wieder mit dem ErodedGrid verknüpft. Während das LidarGrid eher träge das Gedächtnis der Umgebung bildet, aber super das SLAM-Problem löst, zeigt das ShortTimeGrid besser den aktuellen Zustand und kurzzeitige Veränderung an, die der Pathfinder manchmal eben auch benötigt.
Wie gesagt, das alles läuft bei mir auf'm BPI (4x1GHz Arm) unter Mono (Also ein adaptiertes C#-Windoof-Projekt. Was ich so bislang gesehen und ausprobiert habe, liegen die Leistungseinbußen gegenüber nativem C so etwa bei 60%) ca. im Sekundentakt (SLAM + Pathfinding) in der Wohnung (100m²) mit einer Auflösung von 5x5cm. Und ich bin sicher nicht der Optimiergott.
Zitieren
Lesezeichen