PLANIFICATION DE MOUVEMENT DANS DES SCENESDYNAMIQUES

Léonard JAILLET ∗

Directeur de thèse : Thierry Siméon

Laboratoire d’accueil :LAAS-CNRS7, av. du Colonel Roche31077 Toulouse Cedex 4

Etablissement d’inscription :Université Paul Sabatier118, route de Narbonne31062 Toulouse Cedex 4

Résumé

Cet article présente un planificateur de mouvement pour des robots opérant dans des environnementsdynamiques. Le planificateur repose sur des techniques probabilistes à l’origine utilisées pour résoudredes problèmes de requêtes simples ou multiples en environnement statique. Notre méthode s’appuiesur une phase d’initialisation, pendant laquelle un premier graphe est construit. Plusieurs mécanismesdits d’ “évaluation paresseuse” sont ensuite utilisés afin de mettre à jour ce graphe le plus rapidementpossible, en fonction des changements dynamiques. Une étape de renforcement peut aussi conduire à lacréation de cycles correspondant à d’autres classes de chemins non encore capturés par le graphe. Lesrésultats de simulation montrent que cette combinaison de techniques permet d’obtenir un planificateur,capable de résoudre efficacement des problèmes dans des environnements géométriquement complexeset comportant des obstacles mobiles.

Mots-clés

Planification de mouvements, robotique, environnements dynamiques.

1 INTRODUCTION

La planification de mouvement de systèmes robotiques a suscité de nombreux travaux aucours des deux dernières décennies [10]. Ceux-ci ont permis de s’attaquer à des problèmesambitieux, issus de disciplines diverses, telles que la robotique, l’animation graphique, le pro-totypage virtuel ou encore la biologie structurale. Les planificateurs de mouvements proba-bilistes ont été à l’origine conçus pour résoudre des problèmes de requêtes simples [8] oumultiples [7, 9] en environnements statiques. De nombreuses applications de la planificationde mouvement nécessitent cependant de prendre en compte des changements dynamiques.Parmi elles, on peut citer la planification en milieu industriel évolutif [12], la navigation enenvironnement réel [5], ou virtuel [13]. En dépit du succès des méthodes probabilistes, l’adap-tation de ces méthodes à des environnements comportant à la fois des obstacles mobiles etstatiques a jusqu’ici été limitée de part la difficulté de mise à jour efficace de la structuresde graphe. Parmi les récents travaux, on peut tout de même citer : les planificateurs baséssur des “roadmaps” [1,4,11], les techniques de replanification temps-réel, basées sur la notionde bande élastique, [3], ou utilisant une méthode de décomposition [2], ou encore les tech-niques explorant l’espace configuration× temps pour les cas où la trajectoire des obstaclesest connue [6].∗ljaillet@laas.fr

Fig. 1 – Exemple de scène 3D comportant des obstacle mobiles : configurations initiales etfinales du manipulateur mobile à 9 ddls transportant une planche et trois chemins solutionobtenus pour différents contextes de l’environnement (portes ouvertes/fermées et trois autresobstacles mobiles placés autour de la table)

Dans cet article nous proposons un planificateur s’appuyant sur une remise à jour rapidedes portions valides du graphe en fonction de la position des obstacles dynamiques lors dechaque requête. Pour ce faire, ce planificateur combine plusieurs idées, permettant ainsi derépondre très rapidement aux requêtes quand les obstacles mobiles ont un faible impact surla connectivité de l’espace libre. La suite de l’article est organisée de la façon suivante : lasection 2 présente les grandes lignes de notre approche. La section 3 détaille le fonctionne-ment du planificateur et explique comment les requêtes sont exécutées à l’aide d’une mise àjour partielle du graphe et de son renforcement dans les cas nécessaires. La section 4, décritbrièvement comment ce planificateur peut être utilisé dans des applications de mise à jourou de contrôle d’exécution de chemins. Finalement, les performance de ce planificateur sontévaluées expérimentalement dans la section 5, sur plusieurs environnements de simulation.

