Asservissement LQG drone quadrirotor

From Electrolab
Revision as of 07:49, 6 May 2014 by Mael (Talk | contribs)

Jump to: navigation, search

Présentation

Retour à User:Mael
La page suivante présente quelques travaux sur les méthodes d’asservissement modale et LQG d’un drone quadri rotor. Le principe détaillé ici est uniquement théorique, je n’ai pas réalisé de drone quadri rotor pour mettre en œuvre cette démarche. Cependant, j’ai mis en pratique ces équations pour la réalisation d’un drone hybride à voilure tournante Projets:Perso:2013:Drone hybride. Les algorithmes sont très similaires, avec juste une problématique supplémentaire dans le cas du drone hybride que j’aborderais plus loin. La démarche ainsi que les calculs sont issues de plusieurs livres très complémentaires sur le sujet :

  1. Automatique \ Systèmes linéaires, non linéaires, à temps continus, à temps discret, représentation d'état \ Yves Granjon \ DUNOD : très intéressant pour des débutants en automatique, il repose bien toute la problématique en évoquant simplement la représentation d'état pour finir.
  2. Commande et estimation multivariables \ Méthodes linéaires et optimisation quadratique \ Eric OSTERTAG \ Ellipses : dans la continuité du précédent, on entre cette fois dans le vif du sujet avec beaucoup d'explications et de démonstrations des formules utilisées ici. Il est nécessaire de connaitre assez bien l'automatique avant d'attaquer ce livre.
  3. Automatique appliquée \ Philippe de Larminat \ Hermes Lavoisier : Pour les spécialistes, l'approche est plus ardue mais présente un panel complet de l'automatique tout en allant plus loin que les autres sur les aspects théorie et optimisations.
  4. La commande multivariable \ Application au pilotage d'un avion \ Caroline Bérard - Jean-Marc Biannic - David Saussié \ DUNOD : Cas pratique mis en oeuvre, ce livre est sans doute le plus intéressant pour un ingénieur d'application, car il propose un axe pour la réalisation d'une étude de correcteur modale, LQG et H infini. Cependant, seul il n'est pas suffisant pour bien saisir toutes les subtilités et disposer de tous les outils nécessaires (notamment les observateurs).

En résumé, dans l'étude ci-dessous, il y a des choses piochées dans les 4 livres ci-dessus, principalement 4 pour la méthode et 2 pour les observateurs et intégrateurs, ainsi que le modèle discret. Cependant, étant moi-même ingénieur en automatique, je partais avec un certain background en automatique. Aussi je ne saurais trop conseiller à ceux qui souhaitent approfondir le domaine ou qui ne sont pas du sérail de se procurer les 4, même si j'admet que c'est un investissement certain... (environ 30 Euro chaque).

Modèle

Les données du modèle

Le modèle retenu pour le drone quadri-rotor est le suivant :

Drone quadri.png

Dans ce modèle, les paramètres F1, F2, F3 et F4 sont les efforts des actionneurs (moteurs électriques avec hélice) sur le drone (corps jaune) par rapport au sol. Les angles α et β sont respectivement les rotations de ce même corps par rapport à y et x.

Deux sous-ensembles indépendants et génériques apparaissent dès lors que les angles α et β restent petits :

Modele generic.png

Dans le premier cas, on a :

  • θ = α 𝜃 𝛼 ,
  • F i = F 1 subscript 𝐹 𝑖 subscript 𝐹 1 ,
  • F j = F 2 subscript 𝐹 𝑗 subscript 𝐹 2 ,
  • Sur le plan (x,z)

Et dans le second cas :

  • θ = β 𝜃 𝛽 ,
  • F i = F 3 subscript 𝐹 𝑖 subscript 𝐹 3 ,
  • F j = F 4 subscript 𝐹 𝑗 subscript 𝐹 4 ,
  • Sur le plan (x,z)

L’hypothèse des petits angles permet de négliger le couplage des angles l’un sur l’autre. Elle reste vraie tant que C o s ( θ ) θ 𝐶 𝑜 𝑠 𝜃 𝜃 .

