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

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

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

Источник

6 ответов

Решение

Кватернионное представление вращения - это изменение оси и угла. Так что если вы вращаете на 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}.

Источник

Задав кватернион 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-порядка. Для других порядков вращения, внешних и собственных вращений должны использоваться другие преобразования.

Источник

Примечание. Я проверил приведенный ниже код на соответствие уравнениям Википедии и документации 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, у нее есть другой способ сделать это преобразование, однако, это может быть не так оптимизировано, как приведенный выше прямой код:

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];
Источник

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

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

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

Удачи!

Источник

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

Компонент вектора кватерниона описывает независимые повороты вокруг каждой оси, поэтому обнуление 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);
Источник

Преобразование кватерниона в рыскание, тангаж и крен зависит от соглашений, используемых для определения кватерниона и рысканья, тангажа и крена. Для данного соглашения существует много «почти правильных» преобразований, которые будут работать для большинства углов, но только одно действительно правильное преобразование, которое будет работать для всех углов, включая южный и северный полюса, где «почти правильные» преобразования приводят к блокировкам карданного подвеса (ложным флипам). и вращения).

См. этот учебник для получения дополнительной информации:

https://youtu.be/k5i-vE5rZR0

Источник
Другие вопросы по тегам