119

Estimation de l’attitude d’un véhicule à partir d’un système de fusion multi-capteurs

basé sur le filtrage particulaire

Hiba Al-Assaad**, Christophe Boucher**, Ali Daher*, Ahmad Shahin* et Jean-Charles Noyer**

**IUT LCO/ULCO–LISIC (France)*UL (Liban)

Email : hiba.al-assaad@etu.univ-littoral.fr

219

Introduction

• Equation de dynamique

• Equations de mesures GPS, OSM et DEM

Modélisation du problème

• Etape de prédiction

• Etape de correction GPS

• Etape de mise en correspondance OSM

• Etape de triangulation DEM

Estimation de l’attitude du véhicule

• Contexte d'expérimentation

• Estimation de la localisation 3-D

• Estimation de l’attitude

Résultats expérimentaux

Conclusion

Plan

319

Introduction

L'objectif de ce travail est d'estimer la localisation et l'attitude d'unvéhicule terrestre en fusionnant les données GPS, OSM et DEM via unfiltre non linéaire

Ce travail présente les performances en estimation du filtrageparticulaire et des algorithmes probabilistes de mise encorrespondance cartographique pour le suivi d’un véhicule, à l'aide decartes routières numériques afin d'améliorer sa localisation au sol

Nous proposons également une nouvelle méthode de triangulation de données DEM permettant de d’estimer en tout point GPS son élévation MSL correspondante dans le modèle ASTER

Avantages du filtrage particulaire :

- traitement direct des non-linéarités de modèles

- performances en précision d’estimation

419

Modélisation du problème

• Le vecteur d’état est :

• Le mouvement du véhicule est représenté par un ensemble d’équations cinématiques à temps discret :

• Avec :

Equ

atio

n d

e d

ynam

iqu

e d

u

véh

icu

le (

1/2

)

discrétisation des équations de navigation : ∆𝑡 = 1𝑠

bruits blancs gaussiens

519

• En généralisant, on obtient :

Equ

atio

n d

e d

ynam

iqu

e d

u

véh

icu

le (

2/2

)

matrice de dynamique

Modélisation du problème

619

Equ

atio

n d

e m

esu

re d

u

po

siti

on

nem

ent

GP

S

𝑍𝑡+1𝐺𝑃𝑆 =

𝑥𝑡+1𝐺𝑃𝑆

𝑦𝑡+1𝐺𝑃𝑆

𝑧𝑡+1𝐺𝑃𝑆

= ℎ𝐺𝑃𝑆 𝑋𝑡+1 + 𝑉𝑡+1𝐺𝑃𝑆

fonction de mesure GPS (linéaire)

𝑉𝑡+1𝐺𝑃𝑆 ~ 𝑁 0, 𝑅𝑡+1

𝐺𝑃𝑆

Modélisation du problème

719

Equ

atio

n d

e m

esu

re

cart

ogr

aph

iqu

e O

SM

fonction de mesure OSM (non linéaire)

UTM

Modélisation du problème

819

Equ

atio

n d

e m

esu

re d

u

MN

T A

STER

fonction de mesure (non linéaire)UTM

• H est la hauteur entre l’antenne GPS et le sol

• 𝜶𝒕+𝟏 est l’angle de tangage estimé

𝑥𝑡+1𝐷𝐸𝑀 = 𝑥𝑡+1 + 𝒱𝑡+1

𝐷𝐸𝑀,𝑥

𝑦𝑡+1𝐷𝐸𝑀 = 𝑦𝑡+1 + 𝒱𝑡+1

𝐷𝐸𝑀,𝑦

𝑧𝑡+1𝐷𝐸𝑀 = ( 𝑧𝑡+1 −

𝐻

cos 𝛼𝑡+1) + 𝒱𝑡+1

𝐷𝐸𝑀,𝑧

Modélisation du problème

919

Estimation de l’attitude du véhicule

1

2

3

4

5

1019

GP

S re

adin

gs p

roce

ssin

g

Init

ialis

atio

n e

t év

olu

tio

n

des

par

ticu

les

1

• Génération aléatoire de N particules (N = 30000)

avec un poids égal à 1

𝑁:

𝑖=1𝑁 𝑤𝑡

𝑖 = 1• Evolution du vecteur d'état de chaque particule :

𝑋𝑡+1𝑖 = 𝐹 𝑋𝑡

𝑖 + Ω𝑡𝑖

matrice de dynamique bruit blanc gaussien

Estimation de l’attitude du véhicule

1119

GP

S re

adin

gs p

roce

ssin

g

• Nous corrigeons les poids normalisés par la mesure GPS disponible :

• Ensuite, les poids sont normalisés comme suit :

Co

rrec

tio

n à

l’ai