On pose également deux variables complémentaires dont l’utilité sera explicitée plus tard :

  • ϕ 1 = F 1 - F 2 subscript italic-ϕ 1 subscript 𝐹 1 subscript 𝐹 2
  • ϕ 2 = F 3 - F 4 subscript italic-ϕ 2 subscript 𝐹 3 subscript 𝐹 4

Les équations d'état

L’étude se positionne sur le modèle générique, sachant que la dynamique est la même pour α 𝛼 et β 𝛽 . C’est d’ailleurs pour cette raison que les quadri-rotor en croix sont les plus simples à asservir.

Pour lier les efforts F i subscript 𝐹 𝑖 et F j subscript 𝐹 𝑗 des actionneurs à l’angle θ 𝜃 , on applique le principe fondamentale de la dynamique pour les rotations, à savoir :

J y θ ¨ = M ( F i ) i - M ( F j ) j subscript 𝐽 𝑦 ¨ 𝜃 𝑀 subscript subscript 𝐹 𝑖 𝑖 𝑀 subscript subscript 𝐹 𝑗 𝑗

Le Principe Fondamental de la Dynamique s’applique au centre de gravité, qui est également le centre géométrique du drone (point 0), on utilise alors la longueur du bras du drone (a) :

M ( F i ) i = O I F i + M ( F i ) O 𝑀 subscript subscript 𝐹 𝑖 𝑖 𝑂 𝐼 subscript 𝐹 𝑖 𝑀 subscript subscript 𝐹 𝑖 𝑂 Sachant que le moment de F i subscript 𝐹 𝑖 au point O est nul, on peut poser : J y θ ¨ = a . F i - a . F j formulae-sequence subscript 𝐽 𝑦 ¨ 𝜃 𝑎 subscript 𝐹 𝑖 𝑎 subscript 𝐹 𝑗 Et: ϕ n = F i - F j subscript italic-ϕ 𝑛 subscript 𝐹 𝑖 subscript 𝐹 𝑗 D'ou : J y θ ¨ = a . ϕ n formulae-sequence subscript 𝐽 𝑦 ¨ 𝜃 𝑎 subscript italic-ϕ 𝑛 On asservit sur θ 𝜃 à partir de ϕ italic-ϕ car seul ϕ italic-ϕ peut être asservit par rapport θ 𝜃 . En effet, si l’on conserve F i subscript 𝐹 𝑖 et F j subscript 𝐹 𝑗 , il existe une infinité de solution à l’équation (1.3). En effet quel que soit F i subscript 𝐹 𝑖 , on peut trouver un F j subscript 𝐹 𝑗 qui permette d’atteindre l’angle θ 𝜃 de consigne. Ainsi, on utilise la variable ϕ italic-ϕ qui représente l’écart de poussée des deux actionneurs, qui est réellement la variable de contrôle de l’angle. Afin de prendre en compte un modèle proche de la réalité, on va utiliser un modèle du premier ordre pour représenter le moteur : F i ( p ) U i ( p ) = K i 1 + τ i . p subscript 𝐹 𝑖 𝑝 subscript 𝑈 𝑖 𝑝 subscript 𝐾 𝑖 formulae-sequence 1 subscript 𝜏 𝑖 𝑝

Dans ce modèle, K représente le gain de commande entre la commande (entre 0 et 300 pour cet exemple) et la poussée (en Newton) de chaque actionneur.

τ i subscript 𝜏 𝑖 représente le retard de la commande. En effet, entre l'application de la commande et l’atteinte de cette commande par l'actionneur, il va se dérouler un certain temps associée à la dynamique du système.

Dans le cas où les actionneurs F i subscript 𝐹 𝑖 et F j subscript 𝐹 𝑗 sont identiques, avec la même dynamique, il est possible de considérer que la variable θ 𝜃 évolue dans les mêmes conditions, et de poser :

ϕ n ( p ) U n ( p ) = K i , j 1 + τ i , j . p subscript italic-ϕ 𝑛 𝑝 subscript 𝑈 𝑛 𝑝 subscript 𝐾 𝑖 𝑗 formulae-sequence 1 subscript 𝜏 𝑖 𝑗 𝑝

