Chapitre 12
Cinématique inverse

1. Définition et difficultés

La cinématique directe (FK, chapitre 11) part des angles articulaires θ = (θ₁, …, θ_n) et calcule la pose de l'effecteur x = f(θ) dans l'espace cartésien. La cinématique inverse (IK) résout le problème réciproque : étant donné une pose cible x_c, trouver les angles θ tels que f(θ) = x_c.

C'est l'IK qu'on utilise en pratique : on veut placer une pince à un endroit précis, on ne connaît pas d'avance les angles moteurs. Mais l'IK est fondamentalement plus dure que la FK :

• Solutions multiples. Plusieurs configurations articulaires atteignent la même pose (coude « haut » et coude « bas », rotations à 2π près). f n'est pas injective.
• Pas toujours de solution. Si la cible est hors de l'espace de travail (trop loin, trop près), f⁻¹ n'existe pas.
• Singularités. À certaines configurations, le bras perd des degrés de liberté instantanés : le problème devient mal conditionné et les vitesses articulaires explosent.
• Non-linéarité. f contient des sinus/cosinus ; il n'existe pas de formule fermée pour un bras arbitraire à 6+ axes.

Deux familles de méthodes : les solutions analytiques (formules fermées, exactes, rapides, mais spécifiques à une géométrie) et les solutions numériques (itératives, générales, basées sur la jacobienne).

2. Solution analytique du bras planaire 2R

Considérons un bras plan à deux articulations rotoïdes (2R), de longueurs de segments L₁ et L₂, avec les angles θ₁ (base) et θ₂ (coude). La cinématique directe donne la position de l'effecteur :

2.1 Résolution de θ₂

En additionnant les carrés de x et y et en utilisant cos²+sin²=1 ainsi que cos(a−b)=cos a cos b + sin a sin b, les termes croisés se simplifient en cos θ₂ :

D'où l'expression fermée du cosinus de l'angle du coude :

La cible est atteignable si et seulement si −1 ≤ cos θ₂ ≤ 1, c'est-à-dire si la distance radiale r = √(x²+y²) vérifie |L₁−L₂| ≤ r ≤ L₁+L₂. L'espace de travail est un anneau (couronne) de rayon extérieur L₁+L₂ et de rayon intérieur |L₁−L₂|.

Il en découle deux solutions symétriques (le signe du sinus n'est pas déterminé par le cosinus) :

• θ₂ > 0 : configuration coude bas (une des deux « équerres »).
• θ₂ < 0 : configuration coude haut, image miroir par rapport à la ligne base→effecteur.

Les deux atteignent exactement la même pose (x, y) : c'est l'illustration concrète de la non-unicité de l'IK. En bordure d'espace de travail (r = L₁+L₂, bras tendu), cos θ₂ = 1, les deux solutions fusionnent en θ₂=0 : c'est une singularité.

2.2 Résolution de θ₁

Une fois θ₂ connu, on écrit (x,y) sous forme x = k₁ cos θ₁ − k₂ sin θ₁, y = k₁ sin θ₁ + k₂ cos θ₁ avec k₁ = L₁ + L₂ cos θ₂ et k₂ = L₂ sin θ₂. Cela se résout proprement avec atan2 :

3. Méthode numérique générale : la jacobienne

Pour un bras quelconque (plus de 2 axes, 3D, orientations), on n'a pas de formule fermée. On linéarise la FK autour de la configuration courante. La matrice jacobienne J(θ) collecte les dérivées partielles de la pose par rapport aux angles :

où m est la dimension de la pose (2 en planaire, 6 en 3D avec orientation) et n le nombre d'articulations. Au premier ordre, un petit déplacement articulaire produit un petit déplacement cartésien :

L'IK numérique inverse cette relation. On part d'une estimation θ⁽⁰⁾, on calcule l'erreur de pose e = x_c − f(θ), on résout pour un pas Δθ qui réduit l'erreur, on met à jour, on répète.

3.1 Newton / inverse exacte (J carrée)

Si J est carrée et inversible (m = n) :

3.2 Pseudo-inverse (redondance, non carré)

Pour m ≠ n (bras redondant n > m, ou sur-contraint), on utilise la pseudo-inverse de Moore–Penrose. Dans le cas redondant on prend la pseudo-inverse à droite, qui fournit le pas de norme minimale :

3.3 Méthode de la transposée

Astuce peu coûteuse : remplacer l'inverse par la transposée mise à l'échelle. Il n'y a aucune inversion matricielle, donc pas d'explosion aux singularités, au prix d'une convergence plus lente :

On peut montrer que J^T e est une direction de descente pour ½‖e‖² tant que e ≠ 0 et J non nulle : la méthode diminue l'erreur de façon monotone pour un α assez petit.

4. Singularités et amortissement

Une singularité survient quand J perd son rang : det(J J^T) → 0. Physiquement, l'effecteur ne peut plus se déplacer instantanément dans certaines directions (bras tendu, alignement d'axes). Numériquement, (J J^T)⁻¹ explose : un déplacement cartésien minuscule exige des vitesses articulaires énormes — dangereux sur un robot réel.

