Softwarehilfe gesucht - Eulerwinkel durch eine Rotationsmatrix ersetzen (IMU)

[Julian B. (julian220)]

Hallo Zusammen,

ich habe ein kleine IMU, das mittels Gyros (und Beschleunigungssensor)
die Lage bestimmt.

Bei Gyros müssen ja die Achsen verrechnet werden (bei der Rotation),
aus Nick +45Grad, werden ja durch 90Grad Drehung, +45 Grad Roll
(Nick ist dann 0 Grad). Sozusagen überträgt man die Winkelneigung,
je nachdem wie sehr man das IMU dreht.

Bisher mache ich das mit Eulerwinkel.

  static float SinRoll, SinNick, CosNick, CosRoll, Tmp;

  // Nick- und Rollwinkel liegen in Grad vor
  // aus den summierten Gyrowerten
  SinNick = sin(NickWinkel*3.14159/180); // Winkel in Rad umrechnen
  SinRoll = sin(RollWinkel*3.14159/180);
  CosNick = cos(NickWinkel*3.14159/180);
  CosRoll = cos(RollWinkel*3.14159/180);
        
  // 1. Rollachse
  Tmp = ((SinRoll * SinNick) / CosNick) * GyroNick + ((CosRoll * SinNick) / CosNick) * GyroGier;
  GyroRoll += Tmp;
  // 2. Nickachse
  GyroNick = CosRoll * GyroNick - SinRoll * GyroGier;

Funktioniert prima, allerdings nur bis 85 Grad Neigung (Nick und Roll).
Gimbal Lock heißt glaube ich das Problem. Bei >90Grad funktioniert die
Rollachse nicht mehr richtig...

Kann mir hier jemand helfen, das durch eine RotationsMatrix/Drehmatrix
zu ersetzen? Es gibt vieles Infos hierzu im Netz, allerdings würde
ich das gerne mit jemanden machen, der das versteht.

Könnte vielleicht so aussehen, die passende Formel wird gesucht,
ich implementiere alles, und falls etwas nicht funktioniert, kann
ich Rückfragen. Ich bin Programmierer aber kein Mathematiker.

Bitte nur jemand, der das schon mal gemacht hat. Wäre 50 EUR Ok?
Für max. zwei Stunden.

Vielen Dank & Viele Grüße
Julian

[Purzel H. (hacky)]

Das Problem ist, dass die Rotationen nicht kommutieren. Dh die Matrix 
ist von der Reihenfolge der rotationen abhaengig. Im Prinzip kann sich 
eine 3D Rotationsmatix aus den passennden 2D Rotationen zusammensetzten.

Eine 2D rotation hat folgende Matrix

R(phi):=  cos sin
         -sin cos

die Vektoren sind (x,y)

Eigentlich sind Quaternionen besser geeignet, benoetigen aber auch 
nochmals eine Stange Mathematik

[Julian B. (julian220)]

Hallo,

Danke, ganz verstehe ich das leider nicht...
Die meisten anderen Rotation-Matrix Referenzen rechnen
mit 3x3 Matrixen wenn das ich das richtig sehe
hintereinander (X,Y,Z)

sieht bei anderen etwas so aus

   // Perform update
   float cos_phi = cos(phi*3.14159/180);
   float cos_theta = cos(theta*3.14159/180);
   float cos_psi = cos(psi*3.14159/180);

   float sin_phi = sin(phi*3.14159/180);
   float sin_theta = sin(theta*3.14159/180);
   float sin_psi = sin(psi*3.14159/180);

   // Build rotation matrix from inertial frame to body frame (for computing expected sensor outputs given yaw, pitch, and roll angles)
   /*
      [                              cos(psi)*cos(theta),                              cos(theta)*sin(psi),         -sin(theta)]
      [ cos(psi)*sin(phi)*sin(theta) - cos(phi)*sin(psi), cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta), cos(theta)*sin(phi)]
      [ sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta), cos(phi)*sin(psi)*sin(theta) - cos(psi)*sin(phi), cos(phi)*cos(theta)]
   */
   R.data[0][0] = cos_psi*cos_theta;
   R.data[0][1] = cos_theta*sin_psi;
   R.data[0][2] = -sin_theta;

   R.data[1][0] = cos_psi*sin_phi*sin_theta - cos_phi*sin_psi;
   R.data[1][1] = cos_phi*cos_psi + sin_phi*sin_psi*sin_theta;
   R.data[1][2] = cos_theta*sin_phi;

   R.data[2][0] = sin_phi*sin_psi + cos_phi*cos_psi*sin_theta;
   R.data[2][1] = cos_phi*sin_psi*sin_theta - cos_psi*sin_phi;
   R.data[2][2] = cos_phi*cos_theta;

   //....

