Module L21 · Partie I · Robotique et systèmes embarqués

Décider comment bouger : cinématique, planification, commande.

Le robot sait où il est et voit les obstacles. Reste à choisir une trajectoire réalisable, puis à la suivre malgré les perturbations. Ce module relie trois disciplines : la cinématique (comment les moteurs déplacent le robot), la planification de mouvement (trouver un chemin dans un espace continu avec obstacles : RRT, fenêtre dynamique), et la commande (PID revisité, LQR, MPC : la théorie du contrôle que la prépa aborde en SI et que les ingénieurs utilisent partout).

Durée : 4 séances · Prérequis : L10, L12, L16, séances 27-29. Objectifs : modèles cinématiques (différentiel, Ackermann, holonome, bras), espace des configurations, RRT/RRT*, lissage, fenêtre dynamique, suivi de trajectoire (pure pursuit), analyse de stabilité (pôles), LQR, MPC, limites d’actionneurs et sécurité.

Ce que vous saurez faire à la fin

Écrire la cinématique directe et inverse d’un robot différentiel et d’un bras plan.
Planifier un chemin sans collision par RRT dans un espace continu.
Concevoir un LQR pour un pendule inversé et un MPC pour un robot avec contraintes.
Analyser la stabilité d’une boucle fermée par ses valeurs propres.

Références : Modern Robotics (Lynch & Park, gratuit), Planning Algorithms (LaValle, gratuit), cours « Underactuated Robotics » (Tedrake, MIT, gratuit), Feedback Systems (Åström & Murray, gratuit).

Fiche de cours · Définitions

Planification et contrôle : définitions

Définition (espace des configurations). Une configuration q décrit complètement la position du robot (2 ou 3 coordonnées pour un mobile, n angles pour un bras à n articulations). C_libre = configurations sans collision. Planifier = trouver un chemin continu dans C_libre de q_départ à q_but. Un obstacle du plan devient une région de C obtenue par « gonflement » (somme de Minkowski) pour un robot circulaire.

Définition (planificateurs). Sur grille : A*, D* (replanification). Par échantillonnage : PRM (graphe de configurations aléatoires reliées si le segment est libre), RRT (arbre qui croît vers des échantillons aléatoires), RRT* (asymptotiquement optimal). Complétude probabiliste : si une solution existe, la probabilité de la trouver tend vers 1 avec le nombre d’échantillons.

Définition (système dynamique, état, commande). ẋ = f(x, u) (temps continu) ou x_k+1 = f(x_k, u_k). Linéaire : ẋ = Ax + Bu. Point d’équilibre : f(x*, u*) = 0. Stabilité : les trajectoires proches restent proches (Lyapunov) ; asymptotique : elles convergent. Pour un système linéaire : stable ⇔ toutes les valeurs propres de A ont une partie réelle < 0 (temps discret : module < 1).

Définition (PID). u = K_pe + K_i∫e dt + K_d de/dt, e = consigne − mesure. P : réaction proportionnelle (erreur statique résiduelle) ; I : annule l’erreur statique (risque d’oscillation, windup) ; D : anticipe (amortit, sensible au bruit).

Définition (commande optimale, LQR, MPC). LQR : pour ẋ = Ax + Bu, minimiser ∫(xᵀQx + uᵀRu)dt ; la solution est u = −Kx avec K issu de l’équation de Riccati. MPC : à chaque pas, résoudre un problème d’optimisation sur un horizon N sous contraintes (états, commandes), appliquer la première commande, recommencer.

Définition (fenêtre dynamique, DWA). Choisir (v, ω) parmi les vitesses atteignables au prochain pas (limites d’accélération), en simulant chaque candidate sur un court horizon et en notant : distance au chemin/but, dégagement des obstacles, vitesse.

Fiche de cours · Formules

Formules à connaître

Robot différentiel (unicycle) : ẋ = v cos θ, ẏ = v sin θ, θ̇ = ω ; non holonome (ne peut pas se déplacer latéralement)

Linéarisation autour de (x*, u*) : ẋ ≈ A δx + B δu avec A = ∂f/∂x, B = ∂f/∂u évalués à l’équilibre

Système du second ordre : ẍ + 2ζω_nẋ + ω_n²x = ω_n²r ; dépassement = e^{−πζ/√(1−ζ²)} ; temps de réponse à 2 % ≈ 4/(ζω_n)

Régulateur P sur un intégrateur ẋ = u, u = −K_p(x − r) : ẋ = −K_p(x − r), convergence exponentielle en τ = 1/K_p

Ziegler-Nichols (à partir du gain ultime K_u et de la période T_u d’oscillation) : K_p = 0,6K_u, K_i = 1,2K_u/T_u, K_d = 0,075K_uT_u

Riccati (temps continu) : AᵀP + PA − PBR⁻¹BᵀP + Q = 0 ; K = R⁻¹BᵀP ; boucle fermée ẋ = (A − BK)x

Lyapunov : V(x) > 0 pour x ≠ 0, V(0) = 0, V̇(x) = ∇V·f(x) < 0 ⇒ équilibre asymptotiquement stable

Suivi de chemin « pure pursuit » : ω = 2v sin α / L_d (α : angle vers le point cible à distance L_d)

RRT : coût par itération O(log n) avec kd-tree ; RRT* rayon de connexion r_n = γ(log n / n)^1/d

Fiche de cours · Théorèmes et démonstrations

Démonstrations à savoir refaire (1/2)

Théorème 1 (stabilité d’un système linéaire). ẋ = Ax est asymptotiquement stable si et seulement si toutes les valeurs propres de A ont une partie réelle strictement négative.

(Cas diagonalisable.) A = PDP⁻¹ ; posons y = P⁻¹x : ẏ = Dy, donc y_i(t) = y_i(0)e^λ_it. |e^λt| = e^Re(λ)t → 0 ssi Re λ < 0. x = Py tend vers 0 ssi tous les y_i le font. (Le cas non diagonalisable ajoute des facteurs polynomiaux t^ke^λt, qui tendent aussi vers 0 si Re λ < 0.) Temps discret : x_k = A^kx₀, λ^k → 0 ssi |λ| < 1. Application : un régulateur u = −Kx est stabilisant ssi les valeurs propres de A − BK sont dans le demi-plan gauche — on peut les placer (si (A, B) est commandable) : c’est le placement de pôles.

Théorème 2 (Lyapunov, version directe). Si V est C¹, définie positive (V(0) = 0, V(x) > 0 sinon) et V̇ = ∇V(x)·f(x) < 0 pour x ≠ 0, alors l’équilibre 0 de ẋ = f(x) est asymptotiquement stable.

(Idée.) V décroît strictement le long de toute trajectoire non nulle : V(x(t)) est décroissante et minorée par 0, donc converge vers une limite c ≥ 0. Si c > 0, la trajectoire reste dans la couronne {c ≤ V ≤ V(x₀)}, compacte, où V̇ ≤ −δ < 0 (continuité sur un compact), donc V(x(t)) ≤ V(x₀) − δt → −∞ : contradiction. Donc c = 0 et x(t) → 0. Exemple : pendule amorti θ̈ = −sin θ − bθ̇, V = ½θ̇² + (1 − cos θ) (énergie) : V̇ = θ̇θ̈ + sin θ·θ̇ = −bθ̇² ≤ 0 (semi-définie ; LaSalle conclut). Pour un régulateur, V = xᵀPx avec P solution de Lyapunov AᵀP + PA = −Q donne V̇ = −xᵀQx < 0.

Théorème 3 (l’action intégrale annule l’erreur statique). Pour un système stable en boucle fermée avec régulateur PI et consigne constante, l’erreur en régime permanent est nulle, même en présence d’une perturbation constante.

En régime permanent, toutes les dérivées sont nulles ; la commande u = K_pe + K_i∫e dt est constante, donc d/dt(∫e) = e = 0 — sinon l’intégrale, et donc u, continueraient à varier. L’intégrateur « cherche » la valeur de u qui annule e, quelle que soit la perturbation constante (pente, charge). Avec un P seul, u = K_pe : compenser une perturbation d exige e = d/K_p ≠ 0. Contrepartie : l’intégrale ajoute un pôle en 0 et un retard de phase qui peuvent déstabiliser (d’où K_i modéré et anti-windup).