2 PRESENTATION DE L’APPROCHE

2.1 MOTIVATION

Considérons un robot dans un environnement composé d’obstacles dynamiques et sta-tiques. Le problème consiste à trouver un chemin sans collision pour ce robot, allant d’uneconfiguration initiale à une configuration finale, pour des positions données d’obstacles mo-biles. La figure 1 illustre un exemple typique d’un tel scénario. Le robot (un manipulateurmobile à 9 ddls transportant une planche) évolue au sein d’un environnement de type bureau.Les obstacles mobiles dans cet exemple correspondent aux deux portes qui peuvent être enposition ouvertes ou fermées, ainsi que d’autres obstacles mobiles situés autour de la table (c.f.image à droite). Ceux-ci peuvent invalider des portions du graphe, c’est pourquoi, nous avonsbesoin d’un mécanisme efficace pour mettre à jour cette validité. La figure montre égalementpour une même requête de planification, différents chemins solution calculés en temps-réel parle planificateur, en fonction de la position des obstacles mobiles.

Il est bien connu que les planificateurs de type PRM passent la plus part de leur temps àréaliser des tests de collision au cours de la construction du graphe. Plusieurs méthodes [1,14],proposent dans le cas d’environnements statiques, de réduire le nombre de tests en repoussantceux-ci tant qu’ils ne sont pas réellement nécessaires.

Nous appliquons une idée similaire pour le cas d’environnements dynamiques. Dans untel contexte, nous utilisons ici le fait que les obstacles mobiles n’impliquent souvent qu’unemodification partielle de l’espace libre du robot. En s’appuyant sur cette remarque, troisprincipales méthodes sont utilisées pour accélérer le planificateur :

– Une stratégie de type connection paresseuse est utilisée pour tester les connections desnoeuds de la requête au graphe et pour limiter la mise à jour de celui-ci aux portionspertinentes en vue d’obtenir un chemin solution.

– Un mécanisme de reconnection locale tente de reconnecter les arêtes temporairement casséespar les obstacles mobiles.

– Un mécanisme d’étiquetage d’arête mémorise les positions prises par les obstacles mobilesde la scène, pour alléger les tests de collision au sein des arêtes.

Ces méthodes sont détaillées au cours de la section 3.

2.2 APPROCHE GENERALE

L’approche générale se décompose en deux étapes : d’abord, un premier graphe est calculépour le robot en fonction de l’environnement, mais sans prendre en compte les obstaclesmobiles (Fig. 2.1). Ensuite, lors de la résolution d’une requêtes donnée, des portions de cegraphe initial sont mises à jours en testant si les arêtes associées sont en collision ou non visà vis de la position courante des obstacles mobiles. Les arêtes en collision, sont étiquetéescomme bloquées au sein du graphe. Si cet étiquetage permet d’obtenir un chemin qui necontient aucune arête bloquée, alors la solution est trouvée (Fig. 2.2). Si le chemin trouvécontient des arêtes bloquées, un mécanisme de reconnection locale basé sur les techniques RRTessaie de reconnecter les extrémités des arêtes (Fig. 2.3). Finalement, si le graphe ne permetpas d’obtenir une solution, de nouveaux noeuds sont insérés conduisant à un renforcementdu graphe à travers l’apparition de cycles (Fig. 2.4). L’efficacité pratique de l’approche vientaussi de l’utilisation de plusieurs autres méthodes permettant d’éviter des tests de collisioninutiles. Les éléments principaux de cette approche sont détaillés dans la section suivante.

3 DETAILS DE L’APPROCHE

3.1 ETIQUETAGE ET RECHERCHE D’UN CHEMIN SOLUTION

Le planificateur effectue l’étiquetage des arêtes parallèlement à la recherche d’un cheminsolution. Deux opérations sont alternées successivement tant qu’un chemin valide n’a pas ététrouvé ou que toutes les connections envisageables n’ont pas été testées :– Connection des noeuds de requête aux noeuds du graphe.– Recherche d’un chemin valide au sein du graphe.

