⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore ist ein Open-Source-ROS-2-Paket, das einen 23-Zustands-UKF einsetzt, um IMU-, Radencoder-, GPS- und Visual-SLAM-Daten robust zu einem hochfrequenten Odometrie-Stream zu fusionieren und dabei im Vergleich zum Standardpaket robot_localization über diverse NCLT-Datensatzsequenzen hinweg signifikant geringere Trajektorienfehler sowie eine höhere numerische Stabilität erreicht.

Ursprüngliche Autoren: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Veröffentlicht 2026-05-26

📖 6 Min. Lesezeit🧠 Tiefgang

CC BY 4.0

Ursprüngliche Autoren: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Originalarbeit lizenziert unter CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Dies ist eine KI-generierte Erklärung des untenstehenden Papers. Sie wurde nicht von den Autoren verfasst oder gebilligt. Für technische Genauigkeit konsultieren Sie das Originalpaper. Vollständigen Haftungsausschluss lesen

Stellen Sie sich vor, Sie versuchen, einen Roboter durch eine Stadt zu navigieren, sind jedoch blindfoldet, haben die Ohren verstopft und können nur erraten, wo Sie sich befinden, indem Sie spüren, wie sich die Räder drehen. Das ist im Wesentlichen die Herausforderung, der sich Roboter stellen müssen, wenn sie versuchen, ihren Standort zu ermitteln. Sie müssen Daten aus verschiedenen „Sinnen" kombinieren, um ein klares Bild davon zu erhalten, wo sie sich befinden.

Diese Arbeit stellt FusionCore vor, ein neues Software-Tool, das als das „Gehirn" dient, welches diese Sinne für Roboter im ROS-2-System kombiniert. Hier ist die Funktionsweise, erklärt durch einfache Analogien.

Das Problem: Das „verrauschte" Team

Roboter verfügen normalerweise über drei Hauptmethoden, um ihren Standort zu erraten:

Rad-Encoder (der Schrittzähler): Zählt, wie oft sich die Räder drehen. Er ist großartig für kurze Strecken, gerät jedoch in Verwirrung, wenn die Räder auf Eis oder Schlamm durchrutschen.
GPS (die Satellitenkarte): Sagt Ihnen genau, wo Sie sich auf der Erde befinden. Es ist jedoch langsam, verschwindet manchmal (wie beim Fahren unter einer Brücke hindurch) und kann gelegentlich einen „Fehler" liefern, der besagt, dass Sie sich in einer anderen Stadt befinden.
IMU (das Innenohr): Ein Sensor, der Beschleunigung und Drehung spürt. Er ist sehr schnell, beginnt aber mit der Zeit zu „driften" (wie ein Kreisel, der sich langsam neigt), wodurch der Roboter glaubt, er bewege sich, obwohl er stillsteht.

Bestehende Software (genannt robot_localization) versucht, diese Signale zu mischen, weist jedoch einige Mängel auf:

Sie behandelt das „Driften" in den Sensoren als festen Fehler und nicht als etwas, das sich im Laufe der Zeit verändert.
Sie gerät in Verwirrung, wenn GPS-Signale über Karten-Grenzen hinweg springen.
Sie hat Schwierigkeiten, zwischen einem echten GPS-Update und einem verrückten Fehler zu unterscheiden.

Die Lösung: FusionCores „intelligentes Gehirn"

FusionCore ist ein neues „Gehirn" (ein mathematischer Filter), das alle diese Signale zu einem einzigen, glatten, hochfrequenten Strom von Standortdaten verschmilzt (100-mal pro Sekunde). Es verwendet einen 23-dimensionalen „Zustand", um alles zu verfolgen, einschließlich einiger Dinge, die andere Systeme ignorieren.

Hier sind die wichtigsten Tricks, die FusionCore verwendet, erklärt mit Metaphern:

1. Der „Drift-Detektiv" (Online-Bias-Schätzung)

Stellen Sie sich vor, Sie gehen mit einem Freund, der leicht hinkt. Wenn Sie das Hinken nicht bemerken, wird Ihre Karte darüber, wo Sie sich befinden, langsam falsch.

