Skip to content

Autonomous Navigation

AQUA Stack's autonomous navigation system enables underwater vehicles to execute complex missions safely and efficiently.

Overview

The navigation system transforms high-level mission goals into precise vehicle commands through:

  • Global Planning: Optimal paths between waypoints
  • Local Planning: Real-time obstacle avoidance
  • Path Following: Accurate trajectory tracking
  • Behavior Trees: Hierarchical mission logic
  • Dynamic Replanning: Adaptation to changing conditions

Architecture

graph TD
    A[Mission Waypoints] --> B[Global Planner]
    B --> C[Path Smoother]
    C --> D[Local Planner]
    E[SLAM Position] --> D
    F[Obstacle Detection] --> D
    D --> G[Path Tracker]
    G --> H[Vehicle Controller]

    style B fill:#4CAF50
    style D fill:#FF9800
    style G fill:#2196F3

Key Features

3D Waypoint Navigation

Navigate through 3D underwater space with precise waypoint following:

waypoints:
  - position: [0, 0, 0]      # Start
  - position: [10, 0, -5]    # 10m forward, 5m down
  - position: [10, 10, -5]   # 10m right
  - position: [0, 10, 0]     # Return to surface

Learn more →

Adaptive Path Following

Follow planned paths while adapting to: - Water currents - Vehicle dynamics - Sensor noise - Unexpected obstacles

Learn more →

Behavior-Based Control

Compose complex behaviors from simple primitives:

Survey Mission:
├─ GoToDepth(-5m)
├─ Repeat(5)
│  └─ FollowTransect(100m)
└─ ReturnHome()

Learn more →

Dynamic Replanning

Automatically replan when conditions change: - Obstacles detected - Path blocked - Current too strong - Battery low

Learn more →

Mode 1: Waypoint Navigation

Best for: Simple missions, site surveys

mission = WaypointMission([
    Waypoint(x=0, y=0, z=0),
    Waypoint(x=10, y=0, z=-5),
    Waypoint(x=10, y=10, z=-5),
])
mission.execute()

Mode 2: Path Following

Best for: Pipeline inspection, transects

path = generate_pipeline_path(
    start=[0, 0, -5],
    direction=[1, 0, 0],
    length=100,
    offset=2.0  # 2m from pipeline
)
follow_path(path, speed=0.5)

Mode 3: Station Keeping

Best for: Photography, sampling

station_keep(
    position=[10, 5, -8],
    tolerance=0.2,  # meters
    duration=300    # 5 minutes
)

Mode 4: Terrain Following

Best for: Seafloor mapping, inspection

terrain_follow(
    altitude=2.0,    # 2m above seafloor
    max_depth=-50,   # Safety limit
    speed=0.3
)

Coordinate Frames

AQUA Stack uses multiple reference frames:

World Frame (NED)

  • Origin: Mission start or known position
  • X: North
  • Y: East
  • Z: Down (positive downward)

Vehicle Frame (FRD)

  • Origin: Vehicle center of mass
  • X: Forward
  • Y: Right
  • Z: Down
# Transform waypoint from world to vehicle frame
waypoint_vehicle = transform_world_to_vehicle(
    waypoint_world=[10, 5, -8],
    vehicle_pose=current_pose
)

Planning Algorithms

Global Planner

Computes optimal path from start to goal:

Algorithms Available: - A*: Fast, optimal for known maps - RRT*: Good for complex environments - Visibility Graph: Fast for 2D planning

Cost Functions: - Distance (default) - Energy consumption - Risk (proximity to obstacles) - Time (accounting for currents)

Local Planner

Real-time obstacle avoidance:

Algorithms: - DWA (Dynamic Window Approach): Fast, suitable for dynamic environments - TEB (Timed Elastic Band): Optimal velocity profiles - APF (Artificial Potential Field): Simple, reactive

Path Smoother

Optimizes global path for vehicle dynamics: - Removes sharp turns - Respects velocity limits - Minimizes energy - Ensures feasibility

Control System

Path Tracking

Methods Available:

  1. Pure Pursuit: Simple, widely used 

    lookahead_distance = 2.0  # meters
steering = pure_pursuit(path, vehicle_pos, lookahead_distance)

  2. Line-of-Sight (LOS): Better for underwater vehicles 

    los_distance = 3.0
steering = los_guidance(path, vehicle_pos, los_distance)

  3. Model Predictive Control (MPC): Most advanced 

    horizon = 10  # prediction steps
control = mpc_track(path, vehicle_state, horizon)

Velocity Control

Adapts speed based on: - Path curvature (slow in turns) - Obstacle proximity - SLAM confidence - Battery state

velocity_profile = compute_velocity(
    path=planned_path,
    max_speed=1.0,
    min_speed=0.2,
    deceleration_distance=3.0
)

Safety Features

Geofencing

Define safe operating area:

geofence:
  type: polygon
  vertices:
    - [0, 0]
    - [100, 0]
    - [100, 100]
    - [0, 100]
  depth_min: 0
  depth_max: -50

Collision Avoidance

Multiple layers of protection:

  1. Global planning: Avoid known obstacles
  2. Local planning: Reactive avoidance
  3. Emergency stop: Hard limit at 1m

Return-to-Home

Automatic return triggers: - Low battery (<20%) - Lost localization - Communication timeout - Manual activation

rtl_config = {
    'ascent_rate': 0.5,  # m/s
    'surface_swim_speed': 0.3,
    'return_path': 'direct',  # or 'retrace'
}

Performance Metrics

Typical navigation performance:

Metric Value Conditions
Waypoint accuracy ±0.5m Good SLAM
Path following error ±0.3m RMS Steady state
Planning time <1s <100 waypoints
Replan frequency As needed Typically 0.1Hz
Max operating speed 2 m/s Vehicle dependent

Configuration

Key navigation parameters in ~/.config/aqua/navigation.yaml:

navigation:
  global_planner:
    algorithm: astar
    resolution: 0.5  # meters
    inflation_radius: 1.0

  local_planner:
    algorithm: dwa
    horizon: 2.0  # seconds
    sim_time: 3.0

  path_tracker:
    method: los
    lookahead: 2.5  # meters
    cross_track_gain: 0.5

  velocity:
    max_linear: 1.0  # m/s
    max_angular: 30  # deg/s
    acceleration_limit: 0.3  # m/s²

Next Steps

Explore each navigation component in detail:

Or jump to practical guides: