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

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

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

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

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

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

Номер кватерниона представлен в следующем виде:

$a + bi +$ cj + dk

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

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

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

$\cos \frac{(}{} α2) + \frac{}{sin} (α2) (xi +$ yj + zk)

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

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

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

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

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

Открыть сценарий в реальном времени

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

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

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

$^{i2} =^{} {j2}^{} = k2 =$ ijk = -1,

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

q1 * q2

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

q2 * q1

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

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

q1 ./ q2

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

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

q1 .\ q2

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

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

conj(q1)

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

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

$норма (q) \sqrt{^{=}^{a2} +^{} {b2}^{}}$ + c2 + d2.

Нормализовать кватернион.

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

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

Открыть сценарий в реальном времени

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

Формат постмультиплексирования

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

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

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

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

$[\begin{array}{c} ^{} & {x′y′z}^{} \end{array}'] \begin{array}{c} = & [ \end{array} \begin{array}{c} {xyz}_{]} & [_{} & _{} \\ _{} & _{} & _{} \\ _{} & _{} & _{} \end{array} \begin{array}{c} {R11R12R13R21R22R23R31R32R33}_{]} & _{+} & _{[} \end{array}$ txtytz]

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

$[\begin{array}{c} ^{} & ^{} & ^{} \end{array} \begin{array}{c} x′y′z′1]=[xyz1 \end{array}] \begin{array}{c} [_{} & _{} & _{} \\ _{} & _{} & _{} \\ _{} & _{} & _{} \\ _{} & _{} & _{} \end{array} R11R12R130R21R22R230R31R32R330txtytz1$ ]

Формат предварительного перемножения

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

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

$[\begin{array}{c} ^{} \\ {x′y′z}^{} \end{array}'] \begin{array}{c} =_{[} & _{} & _{} \\ _{} & _{} & _{} \\ _{} & _{} & _{} \end{array} \begin{array}{c} R11R12R13R21R22R23R31R32R33 \\ ] \end{array} [\begin{array}{c} {xyz}_{]} \\ _{+} \\ _{[} \end{array}$ txtytz]

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

$[\begin{array}{c} ^{} \\ ^{} \\ ^{} \end{array} \begin{array}{c} _{} & _{} & _{} & _{} \\ _{} & _{} & _{} & _{} \\ _{} & _{} & _{} & _{} \\ x′y′z′1]=[R11R12R13txR21R22R23tyR31R32R33tz0001 \end{array}] \begin{array}{c} [ \end{array} xyz1$ ]

Перевести из формата Postmultiply в формат Premultiply

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

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

См. также

affine3d | quaternion | rigid3d | rotateframe | rotatepoint

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