История

Некоторое время назад я занимался одной интересной задачей, относящейся к спутниковой навигации. Используя фазовый фронт сигнала, объект навигации (ОНВ) измеряет координаты навигационных спутников (НС) в своей системе координат (локальная система, ЛСК). Также ОНВ получает значения положений НС в глобальной системе координат (ГСК), и измеряет время получения сигнала НС (рис. 1). Требовалось вычислить координаты ОНВ в ГСК и системное время, то есть решить навигационную задачу.

Задача была интересна тем, что её решение теоретически позволяет уменьшить число НС в сравнении с тем, сколько НС требуется в методах, реализованных в спутниковых системах навигации. Своё внимание в то время я в основном уделял исследованию качества измерений фазового фронта и получению навигационных уравнений для координат и времени, полагая при этом, что вычисление ориентации и координат ОНВ не вызовет особых проблем. Тем более, что на плоскости задача решалась быстро и просто.

Однако, когда я построил модель в трёхмерном пространстве, неожиданно выяснилось, что вычислить значения ориентации ОНВ при неизвестных его координатах в ГСК не получается. Несколько предпринятых попыток определить ориентацию с помощью матриц направляющих косинусов и поворотов привели к такому нагромождению тригонометрических функций, что продвигаться дальше к решению у меня не получалось. Какое-то время даже казалось, что аналитического решения вообще не существует.

Но оно, конечно, существует. Мне удалось найти решение этой задачи, используя свойства кватернионов. В этом материале я хочу описать саму задачу, ход и её решение, уделяя внимание ориентации и координатам ОНВ, и пока оставляя за рамками измерения координат по фазовому фронту.

Входные данные

Итак, входные данные:

: вектор-столбец положения -го НС в ГСК, : номер НС,
: вектор-столбец положения -го НС в координатах ЛСК; векторы и полагаем известными; ГСК, ЛСК: правые декартовы системы координат в 3-х мерном евклидовом пространстве с разнонаправленными базисами и соответственно; начала координат ГСК и ЛСК не совпадают. Нижний индекс дальше будет обозначать номер вектора, верхний - номер элемента в векторе, если только не указано явно, что это степень.

Задача

Нужно найти:

: вектор-столбец положения ОНВ в ГСК,

: оператор перехода от ЛСК к ГСК, который я условно назвал "оператором ориентации" (рис. 2)

Решение

Теперь ход моих рассуждений и решения.

Разложение векторов и в своих базисах: , где - номер НС. Базисные векторы ЛСК

где , - множество вещественных чисел, . Выписывая эти коэффициенты в матрицу

получаем

полагая, что , , . Следовательно, координаты вектора в базисе ГСК . Можно сказать, что является матрицей некоторого линейного оператора (или "оператора ориентации"), такого, что , определённого в базисе ГСК. Такой матрицей может быть матрица направляющих косинусов или любая из матриц поворотов.

Свойства евклидового пространства позволяют записать уравнение для вычисления вектора положения ОНВ:

(1)

Неудачный поиск решения

Уравнение (1) содержит две неизвестные матричные величины и , и имеет поэтому бесконечное число решений. Аналогичное соотношение для трёх различных НС в виде

где

квадратные невырожденные матрицы, также содержат две неизвестные матричные величины и . Можно переписать (1) и (2) так, чтобы избавиться от величины :

откуда

или

где .

Если бы я смог как-нибудь найти из (3) и подставить в (1), то задача была бы решена. К примеру, была сделана попытка расписать (3) по трём НС аналогично с (2), но в итоге матрицы получались вырожденные и уравнение единственного решения поэтому не имело. Попытки расписать и решить систему уравнение вроде

приводили к тому самому нагромождению синусов и косинусов, о котором я упомянул во вступлении.

Здесь я и подумал, а получится ли найти , если (3) или (4) записать в кватернионном виде. В итоге получилось, но продолжу по порядку.

Теория о кватернионе поворота

Два теоретических момента, которые, думаю, стоит упомянуть.

Кватернионом, как мы знаем, является математический объект вида , где , , , , - скалярная часть (множитель вещественной единицы), - векторная часть; 1, i, j, k - вещественная и три разные мнимые единицы с таблицей умножения:

Кватернион даёт удобную возможность представления трёхмерных преобразований (вращений), определяя одновременно и ось поворота, и угол вращения. Если взять некоторый кватернион , такой, что , то можно записать, что , и значит , где , . Здесь индекс в скобках обозначает номер элемента, а верхний индекс без скобки - возведение в степень. Если вектор представить как

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

где . Если теперь взять произвольный кватернион с нулевой скалярной частью и вектором , то результатом операции

