Localisation et navigation

de robots
UPJV, Département EEA

M2 3EA, EC32, parcours RoVA

Année Universitaire 2018/2019

Fabio MORBIDI
Laboratoire MIS
Équipe Perception Robotique
E-mail: [email protected]

Jeudi 13h30-17h30
Salle CURI 305 ou TP204
 2

Évolution du cours au fil du temps

Localisation et Localisation et
navigation de navigation de
robots (ViRob) robots (RoVA)

Localisation, navigation
et asservissement

2011-2013 • Vision avancée

(ViRob)

• Asservissement
visuel (Option.)

2014-2015 2015-2019
 3

Deux problèmes cruciaux …

Où suis-je?

?
Localisation !

Comment aller de A à B ?
B
?
Navigation !
A
et aussi …
• Comment détecter B ?
• Comment éviter les obstacles ?
 4

Plan du cours

Chapitre 1: Localisation

1.1 Introduction et défis

1.2 Odométrie

1.3 Localisation par filtre de Kalman

Chapitre 2: Navigation

2.1 Stratégies de navigation

2.2 Architectures de contrôle

2.3 Navigation vers un but

2.4 Evitement d’obstacles

 5

Bibliographie

• Introduction to Autonomous Mobile Robots

R. Siegwart, I.R. Nourbakhsh, D. Scaramuzza, MIT press,
2e éd., 2011 (chapitres 4, 5 et 6)

• Principles of Robot Motion: Theory, Algorithms,

and Implementations
H. Choset, K.M. Lynch, S. Hutchinson, G.A. Kantor,
W. Burgard, L.E. Kavraki, S. Thrun, MIT press, 2005

• Robotics: Modelling, Planning and Control

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Springer,
1er éd., 2009 (chapitres 4, 12)

• Estimation with Applications to Tracking and Navigation:

Theory Algorithms and Software
Y. Bar-Shalom, X.R. Li, T. Kirubarajan, John Wiley & Sons, 2001
(chapitres 5, 6 et 12)

Matériel de cours: http://home.mis.u-picardie.fr/~fabio/Teaching_LNR18-19.html

 6

Connaissances préalables
On suppose que vous avez une bonne connaissance de:

• Algèbre linéaire (opérations sur matrices, valeurs et vecteurs propres, etc.)

• Calcul différentiel (gradient, matrice jacobienne, etc.)

• Théorie des probabilités et de l’estimation, variables et processus aléatoires

• Notions de base de robotique mobile (cours PARM)

Structure du cours: CM (2 chapitres), 3 TD

Contrôle: 1 DS (exercices, questions de théorie), 3 TP

 ✓ ◆
1 TP1 + TP2 + TP3
Note finale = DS +
2 3
 Localisation et navigation
de robots
UPJV, Département EEA

M2 3EA, EC32, parcours RoVA

Chapitre 1 : Localisation

F. Morbidi 2018/2019
 8

Plan du chapitre
• Introduction et défis Partie 1

• Odométrie Partie 2

• Localisation par filtre de Kalman Partie 3

• Autres techniques de localisation Partie 4

 9

Partie 1 : Introduction et défis

 10

Introduction
• Localiser: “Déterminer la place de quelque chose,
l'endroit où se situe quelque chose” (Dict. La Rousse)

Vous êtes ici

 11

Introduction
Hypothèse: une carte de l’environnement est disponible

Facile

?
Difficile
 12

Introduction

• Robots manipulateurs
▫ Robot de type série:
où est l’effecteur ?

! Robot « fixe »

! A partir de la mesure des variables articulaires on peut

déduire la pose de l’effecteur (modèle géométrique directe)
! Connaissance des longueurs des segments nécessaire
! Plus un problème de « calibrage » que de localisation

(cf. Cours M1 de “Robotique Industrielle”)

 13

Introduction

• Robots manipulateurs
▫ Robot parallèle:
où est l’effecteur ?

! Grande dynamique
! Modélisation géométrique: plus complexe
que pour les robots de type série ...
... mais volume de travail limité
et facilement identifiable

(cf. Cours M1 de “Robotique Industrielle”)

 14

