MPU6050 et filtrage numérique

Cahier des charges :

Notre objectif est d'obtenir un capteur fiable pour maintenir en équilibre notre robot Gyropode (Balancing Robot).

C'est le même procédé qui est utilisé sur un drone, mais au lieu de travailler que sur le tangage,
il faut aussi prendre en compte le roulis et le lacet.

Nous utiliserons un MPU6050 afin de calculer l'angle de notre robot par rapport à la position d'équilibre.

Cet angle sera la base de notre régulation PID pour maintenir en équilibre notre robot Gyropode.

Il doit être peu bruité, peu sensible aux vibrations et dynamique (temps de réponse rapide).

Pour cela nous disposons sur la même puce (MPU6050) :

d'un accéléromètre qui est stable dans le temps mais génére du bruit (erreur aléatoire).
De plus lorsque le robot fonctionnera, les accélérations et les vibrations vont fausser le calcul de l'angle.
d'un gyroscope qui donne une mesure dynamique très fidèle,
mais a tendance à dériver dans le temps ou lors de variation brusque.

Ces 2 mesures complémentaires nous permettront de calculer un angle fiable.

Pour cela il faudra :

Rafraichir le calcul de notre angle à une cadence de 5 ms
Mesurer l'angle de tangage à l'aide de l'accéléromètre (angle_acc)
Réduire le bruit grâce à un filtre numérique
Prédire l'angle attendu grâce à la vitesse angulaire Gy du gyroscope
Appliquer un filtre complémentaire pour calculer un angle fiable et dynamique :

angle_gyro = K*angle_acc + (1-K)*(angle_gyro+Gy*dt)

K est un réel compris entre 0 et 1

Objectif du TP : obtenir un angle de tangage exploitable pour une régulation.

1. Mesure de l'angle à l'aide d'un accéléromètre

1. A l'aide d'un schéma, montrer comment un accéléromètre permet de calculer l'angle de tangage de notre robot.

conseil : consulter cette page du projet TPIL Ballon Sonde

2.Adapter la formule en fonction de la position du capteur sur le robot.
Déterminer angle = f(ax,ay,az)

3. Le MPU6050 est un capteur MEMS très petit et à bas coût.
Regarder cette vidéo du CEA Leti .
Définir en quelques lignes les acronymes MPU et MEMS.

4. Cabler un MPU6050 sur une platine d'essai vers un Arduino.

5. Tester votre MPU6050 à l'aide d'un programme Exemple de la bibliothèque MPU6050,
ou en vous inspirant du programme ci-dessous :

#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC communication library

MPU6050 mpu;                    //Création de l'objet mpu
float dt=0.005;                 // Pour cadencer la boucle à 5 ms
float angle_acc;
int ax, ay, az, gx, gy, gz;     //3 accélérations et 3 vitesses angulaires
unsigned long t0,i;

void setup() 
{
  Wire.begin();       //Initialisation bus I2C
  Serial.begin(9600);   
  delay(1000);
  mpu.initialize();  //initialisation MPU6050
  delay(2);
}

void loop() 
{
  t0=micros();  // Top départ pour boucle cadencée à dt
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  // Récupérer les 6 axes  ax ay az gx gy gz
  angle_acc = f(ax,ay,az);                       // Remplacer f(ax,ay,az) par votre formule
  if (i%20==0) {                                 // afficher angle toutes les 20*5=100ms
    Serial.println(angle_acc);}
  i++;
  while (micros()-t0<dt*1e6);     // boucle cadencée à dt en µs
}

6. Ecrire un programme Arduino calculant toutes les 5 ms l'angle en °.
Afficher sur le moniteur série l'angle toutes les 100ms.

Appel 1 : Faites valider votre programme
de mesure d'angle de tangage.

2. Monitorage sous Labview

7. Réaliser un programme Labview permettant d'afficher sur un graphique l'angle de tangage.
Prévoir la transmission d'une deuxième valeur (filtrée).

L'angle calculé est bruité (erreur aléatoire de l'accéléromètre).
Pour diminuer ce bruit nous allons devoir réaliser un filtrage numérique.
L'objectif est de diviser par 10 l'écart-type.

Le filtre numérique le plus simple à réaliser est un filtre moyenneur.

