🔄 Преобразование между кватернионами и матрицами поворота
В прошлом посте я рассказывал о представлении вращений с помощью кватернионов. 📐 Это представление имеет множество плюсов: например, оно делает очевидным, как комбинируются вращения. Часто вращения задаются матрицами, поэтому важно уметь переходить между двумя форматами.
Единичный кватернион `(q₀, q₁, q₂, q₃)` задаёт поворот на угол θ вокруг оси, направление которой определяется вектором `(q₁, q₂, q₃)`, где `cos(θ/2) = q₀`. Соответствующая матрица поворота выглядит так:
Обратная задача — восстановление кватерниона из матрицы — сложнее. 💡 Один из методов, математически верных, но неоптимальных численно, называется методом Кьяверини-Сичилиано [1].
Здесь `sgn(x)` — функция знака: 1 при положительном `x` и −1 при отрицательном. 🧐 Любопытно, что компоненты кватерниона зависят только от диагонали матрицы, кроме знаков. Более точные алгоритмы учитывают и недиагональные элементы.
Степени свободы
Как четыре числа (кватернион) определяют девять чисел матрицы 3×3? 🤔 И наоборот: из девяти элементов матрицы мы берём три для восстановления кватерниона.
🌀 Единичные кватернионы имеют три степени свободы, как и ортогональные матрицы. Ось вращения (точка на сфере 🌐) даёт две степени, угол — третью.
Топологически множества единичных кватернионов и ортогональных матриц — трёхмерные многообразия. Но кватернионы — это двойное накрытие (double cover): каждому вращению соответствуют два кватерниона (`q` и `−q`).
🐍 Python-код
Реализуем преобразования:
“`python
import numpy as np
def quaterniontorotation_matrix(q):
q0, q1, q2, q3 = q
return np.array([
[2(q02 + q12) – 1, 2(q1q2 – q0q3), 2(q1q3 + q0*q2)],
[2(q1q2 + q0q3), 2(q02 + q22) – 1, 2(q2q3 – q0*q1)],
[2(q1q3 – q0q2), 2(q2q3 + q0q1), 2*(q02 + q32) – 1]
])
def rotationmatrixto_quaternion(R):
r11, r12, r13 = R[0,0], R[0,1], R[0,2]
r21, r22, r23 = R[1,0], R[1,1], R[1,2]
r31, r32, r33 = R[2,0], R[2,1], R[2,2]
q0 = 0.5 * np.sqrt(1 + r11 + r22 + r33)
q1 = 0.5 np.sqrt(1 + r11 – r22 – r33) np.sign(r32 – r23)
q2 = 0.5 np.sqrt(1 – r11 + r22 – r33) np.sign(r13 – r31)
q3 = 0.5 np.sqrt(1 – r11 – r22 + r33) np.sign(r21 – r12)
return np.array([q0, q1, q2, q3])
“`
Тестирование 🔄➡️⚠️
Проверим код, генерируя случайные кватернионы и матрицы:
“`python
from scipy.stats import norm, specialorthogroup
def randomq():
q = norm.rvs(size=4)
return q / np.linalg.norm(q)
def randomR():
return specialorthogroup.rvs(dim=3)
np.random.seed(20250507)
N = 10
Проверка кватернион → матрица → кватернион
for _ in range(N):
q = randomq()
R = quaterniontorotation_matrix(q)
t = rotationmatrixto_quaternion(R)
print(np.linalg.norm(q – t)) # Выводит ~2 без корректировки знака! ❌
Исправляем: добавляем стандартизацию кватерниона
def randomq_fixed():
q = randomq()
q *= np.sign(q[0]) # Делаем первую компоненту положительной ✅
return q
“`
После исправления ошибки (стандартизация знака) результаты становятся точными: нормы ~10⁻¹⁶–10⁻¹⁴. 🎯
📚 [1] Подробнее в статье «Accurate Computation of Quaternions from Rotation Matrices» (Soheil Sarabandi, Federico Thomas).
—
🧩 Композиция вращений с помощью кватернионов
Каждое 3D-вращение имеет неподвижную ось — это теорема Эйлера (1775). 🌍 Иными словами: как бы вы ни вращали сферу, две точки останутся на месте.
Композиция двух вращений — тоже вращение. Сначала ось фиксирует первый поворот, затем второй, а результат задаёт третью ось. 🌀 С кватернионами это видно сразу!
Кватернион поворота на угол θ вокруг оси `v = (v₁, v₂, v₃)`:
`q = (cos(θ/2), sin(θ/2)v₁, sin(θ/2)v₂, sin(θ/2)v₃)`.
Чтобы повернуть точку `p = (p₁, p₂, p₃)`, представляем её как кватернион `(0, p₁, p₂, p₃)` и вычисляем:
`q p q⁻¹`
(где `q⁻¹` — сопряжённый кватернион, так как `||q|| = 1`).
Композиция вращений проста: поворот на `q`, затем на `r` равен повороту на `r * q`. Доказательство:
`r(q p q⁻¹) r⁻¹ = (r q) p (q⁻¹ r⁻¹) = (r q) p (r q)⁻¹`. ✨
👉 В следующем посте — как переходить между кватернионами и матрицами.
📚 [1] В 2D и чётномерных пространствах это неверно.
[2] Для вектора `v` единичной длины кватернион автоматически единичный:
`||q||² = cos²(θ/2) + sin²(θ/2) = 1`.
Математика — это магия! 🔮