Fiche de cours · Théorèmes et démonstrations

Démonstrations à savoir refaire (2/2)

Théorème 4 (LQR : le retour d’état optimal est linéaire). Pour x_k+1 = Ax_k + Bu_k et le coût Σ_k (x_kᵀQx_k + u_kᵀRu_k) sur un horizon N, la commande optimale est u_k = −K_kx_k avec K_k = (R + BᵀP_k+1B)⁻¹BᵀP_k+1A, et le coût optimal depuis x_k est x_kᵀP_kx_k (récurrence de Riccati à rebours, P_N = Q).

Programmation dynamique (L04, L16) : notons J_k(x) le coût optimal restant. Hypothèse de récurrence : J_k+1(x) = xᵀP_k+1x. Alors J_k(x) = min_u [xᵀQx + uᵀRu + (Ax + Bu)ᵀP_k+1(Ax + Bu)]. Le crochet est quadratique convexe en u ; dérivée nulle : 2Ru + 2BᵀP_k+1(Ax + Bu) = 0 ⇒ u = −(R + BᵀP_k+1B)⁻¹BᵀP_k+1A·x = −K_kx. En substituant, J_k(x) = xᵀ[Q + AᵀP_k+1A − AᵀP_k+1B(R + BᵀP_k+1B)⁻¹BᵀP_k+1A]x = xᵀP_kx : quadratique, l’hypothèse se propage. Initialisation J_N = xᵀQx. Quand N → ∞, P_k converge vers la solution de l’équation de Riccati algébrique et K devient constant. LQR n’est donc rien d’autre que l’itération de valeur sur un MDP linéaire-quadratique.

Théorème 5 (complétude probabiliste de RRT — énoncé et idée). Si C_libre est connexe entre départ et but avec un chemin de « largeur » δ > 0, la probabilité que RRT n’ait pas atteint le but après n itérations décroît exponentiellement en n.

(Idée.) Couvrir le chemin par des boules de rayon δ/2 ; l’arbre atteint la boule suivante dès qu’un échantillon y tombe alors que l’arbre contient déjà un point de la boule courante (l’extension vers l’échantillon reste dans C_libre par construction des boules). Chaque boule a une probabilité p > 0 d’être échantillonnée ; le nombre d’itérations pour franchir les m boules est une somme de m géométriques : P(échec après n) ≤ m·(1 − p)^n/m. RRT trouve un chemin, pas le plus court : RRT* rebranche localement et converge vers l’optimal (Karaman & Frazzoli 2011) au prix d’un facteur log n.

Théorème 6 (un robot non holonome ne peut pas suivre tout chemin). L’unicycle ne peut pas suivre un chemin qui présente une composante latérale instantanée ; mais tout chemin de C_libre peut être approché par une suite de manœuvres (le système est commandable, crochet de Lie [g₁, g₂] ≠ 0).

Les vitesses admissibles sont (v cos θ, v sin θ, ω) : le déplacement latéral (−sin θ, cos θ, 0) n’est pas dans leur espace vectoriel. Mais avancer, tourner, reculer, tourner (créneau) produit un déplacement net latéral au second ordre — l’argument formel est que le crochet de Lie des champs g₁ = (cos θ, sin θ, 0) et g₂ = (0, 0, 1) vaut (sin θ, −cos θ, 0), qui complète l’espace : condition de rang de Chow. Conséquence pratique : planifier dans (x, y) puis lisser avec des courbes réalisables (Dubins, Reeds-Shepp), ou planifier directement dans l’espace d’état avec les primitives de mouvement.

Fiche de cours · Méthodes

Méthodes et pièges

Méthode — régler un PID à la main. K_i = K_d = 0. Augmenter K_p jusqu’à une réponse rapide avec léger dépassement. Ajouter K_d pour amortir. Ajouter K_i juste assez pour supprimer l’erreur statique ; borner l’intégrale (anti-windup) ; filtrer la dérivée (passe-bas, ou dériver la mesure plutôt que l’erreur pour éviter le coup de pied à chaque changement de consigne). Tester sur des échelons et des rampes, avec perturbation.

Méthode — pile de navigation. Carte → planificateur global (A* sur grille gonflée, ou RRT*) → chemin → lissage → suivi local (pure pursuit ou DWA, avec évitement) → commande (v, ω) → boucle bas niveau (PID vitesse des roues à 100 Hz–1 kHz). Chaque couche à sa fréquence ; la sécurité (arrêt d’urgence sur distance minimale) court-circuite tout.

Méthode — passer de la simulation au réel. Identifier les paramètres (constante de temps du moteur, retard) par une réponse à l’échelon ; inclure retard et saturation dans le simulateur ; régler avec marge (un retard de 50 ms réduit la marge de phase de 18° à 1 Hz) ; commencer avec des gains faibles.

Pièges : dériver un signal bruité sans filtre ; windup de l’intégrale pendant une saturation ; gains réglés sans tenir compte de la période d’échantillonnage (K_i·T_s) ; chemin de A* qui frôle les murs (gonfler par le rayon + marge) ; DWA qui oscille entre deux candidats (hystérésis) ; oublier la non-holonomie (chemin impossible) ; LQR sans contraintes qui demande une commande irréalisable (saturer, ou passer au MPC).

Fiche de cours · Exercices corrigés

Exercices corrigés

Exercice 1. Moteur modélisé par ω̇ = −aω + bu avec a = 5 s⁻¹, b = 10. Régulateur P : u = K_p(ω_ref − ω). Donner la constante de temps en boucle fermée et l’erreur statique pour K_p = 1 et 5 ; puis avec un PI, montrer que l’erreur statique est nulle et donner la condition de stabilité.

Correction. Boucle fermée : ω̇ = −(a + bK_p)ω + bK_pω_ref ; pôle −(a + bK_p), τ = 1/(a + bK_p) : K_p = 1 → τ = 1/15 = 67 ms ; K_p = 5 → 18 ms. Régime permanent : ω = bK_pω_ref/(a + bK_p) ; erreur relative a/(a + bK_p) : 33 % (K_p = 1), 9 % (K_p = 5). PI : u = K_pe + K_i∫e ; en dérivant, ω̈ = −aω̇ + b(K_pė + K_ie) ; avec e = ω_ref − ω constant en consigne : ë + (a + bK_p)ė + bK_ie = 0. Polynôme s² + (a + bK_p)s + bK_i : stable ssi tous les coefficients > 0 (Routh, degré 2), c’est-à-dire K_i > 0 et a + bK_p > 0 — toujours vrai ici ; en régime permanent e → 0 (Théorème 3). Amortissement ζ = (a + bK_p)/(2√(bK_i)) : pour ζ = 0,7 avec K_p = 1, K_i = 15²/(4·0,49·10) ≈ 11,5.

Exercice 2. Pendule inversé linéarisé : ẍ = u, θ̈ = θ + u (unités normalisées). Montrer que le système est instable en boucle ouverte et proposer un retour d’état u = −k₁θ − k₂θ̇ qui place les pôles de la dynamique de θ en −2 et −3.

Correction. Boucle ouverte : θ̈ − θ = 0, pôles ±1 : un pôle à partie réelle positive, instable (le pendule tombe). Avec u = −k₁θ − k₂θ̇ : θ̈ = θ − k₁θ − k₂θ̇ ⇒ θ̈ + k₂θ̇ + (k₁ − 1)θ = 0 ; polynôme souhaité (s + 2)(s + 3) = s² + 5s + 6 : k₂ = 5, k₁ = 7. Vérification : gains positifs (on pousse le chariot dans le sens de la chute), réponse sans oscillation (pôles réels). Reste à stabiliser x (le chariot dérive) : ajouter −k₃x − k₄ẋ et placer 4 pôles, ou LQR sur l’état (x, ẋ, θ, θ̇) — voir le module.

Exercice 3. Grille 20×20, cellules de 10 cm, robot de rayon 15 cm, obstacles connus. Décrire précisément comment obtenir un chemin sûr et réalisable par un robot différentiel, et estimer le coût de calcul.

