3D Voxel Navigation — autonomous path planning through unknown terrain.
AEOS builds a live 3D occupancy grid from every OAK-D depth frame, then searches it with A-Star — a graph-search algorithm that finds the lowest-cost route through free voxels while avoiding obstacles and people. RTK GPS corrects position to 2 cm, dual TFMini LiDARs and YOLO detections fuse into a 9-sector reactive map, and the 25 Hz navigator sends MAVLink setpoints to the Pixhawk — all on the Jetson Orin NX in under 23 MB of RAM.
| RTK GPS accuracy | 2 cm |
| Navigator loop rate | 25 Hz |
| A-Star path plan time | <100 ms |
| 360° full scan time | 24 s |
| Outer voxel grid | 4 M cells · 0.5 m |
| Inner voxel grid | 256 K cells · 0.25 m |
| A-Star connectivity | 26-connected 3D |
| Obstacle sectors | 9 directions |
| Navigation RAM budget | 13 MB |
| Depth integration rate | 3 Hz → 48K pts/s |
System Architecture
Six tightly integrated modules run concurrently at 3–25 Hz, sharing state through a typed blackboard with ~189 keys. The mission executor sits above, issuing waypoints and scan commands. The navigator sits below, consuming sensor data and publishing safe velocities to the Pixhawk.
RTK GPS — 2 cm Positioning
The Unicore UM982 dual-antenna receiver connects to Portugal's free ReNEP NTRIP network. RTCM correction data from the nearest base station is injected in real time, lifting GPS accuracy from the standard ~1.5 m down to 2 cm when a fixed solution locks. RTK status (none → float → fixed) is tracked on the blackboard and surfaced live in the ground station telemetry panel.
| Blackboard Key | Value | Meaning |
|---|---|---|
| /nav/rtk_status | "fixed" | Float or fixed RTK solution |
| /nav/rtk_accuracy_m | 0.02 | Estimated horizontal accuracy (m) |
| /nav/ntrip_connected | true | RTCM stream active |
| /nav/ntrip_rtcm_age_ms | <500 | Age of last correction packet |
| /vehicle/gps_fix_type | 6 | 6 = RTK fixed (ArduPilot scale) |
Standard GPS at ±1.5 m means voxel maps from different flights can be misaligned by up to 3 m — more than six 0.5 m voxel widths. With RTK fixed at ±2 cm, maps from consecutive flights align to within 4 cm, allowing the system to accumulate knowledge across missions at the same site.
UM982 dual-antenna RTK on UART GPS1 at 230 400 baud
Position accuracy drives voxel map anchoring precisionNTRIP client injects RTCM corrections from ReNEP base station
Free national network · reconnects automatically on link dropDual-Zone 3D Voxel World Model
AEOS builds a persistent 3D occupancy grid from every OAK-D depth frame. A two-zone hierarchical design gives high resolution where it matters (near the drone) and coarser coverage for path planning at range — without exceeding the 50 MB RAM budget on the Jetson Orin NX.
| Zone | Coverage | Resolution | Cells | RAM |
|---|---|---|---|---|
| Inner | 20×20×10 m | 0.25 m | 256K | 0.49 MB |
| Outer | 100×100×50 m | 0.5 m | 4M | 7.63 MB |
- Re-center inner grid on drone position (shifts when drone moves >2.5 m)
- Subsample every 4th pixel → ~16K 3D points per frame
- Project each point to world NED using drone pose from Pixhawk EKF
- Mark OCCUPIED in inner grid (≤10 m) and outer grid
- Ray-cast from drone to each point → mark intermediate voxels FREE
- Integrate TFMini forward (1D ray) and TFMini ground (terrain layer)
- Track changed voxels for incremental streaming to ground station
Maps save to aeos-flights/maps/{flight_id}.npz on shutdown and auto-save every 60 s during flight. On boot, AEOS loads the most recent map if the GPS home position matches within 50 m — enabling cumulative site knowledge across missions. Compressed size: ~1–2 MB per 100 m³ map.
Resolution vs. Sensor Accuracy
| Range | OAK-D Noise | 0.25 m voxel |
|---|---|---|
| 2 m | 2–4 cm | 6× noise ✓ |
| 5 m | 5–10 cm | 2.5× noise ✓ |
| 10 m | 10–20 cm | 1.25× noise |
YOLO-Driven Obstacle Costs
OAK-D YOLO detections annotate voxels with semantic costs. A-Star prefers low-cost routes even when geometry is clear — a path past a person costs more than a longer path through empty space.
Jetson Orin NX — RAM Allocation
360° Environmental Scan
When situational awareness is incomplete — at mission start, before a tight corridor, or on sensor discrepancy — AEOS commands the drone to perform a full 360° scan in place. The OAK-D Pro captures depth at 8 headings 45° apart. With 72° HFOV, adjacent captures overlap by ≈27°, ensuring no angular blind spots. The full scan takes approximately 24 seconds.
- Mission start — before executing the first waypoint
- Sensor discrepancy — TFMini and OAK-D disagree by >2 m in the same sector
- CORRIDOR mode entry — before path planning in a confined space
- Manual trigger — operator command via ground station
- Hover point — at each inspection waypoint (configurable)
Sensor Fusion & 9-Sector Reactive Map
Every 25 Hz navigator tick, AEOS builds a 9-sector directional obstacle map by fusing OAK-D depth, both TFMini LiDARs, and YOLO spatial detections. Each sector holds the closest obstacle distance, threat level, and source sensor. Cross-validation between TFMini and OAK-D catches sensor faults — disagreements >2 m flag a discrepancy on the blackboard.
| Sensor | Sectors | Rate | Notes |
|---|---|---|---|
| OAK-D depth | FWD, FWD-L, FWD-R, UP, DN | 10 Hz | 72° HFOV split into arcs |
| TFMini forward | FORWARD only | 10 Hz | Cross-validated vs OAK-D depth |
| TFMini ground | DOWN only | 10 Hz | Terrain AGL, altitude safety |
| YOLO detections | any with spatial_xyz | 3 Hz | Semantic override — person <avoid_m → STOP |
| 360° scan voxels | REAR, LEFT, RIGHT | on scan | Populated from voxel map post-scan |
When TFMini forward and OAK-D depth disagree by >2 m for the FORWARD sector, AEOS flags /nav/sensor_discrepancy and trusts the closer reading (conservative). SensorHealth downgrades /nav/health and may trigger a 360° scan to improve situational awareness.
A-Star Path Planning through 3D Voxel Space
AEOS uses a 26-connected A-Star search through the outer voxel grid to find paths from the drone's current position to a goal in NED coordinates. Obstacles are inflated by a configurable safety margin. YOLO semantic costs add weight to routes near people or vehicles. Path planning on the full 200×200×100 grid completes in under 100 ms on the Jetson CPU.
For inspection missions in confined spaces, AEOS extracts corridor centerlines from a horizontal voxel slice at the target altitude:
Navigation Modes
AEOS auto-selects a navigation mode based on mission type and state. Each mode carries its own safety margins, terrain-following behaviour, and path strategy. The mode is exposed on the blackboard as /nav/mode and visible in the ground station telemetry panel.
SURVEY
High-altitude grid or transect missions over fields. Wide obstacle margins (4 m stop, 12 m clear). Terrain following enabled at survey altitude. A-Star used to route around tall obstacles detected during approach. Default mode for agricultural mapping missions.
INSPECT
Low-altitude close-range inspection of structures, tree canopies, or row edges. Tight margins (2 m stop). OAK-D vision is critical — nav_health degrades immediately if OAK-D drops. 360° scan triggered at each inspection waypoint for complete situational awareness before approach.
CORRIDOR
Confined-space navigation through tunnels, rows, or hedged paths. Uses A-Star path planning with corridor centerlines from the voxel slice. Lateral avoidance is primary (no heading into walls). Safety margin tighter still — the planned path keeps the drone centred in the free corridor.
RTL
Return-to-Launch with maximum safety margins. Speed limited to 3 m/s regardless of plan. A-Star plans the RTL route through the already-mapped space. If nav_health drops below 0.3, RTL activates automatically and AEOS publishes a failsafe alert to the ground station.
RGB Texture Mapping — Coloured 3D World
Beyond geometry, AEOS records RGB keyframes paired with exact camera poses from the Pixhawk EKF. Each occupied voxel is coloured from the best available keyframe — the result is a photorealistic coloured point cloud streaming to the ground station in real time, and a textured 3D mesh available post-flight via marching cubes reconstruction.
A new keyframe (RGB frame + pose) is saved when any of the following is true:
Drone moved >1.0 m since last keyframe
Ensures adequate scene coverage without excessive redundancyDrone rotated >10° since last keyframe
Captures new geometry exposed by yaw during 360° scans>50 new occupied voxels discovered since last frame
Rapid scene entry → dense keyframing for texture qualityGround Station — Real-Time 3D Map Viewer
The ground station receives two complementary streams over MQTT. A fast 2D horizontal slice at 1 Hz gives immediate situational awareness. A 3D incremental delta stream at 2 Hz accumulates into a full three-dimensional reconstruction in the browser — orbit-able, zoomable, with drone position and planned path overlaid.
Horizontal voxel slice at drone altitude — RLE compressed, ~3 KB
Drone position + A-Star path + corridor centerlines + scan progress overlaidOnly changed voxels since last frame — (x, y, z, state, r, g, b) tuples
~100–500 changed voxels/frame · 360° scan burst ~5K voxels · 45 KB| Stream | Size | Rate | Bandwidth |
|---|---|---|---|
| 2D map slice | ~3 KB | 1 Hz | ~3 KB/s |
| 3D voxel deltas | ~2 KB | 2 Hz | ~4 KB/s |
| Map stats + path | ~0.5 KB | 1 Hz | ~0.5 KB/s |
| Total map overhead | ~7.5 KB/s | ||
| Existing streams | ~86 KB/s |
