Plan du chapitre

(1)

Localisation et navigation de robots

UPJV, Département EEA M2 EEAII, parcours ViRob

Fabio MORBIDI

Laboratoire MIS

Équipe Perception et Robotique E-mail: fabio.morbidi@u-picardie.fr Année Universitaire 2015/2016

Mercredi 10h00-12h30 et jeudi 9h00-12h30 salle TP202

(2)

1 Plan du chapitre

Stratégies de navigation Architectures de contrôle

Navigation vers un but Evitement d’obstacles

Partie 2

Partie 3 Partie 1

Partie 4

(3)

Partie 4 : Evitement d’obstacles

2

(4)

Evitement d’obstacles: introduction

3 Obstacle

• Statique ou dynamique

Exemples: poteau, mur, marche, humains, autres robots

x

y

A

Objectif: déterminer une trajectoire

de A à B qui permet au robot

d'éviter les collisions avec les obstacles Evitement d’obstacles: géré

normalement par des contrôleurs de bas-niveau

B

obstacle

(5)

• Comportement de base indispensable pour un fonctionnement dans un environnement dynamique

• Il faut gérer les écarts modèle interne/monde réel

• Méthodes (pour obstacles statiques):

0. Véhicule de Braitenberg (approche réactif, pas de carte de l’environ.) 1. Champs de potentiel

2. Vector field histogram 3. Fenêtre dynamique 4. Graphe de Voronoï

5. Planification probabiliste (PRM, RRT)

• Besoin de perceptions précises (par ex. télémètres laser)

• Avec sonars ou autres: une représentation locale de l’environnement peut être nécessaire

4

Robot mobile

Trajectoire désirée du robot Chemin autour

de l'obstacle

Obstacle

Evitement d’obstacles: introduction

(6)

Partie 4 : Evitement d’obstacles

1. Champs de potentiel

5

(7)

Champs de potentiel

▫ Robot: vu comme une particule

▫ Déplacement suivant les lignes de courant d’un potentiel obtenu par la perception de l’environnement

6 “Real-time obstacle avoidance for manipulators and mobile robots", O. Khatib, Int. Journal of Robotics Research, vol. 5, n. 1, pp. 90-98, 1986

Potentiels primitifs

(8)

Champs de potentiel

• Potentiel: différents objectifs

▫ Évitement d’obstacles

▫ Déplacement dans une direction préférée (vers le but)

• Comment le calculer ?

▫ Sommation de potentiels primitifs

• Particularités des potentiels primitifs

▫ Étendue spatiale limitée ou non

▫ Intensité: fonction de la distance ou non

7 “Robotics Modeling, Planning and Control", B. Siciliano, L. Sciavicco,

L. Villani, G. Oriolo, Springer 2009, Sect. 12.6

(9)

Champs de potentiel

déplacement suivant le couloir centrage évitement d’obstacle ponctuel

champ potentiel résultant environnement

8

obstacle

Simple exemple

+ +

(10)

Champs de potentiel

9 F(q) = r U (q)

En d'autres termes, la force F agissant sur le robot dans la position est égale au gradient négatif de la fonction potentielle U

où

F(q) force agissant sur le robot dans la position q = [x, y]

^T

fonction potentiel de l’environnement

1) Potentiel attractif : pour guider le robot vers le but

q

_b

2) Potentiel repulsif : pour éviter la collision avec les obstacles

Formulation mathématique

• Deux types de potentiel U :

robot

{W} F(q) q 2 R²

U (q) : R

²

! R

(11)

Champs de potentiel

10 1) Fonction quadratique de la distance du but :

Potentiels attractifs

U

_a

(q) = 1

2 k

_a

k q

_b

q k

²

, k

_a

> 0 q

_b

2) Fonction linéaire de la distance du but : q

_b

Convergence linéaire à

zéro lorsque tend vers ; tend à augmenter

indéfiniment lorsque la

norme de l’erreur augmente

q

b

q

U

_a

(q) = k

_a

k q

_b

q k , k

_a

> 0

F

_a

(q) = r U

_a

(q) = k

_a

q

_b

q k q

_b

q k

Module de la force constant; n’est pas définie en

q

_b

Bon compromis: utiliser la 2

^e

fonction lorsque le robot est éloigné du but et la 1

^e

près de . Pour avoir une force continue, passer d’une à l’autre lorsque q

_b

k q

_b

q k = 1

F

_a

F

_a

F

_a

(q) = r U

_a

(q) = k

_a

(q

_b

q)

(12)

Champs de potentiel

11 1) Fonction 1 (paraboloïde)

Potentiels attractifs

2) Fonction 2 (cône)

q

_b

q

_b

y y

x x

k

_a

= 1

Dans les deux images, on a choisi

U

_a

U

_a

(13)

Champs de potentiel

12 Pour un obstacle convex on peut définir le potentiel:

Potentiels repulsifs

i 2 { 1, . . . , N }

U

_r,i

(q) = 8 >

<

> :

k

_r,i

✓

1 ⇢(q, q

_o,i

)

1 ⇢

_o,i

◆ si ⇢(q, q

_o,i

)  ⇢

_0,i

0 si ⇢(q, q

_o,i

) > ⇢

_0,i

étant

k

_r,i

> 0

2 { 2, 3, . . . }

⇢(q, q

_o,i

)

⇢

_o,i

: distance minimale entre le robot et les points de l’obstacle i : rayon d'influence de l'obstacle i

robot

obstacle i

⇢(q, q_o,i)

q

(14)

Champs de potentiel

13 Potentiels repulsifs

Si est grand (typiquement, ), la “pente” du potentiel est forte

Contours équipotentiels du potentiel répulsif U

_r,i

pour , = 2

⇢

_o,i

= 2 obstacle i

obstacle j

obstacle

k

_r

= 1

(15)

Champs de potentiel

14 Potentiels repulsifs

Force repulsive résultant du potentiel : U

r,i

(q)

Enfin, on peut définir le potentiel répulsif global (pour les N obstacles), comme:

F_r,i(q) = rU_r,i(q) = 8>

><

>>

:

k_r,i

⇢²(q, q_o,i)

✓ 1

⇢(q, q_o,i)

1

⇢_o,i

◆ 1

r⇢(q, q_o,i) si ⇢(q, q_o,i)  ⇢_0,i

0 si ⇢(q, q_o,i) > ⇢_0,i

U

_r

(q) =

X

N i=1

U

_r,i

(q)

(16)

Champs de potentiel

15 Potentiel total = potentiel attractif + potentiels repulsifs

Ceci se traduit par la force totale agissant sur le robot:

F

_t

(q) = r U

_t

(q) = F

_a

(q) +

X

N

i=1

F

_r,i

(q)

potentiel attractif potentiels repulsifs potentiel total

q

_b

U

_t

(q) = U

_a

(q) + U

_r

(q)

(17)

Champs de potentiel

Obstacle

But

16 Robot

q

_b

Contours équipotentiels

Exemple

(18)

Champs de potentiel

17 Pour atteindre le but , on peut utiliser l’algorithme du gradient.

À partir de: q

_b

q ˙ = r U _t (q)

si est la position initiale du robot, on peut mettre à jour sa position de la manière suivante:

q

_k+1

= q

_k

+ ↵

_k

F

_t

(q

_k

)

k F

_t

(q

_k

) k , ↵

_k

> 0

Problème: convergence vers tous les points critiques où Il faut vérifier la dérivée seconde

du potentiel: la matrice Hessienne doit être définie positive en

pour avoir un minimum local

r U

_t

(q) = 0 , k 2 { 0, 1, . . . }

q

₀

q

^⇤

q

^⇤

2 4

@² Ut

@x²

@² Ut

@x@y

@² Ut

@y @x

@² Ut

@y²

3 5

q=q^⇤

0

(19)

Champs de potentiel

18 Contours équipotentiels Champ de force résultant Obstacle

Minimum local

« zone d'ombre » Minimum

global (but)

Exemple: obstacle rectangulaire

q _b q _b

(20)

Champs de potentiel

• Problèmes

1. Minima locaux du potentiel dans certaines configurations 2. Pas de «décision» de la direction à prendre pour le robot

• Solutions au 1

^er

problème:

▫ Lorsque dans un minimum local, déclencher un comportement différent:

! Par ex. déplacement aléatoire, suivi de murs

▫ « Mondes sphériques » : tous les obstacles ont une

forme sphérique. Le potentiel total a points selles isolés (où le gradient est zéro), mais pas des minima locaux

▫ Fonctions de navigation [Rimon & Koditschek, TRA92]

ou potentiel imposé en fonction harmonique*

! Les fonctions harmoniques garantissent l’absence de minima locaux, mais complexité élevée (seulement en simulation)

19 * Une fonction harmonique est une fonction deux fois

continûment dérivable qui satisfait l'équation de Laplace sur , c.-à.-d. r

²

