By Chapter 20, students have already studied mobile kinematics, odometry, probabilistic localization, mapping, SLAM, local planning, dynamic obstacle handling, outdoor effects, and benchmarking. The capstone now combines these components into a single autonomous mission. The first task is not algorithm design; it is problem specification. A weak problem definition creates misleading results even if the algorithms are excellent.

The capstone mission will be defined by: \( \mathcal{P} = (\mathcal{W}, \mathcal{R}, \mathcal{S}, \mathcal{M}, \mathcal{C}) \), where \( \mathcal{W} \) is the workspace/environment, \( \mathcal{R} \) the robot dynamics/limits, \( \mathcal{S} \) the sensor suite, \( \mathcal{M} \) the mission objectives, and \( \mathcal{C} \) the evaluation constraints.

A capstone lesson should be executable by another student with no hidden assumptions. Therefore every quantity that affects behavior must be explicit: map resolution, frame conventions, time budget, collision policy, and random seed.

The project definition is a design pipeline. We move from a verbal mission statement to a constrained optimization problem and then to a software configuration.

flowchart TD
  A["Mission narrative"] --> B["State success criteria"]
  B --> C["Define workspace and map assumptions"]
  C --> D["Specify robot limits and safety margin"]
  D --> E["Specify sensors and rates"]
  E --> F["Formalize objective and constraints"]
  F --> G["Choose random seed and scenario generator"]
  G --> H["Run environment validation checks"]
  H --> I["Freeze reproducible baseline for capstone"]
        

This flow prevents a common failure mode: tuning algorithms before deciding what “success” actually means.

Let the robot state be \( \mathbf{x}(t) = [x(t), y(t), \theta(t), \mathbf{z}(t)]^\top \), where \( \mathbf{z}(t) \) may include velocities, biases, or filter states from previous chapters. We define a mission horizon \( t \in [0, T] \) and a control signal \( \mathbf{u}(t) \in \mathcal{U} \).

The capstone objective is typically multi-objective (time, safety, energy, map quality). A weighted scalarization is:

\[ J = w_t T_{\text{nav}} + w_e E_{\text{used}} + w_r \sum_{k=0}^{N-1} \rho_k + w_m U_{\text{map}} \]

where \( T_{\text{nav}} \) is mission completion time, \( E_{\text{used}} \) is energy consumption, \( \rho_k \) is a risk proxy (e.g., proximity or dynamic-obstacle risk), and \( U_{\text{map}} \) is residual map uncertainty or coverage deficit.

The planning/control problem is then

\[ \min_{\mathbf{u}(t)} J \quad \text{subject to} \quad \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}, \mathbf{w}), \; \mathbf{y} = h(\mathbf{x}, \mathbf{v}) \]

\[ \mathbf{x}(0)=\mathbf{x}_0,\quad \mathbf{x}(T) \in \mathcal{G},\quad \mathbf{u}(t)\in\mathcal{U},\quad \mathbf{x}(t)\in\mathcal{X}_{\text{safe}} \]

Here \( \mathbf{w} \) and \( \mathbf{v} \) are process and measurement disturbances already introduced in probabilistic robotics chapters. In this lesson, we are not solving the optimization yet; we are defining all symbols and constraints so later lessons can be evaluated consistently.

Let the physical workspace be \( \mathcal{W} \subset \mathbb{R}^2 \). We partition it into free and occupied subsets:

\[ \mathcal{W} = \mathcal{W}_{\text{free}} \cup \mathcal{W}_{\text{occ}}, \qquad \mathcal{W}_{\text{free}} \cap \mathcal{W}_{\text{occ}} = \varnothing \]

In implementation, we use a grid map with cell size \( \Delta \) meters. The occupancy value for cell \( c_{ij} \) is a Bernoulli variable or log-odds value from Chapters 9 and 17. For capstone setup, we initialize a ground-truth occupancy and a separate traversability cost layer:

\[ O_{ij} \in \{0,1\}, \qquad T_{ij} \in [1,\infty] \]

where \( O_{ij}=1 \) marks occupied cells and \( T_{ij}=\infty \) indicates non-traversable cells.

We also define a map inflation radius for safety: \( r_{\text{infl}} = r_{\text{robot}} + r_{\text{margin}} \).

Proposition (Inflation Safety): If a path for a point robot lies entirely in the inflated-free grid \( \mathcal{W}^{(\text{infl})}_{\text{free}} \), then the corresponding disk robot of radius \( r_{\text{robot}} \) remains collision free in the original map, assuming inflation used radius \( r_{\text{infl}} \ge r_{\text{robot}} \).

Proof: Inflation removes every point whose distance to an obstacle is at most \( r_{\text{infl}} \). Therefore every point on the path has distance strictly greater than or equal to \( r_{\text{infl}} \) from \( \mathcal{W}_{\text{occ}} \). Since \( r_{\text{infl}} \ge r_{\text{robot}} \), the closed disk centered at the path point with radius \( r_{\text{robot}} \) does not intersect obstacles. Repeating this argument for all path points gives collision-free motion. \(\square\)

The capstone must commit to explicit frames from the beginning. We use: world frame \( \{W\} \), robot base frame \( \{B\} \), sensor frames \( \{L\} \) (LiDAR), \( \{I\} \) (IMU), and optionally \( \{G\} \) (GPS/ENU).

For any point measured in the LiDAR frame, \( \mathbf{p}^{L} \), the world-frame point is

\[ \mathbf{p}^{W} = \mathbf{T}_{WB}\mathbf{T}_{BL}\mathbf{p}^{L} \]

In 2D homogeneous coordinates for the mobile base:

\[ \mathbf{T}_{WB} = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix} \]

A configuration file must store \( \mathbf{T}_{BL} \) and \( \mathbf{T}_{BI} \); otherwise the later localization and mapping fusion will be inconsistent even if each subsystem works in isolation.

Timestamp alignment bound. If the robot speed is bounded by \( \|\mathbf{v}(t)\| \le v_{\max} \) and the sensor timestamp misalignment is \( |\delta t| \le \bar{\delta} \), then the spatial error due only to temporal misalignment is bounded by:

\[ \|\delta \mathbf{p}\| \le v_{\max}\bar{\delta} \]

This simple inequality is critical during environment setup because it converts a software timing requirement into a geometric error budget.

