accéléromètre et gyroscope MPU6050

lundi 30 novembre 2015, par thebault

La capteur MPU6050 contient à la fois un accéléromètre 3 axes et un gyroscope 3 axes qui communique via i2c (datasheet). Dans cet article, je vais me limiter à déterminer l’angle de rotation autour de l’axe Y.

Les branchements

Récupérer les données "bruts" du capteur.

Sur la page http://playground.arduino.cc/Main/MPU-6050, on a un exemple d’utilisation de la bibliothèque wire.h (i2c) pour récupérer ces données (voir "short example sketch").
Dans la suite de l’article, je vais plutôt utiliser la bibliothèque développée par Jeff Rowberg. Son projet complet est accessible à cette adresse http://www.i2cdevlib.com/.
Pour poursuivre, il faut donc installer les bibliothèque "i2c" et "MPU6050" sous licence MIT qui se trouvent à l’adresse : https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev et https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Télécharger et décompresser dans ../Arduino/librairies les 2 fichiers suivants :


I2Cdev.zip


MPU6050.zip

On utilise le programme suivant pour récupérer les données (les datas s’affichent toutes les 100 ms sur le terminal série) :

#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //bibliothèque I2Cdev à installer
#include "MPU6050.h" //bibliothèque MPU6050 à installer
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
 
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;
 
void setup() {
  Wire.begin();  // bus I2C
  Serial.begin(9600); // liaison série
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
  accelgyro.initialize();  // initialize device
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // On peut aussi utiliser ces méthodes
  //accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getRotation(&gx, &gy, &gz);
 
  // Affichage accel/gyro x/y/z
  Serial.print("a/g:\t");
  Serial.print(ax); 
  Serial.print("\t");
  Serial.print(ay); 
  Serial.print("\t");
  Serial.print(az); 
  Serial.print("\t");
  Serial.print(gx); 
  Serial.print("\t");
  Serial.print(gy); 
  Serial.print("\t");
  Serial.print(gz); 
  Serial.println("\t");
  delay(100);  
}

Télécharger

Déterminer l’angle d’inclinaison autour de l’axe Y à partir du gyroscope.

Le gyroscope nous donne la vitesse angulaire en rad/s. On a donc la relation suivante (Gy, vitesse de rotation autour de l’axe Y, angle=angle d’inclinaison) :

      d(angle)

Gy= --------- d'où d(angle)=Gy.dt donc 

       dt



angle("actuel")=angle("précédent")+Gy.dt

Dans le programme suivant, le dt est fixé à 10ms=0,01s (cf tempo en fin de programme).
Si on lit la documentation, page 12, on voit qu’il faut diviser la valeur lue pour Wy par 131 pour obtenir la vitesse en °/s. Par conséquent, l’équation finale de l’angle est donc : angle=angle+Gy*0.01/131

#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
 
MPU6050 accelgyro;
 
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;
float angle=0;
 
void setup() {
  Wire.begin();  //I2C bus
  //Serial.begin(9600); 
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
  // initialize device
  accelgyro.initialize();
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle=angle+float(gy)*0.01/131; // 131 cf doc MPU6050 et 0.01=dt en s
 
  Serial.println(angle); 
  delay(10);    
}

Télécharger

Malheureusement, au fur et à mesure, le gyroscope dérive et n’est donc pas fiable. Il suffit de partir d’un angle=0°, tourner le capteur de 90° et revenir ... on n’obtient pas 0 !

Déterminer l’angle d’inclinaison autour de l’axe Y à partir de l’accéléromètre.

L’accéléromètre permet d’obtenir l’angle à partir de la mesure du vecteur de pesanteur. Voir schéma :

Dans le programme suivant, on va utiliser la bibliothèque "math.h" pour pouvoir utiliser la fonction "arctan". Le programme arduino est le suivant :

#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
 
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
 
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;
float angle=0;
 
void setup() {
  Wire.begin();  //I2C bus
  //Serial.begin(9600); 
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
 
  // initialize device
  accelgyro.initialize();
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle= atan2((double)ax,(double)az )*180/PI; // 180/PI permet d'avoir la valeur en °
  Serial.println(angle);
  delay(10);
}

Télécharger

Problème : l’accéléromètre est sensible à la moindre vibration. Ainsi, apparaissent des mesures "parasites".

Mesurer l’angle en combinant le gyropode et l’accéléromètre avec un "filtre complémentaire"

On a vu précédemment que le gyroscope dérive et l’accéléromètre présente des mesures brusques "parasites". On va donc combiner les 2 en se débarrassant des variations "lentes" du gyroscope (filtre passe haut) et des variations "rapides" de l’accéléromètre (filtre passe-bas). Pour cela, on utilise un "filtre complémentaire".
Explications sur le filtre complémentaire :http://www.pieter-jan.com/node/11

Ainsi, l’équation de l’angle va être : angle=0,98x(angle +Gy*0.01/131 ) + 0,02x(angle de l’accéléromètre en °)

On peut affiner les coefficients 0,98 et 0,02 du moment que leur somme reste égale à 1

Le programme arduino :

#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
 
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
 
int16_t ax, ay, az;  //mesures brutes
int16_t gx, gy, gz;
uint8_t Accel_range;
uint8_t Gyro_range;
float angle=0;
 
void setup() {
  Wire.begin();  //I2C bus
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
 
  // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro.initialize();
 
  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echec");
  delay(1000);
}
 
void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle=0.98*(angle+float(gy)*0.01/131) + 0.02*atan2((double)ax,(double)az)*180/PI;
  Serial.println(angle); 
  delay(10);
}

Télécharger

Connecter 2 capteurs MPU6050 sur le bus i2c

On peut mettre 2 capteurs sur le bus i2c. Pour cela, il suffit d’agir sur la broche AD0 :
si AD0=0 (ou laissé en l’air) : l’adresse du capteur en hexadécimal est 0x68,
si AD0=1, l’adresse du capteur en hexadécimal est 0x69

Ainsi, si l’on souhaite utiliser 2 capteurs, on les relier a sur le bus i2c de la façon suivante :

Le programme arduino.

On crée 2 instances de l’objet MPU6050 :

accelgyro1(0x68)
accelgyro2(0x69)

#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
 
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro1(0x68);
MPU6050 accelgyro2(0x69);
 
int16_t ax, ay, az;  //mesures brutes capteur 1
int16_t gx, gy, gz;
int16_t ax2, ay2, az2;  //mesures brutes capteur 2
int16_t gx2, gy2, gz2;
float angle1 = 0;
float angle2 = 0;
 
 
void setup() {
  Wire.begin();  //I2C bus
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }
 
  // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro1.initialize();
  accelgyro2.initialize();
 
  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro1.testConnection() ? "MPU6050 n°1 connection reussie" : "MPU6050 n°1 connection echec");
  Serial.println(accelgyro2.testConnection() ? "MPU6050 n°2 connection reussie" : "MPU6050 n°2 connection echec");
 
  delay(1000);
}
 
void loop() {
 
  accelgyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
  angle1 = 0.98 * (angle1 + float(gy1) * 0.01 / 131) + 0.02 * atan2((double)ax1, (double)az1) * 180 / PI;
 
  accelgyro2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
  angle2 = 0.98 * (angle2 + float(gy2) * 0.01 / 131) + 0.02 * atan2((double)ax2, (double)az2) * 180 / PI;
 
  Serial.print(angle1);
  Serial.print("\t");
  Serial.println(angle2);
  delay(10);
}