Dans lequel U n subscript 𝑈 𝑛 est la différence entre les commandes U i subscript 𝑈 𝑖 et U j subscript 𝑈 𝑗 . La commande de chaque actionneur pourra être quelconque, la consigne sur θ 𝜃 sera atteinte dès lors que l’écart de commande U n subscript 𝑈 𝑛 entre U i subscript 𝑈 𝑖 et U j subscript 𝑈 𝑗 sera respecté.

On posera donc comme équations d'état les deux équations suivantes :

θ ¨ = a J y × ϕ n ¨ 𝜃 𝑎 subscript 𝐽 𝑦 subscript italic-ϕ 𝑛 ϕ ˙ n = - 1 τ i , j × ϕ n + K i , j τ i , j × U n subscript ˙ italic-ϕ 𝑛 1 subscript 𝜏 𝑖 𝑗 subscript italic-ϕ 𝑛 subscript 𝐾 𝑖 𝑗 subscript 𝜏 𝑖 𝑗 subscript 𝑈 𝑛

Etude dans le domaine temporel

Commande modale

La commande par retour d’état est une commande dans laquelle l’entrée du système est reliée à sa sortie par un paramètre fixe, appelé ici K c subscript 𝐾 𝑐 et en règle générale vectoriel ou matriciel, dont les valeurs sont associées à certaines caractéristiques attendues du système.

Avant de commencer, on pose le modèle sous sa forme d’état :

X ˙ = A . X + B . U formulae-sequence ˙ 𝑋 𝐴 𝑋 𝐵 𝑈 Y = C . X formulae-sequence 𝑌 𝐶 𝑋 La variable X représente l’état du système, soit un ensemble de variables qui le caractérise dans le temps, et les matrice A, B et C les matrices du modèle qui le font évoluer dans le temps. X = | α α ˙ ϕ 1 β β ˙ ϕ 2 | 𝑋 𝛼 ˙ 𝛼 subscript italic-ϕ 1 𝛽 ˙ 𝛽 subscript italic-ϕ 2 La matrice A est composée deux deux blocs identiques, en α 𝛼 et β 𝛽 , appelés A α subscript 𝐴 𝛼 et A β subscript 𝐴 𝛽  : A α / β = [ 0 1 0 0 0 a J y 0 0 - 1 τ n ] subscript 𝐴 𝛼 𝛽 0 1 0 0 0 𝑎 subscript 𝐽 𝑦 0 0 1 subscript 𝜏 𝑛

dans lequel n vaut 1 pour α 𝛼 et 2 pour β 𝛽 . D'où :

A = [ A α z e r o s ( 3 , 3 ) z e r o s ( 3 , 3 ) A β ] 𝐴 subscript 𝐴 𝛼 𝑧 𝑒 𝑟 𝑜 𝑠 3 3 𝑧 𝑒 𝑟 𝑜 𝑠 3 3 subscript 𝐴 𝛽

La matrice B assure le lien entre les consignes des actionneurs et l'état :

B = [ 0 0 0 0 K 1 τ 1 0 0 0 0 0 0 K 2 τ 2 ] 𝐵 0 0 0 0 subscript 𝐾 1 subscript 𝜏 1 0 0 0 0 0 0 subscript 𝐾 2 subscript 𝜏 2

Enfin, la matrice C filtre dans le vecteur d'état les variables accessibles en sorties, via les capteurs :

C = [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 ] 𝐶 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0

Le vecteur de sortie Y est donc constitué des angles et vitesses angulaires autour des axes x et y.

On négligera d’entrée le lien direct entre la sortie et l’entrée, représenté par la matrice D. En règle générale il vaut mieux faire comme si on ne l'avait pas vu.

Systeme.png

Le correcteur modal

Le correcteur modal est une matrice de gain qui permet, dans le modèle en boucle fermée suivant, de positionner les valeurs propres de la matrice d’état du modèle bouclé. En posant :

U = - K c . X formulae-sequence 𝑈 subscript 𝐾 𝑐 𝑋

On obtient le modèle en boucle fermée :