Introduction

• Robots mobiles
▫ Milieu ouvert
▫ Pas de lien fixe avec
l’environnement
▫ Pas de mesures directes
de la pose (position et
orientation)
▫ Zone de mouvement
« sans limite »

Vidéo ETHZ
 15

Introduction
• Navigation
▫ À grande échelle: on a besoin de la localisation (compétence
importante et critique d’un robot mobile)
▫ En particulier, pour le succès de la navigation nous
avons besoin de:
! Perception: le robot doit interpréter les mesures des
capteurs pour extraire des informations pertinentes

! Localisation: le robot doit déterminer sa position

dans l’environnement

! Décision: le robot doit décider de comment agir

pour atteindre son but

! Commande du mouvement: le robot doit actionner

ses moteurs pour réaliser la trajectoire désirée
 16

Introduction

Perception Décision

Environnement

Action

Paradigme “See-Think-Act”
 17

Introduction
Exemple …

Perception: Il y a un mur en face !

Décision: Tourner à droite

Action: Règler la vitesse de la roue gauche et de la roue droite, par ex.

!G (t) = 0.1 rad/s, !D (t) = 0.5 rad/s, t 2 [ta , tb ]

 18

Introduction
• Localisation: il a reçu la plus grande attention de la part
des chercheurs au cours des dernières décennies*

*Springer Handbook of Robotics, B. Siciliano, O. Khatib (Éditeurs),

2e éd., 2016, (2227 pages)
 19

Où intervient la localisation ?
Connaissance, Mission,
base de données commandes

Localisation, « position » Cognition,

construction de carte carte globale planif. de trajectoire

modèle d’environnement,
trajectoire
carte locale

contrôle de mouvement
Extraction
Réalisation
d’information et
de chemin
perception

interprétation
commande des
données brutes
actionneurs
Environnement,
Capteurs Action
monde réel
 20

• Schéma général de localisation d’un robot mobile

position mise à jour de

position
(estimation ?)
prédiction de
encodeurs position observations
(odométrie) appariées
OK
position prédite
base de appariement
données, carte
données brutes des capteurs
ou primitives extraites

perception
observation
 21

Les défis de la localisation

▫ Le bruit
! de perception (capteurs)
! d’actionnement (actionneurs)

▫ L’aliasing perceptuel
! Il définit le fait que deux lieux distincts
peuvent avoir la même apparence
 22

Les défis de la localisation

GPS !

• Intuitivement
▫ « GPS précis » : solution ultime pour la localisation
! Il informe le robot de sa position exacte
! Intérieur et extérieur
! Réponse immédiate à la question « où suis-je ? »
▫ Mais … un tel capteur n’existe pas !
! GPS précis à quelques mètres (DGPS plus précis, mais cher)
! GPS est inadapté pour des environnements à taille humaine
et encore moins pour les micro- et les nano-robots
! GPS ne fonctionne pas à l’intérieur ou dans un
environnement encombré (« canyons urbains »)
GPS = Global Positioning System (constellation de 24 satellites orbitant à 20000 km)
 23

Les défis de la localisation

• Au delà des limitations du GPS
▫ Localisation est plus que la connaissance
de position absolue

Exemple: robot interactif

! Éventuellement besoin d’une position absolue
! Position relative aux humains aussi importante
! La tâche de localisation doit inclure:
! Perception (multi-capteurs) pour identifier les humains
! Calcul de sa position relative aux humains
 24

Les défis de la localisation

! Atteindre un lieu particulier demande plus
qu’une « simple » localisation
! Étape de décision
! Sélection de stratégie pour atteindre le but
! Besoin d’acquérir ou de créer un modèle de son
environnement: une carte (cf. SLAM)
! Aide le robot dans sa planification de trajectoire

• En résumé, la localisation c’est plutôt:

▫ Construire une carte de l’environnement
▫ Déterminer la position du robot relativement à cette carte
 25

Les défis de la localisation

• Éléments cruciaux de cette localisation:

▫ Les capteurs du robot
▫ Les actionneurs du robot

• Les capteurs et les actionneurs d’un robot

