Преобразование между кватернионами и углами Эйлера

Пространственные вращения в трех измерениях можно параметризовать, используя как углы Эйлера , так и единичные кватернионы . В этой статье объясняется, как преобразовать два представления. На самом деле это простое использование «кватернионов» было впервые предложено Эйлером примерно на семьдесят лет раньше Гамильтона для решения проблемы магических квадратов . По этой причине сообщество динамиков обычно называет кватернионы в этом приложении «параметрами Эйлера».

Определение

Есть два представления кватернионов. В этой статье используется более популярный Гамильтон.

Кватернион имеет 4 скалярных значения: q _w (действительная часть) и q _x q _y q _z (мнимая часть).

Определение нормы кватерниона следующим образом: $$\lVert q\rVert ={\sqrt {\,q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}~}}$$

Единичный кватернион удовлетворяет: $$\lVert q\rVert =1$$

Мы можем связать кватернион с вращением вокруг оси следующим выражением

$\mathbf {q} _{w}=\cos(\alpha /2)$

$\mathbf {q} _{x}=\sin(\alpha /2)\cos(\beta _{x})$

$\mathbf {q} _{y}=\sin(\alpha /2)\cos(\beta _{y})$

$\mathbf {q} _{z}=\sin(\alpha /2)\cos(\beta _{z})$

где α — простой угол поворота (значение угла поворота в радианах ), а cos(β _x ), cos(β _y ) и cos(β _z ) — « направляющие косинусы » углов между тремя координатными осями. и ось вращения. (Теорема Эйлера о вращении).

Интуиция

Чтобы лучше понять, как « направляющие косинусы » работают с кватернионами:

${\begin{array}{lcr}\mathbf {q} _{w}=\cos({\text{rotation angle}}/2)\\\mathbf {q} _{x}=\sin({\text{rotation angle}}/2)\cos({\text{angle between axis of rotation and x axis}})\\\mathbf {q} _{y}=\sin({\text{rotation angle}}/2)\cos({\text{angle between axis of rotation and y axis}})\\\mathbf {q} _{z}=\sin({\text{rotation angle}}/2)\cos({\text{angle between axis of rotation and z axis}})\end{array}}$

Если осью вращения является ось X :

${\begin{array}{lcr}\mathbf {q} _{w}=\cos(\alpha /2)\\\mathbf {q} _{x}=\sin(\alpha /2)\cdot 1\\\mathbf {q} _{y}=\sin(\alpha /2)\cdot 0\\\mathbf {q} _{z}=\sin(\alpha /2)\cdot 0\end{array}}$

Если осью вращения является ось Y :

${\begin{array}{lcr}\mathbf {q} _{w}=\cos(\alpha /2)\\\mathbf {q} _{x}=\sin(\alpha /2)\cdot 0\\\mathbf {q} _{y}=\sin(\alpha /2)\cdot 1\\\mathbf {q} _{z}=\sin(\alpha /2)\cdot 0\end{array}}$

Если осью вращения является ось z :

${\begin{array}{lcr}\mathbf {q} _{w}=\cos(\alpha /2)\\\mathbf {q} _{x}=\sin(\alpha /2)\cdot 0\\\mathbf {q} _{y}=\sin(\alpha /2)\cdot 0\\\mathbf {q} _{z}=\sin(\alpha /2)\cdot 1\end{array}}$

Если ось вращения представляет собой вектор, расположенный под углом 45° ( ⁠ π / 4 ⁠ радиан) между осями x и y :

${\begin{array}{lcr}\mathbf {q} _{w}=\cos(\alpha /2)\\\mathbf {q} _{x}=\sin(\alpha /2)\cdot 0.707\ldots \\\mathbf {q} _{y}=\sin(\alpha /2)\cdot 0.707\ldots \\\mathbf {q} _{z}=\sin(\alpha /2)\cdot 0\end{array}}$

Следовательно, оси x и y «разделяют» влияние на новую ось вращения .

Углы Тейта – Брайана

Углы Тейта–Брайана. последовательность zy'-x″ (внутренние вращения; N совпадает с y' ). Последовательность поворота угла: ψ , θ , φ . Обратите внимание, что в этом случае ψ > 90° и θ — отрицательный угол.

Аналогично для углов Эйлера мы используем углы Тейта Брайана (с точки зрения динамики полета ):

• Заголовок - $\psi$: вращение вокруг оси Z
• Подача - $\theta$: вращение вокруг новой оси Y
• Банк - $\phi$: вращение вокруг новой оси X

