Вектор, кватернион

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

Система координат

Оси координат

Для работы с объектами в трехмерном пространстве необходимо определить систему координат. Как известно, система координат задается тремя взаимно перпендикулярными осями, которые обозначаются как X, Y и Z. Порядок обозначения этих осей зависит от того, какую систему координат мы выбрали — левую или правую:

Левая система координат	Правая система координат

В Flix для всех математических расчетов используется правая система координат, что является стандартом в робототехнике и авиации.

Также необходимо выбрать направление осей — в Flix они выбраны в соответствии со стандартом REP-103. Для величин, заданных в подвижной системе координат, связанной с корпусом дрона, применяется порядок FLU:

• ось X — направлена вперед;
• ось Y — направлена влево;
• ось Z — направлена вверх.

Для величин, заданных в мировой системе координат (относительно фиксированной точки в пространстве) — ENU:

• ось X — направлена на восток (условный);
• ось Y — направлена на север (условный);
• ось Z — направлена вверх.

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

Вектор

Вектор — простой геометрический объект, который содержит три значения, соответствующие координатам X, Y и Z. Эти значения называются компонентами вектора. Вектор может описывать точку в пространстве, направление или ось вращения, скорость, ускорение, угловые скорости и другие физические величины. В Flix векторы задаются объектами Vector из библиотеки vector.h:

Vector v(1, 2, 3);
v.x = 5;
v.y = 10;
v.z = 15;

В прошивке в виде векторов представлены, например:

• acc собственное ускорение с акселерометра.
• gyro — угловые скорости с гироскопа.
• rates — рассчитанная угловая скорость дрона.
• accBias, accScale, gyroBias — параметры калибровки IMU.

Операции с векторами

Длина вектора рассчитывается при помощи теоремы Пифагора; в прошивке используется метод norm():

Vector v(3, 4, 5);
float length = v.norm(); // 7.071

Любой вектор можно привести к единичному вектору (сохранить направление, но сделать длину равной 1) при помощи метода normalize():

Vector v(3, 4, 5);
v.normalize(); // 0.424, 0.566, 0.707

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

Vector a(1, 2, 3);
Vector b(4, 5, 6);
Vector sum = a + b; // 5, 7, 9
Vector diff = a - b; // -3, -3, -3

Операция умножения на число n увеличивает (или уменьшает) длину вектора в n раз (сохраняя направление):

Vector a(1, 2, 3);
Vector b = a * 2; // 2, 4, 6

В некоторых случаях полезна операция покомпонентного умножения (или деления) векторов. Например, для применения коэффициентов калибровки к данным с IMU. В разных библиотеках эта операция обозначается по разному, но в библиотеке vector.h используется простые знаки * и /:

acc = acc / accScale;

Угол между векторами можно найти при помощи статического метода Vector::angleBetween():

Vector a(1, 0, 0);
Vector b(0, 1, 0);
float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)

Скалярное произведение

Скалярное произведение векторов (dot product) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком · или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора сонаправлены.

В Flix используется статический метод Vector::dot():

Vector a(1, 2, 3);
Vector b(4, 5, 6);
float dotProduct = Vector::dot(a, b); // 32

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

Векторное произведение

Векторное произведение (cross product) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком ×, а в прошивке используется статический метод Vector::cross():

Vector a(1, 2, 3);
Vector b(4, 5, 6);
Vector crossProduct = Vector::cross(a, b); // -3, 6, -3

Кватернион

Ориентация в трехмерном пространстве

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

Углы Эйлера

Углы Эйлера — крен, тангаж и рыскание — это наиболее «естественный» для человека способ представления ориентации. Они описывают последовательные вращения объекта вокруг трех осей координат.

В прошивке углы Эйлера сохраняются в обычный объект Vector (хоть и, геометрически говоря, не являются вектором):

• Угол по крену (roll) — vector.x.
• Угол по тангажу (pitch) — vector.y.
• Угол по рысканию (yaw) — vector.z.

Особенности углов Эйлера:

1. Углы Эйлера зависят от порядка применения вращений, то есть существует 6 типов углов Эйлера. Порядок вращений, принятый в Flix (и в роботехнике в целом) — рыскание, тангаж, крен (ZYX).
2. Для некоторых ориентаций углы Эйлера «вырождаются». Так, если объект «смотрит» строго вниз, то угол по рысканию и угол по крену становятся неразличимыми. Эта ситуация называется gimbal lock — потеря одной степени свободы.

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

Axis-angle

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

• Ось вращения (axis) — единичный вектор, определяющий ось вращения.
• Угол поворота (angle или θ) — угол, на который нужно повернуть объект вокруг этой оси.

В Flix ось вращения задается объектом Vector, а угол поворота — числом типа float в радианах:

// Вращение на 45 градусов вокруг оси (1, 2, 3)
Vector axis(1, 2, 3);
float angle = radians(45);

Этот способ более удобен для расчетов, чем углы Эйлера, но все еще не является оптимальным.

Вектор вращения

Если умножить вектор axis на угол поворота θ, то получится вектор вращения (rotation vector). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.

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

Вектора вращения в Flix представляются в виде объектов Vector:

// Вращение на 45 градусов вокруг оси (1, 2, 3)
Vector rotation = radians(45) * Vector(1, 2, 3);

Кватернион

Вектор вращения удобен, но еще удобнее использовать кватернион. В Flix кватернионы задаются объектами Quaternion из библиотеки quaternion.h. Кватернион состоит из четырех значений: w, x, y, z и рассчитывается из вектора оси вращения (axis) и угла поворота (θ) по формуле:

\[ q = \left( \begin{array}{c} w \\ x \\ y \\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\ axis_x \cdot \sin\left(\frac{\theta}{2}\right) \\ axis_y \cdot \sin\left(\frac{\theta}{2}\right) \\ axis_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \]

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

Проиллюстрируем кватернион и описанные выше способы представления ориентации на интерактивной визуализации. Изменяйте угол поворота θ с помощью ползунка (ось вращения константна) и изучите, как меняется ориентация объекта, вектор вращения и кватернион:

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

В прошивке в виде кватернионов представлены, например:

• attitude — текущая ориентация квадрокоптера.
• attitudeTarget — целевая ориентация квадрокоптера.

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

Кватернион создается напрямую из четырех его компонент:

// Кватернион, представляющий нулевую (исходную) ориентацию
Quaternion q(1, 0, 0, 0);

Кватернион можно создать из оси вращения и угла поворота, вектора вращения или углов Эйлера:

Quaternion q1 = Quaternion::fromAxisAngle(axis, angle);
Quaternion q2 = Quaternion::fromRotationVector(rotation);
Quaternion q3 = Quaternion::fromEuler(Vector(roll, pitch, yaw));

И наоборот:

q1.toAxisAngle(axis, angle);
Vector rotation = q2.toRotationVector();
Vector euler = q3.toEuler();

Возможно рассчитать вращение между двумя обычными векторами:

Quaternion q = Quaternion::fromBetweenVectors(v1, v2); // в виде кватерниона
Vector rotation = Vector::rotationVectorBetween(v1, v2); // в виде вектора вращения

Шорткаты для работы с углом Эйлера по рысканью (удобно для алгоритмов управления полетом):

float yaw = q.getYaw();
q.setYaw(yaw);

Применения вращений

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

\[ q_1 \times q_2 = \left( \begin{array}{c} w_1 \\ x_1 \\ y_1 \\ z_1 \end{array} \right) \times \left( \begin{array}{c} w_2 \\ x_2 \\ y_2 \\ z_2 \end{array} \right) = \left( \begin{array}{c} w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2 \\ w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2 \\ w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2 \\ w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2 \end{array} \right) \]

В библиотеке quaternion.h для этой операции используется статический метод Quaternion::rotate():

// Композиция вращений q1 и q2
Quaternion result = Quaternion::rotate(q1, q2);

Также полезной является операция применения вращения к вектору, которая делается похожим образом:

// Вращение вектора v кватернионом q
Vector result = Quaternion::rotateVector(v, q);

Для расчета разницы между двумя ориентациями используется метод Quaternion::between():

// Расчет вращения от q1 к q2
Quaternion q = Quaternion::between(q1, q2);

Дополнительные материалы

• Интерактивный учебник по кватернионам.
• Визуализация вращения вектора с помощью кватернионов.