k15. Математика дрона: геокоординаты — lat/lon/alt, расстояние haversine, направление bearing

1. Цели урока

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

• Вы объясните, что измеряют широта, долгота и высота, и почему координаты — это углы, а не метры.
• Вы вычислите расстояние между двумя географическими точками по формуле гаверсинуса через функцию haversine_distance_m().
• Вы вычислите направление (bearing) из одной точки на другую через функцию bearing_deg() и подадите результат в heading_to_velocity_ned() из урока K13.
• Вы получите координаты точки, смещённой на заданное расстояние в заданном направлении, через функцию destination_point().
• Вы реализуете простейшую геозону: проверка расстояния от домашней точки и потолка высоты через функцию geofence_status().
• Вы напишете тесты геодезических функций трёх классов: опорные значения, свойства и согласованность цепочки «смещение → расстояние → направление».
• Вы завершите модуль M2 «Математика дрона» и подготовитесь к модулю M3 «Протоколы и интерфейсы».

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

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

Векторы и ориентация из уроков K13–K14 живут в локальном мире: метры и секунды вокруг точки старта. Миссии и навигация живут в глобальном мире: широта, долгота и высота. Связка этих двух миров — постоянная работа программиста беспилотных систем. Вот четыре типичные задачи, которые решаются инструментами этого урока.

• Построение маршрута: миссия «квадрат со стороной 200 метров вокруг точки старта» требует вычислить координаты четырёх углов — это смещение точки на расстояние в заданном направлении.
• Чтение телеметрии: аппарат прислал позицию; чтобы понять, как далеко он от дома и в какую сторону движется, нужны расстояние и направление между двумя точками.
• Безопасность: простейшая геозона обязана на каждом витке цикла управления сравнивать расстояние от дома и высоту с разрешёнными пределами.
• Сопровождение цели: камера видит объект, его координаты оценены — чтобы навестись или следовать за ним, вычисляется направление и расстояние до цели (модуль M11).

Этот урок завершает модуль M2. Три модуля курса — M2 (математика), M3 (протоколы) и M5 (MAVLink) — образуют ядро профессии: математика говорит, что означают числа, протоколы — как числа передаются, MAVLink — какие именно числа летают между аппаратами и наземными станциями.

4. Широта, долгота, высота: WGS-84 интуитивно

Широта (latitude, обозначается греческой буквой φ, «фи») — это угол между плоскостью экватора и направлением из центра Земли на точку. Широта равна +90° на Северном полюсе, 0° на экваторе и −90° на Южном полюсе. Долгота (longitude, обозначается λ, «лямбда») — угол между плоскостью Гринвичского меридиана и плоскостью меридиана точки: от −180° на запад до +180° на восток. Третья координата — высота в метрах.

Широта (lat) — угол от плоскости экватора:
    +90 на Северном полюсе, 0 на экваторе, -90 на Южном.

Долгота (lon) — угол от Гринвичского меридиана:
    0 в Гринвиче, до +180 на восток, до -180 на запад.

Меридианы сходятся у полюсов:
    один градус широты — всегда примерно 111 км,
    один градус долготы — 111 км только на экваторе,
    а на широте 55.75 град — уже примерно 63 км.

Система отсчёта, в которой спутниковый приёмник выдаёт координаты, называется WGS-84 (World Geodetic System 1984). Она описывает Землю как эллипсоид — слегка сплюснутый шар: экваториальный радиус 6378,137 км, полярный 6356,752 км; описание системы доступно по адресу https://en.wikipedia.org/wiki/World_Geodetic_System. Для наших задач — дистанции от метров до десятков километров — Землю достаточно считать шаром со средним радиусом \(R = 6\,371\,000\) метров: погрешность такой модели составляет доли процента и заведомо меньше ошибки самого спутникового приёмника, которая измеряется метрами.

