Découvrez notre nouvelle Chaîne YouTube "Ingénierie & Bourse"

Objectifs

  1. Savoir configurer les registres du module MPU-6050 GY-521
  2. Savoir implémenter la boucle d’asservissement d’un drone Bicopter (voir la figure ci-dessous)
  3. Savoir l’effet de l’action proportionnelle sur l’équilibre du drone
  4. Savoir la différence entre l’asservissement d’angle avec l’accéléromètre ou bien le gyroscope
  5. Savoir implémenter une intégration numérique avec Arduino
  6. Voir le tuto pour plus de détails

Boucle de régulation drone bicopter avec Arduino

Câblage et initialisation du Module MPU-6050 GY-521

Veuillez consulter l’article dédié au module pour plus de détails

Câblage

  •   VCC -> 3.3 V / 5 V (préférable)
  •   GND -> GND
  •   SCL -> A5 (Uno, Nano, Mini) (Pin SCL pour le Mega)
  •   SDA -> A4 (Uno, Nano, Mini) (Pin SCL pour le Mega)
  •   XDA -> NC (non connecté)
  •   XCL -> NC
  •   ADO -> NC
  •   INT -> NC

Définition de l’adresse du module et LSB du Gyroscope

#define MPU         0x68

#define LSB_Gyro    65.5 // +/- 500°/s sur 16 bits

Configuration de l’Horloge interne à  8MHz

  Wire.begin();                 // Début transmission

  Wire.beginTransmission(MPU);  // Adresse du Module

  Wire.write(0x6B);             // Adresse du registre de control

  Wire.write(0);                // Valeur

  Wire.endTransmission(true);   // Fin de transmission

  delay(10);                    // Retard avant la nouvelle transmission

Configuration de la sensibilité du Gyroscope à  +/- 500°/s

  Wire.beginTransmission(MPU);

  Wire.write(0x1B);            

  Wire.write(0x08);            

  Wire.endTransmission();      

  delay(10);

Configuration de la sensibilité d’Accéléromètre à : +/- 8g

  Wire.beginTransmission(MPU);

  Wire.write(0x1C);           

  Wire.write(0x10);            

  Wire.endTransmission();      

  delay(10);

Configuration de la bande passante du Filtre passe-bas (Gyro/Accelo): 260Hz/ <1ms Delay

  Wire.beginTransmission(MPU);

  Wire.write(0x1A);            

  Wire.write(0x00);         

  Wire.endTransmission();      

  delay(10);

Initialisation du port série (Vitesse >2500000 afin de réduire la latence de transmission)

  Serial.begin(250000);

Affectation du pin au servomoteur

  M1.attach(PinM1);

  M2.attach(PinM2);

La mesure de l’angle par le Gyro

Principe

Les gyroscopes sont utilisés comme capteur de position angulaire, alors que les gyromètres sont des capteurs de vitesse angulaire. Le gyroscope donne la position angulaire (selon un ou deux axes uniquement) de son référentiel par rapport à un référentiel inertiel (ou galiléen). Lire la suite…

L’obtention de l’angle peut être donc réaliser en utilisant l’intégration numérique de la vitesse angulaire. Dans ce tuto on va utiliser la technique de Simpson pour intégrer une fonction. La technique est analogue à la moyenne glissante d’une fonction sur 3 échantillons (voir la figure ci-dessous).

Méthode de simpson approximation

h=b-a=2*Te, Te étant la période d’échantillonnage

Méthode de simpson 1

Voir le tuto “Comment intégrer une fonction avec Arduino” pour plus de détails. 

La fonction AngleGyro()

La fonction AngleGyro() permet de convertir les données bruts issues du gyroscope en angles dans les trois axes. Elle est basée sur l’intégration de Simpson dans l’axe x, y et z. Ci-dessous les étapes de la mesure des angles :

  • Calcul des valeurs réelles (division par la sensibilité)
  x= (double)GyAccTempp[4]/LSB_Gyro;

  y= (double)GyAccTempp[5]/LSB_Gyro;

  z= (double)GyAccTempp[6]/LSB_Gyro;
  • Intégration – Simpson:

F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6

  h=2.0*Te_s; 
  X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0; 
  Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
  Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;
  • Mise à jour des échantillons dans les trois axes
  xyz[0][1]=xyz[0][0];

  xyz[0][0]=x;




  xyz[1][1]=xyz[1][0];

  xyz[1][0]=y;




  xyz[2][1]=xyz[2][0];

  xyz[2][0]=z;
  • Conversion des angles du Radian en degré
  GyPitchRol[0] = X * (180.0/PI);

  GyPitchRol[1] = Y * (180.0/PI) ;

  GyPitchRol[2] = Z * (180.0/PI) ;