Correction. (1) Gonfler les obstacles de 15 cm + marge 5 cm = 2 cellules (dilatation morphologique) : le robot devient un point. (2) A* 8-connexe avec coûts 1 et √2, heuristique octile ; 400 cellules : < 1 ms. (3) Le chemin en escalier n’est pas lisse : lissage par suppression des points intermédiaires quand le segment direct est libre (« string pulling »), puis spline ou arcs. (4) Vérifier la courbure maximale contre la contrainte non holonome (l’unicycle peut tourner sur place : toute courbure est réalisable, mais à vitesse réduite dans les virages : v ≤ v_max, |ω| = v·κ ≤ ω_max). (5) Suivi par pure pursuit à 20 Hz avec L_d ≈ 30 cm, DWA ou arrêt si un obstacle imprévu apparaît dans le couloir de sécurité. Replanifier (D* Lite ou simplement A* à nouveau, 1 ms) dès que la carte change.

01 / Cinématique

Robot différentiel : des vitesses de roues à la trajectoire, et retour

import numpy as np
import matplotlib.pyplot as plt
L_ESSIEU, R_ROUE = 0.2, 0.05                   # entraxe 20 cm, roues de 5 cm

def roues_vers_corps(w_g, w_d):
    """Cinématique directe : vitesses angulaires des roues → vitesse linéaire v et angulaire ω du châssis."""
    v_g, v_d = R_ROUE * w_g, R_ROUE * w_d
    return (v_g + v_d) / 2, (v_d - v_g) / L_ESSIEU
def corps_vers_roues(v, w):
    """Cinématique inverse : (v, ω) voulus → vitesses des roues."""
    return (v - w * L_ESSIEU / 2) / R_ROUE, (v + w * L_ESSIEU / 2) / R_ROUE
def integrer(pose, v, w, dt):
    """Intégration exacte sur un arc de cercle (mieux qu'Euler quand ω ≠ 0)."""
    x, y, th = pose
    if abs(w) < 1e-9: return np.array([x + v * dt * np.cos(th), y + v * dt * np.sin(th), th])
    R = v / w; return np.array([x + R * (np.sin(th + w * dt) - np.sin(th)), y - R * (np.cos(th + w * dt) - np.cos(th)), th + w * dt])

print("roues (10, 10) rad/s →", roues_vers_corps(10, 10), "| (5, 15) →", tuple(round(a, 3) for a in roues_vers_corps(5, 15)))
print("pour v = 0.3 m/s, ω = 1 rad/s : roues", tuple(round(a, 2) for a in corps_vers_roues(0.3, 1.0)))
pose = np.array([0, 0, 0.0]); traj = [pose]
for t in np.arange(0, 6, 0.05):
    v, w = 0.3, 0.8 * np.sin(0.7 * t); pose = integrer(pose, v, w, 0.05); traj.append(pose)
traj = np.array(traj)
plt.figure(figsize=(6, 3.5)); plt.plot(traj[:, 0], traj[:, 1]); plt.axis("equal"); plt.title("trajectoire pour v = 0.3, ω(t) sinusoïdal"); plt.show()
print("Contrainte non holonome : le robot ne peut pas se déplacer latéralement (ẏ cos θ − ẋ sin θ = 0). Se garer en créneau exige de manœuvrer.")

Les familles de robots mobiles

Type	Commandes	Contrainte	Exemples
Différentiel	v, ω (2 roues)	Non holonome ; tourne sur place	Aspirateurs, TurtleBot
Ackermann	v, angle de braquage	Non holonome ; rayon minimal	Voitures
Holonome (mecanum, omni)	v_x, v_y, ω	Aucune : peut aller de côté	Robots de logistique
Quadrupède / bipède	Couples des articulations	Contacts intermittents, équilibre	Spot, Atlas
Drone	4 poussées	Sous-actionné : 4 entrées pour 6 degrés de liberté	Quadrirotors

01 / Cinématique

Bras articulé : espace des configurations et espace de travail

import numpy as np
import matplotlib.pyplot as plt
L1, L2 = 1.0, 0.7
def cinematique_directe(q):                      # q = (θ1, θ2) → position de l'effecteur (module L10, matrices homogènes)
    return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]), L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])
def cinematique_inverse(p, coude_haut=True):
    """Solution analytique (loi des cosinus) : deux solutions (coude haut / bas), aucune si hors d'atteinte."""
    x, y = p; c2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(c2) > 1: return None
    s2 = np.sqrt(1 - c2**2) * (1 if coude_haut else -1); th2 = np.arctan2(s2, c2)
    th1 = np.arctan2(y, x) - np.arctan2(L2 * s2, L1 + L2 * c2)
    return np.array([th1, th2])
p = np.array([1.2, 0.5]); q = cinematique_inverse(p); print("angles :", np.degrees(q).round(1), "→ vérification :", cinematique_directe(q).round(3))

# Obstacle circulaire dans l'espace de travail → région interdite dans l'espace des configurations (C-space)
obstacle = (np.array([0.9, 0.9]), 0.3)
def collision(q, n=10):
    """Le bras (2 segments) touche-t-il le disque ? Échantillonner des points le long des segments."""
    coude = np.array([L1 * np.cos(q[0]), L1 * np.sin(q[0])]); bout = cinematique_directe(q)
    pts = [coude * s for s in np.linspace(0, 1, n)] + [coude + (bout - coude) * s for s in np.linspace(0, 1, n)]
    return any(np.linalg.norm(pt - obstacle[0]) < obstacle[1] for pt in pts)
th = np.linspace(-np.pi, np.pi, 60); C = np.array([[collision((a, b)) for a in th] for b in th])
fig, axes = plt.subplots(1, 2, figsize=(9, 3.6))
axes[0].add_patch(plt.Circle(obstacle[0], obstacle[1], color="r", alpha=0.5)); axes[0].set_xlim(-2, 2); axes[0].set_ylim(-2, 2); axes[0].set_aspect("equal"); axes[0].set_title("espace de travail")
for qq in [(0.3, 0.8), (1.2, -0.5), (0.7, 0.4)]:
    coude = [L1 * np.cos(qq[0]), L1 * np.sin(qq[0])]; bout = cinematique_directe(qq)
    axes[0].plot([0, coude[0], bout[0]], [0, coude[1], bout[1]], "o-", color="r" if collision(qq) else "g")
axes[1].imshow(C, extent=(-180, 180, -180, 180), origin="lower", cmap="Reds"); axes[1].set_xlabel("θ1 (°)"); axes[1].set_ylabel("θ2 (°)"); axes[1].set_title("espace des configurations (rouge = collision)")
plt.tight_layout(); plt.show()

La planification se fait dans l’espace des configurations (les angles), pas dans l’espace de travail : un obstacle simple y devient une région compliquée, mais le robot y est un point. Pour un bras à 7 axes, l’espace a 7 dimensions et on ne peut plus le quadriller : il faut échantillonner.

02 / Planifier

RRT : explorer un espace continu par un arbre aléatoire

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(0)
# Espace 2D [0,10]² avec obstacles rectangulaires ; robot = point (ou disque de rayon r : gonfler les obstacles)
OBST = [(2, 0, 3, 6), (5, 4, 6, 10), (7, 1, 8, 7)]                     # (x1, y1, x2, y2)
def libre(p, r=0.2): return all(not (x1 - r <= p[0] <= x2 + r and y1 - r <= p[1] <= y2 + r) for x1, y1, x2, y2 in OBST) and 0 <= p[0] <= 10 and 0 <= p[1] <= 10
def segment_libre(a, b, n=20): return all(libre(a + (b - a) * s) for s in np.linspace(0, 1, n))

def rrt(depart, but, n_iter=3000, pas=0.5, p_but=0.1):
    noeuds, parents = [np.array(depart, float)], [None]
    for it in range(n_iter):
        cible = np.array(but, float) if rng.random() < p_but else rng.uniform(0, 10, 2)     # biais vers le but
        i = int(np.argmin([np.linalg.norm(n - cible) for n in noeuds]))                     # plus proche (k-d tree en pratique)
        d = cible - noeuds[i]; nouveau = noeuds[i] + d / max(np.linalg.norm(d), 1e-9) * min(pas, np.linalg.norm(d))
        if segment_libre(noeuds[i], nouveau):
            noeuds.append(nouveau); parents.append(i)
            if np.linalg.norm(nouveau - but) < pas and segment_libre(nouveau, np.array(but, float)):
                noeuds.append(np.array(but, float)); parents.append(len(noeuds) - 2)
                chemin, k = [], len(noeuds) - 1
                while k is not None: chemin.append(noeuds[k]); k = parents[k]
                return np.array(chemin[::-1]), noeuds, parents, it
    return None, noeuds, parents, n_iter

