Motion Planning & Navigation

The robot finally knows where it is. Now it has to figure out how to get somewhere else: a path that dodges the furniture and obeys the way its own body is allowed to move.

For five chapters the robot has been answering where am I. Chapter 5 was the hard-won victory: fuse the sensors, fight the drift, and end up with a decent estimate of the robot's pose on a map. Fine. But knowing where you are is worthless if you can't decide where to go and how to get there without driving through a wall. That's this chapter. Planning is the "Plan" beat of the loop, and its whole job is to turn a start and a goal into a path: one your robot can actually follow, that doesn't clip an obstacle on the way.

The villain here is new and it's brutal: the curse of dimensionality. A wheeled robot lives in a 3-number world (x, y, heading) and you can almost picture the whole space of its poses. A 6-joint arm lives in a 6-number world, and a humanoid in dozens. As that number climbs, the space of possible configurations explodes so fast that the obvious approach (chop it into a grid and search) dies on contact. Every method in this chapter is a fight against that explosion. The hero, when the grid finally breaks, is randomization: stop trying to cover the space and start sampling it.

I'm assuming you've read Chapter 1, specifically that a robot's entire pose is a single point in its configuration space (Fig 1.3), and the holonomic vs nonholonomic split that decides which paths a robot's wheels can even follow. I'm assuming Chapter 5 gave you a map and a pose on it. Everything else we build from zero.

Here's the road, and every stop is a fix for the one before it. First we make configuration space precise, because planning lives there and nowhere else. Then we grid it and search: BFS, then Dijkstra to handle costs, then $\mathrm{A}^*$ to stop wasting effort. Then we watch the grid explode in high dimensions and switch to sampling: PRM, RRT, and $\mathrm{RRT}^*$ that smooths the path. Then we admit a geometric path isn't executable and add timing and smoothness. And we end with the two-layer architecture every real robot ships: a global planner that finds a route and a local planner that dodges the person who just walked in front of it.

Let's go.

Back in Chapter 1 you met configuration space as a slogan: a robot's whole pose is a single point, one axis per degree of freedom. Now we need it as a working tool, because it's the arena the entire chapter plays in.

Here's the problem it solves. In the real world, the robot has shape: it's a two-foot-wide base, or an arm sweeping through a room. Checking whether a shaped robot collides with a shaped obstacle, at every possible position and orientation, is a geometric nightmare. You'd be intersecting polygons forever.

So we play a trick, and it's one of the most elegant moves in all of robotics. We shrink the robot to a single point, and we pay for it by fattening the obstacles. Imagine sliding your robot around the boundary of an obstacle, as close as it can get without touching, and tracing where the robot's center goes. That traced boundary is a new, larger obstacle: the original grown outward by the robot's shape. Do this to every obstacle and you've inflated all of them. Now the robot is a dimensionless dot, and the question "does the robot collide" becomes "is the dot inside a fattened obstacle." Point-vs-region. Trivial.

That fattening has a name, the Minkowski sum of the obstacle and the robot's shape, and we're going to use it purely as intuition, no formula. The picture is enough: the obstacle grows by exactly the robot's footprint, so a point at the robot's center is safe if and only if the whole robot was safe. (Side note: this is why you inflate obstacles by half the robot's width plus a safety margin in practice; you want the center to stay that far from every wall.)

Now the payoff. Once obstacles are inflated, planning happens entirely in configuration space (C-space for short) where:

That's the whole reframing, and it's worth saying slowly: planning is drawing a line through configuration space that avoids the forbidden regions. Nothing more. Everything else in this chapter is a different way to draw that line.

What is C-space, concretely? It depends on the body; this is the Chapter 1 embodiment split coming back:

Hold onto that last one, because it's where the villain lives. A path through joint space is a coordinated sweep of all the joints at once, and picturing it, let alone gridding it, is about to get very hard.

We have a clean problem now: steer a point from start to goal through free C-space. The most obvious thing in the world is to chop that space into a grid of cells, mark each cell free or blocked, and search for a chain of free cells connecting start to goal. Let's build that up the way we build everything: the naive version first, then watch each fix hand us the next one.

Overlay a grid. Each free cell is a node; adjacent free cells are connected. Now it's a graph-search problem, and the simplest correct answer is breadth-first search (BFS): start at the start cell, explore all its neighbors, then all their neighbors, expanding outward in rings. The moment you touch the goal, the ring you touched it on is the fewest steps away, so you've found the shortest path in number of moves.

BFS works and it's optimal, if every step costs the same. But look at what it's doing: it expands in perfect rings in every direction, including directly away from the goal. It has no idea where the goal is until it stumbles into it. On a big open map it'll explore a huge disk of cells before reaching a goal that was straight ahead. Blind. And it can't tell that crossing gravel should cost more than crossing pavement; every step is worth exactly one.

The first crack in BFS is that "shortest in steps" isn't "shortest in cost." Real terrain has costs: diagonal moves are longer than straight ones ($\sqrt{2}$ vs 1), mud is slower than road, going near a wall is riskier. Dijkstra's algorithm (1959) fixes this. Instead of expanding in step-count rings, it keeps a running tally $g(n)$ (the cheapest known cost to reach each cell from the start) and always expands the unexpanded cell with the smallest g so far. When it finally pops the goal off that priority queue, $g(\text{goal})$ is provably the true cheapest cost, and you recover the path by following the "who got me here cheapest" pointers backward.

Dijkstra is BFS with a cost meter. It's optimal for any non-negative edge costs. But it inherits BFS's real sin: it still expands in all directions equally. It grows a cost-weighted blob outward from the start, pouring effort into cells behind and beside the goal, because it has no notion of which way the goal is. On a big map that's a mountain of wasted expansions.

Here's the fix, and it's the whole ballgame. Dijkstra picks the next cell to expand by $g(n)$ alone: cost so far. What if we also gave it a guess of the cost still remaining, and told it to prefer cells that look closer to the goal? That guess is a heuristic, $h(n)$, and the algorithm that adds it is $\mathbf{A}^*$ (Hart, Nilsson & Raphael, 1968).

$\mathrm{A}^*$ expands the cell with the smallest

Reading the symbols: $g(n)$ is the cost already spent getting to cell $n$ from the start (the part you know for sure, same as Dijkstra). $h(n)$ is the estimated cost still to go from $n$ to the goal (the guess). Their sum $f(n)$ is $\mathrm{A}^*$'s best estimate of the total cost of a path through $n$. By always expanding the smallest-$f$ cell, the search leans toward cells that are both cheap to reach and look close to the goal, so it drives at the goal instead of ballooning in every direction.

What do you use for $h$? Something cheap that estimates remaining distance. On a grid where you can move in 8 directions, straight-line (Euclidean) distance is a classic choice; if you can only move in 4, the Manhattan distance ($|\Delta x| + |\Delta y|$) fits. The one property that matters is:

Admissibility: the heuristic must never overestimate the true remaining cost. As long as $h(n)$ never exceeds the real cost-to-go for any cell, $\mathrm{A}^*$ is guaranteed to return the optimal path. The intuition: an admissible $h$ is optimistic; it might undersell how far the goal is, but it never oversells. Because it never claims the goal is farther than it truly is, $\mathrm{A}^*$ can never be tricked into finalizing a suboptimal path and skipping a better one. Straight-line distance is always admissible in the plane, because no path through obstacles is ever shorter than the straight line.

And now the punchline that ties the three together:

Let's watch $\mathrm{A}^*$ actually run. Tiny grid, 4-connected (up/down/left/right, each step costs 1). Start S at column 0, goal G at column 3, same row. One obstacle # blocks the direct middle. Coordinates are (col, row), and the heuristic is Manhattan distance to G at (3,1).

Start S=(0,1), goal G=(3,1). We expand smallest $f = g + h$:

Path: S → (0,0) → (1,0) → (2,0) → (2,1) → G, cost 5, going up and over the obstacle. Here's the thing to notice: $\mathrm{A}^*$ barely touched the cells below the start row. Dijkstra, with $h=0$, would have expanded (0,2), (1,2), (2,2), the whole bottom row, with equal enthusiasm, because it can't tell the goal is up-and-right. The heuristic kept $\mathrm{A}^*$'s $f$-values honest and steered it over the top, expanding a handful of cells instead of flooding the grid. That's the entire reason $\mathrm{A}^*$ is the default planner in games, GPS routing, and a thousand robots.

Grid search is wonderful and you should reach for it first: on a mobile robot navigating a 2D floor, $\mathrm{A}^*$ on an occupancy grid is often the entire answer. But now point it at a 6-joint arm, and watch it detonate.

The problem is counting. Suppose you grid each axis into just 100 cells: coarse, barely enough to squeeze through a doorway. In 2D (a mobile base ignoring heading) that's $100 \times 100 = 10^4$ cells. Totally fine; your laptop laughs. In 3D (add heading) it's $10^6$. Still fine. Now the 6-joint arm: $100^6 = 10^{12}$ cells. A trillion. Even if you could store one bit per cell that's 125 gigabytes just to hold the grid, and $\mathrm{A}^*$ would have to expand a meaningful fraction of it. A 7-DoF arm is $10^{14}$. A two-armed humanoid with a floating base is astronomically worse.

This is the curse of dimensionality, and it's the villain the whole rest of the chapter fights: the number of grid cells grows as (cells-per-axis) raised to the number of dimensions. Add one degree of freedom and you multiply the total by 100. The grid isn't a little too big in high dimensions; it's too big by factors that dwarf the number of atoms you have storage for. There is no clever indexing that saves you: the space itself is that vast.

And here's the cruel part: almost all of that trillion cells is empty free space you'll never care about. You don't need to represent the whole space. You need one thin path through it. Gridding pays to enumerate everything so it can find the one thread. In high dimensions that's a catastrophically bad trade. The fix is to stop enumerating and start sampling: throw down random configurations, keep the valid ones, and stitch a path through those. Which is exactly where we're going.

The insight that saves us is almost cheeky: don't build the space, poke it. Throw a random configuration into C-space. Check if it's collision-free (a fast point-vs-region test, thanks to the inflation trick). If it is, keep it as a node. Do this a few thousand times and you've got a scatter of valid configurations, a sketch of the free space, that you never had to enumerate cell by cell. Randomness beats grids in high dimensions because a random sample doesn't care how many dimensions it's in; a thousand samples is a thousand samples whether the space is 2D or 20D. The grid's cost exploded with dimension; sampling's cost doesn't. That's the whole trick, and it's the hero of high-DoF planning.

There are two great ways to use those samples, and they answer two different questions.

The Probabilistic Roadmap (PRM) (Kavraki, Švestka, Latombe & Overmars, 1996) is the multi-query workhorse. It has two phases:

The beauty is the split: you pay the expensive sampling-and-connecting cost once, and then answer many start–goal queries cheaply on the same graph. If your robot works in a fixed environment (a warehouse, a factory cell) and plans hundreds of paths through it, PRM amortizes gorgeously. Its weakness is the flip side: if the environment changes, the roadmap is stale, and building it in the first place is wasted effort if you only need one path.

For a single query (one start, one goal, get me there now) building a whole roadmap is overkill. The Rapidly-exploring Random Tree (RRT) (LaValle, 1998) grows a single tree outward from the start and stops when it reaches the goal. The loop is beautiful in its simplicity:

Why does this explode outward so efficiently? The magic is step 2. Because you always extend from the nearest existing node toward a random target, the tree is biased toward the large unexplored voids: a random sample is most likely to fall in a big empty region, and the nearest node to it is on the frontier facing that region. So the tree reaches into open space fast, "rapidly exploring," instead of thrashing around where it already is. (Common trick: every so often, make $q_{\text{rand}}$ be the goal instead of a random point. That "goal bias" tugs the tree toward the target without giving up the exploration.)

RRT's real superpower, and the reason it dominates in practice, is that the "steer" step can respect the robot's motion constraints. Remember Chapter 1's nonholonomic car: it can't slide sideways, only drive-and-steer along feasible curves. In a grid or PRM you'd connect nodes with straight lines the car can't follow. But RRT's steer step can be the car's actual dynamics: extend $q_{\text{near}}$ toward $q_{\text{rand}}$ by simulating a legal drive-and-steer motion. Every edge in the tree is then a motion the robot can genuinely execute. This is why RRT and its kin are the go-to for nonholonomic and kinodynamic planning (planning that respects velocity and acceleration limits, not just geometry) from self-parking cars to quadrotors.

Run RRT and it will find a path, usually fast. But look at it: it's jagged, wandering, full of needless zigzags, because it's stitched together from random extensions that never asked "is this the shortest way?" RRT is probabilistically complete (give it enough samples and it'll find a path if one exists) but it makes no promise about the path's quality. The first path it stumbles onto is the path you get, warts and all.

The fix is $\mathbf{RRT}^*$ (Karaman & Frazzoli, 2011) and it's a small, gorgeous addition that makes the tree self-improve. $\mathrm{RRT}^*$ runs the same sample-nearest-steer loop, but adds two rewiring steps every time it inserts a new node $q_{\text{new}}$:

That rewiring is everything. Over iterations, kinks straighten, detours collapse, and the tree's paths converge toward the true optimum. This is what "asymptotically optimal" means: $\mathrm{RRT}^*$ isn't optimal after a fixed number of samples, but as samples $\to \infty$ the cost of its path converges to the best possible. In practice you run it for a fixed time budget and take the best path so far, and it's dramatically smoother and shorter than vanilla RRT's first jagged attempt. The trade is compute: all that neighbor-checking and rewiring costs more per sample. Vanilla RRT when you just need a path fast; $\mathrm{RRT}^*$ when path quality is worth the cycles.

Say the planner hands you a beautiful collision-free path through C-space. You are not done. A path is pure geometry: an ordered set of configurations, a squiggle in joint space. It says where to go but says nothing about when, and a real robot can't execute a where without a when.

Three things are missing, and each one bites:

So the last step is turning a geometric path into an executable trajectory: a smooth, time-parameterized $q(t)$ that respects velocity and acceleration limits. The tools that do this are trajectory optimization methods: you'll hear CHOMP (Covariant Hamiltonian Optimization for Motion Planning, Ratliff et al., 2009) and TrajOpt (Schulman et al., 2013) by name. They take an initial path (often a rough one straight from RRT, or even a naive straight line) and iteratively deform it to minimize a cost that trades off smoothness (low acceleration, no jerky corners) against staying clear of obstacles, all while honoring the robot's limits. The path flows away from obstacles and relaxes into a smooth, dynamically-feasible sweep the controller from Chapter 4 can actually track.

You'll often see this as a two-stage pipeline: a sampling planner finds a feasible path through a cluttered space (its strength), then an optimizer polishes that path into something smooth and fast (its strength). One finds the route; the other makes it graceful. Keep it brief in your head: planning finds a path, trajectory optimization makes it executable.

Here's the last problem, and it's the one that separates a demo from a robot that ships. Everything so far assumed a static, known map. But Chapter 1's Villain #1 never left: the world doesn't hold still. You planned a gorgeous path across the office, and then a person walks across it. A door that was open is now closed. A box appears in the hallway. Replanning your entire global path from scratch, every time a pixel changes, at the speed people move? Hopeless: global planning is too slow to run in the tight sense-plan-act loop.

So real navigation stacks split the job into two layers running at two speeds, and the split maps cleanly onto Chapter 1's loop:

The canonical local planner is the Dynamic Window Approach (DWA) (Fox, Burgard & Thrun, 1997). Its idea is direct and it leans hard on Chapter 1's holonomy lesson. At each tick, DWA looks only at the velocities the robot can actually reach in the next instant given its current speed and its acceleration limits; that reachable set is the "dynamic window," and it exists precisely because the robot can't teleport to any velocity; it's bounded by its own dynamics. (Concretely: rolling at 0.5 m/s with a 1 m/s² acceleration limit and a 0.1 s control tick, the window holds only speeds between 0.4 and 0.6 m/s — a razor-thin slice of everything the motor could ever do, which is exactly why scoring every candidate in it is cheap.) For each candidate velocity in that window, DWA simulates the short arc the robot would trace and scores it on three things: does it head toward the global goal, does it stay clear of obstacles, and does it keep speed up. Pick the best-scoring velocity, execute it for one tick, and repeat. It's sense-plan-act at the reflex level: a tight, fast loop that hugs the global plan but flinches away from anything new.

Put the two together and you get the architecture nearly every mobile robot runs: the global planner draws the route on the map; the local planner walks that route in real time while dodging the world's surprises. The global layer handles where to go; the local layer handles what just happened. It's the sense-plan-act loop of Chapter 1, wearing a bigger coat: a slow deliberate planner wrapped around a fast reactive one, each covering the other's blindness, exactly the way we fused sensors in Chapter 5.

The robot can now go somewhere. Chapter 5 gave it a pose on a map; this chapter turned "I'm here, I want to be there" into an executable motion. The spine of it: planning is drawing a path through configuration space, the space where the robot shrinks to a point (Fig 1.3's promise, cashed in via obstacle inflation) and obstacles become forbidden regions. On a low-dimensional mobile base you grid that space and run $\mathbf{A}^*$, which is just Dijkstra with an admissible heuristic that aims the search at the goal. When the dimension climbs and the grid detonates under the curse of dimensionality, you stop enumerating and start sampling: PRM to build a reusable roadmap for many queries, RRT to grow a single tree fast (and respect nonholonomic constraints), $\mathbf{RRT}^*$ to rewire that tree toward the optimal path. Then trajectory optimization (CHOMP, TrajOpt) turns the geometric squiggle into a smooth, timed, limit-respecting trajectory the controller can track. And in the real, moving world, you split the job: a global planner for the route, a fast local planner (DWA) for the surprises.

Notice what we did not do: we never asked the robot to learn anything. Every planner here was handed a map and a model of how the robot moves, and it computed. That's the classical stack, and it's genuinely enough for enormous swaths of real robotics. But it has a ceiling. It needs a map it can trust and a motion model it can simulate, and it plans for the world as given rather than discovering good behavior on its own. What happens when the "cost" of a motion is too subtle to write down, when good behavior has to be learned from trying, failing, and trying again? That's the turn the guide takes next. Chapter 7 hands the robot a reward instead of a map and lets it discover its own policy through reinforcement learning, trading the planner's guarantees for the ability to learn behaviors no one could hand-code.

Every planner in this chapter was still just the "Plan" beat of Fig 1.1's loop, done deliberately. Next we let the robot learn that beat for itself.

Prev: Chapter 5, Estimation, Localization & SLAM · Next: Chapter 7, Reinforcement Learning for Robots