Skip to main content

AI Assistant

Physical AI & Humanoid Robotics

Hello! I'm your AI assistant for the AI-Native Guide to Physical AI & Humanoid Robotics. How can I help you today?

04:57 AM

Topic 2 — Planning & Navigation Systems

Navigation is the backbone of autonomy. This topic explains how navigation stacks (such as ROS 2 Nav2) combine mapping, localization, path planning, and control to move a humanoid robot through complex environments. You will learn the difference between global and local planners, how to define waypoint-based missions, and how to connect navigation to the rest of your agent architecture.

2.1 The Navigation Stack: From Maps to Motion​

A typical navigation stack consists of:

  • Map: Representation of the environment.
  • Localization: Estimate of robot pose within the map.
  • Global Planner: Computes a route from start to goal using the map.
  • Local Planner / Controller: Converts the global route into safe, smooth motion commands.
  • Recovery Behaviors: Handle stuck or failure states (e.g., clearing costmaps, backing up).

In ROS 2 Nav2, these are composed as:

  • Nodes and plugins managed by a behavior tree.
  • Actions and services for starting/stopping navigation.

The stack continuously:

  1. Receives goal poses (target locations).
  2. Uses localization and maps to compute a global path.
  3. Uses sensor data to adjust motion locally and avoid dynamic obstacles.
  4. Commands velocity or trajectory setpoints to low-level controllers.

2.2 Global vs Local Planning​

Global Planning​

  • Operates on the full map (e.g., 2D occupancy grid).
  • Finds a path from current pose to goal pose.
  • Algorithms often used:
    • Dijkstra’s algorithm.
    • A* and variants.
    • Sampling-based planners for more complex spaces.

Properties:

  • Produces a sequence of waypoints or a continuous path.
  • Assumes the map is mostly static.

Local Planning (Local Control)​

  • Operates on a local window around the robot.
  • Uses:
    • Sensor data (LiDAR, depth, camera).
    • Local costmaps (short-range obstacles).
  • Responsibilities:
    • Track the global path.
    • Avoid newly seen obstacles.
    • Respect dynamic constraints (max speed, acceleration).

Common approaches:

  • Dynamic window approaches.
  • Model predictive control (MPC).
  • Sampling-based local planners.

Dynamic Replanning​

Real environments change:

  • People cross paths.
  • Doors open and close.
  • Furniture moves.

The stack must:

  • Recompute global paths when the map changes significantly.
  • Continuously update local plans based on live sensor data.

2.3 Waypoint Missions and Room Traversal​

Beyond single-goal navigation, autonomous tasks often require:

  • Visiting a sequence of locations (waypoints).
  • Covering an area systematically (patrol or inspection).

Examples:

  • "Go from lab entrance → workstation → storage room → back to entrance."
  • "Visit all rooms on this floor and check for obstacles."

ROS 2 Actions for Missions​

You can model missions as:

  • A sequence of navigation goals sent to the navigation action server.
  • A higher-level mission node that:
    • Stores waypoint lists.
    • Sends goals one by one.
    • Handles failures (retries, skips, or aborts).

Inputs:

  • Waypoints defined in map coordinates.
  • Optionally loaded from configuration files (YAML/JSON).

Outputs:

  • Mission status (in progress, completed, failed).
  • Per-goal success/failure metrics.

2.4 Integration with SLAM and Perception​

Navigation requires:

  • A map (from SLAM in Chapter 4).
  • Accurate localization (pose estimates).
  • Up-to-date obstacle information (from perception).

Integration points:

  • Use SLAM-generated maps as the base map for global planning.
  • Feed perception outputs (dynamic obstacles, humans) into local costmaps.
  • Update navigation parameters depending on:
    • Environment type (narrow corridors vs open spaces).
    • Robot capabilities (step length, turning radius, speed limits).

2.5 Lab: Waypoint Navigation Mission​

This lab focuses on implementing a basic waypoint-based navigation mission.

Objectives​

  • Configure a navigation stack in simulation.
  • Define and execute multi-waypoint missions.
  • Monitor and log navigation performance.

Tasks​

  1. Setup
    • Use a SLAM-generated map from Chapter 4 or a known map in Gazebo.
    • Configure Nav2 (or similar) with:
      • Map server.
      • Localization (AMCL or SLAM pose).
      • Global and local planners.
      • Controller.
  2. Waypoint Definition
    • Select several key locations (rooms, doors, workstations).
    • Store them in a configuration file.
  3. Mission Node
    • Implement a ROS 2 node that:
      • Reads waypoints from the configuration.
      • Sends navigation goals sequentially via actions.
      • Waits for each goal to succeed/fail.
      • Implements simple retry and abort logic.
  4. Evaluation
    • Run several missions and record:
      • Success rate.
      • Time to complete.
      • Failure reasons (e.g., stuck, localization issues).

Deliverables​

  • Navigation configuration and map.
  • Mission node code.
  • Logs and a short report summarizing navigation performance.

This lab establishes navigation as a reliable service within your agent architecture, ready to be invoked by higher-level tasks and language-based commands.