View source for Asservissement LQG drone quadrirotor

== Présentation ==
Retour à [[User:Mael]]<br />
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 : 
# '''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.
# '''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.
# '''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.
# '''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 :

[[File: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 :

[[File:Modele_generic.png]]

Dans le premier cas, on a : 
*<math>\theta = \alpha</math>, 
*<math>F_{i} = F_{1}</math>, 
*<math>F_{j} = F_{2}</math>,
*Sur le plan (x,z)
Et dans le second cas : 
*<math>\theta = \beta</math>,
*<math>F_{i} = F_{3}</math>, 
*<math>F_{j} = F_{4}</math>,
*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 <math>Sin(\theta) \backsimeq \theta</math>.

On pose également deux variables complémentaires dont l’utilité sera explicitée plus tard : 
*<math>\phi_{1}=F_{1}-F_{2}</math>
*<math>\phi_{2}=F_{3}-F_{4}</math>

=== Les équations d'état ===
L’étude se positionne sur le modèle générique, sachant que la dynamique est la même pour <math>\alpha</math> et <math>\beta</math>. C’est d’ailleurs pour cette raison que les quadri-rotor en croix sont les plus simples à asservir.

Pour lier les efforts <math>F_{i}</math> et <math>F_{j}</math> des actionneurs à l’angle <math>\theta</math>, on applique le principe fondamentale de la dynamique pour les rotations, à savoir : 

<math>
{\color{Blue}J_{y}\cdot \ddot{\theta} = \overrightarrow{M}(F_{i})_{i}-\overrightarrow{M}(F_{j})_{j}}
</math>

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) :

<math>
{\color{Blue}\overrightarrow{M}(F_{i})_{i} = \overrightarrow{OI}\wedge\overrightarrow{F}_{i}+\overrightarrow{M}(F_{i})_{O}}
</math>

Sachant que le moment de <math>F_{i}</math> au point O est nul, on peut poser : 

<math>
{\color{Blue}J_{y}\cdot \ddot{\theta} = a.F_{i} - a.F_{j}}
</math>

Et:

<math>
{\color{Blue}\phi_{n}=F_{i}-F_{j}}
</math>

D'ou :

<math>
{\color{Blue}J_{y}\cdot \ddot{\theta} = a.\phi_{n}}
</math>

On asservit sur <math>\theta</math> à partir de <math>\phi</math> car seul <math>\phi</math> peut être asservit par rapport <math>\theta</math>. En effet, si l’on conserve <math>F_{i}</math> et <math>F_{j}</math>, il existe une infinité de solution à l’équation (1.3). Quel que soit <math>F_{i}</math>, on peut trouver un <math>F_{j}</math> qui permette d’atteindre l’angle <math>\theta</math> de consigne. Ainsi, on utilise la variable <math>\phi</math> 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 :

<math>
{\color{Blue}\frac{F_{i}(p)}{U_{i}(p)} = \frac{K_{i}}{1+\tau_{i}.p}}
</math>

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.

<math>\tau_{i}</math> 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 <math>F_{i}</math> et <math>F_{j}</math> sont identiques, avec la même dynamique, il est possible de considérer que la variable <math>\phi</math> évolue dans les mêmes conditions, et de poser :

<math>
{\color{Blue}\frac{\phi_{n}(p)}{U_{n}(p)} = \frac{K_{i,j}}{1+\tau_{i,j}.p}}
</math>

Dans lequel <math>U_{n}</math> est la différence entre les commandes <math>U_{i}</math> et <math>U_{j}</math>. La commande de chaque actionneur pourra être quelconque, la consigne sur <math>\theta</math> sera atteinte dès lors que l’écart de commande <math>U_{n}</math> entre <math>U_{i}</math> et <math>U_{j}</math> sera respecté.

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

<math>
{\color{Blue}\ddot\theta = \frac{a}{J_{y}} \times \phi_{n}}
</math>

<math>
{\color{Blue}\dot\phi_{n} = - \frac{1}{\tau_{i,j}} \times \phi_{n} + \frac{K_{i,j}}{\tau_{i,j}} \times U_{n}}
</math>

== Etude dans le domaine temporel ==
=== Modèle et retour d'état ===
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 <math>K_{c}</math> 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 :

<math>
{\color{Blue}\dot X=A.X+B.U}
</math>

<math>
{\color{Blue}Y=C.X}
</math>

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. 

<math>
{\color{Blue}X=\begin{vmatrix} \alpha \\ \dot{\alpha} \\ \phi_{1} \\ \beta \\ \dot{\beta} \\ \phi_{2} \end{vmatrix}}
</math>

La matrice A est composée deux deux blocs identiques, en <math>\alpha</math> et <math>\beta</math>, appelés <math>A_{\alpha}</math> et <math>A_{\beta}</math> :

<math>
{\color{Blue}A_{\alpha / \beta}=\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & \frac{a}{J_{y}} \\ 0 & 0 & - \frac{1}{\tau_{n}} \end{bmatrix}}
</math>

dans lequel n vaut 1 pour <math>\alpha</math> et 2 pour <math>\beta</math>. D'où :

<math>
{\color{Blue}A=\begin{bmatrix} A_{\alpha} & zeros(3,3) \\ zeros(3,3) & A_{\beta} \end{bmatrix}}
</math>

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

<math>
{\color{Blue}B = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ \frac{K_{1}}{\tau_{1}} & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & \frac{K_{2}}{\tau_{2}} \end{bmatrix}}
</math>

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

<math>
{\color{Blue}C = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \end{bmatrix}}
</math>

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.

[[File:systeme.png]]

=== La commande modale ===
==== 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 :

<math>
{\color{Blue}U=-K_{c}.X}
</math>

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

<math>
{\color{Blue}\dot{X}=(A-K_{c}.B).X}
</math>

Dans ce modèle, le choix des valeurs propres de la matrice <math>(A-K_{c}.B)</math> 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 <math>K_{c}</math> 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 : 

[[File: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 <math>V_{c}</math> tel que :

<math>
{\color{Blue}V_{c}=\begin{bmatrix}\lambda_{\alpha} & \lambda_{\dot{\alpha}} & \lambda_{\phi_{1}} & \lambda_{\beta} & \lambda_{\dot{\beta}} & \lambda_{\phi_{2}}\end{bmatrix}}
</math>

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

<math>
{\color{Blue}K_{c}=place(A,B,V_{c})}
</math>

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.

[[File:correcteur_simple.png]]

Le paramètre <math>K_{c}</math> 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 piloter 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 <math>\alpha,\dot{\alpha},\beta,\dot{\beta}</math> sont accessibles grâce à la centrale inertielle, il reste à estimer les variables <math>F_{1}, F_{2}, F_{3}</math> et <math>F_{4}</math>, c’est-à-dire <math>\phi_{1}</math> et <math>\phi_{2}</math>.

L’observateur assure le calcul de <math>\hat{X}</math>, qui est une estimation de X. La commande sera basée sur ce <math>\hat{X}</math>, et non sur le X qui est inaccessible en totalité.

[[File: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éel dont pour lequel on va pondérer l’influence de la boucle fermée, 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 :

<math>
{\color{Blue}G_{c}=place(A',C',V_{o})'}
</math>

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 <math>V_{o}</math> des valeurs propres choisies pour l’observateur.

Les valeurs propres <math>V_{o}</math> doivent être choisies à gauche (ie plus négative) :

<math>
{\color{Blue}\lambda_{observateur} < \lambda_{correcteur}}
</math>

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.

<math>
{\color{Blue}\dot{X}_{e}=A_{a}.X_{e}+B_{a}.U}
</math>

<math>
{\color{Blue}Y=C_{a}.X_{e}}
</math>

Le système ainsi augmenté deviens : 

[[File:systeme_augmente.png]]

Les matrices A, B et C sont modifiées pour ajouter les paramètres d'intégration :

<math>
{\color{Blue}A_{a}=\begin{bmatrix} A & zeros(6,2) \\ -S_{i}.C & zeros(2,2) \end{bmatrix}}
</math>

<math>
{\color{Blue}B_{a}=\begin{bmatrix} B \\ zeros(2,6) \end{bmatrix}}
</math>

<math>
{\color{Blue}C_{a}=\begin{bmatrix} C & zeros(4,2) \end{bmatrix}}
</math>

La matrice <math>S_{i}</math> permet de filtrer le vecteur Y pour en extraire <math>\alpha</math> et <math>\beta</math>, qui seront réinjectés dans l'étage d'intégration.

<math>
{\color{Blue}S_{i}=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}}
</math>

[[File:correcteur.png]]

L'erreur statique entre les variables <math>\alpha</math> et <math>\beta</math> et leur consigne est intégrée, puis réintégrée à la commande U par une matrice de retour d'état <math>K_{2}</math>. Cette matrice de retour d'état est en réalité une sous-partie d'une matrice de correcteur <math>K_{a}</math> étendue, recalculée sur la base du modèle augmenté.

Elle est calculée de manière similaire à la matrice <math>K_{c}</math> vue précédemment, à partir des matrices <math>A_{a}</math> et <math>B_{a}</math>, ainsi que d'un vecteur <math>\lambda_{c}</math> augmenté de deux valeurs propres, toujours négatives, pour les variables intégrales de <math>\alpha</math> et <math>\beta</math>.

A titre d'exemple, les consignes suivantes sont appliquées au système (<math>\alpha_{c}</math> et <math>\beta_{c}</math>) :

[[File:consigne_quadri_modal3D_tempo.png]]

==== 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. 

[[File: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).

On obtient assez facilement, par placement de pôle (même si lui n'a rien de trivial et qu'il vaut mieux disposer d'un outils puissant pour le faire, comme Simulink) un asservissement avec de bons résultats :
*La sortie est présentée dans la figure ci-dessous :
[[File:Alpha_Beta_quadri_modal3D_tempo.png]]
*La commande des actionneurs est cependant beaucoup trop forte. En effet, pour corriger les bruits, le modèle va commander sur quasiment l'ensemble de sa gamme de commande, entre 0 et 300 :
[[File:commande_quadri_modal3D_tempo.png]]

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 violemment à la moindre perturbation.
En l'état, elle ne sera pas utilisable.

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<sup>ème</sup> ligne du vecteur d'état avec i<sup>ème</sup> 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<sup>ème</sup> ligne du vecteur de commande avec i<sup>ème</sup> 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 être définie positive (ie au moins un paramètre dans la diagonale différent de 0, et nécessairement positif) et R simplement positive (s'il y a des paramètres, ils sont positifs).

Dans la pratique, il revient à jouer les apprentis sorciers que 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 <math>K_{a}</math> calculé pour le système augmenté, et créer les matrices Q et R à partir de là : 

<math>
{\color{Blue}Q=(K_{a}'.K_{a})}
</math>

<math>
{\color{Blue}R=eye(p)}
</math>

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 <math>K_{a}</math>, 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 ====
Le calcul du retour d'état sera basé sur les deux matrices de paramètres Q et R. On va donc modifier ces matrices initialisées précédemment pour paramétrer le système :

<math>
{\color{Blue}Q=Q+\delta Q}
</math>

<math>
{\color{Blue}R=R+\delta R}
</math>

La définition de <math>\delta Q</math> et <math>\delta R</math> est assez simple, elle permet de corriger les matrices initialisées en ajoutant dans la diagonale des paramètres beaucoup plus fort, accélérant le système ou limitant la commande. Ici on va plutôt mettre des paramètres très forts dans <math>\delta R</math> (>10000 dans mon appli) pour limiter l'amplitude des commandes.

==== Calcul du retour d'état LQ ====
Une fois les matrices Q et R initialisée et paramétrées, on calcule <math>K_{lq}</math> le correcteur LQ associé à ces matrices.

<math>
{\color{Blue}[K_{lq}, S, e]=lqr(A_{a}, B_{a}, Q, R, N)}
</math>

Je ne rentre pas dans le détail du calcul de <math>K_{lq}</math>, il est très largement détaillé dans tous les livres traitant de la commande LQ (les références 2, 3 et 4 par exemple). Le calcul passe par la résolution de l'équation de Riccati, assez simple mais gourmand en temps, que la fonction <math>lqr</math> de Matlab fait très bien. Les paramètres S et e ne sont pas utilisés (S : solution de l'équation de Riccati, pour les plus acharnés qui veulent vérifier, et e les valeurs propres de la boucle fermée, pour étudier leur décalage en fonction du paramétrage de Q et R). Le paramètre N est en général négligé dans la formule (ce que Matlab interprète par défaut comme N = 0).

La matrice <math>K_{lq}</math>, comme <math>K_{a}</math>, est découpée en deux parties : 
* <math>K_{lq1}</math> : pour le vecteur <math> \hat{X}</math> issu de l'observateur,
* <math>K_{lq2}</math> : pour le vecteur de commande issu de l'intégrateur.
Les correcteurs remplacent directement ceux du modèle modale, sans autres modifications.

Les courbes ci-dessous présentent le résultat de l'asservissement du système soumis à deux consignes sur <math>\alpha</math> et <math>\beta</math> : 
*La consigne reste la même que pour la commande modale,
*La sortie est présentée dans la figure ci-dessous, elle reste globalement similaire au cas modale :
[[File:Alpha_Beta_quadri_LQG3D_tempo.png]]
*La commande des actionneurs est limitée par rapport à celle de l'asservissement modale, mais elle reste assez forte. En réalité, elle est due à l'influence du bruit, qui sera filtré en discret par un filtre de Kalman :
[[File:commande_quadri_LQG3D_tempo.png]]

== Etude dans le domaine discret ==
=== Initialisation à partir du modèle temporel ===
L'initialisation à partir du modèle temporel est à privilégier lorsque ce modèle existe. Je ne reviens pas sur la théorie (ADU)

L'outil de discrétisation Matlab est la fonction :

'''systeme_discret = c2d(systeme_temporel,Te,'tustin')'''

Avec : 
* system_temporel créé avec la fonction '''ss(A, B, C)''',
* systeme_discret créé à partir du système temporel,
* Te période d'échantillonnage,
* 'tustin' méthode d'échantillonnage, nécessaire dès lors que l'on a un système avec des retards (typiquement le premier ordre des actionneurs).

'''Attention''' : sans la méthode "tustin", on obtient des amplitudes de commande hors norme.

D'après ce qui précède, dans l'étude temporel, il y a deux systèmes à discrétiser :
* Système observé : celui utilisé par l'observateur, il s'agit des matrices A, B et C,
* Système augmenté : incluant l'intégrateur, il est utilisé pour le calcul du correcteur.
Ainsi, on a  : 
* '''system_obs_t = ss(A, B, C)'''
* '''system_obs_k = c2d(system_obs_t, Te, 'tustin')'''
* '''[Ak, Bk, Ck, Dk, dt] = ssdata(system_obs_k)'''
et :
* '''system_aug_t = ss(Aa, Ba, Ca)'''
* '''system_aug_k = c2d(system_aug_t, Te, 'tustin')'''
* '''[Aak, Bak, Cak, Dak, dt] = ssdata(system_aug_k)'''
Da et Dak sont nuls, dt retrouve normalement la valeur de Te.

Le modèle discret étant créé, on va pouvoir reprendre une partie des calculs vus plus haut dans le domaine temporel pour le discret. Le schéma du modèle à asservir quand à lui est le suivant : 

[[File:systeme_augmente_k.png]]

La seule différence avec le cas continu est l'utilisation d'un intégrateur discret (en <math>\frac{1}{Z}</math>) au lieu de l'intégrateur continu.

=== Choix des valeur propres discrètes (modale) ===
Les valeurs propres d'un système discret doivent être à l'intérieur du cercle unité pour que le système soit stable. A l'inverse du domaine temporel, ou la partie réelle de <math>\lambda_{i}<0</math>, dans le domaine discret les valeurs absolues des parties réelles et imaginaires doivent être inférieures à 1 (elles-mêmes étant positives ou négatives).

Comme expliqué plus haut, il est intéressant de réaliser en premier lieu l'étude en temporel puis de discrétiser le système pour pouvoir utiliser le correcteur sur un équipement de contrôle commande (en général un microcontrôleur ou un PC, donc discret). Cependant, puisque l'on a trouvé des valeurs propres intéressantes en temporel, il serait bon de pouvoir les transformer elles aussi en discret.

On va ici utiliser une petite astuce perso, qui me semble logique (et qui a marché pour moi, donc..) : 
* Sachant que la discrétisation d'un modèle se fait par la fonction exponentiel, 
* L'exponentiel d'un nombre négatif est (oh magie!) inférieur à 1,
On réalise la discrétisation des valeur propres : 

<math>
{\color{Blue}\lambda_{i/k} = \exp{\lambda_{i}}}
</math>

Alors certes ça ne marche pas parfaitement, mais c'est le résultat le plus probant que j'ai obtenu. A noter cependant, mais c'est assez normale, un asservissement dans le domaine discret à de moins bonnes performances que dans le domaine temporel (dépassement plus importants, amplitudes de commandes plus fortes, etc.). Par contre, plus la fréquence d'échantillonnage augmente, plus le modèle discret tend à recoller au modèle temporel. Donc si c'est tout moche après discrétisation, pas de panique ! Il faut juste posément reprendre les valeurs propres discrétisées et faire de nouveaux essais, même si, dans le cas d'un drone, une fréquence d'échantillonnage inférieure à 10 Hz aura rapidement de mauvaises performances quoi qu'il arrive.

=== Le correcteur modale discret ===
Celui-ci est facile à recalculer dès lors que l'on a les valeurs propres discrétisées et le système augmenté :

<math>
{\color{Blue}K_{ak} = place(A_{ak}, B_{ak},[L1_{k} L2_{k}])}
</math>

'''Note''' : <math>L1_{k}</math> représente les valeurs propres pour le système standard, associées au vecteur <math>V_{c}</math>, alors que  <math>L2_{k}</math> représente les valeurs propres discrètes de l'intégrateur.

On découpera à nouveau la matrice <math>K_{ak}</math> en deux parties, une pour l'intégrateur et une pour le correcteur.

=== L'observateur modale discret ===
Je ne présente que succinctement l'observateur modale en discret, étant donné que l'on utilisera plutôt un filtre de Kalman (pour supprimer ou atténuer l'influence de w1 et w2, nettement plus concrète en discret).

L'observateur modale discret se calcul exactement comme dans le cas continu :

<math>{\color{Blue}G_{kc}=place(A_{k}',C_{k}',V_{ko})'}</math>

Dans lequel <math>V_{ko}</math> est directement issu de <math> L1_{k} </math>, avec un coefficient k inférieur à 1 :

<math>{\color{Blue}V_{ko} = k*L1_{k}}</math>
=== L'intégrateur discret ===
L'intégrateur discret est différent de l'intégrateur continu, essentiellement à cause de la discrétisation de l'intégrale. On a cette fois le schéma suivant :

[[File:correcteur_k.png]]

La matrice Si est la même que dans le cas continu. Les consignes sont toujours appliquées au système via les deux entrées de commande <math>\alpha_{c}</math> et <math>\beta_{c}</math>. On utilise la consigne suivante dans la suite de cet étude : 

[[File:consigne_quadri_LQG3D_discret.png]]

=== Choix des matrices LQ discrètes ===

On reprend exactement la démarche du LQ continu : 
*Initialisation de ma matrice <math>Q_{k}</math> à partir du correcteur modale : <math>{\color{Blue}Q_{k}=(K_{ak}'.K_{ak})}</math>
*Création d'une matrice <math>R_{k}</math> identité : <math>{\color{Blue}R_{k}=eye(p)}</math>, avec p actionneurs (2 dans notre cas)
*Initialisation de la matrice <math>N_{k}</math> : <math>{\color{Blue}N_{k}=-K_{ak}'}</math>
*Ajout d'un paramètre de réglage sur <math>Q_{k}</math> : je rajoute la valeur 1 sur <math>\alpha</math> et <math>\beta</math>, tel que
<math>
{\color{Blue}\delta Q_{k}=\begin{vmatrix} 1 \\ 0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \\ 0 \end{vmatrix}}
</math>

sans oublier que l'on travaille à partir <math>K_{ak}</math> et qu'il y a donc 8 paramètres à prendre en compte.
*Ajout d'un paramètre de réglage sur <math>R_{k}</math> : afin de bien ralentir le système, il ne faut pas hésiter à mettre de forts paramètres, tels que : 
<math>
{\color{Blue}\delta R_{k}=\begin{vmatrix} 10000 \\ 10000 \end{vmatrix}}
</math>
*Mise a à jour des paramètres Q et R : 
<math>
{\color{Blue}Q_{k}=Q_{k}+\delta Q_{k}}
</math>

<math>
{\color{Blue}R_{k}=R_{k}+\delta R_{k}}
</math>

*Enfin, calcul du retour d'état <math>K_{lqk}</math> par la fonction matlab dlqr (en non lqr comme dans le cas continu) :
<math> {\color{Blue}[K_{lqk},S,e]=dlqr(Q_{k},R_{k},N_{k})} </math>
*On extrait ensuite :
<math>
{\color{Blue}K_{lq1k}=K_{lqk}(:,1:6)}
</math>

<math>
{\color{Blue}K_{lq2k}=K_{lqk}(:,7:8)}
</math>

qui sont en réalité les six première lignes de <math>K_{lqk}</math> pour le correcteur basé sur <math>\hat{X}</math> en sortie de l'observateur, et les deux paramètres qui restent correspondent à la partie intégrale.

Pour finir, on remplace <math>K_{1k}</math> par <math>K_{lq1k}</math> et <math>K_{2k}</math> par <math>K_{lq2k}</math> dans le schéma de l'intégrateur / correcteur pour avoir cette fois un retour d'état discret linéaire quadratique. Reste une dernière étape, le filtrage du bruit de mesure et du bruit de modèle pour éviter au système des réactions trop violentes dans les boucles d'asservissement. On utilisera pour cela un filtre de Kalman, bouclant ainsi notre commande LQG discrète.

=== Calcul du filtre de Kalman steady state ===
Il existe deux types de filtres de Kalman :
*Le premier va évoluer dans le temps, en particulier la matrice de filtrage sera recalculée à chaque coup d'horloge. Ce filtre donnera les meilleurs résultats, car il est le plus dynamique, mais aussi beaucoup plus gourmand car il nécessite de lourd calculs à chaque coup d'horloge.
*Le second est considéré à partir du moment où le système atteint un état stable, c'est à dire que l'on a quitté les états transitoires.
En première approche, je vais présenter ici un filtre steady state, c'est à dire à matrice de gain K invariante. Le second filtre sera présenté plus tard, mais internet fourmille d'études assez détaillées sur l'utilisation de ce type de filtres.

La première hypothèse du filtre de Kalman est d'avoir un bruit blanc, c'est à dire qu'il ne s'agit pas d'un biais défini masqué en bruit (auquel cas il faut ruser autrement).

Le calcul des paramètre du filtre de Kalman est assez similaire à celui du LQ, en utilisant deux matrices de pondération <math>W</math> et <math>V</math>. Ces matrices sont les matrices de covariances associées aux erreurs de modèle (liées à l'équation d'état) et de mesures (liées à l'équation de sortie).

=== Système complet LQG Discret ===

[[File:modele_complet_k.png]]

[[File:Alpha_Beta_quadri_LQG3D_discret.png]]

[[File:commande_quadri_LQG3D_discret.png]]