Hallo,
ich wandel mit folgendem Code Euler-Winkel in ein Gesamt-Quaternion um:
1 | void eul_to_quat(float* roll, float* pitch, float* yaw, QUATERNION* q)
|
2 | {
|
3 | c_roll = cos(*roll / 2);
|
4 | c_pitch = cos(*pitch / 2);
|
5 | c_yaw = cos(*yaw / 2);
|
6 |
|
7 | s_roll = sin(*roll / 2);
|
8 | s_pitch = sin(*pitch / 2);
|
9 | s_yaw = sin(*yaw / 2);
|
10 |
|
11 | c_yaw_pitch = c_yaw * c_pitch;
|
12 | s_yaw_pitch = s_yaw * s_pitch;
|
13 |
|
14 | q->a = c_yaw_pitch * c_roll + s_yaw_pitch * s_roll;
|
15 | q->b = c_yaw_pitch * s_roll - s_yaw_pitch * c_roll;
|
16 | q->c = c_yaw * s_pitch * c_roll + s_yaw * c_pitch * s_roll;
|
17 | q->d = s_yaw * c_pitch * c_roll - c_yaw * s_pitch * s_roll;
|
18 | }
|
Nun möchte ich das berechnete Gesamt-Quaternion mit folgendem Code
wieder in Euler-Winkel umrechnen:
1 | uint8 quat_to_eul(float* roll, float* pitch, float* yaw, QUATERNION* q)
|
2 | {
|
3 | QUATERNION q_norm;
|
4 | float a_2, b_2, c_2, d_2;
|
5 |
|
6 | if(!norm_q(q, &q_norm))
|
7 | return FALSE;
|
8 |
|
9 | a_2 = q_norm.a * q_norm.a;
|
10 | b_2 = q_norm.b * q_norm.b;
|
11 | c_2 = q_norm.c * q_norm.c;
|
12 | d_2 = q_norm.d * q_norm.d;
|
13 |
|
14 | *yaw = atan2(2 * (q_norm.b * q_norm.c + q_norm.a * q_norm.d), a_2 + b_2 - c_2 - d_2);
|
15 | *pitch = asin(2 * (q_norm.a * q_norm.c - q_norm.b * q_norm.d));
|
16 | *roll = atan2(2 * (q_norm.c * q_norm.d + q_norm.a * q_norm.b), a_2 - b_2 - c_2 + d_2);
|
17 |
|
18 | return TRUE;
|
19 | }
|
Wenn ich nun die Euler-Winkel x(phi) = 0, y(teta) = 0, z(psi) = 179°
wähle bekomme ich nach der Transformation in Quaternion und wieder
zurück zu Euler-Winkeln die gleichen Werte raus. Wenn ich jedoch für psi
181° oder größer wähle sind die Werte nicht mehr korrekt. Das gleiche
gillt für den phi Winkel. Wird dieser größer als 179° gewählt, wird er
nicht mehr korrekt von Quaternion zu Euler zurück gerechnet.
Bei einem teta(nicken) Winkel von 90° und größer hab ich das gleiche
Problem. Wobei hier wahrscheinlich der Gimbal Lock das Problem ist.
Vielleicht zur Info. Als Rotationsvorschrift habe ich die ZYX Vorschrift
gewählt. Das heißt zuerst wird um die Z-Achse gedreht(Gier-Winkel), dann
um die Y-Achse(Nick-Winkel) und dann um die X-Achse(Roll-Winkel).
Die Frage ist, wie gehe ich mit diesen falschen Umrechnungen um? Gibt es
Grenzwerte die ich bei der Umrechnung gesondert behandeln muss? Ich
stehe da gerade ordentlich auf dem Schlauch.
