Reconstitution de létat dun micro drone par fusion de données Travail de stage DEA Joan Solà...

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!

Reconstitution de létat dun micro drone par fusion de données Travail de stage DEA Joan Solà...

Documents

Transcript of Reconstitution de létat dun micro drone par fusion de données Travail de stage DEA Joan Solà...