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

Формулы для пересчёта углов Эйлера в кватеринионы и обратно найти можно, но

Опишу коротко суть проблемы:

1. Тело в трёхмерном пространстве имеет 6 степеней свободы: 3 координаты и 3 угла поворота.
2. С координатами всё хорошо, например, если они (4,5,2), то это означает, что тело нужно сдвинуть относительно начала координат на +4 единицы по оси X, на +5 единиц по оси Y и на +2 единицы по оси Z. При этом порядок сдвига не важен. Можно сначала сдвинуть по X, потом по Y, потом по Z, а можно в другой последовательности. От перемены мест слагаемых сумма не меняется.
3. С поворотами всё гораздо хуже. Иногда может сложиться ощущение, что для них тоже просто достаточно задать углы поворота вокруг трёх осей и этого будет достаточно (например: перевернуть предмет на 180 градусов вокруг оси X, потом на 180 градусов вокруг оси Y, а затем на 90 градусов вокруг оси Z - в каком порядке не поворачивай - результат будет один и тот же). Эта ловушка возникает оттого, что нам легче всего оперировать углами типа 90 или 180 градусов, а они-то как раз и представляют из себя очень частный случай. В общем случае порядок поворотов имеет значение.

А как же быть с законом, говорящим, что от перестановки мест слагаемых сумма не меняется? Дело в том, что композиция нескольких поворотов соответствует уже не сумме векторов (как в случае с операциями параллельного переноса), а произведению. И произведению не просто чисел, а специальных объектов - матриц поворота, например - на которые коммутативность «обычного» умножения не распространяется. В зависимости от порядка выбора осей поворота и от того, будут ли поворачиваться оси вместе с объектом или поворачиваться будет только объект, можно выделить 24 типа описаний поворотов. Очень часто углы поворота вокруг осей называются углами Эйлера. Иногда, в некоторых источниках эти углы называются углами Тэйт-Брайана либо углами Эйлера в зависимости от того, все три оси, вокруг которых делается вращение разные (углы Тэйт-Брайана), либо же первая и последняя оси - одна и та же. Также эти углы называют angles of extrinsic rotation - если оси неподвижны или angles of intrinsic rotation - если оси вращаются вместе с объектом.
Чтоб не запутаться, приведу все типы вращений здесь:
Тейт-Брайана, внутренние:
ZYXr; YZXr; XZYr; ZXYr; YXZr; XYZr.
Эйлера, внутренние:
XYXr; XZXr; YZYr; YXYr; ZXZr; ZYZr.
Тейт-Брайана, внешние:
ZYXs; YZXs; XZYs; ZXYs; YXZs; XYZs.
Эйлера, внешние:
XYXs; XZXs; YZYs; YXYs; ZXZs; ZYZs.
Внешние углы комплементарны внутренним, прочитанным задом наперёд, например: внешние углы Эйлера 10, 20, 30 градусов в формате XYXs это то же самое, что и внутренние углы Эйлера 30, 20, 10 градусов в формате XYXr.

Собственно, об этом уже было сказано много раз. Зачем же писать новую статью? Дело в том, что информации о том, как переводить из углов Эйлера в кватернион и обратно - не так уж и много. И в большинстве случаев описывается только 1 или 2, 3, 6 систем углов Эйлера. Но не все 24. И по аналогии вывести остальные (и не ошибиться) не очень-то и просто. Во время «откапывания истины» мне удалось найти несколько онлайн-конвертеров из углов в кватернионы и по тому, в каком направлении увеличивается их возможность по конвертации можно понять, сколько ещё вариантов осталось не охвачено:
quat.zachbennett.com - один тип углов

onlineconversion.com - один тип углов
quaternions.online - три типа углов
andre-gaschler.com - шесть типов углов

