Quaternions pour l'orientation 3D

1. Introduction

Les quaternions sont une extension des nombres complexes utilisés pour représenter les rotations dans l'espace 3D. Ils offrent une alternative aux angles d'Euler, sans leurs inconvénients.

Applications

Navigation inertielle (IMU, drones, satellites)
Robotique (bras manipulateurs, robots mobiles)
Réalité virtuelle et jeux vidéo
Animation 3D
Contrôle d'attitude de véhicules

2. Définition mathématique

Un quaternion est un nombre hypercomplexe à 4 composantes :

\[q = w + xi + yj + zk\]

où :

$w$ est la partie scalaire (réelle)
$x, y, z$ sont les parties vectorielles (imaginaires)
$i, j, k$ sont les unités imaginaires

Notation vectorielle :

\[q = \begin{bmatrix} w \\ x \\ y \\ z \end{bmatrix} = \begin{bmatrix} q_w \\ \vec{q}_v \end{bmatrix}\]

avec $\vec{q}_v = [x, y, z]^T$ la partie vectorielle.

Propriétés des unités imaginaires

\[i^2 = j^2 = k^2 = ijk = -1\]

\[ij = k, \quad jk = i, \quad ki = j\]

\[ji = -k, \quad kj = -i, \quad ik = -j\]

3. Quaternion unitaire pour les rotations

Pour représenter une rotation, on utilise un quaternion unitaire (ou quaternion de rotation) :

\[||q|| = \sqrt{w^2 + x^2 + y^2 + z^2} = 1\]

Représentation axe-angle

Une rotation d'angle $\theta$ autour d'un axe unitaire $\vec{u} = [u_x, u_y, u_z]^T$ est représentée par :

\[q = \begin{bmatrix} \cos\left(\frac{\theta}{2}\right) \\ u_x \sin\left(\frac{\theta}{2}\right) \\ u_y \sin\left(\frac{\theta}{2}\right) \\ u_z \sin\left(\frac{\theta}{2}\right) \end{bmatrix}\]

Exemple : Rotation de 90° autour de l'axe Z

\[\vec{u} = [0, 0, 1]^T, \quad \theta = \frac{\pi}{2}\]

\[q = \begin{bmatrix} \cos(45°) \\ 0 \\ 0 \\ \sin(45°) \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{2}}{2} \\ 0 \\ 0 \\ \frac{\sqrt{2}}{2} \end{bmatrix}\]

4. Opérations sur les quaternions

Multiplication (composition de rotations)

\[q_1 \otimes q_2 = \begin{bmatrix} w_1w_2 - x_1x_2 - y_1y_2 - z_1z_2 \\ w_1x_2 + x_1w_2 + y_1z_2 - z_1y_2 \\ w_1y_2 - x_1z_2 + y_1w_2 + z_1x_2 \\ w_1z_2 + x_1y_2 - y_1x_2 + z_1w_2 \end{bmatrix}\]

⚠️ Non commutative : $q_1 \otimes q_2 \neq q_2 \otimes q_1$

Conjugué

\[q^* = \begin{bmatrix} w \\ -x \\ -y \\ -z \end{bmatrix}\]

Pour un quaternion unitaire : $q^* = q^{-1}$ (rotation inverse)

Norme

\[||q|| = \sqrt{w^2 + x^2 + y^2 + z^2}\]

Normalisation

\[q_{norm} = \frac{q}{||q||}\]

Rotation d'un vecteur

Pour faire tourner un vecteur $\vec{v} = [v_x, v_y, v_z]^T$ par le quaternion $q$ :

Représenter $\vec{v}$ comme quaternion pur : $p = [0, v_x, v_y, v_z]^T$
Calculer : $p' = q \otimes p \otimes q^*$
Extraire la partie vectorielle de $p'$

