Ликбез по кватернионам, часть 8: интегрирование угловых скоростей, матрицы поворота
Оглавление "Ликбеза по кватернионам":
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Интегрирование угловых скоростей с помощью матриц поворота
Продолжаем нашу предвыборную гонку - какой интегратор угловых скоростей займёт своё законное место у руля (в буквальном смысле) нашего изделия?
Мы уже накопали компромата на углы Эйлера-Крылова - это конечно уважаемые и достойные фамилии, но очень уж старенькие - задрать голову в зенит не могут, сразу начинает голова кружиться, да и подвешенные вниз головой резко теряют работоспособность. Да и вообще, уголовники (системы, основанные на углах) нам не нужны!
Сегодня мы рассмотрим матрицы поворота - 9 направляющих косинусов не могут ошибаться, не правда ли?
Кинематическое уравнение для связанных осей (когда угловая скорость проецируется на оси изделия) принимает вид:
Здесь A - матрица ориентации.
Для шага Δt мы можем записать приближенный метод интегрирования:
Этот метод - сугубо линейный, в нём применяются только сложения и умножения, особые точки отсутствуют как класс.
Один недостаток, который мы можем заметить «сходу» - это громоздкость: мы используем 9 чисел для представления матриц, а каждый шаг интегрирования по простейшему методу («первого порядка») требует 18 умножений и 18 сложений (без специализированного метода, «знающего» о единицах по главной диагонали - и вовсе 27 умножений). Если выписать по компонентам, мы получим (с верхним индексом 1 - новые значения, с верхним индексом 0 - старые):
Матричная запись позволяет скрыть эту внутреннюю сложность за красивыми выкладками, поэтому полезно иногда выписать вычисления «в лоб», чтобы помнить, с чем мы имеем дело.
Тем не менее, даже у самых старых бортовых вычислителей не возникло бы проблем, связанных с недостаточной производительностью - ну что такое 36 операций для компьютера!?
Нет, настоящая ахиллесова пята матриц поворота состоит в том, что по прошествии времени они перестают быть матрицами поворота, а вернуть их на путь истинный не так-то просто…
Пусть начальная матрица ориентации - единичная, то есть оси инерциальной и связанной систем координат совпадают:
Далее мы начнём вращение изделия вокруг оси Z, на каждом шагу поворачиваясь на 5 градусов, чуть менее 1/10 радианы. Матрица конечного поворота WΔt будет равна:
После 72 шагов придём к матрице ориентации, равной:
тогда как должны были прийти к единичной матрице (поворот на 360 градусов!).
Можем записать эту матрицу, как произведение двух:
Вторая из них - матрица поворота по оси Z на угол 0,9° - такова накопленная ошибка интегрирования, вызванная слишком крупным шагом. В относительном выражении ошибка не так велика: 0,9/360 = 0,25%, что не так уж плохо, учитывая, сколь крупный шаг мы взяли.
А вот первая матрица - масштабирование по осям X, Y. Вектор с нулевой компонентой Z просто увеличит свою длину - зачастую это не так уж страшно - по крайней мере, это не изменит направление вектора. Точно так же, вектор (0;0;1)T останется без изменений - всё верно.
Самое интересное будет происходить с промежуточными векторами.
К примеру, вектор
превратится в
он не только увеличивается в размерах, но и меняет направление из-за масштабирования! Раньше он «смотрел» под углом 45° к плоскости X-Y, а теперь - под углом 37° - ошибка составляет уже не 0,9°, а целых 8°!
В данном конкретном случае мы легко смогли факторизовать матрицу, вычленив из неё отдельно поворот и отдельно масштабирование. Когда мы можем это сделать - понятно, как исправить ситуацию - нужно оставить только матрицу поворота, а масштабирование убрать!
Но представим теперь, что после поворотов вокруг оси Z, мы осуществили ещё и поворот вокруг приборной оси X на 30 градусов:
Как из этого нагромождения чисел наиболее оптимально вычленить отдельно повороты, избавившись от других преобразований пространства - вопрос по-прежнему открытый…
Напомним, что столбцы матрицы ориентации - это координаты базисных векторов в инерциальной системе отсчета. Эти векторы должны иметь единичную длину и быть взаимно-перпендикулярными, то есть мы можем записать следующие «уравнения связи» (в данном случае двойка наверху - это возведение в степень):
Действительно: при 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:
Преобразованный базис назовём n1, n2, n3.
Первый вектор мы нормируем:
Из второго вектора мы вычитаем его проекцию на первый, после чего тоже нормируем:
Наконец, из третьего вектора вычитаем его проекцию на первый и второй, после чего нормируем:
В этих формулах есть определённое лукавство: кажется, что мы задействовали все 9 исходных коэффициентов, чтобы получить новые 9, теперь уже ортонормированные. На самом деле, от e3 вообще ничего не зависит! Сначала мы вычитаем из него всё лишнее, так, чтобы он шел по прямой, взаимно перпендикулярной n1, n2, а затем нормируем его длину - да от этого вектора живого места не остаётся! Мы с тем же успехом можем записать:
и получить абсолютно тот же самый результат! То есть, в действительности данный метод берёт первые 6 коэффициентов и напрочь выкидывает последние 3. Да и первые 6 оказываются неравнозначны: если первому столбцу мы доверяем «безоговорочно», то второму - только до тех пор, пока он не противоречит первому.
Попробуем процедуру нормировки на нашей матрице B:
При этом, как и следовало ожидать, n3 получился одинаковым до последнего представимого знака после запятой, независимо от способа вычисления - через e3 или через векторное произведение.
Сейчас нам повезло - мы идеально отнормировали матрицу, так что она выражает ровно то, что и должна - поворот на 0,9° вокруг оси Z и поворот на 30° вокруг оси X.
Но попробуем теперь зайти с обратной стороны - не e1, e2, e3, а e3, e2, e1. Получится вот что:
Вместо поворота на 30° по оси X, мы в этот раз получили поворот на 37°, реализовав фактически наихудший случай!
Правильным подходом было бы решение оптимизационной задачи: каждый коэффициент матрицы является суммой полезного сигнала и шума. Найти новые коэффициенты, выраженные через старые, таким образом, чтобы среднеквадратичная ошибка стремилась к минимуму. Но даже тогда мы не гарантируем наилучшей работы - кто сказал, что, используя приближенные матрицы конечного поворота, мы вносим именно случайную ошибку!?
Уточним, в чём наша проблема. Нам не хотелось выписывать точную матрицу конечного поворота, поскольку она выглядит примерно так:
(меняя порядок поворотов, будем получать разные матрицы W, но все они будут являться строго матрицами поворота)
Мы «из жадности» её упростили:
И обнаружили, что выкинутые нами слагаемые второго порядка малости приводят к изменению вида матрицы. В нашем примере поворота вокруг оси Z, получаем
Разложив в ряд Тейлора до 3-го порядка малости, имеем:
Оказывается, что ошибка в интегрировании угловой скорости (правая часть) имеет третий порядок малости (φ3/2 вместо φ3/6), тогда как «паразитное масштабирование» имеет второй порядок малости - именно этот эффект мы обнаружим в первую очередь.
Кажется, что ситуация поправима - нужно использовать более точные аппроксимации и уменьшать шаг интегрирования - тогда матрица будет «портиться» медленнее. Но будет портиться всё равно, теперь уже из-за ошибок плавающей арифметики.
К примеру, теперь мы, наученные горьким опытом, интегрируем очень малыми шагами, разбив полную окружность на 31416 шагов, каждый по 200 мкрад, или 41 угловые секунды. Если все вычисления производятся с одинарной точностью (32 бита, с плавающей точкой), то cos(41’’) округлится до единицы, и после 31416 шагов мы придём к следующей матрице:
И снова мы наблюдаем похожий эффект: ошибка интегрирования на этот раз чрезвычайно мала, составляя менее угловой секунды, тогда как наиболее заметное искажение снова проявилось в масштабе. Кажется, что ошибка весьма мала - менее одной тысячной - но этого достаточно, чтобы вектор, направленный под углом 45° к плоскости XY, «прижался бы» к ней дополнительно на 1 угловую минуту.
Так что даже использование малого шага интегрирования и применение методов высокого порядка не решает проблему накопления «мусора» в матрице. И чем больше проходит времени, тем больше в матрице будет мусора, который мы сможем отбросить только вместе с полезными данными.
Всё сказанное не означает, что матрицы поворота абсолютно не годятся для интегрирования угловых скоростей. Их можно применить, если соблюдать определённую осторожность - обязательно предусмотреть процедуру "ортонормирования", но по возможности до неё не доводить - повсюду перейти на удвоенную или расширенную точность, уменьшить шаг интегрирования, использовать численные методы высокого порядка.
Но как мы узнаем в следующей главе, кватернионы свободны от большинства недостатков матриц поворота и выходят в этой битве победителями.
Отличие кватернионов от матрицы поворота одной картинкой
Продолжение следует...
Poll Кого поставить у руля космического аппарата?
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Интегрирование угловых скоростей с помощью матриц поворота
Продолжаем нашу предвыборную гонку - какой интегратор угловых скоростей займёт своё законное место у руля (в буквальном смысле) нашего изделия?
Мы уже накопали компромата на углы Эйлера-Крылова - это конечно уважаемые и достойные фамилии, но очень уж старенькие - задрать голову в зенит не могут, сразу начинает голова кружиться, да и подвешенные вниз головой резко теряют работоспособность. Да и вообще, уголовники (системы, основанные на углах) нам не нужны!
Сегодня мы рассмотрим матрицы поворота - 9 направляющих косинусов не могут ошибаться, не правда ли?
Кинематическое уравнение для связанных осей (когда угловая скорость проецируется на оси изделия) принимает вид:
Здесь A - матрица ориентации.
Для шага Δt мы можем записать приближенный метод интегрирования:
Этот метод - сугубо линейный, в нём применяются только сложения и умножения, особые точки отсутствуют как класс.
Один недостаток, который мы можем заметить «сходу» - это громоздкость: мы используем 9 чисел для представления матриц, а каждый шаг интегрирования по простейшему методу («первого порядка») требует 18 умножений и 18 сложений (без специализированного метода, «знающего» о единицах по главной диагонали - и вовсе 27 умножений). Если выписать по компонентам, мы получим (с верхним индексом 1 - новые значения, с верхним индексом 0 - старые):
Матричная запись позволяет скрыть эту внутреннюю сложность за красивыми выкладками, поэтому полезно иногда выписать вычисления «в лоб», чтобы помнить, с чем мы имеем дело.
Тем не менее, даже у самых старых бортовых вычислителей не возникло бы проблем, связанных с недостаточной производительностью - ну что такое 36 операций для компьютера!?
Нет, настоящая ахиллесова пята матриц поворота состоит в том, что по прошествии времени они перестают быть матрицами поворота, а вернуть их на путь истинный не так-то просто…
Пусть начальная матрица ориентации - единичная, то есть оси инерциальной и связанной систем координат совпадают:
Далее мы начнём вращение изделия вокруг оси Z, на каждом шагу поворачиваясь на 5 градусов, чуть менее 1/10 радианы. Матрица конечного поворота WΔt будет равна:
После 72 шагов придём к матрице ориентации, равной:
тогда как должны были прийти к единичной матрице (поворот на 360 градусов!).
Можем записать эту матрицу, как произведение двух:
Вторая из них - матрица поворота по оси Z на угол 0,9° - такова накопленная ошибка интегрирования, вызванная слишком крупным шагом. В относительном выражении ошибка не так велика: 0,9/360 = 0,25%, что не так уж плохо, учитывая, сколь крупный шаг мы взяли.
А вот первая матрица - масштабирование по осям X, Y. Вектор с нулевой компонентой Z просто увеличит свою длину - зачастую это не так уж страшно - по крайней мере, это не изменит направление вектора. Точно так же, вектор (0;0;1)T останется без изменений - всё верно.
Самое интересное будет происходить с промежуточными векторами.
К примеру, вектор
превратится в
он не только увеличивается в размерах, но и меняет направление из-за масштабирования! Раньше он «смотрел» под углом 45° к плоскости X-Y, а теперь - под углом 37° - ошибка составляет уже не 0,9°, а целых 8°!
В данном конкретном случае мы легко смогли факторизовать матрицу, вычленив из неё отдельно поворот и отдельно масштабирование. Когда мы можем это сделать - понятно, как исправить ситуацию - нужно оставить только матрицу поворота, а масштабирование убрать!
Но представим теперь, что после поворотов вокруг оси Z, мы осуществили ещё и поворот вокруг приборной оси X на 30 градусов:
Как из этого нагромождения чисел наиболее оптимально вычленить отдельно повороты, избавившись от других преобразований пространства - вопрос по-прежнему открытый…
Напомним, что столбцы матрицы ориентации - это координаты базисных векторов в инерциальной системе отсчета. Эти векторы должны иметь единичную длину и быть взаимно-перпендикулярными, то есть мы можем записать следующие «уравнения связи» (в данном случае двойка наверху - это возведение в степень):
Действительно: при 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:
Преобразованный базис назовём n1, n2, n3.
Первый вектор мы нормируем:
Из второго вектора мы вычитаем его проекцию на первый, после чего тоже нормируем:
Наконец, из третьего вектора вычитаем его проекцию на первый и второй, после чего нормируем:
В этих формулах есть определённое лукавство: кажется, что мы задействовали все 9 исходных коэффициентов, чтобы получить новые 9, теперь уже ортонормированные. На самом деле, от e3 вообще ничего не зависит! Сначала мы вычитаем из него всё лишнее, так, чтобы он шел по прямой, взаимно перпендикулярной n1, n2, а затем нормируем его длину - да от этого вектора живого места не остаётся! Мы с тем же успехом можем записать:
и получить абсолютно тот же самый результат! То есть, в действительности данный метод берёт первые 6 коэффициентов и напрочь выкидывает последние 3. Да и первые 6 оказываются неравнозначны: если первому столбцу мы доверяем «безоговорочно», то второму - только до тех пор, пока он не противоречит первому.
Попробуем процедуру нормировки на нашей матрице B:
При этом, как и следовало ожидать, n3 получился одинаковым до последнего представимого знака после запятой, независимо от способа вычисления - через e3 или через векторное произведение.
Сейчас нам повезло - мы идеально отнормировали матрицу, так что она выражает ровно то, что и должна - поворот на 0,9° вокруг оси Z и поворот на 30° вокруг оси X.
Но попробуем теперь зайти с обратной стороны - не e1, e2, e3, а e3, e2, e1. Получится вот что:
Вместо поворота на 30° по оси X, мы в этот раз получили поворот на 37°, реализовав фактически наихудший случай!
Правильным подходом было бы решение оптимизационной задачи: каждый коэффициент матрицы является суммой полезного сигнала и шума. Найти новые коэффициенты, выраженные через старые, таким образом, чтобы среднеквадратичная ошибка стремилась к минимуму. Но даже тогда мы не гарантируем наилучшей работы - кто сказал, что, используя приближенные матрицы конечного поворота, мы вносим именно случайную ошибку!?
Уточним, в чём наша проблема. Нам не хотелось выписывать точную матрицу конечного поворота, поскольку она выглядит примерно так:
(меняя порядок поворотов, будем получать разные матрицы W, но все они будут являться строго матрицами поворота)
Мы «из жадности» её упростили:
И обнаружили, что выкинутые нами слагаемые второго порядка малости приводят к изменению вида матрицы. В нашем примере поворота вокруг оси Z, получаем
Разложив в ряд Тейлора до 3-го порядка малости, имеем:
Оказывается, что ошибка в интегрировании угловой скорости (правая часть) имеет третий порядок малости (φ3/2 вместо φ3/6), тогда как «паразитное масштабирование» имеет второй порядок малости - именно этот эффект мы обнаружим в первую очередь.
Кажется, что ситуация поправима - нужно использовать более точные аппроксимации и уменьшать шаг интегрирования - тогда матрица будет «портиться» медленнее. Но будет портиться всё равно, теперь уже из-за ошибок плавающей арифметики.
К примеру, теперь мы, наученные горьким опытом, интегрируем очень малыми шагами, разбив полную окружность на 31416 шагов, каждый по 200 мкрад, или 41 угловые секунды. Если все вычисления производятся с одинарной точностью (32 бита, с плавающей точкой), то cos(41’’) округлится до единицы, и после 31416 шагов мы придём к следующей матрице:
И снова мы наблюдаем похожий эффект: ошибка интегрирования на этот раз чрезвычайно мала, составляя менее угловой секунды, тогда как наиболее заметное искажение снова проявилось в масштабе. Кажется, что ошибка весьма мала - менее одной тысячной - но этого достаточно, чтобы вектор, направленный под углом 45° к плоскости XY, «прижался бы» к ней дополнительно на 1 угловую минуту.
Так что даже использование малого шага интегрирования и применение методов высокого порядка не решает проблему накопления «мусора» в матрице. И чем больше проходит времени, тем больше в матрице будет мусора, который мы сможем отбросить только вместе с полезными данными.
Всё сказанное не означает, что матрицы поворота абсолютно не годятся для интегрирования угловых скоростей. Их можно применить, если соблюдать определённую осторожность - обязательно предусмотреть процедуру "ортонормирования", но по возможности до неё не доводить - повсюду перейти на удвоенную или расширенную точность, уменьшить шаг интегрирования, использовать численные методы высокого порядка.
Но как мы узнаем в следующей главе, кватернионы свободны от большинства недостатков матриц поворота и выходят в этой битве победителями.
Отличие кватернионов от матрицы поворота одной картинкой
Продолжение следует...
Poll Кого поставить у руля космического аппарата?