We define mission acceptance as a set of hard constraints plus soft performance goals. A typical capstone acceptance set is:

\[ \mathcal{C}_{\text{acc}} = \left\{ \text{reach goal},\; N_{\text{coll}} = 0,\; T_{\text{nav}} \le T_{\max},\; P(\text{success}) \ge p_{\min} \right\} \]

For reproducibility, every trial also records random seed, map instance ID, and sensor noise settings. This is the bridge from capstone execution to Chapter 19 benchmarking.

Battery-time feasibility. If the average electrical power is \( \bar{P} \) and available battery capacity is \( E_b \) (Wh), then available time is

\[ T_{\text{avail}} = 3600 \frac{E_b}{\bar{P}} \]

The mission is time-feasible at setup stage if \( T_{\text{avail}} \ge T_{\max} + T_{\text{reserve}} \).

Proof (necessary condition): The robot cannot execute longer than the energy-limited runtime under the assumed average power draw. Therefore any mission with \( T_{\max} + T_{\text{reserve}} \) greater than \( T_{\text{avail}} \) is infeasible regardless of navigation quality. Thus the inequality is a necessary precondition for a valid capstone environment. \(\square\)

A second practical condition links map resolution to corridor width \( w_c \). To preserve at least \( n_c \) cells across a corridor,

\[ \Delta \le \frac{w_c}{n_c} \]

Using \( n_c = 4 \) is a common minimum so inflation and costmap gradients remain meaningful.

The environment setup should be a deterministic pipeline. The same seed must recreate the same occupancy, traversability regions, and start/goal pair.

flowchart TD
  A["Read mission + robot + sensor config"] --> B["Generate occupancy grid from seed"]
  B --> C["Generate traversability/slip regions"]
  C --> D["Inflate obstacles by robot+margin"]
  D --> E["Sample start and goal in free space"]
  E --> F["Check distance and mission difficulty"]
  F --> G["Check battery and sensor-rate feasibility"]
  G --> H["Export frozen scenario files"]
        

Typical validation outputs at this stage:

• Grid dimensions and resolution
• Occupied-cell fraction (difficulty proxy)
• Mean finite traversability cost
• Start/goal Euclidean distance
• Battery time margin
• Sensor schedule pass/fail flags

A clean capstone uses a configuration schema rather than scattered constants. Let a configuration tuple be \( \boldsymbol{\eta} = (\eta_{\text{mission}}, \eta_{\text{robot}}, \eta_{\text{sensors}}, \eta_{\text{env}}) \). A schema validator enforces units and value ranges, for example:

\[ 0 < \Delta \le 1,\quad 0 \le \rho_{\text{obs}} \le 0.6,\quad f_{\text{imu}} \ge 50\text{ Hz},\quad f_{\text{wheel}} \ge 20\text{ Hz} \]

These are not universal constants; they are setup assumptions for the capstone baseline. They can later be changed for ablation or stress testing, but must be frozen for the reference run.

This lesson’s code pack implements such a schema in five environments (Python, C++, Java, MATLAB/Simulink, Mathematica) using the same parameter values and seed.

The following code blocks create a reproducible capstone environment, including occupancy and traversability maps, start/goal selection, and feasibility checks. File names follow your naming convention and are also included in the downloadable zip package.

9.1 Python Implementation — Chapter20_Lesson1.py

# Chapter20_Lesson1.py
"""
Chapter 20 - Lesson 1
Problem Definition and Environment Setup for a Full AMR Autonomy Capstone

This script builds a reproducible capstone environment configuration:
1) mission specification
2) occupancy + traversability grid generation
3) sensor noise / update-rate definitions
4) feasibility checks (battery, timing, map bounds)
5) export of a JSON scenario file for later lessons
"""

from __future__ import annotations
from dataclasses import dataclass, asdict
from typing import Dict, Tuple
import json
import numpy as np


@dataclass
class MissionSpec:
    name: str
    start_xytheta: Tuple[float, float, float]
    goal_xy: Tuple[float, float]
    max_time_s: float
    min_success_prob: float
    max_collisions: int
    map_resolution_m: float


@dataclass
class SensorSuite:
    lidar_range_m: float
    lidar_fov_deg: float
    lidar_rate_hz: float
    imu_rate_hz: float
    wheel_rate_hz: float
    gps_rate_hz: float
    gps_available: bool


@dataclass
class RobotLimits:
    radius_m: float
    v_max_mps: float
    w_max_radps: float
    battery_wh: float
    avg_power_w: float


@dataclass
class EnvironmentConfig:
    width_m: float
    height_m: float
    obstacle_density: float
    terrain_classes: int
    slip_regions: int
    seed: int


def create_occupancy_grid(cfg: EnvironmentConfig, resolution_m: float) -> np.ndarray:
    nx = int(np.ceil(cfg.width_m / resolution_m))
    ny = int(np.ceil(cfg.height_m / resolution_m))
    rng = np.random.default_rng(cfg.seed)
    occ = (rng.random((ny, nx)) < cfg.obstacle_density).astype(np.uint8)

    # Clear a margin around the map border for safe spawn/goal placement
    margin = max(2, int(np.ceil(0.8 / resolution_m)))
    occ[:margin, :] = 0
    occ[-margin:, :] = 0
    occ[:, :margin] = 0
    occ[:, -margin:] = 0
    return occ


def create_traversability_map(occ: np.ndarray, cfg: EnvironmentConfig) -> np.ndarray:
    rng = np.random.default_rng(cfg.seed + 10)
    # 1.0 = easy terrain, larger values = harder terrain
    trav = 1.0 + 0.5 * rng.random(occ.shape)

    # Add a few slip-prone regions (higher cost)
    h, w = occ.shape
    for _ in range(cfg.slip_regions):
        cx = rng.integers(low=0, high=w)
        cy = rng.integers(low=0, high=h)
        rad = rng.integers(low=max(3, min(h, w) // 20), high=max(4, min(h, w) // 8))
        yy, xx = np.ogrid[:h, :w]
        mask = (xx - cx) ** 2 + (yy - cy) ** 2 <= rad ** 2
        trav[mask] += 1.0 + 0.8 * rng.random()

    # Obstacles are non-traversable
    trav[occ == 1] = np.inf
    return trav


def place_start_and_goal(occ: np.ndarray, resolution_m: float, seed: int) -> Tuple[Tuple[float, float], Tuple[float, float]]:
    rng = np.random.default_rng(seed + 20)
    free = np.argwhere(occ == 0)
    if len(free) < 2:
        raise RuntimeError("Not enough free cells to place start and goal")

    # Try until distance is sufficiently long for a capstone run
    for _ in range(2000):
        i, j = rng.choice(len(free), size=2, replace=False)
        sy, sx = free[i]
        gy, gx = free[j]
        ds = np.hypot((gx - sx) * resolution_m, (gy - sy) * resolution_m)
        if ds >= 0.4 * max(occ.shape) * resolution_m:
            return (float(sx * resolution_m), float(sy * resolution_m)), (float(gx * resolution_m), float(gy * resolution_m))

    sy, sx = free[0]
    gy, gx = free[-1]
    return (float(sx * resolution_m), float(sy * resolution_m)), (float(gx * resolution_m), float(gy * resolution_m))


def battery_time_feasibility(robot: RobotLimits, mission: MissionSpec) -> Dict[str, float]:
    available_time_s = (robot.battery_wh / robot.avg_power_w) * 3600.0
    return {
        "available_time_s": available_time_s,
        "mission_time_s": mission.max_time_s,
        "time_margin_s": available_time_s - mission.max_time_s,
        "is_feasible": float(available_time_s >= mission.max_time_s),
    }


def sensor_schedule_check(sensors: SensorSuite) -> Dict[str, float]:
    # Basic consistency rules for later fusion and control
    checks = {
        "wheel_vs_control_ok": float(sensors.wheel_rate_hz >= 20.0),
        "imu_vs_fusion_ok": float(sensors.imu_rate_hz >= 50.0),
        "lidar_vs_localization_ok": float(sensors.lidar_rate_hz >= 5.0),
        "gps_flag": float(sensors.gps_available),
    }
    return checks


def export_scenario(path: str, mission: MissionSpec, sensors: SensorSuite, robot: RobotLimits,
                    env: EnvironmentConfig, occ: np.ndarray, trav: np.ndarray) -> None:
    payload = {
        "mission": asdict(mission),
        "sensors": asdict(sensors),
        "robot": asdict(robot),
        "environment": asdict(env),
        "occupancy_shape": list(occ.shape),
        "occupancy_occupied_fraction": float(np.mean(occ)),
        "traversability_finite_mean": float(np.mean(trav[np.isfinite(trav)])),
    }
    with open(path, "w", encoding="utf-8") as f:
        json.dump(payload, f, indent=2)


def main() -> None:
    env = EnvironmentConfig(
        width_m=60.0,
        height_m=40.0,
        obstacle_density=0.14,
        terrain_classes=3,
        slip_regions=4,
        seed=42,
    )

    robot = RobotLimits(
        radius_m=0.25,
        v_max_mps=1.2,
        w_max_radps=1.8,
        battery_wh=180.0,
        avg_power_w=85.0,
    )

    sensors = SensorSuite(
        lidar_range_m=15.0,
        lidar_fov_deg=270.0,
        lidar_rate_hz=10.0,
        imu_rate_hz=100.0,
        wheel_rate_hz=50.0,
        gps_rate_hz=5.0,
        gps_available=True,
    )

    # Initial dummy mission; start and goal are updated after map creation
    mission = MissionSpec(
        name="Capstone_AMR_IndoorOutdoor_Mix",
        start_xytheta=(0.0, 0.0, 0.0),
        goal_xy=(1.0, 1.0),
        max_time_s=1200.0,
        min_success_prob=0.90,
        max_collisions=0,
        map_resolution_m=0.2,
    )

    occ = create_occupancy_grid(env, mission.map_resolution_m)
    trav = create_traversability_map(occ, env)
    start_xy, goal_xy = place_start_and_goal(occ, mission.map_resolution_m, env.seed)

    mission = MissionSpec(
        name=mission.name,
        start_xytheta=(start_xy[0], start_xy[1], 0.0),
        goal_xy=goal_xy,
        max_time_s=mission.max_time_s,
        min_success_prob=mission.min_success_prob,
        max_collisions=mission.max_collisions,
        map_resolution_m=mission.map_resolution_m,
    )

    bt = battery_time_feasibility(robot, mission)
    sched = sensor_schedule_check(sensors)

    print("=== Capstone Mission Spec ===")
    print(mission)
    print("\n=== Battery / Time Feasibility ===")
    print(bt)
    print("\n=== Sensor Schedule Checks ===")
    print(sched)

    export_scenario("Chapter20_Lesson1_scenario.json", mission, sensors, robot, env, occ, trav)
    np.save("Chapter20_Lesson1_occupancy.npy", occ)
    np.save("Chapter20_Lesson1_traversability.npy", trav)
    print("\nFiles written: Chapter20_Lesson1_scenario.json, Chapter20_Lesson1_occupancy.npy, Chapter20_Lesson1_traversability.npy")


if __name__ == "__main__":
    main()

9.2 C++ Implementation — Chapter20_Lesson1.cpp

// Chapter20_Lesson1.cpp
// Chapter 20 - Lesson 1
// Problem Definition and Environment Setup (C++ version)

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <string>
#include <sstream>

struct MissionSpec {
    std::string name;
    double start_x, start_y, start_theta;
    double goal_x, goal_y;
    double max_time_s;
    double min_success_prob;
    int max_collisions;
    double map_resolution_m;
};

struct SensorSuite {
    double lidar_range_m;
    double lidar_fov_deg;
    double lidar_rate_hz;
    double imu_rate_hz;
    double wheel_rate_hz;
    double gps_rate_hz;
    bool gps_available;
};

struct RobotLimits {
    double radius_m;
    double v_max_mps;
    double w_max_radps;
    double battery_wh;
    double avg_power_w;
};

struct EnvironmentConfig {
    double width_m;
    double height_m;
    double obstacle_density;
    int slip_regions;
    unsigned int seed;
};

struct Grid2D {
    int rows;
    int cols;
    std::vector<int> occ;          // 0 free, 1 occupied
    std::vector<double> trav_cost; // inf for occupied

    int idx(int r, int c) const { return r * cols + c; }
};

Grid2D createEnvironment(const EnvironmentConfig& env, double res) {
    int cols = static_cast<int>(std::ceil(env.width_m / res));
    int rows = static_cast<int>(std::ceil(env.height_m / res));
    Grid2D g{rows, cols, std::vector<int>(rows * cols, 0), std::vector<double>(rows * cols, 1.0)};

    std::mt19937 gen(env.seed);
    std::uniform_real_distribution<double> uni(0.0, 1.0);

    for (int r = 0; r < rows; ++r) {
        for (int c = 0; c < cols; ++c) {
            bool occ = (uni(gen) < env.obstacle_density);
            g.occ[g.idx(r, c)] = occ ? 1 : 0;
            g.trav_cost[g.idx(r, c)] = 1.0 + 0.5 * uni(gen);
        }
    }

    int margin = std::max(2, static_cast<int>(std::ceil(0.8 / res)));
    for (int r = 0; r < rows; ++r) {
        for (int c = 0; c < cols; ++c) {
            if (r < margin || r >= rows - margin || c < margin || c >= cols - margin) {
                g.occ[g.idx(r, c)] = 0;
            }
        }
    }

    // Slip regions (increase traversability cost in circular patches)
    std::uniform_int_distribution<int> rr(0, rows - 1);
    std::uniform_int_distribution<int> cc(0, cols - 1);
    int rad_min = std::max(3, std::min(rows, cols) / 20);
    int rad_max = std::max(4, std::min(rows, cols) / 8);
    std::uniform_int_distribution<int> rad_dist(rad_min, std::max(rad_min, rad_max));
    for (int k = 0; k < env.slip_regions; ++k) {
        int cy = rr(gen);
        int cx = cc(gen);
        int rad = rad_dist(gen);
        for (int r = 0; r < rows; ++r) {
            for (int c = 0; c < cols; ++c) {
                int dx = c - cx;
                int dy = r - cy;
                if (dx * dx + dy * dy <= rad * rad) {
                    g.trav_cost[g.idx(r, c)] += 1.0 + 0.8 * uni(gen);
                }
            }
        }
    }

    for (int r = 0; r < rows; ++r) {
        for (int c = 0; c < cols; ++c) {
            if (g.occ[g.idx(r, c)] == 1) {
                g.trav_cost[g.idx(r, c)] = INFINITY;
            }
        }
    }

    return g;
}

bool pickStartGoal(const Grid2D& g, double res, unsigned int seed,
                   double& sx, double& sy, double& gx, double& gy) {
    std::vector<std::pair<int, int>> free_cells;
    for (int r = 0; r < g.rows; ++r) {
        for (int c = 0; c < g.cols; ++c) {
            if (g.occ[g.idx(r, c)] == 0) free_cells.push_back({r, c});
        }
    }
    if (free_cells.size() < 2) return false;

    std::mt19937 gen(seed + 20);
    std::uniform_int_distribution<size_t> pick(0, free_cells.size() - 1);

    for (int it = 0; it < 2000; ++it) {
        auto a = free_cells[pick(gen)];
        auto b = free_cells[pick(gen)];
        if (a == b) continue;
        double dx = (b.second - a.second) * res;
        double dy = (b.first - a.first) * res;
        double d = std::sqrt(dx * dx + dy * dy);
        if (d >= 0.4 * std::max(g.rows, g.cols) * res) {
            sy = a.first * res; sx = a.second * res;
            gy = b.first * res; gx = b.second * res;
            return true;
        }
    }
    return false;
}

double batteryAvailableTime(const RobotLimits& robot) {
    return (robot.battery_wh / robot.avg_power_w) * 3600.0;
}

void writeScenarioJson(const std::string& path, const MissionSpec& mission,
                       const SensorSuite& sensors, const RobotLimits& robot,
                       const EnvironmentConfig& env, const Grid2D& g) {
    double occ_count = 0.0;
    double trav_sum = 0.0, trav_n = 0.0;
    for (int i = 0; i < static_cast<int>(g.occ.size()); ++i) {
        if (g.occ[i] == 1) occ_count += 1.0;
        if (std::isfinite(g.trav_cost[i])) {
            trav_sum += g.trav_cost[i];
            trav_n += 1.0;
        }
    }

    std::ofstream f(path);
    f << std::fixed << std::setprecision(4);
    f << "{\n";
    f << "  \"mission\": {\n";
    f << "    \"name\": \"" << mission.name << "\",\n";
    f << "    \"start_xytheta\": [" << mission.start_x << ", " << mission.start_y << ", " << mission.start_theta << "],\n";
    f << "    \"goal_xy\": [" << mission.goal_x << ", " << mission.goal_y << "],\n";
    f << "    \"max_time_s\": " << mission.max_time_s << ",\n";
    f << "    \"min_success_prob\": " << mission.min_success_prob << ",\n";
    f << "    \"max_collisions\": " << mission.max_collisions << ",\n";
    f << "    \"map_resolution_m\": " << mission.map_resolution_m << "\n";
    f << "  },\n";
    f << "  \"sensors\": {\n";
    f << "    \"lidar_range_m\": " << sensors.lidar_range_m << ",\n";
    f << "    \"lidar_fov_deg\": " << sensors.lidar_fov_deg << ",\n";
    f << "    \"lidar_rate_hz\": " << sensors.lidar_rate_hz << ",\n";
    f << "    \"imu_rate_hz\": " << sensors.imu_rate_hz << ",\n";
    f << "    \"wheel_rate_hz\": " << sensors.wheel_rate_hz << ",\n";
    f << "    \"gps_rate_hz\": " << sensors.gps_rate_hz << ",\n";
    f << "    \"gps_available\": " << (sensors.gps_available ? "true" : "false") << "\n";
    f << "  },\n";
    f << "  \"robot\": {\n";
    f << "    \"radius_m\": " << robot.radius_m << ",\n";
    f << "    \"v_max_mps\": " << robot.v_max_mps << ",\n";
    f << "    \"w_max_radps\": " << robot.w_max_radps << ",\n";
    f << "    \"battery_wh\": " << robot.battery_wh << ",\n";
    f << "    \"avg_power_w\": " << robot.avg_power_w << "\n";
    f << "  },\n";
    f << "  \"environment\": {\n";
    f << "    \"width_m\": " << env.width_m << ",\n";
    f << "    \"height_m\": " << env.height_m << ",\n";
    f << "    \"obstacle_density\": " << env.obstacle_density << ",\n";
    f << "    \"slip_regions\": " << env.slip_regions << ",\n";
    f << "    \"seed\": " << env.seed << "\n";
    f << "  },\n";
    f << "  \"occupancy_shape\": [" << g.rows << ", " << g.cols << "],\n";
    f << "  \"occupancy_occupied_fraction\": " << (occ_count / g.occ.size()) << ",\n";
    f << "  \"traversability_finite_mean\": " << (trav_n > 0 ? trav_sum / trav_n : 0.0) << "\n";
    f << "}\n";
}

int main() {
    EnvironmentConfig env{60.0, 40.0, 0.14, 4, 42};
    RobotLimits robot{0.25, 1.2, 1.8, 180.0, 85.0};
    SensorSuite sensors{15.0, 270.0, 10.0, 100.0, 50.0, 5.0, true};

    MissionSpec mission{
        "Capstone_AMR_IndoorOutdoor_Mix",
        0.0, 0.0, 0.0,
        1.0, 1.0,
        1200.0,
        0.90,
        0,
        0.2
    };

    Grid2D grid = createEnvironment(env, mission.map_resolution_m);

    double sx = 0.0, sy = 0.0, gx = 1.0, gy = 1.0;
    bool ok = pickStartGoal(grid, mission.map_resolution_m, env.seed, sx, sy, gx, gy);
    if (!ok) {
        std::cerr << "Failed to pick start/goal in free space\n";
        return 1;
    }

    mission.start_x = sx; mission.start_y = sy; mission.start_theta = 0.0;
    mission.goal_x = gx; mission.goal_y = gy;

    double t_available = batteryAvailableTime(robot);
    std::cout << "Mission: " << mission.name << "\n";
    std::cout << "Start (x,y,th): " << mission.start_x << ", " << mission.start_y << ", " << mission.start_theta << "\n";
    std::cout << "Goal (x,y): " << mission.goal_x << ", " << mission.goal_y << "\n";
    std::cout << "Battery available time [s]: " << t_available << "\n";
    std::cout << "Mission required time [s]: " << mission.max_time_s << "\n";
    std::cout << "Time feasible? " << (t_available >= mission.max_time_s ? "YES" : "NO") << "\n";

    bool rates_ok = (sensors.wheel_rate_hz >= 20.0) && (sensors.imu_rate_hz >= 50.0) && (sensors.lidar_rate_hz >= 5.0);
    std::cout << "Sensor rate checks ok? " << (rates_ok ? "YES" : "NO") << "\n";

    writeScenarioJson("Chapter20_Lesson1_cpp_scenario.json", mission, sensors, robot, env, grid);
    std::cout << "Wrote Chapter20_Lesson1_cpp_scenario.json\n";

    return 0;
}

9.3 Java Implementation — Chapter20_Lesson1.java

// Chapter20_Lesson1.java
// Chapter 20 - Lesson 1
// Problem Definition and Environment Setup (Java version)

import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;

public class Chapter20_Lesson1 {

    static class MissionSpec {
        String name;
        double startX, startY, startTheta;
        double goalX, goalY;
        double maxTimeS;
        double minSuccessProb;
        int maxCollisions;
        double mapResolutionM;
    }

    static class SensorSuite {
        double lidarRangeM, lidarFovDeg, lidarRateHz;
        double imuRateHz, wheelRateHz, gpsRateHz;
        boolean gpsAvailable;
    }

    static class RobotLimits {
        double radiusM, vMaxMps, wMaxRadps;
        double batteryWh, avgPowerW;
    }

    static class EnvironmentConfig {
        double widthM, heightM;
        double obstacleDensity;
        int slipRegions;
        long seed;
    }

    static class Grid2D {
        int rows, cols;
        int[][] occ;
        double[][] trav;
    }

    static Grid2D createGrid(EnvironmentConfig env, double resolutionM) {
        Grid2D g = new Grid2D();
        g.cols = (int) Math.ceil(env.widthM / resolutionM);
        g.rows = (int) Math.ceil(env.heightM / resolutionM);
        g.occ = new int[g.rows][g.cols];
        g.trav = new double[g.rows][g.cols];

        Random rand = new Random(env.seed);
        for (int r = 0; r < g.rows; r++) {
            for (int c = 0; c < g.cols; c++) {
                g.occ[r][c] = (rand.nextDouble() < env.obstacleDensity) ? 1 : 0;
                g.trav[r][c] = 1.0 + 0.5 * rand.nextDouble();
            }
        }

        int margin = Math.max(2, (int) Math.ceil(0.8 / resolutionM));
        for (int r = 0; r < g.rows; r++) {
            for (int c = 0; c < g.cols; c++) {
                if (r < margin || r >= g.rows - margin || c < margin || c >= g.cols - margin) {
                    g.occ[r][c] = 0;
                }
            }
        }

        for (int k = 0; k < env.slipRegions; k++) {
            int cx = rand.nextInt(g.cols);
            int cy = rand.nextInt(g.rows);
            int rad = Math.max(4, Math.min(g.rows, g.cols) / 10);
            for (int r = 0; r < g.rows; r++) {
                for (int c = 0; c < g.cols; c++) {
                    int dx = c - cx;
                    int dy = r - cy;
                    if (dx * dx + dy * dy <= rad * rad) {
                        g.trav[r][c] += 1.0 + 0.8 * rand.nextDouble();
                    }
                }
            }
        }

        for (int r = 0; r < g.rows; r++) {
            for (int c = 0; c < g.cols; c++) {
                if (g.occ[r][c] == 1) g.trav[r][c] = Double.POSITIVE_INFINITY;
            }
        }

        return g;
    }

    static double[] chooseStartGoal(Grid2D g, double resolutionM, long seed) {
        Random rand = new Random(seed + 20);
        int attempts = 2000;
        while (attempts-- > 0) {
            int sy = rand.nextInt(g.rows);
            int sx = rand.nextInt(g.cols);
            int gy = rand.nextInt(g.rows);
            int gx = rand.nextInt(g.cols);
            if (g.occ[sy][sx] == 1 || g.occ[gy][gx] == 1) continue;
            double dist = Math.hypot((gx - sx) * resolutionM, (gy - sy) * resolutionM);
            if (dist >= 0.4 * Math.max(g.rows, g.cols) * resolutionM) {
                return new double[]{sx * resolutionM, sy * resolutionM, gx * resolutionM, gy * resolutionM};
            }
        }
        return new double[]{0.0, 0.0, resolutionM, resolutionM};
    }

    static double batteryAvailableTimeS(RobotLimits robot) {
        return (robot.batteryWh / robot.avgPowerW) * 3600.0;
    }

    static void writeScenarioJson(String path, MissionSpec m, SensorSuite s, RobotLimits r, EnvironmentConfig e, Grid2D g) throws IOException {
        int occCount = 0;
        double travSum = 0.0;
        int travN = 0;
        for (int i = 0; i < g.rows; i++) {
            for (int j = 0; j < g.cols; j++) {
                if (g.occ[i][j] == 1) occCount++;
                if (Double.isFinite(g.trav[i][j])) {
                    travSum += g.trav[i][j];
                    travN++;
                }
            }
        }

        try (FileWriter fw = new FileWriter(path)) {
            fw.write("{\n");
            fw.write("  \"mission\": {\n");
            fw.write("    \"name\": \"" + m.name + "\",\n");
            fw.write("    \"start_xytheta\": [" + m.startX + ", " + m.startY + ", " + m.startTheta + "],\n");
            fw.write("    \"goal_xy\": [" + m.goalX + ", " + m.goalY + "],\n");
            fw.write("    \"max_time_s\": " + m.maxTimeS + ",\n");
            fw.write("    \"min_success_prob\": " + m.minSuccessProb + ",\n");
            fw.write("    \"max_collisions\": " + m.maxCollisions + ",\n");
            fw.write("    \"map_resolution_m\": " + m.mapResolutionM + "\n");
            fw.write("  },\n");
            fw.write("  \"occupancy_shape\": [" + g.rows + ", " + g.cols + "],\n");
            fw.write("  \"occupancy_occupied_fraction\": " + ((double) occCount / (g.rows * g.cols)) + ",\n");
            fw.write("  \"traversability_finite_mean\": " + (travN > 0 ? travSum / travN : 0.0) + "\n");
            fw.write("}\n");
        }
    }

    public static void main(String[] args) throws IOException {
        EnvironmentConfig env = new EnvironmentConfig();
        env.widthM = 60.0;
        env.heightM = 40.0;
        env.obstacleDensity = 0.14;
        env.slipRegions = 4;
        env.seed = 42;

        RobotLimits robot = new RobotLimits();
        robot.radiusM = 0.25;
        robot.vMaxMps = 1.2;
        robot.wMaxRadps = 1.8;
        robot.batteryWh = 180.0;
        robot.avgPowerW = 85.0;

        SensorSuite sensors = new SensorSuite();
        sensors.lidarRangeM = 15.0;
        sensors.lidarFovDeg = 270.0;
        sensors.lidarRateHz = 10.0;
        sensors.imuRateHz = 100.0;
        sensors.wheelRateHz = 50.0;
        sensors.gpsRateHz = 5.0;
        sensors.gpsAvailable = true;

        MissionSpec mission = new MissionSpec();
        mission.name = "Capstone_AMR_IndoorOutdoor_Mix";
        mission.maxTimeS = 1200.0;
        mission.minSuccessProb = 0.90;
        mission.maxCollisions = 0;
        mission.mapResolutionM = 0.2;

        Grid2D grid = createGrid(env, mission.mapResolutionM);
        double[] sg = chooseStartGoal(grid, mission.mapResolutionM, env.seed);
        mission.startX = sg[0];
        mission.startY = sg[1];
        mission.startTheta = 0.0;
        mission.goalX = sg[2];
        mission.goalY = sg[3];

        double tAvail = batteryAvailableTimeS(robot);
        boolean timeFeasible = tAvail >= mission.maxTimeS;
        boolean ratesOk = sensors.wheelRateHz >= 20.0 && sensors.imuRateHz >= 50.0 && sensors.lidarRateHz >= 5.0;

        System.out.println("Mission: " + mission.name);
        System.out.println("Start: (" + mission.startX + ", " + mission.startY + ", " + mission.startTheta + ")");
        System.out.println("Goal: (" + mission.goalX + ", " + mission.goalY + ")");
        System.out.println("Battery available [s]: " + tAvail);
        System.out.println("Time feasible? " + timeFeasible);
        System.out.println("Sensor-rate checks ok? " + ratesOk);

        writeScenarioJson("Chapter20_Lesson1_java_scenario.json", mission, sensors, robot, env, grid);
        System.out.println("Wrote Chapter20_Lesson1_java_scenario.json");
    }
}

9.4 MATLAB / Simulink Helper — Chapter20_Lesson1.m

% Chapter20_Lesson1.m
% Chapter 20 - Lesson 1
% Problem Definition and Environment Setup (MATLAB / Simulink helper)
%
% This script:
% 1) Creates an occupancy/traversability environment
% 2) Defines a mission and robot/sensor parameters
% 3) Checks battery/time feasibility
% 4) Optionally builds a minimal Simulink model programmatically

clear; clc; close all;
rng(42);

%% Mission and platform specification
mission.name = 'Capstone_AMR_IndoorOutdoor_Mix';
mission.map_resolution_m = 0.2;
mission.max_time_s = 1200;
mission.min_success_prob = 0.90;
mission.max_collisions = 0;

robot.radius_m = 0.25;
robot.v_max_mps = 1.2;
robot.w_max_radps = 1.8;
robot.battery_wh = 180;
robot.avg_power_w = 85;

sensors.lidar_range_m = 15;
sensors.lidar_fov_deg = 270;
sensors.lidar_rate_hz = 10;
sensors.imu_rate_hz = 100;
sensors.wheel_rate_hz = 50;
sensors.gps_rate_hz = 5;
sensors.gps_available = true;

env.width_m = 60;
env.height_m = 40;
env.obstacle_density = 0.14;
env.slip_regions = 4;

%% Environment generation
nx = ceil(env.width_m / mission.map_resolution_m);
ny = ceil(env.height_m / mission.map_resolution_m);

occ = rand(ny, nx) < env.obstacle_density;
trav = 1.0 + 0.5 * rand(ny, nx);

margin = max(2, ceil(0.8 / mission.map_resolution_m));
occ(1:margin, :) = 0; occ(end-margin+1:end, :) = 0;
occ(:, 1:margin) = 0; occ(:, end-margin+1:end) = 0;

