Path Planning & Navigation
Knowing where you are (SLAM) and being able to move your joints (kinematics) aren't enough on their own. The robot also needs to decide how to get from here to there — efficiently, safely, without running into walls or people. Path planning is the field that solves this. From the shortest-path algorithm in your GPS to the motion planner in a robotic arm avoiding obstacles, it's everywhere.
1. Grid-Based Planning — A* and Dijkstra
The simplest approach: discretize space into a grid, mark cells as free or occupied, and run a graph search algorithm to find the shortest path from start to goal.
Dijkstra's algorithm
Explores all reachable cells in order of increasing distance from the start. Guaranteed to find the shortest path but explores in all directions — it wastes time checking cells that are far from the goal direction. Time complexity: O(V log V) where V is the number of cells. Works perfectly for small maps.
A* — Dijkstra with intelligence
A* adds a heuristic — an estimate of the remaining distance to the goal (usually straight-line distance). Instead of expanding the closest-to-start cell, it expands the cell with the lowest (distance-from-start + estimated-distance-to-goal). This focuses the search toward the goal, dramatically reducing computation. A* is the standard choice for 2D robot navigation on occupancy grid maps.
Occupancy grids in ROS 2
In ROS 2, the map is a nav_msgs/OccupancyGrid — a 2D grid where each cell has a value 0 (free), 100 (occupied), or -1 (unknown). The Nav2 planner uses this map and A* variants to compute paths. The global planner (NavFn or Smac Planner) finds the initial path; the local planner (DWB or TEB) executes it while dodging dynamic obstacles.
2. Sampling-Based Planning — RRT and PRM
Grid search works great in 2D, but robot arms operate in high-dimensional configuration spaces. A 6-DoF arm has 6 dimensions of joint angles — you can't build a grid over that. Sampling-based planners solve high-dimensional planning by randomly sampling the configuration space.
RRT (Rapidly-exploring Random Tree)
RRT builds a tree of collision-free configurations, starting at the start configuration. Each iteration: randomly sample a configuration, find the nearest tree node, extend the tree toward the sample (stopping if a collision is detected). Repeat until the tree reaches the goal. RRT explores the configuration space rapidly and naturally avoids obstacles. Time complexity is probabilistically complete — given enough time, it will find a solution if one exists.
RRT* — the optimal version
RRT finds a solution but not necessarily the shortest one. RRT* adds a "rewiring" step: after adding a new node, it checks if nearby nodes can be reached more efficiently through the new node and updates accordingly. With enough iterations, RRT* converges to the optimal path. Used in MoveIt 2 as one of the standard planners for robot arm trajectory planning.
PRM (Probabilistic Roadmap)
Good when you need to plan many paths in the same environment. PRM pre-builds a roadmap by randomly sampling configurations and connecting nearby collision-free configurations with edges. Then, for any start/goal pair, simply find the nearest roadmap nodes and run Dijkstra/A* on the roadmap. Very fast at query time; worth the upfront construction cost for manipulation arms in fixed workcells.
3. Local Planning — Real-Time Obstacle Avoidance
Global planners find a path on a static map. But the real world has dynamic obstacles — people walking, boxes being moved. Local planners handle this by computing velocity commands in real time based on the current sensor data.
DWA — Dynamic Window Approach
Samples possible velocity commands (linear + angular), simulates where each would take the robot in the next second, and scores them by: how close to the global path, how far from obstacles, and how fast. Pick the best-scoring velocity and execute it. Repeat every 100ms. DWA is the default local planner in many ROS robot bases.
TEB — Timed Elastic Band
Models the planned path as an elastic band that gets "pulled" toward the global path and "pushed" away from obstacles. Optimizes over time, velocity, and heading simultaneously. Better than DWA in tight spaces and around dynamic obstacles. Default in the ROS 2 Nav2 for holonomic robots.
4. Behavior Trees — High-Level Robot Behavior
Path planning handles getting from A to B. But what should a robot decide to do? Behavior trees are a hierarchical structure for encoding robot decision-making — when to navigate, when to wait, when to ask for help.
How behavior trees work
A behavior tree is a tree of nodes. Action nodes do something (navigate to waypoint, pick up object). Condition nodes check something (is battery > 20%? is the path clear?). Control nodes (Sequence, Selector, Parallel) determine how children execute. A Sequence runs children left-to-right, stopping if any fails. A Selector tries children until one succeeds. Combine these and you can encode complex reactive behavior.
BehaviorTree.CPP and Nav2
ROS 2 Nav2 uses behavior trees (via BehaviorTree.CPP library) for its high-level navigator logic. The default behavior tree navigates to a goal, but you can customize it to: retry navigation on failure, clear the costmap if stuck, call a recovery behavior, or chain multiple waypoints. BT is the modern replacement for finite state machines in robotics behavior specification.
Frequently Asked Questions
What is a costmap?
A costmap is a 2D grid where each cell has a "cost" for the robot to pass through. Occupied cells (walls, obstacles) have maximum cost (infinite — avoid). Cells near obstacles have inflated cost (penalized but passable). Free cells far from obstacles have low cost. The path planner naturally finds paths through low-cost regions, giving safe clearance from obstacles without hard constraints.
What is the difference between global and local planning?
The global planner computes a path on the full map from start to goal — done once, can take several hundred milliseconds. The local planner runs at 10–20Hz and computes motor commands for the immediate few seconds, reacting to dynamic obstacles the global planner didn't know about. They work in a hierarchy: global plan sets direction, local plan handles real-time execution.
How does path planning work for robot arms?
MoveIt 2 in ROS 2 handles this. The key difference from mobile robots: the "environment" is 3D Cartesian space, and collisions include both the robot's links hitting each other (self-collision) and hitting the environment. MoveIt maintains a collision-aware scene model and uses RRT/RRT* in configuration space to find collision-free joint trajectories.
Can robots plan in real time for dynamic environments?
Yes — this is called online or reactive planning. Techniques include: replanning (run A* again when obstacles change), MPPI (Model Predictive Path Integral — sample-based real-time optimizer), and trajectory optimization with moving constraints. Self-driving cars do real-time planning at 10–20Hz, replanning their entire trajectory every 50–100ms.
Frequently Asked Questions
What will I learn here?
This page covers the core concepts and techniques you need to understand the topic and progress confidently to the next lesson.
How should I use this page?
Start with the overview, then follow the section links to deepen your understanding. Use the table of contents on the right to jump to specific sections.
What should I read next?
Use the navigation below to continue to the next lesson or explore related topics.