будет вектор с той же длиной, что и , но повёрнутый на угол против часовой стрелки вокруг оси, направляющим вектором которой является . Дальше будут встречаться фразы вроде "кватернион выполняет поворот вектора ", которые, конечно, подразумевают применение кватерниона к вектору и получение в соответствии с (5).

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

На этом с теорией всё.

Описание решения с кватернионами

Вернёмся к векторам и . Три замечания об их характере, которые потребуются дальше. Примем пока k = 1 , i = 2.

Нужные замечания

Во-первых, эти векторы являются свободными векторами, которые можно перемещать в пространстве, соблюдая параллельность перемещений.

Во-вторых, они неколлинеарны. Действительно, если бы они были коллинеарными, то (3) обращалось бы в истинное высказывание только при единичной матрице и задачу решать не нужно было.

И, в-третьих, . В самом деле, так как - это ортогональная матрица, которая не меняет длину вектора , то из (3) следует, что длины векторов и равны.

Tак как в (3) длины векторов не имеют значения, для упрощения записей и решения дальше заменю векторы и их нормированными эквивалентами:

и в виде кватернионов:

Кватернионная форма основных уравнений

Выражение (3) в кватернионной форме выглядит теперь так:

где - неизвестный кватернион, эквивалент , а выражение (1) так:

где , .

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

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

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

Чтобы найти неизвестное значение , я добавил два новых объекта:

и ,

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

Теперь можно утверждать, что кватернион , найденный из этой системы уравнений (8), является единственным решением, и он выполняет такой поворот в пространстве, который совмещает тройку векторов базиса ЛСК с векторами ГСК. Следовательно, его можно подставить в выражение (7) и вычислить искомое - положение ОНВ.

Когда я записал (8), отчего-то стало ясно, что здесь безразмерная куча тригонометрии может не возникнуть, и аналитическое решение поэтому можно будет найти более-менее просто.

Геометрия задачи

Геометрически задача теперь выглядит так. Нужно найти в пространстве такую ось, вокруг которой можно выполнить поворот, совмещающий вектор с вектором , и вектор с вектором . Прямые, направляющими векторами которых являются (или ) и (или ), образуют два различных круговых конуса , . Эти конусы имеют общую ось, направляющим вектором которой является искомый . При этом повороты к , i = 1, 2, будут выполняться не по кратчайшему пути, и поэтому углы , измеренные в плоскости оснований конусов , будут отличаться от углов (рис. 5).

Здесь и далее нам понадобятся два таких утверждения.

Утверждение 1. Угол поворота вокруг биссектрисы такого, который совмещает с , равен (рис. 4, в), г)).

Утверждение 2. Поворот, который совмещает с , i = 1, 2, вокруг некоторой прямой можно сделать тогда и только тогда, когда эта прямая лежит в плоскости, натянутой на и (плоскость ), рис. 6). Вокруг любой другой прямой такой поворот выполнить нельзя.

Для доказательства этих утверждений нужно рассмотреть свойства кругового конуса , образованного линией, направляющий вектор которой равен (или ), а ось лежит в плоскости .

Очевидно, что ось, вокруг которой можно сделать такой поворот, который совместит с , i = 1, 2, будет одновременно принадлежать обеим плоскостям , то есть будет совпадать с линией их пересечения. Поэтому вектор будем искать из уравнения линии пересечения плоскостей (рис. 7).

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

Посмотрим на рис. 8. Если взять некоторый кватернион , который поворачивает вектор на в плоскости , т.е.

и повернуть его вокруг начала координат на угол в плоскости , то из

мы получим вектор , который в свою очередь является кватернионом поворота на угол для вектора , причём при этом будет описывать дугу в пространстве. Поворот к может быть выполнен кватернионом , который мы найдём чуть позже из условия его ортогональности к плоскости .

Из утверждения 2 следует, что вектор может быть совмещён с вращением вокруг оси с направляющим вектором на некоторый угол , который, очевидно, функционально зависит от . Поэтому, зная и зависимость , мы сможем построить из кватерниона кватернион , являющийся решением задачи.

Зависимость мы найдём немного позже из простых тригонометрических соотношений.

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

и запишем такое скалярное произведение: . Отметим, что направление не зависит ни от , ни от . Поэтому для вычисления примем и , то есть угол, при котором . Следовательно, в записанном выше скалярном произведении остаётся одна переменная , которую можно вычислить, решив уравнение

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

Осталось найти каждый из описанных выше элементов, чтобы решить задачу. Заметим, что объекты , , , , , определяются аналогично. Поэтому далее нижний индекс "1" или "2" буду заменять на " i ", подразумевая, что i = 1, 2 .

