math - Extrayendo Guiñada de un Quaternion

xna quaternions

Question

Tengo un quaternion de rotación y quiero extraer el ángulo de rotación sobre el eje Up (la guiñada). Estoy usando XNA y, por lo que puedo ver, no hay una función incorporada para esto. ¿Cuál es la mejor manera de hacer esto?

Gracias por cualquier ayuda, Venatu

Answer 1

Habiendo dado un Quaternion q, puedes calcular el balanceo, cabeceo y guiñada de esta manera:

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);

Esto debería ajustarse a la rotación intiática de tait-bryan de orden xyz. Para otras órdenes de rotación, rotaciones extrínsecas y debidas-euler deben usarse otras conversiones.

Answer 2

La representación del cuaternión de la rotación es una variación del eje y el ángulo. Entonces, si rotan r radianes alrededor del eje x , y , z , entonces su cuaternión q es:

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

Si desea crear un cuaternión que solo gire alrededor del eje y , ponga a cero los ejes x y z y luego vuelva a normalizar el cuaternión:

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

Si quieres el ángulo resultante:

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

Esto supone que la representación del cuaternión se almacena: w, x, y, z. Si ambos q [0] y q [2] son ​​cero, o cerca de él, el cuaternión resultante debería ser {1,0,0,0}.

Answer 3

Conversión de Quaternion a Euler

Espero que sepas que yaw, pitch and roll no son buenos para las rotaciones arbitrarias. Los ángulos de Euler sufren de singularidades (ver el enlace de arriba) e inestabilidad. Mire 38:25 de la presentación de David Sachs

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

¡Buena suerte!

Answer 4

Nota : He verificado el código a continuación contra las ecuaciones de Wikipedia más la documentación de Pixhawk y es correcto.

Si está trabajando con drones / aviación, a continuación se muestra el código (tomado directamente de DJI SDK ). Aquí q0, q1, q2, q3 corresponde a las componentes w, x, y, z del cuaternión, respectivamente. También tenga en cuenta que yaw, pitch, roll puede referirse como título, actitud y banco, respectivamente, en algunos estudios.

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 necesita calcular los 3, puede evitar volver a calcular términos comunes mediante las siguientes funciones:

//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;
}

Nota sobre la biblioteca Eigen

Si está utilizando la biblioteca Eigen, tiene otra forma de hacer esta conversión, sin embargo, esto puede no ser tan optimizado como el código directo anterior:

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