Chapter 15: Capstone — Autonomous Navigation Pipeline

This chapter assembles every component developed in the preceding fourteen chapters into a complete autonomous navigation system. The mission: deploy the robot in an unknown room, have it build a map, navigate to a specified goal pose, and stop. All without human intervention after the start command.

15.1 System Architecture Overview

The navigation pipeline has four layers, running at different update rates:

Layer 4 — Mission Control      (0.5 Hz)   Behaviour Tree: mission phases
Layer 3 — Planning             (1–2 Hz)   A* / RRT on occupancy grid
Layer 2 — Localisation         (10 Hz)    EKF: odometry + sensor fusion
Layer 1 — Control              (50 Hz)    PID: wheel speed + heading

Each layer reads from the layer below and sends commands down. Sensor data flows up. The architecture is concurrent: Python threads or asyncio tasks run each layer at its own rate.

15.2 Component Integration Map

15.3 Startup Sequence

config   = RobotConfig()
drive    = DifferentialDrive(config)
odometry = OdometryTracker(config)
ekf      = EKFLocaliser(config)
grid     = OccupancyGrid(5.0, 5.0, resolution_m=0.05)
planner  = AStarPlanner(grid)
camera   = CameraPerception()
mission  = MissionBT(drive, ekf, grid, planner, camera)

try:
    mission.run()
finally:
    drive.stop()

15.4 The Mission Behaviour Tree

The top-level BT (Chapter 13 architecture instantiated):

Sequence [MISSION]
├── Action: hardware_self_test
├── Action: initial_spin_scan          # rotate 360°, build first map estimate
├── Fallback [reach_goal]
│   ├── Sequence [planned_navigation]
│   │   ├── Condition: map_sufficient
│   │   ├── Action: plan_path_to_goal
│   │   └── Action: execute_path
│   └── Action: explore_and_map        # wander reactively, build map
└── Action: report_goal_reached

map_sufficient checks whether the occupancy grid has enough free-space information around the robot and goal to support planning. If not, the robot falls back to explore_and_map — a Bug-1-style wall-following behaviour that simultaneously builds the occupancy grid.

15.5 Path Execution with Local Avoidance

execute_path steps through A* waypoints. At each step it checks the ultrasound for new obstacles. If an obstacle appears ahead:

1. Stop.
2. Update the occupancy grid with the new obstacle.
3. Replan from the current position.
4. Resume.

If replanning fails (the goal is now blocked), escalate to the BT parent (Fallback), which falls back to explore_and_map to find an alternative route.

The blended velocity command combines path-following and reactive avoidance:

v_path, omega_path  = path_follower.command(ekf.pose, next_waypoint)
v_react, omega_react = potential_field(ekf.pose, scan, goal)

# Weight by obstacle proximity
alpha = obstacle_proximity_weight(scan)   # 0 = far, 1 = close
v     = (1 - alpha) * v_path  + alpha * v_react
omega = (1 - alpha) * omega_path + alpha * omega_react
drive.move(v, omega)

This blending ensures smooth transitions: when obstacles are distant, path following dominates; when they are close, reactive avoidance takes over.

15.6 Concurrency and Timing

The Python implementation uses two threads:

• Sensor thread (50 Hz): reads encoders, updates odometry, runs EKF predict step. On ArUco detection (10 Hz sub-rate), runs EKF update.
• Navigation thread (10 Hz): reads EKF pose, takes ultrasound scan, updates grid, checks BT, computes velocity command.
• Control thread (50 Hz): reads velocity command from navigation thread, applies to drive.

A threading.Lock protects the shared EKF pose and occupancy grid from concurrent access.

15.7 Performance Benchmarks

On a Raspberry Pi 4 (4 GB), with a 5 m × 4 m room:

• Occupancy grid (100×80 cells at 0.05 m): update from 19-beam scan in ~2 ms.
• A* on 100×80 grid: ~15 ms.
• EKF predict + update: ~0.5 ms.
• Camera frame capture + ArUco detection: ~25 ms (in background thread).
• Total navigation loop: ~20 ms → 50 Hz achievable for control; 10 Hz for planning.

15.8 Known Limitations and Extensions

Limitation: 2-D flat-world assumption. The entire system assumes a flat floor. Ramps, thresholds, and multi-floor environments require 3-D maps.

Limitation: single ultrasound. The HC-SR04 sees only one direction per measurement. A ring of sensors (or a lidar) would give much richer map updates.

Extension: dynamic obstacle tracking. Track moving obstacles (people walking by) as separate dynamic objects rather than incorporating them into the static map.

Extension: multi-goal missions. Replace the single-goal BT with a waypoint sequence. This is a simple BT modification.

Extension: probabilistic lane following. Replace the line-following PID with a particle filter that tracks the line position, handling line loss and intersections probabilistically.

15.9 Putting It All Together

The robot now has everything it needs:

• A motor control layer that reliably executes velocity commands (Chapters 2–4).
• An odometry and EKF layer that tracks its pose (Chapters 5, 14).
• A sensor layer that perceives lines, obstacles, and landmarks (Chapters 6–8).
• A planning layer that finds collision-free paths (Chapters 9–11).
• A mapping layer that builds and maintains a world model (Chapter 12).
• A behaviour layer that sequences the mission and handles failures (Chapter 13).

Each layer is individually testable (the code/ directory contains unit tests for every module) and replaceable: swap the ultrasound for a lidar, add a GPS module, replace A* with RRT* — the interfaces remain the same.

Robotics is an iterative discipline. The first deployment rarely goes as planned. But with this architecture, failures are diagnosable: add a topic logger to each layer, replay the sensor data, and identify where the estimate, plan, or execution diverged from expectation. That is the engineering cycle that turns a prototype into a reliable system.

Good luck, and drive safely.

Navigation:

• Previous: Chapter 14 – Sensor Fusion Basics
• Table of Contents