Cours:Accelerometre

Retour à la liste des Tps

Éléments de correction (page protégée)

Le but de ce projet est de comprendre et utiliser un capteurs moderne d'accélération.

Introduction aux capteurs IMU

IMU : Inertial Measurement Unit se traduit en français par Centrale à Inertie.

Le capteur que nous allons utiliser porte la référence GY-521 et est architecturé autour d'un MPU 6050 qui est composé de deux capteurs et un processeur :

• un capteur accéléromètre 3 axes (x,y et z) qui mesure une accélération
• un capteur gyroscope 3 axes qui mesure une vitesse angulaire
• Digital Motion Processor (DMP) capable de stocker des données et les restituer

La terminologie anglo-saxonne associée pour ce genre de capteur est 6 DOF (Degree of Freedom). On devrait traduire par 6 degrés de liberté mais il est préférable de traduire par 6 axes (3 pour l'accéléromètre et 3 pour le gyroscope). Le meilleur de ce qui se fait actuellement comporte 9 axes. En général c'est un magnétomètre qui est ajouté pour essayer de repérer le Nord magnétique. Ces 9 axes sont en général suffisants pour réaliser, en combinant les mesures de ces trois capteurs, une centrale à inertie de coût modeste.

Mais ces trois capteurs ont des défauts différents ce qui rend leur utilisation délicate.

Nous allons nous contenter de six axes dans la suite de ce projet.

La notion d'attitude

L'attitude en robotique (et en astronautique) désigne la direction des axes de la pièce mobile du robot. Elle est en général caractérisée par trois angles (roulis, tangage et cap ou lacet).

Ce mot d'attitude ne doit pas être confondu avec l'altitude ! Ces deux mots n'ont absolument rien en commun (à part l'orthographe voisine) !

Utiliser l'accéléromètre seul pour trouver l'attitude

Nous espérons montrer dans cette section que l'accéléromètre est un capteur lent mais surtout qu'il est sensible à l'accélération de pesanteur. C'est cette propriété du capteur que l'on va utiliser pour trouver l'attitude. En effet on sait que cette pesanteur est toujours dirigée vers le bas.

Une autre manière de dire les choses est que si la seule accélération est celle de la pesanteur (pas d'autres forces que le poids) alors la projection de cette accélération sur les trois axes permet trouver l'attitude. On réalisera ce genre de calcul plus tard.

Partie Arduino

La première question à se poser est de savoir comment câbler l'Arduino au capteur qui nous intéresse ?

Câblage

Le câblage d'un Arduino UNO avec le GY-521 se trouve ICI. On ne reproduira donc pas ce schéma.

Exercice 1

1°) Nous allons commencer par le programme tout simple :

// MPU-6050 Short Example Sketch
    // By Arduino User JohnChi
    // August 17, 2014
    // Public Domain
    #include<Wire.h>
    const int MPU=0x68;  // I2C address of the MPU-6050
    int16_t AcX,AcY,AcZ;
    void setup(){
      Wire.begin();
      Wire.beginTransmission(MPU);
      Wire.write(0x6B);  // PWR_MGMT_1 register
      Wire.write(0);     // set to zero (wakes up the MPU-6050)
      Wire.endTransmission(true);
      Serial.begin(9600);
    }
    void loop(){
      Wire.beginTransmission(MPU);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Serial.print("AcX = "); Serial.print(AcX);
      Serial.print(" | AcY = "); Serial.print(AcY);
      Serial.print(" | AcZ = "); Serial.println(AcZ);
      delay(333);
    }

Essayez ce programme sur la liaison série.

La documentation du MPU 6050 dit que la valeur en g de l'accélération se trouve en divisant par 16384 (voir page 13). Êtes-vous d'accord avec la documentation ? Préciser votre méthode pour répondre à la question.

2°) Vous allez reprendre maintenant la mise en œuvre de la première question pour calculer pour chacun des axes la correction d'amplification ainsi que celle de l'offset. La correction d'amplification se trouve en mesurant la valeur de g pour un axe puis en retournant l'axe. La différence des deux valeurs divisée par 2*16384 donne le facteur de correction.

• Si celui-ci est proche de 1 on ne fera aucune correction d'amplification.
• L'offset sera pris entre les deux valeurs mesurées, pour que chacune des deux valeurs soit le plus proche possible de 16384 (au signe près).

Les valeurs trouvées sont à noter quelque part.

Exercice 2

On va maintenant se servir de la position de l'accélération de pesanteur par rapport à nos axes pour essayer de trouver l'attitude de notre repère.

On va d'abord simplifier le problème en le ramenant en deux dimensions. L'accéléromètre que vous utilisez a ses deux axes X et Y clairement indiqués. Naturellement l'axe Z est perpendiculaire à ces deux axes.

On va s'intéresser à une rotation autour de l'axe Y (tangage). L'accéléromètre devra donc rester horizontal par rapport à l'axe X !!!

Projection de l'accélération de pesanteur sur deux axes X et Z

1°) A l'aide de la figure explicative ci-dessous, on vous demande de retrouver l'angle de tangage de votre repère. Utilisez les résultats de la question 2° de l'exercice 1 pour affiner vos résultats.

