k14. Математика дрона: углы Эйлера, кватернионы и DCM

1. Цели урока

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

• Вы объясните, что такое ориентация аппарата, и назовёте три способа её записать: углы Эйлера, матрица поворота, кватернион.
• Вы определите крен, тангаж и рыскание по осям связанной системы и проверите положительное направление по правилу правой руки из урока K13.
• Вы построите матрицу поворота по рысканию и полную матрицу body → NED по трём углам с помощью функции euler_to_dcm().
• Вы проверите свойства матрицы поворота: ортонормированность через np.dot(R, R.T), единичный определитель через np.linalg.det(), обратный поворот через транспонирование R.T.
• Вы продемонстрируете gimbal lock численно: две разные тройки углов при тангаже 90° дают одинаковую ориентацию.
• Вы переведёте углы Эйлера в кватернион и обратно каноническими формулами и объясните, зачем нужен math.atan2() и защемление аргумента math.asin().
• Вы напишете тесты математики ориентации с допусками np.allclose() и pytest.approx.

2. Словарь урока

3. Зачем нужна ориентация: мост между body и NED

Вспомним вывод урока K13. Датчики аппарата — акселерометр и гироскоп — живут в связанной системе body: они измеряют ускорения и скорости вращения вдоль осей корпуса и «не знают», куда корпус смотрит относительно Земли. Позиция, скорость и маршрут живут в земной системе NED. Между этими двумя мирами нужен мост, и строится он из ориентации: зная, как повёрнут корпус, можно пересчитать любой вектор из связанной системы в земнюю и обратно.

Пример из практики: акселерометр в ровном висении показывает ускорение 9,81 м/с² вдоль своей оси Z — «вниз по корпусу». Если аппарат наклонён, часть этого числа «проецируется» на продольную ось, и наивный код решит, что дрон разгоняется вперёд. Автопилот не ошибается именно потому, что знает свою ориентацию и поворачивает вектор из body в NED до того, как делать выводы. Тот же пересчёт понадобится вам, например, чтобы наклонить подвес камеры на цель: цель известна в земных координатах, а моторы подвеса работают в осях корпуса.

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

4. Углы Эйлера: крен, тангаж, рыскание

Ориентация аппарата в авиации традиционно описывается тремя углами Эйлера — тремя углами последовательного поворота, которые переводят корпус из «ровного» положения в текущее. Каждый угол вращает аппарат вокруг своей оси связанной системы: крен (roll) наклоняет аппарат вбок вокруг продольной оси X, тангаж (pitch) поднимает или опускает нос вокруг поперечной оси Y, а рыскание (yaw) поворачивает нос вправо или влево вокруг вертикальной оси Z.

Крен (roll, φ) — вокруг оси X «вперёд»:
    правое крыло вниз — положительный крен.

Тангаж (pitch, θ) — вокруг оси Y «вправо»:
    нос вверх — положительный тангаж.

Рыскание (yaw, ψ) — вокруг оси Z «вниз»:
    нос вправо, то есть по часовой стрелке при взгляде сверху, —
    положительное рыскание.

Знаки направлений следуют прямо из правила правой руки, которое вы освоили на уроке K13: сожмите правую руку так, чтобы отставленный большой палец смотрел вдоль положительного направления оси, — согнутые пальцы покажут положительное направление вращения вокруг этой оси. Для оси X «вперёд» это наклон правого борта вниз, для оси Y «вправо» — подъём носа, для оси Z «вниз» — поворот носа вправо.

Вторая важная договорённость — порядок поворотов. В авиационной схеме, которую используют ArduPilot и PX4, повороты применяются в порядке ZYX: сначала рыскание вокруг вертикальной оси, затем тангаж вокруг новой поперечной оси, затем крен вокруг новой продольной оси. Порядок принципиален, потому что повороты вокруг разных осей не переставляемы: «поднять нос на 90°, затем повернуть направо 90°» и «повернуть направо 90°, затем поднять нос на 90°» приводят аппарат в разные положения. Схема записи углов без указания порядка — неполная схема, и это частая причина расхождений между библиотеками.

Диапазоны углов: крен и рыскание обычно живут в пределах от −180° до +180°, а тангаж ограничен диапазоном от −90° до +90°. Именно эта ограниченность тангажа станет источником проблемы, которую разбирает раздел 6. Сообщение MAVLink ATTITUDE отдаёт поля roll, pitch и yaw в радианах; для человека их переводят в градусы функцией math.degrees(). Справочное описание сообщения доступно по адресу https://mavlink.io/en/messages/common.html (раздел ATTITUDE).

5. Матрица поворота и DCM

Матрица поворота — это таблица из девяти чисел, расположенных три на три, которая превращает координаты вектора в одной системе координат в координаты того же вектора в другой системе. В numpy матрица создаётся функцией np.array() из списка списков, а умножение матрицы на вектор выполняет уже знакомая функция np.dot():

\[ \vec{v}_{\mathrm{NED}} = R \cdot \vec{v}_{\mathrm{body}} \]

Словами: чтобы получить земные координаты вектора, нужно умножить матрицу поворота на столбец его связанных координат. Начнём с самого простого поворота — только рыскание, аппарат летит «плоско», без крена и тангажа. Такую матрицу вы фактически уже видели на уроке K13, когда раскладывали скорость по курсу:

\[ R_z(\psi) = \begin{pmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{pmatrix} \]

# Пример: поворот только по рысканию (фрагмент k14_task1_yaw_rotation.py)
import math
import numpy as np

def yaw_rotation_matrix(yaw_rad: float):
    c = math.cos(yaw_rad)
    s = math.sin(yaw_rad)
    return np.array([
        [c, -s, 0.0],
        [s,  c, 0.0],
        [0.0, 0.0, 1.0],
    ])

def rotate_body_to_ned(v_body, yaw_rad: float):
    matrix = yaw_rotation_matrix(yaw_rad)
    return np.dot(matrix, np.asarray(v_body, dtype=float))

Проверьте результат мысленно: при курсе 90° «нос» аппарата — вектор (1, 0, 0) в связанной системе — должен смотреть на восток, то есть превратиться в (0, 1, 0) в NED. Демо практики печатает ровно это.

Полная матрица ориентации по трём углам называется DCM — direction cosine matrix, «матрица направляющих косинусов». Имя объясняет устройство таблицы: каждый её элемент равен косинусу угла между соответствующей осью связанной системы и осью земной. Каноническая формула для схемы ZYX (источник: https://en.wikipedia.org/wiki/Rotation_matrix) с обозначениями \(s_\phi = \sin(\text{крен})\), \(c_\phi = \cos(\text{крен})\), \(s_\theta = \sin(\text{тангаж})\), \(c_\theta = \cos(\text{тангаж})\), \(s_\psi = \sin(\text{рыскание})\), \(c_\psi = \cos(\text{рыскание})\):

\[ R_{\mathrm{body} \to \mathrm{NED}} = \begin{pmatrix} c_\theta c_\psi & s_\phi s_\theta c_\psi - c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \\ c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{pmatrix} \]

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

# Пример: полная матрица DCM по трём углам (фрагмент k14_task2_euler_dcm.py)
import math
import numpy as np

def euler_to_dcm(roll_rad: float, pitch_rad: float, yaw_rad: float):
    cr, sr = math.cos(roll_rad), math.sin(roll_rad)
    cp, sp = math.cos(pitch_rad), math.sin(pitch_rad)
    cy, sy = math.cos(yaw_rad), math.sin(yaw_rad)
    return np.array([
        [cp * cy, sr * sp * cy - cr * sy, cr * sp * cy + sr * sy],
        [cp * sy, sr * sp * sy + cr * cy, cr * sp * sy - sr * cy],
        [-sp,     sr * cp,                cr * cp],
    ])

def ned_to_body_matrix(roll_rad: float, pitch_rad: float, yaw_rad: float):
    return euler_to_dcm(roll_rad, pitch_rad, yaw_rad).T

У матрицы поворота есть три свойства, которые превращают её в самопроверяемую структуру. Первое: она ортонормирована — произведение матрицы на её транспонированную версию даёт единичную матрицу (в numpy — np.eye(3), единицы по диагонали, нули в остальных клетках). Второе: её определитель (функция np.linalg.det()) равен единице; определитель минус единица означал бы, что в формулу закралось зеркальное отражение. Третье: обратный поворот — это транспонирование: столбцы и строки меняются местами, и обратная матрица достаётся бесплатно, без сложного обращения. Тесты практики проверяют все три свойства на «живой» матрице — это гораздо надёжнее, чем однажды сверить числа глазами.

6. Gimbal lock: потеря степени свободы

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

Карданный подвес из трёх рамок:

   внешняя рамка   — рыскание (ось Z, вертикаль)
      среднее кольцо — тангаж   (ось Y, поперечная)
         внутреннее кольцо — крен (ось X, продольная)

Если поднять среднее кольцо ровно на 90 град,
оси внешнего и внутреннего колец совпадают:
два разных двигателя крутят аппарат вокруг одной
и той же оси — одна степень свободы потеряна.

Этот эффект и называется gimbal lock (дословно — «блокировка подвеса»). На языке чисел: при тангаже ровно ±90° ориентация перестаёт зависеть от крена и рыскания по отдельности и начинает зависеть только от их разности. Два разных набора углов описывают одно и то же положение корпуса — представление перестаёт быть однозначным. Описание эффекта с иллюстрациями доступно по адресу https://en.wikipedia.org/wiki/Gimbal_lock.

Проверим это численно — именно так, как инженер проверяет подозрение: не рассуждением, а экспериментом.

# Пример: численная демонстрация gimbal lock (фрагмент k14_task4_gimbal_lock_demo.py)
import math
import numpy as np
from k14_task2_euler_dcm import euler_to_dcm

dcm_a = euler_to_dcm(math.radians(30.0), math.radians(20.0), math.radians(10.0))
dcm_b = euler_to_dcm(math.radians(50.0), math.radians(20.0), math.radians(30.0))
print(np.allclose(dcm_a, dcm_b))   # False: тангаж 20 град, всё различимо

dcm_c = euler_to_dcm(math.radians(30.0), math.radians(90.0), math.radians(10.0))
dcm_d = euler_to_dcm(math.radians(50.0), math.radians(90.0), math.radians(30.0))
print(np.allclose(dcm_c, dcm_d))   # True: тангаж 90 град, степень свободы потеряна

Почему это важно для беспилотника, а не только для учебника? Во-первых, около вертикального положения перестают быть однозначными сами числа углов: алгоритм, который по матрице восстанавливает крен и рыскание, начнёт выдавать скачущие значения при почти неподвижном аппарате. Во-вторых, плавное изменение ориентации (интерполяция) вблизи блокировки ломается. Для аппаратов вертикального взлёта, включая наш целевой Matek F405 VTOL в квадопланной конфигурации, большие углы тангажа — рабочий режим переходных фаз, поэтому проблема касается нас напрямую.

7. Кватернион: четыре числа вместо трёх углов

Кватернион ориентации — это четыре числа, которые записываются как (w, x, y, z) и образуют вектор единичной длины. Полная математика кватернионов выходит за рамки курса и не нужна для работы: достаточно твёрдо усвоить, что кватернион кодирует поворот как «ось вращения плюс угол вокруг неё», что его норма (длина) у корректной ориентации равна единице и что между кватернионом, углами Эйлера и матрицей DCM существуют точные формулы перехода.

Два главных преимущества кватерниона вытекают из его устройства. Первое: у него нет gimbal lock — все ориентации, включая вертикальные, представляются одинаково хорошо. Второе: два последовательных поворота складываются простым перемножением кватернионов, а численная погрешность легко чинится нормированием — делением четырёх компонент на их общую длину. Именно поэтому автопилот ведёт ориентацию в кватернионах, а MAVLink, помимо ATTITUDE, предлагает сообщение ATTITUDE_QUATERNION.

Формула перехода от углов Эйлера (схема ZYX) к кватерниону — каноническая (источник: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles). В ней участвуют синусы и косинусы половин углов; для краткости обозначим \(c_r = \cos(\phi/2)\), \(s_r = \sin(\phi/2)\) и так же для тангажа (p) и рыскания (y):

\[ \begin{aligned} w &= c_r c_p c_y + s_r s_p s_y \\ x &= s_r c_p c_y - c_r s_p s_y \\ y &= c_r s_p c_y + s_r c_p s_y \\ z &= c_r c_p s_y - s_r s_p c_y \end{aligned} \]

# Пример: углы Эйлера -＞ кватернион (фрагмент k14_task3_quaternion.py)
import math

def euler_to_quaternion(roll_rad: float, pitch_rad: float, yaw_rad: float):
    cr, sr = math.cos(roll_rad / 2.0), math.sin(roll_rad / 2.0)
    cp, sp = math.cos(pitch_rad / 2.0), math.sin(pitch_rad / 2.0)
    cy, sy = math.cos(yaw_rad / 2.0), math.sin(yaw_rad / 2.0)
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy
    return (w, x, y, z)

Обратный переход — от кватерниона к углам — использует две новые функции модуля math. Первая: atan2 — арктангенс двух аргументов, math.atan2(y, x). Обычный арктангенс видит только отношение двух чисел и потому путает противоположные квадранты; atan2 получает оба числа по отдельности, учитывает знаки и возвращает угол в полном диапазоне от −π до π. Вторая: math.asin() — арксинус, определённый только для аргументов от −1 до 1. Из-за погрешности округления значение, математически равное единице, может прийти как 1.0000000000000002, и тогда asin поднимет ValueError. Поэтому аргумент предварительно «защемляют» в допустимых границах.

\[ \phi = \operatorname{atan2}\bigl(2(wx + yz),\; 1 - 2(x^2 + y^2)\bigr) \]

\[ \theta = \arcsin\bigl(\operatorname{clip}(2(wy - zx),\; -1,\; 1)\bigr) \]

\[ \psi = \operatorname{atan2}\bigl(2(wz + xy),\; 1 - 2(y^2 + z^2)\bigr) \]

# Пример: кватернион -＞ углы Эйлера (фрагмент k14_task3_quaternion.py)
import math

def quaternion_to_euler(q):
    w, x, y, z = (float(c) for c in q)
    roll = math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
    sin_pitch = 2.0 * (w * y - z * x)
    sin_pitch = max(-1.0, min(1.0, sin_pitch))
    pitch = math.asin(sin_pitch)
    yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
    return (roll, pitch, yaw)

8. Когда не следует писать свою математику

Этот раздел короткий, но профессионально важный. Математика ориентации выведена, перепроверена и опубликована десятилетия назад; десятки библиотек реализуют её с тестами и исправленными краевыми случаями. Самостоятельно «улучшать» формулы — значит гарантированно получить трудноуловимую ошибку знака или порядка осей, которая проявится не на столе, а в полёте. Поэтому в отрасли принята схема работы, которую мы воспроизвели в этом уроке и которую вам стоит сделать своей привычкой.

• Берите формулу из надёжного источника и сохраняйте ссылку в комментарии к коду — так сделано в файлах k14_task2_euler_dcm.py и k14_task3_quaternion.py.
• Явно фиксируйте соглашения: схема углов (ZYX), порядок компонент кватерниона (w, x, y, z), единицы (радианы), системы координат (body, NED).
• Покрывайте реализацию тестами свойств: ортонормированность матрицы, единичный определитель, единичная норма кватерниона, круговые переводы «туда и обратно».
• Когда проект вырастет из учебного, переходите на проверенные реализации: в экосистеме MAVLink помощники живут в модуле mavextra библиотеки pymavlink (модуль M5), в научных проектах — в классе scipy.spatial.transform.Rotation. Ставить их сейчас не нужно: урок сознательно проходит по формулам один раз, чтобы вы понимали, что именно делают библиотеки.

9. Угловая скорость: как быстро меняется ориентация

Рядом с углами ориентации в телеметрии всегда живёт вторая величина — угловая скорость: как быстро аппарат вращается вокруг каждой из осей связанной системы. Её измеряет гироскоп, и MAVLink отдаёт её в полях rollspeed, pitchspeed и yawspeed сообщения ATTITUDE — в радианах в секунду. Не путайте две величины: ориентация отвечает на вопрос «куда смотрит корпус», угловая скорость — «как быстро он поворачивается». В ровном висении углы почти постоянны, а угловые скорости дрожат вокруг нуля; в резком развороте скорость рыскания велика, а сам угол рыскания меняется плавно.

Связь между величинами прямая: ориентация — это, грубо говоря, накопленный итог угловой скорости (математически — её интеграл по времени). Автопилот вычисляет ориентацию сотни раз в секунду, смешивая показания гироскопа, акселерометра, барометра и спутников в фильтре, который называется EKF (Extended Kalman Filter). Внутреннее устройство фильтра — большая отдельная тема за рамками курса; нам достаточно знать, что на выходе EKF лежат те самые ориентации в углах и кватернионах, которые мы научились читать и переводить, а угловая скорость используется в фильтре как главный источник «быстрой» информации о вращении.

10. Тесты математики ориентации

Тесты этого урока продолжают идею K13: математические функции проверяются с допусками (np.allclose() для матриц и массивов, pytest.approx для чисел и кортежей), а вместо «сверил числа глазами» проверяются свойства и круговые переводы. Перечислим четыре класса проверок, которые используются в файле k14_task5_test_attitude.py.

• Опорные случаи: при рыскании 90° вектор «нос» обязан превратиться из северного в восточный; при нулевых углах матрица обязана быть единичной.
• Свойства структуры: произведение матрицы на транспонированную равно единичной матрице, определитель равен единице — независимо от конкретных углов.
• Круговые переводы: углы → кватернион → углы обязаны вернуть исходные значения; поворот вперёд и обратно через транспонированную матрицу обязан вернуть исходный вектор.
• Вырожденный случай: при тангаже 90° две разные тройки углов обязаны дать одинаковую матрицу — тест фиксирует gimbal lock как ожидаемое поведение формулы, а не как сюрприз.

# Пример: тесты свойств DCM и кругового перевода (фрагмент k14_task5_test_attitude.py)
import math
import numpy as np
import pytest
from k14_task2_euler_dcm import euler_to_dcm
from k14_task3_quaternion import euler_to_quaternion, quaternion_to_euler

def test_dcm_is_orthonormal():
    matrix = euler_to_dcm(math.radians(10.0), math.radians(-5.0), math.radians(30.0))
    assert np.allclose(np.dot(matrix, matrix.T), np.eye(3))

def test_euler_quaternion_round_trip():
    angles = (math.radians(10.0), math.radians(-5.0), math.radians(30.0))
    q = euler_to_quaternion(*angles)
    back = quaternion_to_euler(q)
    assert back == pytest.approx(angles)

11. Типичные ошибки

12. Практика

Практика урока состоит из пяти файлов: четыре модуля математики ориентации и один файл тестов. Порядок работы: создайте файлы задач 14.1–14.5 в каталоге code/M01_python, выполните прогоны из задачи 14.6 и сохраните исходники в Git (задача 14.7). Библиотека numpy уже установлена в уроке K13 — новых пакетов этот урок не требует. В карточках в аккордеонах лежит полный текст каждого файла; он совпадает с содержимым репозитория курса.

# Каталог: code/M01_python (venv активен, numpy установлен на K13)
python k14_task1_yaw_rotation.py
python k14_task2_euler_dcm.py
python k14_task3_quaternion.py
python k14_task4_gimbal_lock_demo.py
python -m pytest k14_task5_test_attitude.py -v

task1: курс  90.0 град: нос в NED [0. 1. 0.], правый борт в NED [-1.  0.  0.]

task2: определитель: 1.000000 (ожидается 1)
       произведение R @ R.T равно единичной матрице [[1, 0, 0], [0, 1, 0], [0, 0, 1]]

task3: углы (град): (10.0, -5.0, 30.0)
       кватернион (w, x, y, z): (0.9604, 0.0954, -0.0194, 0.2613)
       норма кватерниона: 1.000000 (ожидается 1)
       обратно в углы (град): (10.0, -5.0, 30.0)

task4: тангаж 20 град: матрицы одинаковы? False
       тангаж 90 град: матрицы одинаковы? True
       тангаж 90 град: кватернионы одинаковы? True

tests: 10 passed

# Из корня репозитория курса
git add code/M01_python/k14_task1_yaw_rotation.py code/M01_python/k14_task2_euler_dcm.py code/M01_python/k14_task3_quaternion.py code/M01_python/k14_task4_gimbal_lock_demo.py code/M01_python/k14_task5_test_attitude.py
git commit -m "K14: euler angles, DCM, quaternions, gimbal lock demo, 10 tests"

13. Проверьте себя

1. Что такое ориентация аппарата и почему без неё невозможен пересчёт векторов из body в NED?
2. Вокруг каких осей связанной системы измеряются крен, тангаж и рыскание и как правило правой руки задаёт положительное направление?
3. Почему важен порядок поворотов ZYX и что произойдёт, если применить повороты в другом порядке?
4. Что делает матрица поворота DCM с вектором и откуда произошло название «матрица направляющих косинусов»?
5. Какие три свойства матрицы поворота проверяют тесты и что означает каждое из них?
6. Что такое gimbal lock, при каком тангаже он возникает и как это проявляется численно в нашей демонстрации?
7. Что такое кватернион ориентации и какие два преимущества дают ему место внутри автопилота?
8. Зачем в формуле извлечения тангажа нужен math.atan2() для крена и рыскания и защемление аргумента для math.asin()?
9. Чем угловая скорость отличается от углов ориентации и в каких полях сообщения ATTITUDE приходит каждое из них?
10. Что означает принцип «свою архитектуру изобретай, свою математику — нет» и какие четыре шага он предписывает?

14. Чек-лист самопроверки

• Я создал все пять файлов практики в каталоге code/M01_python.
• Я могу показать на себе оси крена, тангажа и рыскания и объяснить знаки через правило правой руки.
• Демо задачи 14.1 показывает, что при курсе 90° «нос» становится восточным вектором.
• Демо задачи 14.2 показывает единичный определитель и единичную матрицу в произведении R @ R.T.
• Демо задачи 14.3 возвращает исходные углы после кругового перевода через кватернион.
• Демо задачи 14.4 напечатало True при тангаже 90° — я могу объяснить, почему две разные тройки углов совпали.
• Команда python -m pytest k14_task5_test_attitude.py -v завершается результатом 10 passed.
• Я могу назвать два преимущества кватерниона перед углами Эйлера и одну причину хранить ссылки на источник формул.
• Я сделал git commit с файлами урока K14.

15. Что дальше

• Предыдущий урок: K13 · Векторы и системы координат: body, NED, ENU
• Текущий урок: K14 · Углы Эйлера, кватернионы и DCM
• Следующий урок: K15 · Геокоординаты: lat/lon/alt, расстояние haversine, направление bearing
• После K15 модуль M2 завершается, и курс переходит к модулю M3 «Протоколы и интерфейсы» (K16–K22) с гейтом G2 после лабораторной работы K22.