La période d’intégration est l’inverse de la période d’échantillonnage!  Elle doit Être assez grande pour une bonne intégration et assez petite pour un système rapide!

void AngleGyro(long *GyAccTempp,  double *GyPitchRol, double Te_s)
{  
  double x,y,z, X, Y, Z, h;
  static double xyz[3][2]; 
  
  // 1. Calcul des valeurs réelles 
  x= (double)GyAccTempp[4]/LSB_Gyro;
  y= (double)GyAccTempp[5]/LSB_Gyro;
  z= (double)GyAccTempp[6]/LSB_Gyro;

  // 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6  
  h=2.0*Te_s; 
  X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0; 
  Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
  Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;

  // 2.2 Mise à jour des échantillons  
  xyz[0][1]=xyz[0][0];
  xyz[0][0]=x; 

  xyz[1][1]=xyz[1][0];
  xyz[1][0]=y; 

  xyz[2][1]=xyz[2][0];
  xyz[2][0]=z; 
  
  // 3. Conversion des angles du Radian en degré
  GyPitchRol[0] = X * (180.0/PI);
  GyPitchRol[1] = Y * (180.0/PI) ;
  GyPitchRol[2] = Z * (180.0/PI) ;
}

A retenir

  • L’action proportionnel est facile à mettre en pratique. Elle ne nécessite pas la connaissance des paramètres physiques du système pour effectuer la commande (Wn, dépassement, etc.). Elle est utilisée pour augmenter la rapidité du système et réduire un peu l’erreur statique (k>1). En revanche elle a tendance à déstabiliser le système.
  • La mesure de l’ange basé sur l’accélération semble inefficace. Le capteur est très sensible aux vibrations durant le décollage du drone. Les mesures sont inutiles face aux vibrations des moteurs. En revanche, la mesure de l’angle par le gyroscope reste peu sensible aux bruits des vibrations. On peut encore améliorer la stabilité des mesure en utilisant une moyenne glissante (voir le tuto). On maintient la mesure par le Gyro dans les tutos à venir pour l’asservissement de l’angle.

Une intégration numérique d’ordre supérieur (plus de points) peut être utilisée pour réduire naturellement l’effet du bruit !

  • L’action (P) n’améliore pas la stabilité du drone. On verra dans le tuto prochain l’action PI et avance de phase.

Le programme complet

#include <math.h>
#include <Servo.h>
#include <Wire.h>

#define NumData     7
#define pi          3.1415926535897932384626433832795  
#define MPU         0x68
#define LSB_Gyro    65.5 // +/- 500°/s sur 16 bits 
#define PinM1       11
#define PinM2       12


#define   Fn      10.00
#define   Zeta    0.05 //0.70710678118
#define   K       1.0
#define   T_ms    1

double Wn=2.0*PI*Fn;
double T_s=(double)T_ms/1000.0;


// Pinout 
/*
  VCC -> 3.3 V / 5 V (préférable)
  GND -> GND
  SCL -> A5 (Uno, Nano, Mini)
  SDA -> A4 (Uno, Nano, Mini)
  
  XDA -> NC (non connecté)
  XCL -> NC 
  ADO -> NC
  INT -> NC
*/

Servo M1, M2;
double Vitesse_M1,Vitesse_M2;  
long  GyAccTemp[NumData];
double PitchRoll[3]; 

double x_nn=0.0;      // Consigne de l'Angle à l'équilibre (Fixe: 0°)! 
double V0=40.0;        // Consigne de la Vitesse (Variable par l'utilisateur: Télécommande)   
double y_n;        
double eps_n;         // Erreur
double y_capt;        // Sortie du capteur   
double y_corr;        // Sortie du correcteur   

// Variables internes du système
double x1[2], y1[3]; 

// Variables internes du correcteur 
double x_c[2], y_c[2]; 