Кватернионы, которые будем искать

Ещё раз перечислим кватернионы и векторы, которые мы сейчас будем строить для получения решения:

• кватернион : совмещает вектор с по кратчайшей траектории,

• кватернион : направляющий вектор биссектрисы угла между векторами и ; векторы и определяют плоскость , в которой лежит искомый кватернион ,

• кватернион : сонаправлен с ; модуль векторной части равен 1,

• кватернион : поворачивает вектор в плоскости на угол ; значение неизвестно,

• зависимость : вычисляет для построения искомого кватерниона из ,

• кватернион : получен поворотом вектора в плоскости на угол ; из будет получено решение,

• кватернион : нужен для вычисления угла ,

• и, наконец, результирующий кватернион : получается из , учитывая найденные и .

Кватернион

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

где - единичный вектор, равный

где

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

и

Кватернион

Найдём кватернион . Он может быть получен из преобразования

где - кватернион, выполняющий поворот вектора на угол . Учитывая (17), он равен

Подставим из (19) в (18), выполним кватернионные умножения и, учитывая (14), получим:

Применяя правило "БАЦ минус ЦАБ" и упрощая, получаем

Заметим, что .

Зависимость

Найдём зависимость . Возьмём конус из рис. 6, для удобства развернём его основанием вниз и изобразим все основные векторы и углы (рис. 9). Также добавим угол .

Будем находить зависимость угла , соответствующего углу поворота вокруг от к , от значения угла , то есть от угла наклона к плоскости векторов , (т.е. к плоскости ). Заметим, что при поворот выполняется по кратчайшей траектории вокруг .

Из соотношений прямоугольных треугольников запишем:

откуда . Так как ), то

откуда

Из выражения (21) видно, что при значение , а при значение , что согласуется с утверждением 1. Заметим также, что угол не зависит от длин векторов, а угол полагается известным.

Кватернион

Продолжим. Построим кватернион . Поскольку он выполняет поворот на угол в плоскости , то

полагая, что , . Учитывая (9), (18), выполнив некоторые преобразования, получим

После упрощающих тригонометрических преобразований, подставляя в (22), получаем

при этом .

Кватернион

Найдём кватернион . Выше мы записали выражение (9), которое определяет . Приведём его здесь ещё раз: . Подставляя сюда полученный результат (23) и выражение (9), после преобразований получим:

где , , . Упрощая, получаем:

при этом .

Кватернион

Получим кватернион . Как мы указывали выше, вектор этого кватерниона ортогонален каждому из векторов и , то есть (выражение (11)), и при этом . Также мы отмечали, что угол не меняет направление вектора , и поэтому можно выбрать такое , при котором длина максимальна. Учитывая (23) при , получаем:

следовательно,

Нормированное значение

Угол

Подставляем результаты в выражение (12) и получаем уравнение, которое нужно решить относительно :

После преобразований получаем это уравнение в форме , которое и решаем:

где

Решение

Зная , можно построить кватернион из (24), подставить его в (13) и записать результат в виде:

где

а вычисляется из выражения (29).

Послесловие

Задача решена. Я попытался упростить итоговое выражение (30), но пока не получилось. В моделях оно работает, поэтому пока оставлю, как есть. На рис. 10 приведён простой пример нахождения ориентации ЛСК для того, чтобы затем определить .

Вначале положение ЛСК неизвестно, и мы полагаем, что оно может быть любым или, например, совпадающим с ГСК (то есть, мы "думаем", что оно совпадает, а на самом деле это не так). Измеренные координаты НС1', НС2' и НС3' существенно отличаются от истинных положений НС1, НС2 и НС3 (рис. 10, а)). Зная координаты НС в ГСК, вычисляется кватернион поворота по (30) и выполняется вращение. На рис. 10, б) показано дискретное вращение с интервалом 10 градусов от старого положения ЛСК к истинному. При этом измеренные координаты НС (показаны светло-серым цветом) всё более приближаются к истинным. На рис. 10, в) показано вычисленное истинное положение ЛСК, и мы теперь можем определить , то есть решить навигационную задачу (точнее, часть навигационной задачи, связанной с определением координат).

На этом пока всё. Отмечу только, что в ближайшем будущем я попробую поработать над одним недостатком выражения (30). При близким к нулю, то есть когда ориентации ГСК и ЛСК мало отличаются, кватернион вычисляется с ошибкой из-за множителя . Это может приводить к значительным ошибкам вычисления ориентации ЛСК и, как результат, ошибкам определения положения . Об этом в следующем материале.