sont inexactes et incomplets
! Une mesure parfaite n’existe pas
! Tout l’environnement ne peut être mesuré d’un coup
 26

Le bruit de perception
• Perception
▫ Capteurs: fondamentaux
▫ Leur degré de discrimination de
l’environnement est critique

• Le bruit des capteurs

▫ Inconsistance de mesures dans un
même environnement
▫ Source:
! Primitives de l’environnement non perçues (ex. droites)
! Négligence de ces primitives
 27

Le bruit de perception

• Exemple 1:
▫ Système de vision pour localisation à l’intérieur
! Utilisation des couleurs détectées par une caméra CCD
! Soleil caché: illumination différente
! Valeurs de teinte non constantes
! Couleurs « bruitées » pour le robot
! Teinte inutilisable sauf si:
! Connaissance de la position du soleil
! Connaissance de la forme et de la position des nuages
! Autres: flou (mise au point, mouvement du robot)
 28

Le bruit de perception

• Exemple 2: émetteur

▫ Sonar: télémètre à ultrasons récepteur

! Rapport signal sur bruit très pauvre
! Ultrasons émis vers une surface lisse, mal orientée
! Signal non reflété vers le capteur, pas d’écho de retour
! Une petite quantité d’énergie peut revenir malgré tout
▫ Si le seuil sur l’amplitude d’énergie reçue en est proche
▫ Parfois le sonar permettra de détecter l’objet … parfois non
▫ Par conséquent :
Pour le robot, un environnement inchangé peut
donner deux lectures différentes pour un sonar !
 29

Le bruit de perception
▫ Plusieurs sonars
! Sur les robots mobiles, en général:
! Entre 8 et 48 sonars sur une
même support (anneau)
! Interférences
! de l’environnement (interférences « multi-chemins »)
! entre émetteurs d’ultrasons (un signal émis
par un sonar est reçu par un autre)
! Elles peuvent engendrer d’importantes erreurs de mesure
(sous-estimation), mais elles se produisent rarement,
moins de 1% du temps
En résumé:
• Le bruit de perception réduit l’information utile des mesures d’un capteur
• La solution est de prendre en compte plusieurs mesures, en les fusionnant
au cours du temps ou avec d’autres capteurs afin d’améliorer globalement
l’information perceptuelle du robot
 30

L’aliasing perceptuel
• Second inconvénient des capteurs
▫ Le contenu informationnel des mesures peut être très faible
(ça exacerbe les problèmes de perception et de localisation)
▫ Problème peu intuitif pour les humains
! Les humains ne rencontrent que rarement ce phénomène
! La perception humaine, particulièrement la vision, tend à
percevoir des informations uniques dans chaque état local unique
! Chaque lieu différent semble différent
! Mais pas facile de:
! Se déplacer dans un bâtiment inconnu
qui est complètement sombre
! Se déplacer dans un labyrinthe
à taille humaine (pas unicité visuelle)
 31

L’aliasing perceptuel
• En robotique
▫ L’aliasing est la norme et non l’exception !
Exemple: sonars ou proximètres IR
! Champ de perception étroit
! Information de profondeur dans une seule direction
! Pas d’info comme couleur, texture, rigidité de surface
! Même avec plusieurs de ces capteurs, plusieurs états dans
l’environnement donnent les mêmes mesures
! Formellement :
Il y a une association n à 1
des états de l’environnement

y [m]
vers la perception du robot

Carte basée
sonars
x [m]
 32

L’aliasing perceptuel
Exemple concret
! Distinguer les humains des objets
! Le robot détecte un obstacle face à lui
▫ Doit-il dire « excusez-moi » car l’obstacle peut
être un humain en mouvement ?
... ou le robot doit-il planifier un chemin autour
de l’obstacle car ce peut être un objet ?
! Pour le(s) sonar(s) seul(s), ces deux
états souffrent d’aliasing et leur
discrimination est impossible !

humain
ou statue ?
 33

L’aliasing perceptuel
• En résumé l’aliasing perceptuel pose le problème suivant

Même avec des mesures non bruités, la quantité d’information

qui vient des capteurs est trop faible pour identifier la pose du
robot à partir d’un seul percept

