Implementing robot path planning with the A* algorithm in Python is a foundational skill for anyone building autonomous robots. Whether you are working on a warehouse robot, a self-driving RC car, or a competition maze solver, A* provides an efficient and optimal solution to the question: “What is the shortest path from A to B, avoiding obstacles?” This tutorial walks through the complete implementation, from grid map representation to actual robot deployment.
Table of Contents
- Why Path Planning Matters in Robotics
- A* Algorithm: Theory and Intuition
- Python Implementation of A*
- Grid Maps and Occupancy Grids
- Deploying A* on a Real Robot
- Beyond A*: Advanced Planning Algorithms
- Frequently Asked Questions
Why Path Planning Matters in Robotics
Path planning is the process of finding a collision-free trajectory from a start position to a goal position. Without it, a robot can only react to immediate obstacles (reactive navigation) but cannot plan efficient routes through complex environments. In real-world applications — from AGVs (Automated Guided Vehicles) in Indian factories to household robots — path planning is indispensable.
The core challenge is balancing three competing requirements:
- Completeness: Will the algorithm always find a path if one exists?
- Optimality: Does it find the shortest (or lowest-cost) path?
- Computational efficiency: Does it run fast enough for real-time use?
A* satisfies all three when given an admissible heuristic — making it the go-to algorithm for grid-based robot path planning.
A* Algorithm: Theory and Intuition
A* is an informed search algorithm that combines the benefits of Dijkstra’s algorithm (guaranteed shortest path) with greedy best-first search (fast exploration towards the goal). It assigns each node a cost:
f(n) = g(n) + h(n)Where:
- g(n): The actual cost from the start node to node n (known)
- h(n): A heuristic estimate of the cost from n to the goal (estimated)
- f(n): The total estimated cost of the path through n
A* explores nodes in order of f(n), always expanding the most promising node first. Common heuristics for grid maps:
- Manhattan distance: |dx| + |dy| — for 4-connected grids (no diagonal movement)
- Euclidean distance: sqrt(dx² + dy²) — for 8-connected grids or continuous spaces
- Diagonal distance: max(|dx|, |dy|) — for 8-connected grids with diagonal movement
A heuristic is admissible if it never overestimates the true cost. Admissibility guarantees A* finds the optimal path.
Python Implementation of A*
import heapq
import numpy as np
import matplotlib.pyplot as plt
from typing import List, Tuple, Optional
class AStarPlanner:
def __init__(self, grid: np.ndarray):
"""
grid: 2D numpy array where 0=free, 1=obstacle
"""
self.grid = grid
self.rows, self.cols = grid.shape
def heuristic(self, a: Tuple, b: Tuple) -> float:
"""Euclidean distance heuristic"""
return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def get_neighbours(self, node: Tuple) -> List[Tuple]:
"""Get valid 8-connected neighbours"""
r, c = node
neighbours = []
for dr in [-1, 0, 1]:
for dc in [-1, 0, 1]:
if dr == 0 and dc == 0:
continue
nr, nc = r + dr, c + dc
if (0 <= nr < self.rows and
0 <= nc Optional[List[Tuple]]:
"""Find optimal path from start to goal using A*"""
if self.grid[start] == 1 or self.grid[goal] == 1:
return None # Start or goal is blocked
# Priority queue: (f_cost, g_cost, node)
open_set = [(0, 0, start)]
came_from = {}
g_scores = {start: 0}
while open_set:
f, g, current = heapq.heappop(open_set)
if current == goal:
# Reconstruct path
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(start)
return path[::-1]
for nr, nc, move_cost in self.get_neighbours(current):
neighbour = (nr, nc)
tentative_g = g + move_cost
if tentative_g < g_scores.get(neighbour, float('inf')):
came_from[neighbour] = current
g_scores[neighbour] = tentative_g
h = self.heuristic(neighbour, goal)
heapq.heappush(open_set,
(tentative_g + h, tentative_g, neighbour))
return None # No path found
# Example usage
grid = np.zeros((20, 20), dtype=int)
# Add some obstacles
grid[5:15, 8] = 1
grid[2:10, 14] = 1
planner = AStarPlanner(grid)
path = planner.plan((0, 0), (19, 19))
if path:
print(f"Path found with {len(path)} steps")
# Visualise
fig, ax = plt.subplots()
ax.imshow(grid, cmap='Greys', origin='upper')
path_arr = np.array(path)
ax.plot(path_arr[:, 1], path_arr[:, 0], 'b-', linewidth=2)
ax.plot(path_arr[0, 1], path_arr[0, 0], 'go', markersize=10) # Start
ax.plot(path_arr[-1, 1], path_arr[-1, 0], 'ro', markersize=10) # Goal
plt.title('A* Path Planning')
plt.savefig('astar_path.png', dpi=100)
print("Path visualised and saved")
else:
print("No path found!")Grid Maps and Occupancy Grids
In real robotics, the environment is represented as an occupancy grid — a probabilistic map where each cell has a probability of being occupied. Sensors (lidar, ultrasonic, infrared) update these probabilities as the robot explores.
Converting sensor readings to a grid map:
class OccupancyGrid:
def __init__(self, width, height, resolution=0.05):
"""resolution in metres per cell"""
self.width = width
self.height = height
self.resolution = resolution
# Log-odds representation for numerical stability
self.log_odds = np.zeros((height, width))
def update(self, x, y, occupied: bool):
"""Update cell at world coordinates (x, y) metres"""
col = int(x / self.resolution)
row = int(y / self.resolution)
if 0 <= row < self.height and 0 <= col threshold).astype(int)Deploying A* on a Real Robot
Connecting path planning to a physical robot involves several steps:
- Map the environment: Use SLAM (Simultaneous Localisation and Mapping) with ultrasonic or lidar sensors to build an occupancy grid.
- Localise the robot: Know where the robot is on the map using wheel odometry, IMU, or external markers.
- Plan the path: Run A* on the current occupancy grid from robot’s position to goal.
- Execute the path: Convert grid waypoints to motor commands. A PID controller handles the actual steering.
- Replan dynamically: If new obstacles appear, rerun A* with the updated grid.
Beyond A*: Advanced Planning Algorithms
Once you are comfortable with A*, explore these more advanced algorithms:
- D* Lite: An incremental version of A* that efficiently replans when obstacles change — critical for dynamic environments.
- RRT (Rapidly-exploring Random Tree): Better for high-dimensional spaces (robot arms, 6-DOF robots) where grid-based approaches are impractical.
- RRT* (RRT-star): Asymptotically optimal version of RRT — finds better paths with more planning time.
- Theta*: Any-angle path planning on grids — produces smoother, shorter paths than A* by allowing movement in any direction, not just grid directions.
Frequently Asked Questions
What is the time complexity of A*?
A*’s worst-case time complexity is O(b^d) where b is the branching factor and d is the depth of the optimal solution. With a good heuristic, it typically explores far fewer nodes than this worst case. For a 100×100 grid, A* usually completes in milliseconds on a Raspberry Pi.
How do I handle moving obstacles with A*?
For moving obstacles, use dynamic replanning. The simplest approach: run A* periodically (e.g., every 500ms) with the latest sensor data. More sophisticated: use D* Lite for efficient incremental replanning, or add time as a third dimension to the grid.
Can A* be used for drone path planning?
Yes, but you need a 3D grid instead of a 2D one, which increases computational cost significantly. For drones, RRT or its variants are often preferred for 3D continuous spaces. A* works well for 2D floor plan navigation of indoor drones.
How do I implement A* in Arduino?
Arduino has limited RAM (2KB on Uno) which severely limits grid size. A 10×10 grid with 4 bytes per cell uses just 400 bytes — feasible. Use int8_t arrays and static allocation. For larger maps, use ESP32 (520KB RAM) or Raspberry Pi.
What is the difference between A* and Dijkstra’s algorithm?
Dijkstra’s algorithm is A* with h(n) = 0 — it expands all nodes in order of their distance from the start, guaranteeing optimality but exploring many more nodes than A*. A* uses the heuristic to focus the search towards the goal, making it much faster in practice.
Add comment