SLAM auf dem ESP32?

[Holomino]

Alles dynamisch
Die neuen Funktionen lassen sich grob durch die Begriffe Kurzzeit- und Langzeitgedächtnis umreißen.
Das Kurzzeitgedächtnis umfasst Kenntnisse über den aktuellen Zustand (so dass der Pathfinder einen Weg finden kann), das Langzeitgedächtnis muss ausreichende Informationen zur dauerhaften Lokalisierung bieten.

Wer mit der letzten Version etwas gespielt hat, wird sicher bemerkt haben: nach der erstmaligen Exploration (Filmchen) tut sich nicht mehr so viel an der Karte. Es war ja auch alles noch statisch. Was tun, wenn Türen in der gezeichneten Wohnung verschlossen werden oder das typische Kinderchaos auf den Böden wütet?

Insbesondere das Kinderchaos (auch Erwachsene bleiben manchmal ewig jung) wirft das nächste Problem auf: Spielzeug oder hingeworfene Klamotten sind oft so flach, dass das Lidar sie gar nicht erst sieht (wegen der freien Rundumsicht wird das Lidar üblicherweise am höchsten Punkt des Roboters angebracht). Den Rest muss die Nahfelddetektion (Hindernissensoren) erledigen. Deren Daten können aber schlecht zur Lokalisierung verwendet werden. Entweder sehen die Nahfeldsensoren Dinge, die das Lidar schpn längst gesehen hat (redundant) oder sie sehen Hindernisse, die sich nicht mit den Lidardaten vereinbaren lassen (der Sache nicht förderlich).

Lösung: Wir können einen Grundzustand der Umgebung in das Langzeitgedächtnis (Lidarkarte) prägen. Wenn wir die Werte der Nahfeldsensoren zusammen mit den Messergebnissen der letzten paar Lidarscans noch in ein temporäres Kurzzeitgedächtnis schreiben und dies ebenso auf die Eroded-Karte übertragen, wie wir es auch schon mit der Lidarkarte gemacht haben, bietet die Eroded-karte einen aktuellen Zustand für den Pathfinder. Es muss nur reichen, um ein umfahrbares Hindernis zu umfahren oder eine wirkliche Blockade (geschlossene Tür) bis zum Abbruch der Mission zu erkennen.

Showtime
Zur Demonstration all dieser Funktionen habe ich Spielkram eingebaut:
In der HTML-Seite kann man jetzt durch Rechtsklick den Basispunkt (blaues Fähnchen) und durch Linksklick das Ziel (gelbes Fähnchen) festlegen. Der Roboter wird versuchen, zwischen den beiden Punkten zu pendeln. Der Unterschied zwischen Basis und Ziel: Wenn das Ziel nicht erreicht werden konnte, kehrt der Roboter nacxh kurzer Wartezeit zur Basis um. Wenn der Pfad zur Basis abgeschnitten wurde, bleibt der Roboter stumpf an seinem Standort (und verhungert). Praktischer Sinn dahinter: Wenn der Roboter während der Fahrt zu einem nicht erreichbaren Ziel auch noch den Kontakt zum WLAN verliert, kehrt er hoffentlich noch zur Basis zurück.

Unterhalb der Kartendarstellung ist ein Stimuli-Feld zur Eingabe eines bitcodierten Wertes:
Bit 0: Sperrt das Mapping im SLAM (Localization läuft aber weiter).
Bit 1: Schaltet zusätzlich beim simulierten Roboter einen frontal ausgerichteten Nahfeldsensor mit Reichweite 1000 ein.
Bit5..7 schaltet die entsprechend gekennzeichneten Felder der Simulationskarte um, dass sie als Hindernisse wirken, die nur für Nahfeldsensorik und Bumper sichtbar sind.
Bit2..4 schaltet die entsprechend gekennzeichneten Felder der Simulationskarte um, dass sie als Hindernisse wirken, die auch vom Lidar gesehen werden.



Wenn ich also beispielsweise den Wert 43 (binär 00101011) im Stimulifeld eingebe (und mit ENTER abschließe), schalte ich damit im Raum unten rechts die große für das Lidar unsichtbare Blockade ein, schließe gleichzeitig die untere Tür. Außerdem ist dann das Mapping blockiert und der Nahfeldsensor eingeschaltet.

