Étape 3: Combinant l’accéléromètre et gyroscope
Reconstituer les - combinant données accéléromètre et gyroscope.
Si vous lisez cet article vous probablement acquis ou envisagez d’acquérir un appareil IMU, ou probablement vous avez l’intention de construire un accéléromètre distinct et dispositifs de gyroscope.
La première étape dans l’utilisation d’un dispositif IMU de combinaison qui combine un accéléromètre et un gyroscope est d’aligner leurs systèmes de coordonnées. La meilleure façon de le faire est de choisir le système de coordonnées d’accéléromètre comme votre repère de référence. Fiches de données de la plupart accéléromètre affiche la direction de X, Y, les axes Z par rapport à l’image de la puce physique ou un dispositif. Par exemple, Voici les directions de X, Y, axes Z comme indiqué dans les spécifications de la Commission du Acc_Gyro :
Prochaines étapes sont :
- Identifier les sorties de gyroscope qui correspondent aux RateAxz, RateAyz valeurs évoquées ci-dessus. Déterminer si ces sorties doivent être inversées en raison de la position physique du gyroscope par rapport à l’accéléromètre
Ne supposez pas que si un gyroscope possède une sortie marquée X ou Y, elle doit correspondre à n’importe quel axe dans le repère de l’accéléromètre, même si cette sortie fait partie d’une unité de l’IMU. La meilleure façon est de le tester. En supposant que vous avez fixé la position de gyroscope par rapport à l’accéléromètre. Il est supposé que les frontières du gyroscope et l’accéléromètre sont parallèles les uns aux autres, c'est-à-dire vous placez le compas gyroscopique sous un angle multiple de 90 ° par rapport à la puce de l’accéléromètre. Si vous avez acquis un Comité IMU les chances sont qu’ils sont déjà alignés de cette façon. Nous n’allons pas discuter dans les modèles de cet article où le gyroscope est placé à un angle irrégulier par rapport à l’accéléromètre (disons 45 ou 30 degrés), bien que cela pourrait être utile dans certaines applications.
Voici une séquence de l’échantillon pour déterminer quelle sortie de gyroscope correspond à la valeur de RateAxz nous l’avons vu.
-Démarrez à partir de placer l’appareil en position horizontale. Sorties de X et Y d’accéléromètre seraient sortie la tension zéro-g (par exemple pour Acc_Gyro board c’est 1.65V)
-ensuite commencer à tourner l’appareil autour de l’axe des Y, une autre façon de dire c’est que vous faites pivoter l’appareil dans le plan XZ, afin que l’accéléromètre X et Z sorties de changement et de sortie Y reste constante.
-en faisant tourner l’appareil à une note de vitesse constante dont gyroscope sortie changements, les autres sorties de gyroscope doivent rester constantes
-la sortie de gyroscope qui a changé au cours de la rotation autour de l’axe Y (rotation dans le plan XZ) fournit la valeur d’entrée pour AdcGyroXZ, d'où nous calculons RateAxz
-la dernière étape consiste à s’assurer que le sens de rotation correspond à notre modèle, dans certains cas, vous devrez peut-être inverser la valeur de RateAxz en raison de la position physique du gyroscope par rapport à l’accéléromètre
-effectuer de nouveau le test ci-dessus, en faisant tourner l’appareil autour de l’axe des Y, cette fois contrôler la sortie de X de l’accéléromètre (AdcRx dans notre modèle). Si AdcRx se développe (les premiers 90 degrés de rotation de la position horizontale), puis AdcGyroXZ devrait également croître. Dans le cas contraire, vous devrez inverser RateAxz, vous pouvez y parvenir en introduisant un facteur signe Eq.3, comme suit :
RateAxz = InvertAxz * (AdcGyroXZ * Vréf / 1023 - VzeroRate) / sensibilité, où InvertAxz est 1 ou -1
même test canne faire pour RateAyz, en faisant tourner l’appareil autour de l’axe X, et vous pouvez identifier quelle sortie de gyroscope correspond à RateAyz, et si elle doit être inversé. Une fois que vous avez la valeur pour InvertAyz, vous devez utiliser la formule suivante pour calculer les RateAyz :
RateAyz = InvertAyz * (AdcGyroYZ * Vréf / 1023 - VzeroRate) / sensibilité
Si vous feriez ces tests à bord de l’Acc_Gyro vous serait obtenir après des résultats :
-la fiche de sortie pour RateAxz soit GX4 et InvertAxz = -1.
-la fiche de sortie pour RateAyz soit GY4 et InvertAyz = -1
À partir de ce moment, nous allons examiner que vous avez configuré votre Mio de telle sorte que vous pouvez calculer des valeurs correctes pour Axr, Ayr, Azr (comme définies Partie 1. Accéléromètre) et RateAxz, RateAyz (tel que défini dans la partie 2. Gyroscope). Ensuite, nous analyserons les relations entre ces valeurs qui s’avèrent utiles pour obtenir une estimation plus précise de l’inclinaison de l’appareil par rapport au plan de masse.
Vous demandez peut-être par ce point, si le modèle de l’accéléromètre nous a déjà donné des angles d’inclinaison de Axr, Ayr, Azr pourquoi voudrions-nous pas s’embêter avec les données de gyroscope ? La réponse est simple : données de l’accéléromètre ne peut pas être toujours confiance 100 %. Il y a plusieurs raisons, n’oubliez pas que l’accéléromètre mesure la force d’inertie, une telle force peut être provoquée par la gravitation (et idéalement seulement par gravitation), mais peut aussi être dû à l’accélération (mouvement) de l’appareil. Ainsi même si l’accéléromètre est dans un état relativement stable, il est toujours très sensible aux vibrations et bruits mécaniques en général. C’est la raison principale pourquoi la plupart des systèmes IMU utilisent un gyroscope pour lisser les éventuelles erreurs de l’accéléromètre. Mais comment cela se fait ? Et le gyroscope n’est exempt de bruit ?
Le gyroscope n’est pas exempt de bruit cependant car il mesure la rotation, il est moins sensible aux mouvements mécaniques linéaires, le type de bruit qu’accéléromètre souffre, mais les gyroscopes ont d’autres types de problèmes comme par exemple dérive (ne pas revenir à taux nul valeur lorsque la rotation s’arrête). Néanmoins, en faisant la moyenne des données qui vient de l’accéléromètre et gyroscope, nous pouvons obtenir une estimation relativement mieux d’inclinaison du dispositif actuel que nous obtiendrions en utilisant les données de l’accéléromètre seules.
Dans les prochaines étapes, que je vais vous présenter un algorithme qui s’inspire de certaines idées utilisées dans le filtre de Kalman, cependant il est de loin la plus simple et plus facile à mettre en œuvre sur les appareils embarqués. Avant cela voyons d’abord ce que nous voulons notre algorithme pour calculer. Eh bien, c’est la direction du vecteur de force de gravitation R = [Rx, Ry, Rz] de qui nous pouvons dériver autres valeurs comme Axr, Ayr, Azr ou cosX, cosZ confortable, qui va nous donner une idée sur l’inclinaison de notre dispositif par rapport au plan de masse, nous discutons de la relation entre ces valeurs dans la partie 1. On pourrait dire - n’avons-nous déjà ces valeurs Rx, Ry, Rz EQ. 2 dans la partie 1 ? Eh bien oui, mais n’oubliez pas que ces valeurs sont issues de données provenant uniquement, donc si vous voulez les utiliser directement dans votre application, vous pouvez obtenir plus de bruit que votre application peut tolérer. Pour éviter encore plus de confusion nous allons re-définir les mesures de l’accéléromètre comme suit :
RACC - est le vecteur de la force d’inertie mesurée par accéléromètre, qui se compose des éléments suivants (projections sur X, Y, axes Z) :
RxAcc = (AdcRx * Vréf / 1023 - VzeroG) / sensibilité
RyAcc = (AdcRy * Vréf / 1023 - VzeroG) / sensibilité
RzAcc = (AdcRz * Vréf / 1023 - VzeroG) / sensibilité
Jusqu'à présent, nous avons un ensemble de valeurs mesurées que nous pouvons obtenir simplement à partir de valeurs ADC accéléromètre. Nous appellerons cet ensemble de données un « vecteur » et nous utiliserons la notation suivante.
RACC = [RxAcc, RyAcc, RzAcc]
Parce que ces composants du Racc peuvent être obtenues à partir de données de l’accéléromètre, on peut considérer une entrée de notre algorithme.
Veuillez noter que parce que Racc mesure la force de gravitation, que vous serez correct si l'on suppose que la longueur de ce vecteur défini comme suit est égale ou proche de 1 g.
| RACC| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),
Cependant pour être sûr, il est logique de mettre à jour ce vecteur comme suit :
RACC(normalized) = [RxAcc / | RACC| , RyAcc / | RACC| , RzAcc / | RACC|].
Cela garantira que la longueur de votre vecteur normalisé de Racc est toujours 1.
Ensuite, nous présenterons un nouveau vecteur et nous allons l’appeler
Reste = [RxEst, RyEst, RzEst]
Ce sera le résultat de notre algorithme, ce sont les valeurs corrigées d’après les données du gyroscope et issu des dernières données estimatives.
Voici ce que notre algorithme va faire :
-accéléromètre nous dit: « Vous êtes maintenant à la position Racc »
-Nous disons « Merci, mais je voudrais vérifier »,
-puis corrigez cette information avec des données de gyroscope, ainsi qu’avec les dernières données reste et nous un nouveau vecteur estimé reste de sortie.
-Nous considérons reste à être notre meilleur « pari » quant à la position actuelle de l’appareil.
Nous allons voir comment nous pouvons le faire fonctionner.
Nous allons commencer notre séquence en faisant confiance à notre accéléromètre et en attribuant :
REST(0) = Racc(0)
Soit dit en passant n’oubliez pas de reste et Racc sont des vecteurs, donc l’équation ci-dessus est juste une façon simple d’écrire 3 définit des équations et éviter la répétition :
RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)
Ensuite, nous ferons des mesures régulières à intervalles égaux de secondes T, et nous allons obtenir des nouvelles mesures que nous définirons comme Racc(1), Racc(2), Racc(3) et ainsi de suite. Nous allons également délivrer des nouvelles estimations à chaque intervalle de temps Rest(1), Rest(2), Rest(3) et ainsi de suite.
Supposons que nous sommes à l’étape n. Nous avons deux ensembles connus des valeurs que nous tenons à utiliser :
REST(n-1) - notre estimation précédente, avec Rest(0) = Racc(0)
RACC(n) - notre actuel mesure accéléromètre
Avant que nous puissions calculer Rest(n), nous allons introduire une nouvelle valeur mesurée, que nous pouvons obtenir de notre gyroscope et une estimation précédente.
Nous allons l’appeler Rgyro, et c’est aussi un vecteur composé de 3 éléments :
Rgyro = [RxGyro, RyGyro, RzGyro]
Nous calculons ce un composant vecteur à la fois. Nous allons commencer par RxGyro.
Commençons par observer la relation suivante dans notre modèle de gyroscope, du triangle à angle droit formé par Rz et Rxz nous pouvons tirer que :
Tan(Axz) = Rx/Rz = > Axz = atan2(Rx,Rz)
Atan2 peut être une fonction vous jamais utilisé avant, il est similaire à atan, sauf qu’elle retourne les valeurs dans la plage de (-PI, PI) par opposition à (-PI/2, PI/2) tel que retourné par atan et il prend 2 arguments au lieu d’un. Il permet de convertir les deux valeurs de Rx, Rz à angles dans la gamme complète de 360 degrés (-PI à PI). Vous pouvez en savoir plus sur atan2 ici.
Sachant RxEst(n-1) et RzEst(n-1), nous pouvons trouver :
Axz(n-1) = atan2 (RxEst(n-1), RzEst(n-1)).
N’oubliez pas que gyromètre mesure le taux de variation de l’angle Axz. Ainsi, nous pouvons estimer le nouvel angle Axz(n) comme suit :
Axz(n) = Axz(n-1) + RateAxz(n) * T
N’oubliez pas que les RateAxz peut être obtenue de nos lectures d’ADC de gyroscope. Une formule plus précise peut utiliser un taux de rotation moyen calculé comme suit :
RateAxzAvg = (RateAxz(n) + RateAxz(n-1)) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T
La même manière, que nous pouvons trouver :
Ayz(n) = Ayz(n-1) + RateAyz(n) * T
OK alors maintenant nous avons Axz(n) et Ayz(n). Où allons-nous partir d’ici pour déduire RxGyro/RyGyro ? EQ 1, nous pouvons écrire la longueur du vecteur Rgyro comme suit :
| Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)
Aussi parce que nous avons normalisé nos vecteur Racc, on peut supposer que sa longueur est 1 et il n’a pas changé après la rotation, donc il est relativement prudent d’écrire :
| Rgyro| = 1
Nous allons adopter une notation plus courte temporaire pour les calculs ci-dessous :
x = RxGyro, y = RyGyro, z = RzGyro
En utilisant les relations ci-dessus on peut écrire :
x = x / 1 = x / SQRT(x^2+y^2+z^2)
Nous allons diviser numérateur et dénominateur de la fraction par SQRT (x ^ 2 + z ^ 2)
x = (x / SQRT (x ^ 2 + z ^ 2)) / SQRT ((x ^ 2 + y ^ 2 + z ^ 2) / (x ^ 2 + z ^ 2))
Notez que x / SQRT (x ^ 2 + z ^ 2) = sin(Axz), donc :
x = sin(Axz) / SQRT (1 + y ^ 2 / (x ^ 2 + z ^ 2))
Maintenant multipliez le numérateur et le dénominateur de la fraction à l’intérieur de la racine carrée de z ^ 2
x = sin(Axz) / SQRT (1 + y ^ 2 * z ^ 2 / (z ^ 2 * (x ^ 2 + z ^ 2)))
Notez que z / SQRT (x ^ 2 + z ^ 2) = cos(Axz) et y / z = tan(Ayz), donc finalement :
x = sin(Axz) / SQRT (1 + cos (Axz) ^ 2 * tan (Ayz) ^ 2)
Revenir à notre notation, nous obtenons :
RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n)) ^ 2 * tan(Ayz(n)) ^ 2)
même façon, nous constatons que
RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n)) ^ 2 * tan(Axz(n)) ^ 2)
Maintenant, enfin, nous pouvons trouver :
RzGyro = signe (RzGyro) * SQRT (1 - RxGyro ^ RyGyro 2 - ^ 2).
Où Sign(RzGyro) = 1 lorsque RzGyro > = 0 et Sign(RzGyro) =-1 quand RzGyro < 0.
Un moyen simple d’estimer ceci est à prendre :
Sign(RzGyro) = Sign(RzEst(n-1))
Dans la pratique être prudent lorsque RzEst(n-1) est proche de 0. Vous pouvez sauter la phase gyroscope tout à fait dans ce cas et assigner : Rgyro = Rest(n-1). RZ est utilisé comme référence pour le calcul des angles Axz et Ayz et lorsqu’il est proche de 0, les valeurs peuvent oveflow et déclencheur de mauvais résultats. Vous serez dans le domaine de grand flottante point numéros où tan() / atan() les implémentations de fonction peuvent manquer de précision.
Alors récapitulons ce que nous avons jusqu'à présent, nous sommes à l’étape n de notre algorithme et nous avons calculé les valeurs suivantes :
RACC - lectures actuelles de notre accéléromètre
Rgyro - obtenu à partir des Rest(n-1) et des lectures actuelles de gyroscope
Quelles valeurs utilisons-nous pour calculer l’estimation Rest(n) ? Vous a sans doute deviné que nous allons utiliser les deux. Nous allons utiliser une moyenne pondérée, afin que :
REST(n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)
Nous pouvons simplifier cette formule en divisant le numérateur et le dénominateur de la fraction de w1.
REST(n) = (Racc * w1/w1 + Rgyro * w2/w1) / (w1/w1 + w2/w1)
et après son remplacement par w2/w1 = wGyro, que nous obtenons :
REST(n) = (Racc + Rgyro * wGyro) / (1 + wGyro)
Dans la formule ci-dessus wGyro nous dit combien nous sommes convaincus de notre gyroscope par rapport à notre accéléromètre. Cette valeur peut être choisie expérimentalement habituellement des valeurs entre 5..20 déclenchera des bons résultats.
La différence principale de cet algorithme du filtre de Kalman est que ce poids est relativement fixe, alors que dans Kalman filtre que les poids sont en permanence mises à jour basé sur le bruit mesuré de la lecture de l’accéléromètre. Filtre de Kalman vise à vous donner des résultats théoriques « the best », considérant que cet algorithme peut vous donner des résultats « assez bon » pour votre application pratique. Vous pouvez implémenter un algorithme qui permet de régler wGyro selon certains facteurs de bruit qui vous mesurer, mais des valeurs fixes fonctionnent bien pour la plupart des applications.
Nous sommes un pas loin de faire nos valeurs estimées de mise à jour :
RxEst(n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)
Maintenant nous allons normaliser de nouveau ce vecteur :
R = SQRT(RxEst(n) ^ 2 + RyEst (n) ^ 2 + RzEst (n) ^ 2)
RxEst(n) = RxEst (n) / r
RyEst(n) = RyEst (n) / r
RzEst(n) = RzEst (n) / r
Et nous sommes prêts à répéter notre boucle.
Ce guide a paru sur starlino.com, j’ai fait quelques modifications légères et réaffiché avec permission. Merci Starlino !
