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

Où suis-je ? Localisation, cartographie, SLAM, et l’architecture d’un robot.

Un robot autonome doit répondre à trois questions : où suis-je (localisation), à quoi ressemble le monde (cartographie), et comment faire les deux en même temps sans connaître ni l’un ni l’autre (SLAM — le problème qui a occupé la robotique pendant 30 ans). Ce module construit un filtre particulaire, une carte d’occupation, un SLAM par graphe, puis présente ROS 2, le système qui assemble tout cela sur un vrai robot.

Durée : 4 séances · Prérequis : L05 (graphes, A*), L11 (Bayes), L19 (EKF). Objectifs : localisation de Markov et filtre particulaire (MCL), modèle de télémètre, cartes d’occupation (log-odds), scan matching (ICP, idée), SLAM par graphe (optimisation de poses), fermeture de boucle, ROS 2 (nœuds, topics, services, tf2, Nav2), et où en est la recherche (SLAM visuel, cartes sémantiques).

Ce que vous saurez faire à la fin

Localiser un robot simulé dans une carte connue avec 500 particules et un lidar.
Construire une carte d’occupation à partir de scans et de poses.
Formuler et résoudre un petit SLAM par graphe avec Gauss-Newton.
Lire l’architecture ROS 2 d’un robot et écrire un nœud.

Références : Probabilistic Robotics chapitres 4-8 et 9-11, cours « SLAM » de Cyrill Stachniss (YouTube, Bonn), documentation ROS 2 et Nav2, « A Tutorial on Graph-Based SLAM » (Grisetti et al.).

Fiche de cours · Définitions

Localisation, cartographie, ROS : définitions

Définition (pose, repères, odométrie). Pose 2D (x, y, θ) ; 3D (position + rotation). Repères : monde (map), odométrie (odom), robot (base_link), capteurs. L’odométrie intègre les déplacements mesurés (encodeurs, IMU) : précise à court terme, dérive sans borne à long terme.

Définition (localisation, cartographie, SLAM). Localisation : estimer la pose dans une carte connue. Cartographie : construire la carte avec des poses connues. SLAM : les deux simultanément — un problème « œuf et poule » résolu probabilistiquement (EKF-SLAM, FastSLAM, graphe de poses).

Définition (modèles de mouvement et de mesure). Modèle de mouvement p(x_t | x_t−1, u_t) : odométrie bruitée (translation, rotations). Modèle de mesure p(z_t | x_t, m) : pour un lidar, produit sur les rayons d’une vraisemblance (gaussienne autour de la distance attendue, obtenue par lancer de rayon dans la carte, plus composantes pour obstacles inattendus, ratés, aléatoire).

Définition (localisation de Monte-Carlo, MCL). Filtre particulaire sur la pose : N particules (x_i, w_i) ; prédiction par le modèle de mouvement échantillonné, pondération par la vraisemblance des mesures, rééchantillonnage proportionnel aux poids. Estimation = moyenne pondérée (ou particule de plus grand poids).

Définition (grille d’occupation). Carte = grille de cellules avec p(occupée). Mise à jour en log-odds ℓ = log(p/(1 − p)) : ℓ ← ℓ + ℓ_mesure − ℓ₀, additive et sans saturation prématurée.

Définition (graphe de poses, ICP). Sommets = poses successives, arêtes = contraintes relatives (odométrie, fermetures de boucle) avec covariances ; on minimise Σ ‖e_ij‖²_{Ω_ij} (moindres carrés non linéaires). ICP (Iterative Closest Point) : aligner deux nuages de points en alternant appariement au plus proche et transformation rigide optimale.

Définition (ROS 2). Middleware : nœuds (processus), topics (publication/abonnement, typés), services (requête/réponse), actions (tâches longues avec retour), paramètres, tf2 (arbre des repères horodatés), bags (enregistrement), launch (démarrage). Communication par DDS avec QoS.

Fiche de cours · Formules

Formules à connaître

Odométrie différentielle : Δs = (Δs_d + Δs_g)/2, Δθ = (Δs_d − Δs_g)/L ; x += Δs cos(θ + Δθ/2), y += Δs sin(θ + Δθ/2), θ += Δθ

Composition de poses (⊕) : (x₁, y₁, θ₁) ⊕ (x₂, y₂, θ₂) = (x₁ + x₂cos θ₁ − y₂sin θ₁, y₁ + x₂sin θ₁ + y₂cos θ₁, θ₁ + θ₂) — matrice homogène T₁T₂

Filtre bayésien : bel⁻(x) = ∫ p(x | x′, u) bel(x′) dx′ ; bel(x) = η p(z | x) bel⁻(x)

Log-odds : ℓ = log(p/(1−p)), p = 1/(1 + e^−ℓ) ; cellule traversée : ℓ −= ℓ_libre ; cellule touchée : ℓ += ℓ_occ

Vraisemblance lidar (modèle « beam ») : p(z | x, m) = Π_k [α_hit N(z_k; z*_k, σ²) + α_short·… + α_max·1_{z=z_max} + α_rand/z_max] en log, une somme ; sous-échantillonner les rayons pour éviter une confiance excessive

N_eff = 1/Σ_i w_i² ; rééchantillonner si N_eff < N/2 (rééchantillonnage systématique à faible variance)

Graphe de poses : min_x Σ_(i,j) e_ij(x)ᵀ Ω_ij e_ij(x), e_ij = (x_i⁻¹ ⊕ x_j) ⊖ z_ij ; Gauss-Newton : (JᵀΩJ)Δx = −JᵀΩe

ICP (rigide 2D) : centrer les nuages, H = Σ p_iq_iᵀ, SVD H = UΣVᵀ, R = VUᵀ (det = +1), t = q̄ − R p̄

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

Démonstrations à savoir refaire (1/2)

Théorème 1 (dérivation du filtre bayésien). Sous l’hypothèse de Markov (x_t ne dépend que de x_t−1 et u_t ; z_t ne dépend que de x_t), bel(x_t) = η p(z_t | x_t) ∫ p(x_t | x_t−1, u_t) bel(x_t−1) dx_t−1.

bel(x_t) = p(x_t | z_1:t, u_1:t). Bayes sur z_t : = η p(z_t | x_t, z_1:t−1, u_1:t) p(x_t | z_1:t−1, u_1:t). Markov des mesures : le premier facteur est p(z_t | x_t). Pour le second (bel⁻), on marginalise sur x_t−1 : ∫ p(x_t | x_t−1, z_1:t−1, u_1:t) p(x_t−1 | z_1:t−1, u_1:t) dx_t−1 ; Markov de l’état réduit le premier terme à p(x_t | x_t−1, u_t), et le second ne dépend pas de u_t (la commande future n’informe pas sur l’état passé) : c’est bel(x_t−1). Tous les filtres (histogramme, Kalman, particulaire) sont des implémentations de cette équation avec des représentations différentes de bel.

Théorème 2 (mise à jour en log-odds d’une grille d’occupation). Si les mesures sont conditionnellement indépendantes sachant l’état de la cellule, la mise à jour bayésienne de p(occ | z_1:t) s’écrit ℓ_t = ℓ_t−1 + log [p(occ | z_t)/(1 − p(occ | z_t))] − ℓ₀.

Bayes : p(occ | z_1:t) = p(z_t | occ) p(occ | z_1:t−1)/p(z_t | z_1:t−1), et de même pour « libre ». Le quotient occ/libre élimine le dénominateur : odds_t = [p(z_t | occ)/p(z_t | libre)]·odds_t−1. Par Bayes encore, p(z_t | occ)/p(z_t | libre) = [p(occ | z_t)/p(libre | z_t)]·[p(libre)/p(occ)]. En passant au log : ℓ_t = ℓ_t−1 + ℓ(z_t) − ℓ₀. Le modèle inverse ℓ(z_t) est une constante par cas (rayon traversant : ℓ_libre < 0 ; extrémité : ℓ_occ > 0). Additif, sans multiplication ni normalisation : idéal en temps réel.

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

Démonstrations à savoir refaire (2/2)

Théorème 3 (transformation rigide optimale — Kabsch/Procrustes). Étant donné des paires appariées (p_i, q_i), la rotation R et la translation t minimisant Σ‖Rp_i + t − q_i‖² sont t = q̄ − Rp̄ et R = VUᵀ où UΣVᵀ est la SVD de H = Σ(p_i − p̄)(q_i − q̄)ᵀ (avec correction de signe si det < 0).

Dérivée en t : Σ(Rp_i + t − q_i) = 0 ⇒ t = q̄ − Rp̄. En substituant et en centrant (p′_i = p_i − p̄, q′_i = q_i − q̄), il reste à minimiser Σ‖Rp′_i − q′_i‖² = Σ‖p′_i‖² + Σ‖q′_i‖² − 2Σ q′_iᵀRp′_i (R conserve les normes). Il faut donc maximiser Σ q′_iᵀRp′_i = tr(RH) avec H = Σ p′_iq′_iᵀ. Avec H = UΣVᵀ : tr(RUΣVᵀ) = tr(ΣVᵀRU) = Σ σ_kM_kk où M = VᵀRU est orthogonale, donc |M_kk| ≤ 1 ; le maximum Σσ_k est atteint pour M = I, soit R = VUᵀ. Si det(VUᵀ) = −1 (réflexion), prendre V·diag(1, …, 1, −1)·Uᵀ. C’est le cœur d’ICP : une étape d’appariement, une SVD 2×2 ou 3×3, répéter.

Théorème 4 (ICP décroît l’erreur et converge). En alternant (a) appariement de chaque p_i à son plus proche q, (b) transformation optimale pour ces appariements, l’erreur Σ‖Rp_i + t − q_π(i)‖² décroît à chaque itération et converge (vers un minimum local).

L’étape (a), à transformation fixée, choisit pour chaque point le q qui minimise sa distance : l’erreur ne peut que diminuer. L’étape (b), à appariement fixé, calcule la transformation qui minimise l’erreur (Théorème 3) : elle diminue encore. La suite des erreurs est décroissante et minorée par 0 : elle converge. Comme il n’y a qu’un nombre fini d’appariements possibles, l’algorithme se stabilise. Rien ne garantit le minimum global : ICP a besoin d’une initialisation proche (odométrie) — sinon il converge vers un alignement faux, symétrie ou glissement le long d’un couloir.

Théorème 5 (une fermeture de boucle borne la dérive). Dans un graphe de poses, ajouter une contrainte entre x₀ et x_n avec covariance Σ_b rend l’incertitude relative de x_n par rapport à x₀ ≤ Σ_b, quelle que soit la dérive accumulée par les n arêtes d’odométrie.

L’information (inverse de la covariance) s’additionne pour des mesures indépendantes (Théorème 1 de L19, forme matricielle) : Ω_total = Ω_odom + Ω_boucle, où Ω_odom (chaîne de n arêtes) est petite (covariance somme de n termes) et Ω_boucle = Σ_b⁻¹. Donc Σ_total = (Ω_odom + Σ_b⁻¹)⁻¹ ≼ Σ_b. L’optimisation répartit la correction sur toutes les poses de la boucle (pondérées par leurs covariances) — c’est le « claquement » de la carte lors d’une fermeture de boucle.

Fiche de cours · Méthodes

Méthodes et pièges

Méthode — mettre en œuvre MCL. Initialisation uniforme (ou autour d’une pose connue). À chaque pas : déplacer chaque particule avec l’odométrie plus du bruit (sinon les particules s’effondrent) ; calculer la vraisemblance avec un sous-ensemble de rayons ; normaliser ; rééchantillonner si N_eff faible ; injecter quelques particules aléatoires (robot kidnappé). Diagnostiquer : la dispersion des particules doit refléter l’ambiguïté réelle (couloir symétrique → deux nuages).

Méthode — architecture ROS 2 d’un robot mobile. Nœuds : pilotes (moteurs, lidar, IMU) → odométrie (tf odom→base_link) → localisation (map→odom) → carte → planificateur global (path) → contrôleur local (cmd_vel) → sécurité (arrêt si obstacle proche, priorité sur cmd_vel). Un topic par donnée, horodatage sur tout, QoS « sensor data » pour les capteurs, « reliable » pour les commandes.

Méthode — déboguer une localisation. Rejouer un bag ; visualiser particules, scan et carte dans RViz ; vérifier tf (arbre complet, horodatages cohérents, pas de saut) ; comparer à une vérité terrain (marqueurs, capture de mouvement) avec l’erreur absolue de trajectoire (ATE) et l’erreur relative (RPE).

Pièges : angles non normalisés ; multiplier 360 vraisemblances de rayons (produit → 0, confiance excessive : sous-échantillonner ou lisser) ; carte et scan dans des repères différents (map vs odom) ; horodatages du lidar et de l’odométrie non synchronisés (à 1 m/s, 50 ms = 5 cm) ; ICP sans initialisation ; fermeture de boucle fausse (deux couloirs identiques) qui détruit la carte.

Fiche de cours · Exercices corrigés

Exercices corrigés

Exercice 1. Robot différentiel, entraxe L = 0,3 m, encodeurs mesurant Δs_g = 0,10 m et Δs_d = 0,13 m sur un pas, pose (1, 2, 0). Nouvelle pose ? Si chaque Δs est connu à ±1 mm, incertitude sur Δθ, et dérive angulaire après 1 000 pas ?

Correction. Δs = 0,115 m, Δθ = 0,03/0,3 = 0,1 rad. x = 1 + 0,115 cos(0,05) = 1,1149 ; y = 2 + 0,115 sin(0,05) = 2,0057 ; θ = 0,1 rad. Incertitude : σ_Δθ = √(σ_d² + σ_g²)/L = √2·10⁻³/0,3 = 4,7 mrad (propagation, Théorème 3 de L19). Après 1 000 pas indépendants : σ_θ = 4,7·√1000 mrad = 0,15 rad ≈ 8,5°, et cette erreur d’angle se traduit en erreur de position ≈ distance × 0,15 = 17 m pour 115 m parcourus. C’est pourquoi l’odométrie seule ne suffit jamais : il faut des mesures absolues (lidar contre carte, fermetures de boucle).

Exercice 2. Une cellule de grille a p = 0,5 initialement. Le modèle inverse donne p(occ | rayon s’arrête ici) = 0,7 et p(occ | rayon traverse) = 0,3. Après 3 « touchés » et 1 « traversé », quelle est p ? Combien de « touchés » pour dépasser 0,99 ?

Correction. ℓ₀ = 0 ; ℓ_occ = log(0,7/0,3) = 0,847 ; ℓ_libre = log(0,3/0,7) = −0,847. ℓ = 3·0,847 − 0,847 = 1,695 ⇒ p = 1/(1 + e^−1,695) = 0,845. Pour 0,99 : ℓ ≥ log(99) = 4,595 ⇒ 4,595/0,847 = 5,4 → 6 touchés nets. En pratique on borne ℓ (par exemple ±5) pour que la carte reste révisable si l’environnement change (porte qui s’ouvre).

Exercice 3. Deux nuages 2D : P = {(0, 0), (1, 0), (0, 1)} et Q = {(1, 1), (1, 2), (0, 1)} (Q est P tourné puis translaté, points appariés dans l’ordre). Trouver R et t par la méthode SVD.

