Повороты, ориентация и кватернионы

Открыть сценарий

В этом примере рассматриваются концепции трехмерных вращений и способы использования кватернионов для описания ориентации и вращений. Кватернионы - поле перекоса гиперкомплексных чисел. Они нашли применение в аэрокосмической, компьютерной графике и виртуальной реальности. В MATLAB ® математика кватерниона может быть представлена путем манипулирования quaternion класс.

HelperDrawRotation используется для иллюстрации нескольких частей этого примера.

dr = HelperDrawRotation;

Вращения в трех измерениях

Все вращения в 3-D могут определяться осью вращения и углом поворота вокруг этой оси. Рассмотрим 3-D изображение чайника в крайнем левом сюжете. Чайник вращается на 45 градусов вокруг оси Z на втором графике. Более сложный поворот на 15 градусов вокруг оси [1 0 1] показан на третьем графике. Кватернионы заключают в себе ось и угол поворота и имеют алгебру для манипулирования этими вращениями. quaternion и в этом примере для определения вращений используется соглашение «правое правило». То есть положительные повороты выполняются по часовой стрелке вокруг оси вращения, если смотреть с начала координат.

dr.drawTeapotRotations;

Поворот точки

Вершины чайника были повернуты вокруг оси вращения в системе координат. Рассмотрим точку (0,7, 0,5), повернутую на 30 градусов вокруг оси Z.

figure;
dr.draw2DPointRotation(gca);

Поворот рамы

Вращение рамы в некотором смысле противоположно вращению точки. При повороте рамки точки объекта остаются фиксированными, но рамка привязки поворачивается. Еще раз рассмотрим точку (0,7, 0,5). Теперь опорная рамка поворачивается на 30 градусов вокруг оси Z. Следует отметить, что в то время как точка (0,7, 0,5) остается фиксированной, она имеет различные координаты в новой повернутой системе координат.

figure;
dr.draw2DFrameRotation(gca);

Ориентация

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

Рассмотрим пример, когда дочерняя опорная рамка поворачивается на 30 градусов вокруг вектора [1/3 2/3 2/3].

figure;
dr.draw3DOrientation(gca, [1/3 2/3 2/3], 30);

Кватернионы

Кватернионы - числа вида

$a + b\textbf{i} + c\textbf{j} + d\textbf{k}$

где

$$i^2=j^2=k^2=ijk=-1$$

и $a, b, c,$ и $d$ являются действительными числами. В остальном примере четыре числа и $d$ называются частями кватерниона.

Кватернионы для вращений и ориентации

Ось и угол поворота заключены в части кватерниона. Для оси единичного вектора вращения [x, y, z] и угла поворота $\alpha$ кватернион, описывающий это вращение, равен

$\cos\left(\frac{\alpha}{2}\right) +
\sin\left(\frac{\alpha}{2}\right)\left(x\textbf{i} + y\textbf{j}
+ z\textbf{k}\right)$

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

$norm(q) = \sqrt{a^2 + b^2 + c^2 + d^2}$

Существует множество способов построения кватерниона в MATLAB, например:

q1 = quaternion(1,2,3,4)

q1 = 

  quaternion


     1 + 2i + 3j + 4k

Массивы кватернионов могут быть выполнены таким же образом:

quaternion([1 10; -1 1], [2 20; -2 2], [3 30; -3 3], [4 40; -4 4])

ans = 

  2x2 quaternion array


      1 +  2i +  3j +  4k     10 + 20i + 30j + 40k
     -1 -  2i -  3j -  4k      1 +  2i +  3j +  4k

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

qmgk = quaternion(magic(4))

qmgk = 

  4x1 quaternion array


     16 +  2i +  3j + 13k
      5 + 11i + 10j +  8k
      9 +  7i +  6j + 12k
      4 + 14i + 15j +  1k

Кватернионы можно индексировать и манипулировать, как и любой другой массив:

qmgk(3)

ans = 

  quaternion


      9 +  7i +  6j + 12k

reshape(qmgk,2,2)

ans = 

  2x2 quaternion array


     16 +  2i +  3j + 13k      9 +  7i +  6j + 12k
      5 + 11i + 10j +  8k      4 + 14i + 15j +  1k

[q1; q1]

ans = 

  2x1 quaternion array


     1 + 2i + 3j + 4k
     1 + 2i + 3j + 4k

Математика кватерниона

Кватернионы имеют четко определенные арифметические операции. Сложение и вычитание подобны комплексным числам: детали добавляются/вычитаются независимо. Умножение сложнее из-за более раннего уравнения:

$$i^2=j^2=k^2=ijk=-1$$

Это означает, что умножение кватернионов не коммутативно. То есть $pq \neq qp$ для кватернионов $p$ и. $q$ Однако каждый кватернион имеет мультипликативный обратный, поэтому кватернионы можно разделить. Массивы quaternion класс можно добавлять, вычитать, умножать и делить на MATLAB.

q = quaternion(1,2,3,4);
p = quaternion(-5,6,-7,8);

Дополнение

p + q

ans = 

  quaternion


     -4 +  8i -  4j + 12k

Вычитание

p - q

ans = 

  quaternion


     -6 +  4i - 10j +  4k

Умножение

p*q

ans = 

  quaternion


    -28 - 56i - 30j + 20k

Умножение в обратном порядке (запишите другой результат)

q*p

ans = 

  quaternion


    -28 + 48i - 14j - 44k

Правое разделение p около q эквивалентно. $p(q^{-1})$

p./q

ans = 

  quaternion


         0.6 +  2.2667i + 0.53333j - 0.13333k

Левое разделение q около p эквивалентно. $p^{-1}q$

p.\q

ans = 

  quaternion


     0.10345 +  0.2069i +       0j - 0.34483k

Конъюгат кватерниона образуется отрицанием каждой из ненастоящих частей, аналогично сопряжению для комплексного числа:

conj(p)

ans = 

  quaternion


    -5 - 6i + 7j - 8k

Кватернионы могут быть нормализованы в MATLAB:

pnormed = normalize(p)

pnormed = 

  quaternion


    -0.37905 + 0.45486i - 0.53067j + 0.60648k

norm(pnormed)

ans =

     1

Повороты точек и рам с кватернионами

Кватернионы можно использовать для поворота точек в статической опорной рамке или для поворота самой опорной рамки. rotatepoint функция поворачивает точку $v = (v_x, v_y, v_z)$ с помощью кватерниона q через следующее уравнение:

$p v_{quat} p^*$

где - $v_{quat}$

$v_{quat} = 0 + v_x\textbf{i} + v_y\textbf{j} + v_z\textbf{k}$

и $p^*$ указывает на кватернионное сопряжение. Обратите внимание, что приведенное выше умножение кватерниона приводит к кватерниону с действительной частью,, $a$ равной 0. $b$ $c$ $d$ Части результата, и образуют повернутую точку (,,). $b$ $c$ $d$

Рассмотрим пример поворота точки сверху. Точку (0,7, 0,5) поворачивают на 30 градусов вокруг оси Z. В трех измерениях эта точка имеет координату 0 Z. Используя формулу ось-угол, кватернион может быть построен с использованием [0 0 1] в качестве оси вращения.

ang = deg2rad(30);
q = quaternion(cos(ang/2), 0, 0, sin(ang/2));
pt = [0.7, 0.5, 0];  % Z-coordinate is 0 in the X-Y plane
ptrot = rotatepoint(q, pt)

ptrot =

    0.3562    0.7830         0

Аналогично, rotateframe функция принимает кватернион $q$ и точку $v$ для вычисления

$p^* v_{quat} p$

Снова приведенное выше умножение кватерниона приводит к кватерниону с 0 вещественной частью. $b$ (,,) $c$ $d$ части результата образуют координату точки в $v$ новом повернутом опорном кадре. Использование quaternion класс:

ptframerot = rotateframe(q, pt)

ptframerot =

    0.8562    0.0830         0

Кватернион и его сопряжение имеют противоположные эффекты из-за симметрии в уравнениях поворота точки и кадра. Вращение сопряжением «отменяет» вращение.

rotateframe(conj(q), ptframerot)

ans =

    0.7000    0.5000         0

Из-за симметрии уравнений этот код выполняет такое же вращение.

rotatepoint(q, ptframerot)

ans =

    0.7000    0.5000         0

Другие представления поворота

Часто вращения и ориентации описываются с помощью альтернативных средств: углов Эйлера, матриц вращения и/или векторов вращения. Все они взаимодействуют с кватернионами в MATLAB.

Углы Эйлера часто используются, потому что их легко интерпретировать. Рассмотрим опорную рамку, повернутую на 30 градусов вокруг оси Z, затем на 20 градусов вокруг оси Y, а затем на -50 градусов вокруг оси X. При этом и на всем протяжении вращения вокруг каждой оси являются собственными: каждое последующее вращение происходит вокруг вновь созданного набора осей. Другими словами, второй поворот происходит вокруг «новой» оси Y, созданной первым поворотом, а не вокруг исходной оси Y.

figure;
euld = [30 20 -50];
dr.drawEulerRotation(gca, euld);

Чтобы построить кватернион из этих углов Эйлера для поворота кадра, используйте quaternion конструктор. Поскольку сначала порядок вращений вокруг оси Z, затем вокруг новой оси Y и, наконец, вокруг новой оси X, используйте 'ZYX' флаг.

qeul = quaternion(deg2rad(euld), 'euler', 'ZYX', 'frame')

qeul = 

  quaternion


      0.84313 -  0.44275i + 0.044296j +  0.30189k

'euler' флаг указывает, что первый аргумент находится в радианах. Если аргумент имеет значение в градусах, используйте 'eulerd' флаг.

qeuld = quaternion(euld, 'eulerd', 'ZYX', 'frame')

qeuld = 

  quaternion


      0.84313 -  0.44275i + 0.044296j +  0.30189k

Для обратного преобразования в углы Эйлера:

rad2deg(euler(qeul, 'ZYX', 'frame'))

ans =

   30.0000   20.0000  -50.0000

Эквивалентно, eulerd может быть использован способ.

eulerd(qeul, 'ZYX', 'frame')

ans =

   30.0000   20.0000  -50.0000

Альтернативно, то же самое вращение может быть представлено в виде матрицы вращения:

rmat = rotmat(qeul, 'frame')

rmat =

    0.8138    0.4698   -0.3420
   -0.5483    0.4257   -0.7198
   -0.1926    0.7733    0.6040

Преобразование обратно в кватернионы аналогично:

quaternion(rmat, 'rotmat', 'frame')

ans = 

  quaternion


      0.84313 -  0.44275i + 0.044296j +  0.30189k

Подобно тому, как кватернион может использоваться для поворота точки или кадра, он может быть преобразован в матрицу поворота (или набор углов Эйлера) специально для поворота точки или кадра. Матрица поворота для поворота точки является транспонированием матрицы для поворота кадра. Для преобразования между представлениями вращения необходимо указать 'point' или 'frame'.

Матрица поворота для секции поворота точки в этом примере:

rotmatPoint = rotmat(q, 'point')

rotmatPoint =

    0.8660   -0.5000         0
    0.5000    0.8660         0
         0         0    1.0000

Чтобы найти расположение повернутой точки, умножьте вправо rotmatPoint по транспонированному массиву pt.

