MicroCephalis Dec 28 2020 at 09:02

Кватернионы, матрицы поворота и перепроецирование векторов между системами координат

5 min

12K

Пришлось это мне в последнее время поработать с задачами, где нужно было оперировать кватернионами и заниматься перепроецированием векторов в разные системы координат (это еще называется заменой базиса). Сначала по чужим формулам — причем с опечатками и даже, как выяснилось, с фактическими ошибками — а потом делать свои, по аналогии. И всё даже работало! Но сохранялся какой-то туман в понимании происходящего. А всё, как оказалось, из-за этих ошибок: их комбинация давала систему, в целом сохраняющую корректность, неверным путем таки достигался верный результат. Зато такая удача сильно мешала осознанию проблемы и прояснению природы феномена «верный итог при подозрительных формулах». При этом разбираться досконально времени все не было — работает же, числа выдает правильные, чего тебе еще надо, собака? Вперед, нужно больше ~~золота~~ кода! А вот сейчас пришел момент, когда я, похоже, окончательно всё понял, и хочу поделиться получившейся картинкой с окружающими. Вдруг кому пригодится, и себе памятка.

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

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

Исходные посылки

Описание кватернионов и матриц поворота как таковых опущу, предполагаю, что читатели знакомы с ними. Вектор здесь будет пониматься в его интуитивном представлении, как отрезок прямой в трехмерном пространстве, имеющий длину и направление. Исходное положение вектора будет иметь индекс «–», конечное — индекс «+».

Кватернионы и матрицы поворота могут быть использованы как для выполнения пространственного поворота векторов, так и для описания взаимной ориентации систем координат. В частности, для описания положения целевой системы координат относительно исходной: как нужно повернуть исходную, чтобы ее оси совпали с одноименными осями целевой. Матрица поворота (она же матрица ориентации, для случая описания взаимной ориентации систем координат) может быть получена из нормированного кватерниона ориентации по известным из литературы формулам. Обратное тоже верно: можно получить кватернион ориентации из матрицы.

Вектор $\widehat{v}$ , подлежащий повороту, задается величинами его проекций (длинами проекций) на оси исходной системы координат: три числа – проекции на три ортогональных оси $\widehat{v}=\begin{bmatrix} v_0\\ v_1\\ v_2 \end{bmatrix}$ . Получается матрица-столбец размерности 3х1, которая также называется вектором. Чтобы не смешивать в тексте подобную запись проекций вектора с интуитивным представлением о нем, я буду и дальше называть эту запись «матрицей-столбцом».

Поворот вектора

Поворачивать вектор можно, как уже сказано, и с помощью матрицы, и с помощью кватерниона.

Поворот с использованием матрицы: матрица поворота $inline$ умножается на исходную матрицу-столбец. На выходе имеем другую, результирующую матрицу-столбец:

$\begin{bmatrix} v_{0+}\\ v_{1+}\\ v_{2+} \end{bmatrix}=\begin{bmatrix}m_{11}\ m_{12}\ m_{13}\\ m_{21}\ m_{22}\ m_{23}\\ m_{31}\ m_{32}\ m_{33} \end{bmatrix}\begin{bmatrix}v_{0-}\\ v_{1-}\\ v_{2-}\end{bmatrix}$

Поворот с использованием кватерниона ориентации выполняется в два шага:

Из исходной матрицы-столбца $\widehat{v}_-$ и кватерниона $\Lambda=[\lambda_0,\lambda_1,\lambda_2,\lambda_3]$ вычисляется промежуточная матрица-столбец $\widehat{t} = 2\cdot \left (\begin{bmatrix}\lambda_1\\ \lambda_2\\ \lambda_3\end{bmatrix}\times \begin{bmatrix}v_{0-}\\ v_{1-}\\ v_{2-}\end{bmatrix} \right )$ , где $\times$ – операция векторного произведения векторов;
Вычисляется значение результирующей матрицы-столбца по формуле
$\begin{bmatrix} v_{0+}\\ v_{1+}\\ v_{2+} \end{bmatrix}=\begin{bmatrix} v_{0-}\\ v_{1-}\\ v_{2-} \end{bmatrix}+\lambda_0\cdot\begin{bmatrix} t_0\\ t_1\\ t_2 \end{bmatrix}+\begin{bmatrix}\lambda_1\\ \lambda_2\\ \lambda_3\end{bmatrix}\times \begin{bmatrix}t_0\\ t_1\\ t_2\end{bmatrix}$

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