for k = 1:env.slip_regions
    cx = randi(nx); cy = randi(ny);
    rad = max(4, floor(min(nx, ny) / 10));
    [X, Y] = meshgrid(1:nx, 1:ny);
    mask = (X - cx).^2 + (Y - cy).^2 <= rad^2;
    trav(mask) = trav(mask) + 1.0 + 0.8 * rand;
end
trav(occ == 1) = inf;

%% Start and goal placement
free_idx = find(~occ);
assert(numel(free_idx) >= 2, 'Not enough free cells.');
picked = false;
for it = 1:2000
    a = free_idx(randi(numel(free_idx)));
    b = free_idx(randi(numel(free_idx)));
    [sy, sx] = ind2sub([ny, nx], a);
    [gy, gx] = ind2sub([ny, nx], b);
    d = hypot((gx - sx) * mission.map_resolution_m, (gy - sy) * mission.map_resolution_m);
    if d >= 0.4 * max(nx, ny) * mission.map_resolution_m
        picked = true;
        break;
    end
end
if ~picked
    [sy, sx] = ind2sub([ny, nx], free_idx(1));
    [gy, gx] = ind2sub([ny, nx], free_idx(end));
end

mission.start_xytheta = [sx * mission.map_resolution_m, sy * mission.map_resolution_m, 0];
mission.goal_xy = [gx * mission.map_resolution_m, gy * mission.map_resolution_m];

