Leitfaden zur Schleifenschließung – Gleichzeitige Behebung der Drift eines SLAM-Systems entlang einer Schleife

Die Odometrie driftet mit jeder zurückgelegten Strecke etwas weiter, die Abweichung wächst stetig. Wenn ein Roboter einen Raum umrundet und zu seinem Ausgangspunkt zurückkehrt, kann ein System, das nur das letzte Bild betrachtet, den Fehler nicht erkennen, selbst wenn seine aktuelle Position auf der Karte um mehrere Zentimeter vom Ausgangspunkt abweicht. Die Schleifenkorrektur erkennt, dass die aktuelle Ansicht mit einer früheren Ansicht übereinstimmt, und nutzt diese Übereinstimmungsbedingung, um die Trajektorie entlang der gesamten Schleife wiederherzustellen.

0. Zusammenfassung (30 Sekunden)

• Die Schleifenkorrektur besteht aus zwei Schritten: dem Abrufen von Ortskandidaten und der geometrischen Bestätigung, ob es sich tatsächlich um denselben Ort handelt.

• Bag-of-Words, globale Bilddeskriptoren und lernbasierte Ortserkennung liefern schnell Kandidaten; Merkmalsabgleich und RANSAC entfernen anschließend falsch positive Ergebnisse.

• Die erkannte Bedingung wird einem Pose-Graphen hinzugefügt, und die SE(2)/SE(3)-Graphoptimierung korrigiert vergangene Posen kontinuierlich. Die lokale Karte bewahrt den Reprojektionsfehler.

Jahreszeiten, Beleuchtung, verschobene Möbel und ähnlich aussehende Korridore können falsche Schleifen erzeugen. Überprüfen Sie dies anhand mehrerer Kriterien: Zeitintervall, zurückgelegte Strecke, Anzahl der Inlier und Kartenkonsistenz.

Schleifenschließung ist kein Zaubertrick, der die Karte sofort korrigiert. Wird eine falsche Schleifenerkennung akzeptiert, verzerrt sich die gesamte Karte fehlerhaft. Daher sind konservative Schwellenwerte und ein reversibles Design unerlässlich.

1. Die Schleifenbedingung als Gleichung formulieren

Diagram 1 · Use the button to switch views

Abbildung 1 – Die Ortserkennung schlägt einen Kandidaten vor, die geometrische Verifizierung bestätigt eine Schleifenkante, und die robuste Pose-Graph-Optimierung verteilt die akkumulierte Drift über die Trajektorie. Ein roter Kandidat wird nicht bedingungslos akzeptiert.

Sei die Pose zum Zeitpunkt i gleich T_i\in SE(3) und Z_{ij} die relative Pose-Beobachtung zum Zeitpunkt j, wenn derselbe Ort erneut besucht wird. Das Residuum der Schleifenbedingung lässt sich wie folgt darstellen:

\mathrm{Log} ist die logarithmische Abbildung, die eine starre Transformation in eine 6-dimensionale infinitesimale Rotation und Translation umwandelt. Durch die Kombination von Odometriekanten \mathcal E_o mit Schleifenkanten \mathcal E_l ergibt sich für die Pose-Graph-Optimierung:

wobei \Omega_{ij} die Informationsmatrix und \rho ein robuster Verlust wie beispielsweise Huber ist. Das Hinzufügen auch nur einer einzigen Schleifenkante ermöglicht es, den über eine lange Trajektorie akkumulierten Fehler über den gesamten Graphen zu verteilen.

2. Kandidatenfindung: Umwandlung eines Bildes in einen „Wortschatz“

Bag-of-Words (BoW) quantisiert lokale Deskriptoren in ein Histogramm visueller Wörter und berechnet die Ähnlichkeit zu früheren Keyframes mithilfe der inversen Dokumentfrequenz (IDF). Systeme der ORB-SLAM-Familie kombinieren einen leichtgewichtigen ORB-Deskriptor mit einem Vokabularbaum und reduzieren die Kandidatenanzahl auf wenige, ohne einen Vergleich durchzuführen. Jedes Einzelbild wird mit der gesamten Karte verglichen.

Ein globaler Deskriptor komprimiert das gesamte Bild zu einem einzigen Vektor, um ähnliche Szenen zu finden. Lernbasierte Methoden wie NetVLAD, CosPlace und EigenPlaces lernen Repräsentationen, die robust gegenüber Beleuchtungs- und Blickwinkeländerungen sind. Ihre Leistungsfähigkeit kann jedoch in Gebäuden, Ackerland oder Fabriken, die sich von ihren Trainingsgebieten unterscheiden, nachlassen. Die Kandidatensuche ist für die Geschwindigkeit, die geometrische Verifizierung für die Genauigkeit zuständig – man sollte sich nicht allein auf eines von beiden verlassen.

3. Geometrische Verifizierung: Ähnlichkeit allein reicht nicht aus

Deskriptoren werden anhand des Kandidatenbildes erneut abgeglichen, um Korrespondenzen zu erhalten. Bei einer monokularen Kamera wird eine Essentiell-/Fundamentalmatrix mittels RANSAC geschätzt; bei bekannten Kartenpunkten wird PnP verwendet; bei planarer Textur wird eine Homographie verwendet. Die Überprüfung der Inlier-Anzahl, des Reprojektionsfehlers, der positiven Tiefeninformation und der physikalischen Plausibilität des Blickwinkelunterschieds erhöht die Wahrscheinlichkeit, dass es sich bei dem Kandidaten tatsächlich um denselben Ort handelt.

