FPGA-Based Real-Time Sensor Fusion for Inertial Navigation Using IMU and GPS Data

Stellen Sie sich vor, Sie versuchen, sich in einem dichten, nebligen Wald ohne Karte zurechtzufinden. Sie haben zwei Werkzeuge, die Ihnen helfen sollen. Das erste ist ein superschnelles, supergenaues Schrittzähler-System mit einem Kreisel, der an Ihr Handgelenk geschnallt ist (ein IMU). Diese zeigen Ihnen exakt an, wie schnell Sie sich bewegen und in welche Richtung Sie sich drehen, aber sie sind etwas „driftig“. Wenn Sie sie zu lange benutzen, beginnen sie langsam zu lügen und lassen Sie glauben, dass Sie sich auf einer geraden Linie bewegen, während Sie in Wirklichkeit eigentlich spiralförmig wandern. Das zweite Werkzeug ist ein Satellitentelefon (GPS). Es sagt Ihnen exakt, wo Sie auf der Karte sind, aber es braucht lange, um eine Verbindung aufzubauen, und liefert nur gelegentlich einen Fixpunkt. Wenn Sie sich nur auf den Satelliten verlassen, könnten Sie eine plötzliche Abbiegung verpassen; wenn Sie sich nur auf den Schrittzähler verlassen, werden Sie irgendwann vom Weg abkommen.

Um dies zu lösen, verwenden Ingenieure einen cleveren mathematischen Trick namens „Sensor Fusion“-Algorithmus, speziell etwas, das als „Extended Kalman Filter“ (EKF) bekannt ist. Denken Sie bei diesem Filter an einen superintelligenten Schiedsrichter, der ständig sowohl dem schnellen Schrittzähler als sie langsamen Satelliten belauscht. Er rät, wo Sie sich befinden, prüft anhand des Satelliten, ob die Vermutung richtig war, und korrigiert dann den Drift des Schrittzählers. Das Problem ist, dass die Durchführung dieser Mathematik schnell genug zu leisten, um mit der hohen Geschwindigkeit des Schrittzählers Schritt zu halten, eine extrem schwere Aufgabe für einen Standard-Computerchip ist. Hier kommen FPGAs ins Spiel. Wenn ein normaler Computerchip ein einzelner Koch ist, der eine Mahlzeit Schritt für Schritt kocht, dann ist ein FPGA eine riesige Küche mit Hunderten von Köchen, die alle zur exakt gleichen Zeit perfekt synchron arbeiten. Dieser Artikel untersucht, wie man diese „Superküche“ baut, um den Navigations-Schiedsrichter in Echtzeit laufen zu lassen.

Die Mission des Papers: Ein super-schnelles Navigationsgehirn bauen

Dieses Paper von Abbas Fadavi präsentiert einen neuen Weg, um diese „Superküche“ für die Navigation zu bauen. Der Autor hat einen komplexen Navigationsalgorithmus, der normalerweise auf einem Computer läuft, erfolgreich in ein Design übersetzt, das direkt auf einem Xilinx Kintex-7 FPGA-Chip lebt. Das Ziel war es, den „Schiedsrichter“ (den EKF) schnell genug zu machen, um die hochgeschwindigkeitsbasierten Daten des IMU zu verarbeiten, während er auf die langsamen GPS-Updates wartet, ohne dabei durch die schwere Mathematik ausgebremst zu werden.

Die Herausforderung: Von der Mathematikklasse zur Hardware
Die Reise begann mit einem großen Hindernis: Der ursprüngliche Algorithmus wurde in MATLAB geschrieben, einem Software-Tool voller ausgefallener „Toolbox“-Funktionen, die Hardware-Chips nicht verstehen können. Es war, als würde man versuchen, ein Rezept für ein hochmodernes molekulargastronomisches Labor mit einem Lagerfeuer zu kochen. Der Autor musste die ganzen schicken Software-Abhängigkeiten entfernen und den Code umschreiben, damit er synthetisiert (also in Hardware-Logik übersetzt) werden konnte.

