SLAM auf dem ESP32?

[Moppi]

Reicht denn die Rechenleistung für sinnvolle Nutzung aus?

[Holomino]

Das ist nicht ganz einfach zu beantworten. Ich versuche es mal:
Die simulierte Umgebung hat eine Grundfläche von ca. 3000*2000 Einheiten (ob Inch, cm oder mm ist nicht festgelegt). Die Auflösung der Karte wird durch SLAM_RASTER (hier 25) bestimmt. Ein Feld auf der Karte ist also 25x25 Einheiten groß.

Wäre die simulierte Umgebung z.B. eine reale Wohnung mit der Grundfläche 12x8m, würde sich als Einheit 4mm ergeben. Damit hättest Du eine Feldauflösung von 10x10cm. Wenn Dir diese Auflösung zum Navigieren reicht, kannst Du mit einer Bearbeitungszeit von 500ms/Scan bei 120 Messungen/Scan für den SLAM rechnen.

Pragmatischer Ansatz, wenn Du noch keinen ESP32 zum Testen hast - ich stelle es gerade für Dich um und lasse es laufen:

Bei SLAM_RASTER = 12,5 (bezogen auf obiges Beispiel also 5x5cm für ein Feld auf der Karte) zeigt mir meine Messung Durchlaufzeiten von bis zu 850ms.

Bei SLAM_RASTER = 50 (bezogen auf obiges Beispiel also 20x20cm für ein Feld auf der Karte) zeigt mir meine Messung Durchlaufzeiten von etwa 420ms.

(Man merkt schon: Die Anzahl der Fließkommaoperationen sinkt durch die Änderung der Auflösung nicht. Die ergibt sich aus "Anzahl Variationen * Messungen pro Scan". Was sich proportional zur Auflösung ändert, ist die Anzahl der Ganzzahloperationen (der Bresenham-Line Algorithmus), die gehen aber auch bei 240MHz recht fix.)

Also grob geschätzt: Typische Wohnung = 500ms Zykluszeit für den SLAM - es fehlen noch Pathfinder, Pathfollower und die intellektuelle Aufgabe, die Karte in Bahnen für einen Staubsauger zu unterteilen oder zeitgesteuert Überwaschungsrouten abzufahren. Das wären sinnvolle Anwendungen, bei denen der Aktualisierungszyklus irgendwo im Sekundenbereich liegen muss

Nicht sinnvoll wäre es, eine Katze damit spielen zu lassen.

[Moppi]

Bresenham ist bei mir Ewigkeiten her, habe ich ganz kurz in den 90gern mal was mit gemacht. Nur zur Verwendung mit Grafikkarte. Habe das dann nie wieder benötigt.

500ms scheint mir schon recht viel. Die 1s pro Zyklus, sind auch nur relativ zur Geschwindigkeit zu sehen oder nicht? Wie dem auch sei, habe ich nur eine begrenzte Vorstellung von dem, was da so im Programm passieren muss, weil ich mich noch nie damit beschäftigt habe. Frage ist, ob ein Roboter bei normaler Geschwindigkeit mit der Rechenleistung auskäme. Oder ob das wirklich nur Sinn macht, wenn der dann entsprechend langsam fährt.

Trotzdem, erstmal schöne Sache!

Gruß
Moppi

[Holomino]

500ms scheint mir schon recht viel. Die 1s pro Zyklus, sind auch nur relativ zur Geschwindigkeit zu sehen oder nicht?

Ich hatte in der Simulation die Robotergeschwindigkeit auf 1Einheit/ Beam (ein Beam=eine Abstandsmessung des Lidars) gesetzt. Mit 120 Beams/Scan sind das also 120 Einheiten, die das Teil sich max. pro Scan fortbewegt. Auf obiges Beispiel mit der Wohnung bezogen (4mm/Einheit) wären das bei Fullspeed (der Roboter rennt wirklich geradeaus) knapp 50cm/Scan.

Auch zeigt mir der "Scan integration report" nach Durchlauf der Simulationsdatei 235 ermittelte Scans an. Bei 1s/Scan hätten die beiden Rundläufe durch die Wohnung etwa 4min gedauert.

Sag Du mir, ob das langsam ist?


Nachtrag:
Ich habe mir gerade einmal eine Wegmessung eingebaut...
In Slam.h:

