Поворот в пространстве и кватернионы

Ориентация в пространстве

Термин «ориентация» подразумевает наличие какой либо начальной системы координат, относительно которой ориентация будет являться простым поворотом. Например, она могла бы быть привязана к Земле, если мы определяем положение относительно нее, или, другой пример, к туловищу модели, ориентацией головы которой мы хотели бы управлять. Если задана такая референсная система координат, то ориентация в пространстве задается поворотом относительно нее.

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

Возможно, вы подумали о двух сферических координатах, однако они задают только направление и остается еще одна ось, идущая вдоль этого направления, вращение вокруг которой не зафиксировано. Вводим еще один параметр- угол поворота вокруг этой оси- и получаем, так называемые, углы Эйлера.

Углы Эйлера

gimbal

Углы Эйлера- крен (roll), тангаж (pitch) и рыскание (yaw) — это три угла, задающих тройной последовательный поворот системы координат вкруг трех ее осей. Этот способ задания поворота изначально кажется самым простым и логичным, однако это иллюзия.  Все было бы просто, если бы ни тот факт, что первый поворот поворачивает всю систему, следовательно второй поворот идет вокруг уже повернутой оси и поворачивает третью ось, вокруг которой идет уже третий поворот. Все это оборачивается рядом проблем:

Углы Эйлера подвержены т.н. гимбал локу («складывание рамок» по-русски, название связано с устройством механического гироскопа) суть которого в том, что на полюсах нашей координатной сферы находятся две точки сингулярности, в которых углы крена и рыскания приводят к повороту вокруг одной и той же оси, что приводит к неоднозначности. Это серьезный минус — везде, где вы имеете дело с углами Эйлера, приходится делать поправку на частный случай гимбал лока.

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

Еще одно важное ограничение- невозможность интерполировать две ориентации, то есть получить промежуточную ориентацию, задав разные веса для двух ориентации, заданных углами Эйлера. Интерполяция нужна как минимум для двух очевидных вещей: для фильтрации шума сенсоров, задающих ориентацию, и для того чтобы получить плавную анимацию поворота из одной ориентации в другую. (Вообще, на самом деле, можно попробовать интерполировать попарно каждый угол Эйлера обоих ориентаций. Проблема в том, что такая интерполяция проходит не по наименьшей дуге.)

В общем, у углов Эйлера только один плюс- они имеют интуитивно понятное геометрическое представление, в остальном одни минусы и ограничения. Не тру.

Матрица поворота

Еще один способ описать поворот пространства- это матрица поворота (3×3 для трехмерного случая). Матрица поворота лишена самого серьезного недостатка углов Эйлера- гимбал лока. Заданные матрицами повороты можно комбинировать обычным умножением матриц. Применить поворот к вектору можно просто умножив его на матрицу поворота. И вообще, у матриц поворота (и других матриц трансформации) есть множество замечательных свойств, поэтому матрицы поворота это тру. Однако, две матрицы нельзя интерполировать напрямую. Матрица поворота является отражением поворота вокруг произвольного единичного вектора на произвольный угол, что, собственно, и подводит нас к кватернионам.

Ось-угол

Есть еще одно представление поворота- трехмерный вектор, описывающий ось вращения, длина которого равна углу поворота. Это просто и понятно, кроме того, экономно по памяти- всего три параметра против 3, и применяется на практике (например, MEMS гироскопы предоставляют как раз вектор-ось с длиной равной угловой скорости)

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

Кватернион

matrix_quaternion_example