void setup()
{
  // Init module GY-512
  // 1.  Horloge interne: 8MHz
  Wire.begin();                 // Début transmission
  Wire.beginTransmission(MPU);  // Adresse du Module 
  Wire.write(0x6B);             // Adresse du registre de control 
  Wire.write(0);                // Valeur 
  Wire.endTransmission(true);   // Fin de transmission 
  delay(10);                    // Retard avant la nouvelle transmission 
  
  // 2. Gryro +/- 500°/s
  Wire.beginTransmission(MPU); 
  Wire.write(0x1B);             
  Wire.write(0x08);             
  Wire.endTransmission();       
  delay(10); 

  // 3. Accelo: +/- 8g 
  Wire.beginTransmission(MPU); 
  Wire.write(0x1C);            
  Wire.write(0x10);             
  Wire.endTransmission();       
  delay(10);

  // 4. Filtre passe-bas (Gyro/Accelo): 260Hz/ <1ms Delay
  Wire.beginTransmission(MPU); 
  Wire.write(0x1A);             
  Wire.write(0x00);          
  Wire.endTransmission();       
  delay(10);
  
  // Init port série 
  Serial.begin(250000);

  // Affectation du pin au servomoteur  
  M1.attach(PinM1);
  M2.attach(PinM2);
}

void loop()
{ 
  // 1. Définition de la consigne: Angle d'équilibre (0°) :)   
  x_nn=0.0; 

  // 2.1 Lecture du capteur: Accélo + Temp + Gyro
  ReadGY521_0(GyAccTemp);       // Lecture des données bruts 
 
  // 2.2 Calcul des Angles (Pitch / Roll / Yaw) 
  //AngleAcc(GyAccTemp, PitchRoll);                       //Accélo +/-90°
  AngleGyro(GyAccTemp,  PitchRoll, (double)T_ms/1000.0);  //Gyro +/-90°
  
  
  // 2.3 Affectation de la sortie du capteur 
  //y_capt=PitchRoll[0]-1.0; // Accélo  
  y_capt=PitchRoll[1]+1.0; // Gyro  
  

  // 2.4 Filtrage de la mesure du capteur (Moyenne glissante)
  /*
  const byte N=5; 
  static byte j=0; 
  double somme =0.0, Cap[N]; 
    
  Cap[j]=y_capt; j++; j%=N; 
  for (int i=0; i<N; i++) somme+=Cap[i];
  y_capt=somme/double(N); somme=0.0; 
  */
  
  // 3. Soustracteur: Calcul de l'erreur eps(n) 
  eps_n=x_nn-y_capt; 
  
  // 4. Correcteur P (voir le lien en commentaire) 
  const double k0=0.03; 
  y_corr= k0*eps_n; 
  
  // 5. Définition de consigne de la vitesse V0 (0(0%, 180(100%))
  V0=V0+0.01; 
  if (V0> 110.0) V0=V0-0.01; 

  // 6.1 Mise à jour de la vitesse  
  Vitesse_M1=V0-y_corr; 
  Vitesse_M2=V0+y_corr; 

  // 6.2. Réglage du Problème de Saturation et Valeurs <0
  const double Vmax=180.0; // Rapport Cyclique Maximal 
  Vitesse_M1=Vmax*tanh(abs(Vitesse_M1)/Vmax); 
  Vitesse_M2=Vmax*tanh(abs(Vitesse_M2)/Vmax); 

  
  // 6.3 Affection des vitesses aux Moteurs 
  M1.write(Vitesse_M1);  
  M2.write(Vitesse_M2); 

  // Affichage: Vérification du Module GY-512 (A commenter)
  Serial.print(y_corr); Serial.print(","); 
  Serial.println(-y_corr); Serial.print(",");
  //Serial.println(y_capt);  
  
  //Serial.print(PitchRoll[0]-1.0); Serial.print(",");
  //Serial.print(PitchRoll[1]); Serial.print(",");
  //Serial.println(y_capt); 
  
  // Période d'échantillonnage 
  delay(T_ms);
}



// Lecture des données des capteurs sans Corrections
void ReadGY521_0( long *GyAccTempp)
{
  long  GATCorr_0[NumData]={0,0,0,0,0,0,0};
  ReadGY521(GyAccTemp,GATCorr_0);
}

// Lecture des données des capteurs avec Corrections
void ReadGY521( long *GyAccTempp, long *GATCorrr)
{
  // Init du module GY-521
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);

  // Lecture des données (3 axes accéléromètre + température + 3 axes gyroscope
  for  (long i=0;i<NumData;i++)
  {
    if(i!=3)
    {
      GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
    }
    else
    {
      GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
      GyAccTempp[i] = GyAccTempp[i]/340 + 36.53;
    }
  }
}

