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.

1

ESX-IOX : la carte d’asservissement du robot 2011

ESX-IOX de STW

Le choix de la technologie assurant l’asservissement est toujours un choix délicat dans la conception d’un robot car  un déplacement maitrisé est la condition sine qua non de l’ensemble des autres actions. Les technologies possibles sont multiples autant en terme de software que de hardware. Les membres de l’équipe ayant déjà testé de multiples variantes autour de pont H et L29xx associés, nous avons décidé cette année d’investir du temps dans l’utilisation d’une carte plus industrielle : l’ESX-IOX de STW.

… Lire la suite

0

Asservissement Etape 2 : Controle de distance sur les 2 roues

Relais C.I. 12V 2RT 8A HF115F

Rappel de l’étape précédente : le robot avancait dans une seule direction, le déplacement était « surveillé » par un codeur sur une seule des roues.

Les nouveautés

Nouvelle étape dans l’asservissement : le robot peut se déplacer avec un controle des pas sur les 2 roues et changement de sens de rotation grace a un relai commandé par la carte d’asservissement (ESX-IOX).

Pour cela nous avons travaillé sur la création de petites cartes de prototype reliant les sorties des codeurs vers les entrées de la carte d’asservissement ainsi que leur alimentation en provenance du 5V de la carte mère. Une deuxième carte permet de rendre plus distribuer les alimentations à chaque carte et de mettre en commun les masses. On évite donc l’amas de pince crocodile de la première étape.
Un relai inverseur a aussi été ajouté entre la sortie PWM de la carte d’asservissement et un des moteurs pour controler son sens de rotation. La commande du relai est faite par une sortie numérique de la carte d’asservissement. Lors de la prochaine étape, on fera la meme chose pour l’autre moteur dès réception du deuxième relai.
1

Utilisation des données en provenance des codeurs avec un LS7084 ou LS7083

screenshot.22.png

L’information en provenance des codeurs est fournie sur 3 signaux dont 2 réellement utiles (le troisième signal étant déduit par le codeur depuis les 2 premiers). L’utilisation des créneaux fournis par un des signaux permet déjà d’avoir une première information sur l’évolution du cycle moteur. Cependant il est indispensable d’étudier les 2 signaux pour connaitre le sens de rotation du moteur (par simple observation du premier front montant entre le signal A et B).

C’est le rôle du LS7084N (http://www.lsicsi.com/pdfs/Data_Sheets/LS7083N_LS7084N.pdf) de LSICSI qui reçoit en entrée le signal A et B et ressort en sortie deux signaux l’un étant un créneau « mixant » les créneaux des entrées A et B et l’autre une indication sur le sens de rotation des moteurs. Il est de plus possible de paramétrer le mode « mixage » des signaux A et B pour récupérer un coefficient multiplicateur entre x1, x2 et x4 entre le nombre de créneau sur une des voies d’entrées A ou B et la sortie.

Le schéma ci-dessous décrit les différents créneaux de sortie (CLK) suivant le mode de multiplication choisi.

screenshot.21

LS7084N Output

Ainsi, chaque codeur est relié à un LS7084N qui est lui-même relié à 2 entrées numériques de la carte d’asservissement. Chaque CLK est relié sur une entrée Fréquence de la carte d’asservissement et les UP/DN sont reliées sur des entrées numériques de la carte d’asservissement. Le paramétrage du mode est fait par une liaison directe au 12V qui permet de sélectionner le mode x4 en continue.

0

Premiers tours de roue

Voici les images des premiers tours de roue réalisés hier soir par la base roulante. Ca semble un peu bancal car pour l’instant, il n’y a pas d’asservissement et un seul des deux codeurs était connecté. Mais les premiers résultats sont là : on maitrise le nombre de tours de roue là ou le codeur est relié.

La carte d’asservissemement sert de carte d’alimentation à la carte mère dans cet exemple. Cette dernière étant uniquement utilisé ensuite comme réducteur 12V / 5V pour alimenter les codeurs.

Prochaine étape, ranger un peu les cables, faire des branchements plus propres et démarrage des essais d’asservissement !!!