How Robots Navigate: Localization, Mapping, and Planning Explained

Every autonomous robot answers three questions in a loop: where am I, what does the world look like, and how do I get to the goal without hitting anything? Localization estimates pose, mapping builds a world model, and planning turns that model into collision-free motion. This guide covers the math, the sensor stack, and a working ROS2 Nav2 configuration you can copy into a project.

Quick Takeaways

• Localization fuses wheel odometry, an IMU, and LiDAR scan matching into a pose estimate (x, y, θ), typically with an EKF or particle filter.
• Mapping produces an occupancy grid (commonly 0.05 m/cell) with SLAM, or reuses a saved map for localization-only runs.
• Planning is two-tier: a global planner (A*, Dijkstra, Smac) finds a route, and a local planner/controller (DWB, MPPI, Pure Pursuit) tracks it at 20 Hz or faster while avoiding obstacles.
• In ROS2, Nav2 wires all three together through nodes, topics, TF transforms, and behavior trees.

The Navigation Pipeline

Data flows in one direction, but the layers depend on each other. Planning needs a map. Mapping needs a pose. Localization needs a map to correct drift. SLAM exists to break that circular dependency by solving mapping and localization at once.

Sensors (LiDAR, IMU, encoders, camera)
        │
        ▼
State Estimation (EKF)  ──►  odom → base_link TF
        │
        ▼
SLAM / AMCL            ──►  map → odom TF + /map
        │
        ▼
Global Planner         ──►  /plan
        │
        ▼
Local Controller       ──►  /cmd_vel  ──►  Motor drivers

Localization: Estimating Pose

Kinematic Foundations: Differential Drive Odometry

Dead reckoning integrates wheel encoder ticks into pose. For a differential-drive robot with wheel radius r, track width L, and wheel angular velocities ωL, ωR:

v   = r * (ωR + ωL) / 2          # linear velocity (m/s)
ω   = r * (ωR - ωL) / L          # angular velocity (rad/s)

x_k+1  = x_k  + v * cos(θ_k) * Δt
y_k+1  = y_k  + v * sin(θ_k) * Δt
θ_k+1  = θ_k  + ω * Δt

Odometry drifts without bound. Wheel slip, uneven floors, and encoder quantization all accumulate. A typical indoor robot sees 1–5% distance error, so a 20 m run can be off by up to a meter. Absolute references from LiDAR or visual landmarks are mandatory.

Sensor Comparison

Sensor Fusion With the Extended Kalman Filter

The EKF predicts state with a motion model, then corrects with measurements. State vector x = [x, y, θ, v, ω]ᵀ.

Predict:
  x̂_k|k-1 = f(x̂_k-1, u_k)
  P_k|k-1 = F_k · P_k-1 · F_kᵀ + Q

Update:
  K_k     = P_k|k-1 · Hᵀ · (H · P_k|k-1 · Hᵀ + R)⁻¹
  x̂_k     = x̂_k|k-1 + K_k · (z_k - h(x̂_k|k-1))
  P_k     = (I - K_k · H) · P_k|k-1

Q is process noise (how much you distrust the motion model). R is measurement noise (how much you distrust the sensor). Tuning these two matrices is the real work. Large R for the IMU yaw smooths heading but lags real turns.

ROS2 uses the robot_localization package for this. Config for wheel odometry plus IMU:

# ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0              # Hz, filter update rate
    two_d_mode: true             # Ignore z, roll, pitch for ground robots
    publish_tf: true             # Publishes odom -> base_link
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom            # odom for continuous, drift-prone local estimate

    odom0: /wheel/odometry
    # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,   # fuse forward velocity only
                   false, false, true,    # fuse yaw rate
                   false, false, false]

    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,     # fuse absolute yaw
                  false, false, false,
                  false, false, true,     # fuse yaw rate
                  true,  false, false]    # fuse forward acceleration
    imu0_remove_gravitational_acceleration: true

Particle Filter Localization (AMCL)

