Umrechnung Quaternionen -> Eulerwinkel

[chris2091]

Hallo zusammen,

ich habe eine IMU von Hillcrest FSM-9. Der Sensor beinhaltet auch einen Modus über den die Maus gesteuert werden kann.
Dabei werden Roll-Bewegung(Drehung um die x-Achse) natürlich nicht berücksichtigt.
Jetzt habe ich einen Cursor in mein Programm eingebaut, der auf die Roll-Bewegungen reagiert und sich entsprechend mitdreht.
Dafür wandelt ich die Quaternionen entsprechend der Formel um.https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles


Dim A = daten_winkel.x 'Zwischenspeichern, damit Formel übersichtlicher
Dim B = daten_winkel.y
Dim C = daten_winkel.z
Dim D = daten_winkel.w
'Umrechnung von Quaternion-Form in die Eulerform (rad) Formel aus Wikipedia
Dim rollen = Atan2(2 * (A * B + C * D), 1 - 2 * (B * B + C * C))
Dim nicken = Asin(2 * (A * C - B * D))
Dim gieren = Atan2(2 * (A * D + C * B), 1 - 2 * (C * C + D * D))
Leider bekomme ich eigenartige Werte raus, die nur bedingt hinkommen. So drehe ich den Sensor um fast 90° um die x-Achse und es werden mir nur Werte zwischen 50-60° angezeigt.

Ich habe im Internet ein wenig geforscht und anscheinend sind Euler-Winkel nicht das Gelbe vom Ei (Gimbal-Lock..usw.)

Hat vielleicht jemand Erfahrung mit der Umrechnung zwischen Quaternionen und Euler-Winkeln und kann mir helfen?

Vielen Dank!

[Defiant]

Mal ne blöde Frage: Wieso hast du nicht einfach den Quellcode aus der Wikipedia (https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#So urce_Code_2) abgeschrieben?

[chris2091]

Hallo Defiant,
habe ich gemacht

gieren und rollen passen halbwegs, allerdings stimmt der pitch-Winkel nicht.
Sobald ich ein Nicken mit dem Sensor vollführe, bewegen sich alle drei Winkel erheblich, obwohl ich den Sensor nur in eine Richtung bewege.
Klar eine ganz genaue Bewegung kriegt man nicht hin, aber die Ausschläge in roll und yaw Richtung sind in der selben Größenordnung wie die des pitch-Winkels

[Defiant]

Ich kann leider in deinem Quellcode (Welche Sprache ist das eigentlich?) nicht so richtig den von der Wikipedia erkennen. Eine ordentliche Benennung der Variablen z.B. würde sicher helfen. Ich hab deine Variante mit dem Python code stichprobenartig verglichen und kann mehrere Unterschiede erkennen, z.B. stimmt deine Berechnung der Roll-Drehung mit der Berechnung um die Z-Achse (Also gieren) des Python-wikipedia codes überein.

[shedepe]

Du musst außerdem dran denken die Quaternionen davor zu normieren. Da bin ich neulich mehrere Stunden dran verzweifelt.

[chris2091]

Ich kann leider in deinem Quellcode (Welche Sprache ist das eigentlich?)... Also die Sprache ist vb.Net, ist dir vielleicht durch Visual Studio bekannt.
Ich habe den Code ein wenig angepasst und näher an das Original aus dem Wikipedia-Artikel gebracht

'Formel zur Umwandlung quatertionen zu Euler-Winkel nach wikipedia
' https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
'****************************
Dim ysqr = daten_winkel.y * daten_winkel.y

' roll (x-axis rotation)
Dim t0 = +2.0 * (daten_winkel.w * daten_winkel.x + daten_winkel.y * daten_winkel.z)
Dim t1 = +1.0 - 2.0 * (daten_winkel.x * daten_winkel.x + ysqr)
Dim rollen = Atan2(t0, t1)

' pitch (y-axis rotation)
Dim t2 = +2.0 * (daten_winkel.w * daten_winkel.y - daten_winkel.z * daten_winkel.x)
t2 = If(t2 > 1.0, 1.0, t2)
t2 = If(t2 < -1.0, -1.0, t2)
Dim nicken = Asin(t2) 'pitch

' yaw (z-axis rotation)
Dim t3 = +2.0 * (daten_winkel.w * daten_winkel.z + daten_winkel.x * daten_winkel.y)
Dim t4 = +1.0 - 2.0 * (ysqr + daten_winkel.z * daten_winkel.z)
Dim gieren = Atan2(t3, t4)

Ich habe heraus gefunden, dass auch die Orientierung eine sehr wichtige Rolle spielt, also um welche Achsen gedreht wird.
So gibt es XYZ aber auch XYX und so weiter. Dazu finde ich diese Seite ganz gut http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
Außerdem gibt es da für fast jede denkbare Orientierung schon Quellcode in C++.

@shedepe Vielen Dank für den Hinweis! Das werde ich gleich umsetzen.