Dynamic Replanning¶

Adaptive mission execution through real-time replanning.

When to Replan¶

AQUA Stack automatically replans when:

Obstacle detected on planned path
Path blocked by unexpected structure
Current too strong to follow path
Battery low - need shorter route home
SLAM confidence low - switch to safer mode

Replanning Triggers¶

Obstacle Detection¶

if obstacle_detected_on_path(current_path, obstacle_map):
    # Replan around obstacle
    new_path = global_planner.replan(
        start=current_position,
        goal=original_goal,
        avoid=detected_obstacles
    )

    if new_path is valid:
        switch_to_path(new_path)
    else:
        abort_mission()

Current Compensation¶

def check_current_feasibility(path, current_estimate):
    # Check if vehicle can make progress against current
    for segment in path:
        required_speed = segment.speed
        current_effect = dot(current_estimate, segment.direction)

        if current_effect > required_speed * 0.8:
            return False  # Current too strong

    return True

# If infeasible, find alternative path
if not check_current_feasibility(path, estimated_current):
    replan_with_current_awareness(estimated_current)

Energy Management¶

def energy_aware_replan(current_position, remaining_battery):
    # Estimate energy to complete mission
    mission_energy = estimate_mission_energy(current_path)

    # Estimate energy to return home
    return_energy = estimate_return_energy(current_position)

    # If insufficient energy, abort mission
    if mission_energy + return_energy > remaining_battery * 0.8:
        # Replan with intermediate waypoints removed
        shortened_mission = remove_optional_waypoints(current_mission)
        return shortened_mission

Replanning Strategies¶

Conservative Replanning¶

Minimal changes to original plan:

conservative_replan(
    original_path,
    obstacle,
    max_deviation=5.0  # meters from original
)

Aggressive Replanning¶

Completely new path if necessary:

aggressive_replan(
    current_position,
    goal,
    cost_map=updated_cost_map
)

Hybrid Approach¶

Try conservative first, fall back to aggressive:

def hybrid_replan(current_pos, goal, obstacle):
    # Try conservative
    new_path = conservative_replan(current_pos, goal, obstacle)

    if new_path is None or not is_safe(new_path):
        # Fall back to aggressive
        new_path = aggressive_replan(current_pos, goal)

    return new_path

Replanning Performance¶

Scenario	Replan Time	Success Rate
Single obstacle	<0.5s	95%
Complex environment	1-3s	80%
No feasible path	<1s (abort)	N/A

Smooth Transitions¶

Ensure smooth switching between paths:

def smooth_path_transition(old_path, new_path, current_pos):
    # Find connection point
    connection = find_nearest_point(new_path, current_pos)

    # Create transition curve
    transition = generate_smooth_curve(
        start=current_pos,
        end=connection,
        max_curvature=vehicle_max_curvature
    )

    # Concatenate
    return transition + new_path[connection_index:]

Replanning Frequency¶

Balance between: - Reactivity (replan often) - Stability (don't replan too often)

replan_config = {
    'min_replan_interval': 2.0,  # seconds
    'max_replan_interval': 10.0,
    'trigger_threshold': 'medium',
    'hysteresis': 0.5  # avoid oscillation
}

Fallback Behaviors¶

If replanning fails:

Stop and assess: Halt vehicle, analyze situation
Return to last known good position
Surface: If underwater
Emergency ascent: Last resort

def replanning_fallback(replan_attempts):
    if replan_attempts < 3:
        return 'retry_replan'
    elif replan_attempts < 5:
        return 'return_to_safe_position'
    else:
        return 'emergency_surface'

Configuration¶

replanning:
  enabled: true
  strategy: hybrid
  min_interval: 2.0
  max_attempts: 5
  deviation_tolerance: 10.0

  triggers:
    obstacle: true
    current: true
    battery: true
    slam_quality: true

  fallback:
    - retry
    - return_to_safe
    - surface

Monitoring Replans¶

# View replanning events
aqua-log --filter replan

# Output:
# [12:34:56] REPLAN: Obstacle detected at (15, 3, -5)
# [12:34:57] REPLAN: New path computed (18 waypoints)
# [12:34:58] REPLAN: Transition started
# [12:35:02] REPLAN: Following new path