Correction. Centres : p̄ = (1/3, 1/3), q̄ = (2/3, 4/3). Centrés : p′ = {(−1/3, −1/3), (2/3, −1/3), (−1/3, 2/3)}, q′ = {(1/3, −1/3), (1/3, 2/3), (−2/3, −1/3)}. H = Σ p′q′ᵀ = [[Σ p′_xq′_x, Σ p′_xq′_y], [Σ p′_yq′_x, Σ p′_yq′_y]] = [[−1/9 + 2/9 + 2/9, 1/9 + 4/9 + 1/9], [−1/9 − 1/9 − 4/9, 1/9 − 2/9 − 2/9]] = [[1/3, 2/3], [−2/3, −1/3]]. H est (1/3)·[[1, 2], [−2, −1]] ; on cherche R orthogonale maximisant tr(RH). Essayons R = rotation de 90° = [[0, −1], [1, 0]] : RH = (1/3)[[2, 1], [1, 2]], trace 4/3 ; c’est le maximum (= σ₁ + σ₂ car HᵀH a pour trace 10/9… vérification : les valeurs singulières de H sont 1 et 1/3, somme 4/3 ✓). Donc R = R(90°), t = q̄ − Rp̄ = (2/3, 4/3) − (−1/3, 1/3) = (1, 1). Vérification : R(0, 0) + t = (1, 1) ✓, R(1, 0) + t = (0 + 1, 1 + 1) = (1, 2) ✓, R(0, 1) + t = (−1 + 1, 0 + 1) = (0, 1) ✓.

01 / Localiser

Le monde simulé : une carte, un robot, un lidar

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(0)

CARTE = ["####################",
         "#........#.........#",
         "#..##....#....###..#",
         "#..##.........#....#",
         "#........###..#....#",
         "#####.........#....#",
         "#.....##...........#",
         "#..#.........####..#",
         "#..#....#..........#",
         "####################"]
grille = np.array([[c == "#" for c in l] for l in CARTE]); H, W = grille.shape   # 1 case = 1 m
def occupe(x, y): return not (0 <= x < W and 0 <= y < H) or grille[int(y), int(x)]

def lidar(pose, n_rayons=16, portee=8.0, pas=0.1, bruit=0.1):
    """Distance au premier mur pour n_rayons angles répartis sur 360°, par lancer de rayon."""
    x, y, th = pose; angles = th + np.linspace(0, 2 * np.pi, n_rayons, endpoint=False); d = []
    for a in angles:
        r = 0.0
        while r < portee and not occupe(x + r * np.cos(a), y + r * np.sin(a)): r += pas
        d.append(min(r, portee))
    return np.array(d) + rng.normal(0, bruit, n_rayons)

def deplacer(pose, v, w, dt=1.0, bruit=(0.1, 0.05)):
    x, y, th = pose; v += rng.normal(0, bruit[0]); w += rng.normal(0, bruit[1])
    return np.array([x + v * dt * np.cos(th), y + v * dt * np.sin(th), th + w * dt])

pose_vraie = np.array([2.5, 1.5, 0.0])
scan = lidar(pose_vraie)
print("scan (16 distances en m) :", scan.round(1))
plt.figure(figsize=(7, 3.5)); plt.imshow(grille, cmap="gray_r", origin="lower", extent=(0, W, 0, H))
for a, d in zip(np.linspace(0, 2 * np.pi, 16, endpoint=False), scan):
    plt.plot([pose_vraie[0], pose_vraie[0] + d * np.cos(a)], [pose_vraie[1], pose_vraie[1] + d * np.sin(a)], "y-", lw=0.6)
plt.plot(*pose_vraie[:2], "ro"); plt.title("le robot et son lidar"); plt.show()

Le lancer de rayon est le modèle de capteur : « si j’étais en (x, y, θ), que verrais-je ? ». Toute la localisation probabiliste repose sur la comparaison entre ce que le robot verrait à une pose hypothétique et ce qu’il voit vraiment.

01 / Localiser

Filtre particulaire (Monte-Carlo Localization) : 300 hypothèses qui votent

import numpy as np
import matplotlib.pyplot as plt
N = 300
def particules_initiales(N):
    """Le robot ne sait pas où il est : particules uniformes sur les cases libres (« robot kidnappé »)."""
    P = []
    while len(P) < N:
        x, y = rng.uniform(0, W), rng.uniform(0, H)
        if not occupe(x, y): P.append([x, y, rng.uniform(-np.pi, np.pi)])
    return np.array(P)

def vraisemblance(scan, pose, sigma=0.4):
    """P(scan | pose) : gaussienne sur l'écart entre scan réel et scan attendu (modèle simplifié, rayons indépendants)."""
    attendu = lidar(pose, bruit=0.0)
    return np.exp(-0.5 * np.sum((scan - attendu)**2) / sigma**2) + 1e-300

def mcl_pas(P, v, w, scan):
    P = np.array([deplacer(p, v, w) for p in P])                           # 1) prédiction : bouger chaque particule (avec bruit)
    P = np.array([p if not occupe(p[0], p[1]) else p for p in P])
    poids = np.array([vraisemblance(scan, p) if not occupe(p[0], p[1]) else 0 for p in P])   # 2) correction : pondérer
    poids /= poids.sum()
    idx = rng.choice(N, N, p=poids)                                        # 3) rééchantillonnage : les bonnes hypothèses se multiplient
    P = P[idx] + rng.normal(0, [0.05, 0.05, 0.02], (N, 3))                 # petit bruit pour garder de la diversité
    return P, poids

P = particules_initiales(N); pose = pose_vraie.copy(); commandes = [(1, 0), (1, 0), (1, 0.5), (1, 0.5), (1, 0), (1, 0), (1, -0.6), (1, 0), (1, 0), (1, 0)]
fig, axes = plt.subplots(2, 5, figsize=(12, 4.2))
for k, (v, w) in enumerate(commandes):
    pose = deplacer(pose, v, w, bruit=(0.05, 0.02)); scan = lidar(pose)
    P, poids = mcl_pas(P, v, w, scan)
    est = P.mean(0); ax = axes.flat[k]
    ax.imshow(grille, cmap="gray_r", origin="lower", extent=(0, W, 0, H)); ax.plot(P[:, 0], P[:, 1], ".", ms=1, alpha=0.4)
    ax.plot(pose[0], pose[1], "ro", ms=4); ax.set_title(f"t={k+1} err {np.hypot(*(est[:2] - pose[:2])):.2f} m", fontsize=8); ax.axis("off")
plt.tight_layout(); plt.show()

Pourquoi des particules

Le filtre de Kalman suppose une croyance gaussienne : une seule bosse. Un robot kidnappé dans un couloir symétrique a une croyance à plusieurs bosses, et la carte rend le modèle de mesure violemment non linéaire. Le filtre particulaire (module L11 : Bayes séquentiel) représente n’importe quelle distribution par des échantillons : prédiction = bouger les particules, correction = les pondérer, rééchantillonnage = concentrer. Coût : O(N × rayons) par pas — d’où les astuces réelles : rééchantillonner seulement quand les poids dégénèrent (N_eff), rayons sous-échantillonnés, tables de distance précalculées, KLD-sampling pour adapter N. C’est l’algorithme d’AMCL dans ROS, utilisé par des dizaines de milliers de robots.

02 / Cartographier

Carte d’occupation : chaque case accumule des preuves (log-odds)

import numpy as np
import matplotlib.pyplot as plt
# Hypothèse : on connaît les poses (par MCL ou GPS). On construit la carte à partir des scans.
RES = 0.25; GW, GH = int(W / RES), int(H / RES)
L = np.zeros((GH, GW))                              # log-odds : 0 = inconnu, >0 occupé, <0 libre
l_occ, l_libre = np.log(0.7 / 0.3), np.log(0.3 / 0.7)   # le lidar a raison 70 % du temps (modèle inverse simple)

