SLAM auf dem ESP32?

[Holomino]

Weiter geht es mit neuen Features.
Die V0.6 liegt hier (https://c.gmx.net/@31902611639422705...GUZEDN7AmUzQ):
Wenn auf der verlinkten Seite etwas von fehlender Freigabe steht, einmal über F5 im Browser aktualisieren.

Als Update einfach entpacken, mit Arduino öffnen, PWD und SSID aus Common.h anpassen und das Spiffs-Laufwerk aus dem data-Verzeichnis aktualisieren. Ggf. noch die serielle Schnittstelle anpassen (sonst kommen keine Daten).

Die Eroded-map
==============



Der im Fall A) gezeigte Pfad geht zwar nach den Informationen der bisher erstellten Karte über freie Felder, trotzdem würde der dem Pfad folgende Roboter mit dem Hinderniss kollidieren (er ist selber ein Körper).
Entweder berechnet man bei der Pfadsuche also die Abmessungen des Roboters ein, oder, wie in Fall B) gezeigt, man vergrößert die Hindernisse entsprechend: man zeichnet jeden Hindernispunkt der Lidar-map als Klecks mit dem Radius des Roboters in die Eroded-map.
Das funktioniert nach folgenden Spielregeln:
Aus dem Slam-Zyklus heraus werden die Felder der Lidar-map über Grid->Set(x,y,value) letztendlich in Tile->Set(x,y,value) manipuliert.

Verpassen wir Tile->Set einen Rückgabewert, der angibt, in welche Richtung sich der Belegtwert des Feldes (>0 oder <=0) ändert...

int8_t Tile::Set(int x, int y, int8_t val)
{
  int8_t rval =0;
  int offset = y*_width+x;
  if(Data[offset]<=0 && val > 0)
    rval= 5;
  if(Data[offset]>0 && val <= 0)
    rval= -5;

  Data[offset] = val;  
  IsDirty= true;
  IsTested= false;
  return rval;
}

... können wir in Grid->Set mithilfe einer vordefinierten Maske in der Eroded-map den nötigen Tintenklecks einfügen.

//from common.h
// 2 fields additional mask:
// XXX
//XXXXX
//XXOXX
//XXXXX
// XXX
int RobotMask[][2]={        {-1, 2},{0, 2},{1, 2},
                    {-2, 1},{-1, 1},{0, 1},{1, 1},{2, 1},
                    {-2, 0},{-1, 0},{0, 0},{1, 0},{2, 0},
                    {-2, -1},{-1, -1},{0, -1},{1, -1},{2, -1},
                            {-1, -2},{0, -2},{1, -2}
                   };



void Grid::Set(int x, int y, int8_t val)
{
  Tile* tile = GetTile(x,y, true);
  if (tile == nullptr) 
    return;
    
  int8_t e = tile->Set((x-xMin)%SLAM_TILESIZE,(y-yMin)%SLAM_TILESIZE, val);
  if (e !=0)
  {
    int ex, ey;
    for (int i=0;i <sizeof(RobotMask)/8;i++)
    {
      ex= x+RobotMask[i][0];
      ey= y+RobotMask[i][1];
            
      tile = GetTile(ex,ey, true);
      if (tile != nullptr) 
        tile->AddToEroded((ex-xMin)%SLAM_TILESIZE,(ey-yMin)%SLAM_TILESIZE, e);
    }
  }
}

Das Resultat: Dickere Wände, freie Felder sind 0, besetzte Felder haben einen positiven Wert.
Negative Werte werden nicht eingenommen.

Wissenswert vielleicht noch: Die Umstellung der Ansicht Lidar-map/ Eroded-map erfolgt über
#define DRAWING_SHOW_ERODED
(wie immer in Common.h)

Der Pathfinder
==============
In der jetzigen Testimplementierung sucht der Pathfinder den Weg von der aktuellen Roboterposition zum Zielpunkt (0;0). Den Zielpunkt kann man im Frontend durch Klicken auf die Map umsetzen. In der Debug-Sektion gibt der "Pathfinder-Report" weitere Infos.

Zum Algorithmus: Nachdem wir mit der Eroded-map quasi das Speicheraufkommen für die lapidare Aufgabe der Kollisionsvermeidung um Hindernisse herum verdoppelt haben, wurde es Zeit, zumindest beim Pathfinder nach einer der Hardware angemessenen Lösung zu suchen.



Das Ergebnis im Bild: Links der aktuell implementierte Algorithmus, im Vergleich rechts ein A* auf dem PC. Das sieht auf den ersten Blick nicht optimal aus.
Auf den zweiten Blick allerdings zeigt auch der A* keinen perfekten Weg. Knoten sind nun mal auf einem Array Nachbarfelder. Die können nur horizontal, vertikal oder diagonal sein. Entsprechend ermittelt auch der A* nicht die über die blaue Linie angedeutete Abkürzung.

Beim A* bietet es sich an, während der Fahrt über den Pfad (Pathfollower) diese Abkürzungen zu suchen, indem man mithilfe des bereits im Slam verwendeten Bresenham die vor sich liegenden Felder abtastet. Wenn man dadurch ein´weiter entferntes Feld hinter der nöchsten Kurve geradlinig erreichen kann, dann kann man die Kurve also schneiden.

