Robot Localisation — Estimating Pose in a World
Localisation is the problem of determining a robot's pose (position x, y and heading θ) within a coordinate frame. Different methods trade accuracy, computational cost, and environmental assumptions:
Wheel odometry integrates encoder measurements of wheel rotation to estimate displacement. It is computationally cheap and requires no external sensors, but accumulates error over time (integration drift) due to wheel slip, uneven terrain, and encoder quantisation. Odometry provides the odom → base_link TF transform.
AMCL (Adaptive Monte Carlo Localisation) is a particle filter algorithm for localising a robot in a known occupancy-grid map. A set of N weighted particles represents the probability distribution over robot poses. At each step: particles are propagated according to the odometry motion model (with added noise), then reweighted by how well the LIDAR scan matches the map at each particle's pose. Low-weight particles are resampled (eliminated) and high-weight particles are duplicated — the distribution converges on the true pose. "Adaptive" refers to dynamically adjusting particle count for efficiency.
EKF/UKF sensor fusion (the robot_localization ROS 2 package) fuses multiple odometry and sensor sources (wheel odometry, IMU, GPS, visual odometry) into a single consistent pose estimate using an Extended or Unscented Kalman Filter. This handles the complementary nature of sensors: IMU is accurate over short time intervals (low drift) but drifts over long periods; GPS is accurate globally but noisy at high frequency.
SLAM simultaneously estimates the robot's pose AND builds the map — addressed in detail below.
Odometry Update Equations
Dead-reckoning pose update from wheel encoder measurements
x_{t+1} = x_t + Δs · cos(θ_t + Δθ/2)
y_{t+1} = y_t + Δs · sin(θ_t + Δθ/2)
θ_{t+1} = θ_t + Δθ
Δs = (Δs_R + Δs_L) / 2
Δθ = (Δs_R - Δs_L) / bΔs: arc length of robot centre (mean of right and left wheel displacements); Δθ: change in heading; Δs_R, Δs_L: right and left wheel displacements from encoder counts × wheel circumference / ticks_per_rev; b: wheel baseline (distance between drive wheels)
This model uses the midpoint heading approximation (θ_t + Δθ/2), which is more accurate than the simpler forward-Euler integration (θ_t). Error accumulates quadratically with distance — odometry alone is insufficient for missions longer than ~10 m without loop closure or external correction.
SLAM — Simultaneous Localisation and Mapping
SLAM solves the chicken-and-egg problem: accurate mapping requires knowing your position, but accurate localisation requires a map. SLAM algorithms solve both problems jointly.
SLAM pipeline structure:
- Frontend: Processes raw sensor data to extract pose constraints. For LIDAR: scan matching (ICP — Iterative Closest Point, or NDT — Normal Distributions Transform). For cameras: feature extraction (ORB, SIFT) and tracking, or dense optical flow. The frontend produces a sequence of relative pose estimates between consecutive frames.
- Backend: Optimises a pose graph — a graph where nodes are robot poses at key timestamps and edges are relative pose constraints from the frontend (and loop closures). Backend optimisation (g2o, GTSAM, Ceres) finds the pose configuration that minimises all constraint residuals. This corrects accumulated drift globally when loop closures are detected.
Loop closure detection identifies when the robot has returned to a previously visited location (by matching current sensor data to a stored scan or image). A detected loop closure adds a long-range edge to the pose graph, dramatically reducing accumulated drift — the core innovation that makes long-duration SLAM viable.
ROS 2 SLAM packages implement complete SLAM systems as ROS nodes consuming sensor topics and publishing map + pose topics. slam_toolbox is the recommended 2D SLAM solution for mobile robots using a laser scanner; RTAB-Map supports 3D RGB-D SLAM for indoor environments; Cartographer (Google) provides high-quality lidar SLAM used in warehouse robots.
ROS 2 SLAM Packages
| Package | Sensor | Map Type | Loop Closure | Use Case |
|---|---|---|---|---|
| slam_toolbox | 2D LIDAR (/scan) | 2D occupancy grid (.pgm + .yaml) | Yes (scan context / geometric) | Indoor mobile robots, lifelong mapping, ROS 2 default SLAM |
| RTAB-Map | RGB-D or stereo camera (+ optional LIDAR) | 3D point cloud + 2D occupancy grid | Yes (bag-of-words visual) | Indoor 3D mapping, RGB-D robots (TurtleBot 3 Waffle) |
| Cartographer | 2D or 3D LIDAR | 2D or 3D probability grid | Yes (branch-and-bound scan matching) | Warehouse AGVs, high-accuracy floor mapping |
| ORB-SLAM3 | Monocular / stereo / RGB-D camera | Sparse 3D point map | Yes (DBoW2 visual vocabulary) | Vision-only SLAM, AR/VR, MAVs without LIDAR |
| LIO-SAM | 3D LIDAR + IMU (tightly coupled) | 3D point cloud | Yes (radius-search scan context) | Outdoor legged/wheeled robots, large-scale 3D mapping |
Nav2 — The ROS 2 Navigation Stack
Nav2 (Navigation2) is the ROS 2 framework for autonomous navigation of mobile robots. It implements the full stack from map serving to velocity commands:
Key Nav2 servers:
- map_server: serves a static occupancy grid map (loaded from .pgm + .yaml files) on the
/maptopic - amcl: localises the robot in the static map using the particle filter algorithm; subscribes to
/scanand/odom, publishesmap → odomTF transform and/amcl_pose - planner_server: computes a global path from current pose to goal pose. Default planners: NavFn (Dijkstra/A* on the costmap), Smac Planner (SE2 lattice / hybrid A* for Ackermann and differential robots)
- controller_server: tracks the global path using a local planner. Default controllers: DWB (Dynamic Window Approach — samples velocity commands, scores by trajectory quality); MPPI (Model Predictive Path Integral — GPU-accelerated, smoother trajectories, better performance in cluttered environments)
- smoother_server: post-processes the global path for smoother execution
- recoveries_server: handles failure recovery behaviours (spin in place, back up, clear costmap, wait)
- bt_navigator: orchestrates the whole pipeline using a configurable Behaviour Tree XML file
Costmaps represent the navigable space. The Nav2 costmap_2d package maintains layered 2D grids: static layer (from map_server), obstacle layer (inflates detected obstacles from /scan), inflation layer (pads obstacles by robot radius + safety margin), spatio-temporal voxel layer (3D obstacle tracking). Cells are FREE (0), INFLATED (1–252), LETHAL (253), or UNKNOWN (255).
Global Planner vs Local Planner
Global Planner (NavFn / Smac / A*)
- Planning horizon: full path from current pose to goal pose across the entire map
- Obstacle handling: static and known obstacles in the global costmap; does not react to dynamic obstacles
- Computational cost: higher — searches over entire costmap grid; runs once per goal or on significant map changes
- Replanning frequency: low — only recomputes when goal changes or path is blocked
- Output: a complete path (list of PoseStamped waypoints) from start to goal
- Algorithm: Dijkstra (NavFn), hybrid A* with kinematic constraints (Smac), or theta* for any-angle paths
- Limitation: does not account for moving obstacles; must be complemented by local planner
Local Planner (DWB / MPPI Controller)
- Planning horizon: short — plans 1–3 seconds ahead in a local window around the robot
- Obstacle handling: reacts to dynamic obstacles (people, moving objects) in the local costmap updated at sensor rate
- Computational cost: lower per query but runs at high frequency (10–50 Hz)
- Replanning frequency: high — recomputes velocity commands every control cycle
- Output: a single Twist velocity command (/cmd_vel) for the current timestep
- Algorithm: DWB samples velocity space and scores trajectories; MPPI samples thousands of trajectories on GPU
- Limitation: may get stuck in local minima (narrow corridors, dead-ends) — recovery behaviours handle these cases