Real-time loosely coupled GNSS and IMU integration via Factor Graph Optimization

Diese Arbeit stellt eine Echtzeit-Implementierung einer lose gekoppelten GNSS/IMU-Integration mittels Faktorgraphen-Optimierung vor, die in urbanen Umgebungen eine erhöhte Verfügbarkeit gegenüber Batch-Verfahren bietet, jedoch mit einem Kompromiss bei der Positionsgenauigkeit einhergeht.

Das Wesentliche

Hier ist eine einfache Erklärung der Forschung, als würden wir sie beim Kaffee besprechen, mit ein paar bildhaften Vergleichen.

Das große Problem: GPS in der "Stadt-Dschungel"

Stell dir vor, du fährst mit deinem Auto durch eine Stadt voller hoher Wolkenkratzer. Dein GPS-Empfänger ist wie ein Tourist, der versucht, den Weg zu finden, indem er nur auf die Sonne schaut. Aber die Gebäude verdecken die Sonne (die Satelliten), reflektieren das Licht (das nennt man "Multipath") oder lassen gar kein Licht durch.

In solchen "urbanen Canyons" verliert das GPS oft den Kontakt oder zeigt dir plötzlich an, du stehst mitten im Fluss, obwohl du auf der Straße bist. Das ist gefährlich, besonders für autonome Fahrzeuge.

Die Lösung: Ein Team aus zwei Experten

Um das zu lösen, nutzen Forscher zwei Systeme gleichzeitig:

1. GPS: Der Experte für den absoluten Standort (funktioniert super draußen, aber schlecht in der Stadt).
2. IMU (Trägheitsmessung): Das ist wie ein blindes Gedächtnis im Auto. Es misst jede Bewegung, Beschleunigung und Drehung extrem schnell. Wenn das GPS ausfällt, weiß das IMU noch eine Weile, wo das Auto war. Aber: Das IMU ist wie ein Betrunkener, der versucht, geradeaus zu laufen. Mit der Zeit "verrutscht" es immer mehr (Drift) und zeigt irgendwann eine völlig falsche Richtung an.

Die Kunst besteht darin, diese beiden zu einem Team zu machen, das sich gegenseitig korrigiert.

Der neue Ansatz: "Factor Graph Optimization" (FGO)

Früher haben Computer diese Daten oft mit einer Art "Einzugs-Filter" (Kalman-Filter) verarbeitet. Das ist wie ein Lehrer, der nur die letzte Antwort eines Schülers bewertet und vergisst, was vorher passiert ist.

Die Autoren dieses Papers nutzen eine neuere Methode namens Factor Graph Optimization (FGO). Stell dir das wie einen Detektiv vor, der einen riesigen Fall löst:

• Der Detektiv legt nicht nur das letzte Indiz auf den Tisch.
• Er legt alle Indizien (GPS-Daten, IMU-Bewegungen) in einer großen Kiste aus.
• Er schaut sich das ganze Bild an und fragt: "Welche Geschichte passt am besten zu allen Beweisen zusammen?"

Das Problem bei dieser "Detektiv-Methode" war bisher: Sie ist sehr rechenintensiv. Sie braucht viel Zeit und Geduld. Deshalb wurde sie oft nur als "Nachbearbeitung" benutzt (Post-Processing). Das heißt: Du fährst erst durch die Stadt, speicherst alles, und danach berechnet der Computer den perfekten Weg. Das ist für ein echtes, autonomes Auto, das sofort reagieren muss, zu langsam.

Der Durchbruch: RTFGO (Echtzeit-Detektiv)

Das Ziel dieses Papers war es, diesen Detektiv so zu trainieren, dass er in Echtzeit arbeitet, ohne den Fall zu schließen, bevor das Auto anhält.

Sie haben eine Methode namens RTFGO (Real-Time FGO) entwickelt. Hier sind die drei Tricks, die sie angewendet haben:

1. Der "Pausen-Trick" (IMU-Propagation): Wenn das GPS kurz ausfällt (z. B. unter einer Brücke), lässt das System das IMU allein weiterarbeiten, aber nur für eine kurze, begrenzte Zeit. Es ist wie ein Sprinter, der kurz ohne Wasser läuft, aber sofort stoppt, wenn er durstig wird, um nicht umzukippen.
2. Der "Rückblick-Trick" (Smoothing Latency): Normalerweise gibt ein Detektiv sofort sein Urteil ab. RTFGO wartet aber kurz (z. B. 1 Sekunde), um zu sehen, ob die nächsten Indizien das vorherige Urteil bestätigen oder korrigieren.
   • Vorteil: Genauere Ergebnisse.
   • Nachteil: Du musst warten.
3. Der "Aufräum-Trick" (Marginalization): Wenn der Detektiv zu viele alte Indizien auf dem Tisch hat, wird es unübersichtlich. RTFGO wirft die sehr alten Indizien (die, die schon lange bestätigt sind) in den Papierkorb, um Platz für neue zu machen.
   • Vorteil: Der Computer bleibt schnell.
   • Nachteil: Man kann alte Fehler nicht mehr so gut korrigieren.

