Leçons de niveau 15

Micro contrôleurs AVR/Travail pratique/Utilisation d'un Accéléromètre MPU6050

Une page de Wikiversité.
Sauter à la navigation Sauter à la recherche
Début de la boite de navigation du travail pratique
Utilisation d'un Accéléromètre MPU6050
Image logo représentative de la faculté
T.P. no 2
Leçon : Micro contrôleurs AVR

TP de niveau 15.

Précédent :Timer et module ultra-son HC-SR04
Suivant :Equilibre sur deux roues d'un Robot
Icon falscher Titel.svg
En raison de limitations techniques, la typographie souhaitable du titre, « Travail pratique : Utilisation d'un Accéléromètre MPU6050
Micro contrôleurs AVR/Travail pratique/Utilisation d'un Accéléromètre MPU6050
 », n'a pu être restituée correctement ci-dessus.

La majeure partie de ce chapitre n'existerait pas sans la publication de Shane Colton du MIT (voir bibliographie en fin de chapitre).

Ce projet vise à comprendre et utiliser un capteur moderne d'accélération couplé à un gyroscope, deux capteurs aujourd'hui facilement trouvés ensemble dans un même composant à coût modeste (3€-4€).

Nous allons mettre en œuvre ce composant avec un Arduino, et le coupler à un programme Processing chargé de réaliser une visualisation 2D ou 3D de l'attitude de l'accéléromètre.

Certaines sections de ce travail pratique (sur les matrices de rotations) sont très théoriques. Elles ne sont utiles que si vous voulez comprendre le fondement des formules. Depuis la rédaction de ces exercices, nous avons réalisé un projet de robotique mobile en équilibre sur deux roues utilisant le même accéléromètre que ce TP (voir chapitre suivant) mais ne nécessite pas une connaissance approfondie des rotations 3D. Les rotations 2D, plus intuitives, permettent déjà de résoudre ce problème d'équilibre.

Nous réalisons aussi un projet de plate forme support d'une caméra comprenant deux servomoteurs. Nous y avons mis un accéléromètre et avons demandé à des étudiants de stabiliser la plate forme. Ils ont pu stabiliser le tangage avec l'accéléromètre. Reste à stabiliser le lacet, ce qui ne peut être fait avec un accéléromètre mais avec le gyroscope ; c'est le genre de détail qu'une bonne connaissance des rotations 3D permet de comprendre (les résultats détaillés seront publiés dans un futur proche).

Introduction aux capteurs IMU[modifier | modifier le wikicode]

IMU : Inertial Measurement Unit se traduit en français par Centrale à Inertie.

La référence du capteur que nous allons utiliser est GY-521 ; il est architecturé autour d'un MPU 6050 qui est composé de deux capteurs et d'un processeur :

  • un capteur accéléromètre 3 axes (x,y et z) qui mesure une accélération ;
  • un capteur gyroscope 3 axes qui mesure une vitesse angulaire ;
  • un « Digital Motion Processor » (DMP) pouvant stocker et restituer des données.
Utiliser Processing pour visualiser des données d'un accéléromètre et d'un gyroscope.

