Angle [deg]

(1)

UPJV, Département EEA Master 2 3EA, EC53

UE optionnelle

Fabio MORBIDI, Mohammed CHADLI

Laboratoire MIS Équipes PR et COVE

{fabio.morbidi,mohammed.chadli}@u-picardie.fr

AU 2018-2019

Vendredi 8h30-12h30, Salle CURI 305 ou TP204

(2)

Ch. 2: Traitement des mesures

Réseaux multi-capteurs

Fusion des mesures Partie 2

Partie 1

(3)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

« Différents » filtres de Kalman:

1. Fusion de mesures

2. Fusion de mesures dans le temps

3. Fusion de mesures de capteurs en mouvement

- Combinaison des évidences (mesures) de façon optimale

- Moyenne récursive

• Filtre de Kalman linéaire (système dyn. linéaire)

• Filtre de Kalman étendu ou EKF (système dyn. non linéaire)

(4)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

Notation

: état du système (scalaire, pour plus de simplicité) : mesure du capteur

: variance de la mesure du capteur

, : estimation (optimale) de et variance de 1. Fusion de mesures

x

Nos hypothèses:

Deux mesures non corrélées:

Bruits blancs gaussiens à moyenne zéro:

Estimation de l’état optimum:

où

x = z

¹

+ K ( z

²

− z

¹

)

σ

²

= (1 − K ) σ

1²

K = σ

1²

σ

₁²

+ σ

₂²

z

_i

σ

_i²

z

_i

= x + r

_i

, i ∈ { 1 , 2 } r

_i

∼ N (0 , σ

_i²

)

i i

σ

²

x

est le gain de Kalman

(5)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

Dans le problème de localisation d’un robot nous avons

combiné les deux mesures de distance et en utilisant l’équation:

1. Fusion de mesures Remarque:

z = (1 − w ) z

¹

+ w z

²

= z

¹

+ w ( z

²

− z

¹

)

avec le poids optimal:

w = σ

1²

σ

₁²

+ σ

₂²

z

¹

z

²

(6)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

1. Fusion de mesures En outre, nous avons vu que:

Var[ z ] = (1 − w )

²

σ

1²

+ w

²

σ

2²

=

1 − σ

1²

σ

₁²

+ σ

₂²

²

σ

1²

+

σ

1²

σ

₁²

+ σ

₂²

²

σ

2²

=

1 − σ

1²

σ

₁²

+ σ

₂²

σ

1²

= (1 − w ) σ

1²

(7)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

est le gain de Kalman

z

¹

z

²

z ( t

¹

) , z ( t

²

) t

¹

< t

²

Estimation de l’état optimum à l’instant :

t

²

x ( t

²

) = z ( t

¹

) + K ( z ( t

²

) − z ( t

¹

))

σ

²

= (1 − K ) σ

_z²₍_t₁₎

où

K = σ

_z²₍_t₁₎

σ

_z²₍_t₁₎

+ σ

_z²₍_t₂₎

• Introduisons maintenant le temps

• Les mesures et sont prises de façon

séquentielle, à savoir nous avons avec 2. Fusion de mesures dans le temps

t

(8)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

• Que se passe-t-il si nous prenons une autre mesure à l’instant ?

Estimation de l’état optimum à l’instant générique

où

z ( t

³

) t

³

> t

²

t

k

K ( t

k−1

) = σ

²

( t

k−1

)

σ

²

( t

_k−¹

) + σ

_z²₍_t_k₎

x ( t

_k

) = x ( t

_k−¹

) + K ( t

_k−¹

)( z ( t

_k

) − x ( t

_k−¹

))

σ

²

( t

k

) = (1 − K ( t

k−1

)) σ

²

( t

k−1

)

_“Moyenne

recursive”

2. Fusion de mesures dans le temps

est le gain de Kalman (variable dans le temps)

(9)

Fusion de capteurs en robotique mobile (localisation) : Filtre de Kalman

Entrée du filtre: la commande et les mesures

Sortie du filtre: l’estimation de l’état du système et la matrice de covariance de l’erreur d’estimation

associée

A B Q R

Matrices du système:

3. Systèmes dynamiques

x

u z

x P

u

z P

x

Etant donné un système dynamique à temps discret d’état

H

(10)

Filtre de Kalman linéaire

• Soit le système dynamique à temps discret, :

x

_k

= A

_k−¹

x

_k−¹

+ B

_k−¹

u

_k−¹

+ w

_k−¹

• Le bruit de modèle et le bruit de mesure sont supposés à moyenne zéro, blancs (sans mémoire) et gaussiens avec matrices

de covariance et , respectivement. Les bruits de modèle et de mesure sont supposés aussi non corrélés. En résumé, nous avons:

w

k−1

Qk

R

k

k ∈ {1, 2, . . .}

Entrée de commande (connue)

z

_k

= H

_k

x

_k

+ r

_k

r

k

Équation d’état Équation de mesure

E [ w

k

] = 0 E [ r

k

] = 0

E [ w

_k

w

_j^T

] = 0, j = k E [ r

_k

r

^T_j

] = 0, j = k E [ r

_k

r

^T_k

] = R

_k

E [ r

_k

w

^T_j

] = 0, ∀ j, k

E [ w

_k

w

_k^T

] = Q

_k

(11)

• Le filtre de Kalman linéaire à temps discret est un estimateur récursif

Pour estimer l'état courant , on a besoin uniquement de connaître l'estimé de l'état précédent et les mesures courantes: l'historique des mesures et des estimés n'est pas nécessaire

: estimé de l'état du système à l'instant , en utilisant l’information jusqu’à l’instant

x

_k|k

P

_k|k

= E [( x

_k

− x

_k|k

)( x

_k

− x

_k|k

)

^T

]

: matrice de covariance de l'erreur d’estimation

de l’état à l'instant , en utilisant l’information jusqu’à l’instant

Erreur d'estimation de l’état du système

x

_k|k xk

1.

2.

xk

Filtre de Kalman linéaire

x

_k

k k

• L' « état » du filtre se compose de deux variables:

(12)

Initialisation du filtre:

Prédiction (ou propagation):

x

_k|k−¹

= A

k−1

x

_k−¹_|k−¹

+ B

k−1

u

k−1

P

_k|k−1

= A

_k−¹

P

_k−1|k−1

A

^T_k−1

+ Q

_k−¹

Pour :

k ∈ { 1 , 2 , . . .}

x

⁰_|⁰

= E [ x

⁰

]

où est l’état initial du système

Filtre de Kalman linéaire

x⁰ ∈ Rⁿ

P

⁰_|⁰

= E [( x

⁰

− x

⁰_|⁰

)( x

⁰

− x

⁰_|⁰

)

^T

] ∈ R

^n×n

(13)

Correction (ou mise à jour):

est la matrice d'estimation a posteriori de la covariance de l'erreur

x

_k|k

= x

_k|k−¹

+ K

k

( z

k

− H

k

x

_k|k−¹

)

où

K

k

= P

_k|k−1

H

^T_k

( H

k

P

_k|k−1

H

^T_k

+ R

k

)

⁻¹ est le gain de Kalman

est la matrice d'estimation a priori de la covariance de l'erreur

P

_k|k

P

_k|k−1

Filtre de Kalman linéaire

Remarque:

P

_k|k

= ( P

⁻_k|k−¹ ₁

+ H

^T_k

R

⁻_k ¹

H

_k

)

⁻¹

= ( I

_n

− K

_k

H

_k

) P

_k|k−1

(14)

• Un chariot à roues encodeuses avance pas-à-pas

• Un laser mesure la distance du chariot par rapport à un mur

Exemple I : une variable d’état

z

σ

^laser

= 0 . 1 m

x

mur

laser

Laser

TD1

Exemple:

Une variable d’état:

la position du chariot

x

σ^odom = 0.01 m

(15)

Dans cet exemple, nous avons que (rappel la forme générale d’un système dynamique à temps discret vue précédemment):

où est le pas de discrétisation (fixe) et est la vitesse moyenne du chariot dans cet intervalle de temps

Exemple I : une variable d’état

Équation d’état:

Équation de mesure:

Δk V

x

_k

= x

_k−¹

+ V Δ k + w

_k−¹

z

_k

= x

_k

+ r

_k

Donc:

xk = xk, uk−1 = uk−1 = V Δk, wk−1 = wk−1, zk = zk, rk = rk

Ak−1 = 1, Bk−1 = 1, Qk = σ_odom² , Hk = 1, Rk = σ_laser²

(16)

: estimé de la position du robot avant le déplacement : estimé de la position du robot après le déplacement

et avant la mesure

: estimé de la position du robot après la mesure

Correction Prédiction

Exemple I : une variable d’état

x

_k−1|k−1

x

_k− _|k−

x

_k|k−1

x

_k|k−1

x

_k|k

x

_k|k

u

_k−¹

u

_k−¹

z

_k

z

_k

z

_k

(17)

Prédiction:

Correction:

x

_k|k

= x

_k|k−¹

+ K

k

( z

k

− x

_k|k−¹

) P

_k|k

= (1 − K

k

) P

_k|k−¹

K

k

= P

_k|k−¹

( P

_k|k−¹

+ R )

⁻¹

= P

_k|k−¹

P

_k|k−¹

+ σ

_laser²

où

Exemple I : une variable d’état

Les équations du filtre de Kalman sont alors:

x

_k|k−1

= x

_k−1|k−1

+ V Δ k

P

_k|k−1

= P

_k−1|k−1

+ σ

odom²

(18)

Exemple II : deux variables d’état

Un gyroscope et un compas magnétique sont placés sur une plate-forme mobile

• Les mesures de vitesse fournies par le gyroscope sont entachées par un bruit blanc gaussien:

(avec biais)

• Les mesures de position fournies par le compas sont aussi entachées par un bruit blanc gaussien:

(sans biais) La plate-forme tourne librement autour de son axe vertical

N ( μ

^gyro

, σ

gyro²

) = N (0 . 1 , 0 . 2

²

) deg/s

N ( μ

^comp

, σ

comp²

) = N (0 , 10

²

) deg

Les deux mesures sont supposés non corrélés

TD1

g c

(19)

Objectif: fusionner les deux mesures ensemble

• Le mesures du gyroscope (capteur proprioceptif) sont

précises, mais elles dérivent (cf. l’odométrie dans l’Exemple I)

• Le mesures du compas (capteur extéroceptif) ne dérivent pas, mais elles sont fortement bruitées

donc, on peut conclure que:

• Les mesures du gyroscope sont bonnes à court terme

• Les mesures du compas sont bonnes à long terme

Exemple II : deux variables d’état

(20)

• L’état du système consiste en la position et en la vitesse angulaire de la plate-forme, à savoir:

• Étant un capteur proprioceptif, les mesures du gyroscope équivalent à une commande: . On peut donc définir le système dynamique suivant:

où est le pas de discrétisation (fixe) et

E [ w

k

] = [0 , μ

^gyro

]

^T

, Q = E [ w

k

w

_k^T

] = diag(0 , 16 σ

gyro²

)

Sur-estime, pour prendre en compte le biais

x = [ θ, θ ˙ ]

^T

A B

(1)

θ ˙

^gyro

( k ) = u

k

Exemple II : deux variables d’état

Δ k

x

_k

=

1 Δ k

0 0

x

_k−¹

+

0

1 u

_k−¹

+ w

_k−¹

(21)

• Les “vraies mesures” sont effectuées uniquement par le compas (capteur extéroceptif)

• En d’autres termes, nous ne voulons pas que le gyroscope intervienne dans la phase de correction du filtre de Kalman, donc on peut définir simplement:

où

E [ r

_k

] = μ

^comp

= 0 , R = E [ r

_k²

] = σ

comp²

(2)

Grâce à l’équation d’état (1) et de mesure (2) ainsi trouvées, on peut écrire facilement les équations du filtre de Kalman pour la fusion des mesures du gyroscope et du compas

z

k

= [1 0] x

k

+ r

k

H

Exemple II : deux variables d’état

(22)

Résultats de la simulation (20 mesures/s)

Angle [deg]Angle [deg]Angle [deg]

Temps [s]

197.5 198 198.5 199 199.5 200

0 20 40 60 80 100 120 140 160 180 200

0 0.5 1 1.5 2 2.5

0 20 40 60 80 0 10 20 3040

-10 -20 0 50 100

-50 -100

Position angulaire réelle Compas

Intégrale des mesures du gyroscope Estim. Kalman

Derive du gyro

Exemple II : simulation

(23)

Exemple III : navigation intégrée, fusion INS/GPS

Système de navigation inertielle (INS) pour véhicules

• Avantages:

• Autonome

• Grande précision pour une navigation à court terme

• Inconvénient majeur:

L’erreur de positionnement augmente sans cesse au fils du

temps, à cause de l’accumulation des erreurs du gyroscope et de l’accéléromètre avec le temps, entre autres

Pour cette raison, pour une navigation à long terme un INS est souvent couplé avec un système d’aide à la navigation,

par exemple:

• Système de radionavigation (ex. LORAN, OMEGA et TACAN)

• Système de navigation par satellites (ex. GPS, GLONASS)

(24)

• La forme d’intégration INS/GPS la plus utilisée en pratique est basée sur le filtrage complémentaire

• Le filtre de Kalman (étendu) peut être utilisé pour mettre en œuvre une configuration de filtre complémentaire

Pour plus de détails, voir la Sect. 12.10 du livre de Bar-Shalom et al.

Schéma en boucle ouverte pour la fusion INS/GPS basé sur le filtre de Kalman

État + erreurs GPS

État + erreurs INS

erreurs GPS − erreurs INS

Estimés de l’état

≈ − erreurs INS

Exemple III : navigation intégrée, fusion INS/GPS

Filtre