def integrer_scan(L, pose, scan, angles, portee=8.0):
    x, y, th = pose
    for a, d in zip(th + angles, scan):
        n = int(d / (RES / 2))
        for k in range(n):                                              # cases traversées par le rayon : libres
            px, py = x + k * RES / 2 * np.cos(a), y + k * RES / 2 * np.sin(a)
            i, j = int(py / RES), int(px / RES)
            if 0 <= i < GH and 0 <= j < GW: L[i, j] += l_libre * 0.5
        if d < portee - 0.1:                                             # case d'impact : occupée
            px, py = x + d * np.cos(a), y + d * np.sin(a); i, j = int(py / RES), int(px / RES)
            if 0 <= i < GH and 0 <= j < GW: L[i, j] += l_occ
    np.clip(L, -10, 10, out=L)                                           # borner : sinon la carte ne peut plus changer d'avis
    return L

angles = np.linspace(0, 2 * np.pi, 48, endpoint=False)
pose = np.array([2.5, 1.5, 0.0]); poses = []
for v, w in [(1, 0)] * 3 + [(1, 0.5)] * 3 + [(1, 0)] * 4 + [(1, -0.8)] * 2 + [(1, 0)] * 6 + [(1, 0.7)] * 3 + [(1, 0)] * 5:
    pose = deplacer(pose, v, w, bruit=(0.02, 0.01)); poses.append(pose.copy())
    L = integrer_scan(L, pose, lidar(pose, 48, bruit=0.05), angles)
prob = 1 / (1 + np.exp(-L))                                              # retour en probabilités
fig, axes = plt.subplots(1, 2, figsize=(10, 3.5))
axes[0].imshow(grille, cmap="gray_r", origin="lower"); axes[0].set_title("vraie carte")
axes[1].imshow(prob, cmap="gray_r", origin="lower", vmin=0, vmax=1); axes[1].plot(np.array(poses)[:, 0] / RES, np.array(poses)[:, 1] / RES, "r.-", ms=3, lw=0.5); axes[1].set_title("carte d'occupation construite (gris = inconnu)")
plt.show()

Log-odds : additionner au lieu de multiplier

Bayes met à jour P(occupé | mesures) par des produits ; en log-odds l = ln(p/(1−p)), chaque mesure ajoute une constante : rapide, stable, et l’inconnu vaut 0. La carte d’occupation (Moravec & Elfes, 1985) est la représentation standard pour la navigation 2D ; en 3D, OctoMap (octrees) ; pour les robots récents, des cartes de distances signées (TSDF) ou des champs neuronaux (NeRF). Sa limite : elle suppose les poses connues. Si les poses viennent de l’odométrie, la carte se déforme avec la dérive — d’où le SLAM.

03 / SLAM

Le problème de la poule et de l’œuf, et le SLAM par graphe

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(1)
# Le robot fait un tour (20 poses). Il mesure : l'odométrie entre poses successives (bruitée, dérive)
# et, quand il revient près du départ, une « fermeture de boucle » : la pose 19 voit la pose 0.
n = 20
vraies = np.array([[5 * np.cos(2 * np.pi * k / n) + 5, 5 * np.sin(2 * np.pi * k / n) + 5, 2 * np.pi * k / n + np.pi / 2] for k in range(n)])
def relatif(a, b):                      # transformation de la pose a vers la pose b, exprimée dans le repère de a
    dx, dy = b[:2] - a[:2]; c, s = np.cos(-a[2]), np.sin(-a[2])
    return np.array([c * dx - s * dy, s * dx + c * dy, (b[2] - a[2] + np.pi) % (2 * np.pi) - np.pi])
aretes = [(k, k + 1, relatif(vraies[k], vraies[k + 1]) + rng.normal(0, [0.1, 0.1, 0.03]), np.diag([100, 100, 1000])) for k in range(n - 1)]
aretes.append((n - 1, 0, relatif(vraies[n - 1], vraies[0]) + rng.normal(0, [0.05, 0.05, 0.01]), np.diag([400, 400, 4000])))   # fermeture

# Estimation initiale : enchaîner l'odométrie (dérive)
def composer(a, r):
    c, s = np.cos(a[2]), np.sin(a[2]); return np.array([a[0] + c * r[0] - s * r[1], a[1] + s * r[0] + c * r[1], a[2] + r[2]])
X = [np.array([10.0, 5.0, np.pi / 2])]
for k in range(n - 1): X.append(composer(X[-1], aretes[k][2]))
X = np.array(X); X0 = X.copy()

# Optimisation : minimiser Σ ‖e_ij‖²_Ω où e_ij = relatif(x_i, x_j) − z_ij, par Gauss-Newton (module L12), jacobiennes numériques
def erreur(X):
    return [(i, j, relatif(X[i], X[j]) - z, O) for i, j, z, O in aretes]
def jac_num(fn, x, h=1e-6):
    J = np.zeros((3, 3))
    for k in range(3):
        d = np.zeros(3); d[k] = h; J[:, k] = (fn(x + d) - fn(x - d)) / (2 * h)
    return J
for it in range(10):
    Hm = np.zeros((3 * n, 3 * n)); b = np.zeros(3 * n); cout = 0
    for i, j, e, O in erreur(X):
        e[2] = (e[2] + np.pi) % (2 * np.pi) - np.pi
        A = jac_num(lambda xi: relatif(xi, X[j]), X[i]); B = jac_num(lambda xj: relatif(X[i], xj), X[j])
        Hm[3*i:3*i+3, 3*i:3*i+3] += A.T @ O @ A; Hm[3*j:3*j+3, 3*j:3*j+3] += B.T @ O @ B
        Hm[3*i:3*i+3, 3*j:3*j+3] += A.T @ O @ B; Hm[3*j:3*j+3, 3*i:3*i+3] += B.T @ O @ A
        b[3*i:3*i+3] += A.T @ O @ e; b[3*j:3*j+3] += B.T @ O @ e; cout += e @ O @ e
    Hm[:3, :3] += np.eye(3) * 1e6                                        # fixer la première pose (jauge)
    dx = np.linalg.solve(Hm, -b); X = X + dx.reshape(n, 3)
    if it in (0, 9): print(f"itération {it}: coût {cout:.1f}, correction max {np.abs(dx).max():.4f}")
plt.figure(figsize=(6, 5)); plt.plot(*vraies[:, :2].T, "w--", label="vérité"); plt.plot(*X0[:, :2].T, "r.-", label="odométrie seule")
plt.plot(*X[:, :2].T, "g.-", label="après optimisation"); plt.axis("equal"); plt.legend(); plt.show()

Pourquoi ça marche, et à quelle échelle

Chaque mesure relative est une contrainte élastique entre deux poses ; la fermeture de boucle ajoute un ressort entre la fin et le début, qui « tire » toute la trajectoire vers sa forme cohérente. On minimise une somme de carrés pondérés par l’information Ω = Σ⁻¹ (module L12, Gauss-Newton). La matrice H est creuse (chaque arête ne touche que 6 lignes) : avec Cholesky creux, on optimise des graphes de 100 000 poses en secondes (g2o, GTSAM, Ceres). Ajouter des amers (positions de balises, points 3D) donne le SLAM complet ; le module « frontal » qui détecte les fermetures de boucle (reconnaissance de lieux par images ou scans) est aussi crucial que le « dorsal » qui optimise. Cartographer (Google), ORB-SLAM3 et RTAB-Map sont construits ainsi.

03 / SLAM

Aligner deux scans : ICP, le scan matching de base

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(2)
# Deux nuages de points 2D du même mur vus depuis deux poses : trouver la rotation + translation qui les superpose
def nuage_scan(pose, n=120):
    angles = np.linspace(0, 2 * np.pi, n, endpoint=False); d = lidar(pose, n, bruit=0.03)
    pts = np.column_stack([d * np.cos(angles), d * np.sin(angles)])
    return pts[d < 7.9]                                           # garder les impacts réels
