Integration von Drehraten einer IMU

[Markus B. (Firma: iunntb) (markusberwanger)]

Hallo,

Ich möchte für kurze Zeit die Lage meines Gerätes im Raum bestimmen. Die 
Zeit ist so kurz, dass Offset-Fehler keine Rolle spielen. Ich habe ein 
massives Problem beim Auswerten meiner  Giro-Daten und mir ist absolut 
nicht klar ob das Problem:

⦁  in meinem Software steckt   ODER
⦁  durch meinen Sensor verursacht wird ODER
⦁  an meinem fehlerhaften math. Verständnis liegt

Drehe ich meine IMU definiert um EINE(!!!) beliebige Achse und an- 
schließend wieder auf gleichem Weg in die Ausgangslage zurück, dann ist 
das Integral der Drehrate über die Zeit praktisch gleich Null (siehe 
Diagramm Winkel_XYZ.gif bzw. Winkel_Summe.gif). Drehe ich die IMU jedoch 
chaotisch um mehrere Achsen und wieder in die Ausgangslage zurück, kommt 
beim Integrieren nur Blödsinn raus. (siehe Diagramm 
Winkel_XYZChaotisch.gif bzw. Winkel_SummeChaotisch.gif). Müsste dann 
nicht auch Null bei rauskommen?
Kann mir hier jemand weiterhelfen? Habe ich ein Problem mit der 
Mathematik oder mit meiner Hardware oder beidem?
Diagramme:
Die Diagramme zeigen einmal die Drehraten der X-, Y- und Z-Achse und die 
geomtrische Summe jeweils für die Drehung um eine Achse bzw. um mehrere 
Achsen.
Testaufbau
Ich verwende die MPU 9150 von InvenSense. Die Daten werden mit 5 Hz in 
einen FIFO geschrieben.  Der Range ist +/- 250 Grad/s. Wenn Daten im 
FIFO stehen liest ein kleiner PIC die Daten  und sendet sie über RS232 
an einen PC, der sie aufzeichnet.
Die IMU wird so langsam gedreht, dass die Drehrate garantiert innerhalb 
der Ranges bleibt.

Viele  Dank

[Timmo H. (masterfx)]

5Hz ist zu langsam wenn die Drehgeschwindigkeit nicht konstant ist. 
Zwischen den sample punkten inkonstante Drehgeschwindigkeiten "verpasst" 
du dadurch. Für meinen quadcopter sample ich mit 100 Hz und errechne 
auch mit 100 Hz meine Lage neu. Das schafft der atxmega mit 32 MHz ohne 
Probleme und das in (Software) float

[RP6conrad (Gast)]

Sie haben ein Mathematisches Problem. Wenn du wirklich alle Achsen 
verdreht, darfst du nicht mehr einfach jeden einzele Achs separat 
integrieren ! Du brauchst dan etwas mehr Math : Stichwort Quaternionen, 
3D IMU usw.

[Timmo H. (masterfx)]

Quaternionen braucht es dafür nicht. Das ist einfach nur eine andere Art 
der Darstellungsweise. Quarternionen verhindern vielleicht einen Gimbal 
Lock, ersetzen aber keinen Filter Algorithmus (Kalman, PID etc).
Natürlich kann es sein, dass sein Algo oder die Testprozedur irgendwie 
probs mit Gimbal Lock oder sowas hat, aber ich bleib dabei das 5 Hz 
einfach zu wenig für eine saubere IMU sind. Man verpasst einbfach zu 
viele Momentanwerte der Drehraten einfach, sodass der aufintegrierte 
Winkel einfach nicht stimmen kann. 100 Hz sollten es schon sein, das ist 
auch mit einfachen µCs heutzutage kein Problem mehr. Da habe ich auch 
bei schnellen Bewegungen (ohne Quaternionen, Kalman oder sonstiges, 
sondern einfach nur einfachen PID mit 1% Accelerometer und Magnetometer 
Anteil) absolut keine Probleme.

[Markus B. (Firma: iunntb) (markusberwanger)]

Ich hatte vergessen, zu erwähnen, dass die Daten von der IMU mit 1 KHz 
gesampled werden und durch ein digitales Tiefpassfilter mit 5Hz 
Grenzfrequenz laufen. Die IMU wirft dann mit 5Hz die Daten raus. Wegen 
dem Tiefpass ging ich davon aus, dass 5Hz schnell genug sind.

Ich probiere jetzt die Quaternionen irgendwie vorm Integrieren 
einzubauen, aber ich weiß halt noch nicht wie.

[Stm M. (stmfresser)]