La recherche d’un chemin valide conduit à l’étiquetage de certaines des arêtes du graphe.Un premier chemin valide par rapport à l’environnement statique est d’abord recherché, puisla validité de ses arêtes sont testées en dynamique. Une solution est trouvée si toutes lesarêtes sont sans collision par rapport aux obstacles dynamiques. Sinon, celles en collision sontinvalidées et l’information est utilisée pour mettre à jour la connectivité du graphe. La mise àjour de cette connectivité est ensuite utilisée lors de la prochaine itération afin de choisir lesmeilleurs noeuds candidats du graphe auxquels se relier afin de rechercher un autre cheminsolution. Dans la sous-section suivante, nous expliquons comment ces noeuds candidats sontchoisis en partant des informations courantes de connectivité.

Fig. 2 – Mécanismes généraux de l’approche.

3.2 CONNECTIONS DES NOEUDS DE REQUÊTE

A chaque itération, les noeuds candidats pour être connectés aux noeuds de requête, sontchoisis en décomposant chacune des composantes connexes en trois sous-composantes : lesnoeuds potentiellement atteignables à partir du noeud initial, les noeuds potentiellement at-teignables à partir du noeud final et les noeuds qui pour le moment ne sont pas potentiellementatteignables à partir des noeuds de requête. Un noeud est dit potentiellement atteignable àpartir d’un autre noeud s’il existe un chemin reliant les deux noeuds, comportant seulementdes arêtes valides ou non encore testées vis à vis du contexte dynamique (mais pas d’arêtesétiquetées comme bloquées).

La première sélection se fait en considérant qu’aucun des noeuds du graphe n’est potentiel-lement atteignable et en choisissant parmi ceux-ci le noeud le plus proche de chacun des noeudsde requête. Un chemin statiquement valide est ensuite recherché à l’intérieur du graphe et lesarêtes du (ou des) chemin(s) trouvé(s) sont étiquetées comme valides ou bloquées en fonctiondu contexte dynamique. Les trois sous composantes sont ensuite utilisées lors de la prochainesélection des noeuds de connection. De nouvelles connections sont maintenant envisagées :celle entre le noeud initial (respectivement final) et l’ensemble des noeuds potentiellementatteignables par l’autre noeud de requête, mais aussi la connection à des portions du grapheactuellement non connectées aux noeuds de requête. Chacun des trois types de connectionscandidats est déterminé en prenant à chaque fois les plus courtes non déjà testées.

3.3 RECONNECTIONS LOCALES

Comme indiqué plus haut, à chaque fois que les noeuds de requête sont connectés augraphe, l’algorithme tente de trouver un chemin solution ne comportant aucune arête bloquée.Si un tel chemin ne peut être extrait du graphe, notre algorithme tente de reconnecter lesextrémités des arêtes cassées. Le principe de l’algorithme bidirectionnel RRT-Connect (c.f.[9]) utilisé dans notre méthode de reconnection, consiste à construire incrémentalement deuxarbres initialisés par les noeuds initiaux et finaux de la requête et guidés par une heuristiquesimple, de telle sorte que chacun des arbres explore l’espace autour de lui tout en avançantl’un par rapport à l’autre. Si ce mécanisme de reconnection échoue, une nouvelle connectiondes noeuds de requête au graphe est tentée, comme expliqué en section 3.B.

3.4 INSERTION DE NOEUDS ET CREATION DE CYCLES

Si tous les types de connections ont été tentés sans succès, de nouveaux noeuds sont in-sérés dans le graphe afin de résoudre la requête. Ces noeuds sont connectés aux différentescomposantes connexes du graphe, prenant en compte le résultat de l’étiquetage partiel pré-cédemment réalisé. Ces nouveaux noeuds sont nécessaires afin de trouver d’autres classes dechemins quand la connectivité du graphe est modifiée par les obstacles mobiles (voir Fig. 2.4).Le fait que cette étape n’apparaisse que lorsque le processus de reconnection locale a échoué,permet de limiter la création d’arêtes et de noeuds inutiles qui pourraient réduire l’efficacitédu planificateur lors de la résolution de nouvelles requêtes.

