← Robotics

Motion planning: A* and RRT

Notes · finding a path · Jan 2024

Motion planning is the problem of getting from a start to a goal without hitting anything. The catch is that "configuration" for a real robot can be high-dimensional, and the obstacles live in that space, not in the world you can see.

Configuration space

Planning happens in configuration space, where each point is a complete pose of the robot. A path is a curve through the free part of that space. For a 2D point robot this is just the map; for an arm or a drone with orientation it has many dimensions, which is what makes planning hard.

A*: search a graph

Discretize the space into a grid or graph and run A*, which expands nodes in order of cost-so-far plus a heuristic estimate of cost-to-go. With an admissible heuristic it returns the optimal path and never over-explores. It is the right tool in low dimensions, but the number of grid cells explodes as dimensions grow.

RRT: sample instead of enumerate

Rapidly-exploring random trees avoid the grid entirely: sample a random configuration, steer the nearest tree node toward it, and add the new node if the motion is collision-free. The tree quickly fills the space, which is what makes RRT practical in high dimensions. Plain RRT finds a path, not the best one; RRT* rewires the tree as it grows and converges to the optimal path over time.

Trade-offs

  • A*: complete and optimal, but limited by the curse of dimensionality.
  • RRT / RRT*: scales to many dimensions, but paths are jagged and usually need smoothing.

Tree-based planners like these are what I used for autonomous navigation in GPS-denied environments, where you cannot rely on a global map and have to plan over the robot's own state.

Step by step: A*

  1. Keep a priority queue ordered by f = g + h (cost so far plus heuristic).
  2. Pop the lowest-f node; if it is the goal, reconstruct the path.
  3. For each free neighbor, compute a tentative g.
  4. If it improves, record the parent and push it with its f.
  5. An admissible heuristic (Manhattan here) guarantees an optimal path.
import heapq

def astar(grid, start, goal):
    """grid: 2D list, 0 = free, 1 = wall. 4-connected, Manhattan heuristic."""
    h = lambda a, b: abs(a[0]-b[0]) + abs(a[1]-b[1])
    R, C = len(grid), len(grid[0])
    pq = [(h(start, goal), 0, start)]      # (f = g + h, g, node)
    came, g = {}, {start: 0}
    while pq:
        _, gc, cur = heapq.heappop(pq)
        if cur == goal:
            path = [cur]
            while cur in came:
                cur = came[cur]; path.append(cur)
            return path[::-1]
        for dr, dc in ((1,0), (-1,0), (0,1), (0,-1)):
            nr, nc = cur[0]+dr, cur[1]+dc
            if 0 <= nr < R and 0 <= nc < C and grid[nr][nc] == 0:
                ng = gc + 1
                if ng < g.get((nr, nc), float('inf')):
                    g[(nr, nc)] = ng
                    came[(nr, nc)] = cur
                    heapq.heappush(pq, (ng + h((nr, nc), goal), ng, (nr, nc)))
    return None

Step by step: RRT

  1. Grow a tree from the start.
  2. Sample a random point and find the nearest tree node.
  3. Step from that node toward the sample; keep it if it is collision-free.
  4. Stop when a new node reaches the goal, then backtrack parents for the path.
import numpy as np

def rrt(start, goal, is_free, bounds, step=0.5, n=5000, tol=0.5):
    rng = np.random.default_rng(0)
    nodes, parent = [np.array(start, float)], {0: None}
    lo, hi = np.array(bounds[0]), np.array(bounds[1])
    goal = np.array(goal, float)
    for _ in range(n):
        rand = rng.uniform(lo, hi)
        i = min(range(len(nodes)), key=lambda k: np.linalg.norm(nodes[k] - rand))
        d = rand - nodes[i]
        new = nodes[i] + d / np.linalg.norm(d) * min(step, np.linalg.norm(d))
        if not is_free(new):
            continue
        nodes.append(new); parent[len(nodes) - 1] = i
        if np.linalg.norm(new - goal) < tol:        # reached the goal
            path, k = [], len(nodes) - 1
            while k is not None:
                path.append(nodes[k]); k = parent[k]
            return path[::-1]
    return None

References