Вращения, ориентации и кватернионы для автоматизированного управления

quaternion является гиперкомплексным числом с четырьмя частями, используемым, чтобы описать трехмерные вращения и ориентации. Кватернионы имеют приложения во многих полях, включая космос, компьютерную графику и виртуальную реальность. В автоматизированном управлении датчики, такие как инерционные модули измерения (IMUs) сообщают о показаниях ориентации как о кватернионах. Чтобы использовать эти данные для локализации, можно получить его с помощью quaternion возразите, выполните математические операции на нем или преобразуйте его в другие форматы вращения, такие как матрицы вращения и Углы Эйлера.

Можно использовать кватернионы, чтобы выполнить 3-D точку и вращения системы координат.

С point rotations вы вращаете точки в статической системе отсчета.
С frame rotations вы вращаете систему отсчета вокруг статической точки, чтобы преобразовать систему координат в систему координат относительно точки.

Можно задать эти вращения при помощи оси вращения и угла вращения вокруг той оси. Кватернионы инкапсулируют ось и угол вращения и имеют алгебру для управления этими вращениями. quaternion возразите использует соглашение "правила правой руки" задать вращения. Таким образом, положительные вращения по часовой стрелке вокруг оси вращения, когда просматривается от источника.

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

Номер кватерниона представлен в этой форме:

$a + b i + c j + d k$

a, b, c и d являются вещественными числами. Эти коэффициенты известны как parts кватерниона.

i, j, и k - комплексные элементы кватерниона. Эти элементы удовлетворяют ^{уравнению i2} = ^j2 = ^k2 = ijk = −1.

Части кватерниона a, b, c и d задают ось и угол вращения. Для вращения радианов ɑ об оси вращения, представленной единичным вектором [x, y, z], кватернион, описывающий вращение, дан этим уравнением:

$\cos (\frac{α}{2}) + \sin (\frac{α}{2}) (x i + y j + z k)$

Создание кватерниона

Открыть скрипт

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

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

q = 

  quaternion


     1 + 2i + 3j + 4k

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

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

qArray = 

  2x2 quaternion array


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

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

qRandom = randrot(4,1)

qRandom = 

  4x1 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k
      0.21908 -  0.89875i -    0.298j +  0.23548k
       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.69704 - 0.060589i +  0.68679j -  0.19695k

Индексируйте и управляйте кватернионами точно так же, как любой другой массив. Например, индексируйте кватернион от qRandom массив кватерниона.

qRandom(3)

ans = 

  quaternion


      0.6375 + 0.49338i - 0.24049j + 0.54068k

Измените размерность массива кватерниона.

reshape(qRandom,2,2)

ans = 

  2x2 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.21908 -  0.89875i -    0.298j +  0.23548k      0.69704 - 0.060589i +  0.68679j -  0.19695k

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

[qRandom; q]

ans = 

  5x1 quaternion array


      0.17446 +  0.59506i -  0.73295j +  0.27976k
      0.21908 -  0.89875i -    0.298j +  0.23548k
       0.6375 +  0.49338i -  0.24049j +  0.54068k
      0.69704 - 0.060589i +  0.68679j -  0.19695k
            1 +        2i +        3j +        4k

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

Скрипт Open Live Script

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

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

q1 = quaternion
     1 + 2i + 3j + 4k

q2 = quaternion(-5,6,-7,8)

q2 = quaternion
    -5 + 6i - 7j + 8k

Сложение кватернионов похоже на комплексные числа, где части добавляются независимо.

q1 + q2

ans = quaternion
     -4 +  8i -  4j + 12k

Вычитание кватернионов работает похожее на сложение кватернионов.

q1 - q2

ans = quaternion
      6 -  4i + 10j -  4k

Поскольку комплексные элементы кватернионов должны удовлетворить уравнению

$i^{2} = j^{2} = k^{2} = ijk = - 1$ ,

умножение кватернионов является более комплексным, чем сложение и вычитание. Учитывая это требование, умножение кватернионов не является коммутативным. Таким образом, когда умножение кватернионов, инвертирование порядка кватернионов изменяют результат их продукта.

q1 * q2

ans = quaternion
    -28 + 48i - 14j - 44k

q2 * q1

ans = quaternion
    -28 - 56i - 30j + 20k

Однако каждый кватернион имеет мультипликативную инверсию, таким образом, можно разделить кватернионы. Правое деление q1 q2 эквивалентно $q 1 (q 2^{- 1})$ .

q1 ./ q2

ans = quaternion
      0.10345 -   0.3908i - 0.091954j + 0.022989k

Левое деление q1 q2 эквивалентно $(q 2^{- 1}) q 1$ .

q1 .\ q2

ans = quaternion
     0.6 - 1.2i +   0j +   2k

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

conj(q1)

ans = quaternion
     1 - 2i - 3j - 4k

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

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

Нормируйте кватернион.

qNormalized = normalize(q1)

qNormalized = quaternion
     0.18257 + 0.36515i + 0.54772j +  0.7303k

Проверьте, что этот кватернион нормированной единицы имеет норму 1.

norm(qNormalized)

ans = 1.0000

Матрица вращения для сопряженного из нормированного кватерниона равна инверсии матрицы вращения для того нормированного кватерниона.