Вообще, стоит открыть википедию, как страх перед незнакомой математикой берет свое и кажется, что лучше уж иметь дело с углами Эйлера. Поэтому мы зайдем с другой стороны, чтобы убедиться что не так страшен чёрт. Итак, кватернион- это всего лишь альтернативное, «более лучшее» представление оси-угла, упомянутой в предыдущем параграфе. Пусть, есть вектор \overrightarrow{n} = \begin{bmatrix} x & y & z \end{bmatrix}  задающий ось, вокруг которой мы поворачиваем систему координат по часовой стрелке на угол \phi . Тогда вот кватернион, описывающий это вращение:
\mathbf{Q} = (cos(\phi/2)\cdot \mathbf{1} + sin(\phi/2)\cdot x\cdot\mathbf{i} + sin(\phi/2)\cdot y\cdot\mathbf{j} + sin(\phi/2)\cdot z\cdot\mathbf{k}) Это четырехмерное комплексное число, с тремя мнимыми единицами и одной реальной. Реальная и все мнимые единицы взаимно перпендикулярны. Их можно представить как единичные вектора, задающие базис четырехмерного пространства. Тогда кватернион (w + xi + yj + zk) можно выразить вектором в этом пространстве [w, x, y, z]. Таким образом, кватернион- это такое четырехмерное число, которое однозначно описывает поворот в трехмерном пространстве и обладает свойствами как вектора так и гиперкомплексного числа, поэтому, все операции над кватернионами очень простые и либо копируют векторную алгебру, либо описывают операцию над комплексными числами:

  • Умножение на скаляр k\cdot \mathbf{q} = [k\cdot w, k\cdot x, k\cdot y, k\cdot z]
  • Сложение \mathbf{q_1} + \mathbf{q_2} = [w_1 + w_2, x_1 + x_2, y_1 + y_2, z_1 + z_2] Сложение кватернионов можно использовать для упрощенной интерполяции.
  • Модуль кватерниона |\mathbf{q}| = \sqrt{w^2 + x^2 + y^2 + z^2}
  • Сопряжение \mathbf{q^*} = [w, -x, -y, -z]
  • Норма \mathbf{\|q\|} = ww+xx+yy+zz
  • Инверсный кватернион \mathbf{q^{-1}} = \frac{q*}{|q|}
  • Умножение
    \mathbf{q_1}\mathbf{q_2}= [w_1, \overrightarrow{v_1}] [w_2, \overrightarrow{v_2}]=[w_1 w_2 -\overrightarrow{v_1}\cdot \overrightarrow{v_2}, w_1 \overrightarrow{v_2} + w_2 \overrightarrow{v_1} + \overrightarrow{v_1} \times \overrightarrow{v_2}]= [w_1 w_2-x_1 x_2-y_1 y_2-z_1 z_2,w_1 x_2+x_1 w_2+y_1 z_2-z_1 y_2,w_1 y_2-x_1 z_2+y_1 w_2+z_1 x_1,w_1 z_2+x_1 y_2-y_1 x_2+z_1 w_2]Результат умножения двух кватернионов это тоже кватернион, который является комбинацией двух поворотов, как если бы повернули сначала одним кватернионом, потом вторым.
  • Скалярное произведение \mathbf{q_1}\cdot \mathbf{q_2}=w_1*w_2 + x_1*x_2 + y_1*y_2 + z_1*z_2

Итак, как видите, ничего сложного. Обычная векторная и комплексная алгебра. Зато, что мы получаем:

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

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

В-третьих, кватернионы можно интерполировать:
\mathbf{q}=q_1\frac{sin((1-t)\phi)}{sin(\phi)}+q_2\frac{sin(t\phi)}{sin(\phi)}

Это сферическая линейная интерполяция. Углы между кватернионами небольшие и вы готовы немного пожертвовать точностью, чтобы избавиться от тригонометрических операций и деления? Тогда вот приближенная интерполяция:
\mathbf{q}=(1-t)\mathbf{q_1}+t\mathbf{q_2}

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

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

Кватернион легко конвертируется в матрицу, и матрица в кватернион.

Итого

Есть множество способов задания поворота в пространстве, но только два выходят в финал- кватернионы и матрицы поворота, достойные отдельной статьи.

Если вы раздумываете, чем воспользоваться, кватернионы- то, что вам нужно. Плюньте на углы Эйлера и всячески избегайте любых алгоритмов завязанных на них и вы забудете о гимбал локе и адовых тригонометрических вычислениях. Храните ориентацию в кватернионах, храните повороты в кватернионах, даже скорость вращения можно хранить в кватернионах. Всего четыре числа- комбинируйте, интерполируйте, визуализируйте, делайте все что вам нужно. И если уж конечное графическое API таки требует матрицу, конвертируйте кватернион в матрицу на последнем этапе.