Note 1 : Il est possible que vous considériez l'axe Z dans l'autre sens ! Tout dépend comment vous tenez l'accéléromètre.

Note 2 : A ce stade il vous est possible de réaliser la première question de l'exercice 3 avant de faire la suite de l'exercice en cours.

Voir aussi : AN-1057. Pour cette note d'application, ce que nous venons de faire est une mesure deux axes. Elle présente aussi une mesure un axe avec ses défauts et prépare à la suite du problème, la mesure 3 axes.

Nous allons donc maintenant essayer de généraliser en trois dimensions.

2°) Modifier ce programme pour sortir la direction verticale. Voici schématiquement ICI sur Internet les calculs que l'on a à faire.

Le travail de la question 2°) est facile à réaliser. Pourtant il n'est en aucun cas satisfaisant ! C'est ce que nous montrons sur la figure ci-contre.

Pourquoi la connaissance des composantes de l'accélération de pesanteur ne suffisent plus en 3D ?

Nous rappelons que la question à laquelle nous cherchons à répondre est :

• est-il possible avec la simple connaissance des trois composantes de g (gx,gy et gz) de retrouver l'orientation du repère dans lequel les mesures sont faites.

Dans cette figure, nous avons dessiné un parallélépipède en vert, les axes x, y et z et le vecteur g d'accélération de pesanteur (qui est la diagonale du parallélogramme. Imaginez cet ensemble comme un solide et faites-le tourner autour de l'axe vertical d'un angle quelconque. Alors aucune composante de g ne change mais le repère n'est plus le même. Ainsi la connaissance de gx, gy, gz ne suffira pas pour trancher parmi tous les repères possibles, lequel est le bon. Votre accéléromètre est insensible aux rotations verticales si l'on s'en tient aux mesures de l'accélération de pesanteur.

Partie Processing

On désire réaliser avec Processing une visualisation des données de l'exercice 2 précédent.

Exercice 3

On va chercher à visualiser concrètement les résultats de l'exercice précédent. On va naturellement commencer par le plus simple, la 2D.

1°) Construire un programme simple de visualisation pour la question 1°) de l'exercice 2. Vous pouvez pour simplifier commencer par n'envoyer que l'angle par la liaison série et plus du tout les composantes de l'accélération.

Indication : Voici un programme capable de récupérer la chaîne "Phi = -0.67" envoyée par l'Arduino, de retirer le baratin "Phi = " et de la transformer en nombre réel :

import processing.serial.*;

Serial port;                         // The serial port
int lf = 10;    // Linefeed in ASCII
int serialCount=0;
float phi;
String myString=null;
char index;

void setup() {
    // ne fonctionne pas sous Windows !!!
    String portName = "/dev/ttyACM0";   
    port = new Serial(this, portName, 9600);
    frameRate(4);
}

void draw() {
  String floatString=null;
  while (port.available() > 0) {
     myString = port.readStringUntil(lf);
  }
  if (myString != null) {
       // On retire les 6 premiers caractères
       floatString = myString.substring(6); 
       print(floatString);print(" converti en : ");
       // la fonction float() fait tout le boulot
       phi = float(floatString);
       println(phi);
   }  
}

2°) Construire ensuite le même programme en vous intéressant à la 3D. Vous allez pour cela prendre le programme de la question 2°) de l'exercice 2 en lui retirant le calcul des angles qui sera déporté dans Processing.

3°) Essayez de vous convaincre que lorsque l'accéléromètre est immobile la direction donnée est correcte (quelle que soit cette direction). Votre programme de visualisation devra maintenant détecter l'immobilité et du coup réajuster ses angles à ce moment là.

Utiliser le gyroscope seul pour trouver l'attitude

Nous espérons montrer dans cette section que le gyromètre est un capteur rapide mais qu'il possède un inconvénient, c'est qu'il dérive !

Nous allons aussi garder la partie Processing de la section précédente puisque la représentation du problème graphique est la même.

Un programme Arduino pour commencer

On donne :

   // MPU-6050 Short Example Sketch
    // By Arduino User JohnChi
    // August 17, 2014
    // Public Domain
    #include<Wire.h>
    const int MPU=0x68;  // I2C address of the MPU-6050
    int16_t GyX,GyY,GyZ;
    void setup(){
      Wire.begin();
      Wire.beginTransmission(MPU);
      Wire.write(0x6B);  // PWR_MGMT_1 register
      Wire.write(0);     // set to zero (wakes up the MPU-6050)
      Wire.endTransmission(true);
      Serial.begin(9600);
    }
    void loop(){
      Wire.beginTransmission(MPU);
      Wire.write(0x43);  // starting with register 0x43 (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU,6,true);  // request a total of 14 registers
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
      Serial.print("GyX = "); Serial.print(GyX);
      Serial.print(" | GyY = "); Serial.print(GyY);
      Serial.print(" | GyZ = "); Serial.println(GyZ);
      delay(333);
    }

Exercice 4

1°) Montrer que ce programme fonctionne.