где ось X направлена ​​вперед, ось Y вправо, а ось Z вниз. В приведенном выше примере преобразования ротация происходит в порядке: заголовок, шаг, банк.

Матрицы вращения

Ортогональная матрица (после умножения вектора-столбца), соответствующая вращению по часовой стрелке/ влево (смотря вдоль положительной оси начала координат) на единичный кватернион. $q=q_{w}+iq_{x}+jq_{y}+kq_{z}$ задается неоднородным выражением :

$R={\begin{bmatrix}1-2(q_{y}^{2}+q_{z}^{2})&2(q_{x}q_{y}-q_{w}q_{z})&2(q_{w}q_{y}+q_{x}q_{z})\\2(q_{x}q_{y}+q_{w}q_{z})&1-2(q_{x}^{2}+q_{z}^{2})&2(q_{y}q_{z}-q_{w}q_{x})\\2(q_{x}q_{z}-q_{w}q_{y})&2(q_{w}q_{x}+q_{y}q_{z})&1-2(q_{x}^{2}+q_{y}^{2})\end{bmatrix}}$

или, что эквивалентно, однородным выражением:

$R={\begin{bmatrix}q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2}&2(q_{x}q_{y}-q_{w}q_{z})&2(q_{w}q_{y}+q_{x}q_{z})\\2(q_{x}q_{y}+q_{w}q_{z})&q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2}&2(q_{y}q_{z}-q_{w}q_{x})\\2(q_{x}q_{z}-q_{w}q_{y})&2(q_{w}q_{x}+q_{y}q_{z})&q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2}\end{bmatrix}}$

Если $q_{w}+iq_{x}+jq_{y}+kq_{z}$ не является единичным кватернионом, то однородная форма по-прежнему является скалярным кратным матрицы вращения, тогда как неоднородная форма, как правило, больше не является ортогональной матрицей. Вот почему в числовых работах следует отдавать предпочтение однородной форме, чтобы избежать искажений.

Матрица направляющего косинуса (от повернутых координат XYZ Тела до исходных координат XYZ Lab для вращения по часовой стрелке/влево), соответствующая последовательности Тела 3-2-1 после умножения с углами Эйлера (ψ, θ, φ), определяется выражением :

${\begin{aligned}{\begin{bmatrix}x\\y\\z\\\end{bmatrix}}&=R_{z}(\psi )R_{y}(\theta )R_{x}(\phi ){\begin{bmatrix}X\\Y\\Z\\\end{bmatrix}}\\&={\begin{bmatrix}\cos \psi &-\sin \psi &0\\\sin \psi &\cos \psi &0\\0&0&1\\\end{bmatrix}}{\begin{bmatrix}\cos \theta &0&\sin \theta \\0&1&0\\-\sin \theta &0&\cos \theta \\\end{bmatrix}}{\begin{bmatrix}1&0&0\\0&\cos \phi &-\sin \phi \\0&\sin \phi &\cos \phi \\\end{bmatrix}}{\begin{bmatrix}X\\Y\\Z\\\end{bmatrix}}\\&={\begin{bmatrix}\cos \theta \cos \psi &-\cos \phi \sin \psi +\sin \phi \sin \theta \cos \psi &\sin \phi \sin \psi +\cos \phi \sin \theta \cos \psi \\\cos \theta \sin \psi &\cos \phi \cos \psi +\sin \phi \sin \theta \sin \psi &-\sin \phi \cos \psi +\cos \phi \sin \theta \sin \psi \\-\sin \theta &\sin \phi \cos \theta &\cos \phi \cos \theta \\\end{bmatrix}}{\begin{bmatrix}X\\Y\\Z\\\end{bmatrix}}\\\end{aligned}}$

Углы Эйлера для последовательности Тела 3-1-3. Система xyz (исходная фиксированная лаборатория) показана синим цветом, система XYZ (повернутое окончательное тело) показана красным. Линия узлов, обозначенная N и показанная зеленым цветом, представляет собой промежуточную ось X Тела, вокруг которой происходит второе вращение.

Углы Эйлера (в последовательности 3-2-1) в преобразование кватернионов

