Path Planning Algorithms: A* and RRT
Path planning is one of the most critical aspects of robotics. It enables robots to navigate from one position to another while avoiding obstacles. Today, we’ll explore two fundamental path planning algorithms: A* (A-star) search and Rapidly-exploring Random Trees (RRT). These algorithms form the foundation of modern robot navigation systems.
Introduction to Path Planning
Path planning involves finding a sequence of configurations that moves a robot from a start configuration to a goal configuration while avoiding obstacles. The key challenges include:
- High-dimensional spaces: Robots often have many degrees of freedom
- Complex obstacles: Real environments contain complex obstacle shapes
- Dynamic environments: Obstacles and goals may change over time
- Optimization: Finding not just any path, but an optimal one
A* Search Algorithm
A* is a popular pathfinding algorithm that combines the strengths of Uniform Cost Search and Greedy Best-First Search. It uses a heuristic function to guide the search toward the goal.
A* Fundamentals
A* evaluates nodes using the function: f(n) = g(n) + h(n)
Where:
- g(n) = cost from start to node n
- h(n) = heuristic estimate from node n to goal
- f(n) = estimated total cost through node n
Implementation of A* for Robot Path Planning
import numpy as np
import heapq
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Circle
import math
class Node:
"""A node in the A* search tree"""
def __init__(self, position, parent=None, g_cost=0, h_cost=0):
self.position = position
self.parent = parent
self.g_cost = g_cost # Cost from start
self.h_cost = h_cost # Heuristic cost to goal
self.f_cost = g_cost + h_cost
def __lt__(self, other):
return self.f_cost < other.f_cost
class AStarPlanner:
def __init__(self, grid_size, obstacles, robot_size=0.2):
"""
Initialize A* path planner
Args:
grid_size: (width, height) of the grid
obstacles: List of obstacle positions and sizes
robot_size: Size of the robot (for collision checking)
"""
self.grid_size = grid_size
self.obstacles = obstacles
self.robot_size = robot_size
self.grid_resolution = 0.1 # meters per grid cell
# Create grid
self.grid_width = int(grid_size[0] / self.grid_resolution)
self.grid_height = int(grid_size[1] / self.grid_resolution)
self.grid = np.zeros((self.grid_height, self.grid_width))
# Mark obstacles in grid
self._mark_obstacles()
def _mark_obstacles(self):
"""Mark obstacle cells in the grid"""
for obs in self.obstacles:
obs_x, obs_y, obs_size = obs
# Convert to grid coordinates
grid_x = int(obs_x / self.grid_resolution)
grid_y = int(obs_y / self.grid_resolution)
grid_size = int(obs_size / self.grid_resolution)
# Mark obstacle cells
for dx in range(-grid_size, grid_size + 1):
for dy in range(-grid_size, grid_size + 1):
x, y = grid_x + dx, grid_y + dy
if (0 <= x < self.grid_width and 0 <= y < self.grid_height):
if dx*dx + dy*dy <= grid_size*grid_size:
self.grid[y, x] = 1
def heuristic(self, pos1, pos2):
"""Euclidean distance heuristic"""
return np.sqrt((pos1[0] - pos2[0])**2 + (pos1[1] - pos2[1])**2)
def get_neighbors(self, node):
"""Get valid neighboring nodes"""
neighbors = []
x, y = node.position
# 8-connected grid
directions = [(-1, -1), (-1, 0), (-1, 1), (0, -1),
(0, 1), (1, -1), (1, 0), (1, 1)]
for dx, dy in directions:
new_x = x + dx * self.grid_resolution
new_y = y + dy * self.grid_resolution
# Check bounds
if (0 <= new_x < self.grid_size[0] and 0 <= new_y < self.grid_size[1]):
# Check collision
if not self._is_collision(new_x, new_y):
neighbors.append((new_x, new_y))
return neighbors
def _is_collision(self, x, y):
"""Check if position collides with obstacles"""
# Check grid
grid_x = int(x / self.grid_resolution)
grid_y = int(y / self.grid_resolution)
if (0 <= grid_x < self.grid_width and 0 <= grid_y < self.grid_height):
return self.grid[grid_y, grid_x] == 1
return True # Out of bounds
def plan_path(self, start, goal):
"""
Plan path from start to goal using A*
Args:
start: (x, y) start position
goal: (x, y) goal position
Returns:
List of waypoints if path found, None otherwise
"""
# Initialize open and closed lists
open_list = []
closed_set = set()
# Create start node
start_node = Node(start, None, 0, self.heuristic(start, goal))
heapq.heappush(open_list, start_node)
# Store all nodes for path reconstruction
all_nodes = {start: start_node}
while open_list:
current = heapq.heappop(open_list)
# Check if we reached the goal
if self.heuristic(current.position, goal) < self.grid_resolution:
# Reconstruct path
path = []
while current:
path.append(current.position)
current = current.parent
return path[::-1] # Reverse to get start->goal
closed_set.add(current.position)
# Explore neighbors
for neighbor_pos in self.get_neighbors(current):
if neighbor_pos in closed_set:
continue
# Calculate costs
g_cost = current.g_cost + self.heuristic(current.position, neighbor_pos)
h_cost = self.heuristic(neighbor_pos, goal)
# Check if neighbor already in open list
if neighbor_pos in all_nodes:
neighbor_node = all_nodes[neighbor_pos]
if g_cost < neighbor_node.g_cost:
neighbor_node.g_cost = g_cost
neighbor_node.f_cost = g_cost + h_cost
neighbor_node.parent = current
else:
neighbor_node = Node(neighbor_pos, current, g_cost, h_cost)
all_nodes[neighbor_pos] = neighbor_node
heapq.heappush(open_list, neighbor_node)
return None # No path found
def visualize_path(self, start, goal, path):
"""Visualize the planned path"""
fig, ax = plt.subplots(1, 1, figsize=(12, 10))
# Draw grid
ax.imshow(self.grid, cmap='gray', origin='lower',
extent=[0, self.grid_size[0], 0, self.grid_size[1]])
# Draw obstacles
for obs in self.obstacles:
obs_x, obs_y, obs_size = obs
obstacle = Circle((obs_x, obs_y), obs_size, color='red', alpha=0.7)
ax.add_patch(obstacle)
# Draw path
if path:
path_array = np.array(path)
ax.plot(path_array[:, 0], path_array[:, 1], 'b-', linewidth=3, label='Planned Path')
ax.plot(path_array[:, 0], path_array[:, 1], 'bo', markersize=4)
# Draw start and goal
ax.plot(start[0], start[1], 'go', markersize=15, label='Start')
ax.plot(goal[0], goal[1], 'ro', markersize=15, label='Goal')
ax.set_xlim(0, self.grid_size[0])
ax.set_ylim(0, self.grid_size[1])
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.legend()
ax.set_xlabel('X Position (m)')
ax.set_ylabel('Y Position (m)')
ax.set_title('A* Path Planning')
plt.tight_layout()
plt.show()
# Example A* usage
grid_size = (10, 8)
obstacles = [
(3, 3, 0.5), (3, 4, 0.5), (3, 5, 0.5), # Vertical wall
(6, 2, 0.3), (7, 2, 0.3), (8, 2, 0.3), # Horizontal obstacles
(5, 6, 0.4) # Single obstacle
]
planner = AStarPlanner(grid_size, obstacles)
start = (1, 1)
goal = (9, 7)
path = planner.plan_path(start, goal)
if path:
print(f"Path found with {len(path)} waypoints")
print("Path waypoints:", path)
planner.visualize_path(start, goal, path)
else:
print("No path found")
Rapidly-exploring Random Trees (RRT)
RRT is a sampling-based algorithm that builds a tree of robot configurations incrementally. It’s particularly effective in high-dimensional spaces.
RRT Fundamentals
RRT works by:
- Starting from an initial configuration
- Randomly sampling configurations in the space
- Extending the tree toward the sampled configurations
- Checking for collisions during extension
- Repeating until the goal is reached or a timeout occurs
RRT Implementation
class RRTNode:
"""A node in the RRT"""
def __init__(self, position, parent=None):
self.position = position
self.parent = parent
class RRTPlanner:
def __init__(self, bounds, obstacles, robot_size=0.2, step_size=0.5, max_iterations=1000):
"""
Initialize RRT planner
Args:
bounds: (min_x, max_x, min_y, max_y) workspace bounds
obstacles: List of obstacle positions and sizes
robot_size: Size of the robot
step_size: Step size for tree extension
max_iterations: Maximum number of iterations
"""
self.bounds = bounds
self.obstacles = obstacles
self.robot_size = robot_size
self.step_size = step_size
self.max_iterations = max_iterations
# Tree storage
self.nodes = []
def _is_collision(self, position):
"""Check if position collides with obstacles"""
x, y = position
# Check bounds
if (x < self.bounds[0] or x > self.bounds[1] or
y < self.bounds[2] or y > self.bounds[3]):
return True
# Check obstacles
for obs_x, obs_y, obs_size in self.obstacles:
if np.sqrt((x - obs_x)**2 + (y - obs_y)**2) < (self.robot_size + obs_size):
return True
return False
def _random_sample(self):
"""Generate a random configuration"""
x = np.random.uniform(self.bounds[0], self.bounds[1])
y = np.random.uniform(self.bounds[2], self.bounds[3])
return (x, y)
def _steer(self, from_pos, to_pos):
"""Steer from one position toward another"""
direction = np.array(to_pos) - np.array(from_pos)
distance = np.linalg.norm(direction)
if distance <= self.step_size:
return to_pos
else:
direction = direction / distance
new_pos = np.array(from_pos) + direction * self.step_size
return tuple(new_pos)
def plan_path(self, start, goal):
"""
Plan path using RRT
Args:
start: (x, y) start position
goal: (x, y) goal position
Returns:
List of waypoints if path found, None otherwise
"""
# Initialize tree with start node
start_node = RRTNode(start)
self.nodes = [start_node]
for iteration in range(self.max_iterations):
# Random sampling with goal bias
if np.random.random() < 0.1: # 10% chance to sample goal
random_pos = goal
else:
random_pos = self._random_sample()
# Find nearest node
nearest_node = self._find_nearest(random_pos)
# Steer toward random position
new_pos = self._steer(nearest_node.position, random_pos)
# Check if new position is collision-free
if not self._is_collision(new_pos):
# Create new node
new_node = RRTNode(new_pos, nearest_node)
self.nodes.append(new_node)
# Check if we reached the goal
if np.linalg.norm(np.array(new_pos) - np.array(goal)) < self.step_size:
# Reconstruct path
path = []
current = new_node
while current:
path.append(current.position)
current = current.parent
return path[::-1]
return None # No path found
def _find_nearest(self, position):
"""Find the nearest node to the given position"""
min_distance = float('inf')
nearest_node = None
for node in self.nodes:
distance = np.linalg.norm(np.array(node.position) - np.array(position))
if distance < min_distance:
min_distance = distance
nearest_node = node
return nearest_node
def visualize_tree(self, start, goal, path=None):
"""Visualize the RRT tree and path"""
fig, ax = plt.subplots(1, 1, figsize=(12, 10))
# Draw obstacles
for obs_x, obs_y, obs_size in self.obstacles:
obstacle = Circle((obs_x, obs_y), obs_size, color='red', alpha=0.7)
ax.add_patch(obstacle)
# Draw tree
for node in self.nodes:
if node.parent:
parent_pos = node.parent.position
child_pos = node.position
ax.plot([parent_pos[0], child_pos[0]],
[parent_pos[1], child_pos[1]], 'k-', alpha=0.3, linewidth=1)
# Draw path if found
if path:
path_array = np.array(path)
ax.plot(path_array[:, 0], path_array[:, 1], 'b-', linewidth=3, label='Planned Path')
ax.plot(path_array[:, 0], path_array[:, 1], 'bo', markersize=4)
# Draw start and goal
ax.plot(start[0], start[1], 'go', markersize=15, label='Start')
ax.plot(goal[0], goal[1], 'ro', markersize=15, label='Goal')
ax.set_xlim(self.bounds[0], self.bounds[1])
ax.set_ylim(self.bounds[2], self.bounds[3])
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.legend()
ax.set_xlabel('X Position (m)')
ax.set_ylabel('Y Position (m)')
ax.set_title('RRT Path Planning')
plt.tight_layout()
plt.show()
# Example RRT usage
bounds = (0, 10, 0, 8) # (min_x, max_x, min_y, max_y)
obstacles = [
(3, 3, 0.5), (3, 4, 0.5), (3, 5, 0.5),
(6, 2, 0.3), (7, 2, 0.3), (8, 2, 0.3),
(5, 6, 0.4)
]
rrt_planner = RRTPlanner(bounds, obstacles)
start = (1, 1)
goal = (9, 7)
path = rrt_planner.plan_path(start, goal)
if path:
print(f"Path found with {len(path)} waypoints")
print("Path waypoints:", path)
rrt_planner.visualize_tree(start, goal, path)
else:
print("No path found")
Hybrid A*-RRT Approach
Combining the strengths of both algorithms:
class HybridPlanner:
def __init__(self, grid_size, bounds, obstacles):
self.grid_size = grid_size
self.bounds = bounds
self.obstacles = obstacles
# Initialize both planners
self.astar = AStarPlanner(grid_size, obstacles)
self.rrt = RRTPlanner(bounds, obstacles)
def plan_path(self, start, goal, use_rrt_first=True):
"""
Hybrid path planning using both A* and RRT
Args:
start: (x, y) start position
goal: (x, y) goal position
use_rrt_first: Whether to try RRT first
Returns:
List of waypoints if path found, None otherwise
"""
if use_rrt_first:
# Try RRT first for global planning
print("Trying RRT first...")
rrt_path = self.rrt.plan_path(start, goal)
if rrt_path:
print("RRT path found!")
return rrt_path
# Fall back to A*
print("RRT failed, trying A*...")
astar_path = self.astar.plan_path(start, goal)
if astar_path:
print("A* path found!")
return astar_path
else:
# Try A* first
print("Trying A* first...")
astar_path = self.astar.plan_path(start, goal)
if astar_path:
print("A* path found!")
return astar_path
# Fall back to RRT
print("A* failed, trying RRT...")
rrt_path = self.rrt.plan_path(start, goal)
if rrt_path:
print("RRT path found!")
return rrt_path
return None
# Example hybrid planning
hybrid_planner = HybridPlanner(grid_size, bounds, obstacles)
start = (1, 1)
goal = (9, 7)
path = hybrid_planner.plan_path(start, goal)
if path:
print(f"Hybrid path found with {len(path)} waypoints")
print("Path waypoints:", path)
# Visualize using A* (since we have the grid)
hybrid_planner.astar.visualize_path(start, goal, path)
else:
print("No path found by either algorithm")
Performance Comparison
Let’s compare the performance of both algorithms:
import time
def compare_algorithms(start, goal):
"""Compare A* and RRT performance"""
# Test A*
print("Testing A*...")
start_time = time.time()
astar_path = planner.plan_path(start, goal)
astar_time = time.time() - start_time
# Test RRT
print("Testing RRT...")
start_time = time.time()
rrt_path = rrt_planner.plan_path(start, goal)
rrt_time = time.time() - start_time
# Results
print("\n=== Performance Comparison ===")
print(f"A*: Path found = {astar_path is not None}, Time = {astar_time:.3f}s, "
f"Path length = {len(astar_path) if astar_path else 0}")
print(f"RRT: Path found = {rrt_path is not None}, Time = {rrt_time:.3f}s, "
f"Path length = {len(rrt_path) if rrt_path else 0}")
# Visualize both paths
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(20, 8))
# A* visualization
ax1.imshow(planner.grid, cmap='gray', origin='lower',
extent=[0, grid_size[0], 0, grid_size[1]])
for obs in obstacles:
obs_x, obs_y, obs_size = obs
obstacle = Circle((obs_x, obs_y), obs_size, color='red', alpha=0.7)
ax1.add_patch(obstacle)
if astar_path:
astar_array = np.array(astar_path)
ax1.plot(astar_array[:, 0], astar_array[:, 1], 'b-', linewidth=3, label='A* Path')
ax1.plot(astar_array[:, 0], astar_array[:, 1], 'bo', markersize=4)
ax1.plot(start[0], start[1], 'go', markersize=15, label='Start')
ax1.plot(goal[0], goal[1], 'ro', markersize=15, label='Goal')
ax1.set_title(f'A* Path Planning (Time: {astar_time:.3f}s)')
ax1.legend()
# RRT visualization
for obs in obstacles:
obs_x, obs_y, obs_size = obs
obstacle = Circle((obs_x, obs_y), obs_size, color='red', alpha=0.7)
ax2.add_patch(obstacle)
for node in rrt_planner.nodes:
if node.parent:
parent_pos = node.parent.position
child_pos = node.position
ax2.plot([parent_pos[0], child_pos[0]],
[parent_pos[1], child_pos[1]], 'k-', alpha=0.3, linewidth=1)
if rrt_path:
rrt_array = np.array(rrt_path)
ax2.plot(rrt_array[:, 0], rrt_array[:, 1], 'b-', linewidth=3, label='RRT Path')
ax2.plot(rrt_array[:, 0], rrt_array[:, 1], 'bo', markersize=4)
ax2.plot(start[0], start[1], 'go', markersize=15, label='Start')
ax2.plot(goal[0], goal[1], 'ro', markersize=15, label='Goal')
ax2.set_title(f'RRT Path Planning (Time: {rrt_time:.3f}s)')
ax2.legend()
plt.tight_layout()
plt.show()
# Compare algorithms
compare_algorithms(start, goal)
Advanced Path Planning Techniques
1. RRT* (Optimal RRT)
RRT* improves upon RRT by:
- Rewiring the tree to find better paths
- Using cost-to-come instead of just distance
- Providing asymptotically optimal paths
class RRTStarPlanner(RRTPlanner):
def __init__(self, bounds, obstacles, robot_size=0.2, step_size=0.5,
max_iterations=1000, search_radius=1.0):
super().__init__(bounds, obstacles, robot_size, step_size, max_iterations)
self.search_radius = search_radius
def plan_path(self, start, goal):
"""Plan path using RRT*"""
start_node = RRTNode(start)
self.nodes = [start_node]
for iteration in range(self.max_iterations):
if np.random.random() < 0.1:
random_pos = goal
else:
random_pos = self._random_sample()
nearest_node = self._find_nearest(random_pos)
new_pos = self._steer(nearest_node.position, random_pos)
if not self._is_collision(new_pos):
# Find nearby nodes
nearby_nodes = self._find_nearby_nodes(new_pos)
# Find parent with minimum cost
min_cost = nearest_node.g_cost + self._distance(nearest_node.position, new_pos)
best_parent = nearest_node
for node in nearby_nodes:
cost = node.g_cost + self._distance(node.position, new_pos)
if cost < min_cost:
min_cost = cost
best_parent = node
# Create new node
new_node = RRTNode(new_pos, best_parent)
new_node.g_cost = min_cost
self.nodes.append(new_node)
# Rewire nearby nodes
self._rewire(new_node, nearby_nodes)
# Check goal
if np.linalg.norm(np.array(new_pos) - np.array(goal)) < self.step_size:
path = []
current = new_node
while current:
path.append(current.position)
current = current.parent
return path[::-1]
return None
def _find_nearby_nodes(self, position):
"""Find nodes within search radius"""
nearby = []
for node in self.nodes:
if self._distance(node.position, position) < self.search_radius:
nearby.append(node)
return nearby
def _rewire(self, new_node, nearby_nodes):
"""Rewire nearby nodes to potentially better parents"""
for node in nearby_nodes:
if node != new_node.parent:
new_cost = new_node.g_cost + self._distance(new_node.position, node.position)
if new_cost < node.g_cost:
node.parent = new_node
node.g_cost = new_cost
def _distance(self, pos1, pos2):
"""Calculate distance between two positions"""
return np.linalg.norm(np.array(pos1) - np.array(pos2))
Key Takeaways
- A Search*: Optimal pathfinding using heuristics, good for grid-based environments
- RRT: Sampling-based algorithm, effective in high-dimensional spaces
- Hybrid Approaches: Combine strengths of both algorithms for robust planning
- Performance Trade-offs: A* is optimal but slower in high dimensions, RRT is faster but suboptimal
- Advanced Variants: RRT* provides asymptotically optimal paths
- Applications: Both algorithms are widely used in robotics, drones, autonomous vehicles
Next Steps
Tomorrow, we’ll explore dynamic path planning and real-time obstacle avoidance. We’ll implement algorithms like DWA (Dynamic Window Approach) and TEB (Timed Elastic Band) for robots that need to navigate in dynamic environments.
Practice Exercise: Implement a comprehensive path planning system that can handle different robot types (differential drive, omnidirectional, articulated arms). Add support for dynamic obstacles and real-time replanning. Create a simulation environment to test your implementation.