SLAM
NAVIGATION

SLAM (Simultaneous Localization and Mapping) is the primary alternative to GPS for autonomous drone position estimation. It uses the drone's existing camera — no additional hardware needed — to estimate position and build a map of the environment.

Varför SLAM när GPS störs

SLAM löser det fundamentala GPS-nekade navigeringsproblemet: utan extern positionsreferens vet drönaren inte var den befinner sig. SLAM använder kamerabilder för att identifiera visuella punkter (hörn, kanter, texturer) i omgivningen, spårar hur dessa punkter rör sig mellan bildrutor, och beräknar drönarens rörelse relativt dem. Resultatet: en 3D-karta av omgivningen OCH drönarens position inom den kartan — simultant.

ORB-SLAM3 extraherar ORB-features (Oriented FAST and Rotated BRIEF) från varje kamerabild — typiskt 500–2 000 punkter per bild beroende på scenens texturrikedom. Skog med varierade trädkronor: 1 500+ features. Snöfält: 50–100 features — otillräckligt för tillförlitlig SLAM. Feature-fattiga miljöer (snö, vatten, dimma) orsakar SLAM-förlust och kräver fallback till IMU-dödräkning.

ORB-SLAM3-bearbetning på Jetson Orin Nano tar 22 ms per bild vid 640×480 upplösning — 45 FPS kapacitet, mer än tillräckligt för 30 FPS kamerainmatning. Vid 1280×960: bearbetningstiden ökar till 55 ms (18 FPS) — acceptabelt men med reducerad marginal för parallella AI-uppgifter. Rekommendation: 640×480 för SLAM, separat kamera vid full upplösning för YOLOv8.

Så fungerar det

Drift ackumuleras linjärt med flygd sträcka: 2–5% typiskt. En drönare som flygit 1 km har 20–50 m positionsosäkerhet. Vid 5 km: 100–250 m — oacceptabelt för precisionsstrike men tillräckligt för ISR (kamerafotavtrycket vid 120 m AGL täcker 300 m). Driften varierar med scenens visuella kvalitet: texturrikt landskap ger lägre drift, monotona ytor ger högre.

Loop closure är SLAM:s mest kraftfulla korrigeringsmekanism. När drönaren återvänder till en tidigare besökt plats identifierar ORB-SLAM3 visuella features som matchar sin sparade karta. Denna igenkänning triggar en grafoptimering som retroaktivt korrigerar hela banan — driften elimineras vid loop closure-punkten. Fischer 26 i elliptisk ISR-bana passerar samma punkter var 15:e minut — perfekt för loop closure.

IMU-integration (Pixhawk ICM-42688) tillhandahåller högfrekvent rörelseestimering (400 Hz) som överbryggar periodera då SLAM tappar visuell spårning (snabb rotation, feature-fattiga scener). EKF3 fusionerar SLAM-position (10–30 Hz, hög noggrannhet) med IMU-data (400 Hz, snabb drift) för att producera en jämn positionsestimering som uppdateras 400 gånger per sekund.

Matematisk härledning — visuell odometri från feature-spårning

Den fullständiga fyrstegs-härledningen av hur ORB-SLAM3 omvandlar sekvenser av kamerabilder till drönarrörelse publiceras på den engelska versionen. Matematiken följer standard multi-view geometry (Hartley & Zisserman 2004 kap. 9–11; Mur-Artal et al. IEEE T-RO 2015). Sidans bidrag: att göra explicit felfortplantningsbanan från featurenivå-brus till drönarnivå-positionsosäkerhet.

Kortfattat: (1) Varje bildruta producerar 500–2000 ORB-features som matchas mellan ramar via Hamming-avstånd med Lowes kvotstest. (2) Essentialmatrisen E uppskattas med RANSAC från matchningar (minimum 5-punktsalgoritm, Nistér 2004). (3) E dekomponeras via SVD till relativ (R, t) — translation utan skala i monokulär SLAM (skala injiceras från IMU eller barometer). (4) Drift ackumuleras som σ_pos ≈ σ_inkremental × √N över N bildramar utan loop closure; loop closure återställer driften genom pose graph-optimering.

Bearbetat exempel 1 — 5-minuters FPV-uppdrag över skog (ingen loop closure)

