Ликбез по кватернионам, часть 10 1/2, интегрирование с поддержанием нормы
Оглавление "Ликбеза по кватернионам":
[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
Когда мы говорили об интегрировании угловых скоростей с помощью кватернионов, мы описывали разные способы задать "кватернион малого поворота", и затем всё интегрирование состояло в умножении кватерниона текущей ориентации на этот кватернион малого поворота. При этом говорилось, что норма кватерниона при этом может немного "ползти", обычно в сторону увеличения, и нужно его нормировать, иначе быть беде.
Но есть ещё класс методов, где "кватернион малого поворота" строится не только по показаниям датчиков, но ещё и по текущей норме кватерниона ориентации, с тем расчётом, чтобы поддерживать норму единичной, поэтому специально проводить нормировку не требуется!
Рассмотрим только метод первого порядка. Наш текущий кватернион ориентации: Λn=λn0+λn1i+λn2j+λn3k. За время Δt мы измерили угловую скорость ω. Тогда мы построим кватернион малого поворота Μ следующим образом:
То есть, в действительной (скалярной) части вместо единице (как в обычном методе первого порядка) ставим величину, обратную к норме текущего кватерниона ориентации, а в мнимой (векторной) части - как обычно, половинка от вектора конечного поворота.
Возьмём пример, на котором пробовали предыдущие методы интегрирования: начинаем с нулевого поворота, т.е Λ0 = 1. Делаем 72 шага по 5°. Применим для вычисления PhysUnitCalc. Он пока что не умеет в кватернионы, зато вполне умеет в комплексные числа, а этого для нашего примера вполне достаточно:
> 1+0i
1 + 0i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
1 + 0,0436332312998582i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,9971442116895 + 0,087224926842418i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,992388642702538 + 0,130650479299362i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,985742806093702 + 0,173827173196459i
...
...
(72 шага)
...
...
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-0,997223573701236 + 0,086312860927519i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-1,00003994399384 + 0,0427185711811284i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-1,00095147229553 - 0,000957087443241873i
Правильным ответом должен был стать кватернион Λ72 = -1, т.е поворот на 360 градусов вокруг оси X. У нас, несмотря на "поддержание нормы", она всё равно не стала единичной и равна 1,00095. Именно такова норма кватерниона 1 + 0,0436i (малого поворота на 5°), и это не случайно. Данный метод как бы отстаёт на один шаг: ранние ошибки по норме устраняет, но затем снова вносит ошибку, когда поворот сколько-нибудь большой.
Угловая ошибка составила 0,11°, что вдвое меньше, чем 0,23° для обычного метода первого порядка и соответствует "обычному" методу второго порядка. Так получается, когда угловая скорость не меняется от итерации к итерации. Квадрат нормы кватерниона составлял
Тогда, если угловая скорость осталась той же, кватернион M ("малого поворота") будет равен
Выражение получается аналогично обычному методу второго порядка, но только если угол поворота от итерации к итерации меняется слабо.
Разберём достоинства и недостатки этого метода
Вычислительные затраты соответствуют методу второго порядка: необходимо 4 умножения и 3 сложения, чтобы приближённо посчитать величину, обратную к норме кватерниона, после чего используется умножение двух кватернионов "в общем виде", требующее 16 умножений и 12 сложений. Зато, после этого не нужно делать нормировки кватерниона (8 умножений и 3 сложения), что делает такой метод весьма привлекательным в вычислительном плане. При этом алгоритм оказывается более простым: не нужно программировать умножение на кватернион с единицей в скалярной части (чтобы сэкономить 4 умножения), не нужна отдельная процедура нормировки.
Точность интегрирования - от первого до второго порядка, в зависимости от задачи. Если мы интегрируем показания датчиков угловой скорости, и эта скорость меняется относительно медленно - получим точность, характерную для второго порядка. Если итеративно решаем задачу определения взаимной ориентации по изображению мишеней на фотоприёмной матрице - стоит ожидать скорее первого порядка.
Точность поддержания нормы определяется максимальным углом поворота, который может возникнуть в каждой итерации. В рассмотренном примере угол поворота составлял 5°, и норма поддерживалась с точностью 0,1%. Если же максимально возможный угол поворота составляет 30° (характерно для моей задачи), то ошибка поддержания нормы может составить 3,37%. Если кватернион затем используется для поворота вектора, этот вектор может вместе с поворотом удлиниться на 6,8% (он оказывается домножен на квадрат нормы). В моей задаче такое удлинение было допустимо, т.е не приводило к срыву сопровождения, а большие углы были характерны именно для процесса входа в сопровождение, затем углы начинают исчисляться в единицах угловых минут.
Но у меня возникли проблемы при реализации этого метода в числах "с фиксированной запятой", формате Q1.15, т.е 16-битные числа, один бит перед запятой, диапазон значений от -1 до 1-2-15≈0,99969. Я начинал с единичного кватерниона Λ0 = 1, интегрировал нулевые углы (самый тривиальный случай), для которых должен был строиться кватернион Μ=1, но поскольку единица была непредставима, бралось самое большое представимое значение, Μ=32767/32768. Оно же получалось и в дальнейшем, когда должна была последовать коррекция нормы в сторону увеличения, т.е умножение на число, больше единицы. Но даже единица непредставима, поэтому мы получили систематическое снижение нормы, которое шло 16384 итерации подряд, и только после этого норма устанавливалась на значении 1/2. Такое поведение было совершенно неприемлимым.
Ситуацию удалось исправить, сменив знак кватерниона малого поворота:
Число -1 представимо в данном числовом формате, благодаря чему норма не уменьшается. Тогда метод становится жизнеспособным. Даже если норма кватерниона из-за каких-то случайных ошибок снизилась, она будет плавно возвращаться к единице, поскольку при наличии углового движения мы будем получать кватернион малого поворота с нормой больше единицы.
Но нужно иметь в виду, что знак кватерниона ориентации будет меняться на каждой итерации! Это корректно с точки зрения математики: при умножении кватерниона на -1 он будет выражать ту же самую ориентацию в пространстве, но может иметь как положительные, так и отрицательные эффекты при реализации. С одной стороны, эту смену знака можно использовать как признак, что мы не пропустили не одного отсчёта, или не выдали одни и те же результаты дважды. Хотя, присоединение к каждому отсчёту метки времени могло бы решать эту задачу столь же успешно.
Однако становится очень опасным "перемешивание" старых и новых данных. Скажем, в памяти лежал кватернион 0,707+0,707i (поворот на 90° по часовой стрелке по оси X). Если к этой памяти может обращаться несколько устройств / процессов / потоков и не налажена правильная синхронизация между ними, то весьма вероятен следующий сценарий:
1. Значение кватерниона было запрошено и начало передаваться на малой скорости. Первым была отправлена скалярная (действительная) компонента, 0,707.
2. Устройство, интегрирующее угловые скорости, посчитало новое значение кватерниона, -0,707-0,707i (тот же самый поворот на 90° по часовой стрелке по оси X) и занесло его в память.
3. Передача продолжилась, и была отправлена уже обновлённая компонента "-0,707i".
В итоге был передан кватернион 0,707-0,707i, выражающий поворот на 90° ПРОТИВ ЧАСОВОЙ СТРЕЛКИ, т.е мы ОШИБЛИСЬ НА 180° (!!!) Заметим, что если бы кватернион малого поворота вычислялся по обычной формуле (в начале поста), такого рода перемешивание данных было бы практически безобидным, мы бы получили что-то среднее между старым и новым кватернионом, и тогда, возможно, мы могли бы обойтись без взаимных блокировок устройств / потоков. Для этого необходимо, чтобы атомарной операцией была запись в память одной компоненты кватерниона. Если и она записывается в несколько заходов (например, сначала старший байт, потом младший), то взаимная блокировка становится обязательной! Иначе старое значение 0x00FF и новое значение 0x0100 (отличающиеся всего на единицу младшего разряда) могут "перемешаться", породив 0x01FF, что даёт ошибку в ДВА РАЗА.
[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
Когда мы говорили об интегрировании угловых скоростей с помощью кватернионов, мы описывали разные способы задать "кватернион малого поворота", и затем всё интегрирование состояло в умножении кватерниона текущей ориентации на этот кватернион малого поворота. При этом говорилось, что норма кватерниона при этом может немного "ползти", обычно в сторону увеличения, и нужно его нормировать, иначе быть беде.
Но есть ещё класс методов, где "кватернион малого поворота" строится не только по показаниям датчиков, но ещё и по текущей норме кватерниона ориентации, с тем расчётом, чтобы поддерживать норму единичной, поэтому специально проводить нормировку не требуется!
Рассмотрим только метод первого порядка. Наш текущий кватернион ориентации: Λn=λn0+λn1i+λn2j+λn3k. За время Δt мы измерили угловую скорость ω. Тогда мы построим кватернион малого поворота Μ следующим образом:
То есть, в действительной (скалярной) части вместо единице (как в обычном методе первого порядка) ставим величину, обратную к норме текущего кватерниона ориентации, а в мнимой (векторной) части - как обычно, половинка от вектора конечного поворота.
Возьмём пример, на котором пробовали предыдущие методы интегрирования: начинаем с нулевого поворота, т.е Λ0 = 1. Делаем 72 шага по 5°. Применим для вычисления PhysUnitCalc. Он пока что не умеет в кватернионы, зато вполне умеет в комплексные числа, а этого для нашего примера вполне достаточно:
> 1+0i
1 + 0i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
1 + 0,0436332312998582i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,9971442116895 + 0,087224926842418i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,992388642702538 + 0,130650479299362i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
0,985742806093702 + 0,173827173196459i
...
...
(72 шага)
...
...
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-0,997223573701236 + 0,086312860927519i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-1,00003994399384 + 0,0427185711811284i
> ans*(3/2-abs(ans)^2/2+(5 deg []) / 2 * 1i)
Для преобразования из [°] в [] выражение домножено на (рад)^-1
-1,00095147229553 - 0,000957087443241873i
Правильным ответом должен был стать кватернион Λ72 = -1, т.е поворот на 360 градусов вокруг оси X. У нас, несмотря на "поддержание нормы", она всё равно не стала единичной и равна 1,00095. Именно такова норма кватерниона 1 + 0,0436i (малого поворота на 5°), и это не случайно. Данный метод как бы отстаёт на один шаг: ранние ошибки по норме устраняет, но затем снова вносит ошибку, когда поворот сколько-нибудь большой.
Угловая ошибка составила 0,11°, что вдвое меньше, чем 0,23° для обычного метода первого порядка и соответствует "обычному" методу второго порядка. Так получается, когда угловая скорость не меняется от итерации к итерации. Квадрат нормы кватерниона составлял
Тогда, если угловая скорость осталась той же, кватернион M ("малого поворота") будет равен
Выражение получается аналогично обычному методу второго порядка, но только если угол поворота от итерации к итерации меняется слабо.
Разберём достоинства и недостатки этого метода
Вычислительные затраты соответствуют методу второго порядка: необходимо 4 умножения и 3 сложения, чтобы приближённо посчитать величину, обратную к норме кватерниона, после чего используется умножение двух кватернионов "в общем виде", требующее 16 умножений и 12 сложений. Зато, после этого не нужно делать нормировки кватерниона (8 умножений и 3 сложения), что делает такой метод весьма привлекательным в вычислительном плане. При этом алгоритм оказывается более простым: не нужно программировать умножение на кватернион с единицей в скалярной части (чтобы сэкономить 4 умножения), не нужна отдельная процедура нормировки.
Точность интегрирования - от первого до второго порядка, в зависимости от задачи. Если мы интегрируем показания датчиков угловой скорости, и эта скорость меняется относительно медленно - получим точность, характерную для второго порядка. Если итеративно решаем задачу определения взаимной ориентации по изображению мишеней на фотоприёмной матрице - стоит ожидать скорее первого порядка.
Точность поддержания нормы определяется максимальным углом поворота, который может возникнуть в каждой итерации. В рассмотренном примере угол поворота составлял 5°, и норма поддерживалась с точностью 0,1%. Если же максимально возможный угол поворота составляет 30° (характерно для моей задачи), то ошибка поддержания нормы может составить 3,37%. Если кватернион затем используется для поворота вектора, этот вектор может вместе с поворотом удлиниться на 6,8% (он оказывается домножен на квадрат нормы). В моей задаче такое удлинение было допустимо, т.е не приводило к срыву сопровождения, а большие углы были характерны именно для процесса входа в сопровождение, затем углы начинают исчисляться в единицах угловых минут.
Но у меня возникли проблемы при реализации этого метода в числах "с фиксированной запятой", формате Q1.15, т.е 16-битные числа, один бит перед запятой, диапазон значений от -1 до 1-2-15≈0,99969. Я начинал с единичного кватерниона Λ0 = 1, интегрировал нулевые углы (самый тривиальный случай), для которых должен был строиться кватернион Μ=1, но поскольку единица была непредставима, бралось самое большое представимое значение, Μ=32767/32768. Оно же получалось и в дальнейшем, когда должна была последовать коррекция нормы в сторону увеличения, т.е умножение на число, больше единицы. Но даже единица непредставима, поэтому мы получили систематическое снижение нормы, которое шло 16384 итерации подряд, и только после этого норма устанавливалась на значении 1/2. Такое поведение было совершенно неприемлимым.
Ситуацию удалось исправить, сменив знак кватерниона малого поворота:
Число -1 представимо в данном числовом формате, благодаря чему норма не уменьшается. Тогда метод становится жизнеспособным. Даже если норма кватерниона из-за каких-то случайных ошибок снизилась, она будет плавно возвращаться к единице, поскольку при наличии углового движения мы будем получать кватернион малого поворота с нормой больше единицы.
Но нужно иметь в виду, что знак кватерниона ориентации будет меняться на каждой итерации! Это корректно с точки зрения математики: при умножении кватерниона на -1 он будет выражать ту же самую ориентацию в пространстве, но может иметь как положительные, так и отрицательные эффекты при реализации. С одной стороны, эту смену знака можно использовать как признак, что мы не пропустили не одного отсчёта, или не выдали одни и те же результаты дважды. Хотя, присоединение к каждому отсчёту метки времени могло бы решать эту задачу столь же успешно.
Однако становится очень опасным "перемешивание" старых и новых данных. Скажем, в памяти лежал кватернион 0,707+0,707i (поворот на 90° по часовой стрелке по оси X). Если к этой памяти может обращаться несколько устройств / процессов / потоков и не налажена правильная синхронизация между ними, то весьма вероятен следующий сценарий:
1. Значение кватерниона было запрошено и начало передаваться на малой скорости. Первым была отправлена скалярная (действительная) компонента, 0,707.
2. Устройство, интегрирующее угловые скорости, посчитало новое значение кватерниона, -0,707-0,707i (тот же самый поворот на 90° по часовой стрелке по оси X) и занесло его в память.
3. Передача продолжилась, и была отправлена уже обновлённая компонента "-0,707i".
В итоге был передан кватернион 0,707-0,707i, выражающий поворот на 90° ПРОТИВ ЧАСОВОЙ СТРЕЛКИ, т.е мы ОШИБЛИСЬ НА 180° (!!!) Заметим, что если бы кватернион малого поворота вычислялся по обычной формуле (в начале поста), такого рода перемешивание данных было бы практически безобидным, мы бы получили что-то среднее между старым и новым кватернионом, и тогда, возможно, мы могли бы обойтись без взаимных блокировок устройств / потоков. Для этого необходимо, чтобы атомарной операцией была запись в память одной компоненты кватерниона. Если и она записывается в несколько заходов (например, сначала старший байт, потом младший), то взаимная блокировка становится обязательной! Иначе старое значение 0x00FF и новое значение 0x0100 (отличающиеся всего на единицу младшего разряда) могут "перемешаться", породив 0x01FF, что даёт ошибку в ДВА РАЗА.