Единственное место, где я смог найти описание преобразований для всех 24 типов углов - это книга «Graphics Gems IV». Репозитарий с исходниками от этой книги находится здесь: Исходники к книге Graphics Gems IV . Если говорить про код преобразования из углов Эйлера в кватернионы и обратно, то эти исходники в репозитарии находятся здесь: .../GraphicsGems/gemsiv/euler_angle. Но у них есть один недостаток: с целью сделать максимально общую функцию расчёта углов и кватернионов, автор очень сильно усложнил код. Т.е. код получился очень компактным, но плохо подходящим для перевода на другие языки или для оптимизации под конкретные случаи. Так как мне очень нужно было разобраться со всеми 24-мя случаями, то пришлось этот код немного поисследовать и развернуть его в набор простых случаев. Также я написал небольшие юнит-тесты и проверил, что мой код работает корректно. Т.к. эти юнит-тесты используют код, скомпилированный из исходников от книги Graphics Gems, то выкладывать их (юнит-тесты) я не стал.

Не буду приводить в тексте статьи свои исходники (они написаны на языке Octave). Дам лишь ссылку на репозитарий и прокомментирую его содержимое:

Обеих функций в Octave нет. В Matlab поддерживаются только 6 типов углов Эйлера на неподвижных осях. В моих реализациях поддерживаются все 24 типа. При этом типы с буквой r на конце (например, XYZr) означают, что оси вращаются вместе с объектом. Типы с буквой s на конце (например, XYZs) означают, что оси остаются неподвижными.

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

С некоторой натяжкой именно так работает вращение с помощью углов Эйлера. Немного непредсказуемо и неудобно, не правда ли? Углы Эйлера имеют несколько недостатков, но есть одно особенно нехорошее свойство из-за которого вы не захотите с ними связываться. Его имя - Gimbal lock.

В русском языке gimbal lock называют по-разному: шарнирный замок, блокировка осей, складывание рамок. К сожалению, по запросам в поисковике с такими ключевыми словами выдаётся много мусора, а статья в Википедии оставляет желать лучшего, поэтому я сам расскажу вам об этом феномене и предложу как с ним бороться.

Внимание! Заходя под кат вы подвергаетесь риску поломать голову.

Для начала напомню что такое углы Эйлера. Вы наверное помните, что это что-то вроде набора из трёх углов вращения вокруг осей X, Y и Z? Не совсем так. Предположим, вы хотите повернуть некий объект, и у вас есть набор конечных углов (X: 45°, Y: 45°, Z: 45°). Один из подвохов эйлеровых углов - необходимость выбора какого-то одного порядка поворотов. Если сначала повернуть на 45° вокруг оси X, затем вокруг Y и в конце вокруг Z, то получится результат как на левой половине картинки снизу. Если порядок будет Z-X-Y, то результат будет другой, как на правой половинке.

На самом деле…

На самом деле выше описаны не просто углы Эйлера, а углы Тэйта - Брайана . Эйлеровы углы имеют много сбивающих с толку вариаций, в одних из которых нужно вращать вокруг глобальных осей, в других оси поворачиваются после каждого шага, в третьих оси всегда закреплены на самом объекте и двигаются вместе с ним. Ко всему прочему добавляется разный порядок поворотов. Если есть возможность - не пользуйтесь углами Эйлера.

От выбора порядка поворотов зависит место появления шарнирного замка. Что же это такое? Возьмём к примеру такой порядок поворотов: Z-X-Y. Если вращение вокруг оси X будет равно 90° или -90°, то вращения вокруг Z и Y будут «есть» друг друга и останется только огрызок от большего из вращений. Например (X: 90°, Y: 90°, Z: 90°) превратится в просто (X: 90°, Y: 0°, Z: 0°). Внимание на иллюстрацию.