Eine wesentliche Änderung betraf den „Zustandsvektor“, was im Grunde nur eine Liste aller Dinge ist, die der Computer über Ihre Position zu erraten versucht (wie etwa, wo Sie sind, wie schnell Sie fahren und in welche Richtung Sie blicken). Das ursprüngliche MATLAB-Modell versuchte, 22 verschiedene Dinge zu erraten, einschließlich der Magnetfelddaten. Der Autor erkannte, dass für dieses spezifische GPS/IMU-Setup die Magnetdaten nicht zwingend erforderlich waren. Durch die Reduzierung der Liste auf 16 Schlüsselwerte reduzierte er die Menge der mathematischen Berechnungen, die der Chip durchführen musste, um etwa 65 %. Es ist, als würde man erkennen, dass man nicht die Temperatur jedes einzelnen Blattes im Wald verfolgen muss, um zu wissen, dass man verloren gegangen ist; man muss nur seinen Pfad und seine Geschwindigkeit verfolgen.

Die magischen Tricks: Wie sie es schnell machten
Um die Mathematik fliegen zu lassen, wandte der Autor einige clevere Tricks an:

1. Analytische Formeln: Anstatt den Computer raten und prüfen zu lassen (iterative Methoden), um komplexe mathematische Probleme zu lösen, verwendete er exakte, vorab berechnete Formeln. Es ist der Unterschied zwischen dem Raten der Antwort auf ein Rätsel, indem man jedes Wort im Wörterbuch ausprobiert, gegenüber dem einfach die Antwort zu kennen.
2. Cholesky-Faktorisierung: Für die schwere Arbeit der Matrix-Mathematik (was so ähnlich ist wie das Organisieren einer riesigen Tabelle von Wahrscheinlichkeiten) verwendete er eine spezifische, effiziente Methode namens Cholesky-Faktorisierung. Dies ist vergleichbar mit der Nutzung einer Abkürzung, um ein Puzzle zu lösen, anstatt zu versuchen, jedes Teil an jede mögliche Stelle einzupassen.
3. Pipelining: Er verwendete spezielle Anweisungen namens „PIPELINE“ und „ARRAY_PARTITION“. Stellen Sie sich eine Fabrik-Montageband vor, bei der jeder Arbeiter einen winzigen Schritt erledigt und die Arbeit sofort an den nächsten Arbeiter weitergibt. Dies ermöglicht es dem Chip, Daten kontinuierlich zu verarbeiten, ohne darauf warten zu müssen, dass der vorherige Schritt abgeschlossen ist.

Die Ergebnisse: Geschwindigkeit und Genauigkeit
Das Team testete ihr Design auf einem Xilinx Kintex-7 FPGA. Die Ergebnisse waren beeindruckend. Der Chip lief mit einer Taktfrequenz von 50 MHz. Jedes Mal, wenn er seine Vermutung über Ihren Standort aktualisieren musste, dauerte dies zwischen 1,12 Millisekunden und 1,21 Millisekunden. Das ist unglaublich schnell – schnell genug, um mit den Hochgeschwindigkeits-IMU-Sensoren Schritt zu halten.

In Bezug auf die Ressourcen war das Design überraschend effizient. Es verbrauchte nur 191 „DSP-Slices“ (die Rechenmotoren des Chips), was lediglich 12 % der gesamten verfügbaren Leistung des Chips ausmacht. Dies lässt reichlich Raum für andere Aufgaben.

Am wichtigsten ist, dass der Autor die Ausgabe des Chips mit dem ursprünglichen Computermodell verglich. Der Unterschied war minimal – weniger als 0,001 % Fehler. Dies beweist, dass der Chip selbst dann die Wahrheit sagt, wenn sie die Mathematik vereinfacht und den Code angepasst wurden, um zur Hardware zu passen. Das Paper kommt zu dem Schluss, dass es durch die Verwendung von High-Level-Synthesis (HLS)-Tools und kluger Vereinfachungen möglich ist, ein echtzeitfähiges Navigationssystem mit geringer Latenz auf einem Mittelklasse-Chip zu bauen, das sowohl schnell als auch präzise ist.