Frei nach https://www.heise.de/ct/artikel/An-d...ks-290662.html ein ganz einfacher SLAM (ohne Lösung für Closed loop oder kidnapped robot-Problem) für Lidaranwendungen
Eingangsseitig emmitiert die Hardware bei einer Sensormessung Abstandswert, Messwinkel und die aus den Odometriedaten ermittelte Pose des Roboters (X/Y/Orientation). Messungen werden auf dem Rechner bis zur vollständigen Umdrehung der Sensorplattform gesammelt und als ScanPackage an den SLAM-Algorithmus versendet (Es kommen also "viele" Messungen als Rundumblick gleichzeitig an).
Aufgabe 1: Der erste Scan wird anhand der subjektiven Roboterpose eingetragen (siehe obiger Artikel). Die Pose der letzten Messung im ScanPackage wird gepuffert. Eine zweite Pose (AlgPose) wird auf die Werte der subjektiven Pose initialisiert.
Aufgabe 2: Neues ScanPackage, neue letzte subjektive Pose. Bilde ein PoseChange (dx/dy, Blickwinkeländerung)
Aufgabe 3: Nimm einen Zufallsgenerator und variiere die drei Bestandteile von PoseChange um z.B. +/-20%. Bei 100 Variationen bekommst Du 100 leicht unterschiedliche PoseChanges zurück.
Aufgabe 4: Schlage jede generierte Variation von PoseChange einmal auf AlgPose auf und teste, wie die Sensorwerte in die bisherige Map passen (Test gibt einen Gewichtungswert (Anzahl der passenden Punkte frei-frei und besetzt-besetzt/Anzahl aller Punkte des Scans) für die Variation zurück)
Aufgabe 5: Trage die Sensorwerte der Variation mit dem größten Gewicht in Deine Map ein und schlage den dazugehörigen PoseChange auf die AlgPose auf.
That's it
Ohne Studium machbar, vielleicht etwas unbequem die Transformationen der Posen und Linien, aber auch nur Mittelstufenmathe.
Neben dem Raster und der Anzahl der Variationen darf man sicher noch den Test/Update verfeinern (z.B. messdistanzabhängige Fehler in der Gewichtung berücksichtigen und so eine Varianz um den gemessenen Punkt einführen). Auch kann man vielleicht mal am Zufallsgenerator spielen.
Sicher eine Menge Holz: Das testen von beispielsweise 100 Linien mit sagen wir durchschnittlich 30 Rasterpunkten in vielleicht 100 Variationen durch den Bresenham sind halt 300000 Rasterpunkte und Vergleiche zur Bearbeitung eines Scan-Paketes. Bei einem RPLidar mit 5 U/s und 360 Messungen/U wird das noch enger. Aber man muss ja nicht alle Messwerte im SLAM berücksichtigen. Für einen schneckigen Staubsauger erst recht nicht.
Code:
public double Test(Pose pose, LidarScanData scan)
{
int mulSum = 0;
int pointSum = 0;
//Create absolute points from pose and scan data
List<Line> transformed = TransformScanToPoints(scan, pose);
//for each scan line
foreach (Line ln in transformed)
{
if (ln != null)
{
int x0 = (int)Math.Round(ln.P1.X / Raster);
int y0 = (int)Math.Round(ln.P1.Y / Raster);
int x1 = (int)Math.Round(ln.P2.X / Raster);
int y1 = (int)Math.Round(ln.P2.Y / Raster);
int dx = Math.Abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
int dy = Math.Abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
int err = (dx > dy ? dx : -dy) / 2, e2;
//rastering (Bresenham's line algorithm)
for (; ; )
{
int mul = (x0 == x1 && y0 == y1) ? this[x0, y0] : -this[x0, y0];
if (mul > 0)
mulSum++;
pointSum++;
if (x0 == x1 && y0 == y1)
break;
e2 = err;
if (e2 > -dx) { err -= dy; x0 += sx; }
if (e2 < dy) { err += dx; y0 += sy; }
}
}
}
return (double)mulSum / (double)pointSum;
}
public List<Line> Add(Pose pose, LidarScanData scan)
{
//Create absolute points from pose and scan data
List<Line> transformed = TransformScanToPoints(scan, pose);
//for each scan line
foreach (Line ln in transformed)
{
if (ln != null)
{
int x0 = (int)Math.Round(ln.P1.X / Raster);
int y0 = (int)Math.Round(ln.P1.Y / Raster);
int x1 = (int)Math.Round(ln.P2.X / Raster);
int y1 = (int)Math.Round(ln.P2.Y / Raster);
int dx = Math.Abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
int dy = Math.Abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
int err = (dx > dy ? dx : -dy) / 2, e2;
for (; ; )
{
//setPixel(x0, y0);
this[x0, y0] = (SByte)Math.Max((int)this[x0, y0] - 1, -128);
if (x0 == x1 && y0 == y1)
{
this[x0, y0] = (SByte)Math.Min((int)this[x0, y0] + 5, 127);
break;
}
e2 = err;
if (e2 > -dx) { err -= dy; x0 += sx; }
if (e2 < dy) { err += dx; y0 += sy; }
}
}
}
return transformed;
}
Zitieren
Zitat von Holomino
bei teensy 4.1
Lesezeichen