de

du

G

PS

2

𝑤𝑡+1𝑖 =

𝑃 𝑍𝑡+1 𝑋𝑡+1𝑖 )

𝑖=1𝑁 𝑃 𝑍𝑡+1 𝑋𝑡+1

𝑖 )𝑤𝑡𝑖

𝑤𝑡+1𝑖 =

𝑤𝑡+1𝑖

𝑖=1𝑁 𝑤𝑡+1

𝑖

Estimation de l’attitude du véhicule

1219

• Une mesure OSM prédite est calculée :

• Calcul de l'ensemble des projections orthogonales :

• Test du cas d'une route à double sens :

• Calcul de la distance Mahalanobis :

• Minimisation du critère de distance :

• Normalisation des poids

Co

rrec

tio

n à

l’ai

de

de

la

cart

e ro

uti

ère

nu

mér

iqu

e O

SM3

𝑍 𝑡+1 𝑡+1𝑂𝑆𝑀 =

𝑖=1

𝑁

𝑤𝑡+1𝑖 ℎ𝑂𝑆𝑀 (𝑋𝑡+1

𝑖 )

Estimation de l’attitude du véhicule

1319

Co

rrec

tio

n à

l’ai

de

du

m

od

èle

nu

mér

iqu

e d

’élé

vati

on

s A

STER

GD

EM4

• La mesure prédite DEM est calculée par :

• Nous calculons le vecteur normal [𝑛1, 𝑛2] des plans (ABC) et (BCD) :

• Triangulation pour obtenir une mesure d’élévation :

𝑍 𝑡+1 𝑡+1𝐷𝐸𝑀 =

𝑖=1

𝑁

𝑤𝑡+1𝑖 ℎ𝐷𝐸𝑀 (𝑋𝑡+1

𝑖 )

𝐴𝑀 . 𝑛1 = 0 𝐬𝐢 𝑀 ∈ (𝐴𝐵𝐶)

𝐷𝑀 . 𝑛2 = 0 𝐬𝐢 𝑀 ∈ (𝐵𝐶𝐷)

Estimation de l’attitude du véhicule

1419

Esti

mat

ion

des

an

gles

de

tan

gage

et

lace

t5

• Nous estimons le vecteur d'état du véhicule :

• Angle de tangage :

• Angle de lacet:

𝛼𝑡+1 =

𝑖=1

𝑁

𝑤𝑡+1𝑖 arctan(

𝑣𝑡+1𝑧,𝑖

(𝑣𝑡+1𝑥,𝑖 )2+ (𝑣𝑡+1

𝑦,𝑖)2)

𝛽𝑡+1 = −

𝑖=1

𝑁

𝑤𝑡+1𝑖 arctan(

𝑣𝑡+1𝑥,𝑖

𝑣𝑡+1𝑦,𝑖

)

Estimation de l’attitude du véhicule

𝑋 𝑡+1 =

𝑖=1

𝑁

𝑤𝑡+1𝑖 𝑋𝑡

𝑖

1519

Contexte d'expérimentation

• Estimation de la localisation 3-D et de l'attitude d'un véhicule terrestre en fusionnant les données GPS, OSM et DEM

• Récepteur GPS u-blox EVK-6T délivre le positionnement 3-D• Hauteur de l’antenne H=1.55m• Taux de navigation fixé à 1Hz

• Réseau routier OSM

• Modèle numérique d’élévations ASTER GDEM V2

Résultats expérimentaux

1619

Estimation de la localisation 3-D

Erreur 3-D Écarts-types

Positionnement 0,91 m

Vitesse 0,45 m/s

Accélération 0,26 m/s²

Résultats expérimentaux

1719

Écart-type atteint 0,02 rad pour l’angle de tangage

Estimation de l’attitude (1/2)

Résultats expérimentaux

1819

Estimation de l’attitude (2/2)

Résultats expérimentaux

Écart-type atteint 0,13 rad pour l’angle de lacet

1919

Ce travail présente une méthode générale d'estimation de lalocalisation 3-D et de l'attitude basée sur la fusion de données GPS,d'un réseau routier OSM et d'un modèle numérique d’élévationsASTER GDEM2 à travers une modélisation statistique des mesures quiprend en compte leurs différentes précisions

A l'aide du processus de mise en correspondance cartographique etde la méthode TIN, les routes parcourues et les zones DEM sontidentifiées pour corriger l'emplacement au sol et l'altitude MSL duvéhicule

Cette méthode générale peut être utilisée pour intégrer d'autrestypes de données telles que des mesures inertielles pour obtenir uneestimation complète de l'attitude du véhicule qui revêt uneimportance significative comme entrée primaire dans les systèmesde gestion de l'énergie des VEH

Conclusion