Path Planning Algorithms Compared: A*, RRT, and Dijkstra

A robot that cannot plan a path is a robot that cannot do useful work. Whether you are building a warehouse AGV that navigates between shelves, a drone surveying a construction site, or a robotic arm reaching around obstacles, path planning is the core capability that turns a collection of motors and sensors into a purposeful machine.

The challenge is choosing the right algorithm. Path planning is a well-studied field with dozens of approaches, each with distinct trade-offs in speed, optimality, memory usage, and applicability. This tutorial compares the most important ones: Dijkstra's algorithm, A*, RRT, RRT*, and PRM. By the end, you will understand how each works, when to use it, and how to implement A* yourself in Python.

The Path Planning Problem

Path planning answers a deceptively simple question: given a start position, a goal position, and a map of obstacles, what is the best route between them?

"Best" usually means shortest distance, but it can also mean lowest energy, fastest time, or smoothest trajectory. The map can be a 2D grid, a 3D voxel space, or a high-dimensional configuration space for a robot arm with six or more joints.

Path planners fall into two broad families:

• Grid-based (combinatorial) planners discretize the space into cells and search through them systematically. Dijkstra and A* belong here.
• Sampling-based planners explore continuous space by randomly sampling configurations. RRT, RRT*, and PRM belong here.

Each family has strengths. Grid-based methods guarantee optimal paths when they exist but struggle in high dimensions. Sampling-based methods handle high-dimensional spaces gracefully but sacrifice optimality guarantees in their basic forms.

Grid-Based Algorithms

Dijkstra's Algorithm

Dijkstra's algorithm finds the shortest path from a start node to every other node in a weighted graph. It works by maintaining a priority queue of nodes ordered by their distance from the start. At each step, it extracts the node with the smallest distance, marks it as visited, and relaxes all its neighbors.

In the context of robot path planning, the graph is typically a 2D occupancy grid where each cell is either free or occupied. Edges connect adjacent cells, with weights equal to the Euclidean distance between cell centers (1.0 for cardinal neighbors, 1.414 for diagonal neighbors).

How it works:

1. Initialize the start node with distance 0 and all others with infinity.
2. Add the start node to a priority queue.
3. Extract the node with the smallest distance.
4. For each unvisited neighbor, calculate the tentative distance through the current node.
5. If the tentative distance is smaller than the stored distance, update it.
6. Repeat until the goal is extracted or the queue is empty.

Complexity: O((V + E) log V) with a binary heap, where V is the number of vertices and E the number of edges.

Key property: Dijkstra guarantees the shortest path. It never needs to revisit a node once that node is finalized.

Limitation: Dijkstra explores uniformly in all directions. It does not know where the goal is, so it wastes time exploring areas that lead away from the target. On a large grid, this translates to thousands of unnecessary node expansions.

A* (A-Star)

A* is Dijkstra with a heuristic. Instead of ordering the priority queue by distance from the start alone, A* uses f(n) = g(n) + h(n), where g(n) is the actual cost from start to node n, and h(n) is an estimated cost from n to the goal.

The heuristic guides the search toward the goal, dramatically reducing the number of nodes expanded compared to Dijkstra. If the heuristic is admissible (never overestimates the true cost) and consistent (satisfies the triangle inequality), A* is guaranteed to find the optimal path.

Common heuristics for grid-based planning:

• Manhattan distance: |x1 - x2| + |y1 - y2|. Admissible when only cardinal movement is allowed.
• Euclidean distance: sqrt((x1 - x2)^2 + (y1 - y2)^2). Admissible for any movement model.
• Octile distance: max(|dx|, |dy|) + (sqrt(2) - 1) * min(|dx|, |dy|). Admissible for 8-connected grids with diagonal movement.

Why it matters: On a 100x100 grid with sparse obstacles, Dijkstra might expand 8,000 nodes. A* with Euclidean heuristic might expand 800. Same optimal path, ten times less work.

Python Implementation of A* on a Grid

Here is a complete, working implementation of A* on a 2D occupancy grid:

import heapq
import math
from typing import Optional
 
 
def astar(
    grid: list[list[int]],
    start: tuple[int, int],
    goal: tuple[int, int],
) -> Optional[list[tuple[int, int]]]:
    """
    Find the shortest path on a 2D grid using A*.
 
    Args:
        grid: 2D list where 0 = free, 1 = obstacle.
        start: (row, col) start position.
        goal: (row, col) goal position.
 
    Returns:
        List of (row, col) from start to goal, or None if no path exists.
    """
    rows = len(grid)
    cols = len(grid[0])
 
    # 8-connected neighbors: (row_offset, col_offset, cost)
    neighbors = [
        (-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
        (-1, -1, 1.414), (-1, 1, 1.414), (1, -1, 1.414), (1, 1, 1.414),
    ]
 
    def heuristic(a: tuple[int, int], b: tuple[int, int]) -> float:
        """Euclidean distance heuristic."""
        return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)
 
    # Priority queue entries: (f_score, counter, node)
    counter = 0
    open_set: list[tuple[float, int, tuple[int, int]]] = []
    heapq.heappush(open_set, (heuristic(start, goal), counter, start))
 
    came_from: dict[tuple[int, int], tuple[int, int]] = {}
    g_score: dict[tuple[int, int], float] = {start: 0.0}
    closed: set[tuple[int, int]] = set()
 
    while open_set:
        _, _, current = heapq.heappop(open_set)
 
        if current == goal:
            # Reconstruct path
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            path.reverse()
            return path
 
        if current in closed:
            continue
        closed.add(current)
 
        for dr, dc, cost in neighbors:
            nr, nc = current[0] + dr, current[1] + dc
 
            if not (0 <= nr < rows and 0 <= nc < cols):
                continue
            if grid[nr][nc] == 1:
                continue
 
            neighbor = (nr, nc)
            if neighbor in closed:
                continue
 
            tentative_g = g_score[current] + cost
 
            if tentative_g < g_score.get(neighbor, float("inf")):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                counter += 1
                heapq.heappush(open_set, (f, counter, neighbor))
 
    return None  # No path found
 
 
# Example usage
if __name__ == "__main__":
    grid = [
        [0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 0, 0, 0],
    ]
 
    start = (0, 0)
    goal = (7, 7)
    path = astar(grid, start, goal)
 
    if path:
        print(f"Path found with {len(path)} steps:")
        for step in path:
            print(f"  ({step[0]}, {step[1]})")
    else:
        print("No path found.")

This implementation uses an 8-connected grid (cardinal and diagonal movement), Euclidean distance as the heuristic, and a counter to break ties in the priority queue. It returns the full path as a list of (row, col) coordinates, or None if the goal is unreachable.

Sampling-Based Algorithms

Grid-based algorithms work well in low-dimensional spaces (2D, maybe 3D). But consider a 6-DOF robot arm. Its configuration space has six dimensions. Discretizing each joint into 100 values produces 10^12 cells. No grid-based planner can handle that.

Sampling-based planners solve this by exploring the space through random samples rather than exhaustive enumeration.

RRT (Rapidly-Exploring Random Tree)

RRT builds a tree rooted at the start configuration by repeatedly sampling random points in the space and growing the tree toward them.

How it works:

1. Initialize the tree with the start node.
2. Sample a random point in the configuration space.
3. Find the nearest node in the tree to the random sample.
4. Extend the tree from the nearest node toward the sample by a fixed step size.
5. If the new node is collision-free, add it to the tree.
6. Repeat until a node is close enough to the goal.

Key properties:

• RRT is probabilistically complete: given enough time, it will find a path if one exists.
• RRT does not guarantee optimality. The first path found is usually jagged and suboptimal.
• RRT explores the space rapidly because random sampling naturally drives the tree toward unexplored regions (the Voronoi bias).
• RRT handles high-dimensional spaces well because it never discretizes the space.

Typical parameters:

• Step size: controls how far the tree extends per iteration. Too small and exploration is slow. Too large and the tree misses narrow passages.
• Goal bias: with some probability (e.g., 5-10%), sample the goal instead of a random point. This helps the tree grow toward the goal.

RRT* (Optimal RRT)

RRT* extends RRT with two modifications that guarantee asymptotic optimality: the path converges to the optimal solution as the number of samples approaches infinity.

Modifications over RRT:

1. Near-neighbor rewiring: When a new node is added, RRT* checks whether any nearby nodes would have a shorter path through the new node. If so, it rewires those connections.
2. Choose best parent: Instead of always connecting to the nearest node, RRT* connects the new node to whichever nearby node yields the lowest total path cost.

