Site ELEC344/ELEC381

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

Catégories

Heliokter bourdonne

J’ai passé la fin des vacances sur le filtre de Kalman, pour l’affiner/le corriger. Je me suis rendu compte avec la carte IMU que je ne prenais en fait pas en compte les gyroscopes et que je me basais seulement sur l’accéléromètre/magnétomètre (l’accéléromètre étant excentré par rapport au centre de rotation de la carte IMU, on s’en aperçoit mieux). Le modèle cinématique de mise à jour de létat en fonction des gyros était en fait légèrement erroné (une petite matrice par-ci …).

Bon bref j’ai réussi à regler tout ça et avoir des resultats corrects et sans lag.

Hier, on a commencé à intégrer tout le code et à contrôler les moteurs correctement avec l’I2C, mais je laisse les autres détailler, je n’etais pas là l’après-midi …

Et aujourd’hui, c’etait au tour de l’asservissement d’être integré dans le code et testé. Il a fallu évidemment faire face aux éternels problème d’orientation  (argh, la carte est à 45° par rapport à l’avant de l’Helico !) et d’angles, en passe d’être résolu ce soir.

Fernando et Miguel s’occupent d’un protocole de communcation par ZigBee ou Bluetooth (compatible avec celui de Mikrkopter) qui nous permettra de régler les coefficients du PID de manière dynamique (sans avoir à reflasher à chaque fois). Je leur laisse le soin de détailler.

Voilà une petite vidéo du rodéo actuel, avec un asservissement inexact et trop fort.

1min30sec d’équilibre pour Wheely

Aujourd’hui deux évolutions ont été apportées au robot :

  • Kellya et Daniel ont amélioré le système de calibration. Désormais, l’accéléromètre est supposé juste et permet au démarrage la calibration du gyro. Ce système a l’avantage d’être robuste une fois que la carte est bien fixée horizontale sur le robot. Il nous reste à éventuellement ajuster un petit offset en dur dans le code.
  • J’ai réglé le PID et trouvé des coefficients très chouettes car on a réussi à le faire tenir en équilibre 1min30sec dans moins d’un mètre. Ensuite, il a trop dérivé et s’est retrouvé au bord de la table. Daniel va poster la vidéo très prochainement !!!

Sur ce, c’est reparti demain pour de folles aventures. What’s next ? Intégrer les données des encodeurs dans le PID de la carte propulsion pour contrôler la dérive suivant x.

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)

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.

Wahba pour Kalman

Lundi

Après les larges lectures des articles sur Kalman, MAV, IMU que Alexis nos a donné maintenant je peut dire que je  comprendre mieux les choses. A début des lectures j’ai eu des problèmes pour comprendre mais après un coup d’œil aux cours de physique, probabilités, et autres aspectes commet les angles d’Euler et les quaternions les choses ont commencé à être plus claires.

Mardi

Les neuf variables d’état du filtre Kalman  sont les trois vitesses de rotation angulaire mesures pour les gyroscopes plus les deux colonnes de la matrice de rotation de la base du mobile à la base de référence. Ces colonnes de matrice de rotation son trouvés à partir du algorithme FQA. FQA prend les valeurs mesures par l’accéléromètre et magnétomètre et donné la matrice rotation. Après lecture de Wahba, FQA, et le code de Fernando on a vu il y a encore le problème de singularité quand le tangage c’est près de quatre-vingt dix degrés.

Mercredi

En cherchant résoudre le problème de singularité j’ai vu que on peut trouver les colonnes de la matrice de rotation sans passer pour les quaternions. Celui-ci utilise plusieurs opérations comme sinus et cosinus qui sont très couteuses. Ainsi, j’ai fait une fonction qui donne les colonnes de la matrice en utilisant le minimum des opérations. Avec F-X on testé sur l’IMU et on a un problème sur le tangage, quand on fait la rotation l’axe n’est pas constante. Il reste de résoudre ce problème et celui de singularité, on a besoin que le problème de Wahba soit bien résolu pour tester le filtre Kalman sur l’IMU.

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…

