Week 9 — Advanced Motion Planning Day 2 of 7 | Difficulty: ⭐⭐⭐⭐

Why Search-Based Planning?

When the C-space can be discretized into a grid or graph, classical search algorithms become powerful motion planners. They are:

The tradeoff is the curse of dimensionality: a 6-DOF arm with 100 bins per joint has $100^6 = 10^{12}$ grid cells.

Dijkstra’s Algorithm

Dijkstra explores outward from the start, always expanding the node with the smallest accumulated cost. It is guaranteed optimal but expands in all directions uniformly.

import heapq

def dijkstra(grid, start, goal):
    """
    grid: 2D array where 0 = free, 1 = obstacle
    start, goal: (x, y) tuples
    """
    heap = [(0, start)]          # (cost, node)
    visited = set()
    came_from = {}
    
    while heap:
        cost, current = heapq.heappop(heap)
        if current in visited:
            continue
        visited.add(current)
        
        if current == goal:
            return reconstruct_path(came_from, current)
        
        for neighbor in neighbors(grid, current):
            if neighbor not in visited:
                new_cost = cost + 1
                heapq.heappush(heap, (new_cost, neighbor))
                came_from[neighbor] = current
    
    return None  # No path found

Time complexity: $O(V \log V + E)$ for a graph with $V$ vertices and $E$ edges.

A* Algorithm

A* is Dijkstra plus a heuristic $h(n)$ that estimates the cost from node $n$ to the goal. It prioritizes nodes that appear to be on the best path.

$$f(n) = g(n) + h(n)$$

Where:

The Heuristic Must Be Admissible

An admissible heuristic never overestimates the true cost. For grid planning:

If $h(n)$ is admissible, A* is optimal. If $h(n)$ is also consistent (monotonic), A* is optimally efficient — no other optimal algorithm expands fewer nodes.

Key Data: A* with a good heuristic expands 10-100× fewer nodes than Dijkstra in 2D grid planning.

Hybrid A*: Planning for Cars

Standard A* assumes the robot can move in any grid direction at each step. A car cannot — it must follow its kinematic constraints (Ackermann steering).

Hybrid A* (Dolgov et al., 2008) addresses this by:

  1. Discretizing the state into $(x, y, \theta)$ cells
  2. Generating children by simulating the vehicle’s motion model forward for a short time
  3. Using a analytic Reeds-Shepp curve as the heuristic (the shortest path for a car without obstacles)

This is the core algorithm behind the Stanford autonomous vehicle (Junior) and many modern self-driving stacks.

When to Use Search vs. Sampling

ScenarioUse SearchUse Sampling
Low-dimensional C-space (≤ 3 DOF)✅ Grid-based A*⚠️ Overkill
High-dimensional C-space (≥ 6 DOF)❌ Infeasible✅ RRT, PRM
Non-holonomic vehicle✅ Hybrid A*✅ Kinodynamic RRT
Need exact optimality✅ A* with admissible h❌ RRT is probabilistically optimal
Real-time, anytime⚠️ Limited✅ RRT* (improves over time)

Review Week 9 Day 1

Day 1 introduced C-space and the path-vs-trajectory distinction. Today we covered the algorithms that navigate C-space when it can be discretized.

Preview Day 3

Day 3: Sampling-based planning — RRT, RRT*, and PRM for high-dimensional C-spaces where grid methods fail.

Further Reading

FAQ

Q: Can A* handle 6-DOF arms?

A: Only with aggressive discretization or hierarchical decomposition. A 6-DOF arm with 10 bins per joint has $10^6 = 1{,}000{,}000$ cells — manageable with efficient collision checking. But 100 bins per joint ($10^{12}$ cells) is impossible.

Details: Multi-resolution A* or lazy collision checking can help. In practice, sampling-based methods (RRT, PRM) are preferred for 6+ DOF because they avoid explicit C-space discretization entirely.

Q: What happens if the heuristic overestimates?

A: A* may return a suboptimal path. If $h(n)$ is too aggressive (e.g., Euclidean distance × 2), A* greedily favors the goal and may miss a cheaper detour.

Details: Weighted A* deliberately inflates the heuristic ($h(n) \times w$ where $w > 1$) to find paths faster at the cost of optimality. This is common in games and real-time robotics.

Q: How does Hybrid A* differ from pure pursuit?

A: Pure pursuit is a tracking algorithm — it follows a pre-planned path by steering toward a lookahead point. Hybrid A* is a planning algorithm — it searches for the path itself while respecting vehicle kinematics.

Details: Pure pursuit cannot handle obstacles (it assumes a clear path). Hybrid A* actively searches around obstacles by simulating vehicle motion forward from each state.