RRT* produces significantly smoother and shorter paths than RRT, but requires more computation per iteration due to the rewiring step.

PRM (Probabilistic Roadmap)

PRM takes a different approach than RRT. Instead of growing a single tree from start to goal, PRM builds a reusable roadmap of the entire space in a preprocessing phase, then queries it for specific start-goal pairs.

Two phases:

1. Construction phase: Sample N random collision-free configurations. For each, attempt to connect it to its k nearest neighbors with straight-line paths. If a connection is collision-free, add the edge to the roadmap graph.
2. Query phase: Connect the start and goal to their nearest roadmap nodes, then run A* or Dijkstra on the roadmap graph.

When PRM shines: Multi-query scenarios. If your robot needs to plan paths between many different start-goal pairs in the same environment, PRM amortizes the construction cost. A warehouse robot that repeatedly navigates the same floor layout is a perfect use case.

When PRM struggles: Single-query scenarios (just use RRT), or environments that change frequently (the roadmap becomes invalid).

Comparison Table

When to Use What

Use Dijkstra when:

• You need guaranteed shortest paths in a small, low-dimensional space.
• You need shortest paths from one source to all destinations (Dijkstra naturally computes this).
• Simplicity matters more than speed.

Use A when:*

• You need the shortest path between a specific start and goal.
• The space is low-dimensional (2D or 3D grid).
• You have a good admissible heuristic.
• This is the default choice for 2D mobile robot navigation.

Use RRT when:

• The configuration space is high-dimensional (robot arms, multi-joint systems).
• You need a path quickly and do not care if it is optimal.
• The environment has narrow passages (RRT's random sampling eventually finds them).
• You need a single path, not a reusable roadmap.

Use RRT when:*

• You need the benefits of RRT but also care about path quality.
• You can afford more computation time for a smoother, shorter path.
• The path will be executed by a physical robot where jerky motion wastes energy or time.

Use PRM when:

• The robot needs to plan many paths in the same environment.
• The environment is relatively static.
• Construction time is acceptable because queries will be fast.

Practical Considerations

Post-Processing

Raw paths from any algorithm often need post-processing before execution:

• Path smoothing: Remove unnecessary waypoints. A common approach is iterative shortcutting: try connecting non-adjacent waypoints with straight lines and keep the shortcut if it is collision-free.
• Trajectory generation: Convert the geometric path into a time-parameterized trajectory with velocity and acceleration profiles. Trapezoidal velocity profiles and cubic splines are common choices.
• Safety margins: Inflate obstacles by the robot's radius before planning. This simplifies the robot to a point and guarantees clearance.

Dynamic Environments

None of the algorithms above handle moving obstacles natively. For dynamic environments, consider:

• D Lite:* An incremental version of A* that efficiently repairs paths when the map changes.
• Replanning: Run A* or RRT at a fixed frequency, replanning from the robot's current position each cycle.
• Velocity obstacles: Plan in velocity space rather than position space, explicitly reasoning about the velocities of moving obstacles.

Real-World Libraries

You do not need to implement these from scratch for production use:

• OMPL (Open Motion Planning Library): The standard C++ library for sampling-based planning. Includes RRT, RRT*, PRM, and many variants.
• Navigation2 (Nav2): The ROS 2 navigation stack. Uses A* or Dijkstra for global planning and the DWB controller for local planning.
• MoveIt 2: The ROS 2 manipulation stack. Uses OMPL under the hood for robot arm path planning.

Summary

Path planning is a foundational skill in robotics. For 2D mobile robots on known maps, A* is the workhorse: fast, optimal, and straightforward to implement. For robot arms and other high-dimensional systems, RRT and its variants are the practical choice. PRM fills the niche of multi-query planning in static environments. Understanding these trade-offs lets you pick the right tool for each problem rather than defaulting to whichever algorithm you learned first.

The Python A* implementation in this post is a working starting point. Extend it with diagonal cost tuning, path smoothing, and obstacle inflation to build a planner suitable for real mobile robot navigation.

Want to see these algorithms in action? Check out our Path Planning lesson in the Robotics From Zero curriculum, where you can run A* and RRT in an interactive 2D simulator and watch the search expand in real time.