Université de Picardie Jules Verne 2017-2018 M2 EEAII ViRob Localisation et navigation de robots
1/4 Fabio Morbidi
TD4: Évitement d'obstacles
Le objectif de ce TD est de mettre en œuvre sur Matlab, quatre des six stratégies pour l'évitement d'obstacles vues dans le cours. Pour les planificateurs probabilistes, la Robotics Toolbox de P. Corke sera utilisée: http://petercorke.com/wordpress/toolboxes/robotics-toolbox
Exercice 1 : Champs de potentiel
Soit qs ∈ 2 la position de départ et qb ∈ 2 la position à atteindre d'un robot mobile dans un environnement rectangulaire [a, b] × [a, b] m2.
• Écrire une fonction Matlab, evit_champ, qui prend en entrée les positions qs, qb, le nombre Nd'obstacles, les paramètres a et b, les gains ka, kr,i> 0 du potentiel attractif et des potentiels répulsifs, le rayon d'influence ρo,i des obstacles et l'exposant γ qui apparaît dans l'expression du potentiel répulsif. Considérez un potentiel quadratique attractif et des obstacles ponctuels dont la position qo,i est distribué uniformémentsur [a, b] × [a, b]. La fonction doit afficher le potentiel total (voir les commandes meshgrid et mesh de Matlab pour tracer une fonction à deux variables) et la trajectoire sans collisions du robot calculée avec l'algorithme du gradient.
• Tester la fonction evit_champ pour qs = [0, 0]Tm, qb = [7, 7]Tm, N = 5, a = 0 m, b = 10 m, ka = kr,i= 0.2, ρo,i = 1 m, i ∈ {1, 2, ..., N}, et γ = 2. Étudier les trajectoires du robot obtenues avec des valeurs de ka, kr,i, ρo,i, γ et αk (le pas discret utilisé dans l'algorithme du gradient) différentes.
Exercice 2 : Graphe de Voronoï
Soit qs∈ 2 la position de départ et qb∈ 2 la position à atteindre d'un robot mobile dans un environnement rectangulaire [a, b] × [a, b] m2.
• Écrire une fonction Matlab, evit_voronoi, qui prend en entrée les positions qs, qb, le nombre Nd'obstacles et les paramètres a et b. La fonction doit afficher le graphe de Voronoï qui a pour générateurs les N d'obstacles (voir la commande voronoi de Matlab) et un chemin sans collisions du robot superposé sur le graphe, calculé selon un critère de votre choix. Comme dans l'Exercice 1, les N obstacles ponctuels sont distribués uniformément dans l'environnment.
• Tester la fonction evit_voronoipour qs = [0, 0]Tm, qb = [7, 7]Tm, N = 15, a = 0 m et b = 10 m.
Université de Picardie Jules Verne 2017-2018 M2 EEAII ViRob Localisation et navigation de robots
2/4 Fabio Morbidi
Exercice 3 : Planification probabiliste (PRM, RRT)
Dans cet exercice nous utiliserons la Robotics Toolbox de P. Corke pour étudier deux planificateurs probabilistes. Télécharger la dernière version de la toolbox (pour Matlab R2017a): "RTB-10.2.zip"
a) Probabilistic Roadmap (PRM)
On réinitialise d'abord le générateur de nombres pseudo-aléatoires à un état connu:
>> randinit
La position de départ qs et la position à atteindre qb du robot sont définies par les commandes:
>> qs = [20 10]';
>> qb = [50 30]';
Nous chargeons maintenant un environnement prédéfini encombré d'obstacles,
>> load map1
qui définit une variable map (une matrice 100 × 100) dans le workspace de Matlab.
Pour utiliser le planificateur de la Robotics Toolbox, nous créons d'abord un objet "PRM":
>> prm = PRM(map);
Ensuite, on génére la roadmap:
>> prm.plan()
Il est à noter que nous n'avons pas passé l'argument qb au planificateur car la roadmap est indépendante de la position à atteindre qb. En fait, la détermination d'un chemin sans collisions est un processus qui comporte deux étapes: 1) l'étape de planification et 2) l'étape de query. L'étape 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 taper:
>> prm =
PRM navigation class:
occupancy grid: 100x100 graph size: 100 dist thresh: 30 2 dimensions 100 vertices 811 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.plot()
Les sommets sont les points générés de façon aléatoire et les arêtes sont les chemins sans collisions entre eux. Seulement les chemins d'une longueur inférieure à 30 cellules sont sélectionnés: cela correspond au paramètre dist thresh (seuil sur la distance) de la classe PRM. Chaque arête du graphe a un coût associé que correspond à la distance entre ses deux sommets.
Université de Picardie Jules Verne 2017-2018 M2 EEAII ViRob Localisation et navigation de robots
3/4 Fabio Morbidi
On remarque ici qu'il y a deux sommets (gris) sur le côté gauche de l'environnement qui sont déconnectés du corps principal de la roadmap.
Dans la phase de query, on trouve le chemin entre qs et qb. À partir de qs, il s'agit simplement de se déplacer vers le sommet le plus proche dans le graphe, de suivre la roadmap (comme un train qui roule sur une ligne de chemin de fer) et de quitter la roadmap lorsque le robot a atteint le sommet le plus proche de qb. La commande:
>> prm.query(qs, qb);
suivie par prm.plot(),montre le parcour du robot sur la roadmap (en noir). Se déplacer sur la roadmap consiste à se diriger vers le sommet 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 sommet du graphe le plus proche de qb, et en partant de là on passe directement à qb.
Un avantage de la PRM est qu'une fois que la roadmap a été crée dans l'étape de planification, qs et qb peuvent être modifiés à un coût très faible: en fait, il faut juste relancer l'étape de query. Toutefois, le chemin trouvé n'est pas optimal et la distance parcourue par le robot:
>> p = prm.query(qs, qb);
>> numrows(p) ans = 28
(28 sommets) est plus grande que la distance optimale.
Il faut alors envisager des compromis entre l'optimalité du chemin du robot et la complexité de calcul.
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é chaque fois que le planificateur est
lancé, ce qu'entraîne des chemins différents, ayant une longueur différente.
Par exemple, si on relance le planificateur une deuxième fois:
>> prm.plan()
nous obtiendrons un graphe avec des sommets situés dans des positions différentes et un nombre de sommets 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 sommet), 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 query signalera une erreur. La seule solution, alors, est de relancer à nouveau prm.plan().
3. Les passages longs et étroits entre les obstacles ont peu de chances d'être exploités, car la probabilité de tirer aléatoirement (avec un densité de probabilité uniforme) des points candidats situés dans ces régions est très faible. Dans le premier graphe, le planificateur a trouvé un chemin autour de l'obstacle dans le sens inverse des aiguilles d'une montre. Ce parcours est plus long que le chemin optimal qui traverse le couloir vertical situé sur le côté gauche.
Université de Picardie Jules Verne 2017-2018 M2 EEAII ViRob Localisation et navigation de robots
4/4 Fabio Morbidi
b) Rapidly-exploring Random Tree (RRT)
Le Rapidly-exploring Random Tree ou RRT est un planificateur probabiliste comme PRM.
L'avantage principal est qu'il permet de prendre en compte le modèle cinématique ou dynamique d'un robot mobile. De cette manière RRT ne se limite pas aux véhicules omni- directionnels comme PRM.
Comme dans le point a) ci-dessus, on réinitialise le générateur de nombres pseudo-aléatoires à un état connu, à savoir:
>> randinit
On définit ensuite un robot car-like ou tricycle ("Bicycle" dans la Robotics Toolbox) avec une limitation sur l'angle de braquage α de ±1.2 rad (par défaut l'angle est limité à ±0.5 rad):
>> veh = Bicycle('steermax', 1.2)
Nous définissons ensuite la pose de départ ps ∈ 3 du robot (c'est-à-dire, la position le long des axes x et y, et l'orientation θ dans le repère monde) et la pose à atteindre pb ∈ 3:
>> ps = [0 0 0]';
>> pb = [0 2 0]';
On considère ici le problème de faire déplacer latéralement un robot car-like (en d'autres termes, nous sommes intéressés à une "manœuvre de parking"). Plus précisément, nous cherchons le chemin qui permet de faire déplacer le robot latéralement de 2 m avec la même orientation initiale et finale (θ = 0 rad).
On génére un objet "RRT" pour un environnement sans obstacles (les obstacles ne sont pas supportés par la Robotics Toolbox pour le moment), d'une façon similaire à la PRM:
>> rrt = RRT(veh, 'goal', pb, 'npoints', 700, 'range', 5)
où on a specifié un arbre aléatoire T avec 700 sommets (par défaut, ils sont 500) et limité la taille de l'environnement à ±5 m. Nous pouvons maintenant créer l'arbre de navigation via:
>> rrt.plan()
et utiliser,
>> rrt.query(ps, pb)
pour visualiser le résultat. Nous avons trouvé un chemin lisse et continu, qui le robot car-like peut emprunter sans problèmes.
Nous remarquons que le mouvement du robot ne se termine pas exactement à pb, mais en correspondance du sommet de l'arbre T le plus proche de pb. Pour éviter ce problème, on peut générer un arbre plus dense (par exemple avec 1500 sommets au lieu de 700) ou on peut agir sur l'angle de braquage du robot.
Il est enfin à noter que comme pour PRM, si nous relançons le planificateur rrt.plan() une deuxième fois, nous obtiendrons un chemin différent pour le robot (qui dépend de l'arbre aléatoire généré sous-jacent).
