SLAM
НАВІГАЦІЯ
SLAM (Simultaneous Localization and Mapping) — основна альтернатива GPS для автономної оцінки позиції дрона. Він використовує наявну камеру дрона — додаткового обладнання не потрібно — для оцінки позиції та побудови карти середовища.
Навіщо SLAM, коли GPS заглушено
GPS покладається на супутникові радіосигнали. Ворог глушить ці сигнали. Ваш дрон не чує супутники. GPS-позиція переходить із 2-метрової точності на «я не знаю, де я».
SLAM покладається на те, що камера БАЧИТЬ. Світло рухається зі швидкістю світла і не може бути заглушене радіообладнанням. Ворог мав би блокувати все світло (повна темрява чи туман), щоб перемогти візуальний SLAM — і навіть тоді теплові камери бачать у темряві. Камера, що дивиться на землю, невразлива до ворожої радіоелектронної боротьби.
Барометр (сенсор тиску повітря) дає висоту — також невразливий до глушіння, бо ніхто не може заглушити тиск повітря. Отже: камера для горизонтальної позиції, барометр для вертикальної. Разом вони замінюють GPS у РЕБ-зоні.
Як це працює
SLAM відстежує візуальні ознаки — кути, краї, характерні візерунки — на зображенні з камери. Коли дрон рухається, ці ознаки зсуваються на зображенні. З того, як вони зсуваються, алгоритм обчислює, як дрон рухався. Сотні ознак, відстежуваних одночасно, дають надійну оцінку, навіть коли деякі втрачаються (за перешкодами, розмиті рухом).
Частина «Mapping»: коли дрон досліджує нову місцевість, він додає нові ознаки до своєї карти. На зворотному шляху він розпізнає раніше бачені ознаки і виправляє накопичений дрейф. Це означає, що довші польоти стають БІЛЬШ точними на місцях, які дрон вже відвідував, а не менш.
Точність без GPS: приблизно 10–30 м абсолютна позиція після зіставлення рельєфу з попередньо завантаженим ортофото. Без зіставлення рельєфу (чиста візуальна одометрія): дрейф приблизно 1–3 м за хвилину польоту. Для 5-хвилинної FPV-ударної місії це 5–15 м помилки — прийнятно для більшості завдань наведення, оскільки точність AI-виявлення все одно становить приблизно 15–50 м.
Як будується карта
Перший політ: камера бачить точки інтересу (ORB features). З кожним кадром ORB-SLAM3 тріангулює 3D-позицію кожної точки. За 5 хвилин: карта з тисяч точок. Дрон повертається тим же маршрутом — впізнає орієнтири і коригує позицію. Замкнення циклу (loop closure) виправляє накопичений дрейф. Точність після loop closure: ±2–5 м.
Обмеження: потребує візуально різноманітної місцевості. Над водою (без орієнтирів) або однорідним лісом (усі дерева схожі) — ORB-SLAM3 втрачає трек. Нічний режим: тепловізор замість візуальної камери, але теплові ознаки менш стабільні. Обчислювальне навантаження: приблизно 30 % GPU Jetson при 30 FPS — залишає 70 % для YOLOv8.
Математичне виведення — візуальна одометрія з відстеження ознак
Повне чотирикрокове виведення того, як ORB-SLAM3 перетворює послідовність кадрів камери на рух дрона, опубліковано в англійській версії. Математика відповідає стандартній багатовидовій геометрії (Hartley & Zisserman 2004, розділи 9–11; Mur-Artal та ін. IEEE T-RO 2015). Внесок сторінки — зробити явним шлях поширення похибки від рівня ознаки до рівня позиційної невизначеності дрона.
Стисло: (1) Кожен кадр виробляє 500–2000 ORB-ознак, що зіставляються між кадрами за відстанню Хеммінга з тестом відношення Lowe. (2) Есенціальна матриця E оцінюється RANSAC з зіставлень (мінімум 5-точковий алгоритм, Nistér 2004). (3) E розкладається через SVD на відносні (R, t) — трансляція без масштабу в монокулярній SLAM (масштаб впроваджується з IMU або барометра). (4) Дрейф накопичується як σ_pos ≈ σ_incremental × √N протягом N кадрів без замикання циклу; замикання циклу скидає дрейф через оптимізацію графа поз.
Опрацьований приклад 1 — 5-хвилинне FPV-завдання над лісом (без замикання циклу)
9000 кадрів × 0,016 м/кадр (ліс знижує швидкість збігів до 40 %) × √9000 = 1,5 м 1σ випадкового блукання. Систематичне зміщення домінує на довгих часових горизонтах: ~3 м загалом 1σ. У поєднанні з невизначеністю виявлення YOLOv8 (±0,56 м на 120 м AGL): σ_total = √(3² + 0,56²) ≈ 3,1 м. Порівняно з GPS-залежним завданням: GPS ±2 м + виявлення ±0,56 м = σ_total ~2,1 м. GPS-denied знижує точність наведення з 2,1 м до 3,1 м — прийнятно для придушення площі, маргінально для точного удару по точкових цілях. Перевірено в provable_claims.py під SLAM_DRIFT_5MIN_FOREST.
Опрацьований приклад 2 — 30-хвилинна ISR Fischer 26 з орбітальними замиканнями циклу
Fischer 26 на орбіті радіусом 3 км, період 6 хв, 4 замикання циклу за 30 хв. 10 800 кадрів/сегмент × 0,008 м/кадр × √10 800 = 0,83 м 1σ піковий безпосередньо перед кожним замиканням, скидається до 0,1 м після. Середня невизначеність під час завдання: ~0,5 м. Без замикань циклу: 1,9 м 1σ, монотонно зростає. Оперативна цінність: 10× покращення для постійного ISR, маргінальне для односторонніх завдань. Перевірено в SLAM_DRIFT_30MIN_ISR_LOOPS.
Чому це виведення є операційно важливим
Чотири оперативні рішення залежать від коректності цієї моделі помилок SLAM. Планування завдань: похибка наведення (3,1 м FPV-ліс проти 1,0 м структурована-місцевість-ISR) визначає, чи SLAM прийнятний для даного завдання. Дизайн орбітального патерну: коротша орбіта (6 хв) дає 0,83 м пікову помилку, довша (15 хв) — 1,3 м — планувальник покриття має балансувати радіус орбіти проти точності позиції. Вибір місцевості: SLAM працює в лісі (terrain_quality = 0,5) з наполовину зменшеною точністю, але повністю провалюється над відкритим снігом чи водою (terrain_quality < 0,3). Дисципліна калібрування: систематичне зміщення домінує в довгостроковій перспективі — калібрування шахівницею кожні 100 льотних годин зменшує зміщення з ~3 м/5 хв до < 1 м/5 хв, більше оперативне покращення, ніж подвоєння частоти кадрів SLAM.
Реалізація
ORB-SLAM3 працює на Jetson Orin Nano паралельно з YOLOv8. Він використовує ту саму камеру — додаткове обладнання не потрібне. Вартість обробки: приблизно 5 мс на кадр на Jetson Orin. Оцінка позиції подається безпосередньо в EKF3 ArduPilot як зовнішнє джерело позиції (замінює GPS). Конфігурація: EK3_SRC1_POSXY=6 (ExternalNav), VISO_TYPE=1 (MAVLink vision position).
Тактичне застосування
Fischer 26 літає ISR-маршрутом і одночасно будує карту місцевості. При наступних вильотах: навігація за збереженою картою без GPS і без дрейфу. Це особливо цінно для повторюваних ISR-маршрутів — перший прохід із дрейфом, наступні — з точністю ±5 м.
Для FPV-дронів SLAM непрактичний: надто короткий політ (5–15 хвилин) і надто мало обчислювальних ресурсів (SpeedyBee F405 не тягне). FPV покладається на EKF3 з барометром і оптичним потоком — більший дрейф, але достатній для ударних місій, де ціль ідентифікується візуально.
Зовнішнє джерело: SLAM — Wikipedia
Реалізація — код
# pip install numpy
# ORB-SLAM3 Integration with ArduPilot via MAVLink
# pip install pymavlink
import time
import numpy as np
class ORBSLAM3Bridge:
"""Bridge ORB-SLAM3 pose to ArduPilot via VISION_POSITION_ESTIMATE."""
def __init__(self, mavlink_conn):
self.mav = mavlink_conn
self.origin_set = False
def send_vision_position(self, x_m, y_m, z_m, roll, pitch, yaw):
"""Send ORB-SLAM3 position to ArduPilot EKF3."""
self.mav.mav.vision_position_estimate_send(
int(time.time() * 1e6), # timestamp_usec
x_m, y_m, z_m, # position NED (meters)
roll, pitch, yaw, # orientation (radians)
[0] * 21 # covariance (not used)
)
def process_frame(self, orbslam3_output):
"""Convert ORB-SLAM3 camera frame to NED frame."""
# ORB-SLAM3 outputs in camera frame (right-down-forward)
# ArduPilot needs NED (north-east-down)
T = orbslam3_output.pose # 4x4 transformation matrix
# Camera to NED rotation
R_cam_to_ned = np.array([
[0, 0, 1], # Camera Z → North
[1, 0, 0], # Camera X → East
[0, 1, 0] # Camera Y → Down
])
position_cam = T[:3, 3]
position_ned = R_cam_to_ned @ position_cam
self.send_vision_position(
position_ned[0], position_ned[1], position_ned[2],
0, 0, 0 # Roll/pitch/yaw from ORB-SLAM3 rotation matrix
)
# ArduPilot config for ORB-SLAM3 integration:
# param set EK3_SRC1_POSXY 6 # ExternalNav
# param set EK3_SRC1_POSZ 6 # ExternalNav
# param set EK3_SRC1_YAW 6 # ExternalNav
# param set VISO_TYPE 1 # MAVLink vision positionМатематика піксель-до-руху — візуальна одометрія на 30 рядків
Математика відстеження ознак, що лежить в основі SLAM, досить проста, щоб записати її повністю. Два послідовні кадри ознаки на координатах зображення (u₁, v₁) і (u₂, v₂) плюс висота дрона h над поверхнею землі дають горизонтальну трансляцію. Якщо камера має фокусну відстань f (у пікселях), спроєктований рух на землю становить: Δx = (u₂ − u₁) × h / f, Δy = (v₂ − v₁) × h / f.
Це плоско-земне, надирне, безобертове наближення. Повний конвеєр ORB-SLAM3 обробляє обертання, перспективу і дисторсію лінзи, але фундаментальний зв\'язок залишається: піксельний рух масштабується лінійно з висотою. Вищий політ означає грубшу роздільність руху; нижчий — тоншу роздільність, але коротший часовий горизонт, перш ніж ознаки покинуть кадр.
# Minimal visual odometry — one feature, nadir camera
# This is the kernel of what ORB-SLAM3 does for ~2000 features at once.
import numpy as np
def feature_motion_to_ground_motion(u1, v1, u2, v2, altitude_m,
focal_length_px=2400):
"""Convert pixel displacement to ground displacement (meters).
Arducam IMX477 @ 1640x1232 with 6mm lens gives f ≈ 2400 px.
Assumes camera points straight down (nadir) and ground is flat.
"""
dx_px = u2 - u1
dy_px = v2 - v1
dx_m = dx_px * altitude_m / focal_length_px
dy_m = dy_px * altitude_m / focal_length_px
return dx_m, dy_m
# Example: drone at 120 m, feature moved 2 px right
dx, dy = feature_motion_to_ground_motion(800, 600, 802, 600, 120)
velocity_ms = dx * 30 # 30 fps
print(f"2 px/frame @ 120m = ground step {dx:.3f} m -> {velocity_ms:.1f} m/s")Накопичення дрейфу — чому замкнення циклу важливе
Кожна оцінка кадр-до-кадру несе малу помилку. Без корекції ці помилки накопичуються як випадкове блукання: стандартне відхилення помилки позиції зростає як квадратний корінь часу. Після n кадрів дрейф масштабується як √n. Замкнення циклу — протиотрута: коли дрон відвідує відомий орієнтир, накопичена помилка згортається до невизначеності замкнення циклу, типово 2–5 м.
# Drift scaling — random walk vs loop closure
import math
SIGMA_PER_FRAME_M = 0.015 # 1.5 cm per-frame uncertainty (typical)
FPS = 30
def drift_after_seconds(t_s, sigma_per_frame=SIGMA_PER_FRAME_M, fps=FPS):
"""Open-loop drift estimate (random walk, 1-sigma)."""
n_frames = t_s * fps
return sigma_per_frame * math.sqrt(n_frames)
for t in [10, 60, 300, 600]:
drift_m = drift_after_seconds(t)
print(f"{t:4d} s open-loop: drift 1-sigma ~= {drift_m:6.2f} m")
# 10 s: 0.26 m | 60 s: 0.64 m
# 300 s: 1.42 m | 600 s: 2.01 m (without loop closure)Злиття оцінок — мінімальний приклад
Дві незалежні SLAM-оцінки позиції можуть бути злиті до єдиної оцінки з нижчою невизначеністю. Правило — зважене середнє за оберненою дисперсією. З оцінками p₁ ± σ₁ і p₂ ± σ₂, злита позиція становить σ²_злита = 1 / (1/σ₁² + 1/σ₂²) і p_злита = σ²_злита × (p₁/σ₁² + p₂/σ₂²).
# Two independent SLAM estimates fused by inverse-variance weighting
def fuse_estimates(p1, sigma1, p2, sigma2):
w1, w2 = 1.0/sigma1**2, 1.0/sigma2**2
var_fused = 1.0 / (w1 + w2)
p_fused = var_fused * (p1*w1 + p2*w2)
return p_fused, var_fused**0.5
# ORB-SLAM3 says x = 100.5 m +/- 3 m; IMU dead reckoning says x = 98.0 m +/- 8 m
x, s = fuse_estimates(100.5, 3.0, 98.0, 8.0)
print(f"Fused x = {x:.2f} m +/- {s:.2f} m")
# -> 100.18 m +/- 2.82 m (better than either input alone)Продуктивність ORB-SLAM3
ORB-SLAM3 витягує Oriented FAST кутові ознаки з кадрів камери і відстежує їх між послідовними зображеннями для оцінки руху камери. Алгоритм потребує візуально характерної місцевості — будівлі, скелі, лінії дерев, перехрестя доріг — для генерації надійних ознак. У сприятливих умовах (міське середовище, структурована місцевість) дрейф позиції зменшується з 200 м/10 хв (лише IMU) до 5 м/10 хв після першого замкнення циклу. Замкнення циклу відбувається, коли дрон повторно відвідує попередньо картовану зону і розпізнає ті самі орієнтири, дозволяючи ретроактивно виправити накопичений дрейф.
У несприятливих умовах (однорідний лісовий покров, сніжні поля, відкрита вода) ORB-SLAM3 не може знайти достатньо ознак і повертається до мертвого числення лише з IMU. Оператор Fischer 26 має розуміти ці обмеження: планувати ISR-маршрути над місцевістю з візуальною різноманітністю, уникати тривалого польоту над безознаковими зонами і прийняти, що точність позиції деградує під час безознакових сегментів. Jetson Orin Nano запускає ORB-SLAM3 при 30 FPS, використовуючи приблизно 30 відсотків GPU-ємності — залишаючи 70 відсотків для одночасного виведення YOLOv8 на обох візуальних і теплових потоках.
Порівняння продуктивності — GPS vs SLAM vs мертве числення
Три режими навігації продукують драматично різну точність позиції за 30-хвилинну ISR-місію Fischer 26. GPS (коли доступний): ±2 м безперервно, нульовий дрейф. ORB-SLAM3 із замкненням циклу: ±5 м після першої орбіти, покращуючись з кожною наступною орбітою. Мертве числення (IMU + барометр + оптичний потік): ±200 м після 30 хвилин, безперервно погіршуючись. Помилка позиції безпосередньо впливає на точність наведення — координати, отримані з GPS-навігації, є артилерійського класу (50 м CEP), із SLAM — FPV-класу (достатньо для візуального пошуку), з мертвого числення — зонного класу (лише як секторний орієнтир).
← Частина Злиття сенсорів EKF3
Пов'язані розділи
Джерела
Параметричні джерела. ORB-SLAM3 — Campos et al., «ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial, and Multi-Map SLAM», IEEE Transactions on Robotics, том 37, 2021. Jetson Orin Nano специфікації — NVIDIA datasheet. Arducam IMX477 характеристики — Sony IMX477 datasheet + Arducam. ArduPilot параметри (EK3_SRC1_POSXY, VISO_TYPE) — документація ardupilot.org/dev. Код ORB-SLAM3 — github.com/UZ-SLAMLab/ORB_SLAM3 (комміт на main гілці).
Операційні оцінки — не верифіковано FSG-A в польових умовах. Дрейф 5 м/10 хв після loop closure в міських умовах — опубліковане значення Campos et al. (2021), не виміряне FSG-A на Fischer 26. «30 % GPU-ємності при 30 FPS на Orin Nano» — типова продуктивність з NVIDIA developer форумів, не виміряна FSG-A. «10–30 м точність після terrain matching» — розрахункова з типової роздільності ортофото, не виміряна. FSG-A не розгорнула ORB-SLAM3 на реальній дроновій платформі.
Зовнішні стандарти та джерела. Campos, C., Elvira, R., Rodríguez, J. J. G., Montiel, J. M. M., & Tardós, J. D. (2021). «ORB-SLAM3», IEEE Transactions on Robotics. ArduPilot документація з візуальної одометрії (ardupilot.org/dev). Меморандум FOI 8336 «GPS-Denied Navigation for UAS» (2024). Jetson Orin Nano продуктивні бенчмарки для visual SLAM (NVIDIA developer forums). Рамка ROS 2 для інтеграції SLAM-стеку.