Матрицы поворота, углы Эйлера и кватернионы (Rotation matrices, Euler angles and quaternions)

Автор: | 18.12.2018

Матрицы преобразований
Матрицы поворота и углы Эйлера
Axis Angle представление вращения
Кватернионы
Основные операции над кватернионами
Умножение кватернионов
Конвертирование между кватернионом и Axis Angle представлением
Конвертирование кватерниона в матрицу поворота
Конвертирование матрицы поворота в кватернион
Полезные ссылки

Матрицы преобразований

Объект обычно определяется в удобной для его описания локальной системы координат (ЛСК), а его положение  в пространстве определяется в глобальной системе координат (ГСК).

В трёхмерном пространстве переход из одной СК в другую описывается в общем случае системой линейных уравнений:

Уравнения могут быть записаны через матрицы геометрических преобразований в однородных координатах одним из  2-х способов:

В ортогональных СК оси XY и Z  взаимно перпендикулярны и расположены по  правилу правой руки:

На рисунке справа большой палец определяет направление оси, остальные пальцы — положительное направление вращения относительно этой оси.

Все три вектора направлений являются единичными.

Ниже приводится единичная матрица для 2-х способов записи уравнений геометрических преобразований.  Такая матрица не описывает ни перемещения, ни вращения. Оси  ЛСК и  ГСК совпадают.

 Далее рассматривается матрица для 2 способа матричной записи уравнений (матрица справа). Этот способ встречается в статьях значительно чаще

При использовании матрицы вы можете игнорировать нижнюю строку. В ней всегда хранятся одни и те же значения 0, 0, 0, 1. Она добавлена для того,чтобы мы могли перемножать матрицы (напомню, что можно перемножать только квадратные матрицы).

Остальные 12 значений определяют координатную систему. Первый столбец описывает компоненты направления оси X(1,0,0). Второй столбец задает направление оси Y(0,1,0), третий – оси Z (0,0,1). Последний столбец определяет положение начала системы координат (0,0,0).

Как будет выглядеть матрица для задания ЛСК, с началом в точке (10,5,0) и повёрнутой на 45° вокруг оси Z глобальной СК, показано на рисунке.

Рассмотрим сначала ось X. Если новая система координат повернута на 45° вокруг оси z, значит и ось x повернута относительно базовой оси X на 45° в положительном направлении отсчета углов. Таким образом, ось X направлена вдоль вектора (1, 1, 0), но поскольку вектор системы координат должен быть единичным, то результат должен выглядеть так (0.707, 0.707, 0). Соответственно, ось Y имеет отрицательную компоненту по X и положительную по Y и будет выглядеть следующим образом (-0.707, 0.707, 0). Ось Z направления не меняет (0, 0, 1). Наконец, в четвертом столбце вписываются координаты точки начала системы координат (10, 5, 0).

Частным случаем матриц геометрических преобразований есть матрицы поворота ЛСК относительно базовых осей ГСК. Вектора осей ЛСК здесь выражены через синусы и косинусы углов вращения относительно оси, перпендикулярной к плоскости вращения.

От матрицы преобразований размером 4*4 можно перейти непосредственно к матрице поворота 3*3 убрав нижний ряд и правый столбик. При этом система линейных уравнений записывается без свободных элементов (лямда, мю, ню), которые определяют перемещение вдоль осей координат.

Путем перемножения базовых матриц можно получать комбинированные вращения. Ниже рассмотрены возможности комбинировать вращениями через матрицы поворота на примерах работы с углами Эйлера.

Матрицы поворота и  углы Эйлера 

Углы Эйлера – углы, описывающие поворот абсолютно твердого тела в трёхмерном евклидовом пространстве.

  

От выбора осей и последовательности вращения зависит конечный результат. На рисунках отображена следующая последовательность вращения относительно осей ЛСК:

  • оси  Z  (угол alpha);
  • оси X  (угол beta);
  • оси Z  (угол gamma).

Можно получить результирующую матрицу, которая определяет положение ГСК относительно ЛСК. Для этого необходимо перемножить матрицы с отрицательными углами в последовательности выполнения поворотов:

Почему знак угла поворота меняется на противоположный?  Объяснение этому простое.  Движение относительно. Абстрагируемся и представим, что ГСК меняет положение относительно неподвижной ЛСК.  При этом направление вращения меняется на противоположное.

Перемножение матриц даст следующий результат:

Результирующую матрицу можно использовать для пересчета координат из ГСК в ЛСК:

Для пересчета координат из ЛСК в ГСК используется результирующая обратная матрица.

В обратной матрице  последовательность поворота и знаки углов меняются на противоположные (в рассматриваемом примере снова на положительные) по сравнению с матрицей определения положения ГСК относительно ЛСК.

Перемножение матриц даст следующий результат:

Выше был рассмотрен случай определения углов Эйлера через вращение относительно осей ЛСК. То же взаимное положение СК можно получить, выполняя вращение  относительно осей ГСК:

  • оси z (угол (gamma+pi/2));
  • оси y (угол угол beta);
  • оси z (угол (-alpha)).

Последовательность преобразований берется не откуда-то свыше, а определяется чисто на здравом смысле, используя пространственное воображение. При выборе последовательности руководствуйтесь простым правилом — преобразования не должны искажать натуральную величину параметров (углов).

Умение правильно выбирать последовательность элементарных геометрических преобразований помогает в решении множества, задач (см. Примеры геометрических преобразований).

Определение углов Эйлера через вращение относительно осей ГСК  позволяет также просто получить зависимости для пересчета координат из ЛСК в ГСК через перемножение матриц поворота.

В рамках рассматриваемой задачи вместо угла gamma в матрицe Az используем угол gamma+pi/2.

Также легко можно перейти к зависимостям для пересчета координат из ГСК в ЛСК.

Обратная матрица получается перемножением обратных матриц в обратном порядке по сравнению с прямым преобразованием. При этом каждая из обратных матриц вращения может быть получена заменой знака угла на противоположный.

Детально с  теоретическими основами аффинных преобразований (включая и вращение) можно ознакомиться в статье Геометрические преобразования в графических приложениях

Примеры преобразований рассмотрены в статьях:

Axis Angle представление вращения

Выбрав подходящую ось (англ. rotation axis) и угол (англ. rotation angle) можно задать любую ориентацию объекта.

Обычно хранят ось вращения в виде единичного вектора и угол поворота вокруг этой оси в радианах или градусах.

q = [ x, y, z, w ] = [ v, w ]

В некоторых случаях удобно хранить угол вращения и ось в одном векторе. Направление вектора при этом совпадает с направлением оси вращения, а его длина равна углу поворота:

q = [ x, y, z];     w=sqrt (x*x +y*y +z*z)

В физике, таким образом хранят угловую скорость.  Направление вектора совпадает с направлением оси вращения, а длина вектора равна скорости (в радианах в секунду).

Можно описать рассмотренные выше углы Эйлера через Axis Angle представление в 3 этапа:

q1 = [ 0, 0, 1, alpha];     q2 = [ 1, 0, 0, beta];  q3 = [ 0, 0, 1, gamma ]

Здесь каждое вращение выполняется относительно осей текущего положения ЛСК. Такое преобразование равнозначно рассмотренному выше преобразованию через матрицы поворота:

Возникает вопрос, а можно ли 3 этапа Axis Angle представления объединить в одно, подобно матрицам поворота? Попробуем решить геометрическую задачу по определению координат последнего вектора вращения в последовательности преобразований через Axis Angle представления:

q = [ x, y, z, gamma ]

Есть ли представление q= [x, y, z, gamma] композицией последовательности из 3-х этапов преобразований? Нет! Координаты x, y, z определяют всего лишь положение оси Z  ЛСК после первого и второго этапов преобразований:

При этом ось Z, отнюдь, не есть вектор вращения для Axis Angle представления, которое могло бы заменить рассмотренные 3-х этапа преобразований.