%% Feasibility checks
available_time_s = (robot.battery_wh / robot.avg_power_w) * 3600;
time_margin_s = available_time_s - mission.max_time_s;

sensor_rates_ok = (sensors.wheel_rate_hz >= 20) && ...
                  (sensors.imu_rate_hz >= 50) && ...
                  (sensors.lidar_rate_hz >= 5);

fprintf('Mission: %s\n', mission.name);
fprintf('Start = [%.2f, %.2f, %.2f]\n', mission.start_xytheta);
fprintf('Goal  = [%.2f, %.2f]\n', mission.goal_xy);
fprintf('Battery available time [s] = %.1f\n', available_time_s);
fprintf('Time margin [s] = %.1f\n', time_margin_s);
fprintf('Sensor rates OK? %d\n', sensor_rates_ok);

%% Visualize
figure('Name', 'Chapter20_Lesson1 Environment');
subplot(1,2,1);
imagesc(occ); axis image; title('Occupancy Grid (1 = obstacle)');
hold on; plot(sx, sy, 'go', 'MarkerSize', 8, 'LineWidth', 2);
plot(gx, gy, 'rx', 'MarkerSize', 8, 'LineWidth', 2); hold off;

subplot(1,2,2);
imagesc(trav); axis image; title('Traversability Cost');
colorbar;

