MPU6050 et filtrage numérique
Cahier des charges :
Notre objectif est d'obtenir un capteur fiable pour maintenir en équilibre notre robot Gyropode (Balancing Robot).
C'est le même procédé qui est utilisé sur un drone, mais au lieu de
travailler que sur le tangage,
il faut aussi prendre en compte le roulis et le lacet.
Nous utiliserons un MPU6050 afin de calculer l'angle de notre robot par rapport à la position d'équilibre.
Cet angle sera la base de notre régulation PID pour maintenir en équilibre notre robot Gyropode.
Il doit être peu bruité, peu sensible aux vibrations et dynamique (temps de réponse rapide).
Pour cela nous disposons sur la même puce (MPU6050) :
- d'un accéléromètre qui est stable dans le
temps mais génére du bruit (erreur aléatoire).
De plus lorsque le robot fonctionnera, les accélérations et les vibrations vont fausser le calcul de l'angle. - d'un gyroscope qui donne une mesure
dynamique très fidèle,
mais a tendance à dériver dans le temps ou lors de variation brusque.
Ces 2 mesures complémentaires nous permettront de calculer un angle fiable.
Pour cela il faudra :
- Rafraichir le calcul de notre angle à une cadence de 5 ms
- Mesurer l'angle de tangage à l'aide de l'accéléromètre (angle_acc)
- Réduire le bruit grâce à un filtre numérique
- Prédire l'angle attendu grâce à la vitesse angulaire Gy du gyroscope
- Appliquer un filtre complémentaire pour calculer un angle
fiable et dynamique :
angle_gyro = K*angle_acc + (1-K)*(angle_gyro+Gy*dt)
K est un réel compris entre 0 et 1
Objectif du TP : obtenir un angle de tangage exploitable pour une régulation.
1. Mesure de l'angle à l'aide d'un accéléromètre
1. A l'aide d'un schéma, montrer comment un accéléromètre permet de calculer l'angle de tangage de notre robot.
conseil : consulter cette page du projet TPIL Ballon Sonde
2.Adapter la formule en fonction de la position du capteur sur le
robot.
Déterminer angle = f(ax,ay,az)
3. Le MPU6050 est un capteur MEMS très petit et à bas coût.
Regarder cette vidéo du
CEA Leti .
Définir en quelques lignes les acronymes MPU et MEMS.
4. Cabler un MPU6050 sur une platine d'essai vers un Arduino.
5. Tester votre MPU6050 à l'aide d'un programme Exemple de la
bibliothèque MPU6050,
ou en vous inspirant du programme ci-dessous :
#include <MPU6050.h> //MPU6050 library #include <Wire.h> //IIC communication library MPU6050 mpu; //Création de l'objet mpu float dt=0.005; // Pour cadencer la boucle à 5 ms float angle_acc; int ax, ay, az, gx, gy, gz; //3 accélérations et 3 vitesses angulaires unsigned long t0,i;void setup() { Wire.begin(); //Initialisation bus I2C Serial.begin(9600); delay(1000); mpu.initialize(); //initialisation MPU6050 delay(2); } void loop() { t0=micros(); // Top départ pour boucle cadencée à dt mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Récupérer les 6 axes ax ay az gx gy gz angle_acc = f(ax,ay,az); // Remplacer f(ax,ay,az) par votre formule if (i%20==0) { // afficher angle toutes les 20*5=100ms Serial.println(angle_acc);} i++; while (micros()-t0<dt*1e6); // boucle cadencée à dt en µs }
6. Ecrire un programme Arduino calculant toutes les 5 ms l'angle en
°.
Afficher sur le moniteur série l'angle toutes les 100ms.
Appel 1 : Faites valider votre programme
de mesure d'angle de tangage.
2.
Monitorage sous Labview
7. Réaliser un programme Labview permettant d'afficher sur un
graphique l'angle de tangage.
Prévoir la transmission d'une deuxième valeur (filtrée).
L'angle calculé est bruité (erreur aléatoire de l'accéléromètre).
Pour diminuer ce bruit nous allons devoir réaliser un filtrage
numérique.
L'objectif est de diviser par 10 l'écart-type.
Le filtre numérique le plus simple à réaliser est un filtre moyenneur.
8. En faisant le lien avec votre cours de métrologie, pour une
moyenne glissante de 4 mesures,
l'incertitude-type de cette moyenne est divisée par combien ?
-
Écart-type expérimental
(sur la série de mesures, caractérise la fidélité d’une chaîne de
mesure…) :
-
Incertitude-type (=
écart-type sur la moyenne, caractérise la justesse d’une chaîne de
mesure…) :
9. Implémenter sur l'arduino un filtre moyenneur qui effectue la moyenne glissante des N dernières mesures.
-
On peut créer un tableau contenant les N=4 derniers angles calculés :
-
On remplit celle file d'attente (First In First Out) au fur à mesure des cycles :
-
On calcule la moyenne des N=4 valeurs de la Fifo :
10.
En vous inspirant de la face avant ci-contre, afficher sur votre
Face Avant les 2 courbes sur le même graphe.
Ajouter une commande qui permet de rendre visible ou non la
courbe souhaitée.
Ajouter un
bouton qui remet à zéro toutes les données stockées.
Calculer sur les 100 dernières valeurs l'écart type des 2
angles transmis.
11. Lorsque le capteur ne bouge pas, quel est
écart-type le plus petit ?
Proposer une solution pour diviser par 10 l'écart-type
de l'angle calculé.
Modifier votre programme et vérifier que
l'écart-type est bien divisé par 10.
Quel est l'inconvénient de cette solution ?
12. Tapez sur la table pour créer une vibration.
Que constatez-vous ?
Appel 2 : Faites valider votre VI Labview par le professeur.
3. Utilisation du gyroscope
Le filtre moyenneur est très efficace en statique.
Par contre il introduit un retard en dynamique :
comme un filtre moyenneur qui introduit un retard de phase en régime
sinusoïdal.
De plus les accélérations et vibrations rendent moins pertinent angle_acc .
Pour réduire le bruit sans introduire de retard nous allons utiliser le gyroscope.
Pour déterminer la vitesse angulaire en °/s nous consultons le
datasheet p12
la sensibilité
ΔN/ΔG
en LSB/(°.s-1)
Gy(°.s-1) = gy (LSB) / 131( LSB/(°.s-1)
Le Gyroscope est très performant pour trouver l'angle du cycle
suivant (dt=5ms) :
angle_gyro = angle_gyro+Gy*dt
Par contre il a tendance à dériver au fur à mesure des cycles (il
y a 200 cycles par seconde).
Nous allons donc réaliser une moyenne pondérée avec l'angle de
l'accéléromètre :
angle_gyro = K*angle_acc + (1-K)*(angle_gyro+Gy*dt)
K est un facteur compris entre 0 et 1.
K=0,40 implique que l'accéléromètre contribue à 40% du calcul de
l'angle.
13. Il est facile de déterminer l'offset du gyroscope :
il suffit de ne pas bouger et de faire la moyenne
sur N mesures du retour gyroscope.
Si on ne corrige pas ce défaut nous risquons
d'avoir une dérive (offset*t) lors du calcul de l'angle.
A l'aide de ce programme déterminer les offset
du gyroscope.
int offset_gx=492,offset_gy=-32, offset_gz=55;
14.
Implémenter ce nouveau filtre dans le programme Arduino.
Des difficultés ? -> Struture du
programme à compléter
L'angle gyroscope est obtenu à l'aide de l'intégration de la vitesse
angulaire.
La constante d'intégration dépend des CI (Conditions Initiales)
Nous utilisons l'accéléromètre et le filtre moyenneur pour
déterminer l'angle initial.
Conseil pour initialiser l'angle_gyro :
Cela revient, en analyse numérique, à ajouter Gy*dt à l'angle_gyro
précédent.
angle_gyro = angle_gyro+Gy*dt
Pour être précis, il faut que dt=5ms soit petit devant les
constantes de temps mécaniques.
Afficher dans le moniteur série les 3
angles.
Pour changer facilement la valeur de K prévoir :
- côté Labview une commande pour modifier K
- côté Arduino, une prise en compte de cette commande
14. Modifier le VI Labview pour monitorer les 3 angles
Optimiser votre VI pour mettre en évidence :
- la diminution du bruit par rapport
à angle_acc
- l'amélioration du temps de réponse
par rapport à moyenne
Montrer la dérive lorsque K=0
(capture d'écran à joindre)
Faites varier K pour optimiser la
réduction de bruit et éviter la dérive.
Remplir le tableau récapitulatif suivant :
- : insuffisant + : correct ++ :
bon +++ : très bon
Grandeur d'intérêt : Angle de tangage Grandeurs mesurées : Ax, Ay, Az, Gx, Gy, Gz
|
Capteur |
Accéléromètre |
Gyroscope |
K*Acc. + (1-K) Gyro. |
|
|
Filtre |
Mesure directe |
Filtre moyenneur |
K = 0 |
Complément |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Ce capteur va nous servir à équilibrer le robot Gyropode.
Un critère insuffisant est éliminatoire.
15. Quelle solution choisir pour que notre régulation soit :
- précise
(robot équilibré)
- stable (peu
d'oscillations)
- rapide (en
cas de perturbation le robot retrouve sa position d'équilibre
rapidement)
- robuste (le
robot récupère des perturbations importantes)
Appel 3 : Faites valider cette étape par le professeur.
4. La régulation d'angle de tangage
|
Les drones utilisent en général un filtre plus sophistiqué :
le filtre de Kalman
Avec un bon paramétrage de K et un bon réglage des offset, notre
filtre complémentaire Il ne reste plus qu'à programmer notre régulation :
void regulation(){
P= ????;
D= ????;
PWM=P+D;
// Attention action opposée
if (balancing) moteur(-PWM,-PWM);
else moteur(0,0);
}
et déterminer expérimentalement les paramètres Kp et Kd Sur ce lien la fonction void moteur(int droite,int gauche) est disponible. Il ne reste pus qu'à écrire les fonctions : - void calcul_angle() : calcul de l'angle_gyro - void affiche() : affiche pour Labview (liaison série) angle_acc;angle_gyro;P;D;
- void reception() : disponible après
void moteur(int droite,int
gauche)
- void commute() : disponible après
void moteur(int droite,int
gauche)
|
void setup()
{
Wire.begin(); //Bus I2C
Serial.begin(9600); //Pour communiquer avec Labview
iniPin(); //Initialisation pinMode du shield
delay(1400);
digitalWrite(buz,HIGH);
mpu.initialize(); //Initialisation MPU6050
delay(10);
digitalWrite(buz,LOW);
}
void loop()
{
t0=micros(); // top départ boucle
calcul_angle();
regulation();
if (i%20==0) affiche(); // Affiche tous les 20*5=100ms
i++;
if (Serial.available())reception();
if (!digitalRead(BP) && i-iBP>50) commute();
if (i-recu>3)digitalWrite(buz,LOW);
while (micros()-t0<dt*1e6);
}
|
Attention : durant la mise au point le robot peut avoir des réactions impévisibles !
Une erreur de signe et le robot part à pleine vitesse dans la direction opposée.
Tenez-vous prêt à rattraper le robot !
Et réaliser l'interface Labview adaptée...
5. Cap d'afficher le cap ?
Avec 2 lignes de code nous pouvons calculer l'angle de lacet.
Avec une boussole pour déterminer la constante d'intégration, cet
angle nous donne le cap suivi.
Cet angle nous sera utile pour contrôler les virages.
16. Modifier le programme du robot pour calculer
et afficher l'angle de lacet.
Prévoir une remise à zéro de cet angle. Par
exemple lors de l'appel du mode 4.
Modifier l'application Labview pour afficher cet
angle.