Tinkerforge IMU zur Lageerkennung von Fahrzeugen

Das Fahrzeugkoordinatensystem ist nach Rechter-Hand-Regel mit X in Fahrtrichtung, Y nach links und Z nach oben. Die Drehung erfolgt jeweils auch mit der Rechten-Hand-Regel um die Achsen.

Es wird bei den nachfolgenden Berechnungen davon ausgegangen, dass die IMU mit X-Achse in Fahrtrichtung und Y-Achse nach links im Fahrzeug eingebaut wurde. Die Achsen sind auf der IMU aufgezeichnet. Sollte eine andere Einbaulage gewählt werden, müssen die nachfolgenden Formeln angepasst werden.

Bestimmung von Roll und Pitch aus der Quaternion

Für den Menschen ist das Beschreiben von Rotation in Euler Winkeln intuitiv. Leider ergibt sich dadurch das Problem des Gimbal-Lock, wodurch in bestimmten Konstellationen keine Lagebestimmung mehr möglich ist. Die Tinkerforge IMU gibt glücklicherweise die Quaternion aus, womit eine Lagebestimmung Gimbal-Lock frei möglich ist. Der API call

IMU.get_quaternion()

gibt die 3 imaginären (x,y,z) und die reale (w) Komponente der Quaternion zurück. Für weitere Details wird (Buchholz, J. J.: Vorlesungsmanuskript Regelungstechnik und Flugregler) empfohlen, aus welchem auch die nachfolgenden Formeln abgeleitet sind:

$$pitch = -\arcsin\left(2\cdot(wy -xz)\right) \\
roll = -\arctan\left(\frac{2\cdot(yz + wx)}{-(w^2 – x^2 – y^2 + z^2)}\right)$$

Eine komplette Python Implementierung liegt im Github.

Yaw

Die IMU erfasst nur die lokale Orientierung des Fahrzeugs. Damit ist Yaw von der IMU nicht Course vom GPS, also nicht die Fahrtrichtung im GPS Bezugsdatum WGS84. Zwar besitzt die IMU ein Magnetometer, welches auch zur Lageerfassung genutzt wird, doch der von Tinkerforge implementiert Fusionsalgorithmus (Madgwick, S.: An efficient orientation filter for inertial and inertial/magnetic sensor arrays) hat genau hier eine Schwäche: Er kann die Orientierung in der Horizontalen bei Störungen des Magnetfelds oder anderen magnetischen Störeinflüssen, nicht gut ausgleichen. Die absolute Orientierung ist damit fehlerbehaftet. Die relative Drehung wird jedoch sicher erfasst.

Diese Information zur absoluten Ausrichtung (Norden/Osten/Süden/Westen) muss vom GPS Modul kommen und mit diesem fusioniert werden: Verbesserte Positionsschätzung durch Kalman Filter.

Drehraten

Die Gierrate (Yawrate), Nickrate (Pitchrate) und Rollrate (Rollrate) sind im lokalen IMU Koordinatensystem und können mit entsprechender Umrechnung auf physikalisch sinnvolle Einheiten (

\(\frac{^\circ}{s}\)) genutzt werden.

rollrate = ang_x/14.375
pitchrate= ang_y/14.375
yawrate = ang_z/14.375

Position

Das GPS Modul erfasst die Position und Höhe auf der Erdoberfläche, welche durch einen Referenzellipsoiden angenähert ist. Nach ein paar Bewegungen kann das GPS Modul auch die Richtung bestimmen, in die sich das Fahrzeug bewegt. Die globale Positionierung (Lat, Lon, Course) ist damit festgelegt.

Geschwindigkeit

Die Geschwindigkeit wird bei GPS Receivern nicht aus der Änderung der Position pro Zeit bestimmt, sondern über den Doppler-Effekt. Dieser verlangt allerdings nach einer Relativgeschwindigkeit. Bei sehr langsamen Bewegungen (ca.

\(<5\frac{km}{h}\)) ist der Effekt nicht mehr sinnvoll im GPS Bricklet nutzbar.

Einwirken externer Beschleunigungen

Der in der IMU implementierte Lageerkennungsalgorithmus (Madgwick, S.: An efficient orientation filter for inertial and inertial/magnetic sensor arrays) fusioniert u.a. auch die Beschleunigungen, die auf den Sensor wirken. Dabei geht er davon aus, dass ein deutlicher und dominierender Wert die Erdbeschleunigung ist. Diese Einschränkung kann für ein Fahrzeug nicht unbedingt getroffen werden. Beim Beschleunigen, Abbremsen, Kurvenfahrt oder auch durch Straßenunebenheiten wirken Beschleunigungen mit ähnlich hohen Amplituden auf die IMU ein, weshalb diese die Lage im dynamischen Fall nicht korrekt bestimmt! Nur bei Stillstand des Fahrzeugs konvergiert die ermittelte Lage auf die Tatsächliche, je nach gewählter Konvergenzgeschwindigkeit. Dabei muss, wie bei jedem Filter, ein Kompromiss zwischen Dynamik und statischer Genauigkeit gefunden werden.