class Slam
  {
  public:
    float Way=0;

...und am Ende der Funktion Slam::IntegrateScan() unter dem Kommentar "// Report to client" die Sache etwas aufgebohrt.

  // Report to client
  Scans++; 
  Way+=max.Distance();
  unsigned long end = millis();
  
  snprintf(Report, sizeof(Report),"{\"MsgID\":3, \"Text\":\"Nr %d, %d Beams | Pose X:%.1f, Y:%.1f, O:%.2f, Way:%.2f, Weight:%.2f| Time:%d ms, Idle:%d ms, Heap: %d bytes\"}\0", Scans, scanLen, PsObj->X, PsObj->Y, PsObj->Orientation, Way, maxWeight, end-start, start-lastEnd, ESP.getFreeHeap());
  lastEnd= end;
  PoseUpdated = true;
  ReportUpdated = true;

Ergebnis der Sache: Am Ende der Simulation ist der Way-Zähler auf etwa 24000 hochgelaufen. Das entspräche dann etwa 100m Netto-Fahrstrecke.

[Moppi]

Ich kenne das Konzept nicht, auch keinen Ablaufplan. Daher weiß ich nicht, wie schnell was ist, bzw. ob es ausreichend schnell ist. Ausreichend schnell, für Dein Konzept. Das interessierte mich, deswegen fragte ich.

Bei Bewegungsabläufen, bei denen auch Reaktionen auf Ereignisse stattfinden, spielen Latenzen immer eine nicht unerhebliche Rolle, auf Systemen, die eine überschaubar geringe Leistung haben. Für drinnen ist meine Schätzung maximal 50cm pro Sekunde. Steuert die Umgebungsauswertung direkt die Fahrt, bedeutet das, dass in einer halben Sekunde 25cm zurückgelegt sind, in denen womöglich nichts passiert. Wenn die Fahrt aber generell anders gesteuert wird, mit anderen Sensoren, und die Umgebungserfassung nur zur groben Richtungsbestimmung dient (im Grunde so, wie bei Autofahren durch einen Menschen, der nebenbei ein Navi benutzt), spielt da eine Verzögerung im Sekundenbereich keine große Rolle. Zur Not hält so ein Roboter dann an und wartet, bis ein Ergebnis vorliegt, das er zum Weiterfahren benötigt. Wenn die Umgebungserkennung und Kartierung eingesetzt wird, um überhaupt nur einen Grundriss zu erstellen, mit Hindernissen, und ein Reinungsroboter (bspw.) würde dann aber nur aufgrund der fertigen Karte im Speicher fahren (ohne auf Echtzeitscans angewiesen zu sein), ist das auch wieder eine andere Sache.

Du hast so eine Kartierung ja schon ein paar mal gemacht / umgesetzt. Ich dachte, wegen dem Thementitel, dass auch eine Einschätzung am Ende steht, wie man so einen SLAM-Algorythmus auf -- jetzt -- einem ESP sinnvoll einsetzt. Was geht, was nicht, in Bezug auf Steuerungskonzepte (oder auch nur Dein Steuerungskonzept).

Lieben Gruß
Moppi

[Holomino]

Du hast so eine Kartierung ja schon ein paar mal gemacht / umgesetzt. Ich dachte, wegen dem Thementitel, dass auch eine Einschätzung am Ende steht, wie man so einen SLAM-Algorythmus auf -- jetzt -- einem ESP sinnvoll einsetzt. Was geht, was nicht, in Bezug auf Steuerungskonzepte (oder auch nur Dein Steuerungskonzept).

Mal ganz allgemein: SLAM ist auch nur eine Teillösung. Ohne Pathfinder und Pathfollower ist es ein Gimmik (schön anzusehen). Diesbezüglich bin ich auch nicht "am Ende" angelangt. Ich muss es allerdings auch erst noch schreiben. Bislang ist es nur eine Preview.
Nach 25 Jahren C++-Abstinenz und ohne wesentliche Vorkenntnisse über den ESP32, RTOS, Arduino, WebServer oder HTML finde ich allerdings schon, dass es vorangeht (aufgrund des Verbreitungsgrades kann man sich viele Dinge aus dem Netz ziehen).

Bezüglich "Steuerungskonzept": Ich gehe jetzt nicht davon aus, dass die laufende Umrechnung von Radencoderwerten in eine Pose oder das Anschließen oder verdrahten von Motortreibern zum jetzigen Zeitpunkt projektrelevant sind. Dazu verwende ich eben die Simulation.
Den einen Teil der Hardwareschnittstelle (das Lidartelegramm) habe ich bereits vorgestellt.
Der Pathfinder hat keine Schnittstelle zur Hardware (nur zur UI) und der Pathfollower wird auch nur zyklisch eine aus der subjektiven Pose abgeleitete Koordinate ausgeben, die der Roboter über eine primitive Regelung ansteuern kann (maximal kann man während der Fahrt durch weitere Hindernissensoren den Pathfollower unterbrechen und am Pathfinder eine Neuberechnung antriggern).

Kurz: Die beiden Telegramme kann ich über die serielle Schnittstelle lösen, die Gegenstelle (Simulation) ist eh da, dann kann ich mich auf's Wesentliche konzentrieren.

Was geht, was nicht.

Es werden einige komplexere Probleme (closed loop, kidnapped robot) wahrscheinlich nicht gehen. Allerdings weiß ich auch nicht, ob die bei z.B. Staubsaugerrobotern gelöst werden, oder ob diese Geräte nicht gerade deshalb die "Verbindung nach Hause" suchen, damit die Softies beim Hersteller die Sache heilen.

[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.