Autonomous Navigation and Mission Planning
Autonomous navigation transforms a drone from a remote-controlled device into an intelligent agent that plans, executes, and adapts missions independently. Think of it as giving the drone a brain that knows where to go, how to get there, and what to do when things change.
Navigation State Machine
Sponsored
A drone's navigation system operates as a state machine, transitioning between distinct modes based on conditions and events.
Navigation State Machine Diagram
PID Controllers for Flight
PID (Proportional-Integral-Derivative) controllers are the backbone of drone stabilization. Each axis (roll, pitch, yaw, altitude) typically has its own PID loop.
PID Controller Concept
PID Controller Implementation
python
from typing import Optional
class PIDController:
def __init__(self, kp: float, ki: float, kd: float,
output_limits: tuple = (-1.0, 1.0),
integral_limits: tuple = (-5.0, 5.0),
derivative_filter: float = 0.1):
self.kp = kp
self.ki = ki
self.kd = kd
self.output_limits = output_limits
self.integral_limits = integral_limits
self.derivative_filter = derivative_filter
self.integral = 0.0
self.prev_error = 0.0
self.prev_derivative = 0.0
self.prev_time = None
def update(self, setpoint: float, measurement: float,
dt: Optional[float] = None) -> float:
current_time = time.time()
if dt is None:
if self.prev_time is None:
dt = 0.01
else:
dt = current_time - self.prev_time
self.prev_time = current_time
error = setpoint - measurement
# Proportional term
p_term = self.kp * error
# Integral term with anti-windup
self.integral += error * dt
self.integral = np.clip(self.integral, *self.integral_limits)
i_term = self.ki * self.integral
# Derivative term with filtering
if dt > 0:
derivative = (error - self.prev_error) / dt
filtered_derivative = (self.derivative_filter * derivative +
(1 - self.derivative_filter) * self.prev_derivative)
self.prev_derivative = filtered_derivative
d_term = self.kd * filtered_derivative
else:
d_term = 0.0
self.prev_error = error
# Total output
output = p_term + i_term + d_term
return np.clip(output, *self.output_limits)
def reset(self):
self.integral = 0.0
self.prev_error = 0.0
self.prev_derivative = 0.0
self.prev_time = None
class CascadedPID:
"""Cascaded PID for position and velocity control."""
def __init__(self):
# Outer loop: position -> velocity setpoint
self.position_pid = PIDController(kp=1.0, ki=0.1, kd=0.5)
# Inner loop: velocity -> acceleration command
self.velocity_pid = PIDController(kp=0.5, ki=0.2, kd=0.1)
def compute(self, target_pos, current_pos, current_vel, dt):
# Outer loop
velocity_setpoint = self.position_pid.update(target_pos, current_pos, dt)
# Inner loop
acceleration = self.velocity_pid.update(velocity_setpoint, current_vel, dt)
return acceleration
Model Predictive Control (MPC)
MPC optimizes control actions over a prediction horizon, handling constraints naturally. It predicts the drone's future state and optimizes inputs accordingly.
MPC Prediction Horizon
MPC Implementation
python
from scipy.optimize import minimize
class MPCController:
def __init__(self, dt=0.1, horizon=20, n_states=4, n_inputs=2):
self.dt = dt
self.horizon = horizon
self.n_states = n_states
self.n_inputs = n_inputs
# Cost weights
self.Q = np.diag([10.0, 10.0, 1.0, 1.0]) # State tracking
self.R = np.diag([0.1, 0.1]) # Control effort
self.Rd = np.diag([0.01, 0.01]) # Control rate
# Constraints
self.u_min = np.array([-2.0, -1.0])
self.u_max = np.array([2.0, 1.0])
self.state_min = np.array([-100, -100, -5, -np.pi])
self.state_max = np.array([100, 100, 5, np.pi])
def dynamics(self, state, control):
"""Simple double integrator dynamics."""
x, y, vx, vy = state
ax, ay = control
new_x = x + vx * self.dt + 0.5 * ax * self.dt**2
new_y = y + vy * self.dt + 0.5 * ay * self.dt**2
new_vx = vx + ax * self.dt
new_vy = vy + ay * self.dt
return np.array([new_x, new_y, new_vx, new_vy])
def cost_function(self, u_sequence, initial_state, reference_trajectory):
"""Compute total cost over prediction horizon."""
cost = 0.0
state = initial_state.copy()
for k in range(self.horizon):
# Get control input
u = u_sequence[k * self.n_inputs:(k + 1) * self.n_inputs]
# State tracking cost
error = state - reference_trajectory[k]
cost += error.T @ self.Q @ error
# Control effort cost
cost += u.T @ self.R @ u
# Control rate cost
if k > 0:
u_prev = u_sequence[(k-1) * self.n_inputs:k * self.n_inputs]
du = u - u_prev
cost += du.T @ self.Rd @ du
# Predict next state
state = self.dynamics(state, u)
return cost
def compute_control(self, current_state, reference_trajectory):
"""Solve MPC optimization problem."""
u0 = np.zeros(self.horizon * self.n_inputs)
bounds = []
for _ in range(self.horizon):
for i in range(self.n_inputs):
bounds.append((self.u_min[i], self.u_max[i]))
result = minimize(
self.cost_function,
u0,
args=(current_state, reference_trajectory),
method='SLSQP',
bounds=bounds,
options={'maxiter': 100}
)
return result.x[:self.n_inputs]
Waypoint Following
Advertisement
Waypoint following guides the drone through a sequence of predefined positions. Common strategies include:
Waypoint Navigation Strategies
Waypoint Navigator Implementation
python
from enum import Enum
from typing import List, Tuple
class WaypointMode(Enum):
STOP_AT = 1
FLY_THROUGH = 2
ORBIT = 3
class WaypointNavigator:
def __init__(self, waypoints: List[np.ndarray], mode: WaypointMode = WaypointMode.FLY_THROUGH):
self.waypoints = waypoints
self.mode = mode
self.current_idx = 0
self.position_threshold = 1.0 # meters
self.orbit_radius = 5.0
self.orbit_speed = 0.5 # rad/s
def get_target(self, current_pos: np.ndarray, current_yaw: float) -> Tuple[np.ndarray, float]:
if self.current_idx >= len(self.waypoints):
return self.waypoints[-1], 0.0
target = self.waypoints[self.current_idx]
if self.mode == WaypointMode.STOP_AT:
return self._stop_at_strategy(current_pos, target)
elif self.mode == WaypointMode.FLY_THROUGH:
return self._fly_through_strategy(current_pos, target)
elif self.mode == WaypointMode.ORBIT:
return self._orbit_strategy(current_pos, target, current_yaw)
def _stop_at_strategy(self, current_pos, target):
distance = np.linalg.norm(target - current_pos)
if distance < self.position_threshold:
self.current_idx += 1
if self.current_idx >= len(self.waypoints):
return current_pos, 0.0
target = self.waypoints[self.current_idx]
direction = np.arctan2(target[1] - current_pos[1], target[0] - current_pos[0])
return target, direction
def _fly_through_strategy(self, current_pos, target):
if self.current_idx >= len(self.waypoints) - 1:
return target, 0.0
next_wp = self.waypoints[self.current_idx + 1]
direction = np.arctan2(next_wp[1] - target[1], next_wp[0] - target[0])
distance = np.linalg.norm(target - current_pos)
if distance < self.position_threshold:
self.current_idx += 1
return target, direction
def _orbit_strategy(self, current_pos, center, yaw):
angle = yaw + self.orbit_speed * 0.01 # Small increment
orbit_x = center[0] + self.orbit_radius * np.cos(angle)
orbit_y = center[1] + self.orbit_radius * np.sin(angle)
target = np.array([orbit_x, orbit_y, center[2]])
tangent = angle + np.pi / 2
return target, tangent
Mission Planning
Mission planning defines the high-level tasks and sequences for drone operations. It includes:
- Task sequencing: Define order of operations
- Resource management: Battery, payload, time
- Contingency planning: What-if scenarios
python
from dataclasses import dataclass
from typing import List, Optional
from enum import Enum
class TaskType(Enum):
TAKEOFF = "takeoff"
NAVIGATE = "navigate"
HOVER = "hover"
ORBIT = "orbit"
PHOTO = "photo"
SCAN = "scan"
LAND = "land"
@dataclass
class MissionTask:
task_type: TaskType
parameters: dict
duration: Optional[float] = None
dependencies: List[int] = None
class MissionPlanner:
def __init__(self):
self.tasks: List[MissionTask] = []
self.battery_capacity = 100.0 # percent
self.battery_per_minute = 2.0 # percent
self.max_flight_time = 40.0 # minutes
def add_task(self, task: MissionTask):
self.tasks.append(task)
def validate_mission(self) -> dict:
issues = []
# Check battery
total_time = sum(t.duration or 0 for t in self.tasks)
if total_time > self.max_flight_time:
issues.append(f"Mission too long: {total_time}min > {self.max_flight_time}min")
# Check dependencies
for i, task in enumerate(self.tasks):
if task.dependencies:
for dep in task.dependencies:
if dep >= i:
issues.append(f"Task {i} depends on future task {dep}")
# Check takeoff/land sequence
task_types = [t.task_type for t in self.tasks]
if TaskType.TAKEOFF in task_types:
takeoff_idx = task_types.index(TaskType.TAKEOFF)
if takeoff_idx != 0:
issues.append("Takeoff must be first task")
return {'valid': len(issues) == 0, 'issues': issues}
def estimate_battery(self) -> float:
total_time = sum(t.duration or 0 for t in self.tasks)
return total_time * self.battery_per_minute
def generate_flight_plan(self) -> dict:
return {
'tasks': [t.__dict__ for t in self.tasks],
'estimated_battery': self.estimate_battery(),
'estimated_time': sum(t.duration or 0 for t in self.tasks),
'validation': self.validate_mission()
}
Hands-On Project: Complete Navigation System
python
class AutonomousDrone:
def __init__(self):
self.position = np.zeros(3)
self.velocity = np.zeros(3)
self.orientation = np.zeros(3) # roll, pitch, yaw
# Controllers
self.position_pid = CascadedPID()
self.altitude_pid = PIDController(kp=1.5, ki=0.3, kd=0.5)
# Navigation
self.navigator = None
self.obstacle_avoidance = MultiSensorAvoidance()
# State
self.state = 'IDLE'
self.battery = 100.0
self.mission = None
def arm(self):
if self.state == 'IDLE':
self.state = 'ARMING'
print("Performing pre-flight checks...")
time.sleep(2)
self.state = 'READY'
return True
return False
def takeoff(self, target_altitude=5.0):
if self.state == 'READY':
self.state = 'TAKEOFF'
print(f"Taking off to {target_altitude}m...")
while abs(self.position[2] - target_altitude) > 0.1:
control = self.altitude_pid.update(target_altitude, self.position[2])
self._apply_throttle(control)
self._update_simulation()
self.state = 'HOVER'
return True
return False
def start_mission(self, mission):
if self.state in ['HOVER', 'READY']:
self.mission = mission
self.state = 'NAVIGATE'
print("Mission started!")
return True
return False
def update(self, dt=0.01):
if self.state == 'NAVIGATE':
# Get next waypoint
target, heading = self.navigator.get_target(self.position, self.orientation[2])
# Check for obstacles
obstacles = self._sense_obstacles()
if obstacles:
self.state = 'AVOID'
return
# Compute control
control = self.position_pid.compute(target, self.position, self.velocity, dt)
self._apply_control(control)
elif self.state == 'AVOID':
# Run obstacle avoidance
avoidance = self.obstacle_avoidance.run_avoidance_loop(
self._get_lidar(), self._get_depth(), self.navigator.current_heading
)
self._apply_avoidance(avoidance)
# Check if clear
if avoidance['min_distance'] > 3.0:
self.state = 'NAVIGATE'
elif self.state == 'RTL':
home = np.array([0, 0, 5.0])
dist = np.linalg.norm(home - self.position)
if dist < 1.0:
self.state = 'LAND'
else:
target, heading = self._compute_rtl_path(home)
control = self.position_pid.compute(target, self.position, self.velocity, dt)
self._apply_control(control)
elif self.state == 'LAND':
self.altitude_pid.update(0, self.position[2])
if self.position[2] < 0.1:
self.state = 'DISARM'
print("Landed safely!")
# Battery check
self.battery -= 0.02 * dt
if self.battery < 20 and self.state not in ['RTL', 'LAND', 'DISARM']:
print("Low battery! Returning to launch...")
self.state = 'RTL'
self._update_simulation()
def _apply_control(self, control):
self.velocity[:2] = control[:2]
self.orientation[2] = control[2] if len(control) > 2 else 0
def _apply_throttle(self, throttle):
self.velocity[2] = throttle
def _update_simulation(self):
self.position += self.velocity * 0.01
Key Takeaways
- State machines provide robust navigation mode management
- PID controllers are simple but effective for stabilization
- MPC handles constraints and optimizes over prediction horizons
- Waypoint following strategies depend on mission requirements
- Mission planning ensures safe and efficient operations
- Always implement battery monitoring and RTL failsafe