Eine falsche Schleife ist gefährlich, da die Graphoptimierung die Karte verzerren kann. Eine Form, die mit einer falschen Beobachtung übereinstimmt – auch wenn sie darauf basiert. Zwei ähnliche Korridore, eine Fensterreihe, Furchen auf einem Feld oder Regale in einer Fabrik lassen sich mit BoW allein nur schwer unterscheiden. Schließen Sie zeitlich zu nahe beieinander liegende Frames von den Kandidaten aus und fordern Sie konsistente Übereinstimmungen über mehrere aufeinanderfolgende Keyframes sowie die Übereinstimmung mit einem unabhängigen Sensor (IMU/LiDAR/GNSS).

4. Aktualisieren des Pose-Graphen und der Karte

Nach dem Hinzufügen einer Schleifenkante optimieren Sie zunächst nur die Keyframe-Posen mithilfe des Pose-Graphen und lassen die Kartenpunkte den Posen folgen. Bei einer großflächigen Karte erleichtert die Trennung der lokalen Karte von der globalen Pose – anstatt die Bündelausgleichung jedes Mal auf jeden Punkt anzuwenden – die Aufrechterhaltung der Echtzeitleistung. Sobald die Optimierung abgeschlossen ist, aktualisieren Sie die aktuelle map→odom-Transformation und gestalten Sie diese so, dass odom→base_link, das die Kontinuität priorisiert, niemals abrupte Sprünge erleidet.

In ROS 2 TF2-Begriffen veröffentlicht die lokale Odometrie odom→base_link und die globale SLAM-Korrektur passen map→odom an. Die Korrektur sollte zeitlich geglättet werden, damit der Roboterkörper beim Schließen der Schleife nicht teleportiert erscheint. Eine zu starke Glättung, um die Diskrepanz zwischen Karte und Realität zu verbergen, führt jedoch dazu, dass die Navigation weiterhin eine veraltete Position verwendet. Daher sollten die Korrekturstärke und die Anwendungsdauer protokolliert werden.

5. Fehleranfällige Umgebungen

Starke Lichtveränderungen | Verschiebung der Helligkeitsdeskriptoren | HDR/Belichtungskorrektur, gelernte Ortserkennung |

Scharfe Kurven/Unschärfe | Weniger Korrespondenzen | IMU-Vorhersage, Anpassung des Keyframe-Intervalls |

6. Checkliste für die Implementierung

1. Speichern Sie Bild, Zeitstempel, Pose und Merkmalsdeskriptoren jedes Keyframes.

2. Schließen Sie aktuelle Frames von den Kandidaten aus und rufen Sie einige ältere Kandidaten mithilfe von BoW-/globalen Deskriptoren ab.

3. Überprüfen Sie Korrespondenzen geometrisch, indem Sie die Anzahl der Inlier, den Reprojektionsfehler und die positive Tiefe als Schwellenwert festlegen.

4. Stellen Sie sicher, dass derselbe Kandidat über mehrere Frames zu verschiedenen Zeitpunkten hinweg gültig ist.

5. Fügen Sie die Schleifenkante dem Pose-Graphen hinzu und führen Sie eine robuste Optimierung durch.

6. Protokollieren Sie die Korrekturstärke, die Ablehnungsgründe und die Rechenzeit. Machen Sie falsche Schleifen umkehrbar.

7. Forschungsrichtungen

Aktuelle Forschung zielt darauf ab, die Bildsuche durch groß angelegtes selbstüberwachtes Lernen und die gemeinsame Nutzung von Deskriptoren robuster zu gestalten. LiDAR-, Kamera- und Ereigniskamera-Modalitäten sowie die saisonale Aktualisierung von Langzeitkarten werden eingesetzt. Es gibt auch Versuche, neue Szenendarstellungen wie NeRF und 3D Gaussian Splatting für die Ortserkennung zu nutzen. Allerdings müssen Inferenzgeschwindigkeit, Speicherbedarf, dynamische Objekte, Lizenzierung und Reproduzierbarkeit geprüft werden.

In Fabriken und auf landwirtschaftlichen Flächen, wo sich identisch geformte Gänge oder Furchen wiederholen, ist die alleinige Bilderkennung oft ungenau. Die Verwendung von Gerätewegen, IMU, Radodometrie, RTK-GNSS und kartierten Arbeitsgrenzen als Vorwissen zur Einschränkung des Suchbereichs reduziert Fehldetektionen. Bei der Erprobung eines Forschungsprototyps im Feld sollte von Anfang an ein Verfahren eingerichtet werden, das bei Erkennung einer falschen Schleife die Karte einfriert und eine manuelle Neuinitialisierung ermöglicht.

8. Fazit

Die Schleifenerkennung ist ein dreistufiger Prozess: Bildabruf zur Rekonstruktion einer früheren Szene, geometrische Verifizierung zum Nachweis der Identität des Ortes und Optimierung zur Verteilung des Fehlers über den gesamten Graphen. Schließen Sie Schleifen nicht allein anhand von BoW- oder gelernten Deskriptorwerten – überprüfen Sie Korrespondenzen, Sensorübereinstimmung und zeitliche Kontinuität. Korrekt angewendet, korrigiert der Schleifenschluss die Odometrieabweichung auf der gesamten Karte und gewährleistet die konsistente Selbstlokalisierung eines Roboters über längere Zeiträume.

Referenzen

• ORB-SLAM3
• DLoopDetector: Erscheinungsbildbasierte Schleifenerkennung
• Gálvez-López und Tardós, Bags of Binary Words )
• NetVLAD
• ROS 2 TF2-Dokumentation