Control Engineering Primer: Path Planning — A*, Dijkstra, RRT, RRT*, and PRM

Moving a robot from start to goal rarely means drawing a straight line. A usable plan must account for walls, passage width, robot footprint, turning limits, localization uncertainty, stale maps, moving people, and stopping distance. Path planning chooses where to go; trajectory generation and control decide how to follow that route under dynamics. A safe stack keeps those responsibilities distinct while sharing their limits.

This primer compares grid Dijkstra and A with continuous-space RRT, RRT, and PRM. It explains obstacle inflation, heuristics, sampling, complexity, SLAM/Nav2 integration, implementation checks, and independent safety behavior. See Visual SLAM Primer, ROS 2 Primer, and Sensor Fusion Primer for adjacent layers.

Practical conclusion

• Dijkstra guarantees a shortest path on a nonnegative weighted graph but expands in directions unrelated to the goal. A* directs expansion with an admissible heuristic while retaining the same optimality.
• RRT tends to find a feasible path quickly in high-dimensional continuous spaces. RRT* approaches an optimal path as samples grow, with added neighbor and rewiring cost. PRM amortizes roadmap construction across many queries in a sufficiently static space.
• A shortest path through an uninflated map is a collision path for a robot with radius, localization error, and stopping distance.
• A returned route is not proof of safety. Map freshness, local sensing, tracking error, dynamic obstacles, replan latency, and E-stop need independent treatment.

Define free space before selecting an algorithm

Let state space be \mathcal X, obstacle space \mathcal X_{obs}, and free space \mathcal X_{free}=\mathcal X\setminus\mathcal X_{obs}. A path \sigma:[0,1]\to\mathcal X_{free} connects x_s to x_g. A point robot in 2D uses (x,y); a vehicle adds heading, speed, and steering; a manipulator includes all joint angles. Simplifying state reduces search cost but can produce curves the downstream vehicle cannot realize.

Obstacle inflation turns a finite robot into a point search by expanding obstacles. A conceptual minimum is

where the terms cover body radius, localization uncertainty, and tracking/stopping margin. In reality, margin varies with map resolution, sensor blind spots, approaching-object speed, and braking capability. Too little margin collides; too much declares viable passages impossible.

Diagram: Duskcoil, conceptual rather than measured. Cell size and inflation must be derived from a real robot footprint, uncertainty, and operating envelope.

Grid search: Dijkstra and A*

For graph G=(V,E) with nonnegative edge cost c(u,v), Dijkstra repeatedly settles the unsettled node with lowest known start cost g(n) and relaxes neighbors. With a binary heap, a representative complexity is O((|V|+|E|)\log|V|). It guarantees shortest graph distance but, without goal knowledge, tends to expand broadly.

A* orders nodes by

where h(n) is a lower bound on remaining cost. An admissible heuristic never overestimates true remaining cost; then A* remains optimal. Manhattan distance suits 4-connected grids, while Euclidean or Chebyshev-related distance may suit 8-connected movement. A consistent heuristic additionally satisfies h(n)\le c(n,n')+h(n') and reduces re-expansion.

Weighted A* uses g+wh with w>1 to seek a feasible route faster at the cost of optimality. This can be a sound operational trade if it is explicit. Edge cost can encode not only length but inflated-obstacle risk, clearance, turning, energy, or terrain. The output is then minimum defined cost, not necessarily minimum geometric length.

Continuous and high-dimensional spaces: RRT, RRT*, PRM

Fine grids explode in a six-joint arm configuration space or vehicle pose space. RRT samples x_{rand} from free space, finds nearest tree node x_{near}, steers a limited distance toward it, collision-checks, and adds x_{new}. It is probabilistically complete: with enough samples, probability of finding a feasible route approaches one when one exists. It does not guarantee a short first route.

RRT* chooses the least-cost parent among nearby vertices and rewires neighbors through a new vertex when cheaper. It is asymptotically optimal, not finite-time optimal; neighbor search, collision tests, and rewiring consume computation. Measure quality at the operational deadline rather than promising “optimal.”

PRM samples free configurations and connects nearby collision-free pairs into a reusable roadmap. It is attractive in a static factory or repeated arm-query setting because preprocessing can be amortized. Dynamic obstacles invalidate edges. Narrow passages are difficult for uniform sampling, so obstacle-boundary, path-biased, or task-informed samples may be needed.

Diagram: Duskcoil, simplified. Sampling, collision checking, and connectivity are not a performance measurement or a final production route.

SLAM, Nav2, local planning, and safety

SLAM supplies a map and pose estimate, but a planner needs timestamp-aligned transforms and a clear occupancy/cost meaning. Loop closure or relocalization can shift a map-frame pose; continuing to follow an old route can be unsafe. Feed uncertainty, localization reset, and map-update events into replanning rules; see Visual SLAM Primer.

In a Nav2-like ROS 2 architecture, a global costmap and planner choose a large-scale route, while a local costmap and controller handle nearby obstacles and velocity. A global A* can select a corridor; the local layer must yield, stop, or detour around a person. A purely local layer can get trapped in a cul-de-sac. Define planner, controller, recovery, map-update rates, deadlines, and priorities explicitly. ROS 2 transport described in ROS 2 Primer is not a real-time or safety guarantee.

Before deployment, measure footprint including payload, sensor field of view, maximum speed/deceleration, localization error, and map resolution. Test actual clearance in narrow passages. Collision-check planned paths against continuous motion and kinematics: a grid route can turn between cells in ways a differential drive, car, or arm cannot.

For dynamic obstacles, measure detection freshness, relative speed, braking distance, and replan time; never keep moving because a planner is late. Treat unknown people, holes, transparent obstacles, and sensor failure as safety cases, not automatically free cells. Slow, stop, or hand over when there is no route, local path is unsafe, covariance is too large, map is stale, or tracking error exceeds its envelope. E-stop must operate independently of planner output.

Implementation checklist and references

1. Define state space, footprint, frames, map resolution, and meaning of unknown cells.
2. Make inflation include localization error, speed, and stopping distance; test real narrow passages.
3. Verify heuristic admissibility or document the deliberately relaxed guarantee.
4. Record collision-check resolution, random seed, deadline, and no-solution behavior for sampling planners.
5. Inject relocalization, map changes, sensor dropout, dynamic obstacles, and communication delay.

6. Verify a safe stop and diagnosable log path for no-route, stale-map, and tracking-deviation events.

7. Hart, Nilsson, Raphael, 1968: A Formal Basis for A*

8. LaValle: Rapidly-Exploring Random Trees
9. Karaman and Frazzoli, 2011: RRT*
10. Nav2 documentation