//Conversion des données accéléromètre en pitch/roll/yaw
void AngleAcc(long *GyAccTempp,  double *PitchRol)
{  
  double x,y,z, X, Y, Z;
  x= (double)GyAccTempp[0];
  y= (double)GyAccTempp[1];
  z= (double)GyAccTempp[2];

  X=sqrt((y*y) + (z*z)); 
  Y=sqrt((x*x) + (z*z)); 
  Z=sqrt((x*x) + (y*y)); 
  
  PitchRol[0] = atan(x/X); //  pitch 
  PitchRol[1] = atan(y/Y); //  roll
  PitchRol[2] = atan(z/Z); //  yaw
  
  //Conversion Radian en degré
  PitchRol[0] = PitchRol[0] * (180.0/pi);
  PitchRol[1] = PitchRol[1] * (180.0/pi) ;
  PitchRol[2] = PitchRol[2] * (180.0/pi) ;
}

//Conversion des données du gyro en pitch/roll/yaw
void AngleGyro(long *GyAccTempp,  double *GyPitchRol, double Te_s)
{  
  double x,y,z, X, Y, Z, h;
  static double xyz[3][2]; 
  
  // 1. Calcul des valeurs réelles 
  x= (double)GyAccTempp[4]/LSB_Gyro;
  y= (double)GyAccTempp[5]/LSB_Gyro;
  z= (double)GyAccTempp[6]/LSB_Gyro;

  // 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6  
  h=2.0*Te_s; 
  X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0; 
  Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
  Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;

  // 2.2 Mise à jour des échantillons  
  xyz[0][1]=xyz[0][0];
  xyz[0][0]=x; 

  xyz[1][1]=xyz[1][0];
  xyz[1][0]=y; 

  xyz[2][1]=xyz[2][0];
  xyz[2][0]=z; 
  
  // 3. Conversion des angles du Radian en degré
  GyPitchRol[0] = X * (180.0/PI);
  GyPitchRol[1] = Y * (180.0/PI) ;
  GyPitchRol[2] = Z * (180.0/PI) ;
}


void AngleGyroNC52(long *GyAccTempp,  double *GyPitchRol, double Te_s)
{  
  double x,y,z, X, Y, Z;
  static double xyz[3][4]; 
  
  // 1. Calcul des valeurs réelles 
  x= (double)GyAccTempp[4]/LSB_Gyro;
  y= (double)GyAccTempp[5]/LSB_Gyro;
  z= (double)GyAccTempp[6]/LSB_Gyro;

  // 2.1 Integration - Formule NC-5-2 d’ordre 7  
  X= 434.0*(xyz[0][3]+x)+1024.0*(xyz[0][2]+xyz[0][0])+864.0*xyz[0][1] ; X=X*Te_s/3780.0; 
  Y= 434.0*(xyz[1][3]+y)+1024.0*(xyz[1][2]+xyz[1][0])+864.0*xyz[1][1] ; Y=Y*Te_s/3780.0; 
  Z= 434.0*(xyz[2][3]+z)+1024.0*(xyz[2][2]+xyz[2][0])+864.0*xyz[2][1] ; Z=Z*Te_s/3780.0; 
  
  // 2.2 Mise à jour des échantillons  
  for(int i=0; i<3; i++) 
  {
    xyz[0][i+1]=xyz[0][i];
    xyz[1][i+1]=xyz[1][i];
    xyz[2][i+1]=xyz[2][i];
  }
  xyz[0][0]=x; 
  xyz[1][0]=y; 
  xyz[2][0]=z; 
  
  // 3. Conversion des angles du Radian en degré
  GyPitchRol[0] = X * (180.0/PI);
  GyPitchRol[1] = Y * (180.0/PI) ;
  GyPitchRol[2] = Z * (180.0/PI) ;
}

double Sys2All(double x_nn, double *x, double *y, double zeta, double wn, double k, double T)
{
  // Paramètre du système 
  double a1=2.0*zeta/wn;
  double a2=1.0/(wn*wn);

  const double b0=(a1/(2.0*T))+(a2/(T*T)); 
  const double b1=-2.0*a2/(T*T); 
  const double b2=(-1.0*a1/(2.0*T))+(a2/(T*T)); 
  const double b[3]={b0,b1,b2};

  // Variables de l'entrée et la sortie 
  double y_nn=0.0;
  
  // Calcul de la nouvelle sortie 
  y_nn= -(y[0]*(1.0+b[1]))-(y[1]*b[2])+(k*x[0]); // y[1]: y(n-2), y[0]: y(n-1)
  y_nn/=b[0];
  
  // Mise à jour de la sortie 
  y[1]=y[0];
  y[0]=y_nn;

  // Mise à jour de la sortie 
  x[0]=x_nn;
  
  // Renvoie du résultat 
  return y_nn;
}

