[SLAM] Lidar zur Erstellung einer Karte und zum Finden der Position des Roboters

[Minekonst]

Hi,
Ich arbeite in einem Schulprojekt schon lange an einem Roboter, der autonom durch ein Labyrinth fahren muss. Der Roboter verfügt über ein Lidar (genauer "RPLidar A1M8"), mit dem er die Umgebung scannen kann. Der Roboter ist dafür gedacht an dem "Rescue Maze" Wettbewerb des RoboCup Juniors teilzunehmen (Der Roboter ist etwa 18x22 cm groß). Der Wettbewerb funktioniert so, dass der Roboter in eine Arena gesetzt wird, die aus vielen „Räumen“ besteht, die jeweils 30x30 cm groß sind. Diese Räume haben dann zwischen 0 und 4 Wänden. Diese Arena, die der Roboter vorher nicht kennt, muss er dann autonom durchfahren. Wir verwenden das Lidar bereits, um die Wände in der Arena zu erkennen und sie in eine Karte einzutragen.

Jetzt würden wir gerne das Lidar nutzen um eine genauere Karte des Labyrinths anzufertigen, während wir durch dieses fahren. Damit wollen wir erkennen, wenn der Roboter nicht mehr dort ist, wo er glaubt, dass er gerade ist. Ich habe mich schon in die Richtung von Algorithmen, wie "Iterative Closest Point" und "Coherent point drift" umgesehen, habe aber bisher keinen Startpunkt für die Umsetzung gefunden. Da wir mit einem Raspberry Pi 3 fahren (wir wollen demnächst auf einen RockPi umsteigen), ist auch die Geschwindigkeit des Algorithmus nicht ganz unerheblich.

Wie ich meine Karte bauen kann, nachdem ich weiß wo der Roboter sich bewegt hat, ist mir relativ klar. Nur weiß ich nicht, wie ich genau herausfinden kann, wie er sich bewegt hat, da im Labyrinth auch Hindernisse stehen können, an denen der Roboter hängengeblieben sein könnte. Am besten wäre für mich ein System, mit dem der Roboter über die durch das Lidar erstellte Karte navigieren könnte, um automatisch Hindernissen auszuweichen und um schneller zu sein, als das Ganze „Feld-für-Feld“ anzugehen.

Habt ihr irgendwelche Ideen oder Erfahrungen, wie ich so ein System bauen kann bzw. welche Algorithmen ich genau verwenden sollte, um herauszufinden, wo sich der Roboter gerade befindet bzw. wie er sich bewegt hat über die Bilder die das Lidar ausgibt?

[Moppi]

Hallo,

ein Teil scheint mir etwas unverständlich:

Nur weiß ich nicht, wie ich genau herausfinden kann, wie er sich bewegt hat, da im Labyrinth auch Hindernisse stehen können, an denen der Roboter hängengeblieben sein könnte.

Wie der Roboter sich bewegt, müsste doch bekannt sein, ob er also geradeaus fährt oder rückwärts, vorwärts oder ....

Ich weiß nicht, ich würde intuitiv anfangen, eine Karte zu bauen. Eine "Karte", die in positive und negative Richtung offen ist. Wie beim "Schiffe versenken" würde ich in ein Raster die Hindernisse eintragen. Wenn ich auch etwas stoße, von dem ich nicht weiß, ob es ein Hinderniss ist, das umfahrbar ist, muss ich versuchen drum herum zu fahren. Ist dies nicht möglich, ist es eben kein umfahrbares Hindernis.
Je nachdem, in welche Richtung ich fahre, trage ich die Hindernisse auf der Karte auch ein.


Ob der Roboter sich von der Stelle bewegt, kann man sicher auch irgendwie feststellen. z.B. ob sich die Räder drehen, wenn der in eine Richtung fahren soll.

Welche Sensoren hat der Roboter denn überhaupt, um sein Vorankommen zu erfassen? Ist noch nicht mal klar, wie der überhaupt beschaffen ist.


MfG

[Minekonst]

Danke für deine schnelle Antwort!

Das ich nicht immer genau weiß, wie er gefahren ist, meine ich so, dass der Roboter an "Bumpern", also Dinge auf dem Boden, die ihn am Fahren hindern, sich gerne so festhängt, dass sich die Räder zwar gedreht haben, aber der Roboter sich nicht von der Stelle bewegt hat. Auch kommt es vor, dass der Roboter so mit einem Hinerniss zusammenprallt, dass er nicht mehr wirklich weiß wo er ist. Das heißt, der Roboter trägt dann in seine Karte, wo die Wände und so verzeichnet sind ein, dass er einen Raum weiter gefahren ist, wobei er eigentlich noch im Raum davor steht. Das führt dann dazu, das späteres Navigieren problematisch wird.

Außer dem Lidar haben wir an Sensoren noch einen Kompass-Sensor (BNO055) und Sensoren, die für den Wettbewerb wichtig sind, wie zum Beispiel Wärmesensoren, Kameras und Lichtsensoren. Das der Roboter sich verfährt kommt zwar nicht so oft vor, aber oft genug, dass wir ein System brauchen, dass das erkennt. Außerdem wäre es cool mehr aus dem Lidar zu machen, also darüber zu navigieren.

[HaWe]

hallo,
du weißt ja, dass die Wände immer 30cm oder ein Vielfaches voneinander entfernt sind, daher kannst du über die Messung der rel. Abstände zu den Wänden deine Positionsbestimmung etwas verbessern: ein Algorithmus dazu wäre z.B. die Monte-Carlo-Methode (MC-Simulation, Partikelfilter).

Dennoch riskierst du hier falsche Ergebnisse, da du dich an Dingen orientierst, die nicht 100% bekannt sind, schließlich musst du dein Labyrinth ja erst per SLAM erforschen, und folglich beißt sich das irgendwie in den eigenen Schwanz.

Wirklich hilfreich , um die Odometrie zu verbessern, wären nur Sensorfusionen mit Gyro, Accelerometer und Kompass durch 9D Sensorfusion, z.B. per Extended Kalmanfilter (es gibt Sensoren, die das schon teilw. durch einen onboard 9D-dsp tun, z.B. CMPS12 (einfach) oder MPU9250 (schwierig und aufwändig), den 9D Kalman musst du dann noch mit der Odometrie fusionieren ).

Auch hier hast du aber nur 1 absolute externe Referenz , nämlich das Erdmagnetfeld (via Kompass), der Rest an Sensorwerten ist theoretisch beliebig verrauscht - das ist etwas dünn für eine exakte Navigation, gerade bei Schlupf und magn. Störfeldern.

Also hilft zur Ortung nur ein zusätzliches externes Ortungssystem, wie Baken per dm-Wellen ("Indoor-GPS"), s. z.B. Pozyx (teuer aber sehr genau).
Falls das nicht in Frage kommt, musst du mit den Ungenauigkeiten aus MC-Simulation und Kalmanfilter leben.

Was nicht-statische Hindernisse angeht, könnte man das virtuell erstellte Bild des Labyrinths "schattieren", d.h. mit Grauwerten versehen.
weiß = frei,
Grautöne = Hindernis mit zunehmender Schwärzung bei wiederholter Messung, kann auch wieder zu weiß ausradiert werden falls es plötzlich wieder verschwindet
schwarz = dauerhaftes Hindernis ab der soundsovielten konstanten Messung

Weiterer Vorteil bei dem grauschattierten virtuellen Labyrinth:
du kannst die Einzelfelder direkt in einem A* (AStern) weiterverarbeiten, zur Ermittlung des kürzesten Weges aus dem Labyrinth heraus, wobei der Grad der Schwärzung mit verschieden hohen Wegekosten korreliert werden kann.

[Minekonst]

Hallo HaWe,
Danke für deine Rückmeldung, ich werde mir die Sensoren, die du beschrieben hast auf jeden Fall ansehen. Ein "Indoor-GPS" System können wir leider nicht verwenden, da das der Wettbewerb nicht zulässt. Auch deine Idee mit den Schattierungen werde ich mal versuchen für unser Projekt umzusetzten.

[Ceos]

Ein Gedanke den ich schon länger mal ausprobieren wollte, aber zumindest privat nie wirklich in angriff genommen habe wäre eine Computermaus (genauer gesagt den Maussensor, kann man als einzelne bastlerplatine kaufen) für die Wegerfassung zu nutzen

Das wäre Kontaktlos und auf 95% aller Oberflächen kein Problem

Da der Sensor geschützt verbaut werden kann und solange man sicher geht dass der robo sich nicht unnötig selber aufbockt (Also den Boden aus dem Fokus nimmt) sollte man damit sehr zuverlässig die Wegstrecke tracken können

Versuch doch mal irgendwo her eine alte optische PS2 Maus zu organisieren, dann kannst du das zumindestens erstmal testen, denn PS2 ist nichts weiteres als gammeliges UART in eiem runden Stecker

Wenn dein Robo eine Gehirn mit USB Port besitzt gehts auch mit einer USB Maus natürlich

[Moppi]

Ein Gedanke den ich schon länger mal ausprobieren wollte, aber zumindest privat nie wirklich in angriff genommen habe wäre eine Computermaus (genauer gesagt den Maussensor, kann man als einzelne bastlerplatine kaufen) für die Wegerfassung zu nutzen

Das wäre Kontaktlos und auf 95% aller Oberflächen kein Problem

Da der Sensor geschützt verbaut werden kann und solange man sicher geht dass der robo sich nicht unnötig selber aufbockt (Also den Boden aus dem Fokus nimmt) sollte man damit sehr zuverlässig die Wegstrecke tracken können

Versuch doch mal irgendwo her eine alte optische PS2 Maus zu organisieren, dann kannst du das zumindestens erstmal testen, denn PS2 ist nichts weiteres als gammeliges UART in eiem runden Stecker

Wenn dein Robo eine Gehirn mit USB Port besitzt gehts auch mit einer USB Maus natürlich

Zwar ist das Problem evtl. gelöst, dass man Stillstand bemerkt, selbst wenn sich die Motoren drehen, aber er kann dennoch nicht feststellen wo er sich genau befindet. Wenn der Roboter nun angefahren wird und einmal versetzt wird, stimmt die Position nicht mehr. Gleiches, sollte er manuell versetzt werden. Er muss sich irgendwie an der Umgebung orientieren, wie mit Kamera, also optisch. Oder er müsste ständig die Umgebung kontrollieren und ein Notprogramm haben, das er drohende Kollision frühzeitig erkennt und dem "Angreifer" ausweicht - so ist er immer Herr über seine eigene Position. GPS fällt ja aus.


MfG

[Ceos]

Wenn der Sensor nicht gerade mitting in der Drehachse verbaut ist kann er wunderbar die Rotation mitlesen

Außerdem hat er Lidar ... ich kenne zwar die präzision nicht, aber sollte er damit nicht in der Lage sein Hindernissen auszuweichen!?

Einwirkungen von außen sind nirgendwo beschrieben und ich habe auch EXTRA gesagt dass man sorge tragen muss den Fokus nicht zu verlieren.

Man kann auch das Lidarbild nutzen um sich zu orientieren, solange das Labyrinth ausreichend "unterscheidbare" Kriterien pro Raum hat(also die Räume nicht zu identisch aussehen) kann man einfach das aktuelle 360° Bild nehmen und in die Karte projezieren und dann mittels Bildsuche die Position und Orientierung ermitteln. Aber das sehe ich kritisch bei der zur Verfügung stehenden REchenleistung.

Warum nicht ein einfacherer Ansatz?

[Holomino]

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.