Université de Picardie Jules Verne 2015-2016 M2 EEAII ViRob Localisation et navigation de robots
1/4 Fabio Morbidi
TD4 Evitement d'obstacles
Exercice 1 : Champs de potentiel
Soient qs, qb ∈ 2 respectivement la position initiale et finale d'un robot dans un environnement 2D. Ecrire une fonction Matlab qui prend en entrée les points qs et qb, et les coordonnées de N obstacles ponctuels disposés de façon aléatoire dans l'environnement, et qui détermine et visualise la trajectoire sans collisions du robot avec la méthode des champs de potentiel. Considérez des potentiels quadratiques avec gains unitaires, et un rayon d'influence de 0.5 mètres pour les N obstacles. Testez la fonction pour qs = [0, 0]T, qb = [10, 10]T et pour N = 15 obstacles tirés d'un distribution uniforme dans (0, 10) × (0, 10). Pour visualiser le champ de potentiel total dans l'espace 3D et la trajectoire du robot, utiliser les commandes Matlab, meshgrid et mesh.
Exercice 2 : Graphe de Voronoï
Répétez l'Exercice 1 en utilisant la méthode basée sur le graphe de Voronoï. Appuyez-vous sur la commande Matlab "voronoi"pour déterminer le diagramme de Voronoï associé aux N obstacles ponctuels (Attention: le bord d'une figure Matlab représente la frontière de l'environnement 2D). Rappelez-vous que les arêtes du graphe de Voronoï sont les points qui sont équidistants des deux obstacles les plus proches.
Exercice 3 : Planification probabiliste
Dans cet exercice nous utiliserons le Robotics Toolbox de P. Corke pour étudier deux méthodes probabilistes pour la planification de trajectoire sans collisions. Ces méthodes sont dites hors ligne (off-line): en fait, différemment des méthodes vues dans les Exercices 1 et 2, elles nécessitent d'une connaissance a priori de la géométrie et de la position de tous les obstacles dans l'espace de travail du robot.
a) Probabilistic Roadmap (PRM)
On réinitialise d'abord le générateur de nombres pseudo-aléatoires à un état connu:
>> randinit
Nous définissons la position de départ, qs et la position à atteindre, qb du robot:
>> start = [20 10]';
>> goal = [50 30]';
Nous chargeons maintenant un environnement prédéfini encombré d'obstacle,
>> load map1
qui definit une variable map (une matrice 100 × 100) dans le workspace de Matlab.
Pour utiliser le planificateur du Robotics Toolbox, nous créons d'abord un objet "PRM":
>> prm = PRM(map)
Université de Picardie Jules Verne 2015-2016 M2 EEAII ViRob Localisation et navigation de robots
2/4 Fabio Morbidi
Ensuite, on génére la roadmap:
>> prm.plan()
Il est à noter que nous n'avons pas passé l'argument goal au planificateur car la roadmap est indépendante de la position à atteindre qb. La détermination d'un chemin sans collisions est un processus qui comporte deux phases: 1) la phase de planification et 2) la phase "query".
La phase de planification détermine N points de façon aléatoire (N = 100, par défaut),qui se trouvent dans l'éspace libre. Chaque point est connecté avec ses voisins les plus proches par une ligne droite qui ne traverse aucun obstacle, afin de créer un graphe avec un nombre
minimal de composantes disjointes. Le graphe résultant est stocké dans l'objet PRM.
Pour afficher les caractéristiques principales de ce graphe, il faut juste taper:
>> prm prm =
PRM: 100x100 graph size: 100
dist thresh: 30.000000 100 vertices
712 edges 3 components
ce qui nous donne des informations sur le nombre d'arêtes et le nombre de composantes connexes du graphe. Le graphe peut être visualisé via:
>> prm.visualize()
Les points bleus sont les points choisis aléatoirement et les lignes droites sont les chemins sans collisions entre eux. Seulement les chemins d'une longueur inférieure à 30 cellules sont sélectionnés: ça corresponde au paramètre dist thresh (seuil sur la distance) de la classe PRM. Chaque arête du graphe a un coût associé qui est la distance entre ses deux nœuds.
On remarque qu'il y a deux nœuds sur le côté gauche de l'environnement qui sont déconnectés du corps principal de la roadmap.
Dans la phase "query", on trouve le chemin entre qs et qb. Il s'agit simplement de se déplacer vers le nœud le plus proche dans la roadmap, de suivre la roadmap (comme un train dans une ligne de chemin de fer) et de quitter la roadmap lorsque le robot a atteint le nœud le plus proche de qb. La commande:
>> prm.path(start, goal)
montre une animation du robot se déplaçant sur le graphe. Le prochain nœud sur la roadmap est indiqué en jaune et une séquence de points verts montre le chemin suivi par le robot.
Se déplacer sur la roadmap consiste à se diriger vers le nœud voisin qui a le moindre coût, c'est-à-dire le plus proche de qb. On répéte la procédure jusqu'à ce que le robot arrive au nœud du graphe le plus proche de qb, et partant de là on passe directement à qb.
Un avantage de la PRM est qu'une fois que la roadmap a été crée dans la phase de planification, qs et qb peuvent être changés à un coût très faible: il faut juste relancer la phase de query. Toutefois, le chemin trouvé n'est pas optimal et la distance parcourue par le robot:
>> p = prm.path(start, goal);
>> numcols(p) ans = 299
est plus grande que la distance optimale, qui comprend 205 points.
Université de Picardie Jules Verne 2015-2016 M2 EEAII ViRob Localisation et navigation de robots
3/4 Fabio Morbidi
Il faut alors envisager des compromis entre l'optimalité du chemin du robot et la complexité de calcul de ce chemin.
1. Comme nous générons des points dans l’espace des configurations du robot d'une façon aléatoire, un graphe différent est déterminé toutes les fois que le planificateur
est lancé, ce qui entraîne des chemins différents et d'une longueur différente.
Par exemple, si on relance le planificateur une deuxième fois:,
>> prm.plan()
nous trouvons un graphe avec des nœuds à différents emplacements et un nombre de nœuds différent.
2. Le planificateur peut échouer en créant un graphe avec des composantes disjointes.
Le premier graphe généré était presque connexe (il avait trois composantes disjointes, dont deux, sur le côté gauche, constituées d'un seul nœud), alors que le deuxième graphe aléatoire généré avait une grande composante déconnectée. Si qs et qb ne sont pas connectés, c'est-à-dire ils se trouvent à proximité de deux composantes différentes de la roadmap, la méthode path signalera une erreur. La seule solution, alors, est de relancer à nouveau le planificateur.
3. Les passages longs et étroits entre les obstacles ont peu de chances d'être exploités, car la probabilité de générer aléatoirement des points situés dans ces endroits est très faible. Dans le premier graphe, le planificateur a trouvé un chemin autour de l'obstacle dans le sens antihoraire. Ce parcours est plus long que le chemin optimal qui traverse l'étroit passage vertical sur le côté gauche.
b) Rapidly-exploring Random Tree (RRT)
Le Rapidly-exploring Random Tree ou RRT est un algorithme de planification probabiliste comme PRM. Son principal avantage est qui il permet de prendre en compte le modèle cinématique ou dynamique d'un robot. De cette manière RRT ne se limite pas aux véhicules omni-directionnels comme PRM.
Comme dans le cas précédent, on réinitialise le générateur de nombres pseudo-aléatoires à un état connu:
>> randinit
On va ensuite définir un robot de type véhicule (car-like) avec une limitation sur l'angle de braquage α de ±1.2 rad (la vitésse longitudinale est limitée de défaut à ± 1 m/s):
>> veh = Vehicle([], 'stlim', 1.2)
Nous définissons maintenant la pose de départ qs (la position le long des axes x et y, et l'orientation θ du robot), et la pose à atteindre qb du robot:
>> start = [0 0 0]';
>> goal = [0 2 0]';
On considère ici le problème difficile de faire déplacer latéralement un robot car-like.
Plus précisément, nous cherchons un chemin pour faire déplacer le robot latéralement de 2 mètres avec la même orientation initiale et finale (θ = 0).
Université de Picardie Jules Verne 2015-2016 M2 EEAII ViRob Localisation et navigation de robots
4/4 Fabio Morbidi
On génére un objet "RRT" pour un environnement sans obstacles (les obstacles ne sont pas encore supportés par le Robotics Toolbox), d'une façon similaire à PRM:
>> rrt = RRT(200, veh, 'goal', goal, 'range', 5)
où on a specifié un arbre avec 200 nœuds et limité la taille de l'environnement à ± 5 m.
Nous pouvons maintenant créer l'arbre de navigation via:
>> rrt.plan()
et utiliser,
>> rrt.path(start, goal)
pour visualiser le résultat. On a trouvé un chemin lisse et continu, qui notre robot non-holonome peut parcourir sans problèmes.
Nous remarquons que le mouvement du robot ne termine pas exactement à qb, mais en correspondance du nœud de l'arbre le plus proche de qb. Pour éviter ce problème, on peut générer un arbre plus dense (par exemple avec 500 nœuds au lieu de 200 nœuds) ou agir sur la commande de braquage du robot dans le dernier segment du chemin généré.