chemin, noeuds, parents, it = rrt((0.5, 0.5), (9.5, 9.5))
def lisser(chemin, essais=200):
    """Raccourcir : relier deux points non consécutifs si le segment est libre."""
    c = list(chemin)
    for _ in range(essais):
        if len(c) < 3: break
        i, j = sorted(rng.choice(len(c), 2, replace=False))
        if j > i + 1 and segment_libre(c[i], c[j]): c = c[:i + 1] + c[j:]
    return np.array(c)
longueur = lambda c: sum(np.linalg.norm(b - a) for a, b in zip(c, c[1:]))
lisse = lisser(chemin)
print(f"trouvé en {it} itérations, {len(noeuds)} nœuds ; longueur {longueur(chemin):.2f} → lissé {longueur(lisse):.2f} (ligne droite : {np.hypot(9, 9):.2f})")
plt.figure(figsize=(5.5, 5))
for x1, y1, x2, y2 in OBST: plt.fill([x1, x2, x2, x1], [y1, y1, y2, y2], color="gray")
for k, p in enumerate(parents):
    if p is not None: plt.plot([noeuds[k][0], noeuds[p][0]], [noeuds[k][1], noeuds[p][1]], "-", color="#446", lw=0.4)
plt.plot(chemin[:, 0], chemin[:, 1], "y-", lw=2, label="RRT"); plt.plot(lisse[:, 0], lisse[:, 1], "c-", lw=2, label="lissé"); plt.legend(); plt.axis("equal"); plt.show()

Pourquoi échantillonner

Le RRT (LaValle, 1998) ne quadrille pas l’espace : il tire des points au hasard et étend l’arbre vers eux. Il est probabilistement complet (s’il existe un chemin, la probabilité de le trouver tend vers 1) et fonctionne en dimension 7, 12 ou 30 — là où A* sur une grille est impossible. Le chemin est mauvais (en zigzag) : d’où le lissage, ou RRT* qui recâble l’arbre pour converger vers l’optimal. Les contraintes cinématiques (voiture qui ne tourne pas sur place) s’intègrent en remplaçant « segment droit » par « arc réalisable » : kinodynamic RRT. En pratique : OMPL (dans MoveIt pour les bras) et les planificateurs de Nav2.

02 / Planifier

Fenêtre dynamique : choisir (v, ω) à chaque instant en simulant

import numpy as np
import matplotlib.pyplot as plt
# Planificateur local (DWA, Fox 1997) : parmi les (v, ω) atteignables en un pas (limites d'accélération), simuler 1 s,
# rejeter les collisions, scorer par (cap vers le but, distance aux obstacles, vitesse), choisir le meilleur.
V_MAX, W_MAX, A_V, A_W, DT = 0.8, 2.0, 1.0, 3.0, 0.1
but = np.array([9.5, 9.5])
def dwa(pose, v, w):
    meilleur, meilleur_score = (0, 0), -np.inf
    for vc in np.linspace(max(0, v - A_V * DT), min(V_MAX, v + A_V * DT), 5):
        for wc in np.linspace(max(-W_MAX, w - A_W * DT), min(W_MAX, w + A_W * DT), 7):
            p = pose.copy(); ok = True; dmin = np.inf
            for _ in range(10):                                            # simuler 1 s
                p = integrer(p, vc, wc, DT)
                if not libre(p[:2], 0.3): ok = False; break
                dmin = min(dmin, min(max(x1 - p[0], p[0] - x2, y1 - p[1], p[1] - y2, 0) for x1, y1, x2, y2 in OBST))
            if not ok: continue
            cap = np.cos(np.arctan2(*(but - p[:2])[::-1]) - p[2])          # 1 si on regarde vers le but
            score = 2.0 * cap + 0.5 * min(dmin, 1.0) + 0.3 * vc / V_MAX
            if score > meilleur_score: meilleur, meilleur_score = (vc, wc), score
    return meilleur

pose = np.array([0.5, 0.5, 0.0]); v = w = 0.0; traj = [pose[:2].copy()]
for k in range(400):
    v, w = dwa(pose, v, w); pose = integrer(pose, v, w, DT); traj.append(pose[:2].copy())
    if np.linalg.norm(pose[:2] - but) < 0.3: break
traj = np.array(traj); print(f"arrivé en {k * DT:.1f} s" if np.linalg.norm(pose[:2] - but) < 0.3 else "bloqué (minimum local !)")
plt.figure(figsize=(5.5, 5))
for x1, y1, x2, y2 in OBST: plt.fill([x1, x2, x2, x1], [y1, y1, y2, y2], color="gray")
plt.plot(traj[:, 0], traj[:, 1], "c-"); plt.plot(*but, "y*", ms=12); plt.axis("equal"); plt.title("fenêtre dynamique (planificateur local)"); plt.show()

La fenêtre dynamique respecte les limites physiques (accélérations) et réagit aux obstacles imprévus, mais elle est myope : elle peut rester coincée dans un cul-de-sac. D’où l’architecture à deux niveaux de Nav2 (module L20) : planificateur global (A*, RRT) pour la stratégie, planificateur local (DWA, MPPI) pour la tactique et la sécurité. Le score à trois termes est un choix de conception : chaque poids est un compromis.

03 / Contrôler

Suivre une trajectoire : pure pursuit et retour d’état

import numpy as np
import matplotlib.pyplot as plt
# Trajectoire de référence : le chemin RRT lissé, rééchantillonné finement
ref = np.concatenate([np.linspace(a, b, 30) for a, b in zip(lisse, lisse[1:])])
def pure_pursuit(pose, ref, lookahead=0.8, v=0.5):
    """Viser un point de la trajectoire à distance L devant : ω = 2 v sin(α) / L (géométrie de l'arc)."""
    d = np.linalg.norm(ref - pose[:2], axis=1); i = int(np.argmin(d))
    while i < len(ref) - 1 and np.linalg.norm(ref[i] - pose[:2]) < lookahead: i += 1
    cible = ref[i]; alpha = np.arctan2(cible[1] - pose[1], cible[0] - pose[0]) - pose[2]
    return v, 2 * v * np.sin(alpha) / lookahead

for la, nom in [(0.4, "L = 0.4 (nerveux)"), (1.5, "L = 1.5 (coupe les virages)")]:
    pose = np.array([0.5, 0.5, 0.0]); tr = []
    for _ in range(600):
        v, w = pure_pursuit(pose, ref, la); w = np.clip(w, -2, 2); pose = integrer(pose, v, w, 0.05); tr.append(pose[:2].copy())
        if np.linalg.norm(pose[:2] - ref[-1]) < 0.2: break
    tr = np.array(tr); ecart = np.mean([np.min(np.linalg.norm(ref - p, axis=1)) for p in tr])
    plt.plot(tr[:, 0], tr[:, 1], label=f"{nom} : écart moyen {ecart:.2f} m")
plt.plot(ref[:, 0], ref[:, 1], "w--", lw=1, label="référence"); plt.legend(fontsize=8); plt.axis("equal"); plt.show()

Du PID au retour d’état

Pure pursuit est un contrôleur géométrique simple et robuste (voitures autonomes DARPA 2005). Plus généralement, un contrôleur est une fonction u = k(x) de l’état vers la commande. Le PID (séance 29) est un retour sur l’erreur et ses dérivée/intégrale ; le retour d’état u = −Kx utilise tout l’état estimé (module L19). La question centrale : comment choisir K pour que le système bouclé soit stable et performant ? Réponse dans les deux slides suivantes.

03 / Contrôler

Stabilité et placement de pôles : où sont les valeurs propres ?

import numpy as np
import matplotlib.pyplot as plt
# Système linéaire discret : x_{k+1} = A x_k + B u_k. Pendule inversé linéarisé, discrétisé à dt = 0.02 s (module L12).
g, l, m, dt = 9.81, 0.5, 0.2, 0.02
Ac = np.array([[0, 1], [g / l, 0]]); Bc = np.array([[0], [1 / (m * l)]])
A = np.eye(2) + Ac * dt; B = Bc * dt                                       # discrétisation d'Euler (suffisante ici)
print("pôles en boucle ouverte :", np.abs(np.linalg.eigvals(A)).round(3), "→ un module > 1 : instable")