Полезные ссылки:

UPD: Кватернионы ориентации

Есть один нюанс, с которым я столкнулся и который следует учитывать. Дело в том, что кватернион ориентации — это поворот от системы отсчета вокруг некого вектора по часовой стрелке, если смотреть вдоль него. Что из этого следует? Да то, что до одной и той же ориентации  можно повернуться двумя разными поворотами- как это ни глупо звучит, но, в одну сторону и в другую 🙂 То есть, себе за спину вы можете повернуться через левый поворот и через правый. В случае с кватернионами, существует два противоположных вектора, задающие одну и ту же ось, а следовательно и два кватерниона, задающие один и тот же поворот: q и -q. В принципе, это может быть не важно для большинства применения кватернионов, однако это важно, если вы фильтруете ориентацию во времени.

Спасибо, что читаете! Вопросы и поправки неточностей смело оставляйте в комментах 🙂

Запись опубликована в рубрике Алгоритмы, Программирование с метками , . Добавьте в закладки постоянную ссылку.

16 комментариев: Поворот в пространстве и кватернионы

  1. Павел говорит:

    Спасибо за статью. Оказывается, кватернион для поворота очень простой. Вот—то я не думал!

  2. E.A.T. говорит:

    Muzhig, спасибо!
    Статья хорошо написана, картинки помогают воображению, мысль переведена на «человеческий среднестатистический» удачно.

    Суть понятна. Но что с этим делать и как теперь жить — нет…
    Можно ли вычислить изменение пространственных координат объекта, если есть данные от акселерометра, гиротахометра и магнетометра(на который особо рассчитывать не приходится в реальных условиях)?

    • muzhig говорит:

      Хм.. если под пространственными координатами вы имеете в виду вектор расстояния до нулевой точки, то, в общем, нет, но можно интегрировать ускорение (за вычетом гравитации) и получить приближенное положение. Если же вы имеете в виду ориентацию в пространстве, то, безусловно, из акселерометра/гироскопа/магнитометра можно получить кватернион ориентации, а из двух таких кватернионов- кватернион поворота. Тому, как это сделать я планирую когда нибудь посвятить отдельную статью, но, если вам интересно, то вкраце:
      У нас есть референсная система, заданная 2-мя векторами- вектором силы тяжести, и вектором магнитного поля. Например, считаем, что гравитация направлена «вниз» вдоль оси Z, а магнитная индукция- в плоскости Y(ось северного полюса) и Z. Кстати, магнитный вектор может быть (и скорее всего) не параллелен поверхности Земли, чем ближе к полюсу- тем острее угол и больше ошибка. Таким образом, вот вектора гравитации и индукции в референсной ориентации (они константные): [0, 0, 1] и [0, By, Bz] (By и Bz- проекции вектора на оси, зависят от угла в вашей местности, можно измерить один раз).
      Вторая система из двух векторов- это текущее показание акселерометра и магнетометра. Кватернион лично я вычисляю в два этапа:
      Наша цель- получить такой кватернион, чтобы, применив его к текущим измеренным векторам, мы получили вектора, совпадающие с референсными.
      Он является комбинацией двух кватернионов: первый приводит вектора к одной плоскости, второй доворачивает вектора в этой плоскости, чтобы они совпали.

      Математика:
      1) Обе пары векторов задают свои плоскости. Угол между плоскостями- это угол между их перпендикулярами, а ось- перпендикуляр к этим перпендикулярам. Перпендикуляр к двум векторам- их векторное произведение. Получаем кватернион по формуле (ось есть, угол есть). Применяем его к измеренным векторам, получаем промежуточные, в плоскости референсных.

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

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

      По части программной реализации, если хочется быстро и просто для понимания- есть комплиментарный фильтр, есть сразу кватернионная версия Madwick/Mahoney AHRS. Если вычислительная мощность и память позволяют и хочется взорвать мозг- добро пожаловать в мир фильтра Калмана (о котором я тоже планирую написать статью).

  3. Е. А. Т. говорит:

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

  4. Alex говорит:

    Здравствуйте!
    Делаю стабилизатор горизонта для авиамодели.
    Использую датчик — трёхосевой акселерометр гироскоп LSM330DLC.
    Данные датчика передаю в программу IMU S.O.H. Madgwick.
    Получаю кватернион q0,q1,q2,q3.
    Как из кватерниона получить два управляющих сигнала пропорциональных отклонению датчика от горизонта по крену и тангажу? Нашёл в книге авторов Бранец В.Н., Шмыглевский И.П. «Применение кватернионов в задачах ориентации твердого тела» об этом. Для меня сложно.

    • muzhig говорит:

      Ну, тут все довольно просто. Крен, тангаж и рысканье — это углы эйлера. По известным формулам кватернион переводим в углы эйлера и находим отклонение- разность между необходимыми значениями и показаниями сенсоров.
      Однако, нужно отдельно обрабатывать два частных случая- когда тангаж +90 и когда тангаж -90.

  5. Alex говорит:

    Ну да преобразую
    roll=atan2(2 * (q0*q1 + q2*q3), 1 — 2 *(q1*q1 + q2*q2));
    pitch=asin(2 * (q0*q2 — q1*q3));
    вижу неадекватное поведение при некоторых углах кратных 90 гр. Можно полностью алгоритм?

  6. Анатолий говорит:

    «Реальная и все мнимые единицы взаимно перпендикулярны». Это цитата из Вашей статьи. Действительная единица и есть действительная и является скаляром, она не обладает свойством единиц векторной части кватерниона. Посмотрите на векторное произведение кватернионов: 1 ведет себя как обычная действительная единица. А вот единицы векторной части действительно обладают дуальными свойствами: с одной стороны свойствами мнимой единицы, а с другой — ведут себя в произведениях как правая тройка единичных векторов.
    Есть еще несколько серьезных вопросов: при описании кватернионами суперпозиции вращений в каких системах координат описываются кватернионы?
    и т.д.
    В статье есть еще несколько некорректных с математической точки зрения высказываний.
    В целом статья нуждается в хорошей переделке или нужна новая статья без недостатков этой.
    Простите за прямоту. 🙂

    Скалярное произведение \mathbf{q_1}\cdot \mathbf{q_2}=w_1*w_2 + x_1*x_2 + y_1*y_2 + z_1*z_2 — если единицы мнимые, то почему стоят знаки + ?

    • muzhig говорит:

      Анатолий, спасибо большое за критику. Действительно, скорее всего имеют место некорректные с математической точки зрения формулировки, постараюсь исправить в меру своих познаний. Если Вы хотели бы помочь с исправлением, то напишите, пожалуйста, на почту mail@muzhig.ru
      Еще раз спасибо!

  7. Иван говорит:

    Мужик, статья отличная. Коротко и по делу.
    Только, кажется, в умножении кватернионов допущена ошибка. В третей компоненте последнее слагаемое Z1X1, а по логике вещей должно быть Z1X2

  8. Дмитрий говорит:

    В сферической линейной интерполяции потеряно деление на sin(Theta) для первого слагаемого.

  9. Дмитрий говорит:

    И для приближенной интерполяции формула немного иначе выглядит:
    q(t) = (1-t)q1 + tq2

  10. Александр говорит:

    Представьте себе полый шар с внутренней зеркальной поверхностью и диаметром А В. Из точки А в точку В вдоль диаметра шара отправляется частица со спином 1, отражается в точке В и возвращается в точку А. В то же время из точки В в точку А (т.е. навстречу первой частице) отправляется вторая частица со спином 1, отражается в точке А и возвращается в точку В. Вопрос: можно ли с помощью кватернионов доказать, что суммарный спин (спин шара) такой системы из двух частиц (каждая со спином 1) в целом равен спину 1/2 ?

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *