2

Trapèzes de vitesses à l’aide du filtre de Kalman

Maintenant qu’on a un filtre de Kalman qui estime les paramètres des moteurs, il va falloir l’utiliser. Première utilisation : faire des trapèzes en vitesse sans aucun autres asservissement. On va envoyer aux moteurs la consigne théorique en vitesse ET accélération, le filtrage de Kalman s’occupant du reste. Ci-suit une petite vidéo de démonstration sur une distance de 1m et une accélération suffisamment faible pour être perçu :

 

Bon sur la video rien d’extraordinaire, mais gardez à l’esprit qu’il n’y a aucun PID !!

Si on regarde les enregistrements voici la comparaison entre la vitesse mesurée par les encodeurs (en rouge résolutions 1° d’où le bruit de quantification) et la consigne en vert. Les spikes sur la courbe vertes sont du au manque de résolution de l’instrumentation.

A titre personnel je trouve que ça fonctionne pas mal !! Si la consigne n’est pas bien suivi pendant la première demi-seconde, c’est lié a l’implémentation du Filtre de Kalman qui ne s’active pas tout de suite après un passage par la vitesse nulle pour éviter les problèmes lié au jeu des moteurs Lego.

Regardons les consignes :

Quelques remarques :

  • la consigne sur le moteur droit est plus importante que sur le moteur gauche : c’est normal j’ai volontairement mis plus de poids sur la roue droite que sur la roue gauche du robot, le filtre de Kalman l’a compensé !!
  • à 5 deci-secondes il y a une discontinuité, cela correspond au premier recalage du filtre de Kalman
  • à 10 deci-secondes : fin de la période d’accélération, la consigne d’accélération devient nulle d’où une consigne qui baisse d’environ 5 points
  • à 50 deci-secondes : on passe en mode décélération, la consigne baisse d’environ 5 points
  • à 60 deci-secondes : fin du trapèze de vitesse, on passe d’une consigne de 5% à 0 ce qui correspond à la commande pour s’affranchir du frottement sec.

Et si on regarde les positions : la différence entre la position théorique et la position mesurée par l’odométrie donne :

Si on omet les 5 première deci-secondes (filtre de Kalman inactif) on accumule 3cm d’erreur de position pour 100cm parcourue. Une précision de 3% sans aucuns asservissements, c’est très bon, surtout si on considère que le lsb de la commande représente 2% de la commande maximale. !!

La suite : on va rajouter un asservissement en position pour compenser les 3% d’erreurs qui restent.

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.

2

Asservissement.net

Comme l’a dit précédemment PAC, cette année, pour le robot métallique, tout passe par du .net. Ou plus précisément en microframework.

Tout passer en .net, ça passe également par une migration de l’asservissement en .net. A première vue, cela peut paraître dangereux, le microFramework n’étant pas un OS temps réel, cependant notre expérience positive de l’an dernier sur la partie IA, capteurs et actionneurs nous a pousser à tenter l’asservissement.

Voici déjà en introduction une petite vidéo de ce que nous avons déjà réussi à obtenir (il reste à régler les PID mais c’est un bon début).

Je passe tous les concepts classique sur les PID de robot que l’on trouve très bien présentés sur d’autres site comme le club elekRCVA ou ce très complet ppt également de RCVA. Mon objectif est de faire un asservissement en polaire donc avec un PID pour l’angle et un pour l’avance.

Pour point de départ, nous sommes partit de ce code trouvé sur codeprojet.net : http://www.codeproject.com/Articles/49548/Industrial-NET-PID-Controllers

C’était un bon point de départ avec tout de même quelques défauts (boucle infinie, tests bizares autour du terme I…)

Dans les bons points, on note l’utilisation de delegate (pointeurs sur fonction) pour passer les valeurs d’entrée et de sortie, ainsi on peut utiliser une même classe pour plusieurs PID (dans notre cas, un polaire et un en distance).

… Lire la suite

0

Les paramètres du PID

052211_2228_Lesparamtre1.png

Tests en cours les paramètres du PID. Les graphiques ci-dessous présentent les différentes réponses des deux moteurs soumis à une même commande. La commande est un trapèze dont le maximum si situe à 3072 qui correspond à notre vitesse max désirée.

Les courbes rouges et bleues présentent la vitesse du moteur (qui oscille autour de la commande de 3072). Les courbes vertes et bleues clairs présentent les commandes en Voltage correspondantes à chacun des moteurs.

Les titres des graphiques correspondent aux paramètres de l’asservissement : P I D (dans cet ordre)

Par dichotomie, on essaye de trouver les meilleurs paramètres. Etonnamment, l’introduction du D dans les derniers graphiques semble entrainer un fort dépassement de la commande.

… Lire la suite

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