Back | << | Index | >> |
Le circuit intégré MPU9250 contient :
Ce genre de circuit est également appelé Centrale Inertielle ou nertial Measurement Unit (IMU)
Le magnétomètre est un composant à part dans le circuit MPU9250 ; à ce titre son adresse I2C est différente.
Dans le projet nous feront abstraction du magnétomètre qui demande un calibrage délicat.
void mpu9250_InitMPU9250() : | Initialisation accéléromètre + gyroscope : réglage des filtres / Intervalles de mesure (résolution). |
void mpu9250_InitAK8963(float * destination) : | Initialisation du magnétomètre : réglage de la résolution |
uint8_t mpu9250_WhoAmI(void) : | Test liaison I2C accel + gyr , doit retourner 0x71 |
uint8_t mpu9250_MagWhoAmI(void) : | Test liaison I2C magnétomètre |
void mpu9250_Step(void) : | Récupération de toutes les mesures |
void mpu9250_ReadAccelData(int16_t * destination) : | Récupération Accélérations |
void mpu9250_ReadGyroData(int16_t * destination) : | Récupération Vitesses Angulaires |
void mpu9250_ReadMagData(int16_t * destination) : | Récupération Mesures Magnétomètre |
int16_t mpu9250_ReadTempData() : | Récupération Température |
void mpu9250_ResetMPU9250() : | Reset |
void mpu9250_CalibrateMPU9250() : | Calibrage Accéléromètre + Gyroscope : détermination de l’erreur initiale (biais). |
Cette erreur est soustraite par la suite à chaque mesure. |
REMARQUE : Au repos et à plat, on doit observer une accélération d’1G sur l’accéléromètre ( si l’on ne corrige pas les valeurs mesurées avec les valeurs au repos ).
A partir de l’intégration de la vitesse angulaire, il est possible de déduire les ‘attitudes’ d’un objet, autrement dit les angles d’Euler.
Matrice de Rotation Repère Body –> Repère Fixe
Il est possible d’estimer la position d’un objet dans un repère fixe à partir des données de la centrale inertielle, en calculant la matrice de rotation.
Un premier problème se pose concernant l’utilisation des angles d’Euler directement.
En effet la superposition de deux axes de rotation entraine la perte d’un degré de liberté.
Ce problème est très bien illustré dans la vidéo suivante :
Video Euler (gimbal lock) Explained
Un Quaternion est un objet mathématique (avec un ensemble de propriétés) permettant de représenter le déplacement et la rotation d’un objet.
V direction du repère body
\( Q=\begin{bmatrix} q _{0},\ q _{1},\ q _{2},\ q _{3}\ \end{bmatrix} \)
\( Q=\begin{bmatrix} cos(\frac{\theta} {2}),\ a.sin(\frac{\theta} {2}),\ b.sin(\frac{\theta} {2}),\ c.sin(\frac{\theta} {2})\ \end{bmatrix} \)
A chaque Te, on peut calculer la nouvelle valeur du vecteur quaternion à partir de la vitesse :
\( Q _{k+1}=Q _{k}+ \frac{1}{2}.T _{e}.\Omega _{k}.Q _{k} \)
L’application de ce calcul permet d’éviter le problème du gimbal lock
Un autre problème important est à considérer dans le traitement des données de la centrale :
Pour des IMUs bon marché, il est invévitable de réaliser une fusion des données pour que l’accéléromètre compense le défaut du gyroscope.
Si l’utilisation d’un filtre de Kalmann est possible, il existe d’autres algorithmes (plus rapides) comme celui de Madgwick.
L’idée est de compenser l’erreur de mesure du gyroscope en modulant ses valeurs par le résultat d’une comparaison entre une estimation
du champ de gravité et le champ de gravité mesuré (avec l’accéléromètre).
Ce calcul met à jour le quaternion, duquel peuvent être calculées les attitudes (angles d’Euler).
Calcul de la Matrice de Rotation à Partir du Quaternion
\( R _{12} = 2.(q{ _{1}}.q{ _{2}}+q{ _{0}}.q{ _{3}} ) \)
\( R _{22} = q{ _{0}}^{2} + q{ _{1}}^{2} - q{ _{2}}^{2} - q{ _{3}}^{2} \)
\( R _{31} = 2.(q{ _{0}}.q{ _{1}}+q{ _{2}}.q{ _{3}}) \)
\( R _{32} = 2.(q{ _{1}}.q{ _{3}}-q{ _{0}}.q{ _{2}}) \)
\( R _{33} = q{ _{0}}^{2} - q{ _{1}}^{2} - q{ _{2}}^{2} + q{ _{3}}^{2} \)
Calcul des Angles d’Euler à Partir de la matrice de rotation
\( phi = atan2(R_{31}, R_{33}) \)
\( theta = -asin (R_{32}) \)
\( psi = atan2(R_{12}, R_{22}) \)
Documents Complémentaires
Invensense Sensor Introduction
Back | << | Index | >> |