Degeneracy-Resilient Teach and Repeat for Geometrically Challenging Environments Using FMCW Lidar

Cet article présente un système de navigation « Teach and Repeat » résilient aux dégénérescences géométriques utilisant un lidar FMCW, qui combine l'odométrie Doppler et une localisation adaptative pour assurer une navigation autonome fiable dans des environnements à structure pauvre où les méthodes ICP conventionnelles échouent.

L'essentiel

🌍 Le Problème : Se perdre dans le "Grand Vide"

Imaginez que vous conduisez une voiture autonome. Dans une ville avec des immeubles, des panneaux et des arbres, c'est facile : le robot utilise ses capteurs (comme un Lidar, qui est un laser très précis) pour voir les formes et se repérer. C'est comme suivre des points de repère familiers.

Mais imaginez maintenant que vous devez conduire sur une plaine de désert plate, sur une piste d'atterrissage vide, ou même sur la Lune. Il n'y a rien : juste du sol uni, sans arbres, sans bâtiments, sans relief.

• Le problème : Pour un robot classique, c'est comme essayer de conduire les yeux bandés en suivant des lignes invisibles. Quand il n'y a pas de formes distinctes, le système de navigation classique (appelé ICP) devient confus. Il ne sait plus où il est, il "hallucine" et finit par s'égarer. C'est ce qu'on appelle la dégénérescence géométrique (un mot compliqué pour dire : "il n'y a pas assez de détails pour se repérer").

💡 La Solution : Le Lidar "Super-Héros" (FMCW)

Les chercheurs de l'Université de Toronto ont développé un nouveau système pour résoudre ce problème. Ils utilisent un type spécial de Lidar appelé FMCW (Lidar à modulation de fréquence continue).

Pour faire simple, voici la différence :

• Le Lidar classique : C'est comme une photo. Il voit où sont les objets, mais il ne sait pas s'ils bougent ou à quelle vitesse.
• Le Lidar FMCW : C'est comme une photo + un radar de vitesse. Grâce à l'effet Doppler (le même principe qui fait changer le son d'une ambulance quand elle passe), ce Lidar mesure la vitesse de chaque point qu'il touche.

L'analogie du vent :
Imaginez que vous êtes dans un brouillard épais (le désert plat).

• Le robot classique regarde le brouillard et ne voit rien.
• Notre robot, lui, sent le vent. Même s'il ne voit pas d'arbres, il sent que l'air glisse sur son capot à telle vitesse. Il peut deviner sa trajectoire en sentant le "flux" de l'air autour de lui, même sans voir de repères visuels.

🛠️ Les Trois Astuces Magiques

Pour que ce robot soit vraiment fiable, les chercheurs ont ajouté trois ingrédients secrets à leur recette :

1. La "Recette de Cuisine" Intelligente (Prétraitement par courbure)

Quand on prend une photo d'un champ plat, on a des millions de pixels inutiles (l'herbe plate). Mais si on veut voir une petite pierre, il faut la garder.

• L'ancienne méthode : Elle prenait des échantillons partout de manière égale, comme si elle coupait des parts de gâteau identiques. Résultat : elle gardait trop de "plat" et perdait les "petites pierres".
• La nouvelle méthode : Elle regarde la courbure. Elle dit : "Ah, ici c'est très plat, je jette la moitié des points pour aller plus vite. Mais là, c'est une pierre ou un rocher (courbure forte), je garde tout !".
• Résultat : Le robot garde les détails importants (les rochers, les irrégularités) et ignore le vide inutile.

2. Le "Détective de la Vérité" (ICP conscient de la dégénérescence)

Quand le robot essaie de se localiser, il compare sa vue actuelle avec une carte qu'il a apprise plus tôt.

• Le problème : Dans un désert plat, il y a mille endroits qui se ressemblent. Le robot classique peut se tromper et dire : "Je suis ici !" alors qu'il est là-bas.
• La solution : Le nouveau système est "conscient de la dégénérescence". Il se dit : "Attends, ici, les formes sont trop plates pour être sûres à 100%. Je vais donc être prudent. Je vais seulement corriger ma position là où je suis sûr (les directions bien définies) et je vais faire confiance à ma mémoire de vitesse (l'odométrie Doppler) pour le reste."
• L'image : C'est comme un navigateur qui, dans le brouillard, dit : "Je ne suis pas sûr de ma position Nord-Sud, mais je suis sûr de ma position Est-Ouest. Je vais donc avancer prudemment en me fiant à mon compas pour l'Est-Ouest."

3. La "Mémoire de la Vitesse" (Odométrie Doppler)

Puisque le Lidar FMCW mesure la vitesse de chaque point, le robot peut calculer son propre mouvement très précisément, même sans voir de murs. C'est comme si le robot avait un sens de l'équilibre interne très développé. Il combine cette "mémoire de vitesse" avec sa "mémoire visuelle" (la carte) pour ne jamais se perdre.

🏁 Le Résultat : Le Test de l'Aéroport

Les chercheurs ont testé leur système sur trois terrains :

1. Un campus universitaire (plein de bâtiments) : Tout le monde s'en sort bien.
2. Un terrain de simulation lunaire (un peu plat, avec quelques rochers) : Le nouveau système est plus stable.
3. Une piste d'aéroport (très plate, presque vide) : C'est ici que la magie opère.
   • Les robots classiques (avec les anciennes méthodes) ont échoué. Ils ont tourné en rond ou se sont perdus dès le début.
   • Le nouveau système a réussi à faire tout le trajet en boucle, même si la trajectoire n'était pas parfaite à 100%. Il est arrivé à destination !

🚀 En Résumé

Cette recherche nous dit que pour explorer des endroits difficiles (comme la Lune, Mars, ou des mines souterraines), nous ne pouvons plus nous fier uniquement à la "vision" des robots. Nous devons leur donner un "sixième sens" (la vitesse Doppler) et leur apprendre à être prudents quand le paysage est trop uniforme.

C'est comme passer d'une voiture qui a juste besoin de voir la route, à une voiture qui peut aussi sentir la route et deviner sa trajectoire même dans le brouillard le plus épais.

Résumé technique

1. Problématique

La navigation autonome dans des environnements sans GPS (mines souterraines, surfaces planétaires, déserts) repose souvent sur des systèmes "Apprendre et Répéter" (Teach and Repeat - T&R). Ces systèmes permettent à un robot de répéter un trajet préalablement enregistré en utilisant des cartes locales.

Cependant, les systèmes T&R basés sur le Lidar actuels utilisent généralement l'algorithme ICP (Iterative Closest Point) pour l'alignement des nuages de points. Dans des environnements géométriquement dégénérés (zones plates, déserts, terrains planétaires peu structurés), l'ICP devient mal conditionné. L'absence de caractéristiques géométriques distinctives (bords, coins) entraîne une association de données erronée, une dérive de la localisation et, in fine, l'échec de la navigation.

L'article propose de surmonter cette limitation en exploitant les capacités des nouveaux capteurs Lidar FMCW (Frequency-Modulated Continuous Wave), qui fournissent non seulement des mesures de distance, mais aussi des vitesses radiales par point via l'effet Doppler.

2. Méthodologie

Les auteurs proposent un système T&R résilient à la dégénérescence, composé de trois modules principaux :

A. Odométrie Doppler sans correspondance avec estimation d'incertitude

• Principe : Utilisation d'une odométrie Lidar-Inertielle basée sur la vitesse Doppler, indépendante de la géométrie et ne nécessitant pas d'appariement de points (correspondence-free).
• Innovation : Les auteurs étendent la méthode existante (Yoon et al.) pour inclure une estimation rigoureuse de l'incertitude de la pose.
• Fonctionnement : En modélisant la vitesse du véhicule comme un processus gaussien et en intégrant les mesures de vitesse angulaire (gyroscope) et de vitesse radiale (Doppler), le système calcule la covariance de la pose. Cette incertitude est ensuite propagée dans l'étape de localisation pour guider l'optimisation.

B. Prétraitement basé sur la courbure

• Problème : Le sous-échantillonnage uniforme (voxel grid) standard tend à préserver les grandes surfaces planes (peu informatives) tout en supprimant les petits détails géométriques critiques.
• Solution : Un algorithme de sous-échantillonnage adaptatif basé sur la courbure locale.
  • Les points sont regroupés par clusters de courbure similaire.
  • Les surfaces à faible courbure (plans) sont fortement sous-échantillonnées.
  • Les zones à haute courbure (bords, rochers, détails) sont préservées avec une résolution plus fine.
• Impact : Cela améliore l'efficacité computationnelle et la qualité des données pour l'appariement dans les environnements dégénérés.

C. Localisation ICP consciente de la dégénérescence (DA-ICP)

• Association de données améliorée : Au lieu de se fier uniquement à la distance euclidienne, le système utilise un score d'appariement conjoint spatial-courbure. Cela permet de distinguer les points sur des surfaces planes ambiguës en se basant sur leur structure locale.
• Détection et atténuation de la dégénérescence :
  • Le système normalise les échelles de translation et de rotation dans la matrice Hessienne de l'optimisation ICP (formulation par blocs).
  • Une analyse des valeurs propres est effectuée pour détecter les directions mal conditionnées (dégénérées).
  • Stratégie de mise à jour : Seules les directions bien conditionnées sont mises à jour par l'ICP. Les directions dégénérées sont laissées à la discrétion de l'odométrie Doppler (qui reste stable même sans structure géométrique).
• Fusion : La pose estimée par l'ICP est fusionnée de manière lâche avec l'odométrie Doppler, en pondérant les contributions selon leurs incertitudes respectives.

3. Contributions Clés

1. Extension de l'odométrie Doppler : Intégration d'une estimation d'incertitude de pose dans une odométrie Lidar-Inertielle sans correspondance, permettant une propagation cohérente de l'incertitude vers la localisation.
2. Algorithme DA-ICP : Développement d'un algorithme d'ICP qui intègre la courbure pour l'appariement et utilise une mise à l'échelle par blocs pour une détection fiable de la dégénérescence, permettant de mettre à jour sélectivement les degrés de liberté du robot.
3. Validation expérimentale complète : Évaluation du système sur trois environnements réels (du riche en caractéristiques au très dégénéré), démontrant la capacité à naviguer de manière autonome là où les systèmes ICP classiques échouent.

4. Résultats Expérimentaux

Les tests ont été réalisés avec un véhicule terrestre non habité (Warthog UGV) équipé d'un Lidar FMCW Aeva Aeries II sur trois itinéraires :

• Route "Campus" (Riche en caractéristiques) : Tous les systèmes fonctionnent. Le système proposé présente une erreur légèrement supérieure (3,5 cm de plus) par rapport à l'ICP standard, due à un seuil de dégénérescence conservateur. Cependant, un ajustement des paramètres réduit cet écart, confirmant le compromis robustesse/précision.
• Route "Space Agency" (Analogie planétaire, modérément structurée) : Le système proposé maintient une précision comparable au système de base, avec une meilleure stabilité dans les zones moins structurées.
• Route "Aéroport" (Dégénérée, piste d'atterrissage plate) : C'est le test critique.
  • Les variantes utilisant l'ICP standard ou un prétraitement non adapté échouent (divergence précoce).
  • Le système proposé réussit à compléter l'ensemble du trajet en boucle fermée.
  • Bien que l'erreur de suivi de trajectoire soit d'environ 40 cm (acceptable pour une mission de survie dans un environnement dégénéré), la capacité à ne pas diverger est un succès majeur.

5. Signification et Impact

Cet article démontre pour la première fois la faisabilité d'une navigation T&R en boucle fermée fiable sur de longues distances dans des environnements extrêmes et peu structurés (comme les surfaces planétaires).

En combinant l'information de vitesse Doppler (indépendante de la géométrie) avec une localisation géométrique intelligente qui reconnaît ses propres limites (dégénérescence), le système évite le piège classique de l'ICP : la confiance excessive dans des données mal conditionnées. Cela ouvre la voie à des missions d'exploration robotique plus ambitieuses sur la Lune ou Mars, où les terrains plats et sans relief sont la norme plutôt que l'exception.