Ликбез по кватернионам, часть 12: навигационная задача: nabbla1

nabbla1

Ликбез по кватернионам, часть 12: навигационная задача

Sep 07, 2018 20:32

Оглавление "Ликбеза по кватернионам":
[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

В этот раз берём пример с Microsoft, у которых за восьмёркой идёт десятка. Часть 11 - "интегрирование угловых скоростей, методы высших порядков" - будет, но попозже.

А сейчас начнём важнейшую тему - астронавигацию.

Пусть у нас есть набор из N «эталонных» единичных векторов $\vec{\mathbf{r}}$
(r - от слова reference - эталон). Это могут быть, к примеру, направления на звёзды, представленные во второй экваториальной системе отсчёта.
Также у нас есть набор из N «измеренных» единичных векторов $\vec{\mathbf{b}}$
(b - от слова oBserved, логика, как всегда, безупречна). В нашем примере это будут направления на звёзды в системе координат звёздного датчика (астроориентатора).
Требуется найти такой поворот, который совместит эталонные вектора $\vec{\mathbf{r}}$
с измеренными векторами $\vec{\mathbf{b}}$
с наименьшей ошибкой.

«Наивные» подходы

Сложно сказать, когда впервые появились эти методы (не удивлюсь, что сразу после появления формализма матриц и векторов), но в статье 1964 года [Black, Harold (July 1964). "A Passive System for Determining the Attitude of a Satellite". AIAA Journal. 2 (7): 1350-1351] они уже упоминались.
Если у нас есть 3 звезды и, соответственно, три эталонных и три измеренных вектора, то мы можем записать набор уравнений:

$A\vec{\mathbf{r}}=\vec{\mathbf{b}},\,\,\,n=1..3$

Здесь A - искомая матрица поворота. Те же уравнения мы можем переписать в матричной форме, собирая 3 вектора в матрицу:

$A\left(\begin{array}{ccc}r_{1x} & r_{2x} & r_{3x} \\ r_{1y} & r_{2y} & r_{2z} \\ r_{1z} & r_{2z} & r_{3z} \end{array} \right) = \left(\begin{array}{ccc}b_{1x} & b_{2x} & b_{3x} \\ b_{1y} & b_{2y} & b_{2z} \\ b_{1z} & b_{2z} & b_{3z} \end{array} \right)$

Отсюда можно найти A:

$A=\left(\begin{array}{ccc}b_{1x} & b_{2x} & b_{3x} \\ b_{1y} & b_{2y} & b_{2z} \\ b_{1z} & b_{2z} & b_{3z} \end{array} \right)\left(\begin{array}{ccc}r_{1x} & r_{2x} & r_{3x} \\ r_{1y} & r_{2y} & r_{2z} \\ r_{1z} & r_{2z} & r_{3z} \end{array} \right)^{-1}$

Если звезды всего 2, то условий для нахождения матрицы как будто не хватит, но можно сделать финт ушами. К эталонным векторам $\vec{\mathbf{r}}_1, \vec{\mathbf{r}}_2$
и измеренным $\vec{\mathbf{b}}_1,\vec{\mathbf{b}}_2$
мы можем добавить «фиктивные» третьи вектора:

$\vec{\mathbf{r}}_3=[\vec{\mathbf{r}}_1, \vec{\mathbf{r}}_2],$

$\vec{\mathbf{b}}_3=[\vec{\mathbf{b}}_1, \vec{\mathbf{b}}_2],$

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

Наконец, если звёзд больше трёх, то мы решаем задачу по методу наименьших квадратов: находим такую матрицу A, чтобы

$\Theta=\sum_{n=1}^{N}{|A\vec{\mathbf{r}}_n-\vec{\mathbf{b}}_n|^2}\rightarrow\,min$

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

Метод «не знает», что от него просят найти поворот, он считает преобразование общего вида. Там, где можно было увидеть очевидную ошибку (угол между измеренными векторами не совпадает с углом между эталонными, тогда как любой поворот сохраняет углы) и не учитывать её, ошибка попадёт на выход и зачастую будет даже усилена.

Приведём пример наведения по двум звёздам. Эталонные вектора:
$\vec{\mathbf{r}}_1(1,0,0),$

$\vec{\mathbf{r}}_2(cos\,\alpha, sin\,\alpha, 0),$

Измеренные вектора:
$\vec{\mathbf{b}}_1(cos\,\varepsilon,-sin\,\varepsilon,0),$

$\vec{\mathbf{b}}_2(cos(\alpha+\varepsilon), sin(\alpha+\varepsilon),0),$

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

Найдём третьи «фиктивные» вектора:
$\vec{\mathbf{r}}_3=[\vec{\mathbf{r}}_1,\vec{\mathbf{r}}_2]=(0,0,sin\,\alpha)^T,$

$\vec{\mathbf{b}}_3=[\vec{\mathbf{b}}_1,\vec{\mathbf{b}}_2]=(0,0,sin\,\alpha+2\varepsilon cos\,\alpha)^T$

(считаем ε малой величиной и отбрасываем слагаемые второго порядка малости во всех векторах)
Теперь уравнение записывается в виде

$A\left(\begin{array}{ccc}1&cos\,\alpha&0\\0&sin\,\alpha&0\\0&0&sin\,\alpha\end{array}\right)=\left(\begin{array}{ccc}1&cos\,\alpha-{\varepsilon}sin\,\alpha&0\\-\varepsilon&sin\,\alpha+{\varepsilon}cos\,\alpha&0\\0&0&sin\,\alpha+2{\varepsilon}cos\,\alpha\end{array}\right)$

Взяв обратную матрицу, находим ответ:

$A=\left(\begin{array}{ccc}1&cos\,\alpha-{\varepsilon}sin\,\alpha&0\\-\varepsilon&sin\,\alpha+{\varepsilon}cos\,\alpha&0\\0&0&sin\,\alpha+2{\varepsilon}cos\,\alpha\end{array}\right)\left(\begin{array}{ccc}1&-ctg\,\alpha&0\\-\varepsilon&1+2{\varepsilon}ctg\,\alpha&0\\0&0&1+2{\varepsilon}ctg\,\alpha\end{array}\right)=$

$=\left(\begin{array}{ccc}1&-\varepsilon&0\\-\varepsilon&1+2{\varepsilon}ctg\,\alpha&0\\0&0&1+2{\varepsilon}ctg\,\alpha\end{array}\right)$

Возьмём для примера $\alpha=10^\circ,\,\varepsilon=10''$
. Приходим к матрице:
$A=\left(\begin{array}{ccc}1&-4,85{\times}10^{-5}&0\\-4,85{\times}10^{-5}&1,00055&0\\0&0&1,00055\end{array}\right)$

Грубо говоря, мы имеем масштабирование по осям Y и Z на 0,055%. Если мы возьмём вектор, наклонённый к оси X под углом 45°:
$\vec{\mathbf{u}}(\frac{\sqrt{2}}{2},0,\frac{\sqrt{2}}{2})$

и применим к нему матрицу A, то получим вектор

$\vec{\mathbf{v}}(0,707107;3,42{\times}10^{-5};0,707495)$

угол наклона которого теперь составляет 45°0’57’’.

Итак, из-за ошибки в 10 угловых секунд в исходных данных мы получаем ошибку наведения почти в 6 раз больше, тогда как более совершенные методы вообще проигнорировали бы «противофазную» ошибку, продемонстрированную здесь, поскольку эта ошибка направлена на увеличение угла между векторами - поворот такого делать «не умеет», а нам нужно было найти именно поворот!

Если расставить эталонные векторы подальше, желательно под прямым углом (α=90°), то эффект будет не столь значительным:

$A=\left(\begin{array}{ccc}1&-4,85{\times}10^{-5}&0\\-4,85{\times}10^{-5}&1&0\\0&0&1\end{array}\right)$

Здесь мы имеем разный масштаб по направлениям, развёрнутым под 45° относительно осей X-Y.
Попробуем повернуть простейший вектор:

$\vec{\mathbf{u}}(1;0;0)$

в итоге получится

$\vec{\mathbf{v}}(1;-4,85{\times}10^{-5};0)$

то есть, он повернулся на те же самые 10 угловых секунд. Ошибка никуда не делась, но хотя бы не стала усиливаться. В данной ситуации звездный датчик, реализующий «наивные методы», будет давать «всего лишь» вдвое худшую точность в сравнении с методами, рассмотренными далее. Так получается, поскольку ошибки в измерении каждого направления можно разложить на «синфазную» и «противофазную» компоненты, которые независимы друг от друга. С «синфазной» составляющей ничего поделать нельзя: она выглядит в точности как поворот в пространстве. Но вот «противофазную» составляющую правильные алгоритмы в принципе не пропускают на выход!

Совсем неудовлетворительные результаты будут получаться при попытке применить данные методы в узкопольном звёздном датчике, где расстояния между эталонными векторами минимальны, и ctg(α), на который домножается угловая ошибка, приобретает совсем уж неприличные значения!

Поэтому данные методы упоминались в статьях исключительно как пример - «никогда так не делайте!».

Формулировка оптимизационной задачи

Чтобы двигаться дальше, надо чётко сформулировать, что мы вообще пытаемся найти. Методы, наиболее распространённые в настоящее время, выводились из следующей формулировки:

Требуется найти матрицу R, такую, что RTR = E, det(R) = 1 (иными словами, это должна быть матрица поворота!) и минимизирующую функцию ошибки:

$L_w=\frac{1}{2}\sum_{n=1}^{N}w_n(R\vec{\mathbf{r}}_n-\vec{\mathbf{b}}_n)^T(R\vec{\mathbf{r}}_n-\vec{\mathbf{b}}_n)$

Здесь wn - весовые коэффициенты, как правило, обратно пропорциональные ожидаемой дисперсии данного измерения. Коэффициент 1/2, разумеется, ни на что не влияет и поставлен сюда пост-фактум, после того, как стало ясно, что из-под суммы «выползет» двойка.

Данная задача в англоязычной литературе называется проблемой Вахбы (Wahba problem) в честь Грейс Вахбы (Grace Wahba), которая сформулировала задачу в 1965 году [Wahba, G., “Problem 65-1: A Least Squares Estimate of Spacecraft Attitude,” SIAM Review, Vol. 7, No. 3, July 1965, pp. 409]. В оригинальной постановке отсутствовали весовые коэффициенты, их добавили позже. Приемлемого решения в статье не содержалось, а год спустя после публикации, Грейс навсегда ушла из задач навигации и ориентации космических аппаратов в математическую статистику. Как говорится, «моё дело - предложить концепцию…».

Начало выкладок, как правило, одинаковое. Вносим знак транспонирования внутрь скобки:

$L_w=\frac{1}{2}\sum_{n=1}^{N}w_n(\vec{\mathbf{r}}_n^{T}R^T-\vec{\mathbf{b}}_n^T)(R\vec{\mathbf{r}}_n-\vec{\mathbf{b}}_n)$

Раскрываем скобки:

$L_w=\frac{1}{2}\sum_{n=1}^{N}w_n(\vec{\mathbf{r}}_n^{T}R^{T}R\vec{\mathbf{r}}_n-\vec{\mathbf{b}}_n^{T}R\vec{\mathbf{r}}_n-\vec{\mathbf{r}}_n^{T}R^{T}\vec{\mathbf{b}}_n+\vec{\mathbf{b}}_n^T\vec{\mathbf{b}}_n)$

Поскольку R-матрица поворота, имеем RTR=E. Кроме того, поскольку все слагаемые внутри скобки - обычные числа (скаляры), то любое из них можно «транспонировать». Мы транспонируем третье. В результате получаем:

$L_w=\frac{1}{2}\sum_{n=1}^{N}w_n(\vec{\mathbf{r}}_n^{T}\vec{\mathbf{r}}_n-2\vec{\mathbf{b}}_n^{T}R\vec{\mathbf{r}}_n+\vec{\mathbf{b}}_n^{T}\vec{\mathbf{b}}_n)\rightarrow{min}$

Первое и третье слагаемые не зависят от R, поэтому их можно выкинуть, сведя задачу к нахождению максимума:

$G_w=\sum_{n=1}^{N}w_n\vec{\mathbf{b}}_n^{T}R\vec{\mathbf{r}}_n\rightarrow{max}$

Буква L - от слова loss, G - от слова gain, индекс w значит Wahba, поскольку можно поставить оптимизационную задачу и по-другому.

Это были достаточно очевидные выкладки. В исходной формулировке мы брали вектор разности между наблюдаемым направлением и «эталонным», повёрнутым с помощью матрицы R, и пытались минимизировать сумму квадратов длин этих векторов (с определенными весами). Теперь мы показали, что с тем же успехом можно максимизировать сумму скалярных произведений между наблюдаемыми направлениями и «эталонными», повёрнутыми матрицей R, это одна и та же задача.

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

$\left(\begin{array}{ccc}cos\,\psi\,cos\,\varphi-sin\,\psi\,cos\,\vartheta\,sin\varphi&cos\,\psi\,sin\,\varphi-sin\,\psi\,cos\,\vartheta\,cos\,\varphi&sin\,\psi\,sin\,\vartheta\\-sin\,\psi\,cos\,\varphi-cos\,\psi\,cos\,\vartheta\,sin\,\varphi&-sin\,\psi\,sin\,\varphi+cos\,\psi\,cos\,\vartheta\,cos\,\varphi&cos\,\psi\,sin\,\vartheta\\sin\,\vartheta\,sin\,\varphi&-sin\,\vartheta\,cos\,\varphi&cos\,\vartheta\end{array}\right)$

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

Читайте в следующей части: Дэвенпорт берёт след!

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