La parade standard est la méthode des moindres carrés amortis (Damped Least Squares, DLS), équivalente à un pas de Levenberg–Marquardt : on ajoute un terme de régularisation λ² qui borne l'amplitude du pas près des singularités :

Loin des singularités et pour λ → 0, on retrouve la pseudo-inverse (précision maximale). Près d'une singularité, λ² I garde la matrice inversible et lisse la solution (robustesse, au prix d'une légère erreur résiduelle). En pratique on choisit λ petit et fixe, ou adaptatif selon le conditionnement de J.

5. Implémentation Rust

5.1 IK analytique du 2R

On renvoie une Option<(f64, f64)> : None si la cible est hors d'atteinte. On calcule le coude bas (θ₂ ≥ 0) ; le coude haut s'obtient en niant θ₂ et en recalculant θ₁.

/// IK analytique d'un bras planaire 2R (configuration coude bas, theta2 >= 0).
/// Renvoie `Some((theta1, theta2))` en radians, ou `None` si (x, y) est
/// hors de l'espace de travail atteignable.
fn ik_2r_analytique(x: f64, y: f64, l1: f64, l2: f64) -> Option<(f64, f64)> {
    let r2 = x * x + y * y;

    // cos(theta2) selon la loi des cosinus.
    let cos_t2 = (r2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);

    // Domaine d'atteignabilité : cos(theta2) doit rester dans [-1, 1].
    // Une petite tolerance absorbe les erreurs d'arrondi sur la frontiere.
    const EPS: f64 = 1e-9;
    if cos_t2 < -1.0 - EPS || cos_t2 > 1.0 + EPS {
        return None;
    }

    // Clamp defensif AVANT acos : sans lui, acos(1.0000000002) => NaN.
    let cos_t2 = cos_t2.clamp(-1.0, 1.0);

    // Coude bas : on prend la racine positive.
    let theta2 = cos_t2.acos();

    // theta1 = atan2(y, x) - atan2(k2, k1)
    let k1 = l1 + l2 * theta2.cos();
    let k2 = l2 * theta2.sin();
    let theta1 = y.atan2(x) - k2.atan2(k1);

    Some((theta1, theta2))
}

/// Variante renvoyant les deux solutions (coude bas, coude haut) quand elles existent.
fn ik_2r_deux_solutions(
    x: f64,
    y: f64,
    l1: f64,
    l2: f64,
) -> Option<[(f64, f64); 2]> {
    let (t1_bas, t2_bas) = ik_2r_analytique(x, y, l1, l2)?;

    // Coude haut : theta2 negatif, theta1 recalcule avec le signe oppose.
    let t2_haut = -t2_bas;
    let k1 = l1 + l2 * t2_haut.cos();
    let k2 = l2 * t2_haut.sin();
    let t1_haut = y.atan2(x) - k2.atan2(k1);

    Some([(t1_bas, t2_bas), (t1_haut, t2_haut)])
}

5.2 IK numérique générique avec nalgebra

On implémente une boucle de Newton amortie (DLS) générique en dimension 2. La jacobienne est fournie par une closure — ici la jacobienne analytique du 2R — mais on donne aussi une version par différences finies pour les cas où la dérivée exacte n'est pas disponible.

use nalgebra::{Matrix2, Vector2};

/// Cinematique directe du 2R : angles -> position (x, y).
fn fk_2r(theta: &Vector2, l1: f64, l2: f64) -> Vector2 {
    let (t1, t2) = (theta[0], theta[1]);
    Vector2::new(
        l1 * t1.cos() + l2 * (t1 + t2).cos(),
        l1 * t1.sin() + l2 * (t1 + t2).sin(),
    )
}

