How to avoid gimbal with Quaternions

Question

I'am working with an LSM6DSO32, so I'am starting with Kalman filter, everything works but with Euler angle I got gimbal lock I think... But if my pitch angle approach to 90° my roll angle jump and I get a bad value and my pitch is like blocked around 90°.

So I searched and I foud this phenomenon "Gimbal lock" after some other researches I have read I should use Quaternions to avoid this Gimbal lock. I found this code that implement MadgwickAHRS algo who use Quaternions here

But if I use computeAngles, this function return Euler angles I have again Gimbal problem. So my question is, how can I only use Quaternions without Euler transformation to compute my roll and pitch angles? My objective is to know if my roll or my pitch angles has move to 90 or -90° aboyt my start position. For example, if my start position is roll 20° and pitch 10°, I would like to detect if my roll is 110° or -70° and if my pitch is 100° or -80°

How can I solve my problem with only q0, q1, q2, q3 values ?

I found this article here I've tried the solution for gimbal lock, but when I set my roll to 0 and compute yaw with 2atan2(q1, q0) for pitch equals PI/2 the value I got isn't good, yaw move a lot...

Answer 1

Quaternions are a more efficient way of storing the orientation matrix of a frame.

I use the vector-scalar convention for quaternions (3+1 = 4 quantities) and have defined the following utility functions

• Quaternion definition $$q=\begin{pmatrix}\vec{v}\\ s \end{pmatrix}$$
• Quatenion from rotation axis $\hat{z}$ and angle $\theta$ $$q=\begin{pmatrix}\hat{z}\sin\left(\frac{\theta}{2}\right)\\ \cos\left(\frac{\theta}{2}\right) \end{pmatrix}$$
• Identity quaternion $$q=\begin{pmatrix}\vec{0}\\ 1 \end{pmatrix}$$
• Magnitude $$ \|q \| = \sqrt{ \vec{v} \cdot \vec{v} + s^2 }$$
• Unit quaternion (to represent a rotation) $$ {q} = \frac{1}{\sqrt{ \vec{v} \cdot \vec{v} + s^2 }} \begin{pmatrix} \vec{v}\\ s \end{pmatrix}$$
• Inverse quaternion $$q^{-1} = \frac{1}{\vec{v} \cdot \vec{v} + s^2}\begin{pmatrix}-\vec{v}\\ s \end{pmatrix}$$
• Sequence of two rotations $$q_1 q_2 = \begin{pmatrix}s_{1}\vec{v}_{2}+s_{2}\vec{v}_{1}+\vec{v}_{1}\times\vec{v}_{2}\\ s_{1}s_{2}-\vec{v}_{1}\cdot\vec{v}_{2} \end{pmatrix}$$
• Transform vector $\vec{p}$ by quatertion rotation $q$ $$ \vec{p}' = \vec{p}+2s\left(\vec{v}\times\vec{p}\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}\right)\right)$$
• Inverse Transform vector $\vec{p}'$ by quatertion rotation $q$ $$ \vec{p} = \vec{p}' - 2s\left(\vec{v}\times\vec{p}'\right)+2\left(\vec{v}\times\left(\vec{v}\times\vec{p}'\right)\right)$$
• Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}=\mathbf{1}+2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$ _{where $[\vec{v}\times] = \begin{vmatrix}0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{vmatrix} $ is the skew symmetric cross product operator matrix.}
• Inverse Rodrigues 3×3 rotation matrix from quatenion $$\mathbf{R}^\top=\mathbf{1}-2s[\vec{v}\times]+2[\vec{v}\times][\vec{v}\times]$$
• Quaternion from 3×3 rotation matrix $\mathbf{R}$ $$\begin{aligned}s & =\frac{1}{2}\sqrt{\frac{\left(\mathbf{R}_{32}-\mathbf{R}_{23}\right)^{2}+\left(\mathbf{R}_{13}-\mathbf{R}_{31}\right)^{2}+\left(\mathbf{R}_{21}-\mathbf{R}_{12}\right)^{2}}{3-\mathbf{R}_{11}-\mathbf{R}_{22}-\mathbf{R}_{33}}}\\ \vec{v} & =\frac{1}{4s}\begin{pmatrix}\mathbf{R}_{32}-\mathbf{R}_{23}\\ \mathbf{R}_{13}-\mathbf{R}_{31}\\ \mathbf{R}_{21}-\mathbf{R}_{12} \end{pmatrix} \end{aligned}$$
• Time derivative of quaternion subject to rotational velocity vector $\vec{\omega}$ $$ \dot{q} = \frac{1}{2}\begin{pmatrix}s\vec{\omega}+\vec{\omega}\times\vec{v}\\ -\vec{\omega} \cdot \vec{v} \end{pmatrix}$$ _{Remeber $\vec{v}$ is the vector part of the quaternion and not a velocity vector.}
• Rotation velocity vector from quaternion and its time derivative $$\begin{pmatrix}\vec{\omega}\\ 0 \end{pmatrix} = 2\begin{pmatrix}s\dot{\vec{v}}-\vec{v}\dot{s}+\vec{v}\times\dot{\vec{v}}\\ s\dot{s}+\vec{v} \cdot \dot{\vec{v}} \end{pmatrix}$$

All of the above functions can be used in the simulation and modeling of rigid bodies by keeping track of each body's orientation $q$ in the configuration space, and doing a time integration with $q \rightarrow q + \dot{q} \Delta t$, just like you would integrate position $\vec{r} \rightarrow \vec{r} + \dot{\vec{r}} \Delta t$

No Euler angles needed for the model. If you need to extract Euler angles, then convert the quaternion into a rotation matrix, and extract from there.

Answer 2

If you define your tested conditions in terms of euler angles, you're going to have to use euler angles. I would suggest finding a way to reframe your question not in terms of euler angles but by the quantities of change.

It's not clear what you mean by q0 - q4 are those the individual values of one quaternion, or 4 different quaternions?

It seems like you should have an initial quaternion and a final quaternion. You can then compute the difference, and likely provide logic that thresholds on properties of that resultant quaternion. Such as the resultant angle is 90 degrees from the original. Or maybe the angle around a specific axis like asked here