Der alte Weg: Die Software geht davon aus, dass das Hinken feststeht und ignoriert es.
Der FusionCore-Weg: Sie hat einen „Drift-Detektiv" im Inneren. Sie beobachtet die Sensoren ständig und fragt: „Rutscht das Rad durch? Wird das Gyroskop müde?" Sie berechnet den genauen Betrag des „Hinkens" (Bias) in Echtzeit und zieht ihn ab. Dies verhindert, dass der Roboter verloren geht, selbst wenn das GPS für eine Weile ausfällt.

2. Die „globale Adresse" (ECEF-natives GPS)

GPS-Koordinaten werden normalerweise in lokale Karten umgewandelt (wie UTM-Zonen). Wenn Sie eine Zonengrenze überqueren, kann die Karte einen Fehler machen und abstürzen.

Der FusionCore-Weg: Anstatt lokale Straßenadressen zu verwenden, spricht sie die Sprache der gesamten Erde (ECEF). Sie behandelt den Planeten als eine einzige riesige Kugel, sodass sie niemals durch Karten-Grenzen verwirrt wird. Es ist wie die Verwendung von Breitengrad und Längengrad für die ganze Welt, anstatt alle 10 Meilen zu einer neuen Stadtkarte zu wechseln.

3. Der „Lügendetektor" (Adaptive Ausreißer-Unterdrückung)

Manchmal sendet GPS eine Nachricht mit dem Inhalt: „Ich bin jetzt im Ozean!", obwohl sich der Roboter eindeutig auf einer Straße befindet.

Der alte Weg: Die Software verwendet eine starre Regel: „Wenn der Fehler größer als 5 Meter ist, ignoriere ihn." Wenn das GPS jedoch nur etwas verrauscht ist, könnte sie gute Daten ignorieren.
Der FusionCore-Weg: Sie verwendet einen „Lügendetektor", der lernt. Sie betrachtet die Historie der Fehler. Wenn das GPS normalerweise genau ist, aber plötzlich einen schlechten Tag hat, lockert der Detektor seinen Griff. Wenn das GPS verrückt spielt, schlägt er die Tür zu. Sie passt ihre eigene Empfindlichkeit automatisch an, sodass sie nicht durch Fehler getäuscht wird oder gute Daten ablehnt.

4. Der „Zeitreisende" (Verzögerungskompensation)

GPS-Daten kommen oft eine Splitsekunde zu spät an, weil der Satellit Mathematik betreiben muss, bevor er das Signal sendet.

Der FusionCore-Weg: Sie hält einen „Speicherpuffer" (einen Ringpuffer) der letzten Sekunde an IMU-Daten vor. Wenn die verspätete GPS-Nachricht endlich eintrifft, wendet die Software sie nicht einfach auf den aktuellen Moment an. Sie „spult zurück" zum exakten Moment, zu dem das GPS-Signal aufgenommen wurde, wendet die Korrektur an und „spult dann wieder vor" bis jetzt. Dies stellt sicher, dass die Position des Roboters perfekt synchronisiert ist, selbst bei langsamen Sensoren.

5. Das „visuelle Gedächtnis" (VSLAM-Fusion)

Wenn der Roboter das GPS verliert (wie beim Fahren in einen Tunnel), kann er eine Kamera (Visual SLAM) verwenden, um Merkmale im Raum zu verfolgen.

Der FusionCore-Weg: Sie kann nahtlos auf die Sicht der Kamera umschalten. Wenn die Kamera den Weg verliert und ihre Karte neu starten muss, weiß FusionCore, wie sie die Position des Roboters ohne Panik oder Absturz wieder mit der Realität abgleichen kann.

Die Ergebnisse: Der große Test

Der Autor testete FusionCore gegen die Standard-Software (robot_localization) unter Verwendung von 12 langen Fahrsequenzen (jeweils bis zu 92 Minuten) aus einem öffentlichen Datensatz.