A = nuage_scan(np.array([3.0, 2.0, 0.0])); B = nuage_scan(np.array([3.4, 2.3, 0.15]))     # B est vu depuis une pose décalée

def icp(A, B, iters=30):
    """Trouve R, t tels que R B + t ≈ A. Alternance : apparier au plus proche, puis résoudre en fermé (SVD, module L10)."""
    R, t = np.eye(2), np.zeros(2); Bt = B.copy()
    for _ in range(iters):
        d = ((Bt[:, None, :] - A[None, :, :])**2).sum(-1); idx = d.argmin(1)       # plus proche voisin (naïf O(nm) ; k-d tree en pratique)
        P, Q = Bt, A[idx]
        mp, mq = P.mean(0), Q.mean(0); Hm = (P - mp).T @ (Q - mq)
        U, _, Vt = np.linalg.svd(Hm); Rk = Vt.T @ U.T
        if np.linalg.det(Rk) < 0: Vt[-1] *= -1; Rk = Vt.T @ U.T
        tk = mq - Rk @ mp
        Bt = (Rk @ Bt.T).T + tk; R = Rk @ R; t = Rk @ t + tk
    return R, t, np.sqrt(d.min(1).mean())
R, t, res = icp(A, B)
print(f"rotation estimée {np.degrees(np.arctan2(R[1, 0], R[0, 0])):.1f}° (vraie ≈ 8.6°), translation {t.round(2)}, résidu {res:.3f} m")
plt.figure(figsize=(5, 4)); plt.plot(*A.T, ".", ms=3, label="scan A"); plt.plot(*B.T, ".", ms=3, alpha=0.4, label="scan B"); plt.plot(*((R @ B.T).T + t).T, "x", ms=3, label="B recalé"); plt.axis("equal"); plt.legend(); plt.show()

ICP (Besl & McKay, 1992) est l’algorithme qui produit les contraintes entre poses successives à partir de scans (ou de nuages 3D). Le sous-problème « meilleure transformation rigide pour des appariements donnés » a une solution exacte par SVD (Procrustes orthogonal) — l’un des joyaux de l’algèbre linéaire appliquée. Limites : bassin de convergence étroit (il faut une bonne initialisation, l’odométrie), sensibilité aux valeurs aberrantes (variantes robustes, point-à-plan, NDT).

04 / ROS 2

ROS 2 : le système d’exploitation (qui n’en est pas un) des robots

# Un robot ROS 2 = des NŒUDS (processus) qui échangent des MESSAGES sur des TOPICS (patron Observateur, module L02)
$ ros2 node list
/lidar_driver  /imu_driver  /amcl  /map_server  /planner_server  /controller_server  /motor_driver  /camera  /detector
$ ros2 topic list
/scan  /imu/data  /odom  /tf  /map  /cmd_vel  /camera/image_raw  /detections  /goal_pose
$ ros2 topic echo /scan --once          # voir un message LaserScan : angle_min, ranges[…]
$ ros2 topic hz /scan                   # 10 Hz
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard    # publier /cmd_vel au clavier
$ ros2 bag record /scan /odom /tf       # enregistrer pour rejouer hors ligne (indispensable pour déboguer)
$ rviz2                                 # visualiser carte, scans, particules, plan