La terminologie anglo-saxonne associée à ce genre de capteur est 6 DOF (Degree of Freedom). On devrait traduire par 6 degrés de liberté mais il est préférable de traduire par 6 axes (3 pour l'accéléromètre et 3 pour le gyroscope). Le meilleur de ce qui se fait actuellement pour les capteurs bon marché ( < 50 € ) comporte 9 axes. En général c’est un magnétomètre qui est ajouté pour essayer de repérer le Nord magnétique. Ces 9 axes sont en général suffisants pour réaliser, en combinant les mesures de ces trois capteurs, une centrale à inertie de coût modeste.

Mais ces trois capteurs ont des défauts rendant leur utilisation délicate.

Nous allons nous contenter de six axes dans la suite du présent projet dont l'objectif peut se résumer dans la figure ci-dessus : réaliser à l'aide d'un traitement des données des capteurs une visualisation avec Processing permettant de mettre en évidence les propriétés des capteurs accélérations et gyroscopes.

La notion d'attitude[modifier | modifier le wikicode]

L'attitude en robotique (et en astronautique) désigne la direction des axes de la pièce mobile du robot ; généralement caractérisée par trois angles (roulis, tangage et cap ou lacet).

Ce mot « attitude » ne doit pas être confondu avec « altitude » (ils n'ont en commun qu'une orthographe voisine) !

La notion d'attitude est très liée aux rotations en trois dimensions. Nous allons donc commencer par examiner ces rotations d'un point de vue mathématique.

Introduction : mathématiques des rotations[modifier | modifier le wikicode]

Les rotations en trois dimensions sont suffisamment complexes pour que nous leur accordions un peu de temps (voir aussi les articles Matrice de Rotation et Angles d'Euler dans wikipédia).

Cette partie mathématique sera agrémentée de code SCILAB pour essayer de découvrir quelques propriétés des rotations 3D.

Soient les matrices :

Elles nous permettent de définir les angles associés aux rotations autour des axes x,y et z.

Voici un code SCILAB permettant de définir ces trois matrices pour des valeurs particulières d'angle :

//******* SCILAB
phi=%pi/4;
theta=-%pi/4;
//theta=0;
psi=-%pi/3;
//psi=0.0;
// see https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf for rotations conventions
rotx=[1 0 0;0 cos(phi) sin(phi);0 -sin(phi) cos(phi)];
roty=[cos(theta) 0 -sin(theta);0 1 0;sin(theta) 0 cos(theta)];
rotz=[cos(psi) sin(psi) 0;-sin(psi) cos(psi) 0;0 0 1];

Euler 123[modifier | modifier le wikicode]

La notation 123 signifie que l’on fait d’abord la rotation 1 (autour de l'axe x) puis la rotation 2 (autour de l'axe y) pour finir par la rotation 3 (autour de l'axe z). La matrice de rotation associée est donc :

Cette matrice vaut donc :

Toute rotation en dimension 3 est équivalente à un produit des trois rotations que l’on vient de définir. Cela signifie que trois paramètres suffisent pour définir une rotation (les trois Angles d'Euler).

Le code SCILAB permettant de réaliser le calcul de la matrice d'Euler 123 est :

//******* SCILAB
// construction matrice de rotation Euler 123
R=rotx*roty*rotz;
// construction matrice de rotation Euler 321
R2=rotz*roty*rotx;

Une rotation en dimension 3 peut aussi être définie par un axe de rotation et un angle (soit à priori 4 paramètres). Les quatre paramètres sont naturellement les trois composantes du vecteur qui définit l'axe et l'angle de rotation. Il est en fait possible d'économiser un paramètre en définissant l'axe à partir du centre d'une sphère ainsi que la longitude et latitude du point de rencontre de l'axe avec la sphère.

Angle de rotation[modifier | modifier le wikicode]

Nous venons de dire que toute rotation est équivalente à un angle de rotation et à un axe de rotation. Commençons dans cette section par rechercher l'angle de rotation en question ; nous le noterons pour éviter toute confusion avec de la section précédente.

La trace d'une matrice est la somme de ses éléments sur sa diagonale. Elle est toujours liée à l'angle de rotation.

Le calcul de l'angle associé à une rotation est tout simple. Il se fait à l'aide de la formule :

Le code SCILAB permettant le calcul d'angle est donc :

//******* SCILAB
// matrice R de rotation 3x3
angle= acos((trace(R)-1)/2)

Axe de rotation[modifier | modifier le wikicode]

L'axe de rotation est plus difficile à calculer si l'on ne dispose pas d'une puissance de calcul suffisante. Sur notre PC ce n’est pas un problème : il faut chercher le vecteur invariant par cette rotation. Techniquement cela revient à chercher le seul et unique vecteur propre associé à la valeur propre +1. Plutôt que d’en faire la théorie, nous préférons donner le code de SCILAB qui permet facilement ce genre de calcul :

//******* SCILAB
// premiere maniere
[Ab,X]=bdiag(R)

affiche :

X  =
 
    0.2850828  - 0.9324527  - 0.2219452  
  - 0.4494128  - 0.3345581    0.8283109  
    0.8466144    0.1363922    0.5144330  
 Ab  =
 
    0.3200597    0.9473974    0.  
  - 0.9473974    0.3200597    0.  
    0.           0.           1.  

La dernière colonne de X, celle en face du 1 comme valeur propre est l'axe de rotation. L'axe est donc défini par le vecteur .

Il existe une autre méthode avec SCILAB :

//******* SCILAB
//deuxième manière
[al,be]=spec(R)

qui donne

 be  =
 
    0.3200597 + 0.9473974i    0                         0   
    0                         0.3200597 - 0.9473974i    0   
    0                         0                         1.  
 al  =
 
    0.6894709                 0.6894709               - 0.2219452  
    0.1333194 - 0.3730636i    0.1333194 + 0.3730636i    0.8283109  
    0.0827997 + 0.6006859i    0.0827997 - 0.6006859i    0.5144330  
 

qui permet de retrouver le même axe : .

Pour la suite on supposera que notre calcul a été fait avec la première méthode et donc que l’on a calculé X, et que, si l’on a besoin de l'axe, il suffit de l'extraire de X.

Matrice de rotation à partir d'un axe et d'un angle[modifier | modifier le wikicode]

Nous cherchons maintenant à résoudre le problème inverse : nous connaissons l'axe de rotation et l'angle associé, comment trouver la matrice de rotation associée ?

On peut calculer la matrice R de rotation autour d'un axe dirigé par un vecteur unitaire (donc avec ) et d'un angle . La formule est :

Si l'espace en 3 dimensions est orienté de façon conventionnelle, cette rotation se fera dans le sens inverse aux aiguilles d'une montre pour un observateur placé de telle sorte que le vecteur directeur pointe dans sa direction (règle de la main droite).

Exemple en SCILAB

//******* SCILAB
// recupération de l'axe : troisième colonne de X
axis = X(:,3);
// construction de la matrice de rotation à partie de l'axe et de l'angle :
R2 = [cos(angle)+axis(1)*axis(1)*(1-cos(angle)) axis(2)*axis(1)*(1-cos(angle))-axis(3)*sin(angle) axis(3)*axis(1)*(1-cos(angle))+axis(2)*sin(angle);
      axis(1)*axis(2)*(1-cos(angle))+axis(3)*sin(angle)  cos(angle)+axis(2)*axis(2)*(1-cos(angle)) axis(3)*axis(2)*(1-cos(angle))-axis(1)*sin(angle);
      axis(1)*axis(3)*(1-cos(angle))-axis(2)*sin(angle) axis(2)*axis(3)*(1-cos(angle))+axis(1)*sin(angle) cos(angle)+axis(3)*axis(3)*(1-cos(angle))];

// calcul matrice rotation avec exponentiation 
Z=[0 -axis(3) axis(2);axis(3) 0 -axis(1);-axis(2) axis(1) 0];
R3=expm(angle*Z); // redonne R2

redonne exactement la même matrice que R pour les deux techniques de construction de R2 et R3.

Rotations du vecteur accélération de pesanteur[modifier | modifier le wikicode]

Tout accéléromètre est sensible au vecteur accélération de pesanteur. Cela en fait un très bon candidat pour mesurer l'attitude d'un solide pourvu que celui-ci ne soit pas soumis à une autre accélération (que l'accélération de pesanteur). Ceci est rarement le cas pour un robot mais l'accélération sera faible par rapport au vecteur d'accélération de pesanteur au moins tant qu’il n'y a pas de chocs.

Le vecteur accélération possède comme tout vecteur trois composantes :

S'il est exprimé dans un repère avec z vertical et vers le haut, et s'il est normalisé, ce vecteur devient :

Après une rotation Euler 123, ce vecteur est transformé en

soit, tout calcul fait,

Cette formule montre qu’à partir d'une mesure de g dans le repère qui a tourné, il est possible de trouver et mais pas  !!!

  • En effet permet de trouver
  • Puis permet de trouver


Axe de rotation du vecteur g[modifier | modifier le wikicode]

Le script suivant :

//******* SCILAB
// réalisation du produit vectoriel
exec("CrossProd.sci")
// valeur de g dans le repère avant rotation
g0 = [0;0;-1];
// valeur du vecteur g après rotation
g1 = R * g0;
// il est inutile de normaliser, mais au cas où ...
g1 = g1 / norm(g1);
// calcul de l'axe à partir du produit vectoriel : IL EST MONTRE PLUS LOIN QUE CETTE IDÉE SIMPLE EST FAUSSE
axis2=CrossProd(g0,g1);

nécessite l’utilisation de CrossProd.sci qui doit contenir :

//******* SCILAB
function [p] = CrossProd(u,v)
//Calculates the cross-product of two vectors u and v.
//Vectors u and v can be columns or row vectors, but
//they must have only three elements each.
[nu,mu] = size(u);
[nv,mv] = size(v);
if nu*mu <> 3 | nv*mv <> 3 then
error('Vectors must be three-dimensional only')
abort;
end
A1 = [ u(2), u(3); v(2), v(3)];
A2 = [ u(3), u(1); v(3), v(1)];
A3 = [ u(1), u(2); v(1), v(2)];
px = det(A1); py = det(A2); pz = det(A3);
p = [px, py, pz]
//end function
L'axe de rotation d'une rotation 3D ne peut pas être retrouvé à l'aide d'un produit vectoriel
Panneau d’avertissement

La réalisation de ce programme nous montre que l'on ne retrouve absolument pas l'axe correctement. Ce code est donc laissé seulement pour information, pour découvrir les chemins tortueux employés par l'esprit de l'auteur.

Cette erreur a permis de tirer la propriété suivante :

Passage en revue du travail de Kimberly Tuck (2007)[modifier | modifier le wikicode]

Nous avons eu l’occasion de commenter le travail Gyroscopes and Accelerometers on a Chip sur Internet et nous désirons expliquer les problèmes rencontrés ici.

Les raisonnements 3D nécessitent une bonne concentration. Nous avons nous-même commis des erreurs plusieurs fois.

La note d'application AN3461 : Tilt Sensing Using Linear Accelerometers by: Kimberly Tuck (2007) est passée partiellement en revue maintenant.

Figure 8 de AN3461 modifiée pour expliquer les formules associées

Cette note comporte un point central : sa "Figure 8" qui nous a posé de sérieux problèmes. Il nous a été impossible de bien comprendre exactement sa partie dessin. Nous ne pouvons pas la publier ici (pour des raisons de copyright) ; nous avons donc refait une figure qui nous semble bien plus explicative que l'original, mais qui, du coup, est plus complexe : c’est la figure associée à cette section (ci-contre).

Les formules qui y apparaissent sont les mêmes que les formules originales (celles qui nous serviront pour l'exercice 4) mais nous nous sommes attachés à faire apparaître les triangles rectangles desquels on peut les déduire facilement si l’on se rappelle qu'une tangente est le rapport du côté opposé sur le côté adjacent.

De son côté, le texte de cette AN3461, nous définit les angles comme :

  • est défini comme l'angle entre l'axe Y et le plan horizontal
  • est défini comme l'angle entre l'axe X et le plan horizontal
  • est défini comme l'angle entre l'axe Z et le vecteur accélération

Or sur la figure que nous proposons, seule la définition de saute immédiatement aux yeux. Pour les deux autres c’est un peu plus caché et cela demande un raisonnement géométrique : il vous faut remarquer que les angles cherchés ont leur côté perpendiculaires aux angles dessinés. Ils sont donc soit égaux soit supplémentaire. Le dessin faisant apparaître des angles plus petit que 90°, ils sont forcément égaux.

Par exemple, pour , est perpendiculaire au côté du fond (l'angle droit est dessiné en rouge) et par définition le plan horizontal est perpendiculaire au vecteur accélération de pesanteur (gros vecteur vertical). Ainsi l'angle de la figure est aussi l'angle entre l'axe Y et le plan horizontal. CQFD.

Panneau d’avertissement Apartir de maintenant les Angles d'Euler sont notés avec des indices 123 pour éviter toute confusion avec les angles calculés par Kimberly Tuck.

L'utilisation de SCILAB nous a permis cependant de trouver une relation entre les deux. En effet le code :

anglex=%pi/4; //phi
angley=-%pi/4; //theta
anglez=-%pi/3; //psi
// see https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf for rotations conventions
rotx=[1 0 0;0 cos(anglex) sin(anglex);0 -sin(anglex) cos(anglex)];
roty=[cos(angley) 0 -sin(angley);0 1 0;sin(angley) 0 cos(angley)];
rotz=[cos(anglez) sin(anglez) 0;-sin(anglez) cos(anglez) 0;0 0 1];
// construction matrice de rotation Euler 123
R=rotx*roty*rotz;

//AN3461 Kimberly Tuck 2007
g0 = [0;0;-1];
g2 = R * g0;
g2 = g2 / norm(g2);
phi = atan(g2(2),sqrt(g2(1)*g2(1)+g2(3)*g2(3)));
rho = atan(g2(1),sqrt(g2(2)*g2(2)+g2(3)*g2(3)));
erreurx = (anglex-phi)*180/%pi
erreury= (angley-rho)*180/%pi

donne :

-->erreurx = (anglex-phi)*180/%pi
 erreurx  =
    75.  
-->erreury= (angley-rho)*180/%pi
 erreury  =
    0. 

Ce qui est surprenant est la valeur de "erreury" qui est nulle :

Est-il donc possible d’utiliser les angles calculés avec les formules de Kimerly Tuck pour trouver l'angle d'Euler manquant ? Nous avons déjà répondu par la négative à cette question mais nous pensons qu’il est utile de creuser la question : qu'est-ce qui fait que les calculs de Kimberly sont insensibles aux rotations autour d'un axe vertical ? Pour comprendre ce qui se passe on se pose la question suivante :

Nous vous donnons les trois angles de Kimberly , pouvez-vous amener la position du parallélépipède conformément à ces angles ?

Malgré cette remarque qui sera retirée dès que les problèmes évoqués seront résolus, nous formulons la propriété suivante :

Le problème de la deuxième formule est son instabilité numérique. Une solution est examinée dans la nouvelle note d'application AN3461.

Passage en revue du travail de Mark Pedley (2013)[modifier | modifier le wikicode]

La note d'application AN3461 a été fortement remaniée : AN3461 : Tilt Sensing Using Linear Accelerometers by: Mark Pedley (2013). Seuls quelques points sont examinés maintenant.

  • Les calculs correspondants à l'équation sont instables quand gy et gz sont tous deux voisins de 0, c'est-à-dire que l'axe x se retrouve vertical (ou presque à cause du bruit des accéléromètres)
  • L'équation est toujours stable dans le champ de gravitation

Le problème de l'instabilité de calcul est résolu dans la note d'application AN3461 et nous renvoyons le lecteur intéressé à sa lecture. Nous nous contentons de donner le résultat final :

Mark Pedley introduit dans une autre section le calcul d'angle entre deux lectures de l'accéléromètre. Nous rappelons au lecteur qui lirait cette section que l'angle qui y est calculé n’est pas forcément l'angle de rotation 3D. Mark Pedley ne prétend rien sur ces angles et n'utilise pas ses formules sur des données d'accéléromètres. Pour nous, il serait plus sage de faire disparaître cette section de cette note d'application pour lever toute ambiguïté.

Voir aussi[modifier | modifier le wikicode]

Utiliser l'accéléromètre seul pour trouver l'attitude[modifier | modifier le wikicode]

Nous espérons montrer dans cette section que l'accéléromètre est un capteur lent mais surtout qu’il est sensible à l'accélération de pesanteur. C'est cette propriété du capteur que l’on va utiliser pour trouver l'attitude. En effet on sait que cette pesanteur est toujours dirigée vers le bas.

Une autre manière de dire les choses est que si la seule accélération est celle de la pesanteur (pas d'autres forces que le poids) alors la projection de cette accélération sur les trois axes permet trouver l'attitude. On réalisera ce genre de calcul plus tard.

Utiliser un Arduino pour lire le capteur[modifier | modifier le wikicode]

Comment câbler l'Arduino au capteur qui nous intéresse ?

Câblage[modifier | modifier le wikicode]

Le câblage d'un Arduino UNO avec le GY-521 se trouve ICI. On ne reproduira donc pas ce schéma.

Exercice 1[modifier | modifier le wikicode]

1°) Nous allons commencer par le programme tout simple :

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.println(AcZ);
  delay(333);
}

Essayez ce programme sur la liaison série.

La documentation du MPU 6050 dit que la valeur en g de l'accélération se trouve en divisant par 16384 (voir page 13). Êtes-vous d'accord avec la documentation ? Préciser votre méthode pour répondre à la question.

2°) Vous allez reprendre maintenant la mise en œuvre de la première question pour calculer pour chacun des axes la correction d'amplification ainsi que celle de l'offset. La correction d'amplification se trouve en mesurant la valeur de g pour un axe puis en retournant l'axe. La différence des deux valeurs divisée par 2*16384 donne le facteur de correction.

  • Si celui-ci est proche de 1 on ne fera aucune correction d'amplification.
  • L'offset sera pris entre les deux valeurs mesurées, pour que chacune des deux valeurs soit le plus proche possible de 16384 (au signe près).

Les valeurs trouvées sont à noter quelque part. Elles sont très dépendantes des circuits utilisés. Vous trouverez donc les nôtres dans les extraits de code donnés dans la suite, mais vous devrez y adapter les vôtres pour un fonctionnement correct.

Exercice 2[modifier | modifier le wikicode]

On va maintenant se servir de la position de l'accélération de pesanteur par rapport à nos axes pour essayer de trouver l'attitude de notre repère.

On va d’abord simplifier le problème en le ramenant en deux dimensions. L'accéléromètre que vous utilisez a ses deux axes X et Y clairement indiqués. Naturellement l'axe Z est perpendiculaire à ces deux axes.

On va s'intéresser à une rotation autour de l'axe Y (tangage). L'accéléromètre devra donc rester horizontal par rapport à l'axe X !!!

Projection de l'accélération de pesanteur sur deux axes X et Z

A l'aide de la figure explicative ci-dessous, on vous demande de retrouver l'angle de tangage de votre repère. Utilisez les résultats de la question 2° de l'exercice 1 pour affiner vos résultats.

Indication : Quelle est la relation entre les coordonnées ax et az et l'angle ? Le calcul d'une tangente inverse est en général mieux réalisé par la primitive atan2 que atan. Chercher la documentation sur ce point sur Internet.

Note : Il est possible que vous considériez l'axe Z dans l'autre sens ! Tout dépend comment vous tenez l'accéléromètre.

Exercice 3[modifier | modifier le wikicode]

On vous demande de réaliser ce que montre la vidéo de cette page (ou directement dans youtube). Cela nécessite un servomoteur et un accéléromètre.

Utiliser Processing pour visualiser[modifier | modifier le wikicode]

On désire réaliser avec Processing une visualisation des données de l'exercice 2 précédent.

Exercice 4[modifier | modifier le wikicode]

On va chercher à visualiser concrètement les résultats de l'exercice précédent. On va naturellement commencer par le plus simple, la 2D.

Construire un programme simple de visualisation pour la question 1°) de l'exercice 2. Vous pouvez pour simplifier commencer par n'envoyer que l'angle par la liaison série et plus du tout les composantes de l'accélération.

Indication : Voici un programme capable de récupérer la chaîne "Phi = -0.67" envoyée par l'Arduino et de la transformer en nombre réel :

import processing.serial.*;

Serial port; // The serial port
int lf = 10; // Linefeed in ASCII
int serialCount=0;
float phi;
String myString=null;

void setup() {
// ne fonctionne pas sous Windows !!!
  String portName = "/dev/ttyACM0";
  port = new Serial(this, portName, 9600);
  frameRate(4);
}

void draw() {
  String floatString=null;
  while (port.available() > 0) {
    myString = port.readStringUntil(lf);
  }
  if (myString != null) {
  // On retire les 6 premiers caractères
    floatString = myString.substring(6);
    print(floatString);print(" converti en : ");
  // la fonction float() fait tout le boulot
    phi = float(floatString);
    println(phi);
  }
}

A partir de maintenant, la partie processing sera fournie. Elle sera bien plus sophistiquée que ce que l’on vient de faire puisqu'elle sera destinée à recevoir les données de notre capteur et d’en faire plusieurs représentations 3D.

Exercice 5[modifier | modifier le wikicode]

Soit le programme Processing suivant :

C'est le programme que l’on utilisera systématiquement par la suite. Il peut être trouvé dans la partie à télécharger de cette page.

Ce programme est sensible au type de liaison série utilisé et particulièrement au système d'exploitation. La ligne en début de programme :

String portName = "/dev/ttyACM0"; // parfois "/dev/ttyACM1" chez moi

est propre à Linux et devra donc être adaptée !


1°) Vous allez vous intéresser à "void serialEvent(Serial p)" qui est appelé chaque fois qu'un événement a lieu sur la liaison série. Son objectif est de recevoir et de séparer les données. Si l’on veut utiliser le programme tel quel, il faut lui fournir des données correctes qu’il interprétera. Pour cela il faut remarquer que trois parallélépipèdes sont dessinés :

  • celui de gauche pour interpréter les données du gyroscope
  • celui du milieu pour interpréter les données de l'accéléromètre
  • celui de droite pour interpréter les données des deux capteurs à la fois (fusion de données)

Pour cette question, on vous demande d'envoyer toutes les données à zéro (nombres réels avec deux chiffres après la virgule) sauf celles de l'accéléromètre. Essayez de réaliser une rotation progressive du parallélépipède du centre (sans travailler sur l'accélération de pesanteur).

2°) Construire ensuite le même programme en vous intéressant maintenant à la position de l'accélération de pesanteur. Pour simplifier commencez par faire fonctionner le programme Processing avec la résolution 2D que vous avez réalisé dans l'exercice précédant. Les données que vous allez envoyer au programme processing seront maintenant liées à votre position de l'accéléromètre par rapport à l'axe x. L'angle doit être fourni en ° et non en radians !

Retrouver l'attitude avec l'accélération de pesanteur

3°) Essayez de généraliser pour trois dimensions avec les formules présentées dans cette section. Elles sont rappelées maintenant :

Seules les deux premières seront utilisées.

N'oubliez pas de réaliser tout en ° ! C'est la figure ci-contre qui nous montre ce que l’on cherche à réaliser.

Améliorer les données de l'accéléromètre avec un filtrage

Une amélioration possible consiste à réaliser un filtre passe-bas pour filter les données de l'accélération. Vous avez, en effet, certainement noté la "nervosité" de la réponse de l'accéléromètre et un filtrage moyenneur s'impose donc de lui même.

4°) Réaliser le filtrage passe-bas en prenant les contraintes suivantes :

  • réalisation de la boucle "loop()" environ 100 fois par seconde
  • réalisation du filtre passe-pas avec "angle = (0.75)*(angle) + (0.25)*(nouveau_calcul);"

Complément sur l'exercice 5[modifier | modifier le wikicode]

Panneau d’avertissement la majorité du code dans cette section est liée à un optimisme excessif de l'auteur. Il comporte de grosses erreurs et finira par disparaître ou sera fortement modifié !

Nous donnons maintenant un code qui calcule au mieux l'attitude à partir des données de l'accéléromètre. Nous le donnons tout fait pour ceux qui voudraient l'essayer.

Ce code montre qu’il est possible de faire un peu mieux que ce l’on a fait dans l'exercice 4 mais de manière bien plus complexe.

Ce travail montre qu’il nous faut ajouter un capteur supplémentaire si l’on veut retrouver correctement l'attitude.

Deuxième complément sur l'exercice 5[modifier | modifier le wikicode]

On vous demande maintenant d'implanter ce qui est présenté dans cette section. Les formules mathématiques à implanter deviennent donc :

Utiliser le gyroscope seul pour trouver l'attitude[modifier | modifier le wikicode]

Nous espérons montrer dans cette section que le gyromètre est un capteur rapide mais qu’il possède un inconvénient, c’est qu’il dérive !

Nous allons aussi garder la partie Processing de la section précédente puisque la représentation du problème graphique est la même.

Un programme Arduino pour commencer[modifier | modifier le wikicode]

On donne :

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain

#include<Wire.h>

const int MPU=0x68; // I2C address of the MPU-6050
int16_t GyX,GyY,GyZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 14 registers
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

Exercice 6[modifier | modifier le wikicode]

1°) Montrer que ce programme fonctionne.