Wheely – filtre complémentaire

Depuis quelques jours, nous travaillons sur les données du gyroscope et de l’accéléromètre que l’on filtre (respectivement en passe haut et passe bas) afin d’utiliser la technique du filtre complémentaire. Pour l’instant, (après pas mal de tests) le temps de stabilisation du filtre se situe aux alentours de 20 secondes (ce qui est un peu long quand même !). De plus, la moyenne des signaux filtrés n’est pas centrée en 0 (ce qui est embêtant pour l’utilisation du LQR par la suite…).

Quand Wheely devient iWheely

Alors, en parallèle du reste, ces derniers jours j’ai aussi finalisé l’application iPhone et le serveur relais en Python. Voici un petit résumé de la joyeuse situation : L’application iPhone fournit des boutons pratiques (pause et connexion) et une IHM sympa (du moins du point de vue de son créateur) pour visualiser la consigne de pilotage données par l’accéléromètre (avec filtrage passe bas pour lisser et une dead zone pour envoyer facilement une consigne « ne bouge pas). Le serveur TCP Python lui attend une connexion de la part de l’application iPhone, typiquement au travers d’un réseau Wifi (ad-hoc ou pas). Il ouvre ensuite une liaison bluetooth profil SPP (RFCOMM), via la fabuleuse bibliothèque lightblue, vers Wheely, et lui transmet les consignes perçues de la part de l’iPhone.

En bref, toute la partie pilotage via iPhone-Wifi-Laptop-Bluetooth du robot est a priori bouclée. Il reste à s’occuper du traitement des consignes coté robot. J’ai aussi fait des tests de portée du Bluetooth, qui sont largement concluants, puisque on obtient une portée de 15m à vue, 10m derrière un mur. Tout ça pour dire que la vie est belle, ou iBelle devrais-je dire.

Kalman farçi, Kalman à l'américaine ...

Aujourd’hui, j’ai fait beaucoup de recherches sur le filtrage de Kalman, afin que ce soit bien clair dans ma tête et pour enfin voir concrètement ce qu’on va faire.

C’est un peu frustrant (surtout à ce stade du projet) de passer une journée où on ne produit rien de concret mais je pense que ça aidera quand même .

J’ai eu de gros moment de doutes dans la journée (notamment sur le calcul d’erreur et la manière dont on l’utilisait pour mettre à jour la variable d’état) qui se sont dissipés au fur et à mesure des lectures. Ca ne fera surement pas de mal non plus de confronter ce que j’en ai compris avec Miguel demain et d’en parler aussi avec Alexis.

Tout d’abord, le mot clé  AHRS (pour Attitude and Heading Reference Systems) m’a ouvert pas mal de portes pour trouver d’autres articles que ceux d’Alexis, certains plus théoriques et aussi des exemples de code. J’ai aussi découvert le projet OpenPilot (dont les sources sont normalement libres, mais je n’ai pu trouver les sources de leur AHRS (!) dans leur dépôt SVN ) et Rotomotion, asez présent sur le net.

Deux liens ici vers des exemples de code que je ne trouve pas parfait non plus (notations peu claires, syntaxe hésitante, utilisation de flottants) mais qui permettent de se donner une idée de ce qu’on devra faire.

Et un article, qui reprend l’idée de considérer le gyroscope comme commande et les accéléromètre comme mesure. Ils utilisent également les quaternions  et donnent les matrices utilisées dans le filtre de Kalman à part les matrices de covariance.

Voilà, j’espère qu’on pourra demain décider finalement ce qu’on met dans notre filtre, éclaircir les dernières zones d’ombre avec Alexis et commencer à le coder !

Quant au code de Mikrokopter, il m’a l’air très artisanal, avec des tests sur les écarts entre gyros et accélérometres et autres. Bref, pas de filtre de Kalman de ce côté là.