%% Save outputs
save('Chapter20_Lesson1_env.mat', 'mission', 'robot', 'sensors', 'env', 'occ', 'trav');

%% Optional Simulink skeleton (programmatic)
% This creates a simple simulation skeleton with reference and kinematic integrator.
modelName = 'Chapter20_Lesson1_Simulink';
if exist(modelName, 'file') == 4
    close_system(modelName, 0);
end
new_system(modelName); open_system(modelName);

add_block('simulink/Sources/Constant', [modelName '/v_ref'], 'Value', '0.5', ...
    'Position', [30 50 80 80]);
add_block('simulink/Sources/Constant', [modelName '/w_ref'], 'Value', '0.2', ...
    'Position', [30 120 80 150]);
add_block('simulink/Continuous/Integrator', [modelName '/x_int'], ...
    'Position', [220 40 250 70]);
add_block('simulink/Continuous/Integrator', [modelName '/y_int'], ...
    'Position', [220 110 250 140]);
add_block('simulink/Continuous/Integrator', [modelName '/th_int'], ...
    'Position', [220 180 250 210]);
add_block('simulink/Sinks/Scope', [modelName '/scope'], ...
    'Position', [380 80 430 140]);

% NOTE: For a full nonholonomic model, students will add trig blocks and couplings in later lessons.
save_system(modelName);
fprintf('Saved %s.slx (skeleton model)\n', modelName);

9.5 Wolfram Mathematica — Chapter20_Lesson1.nb

(* Chapter20_Lesson1.nb *)
(* Chapter 20 - Lesson 1 *)
(* Problem Definition and Environment Setup (Wolfram Mathematica / Wolfram Language) *)

SeedRandom[42];

(* 1) Mission and platform parameters *)
mission = <|
   "Name" -> "Capstone_AMR_IndoorOutdoor_Mix",
   "ResolutionM" -> 0.2,
   "MaxTimeS" -> 1200,
   "MinSuccessProb" -> 0.90,
   "MaxCollisions" -> 0
|>;

robot = <|
   "RadiusM" -> 0.25,
   "Vmax" -> 1.2,
   "Wmax" -> 1.8,
   "BatteryWh" -> 180,
   "AvgPowerW" -> 85
|>;

sensors = <|
   "LidarRangeM" -> 15,
   "LidarFovDeg" -> 270,
   "LidarRateHz" -> 10,
   "ImuRateHz" -> 100,
   "WheelRateHz" -> 50,
   "GpsRateHz" -> 5,
   "GpsAvailable" -> True
|>;

env = <|
   "WidthM" -> 60,
   "HeightM" -> 40,
   "ObstacleDensity" -> 0.14,
   "SlipRegions" -> 4
|>;

nx = Ceiling[env["WidthM"]/mission["ResolutionM"]];
ny = Ceiling[env["HeightM"]/mission["ResolutionM"]];

(* 2) Occupancy and traversability grids *)
occ = Boole[RandomReal[{0, 1}, {ny, nx}] < env["ObstacleDensity"]];
trav = 1.0 + 0.5 RandomReal[{0, 1}, {ny, nx}];

margin = Max[2, Ceiling[0.8/mission["ResolutionM"]]];
occ[[1 ;; margin, All]] = 0;
occ[[ny - margin + 1 ;; ny, All]] = 0;
occ[[All, 1 ;; margin]] = 0;
occ[[All, nx - margin + 1 ;; nx]] = 0;