/// Jacobienne analytique 2x2 du 2R : J[i][j] = d(pos_i) / d(theta_j).
fn jacobienne_2r(theta: &Vector2, l1: f64, l2: f64) -> Matrix2 {
    let (t1, t2) = (theta[0], theta[1]);
    let s1 = t1.sin();
    let c1 = t1.cos();
    let s12 = (t1 + t2).sin();
    let c12 = (t1 + t2).cos();

    // dx/dt1, dx/dt2
    // dy/dt1, dy/dt2
    Matrix2::new(
        -l1 * s1 - l2 * s12, -l2 * s12,
         l1 * c1 + l2 * c12,  l2 * c12,
    )
}

/// Jacobienne par differences finies (centrees), utile quand la FK est une boite noire.
fn jacobienne_num(f: F, theta: &Vector2, h: f64) -> Matrix2
where
    F: Fn(&Vector2) -> Vector2,
{
    let mut j = Matrix2::zeros();
    for col in 0..2 {
        let mut tp = *theta;
        let mut tm = *theta;
        tp[col] += h;
        tm[col] -= h;
        let d = (f(&tp) - f(&tm)) / (2.0 * h); // colonne = d(pos) / d(theta_col)
        j.set_column(col, &d);
    }
    j
}

/// Resultat d'une resolution IK numerique.
struct IkResult {
    theta: Vector2,
    converge: bool,
    iterations: usize,
    erreur: f64,
}

/// IK numerique par moindres carres amortis (Levenberg-Marquardt / DLS).
///
/// - `cible`     : pose cartesienne visee.
/// - `theta0`    : estimation initiale (warm start).
/// - `lambda`    : facteur d'amortissement (stabilite pres des singularites).
/// - `tol`       : critere d'arret sur la norme de l'erreur de pose.
/// - `max_iter`  : borne dure sur le nombre d'iterations (temps reel).
fn ik_numerique(
    cible: Vector2,
    theta0: Vector2,
    l1: f64,
    l2: f64,
    lambda: f64,
    tol: f64,
    max_iter: usize,
) -> IkResult {
    let mut theta = theta0;
    let ident = Matrix2::identity();

    for k in 0..max_iter {
        let e = cible - fk_2r(&theta, l1, l2); // erreur de pose
        let err_norm = e.norm();

        // Critere d'arret : on est assez proche de la cible.
        if err_norm < tol {
            return IkResult { theta, converge: true, iterations: k, erreur: err_norm };
        }

        let j = jacobienne_2r(&theta, l1, l2);

        // Delta theta = J^T (J J^T + lambda^2 I)^-1 e   (DLS)
        // En dimension carree on peut aussi tenter J^-1 directement, mais
        // (J J^T + lambda^2 I) est toujours inversible : plus robuste.
        let jjt = j * j.transpose() + ident * (lambda * lambda);
        let delta = match jjt.try_inverse() {
            Some(inv) => j.transpose() * inv * e,
            None => {
                // Repli extreme : methode de la transposee (jamais singuliere).
                j.transpose() * e * 0.1
            }
        };

        theta += delta;
    }

    // Borne d'iterations atteinte : on renvoie la meilleure estimation courante.
    let erreur = (cible - fk_2r(&theta, l1, l2)).norm();
    IkResult { theta, converge: false, iterations: max_iter, erreur }
}

Exercices

fn ik_2r_analytique(x: f64, y: f64, l1: f64, l2: f64) -> Option<(f64, f64)> {
    let r2 = x * x + y * y;
    let cos_t2 = (r2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);

    // Hors d'atteinte si |cos_t2| > 1 (au-dela de la tolerance).
    if cos_t2.abs() > 1.0 + 1e-9 {
        return None;
    }

    // Clamp obligatoire avant acos (sinon NaN aux frontieres).
    let theta2 = cos_t2.clamp(-1.0, 1.0).acos(); // coude bas
    let k1 = l1 + l2 * theta2.cos();
    let k2 = l2 * theta2.sin();
    let theta1 = y.atan2(x) - k2.atan2(k1);
    Some((theta1, theta2))
}

fn main() {
    // Cible atteignable (r = sqrt(2) dans [0, 2]).
    println!("{:?}", ik_2r_analytique(1.0, 1.0, 1.0, 1.0)); // Some(...)
    // Cible trop lointaine (r = 3 > L1 + L2 = 2).
    println!("{:?}", ik_2r_analytique(3.0, 0.0, 1.0, 1.0)); // None
}