Комбинируя кватернионные представления вращений Эйлера, мы получаем последовательность Тела 3-2-1 , в которой самолет сначала выполняет поворот по рысканью (Body-Z) во время выруливания на взлетно-посадочную полосу, а затем наклоняет (Тело-Y) во время взлета. , и, наконец, перекатывается (Тело-X) в воздухе. Результирующая ориентация последовательности Тела 3-2-1 (вокруг заглавной оси на иллюстрации углов Тейта-Брайана) эквивалентна ориентации лабораторной последовательности 1-2-3 (вокруг строчной оси), где самолет сначала перевернулся (ось lab-x), затем повернулся вокруг горизонтальной оси lab-y и, наконец, вращался вокруг вертикальной оси lab-z ( lB = lab2Body ):

${\begin{aligned}\mathbf {q_{lB}} &={\begin{bmatrix}\cos(\psi /2)\\0\\0\\\sin(\psi /2)\\\end{bmatrix}}{\begin{bmatrix}\cos(\theta /2)\\0\\\sin(\theta /2)\\0\\\end{bmatrix}}{\begin{bmatrix}\cos(\phi /2)\\\sin(\phi /2)\\0\\0\\\end{bmatrix}}\\&={\begin{bmatrix}\cos(\phi /2)\cos(\theta /2)\cos(\psi /2)+\sin(\phi /2)\sin(\theta /2)\sin(\psi /2)\\\sin(\phi /2)\cos(\theta /2)\cos(\psi /2)-\cos(\phi /2)\sin(\theta /2)\sin(\psi /2)\\\cos(\phi /2)\sin(\theta /2)\cos(\psi /2)+\sin(\phi /2)\cos(\theta /2)\sin(\psi /2)\\\cos(\phi /2)\cos(\theta /2)\sin(\psi /2)-\sin(\phi /2)\sin(\theta /2)\cos(\psi /2)\\\end{bmatrix}}\\\end{aligned}}$

Другие последовательности вращения используют другие соглашения.

Исходный код

Ниже код на C++ иллюстрирует приведенное выше преобразование:

struct Quaternion
{
    double w, x, y, z;
};

// This is not in game format, it is in mathematical format.
Quaternion ToQuaternion(double roll, double pitch, double yaw) // roll (x), pitch (y), yaw (z), angles are in radians
{
    // Abbreviations for the various angular functions

    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;

    return q;
}

Преобразование кватернионов в углы Эйлера (в последовательности 3-2-1)

Существует прямая формула для преобразования кватерниона в углы Эйлера в любой из 12 возможных последовательностей. формула для последовательности Тело 3-2-1 В оставшейся части этого раздела будет показана . Если кватернион правильно нормализован , углы Эйлера можно получить из кватернионов посредством соотношений:

${\begin{bmatrix}\phi \\\theta \\\psi \end{bmatrix}}={\begin{bmatrix}{\mbox{atan2}}\left(2(q_{w}q_{x}+q_{y}q_{z}),1-2(q_{x}^{2}+q_{y}^{2})\right)\\-\pi /2+2\,{\mbox{atan2}}\left({\sqrt {1+2(q_{w}q_{y}-q_{x}q_{z})}},{\sqrt {1-2(q_{w}q_{y}-q_{x}q_{z})}}\right)\\{\mbox{atan2}}\left(2(q_{w}q_{z}+q_{x}q_{y}),1-2(q_{y}^{2}+q_{z}^{2})\right)\end{bmatrix}}$

Обратите внимание, что функции arctan , реализованные в компьютерных языках, дают результаты только между -π/2 и π/2 , поэтому atan2 используется для генерации всех правильных ориентаций. Более того, типичные реализации arctan также могут иметь некоторые численные недостатки вблизи нуля и единицы.

В некоторых реализациях используется эквивалентное выражение:

$\theta ={\mbox{arcsin}}(2(q_{w}q_{y}-q_{x}q_{z}))$

Исходный код

Следующая программа C++ иллюстрирует приведенное выше преобразование:

#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

// this implementation assumes normalized quaternion
// converts to Euler angles in 3-2-1 sequence
EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = std::sqrt(1 + 2 * (q.w * q.y - q.x * q.z));
    double cosp = std::sqrt(1 - 2 * (q.w * q.y - q.x * q.z));
    angles.pitch = 2 * std::atan2(sinp, cosp) - M_PI / 2;

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

Особенности

Необходимо учитывать особенности параметризации угла Эйлера, когда угол наклона приближается к ±90° (северный/южный полюс). Эти случаи должны рассматриваться особым образом. Общее название этой ситуации — блокировка подвеса .

Код для обработки особенностей взят на этом сайте: www.euclideanspace.com .

Векторное вращение

