mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 09:39:33 +00:00
310 lines
22 KiB
Markdown
310 lines
22 KiB
Markdown
# Вектор, кватернион
|
||
|
||
В алгоритме управления квадрокоптером широко применяются геометрические (и алгебраические) объекты, такие как **векторы** и **кватернионы**. Они позволяют упростить математические вычисления и улучшить читаемость кода. В этой главе мы рассмотрим именно те геометрические объекты, которые используются в алгоритме управления квадрокоптером Flix, причем акцент будет сделан на практических аспектах их использования.
|
||
|
||
## Система координат
|
||
|
||
### Оси координат
|
||
|
||
Для работы с объектами в трехмерном пространстве необходимо определить *систему координат*. Как известно, система координат задается тремя взаимно перпендикулярными осями, которые обозначаются как *X*, *Y* и *Z*. Порядок обозначения этих осей зависит от того, какую систему координат мы выбрали — *левую* или *правую*:
|
||
|
||
|Левая система координат|Правая система координат|
|
||
|-----------------------|------------------------|
|
||
|<img src="img/left-axes.svg" alt="Левая система координат" width="200">|<img src="img/right-axes.svg" alt="Правая система координат" width="200">|
|
||
|
||
В Flix для всех математических расчетов используется **правая система координат**, что является стандартом в робототехнике и авиации.
|
||
|
||
Также необходимо выбрать направление осей — в Flix они выбраны в соответствии со стандартом [REP-103](https://www.ros.org/reps/rep-0103.html). Для величин, заданных в подвижной системе координат, связанной с корпусом дрона, применяется порядок <abbr title="Forward Left Up">FLU</abbr>:
|
||
|
||
* ось X — направлена **вперед**;
|
||
* ось Y — направлена **влево**;
|
||
* ось Z — направлена **вверх**.
|
||
|
||
Для величин, заданных в *мировой* системе координат (относительно фиксированной точки в пространстве) — <abbr title="East North Up">ENU</abbr>:
|
||
|
||
* ось X — направлена на **восток** (условный);
|
||
* ось Y — направлена на **север** (условный);
|
||
* ось Z — направлена **вверх**.
|
||
|
||
> [!NOTE]
|
||
> Для системы ENU важно только взаимное направление осей. Если доступен магнитометр, то используются реальные восток и север, но если нет — то произвольно выбранные.
|
||
|
||
Углы и угловые скорости определяются в соответствии с правилами математики: значения увеличиваются против часовой стрелки, если смотреть в сторону начала координат. Общий вид системы координат:
|
||
|
||
<img src="img/axes-rotation.svg" alt="Система координат" width="200">
|
||
|
||
> [!TIP]
|
||
> Оси координат <i>X</i>, <i>Y</i> и <i>Z</i> часто обозначаются красными, зелеными и синими цветами соответственно. Запомнить это можно с помощью сокращения <abbr title="Red Green Blue">RGB</abbr>.
|
||
|
||
## Вектор
|
||
|
||
<div class="firmware">
|
||
<strong>Файл прошивки:</strong>
|
||
<a href="https://github.com/okalachev/flix/blob/master/flix/vector.h"><code>vector.h</code></a>.<br>
|
||
</div>
|
||
|
||
**Вектор** — простой геометрический объект, который содержит три значения, соответствующие координатам *X*, *Y* и *Z*. Эти значения называются *компонентами вектора*. Вектор может описывать точку в пространстве, направление или ось вращения, скорость, ускорение, угловые скорости и другие физические величины. В Flix векторы задаются объектами `Vector` из библиотеки `vector.h`:
|
||
|
||
```cpp
|
||
Vector v(1, 2, 3);
|
||
v.x = 5;
|
||
v.y = 10;
|
||
v.z = 15;
|
||
```
|
||
|
||
> [!TIP]
|
||
> Не следует путать геометрический вектор — <code>vector</code> и динамический массив в стандартной библиотеке C++ — <code>std::vector</code>.
|
||
|
||
В прошивке в виде векторов представлены, например:
|
||
|
||
* `acc` — истинное ускорение с акселерометра.
|
||
* `gyro` — угловые скорости с гироскопа.
|
||
* `rates` — рассчитанная угловая скорость дрона.
|
||
* `accBias`, `accScale`, `gyroBias` — параметры калибровки IMU.
|
||
|
||
### Операции с векторами
|
||
|
||
**Длина вектора** рассчитывается при помощи теоремы Пифагора; в прошивке используется метод `norm()`:
|
||
|
||
```cpp
|
||
Vector v(3, 4, 5);
|
||
float length = v.norm(); // 7.071
|
||
```
|
||
|
||
Любой вектор можно привести к **единичному вектору** (сохранить направление, но сделать длину равной 1) при помощи метода `normalize()`:
|
||
|
||
```cpp
|
||
Vector v(3, 4, 5);
|
||
v.normalize(); // 0.424, 0.566, 0.707
|
||
```
|
||
|
||
**Сложение и вычитание** векторов реализуется через простое покомпонентное сложение и вычитание. Геометрически сумма векторов представляет собой вектор, который соединяет начало первого вектора с концом второго. Разность векторов представляет собой вектор, который соединяет конец первого вектора с концом второго. Это удобно для расчета относительных позиций, суммарных скоростей и решения других задач. В коде эти операции интуитивно понятны:
|
||
|
||
```cpp
|
||
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` раз (сохраняя направление):
|
||
|
||
```cpp
|
||
Vector a(1, 2, 3);
|
||
Vector b = a * 2; // 2, 4, 6
|
||
```
|
||
|
||
В некоторых случаях полезна операция **покомпонентного умножения** (или деления) векторов. Например, для применения коэффициентов калибровки к данным с IMU. В разных библиотеках эта операция обозначается по разному, но в библиотеке `vector.h` используется простые знаки `*` и `/`:
|
||
|
||
```cpp
|
||
acc = acc / accScale;
|
||
```
|
||
|
||
**Угол между векторами** можно найти при помощи статического метода `Vector::angleBetween()`:
|
||
|
||
```cpp
|
||
Vector a(1, 0, 0);
|
||
Vector b(0, 1, 0);
|
||
float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)
|
||
```
|
||
|
||
#### Скалярное произведение
|
||
|
||
Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
|
||
|
||
В Flix используется статический метод `Vector::dot()`:
|
||
|
||
```cpp
|
||
Vector a(1, 2, 3);
|
||
Vector b(4, 5, 6);
|
||
float dotProduct = Vector::dot(a, b); // 32
|
||
```
|
||
|
||
Операция скалярного произведения может помочь, например, при расчете проекции одного вектора на другой.
|
||
|
||
### Векторное произведение
|
||
|
||
Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке — статический метод `Vector::cross()`:
|
||
|
||
```cpp
|
||
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* — потеря одной степени свободы.
|
||
|
||
Ввиду этих особенности для углов Эйлера не существует общих формул для самых базовых задач с ориентациями, таких как применение одного вращения (ориентации) к другому, расчет разницы между ориентациями и подобных. Поэтому в основном углы Эйлера применяются в пользовательском интерфейсе, но редко используются в математических расчетах.
|
||
|
||
> [!IMPORTANT]
|
||
> Для углов Эйлера не существует общих формул для самых базовых операций с ориентациями.
|
||
|
||
### Axis-angle
|
||
|
||
Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
|
||
|
||
* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.
|
||
* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.
|
||
|
||
В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
|
||
|
||
```cpp
|
||
// Вращение на 45 градусов вокруг оси (1, 2, 3)
|
||
Vector axis(1, 2, 3);
|
||
float angle = radians(45);
|
||
```
|
||
|
||
Этот способ более удобен для расчетов, чем углы Эйлера, но все еще не является оптимальным.
|
||
|
||
### Вектор вращения
|
||
|
||
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
|
||
|
||
Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
|
||
|
||
> [!IMPORTANT]
|
||
> Чтобы за единичное время прийти к заданной ориентации, собственные угловые скорости объекта должны быть равны компонентам вектора вращения.
|
||
|
||
Вектора вращения в Flix представляются в виде объектов `Vector`:
|
||
|
||
```cpp
|
||
// Вращение на 45 градусов вокруг оси (1, 2, 3)
|
||
Vector rotation = radians(45) * Vector(1, 2, 3);
|
||
```
|
||
|
||
### Кватернион
|
||
|
||
<div class="firmware">
|
||
<strong>Файл прошивки:</strong>
|
||
<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
|
||
</div>
|
||
|
||
Вектор вращения удобен, но для математических расчетов еще удобнее использовать **кватернион**. В 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) \\]
|
||
|
||
На практике оказывается, что **именно такое представление наиболее удобно для математических расчетов**.
|
||
|
||
Проиллюстрируем кватернион и остальные описанные выше способы представления ориентации с помощью интерактивной визуализации. Изменяйте угол поворота *θ* с помощью ползунка (ось вращения константна) и изучите, как меняется ориентация объекта, вектор вращения и кватернион:
|
||
|
||
<div id="rotation-diagram" class="diagram">
|
||
<p>
|
||
<label class="angle" for="angle-range"></label>
|
||
<input type="range" name="angle" id="angle-range" min="0" max="360" value="0" step="1">
|
||
</p>
|
||
<p class="axis"></p>
|
||
<p class="rotation-vector"></p>
|
||
<p class="quaternion"></p>
|
||
<p class="euler"></p>
|
||
</div>
|
||
|
||
<script type="importmap">
|
||
{
|
||
"imports": {
|
||
"three": "https://cdn.jsdelivr.net/npm/three@0.176.0/build/three.module.js",
|
||
"three/addons/": "https://cdn.jsdelivr.net/npm/three@0.176.0/examples/jsm/"
|
||
}
|
||
}
|
||
</script>
|
||
<script type="module" src="js/rotation.js"></script>
|
||
|
||
> [!IMPORTANT]
|
||
> В контексте управляющих алгоритмов кватернион — это оптимизированный для расчетов аналог вектора вращения.
|
||
|
||
Кватернион это наиболее часто используемый способ представления ориентации в алгоритмах. Кроме этого, у кватерниона есть большое значение в теории чисел и алгебре, как у расширения понятия комплексного числа, но рассмотрение этого аспекта выходит за рамки описания работы с вращениями с практической точки зрения.
|
||
|
||
В прошивке в виде кватернионов представлены, например:
|
||
|
||
* `attitude` — текущая ориентация квадрокоптера.
|
||
* `attitudeTarget` — целевая ориентация квадрокоптера.
|
||
|
||
### Операции с кватернионами
|
||
|
||
Кватернион создается напрямую из четырех его компонент:
|
||
|
||
```cpp
|
||
// Кватернион, представляющий нулевую (исходную) ориентацию
|
||
Quaternion q(1, 0, 0, 0);
|
||
```
|
||
|
||
Кватернион можно создать из оси вращения и угла поворота, вектора вращения или углов Эйлера:
|
||
|
||
```cpp
|
||
Quaternion q1 = Quaternion::fromAxisAngle(axis, angle);
|
||
Quaternion q2 = Quaternion::fromRotationVector(rotation);
|
||
Quaternion q3 = Quaternion::fromEuler(Vector(roll, pitch, yaw));
|
||
```
|
||
|
||
И наоборот:
|
||
|
||
```cpp
|
||
q1.toAxisAngle(axis, angle);
|
||
Vector rotation = q2.toRotationVector();
|
||
Vector euler = q3.toEuler();
|
||
```
|
||
|
||
Возможно рассчитать вращение между двумя обычными векторами:
|
||
|
||
```cpp
|
||
Quaternion q = Quaternion::fromBetweenVectors(v1, v2); // в виде кватерниона
|
||
Vector rotation = Vector::rotationVectorBetween(v1, v2); // в виде вектора вращения
|
||
```
|
||
|
||
Шорткаты для работы с вращением по рысканию (удобно для алгоритмов управления полетом):
|
||
|
||
```cpp
|
||
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()`:
|
||
|
||
```cpp
|
||
// Композиция вращений q1 и q2
|
||
Quaternion result = Quaternion::rotate(q1, q2);
|
||
```
|
||
|
||
Также полезной является операция применения вращения к вектору, которая делается похожим образом:
|
||
|
||
```cpp
|
||
// Вращение вектора v кватернионом q
|
||
Vector result = Quaternion::rotateVector(v, q);
|
||
```
|
||
|
||
Для расчета разницы между двумя ориентациями используется метод `Quaternion::between()`:
|
||
|
||
```cpp
|
||
// Расчет вращения от q1 к q2
|
||
Quaternion q = Quaternion::between(q1, q2);
|
||
```
|
||
|
||
## Дополнительные материалы
|
||
|
||
* [Интерактивный учебник по кватернионам](https://eater.net/quaternions).
|
||
* [Визуализация вращения вектора с помощью кватернионов](https://quaternions.online).
|