# Boucle fermée u = −K x : x_{k+1} = (A − B K) x. Stable ⇔ toutes les valeurs propres de module < 1.
def simuler(K, x0=np.array([0.1, 0.0]), n=150):
    x, X, U = x0.copy(), [], []
    for _ in range(n):
        u = float(-K @ x); u = np.clip(u, -10, 10)                          # saturation de l'actionneur : 10 N max
        X.append(x.copy()); U.append(u); x = A @ x + (B * u).ravel()
    return np.array(X), np.array(U)
for K in [np.array([[20.0, 2.0]]), np.array([[40.0, 8.0]]), np.array([[200.0, 3.0]])]:
    poles = np.abs(np.linalg.eigvals(A - B @ K)); X, U = simuler(K)
    print(f"K = {K.ravel()} : |pôles| = {poles.round(3)} → {'stable' if poles.max() < 1 else 'INSTABLE'} ; |u| max = {np.abs(U).max():.1f} N")
    plt.plot(X[:, 0], label=f"K={K.ravel()}")
plt.legend(); plt.ylabel("angle (rad)"); plt.xlabel("pas (20 ms)"); plt.ylim(-0.3, 0.3); plt.show()

En continu, stable ⇔ parties réelles négatives ; en discret, ⇔ modules < 1. Plus les pôles sont « à l’intérieur », plus la réponse est rapide — mais plus la commande est violente, jusqu’à saturer l’actionneur (et là, l’analyse linéaire ne vaut plus rien : K = [200, 3] est stable sur le papier et diverge en pratique). Le placement de pôles choisit K pour des pôles voulus (formule d’Ackermann) ; le LQR choisit les pôles pour vous, selon un compromis explicite.

03 / Contrôler

LQR : le meilleur compromis entre précision et effort

import numpy as np
import matplotlib.pyplot as plt
# Minimiser Σ (xᵀ Q x + uᵀ R u) : Q pénalise l'écart, R l'effort. Solution : u = −K x avec K issu de l'équation de Riccati.
def lqr(A, B, Q, R, iters=500):
    P = Q.copy()
    for _ in range(iters):                                                  # itération de Riccati (programmation dynamique, module L16 : Bellman !)
        K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
        P = Q + A.T @ P @ (A - B @ K)
    return K, P
for q_angle, r in [(1, 1), (100, 1), (100, 0.01)]:
    Q = np.diag([q_angle, 1.0]); R = np.array([[r]])
    K, P = lqr(A, B, Q, R); X, U = simuler(K)
    print(f"Q=diag({q_angle},1) R={r:5} → K = {K.ravel().round(2)}, |pôles| = {np.abs(np.linalg.eigvals(A - B @ K)).round(3)}, |u|max = {np.abs(U).max():.1f} N, retour à 5 % en {np.argmax(np.abs(X[:, 0]) < 0.005) * dt:.2f} s")
    plt.plot(X[:, 0], label=f"Q={q_angle}, R={r}")
plt.legend(); plt.ylabel("angle (rad)"); plt.show()
# Le coût optimal depuis x0 est x0ᵀ P x0 : P est la « fonction de valeur » du module L16, ici quadratique et exacte
print("coût optimal depuis (0.1, 0) :", round(float(np.array([0.1, 0]) @ P @ np.array([0.1, 0])), 3))

Riccati = Bellman pour un système linéaire à coût quadratique

Le LQR est l’apprentissage par renforcement (module L16) résolu exactement : la fonction de valeur est quadratique V(x) = xᵀPx, l’équation de Bellman devient l’équation de Riccati, et la politique optimale est linéaire u = −Kx. Il est robuste (marge de gain infinie, marge de phase ≥ 60° en continu), facile à régler (Q et R ont un sens physique), et c’est le contrôleur des fusées, des drones et des Segways. Ses limites : linéaire (autour d’un point de fonctionnement), sans contraintes (la saturation n’est pas vue). Pour cela : MPC.

03 / Contrôler

MPC : optimiser un horizon, appliquer le premier pas, recommencer

import numpy as np
import matplotlib.pyplot as plt
# Commande prédictive : à chaque instant, choisir u_0…u_{N−1} minimisant Σ xᵀQx + uᵀRu sur N pas SOUS CONTRAINTES |u| ≤ u_max,
# appliquer u_0, réobserver, recommencer. Ici : résolution par gradient projeté (petit problème) ; en pratique : QP (OSQP, acados).
N_H = 20; Q = np.diag([100.0, 1.0]); R = np.array([[0.01]]); U_MAX = 3.0

def cout_et_gradient(u_seq, x0):
    """Simule la séquence, calcule le coût et son gradient par rapport à u (différences finies : 25 variables, ça va)."""
    def cout(u_seq):
        x, c = x0.copy(), 0.0
        for u in u_seq:
            c += x @ Q @ x + u * R[0, 0] * u; x = A @ x + (B * u).ravel()
        return c + 10 * x @ Q @ x                                             # coût terminal
    c0 = cout(u_seq); grad = np.zeros(N_H)
    for i in range(N_H):
        d = np.zeros(N_H); d[i] = 1e-4; grad[i] = (cout(u_seq + d) - c0) / 1e-4
    return c0, grad

def mpc(x0, u_prec):
    u_seq = np.roll(u_prec, -1); u_seq[-1] = 0                                # démarrage à chaud : la solution précédente décalée
    for _ in range(15):
        c, g = cout_et_gradient(u_seq, x0)
        u_seq = np.clip(u_seq - 0.002 * g, -U_MAX, U_MAX)                     # gradient projeté sur les contraintes
    return u_seq

x = np.array([0.25, 0.0]); u_seq = np.zeros(N_H); X, U = [], []               # départ à 14° : difficile avec 3 N max
K_lqr, _ = lqr(A, B, Q, R)
for k in range(60):
    u_seq = mpc(x, u_seq); u = u_seq[0]; X.append(x.copy()); U.append(u); x = A @ x + (B * u).ravel()
X, U = np.array(X), np.array(U)
Xl, Ul = simuler(K_lqr, np.array([0.25, 0.0]), 60); Ul = np.clip(Ul, -U_MAX, U_MAX)
fig, axes = plt.subplots(1, 2, figsize=(10, 3))
axes[0].plot(X[:, 0], label="MPC (contraintes vues)"); axes[0].plot(Xl[:, 0], label="LQR saturé à 3 N"); axes[0].set_ylabel("angle"); axes[0].legend(); axes[0].set_ylim(-0.5, 0.5)
axes[1].plot(U, label="u MPC"); axes[1].plot(Ul, label="u LQR saturé"); axes[1].set_ylabel("force (N)"); axes[1].legend(); plt.show()
print("MPC : angle final", X[-1, 0].round(4), "| LQR saturé : angle final", Xl[-1, 0].round(4), "(a-t-il tenu ?)")

Ce que le MPC apporte, et ce qu’il coûte

Le MPC voit les contraintes (actionneurs, obstacles, vitesses max) et l’avenir prévu (trajectoire de référence à venir) ; il gère naturellement le multivariable. C’est le contrôleur des raffineries depuis les années 1980, des voitures autonomes, des quadrupèdes de Boston Dynamics et des fusées SpaceX (atterrissage : MPC convexe). Le coût : résoudre une optimisation à chaque pas (1-100 ms) — d’où les solveurs spécialisés, le code généré (acados, CasADi), et MPPI (module L20 : version par échantillonnage, parallélisable sur GPU). Théorie : stabilité garantie sous conditions (coût terminal, ensemble terminal invariant) — sujet de recherche toujours actif, surtout pour le MPC non linéaire et le MPC appris.

04 / Sécurité

Le contrôleur ne suffit pas : limites, surveillance, arrêts

Couche	Rôle	Exemple
Limitation	Saturer vitesses, accélérations, couples avant les moteurs	velocity_smoother, rampe de PWM
Bouclier (safety filter)	Modifier au minimum la commande pour rester dans un ensemble sûr	Fonctions barrières de contrôle (CBF), zone de sécurité lidar
Surveillance	Détecter capteur en panne, dérive, timeout	NIS (L19), watchdog (L18), chien de garde de commande (arrêt si pas de /cmd_vel depuis 200 ms)
Arrêt d’urgence	Matériel, indépendant du logiciel	Bouton coup-de-poing qui coupe l’alimentation des moteurs
Vérification	Prouver ou tester massivement avant déploiement	Simulation Monte-Carlo (L11), atteignabilité, preuve (L09)