▫ « Solution »
! Localisation basée capteurs AmigoBot
laser
et mesures multiples

Mesures
laser
 34

Le bruit d’actionnement
• Les difficultés de la localisation ne reposent
pas uniquement sur les capteurs

• Les actionneurs sont aussi « bruités »

▫ À cause de la perception ou non
▫ Une même action ordonnée par le robot peut
engendrer différentes réalisations …
… même si du point de vue du robot, l’état
initial avec l’action est parfaitement connu

p0
 35

Le bruit d’actionnement

• Les actionneurs d’un robot mobile induisent

une incertitude sur l’état futur

• Pour un robot, se déplacer accroît son incertitude

• La couche décisionnelle peut minimiser cet effet

▫ Planification et/ou interprétation adaptée
▫ Retour des capteurs dans la boucle de commande
 36

Le bruit d’actionnement
• Nature du bruit en robotique mobile
▫ Du point de vue du robot
! Impossibilité d’estimer sa propre position à partir
de la connaissance de sa cinématique/dynamique

▫ Vrai source d’erreur

! Modèle incomplet de l’environnement
! Type de surface non modélisé (rigidité, glissement, etc.)
! Événements non envisagés (une personne pousse
le robot, drone perturbé par une rafale de vent, etc.)
 37

Le bruit d’actionnement
• Exemple : l’odométrie
L’odométrie est une technique permettant d'estimer la position
et orientation d'un robot en mouvement. Le terme vient du
grec hodos ("voyage") et metron ("mesure")

• Uniquement les capteurs sur les roues ...

Source
Récepteur
de lumière

Bandes opaques
et transparentes
Encodeur optique incrémental
 38

Le bruit d’actionnement
• Exemple : l’odométrie

… ou couplés à un gyroscope/compas («dead reckoning»)

▫ Estimation de pose basée sur les capteurs proprioceptifs

▫ Mouvement intégré pour déduire la position
! Intégration des erreurs de mesure
! Accumulation d’erreur en position au cours du temps
▫ Besoin d’un autre mécanisme de localisation
de temps en temps
… sinon, la position estimée n’a vite plus de sens !
 39

Le bruit d’actionnement

• Odométrie pure d’un robot à conduite différentielle

• Sources d’erreurs:
▫ Résolution limitée pendant l’intégration
(incréments de temps, résolution de mesure, etc.)
▫ Mauvais alignement des roues (déterministe)
▫ Incertitude sur le diamètre de la roue et/ou
diamètre non constant (déterministe)
▫ Variation du point de contact de la roue
▫ Contact variable avec le sol roue
(glisse, bosse, sol mou, etc.)
sol
 40

L'expérience du chemin carré unidirectionnel

[Borenstein & Feng, TRA96]

 41

Le bruit d’actionnement

▫ Types de sources d’erreurs

! Déterministes (systématiques)
! Résolues (partiellement) par calibrage du système

! Non-déterministes (aléatoires)
! Erreurs résiduelles
! Mènent à des incertitudes sur la pose au fur et à mesure
 42

Le bruit d’actionnement
• Types géométriques d’erreurs
(pour un robot à conduite différentielle)
▫ Erreur en distance
! Longueur du chemin intégré
! Somme des mouvements de roue
▫ Erreur en virage
! Similaire à l’erreur en distance
! Différence des mouvements de roue
▫ Erreur de dérive (drift)
! Une différence d’erreurs entre les roues
mène à une erreur d’orientation du robot
 43

Le bruit d’actionnement
• Sur de longues périodes
▫ Erreurs de virage et de dérive plus importantes
de l’erreur en distance
! Contribution non linéaire à l’erreur de position
▫ Soit un robot de pose initiale connue (x0, y0, θ0)
! Il se déplace en ligne droite selon l’axe x
! Erreur en y pour un mouvement de d mètres: d sin(Δθ)
! L’erreur devient très importante à mesure
d sin( ✓)
que l’erreur angulaire Δθ croît
✓
! Au cours du temps, l’erreur d’orientation x
par rapport au repère d’origine croît rapidement
et ainsi l’incertitude sur la pose du robot
Objectif: établir un modèle d’erreur pour l’exactitude odométrique
d’un robot et étudier la propagation de l’erreur au fil du temps
 44