When you already have a map, AMCL (Adaptive Monte Carlo Localization) tracks hundreds to thousands of pose hypotheses (particles). Each particle is weighted by how well the current LiDAR scan matches the map from that pose, then resampled.

Mapping: Building the World Model

Occupancy Grids

An occupancy grid divides space into cells, each holding the probability that it is occupied. Updates use log-odds so each scan is a simple addition:

l(m_i | z_1:t) = l(m_i | z_1:t-1) + inv_sensor(m_i, z_t, x_t) - l_0

where  l(p) = log( p / (1 - p) )

Cells along a LiDAR beam get a negative increment (free). The cell at the beam endpoint gets a positive increment (occupied). In ROS, values are published as int8: 0 (free), 100 (occupied), -1 (unknown).

Resolution trades memory for detail. At 0.05 m/cell, a 100 m × 100 m floor needs 4 million cells, which fits easily in RAM. At 0.01 m/cell it needs 100 million cells, which usually does not.

SLAM Approaches

Pose-graph SLAM treats poses as nodes and relative-motion constraints as edges. Loop closure adds an edge between non-consecutive poses when the robot recognizes a revisited place. The optimizer then minimizes:

x* = argmin Σ_ij  e_ij(x_i, x_j)ᵀ · Ω_ij · e_ij(x_i, x_j)

e_ij is the residual between the measured and predicted relative pose. Ω_ij is the information matrix (inverse covariance). Solvers like Ceres or g2o run this sparse nonlinear least-squares problem.

Running SLAM Toolbox in ROS2

# Install
sudo apt install ros-$ROS_DISTRO-slam-toolbox

# Launch online asynchronous mapping with your params
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=/path/to/mapper_params_online_async.yaml

# Drive the robot with teleop while mapping
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Save the finished map (writes my_map.pgm + my_map.yaml)
ros2 run nav2_map_server map_saver_cli -f my_map

Key parameters in mapper_params_online_async.yaml:

slam_toolbox:
  ros__parameters:
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping                  # or "localization"
    resolution: 0.05               # meters per cell
    max_laser_range: 12.0          # match your LiDAR spec (m)
    minimum_travel_distance: 0.2   # m moved before processing a new scan
    minimum_travel_heading: 0.2    # rad turned before processing a new scan
    do_loop_closing: true
    loop_search_maximum_distance: 3.0

Path Planning: From Map to Motion

Global Planning

The global planner searches the cost map for a route from start to goal. A* adds a heuristic to Dijkstra’s search:

f(n) = g(n) + h(n)

g(n) = cost from start to node n
h(n) = estimated cost from n to goal (Euclidean or octile distance)

With an admissible heuristic (never overestimates), A* returns an optimal path while expanding far fewer nodes than Dijkstra.

Costmaps

Nav2 maintains two costmaps. The global costmap covers the whole map. The local costmap is a rolling window (commonly 3 m × 3 m) centered on the robot. Each is built from layers: static map, obstacle layer (live sensor data), and inflation layer.

Inflation assigns exponentially decaying cost around obstacles:

cost(d) = (INSCRIBED_COST - 1) * exp(-cost_scaling_factor * (d - inscribed_radius))

d is the distance to the nearest obstacle. A higher cost_scaling_factor makes the robot hug obstacles less tightly.

Local Planning and Control

The local controller turns the global path into velocity commands. DWB samples (v, ω) pairs inside the dynamic window, simulates each trajectory forward over a short horizon, and scores them:

J(v, ω) = α · heading(v, ω) + β · clearance(v, ω) + γ · velocity(v, ω)

The dynamic window limits candidates to what the motors can reach within one control cycle:

v ∈ [v_current - a_max·Δt,  v_current + a_max·Δt]
ω ∈ [ω_current - α_max·Δt,  ω_current + α_max·Δt]

Real-World Workflow: Autonomous Navigation With Nav2

This sequence takes a differential-drive robot from bare hardware to autonomous goal navigation.

Step 1: Verify the TF Tree