Do[
  cx = RandomInteger[{1, nx}];
  cy = RandomInteger[{1, ny}];
  rad = Max[4, Floor[Min[nx, ny]/10]];
  Do[
    If[(x - cx)^2 + (y - cy)^2 <= rad^2, trav[[y, x]] = trav[[y, x]] + 1.0 + 0.8 RandomReal[]],
    {y, 1, ny}, {x, 1, nx}
  ],
  {env["SlipRegions"]}
];

Do[If[occ[[y, x]] == 1, trav[[y, x]] = Infinity], {y, 1, ny}, {x, 1, nx}];

(* 3) Select start and goal from free cells *)
freeCells = Position[occ, 0];
pickGoodPair[] := Module[{a, b, d},
  Do[
    a = RandomChoice[freeCells];
    b = RandomChoice[freeCells];
    d = Norm[(Reverse[b] - Reverse[a]) mission["ResolutionM"]];
    If[a =!= b && d >= 0.4 Max[nx, ny] mission["ResolutionM"], Return[{a, b}]],
    {2000}
  ];
  {First[freeCells], Last[freeCells]}
];

{startCell, goalCell} = pickGoodPair[];
startXY = Reverse[startCell] mission["ResolutionM"];
goalXY = Reverse[goalCell] mission["ResolutionM"];

mission["StartXYTheta"] = {startXY[[1]], startXY[[2]], 0.0};
mission["GoalXY"] = goalXY;

(* 4) Feasibility checks *)
availableTimeS = (robot["BatteryWh"]/robot["AvgPowerW"]) * 3600.0;
timeMarginS = availableTimeS - mission["MaxTimeS"];
sensorRatesOK = sensors["WheelRateHz"] >= 20 && sensors["ImuRateHz"] >= 50 && sensors["LidarRateHz"] >= 5;

Print["Mission: ", mission["Name"]];
Print["Start = ", mission["StartXYTheta"]];
Print["Goal = ", mission["GoalXY"]];
Print["Available battery time [s] = ", availableTimeS];
Print["Time margin [s] = ", timeMarginS];
Print["Sensor rates OK? ", sensorRatesOK];

(* 5) Capstone objective prototype for later lessons *)
(* J = w_t * T + w_e * E + w_r * Sum[risk_i] + w_m * mapUncertainty *)
objective[T_, E_, riskSeq_, mapUnc_, w_] := w[[1]] T + w[[2]] E + w[[3]] Total[riskSeq] + w[[4]] mapUnc];

w = {1.0, 0.2, 4.0, 3.0};
exampleRisk = RandomReal[{0.0, 0.2}, 20];
Jex = objective[850, 120, exampleRisk, 0.35, w];
Print["Example capstone score (lower is better): ", Jex];

(* 6) Visualization *)
occPlot = ArrayPlot[occ, ColorFunction -> "GrayTones", Frame -> True, PlotLabel -> "Occupancy Grid"];
travFinite = trav /. Infinity -> 5.0;
travPlot = ArrayPlot[travFinite, ColorFunction -> "SolarColors", Frame -> True, PlotLabel -> "Traversability Cost"];

Export["Chapter20_Lesson1_occ.png", occPlot];
Export["Chapter20_Lesson1_trav.png", travPlot];

(* Save a text export for downstream usage *)
Export["Chapter20_Lesson1_mission.json",
  <|
    "Mission" -> mission,
    "Robot" -> robot,
    "Sensors" -> sensors,
    "Environment" -> env,
    "AvailableTimeS" -> availableTimeS,
    "TimeMarginS" -> timeMarginS,
    "SensorRatesOK" -> sensorRatesOK
  |>,
  "JSON"
];

Problem 1 (Resolution selection): A corridor in the capstone map has width \( w_c = 1.2 \) m. If you require at least \( n_c = 4 \) cells across the corridor, what is the largest allowable map resolution \( \Delta \)?

Solution: Use the setup inequality \( \Delta \le w_c / n_c \). Therefore

\[ \Delta \le \frac{1.2}{4} = 0.3\text{ m} \]

Any resolution finer than or equal to \( 0.3 \) m is acceptable. Choosing \( \Delta = 0.2 \) m (as in the provided code) preserves more geometric detail and improves costmap gradients.

Problem 2 (Battery feasibility): The robot battery is \( E_b = 180 \) Wh and average power is \( \bar{P} = 85 \) W. Compute available runtime and determine whether a mission with \( T_{\max}=1200 \) s and reserve \( T_{\text{reserve}}=300 \) s is feasible.

Solution:

\[ T_{\text{avail}} = 3600\frac{180}{85} \approx 7623.53\text{ s} \]

Required time is \( 1200 + 300 = 1500 \) s, and \( 7623.53 \ge 1500 \), so the mission is feasible with a large margin.

Problem 3 (Timestamp misalignment error): If the platform maximum speed is \( v_{\max}=1.2 \) m/s and worst-case timestamp offset is \( \bar{\delta}=40 \) ms, bound the geometric error from unsynchronized sensor data.

Solution: Convert \( 40 \) ms to \( 0.04 \) s and apply \( \|\delta \mathbf{p}\| \le v_{\max}\bar{\delta} \):

\[ \|\delta \mathbf{p}\| \le 1.2 \times 0.04 = 0.048\text{ m} \]

The synchronization-only spatial error is bounded by \( 4.8 \) cm. This should be compared against grid resolution and inflation margin.

Problem 4 (Acceptance set design): Propose a mathematically explicit acceptance set for a warehouse capstone mission in which one minor contact is allowed but mission completion probability must be at least 0.95.

Solution: One valid definition is

\[ \mathcal{C}_{\text{acc}}^{\text{warehouse}} = \left\{ \text{reach goal},\; N_{\text{coll}} \le 1,\; T_{\text{nav}} \le T_{\max},\; P(\text{success}) \ge 0.95 \right\} \]

Additional constraints may include localization RMSE thresholds or minimum map coverage, but they must be stated explicitly in the setup document before experiments begin.

We converted the capstone from a narrative objective into a rigorous AMR problem specification with environment assumptions, coordinate frames, feasibility constraints, and reproducible scenario generation. This baseline is intentionally conservative and deterministic, so future capstone lessons can compare localization, mapping, and navigation choices on the same frozen setup.