Overcome Gimbal Lock?

Question

My question is : When I rotate around x and y axis, yaw and pitch, how can I determine the sometimes resulting z axis rotation, the roll, caused by the Gimbal Lock ? Could you please give me any formula ? Another question would be : If I would enable x-axis rotation to go from -90 to 90 degrees, wont I have the Gimbal Lock then ?

Answer 1

This is not an answer, only some advice on how to utilize versors; unit quaternions that represent rotations in three dimensions.

---

1. In computer graphics, both 3×3 and 4×4 matrices are used to represent 3D transforms.

   Point $\vec{p} = \left [ \matrix { x \\ y \\ z } \right ]$ is transformed by matrix $\mathbf{M} = \left [ \matrix { x_x & y_x & z_x \\ x_y & y_y & z_y \\ x_z & y_z & z_z } \right ] $ via multiplication:

   $$\vec{p}' = \mathbf{M} \vec{p}$$

   which means that

   $$\begin{cases} x' = x \; x_x + y \; y_x + z \; z_x \\ y' = x \; x_y + y \; y_y + z \; z_y \\ z' = x \; x_z + y \; y_z + z \; z_z \end{cases}$$

   Note that the three vectors, $\hat{e}_x = \left [ \matrix { x_x \\ x_y \\ x_z } \right ]$, $\hat{e}_y = \left [ \matrix { y_x \\ y_y \\ y_z } \right ]$, and $\hat{e}_z = \left [ \matrix { z_x \\ z_y \\ z_z } \right ]$, are also the unit vectors in the rotated coordinate system.

   Because pure rotation matrixes are orthogonal, the inverse of $\mathbf{M}$ is its transpose, i.e. $\mathbf{M}^{-1} = \mathbf{M}^{T} = \left [ \matrix { x_x & x_y & x_z \\ y_x & y_y & y_z \\ z_x & z_y & z_x } \right ]$.

   If 4×4 matrices are used, then points are implicitly $\vec{p} = \left [ \matrix { x \\ y \\ z \\ 1 } \right ]$, and the transformation matrix is $\mathbf{M} = \left [ \matrix { x_x & y_x & z_x & t_x \\ x_y & y_y & z_y & t_y \\ x_z & y_z & z_z & t_z \\ 0 & 0 & 1} \right ] $, where the vector $\vec{T} = \left [ \matrix { t_x \\ t_y \\ t_z } \right ]$ is translation after rotation.

   I've shown here how to use the translation part $\vec{T}$ to define the matrix as a rotation around some other point than origin, and how to combine translation before and after the rotation into $\vec{T}$.

   In the 4×4 matrix case, very little changes programming-wise:

   $$\begin{cases} x' = x \; x_x + y \; y_x + z \; z_x + t_x \\ y' = x \; x_y + y \; y_y + z \; z_y + t_y \\ z' = x \; x_z + y \; y_z + z \; z_z + t_z \end{cases}$$

   Note that the order in which the matrix elements are stored, varies. Multidimensional arrays in Fortran use column-major order, whereas in C they use row-major order. If you use a 3D toolkit, check its documentation to verify.

---

2. Quaternion $q = (w, x, y, z)$ is an unit quaternion or a versor, if $w^2 + x^2 + y^2 + z^2 = 1$.

   The versor $q$ corresponds to a rotation matrix $\mathbf{M}$, $$\mathbf{M} = \left[\begin{matrix} 1 - 2 (y^2 + z^2) & 2 (x y - z w) & 2 (x z + y w) \\ 2 (x y + z w) & 1 - 2 (x^2 + z^2) & 2 (y z - x w) \\ 2 (x z - y w) & 2 (y z + x w) & 1 - 2 (x^2 + y^2) \end{matrix}\right]$$

---

3. Versor $q$ also corresponds to a rotation around an axis.

   Let $\hat{a} = (a_x, a_y, a_z)$ be the axis, and its norm 1, i.e. $a_x^2 + a_y^2 + a_z^2 = 1$. Let $\theta$ be the rotation around that axis. Then,

   $$\begin{cases} w = \cos\frac{\theta}{2} \\ x = a_x \sin\frac{\theta}{2} \\ y = a_y \sin\frac{\theta}{2} \\ z = a_z \sin\frac{\theta}{2} \end{cases}$$

---

4. To avoid numerical errors, you can always normalize the quaternion.

   This is a safe operation (does not introduce any bias in any direction), and is very useful when multiplying quaternions many times using e.g. floating-point representation.

   First, calculate $n = \sqrt{w^2 + x^2 + y^2 + z^2}$. Then, $$\begin{cases} w' = \frac{w}{n} \\ x' = \frac{x}{n} \\ y' = \frac{y}{n} \\ z' = \frac{z}{n} \end{cases}$$ where $q' = (w', x', y', z')$ is the normalized versor.

   Basically, if you write a program that uses versors, you do this every set of operations, to avoid numerical errors from creeping in.

---

5. You can multiply two versors to combine the rotations. If $q_0 = (w_0, x_0, y_0, z_0)$ and $q_1 = (w_1, x_1, y_1, z_1)$, then rotating by $q_0$ and then by $q_1$ is described by $$q = q_0 q_1 = \begin{cases} w = w_0 w_1 - x_0 x_1 - y_0 y_1 - z_0 z_1 \\ x = w_0 x_1 + x_0 w_1 + y_0 z_1 - z_0 y_1 \\ y = w_0 y_1 - x_0 z_1 + y_0 w_1 + z_0 x_1 \\ z = w_0 z_1 + x_0 y_1 - y_0 x_1 + z_0 w_1 \end{cases}$$

---

6. You can interpolate between two versors, by scaling and adding them together, and normalizing the result (as in step 4. above).

   For example, if you have $0 \le p \le 1$ describing "phase", or state between rotations $q_0$ and $q_1$, then

   $$\begin{cases} w' = (1-p) w_0 + p w_1 \\ x' = (1-p) x_0 + p x_1 \\ y' = (1-p) y_0 + p y_1 \\ z' = (1-p) z_0 + p z_1\end{cases}$$

   and with $n' = \sqrt{w'^2 + x'^2 + y'^2 + z'^2}$, you can compute the desired rotation with $w = w'/n'$, $x = x'/n'$, $y = y'/n'$, and $z = z'/n'$.

---

6. Rudders, ailerons, and attitude thrusters, can be represented by the axis and angle they try to rotate the vessel around.

   The axis does not change (unless the attitude control element suffers damage), but the angle is dependent on both the force and the duration.

   For example, consider a fixed wing aircraft with elevators (controlling pitch), ailerons (controlling roll) and a rudder (controlling yaw).

   To simulate their effect on the current orientation of the plane, a programmer would have one quaternion ($q$) representing the orientation of the plane compared to some fixed reference (world coordinates), as well as three variables describing the state of the three control surfaces ($p$, $r$, $a$, say all within -1 and +1).

   To update the orientation, we need to know how long the state has been applied; i.e. the length of the time step, $\Delta t$. We also need to know how large a rotation (in radians), in one time unit, each control surface can affect (say, $C_p$, $C_r$, $C_a$). Then, we can calculate $\theta_p = p \; C_p \; \Delta t$, $\theta_r = r \; C_r \; \Delta t$, and $\theta_a = a \; C_a \; \Delta t$.

   With these, the programmer can use point 3. above to calculate the versor representing each of the three effects (the axis being the rotation axis the control surface rotates the aircraft around).

   The new orientation is obtained by multiplying the current orientation versor by the three control surface versors, and normalizing the result (step 4. above).

   Note that here, we successively apply the rotation caused by each control surface. If the time steps (and thus changes in orientation) are small -- and you do need them to be small, in any kind of simulation! --, this works just fine. Mathematically speaking, the order in which we multiply the quaternions matters: the first one is applied to the old orientation, but the second one is applied to the resulting orientation -- as if the first control surface affected the second (and both affect the third). This has nothing to do with quaternions per se, but only on how we model the effect of the control surfaces to the current orientation.