Catégories
Algorithme Arduino Asservissement Asservissement Arduino automatique Commande des moteurs drone électronique

Drone Bicopter – Contrôleur / Régulateur PI

Objectifs

  • Test du correcteur (PI)
  • Test du correcteur PI en cascade avec le correcteur Avance de Phase (PD)
  • Intégration numérique par la méthode de Simpson (Ordre 3, 3 points)
  • Intégration numérique par la méthode de Simpson NC-5-2: (Ordre 7, 5 points)
  • Réduction de l’erreur dynamique (double intégrateur)
  • Ajout des blocs de saturation: Protection contre l’emballement de la commande (25%), etc.

Drone Bicopter - Contrôleur / Régulateur PI

Sujets connexes

Les tutos ci-dessous vous permettrons d’assimiler la partie discrétisation et implémentation du correcteur PI et avance de phase (PD)  avec Arduino.

La mesure de l’angle par le Gyro (Simpson) 

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).

Voir le tuto « Implémentation de la Boucle d’Asservissement d’un Drone Bicopter avec Arduino » pour plus de détails.

La mesure de l’angle par le Gyro (NC-5-2) 

NC-m ou bien la formule de Newton-Cotes. La fonction f peut être interpolée à l’aide de son évaluation en m points équidistants (comprenant les deux extrémités si m > 1, méthode du point milieu si m = 1) par un polynôme de degré m – 1 issu d’une base de polynômes de Lagrange et dont l’intégrale est fournie par les formules de Newton-Cotes. Ce procédé permet ainsi une généralisation des résultats précédents, lire la suite (Wikipédia).

L’approximation NC-5-2 est basée sur un polynôme de degré 6, elle est d’ordre 7 décrite par la formule suivante :

Méthode formule de Newton-Cotes

Contrairement à la méthode de Simpson qui nécessite un seul point milieu dans l’intervalle (a,b), la méthode NC-5-2 fait appel à trois points (x1, x2, x3) dans l’intervalle d’après l’équation ci-dessus.

Comment implémenter la méthode NC-5-2 avec Arduino ?

Principe

L’équation ci-dessus monte que l’intégrale est la somme de cinq échantillons de la fonction f(x) ou le signal s(t) avec des coefficients (ou poids) différents. Durant l’implémentation on va utiliser un pas régulier (h) est égal à la période d’échantillonnage. Par conséquent, l’intervalle (b-a) (ou bien la période d’intégration) est égale à 4h (voir la figure ci-dessus). On considère un tableau de 4 éléments f[4] dédié à la mémorisation de 4 échantillons précèdent de la fonction : f(a)(plus ancien), f(x1), f(x2) et f(x3). L’échantillon f(b) étant l’échantillon actuel (le plus récent) avec : f(b)=f[3], f(x1)=f[2], f(x2)=f[1], f(x3)=f[0] et f(a).

Méthode formule de Newton-Cotes

L’objectif sera d’obtenir l’approximation de l’intégrale pour chaque itération de la boucle. Ci-dessous les étapes à suivre :

  1. Lecteur de l’échantillon récent f(b)
  2. Calcul de l’intégration numérique en fonction de f(b), f[0]-f[3] (voir l’équation)
  3. Mise à jour des échantillons du tableau :
    1. f[3]= f[2]
    2. f[2]= f[1]
    3. f[1]= f[0]
    4. f[0]= f(b)

La fonction AngleGyroNC52()

Les essais effectués dans le tuto concernant la méthode NC52 ne sont pas concluants. En effet, l’implémentation de la fonction AngleGyroNC52() n’était pas bonne à cause d’une mauvaise implémentation partie dédiée à la mise à jour des échantillons : Utilisation d’une boucle for avec une indice croissant induisant le mélange des échantillons du tableau.  Ci-dessous la version corrigée de la fonction.

  ...
for(int i=3; i>0; i--)
{
xyz[0][i]=xyz[0][i-1];
xyz[1][i]=xyz[1][i-1];
xyz[2][i]=xyz[2][i-1];
}
xyz[0][0]=x;
xyz[1][0]=y;
xyz[2][0]=z;
...

La fonction AngleGyroNC52() prend en entrée les données de l’accéléromètres, puis elle retourne les angles dans les trois axes en utilisant la technique NC52. Ci-dessous la définition de la fonction (voir le tuto pour plus de détails.

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,h;
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
h=4.0*Te_s; // h=b-a
X= 434.0*(xyz[0][3]+x)+1024.0*(xyz[0][2]+xyz[0][0])+864.0*xyz[0][1] ; X=X*h/3780.0;
Y= 434.0*(xyz[1][3]+y)+1024.0*(xyz[1][2]+xyz[1][0])+864.0*xyz[1][1] ; Y=Y*h/3780.0;
Z= 434.0*(xyz[2][3]+z)+1024.0*(xyz[2][2]+xyz[2][0])+864.0*xyz[2][1] ; Z=Z*h/3780.0;

// 2.2 Mise à jour des échantillons
for(int i=3; i>0; i--)
{
xyz[0][i]=xyz[0][i-1];
xyz[1][i]=xyz[1][i-1];
xyz[2][i]=xyz[2][i-1];
}
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) ;
}

Réglage des correcteurs

Voir le tuto.

Code Arduino


/*
* 1. Test du correcteur (PI)
* 2. Test du correcteur PI + Avance de Phase (PD)
* 3. Intégration par Simpson (Ordre 3, 3 points)
* 4. Intégration par NC-5-2: (Ordre 7, 5 points)
* 5. Réduction de l'erreur dynamique:
* 6. Ajout des blocs de saturation: Protection contre
* l'emballement de la commande (25%), etc.
* 6. Abonnez-vous :)
*/



#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 Tn 25 // 100 ms
#define T_ms 0.5 // ms, 6Tn <T_ms

const double Fn=1000.0/Tn;
const double Wn=2.0*PI*Fn;
const 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=0.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èmes
double x1[2], y1[3];

// Variables internes du correcteur
double x_c[2], y_c[2];
double x_c1[2], y_c1[2];
double x_c2[2], y_c2[2];
double x_c3[2], y_c3[2];
double x_c4[2], y_c4[2];

static double Offset[3]={0.0,0.0,0.0};

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(1000000);

// 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: Acce + Temp + Gyro
ReadGY521_0(GyAccTemp); // Lecture des données bruts

// 2.2 Calcul des Angles (Pitch / Roll / Yaw)
/* Simpson: Ordre 3 */
AngleGyro(GyAccTemp, PitchRoll, (double)T_ms/1000.0); //Gyro +/-90°

/* NC-5-2: Ordre 7 */
//AngleGyroNC52(GyAccTemp, PitchRoll, (double)T_ms/1000.0); //Gyro +/-90°


// 2.3 Affectation de la sortie du capteur
double P, R, Y;
P=PitchRoll[1]+0.1;
R=PitchRoll[0]-0.1;
y_capt=P;
y_capt=90.0*tanh(y_capt/90.0);

// 2.4 Filtrage de la mesure du capteur (Moyenne glissante)

const byte N=3;
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)
const double Vmax=160.0; // Rapport Cyclique Maximal
eps_n=x_nn-y_capt;
eps_n=Vmax*tanh(eps_n/Vmax);

// 4. Correcteur PID (voir le lien en commentaire)
double k0=0.010;
double N_fil=250.0;
double z_PID=1.0;
double w_PID=Wn;
y_corr= PID(eps_n, x_c, y_c, N_fil, z_PID, w_PID, k0, T_s);

// Avance de Phase (HF): Stabilité
/*
double k1=0.2;
double phi_m=35.0*PI/180.0; // 34°
double a=(1.0+sin(phi_m))/(1.0-sin(phi_m)); // a=(1-sin(phi))/(1+sin(phi))
double t0=1.0/(sqrt(a)*(0.1*Wn));
y_corr= CorrPD_AP(y_corr, x_c1, y_c1, a, t0, k1, T_s);
*/

// 5. Définition de consigne de la vitesse V0 (0(0%, 180(100%))
unsigned long t; t=millis();
const double Step=0.2, V_max=130.0;
if(t>15000)
{
V0=V0-Step;
if (V0<0) V0 =0.0;
}
else
{
V0=V0+Step;
if (V0> V_max) V0=V_max;
}


// 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
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(Vitesse_M1); Serial.print(",");
Serial.print(Vitesse_M2); Serial.print(",");
Serial.println(y_capt);

//Serial.print(PitchRoll[0]-0.25); Serial.print(",");
//Serial.println(PitchRoll[1]+0.25);

// Période d'échantillonnage
delayMicroseconds(T_ms*1000);
}



// 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 dy 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,h;
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
h=4.0*Te_s; // h=b-a
X= 434.0*(xyz[0][3]+x)+1024.0*(xyz[0][2]+xyz[0][0])+864.0*xyz[0][1] ; X=X*h/3780.0;
Y= 434.0*(xyz[1][3]+y)+1024.0*(xyz[1][2]+xyz[1][0])+864.0*xyz[1][1] ; Y=Y*h/3780.0;
Z= 434.0*(xyz[2][3]+z)+1024.0*(xyz[2][2]+xyz[2][0])+864.0*xyz[2][1] ; Z=Z*h/3780.0;

// 2.2 Mise à jour des échantillons
for(int i=3; i>0; i--)
{
xyz[0][i]=xyz[0][i-1];
xyz[1][i]=xyz[1][i-1];
xyz[2][i]=xyz[2][i-1];
}
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)

// Saturation
double Vm=45.0; // 25%
y_nn=Vm*tanh(y_nn/Vm);

// 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;

// Saturation
double Vm=45.0; // 25%
y_nn=Vm*tanh(y_nn/Vm);

// 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];

// Saturation
double Vm=45.0; // 25%
y_nn=Vm*tanh(y_nn/Vm);

// 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 sign(double x)
{
double s=0.0;
s=1.0*(x>=0) -1.0*(x<0);
return s;
}

Accueil Drone avec Arduino

2 réponses sur « Drone Bicopter – Contrôleur / Régulateur PI »

Laisser un commentaire