Кватернионы. Решение одной навигационной задачи03.04.2021 18:02

История

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

Рис. 1. Системы координат

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

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

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

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

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

$\mathbf R_i = \begin{bmatrix} x_i & y_i & z_i \end{bmatrix}^T$ : вектор-столбец положения -го НС в ГСК, i = 1, 2, 3 : номер НС,
$\mathbf R_i ^{'} = \begin{bmatrix} x_i^{'} & y_i^{'} & z_i^{'} \end{bmatrix}^T$ : вектор-столбец положения -го НС в координатах ЛСК; векторы $\mathbf R_i$ и $\mathbf R_i^{'}$ полагаем известными; ГСК, ЛСК: правые декартовы системы координат в 3-х мерном евклидовом пространстве E^3 с разнонаправленными базисами $(\mathbf e_x, \mathbf e_y, \mathbf e_z)$ и $( \mathbf e_x^{'}, \mathbf e_y^{'}, \mathbf e_z^{'})$ соответственно; начала координат ГСК и ЛСК не совпадают. Нижний индекс дальше будет обозначать номер вектора, верхний — номер элемента в векторе, если только не указано явно, что это степень.

Задача

Рис. 2. Основные величины

Нужно найти:

$\mathbf R_a = \begin{bmatrix} x_a & y_a & z_a \end{bmatrix}^T$ : вектор-столбец положения ОНВ в ГСК,

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

Решение

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