Panneau d’avertissement

Un gyroscope est sensible à la vitesse de rotation (et non pas à l'angle) !!! D'où la nécessité de calcul de l'intégrale pour obtenir l'angle.

2°) Réaliser une intégration des vitesses angulaires (comment faire ?) pour trouver les angles.

Utiliser le gyroscope pour retrouver l'attitude

L'image ci-contre montre ce que nous cherchons à faire à ce stade.

Indications :

i) On rappelle que l'opérateur intégration est un opérateur qui peut poser un certain nombre de problèmes. Un des plus important est l'intégration de la moyenne (qui n'est jamais nulle même sur un bon GBF !!!) et qui finit par saturer la sortie. Il faudra à tout prix développer la phase de calibration que l’on a un peu laissé tombé jusqu'ici. Un moyen est qu'au départ du programme le GY-521 soit au repos et que l’on effectue une dizaine de mesures sur tous les axes pour en faire une moyenne. Cette moyenne sera considérée comme un décalage à soustraire.

ii) La sortie sera réalisée par quelque chose comme :

// Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));

si l’on veut rester compatible avec Processing.

iii) Réaliser l'intégration par la formule récursive : "angle = angle + gyro*dt" pour chacun des angles apparaissant dans le programme ci-dessus. "dt" sera obtenu avec la primitive "millis()" qui comme son nom l'indique retourne des millisecondes.