3.5 ETIQUETAGE D’ARÊTE

Le planificateur utilise aussi l’information des requêtes passées pour améliorer l’efficacitéde l’étiquetage des arêtes. Pour ce faire, une structure de donnée spécifique est insérée dansle graphe. Cette structure mémorise des informations de collision pour des positions clefsd’obstacles mobiles. Quand la fonction Etiqueter arête est appelée, les positions courantesdes obstacles mobiles sont comparées aux positions déjà stockées et l’information issue desprécédents tests de collision est réutilisée (voir algorithme 1).

Comme certains obstacles peuvent bouger continûment dans l’environnement et avoirainsi une infinité de positions intermédiaires, seules les dernières positions prises par les obs-

Algorithm 1 Etiqueter arête(E)for each Movable Obstacle MOi dofor each Key Position KPj of MOi stored in E doif KPj = Current Pos thenis-collision←Extract-Col-Info(E,KPj)

elseis-collision←Col-test(E, MOi)Store is-collision in R

end ifif is-collision = TRUE then

Return invalidend if

end forend forReturn valid

tacles sont conservées. En pratique, la mémorisation de l’information concernant seulement lesquelques dernières positions est suffisante pour éviter de nombreux tests inutiles, notammentpour les deux types de situation suivants : quand un obstacle a des positions discrètes (i.e.une porte soit fermée soit ouverte), ou quand un obstacle s’arrête (i.e. un objet déplacé ou unrobot à l’arrêt).

4 PROBLEMES TEMPS REEL

Ce planificateur a aussi été testé dans le cas d’applications nécessitant des performancestemps-réel. Deux types de problèmes ont été abordés :– Mise à jour d’un chemin valide.– Contrôle de l’exécution d’un chemin.Le mécanisme utilisé est assez similaire pour les deux types de problèmes. Pour la mise à jourde chemin, on calcule d’abord un premier chemin valide à l’aide de notre planificateur. Savalidité est ensuite testée régulièrement vis à vis des obstacles mobiles, à chaque intervallede temps donné. Si le chemin est en collision, un mécanisme de reconnection locale tel qu’ilavait été proposé pour les arêtes du graphe, est essayé. Lorsqu’il échoue, notre planificateurtrouve un nouveau chemin valide dans le contexte courant. Pour l’exécution de chemin, àchaque intervalle de temps, en plus de la mise à jour du chemin le robot avance le long decelui-ci d’un pas de longueur fixe. En pratique, seules les portions de chemin que le robotva prochainement parcourir sont testées. Ceci permet d´éviter de replanifier dans le cas oùun obstacle croise la trajectoire du robot loin de sa position actuelle, alors qu’au moment oùcelui-ci arrive sur la portion concernée, l’obstacle a des chances de ne plus y être. La section5.3, montre un exemple mettant en oeuvre ce mécanisme.

5 RESULTATS EXPERIMENTAUX

Le planificateur dynamique a été implémenté sur la plate-forme de travail Move3D [16]développée au LAAS. Les temps de calcul indiqués correspondent à des expérimentationsréalisées sur une station de travail Sun Blade 100 333 MHz. Chacune des valeurs données dansles tables est une moyenne sur 20 itérations de l’algorithme. Les tests de collision indiquéscorrespondent à la somme des tests de collision réalisés en statique et en dynamique.

5.1 PERFORMANCE GLOBALE

Lors de la première expérience notre planificateur est comparé au planificateur simplerequête bi-RRT [9] ainsi qu’à un planificateur basé sur un graphe précalculé à l’aide de laméthode de Visibilité-PRM [15], mais étiquetant systématiquement toutes la arêtes du graphe(que nous appelons étiquetage global). L’environnement utilisé est celui de la figure 1. Il possèdedeux portes aléatoirement ouvertes ou fermées au début de chaque requête. Trois autres objetsmobiles sont aussi placés aléatoirement autour de la table. Les résultats comparatifs associésà ces trois types de méthodes sont représentés table 1.