Der Gewinner: FusionCore gewann in 10 von 12 Rennen.
Die Marge: In einigen Fällen war sie 22-mal genauer als die Standard-Software.
Die Verlierer: Die fortgeschrittene Version der Standard-Software (UKF) stürzte bei allen 12 Sequenzen ab (divergierte), was bedeutet, dass sie den Standort des Roboters vollständig aus den Augen verlor.
Warum FusionCore 2 Rennen verlor:
1. Der lange Blackout: In einem Rennen war das GPS-Signal fast 8 Minuten lang weg. Selbst der beste Detektiv kann nicht perfekt raten, wenn so lange keine Karte vorhanden ist, und der Roboter driftete leicht ab.
2. Der „adversarische" Fehler: In einem anderen Rennen sendete das GPS eine Ansammlung von Lügen, die so überzeugend waren, dass sie den Filter täuschten. Die Standard-Software lehnte sie zufällig ab, weil ihre Regeln zu streng waren, während FusionCore versuchte, zu hilfreich zu sein und getäuscht wurde. Der Autor merkt an, dass dies spezifische, behebbare Probleme sind.

Zusammenfassung

FusionCore ist eine intelligentere, anpassungsfähigere Methode, um Robotern zu helfen, ihren Standort zu kennen. Sie behandelt Sensorfehler als lebendige Dinge, die sich im Laufe der Zeit verändern, lernt, Lügen zu ignorieren, und erinnert sich an die Vergangenheit, um die Gegenwart zu korrigieren. Obwohl sie nicht perfekt ist, übertrifft sie den aktuellen Industriestandard in den meisten realen Szenarien erheblich.

Technische Zusammenfassung: FusionCore

Problemstellung

Eine präzise Pose-Schätzung ist grundlegend für die autonome Roboternavigation, doch die praktische Implementierung von Sensor-Fusionssystemen bleibt herausfordernd. Bestehende Lösungen weisen mehrere kritische Einschränkungen auf:

Bias-Drift: Die Bias-Werte von IMU-Gyroskopen und Beschleunigungssensoren driftieren kontinuierlich, und Bias-Werte der Rad-Encoder-Geschwindigkeit (systematische Offsets bei Differentialantrieben) werden selten als Filterzustände behandelt, was zu einer nicht beobachtbaren Anhäufung von Kursfehlern während GPS-Ausfällen führt.
Architektonische Fragilität: Dominante ROS-Pakete wie robot_localization verlassen sich auf separate Knoten für die Umwandlung von GPS-Koordinaten (UTM), was zu Ausfällen an Zonengrenzen führt. Sie verwenden zudem feste Rausch-Kovarianzen und skalare Schwellenwerte zur Ausreißerabweisung, die die Messdimensionalität nicht berücksichtigen.
Operative Lücken: Aktuelle Systeme haben Schwierigkeiten mit dem Betrieb ohne GPS, der Wiederherstellung nach einer Neuinitialisierung der Karte und der Integration verschiedener Sensoren wie Visual SLAM (VSLAM) und Radar ohne komplexe Koordination.

Methodik

FusionCore ist ein Open-Source-ROS-2-Paket, das einen 23-Zustands Unscented Kalman Filter (UKF) implementiert, der IMU-, Rad-Encoder-Odometrie-, GPS- und Visual-SLAM-Posedaten zu einem einzigen 100-Hz-Odometrie-Strom fusioniert.

Kernarchitektur