iv) L'obtention de la vitesse en °/s se fait par une division à l'aide d'une valeur

// Convert gyro values to degrees/sec
  float FS_SEL = 131;

Voici donc un nouveau programme qui respecte les mathématiques des rotations 3D. Il est destiné à comparer l'intégration naïve ci-dessus à celle que l’on est sensé faire.

3°) Tester suffisamment longtemps pour montrer la dérive d'un gyroscope.

Coupler l'accéléromètre et le gyroscope[modifier | modifier le wikicode]

Puisqu'on a pu noter des qualités et défauts des deux capteurs d'accélération et de vitesse angulaire, il est légitime de se demander si l’utilisation de ces deux capteurs ne pourrait pas produire un capteur global de meilleure qualité. C'est ce que nous allons essayer d'examiner maintenant.

Ce couplage est très bien décrit pour le cas particulier deux dimensions dans The Balance Filter, mais en anglais bien sûr.

Un programme simple pour commencer[modifier | modifier le wikicode]

Nous donnons un programme très simple qui relève les donnée de l'accéléromètre et celles du gyroscope. Remarquez que vous avez en bonus la température.

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain

#include<Wire.h>

const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true); // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

Si vous avez suivi jusque là, vous pouvez facilement déduire de ce programme qu’il n’est pas prévu pour des visualisations de données avec processing. En effet il fonctionne à 9600 bauds (au lieu des 38400 exigées). Les données ne peuvent être visibles qu’à l'aide du moniteur série.