Was haben sie herausgefunden? (Der Kompromiss)

Die Forscher haben das System in Hongkong getestet, in einer der schwierigsten Gegenden der Welt.

• Das Ergebnis: Ihr System funktioniert in Echtzeit! Es liefert Positionen, auch wenn das GPS kurz ausfällt.
• Der Preis: Es gibt immer einen Kompromiss (Trade-off).
  • Wenn du sofort eine Antwort willst (Echtzeit), ist die Genauigkeit etwas geringer, weil das System nicht warten kann, um alle Indizien zu prüfen.
  • Wenn du maximale Genauigkeit willst, musst du warten (wie beim Post-Processing), aber dann ist das Ergebnis fast perfekt.
  • Das Wichtigste: Ihr System bietet mehr Verfügbarkeit. Das bedeutet, das Auto verliert weniger oft den Kontakt zum System, selbst wenn die GPS-Signale schlecht sind.

Zusammenfassung in einem Satz

Die Autoren haben einen cleveren Algorithmus gebaut, der wie ein Detektiv arbeitet, der nicht nur auf das letzte Indiz schaut, sondern die ganze Geschichte rekonstruiert – und zwar so schnell, dass er in einem echten Auto mitfahren kann, auch wenn die GPS-Signale in der Stadt verrückt spielen. Sie haben gezeigt, dass man durch geschicktes Warten und Aufräumen die Balance zwischen Geschwindigkeit und Genauigkeit perfekt einstellen kann.

Technische Zusammenfassung

Hier ist eine detaillierte technische Zusammenfassung des Papers „Real-time loosely coupled GNSS and IMU integration via Factor Graph Optimization" auf Deutsch:

1. Problemstellung