\[\vec{v}' = q \otimes \vec{v} \otimes q^*\]

5. Angles d'Euler vs Quaternions

Angles d'Euler

Représentation par 3 angles : roulis ($\phi$), tangage ($\theta$), lacet ($\psi$)

Avantages :

Intuitifs (3 paramètres)
Faciles à visualiser

Inconvénients :

❌ Gimbal lock : perte d'un degré de liberté quand deux axes s'alignent
❌ Singularités mathématiques
❌ Interpolation non linéaire
❌ Ordre des rotations important

Quaternions

Avantages :

✅ Pas de gimbal lock
✅ Pas de singularités
✅ Interpolation linéaire (SLERP)
✅ Calculs plus rapides (moins d'opérations trigonométriques)
✅ Stabilité numérique

Inconvénients :

Moins intuitifs (4 paramètres)
Nécessite normalisation régulière

6. Le problème du Gimbal Lock

Qu'est-ce que le Gimbal Lock ?

Le gimbal lock survient quand deux des trois axes de rotation s'alignent, causant :

Perte d'un degré de liberté
Impossibilité de représenter certaines rotations
Comportements erratiques dans les animations/contrôles

Exemple classique : Tangage de 90° ($\theta = \pm 90°$)

Quand $\theta = 90°$ :

\[\cos(\theta) = 0\]

Les rotations de roulis et lacet deviennent équivalentes → perte d'un axe

Pourquoi les quaternions évitent le Gimbal Lock ?

Les quaternions représentent la rotation comme un point sur une hypersphère 4D. Il n'y a pas de configuration singulière car :

Pas de division par zéro
Pas de discontinuité
Représentation continue de toutes les orientations

7. Conversions

Angles d'Euler → Quaternion

Convention : Roulis ($\phi$), Tangage ($\theta$), Lacet ($\psi$) - ordre ZYX

\[q_w = \cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\]

\[q_x = \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) - \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\]

\[q_y = \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\]

\[q_z = \cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) - \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\]

Code Python :

import numpy as np

def euler_to_quaternion(roll, pitch, yaw):
    """Convertit angles d'Euler (rad) en quaternion"""
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)
    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)

    qw = cr * cp * cy + sr * sp * sy
    qx = sr * cp * cy - cr * sp * sy
    qy = cr * sp * cy + sr * cp * sy
    qz = cr * cp * sy - sr * sp * cy

    return np.array([qw, qx, qy, qz])

Quaternion → Angles d'Euler

\[\phi = \text{atan2}\left(2(q_wq_x + q_yq_z), 1 - 2(q_x^2 + q_y^2)\right)\]

\[\theta = \text{asin}\left(2(q_wq_y - q_zq_x)\right)\]

\[\psi = \text{atan2}\left(2(q_wq_z + q_xq_y), 1 - 2(q_y^2 + q_z^2)\right)\]

⚠️ Attention au gimbal lock : Si $|q_wq_y - q_zq_x| \approx 0.5$, on est proche d'une singularité.

Code Python :

def quaternion_to_euler(q):
    """Convertit quaternion en angles d'Euler (rad)"""
    qw, qx, qy, qz = q

    # Roll (phi)
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx**2 + qy**2)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    # Pitch (theta)
    sinp = 2 * (qw * qy - qz * qx)
    if abs(sinp) >= 1:
        pitch = np.copysign(np.pi / 2, sinp)  # Gimbal lock
    else:
        pitch = np.arcsin(sinp)

    # Yaw (psi)
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy**2 + qz**2)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

8. Interpolation : SLERP

Pour interpoler entre deux orientations $q_1$ et $q_2$ avec un paramètre $t \in [0,1]$ :

\[\text{SLERP}(q_1, q_2, t) = \frac{\sin((1-t)\Omega)}{\sin(\Omega)}q_1 + \frac{\sin(t\Omega)}{\sin(\Omega)}q_2\]

où $\Omega = \arccos(q_1 \cdot q_2)$ est l'angle entre les quaternions.

Propriétés :

Interpolation à vitesse angulaire constante
Chemin le plus court sur la sphère
Essentiel pour les animations fluides

Code Python :

def slerp(q1, q2, t):
    """Interpolation sphérique entre q1 et q2"""
    dot = np.dot(q1, q2)

    # Si dot < 0, inverser q2 pour prendre le chemin le plus court
    if dot < 0.0:
        q2 = -q2
        dot = -dot

    # Si quaternions très proches, interpolation linéaire
    if dot > 0.9995:
        result = q1 + t * (q2 - q1)
        return result / np.linalg.norm(result)

    # SLERP
    theta = np.arccos(dot)
    sin_theta = np.sin(theta)

    w1 = np.sin((1 - t) * theta) / sin_theta
    w2 = np.sin(t * theta) / sin_theta

    return w1 * q1 + w2 * q2

