nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Ликбез по кватернионам, часть 13: Дэвенпорт берёт след!

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

Не зря эта часть 13-я по счёту: она самая сложная, но дело того стоит. Освоив её, вы сможете без особенных проблем осилить 90% всей литературы по астронавигации. Это и есть всем известные тернии, через которые лежит дорога к звёздам!

Итак:

Дэвенпорт берёт след!

Дойдя до 13-й части «ликбеза по кватернионам» читатель вправе предположить: и в навигационной задаче кватернионы должны быть полезны! Действительно, львиная доля всех методов, которые применяются для решения данной задачи, используют кватернионы, и все опираются на труд Дэвенпорта, который, однако, так и не был по-нормальному опубликован. Как правило, все ссылаются на доклад «Для служебного пользования» [A Vector Approach to the Algebra of Rotations with Applications by Paul B. Davenport, Goddard Space Flight Center, Greenbelt, Md, 1968]. Уже название немножко смущает – почему же это «векторный подход», если он должен быть кватернионным?

Оказывается, начальник Дэвенпорта на дух не переносил двух вещей: кватернионов и фильтры Калмана, и, в общем-то, его можно понять. И те, и другие только начинали свой путь в космическую технику, и амбиции были гораздо выше реальных результатов. Математики били себя кулаком в грудь – «фильтр Калмана из любого мусора получит точные измерения», «кватернионы – самый естественный способ описания вращения», но оказывалось, что принцип «мусор на входе – мусор на выходе» никто не отменял, а всем причастным куда как ближе по духу были курс-крен-тангаж, а не «эти ваши кватернионы».


Вот и пришлось Дэвенпорту «скрывать» своё пристрастие к кватернионам, рассмотрев в данной статье так называемый вектор Родрига. Вектор Родрига для поворота на угол φ вокруг единичного вектора равняется



иными словами, это кватернион поворота, который поделили на скалярный компонент, после чего оставили только векторную часть. (У Бранца и Шмыглевского вектором Родрига называют указанную у нас величину но умноженную на 2). Существует формула Родрига для поворота вектора на угол φ вокруг единичного вектора :



В кои-то веки мы можем использовать для описания поворотов всего 3 числа, не прибегая к тригонометрическим функциям. Увы, от злого ежа никуда не уйдёшь. Попытался сэкономить одно число – получай особую точку при φ=180°. Как видно, при этом устремляется в бесконечность.

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

Вектор Родрига сыграет свою роль чуть дальше в нашем повествовании, однако метод Дэвенпорта красивее записывался именно в кватернионной форме, поступим так и мы.

Наиболее короткие выкладки получаются, если воспользоваться следом матрицы Tr A (от английского Trace, иногда пишут Sp A, от немецкого Spur) и его свойствами.

Напомним вкратце, что это такое.
След определён только для квадратных матриц и попросту равен сумме диагональных элементов:



для матрицы NxN.

Можно взять след от числа, поскольку число – это примитивный частный случай матрицы с размером 1х1. Очевидно, что Tr a = a.

След от суммы матриц равен сумме следов: Tr (A+B) = Tr A + Tr B.

Умножение матрицы на число можно вносить и выносить из следа:
Tr(aA) = a Tr A.

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

Tr(ABC) = Tr(CAB)=Tr(BCA).

Заметим, что участвующие в перестановке матрицы не обязаны быть квадратными! Если матрица A имеет размеры n×m, матрица B: m×k, а матрица C: k×n (совпадение размерностей обязательно, чтобы матрицы можно было помножить, и чтобы результатом оказалась квадратная матрица!), то матрица ABC будет иметь размер n×n, матрица CAB: k×k, матрица BCA: m×m, тем не менее, след этих матриц будет один и тот же!


Нашу оптимизационную задачу можно записать следующим образом:



поскольку мы суммировали числа (скалярные произведения), а след от числа попросту равняется этому числу.

Теперь произведём циклическую перестановку находящихся внутри матриц:



Здесь



- матрица 3х3, содержащая в себе произведения компонентов векторов и во всех возможных вариациях:



Уже произошла приятная перемена: дальнейшее решение больше не будет зависеть от числа N – количества эталонных и измеренных направлений! Единственная операция, время которой будет зависеть от N – нахождение матрицы B, и на это требуется O(N) операций, что весьма шустро. Ну и знак ∑ нам больше не понадобится.

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



но в этот раз мы лучше запишем формулу поворота вектора с помощью кватерниона через скалярные произведения (см часть 5 - практическая реализация поворотов):



и затем приведём к матричной форме. Введём ещё одно обозначение, которое очень часто применяется в данной области. Для некоторого вектора , запишем



то есть это просто символ, обозначающий кососимметричную матрицу, составленную из данного вектора. Умножение на такую матрицу эквивалентно векторному произведению:



Преимущество такой записи в том, что мы можем комбинировать операцию векторного произведения с другими, не применяя их сразу к конкретному вектору. Кроме того, если векторное произведение можно произвести только с вектором, то матрицу можно помножить на матрицу размером 3×K, произведя векторные произведения «оптом».

Теперь поворот вектора с помощью кватерниона можно записать в матричной форме:







Выражение в скобках – это матрица 3х3. Мы можем записать более «красивое» выражение для матрицы поворота:



Читатель может спросить: почему мы назвали кватернион «Мю», а не привычной «Лямбдой», как всегда делали? Автор сначала так и сделал, после чего мучительно менял везде обозначения. Проблема в том, что лямбда нам понадобится чуть подальше. У кого-то здесь уже появились нехорошие предчувствия…

Подставим выражение в (13.1) и начнём преобразовывать:







Выражение - попросту число, поэтому можно убрать «след», а также мы можем транспонировать это выражение, результат не изменится:



Мы делаем такие «манёвры», поскольку хотим в итоге прийти к симметричной матрице – это серьёзно упростит нам дело!

Первое слагаемое запишется в виде



И, наконец, найдём значение второго слагаемого, честно перемножив матрицы:


Мы применили мощный метод упрощения выкладок, который широко применяется в функциональном программировании: «ленивые вычисления».
Данное слагаемое можно представить в следующей форме:



где



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



Тогда (13.2) можно записать в виде:



Матрицу K (4х4), составленную из скаляра TrB, матрицы Z (3х1), матрицы ZT (1х3) и матрицы B + BT – E TrB(3x3), называют матрицей Дэвенпорта. Это – его наследие! В большинстве литературы эта матрица и кватернион записаны чуть по-другому: первые три компоненты – векторные, а последняя – скалярная. Понятно, что это лишь вопрос обозначений, суть от этого не меняется. Ещё заметим, что матрица получилась симметричной, что очень важно для дальнейших выкладок.

Нам нужно найти кватернион M с единичной нормой, который максимизирует значение Gw. Воспользуемся методом множителей Лагранжа:



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

Возьмём дифференциал выражения:



Упростим, учитывая, что MT=M (матрица симметричная):



Чтобы наступил максимум функции Gw, необходимо, чтобы дифференциал обратился в ноль, а для этого приравняем нулю коэффициенты при dM и при dλ.

Со вторым всё понятно – мы просто получаем условие единичной нормы.

Из равенства нулю коэффициента при dM получаем



а это типичная задача на собственные значения / собственные вектора. Кватернион M, выражающий оптимальный поворот, необходимо является собственным вектором матрицы K, соответствующим одному из её собственных значений. Поскольку матрица симметричная, все её собственные значения являются действительными.

Зная об этом, взглянем ещё раз на выражение для Gw:



Поскольку KM=λM и MTM=1, то



Таким образом, чтобы найти глобальный максимум этой функции, нам нужно из всех собственных значений матрицы K выбрать наибольшее. Затем остаётся найти собственный вектор (кватернион!), соответствующий этому собственному значению, отнормировать его – и мы получим точный ответ!

Подытожим, как найти кватернион поворота по методу Дэвенпорта.
1. Из "эталонных" векторов и "измеренных" векторов составляем матрицу B размером 3х3:



2. Превращаем матрицу B в вектор Z и матрицу Дэвенпорта K:





(она по определению получается симметричной, поэтому из 16 элементов можно в действительности найти лишь 10)

3. Находим максимальное собственное значение матрицы Дэвенпорта и соответствующий собственный вектор M.

4. Нормируем найденный вектор M на единичную длину - это и будет наш кватернион поворота.


Все последующие работы были, по сути, посвящены вычислительному аспекту этой задачи – как, зная «специфику» данной матрицы, наиболее быстро найти наибольшее собственное число и собственный вектор? Историческим стал метод QUEST (QUaternion ESTimator), предложенный Малкольмом Шустером в конце 70-х годов. Точную дату назвать затруднительно – метод долгое время доводился до ума и реализовывался «в железе» для проекта MagSat, а «на публике» он впервые был представлен в 1978 году на конференции в Калифорнии.

Что интересно, этот самый Шустер первоначально был теоретическим физиком и пришел в задачи ориентации и навигации мало подготовленным. Однако в решении Дэвенпорта он почувствовал что-то знакомое, а именно, стационарное уравнение Шрёдингера, которое, как водится, решается для волновой функции, нормированной на единичку, а найти хочется не что-нибудь, а основное состояние, с минимумом энергии! Да ещё вместо эрмитовой матрицы имеем частный случай – действительную симметричную матрицу, а вместо комплексной волновой функции - частный случай в виде четырёх действительных чисел. Тут-то у него карта и попёрла!

Далее предлагались методы ESOQ1 и ESOQ2 (EStimation Of Quaternion) и многие другие. Кроме основной задачи – определить ориентацию (поворот), рассматривались и другие аспекты – найти матрицу ковариации ошибок (по какой оси ошибка максимальная, и зависят ли они друг от друга) и пр. Как правило, звёздные датчики имеют очень малую ошибку по двум осям, перпендикулярным к оптической оси, и более существенную – вдоль оптической оси, «по крену». Вводилась модель ошибок измерения, и много чего ещё.
Обо всём в этом коротком ликбезе не расскажешь, но есть надежда, что, разобравшись хоть сколько-нибудь в этой главе, читатель сможет без особой опаски прочитать около 90% всех трудов по этой области, ведь самое страшное уже позади.


В следующей части: Линейный (!) метод Мортари-Маркли.
Tags: кватернионы-это просто (том 1), математика, работа
Subscribe

  • А всё-таки есть польза от ковариаций

    Вчера опробовал "сценарий", когда варьируем дальность от 1 метра до 11 метров. Получилось, что грамотное усреднение - это взять с огромными весами…

  • Потёмкинская деревня - 2

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

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

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

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 3 comments