nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

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

Часть 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 - разложение кватерниона на повороты

Интегрирование угловых скоростей с помощью кватернионов

- Старик не согласен со мной. - Он вздохнул. – Немного отстал от жизни, вот в чем дело. Цепляется за свою обожаемую матричную механику, а этот вопрос требует более мощных математических средств. Он так упрям.

- Вы применили переходное уравнение
Митчелла, верно? Так вот, оно здесь неприменимо.
- Почему?
- Во-первых, вы пользуетесь гипермнимыми величинами.

Айзек Азимов, «Я, робот» - «Лжец»


Давайте рванём с места в карьер и сразу рассмотрим пример из прошлой главы. Мы начинаем с единичного кватерниона Λ0=1 (нулевой поворот), после чего начинаем вращение, по 5° за шаг, вокруг оси Z.
В самом простом методе интегрирования – в методе первого порядка – мы просто заявим: угол поворота за один шаг достаточно мал, чтобы применить формулу кватерниона бесконечно малого поворота (3.1):
9quat_eq1.png

В нашем примере получится M ≈ 1 + 0,0436k. В общем случае мы получаем вектор угловой скорости ω, измеренный в связанных осях. Тогда мы записываем кватернион поворота за этот шаг:
9quat_eq2.png

На каждом шаге интегрирования мы будем умножать текущий кватернион ориентации справа на Μ:
9quat_eq3.png

Справа – потому что мы «считываем» показания угловой скорости из датчика, закреплённого на изделии, то есть работаем в связанных осях (подробности описывались в части 4). Разумеется, в нашем примере это неважно, ведь пока все повороты происходят вокруг одной оси, все участвующие в вычислениях кватернионы (равно как и матрицы поворота) коммутируют.
Учитывая, что скалярная (действительная) компонента Μ равна единице, можем расписать данную формулу покомпонентно:
9quat_eq4.png

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

Через 72 шага (360°) мы получаем следующий кватернион ориентации:
Λ72 = -1,0709 + 0,00213k

В первую очередь мы обращаем внимание на знак – он внезапно поменялся, но так и задумано: кватернион Ν = -1 также соответствует нулевому повороту – мы об этом говорили в главе 4 «кватернионы и спиноры». Можно считать это фичей: совершает посадку пилот гражданской авиации, мы смотрим кватернион ориентации, а он с 1 в начале полёта поменялся на -1 – и тут же становится ясно – пилот нехороший человек либо бочку сделал, либо мёртвую петлю, либо лишний круг вокруг аэропорта!

Смотрим на норму кватерниона: она равна 1,07, тогда как должна оставаться единичной. Как видим, увеличение не столь велико, как в случае матриц (тогда масштаб по осям X, Y возрастал на 1,31), но что самое главное – оно куда безобиднее.

Если нас интересуют только направления, а не конкретные векторы, то можно вообще сохранять ненормированные кватернионы – направление он преобразует верно, только длину вектора увеличит. Впрочем, даже при использовании чисел с плавающей точкой норма «пойдёт вразнос» - через 100 оборотов, аналогичных нашему, она станет равна 941, ещё через 100 – 886247, такими темпами и до переполнения недалеко!

Так что нормировать кватернионы надо в любом случае, благо, дело это простое. Формула «в лоб» выглядит так:
9quat_eq5.png

В нашем примере получим:
Λ72 норм = -0,999998 + 0,001991k
Это соответствует повороту на 0,23° относительно исходного положения, тогда как в действительности мы должны были прийти к нулевому повороту. Как видим, точность метода оказалось вчетверо выше, чем при использовании матриц поворота – там ошибка интегрирования составила 0,9°.

На самом деле, и при нормировании кватерниона можно обойтись сложением и умножением, не прибегая к делениям и квадратным корням. Для этого достаточно разложить множитель в ряд, зная, что норма должна быть близка к единице:
9quat_eq6.png

И соответственно:
9quat_eq7.png

Попробуем осуществить такую нормировку в нашем примере:
Λ72 норм' = -0,9923 + 0,00198k