Nous pourrions dans un premier temps nous intéresser au problème à deux dimensions car il y a un certain nombre de problèmes intéressants qui peuvent déjà y être associés. Tapez par exemple "self balancing bot" dans un moteur de recherche pour voir des exemples intéressants.

Mais puisque les exercices 4 et 5 travaillent déjà en 3 dimension, nous allons tout simplement les réunir.

Exercice 7 : Et si l’on regroupait les exercices 5 et 6 ensemble[modifier | modifier le wikicode]

Vous allez maintenant faire fonctionner l'accéléromètre et le gyroscope de manière indépendante sur le parallélépipède central et sur celui de gauche. Cela revient à réunir les codes des exercices 4 et 5. Vous en profiterez pour écrire ce code avec un peu plus de sous-programmes pour en augmenter sa lisibilité.

1°) Si nous mélangeons les données de l'accéléromètre et du gyroscope, nous aurons une sortie comme :

// Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));

On vous demande de réaliser un sous-programme appelé "envoiAProcessing()" ayant un paramètre de type :

struct data
  {
    int x_accel;
    int y_accel;
    int z_accel;
    int temperature;
    int x_gyro;
    int y_gyro;
    int z_gyro;
  };

qui regroupe donc toutes les données importantes et un autre paramètre de type float pour envoyer dt.