С высотой связана важная развилка: существуют высота над уровнем моря (MSL, mean sea level) и относительная высота над точкой старта. Если домашняя точка находится на холме высотой 150 метров над уровнем моря, то относительная высота 20 метров соответствует 170 метрам MSL. MAVLink разделяет эти величины: сообщение GLOBAL_POSITION_INT несёт поле alt (высота над уровнем моря) и поле relative_alt (высота над домом), оба в миллиметрах; координаты lat и lon передаются в том же сообщении целыми числами — градусами, умноженными на \(10^7\). Целые числа используются потому, что протокол передаёт поля фиксированного размера, а целое значение не теряет точность при передаче; подробный разбор сообщений — в модуле M5. Справочник сообщений: https://mavlink.io/en/messages/common.html.

Дисциплина единиц из уроков K13–K14 сохраняется полностью: формулы этого урока работают с радианами, поэтому на входе каждой функции градусы переводятся через math.radians(), а результаты возвращаются в градусах через math.degrees(). Точка в коде представляется структурой GeoPoint — это dataclass из урока K09 с именованными полями lat_deg и lon_deg. Имена полей защищают от классической ошибки: порядок «широта, долгота» и «долгота, широта» перепутать в кортеже очень легко, а в именованных полях — трудно.

5. Расстояние: большой круг и гаверсинус

Почему нельзя обойтись школьной теоремой Пифагора, применённой к разностям градусов? Потому что градусы — неодинаковые единицы длины. Один градус широты всегда соответствует примерно 111 километрам, а один градус долготы «сжимается» по мере движения к полюсам: его длина приблизительно равна \(111.32 \cdot \cos\varphi\) километров. На широте Москвы один градус долготы — это уже около 63 километров. Пифагор по градусам даст тем большую ошибку, чем сильнее широта места отличается от экватора и чем длиннее дистанция.

Правильная модель: кратчайший путь между двумя точками на сфере проходит по дуге большого круга — окружности, центр которой совпадает с центром сферы (экватор и любой меридиан — большие круги; параллель, кроме экватора, — нет). Расстояние по дуге большого круга вычисляет формула гаверсинуса. Канонический источник всех формул этого урока — классический справочник https://www.movable-type.co.uk/scripts/latlong.html. Пусть φ и λ — широта и долгота в радианах, индекс 1 — первая точка, индекс 2 — вторая, Δ — разность:

\[ a = \sin^2\!\left(\frac{\Delta\varphi}{2}\right) + \cos\varphi_1 \cdot \cos\varphi_2 \cdot \sin^2\!\left(\frac{\Delta\lambda}{2}\right) \]

\[ d = 2R \cdot \operatorname{atan2}\!\left(\sqrt{a},\ \sqrt{1-a}\right) \]

Словами: сначала вычисляется вспомогательная величина a — комбинация квадратов синусов половинных разностей координат, затем угловое расстояние через atan2 от двух корней, и наконец расстояние в метрах как произведение углового расстояния на радиус Земли. Именно эта форма, а не эквивалентная через арксинус, сохраняет точность на малых дистанциях — тех самых десятках и сотнях метров, на которых летает беспилотник.

# Пример: расстояние между двумя точками (фрагмент k15_task1_haversine.py)
import math
from dataclasses import dataclass

EARTH_RADIUS_M = 6371000.0

@dataclass
class GeoPoint:
    lat_deg: float
    lon_deg: float

def haversine_distance_m(p1: GeoPoint, p2: GeoPoint) -＞ float:
    lat1, lon1 = math.radians(p1.lat_deg), math.radians(p1.lon_deg)
    lat2, lon2 = math.radians(p2.lat_deg), math.radians(p2.lon_deg)
    d_lat = lat2 - lat1
    d_lon = lon2 - lon1
    a = (
        math.sin(d_lat / 2.0) ** 2
        + math.cos(lat1) * math.cos(lat2) * math.sin(d_lon / 2.0) ** 2
    )
    c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
    return EARTH_RADIUS_M * c

Демонстрация практики печатает три контрольных числа: расстояние от центра Москвы до Балашихи — 22095 метров, расстояние одного градуса широты — 111195 метров (те самые «примерно 111 километров») и ноль для расстояния точки до самой себя. Отдельное полезное свойство: при чисто меридиональном смещении (долгота не меняется) гаверсинус сводится к точной формуле \(d = R \cdot \Delta\varphi\) — этот факт закрепляет тест.

6. Направление на точку: bearing