rotmatPoint * (pt')

Матрица поворота для секции поворота кадра в этом примере:

rotmatFrame = rotmat(q, 'frame')

rotmatFrame =

    0.8660    0.5000         0
   -0.5000    0.8660         0
         0         0    1.0000

Чтобы найти местоположение точки в повернутой опорной рамке, умножьте вправо rotmatFrame по транспонированному массиву pt.

rotmatFrame * (pt')

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

rv = rotvec(qeul)

rv =

   -0.9349    0.0935    0.6375

Для преобразования в кватернион:

quaternion(rv, 'rotvec')

ans = 

  quaternion


      0.84313 -  0.44275i + 0.044296j +  0.30189k

Расстояние

Одним из преимуществ кватернионов над углами Эйлера является отсутствие разрывов. Углы Эйлера имеют разрывы, которые изменяются в зависимости от используемого соглашения. dist функция сравнивает эффект вращения двумя различными кватернионами. Результатом является число в диапазоне от 0 до pi. Рассмотрим два кватерниона, построенных из эйлеровых углов:

eul1 = [0, 10, 0];
eul2 = [0, 15, 0];
qdist1 = quaternion(deg2rad(eul1), 'euler', 'ZYX', 'frame');
qdist2 = quaternion(deg2rad(eul2), 'euler', 'ZYX', 'frame');

Вычитая углы Эйлера, можно увидеть отсутствие поворота вокруг оси Z или оси X.

eul2 - eul1

ans =

     0     5     0

Разница между этими двумя поворотами составляет пять градусов вокруг оси Y. dist показывает также разницу.

rad2deg(dist(qdist1, qdist2))

ans =

    5.0000

Для углов Эйлера, таких как eul1 и eul2, вычисление углового расстояния тривиально. Более сложным примером, который охватывает разрыв угла Эйлера, является:

eul3 = [0, 89, 0];
eul4 = [180, 89, 180];
qdist3 = quaternion(deg2rad(eul3), 'euler', 'ZYX', 'frame');
qdist4 = quaternion(deg2rad(eul4), 'euler', 'ZYX', 'frame');

Хотя eul3 и eul4 представляют почти одинаковую ориентацию, простое вычитание угла Эйлера создает впечатление, что эти две ориентации очень далеко друг от друга.

euldiff = eul4 - eul3

euldiff =

   180     0   180

Использование dist функция на кватернионах показывает, что существует только двухградусная разница в этих вращениях:

euldist = rad2deg(dist(qdist3, qdist4))

euldist =

    2.0000

Кватернион и его негатив представляют одно и то же вращение. Это не очевидно из вычитания кватернионов, но dist функция дает понять.

qpos = quaternion(-cos(pi/4), 0 ,0, sin(pi/4))

qpos = 

  quaternion


    -0.70711 +       0i +       0j + 0.70711k

qneg = -qpos

qneg = 

  quaternion


     0.70711 +       0i +       0j - 0.70711k

qdiff = qpos - qneg

qdiff = 

  quaternion


    -1.4142 +      0i +      0j + 1.4142k

dist(qpos, qneg)

ans =

     0

Поддерживаемые функции

quaternion класс позволяет эффективно описывать вращения и ориентации в MATLAB. Полный список функций, поддерживаемых кватернионом, можно найти в methods функция:

methods('quaternion')

Methods for class quaternion:

angvel              ismatrix            prod                
cat                 isnan               quaternion          
classUnderlying     isrow               rdivide             
compact             isscalar            reshape             
conj                isvector            rotateframe         
ctranspose          ldivide             rotatepoint         
disp                length              rotmat              
dist                log                 rotvec              
double              meanrot             rotvecd             
eq                  minus               single              
euler               mtimes              size                
eulerd              ndims               slerp               
exp                 ne                  times               
horzcat             norm                transpose           
iscolumn            normalize           uminus              
isempty             numel               validateattributes  
isequal             parts               vertcat             
isequaln            permute             
isfinite            plus                
isinf               power               

Static methods:

ones                zeros

Документация