Unité de mesure de l’inertie gyroscopique MPU-6050
Unité de mesure de l’inertie gyroscopique MPU-6050
Dans cette partie, je montre comment lire et traiter les données venant du Gyroscope.
Plus précisément, nous allons extraire les données du Gyro afin que le contrôleur de vol puisse calculer les angles parcourus par le quadrotor pour les axes de mouvements (Pitch, Roll et Yaw) en utilisant des opérations mathématiques simples, puis les transformer en instructions algorithmiques adaptées à Arduino.
Le Gyro est un capteur de taux d’angle à trois axes (x, y et z), il mesure la vitesse angulaire en degrés par seconde de ces axes en même temps.
Les données d’entrées du Gyro sont couplées linéairement avec sa sortie, signifie que la longueur de l’impulsion de sortie change proportionnellement lorsque le Gyro est tourné autour de ces axes.
Pour connaître la valeur de sortie exacte de l’MPU-6050, nous devons jeter un coup d’œil sur la fiche technique du Gyro MPU6050 mentionnée dans la page (12 of 52) de l’annexe de ce mémoire.
La sortie sera de 65,5 en décimal lorsque la vitesse angulaire du Gyro est de 1°/sec, car nous utilisons 500°/sec comme vitesse angulaire maximale pour les trois axes du Gyro.
Par exemple si le Gyro fait tourner autour de son axe de lacet (Yaw) et que nous terminons le tour complet (360°) exactement à une minute (60 sec), la sortie du Gyro à n’importe quel moment de ce parcourt sera de 6°/sec × 65.5 = 393, car à chaque seconde le Gyro tourne 6°.
Dans le programme Arduino il sera donc : Angle°/sec = sortie Gyro / 65,5.
Le résultat « Angle°/sec » sera déclarée comme une variable flottante et la sortie du Gyro est une valeur signée de 16 bits (la sortie du gyroscope est une valeur de complément de 16 bits deux) qui est similaire à un entier « Int » dans l’Arduino.
Cela signifie que la variable « sortie Gyro » doit être déclarée comme un entier car elle peut atteindre 32750 si la vitesse angulaire du Gyroscope atteint les 500°/sec (sortie Gyro = 500°/sec× 65.5 = 32750).
La plupart des Gyro ont un petit décalage moyen (offset) qui provoque la dérive du quadrotor donc le Gyroscope doit être calibré avant d’utiliser ces données de sortie.
Pour obtenir les meilleurs résultats, la sortie du Gyro doit être mise à zéro et ceci est simplement fait en prenant l’offset pour plusieurs lectures et en les soustrayant des plusieurs valeurs de la sortie du Gyro.
Ceci est expliquer dans cet organigramme de boucle for (;;) qui paresse dans le void setup :
Variables entiers 16 bits : Int_cal = 0 Initialiser les Variables 8 bits : lowByte, highByte
Initialiser les Variables 64 bits : gyro_pitch, gyro_roll, gyro_yaw, cal_axe_x, cal_axe_y, cal axe z
Cet organigramme calcule l’offset moyenne de 2000 lectures de Gyro pour les 3 axes, puis il stocke chacune de ces valeurs d’offset dans les variables (cal_axe_x, cal_axe_y et cal_axe_z).
Après cette étape l’offset est soustraite de chaque mesure du Gyro dans le reste de la boucle du programme.
Sous-programme de lecture des données du Gyro
L’Arduino Uno du contrôleur de vol lit les données des angles parcouru par le Gyro à partir des registres de sortie d’adresses 0x69 et 0xA8 qui sont à 16 bits en utilisant le protocole de communication I²C.
Par exemple pour lire l’axe « Yaw » on lit les 8 bits du poids fort du registre 16 bit à part, puis les 8 bits du poids faible et on fait un décalage pour avoir les 16 bit puis on affecte le résultat au variable « gyro_yaw ».
Toutes les autres parties intéressantes pour la communication I²C avec le Gyro sont gérées par la librairie «wire» qui nous facilitent la tâche.
L’organigramme qui explique le processus est situé dans la page suivante.
Après l’étape de calibrage, il est nécessaire de calculer l’angle total parcouru par le Gyro afin que le contrôleur de vol puisse reconnaître la position exacte du quadrotor dans l’espace. Pour cela il suffit simplement d’intégrer au fil de temps tous les valeurs de sortie Gyro (les données de la vitesse angulaire des axes Pitch, Roll et Yaw) par exemple pour l’axe Yaw :
𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀 𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝐩𝒀𝒀𝒀𝒀𝒀𝒀 =∑ 𝐬𝐬𝐩𝐩𝐩𝐩𝐬𝐬𝐬𝐬𝐀𝐀 𝐆𝐆𝐆𝐆𝐩𝐩𝐩𝐩𝒀𝒀𝒀𝒀𝒀𝒀 × ∆𝒇𝒇.
Dans le cas de notre contrôleur de vol, la fréquence de rafraîchissement est de 250 Hz, on peut donc extraire les angles parcouru par le Gyro pour chaque axe toutes les 4 ms au lieu de 1s. Cela signifie également que nous devons diviser les données de 𝐬𝐬𝐩𝐩𝐩𝐩𝐬𝐬𝐬𝐬𝐀𝐀 𝐆𝐆𝐆𝐆𝐩𝐩𝐩𝐩 sur 65,5 et ensuite diviser à nouveau sur 250 pour obtenir l’angle parcouru par le quadrotor chaque 4 ms.
𝑓𝑓𝑐𝑐𝑐𝑐𝑐𝑐𝑡𝑡𝑟𝑟ô𝑙𝑙𝑙𝑙𝑙𝑙𝑟𝑟 𝑑𝑑𝑙𝑙 𝑣𝑣𝑐𝑐𝑙𝑙 = 250 𝐻𝐻𝑧𝑧→ ∆𝒇𝒇 = 4 𝑚𝑚𝑠𝑠
Angle parcouru𝑌𝑌𝑎𝑎𝑌𝑌 = �(sortie Gyro𝑌𝑌𝑎𝑎𝑌𝑌 /(250/65.5))
= ∑(sortie Gyro𝑌𝑌𝑎𝑎𝑌𝑌 × 0.0000611)
L’intégrale en mathématique signifie la sommation d’une fonction sur un intervalle donné, donc dans le code Arduino on fait la somme de toutes les données gyroscopiques de sortie pour chaque axe à part dans un intervalle de 4ms, par exemple pour l’axe z « Yaw » se traduire par cette instruction :
Angle parcouru𝑌𝑌𝑎𝑎𝑌𝑌 est : Angle_yaw_entrée += gyro_z * 0.0000611;
C.à.d. : Angle_yaw_entrée = 𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀_𝐆𝐆𝐩𝐩𝐲𝐲_𝐀𝐀𝐀𝐀𝐬𝐬𝐩𝐩é𝐀𝐀 𝒑𝒑𝒑𝒑é𝒄𝒄é𝒅𝒅𝒅𝒅𝒅𝒅𝒇𝒇𝒅𝒅 + (gyro_z × 0.0000611)
Vérifions la validité de cette relation avec le dernier exemple pour une rotation complète (360°) du Gyro autour de son axe (Yaw) dans 60s, on aura :
La sortie du Gyro en décimal à n’importe quel moment pendant la rotation est : sortie Gyro𝑌𝑌𝑎𝑎𝑌𝑌 = 393.
Pendant la rotation, les données du Gyro sont ajoutées 250 fois par seconde donnant un total de 15000 lectures : 250𝑙𝑙𝑙𝑙𝑐𝑐𝑡𝑡𝑙𝑙𝑟𝑟𝑙𝑙 𝑃𝑃/𝑆𝑆 × 60𝑠𝑠 = 15000 𝑙𝑙𝑙𝑙𝑐𝑐𝑅𝑅𝑢𝑢𝑙𝑙𝑙𝑙
Pour une minute on aura donc :
Angle parcouru𝑌𝑌𝑎𝑎𝑌𝑌 = 15000 𝑙𝑙𝑙𝑙𝑐𝑐𝑅𝑅𝑢𝑢𝑙𝑙𝑙𝑙 × 393 × 0.0000611= 360.18° ≈ 360°
Ce qui est assez proche pour un tour complet, d’où la formule est juste.
L’inconvénient du Gyro MPU6050 c’est que le signal de sortie est très bruyant Figure 3.27. L’utilisation d’un simple filtre complémentaire permet de réduire le bruit qui accompagne le signal de sortie du Gyro, alors l’instruction précédente devienne:
Angle_yaw_entrée = (Angle_yaw_entrée * 0.7) + (gyro_z * 0.0000611);
C.à.d. : Angle_yaw_entrée = (𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀𝐀_𝐆𝐆𝐩𝐩𝐲𝐲_𝐀𝐀𝐀𝐀𝐬𝐬𝐩𝐩é𝐀𝐀 𝒑𝒑𝒑𝒑é𝒄𝒄é𝒅𝒅𝒅𝒅𝒅𝒅𝒇𝒇𝒅𝒅 × 0.7) + (gyro_z × 0.0000611)
Enfin, l’instruction Angle_yaw_entrée détermine le taux angulaire ou l’angle parcourus « Yaw » à introduire pour l’entrée de la boucle PID. La même instruction sera appliquer pour les autres axes de mouvement Pitch et Roll.