Zustandsvektor: Der Filter verwaltet einen 23-dimensionalen Zustandsvektor, der Position ( $p$ ), Orientierung ( $q$ ), lineare/rotationale Geschwindigkeit ( $v, \omega$ ), lineare Beschleunigung ( $a$ ), Gyroskop-Bias ( $b_g$ ), Beschleunigungssensor-Bias ( $b_a$ ) und einen kritischen Rad-Encoder-Geschwindigkeits-Bias ( $b_{ewz}$ ) umfasst.
Bias-Schätzung: Im Gegensatz zu früheren Ansätzen sind Gyroskop-, Beschleunigungssensor- und Encoder-Kurs-Bias-Werte erstklassige Filterzustände mit eigenem Prozessrauschen. Dies ermöglicht dem Filter, sensor-spezifische Bias-Werte online zu konvergieren, ohne eine separate Kalibrierungsphase.
GPS-Fusion: Das System fusioniert GPS-Daten nativ im Erd-zentrierten, erd-fixierten (ECEF) Koordinatensystem und wandelt nur für den Filterzustand in ENU um. Dies eliminiert die Notwendigkeit eines separaten navsat_transform-Knotens und vermeidet Ausfälle an UTM-Zonengrenzen.
Messmodelle:
- IMU: Verarbeitet 6-DOF-Rohdaten und 2/3-DOF-Orientierungsupdates unter Berücksichtigung von Schwerkraft und Bias.
- Rad-Encoder: Subtrahiert den geschätzten $b_{ewz}$ von der gemeldeten Winkelgeschwindigkeit vor der Integration. Erzwingt nicht-holonome Zwangsbedingungen (keine vertikale Geschwindigkeit und Beschleunigung) mit adaptivem Rauschen.
- VSLAM: Akzeptiert 6-DOF-Pose-Messungen von jedem visuellen Odometrie-/SLAM-System, einschließlich der automatischen Wiederherstellung nach einer Neuinitialisierung der Karte durch erneutes Verankern der Transformation nach aufeinanderfolgenden Ausreißerabweisungen.
- Geschwindigkeit: Unterstützt GPS-Doppler-Geschwindigkeit und Radar-Doppler-Eigengeschwindigkeit zur Schlupferkennung.

Innovationen im Filterdesign

Chi-Quadrat-Gating pro Sensor: Implementiert eine Mahalanobis-Distanz-basierte Ausreißerabweisung mit Schwellenwerten, die spezifisch für die Freiheitsgrade (DOF) jedes Sensorpfads kalibriert sind (z. B. 3-DOF-GPS vs. 1-DOF-Kurs), anstatt einen einzelnen skalaren Schwellenwert zu verwenden.
Adaptive Rausch-Kovarianz: Passt die Sensor-Rausch-Kovarianz ( $R$ ) in Echtzeit automatisch unter Verwendung eines gleitenden Fensters von Innovationssequenzen und eines exponentiellen gleitenden Durchschnitts an, wodurch eine manuelle Nachjustierung aufgrund von Umweltänderungen entfällt.
Verzögerungskompensation: Nutzt einen 1-Sekunden-IMU-Ringpuffer, um eine Retrodiktion durchzuführen, verzögerte GPS- oder VSLAM-Messungen auf den korrekten historischen Zustand anzuwenden und anschließend wieder vorwärts zu propagieren.
Zero-Velocity Updates (ZUPT): Erkennt stationäre Phasen, um die Bias-Drift zu unterdrücken.
Coast Mode: Während GPS-Ausfällen wird der geschätzte Encoder-Kurs-Bias von den Encoder-Daten subtrahiert, um die Kursdrift zu reduzieren.

Hauptbeiträge

Die Arbeit präsentiert zehn spezifische Beiträge:

Eine 23-Zustands-UKF-Formulierung mit kontinuierlicher Online-Schätzung von Gyroskop-, Beschleunigungssensor- und Rad-Encoder-Geschwindigkeits-Bias-Werten.
ECEF-native GPS-Fusion, die Abhängigkeiten von UTM-Projektionen beseitigt.
Pro-Sensor Mahalanobis-Chi-Quadrat-Ausreißer-Gating mit dimensionskalibrierten Schwellenwerten.
Automatische Anpassung der Rausch-Kovarianz aus Innovationssequenzen.
Automatische Zero-Velocity Updates (ZUPT) für stationäre Phasen.
IMU-Ringpuffer-basierte Retrodiktion zur Verzögerungskompensation.
Visual-SLAM-Pose-Fusion mit automatischer Wiederherstellung nach Neuinitialisierung der Karte.
GPS-Geschwindigkeitsfusion zur schlupfbasierten Kreuzsensor-Erkennung.
Radar-Doppler-Eigengeschwindigkeitsfusion für 4D-Imaging-Radar-Plattformen.
Eine quantitative Bewertung gegen robot_localization an zwölf vollständigen NCLT-Sequenzen.