X ˙ = ( A - K c . B ) . X fragments ˙ 𝑋 fragments ( A subscript 𝐾 𝑐 . B ) . X

Dans ce modèle, le choix des valeurs propres de la matrice ( A - K c . B ) formulae-sequence 𝐴 subscript 𝐾 𝑐 𝐵 permet d’obtenir la convergence de l’état X vers son état stable :

  • Dans un temps minimum,
  • Avec un minimum de dépassement.

Le calcul de K c subscript 𝐾 𝑐 se base sur un vecteur propre dont les différentes valeurs sont associées aux variables de l’état. Les valeurs propres choisies, dans le cas temporel, doivent être négatives et à partie imaginaire dans la zone ci-dessous :

Zone VP.png

Les valeurs propres du système, choisies "judicieusement", permettent de caractériser le comportement du système. La zone ci-dessus est la zone optimale dans laquelle on obtient les meilleures performances pour le système. On pose le vecteur V c subscript 𝑉 𝑐 tel que :

V c = [ λ α λ α ˙ λ ϕ 1 λ β λ β ˙ λ ϕ 2 ] subscript 𝑉 𝑐 subscript 𝜆 𝛼 subscript 𝜆 ˙ 𝛼 subscript 𝜆 subscript italic-ϕ 1 subscript 𝜆 𝛽 subscript 𝜆 ˙ 𝛽 subscript 𝜆 subscript italic-ϕ 2

Le calcul de la matrice de retour d'état Kc se fait, en utilisant Matlab, par la fonction :

K c = p l a c e ( A , B , V c ) subscript 𝐾 𝑐 𝑝 𝑙 𝑎 𝑐 𝑒 𝐴 𝐵 subscript 𝑉 𝑐

Je ne rentrerais pas ici dans le détail de la fonction <place>. Elle sera étudiée plus tard pour les propriétés de découplage.

Correcteur simple.png

Le paramètre K c subscript 𝐾 𝑐 est utilisé uniquement pour asservir le modèle autour d'un point d'équilibre, ici en l’occurrence 0. Par défaut, le point d'équilibre est 0, sauf si une matrice de préfiltre, appelée M, permet d'intégré une constante dans le retour d'état, "décalant" l'asservissement vers un autre point d'équilibre et permettant ainsi de piloté le système. Je ne détaillerais pas ici le calcul de H puisque j'utiliserais la méthode des intégrateurs pour piloter le système et en profiter pour annuler l'erreur statique.

L'observateur modal

L’observateur est nécessaire car, même si les valeurs α , α ˙ , β , β ˙ 𝛼 ˙ 𝛼 𝛽 ˙ 𝛽 sont accessibles grâce à la centrale inertielle, il reste à estimer les variables F 1 , F 2 , F 3 subscript 𝐹 1 subscript 𝐹 2 subscript 𝐹 3 et F 4 subscript 𝐹 4 , c’est-à-dire ϕ 1 subscript italic-ϕ 1 et ϕ 2 subscript italic-ϕ 2 .

L’observateur assure le calcul de X ^ ^ 𝑋 , qui est une estimation de X. La commande sera basée sur ce X ^ ^ 𝑋 , et non sur le X qui est inaccessible en totalité.

Observateur.png

L’observateur est alimenté par les sorties du modèles, c’est-à-dire les données issues de la centrale inertielle, et par la commande courante du modèle.

L’observateur est une image du modèle réelle dont on va pondérer l’influence de la boucle fermé, en utilisant une matrice G pour corriger l’état reconstruit à partir des valeurs accessibles. Un choix de valeurs propres du système corrigé par G permet de s’assurer que :

  • L’observateur converge vers le modèle réel,
  • La dynamique de convergence est plus rapide pour l’observateur que pour le modèle réel, sans quoi un retard de commande apparaitrait dans le système.

Le calcul de la matrice de gain de l’observateur se fait, par analogie avec la matrice du correcteur, par la fonction « place » de Matlab :

G c = p l a c e ( A , C , V o ) subscript 𝐺 𝑐 𝑝 𝑙 𝑎 𝑐 𝑒 superscript superscript 𝐴 superscript 𝐶 subscript 𝑉 𝑜

On utilise cependant la transposée de A au lieu de A, la transposée de C au lieu de B, et le vecteur propre V o subscript 𝑉 𝑜 des valeurs propres choisies pour l’observateur.

Les valeurs propres V o subscript 𝑉 𝑜 doivent être choisies à gauche (ie plus négative) :

λ o b s e r v a t e u r < λ c o r r e c t e u r subscript 𝜆 𝑜 𝑏 𝑠 𝑒 𝑟 𝑣 𝑎 𝑡 𝑒 𝑢 𝑟 subscript 𝜆 𝑐 𝑜 𝑟 𝑟 𝑒 𝑐 𝑡 𝑒 𝑢 𝑟

En moyenne, une valeur propre entre 1,2 et 2 fois inférieure est une bonne estimation pour démarrer.

L'intégrateur modale

Le système augmenté permet d’ajouter deux variables supplémentaires qui sont l’intégrale de α et β. Ces deux variables supplémentaires seront utilisées pour l’étage d’intégration assurant la correction de l’erreur statique.

X ˙ e = A a . X e + B a . U formulae-sequence subscript ˙ 𝑋 𝑒 subscript 𝐴 𝑎 subscript 𝑋 𝑒 subscript 𝐵 𝑎 𝑈 Y = C a . X e formulae-sequence 𝑌 subscript 𝐶 𝑎 subscript 𝑋 𝑒 Le système ainsi augmenté deviens : Systeme augmente.png Les matrices A, B et C sont modifiées pour ajouter les paramètres d'intégration : A a = [ A z e r o s ( 6 , 2 ) - S i . C z e r o s ( 2 , 2 ) ] subscript 𝐴 𝑎 𝐴 𝑧 𝑒 𝑟 𝑜 𝑠 6 2 formulae-sequence subscript 𝑆 𝑖 𝐶 𝑧 𝑒 𝑟 𝑜 𝑠 2 2 B a = [ B z e r o s ( 2 , 6 ) ] subscript 𝐵 𝑎 𝐵 𝑧 𝑒 𝑟 𝑜 𝑠 2 6 C a = [ C z e r o s ( 4 , 2 ) ] subscript 𝐶 𝑎 𝐶 𝑧 𝑒 𝑟 𝑜 𝑠 4 2 La matrice S i subscript 𝑆 𝑖 permet de filtrer le vecteur Y pour en extraire α 𝛼 et β 𝛽 , qui seront réinjectés dans l'étage d'intégration. Correcteur.png L'erreur statique entre les variables α 𝛼 et β 𝛽 et leur consigne est intégrée, puis réintégrée à la commande U par une matrice de retour d'état K 2 subscript 𝐾 2 . Cette matrice de retour d'état est en réalité une sous-partie d'une matrice de correcteur K a subscript 𝐾 𝑎 étendue, recalculée sur la base du modèle augmenté. Elle est calculée de manière similaire à la matrice K c subscript 𝐾 𝑐 vue précédemment, à partir des matrices A a subscript 𝐴 𝑎 et B a subscript 𝐵 𝑎 , ainsi que d'un vecteur λ c subscript 𝜆 𝑐 augmenté de deux valeurs propres, toujours négatives, pour les variables intégrales de α 𝛼 et β 𝛽 .

Le modèle modale complet

Le schéma ci-dessous résume les 3 étages du l'asservissement modale :

  • Le système augmenté,
  • L'observateur pour reconstruire l'état,
  • Le correcteur avec intégrateur.

Modele complet.png