Die präzise Positions-, Navigations- und Zeitbestimmung (PNT) ist für autonome Systeme und moderne Technologien unverzichtbar. Im Freien ist das Global Navigation Satellite System (GNSS) die primäre Quelle für die Lokalisierung. In städtischen Umgebungen („Urban Canyons") leidet GNSS jedoch unter erheblichen Einschränkungen wie Signalunterbrechungen, Mehrwegeeffekten (Multipath), Verlust der Sichtverbindung (Line-of-Sight, LOS) und Störungen.

Um diese Probleme zu mildern, wird GNSS häufig mit Inertial Measurement Units (IMU) fusioniert. Während IMUs kurzfristig präzise Bewegungen liefern, leiden sie langfristig unter Drift (kumulativer Fehler). Umgekehrt korrigiert GNSS die IMU-Drift, ist aber in Städten oft unzuverlässig.
Herausforderungen bestehen darin, diese Sensorfusion nicht nur offline (Post-Processing) durchzuführen, sondern Echtzeit-fähig zu gestalten. Herkömmliche Methoden wie der Erweiterte Kalman-Filter (EKF) sind zwar schnell, nutzen aber oft nicht alle zeitlichen Korrelationen optimal. Factor Graph Optimization (FGO) bietet zwar eine höhere Genauigkeit durch globale Optimierung, ist jedoch rechenintensiv und wird traditionell als Batch-Verfahren (nachträglich) eingesetzt. Die Frage ist, wie man FGO so modifiziert, dass es in Echtzeit in schwierigen Umgebungen mit hoher Verfügbarkeit und akzeptabler Genauigkeit läuft.

2. Methodik

Die Autoren schlagen eine loosely coupled (locker gekoppelte) Architektur vor, die GNSS- und IMU-Daten im Rahmen eines Factor Graph Optimization (FGO)-Frameworks integriert.

• Zustandsvektor: Jeder Knoten im Faktorgraphen repräsentiert den Navigationszustand $x_k$ zu einem Zeitpunkt $k$, bestehend aus Pose (Rotation), Position, Geschwindigkeit und den Bias-Werten von Beschleunigungsmesser und Gyroskop.
• Faktoren (Constraints):
  • Prior-Faktor: Verankert den Anfangszustand.
  • IMU-Präintegrations-Faktor: Verbindet aufeinanderfolgende Zustände basierend auf der integrierten IMU-Messung (Bewegung zwischen $k$ und $k+1$).
  • Bias-Random-Walk-Faktor: Modelliert die langsame Änderung der Sensoren-Bias.
  • GNSS-Faktor: Bindet die vom GNSS-Empfänger berechnete Position (PVT-Lösung) an den Zustand.
• Optimierungsziel: Minimierung der negativen Log-Posterior-Wahrscheinlichkeit über die gesamte Trajektorie, um den Maximum-A-Posteriori (MAP)-Schätzwert zu finden.

Der Kernbeitrag: RTFGO (Real-Time FGO)
Das Paper stellt eine Erweiterung des bestehenden Standard-FGO (SFGO) vor, das als Post-Processing-Verfahren arbeitet. RTFGO führt folgende Mechanismen ein, um Echtzeitfähigkeit zu erreichen:

1. IMU-only Propagation: Bei GNSS-Ausfällen (z. B. in Tunneln oder engen Gassen) wird die Schätzung nur basierend auf IMU-Daten fortgeführt, um die Dienstverfügbarkeit zu erhöhen. Dies wird durch einen zeitlichen Schwellenwert begrenzt, um die Drift zu kontrollieren.
2. Smoothing Latency ($\tau$): Um die Vorteile der FGO-Glättung zu nutzen, wird ein „Look-ahead"-Fenster eingeführt. Das System wartet auf zukünftige GNSS-Messungen, bevor es den aktuellen Zustand ausgibt. Ein $\tau=0$ bedeutet sofortige Ausgabe (Echtzeit), während ein hohes $\tau$ dem Batch-Verfahren entspricht.
3. Marginalization Lag: Um den Speicher- und Rechenbedarf konstant zu halten, werden alte Zustände (älter als ein festgelegter Lag) aus dem Graphen marginalisiert (entfernt) und durch einen kondensierten Prior ersetzt. Dies verhindert, dass der Graph unendlich wächst, führt aber zum Verlust von Informationen zur Bias-Korrektur.

3. Wichtige Beiträge

• Echtzeit-FGO-Architektur: Entwicklung und Implementierung eines RTFGO-Systems, das FGO von einem reinen Post-Processing-Tool in eine Echtzeit-Lösung überführt.
• Analyse von Trade-offs: Detaillierte Untersuchung der Zielkonflikte zwischen Genauigkeit, Dienstverfügbarkeit (Service Availability) und Rechenleistung durch Variation von Smoothing Latency und Marginalization Lag.
• Robustheit in urbanen Umgebungen: Demonstration der Funktionsfähigkeit in einem realen, dichten städtischen Umfeld (Hongkong) mit starken GNSS-Störungen.
• Open Source: Bereitstellung des Codes und der Konfigurationen für Reproduzierbarkeit.

4. Ergebnisse

Die Evaluation erfolgte auf dem UrbanNav-HK-MediumUrban-1 Datensatz (zwei Schleifen in Hongkong) auf einem MacBook Air M3.

• Genauigkeit (RMSE):
  • Das Batch-RTFGO (maximale Latenz, äquivalent zu SFGO) erreicht eine Genauigkeit, die mit dem State-of-the-Art (SFGO) vergleichbar ist und die GNSS-only-Lösung deutlich verbessert (z. B. Loop 2: GNSS-only 13,24 m vs. RTFGO Batch 9,33 m).
  • Die Echtzeit-Variante ($\tau=0$) zeigt eine leicht reduzierte Genauigkeit (Loop 2: 11,83 m), da zukünftige Messungen nicht zur Korrektur vergangener Zustände genutzt werden können.
• Dienstverfügbarkeit:
  • Die Echtzeit-Variante mit IMU-Propagation erhöht die Verfügbarkeit signifikant im Vergleich zu GNSS-only und reinen Batch-Methoden, da sie auch während GNSS-Ausfällen Positionen liefert.
  • Es wird ein Kompromiss sichtbar: Höhere Verfügbarkeit durch IMU-Propagation führt zu einer Zunahme des Drifts und damit zu einer geringeren Genauigkeit bei langen Ausfällen.
• Einfluss der Parameter:
  • Smoothing Latency: Eine höhere Latenz verbessert die Genauigkeit, solange die zukünftigen Messungen qualitativ hochwertig sind. Bei stark gestörten GNSS-Daten kann eine hohe Latenz die Genauigkeit jedoch verschlechtern, da inkonsistente Messungen in die Optimierung einfließen.
  • Marginalization Lag: Ein größerer Lag (längere Behaltensdauer alter Zustände) verbessert die Genauigkeit, erhöht aber die Rechenzeit. Ein Lag von 50 s führte zu einer mittleren Rechenzeit von 5 ms und einer 3D-RMSE von 12,6 m, was den Zielbereich für Echtzeitanwendungen trifft.

5. Bedeutung und Ausblick

Das Paper zeigt, dass Factor Graph Optimization nicht nur für hochpräzise Post-Processing-Anwendungen geeignet ist, sondern durch geschickte Anpassung (Latenzmanagement und Marginalisierung) auch in Echtzeit-Systemen für autonome Fahrzeuge in schwierigen städtischen Umgebungen eingesetzt werden kann.

Die Arbeit liefert ein quantitatives Verständnis der Kompromisse, die Ingenieure bei der Implementierung solcher Systeme eingehen müssen:

• Will man maximale Genauigkeit? -> Hohe Latenz, kein IMU-only (Risiko: Ausfall).
• Will man maximale Verfügbarkeit in Echtzeit? -> Geringe Latenz, IMU-Propagation (Risiko: Drift/Genauigkeitsverlust).

Zukünftige Arbeiten sollen die Methode auf eine tight-coupled (fest gekoppelte) Integration erweitern (Fusion von Rohdaten wie Pseudoranges statt PVT-Lösungen), was die Robustheit gegenüber Ausreißern und die Genauigkeit weiter steigern soll. Zudem wird die Integration von kontextuellen Faktoren (z. B. Null-Geschwindigkeits-Updates, Karten-Höhenpriorisierungen) geplant.