Ликбез по кватернионам, часть 6 1/4, кратчайший поворот в общем случае: nabbla1

nabbla1

Ликбез по кватернионам, часть 6 1/4, кратчайший поворот в общем случае

Jul 15, 2018 04:23

Оглавление "Ликбеза по кватернионам":
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3

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

Кватернион поворота по кратчайшему пути, общий случай
Нам даны два вектора $\vec{\mathbf{u}}$
и $\vec{\mathbf{v}}$
произвольной длины (но не нулевые!). Мы хотим найти такой поворот, который переведёт направление $\vec{\mathbf{u}}$
в направление $\vec{\mathbf{v}}$
по кратчайшему пути.
Разумеется, задачу можно свести к предыдущей, отнормировав векторы $\vec{\mathbf{u}}$
и $\vec{\mathbf{v}}$
, но тогда нам понадобится в общей сложности три извлечения квадратного корня - мы можем и лучше!

Запишем, чему равны скалярное и векторное произведение $\vec{\mathbf{u}}$
и $\vec{\mathbf{v}}$
:
$(\vec{\mathbf{u}},\vec{\mathbf{v}})=|\vec{\mathbf{u}}||\vec{\mathbf{v}}|cos\varphi,$

$[\vec{\mathbf{u}},\vec{\mathbf{v}}]=\vec{\mathbf{n}}_\bot|\vec{\mathbf{u}}||\vec{\mathbf{v}}|sin\varphi,$
(7.10)

Снова выпишем кватернион поворота по кратчайшему пути:
$\Lambda=cos\frac{\varphi}{2}+sin\frac{\varphi}{2}\vec{\mathbf{n}}_\bot$
(7.5)

и преобразуем его, принимая во внимание (7.10):
$\Lambda=cos\frac{\varphi}{2}+\frac{sin\frac{\varphi}{2}[\vec{\mathbf{u}},\vec{\mathbf{v}}]}{|\vec{\mathbf{u}}||\vec{\mathbf{v}}|sin\varphi}=cos\frac{\varphi}{2}+\frac{\cancel{sin\frac{\varphi}{2}}[\vec{\mathbf{u}},\vec{\mathbf{v}}]}{|\vec{\mathbf{u}}||\vec{\mathbf{v}}|2\cancel{sin\frac{\varphi}{2}}cos\frac{\varphi}{2}}=$

$=cos\frac{\varphi}{2}+\frac{[\vec{\mathbf{u}},\vec{\mathbf{v}}]}{2cos\frac{\varphi}{2}|\vec{\mathbf{u}}||\vec{\mathbf{v}}|}=\frac{1}{2cos\frac{\varphi}{2}|\vec{\mathbf{u}}||\vec{\mathbf{v}}|}(2cos^2\frac{\varphi}{2}|\vec{\mathbf{u}}||\vec{\mathbf{v}}|+[\vec{\mathbf{u}},\vec{\mathbf{v}}])$

Принимая во внимание формулу косинуса двойного угла:
$2cos^2\frac{\varphi}{2}=1+cos\varphi$

Получаем выражение:
$\Lambda=\frac{1}{2cos\frac{\varphi}{2}|\vec{\mathbf{u}}||\vec{\mathbf{v}}|}((1+cos\varphi)|\vec{\mathbf{u}}||\vec{\mathbf{v}}|+[\vec{\mathbf{u}},\vec{\mathbf{v}}])=$

$=\frac{1}{2cos\frac{\varphi}{2}|\vec{\mathbf{u}}||\vec{\mathbf{v}}|}(|\vec{\mathbf{u}}||\vec{\mathbf{v}}|+(\vec{\mathbf{u}},\vec{\mathbf{v}})+[\vec{\mathbf{u}},\vec{\mathbf{v}}])$

Как и раньше, нам не требуется считать знаменатель «в лоб». Лучше найдём сначала ненормированный кватернион:
$\Lambda_{non-unity}=|\vec{\mathbf{u}}||\vec{\mathbf{v}}|+(\vec{\mathbf{u}},\vec{\mathbf{v}})+[\vec{\mathbf{u}},\vec{\mathbf{v}}]$

Причем можно заметить, что первое слагаемое требует извлечения только одного квадратного корня,
$|\vec{\mathbf{u}}||\vec{\mathbf{v}}|=\sqrt{u_x^2+u_y^2+u_z^2}\sqrt{v_x^2+v_y^2+v_z^2}=\sqrt{(u_x^2+u_y^2+u_z^2)(v_x^2+v_y^2+v_z^2)}$

Далее, если $\Lambda_{non-unity}\neq0$
, нормируем его и получаем ответ. Нулевое значение, как и раньше, возникает в ситуации, когда $\vec{\mathbf{u}}$
и $\vec{\mathbf{v}}$
направлены в противоположные стороны, $\vec{\mathbf{u}}=k\vec{\mathbf{v}},k\le0$
. Тогда поворот на 180 градусов по любому из направлений будет одинаково «коротким», и нам придётся выбрать какой-нибудь из них самостоятельно, например, тем же способом, что в главе 7.1.

Тогда, итоговый метод выглядит так:

$\Lambda_{non-unity}=|\vec{\mathbf{u}}||\vec{\mathbf{v}}|+(\vec{\mathbf{u}},\vec{\mathbf{v}})+[\vec{\mathbf{u}},\vec{\mathbf{v}}]$

$\Lambda=\left\{ \begin{array}{l} \frac{\Lambda_{non-unity}}{|\Lambda_{non-unity}|},\,|\Lambda_{non-unity}|\neq\,0 \\ \frac{{(0;-u_z;u_y)}^T}{\sqrt{u_z^2+u_y^2}},\,|\Lambda_{non-unity}|=0,\,|u_x|<0,58 \\ \frac{{(u_z;0;-u_x)}^T}{\sqrt{u_z^2+u_x^2}},\,|\Lambda_{non-unity}|=0,\,|u_x|\ge0,58 \end{array} \right.$
(7.11)

Одна из задач, где может пригодиться данный метод, такова.
Мы делаем бесплатформенную инерциальную навигационную систему, причем иногда возникают моменты, когда нам говорят - никакого ускорения сейчас не действует, всё, что чувствует акселерометр - это сила тяжести. И тогда нам хочется подправить кватернион ориентации, чтобы хоть вертикальная ось стала сонаправлена вектору силы тяжести.

Взяв за вектор $\vec{\mathbf{u}}$
направление "вертикально вниз" в координатах прибора (с накопленным дрейфом), а за $\vec{\mathbf{v}}$
- показания акселерометра, по описанному методу мы можем посчитать корректирующий кватернион, на который надо будет помножить текущий кватернион ориентации, чтобы выправить вертикальную ось.

UPD. Страшно подумать - я начал писать этот ликбез уже чуть более года назад... Но не беспокойтесь - у меня в загашниках лежит 3 главы, которые я пока не переложил в формат ЖЖ, 2 приложения, список литературы, да и коллекция упражнений потихоньку набирается.

кватернионы-это просто (том 1), математика, работа