Zum Lösen des Zielkonflikts könnte eine dynamische Anpassung an einwirkende Beschleunigungen definiert werden:

if ((ax**2+ay**2+(az+9.81)**2)**(1/2)) < 1.0:
    IMU.set_convergence_speed = 200  # mit 200°/s konvergieren
else:
    IMU.set_convergence_speed = 1  # mit 1°/s konvergieren

Dies stellt sicher, dass bei ruhendem Fahrzeug die Konvergenzgeschwindigkeit erhöht wird. Sollte das Fahrzeug dynamisch bewegt werden, wird hauptsächlich auf die Drehraten zur Lagebestimmung gesetzt.

Korrektur von Gain

Ein besonderer Vorteil der Tinkerforge IMU ist auch, dass der darin implementierte Datenfusionsalgorithmus automatisch eine Korrektur von Temperaturdrift und statischer Ungenauigkeit der Drehratensensoren hat. Dabei benutzt die IMU absolute Werte, wie das Erdmagnetfeld oder die Erdbeschleunigung. Die Konvergenzgeschwindigkeit muss dafür allerdings größer 0 sein, was zu Lasten der Dynamik geht.

Beispiel

Um ein Gefühl für die Qualität der aufgezeichneten Daten zu bekommen, kann folgender Versuch eine Einschätzung geben: Man geht davon aus, dass sich ein Fahrzeug innerhalb eines Abtastschritts (hier:

\(50 Hz\), also

\(T=\frac{1}{50}=0{,}02s\)) mit konstanter Geschwindigkeit und Gierrate bewegt. Die Bewegungsgleichungen beschreiben die Bewegung des Fahrzeugs in der Ebene:

$$x = x + \frac{v}{\dot\psi} \left(- \sin{\left (\psi \right )} + \sin{\left (T \dot\psi + \psi \right )}\right)\\
y = y + \frac{v}{\dot\psi} \left(\cos{\left (\psi \right )} – \cos{\left (T \dot\psi + \psi \right )}\right)\\
\psi = \psi + T \dot\psi\\
v = v\\
\dot\psi = \dot\psi$$

Dabei ist

\(x=\text{x-Position}\),

\(y=\text{y-Position}\), 

\(T=\text{Abtastzeit}\), 

\(\psi=\text{Fahrtrichtung}\), 

\(\dot\psi=\text{Gierrate}\) und 

\(v=\text{Geschwindigkeit}\).

Nun wird mit den aufgezeichneten Daten Geschwindigkeit (aus GPS Bricklet) und Gierrate (aus IMU Brick) berechnet, wohin sich das Fahrzeug bewegt hat. Ohne eine Korrektur vorzunehmen, integrieren sich die Messfehler auf und die Position wird, um so länger man fährt, falscher. Doch wie gut es trotzdem funktioniert, zeigt folgende beispielhafte Fahrt:

Die orangenen Punkte sind aufgezeichnete GPS Punkte und stellen das Soll dar. Lässt man das Fahrzeug nun ausschließlich mit Gierrate und Geschwindigkeit (ohne GPS Positionsmessung!) fahren, so berechnen die Bewegungsgleichungen folgende Trajektorie aus Gierrate und Geschwindigkeit:

Das Ergebnis zeigt eindrucksvoll, was mit relativ preisgünstiger Sensorik möglich ist. Die akkumulierten Fehler zum Ende der Fahrt sind hauptsächlich auf ungenaue Geschwindigkeitsmessungen des GPS auf Grund von schlechter Signalqualität (Abschattung etc.) zurück zu führen.

Mit entsprechender Sensordatenfusion lässt sich die Positions- und Lagebestimmung erheblich verbessern.

Lage des Fahrzeugs

Fazit

Die Tinkerforge IMU ist auf Grund ihrer schaltungstechnisch hervorragenden Qualität, der implementierten Algorithmen sowie der guten Dokumentation bestens geeignet, um schnell und qualitativ hochwertig Algorithmenentwicklung zu ermöglichen.

Der relativ günstige Preis sowie die aktive Community als auch der Support seitens Tinkerforge machen die Bricks zu einem guten Partner für unseren Low Budget Car PC.