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).
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 :
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).
L’objectif sera d’obtenir l’approximation de l’intégrale pour chaque itération de la boucle. Ci-dessous les étapes à suivre :
Lecteur de l’échantillon récent f(b)
Calcul de l’intégration numérique en fonction de f(b), f[0]-f[3] (voir l’équation)
Mise à jour des échantillons du tableau :
f[3]= f[2]
f[2]= f[1]
f[1]= f[0]
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.
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.
// 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 :) */
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
// 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);
// 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) ; }
2 réponses sur « Drone Bicopter – Contrôleur / Régulateur PI »
Magnifiques vidéos, je ne me rendais pas compte qu’il y avait autant d’algorithme derrière tout cela…
Quel boulot… BRAVO!
A quand la suite? 🙂
Merci 🙂