Localisa(on et naviga(on de robots

Localisa(on et naviga(on de robots

TD2: Localisa-on par filtre de Kalman étendu

Fabio MORBIDI

Localisa(on d’un robot mobile basée sur carte

Pose ini(ale connue

M

x y

Prédic(on (odométrie)

Correc(on (mesure)

x₀

Représenta(on de la carte

1.  Décomposi(on exacte en cellules

2.  Décomposi(on approxima,ve en cellules

•  Cellules de taille fixe

•  Cellules de taille variable

3.  Cartes topologiques

4.  Représenta(on con(nue basée sur droites

Représenta(on de la carte

1.   Décomposi(on exacte en cellules [Latombe, 1991]

Représenta(on de la carte

2.  Décomposi(on approxima(ve en cellules

•  Cellules de taille fixe (« occupancy grid »)

•  Simple et très u(lisée

•  Les passages étroits disparaissent (pas une représenta(on complète)

•  Grande mémoire nécessaire si la taille des cellules est pe(te

Représenta(on de la carte

•  Cellules de taille variable

•  Décomposi(on adapta(ve

Représenta(on de la carte

3.   Cartes topologiques

•  Elles représentent l'environnement comme un graphe avec nœuds et arêtes:

•  Les nœuds correspondent à l’espace géométrique

•  Les arêtes correspondent aux connexions physiques

entre les nœuds

•  Les cartes topologiques sont

dépourvues d’échelle et de distance, mais les rela,ons topologiques (ex. gauche, droite) sont maintenues

Nœud (loca(on)

Arête (connexion)

Représenta(on de la carte

•  Exemple de carte topologique

•  Carte du métro de Paris

Représenta(on de la carte

4.  Représenta(on con(nue basée sur droites

a)  Représenta(on avec un ensemble de droites (finies ou infinies)

b)  Extrac(on de droites à par(r de l’informa(on 2D de caméras, sonars ou lasers a)  Transformée de Hough

b)  RANSAC (RAndom Sample Consensus)

c)  Split-and-Merge, Line Regression (seulement pour mesures laser)

Représenta(on de la carte: sommaire

1) Carte réelle avec murs, mobilier, portes, etc.

4) Représenta(on con(nue basée sur droites:

100 droites avec 2 paramètres

2) Décomposi(on approxima(ve: environ 3000 cellules de taille fixe 50 × 50 cm. Chaque cellule est soit remplie (par(e d'un obstacle) ou vide (espace libre)

3) Carte topologique: 18 nœuds

Posi(on du robot

Nœud i

(b) (c)

(a)

(d)

(1) (2) (3)

(4)

Problème de localisa(on probabiliste

Approche probabiliste au problème de localisa(on:

Il permet de calculer la densité de probabilité (pdf) de la pose d’un robot dans chaque étape de prédic(on et de correc(on

Ingrédients:

1.  Densité de probabilité ini(ale: croyance(x₀)

2.  Modèle d'erreur sta(s(que des capteurs propriocep,fs (ex. encodeurs des roues)

3.  Modèle d'erreur sta(s(que des capteurs extérocep,fs (ex. laser, sonar, caméra)

4.  Carte de l’environnement

M

Fusion des capteurs propriocep(fs et extérocep(fs

Landmark = marqueur passif

Odometry

Localisa(on d’un robot par EKF

•  Robot de type unicycle à conduite différen-elle

•  Capteurs: encodeurs sur les roues (propriocep(fs), télém. laser (extérocep(f)

•  Repère du télémètre laser {S} repère du robot {R}

•  Primi-ves de M : droites (dans le repère monde {W})

Hypothèses

⌘

{R}

{S}

m

laser

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteurs b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)  Es(ma(on

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteurs b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)  Es(ma(on

①   Prédic-on

Prédic-on de la nouvelle pose à par-r de:

•  L’ancienne pose:

•  Le déplacement des roues (gauche, droite):

On peut u(liser le modèle pour l’exac(tude odométrique développé dans le TD1:

P b

= F

P

F

+ F

Q

F

covariance de l’état précédent du robot

covariance du bruit du modèle de mouvement:

u

= [ s

, s

]

x

= [x

, y

, ✓

]

b

x_t = f(x_{t 1}, u_t) = 2 64

x_{t 1} y_{t 1}

✓_{t 1} 3 75 +

2 66 64

s_d+ s_g

2 cos⇣

✓_{t 1} + ^sd_2L ^sg ⌘

s_d+ sg

2 sin⇣

✓_{t 1} + ^sd_2L ^sg ⌘

sd+ sg

L

3 77 75

P

x

Q

où Q_t = diag(k_d| s_d|, k_g| s_g|)

Modèle non linéaire !

{W}

Prédic(on de la pose

①   Prédic-on

Temps t

Temps t - 1

x

b

x

Rappel le TD1 - Exercices 1-2

Fonc(ons « MAJEtatOdometrie » et «propageErreurs» pour la mise à

jour de la pose du robot et le calcul de la matrice de covariance ⌃

①   Prédic-on

Exercice 1

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteur b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)   Es(ma(on

②   Correc-on

a)   Observa-on des primi-ves par mesure capteur

•  Primi(ves de type droite

•  Une droite est paramétrée en coordonnées polaires:

•  n observa(ons :

•  Données dans le repère local {R} du robot

R

, i 2 { 1, . . . , n }

z

=

"

↵

r

#

z = ⇥

(z

)

, . . . , (z

)

⇤

2 R

i i

{R}

droite i

②   Correc-on

a)  Observa-on des primi-ves par mesure capteur

•  n droites observées, 2 n paramètres

•  L’incer(tude sur les droites est representée par la matrice de covariance:

•  Ce@e matrice de covariance peut être calculée à par(r des incer(tudes des mesures contribuant à chaque droite

•  Dans notre implémenta(on Matlab, on va choisir:

R

=



r↵ rr

i

t

, i 2 { 1, . . . , n }

R

=

"

2.5 ⇡/180 0

0 10

#

, i 2 { 1, . . . , n }

②   Correc-on

a)   Observa-on des primi-ves par mesure capteur

Données brutes généréees par le

télémétre laser

Les droites sont extraites (ex. avec RANSAC)

Représenta(on des paramètres des droites et des incer(tudes associées

dans l’espace du modèle

↵ⁱ Droite i

Droite i

Espace du modèle

r

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteur b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)   Es(ma(on

②   Correc-on

b)   Prédic-on de mesure des primi-ves

•  U-lise:

•  La posi(on prédite du robot (calculée dans l’étape 1)

•  Le modèle de la primi(ve (une droite)

•  Donne:

•  L’observa(on prédite pour chaque primi(ve

dans le repère local {R} du robot b x

W

b z

= h

( x b

, m

) m

m

=

"

↵

r

#

②   Correc-on

b)   Prédic-on de mesure des primi-ves

b z

=

"

b

↵

b r

#

= h

( x b

, m

) =

"

↵

✓ b

r

x b

cos( ↵

) y b

sin( ↵

)

#

W W W

r

b

R

droite j

R

r b

R↵b^j_t

W

↵

W

r

{W}

②   Correc-on

b)   Prédic-on de mesure des primi-ves

h

( x b

, m

)

pour pouvoir implementer le filtre de Kalman b

x

Par la suite on va u(liser la matrice jacobienne de par rapport à :

H

= @ h

( x b

, m

)

@ x b

=

"

0 0 1

cos(

↵

) sin(

↵

) 0

#

②   Correc-on

b)  Prédic-on de mesure des primi-ves

•  Les prédic,ons de mesure peuvent être représentées dans l'espace du modèle d’un façon similaire aux observa(ons

b

↵^j

b r

•  Le résultat de la prédic(on de mesure sont droites représentées dans le repère du robot {R}

•  Elles sont incertaines, parce que la prédic(on de pose du robot

est incertaine x b

Droite j

Espace du modèle

②   Correc-on

b)   Prédic-on de mesure des primi-ves

W

b z

=

"

b

↵

b r

#

= h

( x b

, m

) =

"

↵

✓ b

r

x b

cos( ↵

) y b

sin( ↵

)

#

W W W

R

Exercice 2.2 (voir “initEnviro.m”, “genereTrajs.m” pour la ges(on des droites)

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteur b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)   Es(ma(on

②   Correc-on

c)   Appariement des mesures

•  Trouver la meilleur correspondance entre:

•  Les mesures courantes (« index i »)

•  Les prédic(ons de mesure (« index j »)

•  Pour chaque appariement on calcule:

•  L’innova(on

•  La matrice de covariance de l’innova(on

R R

z

=

"

↵

r

# ?

b z

=

"

b

↵

b r

#

(i, j )

②   Correc-on

c)   Appariement des mesures

•  Pour chaque appariement :

•  Calculer l’innova(on:

•  Calculer la covariance de l’innova(on:

•  Pour valider l’appariement, on u(lise la distance de Mahalanobis avec un seuil :

⌃

t

= H

P b

(H

)

+ R

(v

)

(⌃

)

v

 g

v

= z

h

( x b

, m

) =

"

↵

r

# "

↵

✓ b

r

x b

cos( ↵

) y b

sin( ↵

)

#

R _W

W W W

g > 0

(i, j )

②   Correc-on

c)   Appariement des mesures

Espace du modèle

Appariement i, j

Mesure (épais), prédic(on (fine)

Pas d’appariement.

Le mur n'a pas été observé !

On cherche les meilleurs appariements:

b r

r

Aberrante

Mur

②   Correc-on

c)   Appariement des mesures

•  Trouver la correspondance entre les mesures et les observa(ons prédites

•  Calculer l’innova(on composite

•  Calculer la covariance de l’innova(on composite

•  Empiler les en

v

⌃

H

H

[v

, H

, ⌃

] = appariement( b z

, z

, P b

, R

, g)

b z

z

Exercice 2.3

②   Correc-on

c)   Appariement des mesures Exercice 2.3

Localisa(on d’un robot par EKF

①   Prédic-on

②   Correc-on

a)  Observa(on des primi(ves par mesure capteur b)  Prédic(on de mesure des primi(ves

c)  Appariement des mesures

d)   Es(ma(on

d)   Es-ma-on En se basant sur:

•  La pose prédite et la covariance

•  Les mesures appariées

(I) Mise à jour de la pose:

•  Gain du filtre de Kalman:

(II) Mise à jour de la covariance:

b x

x

= x b

+ K

v

K

= P b

H

(⌃

)

P

= P b

K

⌃

K

[x

, y

, ✓

, P

] = estimation( x b

, y b

, ✓ b

, P b

, v

, H

, ⌃

)

②   Correc-on

Exercice 2.4

P b

Exercice 2.4

②   Correc-on

Exercice 2.1

②   Correc-on

En fusionnant la prédic,on de la pose du robot (fine) avec l'innova,on obtenue par les mesures (épais), nous obtenons l'es(ma(on finale de la pose du robot (très épais)

Es(ma(on finale

Prédic(on (odométrie)

Innova(on par mesures

②   Correc-on

x

b

x

, P b

Sommaire des équa(ons de l’EKF

Prédic-on

Avantage

•  Efficacité au niveau de calcul (mécanisme récursif) Inconvénient

•  Si l'incer(tude du robot devient trop importante (ex. collision avec un objet)

Correc-on

P b

= F

P

F

+ F

Q

F

b

x

= f (x

, u

)

x

= x b

+ K

v

P

= P b

K

⌃

K

K

= P b

H

(⌃

)