К сожалению, никакие операции  (типа объединения нескольких преобразований в одно) с Axis Angle представлениями нельзя выполнить.  Не будем расстраиваться. Это можно сделать  через кватернионы, которые также определяют вращение через через параметры оси и угол.

Кватернионы

Кватернион (как это и видно по названию) представляет собой набор из четырёх параметров, которые определяют вектор и угол вращения вокруг этого вектора. По сути такое определение ничем не отличается от Axis Angle представления вращения. Отличия лишь в способе представления. Как же хранят вращение в кватернионе?

q = [ V*sin(alpha/2), cos(alpha/2) ]

В кватернионе параметры единичного вектора умножается на синус половины угла поворота. Четвертый компонент — косинус половины угла поворота.

// Угол вращения RotationAngle задан в радианах
x = RotationAxis.x * sin(RotationAngle / 2)
y = RotationAxis.y * sin(RotationAngle / 2)
z = RotationAxis.z * sin(RotationAngle / 2)
w = cos(RotationAngle / 2)

Таблица с примерами значений кватернионов:

Представление вращения кватернионом кажется необычным по сравнению с Axis Angle представлением. Почему параметры вектора умножаются на синус половины угла вращения, четвертый параметр — косинус половины угла вращения, а не просто угол?

Откуда получено такое необычное представление кватерниона детально можно ознакомиться в статье Доступно о кватернионах и их преимуществах  Хотя программисту не обязательно знать эти детали, точно также как и знать, каким образом получены матрицы преобразования пространства. Достаточно лишь знать основные операции с кватернионами, их смысл и правила применения.

Основные операции над кватернионами

Кватернион удобно рассматривать как 4d вектор, и некоторые операции с ним выполняются как над векторами.

Сложение, вычитание и умножение на скаляр.

 q + q' = [ x + x', y + y', z + z', w + w' ]
 q – q' = [ x - x', y - y', z - z', w - w' ]
 qs = [ xs, ys, zs, ws ]

Смысл операции сложения можно описать как «смесь» вращений, т.е. мы получим вращение, которое находится между q и  q’.

Что-то подобное сложению кватернионов выполнялось при неудачной попытке объединить 3 этапа Axis Angle представления.

Умножение на скаляр на вращении не отражается. Кватернион, умноженный на скаляр, представляет то же самое вращение, кроме случая умножения на 0. При умножении на 0 мы получим «неопределенное» вращение.

Пример сложения 2-х  кватернионов:

// Axis Angle представление
q1 = [0, 0, 1, alpha];     
q2 = [1, 0, 0, beta];
// Кватернионы   
q1 = [0, 0, sin(alpha/2), cos(alpha/2)];
q2 = [sin(beta/2), 0, 0, cos(beta/2)];
// Сумма  кватернионов  
q1 + q2 = [sin(beta/2), 0, sin(alpha/2), cos(alpha/2) + cos(beta/2)]

Норма и модуль

Следует различать (а путают их часто) эти две операции:

Норма (norm)

N(q) = xx + yy + zz + ww

Модуль (magnitude), или как иногда говорят «длина» кватерниона:

|q| = sqrt( N(q) ) = sqrt( xx + yy + zz + ww )

Через модуль кватернион можно нормализовать. Нормализация кватерниона  — это  приведение к длине = 1 (так же как и в векторах):

Public Function normal(v As TVector) As TVector
 Dim length As Double
 length = (v.x ^ 2 + v.y ^ 2 + v.z ^ 2) ^ 0.5 
normal.x = v.x / length 
normal.y = v.y / length 
normal.z = v.z / length
End Function

Обратный кватернион или сопряжение ( conjugate )

Обратный кватернион задает вращение, обратное данному. Чтобы получить обратный кватернион  достаточно развернуть вектор оси в другую сторону и при необходимости нормализовать кватернион.

conjugate(q) = [-v, w]

Например, если разворот вокруг оси Y на 90 градусов = (w=0,707; x = 0; y = 0,707; z=0), то обратный = (w=0,707; x = 0; y = -0,707; z=0).

Казалось бы, можно инвертировать только компоненту W, но при поворотах на 180 кватернион представляется как (w=1;  x = 0; y = 0; z=0), то есть, у него длина вектора оси = 0.

Фрагмент программной реализации:

Public Function quat_scale(q As TQuat, val As Double) As TQuat
q.w = q.w * val ' x
 q.x = q.x * val ' x
 q.y = q.y * val ' y
 q.z = q.z * val ' z
 quat_scale = q
End Function

Public Function quat_length(q As TQuat) As Double
quat_length = (q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z) ^ 0.5
End Function

Public Function quat_normalize(q As TQuat) As TQuat
Dim n As Double
 n = quat_length(q)
 quat_normalize = quat_scale(q, 1 / n)
End Function

Public Function quat_invert(q As TQuat) As TQuat
 Dim res As TQuat
 res.w = q.w
 res.x = -q.x
 res.y = -q.y
 res.z = -q.z
 quat_invert = quat_normalize(res)
End Function

Инверсный (inverse) кватернион

Существует такой кватернион, при умножении на который произведение дает нулевое вращение и соответствующее тождественному кватерниону (identity quaternion), и определяется как:

 q–1 = conjugate(q)/N(q)

Тождественный кватернион

Записывается как q[0, 0, 0, 1]. Он описывает нулевой поворот (по аналогии с единичной матрицей), и не изменяет другой кватернион при умножении.

Скалярное произведение

q•q' = x*x' + y*y' + z*z' + w*w'

Скалярное произведение полезно тем, что дает косинус половины угла между двумя кватернионами умноженную на их длину. Соответственно скалярное произведение двух единичных кватернионов даст косинус половины угла между двумя ориентациями. Угол между кватернионами это угол поворота из q  в  q’ (по кратчайшей дуге).

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

Вращение 3d вектора v кватернионом q определяется как

V' = qvq–1

причем вектор конвертируется в кватернион как

q = [x, y, z, 0]

и кватернион обратно в вектор как

v = [x, y, z]

Умножение кватернионов

Одна из самых полезных операций, она аналогична умножению двух матриц поворота. Итоговый кватернион представляет собой комбинацию вращений — сначала объект повернули на q, а затем на q’ (если смотреть из глобальной системы координат).

qq' = [ vv' + wv' + w'v, ww' – v•v' ]

где vv’ — векторное произведение, v•v’ — скалярное произведение векторов.

Примеры векторного и скалярного перемножения 2-х векторов
     
векторное произведение — вектор:
Скалярное произведение — число:{\displaystyle \mathbf {a} \cdot \mathbf {b} =\sum _{i=1}^{n}a_{i}b_{i}=a_{1}b_{1}+a_{2}b_{2}+\cdots +a_{n}b_{n}}

Пример умножения 2-х  кватернионов:

// Кватернионы   
q1 = [ 0, 0, sin(alpha/2), cos(alpha/2)];  // q1 = [x1,y1,z1,w1] или q1 = [v1,w1]
q2 = [ sin(beta/2), 0, 0, cos(beta/2)];    // q2 = [x2,y2,z2,w2] или q2 = [v2,w2]

v1v2  = [0, sin(alpha/2) sin(beta/2), 0];   // векторное произведение
w1v2  = [cos(alpha/2) sin(beta/2), 0, 0];   // умножение вектора на скаляр
w2v1  = [0, 0, cos(beta/2) sin(alpha/2)];   // умножение вектора на скаляр
w1w2  = cos(alpha/2) cos(beta/2);           // перемножение скалярных величин
v1.v2 = 0;   // скалярное произведение векторов

// Произведение кватернионов q1 на q2
q1q2 = [v1v2 + w1v2 + w2v1, w1w2 – v1.v2] =
   = [cos(alpha/2) sin(beta/2), sin(alpha/2) sin(beta/2), cos (beta/2) sin(alpha/2),
                                                          cos(alpha/2) cos(beta/2)];

Фрагмент кода:

Public Function quat_mul_quat(a As TQuat, b As TQuat) As TQuat
Dim res As TQuat
 res.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
 res.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y
 res.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x
 res.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w
 quat_mul_quat = res
End Function

Конвертирование между кватернионом и Axis Angle представлением

Алгоритм:

В разделе Axis Angle представление вращения была сделана неудачная попытка объединить 3  Axis Angle представления  в одно . Это можно сделать опосредовано. Сначала Axis Angle представления конвертируются  в кватернионы, затем кватернионы перемножаются и результат конвертируется в Axis Angle представление.

Пример конвертирования произведения 2-х кватернионов в Axis Angle представление:

// Произведение кватернионов q1 на q2 равно  q1q2 = [x,y,z,w]
q1q2 = [cos(alpha/2) sin(beta/2),     // x
        sin(alpha/2) sin(beta/2),     // y
        cos (beta/2) sin(alpha/2),    // z
        cos(alpha/2) cos(beta/2)]     // w
// Axis Angle представление угла
         scale = xx + yy + zz,
         ax = x/scale,
         ay = y/scale,
         az = z/scale,
         angle = 2 acos(cos(alpha/2) cos(beta/2))

Фрагмент программы на C:

/**
 create a unit quaternion rotating by axis angle representation
 */
 inline void unit_from_axis_angle(const vector3 &axis, const float &angle)
 {
 vector3 v(axis);
 v.norm();
 float half_angle = angle*0.5f;
 float sin_a = (float)sin(half_angle);
 set(v.x*sin_a, v.y*sin_a, v.z*sin_a, (float)cos(half_angle));
 };
 //-----------------------------------
 /**
 convert a quaternion to axis angle representation, 
 preserve the axis direction and angle from -PI to +PI
 */
 inline void to_axis_angle(const vector3 &axis, float &angle) 
 {
 float vl = (float)sqrt( x*x + y*y + z*z );
 if( vl > TINY )
 {
 float ivl = 1.0f/vl;
 axis.set( x*ivl, y*ivl, z*ivl );
 if( w < 0 )
 angle = 2.0f*(float)atan2(-vl, -w); //-PI,0 
 else
 angle = 2.0f*(float)atan2( vl, w); //0,PI 
 }else{
 axis = vector3(0,0,0);
 angle = 0;
 }
 };

Конвертирование кватерниона в матрицу поворота

Матрица поворота выражается через компоненты кватерниона следующим способом:

где

Проверим формулы конвертирования  на примере конвертирования  произведения 2-х кватернионов в матрицу поворотов:

// Кватернионы  
q1 = [ 0, 0, sin(alpha/2), cos(alpha/2)];  // q1 = [x1,y1,z1,w1] 
q2 = [ sin(beta/2), 0, 0, cos(beta/2)];    // q2 = [x2,y2,z2,w2] 
// Произведение кватернионов q1 на q2 
q1q2 = [cos(alpha/2) sin(beta/2),   //  x
        sin(alpha/2) sin(beta/2),   //  y
        cos (beta/2) sin(alpha/2),  //  z
        cos(alpha/2) cos(beta/2)]   //  w

Определяем элемент матрицы m[0][0] через параметры кватерниона:

При упрощении записи использовались тригонометрические формулы.

Соответствующее произведению кватернионов (q1 и q2) произведение матриц поворотов было получено ранее (см. Матрицы поворота и углы Эйлера):

Как видим, результат m[0][0], полученный через конвертирование, совпал с значением в матрице поворота.

Фрагмент программного кода на С для конвертирования кватерниона в матрицу поворота:

/**
 set the rotation to matrix
 */
 inline void to_matrix( matrix33& m )const {
 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
 float s = 2.0f/norm(); // 4 mul 3 add 1 div
 x2 = x * s; y2 = y * s; z2 = z * s;
 xx = x * x2; xy = x * y2; xz = x * z2;
 yy = y * y2; yz = y * z2; zz = z * z2;
 wx = w * x2; wy = w * y2; wz = w * z2;

m.m[0][0] = 1.0f - (yy + zz);
 m.m[1][0] = xy - wz;
 m.m[2][0] = xz + wy;

m.m[0][1] = xy + wz;
 m.m[1][1] = 1.0f - (xx + zz);
 m.m[2][1] = yz - wx;

m.m[0][2] = xz - wy;
 m.m[1][2] = yz + wx;
 m.m[2][2] = 1.0f - (xx + yy);
 }