f = 0 f : ⌦ ⇢ R

ⁿ

! R

⌦

(21)

Champs de potentiel

• Avantages:

1) Capacité de générer « en temps réel » des trajectoires pour éviter les obstacles

2) Facilité de mise en œuvre

• Formalisme du schéma moteur

▫ Action sous forme de potentiel

▫ Dépend des perceptions

• Utilisation

▫ Bas-niveau dans une architecture hybride

20

(22)

Partie 4 : Evitement d’obstacles

2. Vector field histogram (VFH)

21

(23)

Vector field histogram

• Historiquement inventé pour les sonars (capteurs à ultra-sons)

1. Grille d’occupation (GO) locale

▫ Représentation de l’environnement

▫ Discrétisation de l’espace en cellules régulières

▫ Chaque cellule contient une probabilité p

_i

! Haute si souvent perçue contenant un obstacle

! Faible sinon

22

p₁

p₂

GO Secteur

angulaire

“The vector field histogram - fast obstacle avoidance for mobile robots”, Y. Koren, J. Borenstein, IEEE Trans. Robot. Autom., vol. 7, n. 3, pp. 278-288, 1991

Seuil

Secteurs angulaires Robot

θ

2. Histogramme d’occupation

Représente l’occupation de l’espace autour du robot

(24)

Vector field histogram

1. Construction de la grille d’occupation (GO)

▫ Estimation de la position du robot (cf. chapitre 1, localisation par odométrie)

▫ Perception

▫ Mise à jour des cellules de la GO

! Modèle probabiliste:

23 p(occ

_i

| s)

Pour une perception s donnée, nous fournit la

probabilité d’occupation de la cellule i dans le champ de vision du capteur du robot en fonction de la valeur renvoyée par le capteur

p(occ

_i

| s)

(25)

Construction d’une GO

! Modèle probabiliste du télémètre à ultra-sons

! Probabilité de mesure en fonction de la distance de l’obstacle

Π

Perception obstacle

Perception obstacle imprévu

Non perception:

mesure à la distance maximale

24 Résultat

dist. dist. dist. dist.

pdf pdf pdf pdf

p(obstacle | balayage) =

Y

T j = 1

p(s

_j

| distObstacle)

(26)

Construction d’une GO

!

! Erreur gaussienne pour l’écart entre :

▫ La direction de la cellule

▫ La direction du capteur

! Objectif

! Accumuler les T mesures

! Estimer la probabilité de la cellule i d’être occupée:

25 p(occ

_i

| s)

p(occ ^T _i ) = p(occ _i | s ₁ , . . . , s _T )

s

₁

, . . . , s

_T

(27)

Construction d’une GO

• Théorème de Bayes pour extraire la probabilité en fonction de la dernière mesure s

_T

. Rappel que:

• Dans notre cas:

26 p(x | y) = p(y | x) p(x) p(y )

« Monde statique » : toutes les mesures sont conditionnellement indépendantes si on connaît la valeur d’une cellule de la GO (ce qui est faux en pratique et produit des limitations de la méthode)

p(occ

^T_i

) = p(s

_T

| occ

_i

, s

₁

, . . . , s

_T ₁

) p(occ

^T_i ¹

) p(s

_T

| s

₁

, . . . , s

_T ₁

)

p(occ

^T_i

) = p(s

_T

| occ

_i

) p(occ

^T_i ¹

) p(s

_T

| s

₁

, . . . , s

_T ₁

)

• Avec une hypothèse de « monde statique », on simplifie:

(28)

Construction d’une GO

! Notre modèle de capteur nous donne , alors nous le faisons apparaître (à nouveau théor. Bayes):

! Mais la probabilité d’«inoccupation»

! La probabilité que la cellule i soit vide est donc:

27 p(occ

_i

| s

_T

)

p(occ

^T_i

) = p(occ

_i

| s

_T

) p(s

_T

) p(occ

_i

)

p(occ

^T_i ¹

)

p(s

_T

| s

₁

, . . . , s

_T ₁

)

p(occ

^T_i

) = p(occ

_i

| s

_T

) p(s

_T

) p(occ

_i

)

p(occ

^T_i ¹

)

p(s

_T

| s

₁

, . . . , s

_T ₁

)

p(occ

_i

) = 1 p(occ

_i

)

(29)

Construction d’une GO

! Le rapport des deux probabilités nous donne:

! En sachant que , tout est connu:

! Nous pourrions exprimer , mais il est plus simple déterminer la quantité:

28 p(occ

^T_i

)