Nav2 requires this chain: map → odom → base_link → base_scan.

ros2 run tf2_tools view_frames      # generates frames.pdf
ros2 topic hz /scan                 # confirm LiDAR publishes (expect ~10 Hz)
ros2 topic hz /odom                 # confirm odometry (expect 30-50 Hz)

Step 2: Build and Save a Map

Run SLAM Toolbox, drive the robot through every area at under 0.3 m/s, close at least one loop, then save with map_saver_cli.

Step 3: Configure Nav2

Essential controller and planner settings in nav2_params.yaml:

controller_server:
  ros__parameters:
    controller_frequency: 20.0          # Hz, local control loop rate
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      min_vel_x: 0.0
      max_vel_x: 0.5                    # m/s
      max_vel_theta: 1.0                # rad/s
      acc_lim_x: 2.5                    # m/s^2
      acc_lim_theta: 3.2                # rad/s^2
      sim_time: 1.7                     # s, trajectory rollout horizon
      vx_samples: 20
      vtheta_samples: 20

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5                    # m, acceptable goal offset
      use_astar: true                   # A* instead of Dijkstra

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22                # m, measure your chassis
      inflation_layer:
        inflation_radius: 0.55
        cost_scaling_factor: 3.0

Step 4: Launch and Send a Goal

ros2 launch nav2_bringup bringup_launch.py \
  map:=/path/to/my_map.yaml \
  params_file:=/path/to/nav2_params.yaml \
  use_sim_time:=false

# Send a goal programmatically from the CLI
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}}"

In RViz2, set the initial pose with 2D Pose Estimate, then use Nav2 Goal. Watch the AMCL particle cloud collapse as the robot moves.

Step 5: Send Goals From Python

import rclpy
from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PoseStamped

rclpy.init()
nav = BasicNavigator()

# Seed AMCL with a starting pose
init = PoseStamped()
init.header.frame_id = 'map'
init.header.stamp = nav.get_clock().now().to_msg()
init.pose.position.x = 0.0
init.pose.position.y = 0.0
init.pose.orientation.w = 1.0       # quaternion w=1 means yaw of 0
nav.setInitialPose(init)
nav.waitUntilNav2Active()           # blocks until all lifecycle nodes are active

goal = PoseStamped()
goal.header.frame_id = 'map'
goal.header.stamp = nav.get_clock().now().to_msg()
goal.pose.position.x = 2.0
goal.pose.position.y = 1.0
goal.pose.orientation.w = 1.0
nav.goToPose(goal)

while not nav.isTaskComplete():
    fb = nav.getFeedback()          # includes distance_remaining in meters
    if fb:
        print(f"Remaining: {fb.distance_remaining:.2f} m")

print(nav.getResult())              # SUCCEEDED, CANCELED, or FAILED
rclpy.shutdown()

Tuning and Debugging Checklist

Test in simulation first. Gazebo with the TurtleBot3 packages reproduces the full stack and costs nothing when the robot crashes into a virtual wall.

FAQ

What is the difference between localization and SLAM?

Localization estimates the robot’s pose inside a map that already exists. SLAM builds the map and estimates the pose at the same time, which is required when no map is available.

Which path planning algorithm is best for mobile robots?

A* or Smac Hybrid-A* for global planning, paired with DWB, MPPI, or Regulated Pure Pursuit for local control. Use Hybrid-A* when the robot has a minimum turning radius, such as an Ackermann platform. Use A* for differential-drive robots.

Can a robot navigate without LiDAR?

Yes. Depth cameras, stereo cameras, and ultrasonic arrays can all feed obstacle layers, and visual SLAM such as ORB-SLAM3 or RTAB-Map can localize from images. LiDAR remains the most robust choice in low light and on featureless walls.

How accurate is robot localization?

A well-tuned 2D LiDAR system with AMCL typically holds 2–5 cm position error and 1–2° heading error indoors. RTK GNSS can reach 1–2 cm outdoors. Accuracy drops in long corridors and symmetric rooms where scans look identical.