При конвертировании используется только умножения и сложения, что является несомненным преимуществом на современных процессорах.

Часто для задания вращений используют только кватернионы единичной длины, но это не обязательно и иногда даже не эффективно. Разница между конвертированием единичного и неединичного кватернионов составляет около 6-ти умножений и 3-х сложений, зато избавит во многих случаях от необходимости нормировать (приводить длину к 1) кватернион. Если кусок кода критичен по скорости и вы пользуетесь только кватернионами единичной длины тогда можно воспользоваться фактом что норма его равна 1.

Конвертирование матрицы поворота в кватернион

Алгоритм:

Конвертирование матрицы в кватернион выполняется не менее эффективно, чем кватерниона в матрицу, но в итоге мы получим кватернион неединичной длины. Его можно нормализовать.

Фрагмент программного кода конвертирования матрицы поворота в кватернион:

/**
 create a unit quaternion by rotation matrix
 martix must contain only rotation (not scale or shear)

For numerical stability we find first the greatest component of quaternion
 and than search others from this one
 */
 inline void unit_from_matrix( const matrix33 &mtx)
 {
 typedef float mtx_elm[3][3];
 const mtx_elm &m = mtx.m;
 float tr = m[0][0] + m[1][1] + m[2][2]; // trace of martix
 if (tr > 0.0f){ // if trace positive than "w" is biggest component
 set( m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0], tr+1.0f );
 scale( 0.5f/(float)sqrt( w ) ); // "w" contain the "norm * 4"

}else // Some of vector components is bigger
 if( (m[0][0] > m[1][1] ) && ( m[0][0] > m[2][2]) ) {
 set( 1.0f + m[0][0] - m[1][1] - m[2][2], m[1][0] + m[0][1],
 m[2][0] + m[0][2], m[1][2] - m[2][1] );
 scale( 0.5f/(float)sqrt( x ) );

}else 
 if ( m[1][1] > m[2][2] ){
 set( m[1][0] + m[0][1], 1.0f + m[1][1] - m[0][0] - m[2][2],
 m[2][1] + m[1][2], m[2][0] - m[0][2] ); 
 scale( 0.5f/(float)sqrt( y ) );

}else{
 set( m[2][0] + m[0][2], m[2][1] + m[1][2],
 1.0f + m[2][2] - m[0][0] - m[1][1], m[0][1] - m[1][0] );
 scale( 0.5f/(float)sqrt( z ) );

}
 }

//----------------------------------------------------------------------------
 /**
 create a nonunit quaternion from rotation matrix 
 martix must contain only rotation (not scale or shear)
 the result quaternion length is numerical stable 
 */
 inline void from_matrix( const matrix33 &mtx)
 {
 typedef float mtx_elm[3][3];
 const mtx_elm &m = mtx.m;
 float tr = m[0][0] + m[1][1] + m[2][2]; // trace of martix
 if (tr > 0.0f){ // if trace positive than "w" is biggest component
 set( m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0], tr + 1.0f );
 }else // Some of vector components is bigger
 if( (m[0][0] > m[1][1] ) && ( m[0][0] > m[2][2]) ) {
 set( 1.0f + m[0][0] - m[1][1] - m[2][2], m[1][0] + m[0][1],
 m[2][0] + m[0][2], m[1][2] - m[2][1] );
 }else 
 if ( m[1][1] > m[2][2] ){
 set( m[1][0] + m[0][1], 1.0f + m[1][1] - m[0][0] - m[2][2],
 m[2][1] + m[1][2], m[2][0] - m[0][2] ); 
 }else{
 set( m[2][0] + m[0][2], m[2][1] + m[1][2],
 1.0f + m[2][2] - m[0][0] - m[1][1], m[0][1] - m[1][0] );
 }
 }

Полезные ссылки:

 

 

Автор: Николай Свирневский

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *