0

Asservissement en vitesse à l’aide d’un Filtre de Kalman

Sur le petit robot Lego de cette année, on va introduire un asservissement utilisant un Filtre de Kalman. Le filtre de Kalman va servir à estimer les paramètres des moteurs qu’on utilisera par la suite pour la commande. Afin de vous mettre l’eau à la bouche sur les performances exceptionnelle (ou pas) de cet asservissement voici quelques détails sur l’implémentation.

 

Modélisation d’un moteur

Le modèle qui a été choisis pour le moteur est le suivant :

C : le couple fournis par le moteur
Ki : constante de couple du moteur
Kr : la résistance à l’entrainement du moteur (frottements)
Kfs : la force de frottement sec du moteur (très importante sur un moteur lego NXT)

On n’a pas à notre disposition de mesure du courant, on ne commande que le PWM appliqué au moteur qu’on notera PWM et Vbat la tension de la batterie d’où ensuite V = PWM.Vbat

L’équation du courant dans le moteur est la suivante :

Lm : l’inductance du moteur -> qu’on considérera négligeable (ça tombe bien ce terme nous gène pas mal)
V : la tension appliqué au moteur, dans notre cas V = PWM.Vbat
Kb : une constante représentant la force contre-électromotrice.
Rm : la résistance interne du moteur

Le courant vaut donc :

Enfin on considère l’accélération angulaire du moteur comme proportionnelle au couple :

On obtient ainsi l’équation différentielle :

On définit pour simplifier les équations un terme représentant l’accélération angulaire en fonction de la commande

Un terme représentant la résistance à l’entrainement

Un terme représentant les frottements secs

On a ainsi une équation simplifiée :

A cause du terme de frottement sec cette équation n’est pas valable lorsque le moteur est statique. On désactivera donc le recalage du filtre de Kalman lors de ces périodes.

 

Représentation d’état

Passons à la représentation d’état : on choisit le vecteur d’état suivant :

On a choisis de ne pas intégrer et dans le vecteur d’état pour économiser du temps de calcul, très problématique sur le Lego NXT. 4 états c’est des matrices de 16 éléments, rajouter 2 états nous donne des matrices de 36 éléments, grosso modo on peut s’attendre que la charge de calcul évolue avec la même proportion.

On discrétise à la louche l’équation différentielle du comportement du moteur et on considère que les paramètres du moteur n’ont pas de modèle d’évolution et on obtient :

Cette équation n’est pas vraiment linéaire car on observe que intervient dans la matrice d’évolution, et la fonction sign() n’est pas non plus du meilleur effet. En pratique ce genre d’astuce fonctionne, on perd juste le caractère optimal de l’estimateur du Filtre de Kalman.

On observe la vitesse, la matrice d’observation est donc :

PS : si on a de la marge dans la charge calcul on peut réintégrer et dans les états ainsi qu’un état modélisant le jeu sur la mesure de l’angle su moteur (important en lego NXT). On discrétise au premier ordre et on obtient la représentation d’état suivante :
;  ;

Cette représentation a l’avantage d’avoir en état directement la lecture de l’encodeur ce qui permet de prendre en compte le caractère intégré de la vitesse et facilite la détermination du bruit de mesure qui est égal à la précision de l’encodeur. L’introduction de l’état de jeu sur la mesure de l’encodeur permet de modéliser de manière plus réaliste l’imperfection de la mesure, cet état de jeu est d’ailleurs inobservable. Le défaut de cette représentation c’est qu’elle introduit un retard de 2*dt entre l’accélération et le déplacement. Pour réduire ce retard on peut supprimer l’état d’accélération et mettre les termes relatifs à l’accélération directement dans l’état de vitesse. Pour l’avoir testé sous Matlab c’est la représentation d’état qui m’a semblée la meilleure :
;

 

Réglage du filtre de Kalman

A partir de là on peut implémenter un filtre de Kalman avec les formules qu’on trouvera dans la littérature sur le sujet. Il nous manque juste la définition des bruits d’observation et des bruits de modèle.

Le bruit d’observation est simple, on va considérer qu’il correspond à la quantification de la mesure de vitesse :

R = Lsb_mesure.dt

Avec dt : le pas de temps de l’itération du filtre de Kalman

Le bruit de modèle est plus compliqué à déterminer car on ne maitrise pas grand-chose sur la variabilité dans le temps des paramètres des moteurs. La solution choisie a été de déterminer un talon sur la covariance des termes du moteur. Par exemple on considère arbitrairement qu’on ne peut pas connaitre à mieux de n% les termes du moteur. Si la covariance d’un des termes tombe en dessous de n% de l’estimé de l’état, on calcule le bruit de modèle approprié pour faire remonter sa covariance à n% de l’estimé de l’état. En premier réglage on avait choisis 5%. Ce chiffre a été choisi en post-traitement car il minimisait les erreurs de prédiction sur un horizon de 1s. On peut ensuite raffiner un peu ces bruits de modèle en considérant que certains états sont plus variables que d’autres.

 

Implémentation

La première implémentation du Filtre de Kalman sur la brique NXT a été un échec, le temps de calcul était de 130ms par moteur, la faute a tous les calculs matriciel effectué en flottants. Difficile de faire dans ce cas du temps réel. On a donc optimisé le codage pour les spécificités de nos matrices.

1ère optimisation : faire le calcul de F.P.F’ par bloc
Si on observe la matrice F, on observe que la partie inférieure droite est l’identité. Utiliser cette spécificité permet d’accélérer le calcul des produits de matrice.

2ème optimisation : on n’a qu’une seule observation
le calcul de H.P.H’ revient à sélectionner un scalaire dans P. Scalaire dont l’inversion ensuite est sacrément simple !!

3ème optimisation : on n’a toujours qu’une seule observation
Le calcul de K.H.P : la ligne n du résultat de ce produit est la ligne de l’état observé de P multiplié par le scalaire K(n)

Avec ces optimisations on passe de 130ms à 11ms de temps de calcul. Le temps réel devient possible !!

 

Commande du moteur

Maintenant qu’on a une estimée des paramètres du moteur on peut en tirer des choses intéressantes :

Commande en vitesse : on va calculer la commande pour avoir la vitesse désiré en stabilisé (accélération nulle)

Commande en accélération : surement la commande la plus intéressante. A noter qu’une accélération nulle ne veut pas dire une commande nulle !!

Vitesse maximale du robot : cas stabilisé à accélération et commande maximale

On peut aussi connaitre l’accélération maximale en fonction de la vitesse actuelle

En pratique sur la base roulante allégée de 2011 on a

Ce qui nous donne avec des roues de 70mm de diamètre
= 13.14 rad/s = 2tr/s -> Vmax = 460 mm/s

A vitesse nulle l’accélération maximale est
92rad/s² -> Acc_max = 3.2m/s². Cette accélération peut sembler énorme mais les moteurs NXT ont beaucoup de couple et atteignent vite leur vitesse maximale.

 

Intérêt du Filtre de Kalman

Commençons par la réflexion que tout lecteur a du se faire : c’est quoi les avantages du Filtre de Kalman par rapport à un PID bien réglé ?

Commençons par les inconvénients

  • importante charge de calcul, le filtre de Kalman tournera plus lentement que l’asservissement en position sur le NXT.
  • La stabilité (que ce soit du modèle ou numériquement) du Filtre de Kalman n’est pas prouvé, il y a donc un risque de division par zéros si il part en vrac -> le filtre devra être surveillé en temps réel.
  • réglages des bruits de modèles sont réglés à la main, c’est le point faible de la modélisation .
  • temps de convergence du filtre : l’asservissement n’est pas optimal lors des premiers tours de roue, en pratique on initialise le filtre à des valeurs cohérentes histoire que le robot ne fasse pas n’importe quoi pendant les premiers tours de roues.
  • Beaucoup de travail à mettre a point un premier prototype et à débugger.

Et maintenant les avantages

  • estimation des frottements secs : c’est un gros avantage par rapport au PID, les moteurs lego ont des frottements secs qui peuvent atteindre 10% de la commande maximale du moteur. Ça permet d’avoir un comportement plus propre à faible vitesse
  • commande en accélération : je compte bien m’en servir, sur un trapèze en vitesse, la commande sera différente qu’on soit en phase d’accélération ou en phase de décélération
  • Insensibilité au niveau des batteries, qu’on ait 7V ou 9V sur les batteries, le comportement est le même (cf les problèmes de Oleg)
  • Connaissance de la vitesse maximale du robot : au lieu de définir en dur une vitesse maximale du robot, on pourra définir que la vitesse maximale du robot lors des trapèzes de vitesse sera de n% de la vitesse maximale des moteurs.
  • Détection des blocages / dérapages: j’espère bien qu’en observant comment évolue les termes du filtre de pouvoir détecter les blocages et les dérapages.
  • Faire son fier en disant que son robot utilise un Filtre de Kalman pour l’asservissement alors qu’on aurait pu passer ce temps à mettre au point quelque chose de plus utile.

Voila pour la partie théorique, dans un prochain article la démonstration en vidéo et en courbes de la performance de l’asservissement par filtrage de Kalman. Pour l’instant je n’ai pas pris le temps de faire des vidéos propres.