k13. Математика дрона: векторы и системы координат — body, NED, ENU

1. Цели урока

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

• Вы установите библиотеку numpy в виртуальное окружение и выполните базовые операции над векторами: сложение, вычитание, умножение на число, вычисление длины и нормирование.
• Вы объясните, что такое система координат (фрейм), и проверите тройку осей на «правость» с помощью правила правой руки.
• Вы назовёте оси систем body, NED и ENU и скажете, какая экосистема какую систему использует.
• Вы переведёте вектор из NED в ENU и обратно с помощью функций ned_to_enu() и enu_to_ned().
• Вы разложите скорость на компоненты NED по модулю скорости и курсу с помощью math.cos, math.sin и math.radians.
• Вы вычислите скалярное произведение вектора ветра и направления полёта и истолкуете его знак как попутный или встречный ветер.
• Вы напишете тесты математических функций с допусками pytest.approx и np.allclose и объясните, почему точное сравнение дробных чисел ненадёжно.

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

3. Зачем программисту БПЛА системы координат

Начнём с конкретной ситуации. Бортовой компьютер получил число: вертикальная составляющая позиции равна минус пять метров. Само по себе это число ничего не утверждает. Если оно записано в системе NED, где ось Z направлена вниз, то минус пять означает «пять метров выше точки старта». Если то же число записано в системе ENU, где ось Z направлена вверх, то оно означает «пять метров ниже точки старта». Одно и то же число — два противоположных физических смысла. Различает их только договорённость о системе координат.

Цена такой путаницы в нашей отрасли высока. Если наземная станция отправит автопилоту команду скорости, в которой оси перепутаны местами, аппарат полетит на восток вместо севера; если перепутан знак вертикали — начнёт снижаться вместо набора высоты. В симуляторе SITL ошибка обернётся падением виртуального аппарата и строкой в журнале; на реальном беспилотном летательном аппарате — аварией. Поэтому у разработчиков есть простая дисциплина: каждое имя переменной и каждый комментарий обязаны называть систему координат явно, например v_ned или velocity_enu, а числа без указания системы считаются подозрительными.

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

4. Вектор, скаляр и операции с numpy

Скаляр — это величина, которую полностью задаёт одно число: напряжение батареи 14,8 В, температура 23 °C, масса 1,2 кг. Вектор — величина, у которой кроме значения есть направление: скорость аппарата, перемещение из одной точки в другую, вектор ветра. Вектор записывают упорядоченным набором из трёх чисел — компонент по осям:

\[ \vec{v} = (v_x,\; v_y,\; v_z) \]

Словами: вектор — это тройка чисел, но не любая, а договорённая: первое число относится к оси X, второе к оси Y, третье к оси Z выбранной системы координат. Сами по себе операции над векторами не требуют выбора системы: векторы можно складывать (два последовательных перемещения дают суммарное), вычитать (разность позиций даёт перемещение «из B в A» и наоборот) и умножать на число (растягивать или сжимать длину, не меняя направления).

Норма вектора (его длина, или модуль) вычисляется по теореме Пифагора, обобщённой на три измерения:

\[ \|\vec{v}\| = \sqrt{v_x^2 + v_y^2 + v_z^2} \]

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

Вычислять всё это вручную несложно, но в отрасли принят инструмент numpy — библиотека численных вычислений для Python. Она добавляет тип «массив чисел» и готовые операции: сложение массивов, умножение на число, вычисление нормы, скалярное произведение. numpy устанавливается в виртуальное окружение привычной командой из урока K06:

python -m pip install numpy

# Пример: перемещение между точками, длина и единичный вектор
import numpy as np

start = (0.0, 0.0, -5.0)   # точка A в NED: 5 м над началом координат
end = (30.0, 40.0, -5.0)   # точка B: 30 м север, 40 м восток, та же высота

shift = np.asarray(end, dtype=float) - np.asarray(start, dtype=float)
length = np.linalg.norm(shift)
direction = shift / length
step = direction * 10.0

5. Системы координат и правая тройка

Система координат (по-английски — frame, «рамка отсчёта») — это договорённость из двух частей: где находится начало отсчёта и куда направлены три оси. Один и тот же физический вектор в разных системах получает разные координаты. Бытовая аналогия: инструкцию «пройти два квартала вперёд и один направо» вы привязываете к себе и своему направлению взгляда, а инструкцию «два квартала на север и один на восток» — к карте города. Оба описания верны, но пересчитать одно в другое можно, только зная, куда вы смотрите. Аналогия заканчивается там, где начинается строгость: в математике правило пересчёта между системами известно точно и выражается формулами.

Оси трёхмерной системы принято ориентировать согласованно — в правую тройку. Проверка выполняется правилом правой руки:

Правило правой руки: согните пальцы правой руки от оси X
к оси Y по короткой дуге — отставленный большой палец
укажет направление оси Z.

NED:  X — север,  Y — восток  ⇒  Z — вниз
ENU:  X — восток, Y — север   ⇒  Z — вверх
body: X — вперёд, Y — вправо  ⇒  Z — вниз

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

Соберём три главные системы курса в одну таблицу.

6. NED и ENU: две «земные» системы

NED и ENU описывают один и тот же мир — окрестность точки старта относительно сторон света, — но оси у них направлены по-разному. Переход между ними выражается простым правилом: первая и вторая компоненты меняются местами, а третья меняет знак.

\[ (n,\; e,\; d)_{\mathrm{NED}} \;\mapsto\; (e,\; n,\; -d)_{\mathrm{ENU}} \]

Словами: вектор «30 метров на север, 40 на восток, 5 вниз» в NED записывается как (30, 40, 5), а в ENU — как (40, 30, −5): восток стал первой компонентой, север второй, а «вниз» превратилось в «минус вверх». Обратите внимание на важное свойство: это преобразование симметрично. Обратный перевод из ENU в NED выполняется той же формулой — обмен местами и смена знака третьей компоненты. Применив правило дважды, вы получите исходный вектор; тесты практики проверяют именно этот «круговой» возврат.

# Пример: перевод векторов между NED и ENU
def ned_to_enu(v_ned):
    n, e, d = float(v_ned[0]), float(v_ned[1]), float(v_ned[2])
    return (e, n, -d)

def enu_to_ned(v_enu):
    x, y, z = float(v_enu[0]), float(v_enu[1]), float(v_enu[2])
    return (y, x, -z)

7. Связанная система body: оси на корпусе

Связанная система координат (body frame) отличается от земных принципиальным свойством: её оси жёстко привязаны к корпусу аппарата и вращаются вместе с ним. Ось X всегда смотрит вперёд, к «носу» — у мультиротора это направление переднего луча; ось Y — вправо по борту; ось Z — вниз, к «животу». Когда дрон наклоняется, «вперёд» наклоняется вместе с ним: вектор тяги моторов живёт именно в связанной системе, потому что моторы прикручены к раме, а не к планете.

Связанная система — это мир бортовых датчиков. Акселерометр измеряет ускорение вдоль осей корпуса, гироскоп — угловые скорости вращения вокруг этих осей. Оба прибора физически установлены на плате и «не знают», куда аппарат смотрит относительно севера. Поэтому в телеметрии MAVLink угловые скорости приходят в body-системе, а позиция и скорость относительно земли — в NED; подробнее о сообщениях мы поговорим в модуле M5.

Главный вопрос, который отсюда следует: как пересчитать вектор из связанной системы в земную и обратно? Ответ требует знания ориентации аппарата — углов крена (roll), тангажа (pitch) и рыскания (yaw), которые показывают, как именно повёрнут корпус относительно Земли. Полный пересчёт — тема следующего урока K14. Здесь зафиксируем два простых случая, которые понятны уже сейчас.

• Аппарат стоит на земле без наклона, «нос» смотрит на север. Связанная система совпадает с NED: вперёд — это север, вправо — восток, вниз — вниз.
• Аппарат без наклона повёрнул «нос» на восток (курс 90°). Теперь X связанной системы смотрит вдоль Y системы NED: «вперёд» стало «на восток». Пересчёт вектора сводится к повороту вокруг вертикальной оси на угол курса — ровно эту операцию выполняет раздел 8.

8. Скорость в NED и курс аппарата

Курс (heading, или рыскание yaw) — это угол между направлением на север и направлением «носа» аппарата, измеренный по часовой стрелке, если смотреть сверху. Курс 0° — аппарат смотрит на север, 90° — на восток, 180° — на юг, 270° — на запад. Именно в таком виде — градусы от севера по часовой стрелке — курс показывает компас и именно так его отдаёт наземная станция Mission Planner.

Пусть аппарат летит горизонтально со скоростью \(v\) метров в секунду по курсу \(\psi\). Разложение вектора скорости на компоненты NED задаётся школьными синусом и косинусом:

\[ v_N = v \cos\psi, \qquad v_E = v \sin\psi, \qquad v_D = 0 \]

Словами: северная составляющая скорости равна модулю скорости, умноженному на косинус курса, а восточная — на синус курса. Косинус даёт «долю движения вдоль севера», синус — «долю движения вдоль востока»; при курсе 0° весь полёт идёт на север, при 90° — на восток. Вертикальная составляющая в горизонтальном полёте равна нулю.

север (v_north = v · cos ψ)
  ↑
  │   ╱ направление полёта
  │  ╱   длиной v
  │ ╱
  │╱ ψ — курс от севера
  │      по часовой стрелке
  └────────→ восток (v_east = v · sin ψ)

Остаётся техническая деталь: тригонометрические функции Python работают не с градусами, а с радианами. Радиан — угловая мера, в которой полный оборот равен не 360°, а \(2\pi\) радиан; прямой угол — это \(\pi/2\) радиан, то есть примерно 1,5708. Переводит градусы в радианы функция math.radians(). Забыть этот перевод — одна из самых частых математических ошибок новичка: math.cos(90) вернёт не ноль, потому что 90 здесь — радианы, а не градусы.

# Пример: скорость в NED по модулю и курсу
import math

def heading_to_velocity_ned(speed_ms: float, heading_deg: float) -＞ tuple:
    if speed_ms ＜ 0:
        raise ValueError(f"модуль скорости не может быть отрицательным: {speed_ms}")
    yaw_rad = math.radians(heading_deg % 360.0)
    v_north = speed_ms * math.cos(yaw_rad)
    v_east = speed_ms * math.sin(yaw_rad)
    return (v_north, v_east, 0.0)

Проверим формулу на опорных направлениях: при скорости 10 м/с должны получаться «круглые» числа.

9. Скалярное произведение и проекция: ветер

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

\[ \vec{a} \cdot \vec{b} = a_x b_x + a_y b_y + a_z b_z \]

Геометрически то же самое число равно произведению длин векторов на косинус угла \(\theta\) между ними:

\[ \vec{a} \cdot \vec{b} = \|\vec{a}\| \, \|\vec{b}\| \cos\theta \]

Словами и по знакам: если векторы смотрят примерно в одну сторону (угол острый), произведение положительно; если в противоположные (угол тупой) — отрицательно; если перпендикулярны — равно нулю, потому что косинус прямого угла равен нулю. Отдельно отметим частный случай: скалярное произведение вектора на единичный вектор направления даёт проекцию — составляющую вектора вдоль этого направления, со знаком.

Прикладная задача этого раздела: летим по известному курсу, известен вектор ветра. Насколько ветер попутный или встречный? Договоримся о соглашении (и запишем его в комментарии к коду): вектор ветра указывает, куда воздух движется. Метеорологи, наоборот, называют ветер по направлению, откуда он дует: «северный ветер» дует с севера на юг, то есть его вектор направлен на юг. Перепутать эти два соглашения — классический способ получить знак «наоборот».

# Пример: проекция ветра на направление полёта
import numpy as np

def wind_along_track_ms(wind_ned, heading_deg: float) -＞ float:
    track = np.asarray(heading_to_velocity_ned(1.0, heading_deg), dtype=float)
    wind = np.asarray(wind_ned, dtype=float)
    return float(np.dot(wind, track))

Разберём числа из демонстрации практики. Полёт на север (курс 0°), единичный вектор направления равен (1, 0, 0). Ветер (5, 0, 0) — воздух движется на север вместе с нами: произведение +5, попутный ветер. Ветер (−8, 0, 0) — воздух движется на юг, навстречу: произведение −8, встречный ветер. Ветер (0, 6, 0) — дует строго на восток, перпендикулярно курсу: произведение 0, чистая боковина, которая сносит аппарат в сторону, но не меняет путевую скорость.

10. Тесты математики: pytest.approx

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

# Пример: почему точное сравнение дробных чисел подводит
print(0.1 + 0.2 == 0.3)              # False
print(0.1 + 0.2)                     # 0.30000000000000004

import math
print(math.cos(math.radians(90.0)))  # 6.123233995736766e-17, а не 0.0

Отсюда правило для тестов математики: вместо точного равенства использовать сравнение с допуском. В pytest для этого служит pytest.approx — он считает числа равными, если они отличаются в пределах крошечной относительной погрешности (по умолчанию около одной миллионной). Для массивов numpy тот же смысл несёт функция np.allclose(). Важно понимать: допуск не ослабляет тест до бесполезности. Погрешность округления составляет единицы в триллионных долях, тогда как физическая ошибка, которую тест обязан ловить, — это перепутанный знак, обмен осей или лишний множитель; такие ошибки больше допуска на многие порядки.

# Пример: тесты математики с допусками
import numpy as np
import pytest
from k13_task2_ned_enu import enu_to_ned, ned_to_enu
from k13_task3_velocity_ned import heading_to_velocity_ned

def test_heading_east():
    assert heading_to_velocity_ned(10.0, 90.0) == pytest.approx((0.0, 10.0, 0.0))

def test_round_trip_returns_original():
    v = (30.0, 40.0, -5.0)
    assert enu_to_ned(ned_to_enu(v)) == pytest.approx(v)

def test_displacement_and_distance():
    shift = np.array([30.0, 40.0, 0.0])
    assert np.allclose(shift, (30.0, 40.0, 0.0))
    assert np.linalg.norm(shift) == pytest.approx(50.0)

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

12. Практика

Практика урока состоит из пяти файлов: четыре демонстрационных модуля с функциями и один файл тестов. Порядок работы следующий: установите numpy, создайте файлы задач 13.1–13.5 в каталоге code/M01_python, выполните прогоны из задачи 13.6 и сохраните исходники в Git (задача 13.7). В карточках в аккордеонах лежит полный текст каждого файла; он совпадает с содержимым репозитория курса.

# Каталог: code/M01_python (venv активен)
python -m pip install numpy
python k13_task1_vector_basics.py
python k13_task2_ned_enu.py
python k13_task3_velocity_ned.py
python k13_task4_wind_projection.py
python -m pytest k13_task5_test_frames.py -v

task1: перемещение NED: [30. 40.  0.] м
       расстояние: 50.00 м
       направление (единичный вектор): [0.6 0.8 0. ]
       шаг 10 метров вдоль направления: [6. 8. 0.] м

task2: 1 м на север: NED (1.0, 0.0, 0.0) -＞ ENU (0.0, 1.0, -0.0) -＞ обратно (1.0, 0.0, 0.0)
       1 м вниз: NED (0.0, 0.0, 1.0) -＞ ENU (0.0, 0.0, -1.0) -＞ обратно (0.0, 0.0, 1.0)
       произвольный вектор: NED (30.0, 40.0, -5.0) -＞ ENU (40.0, 30.0, 5.0) -＞ обратно (30.0, 40.0, -5.0)

task3: курс   0.0 град: v_north=  10.00 м/с  v_east=   0.00 м/с  v_down= 0.0 м/с
       курс  45.0 град: v_north=   7.07 м/с  v_east=   7.07 м/с  v_down= 0.0 м/с
       курс  90.0 град: v_north=   0.00 м/с  v_east=  10.00 м/с  v_down= 0.0 м/с

task4: ветер дует на север (+5 м/с вдоль курса): проекция +5.00 м/с (попутный)
       ветер дует на юг (8 м/с навстречу): проекция -8.00 м/с (встречный)
       ветер дует на восток (чистая боковина): проекция +0.00 м/с (боковой, проекция равна нулю)

tests: 14 passed

# Из корня репозитория курса
git add code/M01_python/k13_task1_vector_basics.py code/M01_python/k13_task2_ned_enu.py code/M01_python/k13_task3_velocity_ned.py code/M01_python/k13_task4_wind_projection.py code/M01_python/k13_task5_test_frames.py
git commit -m "K13: vectors, NED/ENU frames, heading decomposition, wind projection, 14 tests"

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

1. Чем вектор отличается от скаляра? Приведите по два примера из телеметрии аппарата.
2. Что фиксирует система координат и почему одни и те же числа в разных системах означают разное?
3. Как с помощью правила правой руки проверить, что тройка осей — правая? Проверьте так NED и ENU.
4. Назовите оси систем body, NED и ENU и скажите, какая экосистема какую систему использует.
5. Сформулируйте правило перевода вектора из NED в ENU. Почему обратный перевод выполняется той же формулой?
6. Аппарат снижается со скоростью 2 м/с. Какой знак у вертикальной компоненты в NED и в ENU?
7. Как измеряется курс? Какие компоненты скорости NED получаются при курсе 0° и при курсе 90°?
8. Зачем нужен перевод градусов в радианы и какая функция Python его выполняет?
9. Что означает знак скалярного произведения вектора ветра и единичного вектора направления полёта?
10. Почему в тестах математических функций используют pytest.approx, а не точное равенство? Ослабляет ли допуск тест?

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

• Я установил numpy в виртуальное окружение командой python -m pip install numpy.
• Я создал все пять файлов практики в каталоге code/M01_python.
• Четыре демо-скрипта запускаются, и я могу объяснить каждую напечатанную строку.
• Я могу записать по памяти правило перевода NED → ENU и объяснить, почему оно симметрично.
• Я вывел компоненты скорости для курсов 0°, 90°, 180° и 270° без подсказки.
• Я объясняю знак проекции ветра как попутный, встречный или боковой.
• Команда python -m pytest k13_task5_test_frames.py -v завершается результатом 14 passed.
• Я могу объяснить, почему точное сравнение дробных чисел ненадёжно и что такое отрицательный ноль.
• Я сделал git commit с файлами урока K13.

15. Что дальше

• Предыдущий урок: K12 · Мини-проект CLI «Fake GCS» (гейт G1 сдан)
• Текущий урок: K13 · Векторы и системы координат: body, NED, ENU
• Следующий урок: K14 · Углы Эйлера, кватернионы и DCM
• Модуль M2 не имеет собственного гейта; следующий гейт G2 сдаётся после лабораторной работы K22 в модуле M3 «Протоколы и интерфейсы».