rotmat(conj(qNormalized),'point')

ans = 3×3

   -0.6667    0.6667    0.3333
    0.1333   -0.3333    0.9333
    0.7333    0.6667    0.1333

inv(rotmat(qNormalized,'point'))

ans = 3×3

   -0.6667    0.6667    0.3333
    0.1333   -0.3333    0.9333
    0.7333    0.6667    0.1333

Извлеките кватернионы из матрицы преобразования

Скрипт Open Live Script

Если у вас есть 3-D матрица преобразования, созданная с помощью функций, таких как rigid3d или affine3d, можно извлечь матрицу вращения из него и представлять его в форме кватерниона. Однако прежде, чем выполнить это преобразование, необходимо сначала преобразовать матрицу вращения от постумножить формата до предварительно умножить формата, ожидаемого кватернионами.

Постумножьте формат

Чтобы выполнить вращения с помощью матричной части вращения матрицы преобразования, умножьтесь (x, y, z) точка этой матрицей вращения.

Во вращениях точки эта точка вращается в системе отсчета.
Во вращениях системы координат система отсчета вращается вокруг этой точки.

Матрицы преобразования представлены rigid3d или affine3d объекты используют постумножить формат. В этом формате точка умножается на матрицу вращения в том порядке. Чтобы удовлетворить умножению матриц, точка и ее соответствующий вектор сдвига должны быть векторами-строками.

Это уравнение показывает постумножить формат для матрицы вращения, R, и вектор сдвига, t.

$[\begin{array}{c} x' & y^{'} & z^{'} \end{array}] = [\begin{array}{c} x & y & z \end{array}] [\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] + [\begin{array}{c} t_{x} & t_{y} & t_{z} \end{array}]$

Этот формат также применяется, когда R и t объединены в гомогенную матрицу преобразования. В этой матрице этот 1 используется, чтобы удовлетворить умножению матриц и может быть проигнорирован.

$[\begin{array}{c} x^{'} & y^{'} & z^{'} & 1 \end{array}] = [\begin{array}{c} x & y & z & 1 \end{array}] [\begin{array}{c} R_{11} & R_{12} & R_{13} & 0 \\ R_{21} & R_{22} & R_{23} & 0 \\ R_{31} & R_{32} & R_{33} & 0 \\ t_{x} & t_{y} & t_{z} & 1 \end{array}]$

Предварительно умножьте формат

В предварительно умножить формате матрица вращения умножается на точку в том порядке. Чтобы удовлетворить умножению матриц, точка и ее соответствующий вектор сдвига должны быть вектор-столбцами.

Это уравнение показывает предварительно умножить формат, где R является матрицей вращения, и t является вектором сдвига.

$[\begin{array}{c} x' \\ y^{'} \\ z^{'} \end{array}] = [\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] [\begin{array}{c} x \\ y \\ z \end{array}] + [\begin{array}{c} t_{x} \\ t_{y} \\ t_{z} \end{array}]$

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

$[\begin{array}{c} x^{'} \\ y^{'} \\ z^{'} \\ 1 \end{array}] = [\begin{array}{c} R_{11} & R_{12} & R_{13} & t_{x} \\ R_{21} & R_{22} & R_{23} & t_{y} \\ R_{31} & R_{32} & R_{33} & t_{z} \\ 0 & 0 & 0 & 1 \end{array}] [\begin{array}{c} x \\ y \\ z \\ 1 \end{array}]$

Преобразуйте от, постумножаются, чтобы предварительно умножить формат

Чтобы преобразовать матрицу вращения в предварительно умножить формат, ожидаемый кватернионами, возьмите транспонирование матрицы вращения.

Создайте 3-D твердый объект геометрического преобразования из матрицы вращения и вектора сдвига. Угол вращения, $θ$ , в градусах.

theta = 30;
rot = [ cosd(theta) sind(theta) 0; ...
       -sind(theta) cosd(theta) 0; ...
                  0           0 1];
trans = [2 3 4];

tform = rigid3d(rot,trans)

tform = 
  rigid3d with properties:

       Rotation: [3x3 double]
    Translation: [2 3 4]

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

rotPost = tform.Rotation

rotPost = 3×3

    0.8660    0.5000         0
   -0.5000    0.8660         0
         0         0    1.0000

rotPre = rotPost.'

rotPre = 3×3

    0.8660   -0.5000         0
    0.5000    0.8660         0
         0         0    1.0000

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

q = quaternion(rotPre,'rotmat','point')

q = quaternion
     0.96593 +       0i +       0j + 0.25882k

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

point = [1 2 3];
rotatedPointQuaternion = rotatepoint(q,point)

rotatedPointQuaternion = 1×3

   -0.1340    2.2321    3.0000

rotatedPointRotationMatrix = point * rotPost

rotatedPointRotationMatrix = 1×3

   -0.1340    2.2321    3.0000

Чтобы преобразовать назад в исходную матрицу вращения, извлеките матрицу вращения из кватерниона. Затем создайте rigid3d объект при помощи транспонирования этой матрицы вращения.

R = rotmat(q,'point');
T = rigid3d(R',trans);
T.Rotation

ans = 3×3

    0.8660    0.5000         0
   -0.5000    0.8660         0
         0         0    1.0000

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