Einführung in die Totwinkelnavigation – Integration von Rädern, IMU und GNSS zur Verfolgung der Roboterbewegung

Die einfachste Methode für einen Roboter, seine Position zu bestimmen (z. B. „1 m nach rechts, 2 m nach vorn“), besteht darin, seine eigene Position kontinuierlich zu erfassen. Er integriert Radumdrehungen, Motordrehzahlen und IMU-Winkelraten über die Zeit, um seine aktuelle Position zu schätzen. Diese Methode wird als Koppelnavigation bezeichnet. Sie funktioniert auch dann, wenn GPS oder eine Kamera vorübergehend nicht verfügbar sind. Geringfügiger Radschlupf oder Sensorabweichungen führen jedoch zu Positionsfehlern, die mit der Zeit zunehmen.

Absolute GNSS-Beobachtung

Kostengünstiges IMU-Beispiel

Bilder: Surveyor Verwendung eines GNSS-Empfängers mit RTK-Lösung (SweetCanadianMullet, CC BY 4.0) / GY-521 MPU-6050-Modul (Nevit) Dilmen, CC BY-SA 3.0), Wikimedia Commons. Repräsentative Sensoren, keine zwingende Hardware-Kombination.

30-Sekunden-Zusammenfassung

• Die Koppelnavigation berechnet den aktuellen Zustand aus dem vorherigen Zustand plus der gemessenen Positionsänderung. Sie kann auch ohne Beobachtungen schnell aktualisiert werden.

• Bei einem Roboter mit Differenzialantrieb bestimmt die Differenz der Positionsänderungen des linken und rechten Rades den Drehwinkel, deren Mittelwert die Vorwärtsstrecke. Fehler im Radradius und der Spurbreite zeigen sich als systematische Kurve.

• Der Odometriefehler akkumuliert sich durch Integration. Zur Bestimmung einer absoluten Position sind Korrekturen durch Beobachtungen wie GNSS, LiDAR, Kamera oder reflektierende Marker erforderlich.

• Eine IMU ist bei kurzfristigen Lageänderungen robust, die Position driftet jedoch schnell ab, wenn die Beschleunigung doppelt integriert wird. Die Fusion von Rad- und IMU-Daten mittels eines erweiterten Kalman-Filters (EKF) oder eines Faktorgraphen ist ein gängiges Verfahren.

Bei der Implementierung müssen Zeitstempel, Koordinatensysteme, Vorzeichenkonventionen für links/rechts, Radradius, Schlupferkennung und Kovarianz von Anfang an korrekt definiert sein – Fehler lassen sich selbst mit teuren Sensoren nicht beheben.

1. Kinematik des Differentialantriebs

Der linke und rechte Radradius seien r, der Abstand zwischen den Rädern (Spurweite) b und die Winkelgeschwindigkeiten links/rechts \omega_L,\omega_R. Die linearen Geschwindigkeiten der Räder sind v_L=r\omega_L und v_R=r\omega_R, daher sind die Translationsgeschwindigkeit v und die Gierrate \dot{\theta} des Fahrzeugmittelpunkts:

Sei die Position zum Zeitpunkt k gleich (x_k,y_k,\theta_k) und die Abtastperiode gleich \Delta t, so ergibt die einfache Euler-Integration:

Beim Befahren eines Kreisbogens mit konstanter Geschwindigkeit hält die Verwendung der exakten Integration (Exponentialfunktion in Körperkoordinaten) den Fehler auch bei niedriger Geschwindigkeit und hoher Kurvenrate gering.

Abbildung 1 – Im Differenzialantrieb erzeugt die Differenz zwischen der Geschwindigkeit des linken und rechten Rades die Gierrate. Durch das Anhalten eines Rades kann der Roboter um dieses Rad drehen.

2. Warum die Wegmessung driftet

Wenn der Radradius einen Fehler von 1 % aufweist und man annimmt, dass links und rechts während der Fahrt gleich groß sind, dreht sich das Fahrzeug allmählich, selbst wenn man glaubt, geradeaus zu fahren. Die Auflösung der linken/rechten Encoder, das Zahnflankenspiel, Unebenheiten des Bodens, der Reifenverschleiß und die lastbedingte Verformung führen zu systematischen Fehlern. Rutscht ein Rad auf nassem oder Schotterboden durch, stimmen die gemessene Drehung und die tatsächliche Verschiebung nicht mehr überein.

Der Lagefehler \delta\theta führt zu einem Fehler in der seitlichen Position. Für die Vorwärtsstrecke s ergibt eine Kurzzeitnäherung:

Bei einer Fahrt von 1 km mit einem Winkelfehler von 0,5 Grad beträgt die seitliche Abweichung etwa 8,7 m. Die einfache Angabe des Positionsfehlers als „so und so viele Zentimeter pro Sekunde“ erfasst nicht die unterschiedlichen Fehlerstrukturen beim Geradeausfahren, in Kurven und beim Rutschen.

Die Kovarianz der Radodometrie überträgt die Unsicherheit der Links-/Rechtsverschiebung auf die Position und aktualisiert diese entsprechend. In der Implementierung wird die Kovarianz bei Kurvenfahrten oder Schlupf erhöht, damit die nachfolgende Sensorfusion der Odometrie nicht zu viel Vertrauen schenkt.

3. Hinzufügen einer IMU

Die Integration der Winkelgeschwindigkeit \omega_z vom Gyroskop der IMU ergibt

wobei b_g die Gyroskop-Bias und n_g das Rauschen darstellt. Selbst im Moment des Radschlupfs kann die kurzfristige Änderung der Winkelgeschwindigkeit gemessen werden, was eine gleichmäßigere Lagebestimmung als mit Rädern allein ermöglicht. Bei der doppelten Integration der Beschleunigung zur Bestimmung von Geschwindigkeit und Position wächst der Fehler durch die Bias b_a proportional zu

daher kann eine IMU allein nicht als Langzeit-Positionssensor dienen.

Kombination von Rädern und einer IMU: Die Räder erfassen niederfrequente Verschiebungen, die IMU hingegen hochfrequente Winkelgeschwindigkeit und Beschleunigung. Roll- und Nickwinkel werden aus der Schwerkraftrichtung initialisiert. Ist ein Magnetometer oder GNSS verfügbar, wird zusätzlich eine absolute Gierachse als Referenzwert hinzugefügt.

4. Korrektur mit einem EKF oder Faktorgraphen

Ein Beispiel für einen Zustandsvektor ist:

Im Vorhersageschritt wird ein nichtlineares Bewegungsmodell f aus Rad- und IMU-Daten berechnet. Bei Eintreffen einer Beobachtung wird das Residuum zur Korrektur verwendet.

Der erweiterte Kalman-Filter (EKF) linearisiert das nichtlineare Modell lokal mithilfe der Jacobi-Matrix. GNSS-Position, LiDAR-Scan-Matching, Kamera-Feature-Punkte und bekannte Markerpositionen können als Beobachtungen \mathbf{z} verwendet werden. Bei großen Schleifen oder Wiederholungsfahrten werden vergangene Positionen und Beobachtungen in einem Faktorgraphen gespeichert und das gesamte Verfahren anschließend neu optimiert.

Für ROS 2 (unter der Annahme der Jazzy-Standard-Nachrichtendefinitionen) sollten die Rad- (nav_msgs/msg/Odometry), IMU- (sensor_msgs/msg/Imu) und TF-Transformationen (odom → base_link) eine gemeinsame Zeitbasis verwenden. Diese Nachrichtentypen entsprechen den ROS-1-Bezeichnungen nav_msgs/Odometry und sensor_msgs/Imu. Implementierungsdetails, QoS und Distributionseinstellungen von ROS 2 müssen jedoch separat geprüft werden. Der EKF von robot_localization ist ein guter Ausgangspunkt für die Implementierung, jedoch führt das Nullsetzen von Kovarianzen oder die doppelte Eingabe derselben Informationen zu einer unnatürlich steifen Schätzung.

5. Anwendung in aktuellen Robotern

Autonome mobile Roboter (AMRs) für den Innenbereich verwenden permanent Radencoder und eine IMU und korrigieren die Drift mittels LiDAR oder Kamera-Map-Matching. Beim autonomen Fahren werden Radgeschwindigkeit, Lenkwinkel, IMU, GNSS und LiDAR fusioniert, sodass die Schätzung auch bei GNSS-Ausfall in Tunneln oder unter erhöhten Bauwerken über Sekunden bis zu mehreren zehn Sekunden zuverlässig ist. Bei Agrarrobotern erhöhen Schlamm, Spurrillen und Steigungen den Radschlupf, weshalb RTK-GNSS, IMU und Kraftsensoren kombiniert werden.

Unterwasserroboter und Raumfahrzeuge legen lange Strecken ohne GNSS zurück. Um die Kovarianz der Koppelnavigation zu kontrollieren, werden ein Doppler-Geschwindigkeitslog (DVL), ein Sternentracker oder eine Geländeanpassung als niedrigfrequente absolute Beobachtung hinzugefügt. Entscheidend ist die Weitergabe der Schätzung an Planung und Steuerung zusammen mit der Frage: „Wie zuverlässig ist diese Schätzung?“

6. Checkliste für die Implementierung

1. Konvertieren Sie die Sensorzeitstempel in eine gemeinsame Uhrzeit und messen Sie die Latenz.

2. Zeichnen Sie die Koordinatensysteme base_link, odom und map sowie deren rechtshändige Vorzeichenkonventionen.

3. Ermitteln Sie Radradius, Profiltiefe und Encoderauflösung anhand realer Fahrtests.

4. Bewerten Sie den Fehler separat für Geradeausfahrt, Kurvenfahrten, Achterfiguren, Unebenheiten und reibungsarme Oberflächen.

5. Erhöhen Sie die Kovarianz bei Schlupf oder Aussetzern und korrigieren Sie sie bei Vorliegen eines absoluten Messwerts.

6. Spielen Sie die Protokolle erneut ab, um zu bestätigen, dass dieselbe Eingabe dieselbe Schätzung liefert.

Erfassen Sie während der Tests nicht nur den Endpunktpositionsfehler, sondern auch den Lagefehler, den maximalen Fehler, das 95. Perzentil und die Driftrate bei Aussetzern. Eine Schätzung, die zwar meist genau ist, aber gelegentlich stark abweicht, eignet sich nicht für eine sichere Pfadplanung.

Zusammenfassung

Die Koppelnavigation ist die grundlegende Technik zur Schätzung der Roboterbewegung mit minimaler Verzögerung. Es integriert Radverschiebung und IMU-Winkelrate und unterdrückt Drift mithilfe absoluter Messwerte von GNSS, LiDAR oder Kamera. Bevor weitere Sensoren hinzugefügt werden, ist die korrekte Bestimmung des Bewegungsmodells, der Zeitstempel, der Koordinatensysteme und der Kovarianz entscheidend für Genauigkeit und Wiederholbarkeit.