да, неидеально – норма с 1,07 уменьшилась до 0,9923, ошибка составила 0,78%. Если проводить подобную нормировку вдвое чаще: на 36-м шагу, а затем на 72-м, то к концу работы норма составит 0,9983, ошибка 0,17%.

Повторение – мать учения, поэтому напомним важную информацию. Если мы поворачиваем единичный вектор с помощью кватерниона, а потом хотим преобразовать его в сферические углы, то ни в коем случае не надо применять формулу наподобие
θ=arcsin⁡(y)
Если норма кватерниона хоть чуть-чуть превышает единицу, мы можем «нарваться» на случай y > 1 – последствия могут быть катастрофическими! У автора был опыт использования тригонометрических функций, реализованных в DSP, где исключение выкинуть совершенно некуда. А раз так, то и проверки никакой не делалось, шли вычисления через ряд, а там уж что получится! Оказалось, что синус и косинус от действительного угла очень даже может превышать единицу «в военное время»! Даже если мы специально рассмотрим такой случай, пересчёты вблизи зенита будут приводить к существенным ошибкам – не только из-за изменившейся длины вектора. Даже если длина осталась правильной, мы сталкиваемся с потерей точности из-за самого вида функции arcsin(y) – она вблизи единицы становится вертикальной, т.е. малейшая ошибка в аргументе приводит к огромной ошибке в значении.
Гораздо лучше использовать более сложную формулу
9quat_eq8.png

Она хорошо работает по всей сфере, и ей уже совершенно наплевать на длину вектора, главное, чтобы направление было правильным!

Подведём предварительные итоги по методу первого порядка. Вместо 9 коэффициентов матрицы, нам достаточно четырёх компонент кватерниона. Вместо 18 умножений и 18 сложений, нам требуется 12 умножений и 12 сложений – чуть лучше. Матрицу надо было ортонормировать через некоторое время, что является операцией очень нетривиальной и угрожающей внести ошибку ориентации, пропорциональную квадрату от угла поворота на одном шаге. Нормировка кватерниона – операция куда более простая и может быть выполнена одними только сложениями и умножениями, причем даже ненормированный (или плохо нормированный) кватернион не искажает направления вектора, он может лишь изменить его длину. И наконец, ошибка интегрирования снизилась в 4 раза, причем это верно практически во всех случаях. И ещё один «пряник». Взглянув на матрицу, мы не можем сходу понять, что она означает, тогда как из кватерниона мы можем сразу сказать, вокруг какой оси произошёл поворот и какова его величина. Мы всегда можем превратить кватернион в матрицу (см главу «Вычисление матрицы поворота»), тогда как обратная операция не столь тривиальна.
Хорошая заявка на успех!

Об ошибках интегрирования
Ошибки интегрирования в методе, изложенном выше, можно условно поделить на две категории:
- ошибки линейного приближения – чем меньше угол конечного поворота на шаге интегрирования, тем меньше они будут становиться;
- ошибки, возникающие при переменных угловых скоростях, когда мы формулой (3.1) предполагаем, будто бы предмет вращался с постоянной скоростью на всём шаге интегрирования, а на самом деле он совершал более сложное движение - вращался с угловыми ускорениями, меняющими величину и знак угловой скорости, причем и сами ускорения могут быть непостоянными.

Рассмотрим их независимо друг от друга. Примем для начала, что изделие вращается с постоянной угловой скоростью ω вокруг оси n.
Истинный кватернион поворота за время Δt составит
9quat_eq9.png

Он заведомо имеет единичную норму.
Для упрощения дальнейших выкладок, обозначим
9quat_eqA.png

Тогда, истинный кватернион поворота запишется в виде
9quat_eqB.png

Наш приближенный кватернион (по методу первого порядка):
9quat_eqC.png

После нормировки он станет равен:
9quat_eqD.png

Сравнивая скалярную и векторную часть с разложением в ряд косинуса и синуса, обнаруживаем, что линейные и квадратичные слагаемые у нас верны, ошибка наступает лишь в слагаемых третьей степени и составляет:
9quat_eq_E.png

Угловая ошибка будет вдвое выше указанного здесь результата, поскольку в кватернионах участвуют половинные углы – получаем оценку
9quat_eqF.png

тогда как при использовании матрицы поворота мы получали
9quat_eq_G.png
(как всегда, без учета ошибок, внесенных при ортонормировании матрицы)

Угловая ошибка при использовании кватернионов действительно ниже в 4 раза (при использовании методов первого порядка), что и было подтверждено нашим примером.

Теперь оценим ошибку, которая возникает из-за наличия углового ускорения. Это не так-то просто, учитывая, что аналитическое интегрирование угловых скоростей удаётся провести в очень редких частных случаях. По-хорошему нужно преобразовать дифференциальное уравнение в интегральное уравнение Фредгольма, после чего применить метод Пикара, матрицианты и много других страшных слов - именно так это было проделано в неоднократно упомянутой нами монографии «Применение кватернионов…». Мы же немного извернёмся и обойдёмся в итоге простой арифметикой.

Представим, что мы вдруг разучились интегрировать линейную функцию f(t) = v + at, но хотим оценить точность численного метода – «метода прямоугольников».
9quat_f0.png

Сначала оценим площадь под графиком на интервале 0..Δt, используя всего один прямоугольник:
S1=f(0)Δt = vΔt
Затем – разделив его на два прямоугольника поменьше:
S2=f(0)Δt/2 + f(Δt/2)Δt/2 = vΔt + aΔt2/4
Во втором случае у нас появилось слагаемое, пропорциональное Δt2, которого не было ранее, и этого достаточно, чтобы понять: ошибка является квадратичной, т.е «метод прямоугольников» - метод первого порядка. При этом точный множитель при Δt2 остаётся неизвестен, но это и не так важно.

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

то есть, присутствует угловое ускорение. Рассмотрим промежуток времени –Δt/2 .. Δt/2. Средняя угловая скорость на этом промежутке равна ω0 - именно её замерят наши датчики угловой скорости. Соответственно, будет построен кватернион поворота
9quat_f2.png

А затем попробуем увеличить вдвое частоту опроса датчиков, благодаря чему получим сначала значение ω0-βΔt/4, среднее на промежутке –Δt/2 .. 0,
а на следующем шаге: ω0+βΔt/4, среднее на промежутке 0 .. Δt/2. Кватернион поворота будет равен произведению двух кватернионов:
9quat_f3.png

Слагаемое второго порядка здесь не содержит углового ускорения, и было, фактически, учтено при рассмотрении движения с постоянной угловой скоростью. Как оказалось, это слагаемое влияет на норму кватерниона, тогда как ошибки интегрирования второго порядка не возникает.
Ну а минимальный порядок, который имеет слагаемое с угловым ускорением – третий. Выходит, что самый простой метод «первого порядка», который мы рассмотрели, на самом деле даёт ошибки, пропорциональные Δt3, поэтому каждое уменьшение шага в 2 раза уменьшает ошибку интегрирования в 4 раза. Очень хороший результат!

В следующей части мы обсудим методы интегрирования второго порядка и выше, после чего пора будет переходить к практическим примерам. А помните, как я надеялся обойтись тремя частями?
Tags: кватернионы-это просто (том 1), математика, моделирование, работа
Subscribe

  • Лестница для самых жадных

    В эти выходные побывал на даче, после 3-недельной "самоизоляции". Забавно, как будто зима началась! Особенно грязные галоши остались на улице, в…

  • Возвращаемся к макету

    Очень давно макетом видеоизмерителя параметров сближения не занимался: сначала "громко думал" по поводу измерения его положения на аппарате, а потом…

  • Минутка живописи

    В процессе разгребания содержимого квартиры (после нескольких ремонтов) дошёл, наконец, и до картин. В кои-то веки их повесил. Куда их вешать -…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 8 comments