# Un nœud minimal en Python (rclpy) : s'abonne au lidar, publie une commande d'arrêt d'urgence
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class ArretUrgence(Node):
    def __init__(self):
        super().__init__("arret_urgence")
        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
        self.create_subscription(LaserScan, "/scan", self.sur_scan, 10)
        self.declare_parameter("distance_min", 0.3)          # réglable sans recompiler : ros2 param set
    def sur_scan(self, msg):
        devant = [r for r in msg.ranges[len(msg.ranges)//2 - 10 : len(msg.ranges)//2 + 10] if msg.range_min < r < msg.range_max]
        if devant and min(devant) < self.get_parameter("distance_min").value:
            self.pub.publish(Twist())                        # vitesses nulles
            self.get_logger().warn(f"obstacle à {min(devant):.2f} m : STOP")

rclpy.init(); rclpy.spin(ArretUrgence())

Les concepts

Concept	Rôle
Nœud	Un processus avec une responsabilité (pilote, algorithme, interface).
Topic	Flux de messages typés, publication/abonnement, plusieurs à plusieurs.
Service	Requête/réponse synchrone (« recharge la carte »).
Action	Tâche longue avec retour de progression et annulation (« va à ce but »).
Paramètre	Configuration modifiable à chaud.
tf2	Arbre des repères (map → odom → base_link → lidar) et leurs transformations dans le temps (module L10).
Launch	Fichier Python qui démarre et configure des dizaines de nœuds.
DDS / QoS	Couche de transport (UDP multicast) avec qualité de service : fiable ou « best effort », historique, échéances.

ROS 2 apporte ce que la super-boucle ne peut pas : composer des logiciels écrits par des équipes différentes (un lidar Velodyne, un planificateur Nav2, votre détecteur), en C++ ou Python, sur plusieurs machines. Le prix : latences de quelques millisecondes et une complexité qu’il faut apprivoiser. Le microcontrôleur, lui, garde le temps réel dur (module L18) et parle à ROS par micro-ROS ou un pont série.

04 / ROS 2

La pile de navigation : de la carte au mouvement

Étage	Nœud Nav2	Ce module / le cours
Carte	map_server, slam_toolbox	Carte d’occupation, SLAM par graphe
Localisation	amcl	Filtre particulaire
Planification globale	planner_server (NavFn, Smac)	A* / Dijkstra sur la grille (L05), avec coûts d’inflation autour des obstacles
Planification locale / contrôle	controller_server (DWB, MPPI, RPP)	Fenêtre dynamique : échantillonner (v, ω), simuler 1 s, scorer, choisir (L21)
Comportements	bt_navigator (arbre de comportement)	Machine à états de la séance 28, en plus structuré : « si bloqué, reculer, replanifier »
Sécurité	collision_monitor, velocity_smoother	Arrêt d’urgence par lidar, limitation d’accélération

import numpy as np, heapq
# Planificateur global sur la carte d'occupation construite plus haut : A* avec coût d'inflation (rester loin des murs)
occ = prob > 0.6
# Distance au mur le plus proche par BFS (module L05), puis coût = 1 + 10·exp(−distance)
from collections import deque
dist = np.full(occ.shape, np.inf); file = deque()
for i, j in zip(*np.where(occ)): dist[i, j] = 0; file.append((i, j))
while file:
    i, j = file.popleft()
    for a, b in ((i+1, j), (i-1, j), (i, j+1), (i, j-1)):
        if 0 <= a < GH and 0 <= b < GW and dist[a, b] == np.inf: dist[a, b] = dist[i, j] + 1; file.append((a, b))
cout = 1 + 10 * np.exp(-dist * RES / 0.4)
def a_star(depart, but):
    ouvert = [(0, depart)]; g = {depart: 0}; pred = {depart: None}
    while ouvert:
        _, u = heapq.heappop(ouvert)
        if u == but: break
        for a, b in ((u[0]+1, u[1]), (u[0]-1, u[1]), (u[0], u[1]+1), (u[0], u[1]-1)):
            if 0 <= a < GH and 0 <= b < GW and not occ[a, b]:
                ng = g[u] + cout[a, b]
                if ng < g.get((a, b), np.inf): g[(a, b)] = ng; pred[(a, b)] = u; heapq.heappush(ouvert, (ng + abs(a - but[0]) + abs(b - but[1]), (a, b)))
    c = []; u = but
    while u: c.append(u); u = pred.get(u)
    return c[::-1]
chemin = a_star((6, 10), (30, 60))
print(len(chemin), "cases ; distance minimale au mur le long du chemin :", round(min(dist[i, j] for i, j in chemin) * RES, 2), "m")
plt.figure(figsize=(7, 3.5)); plt.imshow(prob, cmap="gray_r", origin="lower"); plt.plot([j for i, j in chemin], [i for i, j in chemin], "c-"); plt.show()

05 / Frontière

Où en est la recherche

SLAM visuel et visuel-inertiel (ORB-SLAM3, VINS, Kimera) : une caméra et une IMU suffisent ; c’est ce qui tourne dans les casques de réalité augmentée et les drones. Questions ouvertes : robustesse à la lumière, aux scènes dynamiques, à long terme (le monde change).
Cartes neuronales : NeRF et gaussian splatting représentent la scène par un réseau ou des gaussiennes 3D ; on peut y localiser une caméra et y planifier. Compromis mémoire / précision / temps réel encore débattus.
Cartes sémantiques : ne pas cartographier des cases occupées mais des objets (« la tasse est sur la table ») avec des modèles de vision-langage ; nécessaire pour les instructions en langage naturel (module L15).
Multi-robots : fusionner les cartes de plusieurs robots avec des communications intermittentes.
Apprentissage de bout en bout : des politiques (module L16) qui vont des pixels aux moteurs sans carte explicite. Marche pour certaines tâches, mais difficile à vérifier et à déboguer — le débat « carte explicite ou implicite » est vif.
Certification : comment prouver (module L09) qu’un système de localisation probabiliste est sûr ? Sujet central pour la voiture autonome.

Un projet de TIPE ou de stage de L3 accessible : implémenter un SLAM 2D par graphe sur un robot à lidar bon marché (RPLidar A1) avec ROS 2, et comparer à slam_toolbox. C’est faisable en un semestre avec les outils de ce module.

Cours

Cours 1 — Le filtre bayésien général et ses quatre incarnations

L’équation unique. Croyance bel(x_t) = P(x_t | z_1..t, u_1..t). Deux étapes récursives : prédiction bel⁻(x_t) = ∫ P(x_t | x_t−1, u_t) bel(x_t−1) dx_t−1 (le modèle de mouvement étale la croyance) ; correction bel(x_t) = η P(z_t | x_t) bel⁻(x_t) (le modèle de mesure la concentre). Tout dépend de comment on représente bel :

Représentation de bel	Filtre	Prédiction	Correction	Forces / limites
Gaussienne (μ, Σ), modèles linéaires	Kalman (L19)	μ ← Fμ + Bu ; Σ ← FΣFᵀ + Q	Gain K	Exact et optimal ; unimodal, linéaire
Gaussienne, modèles non linéaires	EKF / UKF	Linéarisation / points sigma	Idem	Rapide ; peut diverger ; unimodal
Histogramme sur une grille	Filtre à grille (localisation de Markov)	Convolution par le modèle de mouvement	Multiplication case par case	Multimodal, exact sur la grille ; coût exponentiel en dimension
Échantillons pondérés	Particulaire (MCL)	Déplacer chaque particule	Pondérer, rééchantillonner	Multimodal, tout modèle ; coût en nombre de particules, dégénérescence

Choisir. État de petite dimension, croyance possiblement multimodale (localisation globale, symétries) → particulaire. État de grande dimension (SLAM avec 1000 amers), croyance unimodale → EKF/UKF ou, mieux, optimisation par graphe. Beaucoup de systèmes combinent : particules pour initialiser, Kalman pour suivre (Rao-Blackwellisation : FastSLAM).

import numpy as np
# Filtre à grille 1D : un robot sur un couloir de 100 cases avec 3 portes ; capteur « porte / pas porte » ; mouvement +3 ± 1
NC = 100; portes = [20, 50, 80]; bel1 = np.ones(NC) / NC
def predire1(bel1, u):                          # convolution par le modèle de mouvement (déplacement u avec incertitude ±1)
    nb = np.zeros(NC)
    for d, p in ((u - 1, 0.25), (u, 0.5), (u + 1, 0.25)): nb += p * np.roll(bel1, d)
    return nb
def corriger1(bel1, z):                         # z = 1 si porte vue ; capteur fiable à 90 %
    vrais = np.array([1 if any(abs(i - p) <= 1 for p in portes) else 0 for i in range(NC)])
    v = np.where(vrais == z, 0.9, 0.1); b = bel1 * v; return b / b.sum()
vraie = 18; trace = []
for k in range(12):
    z = 1 if any(abs(vraie - p) <= 1 for p in portes) else 0
    bel1 = corriger1(bel1, z); trace.append((vraie, int(bel1.argmax()), round(bel1.max(), 2)))
    vraie = (vraie + 3) % NC; bel1 = predire1(bel1, 3)
print("(vraie position, estimation, confiance) :", trace)

Cours

Cours 2 — Exemple travaillé : le modèle de vraisemblance d’un télémètre, et pourquoi le gaussien naïf échoue

Le cours utilise P(z | x) ∝ exp(−Σ(z_i − ẑ_i)²/2σ²) : gaussienne, rayons indépendants. C’est faux de trois façons, et chaque défaut a une parade classique (Probabilistic Robotics, ch. 6) :

Aberrations : un rayon qui touche un objet non cartographié (personne, chaise) donne une distance plus courte. Parade : mélange de lois — gaussienne + exponentielle décroissante (objet inattendu) + pic à la portée max (rien vu) + uniforme (bruit). Poids ≈ 0,7 / 0,1 / 0,1 / 0,1 : une aberration ne tue plus la particule.
Indépendance : 360 rayons ne sont pas indépendants (un mur = des centaines de rayons corrélés) ; la vraisemblance devient sur-confiante (10⁻³⁰⁰). Parade : sous-échantillonner (30 rayons), ou élever la vraisemblance à une puissance < 1.
Coût : le lancer de rayon pour chaque particule est lent. Parade : modèle de champ de vraisemblance : précalculer la distance de chaque case au mur le plus proche (transformée de distance, BFS L05) ; la vraisemblance d’un impact = gaussienne de cette distance ; pas de lancer de rayon.

import numpy as np
import matplotlib.pyplot as plt
# Modèle « beam » : mélange de 4 composantes, pour une distance attendue de 3 m, portée 8 m
z = np.linspace(0, 8, 400); attendu, sigma, lam, zmax = 3.0, 0.2, 0.5, 8.0
gauss = np.exp(-(z - attendu)**2 / (2 * sigma**2)) / (sigma * np.sqrt(2 * np.pi))
court = np.where(z < attendu, lam * np.exp(-lam * z), 0); court /= max(court.sum() * (z[1] - z[0]), 1e-9)
maxi = np.where(z > zmax - 0.1, 1 / 0.1, 0); unif = np.ones_like(z) / zmax
p = 0.7 * gauss + 0.1 * court + 0.1 * maxi + 0.1 * unif
plt.figure(figsize=(7, 2.8)); plt.plot(z, p); plt.xlabel("distance mesurée (m)"); plt.ylabel("P(z | x)"); plt.title("modèle de télémètre à 4 composantes (attendu 3 m)"); plt.show()
# Effet sur une particule : une seule aberration (z = 0.5 m au lieu de 3) — gaussien pur vs mélange
naif = lambda z: np.exp(-(z - attendu)**2 / (2 * sigma**2)); melange = lambda z: np.interp(z, np.linspace(0, 8, 400), p)
print(f"vraisemblance z = 0.5 : gaussien {naif(0.5):.1e} (la particule est morte) | mélange {melange(0.5):.3f} (elle survit)")
# Transformée de distance sur la carte du module : distance au mur le plus proche pour chaque case (précalcul du champ de vraisemblance)
from collections import deque
dist = np.full(grille.shape, np.inf); file = deque()
for i, j in zip(*np.where(grille)): dist[i, j] = 0; file.append((i, j))
while file:
    i, j = file.popleft()
    for a, b in ((i+1, j), (i-1, j), (i, j+1), (i, j-1)):
        if 0 <= a < H and 0 <= b < W and dist[a, b] == np.inf: dist[a, b] = dist[i, j] + 1; file.append((a, b))
print("distance au mur (case par case) :"); print(dist.astype(int))

Cours

Cours 3 — Anatomie d’un système ROS 2 de navigation : fichiers, nœuds, repères, débogage

mon_robot/                          # un « workspace » ROS 2 (colcon)
└── src/
    ├── mon_robot_description/      # URDF : géométrie, repères (base_link, laser, imu), inertie ; visualisable dans RViz
    ├── mon_robot_bringup/          # launch/ : robot.launch.py démarre pilotes + tf + nav ; config/ : YAML des paramètres
    ├── mon_robot_driver/           # nœud C++/Python qui parle au microcontrôleur (série, L22) : publie /odom, /imu, souscrit /cmd_vel
    ├── mon_robot_nav/              # config Nav2 (planificateur, contrôleur, coûts), cartes, comportements
    └── mon_robot_perception/       # vos nœuds : détecteur, filtre, ...

$ colcon build --symlink-install && source install/setup.bash
$ ros2 launch mon_robot_bringup robot.launch.py
$ ros2 run tf2_tools view_frames            # PDF de l'arbre : map → odom → base_link → laser
$ ros2 topic hz /scan ; ros2 topic echo /tf ; ros2 param list /amcl
$ ros2 bag record -a ; ros2 bag play ma_session

Repère	Publié par	Signification
map → odom	AMCL / SLAM	Correction lente, peut sauter (fermeture de boucle)
odom → base_link	Odométrie (pilote, EKF `robot_localization`)	Continu, lisse, dérive
base_link → laser / imu / camera	URDF (statique)	Où sont les capteurs sur le châssis : à mesurer au millimètre

Les bugs ROS les plus fréquents : horloges non synchronisées (use_sim_time, horodatages : « message too old ») ; repères manquants ou mal nommés (tf : « could not transform ») ; QoS incompatibles (l’abonné ne reçoit rien : « best effort » vs « reliable ») ; deux nœuds publiant le même tf ; unités (degrés au lieu de radians, mm au lieu de m). Outils : rqt_graph, rqt_tf_tree, ros2 doctor, rviz2.

TP guidé

TP — SLAM et navigation avec ROS 2 en simulation, puis sur robot (sur PC, 4 h)

Installation. Ubuntu 22.04 (natif, VM, ou WSL2 avec WSLg) ; ROS 2 Humble ou Jazzy (paquets ros-humble-desktop, ros-humble-navigation2, ros-humble-turtlebot3*, ros-humble-slam-toolbox). Tutoriels « Beginner: CLI tools » (2 h) : nœuds, topics, services, paramètres, launch.
Simulation TurtleBot3. export TURTLEBOT3_MODEL=burger ; ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py. Pilotez au clavier ; ros2 topic echo /scan ; enregistrez un bag de 2 minutes en explorant.
Cartographie. ros2 launch slam_toolbox online_async_launch.py ; explorez ; observez la carte se construire dans RViz ; sauvegardez (ros2 run nav2_map_server map_saver_cli -f ma_carte). Rejouez le bag avec SLAM : même carte ? Retirez la fermeture de boucle (paramètre) : constatez la dérive.
Localisation et navigation. ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=ma_carte.yaml ; pose initiale dans RViz ; envoyez un but ; observez le plan global, le chemin local, les particules d’AMCL. Kidnappez le robot (téléportation dans Gazebo) : AMCL récupère-t-il ? Réglez max_particles, alpha et mesurez.
Votre nœud. Un nœud Python qui souscrit /scan, publie un Marker RViz sur l’obstacle le plus proche, et un service « aller au point le plus dégagé » qui envoie un but à Nav2 via l’action NavigateToPose. Paramètres déclarés, QoS explicites, tests avec launch_testing.
Robot réel (si disponible). Un châssis + RPLidar A1 + Raspberry Pi 4 : pilote série vers le microcontrôleur (L22), URDF avec la position du lidar, robot_localization pour fusionner odométrie et IMU, slam_toolbox, Nav2. Objectif : cartographier une pièce et naviguer d’un point à l’autre de façon autonome.
Livrable. Workspace sur GitHub, carte, vidéo/gif de navigation, arbre tf (PDF), RESULTATS.md avec les réglages AMCL et leurs effets, et un schéma des nœuds (rqt_graph).

Exercices

Exercices auto-corrigés — filtres à grille et particules

Exercice 1 — Filtre à grille 2D avec modèle de mouvement incertain

Sur la carte du module (grille, cases libres), écrivez predire2d(bel, du, dv) (déplacement demandé (du, dv) réalisé avec probabilité 0,8, glissement ±1 case perpendiculaire avec 0,1 chacun, murs = on reste sur place) et corriger2d(bel, scan_obs, sigma) avec le lidar du module (4 rayons suffisent) ; le robot part inconnu, fait 8 pas ; vérifiez que l’argmax converge vers la vraie position (à 1 case près).

Correction

def predire2d(bel, du, dv):
    nb = np.zeros_like(bel); perp = (dv, du)
    for (dx, dy), p in (((du, dv), 0.8), ((du + perp[0], dv + perp[1]), 0.1), ((du - perp[0], dv - perp[1]), 0.1)):
        for y, x in zip(*np.where(bel > 0)):
            ny, nx = y + dy, x + dx
            if 0 <= ny < H and 0 <= nx < W and libres[ny, nx]: nb[ny, nx] += p * bel[y, x]
            else: nb[y, x] += p * bel[y, x]
    return nb / nb.sum()
def corriger2d(bel, scan_obs, sigma=0.5):
    v = np.zeros_like(bel)
    for y, x in zip(*np.where(libres)):
        att = lidar((x + 0.5, y + 0.5, 0), n_rayons=4, bruit=0.0); v[y, x] = np.exp(-0.5 * np.sum((scan_obs - att)**2) / sigma**2)
    b = bel * v; return b / b.sum()

Exercice 2 — Rééchantillonnage systématique et N_eff

Écrivez n_eff(poids) et reechantillonner_systematique(particules, poids) (une seule valeur aléatoire, variance minimale : algorithme du « peigne »). Vérifiez : la moyenne pondérée est conservée en espérance ; avec des poids uniformes, chaque particule est copiée exactement une fois ; N_eff vaut N pour des poids uniformes et 1 pour un poids concentré.

Correction

def n_eff(w): return 1.0 / np.sum(np.asarray(w)**2)
def reechantillonner_systematique(P, w):
    N = len(w); u = (rng.random() + np.arange(N)) / N; cum = np.cumsum(w); idx = np.searchsorted(cum, u)
    return P[np.minimum(idx, N - 1)]

Exercices

Exercices auto-corrigés — cartes et graphes de poses

Exercice 3 — Transformations de poses 2D : composer, inverser, relatif

Écrivez composer(a, b) (pose b exprimée dans le repère a → pose dans le repère monde), inverse(a) et relatif(a, b) = composer(inverse(a), b), avec angles ramenés dans ]−π, π]. Vérifiez les identités : composer(a, relatif(a, b)) = b ; composer(a, inverse(a)) = identité ; associativité.