Principe : la sécurité ne dépend pas de la qualité du planificateur ni de l’IA. Chaque couche suppose que celle du dessus peut se tromper. Un robot dont la sécurité repose sur un réseau de neurones n’est pas sûr.

Cours

Cours 1 — Systèmes dynamiques linéaires : modèle d’état, discrétisation, réponse

Modèle d’état continu : ẋ = A x + B u, y = C x + D u. x : état (positions, vitesses, courants…), u : commandes, y : mesures. Toute équation différentielle linéaire d’ordre n se met sous cette forme avec n états (masse-ressort : x = (position, vitesse)). Réponse libre : x(t) = e^At x(0), somme de modes e^λ_it ; stable ⇔ Re(λ_i) < 0.

Discrétisation à période T (le contrôleur numérique n’agit qu’aux instants kT, commande constante entre deux) : x_k+1 = A_d x_k + B_d u_k avec A_d = e^AT (exact, scipy.linalg.expm) et B_d = ∫₀^T e^Aτ dτ B. Approximation d’Euler : A_d ≈ I + AT, B_d ≈ BT (valable si T ≪ constantes de temps). Les pôles discrets sont z_i = e^λ_iT : le demi-plan gauche devient l’intérieur du cercle unité.

Grandeur	Continu	Discret
Stabilité	Re λ < 0	\|z\| < 1
Rapidité	\|Re λ\| grand (constante de temps 1/\|Re λ\|)	\|z\| petit (proche de 0 : réponse en quelques pas)
Oscillation	Im λ ≠ 0 (pulsation)	z complexe (angle = pulsation × T)
Commandabilité	rang [B, AB, A²B, …] = n : on peut placer les pôles où l’on veut (sinon, un mode est hors d’atteinte)
Observabilité	rang [C ; CA ; CA² ; …] = n : on peut reconstruire l’état depuis y (sinon, un mode est invisible)

import numpy as np
from scipy.linalg import expm
# Moteur à courant continu : états (vitesse ω, courant i), commande tension u. Constantes typiques d'un petit moteur de robot.
J, b, Kt, Ke, R, L = 1e-4, 1e-5, 0.02, 0.02, 2.0, 1e-3
Am = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]]); Bm = np.array([[0], [1 / L]]); Cm = np.array([[1, 0]])
lam = np.linalg.eigvals(Am); print("pôles continus :", lam.round(1), "→ constantes de temps", (-1 / lam.real).round(5), "s")
Tm = 0.001; Adm = expm(Am * Tm); Bdm = np.linalg.solve(Am, (Adm - np.eye(2)) @ Bm)          # B_d exact quand Am est inversible
Ad_euler = np.eye(2) + Am * Tm
print("pôles discrets exacts :", np.abs(np.linalg.eigvals(Adm)).round(4), "| Euler :", np.abs(np.linalg.eigvals(Ad_euler)).round(4), "(Euler instable pour le mode rapide !)")
print("commandable :", np.linalg.matrix_rank(np.hstack([Bm, Am @ Bm])) == 2, "| observable depuis ω seule :", np.linalg.matrix_rank(np.vstack([Cm, Cm @ Am])) == 2)
x = np.zeros(2); traj = []
for k in range(300): x = Adm @ x + (Bdm * 6.0).ravel(); traj.append(x[0])           # échelon de 6 V
print("vitesse finale :", round(traj[-1], 1), "rad/s | 63 % atteints à", round(np.argmax(np.array(traj) > 0.63 * traj[-1]) * Tm, 4), "s")

Cours

Cours 2 — Exemple travaillé : concevoir un contrôleur de vitesse de moteur, du PID au retour d’état

import numpy as np
from scipy.linalg import expm
# 1) Placement de pôles : on veut x_{k+1} = (Adm − Bdm K) x_k avec des pôles discrets choisis (formule d'Ackermann, 2 états)
def ackermann(Adm, Bdm, poles):
    Cc = np.hstack([Bdm, Adm @ Bdm])                                       # matrice de commandabilité
    p = np.poly(poles)                                                  # polynôme caractéristique voulu
    phi = p[0] * Adm @ Adm + p[1] * Adm + p[2] * np.eye(2)
    return (np.array([[0, 1]]) @ np.linalg.inv(Cc) @ phi)
K = ackermann(Adm, Bdm, [0.95, 0.90]); print("K (placement) :", K.round(3), "| pôles obtenus :", np.abs(np.linalg.eigvals(Adm - Bdm @ K)).round(3))
# 2) Suivi de consigne : pré-compensation N pour que la sortie tende vers la référence (gain statique = 1)
N = 1 / (Cm @ np.linalg.inv(np.eye(2) - Adm + Bdm @ K) @ Bdm)[0, 0]
# 3) Simulation avec saturation de tension (±12 V) et une perturbation de couple à t = 0.15 s
def simuler(K, N, ref=200.0, perturbation=True):
    x = np.zeros(2); U, Y = [], []
    for k in range(400):
        u = float(np.clip(N * ref - K @ x, -12, 12)); U.append(u)
        x = Adm @ x + (Bdm * u).ravel()
        if perturbation and k > 150: x[0] -= 0.02                       # charge qui freine
        Y.append(x[0])
    return np.array(Y), np.array(U)
Y, U = simuler(K, N)
print(f"vitesse à 0.1 s : {Y[100]:.1f} (consigne 200) | après perturbation : {Y[-1]:.1f} → erreur statique {200 - Y[-1]:.1f} rad/s (pas d'intégrateur)")
# 4) Ajouter un état intégral : x_aug = (x, ∫e). Le contrôleur annule l'erreur statique.
Aa = np.block([[Adm, np.zeros((2, 1))], [-Cm @ Adm * Tm, np.ones((1, 1))]]); Ba = np.vstack([Bdm, -Cm @ Bdm * Tm])
from numpy.linalg import solve
Q = np.diag([1e-2, 1e-4, 50.0]); Rr = np.array([[1e-3]]); P = Q.copy()
for _ in range(2000): Ka = solve(Rr + Ba.T @ P @ Ba, Ba.T @ P @ Aa); P = Q + Aa.T @ P @ (Aa - Ba @ Ka)     # LQR augmenté
x = np.zeros(3); Y2 = []
for k in range(400):
    u = float(np.clip(-Ka @ x, -12, 12)); x[:2] = Adm @ x[:2] + (Bdm * u).ravel(); x[2] += (200.0 - x[0]) * Tm
    if k > 150: x[0] -= 0.02
    Y2.append(x[0])
print(f"avec intégrateur (LQR augmenté) : après perturbation {Y2[-1]:.1f} → erreur {200 - Y2[-1]:.2f}")

Ce qu’on retient. Le placement de pôles choisit la dynamique mais pas la commande (saturation possible) ; le LQR équilibre les deux ; un intégrateur est nécessaire pour rejeter une perturbation constante (comme le I du PID). Le vrai PID de la séance 29 est un cas particulier de retour d’état sur (erreur, ∫erreur, dérivée) — la théorie explique pourquoi il marche et comment le régler sans tâtonner.

Cours

Cours 3 — Planification de mouvement : garanties, coûts, et ce que RRT ne dit pas

Méthode	Complétude	Optimalité	Coût par requête	Contraintes cinématiques	Usage
Grille + A*	Complète à résolution fixée	Optimale sur la grille	O(cellules log)	Difficiles (états augmentés)	2D mobile, Nav2
PRM (roadmap probabiliste)	Probabiliste	Asymptotique (PRM*)	Précalcul lourd, requêtes rapides	Moyennes	Bras, environnements statiques, requêtes multiples
RRT / RRT-Connect	Probabiliste	Non	Une requête, rapide	Bonnes (kinodynamic)	Bras 7 axes, planification unique
RRT* / Informed RRT*	Probabiliste	Asymptotique	Plus lent (recâblage)	Bonnes	Quand la qualité compte
Lattice (primitives de mouvement)	Complète sur le lattice	Optimale sur le lattice	Moyen	Exactes (arcs précalculés)	Voitures, Smac Planner
Optimisation de trajectoire (CHOMP, TrajOpt)	Locale	Locale	Moyen	Exactes	Lisser/raffiner un chemin initial