2°) Une fois le sous-programme réalisé, faire un sous-programme "aquisitionDonnees()" à partir des programmes d'exemples donnés et tester l'ensemble. Maintenant deux parallélépipèdes sont concernés (celui de gauche et celui du centre).

Exercice 8 : apothéose finale[modifier | modifier le wikicode]

Utiliser un filtre balance pour retrouver l'attitude

Nous allons réaliser le filtre balance représenté dans la figure ci-contre.

Les formules utilisées sont rappelées dans la figure. La formule à droite peut être décomposée comme :

  • angle = (angle + gyro * dt) est une intégration
  • (0.02)*(x_acc) ajouté à l'intégration agit comme un filtre passe-bas des données de l'accéléromètre
  • la multiplication de l'intégration par 0.98 agit comme un filtre passe-haut

Le fait que le filtre numérique passe-bas se retrouve encore dans ce calcul fait que l’on peut prendre directement les entrées de l'accéléromètre non filtrée comme entrée de notre filtre balance. Ceci ne correspond pas tout à fait à la figure présentée. Des tests nous permettrons de choisir entre les deux.

Plus loin avec le Digital Movements Processor (DMP)[modifier | modifier le wikicode]

Ce qui fait la richesse de ce capteur MPU6050 c'est qu'il est associé avec un DMP. Il est donc possible d'utiliser ce processeur en lieu et place du filtre complémentaire (déjà rencontré) ou du filtre de Kalman.

