Chapitre 2 : Navigation

F. Morbidi 2014/2015

Localisation et navigation de robots

UFR des Sciences, Département EEA

M2 EEAII, parcours ViRob

2

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

Partie 4 : Evitement d’obstacles

3

Evitement d’obstacles

4

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 algorithmes de bas-niveau

B

obstacle

Evitement d’obstacles • 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:

1.  Véhicule de Braitenberg (modèle réactif, déjà vu) 2.  Champs de potentiel 3.  Vector field histogram 4.  Fenêtre dynamique 5.  Graphe de Voronoï

• Besoin de perceptions précises (ex. laser) • Avec sonars ou autres: une représentation locale

de l’environnement est nécessaire

5

Robot mobile

Trajectoire du robot désirée

Chemin autour de l'obstacle

Obstacle

Partie 4 : Evitement d’obstacles 1. Champs de potentiel

6

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

7

“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

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

8

“Robotics Modeling, Planning and Control", B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Springer 2009, Sect. 12.6

Champs de potentiel

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

champ potentiel résultant

environnement

9

obstacle Simple exemple

+ +

Champs de potentiel

10

F(q) = −∇U(q)

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

où

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

U(q) : fonction potentiel de l’environnement

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

q

qb

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

Un peu de mathématiques …

•  Deux types de potentiel U :

F

robot

q

Champs de potentiel

11

1) Fonction quadratique de la distance du but

Potentiels attractifs

Ua(q) =1

2kaqb − q2, ka > 0

qb

2) Fonction linéaire de la distance du but qb

Convergence linéaire à zéro lorsque tend à ; tend à croître indéfiniment lorsque la norme de l’erreur augmente

qbq

Ua(q) = kaqb − q, ka > 0

Fa(q) = −∇Ua(q) = kaqb − q

qb − qModule de la force constant; non définie en qb

Bon compromis: utiliser la 2ième fonction lorsque le robot est éloigné du but et la 1ère près de . Pour une force continue, faire la transition lorsque qb qb − q = 1

Fa

FaFa(q) = −∇Ua(q) = ka (qb − q)

Champs de potentiel

12

1) Fonction quadratique (paraboloïde)

Potentiels attractifs

2) Fonction linéaire (cône)

qbqb

y y

x x

ka = 1Dans les deux images, on a utilisé

UaUa

Champs de potentiel

13

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

Potentiels repulsifs

i ∈ 1, . . . , N

Ur,i(q) =

kr,iγ

1

ρ(q, qo,i)− 1

ρo,i

γ

si ρ(q, qo,i) ≤ ρ0,i

0 si ρ(q, qo,i) > ρ0,ioù

kr,i > 0

γ ∈ 2, 3, . . .

ρ(q, qo,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, qo,i)

Champs de potentiel

14

Potentiels repulsifs

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

Contours équipotentiels du potentiel répulsif Ur pour , ka = 1 γ = 2

ρo,i

γ γ = 2

obstacle i

obstacle j

obstacle

Champs de potentiel

15

Potentiels repulsifs

Force repulsive résultant du potentiel : Ur,i(q)

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

Ur(q) =N

i=1

Uri(q)

Fr,i(q) = −∇Ur,i(q) =

kr,iρ2(q, qo,i)

1

ρ(q, qo,i)− 1

ρo,i

γ−1

∇ ρ(q, qo,i) si ρ(q, qo,i) ≤ ρ0,i

0 si ρ(q, qo,i) > ρ0,i

Champs de potentiel

16

Potentiel total = potentiel attractif + potentiels repulsifs

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

Ft(q) = −∇Ut(q) = Fa(q) +N

i=1

Fr,i(q)

potentiel attractif potentiels repulsifs potentiel total

qb

Ut(q) = Ua(q) + Ur(q)

Champs de potentiel Obstacle

But

17

Robot

qb

Contours équipotentiels

Exemple

Champs de potentiel

18

Pour atteindre le but , on peut utiliser l’algorithme du gradient. À partir de:

qb

q = −∇Ut(q)

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

qk+1 = qk + αkFt(qk)

Ft(qk), αk > 0

Problème: convergence vers tous les points critiques où •  Il faut vérifier la deuxième dérivée du potentiel: la matrice Hessienne doit être définie positive pour un minimum local

∇Ut(q) = 0

, k ∈ 0, 1, . . .

