web-dev-qa-db-fra.com

Accéléromètre 3D calculer l'orientation

J'ai des valeurs d'accéléromètre pour les 3 axes (généralement lorsqu'il n'y a que la gravité contient des données comprises entre -1,0 et 1,0):

  float Rx;
  float Ry;
  float Rz;

Je fais des calculs soma, puis je reçois les angles pour chaque axe.

  float R =  sqrt(pow(Rx,2)+pow(Ry,2)+pow(Rz,2));
  float Arx = acos(Rx/R)*180/M_PI;
  float Ary = acos(Ry/R)*180/M_PI;
  float Arz = acos(Rz/R)*180/M_PI;

Ensuite, je règle les valeurs des angles de la boîte en opengl

rquad = Arx;
yquad = Ary;

Qui tourne ma boîte:

glRotatef(yquad,1.0f,0.0f,0.0f);
glRotatef(rquad,0.0f,1.0f,0.0f);

Cela fonctionne sur l'hémisphère. J'aimerais utiliser la totalité de la sphère et je sais que je dois utiliser la valeur Arz pour que cela fonctionne, mais je ne sais pas comment puis-je l'utiliser pour cette rotation. Pourriez-vous m'aider?

Mise à jour: La réponse finale est dans mon cas:

  rquad = -atan2(Rx/R, Rz/R)*180/M_PI;
  yquad = -atan2(Ry/R, Rz/R)*180/M_PI;
cmathopengl
26
Roland Soós

La bonne réponse est:

Roll = atan2(Y, Z) * 180/M_PI;
Pitch = atan2(-X, sqrt(Y*Y + Z*Z)) * 180/M_PI;

Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf (page 10, équations 25 et 26)

la réponse de Uesp est fausse. Cela semble être une approximation acceptable jusqu'à ce que le tangage et le roulis dépassent les 45 degrés.

J'assume peut-être une convention d'orientation différente, mais même si vous permutez les axes et inversez les valeurs de manière cohérente, les calculs de uesp ne seront jamais équivalents.

40
matteo

Bien que la réponse de matteo soit correcte, elle ne fournit pas la solution complète et complète: Les formules sont correctes:

Roll = atan2(Y, Z) * 180/M_PI;
Pitch = atan2(-X, sqrt(Y*Y + Z*Z)) * 180/M_PI;

Toutefois, lorsque la hauteur tonale est de + 90/-90 degrés et que l’axe des X est vertical pointant vers le haut/le bas, la sortie normalisée idéale de l’accéléromètre devrait être:

accX = -1  / accX = 1 
accY = 0
accZ = 0

Ce qui signifie un roll angle of 0 degrees; correct . Mais en pratique, la sortie de l'accéléromètre est bruyante et vous vous rapprocheriez de:

accX = -1  / accX = 1 
accY = 0.003
accZ = 0.004

Cela peut sembler faible, mais l’angle de roulis sera d’environ 30 degrés, ce qui n’est pas correct.

L'instinct évident serait de filtrer les derniers chiffres, mais cela affecterait la précision, ce qui n'est pas toujours acceptable.

Le compromis, qui est très bien expliqué dans la note d'application de référence, consiste à inclure un très faible pourcentage de la lecture de l'axe X de l'accéléromètre dans la formule pour le rouleau:

Roll  = atan2( Y,   sign* sqrt(Z*Z+ miu*X*X));
sign  = 1 if accZ>0, -1 otherwise 
miu = 0.001

L'erreur introduite de cette manière est considérablement plus petite que dans le cas précédent: 2 à 3 degrés lors de la mesure d'un rouleau dans les mêmes conditions que celles décrites ci-dessus.

18
Pandrei

J'ai essayé la solution recommandée (matteo) et, bien que cela semblait fonctionner au premier abord, j'ai remarqué que lorsque le ton s'approche de 90 degrés (commençant à environ 70 degrés mais pas nécessairement cohérent sur différents téléphones), le roulement augmente soudainement. Quand le pitch est à 90, le résultat qui devrait être autour de 0 est maintenant à plus de 100 et continue d'augmenter jusqu'à 180. J'essaie de trouver un moyen mathématiquement d'éviter cela, si je limite le résultat à + 90/-90, se comporte normalement mais je n'obtiens pas la plage que je veux (+ 180/-180): Math.atan2 (y, Math.sqrt ((xx) + (z z)))) * (180 /Math.PI))

3
FaithNoMan

Pour roll, j’ai trouvé que vous pouvez utiliser arctan(y/sqrt(X*X)+(z*z)). Cela donnera le code -90/90 qui correspond au standard de l’aviation sans donner le problème de hauteur

0
Ian Bloomfield