nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Ликбез по кватернионам, часть 8: интегрирование угловых скоростей, матрицы поворота

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

Продолжаем нашу предвыборную гонку - какой интегратор угловых скоростей займёт своё законное место у руля (в буквальном смысле) нашего изделия?

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

Сегодня мы рассмотрим матрицы поворота - 9 направляющих косинусов не могут ошибаться, не правда ли?


Кинематическое уравнение для связанных осей (когда угловая скорость проецируется на оси изделия) принимает вид:
Part8_kinematic_eq.png

Здесь A – матрица ориентации.
Для шага Δt мы можем записать приближенный метод интегрирования:
Part8_kinematic_first_order.png

Этот метод – сугубо линейный, в нём применяются только сложения и умножения, особые точки отсутствуют как класс.
Один недостаток, который мы можем заметить «сходу» - это громоздкость: мы используем 9 чисел для представления матриц, а каждый шаг интегрирования по простейшему методу («первого порядка») требует 18 умножений и 18 сложений (без специализированного метода, «знающего» о единицах по главной диагонали – и вовсе 27 умножений). Если выписать по компонентам, мы получим (с верхним индексом 1 – новые значения, с верхним индексом 0 – старые):

Part8_kinematic_components.png

Матричная запись позволяет скрыть эту внутреннюю сложность за красивыми выкладками, поэтому полезно иногда выписать вычисления «в лоб», чтобы помнить, с чем мы имеем дело.
Тем не менее, даже у самых старых бортовых вычислителей не возникло бы проблем, связанных с недостаточной производительностью – ну что такое 36 операций для компьютера!?

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

Part8_unity_matrix.png

Далее мы начнём вращение изделия вокруг оси Z, на каждом шагу поворачиваясь на 5 градусов, чуть менее 1/10 радианы. Матрица конечного поворота WΔt будет равна:

Part8_after360turn.png

После 72 шагов придём к матрице ориентации, равной:
Part8_after360turn_for_real.png

тогда как должны были прийти к единичной матрице (поворот на 360 градусов!).
Можем записать эту матрицу, как произведение двух:
Part8_factorization.png

Вторая из них – матрица поворота по оси Z на угол 0,9° - такова накопленная ошибка интегрирования, вызванная слишком крупным шагом. В относительном выражении ошибка не так велика: 0,9/360 = 0,25%, что не так уж плохо, учитывая, сколь крупный шаг мы взяли.
А вот первая матрица – масштабирование по осям X, Y. Вектор с нулевой компонентой Z просто увеличит свою длину – зачастую это не так уж страшно – по крайней мере, это не изменит направление вектора. Точно так же, вектор (0;0;1)T останется без изменений – всё верно.
Самое интересное будет происходить с промежуточными векторами.
К примеру, вектор
Part8_45degVector.png

превратится в
Part8_not45degAnyMore.png

он не только увеличивается в размерах, но и меняет направление из-за масштабирования! Раньше он «смотрел» под углом 45° к плоскости X-Y, а теперь – под углом 37° - ошибка составляет уже не 0,9°, а целых 8°!

В данном конкретном случае мы легко смогли факторизовать матрицу, вычленив из неё отдельно поворот и отдельно масштабирование. Когда мы можем это сделать – понятно, как исправить ситуацию – нужно оставить только матрицу поворота, а масштабирование убрать!
Но представим теперь, что после поворотов вокруг оси Z, мы осуществили ещё и поворот вокруг приборной оси X на 30 градусов:

Part8_another30degTurn.png

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

Part8_constraints.png

Действительно: при 9 коэффициентах матрицы, мы должны иметь ровно 3 степени свободы, поэтому нам и нужны дополнительные 6 уравнений. Проверим, что происходит у нас:
Длина 1-го базисного вектора: 1,314
Длина 2-го базисного вектора: 1,243
Длина 3-го базисного вектора: 1,087
Угол между 1 и 2: 90°
Угол между 2 и 3: 103,47°
Угол между 1 и 3: 90°

Ортонормированный базис перестал быть таковым! Мы понимаем это, но как именно скорректировать 9 значений, чтобы он снова стал ортонормированным?

Можно применить старый добрый метод построения ортонормированного базиса. Исходные базисные векторы обозначим e1, e2, e3:

Part8_e1e2e3_basis.png

Преобразованный базис назовём n1, n2, n3.
Первый вектор мы нормируем:

Part8_firstVecNormalization.png

Из второго вектора мы вычитаем его проекцию на первый, после чего тоже нормируем:
Part8_secondVecNormalization.png

Наконец, из третьего вектора вычитаем его проекцию на первый и второй, после чего нормируем:

Part8_thirdVecNormalization.png

В этих формулах есть определённое лукавство: кажется, что мы задействовали все 9 исходных коэффициентов, чтобы получить новые 9, теперь уже ортонормированные. На самом деле, от e3 вообще ничего не зависит! Сначала мы вычитаем из него всё лишнее, так, чтобы он шел по прямой, взаимно перпендикулярной n1, n2, а затем нормируем его длину – да от этого вектора живого места не остаётся! Мы с тем же успехом можем записать:

Part8_VecMultiply.png

и получить абсолютно тот же самый результат! То есть, в действительности данный метод берёт первые 6 коэффициентов и напрочь выкидывает последние 3. Да и первые 6 оказываются неравнозначны: если первому столбцу мы доверяем «безоговорочно», то второму – только до тех пор, пока он не противоречит первому.

Попробуем процедуру нормировки на нашей матрице B:
Part8_GoodNormalization.png

При этом, как и следовало ожидать, n3 получился одинаковым до последнего представимого знака после запятой, независимо от способа вычисления – через e3 или через векторное произведение.

Сейчас нам повезло – мы идеально отнормировали матрицу, так что она выражает ровно то, что и должна – поворот на 0,9° вокруг оси Z и поворот на 30° вокруг оси X.
Но попробуем теперь зайти с обратной стороны – не e1, e2, e3, а e3, e2, e1. Получится вот что:

Part8_BadNormalization.png

Вместо поворота на 30° по оси X, мы в этот раз получили поворот на 37°, реализовав фактически наихудший случай!
Правильным подходом было бы решение оптимизационной задачи: каждый коэффициент матрицы является суммой полезного сигнала и шума. Найти новые коэффициенты, выраженные через старые, таким образом, чтобы среднеквадратичная ошибка стремилась к минимуму. Но даже тогда мы не гарантируем наилучшей работы – кто сказал, что, используя приближенные матрицы конечного поворота, мы вносим именно случайную ошибку!?
Уточним, в чём наша проблема. Нам не хотелось выписывать точную матрицу конечного поворота, поскольку она выглядит примерно так:

Part8_AwfulMatrix.png

(меняя порядок поворотов, будем получать разные матрицы W, но все они будут являться строго матрицами поворота)
Мы «из жадности» её упростили:

Part8_simple_matrix.png

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

Part8_more_general_factorization.png

Разложив в ряд Тейлора до 3-го порядка малости, имеем:
Part8_Factorization_Taylor.png

Оказывается, что ошибка в интегрировании угловой скорости (правая часть) имеет третий порядок малости (φ3/2 вместо φ3/6), тогда как «паразитное масштабирование» имеет второй порядок малости – именно этот эффект мы обнаружим в первую очередь.
Кажется, что ситуация поправима – нужно использовать более точные аппроксимации и уменьшать шаг интегрирования – тогда матрица будет «портиться» медленнее. Но будет портиться всё равно, теперь уже из-за ошибок плавающей арифметики.
К примеру, теперь мы, наученные горьким опытом, интегрируем очень малыми шагами, разбив полную окружность на 31416 шагов, каждый по 200 мкрад, или 41 угловые секунды. Если все вычисления производятся с одинарной точностью (32 бита, с плавающей точкой), то cos(41’’) округлится до единицы, и после 31416 шагов мы придём к следующей матрице:

Part8_single_precision.png

И снова мы наблюдаем похожий эффект: ошибка интегрирования на этот раз чрезвычайно мала, составляя менее угловой секунды, тогда как наиболее заметное искажение снова проявилось в масштабе. Кажется, что ошибка весьма мала – менее одной тысячной – но этого достаточно, чтобы вектор, направленный под углом 45° к плоскости XY, «прижался бы» к ней дополнительно на 1 угловую минуту.

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

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

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

Продолжение следует...

Poll #2078927 Кого поставить у руля космического аппарата?

Что вы выберите для интегрирования угловых скоростей

Углы Эйлера/Крылова
0(0.0%)
Матрицы поворота
0(0.0%)
Кватернионы / гиперкомплексные числа
4(57.1%)
Я человек простой: просуммирую по каждой оси, придумают тут хрень всякую!
2(28.6%)
Против всех: это нарушение свободы космического аппарата!
1(14.3%)
Другое (напишите в комментах)
0(0.0%)
Tags: кватернионы-это просто (том 1), математика, моделирование, работа
Subscribe

  • Великая Октябрьская резня бензопилой

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

  • Очередная несуразность в единицах измерения

    Когда-то я написал программу PhysUnitCalc - калькулятор, умеющий работать с размерностями. Мне казалось, что я наступил уже на все грабли, которые…

  • Big Data, чтоб их... (3)

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

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 1 comment