Extraction de lacet d'un Quaternion

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

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 correspondre à la rotation intrinsèque tait-bryan de l'ordre xyz. Pour les autres ordres de rotation, les rotations extrinsèques et proprement dites d'autres conversions doivent être utilisées.

Conversion Quaternion en Euler

J'espère que vous savez que le lacet, le tangage et le roulis ne sont pas bons pour les 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!

Note : j'ai vérifié ci-dessous le code par rapport aux équations de Wikipedia plus la documentation de Pixhawk et c'est correct.

Si vous travaillez avec des drones / aviation, voici le code (pris directement à partir 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 respectivement Cap, attitude et inclinaison dans certains ouvrages.

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 avez besoin de calculez tous les 3 alors vous pouvez éviter de recalculer les termes communs 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 propre

Si vous utilisez la bibliothèque Eigen, elle a une autre façon de faire cette conversion, cependant, cela peut ne pas être aussi optimisé que ci-dessus Code direct:

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

Un quaternion est constitué de deux composantes: une composante vectorielle 3d et une composante scalaire.

La composante vectorielle du quaternion décrit des rotations indépendantes autour de chaque axe, de sorte que zéro les composantes x et y de la composante vectorielle et laissant la composante z telle quelle est tout ce que vous devez faire pour résoudre le terme vectoriel:

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

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

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

Puisque qx et qy sont nuls, le composant scalaire est donné 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);