Второй вопрос после «как далеко?» — «в какую сторону?». Bearing (начальный азимут) — это угол от направления на север до направления на целевую точку, отсчитанный по часовой стрелке: 0° — цель на севере, 90° — на востоке, 180° — на юге, 270° — на западе. Обратите внимание: это в точности тот же компасный договор, который использовал курс (heading) в уроке K13. Поэтому результат bearing_deg() можно напрямую подать в heading_to_velocity_ned() — и получить вектор скорости в NED, ведущий к цели. Так математика двух уроков собирается в одну цепочку.

\[ \theta = \operatorname{atan2}\bigl(\sin\Delta\lambda \cdot \cos\varphi_2,\;\; \cos\varphi_1 \cdot \sin\varphi_2 - \sin\varphi_1 \cdot \cos\varphi_2 \cdot \cos\Delta\lambda \bigr) \]

# Пример: направление из точки в точку (фрагмент k15_task2_bearing.py)
import math
from k15_task1_haversine import GeoPoint

def bearing_deg(p1: GeoPoint, p2: GeoPoint) -＞ float:
    lat1, lon1 = math.radians(p1.lat_deg), math.radians(p1.lon_deg)
    lat2, lon2 = math.radians(p2.lat_deg), math.radians(p2.lon_deg)
    d_lon = lon2 - lon1
    y = math.sin(d_lon) * math.cos(lat2)
    x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(d_lon)
    return math.degrees(math.atan2(y, x)) % 360.0

Два уточнения для честности картины. Первое: вдоль дуги большого круга направление постепенно меняется — формула возвращает начальный азимут. На дистанциях дрона (единицы километров) изменение составляет доли градуса и практики не касается. Второе: демонстрация печатает для цели строго на восток значение 89.96°, а не ровно 90°. Это не ошибка, а свойство сферы: дуга большого круга между двумя точками одной параллели слегка выгибается к полюсу, поэтому стартует чуть севернее чистого востока. Тест фиксирует направление с допуском и объясняет причину в комментарии.

7. Смещение точки на N метров

Третья задача — обратная к расстоянию и направлению: даны исходная точка, направление и дистанция; найти координаты конечной точки. Именно она превращает математику в маршрут: «отойти на 150 метров на северо-восток от дома», «построить квадрат 200 × 200 метров вокруг точки съёмки», «держать круг радиусом 50 метров вокруг цели». Формула конечной точки (destination point) также берётся из канонического справочника. В ней появляется угловое расстояние \(\delta = d / R\) — дистанция в метрах, выраженная в радианах дуги:

\[ \begin{aligned} \delta &= d / R \\ \varphi_2 &= \arcsin\bigl( \sin\varphi_1 \cos\delta + \cos\varphi_1 \sin\delta \cos\theta \bigr) \\ \lambda_2 &= \lambda_1 + \operatorname{atan2}\bigl( \sin\theta \sin\delta \cos\varphi_1,\;\; \cos\delta - \sin\varphi_1 \sin\varphi_2 \bigr) \end{aligned} \]

# Пример: смещение точки (фрагмент k15_task3_destination.py)
import math
from k15_task1_haversine import EARTH_RADIUS_M, GeoPoint

def destination_point(origin: GeoPoint, bearing_target_deg: float, distance_m: float) -＞ GeoPoint:
    lat1 = math.radians(origin.lat_deg)
    lon1 = math.radians(origin.lon_deg)
    theta = math.radians(bearing_target_deg)
    delta = distance_m / EARTH_RADIUS_M

    lat2 = math.asin(
        math.sin(lat1) * math.cos(delta)
        + math.cos(lat1) * math.sin(delta) * math.cos(theta)
    )
    lon2 = lon1 + math.atan2(
        math.sin(theta) * math.sin(delta) * math.cos(lat1),
        math.cos(delta) - math.sin(lat1) * math.sin(lat2),
    )
    return GeoPoint(math.degrees(lat2), math.degrees(lon2))

Как убедиться, что формула перенесена в код верно? Тем же приёмом, что и в уроке K14: не сверять числа глазами, а проверить согласованность цепочки. Если сместить точку на 300 метров по направлению 45°, то обратный пересчёт обязан вернуть дистанцию 300 метров (гаверсинус) и направление 45° (bearing). Демо практики печатает именно эту обратную проверку, а тесты фиксируют её с допусками pytest.approx.