p(occ

^T_i

) = p(occ

_i

| s

_T

) p(occ

_i

| s

_T

)

p(occ

^T_i ¹

) p(occ

^T_i ¹

)

p(occ

_i

) p(occ

_i

)

p(occ

_i

) = 1 p(occ

_i

)

p(occ

^T_i

)

1 p(occ

^T_i

) = p(occ

_i

| s

_T

) 1 p(occ

_i

| s

_T

)

1 p(occ

^T_i ¹

) p(occ

^T_i ¹

)

1 p(occ

_i

) p(occ

_i

) p(occ

^T_i

)

`

^T_i

= log

✓ p(occ

^T_i

) 1 p(occ

^T_i

)

◆ : logarithme

naturel (de base e)

log( · )

(30)

Construction d’une GO

! Ce qui se calcule simplement par (rappel:

et )

! Mise à jour incrémentale de . On utilise:

! Les valeurs précédentes

! Le modèle du capteur

! généralement initialisée à ½ (l’initialisation intègre l’a priori sur le fait que l’environnement contienne plus ou moins d’obstacles)

! permet de calculer la probabilité d’occupation :

29 log(x · y ) = log(x) + log(y )

`

^T_i

log(x

^b

) = b log(x)

`

^T_i

= log

✓ p(occ

_i

| s

_T

) 1 p(occ

_i

| s

_T

)

◆ `

^T_i ¹

+ log

✓ 1 p(occ

_i

) p(occ

_i

)

◆ p(occ

_i

)

`

^T_i

p(occ

^T_i

) = 1 1

exp(`

^T_i

)

p(occ

^T_i

)

(31)

Construction d’une GO

▫ Autres possibilités de mise à jour des GO

1. Calcul de en fonction du nombre de

perceptions de la cellule i (« approche fréquentiste vs. bayésianne»)

Nombre de fois que un obstacle a été détecté

dans la cellule i (pour les T mesures) Nombre de fois

que la cellule i a été détectée vide (pour les T mesures)

30 p(occ

^T_i

)

p(occ

^T_i

) = nbocc

^T_i

nbvide

^T_i

+ nbocc

^T_i

(32)

Construction d’une GO

▫ Autres possibilités de mise à jour des GO

2. Histogramic in-motion mapping (HIMM)

31 p(occ

^T_i

) = p(occ

^T_i ¹

) + 

"Histogramic in-motion Mapping for Mobile Robot Obstacle Avoidance", J. Borenstein, Y. Koren, IEEE Journal of Robotics and Automation, vol. 7, n. 4, pp. 535-539, 1991

HIMM est très simple, mais:

• Pas de convergence quand le nombre de perceptions tend vers l’infini

• Sensible au bruit

• Réglage délicat des paramètres ( ) pour être adapté à un robot 

=

( 1 1

si la cellule i presénte un obstacle sinon

Valeur

d’incrémentation (paramètre positif)

(33)

2. Histogramme d’occupation

32 Obstacle 1

Obstacle 2 Robot

•

La GO est construite dans le référentiel du robot

•

Un compteur est incrémenté pour chaque cellule appartenant au secteur angulaire dans lequel un obstacle a été détecté (ex. car probabilité > 0.5)

• Les valeurs sont déplacées d’une cellule à l’autre en fonction des déplacements du robot

+1

Robot

Secteur angulaire

(34)

•

Construction d’un histogramme polaire des obstacles à partir des valeurs des cellules (une barre pour chaque secteur)

•

On utilise l’histogramme pour déterminer la direction de déplacement du robot

•

Seuillage pour éliminer une partie du bruit

33 2. Histogramme d’occupation

12 secteurs (ouverture angulaire de 30^o)

Direction sans collisions Seuil

Somme des compteurs des cellules dans les secteurs

2 3

3 4 10

0 10

1

Robot

4 5

9 8

7

6

5 6 8 9

11

1

Secteur

1 2 11

14 GO

(35)

34 Histogramme d’occupation

“VFH+: Reliable obstacle avoidance for fast mobile robots”, I. Ulrich, J. Borenstein, in Proc. Int. Conf. on Robotics and

Automation, vol. 2, pp. 1572-1577, 1998

VFH+

Histogramme “masqué”

▫ Après seuillage

! Ensemble de directions possibles

! Choix selon un critère:

! Exemple: direction la plus proche de celle du but

! On peut prendre en considération le modèle cinématique du robot (VFH+)

▫ Avantage du VFH

! Très rapide

Robot unicycle

(36)