Heureusement, la bibliothèque de Jeff Rowberg et en particulier un de ses exemples "MPU6050_DMP6.ino" fait tout le travail. L'exemple fonctionne avec le fichier "MPU6050_6Axis_MotionApps20.h" : lisez donc ce fichier pour vous convaincre du travail pour écrire cette librairie !

Plus loin avec Kalman[modifier | modifier le wikicode]

Le filtre de Kalman est tout indiqué pour ce que l’on est en train de réaliser. Son approche de manière théorique est pourtant difficile. Ce type de filtre est approprié pour :

  • la fusion de données, c’est exactement ce que l’on est en train de faire : trouver des valeurs qui correspondent au mieux aux données bruitées provenant de deux capteurs
  • le bruit dont on vient de parler doit être distribué suivant une loi normale ou gaussienne

Routines mathématiques 3D pour l'Arduino[modifier | modifier le wikicode]

Travail sur les vecteurs 3D[modifier | modifier le wikicode]

Voici un ensemble de routines permettant de réaliser un ensemble de calculs sur des vecteurs. Sont au menu :

  • normalisation
  • produit scalaire (dot product)
  • produit vectoriel (cross product)

Elles nécessitent l’utilisation des vecteurs sous forme de tableau.

Travail sur les matrices 3x3[modifier | modifier le wikicode]