Bewertung und Ergebnisse

Das System wurde an zwölf vollständigen Sequenzen (jeweils 55–92 Minuten) aus dem öffentlichen NCLT-Datensatz bewertet, die diverse saisonale Bedingungen abdecken. Die Bewertung verglich FusionCore mit dem robot_localization EKF und UKF unter Verwendung des Root-Mean-Square-Fehlers (RMSE) des absoluten Trajektorienfehlers (ATE).

Leistung: FusionCore erzielte einen niedrigeren ATE als der robot_localization EKF bei 10 von 12 Sequenzen. Die Verbesserungen bei den gewonnenen Sequenzen lagen zwischen 1,2× und 22,2×.
Stabilität: Der robot_localization UKF divergierte numerisch (NaN) bei allen zwölf Sequenzen innerhalb der ersten 30 Sekunden, was bekannte Instabilitätsprobleme bei hochfrequenten Eingaben bestätigt.
Fehleranalyse:
- 2012-06-15 (FC Loss): FusionCore unterlag dem robot_localization EKF (49,2 m vs. 18,2 m) aufgrund eines 7,7-minütigen GPS-Ausfalls. Die Arbeit führt dies auf die Grenzen der toten Rechnung auf Basis von Encodern über so lange Zeiträume ohne absoluten Kursbezug (z. B. Magnetometer) zurück.
- 2012-08-20 (FC Loss): FusionCore unterlag erheblich (98,3 m vs. 10,6 m) aufgrund eines „adversarialen GPS-Clusters" von 105 Fixes, die 720–840 m vom wahren Wert entfernt bei einer Ausfallgrenze auftraten. Die Arbeit stellt fest, dass das falsch kalibrierte Gating von robot_localization zufällig diese Ausreißer ablehnte, während das gelockerte Gating von FusionCore (notwendig für die Wiederherstellung) sie akzeptierte. Die Autoren identifizieren eine Vorabprüfung der Geschwindigkeitskonsistenz als Lösung.
Kovarianz-Mismatch: Die Arbeit hebt hervor, dass die Abhängigkeit von robot_localization von festen, oft ungenauen GPS-Kovarianzberichten zur Ablehnung valider Daten und zum Zusammenbruch in die tote Rechnung führt. Das adaptive Rauschmodell von FusionCore mildert dies, indem es das Rauschmodell aufbläht, wenn Innovationsfehler die berichteten Kovarianzen überschreiten.

Bedeutung und Behauptungen

Die Arbeit behauptet, dass FusionCore die praktischen Einschränkungen bestehender ROS-2-Sensor-Fusion-Stapel durch Bereitstellung eines einzelnen Lifecycle-Knotens adressiert, der keine externen Koordinatoren erfordert. Seine Bedeutung liegt in:

Robustheit: Erzielung überlegener Trajektorienpräzision in langfristigen, realen Szenarien, in denen bestehende UKF-Implementierungen numerisch versagen.
Anpassungsfähigkeit: Bewältigung dynamischer Sensorqualität und GPS-Kovarianz-Mismatches durch adaptives Rauschen und dimensionsbewusstes Gating.
Operative Kontinuität: Ermöglichung des Betriebs ohne GPS und nahtloser Wiederherstellung nach einer Neuinitialisierung von VSLAM-Karten.
Reproduzierbarkeit: Bereitstellung einer vollständig Open-Source-Implementierung mit Benchmark-Skripten und Konfigurationen für Standardplattformen (TurtleBot3, Husky) und Integration mit rtabmap_ros.

Die Autoren kommen zu dem Schluss, dass zwar spezifische Fehlermodi (lange Ausfälle, adversarische GPS-Cluster) existieren, diese jedoch diagnostizierbar sind und architektonische Lösungen (Integration von Magnetometern, Vorabprüfungen der Geschwindigkeit) für zukünftige Arbeiten identifiziert wurden.