Robot Path Planning: A*, RRT, Dijkstra, Trajectory Optimization
Path planning is the computational problem of finding a collision-free path from a start configuration to a goal configuration through an environment with obstacles. It is a core component of autonomous robotics — without reliable path planning, even the most sophisticated perception or control system is useless. The field has produced a rich ecosystem of algorithms, each suited to different configuration space dimensionalities, environmental characteristics, and performance requirements. This guide covers the major path planning paradigms: graph-based search, sampling-based planning, trajectory optimization, kinodynamic planning, collision avoidance, and multi-robot coordination.
Path Planning Fundamentals
The path planning problem is formally defined as: given a configuration space C, a set of obstacle configurations C_obs, a start configuration q_start ∈ C_free (where C_free = C \ C_obs), and a goal configuration q_goal, find a continuous function τ: [0,1] → C_free such that τ(0) = q_start and τ(1) = q_goal. The configuration space dimension equals the robot’s degrees of freedom. A rigid mobile robot operating on a plane has a 3D configuration space (x, y, θ). An articulated industrial robot with six joints has a 6D configuration space.
The curse of dimensionality makes path planning computationally challenging as DOF increases — grid-based approaches scale exponentially with dimension, while sampling-based methods scale polynomially. The workspace and configuration space are distinct. Even workspace that appears simple — an empty room with a single pillar — maps to complex configuration space obstacles depending on robot geometry. The Minkowski sum of the robot shape with workspace obstacles produces configuration space obstacles; computing this exactly is computationally infeasible for non-convex shapes, motivating collision-checking approaches that test sampled configurations against workspace obstacles. Latombe provides the foundational mathematical treatment of configuration space planning (Latombe, Robot Motion Planning, Kluwer Academic Publishers, 1991).
Graph-Based Search Algorithms
Dijkstra’s algorithm, published by Edsger Dijkstra in 1959, finds the shortest path in a graph from a start node to all other nodes. In path planning, the graph nodes represent robot configurations, and edges represent feasible transitions between them. Dijkstra’s algorithm guarantees optimality, exploring nodes in order of increasing distance from the start. For grid-based costmaps common in mobile robot navigation, Dijkstra’s algorithm computes the shortest path on the eight-connected grid. Each cell’s traversal cost incorporates obstacle clearance and terrain difficulty.
A* (pronounced “A-star”), introduced by Hart, Nilsson, and Raphael in 1968, extends Dijkstra’s algorithm with a heuristic function h(n) estimating the remaining cost from node n to the goal (Hart, Nilsson, and Raphael, “A Formal Basis for the Heuristic Determination of Minimum Cost Paths,” IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968). Using the Euclidean distance as a heuristic, A* typically explores significantly fewer nodes than Dijkstra while maintaining optimality, provided the heuristic is admissible (never overestimates actual cost). Weighted A* — scaling the heuristic by a factor ε > 1 — trades optimality for speed, producing near-optimal paths with dramatically fewer node expansions.
D* Lite, developed by Koenig and Likhachev, enables efficient replanning when the environment changes (Koenig and Likhachev, “D* Lite,” AAAI, 2002). When new obstacle information arrives, D* Lite repairs the previous search rather than recomputing from scratch. This incremental approach is essential for robots operating in partially unknown or dynamic environments. The algorithm propagates cost changes through the search tree, updating only affected nodes. Field D* further extends this approach with continuous interpolation between grid cells for smoother paths.
Sampling-Based Planning
For high-dimensional configuration spaces characteristic of articulated robots, sampling-based methods avoid explicitly constructing the configuration space by randomly sampling configurations and connecting them into a graph.
Probabilistic roadmaps (PRM), introduced by Kavraki et al. in 1996, build a graph in configuration space by sampling configurations uniformly at random, retaining collision-free samples as nodes, and connecting nearby nodes with collision-free straight-line segments (Kavraki, Svestka, Latombe, and Overmars, “Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996). The resulting roadmap captures the connectivity of free space. PRM is a multi-query planner — one roadmap supports many start-goal pairs. Variants like PRM* adjust connection radius based on the number of samples to ensure asymptotic optimality.
Rapidly-exploring random trees (RRT), developed by LaValle in 1998, grows a tree from the start configuration by sampling random configurations and extending the nearest tree node toward the sample (LaValle, “Rapidly-Exploring Random Trees: A New Tool for Path Planning,” Technical Report TR 98-11, Iowa State University, 1998). The tree biases exploration toward unexplored regions, rapidly covering the configuration space. RRT is a single-query planner efficient for problems where the roadmap would be unused.
RRT*, introduced by Karaman and Frazzoli in 2011, provides asymptotic optimality by rewiring the tree — when a new node is added, the algorithm checks whether nearby nodes would have shorter paths through the new node and updates the tree accordingly (Karaman and Frazzoli, “Sampling-based Algorithms for Optimal Motion Planning,” International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011). RRT* converges to the optimal path as the number of samples approaches infinity. Informed RRT* focuses sampling within the ellipsoidal region bounded by the current best path, accelerating convergence by orders of magnitude.
Collision checking dominates the runtime of sampling-based planners. Libraries like FCL (Flexible Collision Library) use hierarchical bounding volume hierarchies to accelerate collision checking. For each sampled configuration, the library tests the robot geometry against obstacle geometry using oriented bounding boxes (OBB) or axis-aligned bounding boxes (AABB) trees.
Kinodynamic Planning
Kinodynamic planning incorporates both kinematic constraints (joint limits, non-holonomic constraints) and dynamic constraints (velocity limits, acceleration bounds, torque limits) into the planning process. Unlike geometric planners that only consider configuration feasibility, kinodynamic planners produce trajectories that can be executed by the actual robot.
State-space planning extends the configuration space with velocity dimensions. For a mobile robot with non-holonomic constraints (cannot move sideways), the state space is (x, y, θ, v, ω) — adding linear and angular velocity to the Cartesian position and heading. RRT variants for kinodynamic planning use forward propagation: rather than extending toward a random sample, the planner applies a random control input for a short duration and adds the resulting state to the tree.
Hybrid A*, developed by Dolgov et al. for the DARPA Urban Challenge, discretizes the state space into cells but maintains continuous state representation within each cell (Dolgov, Thrun, Montemerlo, and Diebel, “Practical Search Techniques in Path Planning for Autonomous Driving,” AAAI, 2008). When multiple states fall in the same cell, only the one with the lower cost is retained. Hybrid A* enables autonomous vehicles to plan paths that respect non-holonomic constraints while achieving real-time performance.
Trajectory optimization refines initial paths into smooth, dynamically feasible trajectories. CHOMP (Covariant Hamiltonian Optimization for Motion Planning), developed by Zucker et al. at CMU, iteratively improves an initial trajectory by following the gradient of a cost function balancing obstacle avoidance, smoothness, and joint limit constraints (Zucker, Ratliff, Dragan, Pivtoraiko, Klingensmith, Dellin, and Srinivasa, “CHOMP: Covariant Hamiltonian Optimization for Motion Planning,” International Journal of Robotics Research, vol. 32, no. 9–10, pp. 1164–1193, 2013). STOMP (Stochastic Trajectory Optimization) replaces gradient computation with stochastic sampling, handling non-differentiable cost terms.
Collision Avoidance
Path planners compute a collision-free path before execution begins, but dynamic obstacles require online collision avoidance during motion. The velocity obstacle (VO) paradigm, formulated by Fiorini and Shiller, computes the set of robot velocities that will result in collision with a moving obstacle at some future time (Fiorini and Shiller, “Motion Planning in Dynamic Environments Using Velocity Obstacles,” International Journal of Robotics Research, vol. 17, no. 7, pp. 760–772, 1998). The robot selects a velocity outside all velocity obstacles and toward the goal.
Reciprocal velocity obstacles (RVO) and optimal reciprocal collision avoidance (ORCA) extend VO to multi-agent scenarios. Under ORCA, each agent assumes others will reciprocally avoid collisions, halving the collision avoidance responsibility. ORCA scales to hundreds of agents and provides collision-free navigation guarantees assuming agents follow the protocol (van den Berg, Guy, Lin, and Manocha, “Reciprocal n-Body Collision Avoidance,” Springer Tracts in Advanced Robotics, vol. 70, 2011).
For manipulators, artificial potential fields attract the robot toward the goal while repelling from obstacles. Navigation functions provide a potential field with a single global minimum, avoiding the local minima problem that plagues naive potential field approaches.
Multi-Robot Path Planning
Coordinating multiple robots sharing workspace requires planning that prevents inter-robot collisions. Prioritized planning assigns each robot a priority and plans paths sequentially — higher-priority robots plan first, and lower-priority robots treat higher-priority robot trajectories as moving obstacles. Centralized planning treats the multi-robot system as a single composite robot with combined degrees of freedom — complete but scaling exponentially. Conflict-based search (CBS), developed by Sharon et al., interleaves low-level single-robot planners with a high-level constraint tree, finding optimal multi-robot paths while exploring only a fraction of the joint configuration space (Sharon, Stern, Felner, and Sturtevant, “Conflict-Based Search for Optimal Multi-Agent Path Finding,” AI Journal, vol. 219, pp. 40–66, 2015).
FAQ
Which path planning algorithm should I use for a mobile robot?
For 2D navigation on a known map, A* on a costmap provides fast, optimal paths. For unknown or dynamic environments, D* Lite or RRT* with replanning adapts to changing obstacle configurations. For robots with kinematic constraints, Hybrid A* or state-lattice planning is preferred.
What is the difference between path planning and trajectory planning?
Path planning computes a geometric path (a sequence of configurations) without considering time. Trajectory planning assigns a time parameterization to the path, respecting velocity, acceleration, and torque limits.
How do sampling-based planners guarantee completeness?
RRT and PRM are probabilistically complete: as the number of samples approaches infinity, the probability of finding a feasible path (if one exists) approaches 1. Resolution-complete methods like A* on a grid guarantee completeness relative to the grid resolution.
Can path planning run in real time on a robot?
Two-dimensional A* on typical costmap sizes (100x100 to 500x500 cells) runs in milliseconds on embedded hardware. Sampling-based planners for high-DOF robots typically run at 1–10 Hz — adequate for replanning in slowly changing environments.
How do I handle dynamic obstacles during path execution?
Use a layered planner approach: a global planner computes a path around static obstacles, while a local planner (DWA, TEB, MPC) tracks the global path while reactively avoiding dynamic obstacles detected by onboard sensors.
Related: Study robot kinematics for understanding configuration spaces and degrees of freedom. Learn autonomous mobile robots for the full navigation stack integrating path planning. Explore robotics software architecture for implementing planners in ROS.