Und ja, ich bin der Meinung, das kann man auch mit dem Ergebnis des ESP-Patfinders. Mit ein bisschen Spielen wird also während der Fahrt nicht viel Anderes bei rumkommen.

Die Handbremse lösen
===================
Wer noch einmal die alte Version 0.5 hervorholt und hier in der loop() in ESP32Slam.ino ein vTaskDelay(100) einfügt, wird bemerken, dass sich die Zykluszeiten des Slam auf dem ESP32-S2 um ca. 40% verbessern.
Ich denke mal, dass auch die loop() eine RTOS-Task mit Priority>0 ist und der leere Rumpf alleine die anderen Tasks gewaltig ausbremst. Anders kann ich es mir nicht erklären.

[Holomino]

Der Simulator ist da
Als Vorbereitung für den Pathfollower oder einfach zum Spielen.
Ein Klick auf die Karte in der HTML-Seite löst beim simulierten Roboter ein primitives Bewegungsmuster (drehen in Richtung/ Vorwärtsfahrt bis zum Zielpunkt) aus. Kollidiert der Roboter dabei mit einem Hindernis, hält er stumpf an.

Sowohl die Bewegung als auch die kontinuierlichen Lidarmessungen werden mit Fehlern versehen, so dass der Slam etwas zu tun hat (sieht man nach kurzer Fahrt recht gut, wenn man den Slam über SLAM_ANGLEDEVIATION/SLAM_DISTANCEDEVIATION deaktiviert).

Die simulierte Karte ist in sim.h als Array mit 32x32 Feldern definiert. Eine 1 als Element definiert ein Hindernis, eine 0 ist frei befahrbar. Entsprechend der angegebenen Feldgröße 400 (SIM_FIELDSIZE) hätte die Karte eine Abmessung von 12800x12880 Einheiten.
Mit der Einheit "mm" und der Rastereinstellung 100 im Slam (SLAM_RASTER) wäre die Simulationsumgebung von 12,8x12,8m also in 10cm-Kästchen gerastert.
Ebenso in sim.h befinden sich die anderen zugehörigen Parameter, so dass man mit unterschiedlichen Sensor- (Reichweite, Messungen/Umdrehung, Genauigkeit) und Fahrsettings (Robotergröße, Odometriegenauigkeit, Geschwindigkeit) spielen kann.


Stabilere Ausgabe
Ich Suche Fehler eigentlich immer eher bei mir, es hat also etwas gedauert (nach Betrachtung der Quellen zusammen mit meinem personal Informatiker waren uns beiden die Quellen nicht so recht geheuer), bis ich diesen Link gesucht/gefunden habe:
https://github.com/me-no-dev/ESPAsyn...ver/issues/900

Bedauerlich: Es gibt etliche begeisterte Netzbeiträge über den AsyncWebserver und mindestens genauso viele Rückfragen bezüglich Instabilität. Die Lösung liegt (versteckt den Dornröschenschlaf fristend) gleich nebenan:
https://github.com/yubox-node-org/ESPAsyncWebServer.
Warum verlinkt da keiner drauf?

Lässt sich jedenfalls, genau wie das Original, als Zip-Lib in Arduino installieren (vorher das Original einfach von der Platte löschen)
Für diese Anwendung ist der Wert 4 in der Konstante WS_MAX_QUEUED_MESSAGES ein guter Kompromiss.

Das neue Board-Package 2.0.2
...funktioniert bei mir nicht. Soweit ich nach kurzer Testinstallation (mal so nebenbei, sollte ja nur 10 min dauern und hat dann doch wieder 2h Stirnrunzeln gekostet, grummel) gesehen habe, läuft UART1 auf dem S2 nicht mehr. Ob's nun am Programm bei mir liegt oder am Package? Keine Ahnung - ich muss es noch testen. Also:
Zur Zeit ist das gültige Board-Package für dieses Projekt noch die Version 2.0.1.



Die V0.7 liegt hier: (https://c.gmx.net/@31902611639422705...SkGUZEDN7AmUzQ). Wie gehabt: Wenn "diese Freigabe leider ungültig ist", einmal über F5 aktualisieren, dann klappt das schon.
Der Simulator sendet seine Daten, wie bisher schon zelebriert, weiterhin über die UART. Also auch das Patchkabel zum Brücken von RxD/TxD stecken lassen.

[Holomino]

Der Pathfollower

Ein Linksklick auf die Map lässt den Roboter jetzt optimiert über den Pathfollower den Pfad zum Zielpunkt folgen.
Rechtsklick ist die aus der letzten Version bereits bekannte Fahrt per Luftlinie (da klatscht der Roboter halt vor die nächste Wand).

Netter Nebeneffekt für Indoor: Wenn man auf einen Punkt außerhalb der Umrisse klickt, sucht sich der Roboter iterativ den Pfad und exploriert so nebenbei die gesamte Umgebung:





Die V0.8 liegt hier (https://c.gmx.net/@31902611639422705...SkGUZEDN7AmUzQ). Wie gehabt: Wenn "diese Freigabe leider ungültig ist", einmal über F5 aktualisieren, dann klappt das schon.

Um Links-/Rechtsklick nutzen zu können, muss auch wieder das SPIFFS-Verzeichnis aktualisiert werden.

[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