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

nabbla1

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

Mar 20, 2018 12:09

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

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

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

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

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

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

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

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

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

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

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

И соответственно:

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

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

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

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

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

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

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

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

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

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

После нормировки он станет равен:

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

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

тогда как при использовании матрицы поворота мы получали

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

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

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

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

Сначала оценим площадь под графиком на интервале 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 остаётся неизвестен, но это и не так важно.

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

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

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

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

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

моделирование, кватернионы-это просто (том 1), математика, работа