Определим скаляр $q_{w}$ и вектор ${\vec {q}}$ такой, что кватернион $\mathbf {q} =(q_{w},{\vec {q}})$.

Обратите внимание, что канонический способ вращения трехмерного вектора ${\vec {v}}$ по кватерниону $q$ определение вращения Эйлера осуществляется по формуле

$\mathbf {v} ^{\,\prime }=\mathbf {qvq} ^{\ast }$

где $\mathbf {v} =(0,{\vec {v}})$ представляет собой кватернион, содержащий встроенный вектор ${\vec {v}}$, $\mathbf {q} ^{\ast }=(q_{w},-{\vec {q}})$ является сопряженным кватернионом , и $\mathbf {v} ^{\,\prime }=(0,{\vec {v}}^{\,\prime })$ повернутый вектор ${\vec {v}}^{\,\prime }$. В вычислительных реализациях для этого требуются два умножения кватернионов. Альтернативный подход заключается в применении пары отношений

${\vec {t}}=2{\vec {q}}\times {\vec {v}}$

${\vec {v}}^{\,\prime }={\vec {v}}+q_{w}{\vec {t}}+{\vec {q}}\times {\vec {t}}$

где $\times$ указывает на трехмерное векторное произведение. Это требует меньшего количества умножений и, следовательно, быстрее в вычислительном отношении. Численные тесты показывают, что последний подход может достигать 30%. быстрее, чем оригинал для вращения вектора.

Доказательство

Общее правило умножения кватернионов, включающее скалярные и векторные части, определяется выражением

${\begin{aligned}\mathbf {pq} &=(p_{w},{\vec {p}})(q_{w},{\vec {q}})\\&=(p_{w}q_{w}-{\vec {p}}\cdot {\vec {q}},p_{w}{\vec {q}}+q_{w}{\vec {p}}+{\vec {p}}\times {\vec {q}})\\\end{aligned}}$

Используя это соотношение, находим для $\mathbf {v} =(0,{\vec {v}})$ что

${\begin{aligned}\mathbf {vq^{\ast }} &=(0,{\vec {v}})(q_{w},-{\vec {q}})\\&=({\vec {v}}\cdot {\vec {q}},q_{w}{\vec {v}}-{\vec {v}}\times {\vec {q}})\\\end{aligned}}$

и при замене тройного произведения

${\begin{aligned}\mathbf {qvq^{\ast }} &=(q_{w},{\vec {q}})({\vec {v}}\cdot {\vec {q}},q_{w}{\vec {v}}-{\vec {v}}\times {\vec {q}})\\&=(0,q_{w}^{2}{\vec {v}}+q_{w}{\vec {q}}\times {\vec {v}}+({\vec {v}}\cdot {\vec {q}}){\vec {q}}+q_{w}{\vec {q}}\times {\vec {v}}+{\vec {q}}\times ({\vec {q}}\times {\vec {v}}))\\\end{aligned}}$

где антикоммутативность векторного произведения и ${\vec {q}}\cdot {\vec {v}}\times {\vec {q}}=0$ был применен. Следующим эксплуатированием собственности, которая $\mathbf {q}$ является единичным кватернионом, так что $q_{w}^{2}=1-{\vec {q}}\cdot {\vec {q}}$, наряду со стандартным векторным тождеством

${\vec {q}}\times ({\vec {q}}\times {\vec {v}})=({\vec {q}}\cdot {\vec {v}}){\vec {q}}-({\vec {q}}\cdot {\vec {q}}){\vec {v}}$

получается

${\begin{aligned}\mathbf {v} ^{\prime }&=\mathbf {qvq^{\ast }} =(0,{\vec {v}}+2q_{w}{\vec {q}}\times {\vec {v}}+2{\vec {q}}\times ({\vec {q}}\times {\vec {v}}))\\\end{aligned}}$

который после определения ${\vec {t}}=2{\vec {q}}\times {\vec {v}}$ можно записать через скалярную и векторную части как

$(0,{\vec {v}}^{\,\prime })=(0,{\vec {v}}+q_{w}{\vec {t}}+{\vec {q}}\times {\vec {t}}).$

См. также

• Оператор вращения (векторное пространство)
• Кватернионы и пространственное вращение
• Углы Эйлера
• Матрица вращения
• Формализмы вращения в трех измерениях

Внешние ссылки

• Вопрос 60. Как преобразовать углы вращения Эйлера в кватернион? и связанные вопросы в FAQ по матрицам и кватернионам