Site ELEC344/ELEC381

Partie interactive du site pédagogique ELEC344/ELEC381 de Télécom ParisTech (occurrence 2010).

Catégories

Quel fourbe cet accéléromètre !

En ce début de semaine nous avons travaillé à essayer de faire tenir Wheely en équilibre. Ce n’est pas convainquant du tout pour l’instant.

Nous avons passé une journée entière à jouer sur les paramètres du PID pour essayer de stabiliser le robot, mais en vain. Impossible.

Suite à une remarque d’Alexis, j’ai trouvé d’où venait le problème. Bien qu’ayant scrupuleusement testé l’accéléromètre, je l’avais fait en rotation et non en translation. Or puisque on utilisait bêtement l’équation g*sin(theta) = acc_x, lorsque l’on translate le capteur ça fausse tout.

Hier j’ai établi avec Fabien et Sam une équation plus complète :

z=g*cos(t)+(x+g*sin(t))*tan(t)

où : z = acc_z, x = acc_x, t = theta

Je l’ai ensuite donnée à MATLAB qui l’a résolue ainsi :

>> solve(‘z=g*cos(t)+(x+g*sin(t))*tan(t)’,'t’)
ans =
(-2)*atan((x + (- g^2 + x^2 + z^2)^(1/2))/(g + z))
(-2)*atan((x – (- g^2 + x^2 + z^2)^(1/2))/(g + z))

C’est là que ça se corse. Il y a deux solutions. J’essaie la première. Zut ça marche pas. La seconde ? Non plus.

Je me suis rendu compte que je faisais la racine carrée d’un nombre négatif en pratique en utilisant cette formule. En théorie ça n’arrive jamais, mais avec le bruit ça peut déborder un peu. Je corrige en prenant la racine de 0 si le nombre est négatif. Toujours pas. Je réfléchis un peu, et je tente une combinaison des deux solutions, en prenant la première si x est négatif, la deuxième sinon.

C’est beaucoup mieux. Maintenant, en translation, si la carte est horizontale, l’angle reste bien égal à 0. Super ! C’est normal, dans ce cas précis z = g et donc on fait atan(0)

Mais en translation inclinée, ça ne fait toujours pas ce que je veux. L’angle varie de 7 ou 8 degrés… Alors qu’il ne devrait pas d’après la démarche que nous avons faite. J’ai passée toute la journée à creuser la dessus, à tenter différentes combinaisons des solutions, à tracer les courbes de chacune des deux solutions pour trouver une idée… En vain.

J’ai l’impression d’avoir tout essayé, je ne sais plus quoi faire… :( Je suis désespéré !

Quand l'incroyable se produit

Après quelques essais infractueux, je m’étais un peu résigné sur cette histoire de filtre de Kalman.

Mais là ce soir, dans un éclair d’espérance, j’ai relu mon code. Après la correction d’une erreur de signe, d’une erreur de calcul (je faisais la somme d’une matrice et de sa transposée « en place », très mauvaise idée …) et avec un pas de discrétisation plus petit, mon filtre s’est soudainement mis à marcher.

Yeeepeee !

Si, si !  et même que le changement des paramètres de covariance influe de manière à peu près logique sur le système. Si je fais trop confiance aux gyros, j’ai un petit effet « flamby inertiel » (lhélico tourne un peu plus loin qu’il aurait dû et revient un peu après le mouvement pour se trouver au bon endroit) mais si je fais trop confiance aux accelerometre/magnétomètre, il reprend un peu la tremblotte (comme sans filtre en fait).

Il faut encore que je comprenne comment régler la puissance de la correction sur l’offset des gyros, même si ça marche vraiment pas trop mal pour le moment.

Normalement, on va essayer de voir ce que ça donne demain mercredi sur un vrai vol : L’helicoptère devrait être attaché par des cordes de sorte qu’il ne puisse pas trop se déplacer mais en laissant de la marge sur son assiette pour voir si notre asservissement fonctionne.

Je vous aurais bien montré des vidéos de la visualisation de l’orientation de la carte mais je n’ai pas d’iPhone 3GS :’( (ou d’autres outils de prises de vue animées non propriétaires)

Résumé des épisodes précédents

Après avoir perdu sur la représentation des rotations de la carte en OpenGL, je me suis finalement mis à lire directement la DCM (matrice qui détermine l’attitude de la carte) pour savoir si l’algo FQA était bon. Ca permet surtout de vérifier pas mal de cas et de se convaincre que ça marche (quand la matrice n’a que des 0 et des +/- 1). Et le FQA a bien l’air de marcher !!

Sur la carte Heliokter, j’ai repris la méthode de Samuel pour déterminer comment les composants détectaient les axes et ça a presque tout de suite marché avec l’algorithme FQA. Vendredi soir j’ai juste eu le temps d’implémenter le FQA sur la carte et le filtre de Kalman sans pouvoir tout tester.

Aujourd’hui lundi, j’ai d’une part repris le code de visualisation OpenGL pour recevoir par Bluetooth directement les coefficients de la DCM calculés sur la carte et les représenter (sans se tromper ce coup-ci). C’est OK de ce point de vue là et ça marche bien. Il n’y a pas trop de singularité mais les changements d’attitude manquent de fluidité. C’etait prévu, mais ça permet aussi de voir que c’est effectivement très sensible aux vibrations. Après, peut-être que les vibrations faites avec la main sont différentes (notamment en terme de fréquence) de celles ressenties sur l’Heliokter.

D’autre part, j’ai regardé le comportement de mon filtre de Kalman  en fonction de différentes valeurs sur les incertitudes de mesures et de prédiction. J’ai surtout fait du cas extrême pour voir s’il n’y avait pas de grosses erreurs. C’est un peu le flou puisque je ne sais pas non plus si le modèle cinématique que j’ai implémenté dans le filtre est juste  mais j’ai observé des choses qui me semblaient cohérentes.

  1. Quand l’incertitude de prédiction est beaucoup plus faible (donc plus fiable !) que celle de mesure, j’ai mon état qui se décale lentement mais surement de l’état réel. C’est visiblement du au biais des gyroscopes, qui n’est pas corrigé dans l’état. C’est cohérent avec les incertitudes qu’on fixe.
  2. Quand l’incertitude de mesure est beaucoup plus faible que celle de prédiction, j’ai un filtre qui diverge très rapidement (en quelques itérations) sans que je puisse vraiment l’expliquer.
  3. Dans les autres cas (c’est à dire quand les incertitudes sont du même ordre de grandeur), mon état commence par se décaler de l’état réel puis le biais calculé dans l’état du filtre commence à se rapprocher du biais réél, ramenant l’état vers sa valeur réelle. Jusque là tout va bien. Mais le problème c’est que cela va de plus en plus vite et le biais calculé finit par dépasser rapidement la valeur du biais réel et s’écrase dans les choux (genre dépasement de capacité) quelques itérations plus tard.

Le jeu sur les valeurs de l’incertitude influe sur le temps que met le filtre à diverger. Ca va de jamais (cas extreme 1) à tout de suite (cas extreme 2). Je n’ai pas encore trouvé de valeurs qui marchent.

Voilà, après j’imagine qu’il faudra régler les incertitudes plus finement (c’est à dire avoir des coefficients différents pour chaque mesure et chaque prédiction, pour le moment) mais je ne sais pas si le comportement que j’observe présentement est typique d’un filtre de Kalman correct mal calibré ou d’un filtre complètement foireux …

Encore une victoire de canard !

Suite à mon dernier article j’ai tenté toutes sortes de manières de debug pour trouver d’où le petit problème venait. Je suis passé par l’implémentation d’un programme en C sur mon ordinateur pour tester le filtre sur un échelon pour comparer à ce que donnait MATLAB (pareil), tester l’échelon sur la carte logique, j’en passe et des meilleures.

Après de longues heures à m’arracher les cheveux, j’ai enfin réussi à corriger le code. Le problème venait de l’accéléromètre. La récupération des valeurs via SPI posait des soucis et désynchronisait le filtre. J’ai donc créé une tâche  spécifique qui se charge d’updater une variable locale au programme avec ce qu’elle récupère via SPI. Ca permet d’éviter de perturber le bon déroulement du filtrage.

Voici le résultat obtenu lors du benchmark. Cela correspond pile poil aux mouvements donné à la carte par le servomoteur. Et l’axe des ordonnées indique pile poil l’angle d’inclinaison de la carte en centièmes de degrés. C’est trop cool, et surtout, ça va permettre de bosser sur la suite en ayant une totale confiance dans les angles que l’on va manipuler.

Matrix reloaded ...

Hier, fort de la lecture de différents articles et codes source, je me suis lancé dans le code du filtre de Kalman, sans pouvoir toutefois réellement le tester (mais il compile !). J’ai fait attention d’optimiser un peu les opérations sur les flottants même si on peut sûrement faire quelque chose de mieux ‘hardcore style’. J’espère aussi qu’une opération flottante avec 0 prend moins de temps qu’une autre !

Au total, une passe de mon filtre de Kalman prend environ 870 multiplciations, 950 additions et 9 divisions. Je n’ai pas encore relié ça rigoureusement aux temps d’execution qu’on a mesuré avec Etienne mais il semble me souvenir que ça devrait faire aux alentours de 5ms. Vu le temps de réaction et l’inertie de l’hélicoptère, je pense qu’on est bon de ce point de vue là.

L’objectif de la journée d’aujourd’hui était d’intégrer l’algo FQA (pour la résolution du problème de Wahba) et le filtre de Kalman pour pouvoir le tester. Seulement, pour faire ça, il fallait d’abord vérifier que l’algo FQA marchait bien, à l’aide du visualisateur 3D. Le protocole est simple: Le visualisateur récupère les valeurs du capteur sur la carte (envoyées par Bluetooth), execute l’algo en local  (pour eviter des problèmes supplémentaires …) et met à jour l’affichage.

Et là, ça a été trèèèèèèèèèèèèèèèès long (oui, une journée complète de 10h, je ne comprends toujours pas comment le temps a pu filer ainsi !) pour avoir le lien entre l’affichage et la matrice renvoyée par l’algo avec tous les problèmes de changements de base (carte->monde physique réél->monde openGL->représentation de la carte dans le monde OpenGL), de matrices transposées ou pas, bref !

Voyant que je pataugeais, Etienne D. m’a bien aidé pour essayer de voir les choses clairement et systématiquement et on est (visiblement !) arrivé à quelque chose de cohérent là-dessus.

Ce qui nous a permis de voir que l’algo FQA (ainsi qu’une version allégée que Miguel a codé en parallèle) ne marche pas très bien, vraisemblablement à cause d’un problème d’ordre de rotation. J’ai essayé aussi de reprendre l’algo TRIAD (qui n’utilise que des opérations vectorielles donc normalement exempt de ces problèmes d’angle) sans grand succès non plus…

Bref, c’est un peu une journée désespérante de ce côté là, heureusement qu’Etienne avait à côté des résultats positifs (et que Pulse aussi, derrière nous, on pouvait s’amuser à les pinger ou à les synflooder pour se détendre :P ).

Filtres complémentaires : mission accomplie

Bon, suite aux travaux d’aujourd’hui, j’ai réussi à obtenir de très bonnes courbes. Notre problème d’offset sur les valeurs et de temps de réponse du filtre a été résolu en choisissant une valeur beaucoup plus grande pour la fréquence de coupure.

J’ai ensuite mis en place un benchmark assez complet : oscillation rapide, oscillation lente avec plateau ou non, oscillations rapides de demi amplitudes.

J’ai ensuite calibré le coefficient relatif entre l’accéléromètre et le gyroscope en utilisant des oscillations de différentes fréquence, l’un et l’autre se manifestant ainsi différemment. Le bon coefficient relatif est celui qui permet d’avoir une même mesure de l’angle pour des oscillations de fréquence différente.

Voici le graphique obtenu, avec en haut la commande d’angle donnée aux servomoteurs, et en dessous, l’inclinaison, calculée en degré, en fonction des données recueillie par les capteurs lors du mouvement. Le seul défaut que l’on peut constater est un angle calculé un chouilla en dessous de 0 lorsque l’on vient de remonter la carte (aucun soucis lorsqu’elle vient de descendre). Cela est probablement du à un petit défaut des servo vis à vis de la gravité, et n’est donc a priori pas lié à notre travail.

J’ai ensuite implémenté le nouveau filtre en C, modifié le code du gyroscope pour intégrer après filtrage (et non avant comme c’était fait), et créé deux fonctions get_angle_degree et get_angle_radian qui intègrent les bons coefficients.

Enfin, j’ai testé l’execution sur la carte de tout ça, envoyant via bluetooth le résultat de la commande get_angle_degree*100 (pour avoir de la précision malgré le cast en entier pour l’itoa).

J’obtiens des résultats à l’image de la simulation MATLAB, plutôt satisfaisants. Hormis pour le troisième type d’impulsion, ou ça ne colle pas. Ca n’atteint pas les 10°, et surtout, ça fait deux oscillations au lieu d’une.

Compte tenu que j’ai bien vérifié que le filtre et les données traitées (vitesse donnée par le gyroscope et accélération donnée par l’accéléromètre) sont exactement les mêmes je ne comprends pas trop. Si quelqu’un à une piste je suis preneur, en attendant, je continue à chercher…

Que le filtre soit et le MATLAB fût

Aujourd’hui nous avons travaillé sur la fusion de capteurs et la calibration du filtre. Nous avons mis au point une méthode expérimentale avec un servomoteur qui permet de contrôler précisément le mouvement donné à la carte et de les analyser ensuite.

Après avoir bataillé un peu dans tous les sens et joué avec les différents paramètres nous obtenons des courbes plutôt sympathique… Mais affaire à suivre !

Fusion de capteurs et petits oscillations

Comme annoncé dans mon précédent billet, j’ai relevé des mesures pour des petites oscillations, plus proche de la réalité. La méthode d’analyse des courbes me parait très bonne pour calibrer et analyser la fusion de nos capteurs. De plus les résultats mesurés sont plutôt bons et reflètent bien la réalité.

Dans l’idéal, si l’on pouvait monter la carte inertielle sur une servomoteur, et lui faire subir des oscillations bien contrôlées (dont on connait l’angle et la fréquence), il serait possible de faire par ma méthode une calibration parfaite ! Je sais pas si Alexis à un servomoteur adéquat qui traîne ?

Voici les résultats obtenus :

Fusion de Capteurs

Aujourd’hui j’ai travaillé avec Daniel sur la fusion de capteurs et l’accéléromètre.

J’ai codé en C le filtre fournis sous forme mathématique par Fabien. Daniel s’est ensuite chargé de l’intégrer à son fichier de gestion de l’accéléromètre, puis de fusionner les valeurs recueillies par l’accéléromètres et celles recueillies par mon programme de gestion du gyroscope.

Nous avons ensuite planché sur la façon de fusionner de manière optimale ces valeurs (poids relatif de chacun des deux capteurs). Empiriquement nous avons trouvé un coeff. plutot satisfaisant.

Afin d’aller plus loin, j’ai continué dans le TGV à essayer de trouver le coefficient idéal. Pour cela, j’ai enregistré des mesures et j’ai tracé différentes courbes  pour différents coefficients. Voici le travail :

Je me suis ensuite dit qu’il était un peu bête de faire varier l’angle de 90° et que ça ne correspondait pas à la pratique. Ni à l’approximation des petits angles adoptées pour le calcul du sinus. Je vais donc faire une nouvelle vague de mesures pour un mouvement beaucoup plus restreint, voir ce que ça donne.