Les paramètres w1 et w2 sont une représentation (pour l'instant symbolique) des perturbations sur le modèle. La modélisation nécessite de faire des hypothèses, qui engendrent nécessairement des erreurs de modélisation. Le système ne réagit pas exactement comme il est modélisé. A noter, on le verra plus tard, il y a aussi un bruitage à prendre en compte sur la sortie (sommé à Y).

Le problème de cette commande modale, c'est que l'on a fixé la dynamique du système avec le choix des valeurs propres de chacun des paramètres d'état, mais que cette dynamique engendre en général deux défauts majeurs :

  • Les consignes transmises aux actionneurs sont trop fortes,
  • Le système réagit beaucoup trop fortement à la moindre perturbation.

Dernier point, et non des moindres, on ne pourra rien asservir avec un modèle en continu, il faudra passer sur du discret avec toutes les pertes de performances associées à un correction discrète (donc tous les dt secondes) au lieu d'une correction continue. Il reste du boulot...

Commande LQ

La commande LQ (Linéaire Quadratique) permet de pondérer les efforts d'asservissement entre l'amplitude des commandes et la dynamique du système. Dit autrement, il est possible d'agir sur le système asservit par deux moyens différents :

  • Augmenter la vitesse de réaction du système, c'est à dire qu'il atteindra sa consigne plus rapidement mais avec des consignes souvent irréaliste pour des systèmes physiques classiques,
  • Diminuer les consignes du système, afin de les rendre compatibles avec les capacités physiques des actionneurs, au détriment de la vitesse du système.

A noter, la synthèse modale d'un retour d'état va déjà permettre de fixer les dépassements et la vitesse globale du système, aussi va-t-on plutôt agir dans le sens d'une diminution des niveaux de consignes pour les rendre compatibles avec les actionneurs.

Initialisation du retour d'état

Le calcul d'une matrice de retour d'état de type LQ se base sur deux paramètres matriciels :

  • Matrice Q : matrice de pondération de la dynamique du système. Chaque paramètre dans la diagonale de cette matrice permet, s'il augmente, d'accroître la vitesse de convergence vers sa consigne du paramètre d'un vecteur d'état (ième ligne du vecteur d'état avec ième ligne de la matrice).
  • Matrice R : matrice de pondération de l'amplitude de la commande. Chaque paramètre dans la diagonale de cette matrice permet, s'il augmente, de diminuer l'amplitude de la commande associée dans le vecteur de commande (ième ligne du vecteur de commande avec ième ligne de la matrice).

Ainsi, la matrice Q est une matrice carrée de dimension n x n (n dimension du vecteur d'état) et R une matrice carrée de dimension p x p (p dimension du vecteur de commande). Q doit doit être définie positive (ie au moins un paramètre dans la diagonale différent de 0, et tous positifs) et R simplement positive (s'il y a des paramètres, ils sont positifs).

Dans la pratique, il revient à jouer les apprentis sorciers d'essayer de trouver "à l'oeil" des paramètres pour Q et R qui donnent au système le comportement attendu. Il existe plusieurs méthodes pour y arriver (cf référence 3), j'en propose une issue du livre sur la commande multivariables (référence 4), qui consiste à repartir du correcteur modale et jouer ensuite sur le paramètre R pour limiter l'amplitude des commandes.

On va donc repartir du correcteur K a subscript 𝐾 𝑎 calculé pour le système augmenté, et créer les matrices Q et R à partir de là :

Q = ( K a . K a ) fragments Q fragments ( superscript subscript 𝐾 𝑎 . subscript 𝐾 𝑎 )

R = e y e ( p ) 𝑅 𝑒 𝑦 𝑒 𝑝

Le correcteur calculé à partir de ces deux matrices Q et R, donnera au système la même dynamique que dans le cadre du correcteur modale. On le constate d'après les équations : on positionne la pondération sur les variables d'état en fonction du correcteur K a subscript 𝐾 𝑎 , pour avoir une dynamique équivalente, mais on ne touche pas aux paramètres de commandes ("eye(n)" est un fonction Matlab construisant une matrice unité de dimension n x n, c'est à dire carrée avec des 0 partout sauf dans la diagonale ou l'on a que des 1).

Cependant, désormais il est possible de distinguer les pondérations "dynamique du système" des pondérations "commande". Il est donc plus facile de paramétrer le système.

Paramétrage du retour d'état

Etude dans le domaine discret

Initialisation à partir du modèle temporel

Choix des valeur propres discrètes (modale)

Choix des matrices LQ discrètes

Calcul du filtre de Kalman steady state