Matrices avec un tableau à une dimension[modifier | modifier le wikicode]

Les matrices sont supposées être des tableaux à une seule dimension dans les routines proposées ci-dessous.

Pour ceux qui seraient frustrés d’utiliser un tableau à une dimension pour les matrices, on vous présente maintenant un autre code.

Matrice avec classe C++[modifier | modifier le wikicode]

Voici d’abord le fichier d'entête :

Et maintenant le code (Merci à Loïc Kaemmerlen et Pascal Madesclair)

Quaternions[modifier | modifier le wikicode]

Voici un ensemble de routines utilisant les quaternions proposé par Jeff Rowberg.

Calibration du Invensense MPU6050[modifier | modifier le wikicode]

Le MPU6050 demande à être calibré parce qu'il n'est pas possible de le faire en usine. En effet cette calibration consiste à ajouter (ou retirer) des offsets qui sont dépendants de chacun de vos MPU6050. Si vous en avez donc plusieurs, il vous faudra automatiquement faire cette opération pour chacun d'eux.

Dans les codes d'exemple d'utilisation de l'accéléromètre, vous pouvez trouver des lignes du genre :

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

et votre problème consiste à trouver toutes les valeurs correspondants à votre MPU6050. Pour cela vous pouvez utiliser du code que l'on trouve sur Internet.

Le code précédent n'est pas complet. Une page en français sur le sujet de la calibration existe aussi avec un code complet.

Aussi sur Internet[modifier | modifier le wikicode]