(??? :))

Ich glaube 2D Rotationen reichen da nicht aus.
(Gier (Z) habe ich oben mit Euler nicht berechnet, aber Gier wird
für Nick(X) und Roll(Y) mit einbezogen).

Quaternionen hab ich auch schon gelesen, allerdings,
bin ich da echt überfordert, sehe im IMU Bereich auch
keine Projekte dies so umgesetzt haben. Müßte auch
mit Rotation-Matrixen gehen.

Viele Grüße
Julian

[Purzel H. (hacky)]

Der Nachteil der Euler Rotationen liegt in der endlichen Abfolge. Wenn 
wir einen Einheitsvektor nehmen und den mit dem Gyro 1000 Mal drehen, 
ist er nicht mehr Eins lang. Dh man muss ihn periodisch wieder auf die 
Laenge anpassen. Da sind die Quaternionen besser.
Sicher gehen die normalen 3D Rotationsmatritzen. ueber den Gimbal Lock 
wuerd ich mir nicht allzu viele Sorgen machen denn man dreht ja 
incrementell, allenfalls 5 Grad aufs Mal.

[Martin S. (strubi)]

Wenn Du vollständige Rundum-Rotationen erfassen willst, also Loopings 
etc. erlauben willst, wird's mit den Quaternionen viel einfacher. 
Delta-Rotationen mit Euler-Winkeln gehn natürlich schon, sind aber 
numerisch nicht stabil, sobald dir z.B. bei deinem Fliegerchen mal son 
richtig fieser Rotationsschock reinhaut, wirds knifflig und du musst an 
den "Grenzwinkeln" die Spezialfälle abchecken.
Die Quaternionen-Algebra schreckt zwar ev. bisschen ab, aber ansich 
kannst du die Sache als Blackbox-Operation (Rotation um eine Achse) 
betrachten.
Hier mal die API der-Quat-Section:

void quatmulquat(pQuat out, pQuat p, pQuat q);
void quat_from_rotaxis(pQuat out, pVector3 axis, vfloat phi);
void rotaxis_from_quat(vfloat *angle, pVector3 axis, pQuat q);
void quat_from_euler(pQuat out, pVector3 eul);
void euler_from_quat(pVector3 out, pQuat q);
void mat3_from_quat(pMatrix3 m, pQuat q);
void mat4_from_quat(pMatrix4 m, pQuat q);

Sprich: Erst mal aus einer Orientierung einen Quat machen, dann die 
(Delta-) Rotationen mit quatmulquat kombinieren, und bei Bedarf dann auf 
die nötigen Grössen zurückrechnen.
Den Quat-Code kann ich dir auch schicken (C/C++).

Grüsse,

- Strubi

[Julian B. (julian220)]

Hallo Srubi,

super vielen Dank. In der Zwischenzeit hab ich den ganzen Abend damit
zugebraucht das hier nachzubauen
http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/program/index.htm
hat auch soweit geklappt, (wenn es interessiert, poste ich gerne)
Quaternionen gibts da auch :)

ähm, wo ich jetzt überhaupt schlau drau's werde ist der Weg
insgesammt.

Ich mit Gyrowerten (Nick -45 Grad, Roll 12 Grad, Gier +X etc.) hab ich
vermutlich "Axis Angle", die ich vermutlich
in Euler, Matrix und Quaternionen etc. umwandeln kann.
Hoffe mal das mache ich oben schon mit Euler.

Am Ende brauch ich auch wieder die Gyrowerte, nur eben
in richtig verrechnet, bei Rotationen (Gier).

Wäre das dann
- Meine Gyrowerte (Nick, Roll, Gier) sind die "Axis Angle"
- "Axis Angle" in eine Quaternion z.B.
   http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
- Die Quaternion dreht das ganze (???)
- Aus der Quaternion dann wieder die "Axis Angle"  z.B.
   http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm

Das Ganze 300x pro Sekunde (habe einen STM32 72Mhz). Vielleicht
schafft der das.

Quad Code wäre natürlich super. Stabil sollte es werden.

Vielen Dank & Viele Grüße
Julian

