Localisa)on et naviga)on de robots

Localisa)on et naviga)on de robots

TD1: Odométrie

Fabio MORBIDI

TD1: Odométrie

Objec2f: Es)ma)on de la trajectoire d’un robot mobile

•  Calculer la pose d’un robot unicycle (à conduite différen)elle) à par)r de l’odométrie

•  Tracer la posi)on instantanée et simuler la trajectoire du robot

•  Iden)fica)on de l’erreur (incer)tude) par le calcul de la matrice de covariance

•  Extrac)on et affichage des ellipses d’erreur sur la

(x, y, ✓)

TD1: Odométrie

Matériel: Répertoire packTD1.zip Fichier « simu.m » u)lise :

•  Fichier « initRobot » pour les paramètres du robot

•  Fichier « initBruits » pour simuler:

•  Le bruit d’une commande mal appliquée

•  Le bruit de percep)on des capteurs

•  Fichier « genereTrajs »

•  pour générer la trajectoire du robot

•  S’il applique parfaitement la commande

•  Si on prends en compte les bruits

•  Donne les valeurs des angles de chaque roue: et ^'

TD1: Odométrie

Matériel: Répertoire packTD1.zip

Répertoire « ou)ls » con)ent des fonc)ons:

•  « Drawarrow » : Dessiner une flèche

•  « Drawroundedrect » : Dessiner des rectangles

•  « Drawellipse » : Dessiner une ellipse

•  « Drawrobot » : Dessiner un robot

Exercice 1

Fonc2on « MAJEtatOdometrie »:

•  Entrées:

•  Les distance parcourues par la roue gauche et droite entre deux instants de temps et la pose courante du robot

•  La largeur de l'essieu,

•  Sor2e:

•  Mise à jour de la pose :

U)liser ce@e fonc)on dans le fichier « simu.m » pour tracer le robot à chaque instant

p

= [x

, y

, ✓

]

p

L

Exercice 1

Fonc2on « MAJEtatOdometrie »:

x = s cos(✓ + ✓/2)

y = s sin(✓ + ✓/2)

✓ = s

s

L

s = s

+ s

2

p ⁰ = 2 6 4

x ⁰ y ⁰

✓ ⁰ 3

7 5 = p + 2 6 4

s cos(✓ + ✓/2) s sin(✓ + ✓/2)

✓

3 7 5 p =

2 6 4

x y

✓ 3 7 5

,

,

,

Exercice 1

Fonc2on « MAJEtatOdometrie »:

x = s cos(✓ + ✓/2)

y = s sin(✓ + ✓/2)

✓ = s

s

L

s = s

+ s

2

p = 2 6 4

x y

✓ 3 7 5

p

= f (x, y, ✓, s

, s

) = 2 6 4

x y

✓ 3 7 5 +

2 6 6 6 6 4

s_d+ s_g

2

cos ⇣

✓ +

⌘

s_d+ s_g

2

sin ⇣

✓ +

⌘

s_d s_g L

3 7 7 7 7 5

,

,

Exercice 1

Fonc2on « MAJEtatOdometrie »:

Exercice 1

Dans le ficher « simu.m »

Ssss

s

Exercice 1

Ss

Exercice 2

Propaga2on de l’erreur:

Fonc)on « propageErreurs » pour le calcul de la matrice de covariance

•  Entrées:

•  La matrice de covariance associée à l’état précédent

•  L’état à l’instant précédent

•  L’incrément courant

•  Les paramètres

•  Sor2e:

•  Matrice de covariance

⌃ _p

⌃ _p

⌃

L, k

, k

Exercice 2

⌃ = cov( s

, s

) =

"

k

| s

| 0

0 k

| s

|

#

⌃

= r

f · ⌃

· ( r

f )

+ r

f · ⌃ · ( r

f )

r

f =

 @ f

@ x

@ f

@ y

@ f

@✓ = 2 6 4

1 0 s sin(✓ + ✓/2) 0 1 s cos(✓ + ✓/2)

0 0 1

3 7 5

r dgf =

 @f

@ s_d

@f

@ s_g = 2 64

1

2 cos(✓ + ✓/2) _2L^s sin(✓ + ✓/2) ¹₂ cos(✓ + ✓/2) + _2L^s sin(✓ + ✓/2)

1

2 sin(✓ + ✓/2) + _2L^s cos(✓ + ✓/2) ¹₂ sin(✓ + ✓/2) _2L^s cos(✓ + ✓/2)

1/L 1/L

3 75

Exercice 2

Propaga2on de l’erreur:

Fonc)on « propageErreurs » pour le calcul de la matrice

de covariance ^⌃ p

Exercice 2

Propaga2on de l’erreur:

Fonc)on « propageErreurs » pour le calcul de la matrice de covariance ^⌃ p

Dans le ficher « simu.m »

Exercice 3

Propaga2on de l’erreur:

Fonc)on « extraitEllipse » pour calculer l’ellipse d’incer)tude associée à

Affichez ces ellipses (cf. dossier ou<ls) à un pas régulier sur la trajectoire du robot (mais n’affichez pas toutes les ellipses, sinon ce sera illisible !)

⌃ _p

Exercice 3

Propaga2on de l’erreur:

Fonc)on « extraitEllipse » pour calculer l’ellipse d’incer)tude associée à

Remarque:

•  Les racines carrées des valeurs propres les plus fortes de la matrice de covariance donnent les longueurs des demi-axes de l’ellipse

•  Le vecteur propre associé à la valeur propre la plus forte donne l’orienta)on de l’ellipse

⌃ _p

Exercice 3

Propaga2on de l’erreur:

Fonc)on « extraitEllipse » pour calculer l’ellipse

d’incer)tude associée à ^⌃ p

Exercice 3

Propaga2on de l’erreur:

Fonc)on « extraitEllipse » pour calculer l’ellipse d’incer)tude associée à ^⌃ p

Dans le ficher « simu.m »