Le coût réel est dans la détection de collision (90 % du temps d’un planificateur) : volumes englobants hiérarchiques (AABB, sphères), distances signées précalculées (L20), et pour un bras, tester chaque segment du bras contre chaque obstacle. Un planificateur est bon d’abord parce qu’il pose peu de questions de collision (RRT-Connect : deux arbres qui se rejoignent, très efficace).

import numpy as np
rng = np.random.default_rng(0)
# RRT-Connect : deux arbres (départ, but) étendus alternativement l'un vers l'autre — beaucoup moins d'itérations que RRT
def etendre(noeuds, parents, cible, pas=0.5):
    i = int(np.argmin([np.linalg.norm(n - cible) for n in noeuds])); d = cible - noeuds[i]; dist = np.linalg.norm(d)
    nouveau = noeuds[i] + d / max(dist, 1e-9) * min(pas, dist)
    if segment_libre(noeuds[i], nouveau): noeuds.append(nouveau); parents.append(i); return len(noeuds) - 1
    return None
def rrt_connect(depart, but, n_iter=2000):
    Am, pA, Bm, pB = [np.array(depart, float)], [None], [np.array(but, float)], [None]
    for it in range(n_iter):
        q = rng.uniform(0, 10, 2); ia = etendre(Am, pA, q)
        if ia is not None:
            while True:                                                 # « connect » : avancer Bm vers le nouveau nœud de Am tant que c'est libre
                ib = etendre(Bm, pB, Am[ia])
                if ib is None: break
                if np.linalg.norm(Bm[ib] - Am[ia]) < 1e-6:
                    ca, k = [], ia
                    while k is not None: ca.append(Am[k]); k = pA[k]
                    cb, k = [], ib
                    while k is not None: cb.append(Bm[k]); k = pB[k]
                    return np.array(ca[::-1] + cb[1:]), it, len(Am) + len(Bm)
        Am, pA, Bm, pB = Bm, pB, Am, pA                                     # alterner les rôles
    return None, n_iter, len(Am) + len(Bm)
chemin, it, nn = rrt_connect((0.5, 0.5), (9.5, 9.5))
print(f"RRT-Connect : chemin en {it} itérations, {nn} nœuds, longueur {longueur(chemin):.2f} (RRT simple du cours : ~{len(noeuds)} nœuds)")

TP guidé

TP — Identification, LQR et MPC sur un vrai moteur, puis un bras planifié (sur carte + PC, 4 h)

Identification. Sur le robot (séance 27) ou un moteur seul avec encodeur : appliquez une séquence de PWM pseudo-aléatoire (PRBS) pendant 20 s, enregistrez (PWM, vitesse) à 200 Hz. Ajustez un modèle du premier ordre discret v_k+1 = a v_k + b u_k par moindres carrés (L10) ; validez sur un autre enregistrement (erreur RMS, graphe superposé). Estimez la zone morte.
LQR. Sur le modèle identifié (état augmenté avec l’intégrale de l’erreur), calculez K par Riccati pour trois couples (Q, R) ; simulez avec saturation ; choisissez. Implémentez sur la carte (C, virgule flottante ; le calcul de K se fait hors ligne, seul u = −Kx tourne en temps réel). Comparez au PID de la séance 29 : temps de réponse, dépassement, rejet d’une perturbation (freiner la roue à la main).
MPC hors ligne puis en ligne. Sur PC : MPC avec contrainte de tension et de pente de commande (Δu ≤ 2 V par pas, pour ménager le pont en H) via cvxpy ou osqp ; comparez au LQR saturé. Puis MPC « explicite » : tabulez la loi u(x) sur une grille et embarquez la table sur la carte (interpolation) — le MPC tourne en 10 µs.
Bras et planification. Un bras 2 ou 3 axes (servos) : cinématique directe/inverse ; RRT-Connect dans l’espace des angles avec des obstacles définis dans l’espace de travail ; lissage ; exécution sur les servos avec des profils trapézoïdaux de vitesse (pas de sauts). Mesurez l’erreur de position finale à la règle.
Sécurité. Bouclier : vitesse max fonction de la distance lidar/ultrason ; arrêt si le microcontrôleur ne reçoit plus de commande depuis 200 ms ; test volontaire des deux.
Livrable. Dépôt : données d’identification et modèle validé, scripts LQR/MPC avec figures, firmware, vidéo du bras, tableau comparatif PID / LQR / MPC.

Exercices

Exercices auto-corrigés — cinématique et planification

Exercice 1 — Odométrie différentielle et son incertitude

odometrie(pose, ticks_g, ticks_d) : nouvelle pose à partir des ticks d’encodeurs (résolution TICKS_PAR_TOUR, rayon R_ROUE, entraxe L_ESSIEU), par intégration sur un arc exact. Puis incertitude(poses) : simulez 500 trajets « tout droit 2 m » avec ±2 % d’erreur aléatoire sur chaque roue et renvoyez l’écart-type latéral final — il doit être bien plus grand que l’écart-type longitudinal.

Correction

def odometrie(pose, tg, td):
    dg, dd = 2 * np.pi * R_ROUE * tg / TICKS_PAR_TOUR, 2 * np.pi * R_ROUE * td / TICKS_PAR_TOUR
    d, dth = (dg + dd) / 2, (dd - dg) / L_ESSIEU; x, y, th = pose
    if abs(dth) < 1e-9: return np.array([x + d * np.cos(th), y + d * np.sin(th), th])
    Rc = d / dth; return np.array([x + Rc * (np.sin(th + dth) - np.sin(th)), y - Rc * (np.cos(th + dth) - np.cos(th)), th + dth])
def incertitude(n=500, distance=2.0, pas=0.05, erreur=0.02):
    ticks = pas / (2 * np.pi * R_ROUE) * TICKS_PAR_TOUR; finales = []
    for _ in range(n):
        pose = np.zeros(3)
        for _ in range(int(distance / pas)):
            pose = odometrie(pose, ticks * (1 + rng.normal(0, erreur)), ticks * (1 + rng.normal(0, erreur)))
        finales.append(pose[:2])
    finales = np.array(finales); return finales[:, 0].std(), finales[:, 1].std()

Exercice 2 — Profil trapézoïdal de vitesse

profil(distance, v_max, a_max, dt) renvoie les positions échantillonnées d’un mouvement qui accélère à a_max, croise à v_max, décélère à a_max, et s’arrête exactement à distance ; cas triangulaire si v_max n’est pas atteint. C’est ce que fait tout contrôleur de servo/CNC.

Correction

def profil(distance, v_max, a_max, dt=0.01):
    v_pic = min(v_max, np.sqrt(distance * a_max)); t_acc = v_pic / a_max; d_acc = 0.5 * a_max * t_acc**2
    t_cro = (distance - 2 * d_acc) / v_pic; T = 2 * t_acc + t_cro; t = np.arange(0, T + dt, dt); pos = np.zeros_like(t)
    for i, ti in enumerate(t):
        if ti < t_acc: pos[i] = 0.5 * a_max * ti**2
        elif ti < t_acc + t_cro: pos[i] = d_acc + v_pic * (ti - t_acc)
        else: tau = min(T, ti) - t_acc - t_cro; pos[i] = d_acc + v_pic * t_cro + v_pic * tau - 0.5 * a_max * tau**2
    pos[-1] = distance; return pos

Exercices

Exercices auto-corrigés — contrôle

Exercice 3 — Régler un PID discret par la méthode et par l’expérience

Sur le modèle de moteur discret v_k+1 = 0,98 v_k + 0,4 u_k (u en volts, saturé ±12), écrivez simuler_pid(Kp, Ki, Kd, consigne, n) (PID discret avec anti-emballement de l’intégrale par écrêtage) et meilleur_pid(grille) qui choisit les gains minimisant ∫|erreur| + pénalité de dépassement > 5 %. Le gagnant doit atteindre 95 % de la consigne en moins de 40 pas sans dépasser 5 %.

Correction