[Martin S. (strubi)]

Hi,

hab mal den Source-Teil angehaengt (Unix-LF-Format).
Aus den Anfangs-Eulerwinkeln im Erde-System machste deinen 
Urzustand-Quat, und die roll-nick-gear (oder roll-yaw-pitch)-Achsen 
berechnest du jeweils aus der momentanen Orientierung Q(t). Diese ganzen 
Delta-Rotationen um die verschiedenen Achsen kombinierst du per 
Multiplikation.
Das ganze ist womoeglich nicht doll effizient wegen einiger redundanter 
Multiplikationen, sollte aber nicht mords ins Gewicht fallen.
Gegenüber der ganzen Euler-Geschichte sparst Du dir sowieso die 
eventuellen Umrechnung in lokales ("Flugzeug") und globals ("Erde") 
Koo-system.

Gruss,

- Strubi

[Julian B. (julian220)]

Hallo Strubi,

super Vielen Dank für Deine Hilfe, so wild siehts mit Quad
ja garnicht aus :) Ich werds mal im Detail durchgehen.

Viele Grüße
Julian

[Julian B. (julian220)]

Super es funktioniert :) Nochmal ganz herzlichen Dank.

Nur leider seh ich da gearde ein konzeptionelles Problem,
was ich noch nicht ganz verstehe.

Mit Quads ist der Anzeigenbereich für

Nick +- 90 Grad
Roll +-180 Grad
Gier +-180 Grad

Sprich, wenn ich z.B. einen Vorwärts Looping vorhabe (Nick)
springt so bei Nick +90 Grad - Roll und Gier auf -180 Grad,
wenn das IMU dann ganz auf dem Rücken liegt ist Nick dann
wieder 0 Grad (geht von +90 Grad langsam zurück) und Gier
und Roll bleibt bei -180 Grad.

Verhalten müsste richtig sein, bei Xsense.com ist
der Bereich auch so angegeben.

Aber :). Das ganze soll mal eine Flugsteuerung werden,
(RC Modell), wenn ich den Flieger steil nach unten
ziehe (Sturzflug), würde sich durch diesen Effekt
eine Rolle und eine Volldrehung einschleichen,
automatisch.

Gibt es vielleicht eine Möglichkeit (Algorithmus)
die Steuersignal entsprechend umzurechnen, oder Nick
Roll und Gier zu verändern, das das ausbleiben würde?

Vielen Dank
Julian

[Julian B. (julian220)]

Hallo,

ich hab nochmal ein Beispiel gefunden, welche Angaben ich aus
der Quaternion benötige

http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToAngle/program/index.htm

Hier werden Winkel "Angles" angeben um den "Axis Angle" zu
berechnen.

Ich bräuchte aus der Quaternion die "Angles", Heading,
Attitude, Bank +-360 Grad. Das wäre perfekt.

Ist das möglich?
(Wie wild gegoogelt aber gibt nur
Quaternion > Axis Angle
Quaternion > Euler Angles
....
etc.)

Vielen Dank & Viele Grüße
Julian

[Dibblinsch (Gast)]

Vielleicht sei hier ein Hinweis gestattet.
Richtig vertiefen will ich mich in die Lösung dieser Fragestellung 
jedoch nicht.
Ermittelt man eulersche Winkel aus einer Rotationsmatrix oder eines 
Quaternion, so geben diese lediglich an, durch welche Abfolge von 
Drehungen die entsprechende Rotation erreicht werden kann.
Die Lösung ist jedoch keineswegs eindeutig. D.h. es gibt in der Regel 
viele Lösungen. Dies leuchtet schon insoweit ein, als jeweils eine 
Drehung um weitere 360° am Ergebnis nichts ändert.
Auch sind "eulersche Winkel" nicht gleich "eulersche Winkel", will 
sagen, dass es mehrere Varianten hinsichtlich der Abfolge der Drehachsen 
gibt.
z.B. Z-Y'-Z' oder X-Y-Z usw.
Wichtig ist jeweils die richtige Abfolge der Drehungen. D.h. Jede 
Drehung muss um die einzelne, bestimmte Achse vollendet werden, bevor um 
die nächste Achse gedreht wird, um die gegebene räumliche Lage zu 
erreichen.
Insoweit erscheint mir der Ansatz hier nicht ganz passend.

Veranschaulichung:
http://www.itp.tu-berlin.de/menue/lehre/owl/mechanik/euler_winkel/