Correction

def composer(a, b):
    c, s = np.cos(a[2]), np.sin(a[2])
    return np.array([a[0] + c * b[0] - s * b[1], a[1] + s * b[0] + c * b[1], (a[2] + b[2] + np.pi) % (2 * np.pi) - np.pi])
def inverse(a):
    c, s = np.cos(a[2]), np.sin(a[2])
    return np.array([-c * a[0] - s * a[1], s * a[0] - c * a[1], (-a[2] + np.pi) % (2 * np.pi) - np.pi])
def relatif(a, b): return composer(inverse(a), b)

Exercice 4 — Détecter une fermeture de boucle par comparaison de scans

Le robot enregistre des scans à 36 rayons à chaque pose. Écrivez signature(scan) invariante par rotation (par exemple l’histogramme des distances, ou le spectre d’amplitude de la FFT du scan) et candidats_boucle(scans, i, seuil) qui renvoie les indices j ≤ i − 10 dont la signature est proche de celle de i. Vérifiez sur des scans simulés depuis la carte du module : deux scans pris au même endroit avec un cap différent doivent être détectés ; deux endroits différents non.

Correction

def signature(scan):
    return np.abs(np.fft.rfft(scan))[1:8]            # amplitudes des basses fréquences : invariantes par rotation du scan (décalage circulaire)
