Извлечение рыскания из кватерниона

У меня есть кватернион вращения и я хочу извлечь угол поворота вокруг оси вверх (рыскание). Я использую XNA, и, насколько я могу судить, для этого нет встроенной функции. Как лучше всего это сделать?

Спасибо за любую помощь, Венату

5 23
quaternions xna

5 ответов:

Кватернионное представление вращения-это изменение оси и угла. Поэтому, если вы вращаетесь на r Радиан вокруг оси x, y, z , то ваш кватернион q равен: 

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

Если вы хотите создать кватернион, который вращается только вокруг оси y , вы обнуляете оси x и z, а затем повторно нормализуете кватернион: 

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

Если вы хотите получить результирующий угол: 

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

Это предполагает, что кватернионное представление сохраняется: w, x, y, z. Если q[0] и q[2] равны нулю или близки к нему, то результирующий кватернион должен быть просто {1,0,0,0}.

26

Задав кватернион q, вы можете рассчитать крен, тангаж и рыскание следующим образом: 

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

Это должно соответствовать внутреннему вращению Тейт-Брайана порядка xyz. Для других порядков вращения, внешних и собственных-эйлеровых вращений должны использоваться другие преобразования.

21

Преобразование кватерниона в Эйлера

Я надеюсь, вы знаете, что рыскание, тангаж и крен не годятся для произвольных вращений. Эйлеровы углы страдают от особенностей (см. выше ссылку) и нестабильности. Посмотрите на 38: 25 из презентации Дэвида Сакса

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

Удачи!

4

Примечание : я проверил приведенный ниже код по уравнениямВикипедии плюсдокументацию Pixhawk , и это правильно.

Если вы работаете с дронами / авиацией, ниже приведен код (взятый непосредственно из DJI SDK). Здесь q0, q1, q2,q3 соответствуют W,x,y, z компонентам кватерниона соответственно. Также обратите внимание, что рыскание, тангаж, крен в некоторых литературных источниках могут называться соответственно курсом, положением и креном.

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

Если вам нужно вычислите все 3, тогда вы можете избежать пересчета общих терминов, используя следующие функции: 

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

Примечание о библиотеке Eigen

Если вы используете библиотеку Eigen, у нее есть другой способ сделать это преобразование, однако это может быть не так оптимизировано, как приведенный выше прямой код: 

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

Кватернион состоит из двух компонентов: трехмерной векторной компоненты и скалярной компоненты.

Векторная составляющая кватерниона описывает независимые вращения вокруг каждой оси, поэтому обнуление x-и y-компонент векторной компоненты и оставление z-компонента как есть-это все, что вам нужно сделать, чтобы решить для векторного члена: 
// Don't modify qz
double qx = 0;
double qy = 0;
Скалярный член представляет собой величину вращения. Для единичного кватерниона (такого, как тот, который используется для представления отношения), Весь кватернион должен иметь величину 1. Таким образом, скалярный член может быть решен следующим образом: 
double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

Поскольку qx и qy равны нулю, скалярная составляющая задается через 

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

Таким образом, полный кватернион, представляющий рыскание, задается 

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