def simuler_pid(Kp, Ki, Kd, consigne=100.0, n=300):
    v, I, e_prec, V = 0.0, 0.0, 0.0, []
    for _ in range(n):
        e = consigne - v; I = np.clip(I + e, -2000, 2000); u = np.clip(Kp * e + Ki * I + Kd * (e - e_prec), -12, 12)
        e_prec = e; v = 0.98 * v + 0.4 * u; V.append(v)
    return np.array(V)
def meilleur_pid(grille):
    def cout(g):
        v = simuler_pid(*g); return np.abs(100 - v).sum() + 1e4 * (v.max() > 105) + 1e4 * (abs(v[-1] - 100) > 1)
    return min(grille, key=cout)

Exercice 4 — Observateur de Luenberger : estimer une vitesse qu’on ne mesure pas

Pour le moteur du cours 1 (états ω, i ; on ne mesure que ω), écrivez observateur(Ad, Bd, C, poles) qui renvoie le gain L (placement de pôles sur le système dual : Aᵀ, Cᵀ) et simuler_observateur(L) qui fait tourner en parallèle le vrai système et l’estimateur x̂_k+1 = A_d x̂_k + B_d u_k + L (y_k − C x̂_k) à partir d’une estimation initiale fausse ; l’erreur d’estimation du courant doit converger vers 0 en quelques dizaines de pas.

Correction

def observateur(Adm, Bdm, Cm, poles): return ackermann(Adm.T, Cm.T, poles).T
def simuler_observateur(L, n=200):
    x = np.array([50.0, 1.0]); xh = np.array([0.0, 0.0]); err = []
    for k in range(n):
        u = 6.0; y = Cm @ x
        xh = Adm @ xh + (Bdm * u).ravel() + (L @ (y - Cm @ xh)).ravel(); x = Adm @ x + (Bdm * u).ravel()
        err.append(abs(x[1] - xh[1]))
    return np.array(err)

Le principe de séparation dit qu’on peut concevoir le contrôleur (K) et l’observateur (L) indépendamment : les pôles du système bouclé sont ceux de A − BK et de A − LC réunis. Avec du bruit, l’observateur optimal est… le filtre de Kalman (L19) : L est alors le gain de Kalman stationnaire.

05 / Défis

Défi ★ — RRT pour le bras, et Ackermann

Consigne

1) Faites tourner le RRT dans l’espace des configurations du bras à 2 segments (avec la fonction collision) d’une configuration de départ à une configuration but ; animez (ou tracez 10 poses intermédiaires) dans l’espace de travail. 2) Écrivez la cinématique d’une voiture (Ackermann : ẋ = v cos θ, ẏ = v sin θ, θ̇ = v tan φ / L) et montrez qu’un pure pursuit y fonctionne avec φ = arctan(2 L sin α / L_lookahead). 3) Quel est le rayon de braquage minimal pour L = 0,3 m et φ_max = 30° ?

Piste

Pour le bras : les nœuds sont des (θ1, θ2) ; libre devient not collision(q) ; les angles sont périodiques (distance modulo 2π). Rayon minimal : R = L / tan φ_max ≈ 0,52 m.

05 / Défis

Défi ★★ — LQR sur le vrai pendule non linéaire, avec observateur

Consigne

1) Appliquez le K du LQR (calculé sur le modèle linéarisé) au pendule non linéaire (sin θ) simulé par RK4 (module L12) : jusqu’à quel angle initial le contrôleur récupère-t-il (bassin d’attraction) ? 2) Supposez qu’on ne mesure que l’angle (bruité, σ = 0,01 rad) et pas la vitesse : ajoutez un filtre de Kalman (module L19) pour estimer l’état et bouclez sur l’estimation (principe de séparation). Comparez à une dérivée numérique brute de l’angle. 3) Ajoutez un retard de 40 ms dans la mesure : à partir de quel retard le système devient-il instable ? Expliquez avec les pôles (le retard ajoute des états).

Piste

Bassin d’attraction avec u_max = 10 N : environ ±0,35 rad ; il chute avec une saturation plus stricte. Le retard : modéliser z_k = x_{k−d} en augmentant l’état de d copies ; les pôles se rapprochent du cercle unité ; typiquement instable au-delà de ~80-100 ms pour ce pendule rapide. C’est pourquoi la latence caméra → commande est LA contrainte de conception des drones et des robots agiles.

05 / Défis

Défi ★★★ — Esprit prépa : commande optimale par principe de Pontryagin et MPC convexe

Consigne

1) Problème à temps minimal : amener un chariot (ẍ = u, |u| ≤ 1) de (x₀, 0) à (0, 0) le plus vite possible. Montrez (principe du maximum de Pontryagin, ou raisonnement direct) que la commande optimale est bang-bang : +1 puis −1 avec une seule commutation, sur la courbe x = −½ ẋ|ẋ|. Implémentez ce contrôleur et comparez son temps à celui du LQR. 2) Formulez le MPC du pendule comme un programme quadratique (variables : la séquence u, contraintes linéaires |u| ≤ u_max) et résolvez-le par la méthode du gradient projeté accéléré (Nesterov) ou par une implémentation maison d’ADMM ; mesurez le temps par pas et comparez à 20 ms. 3) Lisez l’introduction de Underactuated Robotics (Tedrake, chapitres 1-3) et expliquez pourquoi un pendule simple ne peut pas être stabilisé en haut avec un couple trop faible sans balancer (swing-up par énergie) : concevez ce swing-up.

Piste (bang-bang)

def bang_bang(x, v):
    """Courbe de commutation : s = x + 0.5 * v * abs(v). Au-dessus : freiner (−1), en dessous : accélérer (+1)."""
    s = x + 0.5 * v * abs(v)
    if abs(s) < 1e-3 and abs(v) < 1e-3: return 0.0
    return -1.0 if s > 0 else 1.0
x, v, t, dt = 3.0, 0.0, 0.0, 0.001
while abs(x) + abs(v) > 1e-2:
    u = bang_bang(x, v); v += u * dt; x += v * dt; t += dt
print(f"temps minimal ≈ {t:.2f} s (théorie : 2√3 ≈ {2 * np.sqrt(3):.2f})")

Le swing-up par énergie : u = k (E − E_haut) · signe(θ̇ cos θ) pompe de l’énergie jusqu’à approcher l’équilibre haut, puis on bascule sur le LQR quand |θ − π| < 0,3. Ce « contrôle hybride » (deux contrôleurs et une règle de commutation) est la solution classique et un beau sujet d’oral.

06 / Vérification

Un système discret bouclé x_{k+1} = (A − BK) x_k est stable si :

Deux questions supplémentaires

1. Pourquoi planifier dans l’espace des configurations ? Parce que le robot y est un point ; les collisions deviennent des régions à éviter et les algorithmes de graphe/échantillonnage s’appliquent.

2. Que fait un MPC qu’un LQR ne fait pas ? Respecter des contraintes (saturation, obstacles) et anticiper une référence future.

Référence

Les mots à retenir

Mot	Définition
Cinématique directe / inverse	Commandes → mouvement / mouvement voulu → commandes.
Non holonome	Contrainte sur les vitesses (pas de déplacement latéral).
Espace des configurations	Espace des paramètres du robot (angles) ; le robot y est un point.
RRT / RRT*	Arbre aléatoire ; probabilistement complet / asymptotiquement optimal.
Fenêtre dynamique	Choix de (v, ω) par simulation courte et score.
Pure pursuit	Suivi géométrique par point d’anticipation.
Pôles	Valeurs propres du système bouclé ; \|λ\| < 1 en discret.
LQR / Riccati	Retour d’état optimal pour coût quadratique.
MPC	Optimisation sur horizon glissant avec contraintes.
Bang-bang	Commande saturée à temps minimal.
Bouclier de sécurité	Filtre qui garantit un ensemble sûr quelle que soit la commande demandée.

Pour continuer

Le robot bouge comme il faut

Module suivant : communication et réseaux — bus embarqués (SPI, I2C, CAN), protocoles, sockets, MQTT, sécurité : faire parler les composants entre eux et avec le monde.

À faire chez soi

Suivre les chapitres 1-4 de Underactuated Robotics (avec les notebooks Drake).
Sur robot : remplacer le PID de la séance 29 par un LQR sur le modèle identifié (vitesse en fonction de la PWM : identification par moindres carrés, module L10).
Implémenter RRT* et comparer la longueur des chemins à RRT sur 50 problèmes aléatoires.

← L20 Sommaire L22 : Communication et réseaux →