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 compte des 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
L
p
�= [x
�, y
�, θ
�]
Tp
Exercice 1
Fonc2on « MAJEtatOdometrie »:
∆x = ∆s cos(θ + ∆θ/2)
∆y = ∆s sin(θ + ∆θ/2)
∆θ = ∆s d − ∆s g L
∆s = ∆s d + ∆s g 2
p � =
x � y � θ �
= p +
∆s cos(θ + ∆θ/2)
∆s sin(θ + ∆θ/2)
∆θ
p =
x y θ
,
,
,
Exercice 1
Fonc2on « MAJEtatOdometrie »:
∆x = ∆s cos(θ + ∆θ/2)
∆y = ∆s sin(θ + ∆θ/2)
∆θ = ∆s d − ∆s g L
∆s = ∆s d + ∆s g 2
p =
x y θ
p
�= f (x, y, θ , ∆s
d, ∆s
g) =
x y θ
+
∆sd+∆sg
2
cos �
θ +
∆sd2L−∆sg�
∆sd+∆sg
2
sin �
θ +
∆sd2L−∆sg�
∆sd−∆sg L
,
,
Exercice 1
Fonc2on « MAJEtatOdometrie »:
Exercice 1
Dans le ficher « simu.m »
Ssss
s
Exercice 1
Ss
Exercice 2
Propaga2on d’erreurs:
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
g, k
dExercice 2
Σ
∆= cov(∆s
d, ∆s
g) =
� k
d| ∆s
d| 0
0 k
g| ∆s
g|
�
Σ
p�= ∇
pf · Σ
p· ( ∇
pf )
T+ ∇
∆dgf · Σ
∆· ( ∇
∆dgf )
T∇
pf =
� ∂f
∂ x
∂ f
∂ y
∂ f
∂θ
�
=
1 0 − ∆s sin(θ + ∆θ/2) 0 1 ∆s cos(θ + ∆θ/2)
0 0 1
∇∆dgf =
� ∂f
∂∆sd
∂f
∂∆sg
�
=
1
2 cos(θ+∆θ/2)− ∆s2L sin(θ+ ∆θ/2) 12 cos(θ+∆θ/2) + ∆s2L sin(θ+ ∆θ/2)
1
2 sin(θ +∆θ/2) + ∆s2L cos(θ+ ∆θ/2) 12 sin(θ +∆θ/2)− ∆s2L cos(θ+ ∆θ/2)
1/L −1/L