Es liegt an den konstanten Drift von Gyroskop, der immer weiter 
integriert wird. Wenn du das Gyroskop auf dem Tisch stellst, steigt der 
Winkel an. Sodass müsst du den Drift bestimmen, indem du FIR ARX Filter 
am besten Zustandsbeobachter wie Kalman Luneberger High Gain einsetzt. 
Wenn du den IMU nicht im Kleinsignal-Bereich einsetzt, müsst du dich 
wohl um die Singularität kümmern, das geht wie oben erwähnt mit 
Quaternionen.

[Pandur S. (jetztnicht)]

Erst sollte man realisiert haben, dass Rotationen nicht permutieren. Dh 
man kan einzelrotationen nicht vertauschen. Speziell weil solche 
Rotationen im eigenen Koordinatensystem ablaufen. Aber auch wenn 
Rotationen im globalen System beschrieben werden kann man sie nicht 
vertauschen. Deswegen muss man die Dreh-Position als Ganzes mitfuehren.

[Sensorfusionierer (Gast)]

Oder D. schrieb:
> Erst sollte man realisiert haben, dass Rotationen nicht permutieren. Dh
> man kan einzelrotationen nicht vertauschen.

Genau so ist es.

Das Stichwort lautet Strapdown-Algorithmus. Dieser löst genau dein 
Problem.

Ein Buch das ich zu diesem Thema empfehlen kann ist "Integrierte 
Navigationssysteme" von Jan Wendel.

[Sensorfusionierer (Gast)]

Genaugenommen geht es um die (näherungsweise, zeitdiskrete) Lösung der 
Eulerwinkeldifferentialgleichungen:

$$\dot{\phi} = \left ( \omega_y \sin \phi + \omega_z \cos \phi \right ) \tan \theta + \omega_x$$

$$\dot{\theta} = \omega_y \cos \theta - \omega_z \sin \phi$$

$$\dot{\psi} = \left( \omega_y \sin \theta + \omega_z \cos \phi \right ) / \cos \theta$$

Phi, Theta und Psi sind dabei Roll-, Pitch- und Yaw-Winkel der IMU. 
Omega ist die Drehrate der IMU (plus Erddrehrate etc.). Wie man sieht 
sind die DGLs nichtlinear und verkoppelt. Man sieht auch schön, dass bei 
90° Pitch eine Singularität auftritt: der Nenner der 3. Gl. wird null. 
Die Lage kann per Eulerwinkel zwar noch dargestellt, aber die DGLn nicht 
mehr eindeutig gelöst werden.

Die Lösung der DGLn erfolgt durch Umrechnung in Orientierungsvektoren 
bzw. Quaternionen. Es sei hier auf das Kapitel 3.3.1 des o.g. Buches 
verwiesen. In der Originalquelle "Strapdown Inertial Navigation 
Technology" von Titterton sollte das ebenfalls zu finden sein.

In diesem Video: https://www.youtube.com/watch?v=C7JQ7Rpwn2k geht es 
ebenfalls um ziemlich genau das Thema, aber wenn ich mich recht erinnere 
werden keine konkreten Berechnungsvorschriften gegeben.

[Markus B. (Firma: iunntb) (markusberwanger)]

RP6conrad schrieb:
> Sie haben ein Mathematisches Problem. Wenn du wirklich alle Achsen
> verdreht, darfst du nicht mehr einfach jeden einzele Achs separat
> integrieren ! Du brauchst dan etwas mehr Math : Stichwort Quaternionen,
> 3D IMU usw.

Ich hatte wohl ein mathematisches Problem. Der im ersten Diagramm 
dargestellte Algorithmus integriert die Drehrahten richtig. Er arbeitet 
wie folgt:

Zuerst wird aus den 3 Winkeln ein Quaternion gebildet

Diese Quaternion wird mit einem Quaternion, das die Ausgangslage zm 
Zeitpunkt t - 1 repräsentiert, multipliziert. Initial hat dieses 
Quaternion den Wert [1 0 0 0], was keiner Drehung entspricht.

Das Ergebnis der Quaternionenmultiplikation ist die neue Lage undwird in 
den Schieberegistern für den nächsten Zeitschritt gespeichert.

Das Diagramm ScilabPlot zeigt einen Vector mit Ausgangslage in 
X-Richtung der wild gedreht wird, dann 90° gedreht abgelegt, wieder wild 
gedreht, wieder abgelgt wird usw. Prinzipiell wird die Drehung richtig 
integriert.

Der Algorithmus ist so erweitert, dass er die Erdbeschleunigung nutzt, 
im die Drift um X- und Y-Achse zu korrigieren, Z-Achse wird nicht 
korrigiert. Deshalb liegen Y- und Z- nach 440 Sekunden(!) nicht mehr bei 
0.

@ RP6conrad, Danke für den Tip. Ich denke der war richtig.