def candidats_boucle(scans, i, seuil):
    s = signature(scans[i]); return [j for j in range(i - 9) if np.linalg.norm(signature(scans[j]) - s) < seuil]

Un décalage circulaire du scan (changement de cap) ne change pas le module de sa transformée de Fourier : c’est la propriété qui rend la signature invariante. Les vrais systèmes utilisent des descripteurs plus riches (Scan Context, sacs de mots visuels), puis vérifient géométriquement par ICP avant d’ajouter la contrainte au graphe — sinon une fausse fermeture détruit la carte (défi ★★★).

06 / Défis

Défi ★ — Le robot kidnappé et le rééchantillonnage adaptatif

Consigne

1) Après convergence du MCL, téléportez le robot ailleurs : combien de pas pour que le filtre le retrouve ? Pourquoi peut-il échouer (toutes les particules sont concentrées au mauvais endroit) ? 2) Ajoutez l’injection de 5 % de particules aléatoires à chaque pas : mesurez le temps de récupération. 3) Implémentez le nombre effectif de particules N_eff = 1/Σw² et ne rééchantillonnez que si N_eff < N/2 : quel effet sur la stabilité ? 4) Placez le robot dans un couloir symétrique de la carte et montrez l’ambiguïté (deux nuages).

Piste

La dégénérescence : après rééchantillonnage, toutes les particules descendent de quelques ancêtres ; si l’ancêtre était faux, plus rien ne peut corriger. L’injection aléatoire (ou proportionnelle à la baisse soudaine de la vraisemblance moyenne, « augmented MCL ») est la parade standard. N_eff évite de perdre la diversité quand les poids sont déjà uniformes.

06 / Défis

Défi ★★ — SLAM complet en 2D : poses + amers

Consigne

Étendez le SLAM par graphe : le robot parcourt une trajectoire et observe 8 balises (position relative bruitée, identité connue) ; les inconnues sont les 20 poses ET les 8 positions de balises. 1) Écrivez les deux types d’arêtes (pose-pose, pose-balise) avec leurs jacobiennes numériques. 2) Optimisez ; comparez la carte des balises à la vérité. 3) Retirez la fermeture de boucle : que devient l’erreur ? 4) Vérifiez que H est creuse (affichez sa structure) et mesurez le temps avec scipy.sparse.linalg.spsolve si disponible, sinon commentez.

Piste

Arête pose-balise : e = R(θ_i)ᵀ(l_j − p_i) − z_ij (position de la balise dans le repère du robot). Les blocs de H pour la balise sont 2×2. Indexation : poses aux indices 3i, balises à 3n + 2j. Ce défi est exactement l’exercice du cours de Stachniss « least squares SLAM » ; le résoudre vous met au niveau d’un M1 en robotique.

06 / Défis

Défi ★★★ — Esprit prépa : consistance, jauge et fermeture de boucle robuste

Consigne

1) Montrez que sans fixer la première pose, la matrice H du SLAM par graphe est singulière (noyau de dimension 3 : translation et rotation globales) — expliquez pourquoi et vérifiez numériquement les valeurs propres. 2) Fausse fermeture de boucle : ajoutez une contrainte erronée (le frontal s’est trompé de lieu). Montrez que Gauss-Newton déforme toute la carte. Implémentez une fonction de coût robuste (Huber, ou « switchable constraints » : un poids s_ij ∈ [0, 1] optimisé avec un prior vers 1) et montrez que la fausse contrainte est désactivée. 3) Prouvez que l’EKF-SLAM (état = pose + toutes les balises, covariance pleine) a un coût O(n²) par mise à jour, et expliquez pourquoi le SLAM par graphe le supplante. 4) Discutez : quelle garantie peut-on donner qu’un SLAM ne « diverge » pas ? Cherchez « SLAM consistency » et résumez en 10 lignes l’état de l’art.

Piste (Huber)

Remplacer e ᵀΩe par ρ(‖e‖_Ω) avec ρ(x) = x² si x < δ, 2δx − δ² sinon : au-delà de δ, le coût croît linéairement et le gradient est borné, donc une contrainte aberrante ne « tire » plus que d’une force constante. En pratique (g2o, GTSAM) : IRLS — repondérer chaque arête par ρ′(x)/x à chaque itération. Le noyau de H : ajouter (δx, δy, 0) à toutes les poses, ou une rotation globale, ne change aucune erreur relative ; ces 3 directions sont les degrés de liberté de jauge.

07 / Vérification

Pourquoi AMCL utilise-t-il un filtre particulaire plutôt qu’un EKF pour localiser un robot dans une carte ?

Deux questions supplémentaires

1. Pourquoi borner les log-odds d’une carte d’occupation ? Pour qu’une case puisse changer d’état (porte ouverte/fermée) en un nombre raisonnable de mesures.

2. Que fait une fermeture de boucle dans un SLAM par graphe ? Elle ajoute une contrainte entre deux poses éloignées dans le temps, ce qui corrige la dérive accumulée sur tout le tour.

Référence

Les mots à retenir

Mot	Définition
Lancer de rayon	Modèle de capteur : ce qu’on verrait depuis une pose.
MCL / filtre particulaire	Localisation par échantillons : prédire, pondérer, rééchantillonner.
N_eff, injection	Parades à la dégénérescence des particules.
Carte d’occupation	Grille de probabilités mises à jour en log-odds.
SLAM	Localisation et cartographie simultanées.
SLAM par graphe	Poses = nœuds, mesures = arêtes ; moindres carrés creux.
Fermeture de boucle	Reconnaître un lieu déjà visité ; contrainte qui corrige la dérive.
ICP	Aligner deux nuages de points par appariements itératifs et SVD.
Jauge	Degrés de liberté non observables (repère global) ; fixer une pose.
ROS 2	Nœuds, topics, services, actions, tf2, launch, DDS.
Nav2	Pile de navigation : carte, AMCL, planificateurs, contrôleur, arbre de comportement.

Pour continuer

Le robot sait où il est

Module suivant : planification et contrôle — cinématique, planification de mouvement (RRT), commande optimale (LQR, MPC) : décider comment bouger.

À faire chez soi

Installer ROS 2 (Ubuntu 22.04/24.04 ou Docker), faire les tutoriels « Beginner: CLI tools » et « Client libraries », lancer TurtleBot3 en simulation (Gazebo) avec Nav2.
Regarder les cours SLAM de Cyrill Stachniss (« Graph-based SLAM », « Particle filter »).
Lire Probabilistic Robotics chapitres 7 et 8 ; refaire les exercices en Python.

← L19 Sommaire L21 : Planification et contrôle →