Path Planning Algorithms for Drone Navigation
Finding the optimal path from point A to point B while avoiding obstacles is one of the most fundamental problems in drone autonomy. Think of it like planning a road trip โ you need the fastest route that avoids construction, tolls, and traffic jams, but for a drone navigating through 3D airspace.
The Path Planning Problem
Sponsored
Path planning asks a deceptively simple question: "How do I get there safely and efficiently?" The challenge explodes in complexity when you add obstacles, wind, dynamic environments, and the drone's own physical constraints.
Key Definitions
- Configuration Space (C-Space): The set of all possible positions and orientations the drone can occupy
- Free Space: The subset of C-Space without collisions
- Cost Function: A mathematical measure of path quality (distance, energy, time)
- Kinodynamic Planning: Planning that respects the drone's dynamics (velocity, acceleration limits)
Graph-Based Search: A* Algorithm
A* is the gold standard for grid-based path planning. It combines the best of Dijkstra's algorithm (guaranteed shortest path) with greedy best-first search (directional guidance).
How A* Works
The algorithm maintains two lists:
- Open Set: Nodes to be evaluated
- Closed Set: Nodes already evaluated
For each node, A* calculates:
Architecture Diagram
f(n) = g(n) + h(n)
where g(n) is the cost from start, and h(n) is the heuristic estimate to goal.
A* Grid Search Visualization
A* Implementation
python
from typing import List, Tuple, Optional
class Node:
def __init__(self, position: Tuple[int, int], parent=None):
self.position = position
self.parent = parent
self.g = float('inf') # Cost from start
self.h = 0.0 # Heuristic to goal
self.f = float('inf') # Total cost
def __lt__(self, other):
return self.f < other.f
def heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float:
"""Octile distance heuristic for 8-connected grid."""
dx = abs(a[0] - b[0])
dy = abs(a[1] - b[1])
return max(dx, dy) + (1.414 - 1) * min(dx, dy)
def astar(grid: np.ndarray, start: Tuple[int, int],
goal: Tuple[int, int]) -> Optional[List[Tuple[int, int]]]:
rows, cols = grid.shape
open_set = []
closed_set = set()
start_node = Node(start)
start_node.g = 0
start_node.h = heuristic(start, goal)
start_node.f = start_node.h
heapq.heappush(open_set, start_node)
nodes = {start: start_node}
directions = [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]
while open_set:
current = heapq.heappop(open_set)
if current.position == goal:
path = []
while current:
path.append(current.position)
current = current.parent
return path[::-1]
closed_set.add(current.position)
for dx, dy in directions:
nx, ny = current.position[0] + dx, current.position[1] + dy
neighbor_pos = (nx, ny)
if (0 <= nx < rows and 0 <= ny < cols and
grid[nx, ny] == 0 and neighbor_pos not in closed_set):
move_cost = 1.414 if dx != 0 and dy != 0 else 1.0
tentative_g = current.g + move_cost
if neighbor_pos not in nodes:
nodes[neighbor_pos] = Node(neighbor_pos)
neighbor = nodes[neighbor_pos]
if tentative_g < neighbor.g:
neighbor.parent = current
neighbor.g = tentative_g
neighbor.h = heuristic(neighbor_pos, goal)
neighbor.f = neighbor.g + neighbor.h
heapq.heappush(open_set, neighbor)
return None # No path found
Sampling-Based Methods: RRT and RRT*
When the configuration space is high-dimensional (6+ DOF for a drone), grid-based methods become computationally expensive. Rapidly-exploring Random Trees (RRT) solve this by randomly sampling the space.
RRT Tree Growth Visualization
RRT Implementation
python
from typing import List, Tuple, Optional
class RRTNode:
def __init__(self, position: np.ndarray, parent=None):
self.position = position
self.parent = parent
class RRT:
def __init__(self, start: np.ndarray, goal: np.ndarray,
bounds: np.ndarray, obstacles: list,
step_size: float = 1.0, max_iter: int = 1000):
self.start = RRTNode(start)
self.goal = goal
self.bounds = bounds
self.obstacles = obstacles
self.step_size = step_size
self.max_iter = max_iter
self.tree = [self.start]
def sample_free(self) -> np.ndarray:
"""Sample a random point in free space."""
while True:
point = np.random.uniform(self.bounds[:, 0], self.bounds[:, 1])
if not self.in_obstacle(point):
return point
def nearest(self, point: np.ndarray) -> RRTNode:
"""Find nearest node in tree to given point."""
distances = [np.linalg.norm(node.position - point) for node in self.tree]
return self.tree[np.argmin(distances)]
def steer(self, from_pos: np.ndarray, to_pos: np.ndarray) -> np.ndarray:
"""Steer from one point toward another by step_size."""
direction = to_pos - from_pos
dist = np.linalg.norm(direction)
if dist > self.step_size:
direction = direction / dist * self.step_size
return from_pos + direction
def in_obstacle(self, point: np.ndarray) -> bool:
for obs in self.obstacles:
if np.linalg.norm(point - obs['center']) < obs['radius']:
return True
return False
def collision_free(self, p1: np.ndarray, p2: np.ndarray) -> bool:
steps = int(np.linalg.norm(p2 - p1) / 0.1) + 1
for i in range(steps + 1):
t = i / steps
point = p1 + t * (p2 - p1)
if self.in_obstacle(point):
return False
return True
def plan(self) -> Optional[List[np.ndarray]]:
for _ in range(self.max_iter):
q_rand = self.sample_free()
q_nearest = self.nearest(q_rand)
q_new_pos = self.steer(q_nearest.position, q_rand)
if self.collision_free(q_nearest.position, q_new_pos):
q_new = RRTNode(q_new_pos, parent=q_nearest)
self.tree.append(q_new)
if np.linalg.norm(q_new_pos - self.goal) < self.step_size:
path = []
node = q_new
while node:
path.append(node.position)
node = node.parent
return path[::-1]
return None # No path found within max iterations
Probabilistic Roadmap (PRM)
Sponsored
PRM builds a roadmap of the configuration space by randomly sampling collision-free configurations and connecting nearby ones. It excels in multi-query scenarios where the same environment is used for many planning tasks.
PRM Algorithm Steps
- Construction Phase: Sample nodes and connect nearby ones
- Query Phase: Connect start/goal to roadmap, find shortest path
python
class PRM:
def __init__(self, n_samples: int = 500, k_neighbors: int = 10):
self.n_samples = n_samples
self.k_neighbors = k_neighbors
self.graph = {}
def build_roadmap(self, bounds, obstacles):
# Sample collision-free nodes
nodes = []
for _ in range(self.n_samples):
point = np.random.uniform(bounds[:, 0], bounds[:, 1])
if not self.in_obstacle(point, obstacles):
nodes.append(point)
# Connect k-nearest neighbors
for i, node in enumerate(nodes):
distances = [(np.linalg.norm(node - other), j)
for j, other in enumerate(nodes) if i != j]
distances.sort()
self.graph[i] = []
for dist, j in distances[:self.k_neighbors]:
if self.edge_collision_free(node, nodes[j], obstacles):
self.graph[i].append((j, dist))
if j not in self.graph:
self.graph[j] = []
self.graph[j].append((i, dist))
return nodes
Artificial Potential Fields
Potential fields treat the drone as a particle in an artificial force field. The goal attracts (attractive potential) while obstacles repel (repulsive potential). The drone follows the resultant force gradient.
Potential Field Force Diagram
Potential Fields Implementation
python
from typing import List
class PotentialFieldPlanner:
def __init__(self, goal: np.ndarray, obstacles: list,
k_att: float = 1.0, k_rep: float = 100.0,
d0: float = 5.0):
self.goal = goal
self.obstacles = obstacles
self.k_att = k_att # Attractive gain
self.k_rep = k_rep # Repulsive gain
self.d0 = d0 # Influence distance
def attractive_force(self, position: np.ndarray) -> np.ndarray:
return self.k_att * (self.goal - position)
def repulsive_force(self, position: np.ndarray) -> np.ndarray:
force = np.zeros_like(position)
for obs in self.obstacles:
diff = position - obs['center']
dist = np.linalg.norm(diff)
if dist < self.d0 and dist > 0.01:
magnitude = self.k_rep * (1/dist - 1/self.d0) * (1/dist**2)
force += magnitude * (diff / dist)
return force
def compute_force(self, position: np.ndarray) -> np.ndarray:
f_att = self.attractive_force(position)
f_rep = self.repulsive_force(position)
return f_att + f_rep
def plan(self, start: np.ndarray, dt: float = 0.1,
steps: int = 1000) -> List[np.ndarray]:
path = [start.copy()]
position = start.copy()
for _ in range(steps):
force = self.compute_force(position)
position = position + force * dt
if np.linalg.norm(position - self.goal) < 0.5:
path.append(self.goal.copy())
break
path.append(position.copy())
return path
Trajectory Optimization
Trajectory optimization refines a initial path into a smooth, dynamically feasible trajectory. It considers the drone's full dynamics model.
Cost Function Components
The optimization typically minimizes a weighted sum of:
- Path length: Total distance traveled
- Smoothness: Jerk (derivative of acceleration)
- Obstacle clearance: Minimum distance to obstacles
- Dynamic feasibility: Actuator limits, velocity constraints
python
from scipy.optimize import minimize
def trajectory_cost(waypoints, n_waypoints, obstacles, goal):
cost = 0.0
# Smoothness cost (jerk penalty)
for i in range(1, len(waypoints) - 1):
jerk = waypoints[i+1] - 3*waypoints[i] + 3*waypoints[i-1] if i < len(waypoints)-1 else 0
cost += np.sum(jerk**2)
# Obstacle avoidance cost
for wp in waypoints:
for obs in obstacles:
dist = np.linalg.norm(wp - obs['center']) - obs['radius']
if dist < obs['safety_margin']:
cost += 1000 * (obs['safety_margin'] - dist)**2
# Goal proximity cost
cost += 5.0 * np.linalg.norm(waypoints[-1] - goal)
return cost
def optimize_trajectory(initial_path, obstacles, goal):
n_wp = len(initial_path)
x0 = np.array(initial_path).flatten()
result = minimize(
trajectory_cost,
x0,
args=(n_wp, obstacles, goal),
method='L-BFGS-B'
)
return result.x.reshape(-1, 2)
RRT* โ Asymptotically Optimal
RRT* extends RRT with rewiring operations that improve path quality over time. As the number of iterations approaches infinity, RRT* converges to the optimal solution.
Key additions to RRT:
- Nearby node discovery: Find all nodes within a changing radius
- Rewiring: Reconnect nodes through lower-cost parents
- Cost tracking: Maintain
cost_to_comefor each node
python
class RRTStar(RRT):
def __init__(self, *args, gamma: float = 2.0, **kwargs):
super().__init__(*args, **kwargs)
self.gamma = gamma
self.costs = {id(self.start): 0.0}
def near(self, point: np.ndarray) -> list:
n = len(self.tree)
radius = min(self.gamma * (np.log(n) / n) ** 0.5, self.step_size * 3)
return [node for node in self.tree
if np.linalg.norm(node.position - point) < radius]
def choose_parent(self, new_node, nearby_nodes):
best_parent = None
best_cost = float('inf')
for node in nearby_nodes:
cost = self.costs[id(node)] + np.linalg.norm(node.position - new_node.position)
if cost < best_cost and self.collision_free(node.position, new_node.position):
best_parent = node
best_cost = cost
if best_parent:
new_node.parent = best_parent
self.costs[id(new_node)] = best_cost
return new_node
def rewire(self, new_node, nearby_nodes):
for node in nearby_nodes:
new_cost = self.costs[id(new_node)] + np.linalg.norm(new_node.position - node.position)
if new_cost < self.costs.get(id(node), float('inf')):
if self.collision_free(new_node.position, node.position):
node.parent = new_node
self.costs[id(node)] = new_cost
Algorithm Comparison
Advertisement
| Algorithm | Optimality | Completeness | Time Complexity | Best For |
|---|---|---|---|---|
| A* | Optimal | Complete | O(b^d) | Grid worlds, 2D |
| RRT | Not optimal | Probabilistic | O(n log n) | High-dimensional |
| RRT* | Asymptotically optimal | Probabilistic | O(n log n) | When path quality matters |
| PRM | Near-optimal | Probabilistic | Precompute O(n^2) | Multi-query environments |
| Potential Fields | Local minimum | Complete (simple) | O(nยทk) | Real-time reactive |
| Trajectory Opt | Optimal | Depends on init | O(n^3) | Smooth flight paths |
Hands-On Project: 3D Path Planner
Build a complete 3D path planner for a quadrotor drone:
python
from mpl_toolkits.mplot3d import Axes3D
class Drone3DPathPlanner:
def __init__(self, bounds_3d, obstacles_3d):
self.bounds = bounds_3d # [[x_min,x_max],[y_min,y_max],[z_min,z_max]]
self.obstacles = obstacles_3d
self.drone_radius = 0.5
self.altitude_min = 2.0
self.altitude_max = 50.0
def is_valid_position(self, pos):
# Check bounds
for i in range(3):
if pos[i] < self.bounds[i][0] or pos[i] > self.bounds[i][1]:
return False
# Check altitude constraints
if pos[2] < self.altitude_min or pos[2] > self.altitude_max:
return False
# Check obstacle clearance
for obs in self.obstacles:
dist = np.linalg.norm(pos - obs['center'])
if dist < obs['radius'] + self.drone_radius:
return False
return True
def plan_with_rrt_star(self, start, goal, max_iter=5000):
tree = [{'pos': start, 'parent': None, 'cost': 0}]
goal_threshold = 2.0
for iteration in range(max_iter):
# Sample with bias toward goal
if np.random.random() < 0.1:
q_rand = goal
else:
q_rand = np.array([
np.random.uniform(self.bounds[i][0], self.bounds[i][1])
for i in range(3)
])
# Find nearest
distances = [np.linalg.norm(n['pos'] - q_rand) for n in tree]
nearest_idx = np.argmin(distances)
q_nearest = tree[nearest_idx]
# Steer
direction = q_rand - q_nearest['pos']
dist = np.linalg.norm(direction)
step = min(dist, 2.0)
q_new = q_nearest['pos'] + (direction / dist) * step
if self.is_valid_position(q_new):
new_cost = q_nearest['cost'] + step
# Find nearby nodes for rewiring
nearby = [i for i, n in enumerate(tree)
if np.linalg.norm(n['pos'] - q_new) < 5.0]
# Choose best parent
best_parent = nearest_idx
best_cost = new_cost
for i in nearby:
potential_cost = tree[i]['cost'] + np.linalg.norm(tree[i]['pos'] - q_new)
if potential_cost < best_cost:
best_parent = i
best_cost = potential_cost
tree.append({
'pos': q_new,
'parent': best_parent,
'cost': best_cost
})
# Check if reached goal
if np.linalg.norm(q_new - goal) < goal_threshold:
return self._extract_path(tree, len(tree) - 1)
return None
def _extract_path(self, tree, goal_idx):
path = []
idx = goal_idx
while idx is not None:
path.append(tree[idx]['pos'])
idx = tree[idx]['parent']
return path[::-1]
def visualize_3d_path(self, path):
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')
# Plot obstacles
for obs in self.obstacles:
u = np.linspace(0, 2*np.pi, 20)
v = np.linspace(0, np.pi, 20)
x = obs['center'][0] + obs['radius'] * np.outer(np.cos(u), np.sin(v))
y = obs['center'][1] + obs['radius'] * np.outer(np.sin(u), np.sin(v))
z = obs['center'][2] + obs['radius'] * np.outer(np.ones(np.size(u)), np.cos(v))
ax.plot_surface(x, y, z, alpha=0.3, color='gray')
# Plot path
path = np.array(path)
ax.plot(path[:,0], path[:,1], path[:,2], 'o-', color='#10b981',
linewidth=2, markersize=4, label='Flight Path')
# Start and goal
ax.scatter(*path[0], color='#0ea5e9', s=100, label='Start')
ax.scatter(*path[-1], color='#f59e0b', s=100, marker='*', label='Goal')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Altitude (m)')
ax.legend()
plt.tight_layout()
plt.show()
# Usage example
bounds = [[0, 100], [0, 100], [2, 50]]
obstacles = [
{'center': np.array([30, 40, 15]), 'radius': 5},
{'center': np.array([60, 30, 20]), 'radius': 8},
{'center': np.array([50, 70, 10]), 'radius': 6},
]
planner = Drone3DPathPlanner(bounds, obstacles)
path = planner.plan_with_rrt_star(
start=np.array([5, 5, 10]),
goal=np.array([95, 95, 25])
)
Key Takeaways
- A* excels in grid environments with guaranteed optimality
- RRT/RRT* scale to high dimensions and handle complex constraints
- PRM is ideal when replanning frequently in the same environment
- Potential fields provide real-time reactive control
- Trajectory optimization produces smooth, dynamically feasible paths
- Always combine global planning with local reactive avoidance for safety