8. En faisant le lien avec votre cours de métrologie, pour une moyenne glissante de 4 mesures,
l'incertitude-type de cette moyenne est divisée par combien ?

- Écart-type expérimental (sur la série de mesures, caractérise la fidélité d’une chaîne de mesure…) :

- Incertitude-type (= écart-type sur la moyenne, caractérise la justesse d’une chaîne de mesure…) :

9. Implémenter sur l'arduino un filtre moyenneur qui effectue la moyenne glissante des N dernières mesures.

On peut créer un tableau contenant les N=4 derniers angles calculés :
On remplit celle file d'attente (First In First Out) au fur à mesure des cycles :
On calcule la moyenne des N=4 valeurs de la Fifo :

10. En vous inspirant de la face avant ci-contre, afficher sur votre Face Avant les 2 courbes sur le même graphe.
     Ajouter une commande qui permet de rendre visible ou non la courbe souhaitée.
                   Ajouter un bouton qui remet à zéro toutes les données stockées.

                                                Calculer sur les 100 dernières valeurs l'écart type des 2 angles transmis.

11. Lorsque le capteur ne bouge pas, quel est écart-type le plus petit ?
      Proposer une solution pour diviser par 10 l'écart-type de l'angle calculé.
       Modifier votre programme et vérifier que l'écart-type est bien divisé par 10.
       Quel est l'inconvénient de cette solution ?

12. Tapez sur la table pour créer une vibration.
Que constatez-vous ?

Appel 2 : Faites valider votre VI Labview par le professeur.

3. Utilisation du gyroscope

Le filtre moyenneur est très efficace en statique.

Par contre il introduit un retard en dynamique :
comme un filtre moyenneur qui introduit un retard de phase en régime sinusoïdal.

De plus les accélérations et vibrations rendent moins pertinent angle_acc .

Pour réduire le bruit sans introduire de retard nous allons utiliser le gyroscope.

Pour déterminer la vitesse angulaire en °/s nous consultons le datasheet p12
la sensibilité ΔN/ΔG en LSB/(°.s^-1) est de 131
Gy(°.s^-1) = gy (LSB) / 131( LSB/(°.s^-1) ) => Gy=gy/131;

Pour ne pas être décalé, il faut corriger l'offset du gyroscope :

Gy = (gy+offset_gy ) / 131;

Il faudra déterminer offset_gy avec ce programme par exemple.

Le Gyroscope est très performant pour trouver l'angle du cycle suivant (dt=5ms) :
angle_gyro = angle_gyro+Gy*dt

Par contre il a tendance à dériver au fur à mesure des cycles (il y a 200 cycles par seconde).
Nous allons donc réaliser une moyenne pondérée avec l'angle de l'accéléromètre :
angle_gyro = K*angle_acc + (1-K)*(angle_gyro+Gy*dt)

K est un facteur compris entre 0 et 1.
K=0,40 implique que l'accéléromètre contribue à 40% du calcul de l'angle.

13. Il est facile de déterminer l'offset du gyroscope :
      il suffit de ne pas bouger et de faire la moyenne sur N mesures du retour gyroscope.
      Si on ne corrige pas ce défaut nous risquons d'avoir une dérive (offset*t) lors du calcul de l'angle.
      A l'aide de ce programme déterminer les offset du gyroscope.

int offset_gx=492,offset_gy=-32, offset_gz=55;

14. Implémenter ce nouveau filtre dans le programme Arduino.
Des difficultés ? -> Struture du programme à compléter

L'angle gyroscope est obtenu à l'aide de l'intégration de la vitesse angulaire.
La constante d'intégration dépend des CI (Conditions Initiales)
Nous utilisons l'accéléromètre et le filtre moyenneur pour déterminer l'angle initial.

Conseil pour initialiser l'angle_gyro :

Cela revient, en analyse numérique, à ajouter Gy*dt à l'angle_gyro précédent.
angle_gyro = angle_gyro+Gy*dt
Pour être précis, il faut que dt=5ms soit petit devant les constantes de temps mécaniques.

Afficher dans le moniteur série les 3 angles.

Pour changer facilement la valeur de K prévoir :
- côté Labview une commande pour modifier K
- côté Arduino, une prise en compte de cette commande

14. Modifier le VI Labview pour monitorer les 3 angles
      Optimiser votre VI pour mettre en évidence :
       - la diminution du bruit par rapport à angle_acc
       - l'amélioration du temps de réponse par rapport à moyenne

        Montrer la dérive lorsque K=0 (capture d'écran à joindre)
        Faites varier K pour optimiser la réduction de bruit et éviter la dérive.

Remplir le tableau récapitulatif suivant :
- : insuffisant + : correct ++ : bon +++ : très bon

Grandeur d'intérêt : Angle de tangage Grandeurs mesurées : Ax, Ay, Az, Gx, Gy, Gz

Capteur	Accéléromètre		Gyroscope	*KAcc. + (1-K) Gyro.**
Filtre	Mesure directe	Filtre moyenneur	K = 0	Complément
Justesse	++
Fidélité	-
Rapidité	++
Robustesse	-

Ce capteur va nous servir à équilibrer le robot Gyropode.
Un critère insuffisant est éliminatoire.

15. Quelle solution choisir pour que notre régulation soit :
            - précise (robot équilibré)
            - stable (peu d'oscillations)
            - rapide (en cas de perturbation le robot retrouve sa position d'équilibre rapidement)
            - robuste (le robot récupère des perturbations importantes)

Appel 3 : Faites valider cette étape par le professeur.

4. La régulation d'angle de tangage

Les drones utilisent en général un filtre plus sophistiqué : le filtre de Kalman
Le gyroscope permet d'établir la dynamique du robot et de faire des prédictions fiables.

Avec un bon paramétrage de K et un bon réglage des offset, notre filtre complémentaire
est quasiment aussi performant que le filtre de Kalman proposé par Keyestudio.

Il ne reste plus qu'à programmer notre régulation :

void regulation(){
  P= ????;
  D= ????;
  PWM=P+D;
  // Attention action opposée
  if (balancing) moteur(-PWM,-PWM);
        else moteur(0,0);
}

et déterminer expérimentalement les paramètres Kp et Kd

Sur ce lien la fonction void moteur(int droite,int gauche) est disponible.

Il ne reste pus qu'à écrire les fonctions :

- void calcul_angle() : calcul de l'angle_gyro

- void affiche() : affiche pour Labview (liaison série) angle_acc;angle_gyro;P;D;

- void reception() : disponible après void moteur(int droite,int gauche)
Côté Labview il faudra écrire sur la liaison série : mode;consigne;Kp;Kd;

- void commute() : disponible après void moteur(int droite,int gauche)
pour passer en mode balancing ou en mode repos à l'aide du Bouton Poussoir 13

void setup() 
{
  Wire.begin();               //Bus I2C
  Serial.begin(9600);        //Pour communiquer avec Labview
  iniPin();                  //Initialisation pinMode du shield     
  delay(1400);
  digitalWrite(buz,HIGH);
  mpu.initialize();      //Initialisation MPU6050
  delay(10);
  digitalWrite(buz,LOW);
}

void loop() 
{
  t0=micros();  // top départ boucle
  calcul_angle();
  regulation();
  if (i%20==0) affiche();     // Affiche tous les 20*5=100ms
  i++;
  if (Serial.available())reception();
  if (!digitalRead(BP) && i-iBP>50) commute();
  if (i-recu>3)digitalWrite(buz,LOW);
  while (micros()-t0<dt*1e6);
}

Attention : durant la mise au point le robot peut avoir des réactions impévisibles !

Une erreur de signe et le robot part à pleine vitesse dans la direction opposée.

Tenez-vous prêt à rattraper le robot !

Et réaliser l'interface Labview adaptée...

5. Cap d'afficher le cap ?

Avec 2 lignes de code nous pouvons calculer l'angle de lacet.
Avec une boussole pour déterminer la constante d'intégration, cet angle nous donne le cap suivi.

Cet angle nous sera utile pour contrôler les virages.

16. Modifier le programme du robot pour calculer et afficher l'angle de lacet.
Prévoir une remise à zéro de cet angle. Par exemple lors de l'appel du mode 4.
Modifier l'application Labview pour afficher cet angle.