Catégories
Algorithme Arduino Asservissement Asservissement Arduino automatique Commande des moteurs drone Projets électroniques

Implémentation de la Boucle d’Asservissement d’un Drone Bicopter avec Arduino

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

Laisser un commentaire