Upload
buianh
View
240
Download
0
Embed Size (px)
Citation preview
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