double CorrPI(double x_nn, double *xpi, double *ypi, double kp, double ki, double T)
{
  // Variables de l'entrée et la sortie 
  double y_nn=0.0;
  
  // Calcul de la nouvelle sortie 
  y_nn=ypi[1] + kp*x_nn + (2.0*T*ki)*xpi[0] -kp*xpi[1]; 
  // y(n)=y(n-2)+ k1*x(n) + 2*T*k2*x(n-1) - k1*x(n-2)
  
  // Mise à jour de la sortie 
  ypi[1]=ypi[0];
  ypi[0]=y_nn;

  // Mise à jour de la sortie 
  xpi[1]=xpi[0];
  xpi[0]=x_nn;
  
  // Renvoie du résultat 
  return y_nn;
}

double CorrPD_AP(double x_nn, double *x_cc, double *y_cc, double a, double t0, double k0, double T)
{
  // Variables de l'entrée et la sortie 
  double y_nn=0.0;

  // Paramètres du correcteur
  double alfa=a*t0;
  double beta=t0;  
  
  // Calcul de la nouvelle sortie
  // Modèle Analogique: C(p)= k*(1+aTp)/(1+Tp)
  // Modèle numérique:  y(n)=[alfa*k 2kT -alfa*k]*[x(n) x(n-1) x(n-2)]'
  //                        -[2T -beta]*[y(n-1) y(n-2)]'
  //                    y(n)=y(n)/beta 
  y_nn=(alfa*k0*x_nn)+(2*k0*T*x_cc[0]) - (alfa*k0*x_cc[1]);
  y_nn=y_nn- (2*T*y_cc[0]) - (beta*y_cc[1]); 
  y_nn/=beta; 
  
  // Mise à jour de la sortie 
  y_cc[1]=y_cc[0];
  y_cc[0]=y_nn;

  // Mise à jour de la sortie 
  x_cc[1]=x_cc[0];
  x_cc[0]=x_nn;
  
  // Renvoie du résultat 
  return y_nn;
}

double PID(double x_nn, double *x_cc, double *y_cc, double N_fil, double z, double w, double k, double T)
{
  
  double t1=1.0/(w*(z+sqrt(z*z-1.0))); 
  double t2=1.0/(w*(z-sqrt(z*z-1.0)));

  double td=t1+t2; 
  double ti=t1*t2/td; 

  /*
  Serial.print(t1); 
  Serial.print("\t"); 
  Serial.print(t2); 
  Serial.print("\t"); 
  Serial.print(td); 
  Serial.print("\t"); 
  Serial.println(ti); 
  */
  
  double a1=2.0*z/w; 
  double a2=1.0/(w*w); 
  double b1=ti; 
  double b2=ti*t2/N_fil; 

  double b[3]; 
  b[0]= a1/(2*T)+a2/(T*T); 
  b[1]= 1.0-2.0*a2/(T*T); 
  b[2]=-a1/(2*T)+a2/(T*T); 

  
  double c[3];
  c[0]= b1/(2*T)+b2/(T*T);
  c[1]= -2.0*b2/(T*T);
  c[2]= -b1/(2*T)+b2/(T*T);
  
  // Variables de l'entrée et la sortie 
  double y_nn=0.0;

  
  // Calcul de la nouvelle sortie
  y_nn=k*(b[0]*x_nn + b[1]*x_cc[0] + b[2]*x_cc[1]);  
  y_nn=y_nn-(c[1]*y_cc[0] + c[2]*y_cc[1]);  
  y_nn=y_nn/c[0]; 
  
  
  // Mise à jour de la sortie 
  y_cc[1]=y_cc[0];
  y_cc[0]=y_nn;

  // Mise à jour de l'entrée 
  x_cc[1]=x_cc[0];
  x_cc[0]=x_nn;
  
  // Renvoie du résultat 
  return y_nn;
}

Accueil Drone avec Arduino


0 commentaire

Laisser un commentaire

Avatar placeholder

Votre adresse e-mail ne sera pas publiée. Les champs obligatoires sont indiqués avec *

Anti-Robot *

You have successfully subscribed to the newsletter

There was an error while trying to send your request. Please try again.

FPGA | Arduino | Matlab | Cours will use the information you provide on this form to be in touch with you and to provide updates and marketing.