Diese Seite wurde mithilfe maschineller Übersetzung übersetzt. Klicken Sie hier, um die neueste Version auf Englisch zu sehen.

Implementieren Sie Simultaneous Localization And Mapping (SLAM) mit Lidar-Scans

Live Script öffnen

Dieses Beispiel zeigt, wie der SLAM-Algorithmus (Simultaneous Localization And Mapping) mithilfe der Pose-Graph-Optimierung auf einer gesammelten Reihe von Lidar-Scans implementiert wird. Das Ziel dieses Beispiels besteht darin, mithilfe der Lidar-Scans eine Karte der Umgebung zu erstellen und die Flugbahn des Roboters abzurufen.

Um die Karte der Umgebung zu erstellen, verarbeitet der SLAM-Algorithmus die Lidar-Scans schrittweise und erstellt einen Pose-Graph, der diese Scans verknüpft. Der Roboter erkennt durch Scan-Matching einen zuvor besuchten Ort und kann entlang seiner Bewegungsbahn einen oder mehrere Loop-Closures herstellen. Der SLAM-Algorithmus nutzt die Informationen zum Schleifenschluss, um die Karte zu aktualisieren und die geschätzte Roboterbahn anzupassen.

Laserscandaten aus Datei laden

Laden Sie einen herunterskalierten Datensatz, der aus Laserscans besteht, die von einem mobilen Roboter in einer Innenumgebung erfasst wurden. Der durchschnittliche Abstand zwischen jeweils zwei Scans beträgt etwa 0,6 Meter.

Die Datei offlineSlamData.mat enthält die Variable scans, die alle in diesem Beispiel verwendeten Laserscans enthält.

load('offlineSlamData.mat');

Zur Veranschaulichung werden ein Grundriss und eine ungefähre Wegstrecke des Roboters bereitgestellt. Dieses Bild zeigt die kartierte relative Umgebung und die ungefähre Flugbahn des Roboters.

Führen Sie den SLAM-Algorithmus aus, erstellen Sie eine optimierte Karte und zeichnen Sie die Flugbahn des Roboters auf.

Erstellen Sie ein lidarSLAM-Objekt und legen Sie die Kartenauflösung und die maximale Lidar-Reichweite fest. In diesem Beispiel wird ein Jackal™-Roboter von Clearpath Robotics™ verwendet. Der Roboter ist mit einem SICK™ TiM-511-Laserscanner mit einer maximalen Reichweite von 10 Metern ausgestattet. Stellen Sie die maximale Lidar-Reichweite etwas kleiner ein als die maximale Scan-Reichweite (8m), da die Lasermessungen in der Nähe der maximalen Reichweite weniger genau sind. Stellen Sie die Rasterkartenauflösung auf 20 Zellen pro Meter ein, was eine Genauigkeit von 5 cm ergibt.

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);

Die folgenden Parameter für den Schleifenschluss werden empirisch festgelegt. Die Verwendung eines höheren Loop-Closure-Schwellenwerts hilft dabei, Fehlalarme im Loop-Closure-Identifizierungsprozess abzulehnen. Bedenken Sie jedoch, dass ein Spiel mit hoher Punktzahl dennoch ein schlechtes Spiel sein kann. Beispielsweise führen Scans, die in einer Umgebung mit ähnlichen oder sich wiederholenden Merkmalen durchgeführt werden, eher zu falsch positiven Ergebnissen. Durch die Verwendung eines höheren Suchradius für Schleifenschlüsse kann der Algorithmus einen größeren Kartenbereich rund um die aktuelle Pose-Schätzung nach Schleifenschlüssen durchsuchen.

slamAlg.LoopClosureThreshold = 210;  
slamAlg.LoopClosureSearchRadius = 8;

Beobachten Sie den Kartenerstellungsprozess mit den ersten 10 Scans

Fügen Sie dem slamAlg-Objekt schrittweise Scans hinzu. Scannummern werden gedruckt, wenn sie der Karte hinzugefügt werden. Das Objekt lehnt Scans ab, wenn der Abstand zwischen den Scans zu gering ist. Fügen Sie zuerst die ersten 10 Scans hinzu, um Ihren Algorithmus zu testen.

for i=1:10
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        fprintf('Added scan %d \n', i);
    end
end
Added scan 1 
Added scan 2 
Added scan 3 
Added scan 4 
Added scan 5 
Added scan 6 
Added scan 7 
Added scan 8 
Added scan 9 
Added scan 10

Rekonstruieren Sie die Szene, indem Sie die vom slamAlg verfolgten Scans und Posen aufzeichnen.

figure;
show(slamAlg);
title({'Map of the Environment','Pose Graph for Initial 10 Scans'});

Beobachten Sie die Auswirkungen von Schleifenschließungen und den Optimierungsprozess

Fügen Sie weiterhin in einer Schleife Scans hinzu. Schleifenschlüsse sollten während der Bewegung des Roboters automatisch erkannt werden. Die Pose-Graph-Optimierung wird immer dann durchgeführt, wenn ein Schleifenschluss identifiziert wird. Die Ausgabe optimizationInfo hat ein Feld, IsPerformed, das angibt, wann eine Pose-Graph-Optimierung erfolgt.

Plotten Sie die Scans und Posen, wenn ein Loop-Closure erkannt wird, und überprüfen Sie die Ergebnisse visuell. Dieses Diagramm zeigt überlagerte Scans und ein optimiertes Pose-Diagramm für den ersten Loop-Abschluss. Eine Schleifenverschlusskante wird als rote Verbindung hinzugefügt.

firstTimeLCDetected = false;

figure;
for i=10:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % visualize the first detected loop closure, if you want to see the
    % complete map building process, remove the if condition below
    if optimizationInfo.IsPerformed && ~firstTimeLCDetected
        show(slamAlg, 'Poses', 'off');
        hold on;
        show(slamAlg.PoseGraph); 
        hold off;
        firstTimeLCDetected = true;
        drawnow
    end
end
title('First loop closure');

Visualisieren Sie die erstellte Karte und Flugbahn des Roboters

Plotten Sie die endgültige Karte, nachdem alle Scans zum slamAlg-Objekt hinzugefügt wurden. Obwohl die vorherige for-Schleife nur den ersten Abschluss aufgezeichnet hat, wurden alle Scans hinzugefügt.

figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});

Visuelle Überprüfung des erstellten Plans im Vergleich zum ursprünglichen Grundriss

Ein Bild der Scans und des Pose-Graphs wird über den ursprünglichen Grundriss gelegt. Wie Sie sehen, entspricht die Karte nach dem Hinzufügen aller Scans und der Optimierung des Pose-Graphen gut dem ursprünglichen Grundriss.

Belegung des Gebäudes – Rasterkarte

Aus den optimierten Scans und Posen lässt sich ein occupancyMap generieren, welches die Umgebung als probabilistisches Belegungsraster darstellt.

[scans, optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);

Visualisieren Sie die Belegungsrasterkarte, die mit den Laserscans und dem optimierten Pose-Diagramm gefüllt ist.

figure; 
show(map);
hold on
show(slamAlg.PoseGraph, 'IDs', 'off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');

Siehe auch

| |

Themen