ATTENTION un gyroscope est sensible à la vitesse de rotation (et non pas à l'angle) !!! D'où la nécessité de calcul de l'intégrale pour obtenir l'angle.

2°) Réaliser une intégration des vitesses angulaires (comment faire ?) pour trouver les angles.

Coupler l'accéléromètre et le gyroscope

Puisqu'on a pu noter des qualités et défauts des deux capteurs d'accélération de de vitesse angulaire, il est légitime de se demander si l'utilisation de ces deux capteurs peut pas produire un capteur global de meilleure qualité. C'est ce que nous allons essayer d'examiner maintenant.

Un programme simple pour commencer

Nous donnons un programme très simple qui relève les donnée de l'accéléromètre et celles du gyroscope. Remarquez que vous avez en bonus la température.

   // MPU-6050 Short Example Sketch
    // By Arduino User JohnChi
    // August 17, 2014
    // Public Domain
    #include<Wire.h>
    const int MPU=0x68;  // I2C address of the MPU-6050
    int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
    void setup(){
      Wire.begin();
      Wire.beginTransmission(MPU);
      Wire.write(0x6B);  // PWR_MGMT_1 register
      Wire.write(0);     // set to zero (wakes up the MPU-6050)
      Wire.endTransmission(true);
      Serial.begin(9600);
    }
    void loop(){
      Wire.beginTransmission(MPU);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU,14,true);  // request a total of 14 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
      Serial.print("AcX = "); Serial.print(AcX);
      Serial.print(" | AcY = "); Serial.print(AcY);
      Serial.print(" | AcZ = "); Serial.print(AcZ);
      Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
      Serial.print(" | GyX = "); Serial.print(GyX);
      Serial.print(" | GyY = "); Serial.print(GyY);
      Serial.print(" | GyZ = "); Serial.println(GyZ);
      delay(333);
    }

Annexe

On donne ici quelques utilitaires de calcul :

 /**************************************************/
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]){
  float op[3]; 
  for(int x=0; x<3; x++){
    for(int y=0; y<3; y++){
      for(int w=0; w<3; w++){
       op[w]=a[x][w]*b[w][y];
      } 
      mat[x][y]=0;
      mat[x][y]=op[0]+op[1]+op[2];
      
      float test=mat[x][y];
    }
  }
}

//Computes the dot product of two vectors
float Vector_Dot_Product(float vector1[3],float vector2[3]){
  float op=0;
  
  for(int c=0; c<3; c++){
  op+=vector1[c]*vector2[c];
  }
  
  return op; 
}

//Computes the cross product of two vectors
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]){
  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
}

//Multiply the vector by a scalar. 
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2){
  for(int c=0; c<3; c++){
   vectorOut[c]=vectorIn[c]*scale2; 
  }
}

void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]){
  for(int c=0; c<3; c++){
     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
  }
}

/**************************************************/
void Normalize(void){
  float error=0;
  float temporary[3][3];
  float renorm=0;
  
  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19

  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
  
  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
  
  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}

/**************************************************/
void Drift_correction(void){
  float mag_heading_x;
  float mag_heading_y;
  float errorCourse;
  //Compensation the Roll, Pitch and Yaw drift. 
  static float Scaled_Omega_P[3];
  static float Scaled_Omega_I[3];
  float Accel_magnitude;
  float Accel_weight;
  
  
  //*****Roll and Pitch***************

  // Calculate the magnitude of the accelerometer vector
  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
  // Dynamic weighting of accelerometer info (reliability filter)
  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  

  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
  
  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
  
  //*****YAW***************
  // We make the gyro YAW drift correction based on compass magnetic heading
 
  mag_heading_x = cos(MAG_Heading);
  mag_heading_y = sin(MAG_Heading);
  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
  
  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
  
  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
/**************************************************/
/*
void Accel_adjust(void){
 Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
 Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
}
*/
/**************************************************/

void Matrix_update(void){
  Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
  Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
  Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
  
  Accel_Vector[0]=accel_x;
  Accel_Vector[1]=accel_y;
  Accel_Vector[2]=accel_z;
    
  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

  //Accel_adjust();    //Remove centrifugal acceleration.   We are not using this function in this version - we have no speed measurement
  
 #if OUTPUTMODE==1         
  Update_Matrix[0][0]=0;
  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
  Update_Matrix[1][1]=0;
  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
  Update_Matrix[2][2]=0;
 #else                    // Uncorrected data (no drift correction)
  Update_Matrix[0][0]=0;
  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
  Update_Matrix[1][1]=0;
  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
  Update_Matrix[2][2]=0;
 #endif

  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c

  for(int x=0; x<3; x++) //Matrix Addition (update){
    for(int y=0; y<3; y++){
      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
    } 
  }
}

void Euler_angles(void){
  pitch = -asin(DCM_Matrix[2][0]);
  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}

Aussi sur Internet

• Le GY-521 architecturé autour d'un MPU 6050 peut être trouvé pour 3€25 chez Amazon
• MPU-6050 Accelerometer + Gyro page qui contient un programme d'exemple.
• gyroscope et accéléromètre dans une puce
• Tilt Sensing Using Linear Accelerometers
• The Balance Filter