q0

Champs de potentiel

19

qb

Contours équipotentiels Champ de force résultant

Obstacle

Minimum local « zone d'ombre »

Minimum global (but)

Exemple: obstacle rectangulaire

qb

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 1er problème:   Lorsque dans un minimum local, déclencher un

comportement différent:   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)

20

* Une fonction harmonique est une fonction deux fois continûment dérivable qui satisfait l'équation de Laplace sur , c.-à-d. ∇2f = 0

f : Ω ⊂ Rn → RΩ

Champs de potentiel

• Avantages: 1) Capacité de générer « en temps réel »

des trajectoires pour éviter les obstacles 2) Facilité d’implementation

• Formalisme du schéma moteur  Action sous forme de potentiel  Dépend des perceptions

• Utilisation  Architecture hybride  Bas-niveau

21

Partie 4 : Evitement d’obstacles 2. Vector field histogram

22

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é

  Haute si souvent perçue contenant un obstacle

  Faible sinon 2. Construction d’un histogramme

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

23

p1

p2

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

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:

24

p(occi | 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(occi | s)

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

25

Résultat

p(obstacle | scan) =

j

p(mesure j | distObstacle)

dist. dist. dist. dist.

Construction d’une GO

   Erreur gaussienne pour l’écart entre :  La direction de la cellule  La direction du capteur

 Objectif  Accumuler les T mesures (au cours du temps)  Estimer la probabilité de la cellule i d’être occupée

26

p(occi | s)

p(occTi ) = p(occi | s1, . . . , sT )

s1, . . . , sT

Construction d’une GO • Théorème de Bayes pour extraire la probabilité en

fonction de la dernière mesure. Rappel que:

• Dans notre cas:

27

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 carte (ce qui est faux en pratique et produit des limitations de la méthode)

p(occTi ) =p(sT | occi, s1, . . . , sT−1) p(occ

T−1i )

p(sT | s1, . . . , sT−1)

p(occTi ) =p(sT | occi) p(occT−1

i )

p(sT | s1, . . . , sT−1)

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

Construction d’une GO

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

 Mais la probabilité d’«inoccupation»

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

28

p(occi | sT )

p(occTi ) =p(occi | sT ) p(sT )

p(occi)

p(occT−1i )

p(sT | s1, . . . , sT−1)

p(occTi ) =p(occi | sT ) p(sT )

p(occi)

p(occT−1i )

p(sT | s1, . . . , sT−1)

p(occi) = 1− p(occi)

Construction d’une GO  Le ratio des deux probabilités nous donne

 Sachant que , tout est connu:

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

29

p(occTi )

p(occTi )=

p(occi | sT )p(occi | sT )

p(occT−1i )

p(occT−1i )

p(occi)

p(occi)

p(occi) = 1− p(occi)

p(occTi )

1− p(occTi )=

p(occi | sT )1− p(occi | sT )

1− p(occT−1i )

p(occT−1i )

1− p(occi)

p(occi)

p(occTi )

Ti = log

p(occTi )

1− p(occTi )

: logarithme naturel (de base e) log(·)

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 à 1/2   permet de calculer la probabilité d’occupation :

30

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

Ti

log(xb) = b log(x)

Ti = log

p(occi | sT )

1− p(occi | sT )

− T−1

i + log

1− p(occi)

p(occi)

p(occi)

Ti

p(occTi ) = 1− 1

exp(Ti )

p(occTi )

Construction d’une GO  Autres possibilités de mise à jour des GO

1. Calcul 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)

31

p(occTi )

p(occTi ) =nboccTi

nbvideTi + nboccTi

Construction d’une GO  Autres possibilités de mise à jour des GO

2. Histogramic in-motion mapping (HIMM)

32

p(occTi ) = p(occT−1i ) + λκ

"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élicats des paramètres ( ) pour être adaptée à un robot κ

λ =

−1

1

si la cellule i presénte un obstacle

sinon

Valeur d’incrémentation

(paramètre positif)

Histogramme d’occupation

33

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é

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

+1

+1

Robot

•  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

34

Histogramme d’occupation

11 secteurs

Direction sans collisions

Seuil

Somme des cellules

2

3

3 10 4 0 10

1

Robot

4

5

9

8

7 6

5 6 8 9

11

1

Secteur

35

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+

Trajectoire du robot cinématiquement

admissible

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