Angle [deg]

(1)

UPJV, Département EEA Master 2 EEAII

Parcours ViRob

Fabio MORBIDI

Laboratoire MIS

Équipe Perception Robotique E-mail: [email protected] Mercredi 10h00-12h30, Jeudi 14h00-17h00

Salle TP101

(2)

Plan du cours

Chapitre 1: Perception pour la robotique

Chapitre 2: Modélisation de l’incertitude

1. Introduction

3. Typologies de capteur 2. Classification des capteurs

1. Introduction

3. Incertitude d’un capteur 2. Représentation de l’erreur

4. Propagation d’incertitude

Chapitre 3: Traitement des mesures

1. Réseau multi-capteurs 2. Fusion des mesures

Partie I : Perception Avancée

(3)

Ch. 3: Traitement des mesures

Réseau multi-capteurs

Fusion des mesures Partie 2 Partie 1

(4)

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)

(5)

Notation

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

: variance de la mesure (notre “confiance” en elle) : estimation optimale de

1. Fusion de mesures

x x

x

Hypothèses:

Deux mesures de capteurs (non corrélées):et

Bruits gaussiens à moyenne zéro:

Estimation de l’état optimum:

où est le gain de Kalman

z¹ z²

x = z¹ + K(z² − z¹)

σ² = (1 − K)σ1² K = σ1²

σ₁² + σ₂²

z_i ∼ N(0, σ_i²), i ∈ {1,2}

z_i

σ_i²

(6)

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

(7)

• 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³) t3 > t2

tk

K(tk−1) = σ²(tk−1)

σ²(t_k−¹) + σ_z²₍_t_k₎

x(tk) = x(tk−1) + K(tk−1)(z(tk) − x(tk−1))

σ²(t_k) = (1 − K(t_k−¹))σ²(t_k−¹) _“Moyenne

recursive”

2. Fusion de mesures dans le temps

est le gain de Kalman (variable dans le temps)

k ∈ N

(8)

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

A B Q R

Matrices:

3. Systèmes dynamiques

u z x

x P

u

z

^P

x

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

H

(9)

Filtre de Kalman linéaire

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

xk = Ak−1xk−1 + Bk−1uk−1 + wk−1

• 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 non corrélés. En résumé, nous avons:

wk−1

E(w_k) = 0

E(wkw^T_k ) = Qk

E(w_kw_j^T) = 0, j = k

Qk Rk

k ∈ {1, 2, . . .}

Entrée de commande (connue)

z_k = H_k x_k + r_k

r_k

E(r_k) = 0

E(r_k r^T_j ) = 0, j = k E(rk r^T_k ) = Rk

E(r_k w_j^T) = 0, ∀ j, k

Equation d’état Equation de mesure

(10)

• 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

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

: 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

x_k

k k

(11)

Initialisation du filtre:

P⁰_|⁰ = E[(x⁰ − x⁰_|⁰)(x⁰ − x⁰_|⁰)^T ]

Phase de prédiction:

x_k|k−¹ = Ak−1x_k−¹_|k−¹ + Bk−1uk−1

P_k|k−¹ = Ak−1P_k−¹_|k−¹A^T_k−1 + Qk−1

Pour : k ∈ {1, 2, . . .}

x0|0 = E[x⁰] où est l’état initial du système x⁰

(12)

Phase de correction:

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

x_k|k = x_k|k−¹ + Kk(zk − Hk x_k|k−¹)

où

Kk = P_k|k−¹ H^T_k (Hk P_k|k−¹H^T_k + Rk)⁻¹ P_k|k = (P⁻_k|k−¹ ₁ + H^T_k R⁻_k ¹Hk)⁻¹

est le gain de Kalman Innovation

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

P_k|k P_k|k−1

Remarque:

(13)

• 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

σ^pas = 0.01 m σ^laser = 0.1 m

x

mur

laser

pas

Laser

TD3

Exemple:

(14)

x_k|k−¹

x_k−¹_|k−¹ : 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

x_k|k−¹

zk

x_k|k zk

zk

u_k−¹

x_k−¹_|k−¹ x_k|k Exemple I: une variable d’état

u_k−¹

Correction Prédiction

(15)

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

x_k = x_k, u_k−¹ = u^pas_k−₁, A_k−¹ = 1, B_k−¹ = 1, H_k = 1

Phase de prédiction:

x_k|k−¹ = x_k−¹_|k−¹ + u^pas_k−₁ P_k|k−¹ = P_k−¹_|k−¹ + σpas²

Phase de correction:

x_k|k = x_k|k−¹ + Kk(zk − x_k|k−¹) P_k|k = (1 − Kk)P_k|k−¹

Kk = P_k|k−¹(P_k|k−¹ + R)⁻¹ = P_k|k−¹

P_k|k−¹ + σ_laser² Exemple I: une variable d’état

où

(16)

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 gaussien:

(avec biais)

• Les mesures de position fournies par le compas sont aussi entachées par un bruit 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 bruits sont supposés non corrélés

g c

TD3

(17)

Objectif: fusionner les deux mesures ensemble

• Le mesures du gyroscope sont précises, mais elles dérivent (cf. l’odométrie dans Exemple I)

- Gyroscope: capteur proprioceptif

- Compas magnétique: capteur extéroceptif

• Le mesures du compas ne dérivent pas, mais elles sont fortement bruitées

donc, on peut conclure que:

Les mesures du gyroscope sont bonnes à court terme tandis que celles du compas sont bonnes à long terme

(18)

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

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

où est la cadence d'échantillonnage et

E[wk] = [0, μ^gyro]^T , Q = E[wkw^T_k ] = diag(0, 16σgyro² )

Sur-estime, pour tenir compte du biais

x = [θ, θ˙]^T

A B

(*)

ΔT

θ˙^gyro(k) = uk

xk =

1 ΔT

0 0

xk−1 +

0

1

uk−1 + wk−1

(19)

• 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[rk] = μ^comp = 0, R = E[r_k²] = σcomp²

(**)

Grâce au système dynamique (*) et (**) ainsi trouvé, on peut écrire 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

(20)

Exemple II: simulation

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 gyro

Estim. Kalman

Derive du gyro

(21)

Exemple III: Projet ANR CityVIP (2009-2011)

“Déplacement sûr de véhicules individuels adaptés à l’environnement urbain” (source: D. Bétaille, IFSTTAR)

• Le milieu urbain cumule:

– l’intérêt d’un positionnement précis et intègre – mais plusieurs difficultés ...

• pour le positionnement GPS:

– les masquages de satellites – les trajets multiples

• pour le positionnement par traitement de vidéos:

– les problèmes d’ombres et d’éblouissements – les problèmes de réflexions

Mais le milieu urbain est fortement structuré et la connaissance d’une carte est d’un précieux secours Problématique de la géolocalisation en ville

(22)

Objectifs du projet CityVIP

• Développer un système de localisation adapté aux véhicules urbains:

– précis (erreur < 1 m à 95%)

– intègre (estimation sûre d’un majorant de l’erreur) – bas coût (utilisation de capteurs bas de gamme)

• Le système de localisation exploite:

– un récepteur GPS bas de gamme (u-blox)

– les codeurs CAN des roues arrière du véhicule – une caméra vidéo N&B standard (Marlin)

– une carte numérique 3D précise

... et on tire le meilleur parti de toutes ces sources d’information (fusion)

(23)

Coopération des modules actualisant la pose

• Intégration de mesures proprioceptives (odométrie)

• Mise en correspondance avec la carte numérique 3D (map matching)

• Utilisation de données satellitaires en couplage lâche ou serré:

⇒ Test préalable de validité

Force du signal Visibilité directe

• Fusion avec la localisation par vision

(24)

Localisation par vision: construction de la base d'amers visuels

Séquence vidéo de référence

Base d'amers visuels géoréférencés (points 3D +

descripteurs associés) Trajectoire de référence

(25)

Localisation par vision: algorithme temps réel

Points d'intérêt sur l'image courante

Base d'amers visuels

Pose du véhicule et incertitude associée

Appariement 3D/2D

(26)

Localisation globale par fusion multi-capteurs

Configuration du véhicule dans le repère local ENU (East-North-Up):

Modèle cinématique 3D du véhicule (non linéaire à temps continu):

⎡

⎢⎢

⎣ x˙ y˙ z˙ ψ˙

θ˙ φ˙

⎤

⎥⎥

⎦

= v

⎛

⎜⎜

⎜⎝

⎡

⎢⎢

⎢⎣

cos ψ sin ψ

θ 0 0 0

⎤

⎥⎥

⎥⎦ +

⎡

⎢⎢

⎢⎣ 0 0 0 1

−φ θ

⎤

⎥⎥

⎥⎦ w

⎞

⎟⎟

⎟⎠

v

où est la vitesse longitudinale et la vitesse angulaire du véhicule (les entrées du système dynamique)

x = [x, y, z, ψ, θ, φ]^T

w x˙ =

.

(27)

Configuration du véhicule:

ψ φ θ

Repère inertiel

Repère du véhicule

v

x = [x, y, z, ψ, θ, φ]^T

Roue

Localisation globale par fusion multi-capteurs

(28)

Intégration dans le véhicule VERT

Mesure de référence des trajectoires

Codeurs de roues Baie d’acquisition

(RT-Maps, Aroccam)

Récepteur GPS bas-coût

(29)

Localisation globale par fusion multi-capteurs et carte: résultats

(30)

Localisation Lot 2

Segment sélectionné dans le map matching Localisation après fusion

lot 1 (carte) / lot 2 (vision)

Localisation globale par fusion multi-capteurs et carte