9000 bildramar × 0,016 m/ram (skog degraderar match-rate till 40 %) × √9000 = 1,5 m random-walk-1σ. Systematisk bias dominerar på långa tidshorisonter: ~3 m totalt 1σ. Kombinerat med YOLOv8-detekterings­osäkerhet (±0,56 m vid 120 m AGL): σ_total = √(3² + 0,56²) ≈ 3,1 m. Jämförelse med GPS-beroende uppdrag: GPS ±2 m + detektion ±0,56 m = σ_total ~2,1 m. GPS-denied degraderar måluppbyggnad från 2,1 m till 3,1 m — acceptabelt för områdesundertryckning, marginellt för precisionsanfall mot punktmål. Verifierat i provable_claims.py under SLAM_DRIFT_5MIN_FOREST.

Bearbetat exempel 2 — 30-minuters Fischer 26 ISR med orbit-loop-closures

Fischer 26 i 3 km-radie orbit, 6-minutersperiod, 4 loop closures under 30 min. 10 800 bildramar/segment × 0,008 m/ram × √10 800 = 0,83 m 1σ topp precis före varje closure, återställs till 0,1 m efter. Genomsnittlig osäkerhet över uppdraget: ~0,5 m. Utan loop closures: 1,9 m 1σ och växande monotont. Operativt värde: 10× förbättring för persistent ISR, marginell för envägs-uppdrag. Verifierat i SLAM_DRIFT_30MIN_ISR_LOOPS.

Varför denna härledning är operativt viktig

Fyra operativa beslut beror på att denna SLAM-felmodell är korrekt. Uppdragsplanering: måluppbyggnadsfelet (3,1 m skog-FPV mot 1,0 m strukturerad-terräng-ISR) avgör om SLAM är acceptabelt för ett givet uppdrag. Orbit-mönsterdesign: kortare orbit (6 min) ger 0,83 m toppfel, längre (15 min) ger 1,3 m — täcknings­planeraren måste avväga orbitradie mot positionsnoggrannhet. Terrängval: SLAM fungerar i skog (terrain_quality = 0,5) med halverad noggrannhet men misslyckas helt över öppen snö eller vatten (terrain_quality < 0,3). Kalibreringsdisciplin: systematisk bias dominerar över lång tid — schackbrädes-kalibrering var 100:e flygtimme minskar bias från ~3 m/5 min till < 1 m/5 min, större operativ förbättring än att fördubbla SLAM-bildfrekvensen. Detta säger ingenjörerna var de ska lägga sin ansträngning: kalibrerings­disciplin slår algoritmoptimering för fältnoggrannhet.

Implementering

ORB-SLAM3 runs on the Jetson Orin Nano alongside YOLOv8. It uses the same camera — no extra hardware. Processing cost: ~5ms per frame on Jetson Orin. Position estimate fed directly into ArduPilot's EKF3 as an external position source (replaces GPS). Configuration: EK3_SRC1_POSXY=6 (ExternalNav), VISO_TYPE=1 (MAVLink vision position).

External source: SLAM – Wikipedia

Implementering

# 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

Källor

SLAM fungerar inte vid natt (ingen visuell information) utan tilläggsbelysning. Fischer 26:s IR-strålkastare (940 nm, osynlig för blotta ögat) belyser terrängen nedanför och ger ORB-SLAM3 tillräcklig visuell information för natt-SLAM — men räckvidden är begränsad till 100 m AGL (IR-intensiteten avtar med kvadraten på avståndet). Över 100 m: SLAM degraderar till IMU-dödräkning.

ORB-SLAM3 Performance Characteristics

ORB-SLAM3:s prestandakrav varierar med uppdraget. ISR (Fischer 26 vid 200 m, långsam orbithastighet): drift acceptabel, loop closure var 15 minut. FPV-strike (80+ km/h, låg höjd): SLAM-drift kritisk vid terminala inflygningen — de sista 100 metrarna kräver visuell pilotförvärvning oavsett SLAM-prestanda.

ORB-SLAM3 öppen källkod (C++, GPU-accelererad). Jetson Orin Nano CUDA-optimering. EKF3 sensorfusionsdokumentation (ArduPilot). SLAM-prestandadata från FSG-A fälttester vid Vidsel 2024–2025.

Performance Comparison — GPS vs SLAM vs Dead Reckoning

Tre navigeringslägen ger dramatiskt olika positionsnoggrannhet under en 30-minutersflygning med Fischer 26: GPS-aktiverat läge ger 3–5 meters noggrannhet konstant. SLAM med loop closure ger 25 meters drift per 10 minuter med periodisk korrigering. Enbart IMU-dödräkning ger 140 meters drift per 10 minuter — oacceptabelt för ISR men tillräckligt för att nå det allmänna målområdet under en nödåtervändning.

Relaterade kapitel