Calcul du tangage, du lacet et du roulis à partir des données mag, acc et gyro

19

J'ai une carte Arduino avec un capteur de 9 degrés de liberté, à partir duquel je dois déterminer le tangage, le lacet et le roulis de la carte.

Voici un exemple d'un ensemble de données du capteur 9-DOF:

Accéléromètre (m / s)

  • AccX = -5,85
  • AccOui = 1,46
  • AccZ = 17,98

Gyroscope (RPM)

  • GyrX = 35,14
  • GyrOui = -40,22
  • GyrZ = -9,86

Magnétomètre (Gauss)

  • MagX = 0,18
  • MagOui = -0,04
  • MagZ = -0,15

Comment puis-je calculer le tangage, le lacet et le roulis à partir de ces données?

MaltDew
la source
1
Principe de base: à partir de la détection de la gravité dans votre accéléromètre, vous savez dans quel sens est descendu; grâce à la détection du champ magnétique terrestre dans votre magnétomètre, vous savez dans quelle direction se trouve le Nord. Sur cette base et en supposant qu'aucune autre accélération significative ou champ magnétique puissant ne vous permet de déterminer votre propre attitude.
welf
1
Les données du gyroscope fournissent une vitesse de rotation, mais pas une position absolue. Il peut être intégré pour estimer le changement par rapport à une attitude connue, mais il est généralement bruyant et sujet à dérive s'il n'est pas utilisé en conjonction avec les autres capteurs.
welf
1
se réfèrent également aux filtres de Kalman, car les nombres statiques doivent être traités beaucoup, pour donner des estimations fiables du roulis et du lacet. Notez également que la position du capteur est importante (vous devez en tenir compte).
Gürkan Çetin

Réponses:

15

Le tangage, le roulis et le lacet sont définis comme la rotation autour des axes X, Y et Z. Ci-dessous comme image pour illustrer la définition.

Roll Pitch and Yaw

Dans un projet précédent, j'ai utilisé un accéléromètre ADXL345 d'Analog Devices pour calculer le roulis et la hauteur. Voici les équations utilisées pour calculer le roulis et le tangage. J'ai mis du code source à la disposition du public.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Le code source complet peut être trouvé ici .

Base sur les définitions ci-dessus

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Remarque: M_PI = 3,14159265358979323846 c'est une constante définie dans math.h

Voici quelques références, y compris le code source de base Arduino, qui pourraient vous aider.


Les références:

Mahendra Gunawardena
la source
2
Belle réponse, il vaudrait la peine d'ajouter que la position et l'orientation du capteur dans le véhicule seraient importantes, et que les données doivent être traitées plus avant, pour donner des résultats fiables. (filtré ou fusionné avec des données basse fréquence plus fiables, comme le GPS)
Gürkan Çetin
(@Zubair) "lacet = 180 * atan (accélérationZ / sqrt (accélérationXaccélérationX + accélérationZaccelerationZ)) / M_PI;" qu'est-ce que 'M_PI' ??
Wasabi
@Wasabi M_PI = 3.14159265358979323846. Il est constant défini dans la bibliothèque math.h.
Mahendra Gunawardena
8

Donc, ma réponse plus longue ci-dessous suppose que la planche subira une accélération et pendant ce temps, vous devez toujours être en mesure de mesurer votre tangage, roulis et lacet dans un court laps de temps. Si la carte est immobile pour toutes les mesures, la réponse de Mahendra Gunawardena fonctionnera parfaitement pour vous. Si cela va dans un appareil comme un segway ou un avion modèle ou un multirotor ou tout ce qui se déplace, vous voudrez peut-être continuer à lire. Cet article explique comment utiliser les trois capteurs grâce à une méthode appelée fusion de capteurs. La fusion des capteurs vous permet d'obtenir les points forts de chaque capteur et de minimiser les effets des faiblesses de chaque capteur.

Caractéristiques et arrière-plan du capteur

Comprenez d'abord qu'un accéléromètre mesure toutes les forces qui lui sont appliquées, pas seulement la force de gravité. Donc, dans un monde parfait avec l'accéléromètre dans une position stationnaire sans aucune vibration, vous pouvez parfaitement déterminer la direction vers le haut en utilisant une trigonométrie de base comme le montre la réponse de Mahendra Gunawardena. Cependant, comme un accéléromètre captera toutes les forces, toute vibration entraînera du bruit. Il convient également de noter que si la planche accélère, vous ne pouvez pas simplement utiliser la trigonométrie simple car la force rapportée par l'accéléromètre n'est pas seulement la force de gravité terrestre, mais aussi la force qui vous fait accélérer.

Un magnétomètre est plus simple qu'un accéléromètre. Le mouvement ne causera pas de problèmes, mais des choses comme le fer et d'autres aimants finiront par affecter votre sortie. Si les sources provoquant ces interférences sont constantes, il n'est pas difficile de les gérer, mais si ces sources ne sont pas constantes, cela créera des tonnes de bruit qu'il sera difficile d'éliminer.

Des trois capteurs, le gyroscope est sans doute le plus fiable et ils sont normalement très très bons pour mesurer la vitesse de rotation. Il n'est pas affecté par des éléments comme les sources de fer et les accélérations n'ont pratiquement aucun impact sur leur capacité à mesurer la vitesse de rotation. Ils font un très bon travail pour signaler la vitesse à laquelle l'appareil tourne, mais comme vous recherchez un angle absolu, vous devez intégrer la vitesse pour obtenir la position. Cela ajoutera l'erreur de la dernière mesure à l'erreur des nouvelles mesures, car l'intégration est essentiellement une somme de valeurs sur une plage, même si l'erreur pour une mesure n'est que de 0,01 degré par seconde, sur 100 mesures, votre position peut être désactivé de 1 degré, par 1000 mesures, vous pouvez désactiver de 10 degrés. Si vous prenez des centaines de mesures par seconde, vous pouvez voir que cela cause des problèmes. Ceci est communément appelé dérive gyroscopique.

Fusion de capteurs

Maintenant, la beauté de tous ces capteurs fonctionne ensemble, c'est que vous pouvez utiliser les informations de l'accéléromètre et du magnétomètre pour annuler la dérive gyroscopique. Cela vous permet de vous donner la précision et la vitesse du gyroscope sans le défaut fatal de la dérive du gyroscope.

La combinaison des données de ces trois capteurs peut se faire de plus d'une manière, je parlerai de l'utilisation d'un filtre complémentaire car c'est beaucoup plus simple qu'un filtre kalman et des filtres kalman consommeront beaucoup plus de ressources sur les systèmes embarqués. Souvent, un filtre complémentaire est assez bon, plus simple à implémenter (en supposant que vous n'utilisez pas de bibliothèque pré-construite) et vous permet de traiter les données plus rapidement.

Passons maintenant au processus. Les premières étapes que vous devez faire sont d'intégrer la sortie du gyroscope pour convertir la vitesse angulaire en position angulaire. Vous devrez également très probablement appliquer un filtre passe-bas sur l'accéléromètre et le magnétomètre pour faire face au bruit dans la sortie. Un simple filtre FIR comme celui illustré ci-dessous fonctionne ici. Avec une trigonométrie, vous pouvez trouver le tangage et le roulis avec l'accéléromètre et le lacet avec le magnétomètre.

filteredData = (1-weight)*filteredData + weight*newData

Le poids est juste une constante qui peut être ajustée en fonction de la quantité de bruit à traiter, plus le bruit est élevé, plus la valeur de poids sera petite. Désormais, la combinaison des données des capteurs peut être effectuée par la ligne de code suivante.

fusedData = (1-weight)*gyroData + weight*accelMagData

Il convient de noter que les données sont un vecteur de tangage, de roulis et de lacet. Vous pouvez simplement utiliser trois variables pour le faire également au lieu de tableaux si vous le souhaitez. Pour ce calcul, le gyroscope fournit une position en degrés en tangage, roulis et lacet, le magnétomètre fournit un angle pour le lacet tandis que l'accéléromètre fournit ses propres nombres pour le tangage et le roulis.

Si vous voulez toujours plus d'informations, vous pouvez google "fusion de capteurs avec filtre complémentaire", il y a beaucoup d'articles à ce sujet.

Dave851
la source
3

À partir des données du capteur d'accélérateur, vous ne pouvez calculer que le tangage et le roulis. Le document ci-dessous de Freescale explique avec beaucoup d'informations ce dont vous avez besoin:

AN3461 - Détection d'inclinaison à l'aide d'un accéléromètre à trois axes

Sur la base des paroles du document,

bronzerϕXyz=gpygpz

bronzerθXyz=-gpXgpypéchéϕ+gpzcosϕ=-gpXgpy2+gpz2

ce qui équivaut à:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Bien sûr, le résultat n'est que lorsque les rotations se produisent sur un ordre spécifique (Rxyz):

  1. ϕ
  2. θ
  3. ψ

RXyzψ

ekalyvio
la source
1
Bienvenue sur Engineering SE! Ce site prend en charge Latex, regardez à quel point votre réponse est devenue belle maintenant. :-)
peterh