Requête Étiquetage PlanificateurSimple Global Dynamique

Temps (s) 4.3 1.1 0.2Collisions 5697 1722 356

Arêtes 45 78 78Arêtes testées 45 78 18

Tab. 1 – Performance du planificateur dynamique comparativement à un planificateur simplerequête et à un planificateur avec étiquetage global du graphe.

Notons d’abord que la méthode d´ étiquetage global est plus efficace que celle dite de“simple requête”. Même si plus d’arêtes sont testées (78 contre 45), celles-ci ne le sont quepar rapport aux obstacles mobiles, ce qui diminue significativement le coût global des testsde collision. Notons également le gain dû à notre méthode de “connection paresseuse” etd’étiquetage d’arête, comparativement à une méthode d’étiquetage systématique. Le nombrede tests est à peu près divisé par 5, même pour la faible taille du graphe de l’exemple considéré.

Fig. 3 – Un exemple de requête (gauche) et un chemin solution (droite), pour un environne-ment comportant des colonnes statiques et 20 obstacles mobiles à forme cubique.

5.2 CONNECTION PARESSEUSE

Pour évaluer d’avantage les performances de notre mécanisme de connection paresseuse,nous avons réalisé des tests sur l’environnement de la figure 3, pour différentes tailles degraphes précalculés (contenant ou non des cycles) et pour différents nombres d’obstacles mo-biles. Ces éléments sont résumés dans le tableau ci-dessous :

Test Obstacles Mobiles Noeuds ArêtesA 5 100 99B 5 100 318C 20 100 99D 20 100 318E 20 200 700

Les configurations initiales et finales et les positions des obstacles mobiles sont déterminéesaléatoirement. La table 2 présente les résultats de ces tests.

Les tests utilisant la méthode de connection paresseuse sont de 7 à 12 fois plus rapides queceux basés sur un étiquetage global. Ceci peut notamment s’expliquer par la faible portionde graphe alors explorée en utilisant notre méthode. L´étiquetage paresseux est sensible aunombre d’arêtes, cependant, le gain par rapport à une méthode d’étiquetage global restesensiblement le même quand la taille du graphe augmente (E/C, B/A).

5.3 APPLICATIONS TEMPS-REEL

La figure 4 montre un exemple d’exécution de chemin contrôlée par le planificateur dy-namique. Le robot mis en jeu est un bras manipulateur à 6 ddls, utilisé sur une ligne d’as-semblage. Il est entouré de deux autres bras articulés jouant le rôle d’obstacles mobiles. Lapremière image montre les positions initiales et finales du problème (la position finale estreprésentée en semi-transparence) ainsi que la trajectoire calculée initialement. Celle-ci estensuite exécutée, mais elle est perturbée successivement par le déplacement de chacun desdeux bras manipulateurs présents dans la scène. C’est pourquoi au cours de son exécution, latrajectoire est mise à jour en l’aide de notre planificateur dynamique.

Fig. 4 – Execution de trajectoire contrôlée par le planificateur dynamique pour un bras mani-pulateur à 6 ddls utilisé sur une ligne d’assemblage.

Cet exemple permet d’illustrer les capacités temps-réel de notre planificateur. Plusieursfilms illustrant ces mécanismes de mise à jour et d’exécution de chemins sont égalementdisponibles à l’adresse suivante : http://www.laas.fr/~ljaillet.

A B C D EConnection paresseuse

Temps (s) 0.2 0.3 0.7 1.0 1.7Collisions 340 478 640 681 1379

% d’arêtes testées 9.1 4.1 10.1 5.7 6.1Etiquetage global

Temps (s) 2.4 3.6 4.8 7.3 15.7Collisions 4602 7123 4882 6809 14338

% d’arêtes testées 100 100 100 100 100

Tab. 2 – Influence du mécanisme de connection paresseuse en fonction du nombre d’obstaclesmobiles et de la taille du graphe.

6 CONCLUSION ET PERSPECTIVES

