Navigation & Path Planning
Week 11 • CMPSC 304 Robotic Agents
Press Space or → to advance
From Mapping to Navigation
Wednesday
SLAM & Mapping
→
Today
Navigation
→
P4 Parts 1-2
Map & Navigate
Wednesday: SLAM
- Built maps from lidar data
- Occupancy grids, scan matching
- Output: a map file
Today: Navigation
- Use the map to navigate
- Localization, path planning, obstacle avoidance
- Output: autonomous movement
The Navigation Problem
Given: a map and a goal position
Find: a safe path from the robot's current position to the goal
Execute: follow the path while avoiding obstacles
- Where am I? → Localization
- Where do I want to go? → Goal specification
- How do I get there? → Path planning
- How do I avoid obstacles? → Local planning / obstacle avoidance
Localization: Where Am I?
AMCL (Adaptive Monte Carlo Localization) is a particle filter that estimates the robot's position on a known map.
- Particles = many guesses of where the robot might be
- Each particle is a possible (x, y, θ) pose
- Compare each particle's expected lidar scan to the actual scan
- Good matches survive; poor matches are removed
- Over time, the cloud converges to the true pose
In RViz: Green arrows start spread out, then tighten as confidence increases.
AMCL: How Many Particles?
AMCL adapts its particle count between a minimum (when confident) and a maximum (when uncertain). More particles = more hypotheses tracked = more computation.
Few particles (confident)
- Particles cluster tightly
- Robot moves through a distinctive area
- Lidar scans match the expected map well
- Only a small pose neighborhood is tracked
Many particles (uncertain)
- Particles spread over a larger area
- Several map regions look similar
- Long featureless corridors are ambiguous
- Startup is uncertain until scans accumulate
Rule of thumb: Ambiguous scans require more particles.
Setting the Initial Pose
Why you must set the initial pose: AMCL doesn't know where the robot starts. You give it a hint by clicking in RViz.
- Click the "2D Pose Estimate" button in the RViz toolbar
- Click on the map where the robot is
- Drag to set the direction the robot is facing
- The particle cloud should jump to that location and then converge
What if you get it wrong? AMCL may still converge as the robot moves and gets more scan data — but it takes longer.
Costmaps: The Planner's View
A costmap is a grid where each cell has a cost — how expensive it is for the robot to be there.
| Cost | Meaning | Visual |
|---|
| 0 | Free space — safe to traverse | No color |
| 1–252 | Increasing cost (near obstacles) | Blue → red |
| 253 | Inscribed — robot center would hit obstacle | Cyan ring |
| 254 | Lethal — occupied cell | Purple/pink |
| 255 | Unknown | No data |
Inflation layer: Obstacles expand by inflation_radius; nearby cells get higher cost, so paths stay farther from walls.
Inflation: A Worked Example
Config: resolution: 0.05 m/cell | inflation_radius: 0.15 m
Buffer width: 0.15 ÷ 0.05 = 3 cells
Start with a 6×6 map with a wall on the left:
Raw map (# = wall)
0 1 2 3 4 5
0 # . . . . .
1 # . . . . .
2 # . . . . .
3 # . . . . .
After inflation (I = inflated)
0 1 2 3 4 5
0 # I I I . .
1 # I I I . .
2 # I I I . .
3 # I I I . .
4 # I I I . .
5 # I I I . .
Cols 1–3 are within 3 cells of col 0; cols 4–5 remain cost 0.
Key rule: If a free cell is within the buffer width (in cells) of the nearest obstacle, mark it inflated.
How the Planner Uses Cost
The global planner finds the minimum-total-cost path from start to goal.
Two routes, same length
0 1 2 3 4 5
0 . . . . . G
1 . I I . . .
2 . I # . . .
3 . I # . . .
4 . . . . . .
5 S . . . . .
Which path wins?
- Route A (left, through
I cells): high cost
- Route B (right, cost-0 cells): low cost
- Planner picks Route B
This is why paths in RViz sometimes curve away from walls even when a straight line would work geometrically.
Global vs. Local Costmap
Global Costmap
- Covers the entire map
- Based on the static map you saved
- Updated slowly (mostly static)
- Used for long-range planning
Local Costmap
- Small area around the robot
- Updated in real-time with lidar
- Includes dynamic obstacles
- Used for immediate decisions
Why two costmaps? The global costmap gives the big-picture route. The local costmap handles moment-to-moment driving.
Global Path Planning
The global planner finds a path from the robot's current position to the goal on the global costmap.
- Treats the costmap as a graph — each cell is a node, edge weights = cell costs
- Explores cells and builds up the lowest-cost route to the goal
- High-cost (inflated) cells are allowed but penalized, so cheaper routes are preferred
- If inflation completely fills a corridor, no path can pass through it
In RViz: The path often curves away from walls because inflation pushes it toward lower-cost cells.
robot_radius and Safety
The costmap uses robot_radius to set how close the robot center can approach walls. If it mismatches hardware, planning fails.
robot_radius too small
- Planner thinks the robot is smaller than it is
- Plans gaps the real robot cannot fit through
- Collision risk increases near walls
- Common when reusing configs across robots
robot_radius too large
- Planner thinks the robot is larger than it is
- Avoids passages the robot could actually navigate
- May fail to find any path in tight environments
- Safe, but can fail unnecessarily
Together: robot_radius sets the inscribed collision zone; inflation_radius adds extra clearance.
Reaching the Goal: Tolerance
tolerance = how close robot must get before Nav2 declares succeeded.
- Large (1.0 m): Fast arrival, imprecise — may stop in wrong places
- Small (0.02 m): Very precise, but odometry drift → oscillation or timeout
- Rule of thumb: General nav → 0.3–0.5 m. Precise positioning → smaller, expect more failures
Recovery behaviors trigger when goal can't be reached (including tight tolerance). Your code must handle goal failure gracefully.
Path Following: Local Planner
The local planner (controller) generates velocity commands to follow the global path while reacting to real-time sensor data.
- Nav2 uses DWB (Dynamic Window Based) controller by default
- Simulates many velocity commands forward in time, scores each trajectory
- Picks the best-scoring trajectory and sends velocity commands
Global
Path
→
Local
Planner
→
/cmd_vel
→
Robot
Moves
Nav2: The Full Picture
Nav2 = the ROS2 navigation framework combining all components.
| Component | Role | Node |
|---|
| Map Server | Loads/serves static map | map_server |
| AMCL | Localizes robot on map | amcl |
| Global Costmap | Full-map cost grid | global_costmap |
| Local Costmap | Real-time local cost grid | local_costmap |
| Planner Server | Global path planning | planner_server |
| Controller Server | Local path following | controller_server |
| BT Navigator | Coordinates everything | bt_navigator |
Activity 7: Parameter Cheat Sheet
For Q5–Q8 — focus on what changes behavior, not memorizing defaults.
| Parameter | Controls | If Increased |
|---|
min/max_particles | AMCL confidence vs compute | More robust, slower |
odom_alpha1/2 | Assumed odom noise | AMCL trusts odom less |
pf_err | Target localization error | Fewer particles needed |
use_astar | A* vs Dijkstra | Often faster planning |
tolerance | Goal reached distance | Easier completion, less precise |
desired_linear_vel | Speed cap | Faster, less control margin |
lookahead_dist | Controller look-ahead | Smoother, may overshoot turns |
max_angular_vel | Turn-rate cap | Quicker turns, oscillation risk |
Rule of thumb: Tune speed, lookahead, and tolerance together for the robot platform.
Sending Navigation Goals
Two ways to send goals:
RViz (Manual)
- Click "Nav2 Goal" button
- Click destination, drag to set heading
- Robot plans and drives
Great for testing
Code (Programmatic)
- Use
NavigateToPose action
- Send goals from a Python node
- Chain multiple waypoints
Required for P4 Part 3
What Happens When Things Go Wrong?
Nav2 has built-in recovery behaviors for when the robot gets stuck.
- Clear costmap: Remove ghost obstacles from old sensor data
- Spin: Rotate in place for new sensor readings
- Back up: Drive backwards from tight space
- Wait: Pause for a dynamic obstacle to move
If all fail: Nav2 aborts the goal. Your code must handle goal failure — this is common with physical robots.
Using Nav2 with TurtleBot3
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py # Terminal 1
ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=$HOME/map.yaml # Terminal 2
- Wait for Nav2 to start ("Creating bond timer" in terminal)
- Set initial pose with "2D Pose Estimate" in RViz
- Send a goal with "Nav2 Goal" in RViz
Important: Use the map you built in the SLAM exercise. Nav2 needs a pre-built map.
Summary
| Concept | What It Does |
|---|
| AMCL | Particle filter — localizes robot on a known map |
| Costmap | Grid with obstacle costs + inflation for safe planning |
| Global Planner | Optimal path on full map (Dijkstra/A*) |
| Local Planner | Follows path reactively, avoids dynamic obstacles |
| Nav2 | Framework combining all navigation components |
| Recovery | Built-in behaviors when robot gets stuck |
Next: Activity 7 — analyze costmap inflation and Nav2 parameters