Résumé technique : Terminal Matters : Planification kinodynamique avec un coût terminal et une incertitude apprise dans l'espace état-coût de croyance

Énoncé du problème

Les systèmes robotiques opérant dans des environnements réels doivent générer des mouvements dynamiquement réalisables qui satisfont les contraintes kinodynamiques tout en tenant compte de l'incertitude d'exécution découlant des erreurs de modélisation, du bruit de perception et des interactions de contact complexes. Bien que les planificateurs kinodynamiques basés sur l'échantillonnage existants (par exemple, AO-RRT, SST) offrent une complétude probabiliste et une optimalité asymptotique, ils optimisent généralement uniquement le coût de fonctionnement (accumulé le long de la trajectoire, telle que la longueur du chemin ou l'énergie) et traitent l'atteinte de l'objectif uniquement comme une vérification de faisabilité.

Cette approche présente deux limitations critiques :

1. Qualité de l'état terminal : De nombreuses tâches dépendent de la qualité de l'état final, et non seulement du chemin parcouru. Par exemple, dans la poussée plane, un chemin plus court peut entraîner une incertitude terminale élevée, réduisant la probabilité d'atteindre avec succès l'objectif. Dans les scénarios multi-objectifs (par exemple, le stationnement de voiture), les planificateurs peuvent sélectionner un objectif géométriquement plus proche qui est moins souhaitable ou plus difficile à quitter, ignorant les préférences pour des régions terminales spécifiques.
2. Propagation de l'incertitude : La planification dans l'espace de croyance (où l'état est une distribution de probabilité) est essentielle pour la robustesse. Cependant, les méthodes existantes reposent souvent sur des seuils de probabilité rigides ou des substituts au niveau de la trajectoire plutôt que sur l'optimisation explicite de l'état de croyance terminal. De plus, les systèmes complexes (comme la poussée riche en contacts) manquent souvent de modèles d'incertitude analytiques, nécessitant des approches basées sur les données qui sont rarement intégrées étroitement aux planificateurs dans l'espace de croyance.

Méthodologie

Les auteurs proposent KiTe (Planification kinodynamique avec un coût terminal), un cadre qui enrichit l'Arbre Aléatoire de Recherche Rapide Optimal Asymptotiquement (AO-RRT) avec un objectif de coût terminal explicite. La méthodologie opère en trois couches principales :

1. KiTe avec coût terminal dans l'espace d'état

L'algorithme central reformule le problème de planification kinodynamique optimale dans un espace état-coût augmenté $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$.

• Dynamiques augmentées : L'état est étendu de $x$ à $y = (x, c)$, où $c$ accumule le coût de fonctionnement $\ell(x, u)$. Les dynamiques sont étendues pour inclure le taux de coût : $\dot{y} = (f(x, u), \ell(x, u))$.
• Intégration du coût terminal : Contrairement aux formulations précédentes où le coût terminal est ignoré pendant la recherche, KiTe évalue le coût total $C_T = \int \ell dt + \phi(x(T))$ uniquement lors de l'atteinte de la région objectif.
• Stratégie de recherche : L'algorithme maintient une borne de coût $c_{best}$. Lorsqu'une solution est trouvée, la borne est mise à jour à $c_{new} + \phi(x_{new})$. La stratégie d'échantillonnage se concentre ensuite sur l'ensemble de sous-niveau défini par cette borne, élaguant les branches qui ne peuvent pas améliorer le coût total (fonctionnement + terminal).
• Garantie théorique : Les auteurs fournissent une preuve rigoureuse que l'AO-RRT avec un coût terminal préserve l'optimalité asymptotique, à condition que les dynamiques du système, le coût de fonctionnement et le coût terminal satisfont des conditions de continuité de Lipschitz.

2. Extension à l'espace de croyance

La formulation est étendue à l'espace état-coût de croyance $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$, où $\mathcal{B}$ est l'espace des distributions de probabilité sur l'état.

• Métrique : La distance de Wasserstein 2 est adoptée comme métrique pour l'espace de croyance, car elle capture naturellement les changements à la fois de la moyenne et de la covariance des croyances gaussiennes.
• Minoration de l'atteinte de l'objectif : Une contribution théorique clé est la preuve que la probabilité d'atteindre une région objectif $G$ (définie comme une boule de rayon $r$) est minorée par la distance de Wasserstein entre la croyance terminale $b(T)$ et la mesure de Dirac à l'objectif $\nu_g$ :
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  Cela permet au planificateur d'utiliser la distance de Wasserstein comme coût terminal $\psi(b(T))$, optimisant directement une minoration de la réussite de l'atteinte de l'objectif sans nécessiter de seuils de probabilité rigides et manuellement ajustés.
• Continuité de Lipschitz : Les auteurs prouvent que sous des hypothèses légères sur les dynamiques stochastiques (spécifiquement concernant la continuité de Lipschitz de l'incertitude de processus et de l'injection de bruit), les dynamiques de croyance et les fonctions de coût restent continues de Lipschitz par rapport à la métrique de Wasserstein. Cela garantit que l'optimalité asymptotique de KiTe s'étend à l'espace de croyance.

3. Apprentissage des dynamiques de croyance

Pour traiter les systèmes où les modèles d'incertitude analytiques ne sont pas disponibles (par exemple, la poussée riche en contacts), KiTe intègre un modèle de dynamique de croyance appris.

• Architecture du modèle : Un réseau de neurones (MLP) est entraîné pour prédire la transition stochastique locale. Étant donné un état $x_i$ et une commande $u_i$, le réseau sort une transition moyenne $\Delta x_i$ et un paramètre de covariance de processus paramétré par un vecteur de variance $\sigma_i^2$.
• Objectif d'entraînement : Le modèle est entraîné en utilisant la perte de Négatif Log-Vraisemblance (NLL) sur des données d'interaction. Cela permet au réseau d'apprendre à la fois les dynamiques moyennes et l'incertitude aléatoire (bruit de processus) directement à partir des données.
• Propagation : Les moyenne et covariance apprises sont utilisées pour propager l'état de croyance (moyenne et covariance) vers l'avant dans le planificateur, prenant en charge à la fois les espaces d'état euclidiens et de groupes de Lie (par exemple, SE(2)).

Contributions clés

1. Extension algorithmique et preuve : Implémentation de l'AO-RRT avec un coût terminal explicite, accompagnée d'une preuve rigoureuse que l'optimalité asymptotique est préservée dans les espaces d'état et de croyance.
2. Lien théorique avec la probabilité de réussite : Preuve que la minimisation de la distance de Wasserstein entre la croyance terminale et l'objectif induit une minoration de la probabilité de réussite de l'atteinte de l'objectif, fournissant un objectif terminal fondé sur des principes pour la fiabilité.
3. Propagation de croyance basée sur les données : Démonstration de la manière d'apprendre des modèles de dynamique de croyance (moyenne et incertitude) directement à partir de données d'interaction en utilisant la perte NLL et de les intégrer dans le cadre KiTe pour les systèmes dépourvus de modèles analytiques.
4. Validation empirique : Évaluation complète sur trois tâches diverses (Flappy Bird, Stationnement de voiture, Poussée plane) en simulation et dans le monde réel, comparant KiTe à des références de l'état de l'art (AO-RRT, SST, Arbre de croyance gaussien).

Résultats expérimentaux

L'article valide KiTe sur trois tâches :

• Flappy Bird (Déterministe) : Démontre que KiTe peut encoder des préférences d'objectif (par exemple, centrer l'oiseau dans l'ouverture) en optimisant le coût terminal, alors que les références arrêtent d'affiner une fois la région objectif atteinte. KiTe réalise des trajectoires plus proches du centre de l'objectif avec seulement une augmentation modeste du coût de fonctionnement.
• Stationnement de voiture (SE(2) avec incertitude) : Dans une tâche avec plusieurs places de stationnement réalisables, KiTe priorise avec succès une région d'objectif « préférée » (place avant) et améliore les taux de réussite de l'atteinte de l'objectif par rapport aux références qui optimisent uniquement la longueur du chemin ou utilisent des contraintes de chance. La planification dans l'espace de croyance avec coût terminal a produit les taux de réussite d'exécution les plus élevés.
• Poussée plane (Riche en contacts, Monde réel) : En utilisant des dynamiques apprises pour des objets tels qu'un camion à ordures et une boîte de biscuits, KiTe a surpassé les références (y compris la planification active avec incertitude de modèle) en termes de taux de réussite d'exécution. Le planificateur a sélectionné des chemins géométriques plus longs qui ont entraîné une incertitude terminale plus faible et des probabilités de réussite plus élevées, démontrant la valeur de l'optimisation de la qualité de la croyance terminale.

Importance et affirmations

L'article affirme que KiTe comble une lacune fondamentale dans la planification kinodynamique : la séparation de l'optimisation de trajectoire de la qualité de l'état terminal. En optimisant explicitement l'état terminal, la méthode :

• Améliore la fiabilité : Augmente directement la probabilité d'atteindre l'objectif dans l'incertitude en minimisant la distance de Wasserstein vers l'objectif.
• Encode les préférences : Permet aux planificateurs de distinguer entre plusieurs objectifs réalisables basés sur la désirabilité (par exemple, facilité de départ, stabilité) plutôt que sur la simple proximité géométrique.
• Fait le lien entre apprentissage et planification : Fournit un cadre où les modèles d'incertitude appris ne sont pas seulement utilisés pour biaiser l'échantillonnage, mais sont étroitement intégrés dans la boucle de propagation de croyance et d'optimisation.

Les auteurs concluent que, bien que la méthode introduise des paramètres de conception (poids terminaux) et repose sur des modèles appris, elle offre une approche fondée sur des principes et théoriquement solide pour générer des mouvements non seulement dynamiquement réalisables, mais aussi fiables et conscients des préférences dans des environnements incertains.