Dans cet article, nous avons proposé un planificateur de mouvement pour agir dans desenvironnement pouvant comporter des changements dynamiques. Celui-ci repose sur plusieursmécanismes dits d” ’évaluation paresseuse”, permettant une mise à jour partielle mais rapidedu graphe en vue de répondre efficacement aux requêtes de planification. Les résultats préli-minaires précédemment présentés sont encourageants. Ils montrent que le planificateur peutrésoudre avec des performances de l’ordre du temps réel, des problèmes dans des scènes géo-métriquement complexes et comportant plusieurs obstacles mobiles. Plusieurs améliorationsrestent cependant à envisager. En particulier, il pourrait être utile de développer une mé-thode capable de reporter directement des changements locaux de l’environnement sur lesportions du graphe potentiellement affectées, afin de limiter les tests de validité aux portionsvéritablement pertinentes du graphe.

Références

[1] R. Bohlin, L. Kavraki. “Path Planning Using Lazy PRM”. In Proc. of the Int. Conf. on Roboticsand Automation, 2000.

[2] O. Brock, L. Kavraki. “Decomposition-based Motion Planning: A Framework for Real-time Mo-tion Planning in High-dimensional Configuration Spaces”. In IEEE Int. Conf. on Robotics andAutomation, 2001.

[3] O. Brock and O. Khatib. “Elastic Strips: A Framework for Integrated Planning and Execution”.In Proc. Int. Symp. on Experimental Robotics, 1999.

[4] M. Cherif, M. Vidal. “Planning handling operations in changing industrial plants”. In Procs ofthe IEEE on Robotics and Automation, 1998.

[5] E. Feron, E. Frazzoli and M. Dahleh. “Real-time motion planning for agile autonomous vehicles”.In AIAA Conf. on Guidance, Navigation and Control, Denver, August 2000.

[6] D. Hsu, R. Kindel, J.C. Latombe, S. Rock. “Randomized Kinodynamic Motion Planning with Mo-ving Obstacles”. In Proc. of the Workshop on Algorithmic Foundations of Robotics (WAFR’00),2000.

[7] D. Hsu, J.C. Latombe, R. Motwani. “Path planning in Expansive Configuration Spaces”. In Proc.of the IEEE Int. Conf. on Robotics and Automation, 1997.

[8] L. Kavraki, P. Svestka, J.-C. Latombe, and M. OverMars. “Probabilistic Roadmaps for PathPlanning in High-Dimensional Configuration Spaces”. In IEEE Transactions on Robotics andAutomation,12(4), 1996.

[9] J. Kuffner, S.LaValle. “RRT-Connect: An efficient Approach to Single Query Path Planning”. InProc. of the IEEE Int. Conf. on Robotics and Automation, 2000.

[10] J.C. Latombe. “Robot motion planning”. In Kluwer academic Press, 1991.[11] P. Leven S. Hutchinson “Toward Real-Time Path Planning in Changing Environments”. In Proc.

of the Workshop on Algorithmic Foundations of Robotics, 2000.[12] A. McLean, I. Mazon. “Incremental Roadmaps and Global path Planning in Evolving Industrial

Environments”. In Proc. of the IEEE Int. Conf. on Robotics and Automation, 1996.[13] M.H. Overmars “Algorithms for motion and navigation in virtual environments and games”. In

Proc. of the Workshop on Algorithmic Foundations of Robotics, 2002.[14] G. Sanchez, J.C. Latombe, “A Single Query Bi-Directional Probabilistic Roadmap Planner with

Lazy Collision Checking”. In International Symposium on Robotics Research, Lorne, Victoria,Australia, November 2001.

[15] T. Siméon, J.P. Laumond, C. Nissoux. “Visibility based Probabilistic Roadmaps for MotionPlanning”. In Advanced Robotics Journal, 14(6), 2000.

[16] T. Siméon, J.P. Laumond, C. van Geem and J. Cortés. “Computer Aided Motion: Move3D withinMOLOG.”. In IEEE Int. Conf. on Robotics and Automation, 2001.