Представление матриц поворота через углы эйлера

Ликбез по кватернионам, часть 7: интегрирование угловых скоростей, углы Эйлера-Крылова February 27th, 2018

Интегрирование угловых скоростей

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

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

Задача такова: мы знаем ориентацию нашего изделия в начальный момент времени, и у нас есть датчики угловой скорости (ДУСы). Это могут быть старомодные механические датчики, основанные на гироскопах (именно их неправильно смонтировали на печально известном «Протоне»), либо микроэлектромеханические (MEMS) датчики, либо более точные волоконно-оптические, либо лазерные. Последние два упорно называют гироскопами, и действительно, свет там бежит по кругу, но это название всё равно не вполне корректно. Пользуясь показаниями этих датчиков, мы должны отслеживать, какой именно поворот совершило изделие, иными словами, отслеживать его ориентацию.

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

Первоначально самолёт летел с нулевым креном, тангажом и курсом. Затем он совершил поворот на 90 градусов по крену, после чего на 90 градусов по курсу. Как мы увидели ранее, после этих двух поворотов самолёт начал лететь вертикально вниз, то есть его тангаж стал равен -90°, хотя непосредственно по оси тангажа мы вообще не производили поворотов!

Кроме того, данная ориентация самолёта демонстрирует явление «складывания рамок» или «шарнирного замка». Согласно ГОСТ 20058-80 и аналогичным DIN 9300 и ISO 1151-2:1985, когда мы говорим, что самолёт имеет определенный курс, тангаж и крен, это значит: соответствующая ориентация в пространстве будет достигнута, если мы начнём с горизонтального положения на север, затем повернём самолёт по курсу, вслед за этим – по тангажу и, наконец, по крену (см. рисунок). Когда тангаж равен ±90° (самолёт "смотрит" вертикально вверх или вертикально вниз), курс и крен начинают работать одинаково (курс 0° и крен 90° дадут такое же положение, как и курс 90° и крен 0°, и ещё бесконечно много других комбинаций), что и называется складыванием рамок. Если мы примем, что в данной ориентации курс равен 90°, а крен – нулю (именно так рекомендуется разрешать неоднозначность), то сколько угодно малый поворот самолёта по курсу (в смысле, в сторону крыла, т.е при работе рулём направления) заставит скачкообразно изменить показания курса до 0°, крена – до 90°, а тангаж уменьшится на этот самый малый поворот. «Скачкообразно» означает бесконечную производную в этой точке – а это явно нехорошо…

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

Углы Крылова: рысканье, дифферент, крен. Рысканье – то же самое, что курс, дифферент – это морской термин для тангажа. Кажется, что знакомые нам курс-тангаж-крен – это и есть углы Крылова.

Не тут-то было.
Вот как определены углы Крылова:

(боролся с искушением прифотошопить сюда лебедя, рака и щуку, тянущих в трёх взаимно-перпендикулярных направлениях)

Приведём цитату из книги Бранца В.Н. и Шмыглевского И.П. - Применение кватернионов в задачах ориентации твердого тела (1973), стр. 79:

Первый поворот выполняется вокруг оси i 3 на угол курса φ, второй поворот происходит по оси i` 2 на угол крена ψ и третий – вокруг оси e 1 на угол тангажа ϑ.

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

Возьмём всё-таки «вырожденный» пример – самолёт полетел вверх ногами, при этом, чтобы не свалиться, немного задрал нос вверх. Когда мы опишем положение самолёта через углы Крылова, окажется, что самолёт летит с отрицательным тангажом, ведь первым осуществляется поворот по крену, а только потом – на перевёрнутом самолёте – поворот по тангажу, из-за чего он и должен поменять знак – именно в этом случае нос задерётся вверх.

Однако ГОСТ 20058-80 «ДИНАМИКА ЛЕТАТЕЛЬНЫХ АППАРАТОВ В АТМОСФЕРЕ» (http://docs.cntd.ru/document/gost-20058-80) даёт несколько иное определение тангажа:
26. Угол тангажа ϑ - угол между продольной осью OX и горизонтальной плоскостью OXgZg нормальной системы координат.

То есть, когда нос смотрит вверх, тангаж всегда должен получаться положительным, без разницы, как накренён самолёт!

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

Да и в целом кинематические уравнения для углов не очень радостны. Приведём их для углов Эйлера и для угловых скоростей, измеренных в связанном базисе (то есть, датчики стоят на объекте и поворачиваются вместе с ним):

Разумеется, для работы с углами тангажа, курса и крена, описанными в ГОСТ 20058-80, эти формулы не годятся – нужно вывести другие. Оставим это в качестве упражнения для самых упорных читателей.

В описании ориентации твёрдого тела в виде трёх углов есть определённые преимущества:
- оно наиболее компактно, требуя лишь 3 числа,
- более-менее понятно человеку,
- иногда позволяет найти аналитическое решение кинематических уравнений - для этого Эйлер свои углы и вводил когда-то.

Всё остальное – недостатки: многоэтажные формулы с множеством тригонометрических функций, появление особых точек, в которых нужно ставить свои «костыли» или заранее сдаться, сказав – сюда не заходите, иначе мы потеряемся в пространстве! Ещё мы можем заметить, что все углы способны неограниченно расти, поэтому неплохо бы каждый из них держать в разумных рамках, прибавляя или вычитая 2π, когда понадобится. Для тангажа и вовсе неплохо бы ограничиться -π .. π, что требует коррекции не только самого тангажа, но и курса. Практически любая работа с тремя углами сложна – поворот векторов, сравнение двух положений, композиция поворотов и пр. – везде мы натыкаемся на двухэтажные выражения и особые точки.

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

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

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

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

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

Первая из систем углов Эйлера обычно используется при описании движения гироскопов и соответствует следующей последовательности поворотов (рис. 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. Крен, тангаж, рысканье (третья система углов Эйлера)

Введение

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

Рис.1. Углы Эйлера - параметры, знакомые каждому, кто занимался компьютерной графикой и моделированием пространственного движения тел. И каждый, кому они знакомы, знает, насколько проблематичным бывает их использование.

Обычно углы поворота используют совместно с декартовой системой координат, при этом говорят, что связанная система координат может быть совмещена с базовой путем трех последовательных поворотов вокруг её осей. При этом каждый следующий поворот осуществляется вокруг оси, полученной после предыдущего поворота. Кроме того, следующий поворот не должен происходить вокруг оси, относительно которой совершен предыдущий поворот. В связи с этим существует 12 различных комбинаций углов поворота, самыми известными из которых являются углы Эйлера (рисунок 1). Базовую систему координат поворачивают на угол вокруг оси Z (угол прецессии), затем на угол вокруг оси X (угол нутации), и снова вокруг оси Z на угол (угол собственного вращения) до совмещения её со связанной системой координат.

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

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

Другой вариант углов поворота - самолетные углы: - рыскание, - тангаж и - крен (рисунок 3).

Данные параметры поворота вырождаются при тангажах , при этом неразличимы становятся крен и рыскание. Матерые симуляторщики знают, как сходит с ума КПП при выходе на крутые тангажи.

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

Однако, ещё великий Леонард Эйлер ввел в рассмотрение четыре параметра, которые не имеют вырождения. На его публикацию по этому поводу тогдашний научный мир особого внимания не обратил. Данная идея, независимо от Эйлера была развита Олидом Родригом, а в работах Уильяма Гамильтона получила окончательное теоретическое обоснование. Встречайте -

1. Кватернионы и действия над ними

Кватернионом называют число вида

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

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

А их попарные произведения хорошо описываются диаграммой

Смысл которой прост - если перемножать пары мнимых единиц в порядке, указанном стрелкой, то получается третья мнимая единица со знаком "+". Если порядок перемножения изменить на противоположный - получится третья мнимая единица со знаком "-". Не напоминает правило векторного перемножения ортов в декартовых координатах? Это оно и есть, то есть мы получаем

Пользуясь этими правилами перемножим два кватерниона

Ого! Не слабо, но мы смело приводим подобные слагаемые

И, ну наверняка вы видите тут до боли знакомые действия над векторами. Пусть у нас будут заданы векторы

Тогда каждый кватернион можно представить парой скаляр-вектор

А результат их умножения

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

А кватернионы вида и называются векторными кватернионами, и их произведение

Дает скалярное, со знаком минус, и векторное произведение составляющих их векторов.

Из-за наличия в результате произведения векторного умножения, операция умножения кватернионов не коммутативна

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

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

О связанной с ним операции вычисления нормы

И операции вычисления обратного кватерниона

И ещё одно полезное свойство, касающееся сопряжения произведения кватернионов

Кроме того, норма обратного кватерниона - величина обратная норме исходного

Эти операции имеют непосредственное отношение к тому, для чего в наши дни используют гиперкомплексные числа

2. Кватернион как линейный оператор поворота

Теперь посмотрим на вот такой фокус. Пусть - один кватернион, а - другой кватернион. Докажем небольшую теорему

Преобразование вида не меняет нормы кватерниона .

Проверяется это утверждение прямым вычислением

Действительно, норма кватерниона не изменяет при подобном преобразовании. А если кватернион будет векторным кватернионом, то не изменится норма вектора, которым он определяется. То есть описанное преобразование над вектором, не меняет его длины, оно будет ортогональным, или преобразованием поворота! Дело за малым - выяснить, вокруг какой оси и на какой угол происходит поворот, определяемый конкретным кватернионом. Для этого возьмем (нам ведь никто не мешает так сделать) и представим кватернион в виде

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

Никто не мешает делать нам эквивалентных преобразований, вот мы и делаем их. Теперь введем замену

На каком основании? Да на том, что сумма квадратов этих величин всегда даст единицу, а если это так, то никто не мешает представить данные величины как синус и косинус некоторого угла. Почему угол делим на два? Нам так хочется, потом это нам пригодится, ведь угол можно взять произвольный. Исходя из введенной замены мы можем переписать кватернион в виде

Заметим, что вектор , введенный нами, является единичным, так как

В довершении позволим себе ещё одно допущение - пусть кватернион будет единичным, то есть

Теперь аккуратно выполним ортогональное преобразование над векторным кватернионом

Теперь умножим результат (2) на обратный кватернион

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

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

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

Неподвижная система отсчета, в которой рассматривается угловое положение твердого тела (летательного аппарата), образована правой тройкой векторов. Ось направляется по местной вертикали от центра Земли, ось располагается в плоскости горизонта и направляется на географический север (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°.

Матрица поворот применяется для вращения системы координат или объекта, сцены.

Матрицы поворота вокруг основных осей.

Матрица поворота вокруг произвольной оси.

Обобщённая матрица поворота.

Хочется задавать положение объекта в пространстве однозначно. Достаточно очевидно что любое положение однозначно определяется 3 поворотами вокруг разных осей. Но встаёт вопрос в каком порядке вращать и как выбрать оси?

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

Можно выделить 2 популярные схемы.
1) Матрица поворота через углы Эйлера.
2) Матрица поворота через углы летательного аппарата (ЛА): рыскание, тангаж и крен(yaw, pitch и roll).
В виду того что первая требует большого числа вычислений, то на практике обычно применяют вторую.

Матрица поворота через углы Эйлера.

Углы Эйлера - три угла однозначно определяющие ориентацию твёрдого тела, определяющие переход от неподвижной системы координат к подвижной.
Подвижная система координат это система координат привязанная к телу. Иногда говорят в мороженная в тело. Прежде чем дать определения углов нам понадобиться ещё одно. Линия узлов ON - линия пересечение плоскости OXY и Oxy

α (или φ) это угол между осью Оx и осью ON. Диапазон значений }