Reconstitution de létat dun micro drone par fusion de données Travail de stage DEA Joan Solà Ortega, stagiaire Yves Brière, responsable à lENSICA Département

Reconstitution de l’état d’un micro drone par fusion de données

Travail de stage DEA

Joan Solà Ortega, stagiaireYves Brière, responsable à l’ENSICA

Département Avionique et Systèmes - ENSICA

ECOLE DOCTORALE SYSTÈMESDEA Systèmes AutomatiquesAnnée universitaire 2002-2003

Reconstitution de l'état d'un micro drone par fusion de données

2

1 Le projet Micro Drone

Pas de pilote

Gyromètres peu précis

Puissance de calcul limitée

Dynamique très rapide


3

2 Le problème à résoudre

• Ce que l’on veut : Les états à observer

– Localisation dans l'espace : r (m)– Vitesse de translation : v (m/s)– Accélération : a (m/s2)– Orientation : q (quaternion)– Vitesse d'orientation : (rad/s)


4


• Ce que l’on a : Les capteurs embarqués– L'accéléromètre 3D ac

– Le gyromètre 3D c

– Le magnétomètre 3D mc

– Le récepteur GPS rc , vc

GPS

Acc

Gyro

Mag

Pré

cisi

on

Dynamique


5


• Ce qu’il faut savoir : Les systèmes de référence

• La référence ECEF– GPS

• La référence NED– État : r, v, a, q

• La référence avion– Acc, Mag, Gyro

– État :

zavion

xavion

yavion

Greenwich

Equateur

Pôle Nord

zECEF

yECEF

xECEF

NE

D


6


• Ce qu’il faut avoir : Les Idées !

Dynamique Relative

Statique

Absolue

Orientation

Localisation

Acc

GyroAcc + Mag

GPS

FUSION DE FUSION DE DONNÉESDONNÉES


7

3 La solution : Kalman étendu

• Le modèle cinématique d'état

• Séparation en sous systèmes– Le sous système de

localisation – Le sous système

d'orientation

½

0

0

0

0

e

e

e

T

T

T

p q r

p r q

q r p

r q p

r

v

a

q

ω

r r v v

v v a v

a a v

q q Ω q v

ω ω v

Ω


8


• Les modèles des mesures GPS– Amplitude réelle de l’état– Rotation constante– Translation constante

0

0

0

ECEF ECEF NED

ECEF ECEF NED

ECEF ECEF NED

ECEF

ECEF

ECEF

rx x xx xy xz x x

ry y yx yy yz y y

rz z zx zy zz z z

x xx xy xz

y yx yy yz

z zx zy zz

r r c c c r

r r c c c r

r r c c c r

v c c c

v c c c

v c c c

NED

NED

NED

vx x

vy y

vz z

v

v

v


9


• Les modèles des mesures Acc, Mag et Gyro– Amplitude réelle (m, ou bien a+g)– Changement de repère variable (pas pour le Gyro)– Biais– Gains: K– Désalignements: L

cx x xy xz xx xy xz x x x

cy yx y yz yx yy yz y x y

cz zx zy z zx zy zz z x z

m k s s c c c m v

m s k s c c c m v

m s s k c c c m v

avionNEDC q

S = K+L


10


• Augmentation du vecteur d’état avec les incertitudes des capteurs

Symbole r v a q Sg Sa Sm

Dimension 3x1 3x1 3x1 4x1 3x1 3x1 3x3 3x1 3x3 3x1 3x3

Sous vecteur

3x1 3x1 3x1 4x1 3x1 3x1 9x1 3x1 9x1 3x1 9x1

Indices 1-3 4-6 7-9 10-13 14-16 17-19 20-28 29-31 32-40 41-43 44-52

MICRO drone ??Incertitudes : mouvements browniens


11

4 Les astuces

• Observateurs de poursuite: état du drone plus biais du gyromètre. – Algorithmes embarqués différenciés pour la localisation et

l’orientation.– États à estimer :

Symbole r v a q

Dimension 3x1 3x1 3x1 4x1 3x1 3x1

Sous vecteur

3x1 3x1 3x1 4x1 3x1 3x1

Indices 1-3 4-6 7-9 1-4 5-7 8-10

MICRO drone !!

9+10=19


12

4 Les astuces

• Traitement des mesures asynchrones

ˆ

ˆ ˆ ˆ

ii

TiT

i i i

i

i i

h

y h

x

Hx

PHK

H PH r

P I KH P

x x K x

Nouvelle mesure ?

NON

OUI

Boucle de prédiction synchrone à 40 ms

Boucle de correctionà l’arrivée de chaque

mesure

ˆ

ˆ ˆ

T

x

φF

x

P F P F Q

x φ x

ˆ

ˆ ˆ ˆ

ii

TiT

i i i

i

i i

h

y h

x

Hx

PHK

H PH r

P I KH P

x x K x


13

4 Les astuces

• Ne pas utiliser les mesures avec peu d’information– Pour des angles

d’inclinaison petitsles composantesverticales de m et gne varient pas beaucoup.

– L’orientation est bienobservable sans cesmesures zavion

xavion

yavion

gm


14

5 Les résultats

• Sous système d’orientation

– Calibrage du gyromètre: gains et désalignements (19 états)

– Poursuite avec estimation du biais (10 états). Recherche d’un jeu de mesures réduit.

– Données artificielles et réelles


15

5 Les résultats

• Calibration. Données artificielles Gxyz Axyz Mxyz

0 5 10 15 20 25 30 35 40 45-1

0

1Attitude

0 5 10 15 20 25 30 35 40 45

-2

0

2

Angular rates

0 5 10 15 20 25 30 35 40 45

0

0.2

0.4Gyro drifts

0 5 10 15 20 25 30 35 40 45

0

0.5

1

1.5Gyro gains and disalignements


16

5 Les résultats

• Calibration. Données réelles Gxyz Axyz Mxyz

0 50 100 150-1

0

1Attitude

0 50 100 150

-2

0

2

Angular rates

0 50 100 150

0

0.05

0.1

0.15

Gyro drifts

0 50 100 1500

0.5

1

Gyro gains and disalignements


17

5 Les résultats

• Poursuite. Données artificielles Gxyz Axy Mxy

0 5 10 15 20 25 30 35 40 45-1

0

1Attitude

0 5 10 15 20 25 30 35 40 45

-2

0

2

Angular rates

0 5 10 15 20 25 30 35 40 45-1

0

1Gyro drifts

0 5 10 15 20 25 30 35 40 45

0.01

0.02

0.03

Variances


18

5 Les résultats

• Poursuite. Données réelles Gxyz Axy Mxy

0 10 20 30 40 50 60 70-1

0

1Attitude

0 10 20 30 40 50 60 70

-2

0

2

Angular rates

0 10 20 30 40 50 60 70

0

0.1

0.2

0.3

Gyro drifts

0 10 20 30 40 50 60 70

5

10

15

x 10-3 Variances


19

Conclusions

• Une méthode optimale pour l’observation de l’état du micro drone a été développée

• Des méthodes simplifiées ont été conçues pour être embarquées

• L’observabilité peut être explorée pour différentes configurations de mesures

• La validation expérimentale de ces méthodes par le système embarqué est en cours et en bonne voie !


20

Merci!

Documents

Reconstitution de létat dun micro drone par fusion de données Travail de stage DEA Joan Solà Ortega, stagiaire Yves Brière, responsable à lENSICA Département