Plan du chapitre
• Introduction et défis Partie 1

• Odométrie Partie 2

• Localisation par filtre de Kalman Partie 3

• Autres techniques de localisation Partie 4

 45

Partie 2 : Odométrie
 46

Notation
a, , M 2 R scalaires (nombres réels)

u 2 Rn vecteur colonne de dimension n

A 2 Rn⇥n matrice carrée avec n lignes et n colonnes

i (A) 2 C valeur propre i de la matrice A 2 Rn⇥n

In 2 Rn⇥n matrice identité n ⇥ n

0n⇥m 2 Rn⇥m matrice de zéros n ⇥ m

2 3
a1 0 ··· 0
60 a2 ··· 07
6 7
diag(a1 , a2 , . . . , an ) = 6
6 .. 7 matrice diagonale n ⇥ n
7
40 0 . 05
0 0 ··· an
blkdiag(A1 , A2 , . . . , An ) matrice par blocs diagonales ( Ai est une matrice carrée)
 47

Modèle d’erreur pour l’exactitude odométrique

• Généralement la pose (position et orientation) d’un robot
mobile est représentée par le vecteur:

p = [x, y, ✓]T
• Pour un robot de type unicycle (à conduite différentielle),
la pose est estimée en partant d’une pose connue en intégrant
le mouvement (somme des incréments des distances parcourues)

v
✓ Trajectoire
y du robot
!

Rm x (v, !) : commande (vitesse longitudinale et angulaire)

 48

Modèle d’erreur pour l’exactitude odométrique

