Estimation d`état pour les drones aériens

Estimation d’état pour les drones aériens
Philippe Martin1
1 Centre
Erwan Salaün2
Automatique et Systèmes
MINES ParisTech
2 Decision and Control Laboratory
School of Aerospace Engineering
Georgia Institute of Technology
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
L’estimation d’état, un problème nouveau ?
Estimer l’état (orientation, vitesse,...) d’un engin volant à partir
de mesures fournies par plusieurs capteurs
Motivations pratiques :
commande en boucle fermée d’un mini-drone aérien
(taille ≤ 70cm, poids ≤ 1kg)
mise à jour ≈ 100Hz, temps de latence ≤ qques ms
puissance de calcul limitée (micro-contrôleur sans FPU)
poids limité (système d’avionique complet < 100g)
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Drone aérien ?
Drone aérien (voilure fixe ou tournante) ≈ “corps rigide"
··· = ···
(relation cinématique translation)
V̇ = A + q ∗
1
q∗ω
2
··· = ···
q̇ =
f
∗ q −1
m
(équation des forces)
(relation cinématique rotation)
(équation des moments)
q orientation du corps (quaternion)
ω vitesse angulaire (dans repère avion)
V vitesse linéaire du centre de masse (dans repère Terre)
A = (0, 0, g)T gravité projetée (dans repère Terre)
f forces aérodynamiques (dans repère avion)
Terre plate, sans rotation, repère galiléen ; gravité constante
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Capteurs inertiels
Mesure fournie par gyroscope triaxial idéal au point P :
ω, vitesse angulaire dans repère avion
Mesure fournie par accéléromètre triaxial idéal au point P :
a = q −1 ∗ (A − V̇P ) ∗ q, accélération spécifique dans rep. avion
En particulier a =
f
m
si P centre de masse
Modèle “générique”
1
q∗ω
2
V̇ = A + q ∗ a ∗ q −1
q̇ =
(relation cinématique rotation)
(définition accélération spécifique)
Deux catégories de capteurs inertiels :
les capteurs de classe inertielle, et les autres (dont MEMS)...
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Autres capteurs
Mesure fournie par magnétomètre triaxial idéal au point P :
q −1 ∗ B ∗ q, champ magnétique terrestre dans repère avion
B varie avec latitude ; mesure facilement perturbée !
GPS :
mesures brutes : “pseudo-distance” et décalage Doppler
“solutions de navigation” : position (bof) et vitesse (super)
il faut “voir” au moins 4 satellites
capteur de pression, télémètre(s), systèmes de vision
(anémomètre(s), radar Doppler : pas pour mini-drones)
inclinomètre : ça n’existe pas !
Conclusion : les capteurs inertiels sont importants
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Estimateur, observateur, etc.
Modèle du système étudié
ẋ = f (x, u)
y (t), u(t) connus à tout instant t
y = h(x, u)
état x(t) à estimer
Observateur : “filtre estimateur récursif sans déphasage”
ż = F(z, u, y )
x̂ = G(z)
G 0 (z) · F z, u, h G(z), u = f G(z), u
pour tous z, u
x̂(t) → x(t) quand t → +∞ (estimation asymptotique)
Réécriture (localement) équivalente
x̂˙ = f (x̂, u) + g(x̂, u, y , P)
avec g x̂, u, h(x̂, u), P = 0
Ṗ = G(x̂, u, y , P)
Philippe Martin et Erwan Salaün
mise à jour des paramètres
Estimation d’état pour les drones aériens
Convergence(s) : x̂(t) → x(t) ?
(cg) convergence globale : toute condition initiale (x0 , u0 , x̂0 , P0 )
(cltt) convergence locale autour de toute trajectoire : toute
CI
(x0 , u0 , x̂0 ≈ x0 , P0 ) (et P0 tq G x̂0 , u0 , h(x0 , u0 ), P0 ≈ 0)
(cltpe) convergence locale autour de tout point d’équilibre : toute
CI (x0 , u0 ; x̂0 , u0 , P0 ) ≈ (x̄, ū, x̄, P̄) avec (x̄, ū, P̄)
quelconque tq f (x̄, ū) = 0 et G x̄, ū, h(x̄, ū), P̄ = 0
(cl1pe) convergence locale autour d’un point d’équilibre : toute CI
(x0 , u0 ; x̂0 , u0 , P0 ) ≈ (x̄, ū, x̄, P̄) avec (x̄, ū, P̄) donné tq
f (x̄, ū) = 0 et G x̄, ū, h(x̄, ū), P̄ = 0
Observateur : un problème largement ouvert...
(cltt) : serait déjà très bien en pratique !
(cltpe) : ce qu’on sait faire en toute généralité
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Conditions à vérifier par le système
Observabilté : relation entre l’état et les signaux connus
Système observable (localement génériquement) ssi
x = M(y , ẏ , ÿ , · · · , u, u̇, ü, · · · )
formule inutilisable en pratique
observateur : “implémentation asymptotique sans dériver“
Si système pas observable, on peut (localement) le réécrire :
ẋ1 = f1 (x1 , x2 , u)
partie inobservable
ẋ2 = f1 (x2 , u)
partie observable
y = h(x2 , u)
Seule façon d’estimer x1 : recopie du système... si stabilité
x̂˙ = f (x̂ , x̂ , u)
(on suppose x̂ connu)
1
1
1
2
Philippe Martin et Erwan Salaün
2
Estimation d’état pour les drones aériens
Intermède : le cas linéaire stationnaire
ẋ = Ax + Bu
y , u signaux connus
y = Cx + Du
Observateur à gains constants
x̂˙ = Ax̂ + Bu + L y − (C x̂ + Du)
L matrice à choisir
convergence arbitrairement rapide ssi (A, C) observable
réglage judicieux : compromis confiance modèle/mesure
Filtre de Kalman : compromis “optimal”
x̂˙ = Ax̂ + Bu + PC T (NN T )−1 y − (C x̂ + Du)
Ṗ = AP + PAT + MM T − PC T (NN T )−1 CP
M, N matrices de “réglage” (stochastique ou déterministe)
convergence si (A, C) et (AT , M T ) observables
pas très intéressant en linéaire...
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Facile en linéaire, dur en non-linéaire : pourquoi ?
Equation de l’erreur e := x̂ − x en linéaire :
ė = (A − LC)e
Ṗ = G(P)
Ne dépend pas de la trajectoire suivie !
Equation de l’erreur e := x̂ − x en non-linéaire :
ė = η(x̂, u, P)
Ṗ = G x̂, u, h(x̂ − e, u)
x̂˙ = f (x̂, u) + g x̂, u, h(x̂ − e, u), P
dépend de la trajectoire suivie !
quand on linéarise, matrices instationnaires
seulement conv. locale autour de tout point d’équilibre
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Observateurs dans le cas non-linéaire (1/2)
Observateur à gains "schedulés”
x̂˙ = f (x̂, u) + L(x̂, u, y ) · y − h(x̂, u)
En posant A := ∂1 f (x̂,u) et C := ∂1 h(x̂, u), l’équation
d’erreur
linéarisée s’écrit ė ≈ A − L x̂, u, h(x̂, u) C e, donc :
si (x̂, u) toujours proche de point d’éq, “facile” de choisir L
seulement conv. locale autour de tout point d’équilibre
Filtre de Kalman étendu (Extended Kalman Filter, EKF)
x̂˙ = f (x̂, u) + PC T (NN T )−1 · y − h(x̂, u)
Ṗ = AP + PAT + MM T − PC T (NN T )−1 CP
OK si (x̂, u) toujours proche de point d’éq (cltpe)
choix M, N ? ; implémentation numérique gourmande
EKF 6= magie
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Observateurs dans le cas non-linéaire (2/2)
Il y a plein de méthodes particulières...
Observateurs invariants : utiliser les symétries
x̂˙ = f (x̂, u) + W (x̂)L I(x̂, u), E(x̂, u, y ) E(x̂, u, y )
L(I, E) matrice à choisir
E(x̂, u, y ) erreur de sortie invariante
W (x̂) = w1 (x̂), .., wn (x̂) base invariante ; I(x̂, u) invariant
construction “explicite” (méthode du repère mobile)
Equation d’erreur “presque” autonome si “assez” de symétries
η̇ = Υ(η, I)
η erreur d’état invariante
mieux que cltpe
réglage “naturel”
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Drones aériens : estimateur générique ou spécifique ?
Générique : pas besoin de modèle des forces
1
q∗ω
2
V̇ = A + q ∗ a ∗ q −1
q̇ =
ω, a “entrées” mesurées
S’applique à tout drone, mais pour estimation “satisfaisante” il
faut mesurer la vitesse...
Spécifique : il faut un modèle des forces
q̇ =
1
q∗ω
2
ω “entrée” mesurée
f (V , q)
f (V , q)
∗ q −1
a=
“sortie” mesurée
m
m
bonne connaissance de l’aérodynamique de l’engin
V̇ = A + q ∗
spécifique du drone considéré...
accéléromètre au centre de masse
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Estimateur générique avec juste capteurs inertiels ?
Non avec modèle terre plate...
Système complètement inobservable et instable :
l’erreur d’orientation est constante
l’erreur de vitesse horizontale diverge linéairement
l’erreur de vitesse verticale est constante
Encore pire avec biais de capteurs !
Oui avec modèle terre “ronde” et “excellents” capteurs = Nav In
Système complètement inobservable, mais oscillant :
l’erreur d’orientation oscille (autour de 0) avec Ts ≈ 85min
l’erreur de vitesse horizontale oscille avec Ts ≈ 85min
l’erreur de vitesse verticale diverge avec Ts /2π ≈ 14min
Reste vrai même avec biais de capteurs
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
La navigation inertielle du pauvre : AHRS
Avec capteurs MEMS, il faut renoncer à estimer la vitesse...
Approximation “classique” V̇ ≈ 0
a = q −1 ∗ (V̇ − A) ∗ q ≈ −q −1 ∗ A ∗ q
l’accéléromètre “mesure” les angles de roulis et tangage
pertinence de l’approximation ?
magnétomètre indispensable pour le cap
Modèle :
1
q̇ = q ∗ ω
2
yA
q −1 ∗ A ∗ q
=
.
yB
q −1 ∗ B ∗ q
ω “entrée” mesurée
yA , yB “sorties” mesurées
Observateur invariant marche très bien “en vrai”
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
AHRS “aidé” par mesure de vitesse
3 gyros mesurent ωm := ω + ωb (repère avion)
3 accéléros mesurent am := as a (repère avion)
3 magnétos mesurent le champ magnétique (repère avion)
un module GPS mesure le vecteur vitesse (repère Terre)
Modèle avec capteurs imparfaits
1
q̇ = q ∗ (ωm − ωb )
2
1
V̇ = A + q ∗ am ∗ q −1
as
ω̇b = 0
ȧs = 0
yV
yB
=
V
−1
q ∗B∗q
Philippe Martin et Erwan Salaün
Groupes de symétries “naturel”
  

q̃
q ∗ q0
 Ṽ  

  =  −1 V + V0

ω̃b  q ∗ ωb ∗ q0 + ω0 
0
as a0
−1
ω̃m
q0 ∗ ωm ∗ q0 + ω0
=
ãm
a0 q0−1 ∗ am ∗ q0
yV + V 0
ỹV
=
ỹB
q0−1 ∗ yB ∗ q0
ãs
Estimation d’état pour les drones aériens
Observateur invariant :
1
q̂˙ = q̂ ∗ (ωm − ω̂b ) + (LV EV + LB EB ) ∗ q̂
2
1
˙
V̂ = q̂ ∗ am ∗ q̂ −1 + A + (MV EV + MB EB )
âs
˙ω̂b = q̂ −1 ∗ (NV EV + NB EB ) ∗ q̂
â˙ s = âs (O E + O E )
V
V
B
B
EV
V̂ − yV
=
EB
B − q̂ ∗ yB ∗ q̂ −1
q̂ ∗ (ωm − ω̂b ) ∗ q̂ −1
Iω
=
1
q̂ ∗ am ∗ q̂ −1
Ia
âs
LV , LB , MV , MB , NV , NB , OV , OB matrices de gains à choisir,
dépendant possiblement de EV , EB , Ia , Iw
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Erreur d’état invariante

  
q̂ ∗ q −1
η

ν  
V̂ − V

 =
.

ω  q ∗ (ω̂b − ωb ) ∗ q −1 

âs
α
a
s
Système d’erreur linéarisé autour de (η, ν, ω, α) = (1, 0, 0, 1)
1
δ η̇ = − δω + (LV δEV + LB δEB )
2
δ ν̇ = −2Ia × δη − δαIa + (MV δEV + MB δEB )
δ ω̇ = Iω × δω + (NV δEV + NB δEB )
δ α̇ = (OV δEV + OB δEB )
δEV = δν
δEB = 2B × δη
Convergence locale autour de toute trajectoire tq Ia constant
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Real time implementation on a SMALL processor
8-bit Atmel ATmega128
4ko RAM, 16MHz clock, no FPU
cheap (5$)
update rate 100Hz
crude numerical scheme
C language with emulated
floating point library
the processor handles many
sensor-generated interrupts
(don’t think about EKF)
yet it works fine !
Philippe Martin et Erwan Salaün
Estimation d’état pour les drones aériens
Estimateur spécifique pour quadrotor
Modèle des forces aérodynamiques :
 λ

− m (ω1 + ω2 + ω3 + ω4 )u
f
λ
≈ − m
(ω1 + ω2 + ω3 + ω4 )v 
m
α
− m ω12 + ω22 + ω32 + ω42
u, v composantes de la vitesse dans repère avion
ω1 à ω4 vitesses de rotation des hélices ;
λ, α constantes liées à la géométrie des hélices
un accéléromètre au centre de masse mesure la vitesse !
on peut donc construire un observateur (horizontal)

Modèle “standard” :
f
≈
m
α
−m
Philippe Martin et Erwan Salaün

0

0
2
2
2
2
ω1 + ω2 + ω3 + ω4
Estimation d’état pour les drones aériens