Разложение векторов $\mathbf R_i$ и $\mathbf R_i^{'}$ в своих базисах: $\mathbf R_i = x_i \mathbf e_x + y_i \mathbf e_y + z_i \mathbf e_z,\quad \mathbf R_i^{'} = x_i^{'} \mathbf e_x^{'} + y_i^{i} \mathbf e_y^{'} + z_i^{'} \mathbf e_z^{'}$ , где i=1,2,3 — номер НС. Базисные векторы ЛСК

$\begin{cases} \mathbf e_x^{'} = a_1^1 \mathbf e_x + a_1^2 \mathbf e_y + a_1^3 \mathbf e_z,\\ \mathbf e_y^{'} = a_2^1 \mathbf e_x + a_2^2 \mathbf e_y + a_2^3 \mathbf e_z,\\ \mathbf e_z^{'} = a_3^1 \mathbf e_x + a_3^2 \mathbf e_y + a_3^3 \mathbf e_z, \end{cases}$

где $\lbrace a_j^i: a_j^i \in \mathcal R \rbrace$ , $\mathcal R$ — множество вещественных чисел, i = 1, 2, 3 . Выписывая эти коэффициенты в матрицу

$\mathbf M = \begin{bmatrix} a_1^1 & a_2^1 & a_3^1\\ a_1^2 & a_2^2 & a_3^2\\ a_1^3 & a_2^3 & a_3^3 \end{bmatrix},$

получаем

$\mathbf e_x^{'} = \mathbf M \mathbf e_x,\; \mathbf e_y^{'} = \mathbf M \mathbf e_y,\; \mathbf e_z^{'} = \mathbf M \mathbf e_z,\;$

полагая, что $\mathbf e_x = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^T$ , $\mathbf e_y = \begin{bmatrix} 0 & 1 & 0 \end{bmatrix}^T$ , $\mathbf e_z = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^T$ . Следовательно, координаты вектора $\mathbf R_i^{'}$ в базисе ГСК $\mathbf R_i^{''} = \mathbf M \mathbf R_i^{'}$ . Можно сказать, что $\mathbf M$ является матрицей некоторого линейного оператора (или «оператора ориентации»), такого, что $E^3 \to E^3$ , определённого в базисе ГСК. Такой матрицей может быть матрица направляющих косинусов или любая из матриц поворотов.

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

$\mathbf R_a = \mathbf R_i - \mathbf M \mathbf R_i^{'}.$ (1)

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

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

$\mathbf R = \mathbf R_a + \mathbf M \mathbf R^{'},\qquad (2)$

где

$\mathbf R = \begin{bmatrix} x_1 & x_2 & x_3\\ y_1 & y_2 & y_3\\ z_1 & z_2 & z_3 \end{bmatrix},\quad \mathbf R_1 = \begin{bmatrix} x_1^{'} & x_2^{'} & x_3^{'}\\ y_1^{'} & y_2^{'} & y_3^{'}\\ z_1^{'} & z_2^{'} & z_3 ^ {'} \end{bmatrix} -$

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

$\mathbf R_a = \mathbf R_i - \mathbf M \mathbf R_i^{'} = \mathbf R_k - \mathbf M \mathbf R_k^{'},\quad i \ne k;\quad i,k=1,2,3,$

откуда

$(x_k^{'} - x_i^{'}) \mathbf M \mathbf e_x + (y_k^{'} - y_i^{'}) \mathbf M \mathbf e_y + (z_k^{'} - z_i^{'}) \mathbf M \mathbf e_z =\\ = (x_k - x_i) \mathbf M \mathbf e_x + (y_k - y_i) \mathbf M \mathbf e_y + (z_k - z_i) \mathbf M \mathbf e_z,$

или

$\mathbf M \mathbf R_{ki}^{'} = \mathbf R_{ki}, \qquad (3)$

где $\mathbf R_{ki}^{'} = \mathbf R_k^{'} - \mathbf R_i^{'}$ .

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

$\begin{cases} \mathbf M \mathbf R_{12}^{'} = \mathbf R_{12},\\ \mathbf M \mathbf R_{13}^{'} = \mathbf R_{13} , \end{cases} \qquad (4)$

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

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

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

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

Кватернионом, как мы знаем, является математический объект вида $\dot q = q^0 + iq^1 + jq^2 + kq^3 = q^0 + \dot{ \mathbf q}$ , где q^0 , q^1 , q^2 , q^3 $\in \mathcal R$ , q^0 — скалярная часть (множитель вещественной единицы), $\dot{\mathbf q}$ — векторная часть; 1, i, j, k — вещественная и три разные мнимые единицы с таблицей умножения:

$\begin{split} 1^2 = 1,\quad 1i = i1,\quad 1j = j1,\quad 1k = k1,\quad i^2 = j^2 = k^2 = -1,\\ ij = -ji = k,\quad jk = -kj = i,\quad ki = -ik = j. \end{split}$

Кватернион даёт удобную возможность представления трёхмерных преобразований (вращений), определяя одновременно и ось поворота, и угол вращения. Если взять некоторый кватернион $\dot q = q^0 + \dot{\mathbf q} = q^0 + iq^1 + jq^2 + kq^3$ , такой, что $\Vert \dot q \Vert = 1$ , то можно записать, что $\Vert \dot q \Vert = q^{(0)^2} + \Vert \dot {\mathbf q } \Vert = 1$ , и значит $\Vert \dot q \Vert = \cos^2 \gamma + \sin^2 \gamma = 1$ , где $\cos^2 \gamma = q^{(0)^2}$ , $\sin^2 \gamma = \Vert \dot { \mathbf q} \Vert = q^{(1)^2} + q^{(2)^2} + q^{(3)^2}$ . Здесь индекс в скобках обозначает номер элемента, а верхний индекс без скобки — возведение в степень. Если вектор $\dot { \mathbf q}$ представить как

$\mathbf{ \dot q} = \frac{1}{\Vert \dot {\mathbf q} \Vert} (q^{(1)^2} + q^{(2)^2} + q^{(3)^2}) \Vert \dot {\mathbf q} \Vert = (q^{(x)^2} + q^{(y)^2} + q^{(z)^2}) \Vert \dot {\mathbf q} \Vert = (q^{(x)^2} + q^{(y)^2} + q^{(z)^2}) \sin^2 \gamma,$

где $q^x = \frac{q^1}{\vert \dot {\mathbf q} \vert}$ , $q^y = \frac{q^2}{\vert \dot {\mathbf q} \vert}$ , $q^z = \frac{q^3}{\vert \dot {\mathbf q} \vert}$ , то кватернион $\dot q$ запишется так:

$\dot q = \cos \gamma + \dot {\mathbf q}_n \sin \gamma,$

где $\dot {\mathbf q}_n = iq^x + jq^y + kq^z$ . Если теперь взять произвольный кватернион $\dot R$ с нулевой скалярной частью и вектором $\dot {\mathbf R}$ , то результатом операции

$\dot R^{'} = \dot q \circ \dot R \circ \dot q^{-1} \qquad (5)$

будет вектор $\dot{\mathbf R}^{'}$ с той же длиной, что и $\dot {\mathbf R}$ , но повёрнутый на угол $2 \gamma$ против часовой стрелки вокруг оси, направляющим вектором которой является $\dot {\mathbf q}_n$ . Дальше будут встречаться фразы вроде «кватернион $\dot q$ выполняет поворот вектора $\dot R$ », которые, конечно, подразумевают применение кватерниона $\dot q$ к вектору $\dot{\mathbf R}$ и получение $\dot{\mathbf R}^{'}$ в соответствии с (5).

Последний теоретический момент. Для обозначения разных видов умножения используются такие общеизвестные значки: $\dot {\mathbf q} \dot {\mathbf r}$ или $\dot {\mathbf q} \cdot \dot {\mathbf r}$ : скалярное произведение, $\dot q \circ \dot r$ : кватернионное произведение, $\dot {\mathbf q} \times \dot {\mathbf r}$ : векторное произведение.

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

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

Вернёмся к векторам $\mathbf R_{ki}^{'}$ и $\mathbf R_{ki}$ . Три замечания об их характере, которые потребуются дальше. Примем пока k = 1, i = 2.

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

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

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

И, в-третьих, $\vert \mathbf R_{12}^{'} \vert = \vert \mathbf R_{12} \vert$ . В самом деле, так как $\mathbf M$ — это ортогональная матрица, которая не меняет длину вектора $\mathbf R_{12}^{'}$ , то из (3) следует, что длины векторов $\mathbf R_{12}^{'}$ и $\mathbf R_{12}$ равны.

Tак как в (3) длины векторов не имеют значения, для упрощения записей и решения дальше заменю векторы $\mathbf R_{12}^{'}$ и $\mathbf R_{12}$ их нормированными эквивалентами:

$\mathbf r_1 \equiv \frac{\mathbf R_{12}}{\vert \mathbf R_{12} \vert} = \begin{bmatrix} r_1^1 & r_1^2 & r_1^3 \end{bmatrix}^T, \qquad \mathbf p_1 \equiv \frac{\mathbf R_{12}^{'}}{\vert \mathbf R_{12}^{'} \vert} = \begin{bmatrix} p_1^1 & p_1^2 & p_1^3 \end{bmatrix}^T,$

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

$\dot r = 0 + \mathbf{\dot r}_1 = 0 + ir_1^1 + jr_1^2 + kr_1^3, \quad \dot p = 0 + \mathbf{\dot p}_1 = 0 + ip_1^1 + jp_1^2 + kp_1^3.$

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

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

$\dot q \circ \dot r_1 \circ \dot q^{-1} = \dot p_1,\qquad (6)$

где $\dot q$ — неизвестный кватернион, эквивалент $\mathbf M$ , а выражение (1) так:

$\dot R_a = \dot R_1 - \dot q \circ \dot R_1 \circ \dot q^{-1}, \qquad (7)$

где $\dot R_a = 0 + ix_a + jy_a + kz_a$ , $\dot R_1 = 0 + ix_1 + jy_1 + kz_1$ .

Рис. 3. Основные векторы

Так как векторы $\dot {\mathbf r}_1$ и $\dot {\mathbf p}_1$ свободны, неколлинеарны, то можно совместить их концы таким образом, чтобы $\dot {\mathbf r}_1$ и $\dot {\mathbf p}_1$ образовали угол $\gamma_1$ на плоскости, натянутой на эти векторы (пусть эта плоскость будет $\lambda_1$ ). Начало координат поместим в точку, общую для $\dot {\mathbf r}_1$ и $\dot {\mathbf p}_1$ (рис. 3), и будем полагать, что значение $\gamma_1$ известно.

Уравнение (6), аналогично уравнению (3), по-прежнему имеет бесконечное множество решений: можно найти сколько угодно различных $\dot q$ , которые удовлетворяют (6). Геометрически это обозначает, что можно найти бесконечное число различных прямых, вокруг которых можно выполнить вращение, совмещающее $\dot {\mathbf r}_1$ с $\dot {\mathbf p}_1$ . На рис. 3 приведён пример, в котором поворот выполняется по кратчайшему пути вокруг прямой с направляющим вектором $\dot {\mathbf d}_1$ , ортогональным плоскости $\lambda_1$ . Очевидно, что $\gamma_{d_1}\ = \gamma_1$ .

На рис. 4, а) и б) приведён пример, в котором вокруг некоторой оси с направляющим вектором $\dot {\mathbf q}_1$ построен круговой конус $\tau_1$ с направляющими прямыми $\dot {\mathbf r}_1$ и $\dot {\mathbf p}_1$ . Ясно, что вокруг такой оси можно сделать поворот, совмещающий $\dot {\mathbf r}_1$ с $\dot {\mathbf p}_1$ , который будет выполнен не по кратчайшему пути (не в плоскости $\lambda_1$ ), и угол $\gamma_{q_1}$ , измеряемый в плоскости основания конуса $\tau_1$ , не будет равен $\gamma_1$ , измеряемый в плоскости $\lambda_1$ .

Рис. 4. Поворот r к p вокруг произвольной оси

Чтобы найти неизвестное значение $\dot q$ , я добавил два новых объекта $\dot r_2 = \dot{R}_3^{'} - \dot{R}_1^{'}$ и $\dot p_2 = \dot R_3 - \dot R_1$ , получаемые из векторов $\mathbf R_3$ и $\mathbf R_1$ . Выражение (6), аналогично (4), расширяется до системы уравнений

$\begin{cases} \dot q \circ \dot r_1 \circ \dot q^{-1} = \dot p_1,\\ \dot q \circ \dot r_2\circ \dot q^{-1} = \dot p_2. \end{cases} \qquad (8)$

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

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

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

Рис. 5. Геометрия задачи

Геометрически задача теперь выглядит так. Нужно найти в пространстве такую ось, вокруг которой можно выполнить поворот, совмещающий вектор $\dot {\mathbf r}_1$ с вектором $\dot {\mathbf p}_1$ , и вектор $\dot {\mathbf r}_2$ с вектором $\dot {\mathbf p}_2$ . Прямые, направляющими векторами которых являются $\dot {\mathbf r}_1$ (или $\dot {\mathbf p}_1$ ) и $\dot {\mathbf r}_2$ (или $\dot {\mathbf p}_2$ ), образуют два различных круговых конуса $\tau_1$ , $\tau_2$ . Эти конусы имеют общую ось, направляющим вектором которой является искомый $\dot {\mathbf q}$ . При этом повороты $\dot {\mathbf r}_i$ к $\dot {\mathbf p}_i$ , i = 1, 2, будут выполняться не по кратчайшему пути, и поэтому углы $\gamma_{q_i}$ , измеренные в плоскости оснований конусов $\tau_i$ , будут отличаться от углов $\gamma_i$ (рис. 5).

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

Утверждение 1. Угол поворота вокруг биссектрисы $\dot {\mathbf b}_1$ такого, который совмещает $\dot {\mathbf r}_1$ с $\dot {\mathbf p}_1$ , равен $\pi$ (рис. 4, в), г)).

Утверждение 2. Поворот, который совмещает $\dot {\mathbf r}_i$ с $\dot {\mathbf p}_i$ , i = 1, 2, вокруг некоторой прямой можно сделать тогда и только тогда, когда эта прямая лежит в плоскости, натянутой на $\dot {\mathbf d}_i$ и $\dot {\mathbf b}_i$ (плоскость $\delta_i$ ), рис. 6). Вокруг любой другой прямой такой поворот выполнить нельзя.

Рис. 6. Плоскости векторов b и d

Для доказательства этих утверждений нужно рассмотреть свойства кругового конуса $\tau_i$ , образованного линией, направляющий вектор которой равен $\dot {\mathbf r}_i$ (или $\dot {\mathbf p}_i$ ), а ось лежит в плоскости $\delta_i$ .

Очевидно, что ось, вокруг которой можно сделать такой поворот, который совместит $\dot {\mathbf r}_i$ с $\dot {\mathbf p}_i$ , i = 1, 2, будет одновременно принадлежать обеим плоскостям $\delta_i$ , то есть будет совпадать с линией их пересечения. Поэтому вектор $\dot {\mathbf q}$ будем искать из уравнения линии пересечения плоскостей $\delta_i$ (рис. 7).

Рис. 7. Линия пересечения плоскостей

Эта прямая не будет совпадать с прямыми, определяемые направляющими векторами $\dot {\mathbf d}_i$ . Проходя через начало координат, она образует некоторый угол $\beta_1$ с вектором $\dot {\mathbf d}_1$ , и угол $\beta_2$ с вектором $\dot {\mathbf d}_2$ (рис. 8). Оба этих угла нам пока неизвестны и $\beta_1 \ne \beta_2$ .

Рис. 8. Все векторы

Посмотрим на рис. 8. Если взять некоторый кватернион $\dot e_{d_1}$ , который поворачивает вектор $\dot {\mathbf r}_1$ на $\pi$ в плоскости $\lambda_1$ , т.е.

$\dot e_{d_1} = 0 + \dot {\mathbf s}_1, \qquad (9)$

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

$\dot e_{q_1}(\beta_i) = \dot h_1(\beta_1) \circ \dot e_{d_1} \circ \dot h_1^{-1}(\beta_1) \qquad (10)$

мы получим вектор $\dot {\mathbf e}_{q_1}(\beta_1)$ , который в свою очередь является кватернионом поворота на угол $\pi$ для вектора $\dot {\mathbf r}_1$ , причём $\dot {\mathbf r}_1$ при этом будет описывать дугу в пространстве. Поворот $\dot {\mathbf e}_{d_1}$ к $\dot {\mathbf e}_{q_1}$ может быть выполнен кватернионом $\dot h_1(\beta_1)$ , который мы найдём чуть позже из условия его ортогональности к плоскости $\delta_1$ .

Из утверждения 2 следует, что вектор $\dot {\mathbf r}_1$ может быть совмещён с $\dot {\mathbf p}_1$ вращением вокруг оси с направляющим вектором $\dot {\mathbf e}_{q_1}(\beta_1)$ на некоторый угол $\gamma_{q_1}$ , который, очевидно, функционально зависит от $\beta_1$ . Поэтому, зная $\beta_1$ и зависимость $\gamma_{q_1} = f(\beta_1)$ , мы сможем построить из кватерниона $\dot {\mathbf e}_{q_i}$ кватернион $\dot q$ , являющийся решением задачи.

Зависимость $\gamma_{q_1} = f(\beta_1)$ мы найдём немного позже из простых тригонометрических соотношений.

Угол $\beta_1$ будем искать из следующих соображений. Так как $\dot {\mathbf q}$ ортогонален одновременно $\dot {\mathbf h}_1$ и $\dot {\mathbf h}_2$ , то результат векторного произведения $\dot {\mathbf h}_1(\beta_1) \times \dot {\mathbf h}_2(\beta_2)$ будет сонаправлен с $\dot {\mathbf q}$ . Обозначим

$\dot {\mathbf q}_e \equiv \dot {\mathbf h}_1(\beta_1) \times \dot {\mathbf h}_2(\beta_2) \qquad (11)$

и запишем такое скалярное произведение: $\dot {\mathbf e}_{q_1}(\beta_1) \dot {\mathbf q}_e$ . Отметим, что направление $\dot {\mathbf q}_e$ не зависит ни от $\beta_1$ , ни от $\beta_2$ . Поэтому для вычисления $\dot {\mathbf q}_e$ примем $\dot h_1 = \dot h_1 (\beta_1) \vert _{\beta_1 = \pi}$ и $\dot h_2 = \dot h_2 (\beta_2) \vert _{\beta_2 = \pi}$ , то есть угол, при котором $\vert \dot {\mathbf h}_i \vert = 1$ . Следовательно, в записанном выше скалярном произведении остаётся одна переменная $\beta_1$ , которую можно вычислить, решив уравнение

$\dot {\mathbf e}_{q_1}(\beta_1) \frac{\dot {\mathbf q}_e}{ \vert \dot {\mathbf q}_e \vert } = \dot {\mathbf e}_{q_1}(\beta_1) \dot{\mathbf q}_{e_{n}} = 1, \qquad (12)$

полагая, что $\vert \dot {\mathbf e}_{q_1} (\beta_1) \vert = 1$ . Теперь, зная $\beta_1$ , зависимость $\gamma_{q_1} = f(\beta_1)$ и вектор $\dot {\mathbf e}_{q_1}(\beta_1)$ , искомый кватернион $\dot {\mathbf q}$ будет выглядеть так:

$\dot {\mathbf q} = \cos \frac{\gamma_{q_1}}{2} + \dot {\mathbf e}_{q_1} \sin \frac{\gamma_{q_1}}{2}. \qquad (13)$

Осталось найти каждый из описанных выше элементов, чтобы решить задачу. Заметим, что объекты $\dot e_{d_2 }$ , $\dot e_{q_2}$ , $\dot h_2$ , $\beta_2$ , $\gamma_2$ , $\gamma_{q_2}$ определяются аналогично. Поэтому далее нижний индекс »1» или »2» буду заменять на » i », подразумевая, что i = 1, 2 .

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

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

кватернион $\dot d_i$ : совмещает вектор $\dot{\mathbf r}_i$ с $\dot{\mathbf p}_i$ по кратчайшей траектории,
кватернион $\dot b_i$ : направляющий вектор биссектрисы угла между векторами $\dot{\mathbf r}_i$ и $\dot{\mathbf p}_i$ ; векторы $\dot{\mathbf d}_i$ и $\dot{\mathbf b}_i$ определяют плоскость $\delta_i$ , в которой лежит искомый кватернион $\dot q$ ,
кватернион $\dot e_{d_i}$ : сонаправлен с $\dot d_i$ ; модуль векторной части $\dot e_{d_i}$ равен 1,
кватернион $\dot h_i(\beta_i)$ : поворачивает вектор $\dot{\mathbf e}_{d_i}$ в плоскости $\delta_i$ на угол $\beta_i$ ; значение $\beta_i$ неизвестно,
зависимость $\gamma_{q_i} = f(\beta_i)$ : вычисляет $\gamma_{q_i}$ для построения искомого кватерниона $\dot q$ из $\dot e_{q_i}$ ,
кватернион $\dot e_{q_i} (\beta_i)$ : получен поворотом вектора $\dot{\mathbf e}_{d_i}$ в плоскости $\delta_i$ на угол $\beta_i$ ; из $\dot e_{q_i}$ будет получено решение,
кватернион $\dot q_e$ : нужен для вычисления угла $\beta_i$ ,
и, наконец, результирующий кватернион $\dot q$ : получается из $\dot e_{q_i}$ , учитывая найденные $\beta_i$ и $\gamma_{q_i}$ .

Кватернион $\dot d_i$

Найдём кватернион $\dot d_i$ , который совмещает $\dot {\mathbf r}_i$ с $\dot {\mathbf p}_i$ по кратчайшей траектории. Вектор $\dot {\mathbf r}_i$ перемещае