Programmtechnisch befindet sich das Kurzzeitgedächtnis als "ShortTimeList" im Grid-Objekt (Common.cpp/.h). Das Sperren der Lidarkarte beschränkt sich auf die Funktion Slam::IntegrateVariation und der rudimentärste aller Routenplaner versteckt sich in SLAM.cpp/.h


Diese Version (0.9) liegt, wie gewohnt auf https://c.gmx.net/@31902611639422705...SkGUZEDN7AmUzQ
Viel Spaß damit

[Holomino]

Layout:
Der gefüllte Kreis mit Richtungszeiger in der HTML-Seite sah eh schon fast wie ein Pacman aus. Dann können wir die Sauerei mithilfe einer einfachen farblichen Konvertierung im TileConverter auch komplett machen.

Wifi reconnect:
Damit der Roboter sich nach Verbindungsverlust wiederverbindet.

Putzen einiger Magic-Numbers im Code:
Eine Aufgabe eher allgemeiner Natur.

Speicherauslastung/-fragmentierung im Scan-Report:
min Heap, max Heap block size, used PSRAM. Das gehört zum Langzeittest dazu (seit der letzten Version kann die Simulation auch mal eigenständig hundert virtuelle Kilometer abspulen).


Alles in allem also eher Konsolidierungsmaßnahmen, konzeptionell nichts Neues. Im Augenblick strecke ich die Fühler etwas in Richtung OpenSlam.org aus: zumindest sehen TinySlam und GridSlam oberflächlich so aus, als würden sie sich irgendwie zwischen Ausgabe, Simulator, Grid und Pathfinder auf den ESP quetschen lassen.

Wie immer:
Die V 0.10 liegt auf https://c.gmx.net/@31902611639422705...SkGUZEDN7AmUzQ

[Holomino]

tinySlam



Im Groben sind Aufbau und Struktur des tinySLAM identisch zum Bisherigen. Es wird über die Variation der Pose getestet und anschließend das beste Ergebnis in die Karte eingetragen. Augenscheinliche Unterschiede:

• Der Datentyp der Kartenfelder muss erweitert werden auf uint16_t
• Belegte und freie Felder unterscheiden sich in der Richtung (bisher wurden freie Felder dekrementiert, jetzt werden sie inkrementiert)
• Der Test erfolgt nur noch über den Vergleich der Endpunkte (schneller)
• Das Eintragen der Hindernisse erfolgt über einen Fangbereich (hole_width), wobei die Wertigkeit der betroffenen Felder per Tiefpass gesetzt wird.

Weniger augenscheinliche Unterschiede (Macken):
Messungen im spitzen Winkel (nah vor der Wand oder bei der Fahrt um eine Innenecke herum) reißen bisher geschlossene Umrissstrukturen wieder auf. Das kann man aber filtern. Aus Slam.cpp:

//Suppress small angle entries (rays disturbing walls)
    if (x<= 3*dxc/4 && oldVal <0x8000)
      return;

Wo einerseits hinzugefügt, aber offensichtlich auch abgetragen wird, muss mit einem Langzeitdrift gerechnet werden. D.h. die Karte läuft langsam unter den Absolutkoordinaten des Routenplaners weg.

Dagegen hilft eigentlich nur das Sperren der Lidarkarte.


Deutliche Vorteile gegenüber der bisherigen SLAM-Lösung beginnen sich abzuzeichnen, wenn man in sim.h die Reichweite des Lidars drastisch reduziert (z.B. #define SIM_LIDARRANGE 2500). Auch wenn die langfristige Lokalisierung nach Abschluss der Exploration noch nicht zuverlässig läuft (hier fehlt im Wesentlichen noch die Erweiterung der Variationsbereiche in Richtung der OutOfRange-Messungen), funktioniert die Exploration in der Regel so gut, dass man wirklich einmal über den Bau eines "Poor mans Lidar" nachdenken darf. Zumindest nominal kommt man hier schon recht nah an den realen Messbereich eines VL53L1X - frei nach Lou Ottens also: Wenn's kleiner, einfacher und günstiger ist, könnte es was werden...

Der aktuelle Spielkram befindet sich als V0.10TS.zip unter: https://c.gmx.net/@31902611639422705...SkGUZEDN7AmUzQ

Weitere Infos über tinySLAM:
www.OpenSLAM.org
https://www.researchgate.net/profile...-of-C-code.pdf