web-dev-qa-db-fra.com

Extraire le lacet d'un quaternion

J'ai un quaternion de rotation et je souhaite extraire l'angle de rotation autour de l'axe haut (le lacet). J'utilise XNA et, autant que je sache, il n'y a pas de fonction intégrée pour cela. Quelle est la meilleure façon de procéder?

Merci pour toute aide, Venatu

mathxnaquaternions
25
Venatu

La représentation quaternion de la rotation est une variation d’axe et d’angle. Donc, si vous faites pivoter de r radians autour de l’axe x , y , z , alors votre quaternion q est:

q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;

Si vous souhaitez créer un quaternion ne faisant que pivoter autour de l'axe y , vous devez mettre à zéro les axes x et z , puis vous normaliser de nouveau:

q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;

Si vous voulez l'angle résultant:

double ang = 2*acos(q[0]);

Cela suppose que la représentation quaternion est stockée: w, x, y, z. Si q [0] et q [2] sont tous deux égaux à zéro ou à une valeur proche de celle-ci, le quaternion résultant doit simplement être {1,0,0,0}.

28
JCooper

Après avoir donné un Quaternion q, vous pouvez calculer le roulis, le tangage et le lacet comme suit:

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

Cela devrait convenir à la rotation intrinsèque tait-bryan de l'ordre xyz. Pour les autres ordres de rotation, les rotations extrinsèque et appropriée, il faut utiliser d'autres conversions.

22
thewhiteambit

Conversion de Quaternion à Euler

J'espère que vous savez que le tangage, le tangage et le roulis ne conviennent pas aux rotations arbitraires. Les angles d'Euler souffrent de singularités (voir le lien ci-dessus) et d'instabilité. Regardez 38:25 de la présentation de David Sachs

http://www.youtube.com/watch?v=C7JQ7Rpwn2k

Bonne chance!

4
Ali

Note: J'ai vérifié le code ci-dessous avec les équations de Wikipedia plus la documentation de Pixhawk et elle est correcte.

Si vous travaillez avec des drones/aviation, voici le code (extrait directement de DJI SDK ). Ici, q0, q1, q2, q3 correspond respectivement aux composantes w, x, y, z du quaternion. Notez également que le lacet, le tangage, le roulis peuvent être appelés cap, attitude et inclinaison dans certaines publications.

float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

Si vous devez calculer tous les 3, vous pouvez éviter de recalculer les termes courants en utilisant les fonctions suivantes:

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
    EulerianAngle ans;

    double q2sqr = data.q2 * data.q2;
    double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
    double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
    double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
    double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
    double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;

    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;

    ans.pitch = asin(t2);
    ans.roll = atan2(t3, t4);
    ans.yaw = atan2(t1, t0);

    return ans;
}

QuaternionData Flight::toQuaternion(EulerianAngle data)
{
    QuaternionData ans;
    double t0 = cos(data.yaw * 0.5);
    double t1 = sin(data.yaw * 0.5);
    double t2 = cos(data.roll * 0.5);
    double t3 = sin(data.roll * 0.5);
    double t4 = cos(data.pitch * 0.5);
    double t5 = sin(data.pitch * 0.5);

    ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
    ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
    ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
    ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
    return ans;
}

Note sur la bibliothèque Eigen

Si vous utilisez la bibliothèque Eigen, il existe un autre moyen d'effectuer cette conversion. Toutefois, cette optimisation pourrait ne pas être aussi optimisée que le code direct ci-dessus:

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];
4
Shital Shah

Un quaternion est constitué de deux composants: un composant vectoriel 3D et un composant scalaire. 

La composante vectorielle du quaternion décrit des rotations indépendantes autour de chaque axe. Il vous suffit donc de mettre à zéro les composantes x et y de la composante vectorielle et de laisser la composante z telle quelle, afin de résoudre le vecteur. terme:

// Don't modify qz
double qx = 0;
double qy = 0;

Le terme scalaire représente la magnitude de la rotation. Pour un quaternion unitaire (tel que celui utilisé pour représenter l'attitude), le quaternion entier doit avoir une magnitude de 1. Ainsi, le terme scalaire peut être résolu en:

double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

Puisque qx et qy sont nuls, la composante scalaire est donnée par

double qw = sqrt(1 - qz*qz);

Ainsi, le quaternion complet représentant le lacet est donné par

double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);
0
Eric Cox