fn fk_2r_xy(t1: f64, t2: f64, l1: f64, l2: f64) -> (f64, f64) {
    (
        l1 * t1.cos() + l2 * (t1 + t2).cos(),
        l1 * t1.sin() + l2 * (t1 + t2).sin(),
    )
}

fn main() {
    let (l1, l2) = (1.5, 1.0);
    let cibles = [(1.0, 0.5), (0.6, 1.2), (-0.8, 0.9), (2.0, 0.0)];

    for &(x, y) in &cibles {
        match ik_2r_analytique(x, y, l1, l2) {
            Some((t1, t2)) => {
                let (xr, yr) = fk_2r_xy(t1, t2, l1, l2);
                let err = ((xr - x).powi(2) + (yr - y).powi(2)).sqrt();
                assert!(err < 1e-9, "erreur de round-trip trop grande : {err}");
                println!("cible ({x}, {y}) -> theta ({t1:.4}, {t2:.4}), err={err:.2e}");
            }
            None => println!("cible ({x}, {y}) hors d'atteinte"),
        }
    }
}

use nalgebra::{Matrix2, Vector2};

fn jacobienne_2r(theta: &Vector2, l1: f64, l2: f64) -> Matrix2 {
    let (t1, t2) = (theta[0], theta[1]);
    let s1 = t1.sin();  let c1 = t1.cos();
    let s12 = (t1 + t2).sin();  let c12 = (t1 + t2).cos();
    Matrix2::new(
        -l1 * s1 - l2 * s12, -l2 * s12,
         l1 * c1 + l2 * c12,  l2 * c12,
    )
}

fn main() {
    let (l1, l2) = (1.0, 1.0);
    let theta = Vector2::new(0.7, 0.4);

    // Verification par differences finies centrees.
    let h = 1e-6;
    let fk = |th: &Vector2| Vector2::new(
        l1 * th[0].cos() + l2 * (th[0] + th[1]).cos(),
        l1 * th[0].sin() + l2 * (th[0] + th[1]).sin(),
    );
    let mut j_num = Matrix2::zeros();
    for col in 0..2 {
        let (mut tp, mut tm) = (theta, theta);
        tp[col] += h; tm[col] -= h;
        j_num.set_column(col, &((fk(&tp) - fk(&tm)) / (2.0 * h)));
    }

    let j_ana = jacobienne_2r(&theta, l1, l2);
    println!("ecart max = {:.2e}", (j_ana - j_num).abs().max());

    // Singularite : det(J) = l1 * l2 * sin(t2). Nul quand t2 = 0 ou pi (bras
    // tendu ou replie) -> le determinant s'annule, J n'est plus inversible.
    let sing = Vector2::new(0.7, 0.0);
    println!("det a t2=0 : {:.3}", jacobienne_2r(&sing, l1, l2).determinant());
}

use nalgebra::{Matrix2, Vector2};

fn fk_2r(theta: &Vector2, l1: f64, l2: f64) -> Vector2 {
    Vector2::new(
        l1 * theta[0].cos() + l2 * (theta[0] + theta[1]).cos(),
        l1 * theta[0].sin() + l2 * (theta[0] + theta[1]).sin(),
    )
}

fn jacobienne_2r(theta: &Vector2, l1: f64, l2: f64) -> Matrix2 {
    let (t1, t2) = (theta[0], theta[1]);
    Matrix2::new(
        -l1 * t1.sin() - l2 * (t1 + t2).sin(), -l2 * (t1 + t2).sin(),
         l1 * t1.cos() + l2 * (t1 + t2).cos(),  l2 * (t1 + t2).cos(),
    )
}

fn main() {
    let (l1, l2) = (1.0, 1.0);
    let cible = Vector2::new(1.2, 0.8);
    let mut theta = Vector2::new(0.1, 0.1); // estimation initiale
    let (lambda, tol, max_iter) = (1e-3, 1e-10, 50);
    let ident = Matrix2::identity();

    for k in 0..max_iter {
        let e = cible - fk_2r(&theta, l1, l2);
        let err = e.norm();
        println!("iter {k:2} : ||e|| = {err:.3e}");
        if err < tol {
            println!("converge ! theta = ({:.4}, {:.4})", theta[0], theta[1]);
            break;
        }
        let j = jacobienne_2r(&theta, l1, l2);
        let jjt = j * j.transpose() + ident * (lambda * lambda);
        // DLS : robuste meme si J devient mal conditionnee.
        let delta = j.transpose() * jjt.try_inverse().unwrap() * e;
        theta += delta;
    }
}

Récapitulatif