Так же можно подставить (X: 90°, Y: 130°, Z: 140°) или (X: 90°, Y: 30°, Z: 40°), но в результате всё равно будет получаться (X: 90°, Y: 0°, Z: 10°). Немного не интуитивно, вам не кажется? Это всё из-за шарнирного замка. Когда вращение вокруг оси X становится равным 90° или -90°, ещё не использованная локальная ось вращения Y становится параллельной оси Z, но с обратным направлением, поэтому вращение вокруг неё вступает в конфликт с предыдущим вращением вокруг Z.

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

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

Шарнирный замок появляется в середине иерархии поворотов. Если использовать порядок X-Y-Z или Z-Y-X, то поворот направо или налево будет заклинивать анимацию. Поскольку такой поворот встречается гораздо чаще чем, например, поворот в сторону зенита или надира, то во многих программах используют последовательность Z-X-Y. Такая иерархия поворотов используется в Unity3d, правда внутри все вращения всё равно хранятся в кватернионах. Что такое кватернионы? Об этом лучше рассказать отдельно. Кватернионы и матрицы вращения это один из способов избежать шарнирного замка. Также существуют хитрые алгоритмы, которые плавно обходят замок стороной, но это отражается на качестве анимации. Лучше всего использовать углы Эйлера только для простых случаев: пропеллеры, колёса, маятники. Иногда можно поменять иерархию поворотов, но тогда всё равно придётся помнить о замке.

Кинематические уравнения в обобщенных координатах. Углы Эйлера, Крылова, кватернионы.

В курсе теоретической механики сферическое движение задавалось углами Эйлера (рис. 1.2) – углом прецессии y (поворот вокруг неподвижной оси Oz ), углом нутации q (поворот вокруг полуподвижной оси ОК – линии пересечения плоскостей Oxy и O ξη, называемой линией узлов) и углом собственного вращения j (поворот вокруг связанной с телом оси Oz ).

Рис. 1.2. Система ориентационных углов Эйлера твердого тела

Углы Эйлера перечислены здесь в порядке поворотов, которые надо совершить над неподвижной СК Oxyz чтобы она совместилась с подвижной СК O ξηζ. Использование углов Эйлера в сферическом движении делалось для демонстрации принципиальной возможности решения соответствующих задач кинематики. Здесь же у нас стоит задача более оптимально описать такое движение. Кинематические соотношения, выражающие проекции угловой скорости тела на оси связанной СК через угловые скорости указанных углов представляются для углов Эйлера формулами (сверены с программой КИДИМ):

(1.1)

Несмотря на лаконичность такого задания положения тела при сферическом движении (3 степени свободы, 3 координаты), в современной механике оно используется редко. Это объясняется, в частности тем, что формулы вычисления обобщенных скоростей через проекции угловой скорости тела (обратные кинематические соотношения) содержат особенности и несимметричны, что затрудняет анализ результатов и приводит к вычислительным погрешностям. Для углов Эйлера эти соотношения представляются формулами:

(1.2)

Более предпочтительным является использование параметров Родрига-Гамильтона, кватернионов, параметров Кейли-Клейна.

Докажем теорему д’Аламбера-Эйлера .

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

Движение тела полностью определяется движением любого треугольника, принадлежащего телу. Поэтому для сферического движения это эквивалентно движению двух точек на некоторой сфере, центр которой совпадает с неподвижной точкой, или движению дуги, соединяющей эти точки. Предположим, что в результате перемещения тела за время Dt некоторая точка А переместилась по сфере в положение В (рис. 1.3). В то же самое время точка, которая находилась в положении В , заняла новое положение С .

Рис. 1.3.

Плоскость ABC пересекает неподвижную сферу по окружности (малого или большого круга). Если D один из полюсов этого круга на сфере, то , т.к. они равнобедренные сферические и , так как они являются двумя положениями одной и той же дуги сферы АВ . по построению (равноудалены от полюса). Следовательно, может быть совмещена с при помощи вращения вокруг оси OD на угол ADB . Теорема доказана.