С поворотом вектора дело ясное, вопросов нет.

Перепроецирование вектора

А вот задача перепроецирования вектора из одной системы координат в другую несколько контринтуитивна. Формулировка задачи: необходимо узнать, как некоторый вектор, заданный в исходной системе координат, выглядит в целевой системе. Иначе говоря, по проекциям вектора на оси исходной системы узнать проекции вектора на оси целевой. При этом задан кватернион или матрица ориентации целевой системы относительно исходной.

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

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

Задать в целевой системе координат «временный» вектор с теми же величинами проекций на оси этой целевой системы, какие есть у интересующего нас («заданного») вектора на оси исходной системы. Таким образом, «временный» вектор будет ориентирован относительно целевой системы так же, как заданный вектор ориентирован относительно исходной;
Определить ориентацию исходной системы относительно целевой. Здесь легко: нужная ориентация описывается либо кватернионом, сопряженным к имеющемуся ( $\Lambda^*=[\lambda_0,-\lambda_1,-\lambda_2,-\lambda_3]$ ), либо матрицей $M^{-1}$ , обратной по отношению к имеющейся; причем для матрицы ориентации, чей определитель всегда равен 1, обратная матрица совпадает с транспонированной $inline$ , поэтому достаточно транспонировать имеющуюся матрицу ориентации;
Повернуть «временный» вектор с использованием полученного кватерниона/матрицы. Вектор повернется относительно целевой системы координат так же, как относительно нее повернута исходная система. При этом, в силу всего сказанного ранее, а) «временный» вектор станет ориентирован уже относительно исходной системы так, как был ранее ориентирован относительно целевой, и, следовательно, точно совпадет с заданным вектором, и б) результатом операции будет матрица-столбец, описывающая, как теперь «временный» (а значит, и заданный) вектор выглядит в осях целевой системы координат.

Ура, это именно то, что нам нужно!

Кроме того, быстро становится очевидно, что с точки зрения реализации никакой «временный» вектор (в смысле «матрица-столбец») на самом деле не нужен: в ходе операции он никак не модифицируется, и при этом численно является копией исходного вектора. Так что, разумеется, можно спокойно брать сам исходный вектор и проводить операции с ним. Описание выше лишь подробнее иллюстрирует смысл происходящего.

Пример конкретного применения

В задачах навигации требуется вычитать из показаний датчиков угловой скорости скорость вращения Земли. Вращение Земли задается вектором, например, в геоцентрической системе координат (ГСК, прямоугольная декартова система, начало — в центре Земли, ось Z направлена на Северный полюс, ось Х — в точку пересечения экватора и нулевого меридиана, ось Y — в точку пересечения экватора и 90-го меридиана, получается правая система координат). Геоцентрическая система удобна, в числе прочего, тем, что в ней вращение Земли идет только вокруг оси Z. Также в любой момент известна ориентация связанной системы координат (ССК, прямоугольная декартова система, начало — в центре масс аппарата, ось Х направлена в его нос, ось Z — в правое крыло, ось Y — вверх, тоже правая тройка) летательного аппарата относительно геоцентрической, а оси датчиков угловой скорости в первом приближении совпадают с осями связанной системы координат. Необходимо определить, как скорость вращения Земли отображается в показаниях датчиков угловой скорости, чтобы можно было вычесть из их показаний составляющие, обусловленные ею, и посчитать вращение летательного аппарата чисто относительно местной вертикали. Итого дано: исходная система координат — ГСК, целевая — ССК, известен кватернион ориентации ССК относительно ГСК $\Lambda_{СГ}$ . Решение: задаем вектор $\begin{bmatrix} 0,\ 0,\ \Omega_З \end{bmatrix}^T$ , получаем сопряженный кватернион $\Lambda_{СГ}^*$ , выполняем с помощью него поворот вектора. Вуаля: имеем матрицу 3х1 проекций скорости вращения Земли на оси ССК. Можно вычитать.

P.S. Не могу не упомянуть огромный материал по кватернионам — серию постов в ЖЖ, созданных неким благородным мужем многих достоинств под ником Nabbla, содержащий массу информации по теме: первый пост из серии. Среди прочего в материале поясняется, например, почему способ поворота вектора с помощью кватерниона имеет именно такой вид, какой имеет. Чрезвычайно ему благодарен за труд.

Tags:

Hubs:

Mathematics