9. Intégration pour IMU/Gyroscope

Pour mettre à jour l'orientation à partir de mesures gyroscopiques $\vec{\omega} = [\omega_x, \omega_y, \omega_z]^T$ :

\[\dot{q} = \frac{1}{2} q \otimes \begin{bmatrix} 0 \\ \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}\]

Discrétisation (Euler explicite) :

\[q_{k+1} = q_k + \frac{T_e}{2} q_k \otimes \begin{bmatrix} 0 \\ \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}\]

Puis normaliser : $q_{k+1} = \frac{q_{k+1}}{||q_{k+1}||}$

Code Python :

def integrate_gyro(q, omega, dt):
    """Intègre les mesures gyro pour mettre à jour le quaternion"""
    # Quaternion de vitesse angulaire
    q_omega = np.array([0, omega[0], omega[1], omega[2]])

    # Dérivée du quaternion
    q_dot = 0.5 * quaternion_multiply(q, q_omega)

    # Intégration d'Euler
    q_new = q + q_dot * dt

    # Normalisation
    return q_new / np.linalg.norm(q_new)

10. Résumé pratique

Quand utiliser les quaternions ?

✅ Oui :

Navigation 3D (drones, robots, satellites)
Animation et interpolation d'orientations
Filtrage d'attitude (Kalman, complémentaire)
Contrôle d'orientation en temps réel

❌ Non (angles d'Euler suffisent) :

Rotations 2D
Affichage pour l'utilisateur (convertir en Euler)
Rotations simples autour d'un seul axe

Propriétés essentielles à retenir

Quaternion unitaire : $||q|| = 1$ pour les rotations
Conjugué = Inverse : $q^* = q^{-1}$ si $||q|| = 1$
Composition : $q_{total} = q_2 \otimes q_1$ (ordre inverse !)
Normalisation régulière : Évite l'accumulation d'erreurs numériques
Pas de gimbal lock : Toutes les orientations sont représentables

Formules clés

Opération	Formule
Rotation axe-angle	$q = [\cos(\theta/2), \vec{u}\sin(\theta/2)]$
Rotation vecteur	$\vec{v}' = q \otimes \vec{v} \otimes q^*$
Composition	$q_{total} = q_2 \otimes q_1$
Inverse	$q^{-1} = q^* /
Intégration gyro	$\dot{q} = \frac{1}{2}q \otimes [0, \vec{\omega}]$

11. Exemple complet : Filtre complémentaire

Fusion accéléromètre + gyroscope pour estimer l'attitude :

def complementary_filter(q, accel, gyro, dt, alpha=0.98):
    """
    Filtre complémentaire pour fusion IMU
    q: quaternion actuel
    accel: [ax, ay, az] accélération (m/s²)
    gyro: [wx, wy, wz] vitesse angulaire (rad/s)
    dt: période d'échantillonnage (s)
    alpha: coefficient du filtre (0.95-0.99)
    """
    # 1. Intégration gyroscope (haute fréquence)
    q_gyro = integrate_gyro(q, gyro, dt)

    # 2. Estimation depuis accéléromètre (basse fréquence)
    # Calcul du roulis et tangage depuis la gravité
    roll_acc = np.arctan2(accel[1], accel[2])
    pitch_acc = np.arctan2(-accel[0], 
                           np.sqrt(accel[1]**2 + accel[2]**2))
    q_acc = euler_to_quaternion(roll_acc, pitch_acc, 0)

    # 3. Fusion complémentaire
    q_fused = slerp(q_acc, q_gyro, alpha)

    return q_fused / np.linalg.norm(q_fused)

Ce filtre combine :

Gyroscope : Précis à court terme, dérive à long terme
Accéléromètre : Bruité à court terme, fiable à long terme

Opération	Formule
Rotation axe-angle	\(q = [\cos(\theta/2), \vec{u}\sin(\theta/2)]\)
Rotation vecteur	\(\vec{v}' = q \otimes \vec{v} \otimes q^*\)
Composition	\(q_{total} = q_2 \otimes q_1\)
Inverse	$q^{-1} = q^* /
Intégration gyro	\(\dot{q} = \frac{1}{2}q \otimes [0, \vec{\omega}]\)