Параметры Родрига-Гамильтона . Для задания такого поворота, который будем называть конечным поворотом тела , очевидно, надо задать положение оси, направление и угол поворота. Ось поворота можно задать единичным вектором, направленным в ту сторону, откуда поворот тела будет наблюдаться против часовой стрелки. Этот вектор определится своими проекциями на оси некоторой СК (направляющими косинусами его углов с осями этой СК). Таким образом, конечный поворот определится четырьмя скалярными величинами ‑ проекциями единичного вектора оси и величиной угла самого поворота вокруг этой оси.

Воспользуемся для задания таких четырех величин параметрами Родрига-Гамильтона, которые обозначим здесь λ 0 , λ 1 , λ 2 , λ 3 . Последние три параметра обычно объединяют в вектор ={λ 1 , λ 2 , λ 3 } T . Таким образом, будем рассматривать совокупность скалярной и векторной величин λ 0 , . Эти параметры вводятся через элементы конечного поворота и могут быть определены следующим образом. Пусть ‑ направляющий орт оси, вокруг которой поворот совершается, а ψ ‑ величина угла поворота. Тогда

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

В качестве обобщённых координат можно использовать углы Эйлера j , q и y .

Таблица 3.1. Три системы углов Эйлера

Последова-тельность поворотов	На j вокруг оси OZ	На j вокруг оси OZ	На y вокруг оси OX
	На q вокруг оси OU	На q вокруг оси OV	На q вокруг оси OY
	На y вокруг оси OW	На y вокруг оси OW	На j вокруг оси OZ

Первая из систем углов Эйлера обычно используется при описании движения гироскопов и соответствует следующей последовательности поворотов (рис. 3.2):

Рисунок 3.2. Первая система углов Эйлера

R j , q , y = R z , j ×R u , q ×R w , y =
=

=
. (3-2)

R j , q , y , может быть также получен в результате выполнения последовательности следующих поворотов вокруг осей неподвижной системы координат: сначала на угол y вокруг оси OZ , затем на угол q вокруг оси OX , затем на угол j вокруг оси OZ .

На рисунке 3.3 показана вторая система углов Эйлера, определяемая следующей последовательностью поворотов:

Поворот на угол j вокруг оси OZ (R z , j).

Поворот на угол q вокруг оси OV (R v , q).

Поворот на угол y вокруг повёрнутой оси OW (R w , y).

Результирующая матрица поворота имеет следующий вид:

R j , q , y = R z , j ×R v , q ×R w , y =
=

=
. (3-3)

Поворот, описываемый матрицей R j , q , y для этой системы углов Эйлера, может быть получен также в результате выполнения последовательных поворотов: на угол y вокруг оси OZ , на угол q вокруг оси OY , на угол j вокруг оси OZ .

Рисунок 3.3. Вторая система углов Эйлера

Ещё одну систему углов Эйлера составляют так называемые углы крена , тангажа и рыскания . Эти углы обычно применяются в авиации для описания движения самолётов.

Они соответствуют следующей последовательности поворотов:

Поворот на угол y вокруг оси OX (R x , y ) – рыскание.

Поворот на угол q вокруг оси OY (R y , q ) – тангаж.

Поворот на угол j вокруг оси OZ (R z , j ) – крен.

Результирующая матрица поворота имеет вид:

R j , q , y = R z , j ×R y , q ×R x , y =
=

=
. (3-4)

Поворот, описываемый матрицей R j , q , y в переменных «крен, тангаж, рыскание» может быть также получен в результате выполнения следующей последовательности поворотов вокруг осей абсолютной и подвижной систем координат: на угол j вокруг оси OZ , затем на угол q вокруг повёрнутой оси OV , на угол y вокруг повёрнутой оси OU (продольная ось аппарата – Z ) (рис. 3.4).

Рисунок 3.4. Крен, тангаж, рысканье (третья система углов Эйлера)

Углы Эйлера-Крылова

Однозначно задать угловое положение твердого тела в пространстве позволяют три угла Эйлера-Крылова и, отсчитываемые против часовой стрелки. На рисунке показана одна из разновидностей углов Крылова - так называемые самолетные углы, используемые в авиации

Углы Эйлера-Крылова

Неподвижная система отсчета, в которой рассматривается угловое положение твердого тела (летательного аппарата), образована правой тройкой векторов. Ось направляется по местной вертикали от центра Земли, ось располагается в плоскости горизонта и направляется на географический север (N, North), а ось дополняет систему координат до правой. С подвижным объектом - например, летательным аппаратом (ЛА), - жестко связана подвижная система координат. Её ось направляется вдоль строительной (продольной) оси летательного аппарата, ось - вдоль нормальной в направлении зенита, а ось - вдоль поперечной в направлении правого борта ЛА. Угловое положение (ориентация) ЛА в системе координат задается курсом (), тангажом () и креном (). Наличие знака минус перед самолетными углами обусловлено тем, что их положительные значения, в отличие от классических углов Эйлера-Крылова, отсчитывается по ходу часовой стрелки. Итоговое положение ЛА определяется последовательностью поворотов

Выставка по сигналам MEMS акселерометра

Процедура определения начальных угловых координат называется выставкой. Для выставки крена и тангажа с помощью трёхосного MEMS акселерометра, выдающего ускорения, и по осям X, Y и Z связанной с ним подвижной системы координат OXYZ, соответствующие значения углов можно найти по проекциям вектора ускорения свободного падения g=9,81 м/с2 на каждую из осей, используя математический аппарат матриц поворота (3.1)

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

Выразим из (3.2) вектор ускорения свободного падения, для чего умножим обе части равенства слева на матрицу:

Из первых двух уравнений системы (3.4) получим

Калибровка MEMS акселерометра

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

Ошибки показаний трёхосного акселерометра (ТОА) возникают из-за трех факторов :

Наличие постоянного смещения;

«просачивание» сигнала из одного канала в другой, вызванное неколлинеарностью троек векторов, образующих две системы координат: связанную с калибровочной поворотной платформой OXYZ и связанную с ТОА (3.4);

Собственные фликкер-шумы.

Неколлинеарность осей систем координат объекта и систем координат акселерометра

Из этого следует, что математическая модель сигнала трёхосного MEMS акселерометра будет выглядеть следующим образом :

где - вектор показаний акселерометра, - диагональная матрица масштабных коэффициентов, - матрица коррекции, - проекции вектора ускорения свободного падения на оси правой тройки векторов системы координат, связанной с акселерометром, - вектор постоянных смещений, - вектор собственных шумов ТОА.

Без учета шума систему уравнений (3.6), выполнив операции умножения матриц и векторов, можно записать в виде:

Из (3.7) следует, что для нахождения калибровочных параметров для одной из осей требуется количество измерений, равное количеству неизвестных параметров этой оси: для оси Z - 2, для оси Y-3, для оси X-4.

Калибровка трёхосного MEMS акселерометра подразумевает установку датчика в априорно известные положения и решение переопределенной системы уравнений для его выходных сигналов. При выполнении данной процедуры принято устанавливать акселерометр в 12 фиксированных положений

12 калибровочных положений MEMS акселерометра

В показано, что для уменьшения погрешности оценивания следует выполнять усреднение калибровочных коэффициентов, найденных по числу сочетаний. Однако, с целью уменьшения времени калибровки можно использовать только шесть так называемых ортогональных положений: 2), 4), 6), 7), 8) и 11); в этом случае уменьшение числа сочетаний до приводит к увеличению погрешности измерения элементов матрицы масштабных коэффициентов k и элементов вектора смещений b не более чем на 0,21% и 0,02% соответственно. Следует отметить, что погрешность измерения элементов матрицы коррекции T может возрастать до сотен процентов, но, так как недиагональные элементы T обычно не превосходят, при малых углах крена и тангажа (не более 30°), погрешность измерения указанных углов увеличивается не более чем на 0,5°.