8. Домашняя позиция и простейшая геозона

Домашняя позиция (home position, «дом») — точка, которую автопилот запоминает при первом уверенном определении позиции и при взведении моторов. К ней аппарат возвращается по команде RTL (Return To Launch — «возврат на точку старта»), она же служит началом отсчёта относительной высоты. В симуляторе SITL домашняя позиция создаётся при запуске; на реальном аппарате — на месте старта.

Геозона (geofence) — виртуальная граница, за которую аппарату вылетать запрещено. В ArduPilot существует полноценный механизм геозоны (geofence) с группой параметров, имена которых начинаются с FENCE_ (например, FENCE_ENABLE, FENCE_RADIUS, FENCE_ALT_MAX); настраивать их мы будем в модулях M4 и M6. Учебная версия повторяет логику простейшего случая: разрешённая зона — круг заданного радиуса вокруг домашней точки с потолком высоты. Функция проверки возвращает один из трёх статусов: "OK", "OUT_OF_RADIUS" или "ABOVE_CEILING". Настоящая система по результату проверки запускает защитное действие — возврат домой или посадку; этот механизм называется failsafe, и вы знакомы с ним со вводного урока K00.

# Пример: простейшая геозона (фрагмент k15_task4_geofence.py)
from k15_task1_haversine import GeoPoint, haversine_distance_m

def geofence_status(position: GeoPoint, alt_m: float, home: GeoPoint,
                    radius_m: float, max_alt_m: float) -＞ str:
    distance_m = haversine_distance_m(home, position)
    if distance_m ＞ radius_m:
        return "OUT_OF_RADIUS"
    if alt_m ＞ max_alt_m:
        return "ABOVE_CEILING"
    return "OK"

9. Тесты геодезического модуля

Тесты урока продолжают стратегию K14: вместо «сверить числа глазами» проверяются опорные значения, свойства функций и согласованность цепочки преобразований. Файл k15_task5_test_geo.py содержит двенадцать тестов трёх классов.

• Опорные значения: расстояние от точки до самой себя равно нулю; направление строго на север равно 0°, строго на юг — 180°.
• Свойства: расстояние симметрично (порядок точек не важен); смещение строго на север увеличивает широту и не меняет долготу; один градус широты равен \(R \cdot \Delta\varphi\) в радианах.
• Согласованность цепочки: сместили точку — гаверсинус вернул исходную дистанцию, bearing вернул исходное направление; статусы геозоны соответствуют трём сценариям нарушения и одному корректному.

# Пример: тесты свойств и согласованности (фрагмент k15_task5_test_geo.py)
import math
import pytest
from k15_task1_haversine import EARTH_RADIUS_M, GeoPoint, haversine_distance_m
from k15_task3_destination import destination_point

HOME = GeoPoint(55.7558, 37.6173)

def test_one_degree_latitude():
    north = GeoPoint(HOME.lat_deg + 1.0, HOME.lon_deg)
    expected = EARTH_RADIUS_M * math.radians(1.0)
    assert haversine_distance_m(HOME, north) == pytest.approx(expected, rel=1e-9)

def test_destination_round_trip_distance():
    target = destination_point(HOME, 45.0, 300.0)
    assert haversine_distance_m(HOME, target) == pytest.approx(300.0, rel=1e-6)

Отдельно прокомментируем выбор допусков. Для расстояний используется относительный допуск rel: он масштабируется вместе с величиной. Для направлений в градусах используется абсолютный допуск abs: например, проверка «на восток» написана как pytest.approx(90.0, abs=0.5), потому что истинное значение 89.96 — не ошибка кода, а физика большого круга, и тест обязан это учитывать, оставаясь при этом строгим к настоящим поломкам.

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

11. Практика

Практика урока состоит из пяти файлов: четыре модуля геодезической математики и один файл тестов. Новые пакеты не потребуются: урок использует только стандартную библиотеку (math, dataclasses) и уже установленный pytest. Порядок работы: создайте файлы задач 15.1–15.5 в каталоге code/M01_python, выполните прогоны из задачи 15.6 и сохраните исходники в Git (задача 15.7). В карточках в аккордеонах лежит полный текст каждого файла; он совпадает с содержимым репозитория курса.

# Каталог: code/M01_python (venv активен, новых пакетов не требуется)
python k15_task1_haversine.py
python k15_task2_bearing.py
python k15_task3_destination.py
python k15_task4_geofence.py
python -m pytest k15_task5_test_geo.py -v

task1: Москва -＞ Балашиха: 22095 м
       один градус широты: 111195 м
       та же точка: 0.00 м

task2: направление на север:   0.00 град
       направление на восток:  89.96 град
       направление на юг: 180.00 град
       направление на запад: 270.04 град
       направление Москва -＞ Балашиха:  73.52 град

task3: север: lat=55.7566993 lon=37.6173000 | обратная проверка: 100.00 м, 0.00 град
       восток: lat=55.7558000 lon=37.6188982 | обратная проверка: 100.00 м, 90.00 град
       северо-восток по диагонали: lat=55.7577077 lon=37.6206904 | обратная проверка: 300.00 м, 45.00 град

task4: висение на 20 м в 50 м от дома: dist   50.0 м, alt 20.0 м -＞ OK
       вылетел за радиус на 150 м: dist  150.0 м, alt 20.0 м -＞ OUT_OF_RADIUS
       поднялся на 60 м в пределах радиуса: dist   50.0 м, alt 60.0 м -＞ ABOVE_CEILING

tests: 12 passed

# Из корня репозитория курса
git add code/M01_python/k15_task1_haversine.py code/M01_python/k15_task2_bearing.py code/M01_python/k15_task3_destination.py code/M01_python/k15_task4_geofence.py code/M01_python/k15_task5_test_geo.py
git commit -m "K15: geo haversine, bearing, destination point, geofence, 12 tests"

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

1. Что измеряют широта и долгота и в каких диапазонах они изменяются?
2. Что такое WGS-84 и почему для наших задач Землю достаточно считать шаром среднего радиуса?
3. Почему один градус долготы соответствует разным расстояниям на разных широтах и как это связано с отказом от теоремы Пифагора по градусам?
4. Что такое большой круг и какую величину вычисляет формула гаверсинуса?
5. В какой договорённости функция bearing_deg() возвращает направление и в какую функцию урока K13 его можно подать напрямую?
6. Почему направление на точку той же широты равно 89.96°, а не ровно 90°?
7. Какую задачу решает функция destination_point()? Приведите два примера из планирования миссий.
8. Что такое домашняя позиция и как она связана с командой RTL и относительной высотой?
9. Какие статусы возвращает наша геозона и какие условия за ними стоят?
10. Почему сообщение GLOBAL_POSITION_INT передаёт координаты целыми числами, умноженными на 10⁷, и в каких единицах приходят высоты?

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

• Я создал все пять файлов практики в каталоге code/M01_python.
• Демо задачи 15.1 напечатало 22095 м для пары Москва — Балашиха и 111195 м для одного градуса широты.
• Я могу объяснить, почему направление строго на восток получилось 89.96°, а не 90.00°.
• Демо задачи 15.3 показало обратную проверку: дистанция и направление восстанавливаются после смещения.
• Демо задачи 15.4 показало все три статуса геозоны: OK, OUT_OF_RADIUS, ABOVE_CEILING.
• Команда python -m pytest k15_task5_test_geo.py -v завершается результатом 12 passed.
• Я могу объяснить разницу высоты над уровнем моря и относительной высоты и назвать поля GLOBAL_POSITION_INT.
• Я сделал git commit с файлами урока K15.

14. Что дальше

• Предыдущий урок: K14 · Углы Эйлера, кватернионы и DCM
• Текущий урок: K15 · Геокоординаты: lat/lon/alt, расстояние haversine, направление bearing
• Следующий урок: K16 · Слои связи «для дронов» — начало модуля M3 «Протоколы и интерфейсы»
• Модуль M3 пройдёт путь от физики байтов на проводе до UDP-сокетов и ручного разбора бинарных кадров; после лабораторной работы K22 сдаётся гейт G2.