• Dans un système discret avec une période d’échantillonnage
fixée, les incréments de distances parcourues sont:
( x = s cos(✓ + ✓/2) t+ t

y = s sin(✓ + ✓/2) p0

sd sg t p y
✓ =
L
L x
sd + sg
où s =
2

( x, y, ✓) Chemin parcouru pendant le dernier intervalle de temps [t, t + t]

sd , sg Distances parcourues pour la roue droite et gauche, respectivement

L Distance entre les deux roues du robot (largeur de l’essieu)

 49

Modèle d’erreur pour l’exactitude odométrique

• La mise à jour de la pose en p0 se fait par:
2 03 2 3
x s cos(✓ + ✓/2)
6 7 6 7
p0 = 4 y 0 5 = p + 4 s sin(✓ + ✓/2) 5
✓0 ✓

• En utilisant les relations pour s et ✓ vues

précédemment, on obtient les équations de base de
mise à jour de pose odométrique pour un robot unicycle:
2 ⇣ ⌘3
sd + sg sd sg
2 3 2 cos ✓ + 2L
x 6
6y7 6 ⇣ ⌘7
7
p0 = f (x, y, ✓, sd , sg ) = 4 5 + 6
6
sd + sg
sin ✓ +
sd sg 7
7
2 2L
4 5
✓ sd sg
L
 50

Modèle d’erreur pour l’exactitude odométrique

• Ces équations n’ont d’intérêt que pour de faibles déplacements

• Plus le déplacement entre deux instants sera important, plus l’estimation

de position sera grossière vis-à-vis de la réalité

• À cause de l’augmentation de l’erreur d’orientation et des erreurs de

mouvement réalisées pendant l’incrément ( sd , sg ), l’erreur en position
augmente avec le temps

• Etablissons un modèle d’erreur pour la pose intégrée p0 pour obtenir

la matrice de covariance
⌃p0 2 R3⇥3
de l’estimé odométrique de la pose

• Pour ce faire, on va supposer la connaissance d’une matrice

de covariance initiale: 3⇥3
⌃p 2 R
 51

Modèle d’erreur pour l’exactitude odométrique

• Pour l’incrément de mouvement des roues ( sd , sg ) , la matrice ⌃ doit
être connue aussi:
" #
kd | sd | 0
⌃ = cov( sd , sg ) =
0 kg | s g |
où

kd , kg sont des constantes positives représentant les paramètres non déterministes

du moteur et de l’interaction de la roue (droite et gauche) avec le sol

Deux hypothèses sont englobées dans l’expression précédente:

• Les erreurs de chaque roue sont indépendantes
• Les variances des erreurs sont proportionnelles au module
des distances parcourues
• Ces hypothèses, bien qu’imparfaits, sont satisfaisantes dans la plupart des cas
en environnement structuré. Les sources d’erreur sur le mouvement sont:
roue déformée, glissement, sol non lisse, encodeurs imprécis, etc.
• Les valeurs des constantes kd , kg dépendent du robot et de l’environnement,
et elles sont estimées expérimentalement
 52

Modèle d’erreur pour l’exactitude odométrique

• En admettant que p et dg = [ sd , sg ]T ne sont pas corrélés et que la
fonction f est bien approximée par la décomposition de Taylor du premier
ordre, on peut appliquer la loi de propagation de l’erreur*, obtenant:
⌃p0 = rp f · ⌃p · (rp f )T + r dg
f ·⌃ · (r dg
f ) T

• La matrice ⌃p est directement égale à la ⌃p0 précédente (et elle peut

être calculée en spécifiant une valeur initiale)
• Etant donnée f on peut calculer les deux matrices jacobiennes rp f , r dg
f:
2 3
 1 0 s sin(✓ + ✓/2)
@f @f @f 6 7
rpp ff =
r = 40 1 s cos(✓ + ✓/2) 5
@x @y @✓
0 0 1
21 s 1 s 3
 2 cos(✓ + ✓/2) 2L sin(✓ + ✓/2) 2 cos(✓ + ✓/2) + 2L sin(✓ + ✓/2)
@f @f 6 7
r dg
f = = 4 12 sin(✓ + ✓/2) + s
2L cos(✓ + ✓/2) 1
2 sin(✓ + ✓/2) s
2L cos(✓ + ✓/2) 5
@ sd @ sg
1/L 1/L

*Cf. le cours de « Surveillance Distribuée » (2° sem.): Var[f (X)] ' Jf (X0 )Var[X] JTf (X0 ), X0 = E[X]
 53

Modèle d’erreur pour l’exactitude odométrique

• Les figures montrent comment croît typiquement l’incertitude sur la position d’un
robot au cours du temps (l'incertitude de l'orientation n'est pas représentée dans les
figures, bien que son effet peut être observé indirectement)
• Les résultats ont été obtenus en appliquant le modèle de propagation de l’erreur
précédent et en représentant la matrice de covariance par une ellipse d’incertitude
(grâce à la décomposition en valeurs propres de ⌃p0)

Mouvement rectiligne Mouvement circulaire (rayon const.)

 54

Remarques
• L’incertitude dans la direction perpendiculaire au mouvement croît plus
rapidement que dans le sens du mouvement. Cela résulte de l’intégration
de l’erreur en orientation
• L’axe principal de l’ellipse d’incertitude ne reste généralement pas exactement
perpendiculaire à la direction du mouvement

Mouvement rectiligne Mouvement circulaire (rayon const.)

 55

Calcul des ellipses d'incertitude

La decomposition d’une matrice carrée en valeurs et vecteurs propres
est un outil de base que l’on trouve dans de nombreuses disciplines
(par ex. dans l’algorithme PageRank de Google ou en théorie des graphes)

Définition
Un vecteur non nul u 2 Rn est un vecteur propre d'une matrice A 2 Rn⇥n
si et seulement si il existe un scalaire tel que :

Au = u
où est appelé valeur propre associée à u .

On détermine les valeurs et vecteurs propres en résolvant l’équation:

(A In )u = 0
Remarque
Les valeurs propres d'une matrice A symétrique (à savoir A = AT ) sont
réelles et tous ses vecteurs propres sont orthogonaux
 56

Calcul des ellipses d'incertitude

Soit A 2 Rn⇥n une matrice symétrique.

En définissant les deux matrices n ⇥ n suivantes:

⇤ = diag( 1, 2, ..., n ), U = [u1 u2 · · · un ]

où

1, 2, ..., n valeurs propres de la matrice A triées par

ordre décroissant (à savoir 1 2 ··· n)

u1 vecteur propre associé à la valeur propre de A

la plus forte (à savoir 1 )

on peut écrire:
AU = U⇤
 57

Calcul des ellipses d'incertitude

Exemple:
2 3
1 2 4
A = 4 2 5 3 5
4 3 1

Valeurs propres triées par ordre décroissant: {6.2768, 4.0937, 5.3706}

2 3 2 3
6.2768 0 0 0.1070 0.8118 0.5740
⇤ = 4 0 4.0937 0 5, U = 4 0.9386 0.1081 0.3278 5
0 0 5.3706 0.3281 0.5738 0.7504

Attention:
• Les valeurs propres sont triées
Commande Matlab: [U, Lambda] = eigs(A) par ordre croissant en module
• Les vecteurs propres sont choisis unitaires
 58

Calcul des ellipses d'incertitude

• Il est pratique courante de représenter l’incertitude de localisation en deux
dimensions (plan du sol) mais à trois degrés de liberté, par une ellipse
d’incertitude autour de la position estimée

• La position réelle du robot, si les paramètres du modèle d’erreur ont été

bien choisis, doit se trouver dans cette ellipse

• En appliquant la décomposition en valeurs/vecteurs propres à ⌃p0 , on obtient

les deux matrices U et ⇤ d'où:
⌃p0 U = U⇤
• En prenant le vecteur propre u1 associé à la plus forte valeur propre 1 de ⌃ p0
on obtient des informations sur l’orientation de l’ellipse d’incertitude

Remarque:
Par définition, une matrice de covariance est une matrice symétrique et
semi-définie positive (donc, les valeurs propres sont réelles et non négatives)
⌃p0 = ⌃Tp0 , ⌃p0 ⌫ 0
 59

Calcul des ellipses d'incertitude

• Les deux premières coordonnées u1,x et u1,y du vecteur u1 permettent
de déduire l’orientation de l’ellipse dans le plan du sol:
✓ ◆
u1,y
✓e = arctan
u1,x
• En pratique, on préfèrera l’utilisation de la fonction atan2 , qui teste dans
quel quart se trouve la position (u1,x , u1,y ) pour que ✓e ait un signe cohérent

• La longeur des demi-axes de l’ellipse est définie par la racine carrée des
deux valeurs propres de ⌃p0 les plus fortes, à savoir 1 et 2

• Il faut noter que 1 et 2 peuvent être interprétées comme des variances.

• Par exemple, si les paramètres du modèle d’erreur sont corrects, pour
assurer que la position réelle se situe dans l’ellipse d’incertitude avec p
une
confiance
p de 99.7%, les demi-axes de celle-ci auront pour longeurs, 3 1
et 3 2 , respectivement
 60
p
3 2
p En résumé …
3 1
Cf. TD1

✓e Ellipse d’incertitude ( 3 )
✓ ◆
u1,y
✓e = arctan
u1,x

Remarque:
Si X est une variable aléatoire gaussienne,
µ est l’espérance de la loi et est l’écart type:
2
X ⇠ N (µ, )
Pr(µ  X  µ + ) ' 0.68269
Pr(µ 2  X  µ + 2 ) ' 0.95450
Pr(µ 3  X  µ + 3 ) ' 0.99730
Pr(µ 4  X  µ + 4 ) ' 0.99993
Pr(µ 5  X  µ + 5 ) ' 0.99999
 61

Odométrie … et les humains ?

• Nous “connaissons” la longueur de nos jambes et les butées mécaniques
des nos articulations depuis notre enfance

• La longueur des nos pas est donc “codée” dans notre cerveau
(bien que nous ne sommes pas complètement coscients)

• Nous pouvons estimer assez précisément la distance parcourue

Même sans retour visuelle (et sans compter les pas), la précision est
plutôt bonne avec des distances jusqu'à 27 m [Andre & Rogers, 2006]

“Using verbal and blind-walking distance estimates to investigate the two visual systems
hypothesis”, J. Andre, S. Rogers, Percept. Psychophys., vol. 68, pp. 353–361, 2006