Вращение, ориентация и кватернионы для беспилотного вождения

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

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

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

Можно задать эти повороты с помощью оси поворота и угла поворота вокруг этой оси. Кватернионы инкапсулируют ось и угол поворота и имеют алгебру для манипулирования этими вращениями. The quaternion объект использует соглашение «right-hand rule» для определения вращений. То есть положительное вращение происходит по часовой стрелке вокруг оси вращения, если смотреть с источника.

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

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

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

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

i, j, и k являются сложными элементами массива кватерниона. Эти элементы удовлетворяют уравнению i² = j² = k² = 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

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

Открыть 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

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

Открыть 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}]$

Предварительный формат

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

Это уравнение показывает преждевременный формат, где 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}]$

Преобразование из Postmultily в Premultily Format

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

Создайте 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]

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

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

См. также

affine3d | quaternion | rigid3d | rotateframe | rotatepoint

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