Agricultural and delivery robots are both outdoor AMR systems, but they occupy different operating regimes. Agricultural robots face deformable terrain, row geometry, long missions, and intermittent perception quality due to dust, lighting, and crop occlusion. Delivery robots face sidewalks, curbs, pedestrians, time-window commitments, and stringent stopping-safety requirements.

From earlier chapters, students already know localization, mapping, local planning, and exploration. The present lesson focuses on system integration: how to combine those modules into mission-level decisions under uncertainty. A useful abstraction is a directed graph \( G=(V,E) \), where each edge represents a traversable path segment with associated time, energy, and risk costs.

\[ J(\pi) = \sum_{e \in \pi} \left( w_t\, t_e + w_E\, E_e + w_R\, R_e \right) + \Phi(\pi) \]

Here \( \pi \) is a candidate route or mission plan, \( t_e \) is edge traversal time, \( E_e \) is energy, \( R_e \) is a traversability / safety risk score, and \( \Phi(\pi) \) encodes mission-specific penalties such as overlap (agriculture) or deadline violation (delivery). The central engineering question is not whether one term is “best,” but how to choose weights and constraints consistent with the mission objective.

A practical outdoor AMR stack for both case studies couples global localization (e.g., GNSS/RTK + inertial fusion), local traversability estimation, route planning, and runtime supervision. The architecture below emphasizes the feedback paths used in real deployments:

flowchart TD
  A["Mission Goal"] --> B["Map + GNSS/RTK + IMU + Wheel Odometry"]
  B --> C["Localization and Covariance Estimate"]
  C --> D["Traversability / Sidewalk Risk Map"]
  D --> E["Global Route Candidate Set"]
  C --> E
  E --> F["Local Motion Generation and Tracking"]
  F --> G["Runtime Monitors: slip, clearance, ETA, battery"]
  G --> H["Execute / Replan / Safe Stop"]
  H --> D
  H --> C
        

The feedback loop \( H \to D \) reflects online updates to terrain or sidewalk occupancy, while \( H \to C \) reflects covariance growth and relocalization events. This is the key difference between a classroom path planner and a deployable outdoor robot: the mission planner must respect evolving uncertainty and not assume a static world model.

We collect a compact set of models reused across both domains. Let robot pose be \( \mathbf{x}=[x,\;y,\;\psi]^\top \) and commanded speed be \( v \). For an outdoor ground robot with low-speed slip effects, a useful applied kinematic approximation is:

\[ \dot{x} = v\cos\psi,\qquad \dot{y} = v\sin\psi,\qquad \dot{\psi} = \frac{v}{L}\tan\delta \cdot \lambda_{\psi} \]

where \( L \) is wheelbase (or equivalent turning geometry), \( \delta \) is steering angle, and \( \lambda_{\psi}\in(0,1] \) is a yaw-effectiveness factor that decreases under slip or soft soil. In differential-drive or skid-steer agricultural bases, an analogous factor can scale angular velocity.

A practical edge energy model for motion over slope \( \alpha_e \) is:

\[ P_e(v) = \Big(c_r m g \cos\alpha_e + m g \sin\alpha_e + \tfrac{1}{2}\rho C_d A v^2\Big)v + P_{\text{aux}} \]

\[ E_e = P_e(v)\, t_e,\qquad t_e=\frac{\ell_e}{v} \]

The auxiliary term \( P_{\text{aux}} \) includes onboard computing, perception sensors, refrigeration (delivery), or implement actuation (agriculture). Note that \( P_{\text{aux}} \) can dominate at low speed, so a “slower is always better” policy is often false.

We also require a risk score for each segment. Let normalized terrain/sidewalk features be \( r_e \) (roughness), \( s_e \) (slip or sidewalk conflict likelihood), and \( q_e \) (slope magnitude proxy). Define

\[ R_e = \beta_1 r_e + \beta_2 s_e + \beta_3 q_e,\qquad \beta_i \ge 0,\quad \sum_i \beta_i = 1 \]

Then the composite edge cost is:

\[ C_e(v) = w_t\,\frac{\ell_e}{v} + w_E\,E_e(v) + w_R\,R_e \]

Monotonicity property. If two paths \( \pi_1,\pi_2 \) satisfy \( \sum_{e\in\pi_1} t_e \le \sum_{e\in\pi_2} t_e \), \( \sum_{e\in\pi_1} E_e \le \sum_{e\in\pi_2} E_e \), and \( \sum_{e\in\pi_1} R_e \le \sum_{e\in\pi_2} R_e \), then \( J(\pi_1) \le J(\pi_2) \) for any nonnegative weights. This follows directly by termwise comparison.

Consider a row-crop robot performing scouting, spraying, or selective harvesting. The robot must traverse long lanes, perform repeated turns, and maintain bounded overlap between adjacent passes. Let field dimensions be length \( L_f \) and width \( W_f \), implement width \( w \), and overlap ratio \( \eta \in [0,1) \). The effective swath width is:

\[ w_{\text{eff}} = w(1-\eta) \]

The minimum number of passes needed to cover the field width is

\[ N_{\text{pass}} = \left\lceil \frac{W_f}{w_{\text{eff}}} \right\rceil \]

and an idealized coverage time model (neglecting stoppages) is

\[ T_{\text{cov}} = \frac{N_{\text{pass}}L_f}{v_c} + (N_{\text{pass}}-1)T_{\text{turn}} \]

where \( v_c \) is cruise speed and \( T_{\text{turn}} \) is average turnaround time. This formula immediately shows a nontrivial trade-off: reducing overlap \( \eta \) decreases \( N_{\text{pass}} \) but can increase missed strips when GNSS covariance rises or wheel slip grows.

To incorporate localization uncertainty, suppose the lateral path error at row entry is approximately Gaussian, \( e_y \sim \mathcal{N}(0,\sigma_y^2) \). If we require probability of missing the intended swath edge to be at most \( \epsilon \), a conservative condition is:

\[ z_{1-\epsilon}\sigma_y \le \frac{\eta w}{2} \]

Thus overlap is not just an agronomic choice; it is coupled to localization performance. When dust or canopy occlusion increases \( \sigma_y \), the supervisor should either increase overlap, slow down, or request relocalization before the next lane.

For traction-limited motion, a standard safety bound is the stopping-distance inequality:

\[ d_{\text{stop}}(v) = v t_r + \frac{v^2}{2\mu g} + d_m \]

where \( t_r \) is reaction/latency time, \( \mu \) is effective friction (or traction proxy), and \( d_m \) is a fixed margin. If sensed free-space clearance is \( d_c \), safe operation requires \( d_{\text{stop}}(v) \le d_c \). We derive the closed-form speed limit in the problem set.

For a sidewalk delivery robot, the mission objective is usually deadline compliance with high safety and low nuisance to pedestrians. The route planner must account for uncertain segment times caused by crossings, pedestrian density, curb ramps, and temporary obstacles.

Let segment travel times be random variables \( T_1,\dots,T_n \) with means \( \mu_i \) and standard deviations \( \sigma_i \). If we assume independent segment delays for planning (an approximation), then total route time \( T=\sum_i T_i \) satisfies:

\[ \mathbb{E}[T] = \sum_{i=1}^{n}\mu_i,\qquad \operatorname{Var}(T)=\sum_{i=1}^{n}\sigma_i^2 \]

A one-sided chance constraint for a promised deadline \( D \) is \( \Pr(T \le D)\ge 1-\epsilon \). Under a Gaussian approximation:

\[ \sum_{i=1}^{n}\mu_i + z_{1-\epsilon}\sqrt{\sum_{i=1}^{n}\sigma_i^2} \le D \]

This inequality is operationally useful because it separates route choice (which affects both \( \mu_i \) and \( \sigma_i \)) from service-level policy (which sets \( \epsilon \)). Tighter service guarantees require either safer/faster routes or a longer promised delivery window.

Delivery robots also benefit from a social or conflict penalty derived from dynamic obstacle predictions. If \( p_{ij}(t) \) is the predicted collision probability between robot path segment \( i \) and pedestrian track \( j \), a route-level interaction penalty can be approximated by:

\[ \Phi_{\text{social}} = \lambda \sum_{i}\sum_{j}\int_{0}^{H} p_{ij}(t)\,dt \]

In practice, systems often substitute learned scores or occupancy forecasts for explicit probability integrals, but the mathematical form clarifies that “social compliance” is a measurable resource, not a vague preference.

Finally, deadline planning and safety planning should be coupled. A route that is fast in expectation but repeatedly forces near-braking events can increase both delay variance and pedestrian discomfort. Therefore, the composite mission cost should include both expected time and safety-risk penalties, rather than optimizing ETA in isolation.

In both case studies, the localization module must detect measurement outliers (multipath GNSS, visual failures, wheel-slip transients) before they corrupt the mission planner. Let predicted measurement residual be \( \mathbf{r}_k = \mathbf{z}_k - \hat{\mathbf{z}}_{k|k-1} \) and innovation covariance be \( \mathbf{S}_k \). The standard validation gate is:

\[ \mathbf{r}_k^\top \mathbf{S}_k^{-1}\mathbf{r}_k \le \chi^2_{m,\,1-\alpha} \]

where \( m \) is measurement dimension and \( \alpha \) is the false-rejection rate. When the gate is violated, the system should down-weight or reject the update, and optionally raise mission penalties due to reduced confidence:

\[ J_{\text{aug}}(\pi) = J(\pi) + w_{\Sigma}\sum_{k \in \pi}\operatorname{tr}(\mathbf{P}_k) \]

with \( \mathbf{P}_k \) the state covariance on segment \( k \). This augmentation is especially useful in long agricultural routes and urban canyons where covariance growth is mission-critical.

Runtime supervision logic. A compact rule set is:

\[ \text{Execute if } \big(d_{\text{stop}}(v)\le d_c\big)\ \land\ \big(\operatorname{tr}(\mathbf{P})\le P_{\max}\big)\ \land\ \big(E_{\text{rem}}\ge E_{\text{need}}+E_{\text{reserve}}\big) \]

\[ \text{Otherwise } \rightarrow \ \text{Replan or Safe Stop} \]

This formulation is intentionally simple and implementable. It lets the planner and controller remain modular while the supervisor enforces mission-level invariants.

The two robots differ mainly in objective weighting and constraints:

• Agriculture: prioritize coverage quality, traction safety, and battery endurance over strict ETA.
• Delivery: prioritize deadline reliability and pedestrian-safe behavior with strong stopping and interaction constraints.

A useful deployment workflow is shown below.

flowchart TD
  A["Define mission objective and constraints"] --> B["Calibrate terrain/sidewalk risk features"]
  B --> C["Estimate time and energy models from logs"]
  C --> D["Tune weights and hard safety bounds"]
  D --> E["Offline replay on recorded routes"]
  E --> F["Field pilot with runtime monitors"]
  F --> G["Collect failures and near-misses"]
  G --> H["Retune thresholds / replanning policy"]
  H --> F
        

The repeated loop \( G \rightarrow H \rightarrow F \) is essential. Outdoor deployments are rarely solved in one pass because weather, terrain moisture, pedestrian density, and construction patterns vary by day.

The Python code below implements a composite-cost shortest-path planner, an agricultural coverage-time calculator, a delivery ETA chance-constraint check, and a closed-form safety speed bound.

Recommended ecosystem extensions for real deployments: numpy, scipy, networkx for graph routing, and ROS 2 Python bindings (rclpy) for onboard integration. For agricultural geospatial workflows, rasterio / geopandas are also common.

Code: Chapter18_Lesson5.py

# Chapter18_Lesson5.py
# Case Study: Agricultural / Delivery Robots
# Risk-aware mission scoring with energy, time, and traversability terms

from dataclasses import dataclass
from typing import List, Tuple
import math
import heapq


@dataclass
class Edge:
    u: int
    v: int
    length_m: float
    slope_rad: float
    roughness: float        # 0..1
    slip_risk: float        # 0..1
    speed_limit: float      # m/s


def edge_time(edge: Edge, v_cmd: float) -> float:
    v = max(0.2, min(v_cmd, edge.speed_limit))
    return edge.length_m / v


def edge_energy(edge: Edge, mass: float, v_cmd: float, c_rr: float = 0.03,
                rho_air: float = 1.2, CdA: float = 0.45, P_aux: float = 80.0) -> float:
    v = max(0.2, min(v_cmd, edge.speed_limit))
    F_roll = c_rr * mass * 9.81 * math.cos(edge.slope_rad)
    F_grade = mass * 9.81 * math.sin(edge.slope_rad)
    F_drag = 0.5 * rho_air * CdA * v * v
    P = max(0.0, (F_roll + F_grade + F_drag) * v) + P_aux
    return P * (edge.length_m / v)  # Joules


def edge_risk(edge: Edge) -> float:
    # Simple normalized traversability risk score
    return 0.45 * edge.roughness + 0.55 * edge.slip_risk + 0.15 * abs(edge.slope_rad)


def stopping_distance(v: float, reaction_s: float, mu: float, margin: float = 0.5) -> float:
    return v * reaction_s + v * v / (2.0 * mu * 9.81) + margin


def safe_speed_from_clearance(clearance_m: float, reaction_s: float, mu: float, margin: float = 0.5) -> float:
    # Solve v*t_r + v^2/(2*mu*g) + margin <= clearance
    d = max(0.0, clearance_m - margin)
    a = 1.0 / (2.0 * mu * 9.81)
    b = reaction_s
    c = -d
    disc = max(0.0, b * b - 4 * a * c)
    return (-b + math.sqrt(disc)) / (2 * a)


def build_graph(edges: List[Edge], n_nodes: int):
    g = [[] for _ in range(n_nodes)]
    for e in edges:
        g[e.u].append(e)
    return g


def shortest_path(edges: List[Edge], n_nodes: int, source: int, target: int,
                  mass: float, v_cmd: float, w_t: float, w_e: float, w_r: float,
                  reaction_s: float, mu: float, clearance_default: float = 3.0):
    g = build_graph(edges, n_nodes)
    dist = [float("inf")] * n_nodes
    parent = [-1] * n_nodes
    dist[source] = 0.0
    pq = [(0.0, source)]
    while pq:
        d, u = heapq.heappop(pq)
        if d > dist[u]:
            continue
        if u == target:
            break
        for e in g[u]:
            v_safe = safe_speed_from_clearance(clearance_default, reaction_s, mu)
            v_use = min(v_cmd, e.speed_limit, max(0.25, v_safe))
            c = (
                w_t * edge_time(e, v_use)
                + w_e * edge_energy(e, mass, v_use) / 1000.0  # kJ scaling
                + w_r * edge_risk(e)
            )
            nd = d + c
            if nd < dist[e.v]:
                dist[e.v] = nd
                parent[e.v] = u
                heapq.heappush(pq, (nd, e.v))

    # Reconstruct
    path = []
    cur = target
    if parent[cur] == -1 and cur != source:
        return [], float("inf")
    while cur != -1:
        path.append(cur)
        cur = parent[cur]
    path.reverse()
    return path, dist[target]


def agricultural_coverage_metrics(field_length: float, field_width: float,
                                  tool_width: float, overlap_ratio: float,
                                  turn_loss_s: float, cruise_speed: float) -> Tuple[int, float, float]:
    w_eff = tool_width * (1.0 - overlap_ratio)
    n_pass = math.ceil(field_width / w_eff)
    travel_dist = n_pass * field_length
    time_s = travel_dist / max(0.2, cruise_speed) + max(0, n_pass - 1) * turn_loss_s
    covered_area = field_length * field_width
    return n_pass, time_s, covered_area


def delivery_eta_chance_constraint(mean_times: List[float], std_times: List[float],
                                   deadline_s: float, z_quantile: float = 1.645) -> bool:
    mu = sum(mean_times)
    sigma = math.sqrt(sum(s * s for s in std_times))
    return (mu + z_quantile * sigma) <= deadline_s


if __name__ == "__main__":
    # Example graph shared by both robot types
    edges = [
        Edge(0, 1, 25,  0.02, 0.10, 0.15, 2.5),
        Edge(1, 2, 18,  0.05, 0.30, 0.35, 1.8),
        Edge(0, 3, 20, -0.01, 0.08, 0.10, 2.0),
        Edge(3, 2, 24,  0.03, 0.12, 0.20, 2.2),
        Edge(2, 4, 30,  0.01, 0.15, 0.18, 3.0),
        Edge(3, 4, 28,  0.07, 0.40, 0.45, 1.5),
    ]

    # Agricultural robot: heavier, risk and energy weighted
    ag_path, ag_cost = shortest_path(
        edges, n_nodes=5, source=0, target=4,
        mass=180.0, v_cmd=1.7, w_t=1.0, w_e=0.03, w_r=8.0,
        reaction_s=0.4, mu=0.55, clearance_default=2.5
    )
    print("Agricultural path:", ag_path, "cost:", round(ag_cost, 3))

    passes, t_cov, area = agricultural_coverage_metrics(
        field_length=120.0, field_width=48.0, tool_width=2.4,
        overlap_ratio=0.12, turn_loss_s=7.0, cruise_speed=1.4
    )
    print("Coverage passes:", passes, "time [min]:", round(t_cov / 60.0, 2), "area [m^2]:", area)

    # Delivery robot: lighter, time weighted, stricter deadline
    del_path, del_cost = shortest_path(
        edges, n_nodes=5, source=0, target=4,
        mass=45.0, v_cmd=2.2, w_t=1.5, w_e=0.01, w_r=5.0,
        reaction_s=0.6, mu=0.65, clearance_default=3.5
    )
    print("Delivery path:", del_path, "cost:", round(del_cost, 3))

    mean_segments = [110.0, 95.0, 150.0]
    std_segments = [12.0, 10.0, 20.0]
    ok = delivery_eta_chance_constraint(mean_segments, std_segments, deadline_s=390.0)
    print("Delivery ETA chance constraint satisfied:", ok)

    # Demonstrate safety speed bound
    vmax = safe_speed_from_clearance(clearance_m=4.0, reaction_s=0.5, mu=0.6, margin=0.6)
    print("Safe speed from clearance [m/s]:", round(vmax, 3))

The C++ version is suitable for performance-oriented onboard implementations where deterministic memory and runtime behavior matter. It computes the same composite path cost and safety speed bound.

In production C++ stacks, this logic is often embedded in ROS 2 (rclcpp) and paired with Eigen for linear algebra, Ceres or g2o for optimization, and navigation components from nav2.

Code: Chapter18_Lesson5.cpp

// Chapter18_Lesson5.cpp
// Case Study: Agricultural / Delivery Robots
// Composite-cost Dijkstra planner for outdoor AMR case studies

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <limits>
#include <algorithm>

struct Edge {
    int u, v;
    double length_m;
    double slope_rad;
    double roughness;
    double slip_risk;
    double speed_limit;
};

double edgeTime(const Edge& e, double v_cmd) {
    double v = std::max(0.2, std::min(v_cmd, e.speed_limit));
    return e.length_m / v;
}

double edgeEnergy(const Edge& e, double mass, double v_cmd) {
    const double g = 9.81;
    double v = std::max(0.2, std::min(v_cmd, e.speed_limit));
    double c_rr = 0.03;
    double rho_air = 1.2;
    double CdA = 0.45;
    double P_aux = 80.0;

    double F_roll = c_rr * mass * g * std::cos(e.slope_rad);
    double F_grade = mass * g * std::sin(e.slope_rad);
    double F_drag = 0.5 * rho_air * CdA * v * v;
    double P = std::max(0.0, (F_roll + F_grade + F_drag) * v) + P_aux;
    return P * (e.length_m / v); // Joules
}

double edgeRisk(const Edge& e) {
    return 0.45 * e.roughness + 0.55 * e.slip_risk + 0.15 * std::abs(e.slope_rad);
}

double safeSpeedFromClearance(double clearance_m, double reaction_s, double mu, double margin = 0.5) {
    double d = std::max(0.0, clearance_m - margin);
    double a = 1.0 / (2.0 * mu * 9.81);
    double b = reaction_s;
    double c = -d;
    double disc = std::max(0.0, b*b - 4.0*a*c);
    return (-b + std::sqrt(disc)) / (2.0*a);
}

struct NodeState {
    double cost;
    int node;
    bool operator>(const NodeState& other) const { return cost > other.cost; }
};

std::pair<std::vector<int>, double> dijkstraComposite(
    const std::vector<Edge>& edges,
    int n_nodes,
    int source,
    int target,
    double mass,
    double v_cmd,
    double wt, double we, double wr,
    double reaction_s, double mu, double clearance_default
) {
    std::vector<std::vector<Edge>> g(n_nodes);
    for (const auto& e : edges) {
        g[e.u].push_back(e);
    }

    std::vector<double> dist(n_nodes, std::numeric_limits<double>::infinity());
    std::vector<int> parent(n_nodes, -1);
    dist[source] = 0.0;

    std::priority_queue<NodeState, std::vector<NodeState>, std::greater<NodeState>> pq;
    pq.push({0.0, source});

    while (!pq.empty()) {
        NodeState cur = pq.top();
        pq.pop();
        if (cur.cost > dist[cur.node]) continue;
        if (cur.node == target) break;

        for (const auto& e : g[cur.node]) {
            double v_safe = safeSpeedFromClearance(clearance_default, reaction_s, mu);
            double v_use = std::min(v_cmd, std::min(e.speed_limit, std::max(0.25, v_safe)));

            double c = wt * edgeTime(e, v_use)
                     + we * (edgeEnergy(e, mass, v_use) / 1000.0)
                     + wr * edgeRisk(e);

            double nd = dist[cur.node] + c;
            if (nd < dist[e.v]) {
                dist[e.v] = nd;
                parent[e.v] = cur.node;
                pq.push({nd, e.v});
            }
        }
    }

    std::vector<int> path;
    if (!std::isfinite(dist[target])) return {path, dist[target]};
    for (int v = target; v != -1; v = parent[v]) path.push_back(v);
    std::reverse(path.begin(), path.end());
    return {path, dist[target]};
}

int main() {
    std::vector<Edge> edges = {
        {0,1,25, 0.02,0.10,0.15,2.5},
        {1,2,18, 0.05,0.30,0.35,1.8},
        {0,3,20,-0.01,0.08,0.10,2.0},
        {3,2,24, 0.03,0.12,0.20,2.2},
        {2,4,30, 0.01,0.15,0.18,3.0},
        {3,4,28, 0.07,0.40,0.45,1.5}
    };

    auto ag = dijkstraComposite(edges, 5, 0, 4, 180.0, 1.7, 1.0, 0.03, 8.0, 0.4, 0.55, 2.5);
    std::cout << "Agricultural path: ";
    for (int n : ag.first) std::cout << n << " ";
    std::cout << " | cost = " << ag.second << "\n";

    auto del = dijkstraComposite(edges, 5, 0, 4, 45.0, 2.2, 1.5, 0.01, 5.0, 0.6, 0.65, 3.5);
    std::cout << "Delivery path: ";
    for (int n : del.first) std::cout << n << " ";
    std::cout << " | cost = " << del.second << "\n";

    double vmax = safeSpeedFromClearance(4.0, 0.5, 0.6, 0.6);
    std::cout << "Safe speed from clearance = " << vmax << " m/s\n";

    return 0;
}

The Java example emphasizes mission orchestration: state transitions, deadline feasibility, and battery checks. This is useful for server-side fleet orchestration prototypes or Android-based HMI testing tools.

Java is less common on the robot itself, but very useful for fleet backends, dashboards, and mission services. Typical libraries include Apache Commons Math or EJML for matrix operations and Spring Boot for fleet orchestration APIs.

Code: Chapter18_Lesson5.java

// Chapter18_Lesson5.java
// Case Study: Agricultural / Delivery Robots
// Mission-state machine and ETA/chance checks for outdoor AMR

import java.util.ArrayList;
import java.util.List;

public class Chapter18_Lesson5 {

    enum Mode {
        LOCALIZE,
        PLAN,
        EXECUTE,
        REPLAN,
        SAFE_STOP,
        COMPLETE
    }

    static class Segment {
        double meanTime;
        double stdTime;
        double risk;
        double energy;
        Segment(double meanTime, double stdTime, double risk, double energy) {
            this.meanTime = meanTime;
            this.stdTime = stdTime;
            this.risk = risk;
            this.energy = energy;
        }
    }

    static class Mission {
        String type;  // "agriculture" or "delivery"
        double deadlineSeconds;
        double batteryJoules;
        List<Segment> segments = new ArrayList<>();
        Mission(String type, double deadlineSeconds, double batteryJoules) {
            this.type = type;
            this.deadlineSeconds = deadlineSeconds;
            this.batteryJoules = batteryJoules;
        }
    }

    static double scoreMission(Mission m, double wt, double we, double wr) {
        double total = 0.0;
        for (Segment s : m.segments) {
            total += wt * s.meanTime + we * s.energy + wr * s.risk;
        }
        return total;
    }

    static boolean chanceDeadlineSatisfied(Mission m, double z) {
        double mu = 0.0;
        double var = 0.0;
        for (Segment s : m.segments) {
            mu += s.meanTime;
            var += s.stdTime * s.stdTime;
        }
        double sigma = Math.sqrt(var);
        return (mu + z * sigma) <= m.deadlineSeconds;
    }

    static boolean batterySatisfied(Mission m) {
        double energy = 0.0;
        for (Segment s : m.segments) {
            energy += s.energy;
        }
        return energy <= m.batteryJoules;
    }

    static Mode step(Mode mode, Mission mission) {
        switch (mode) {
            case LOCALIZE:
                return Mode.PLAN;
            case PLAN:
                if (!chanceDeadlineSatisfied(mission, 1.645)) return Mode.REPLAN;
                if (!batterySatisfied(mission)) return Mode.REPLAN;
                return Mode.EXECUTE;
            case EXECUTE:
                // Runtime check (placeholder for obstacle and confidence monitoring)
                boolean hazardDetected = false;
                boolean covarianceTooLarge = false;
                if (hazardDetected || covarianceTooLarge) return Mode.SAFE_STOP;
                return Mode.COMPLETE;
            case REPLAN:
                // In practice: lower speed, choose safer sidewalk/lane, or split mission
                return Mode.PLAN;
            case SAFE_STOP:
                return Mode.REPLAN;
            default:
                return Mode.COMPLETE;
        }
    }

    static Mission buildAgriculturalMission() {
        Mission m = new Mission("agriculture", 1800.0, 550000.0);
        m.segments.add(new Segment(250.0, 20.0, 0.22, 52000.0));
        m.segments.add(new Segment(310.0, 25.0, 0.35, 70000.0));
        m.segments.add(new Segment(280.0, 15.0, 0.18, 61000.0));
        return m;
    }

    static Mission buildDeliveryMission() {
        Mission m = new Mission("delivery", 420.0, 180000.0);
        m.segments.add(new Segment(110.0, 12.0, 0.12, 9000.0));
        m.segments.add(new Segment(95.0, 10.0, 0.20, 8000.0));
        m.segments.add(new Segment(150.0, 20.0, 0.28, 12000.0));
        return m;
    }

    public static void main(String[] args) {
        Mission ag = buildAgriculturalMission();
        Mission dr = buildDeliveryMission();

        double agScore = scoreMission(ag, 1.0, 0.0005, 12.0);
        double drScore = scoreMission(dr, 1.6, 0.0008, 8.0);

        System.out.println("Agricultural score = " + agScore);
        System.out.println("Agricultural deadline feasible = " + chanceDeadlineSatisfied(ag, 1.282)); // 90%
        System.out.println("Agricultural battery feasible = " + batterySatisfied(ag));

        System.out.println("Delivery score = " + drScore);
        System.out.println("Delivery deadline feasible = " + chanceDeadlineSatisfied(dr, 1.645)); // 95%
        System.out.println("Delivery battery feasible = " + batterySatisfied(dr));

        Mode mode = Mode.LOCALIZE;
        for (int k = 0; k < 10 && mode != Mode.COMPLETE; k++) {
            mode = step(mode, dr);
            System.out.println("State -> " + mode);
        }
    }
}

The MATLAB script computes agricultural and delivery metrics, derives a safe speed bound, and programmatically builds a simple Simulink longitudinal speed-tracking model with PI control and acceleration saturation.

MATLAB/Simulink users typically connect this workflow to Robotics System Toolbox, Navigation Toolbox, and Automated Driving Toolbox for richer sensor, planner, and scenario components.

Code: Chapter18_Lesson5.m

% Chapter18_Lesson5.m
% Case Study: Agricultural / Delivery Robots
% MATLAB + Simulink-oriented script for risk-aware mission calculations

clear; clc;

%% 1) Agricultural coverage and energy model
fieldLength = 120;   % m
fieldWidth  = 48;    % m
toolWidth   = 2.4;   % m
overlap     = 0.12;  % 12 percent
v_ag        = 1.4;   % m/s
turnLoss    = 7.0;   % s per turn

wEff   = toolWidth * (1 - overlap);
nPass  = ceil(fieldWidth / wEff);
distAg = nPass * fieldLength;
tAg    = distAg / v_ag + (nPass - 1) * turnLoss;

mAg    = 180;      % kg
crr    = 0.03;
rhoAir = 1.2;
CdA    = 0.45;
Paux   = 80;
slope  = 0.03;     % rad
g      = 9.81;

Froll = crr * mAg * g * cos(slope);
Fgrade = mAg * g * sin(slope);
Fdrag = 0.5 * rhoAir * CdA * v_ag^2;
Pag = max(0, (Froll + Fgrade + Fdrag) * v_ag) + Paux;
Eag = Pag * tAg;

fprintf('Agricultural passes = %d\n', nPass);
fprintf('Agricultural mission time = %.2f min\n', tAg / 60);
fprintf('Agricultural mission energy = %.2f kJ\n', Eag / 1000);

%% 2) Delivery ETA chance constraint
muSeg = [110, 95, 150];      % s
sigSeg = [12, 10, 20];       % s
deadline = 390;              % s
z95 = 1.645;

muTot = sum(muSeg);
sigTot = sqrt(sum(sigSeg.^2));
etaBound = muTot + z95 * sigTot;
feasibleDeadline = etaBound <= deadline;

fprintf('Delivery ETA bound (95%%) = %.2f s\n', etaBound);
fprintf('Delivery deadline feasible = %d\n', feasibleDeadline);

%% 3) Safety speed bound from stopping distance
clearance = 4.0;  % m
tReaction = 0.5;  % s
mu = 0.6;
margin = 0.6;

% Solve v*tReaction + v^2/(2*mu*g) + margin <= clearance
a = 1 / (2 * mu * g);
b = tReaction;
c = -(clearance - margin);
vmax = (-b + sqrt(b^2 - 4*a*c)) / (2*a);
fprintf('Safe speed from clearance = %.3f m/s\n', vmax);

%% 4) Simulink model (programmatic) for longitudinal speed tracking
% This builds a simple model: reference -> PI -> saturation -> vehicle integrator
mdl = 'Chapter18_Lesson5_Simulink';
if bdIsLoaded(mdl)
    close_system(mdl, 0);
end
if exist([mdl '.slx'], 'file')
    delete([mdl '.slx']);
end

new_system(mdl);
open_system(mdl);

add_block('simulink/Sources/Step', [mdl '/v_ref'], 'Position', [30 40 60 70]);
set_param([mdl '/v_ref'], 'Time', '0', 'Before', '0', 'After', num2str(vmax));

add_block('simulink/Math Operations/Sum', [mdl '/sum'], 'Inputs', '+-', ...
    'Position', [110 35 130 75]);

add_block('simulink/Continuous/PID Controller', [mdl '/PI'], ...
    'P', '2.0', 'I', '1.0', 'D', '0.0', 'Position', [170 25 240 85]);

add_block('simulink/Discontinuities/Saturation', [mdl '/sat_acc'], ...
    'UpperLimit', '1.2', 'LowerLimit', '-1.8', 'Position', [280 35 330 75]);

add_block('simulink/Continuous/Transfer Fcn', [mdl '/vehicle'], ...
    'Numerator', '[1]', 'Denominator', '[0.6 1]', ...
    'Position', [380 35 460 75]);

add_block('simulink/Sinks/Scope', [mdl '/scope'], 'Position', [520 35 550 75]);

add_block('simulink/Signal Routing/Mux', [mdl '/mux'], ...
    'Inputs', '2', 'Position', [490 95 510 135]);

add_line(mdl, 'v_ref/1', 'sum/1');
add_line(mdl, 'sum/1', 'PI/1');
add_line(mdl, 'PI/1', 'sat_acc/1');
add_line(mdl, 'sat_acc/1', 'vehicle/1');
add_line(mdl, 'vehicle/1', 'sum/2');
add_line(mdl, 'vehicle/1', 'mux/1');
add_line(mdl, 'v_ref/1', 'mux/2');
add_line(mdl, 'mux/1', 'scope/1');

set_param(mdl, 'StopTime', '8');
save_system(mdl);

% Uncomment to simulate:
% simOut = sim(mdl);
% disp('Simulink model generated and saved.');

The Mathematica code derives the closed-form stopping-distance speed root symbolically, evaluates the delivery chance constraint, and computes agricultural coverage metrics.

Wolfram Mathematica is especially useful for symbolic derivations, parameter sensitivity studies, and quick validation of design inequalities before embedded implementation.

Code: Chapter18_Lesson5.nb

(* Chapter18_Lesson5.nb *)
(* Case Study: Agricultural / Delivery Robots *)
(* Wolfram Mathematica / Wolfram Language notebook-style code *)

ClearAll["Global`*"];

g = 9.81;

(* 1) Symbolic stopping-distance speed bound *)
dstop[v_, tr_, mu_, dm_] := v*tr + v^2/(2*mu*g) + dm;

sol = FullSimplify[
  Solve[dstop[v, tr, mu, dm] == dc, v],
  {mu > 0, tr >= 0, dc > dm}
];

Print["Closed-form roots for stopping-distance equality:"];
Print[sol];

vmaxExpr = Assuming[{mu > 0, tr >= 0, dc > dm},
  FullSimplify[
    v /. sol[[2]]
  ]
];

Print["Positive speed root (vmax):"];
Print[vmaxExpr];

(* 2) Numerical evaluation for a delivery robot *)
params = {tr -> 0.5, mu -> 0.6, dm -> 0.6, dc -> 4.0};
vmaxNum = N[vmaxExpr /. params];
Print["Safe speed from clearance [m/s] = ", vmaxNum];

(* 3) ETA chance constraint for stochastic segment times *)
meanSeg = {110, 95, 150};
stdSeg  = {12, 10, 20};
deadline = 390;
z95 = 1.645;

muTot = Total[meanSeg];
sigmaTot = Sqrt[Total[stdSeg^2]];
etaBound = muTot + z95*sigmaTot;
feasible = etaBound <= deadline;

Print["ETA 95% bound [s] = ", N[etaBound]];
Print["Deadline feasible = ", feasible];

(* 4) Agricultural coverage-time formula *)
coverageTime[fieldLength_, fieldWidth_, toolWidth_, overlap_, v_, turnLoss_] :=
  Module[{weff, nPass, dist},
    weff = toolWidth*(1 - overlap);
    nPass = Ceiling[fieldWidth/weff];
    dist = nPass*fieldLength;
    <|
      "EffectiveWidth" -> weff,
      "Passes" -> nPass,
      "Distance" -> dist,
      "TimeSeconds" -> dist/v + (nPass - 1)*turnLoss
    |>
  ];

ag = coverageTime[120, 48, 2.4, 0.12, 1.4, 7.0];
Print["Agricultural metrics = ", ag];

(* 5) Composite edge score for route selection (symbolic form) *)
score[tt_, ee_, rr_, wt_, we_, wr_] := wt*tt + we*ee + wr*rr;

expr = score[t, e, r, wt, we, wr];
Print["Composite route score expression = ", expr];
Print["Gradient wrt weights = ", Grad[expr, {wt, we, wr}]];

Problem 1 (Stopping-distance speed bound): Suppose a robot must maintain safety clearance \( d_c \) and obey \( v t_r + \frac{v^2}{2\mu g} + d_m \le d_c \). Derive the maximum allowable speed \( v_{\max} \).

Solution: Rearrange into a quadratic inequality:

\[ \frac{1}{2\mu g}v^2 + t_r v + (d_m-d_c) \le 0 \]

Since \( \mu > 0 \), the parabola opens upward. Feasible speeds lie between the two roots. The physically relevant bound is the nonnegative root:

\[ v_{\max} = \mu g\left(-t_r + \sqrt{t_r^2 + \frac{2(d_c-d_m)}{\mu g}}\right) \]

Therefore, a supervisory controller can compute \( v_{\max} \) online from estimated \( \mu \), reaction latency, and measured clearance.

Problem 2 (Coverage pass lower bound and time formula): Let field width be \( W_f \) and effective swath width be \( w_{\text{eff}} = w(1-\eta) \). Prove that any full-width coverage strategy requires at least \( \left\lceil W_f/w_{\text{eff}} \right\rceil \) passes under the no-gap approximation, and derive the idealized coverage time expression.

Solution: Each pass can cover at most \( w_{\text{eff}} \) of lateral width. After \( N \) passes, the maximum covered width is \( N w_{\text{eff}} \). Full coverage requires

\[ N w_{\text{eff}} \ge W_f \quad \Rightarrow \quad N \ge \frac{W_f}{w_{\text{eff}}} \]

Since \( N \) must be an integer, the minimum is \( N_{\text{pass}} = \left\lceil W_f/w_{\text{eff}} \right\rceil \). If each pass length is \( L_f \) and cruise speed is \( v_c \), translational time is \( N_{\text{pass}}L_f/v_c \). There are \( N_{\text{pass}}-1 \) turnarounds, so:

\[ T_{\text{cov}} = \frac{N_{\text{pass}}L_f}{v_c} + (N_{\text{pass}}-1)T_{\text{turn}} \]

Problem 3 (Chance-constrained deadline planning): Let total route time be \( T=\sum_{i=1}^{n}T_i \) with independent segment times \( T_i \) having means \( \mu_i \) and variances \( \sigma_i^2 \). Show that the Gaussian approximation of \( \Pr(T\le D)\ge 1-\epsilon \) yields the route test used in this lesson.

Solution: By linearity of expectation and independence:

\[ \mu_T = \sum_i \mu_i,\qquad \sigma_T^2 = \sum_i \sigma_i^2 \]

Approximating \( T \sim \mathcal{N}(\mu_T,\sigma_T^2) \) gives

\[ \Pr(T \le D) = \Phi\!\left(\frac{D-\mu_T}{\sigma_T}\right) \ge 1-\epsilon \]

Applying \( \Phi^{-1} \) to both sides:

\[ \frac{D-\mu_T}{\sigma_T} \ge z_{1-\epsilon} \quad \Rightarrow \quad \mu_T + z_{1-\epsilon}\sigma_T \le D \]

Substituting the expressions for \( \mu_T \) and \( \sigma_T \) produces the planning test:

\[ \sum_{i=1}^{n}\mu_i + z_{1-\epsilon}\sqrt{\sum_{i=1}^{n}\sigma_i^2} \le D \]

Problem 4 (Weight threshold between two routes): Consider two candidate routes with aggregate metrics \( (T_1,E_1,R_1) \) and \( (T_2,E_2,R_2) \). Assume \( T_1 < T_2 \) but \( R_1 > R_2 \), and ignore energy for simplicity (\( E_1=E_2 \)). Derive the condition on \( w_R/w_t \) under which route 1 is preferred.

Solution: Route 1 is preferred if

\[ w_t T_1 + w_R R_1 \le w_t T_2 + w_R R_2 \]

Rearranging:

\[ w_t(T_2-T_1) \ge w_R(R_1-R_2) \]

Since \( T_2-T_1 > 0 \) and \( R_1-R_2 > 0 \), divide both sides to obtain

\[ \frac{w_R}{w_t} \le \frac{T_2-T_1}{R_1-R_2} \]

This threshold is extremely useful during tuning: if field tests show too many risky shortcuts, increase \( w_R/w_t \); if delivery deadlines are missed, decrease it or add a hard chance constraint.

Problem 5 (Innovation gate interpretation): Let the residual satisfy \( \mathbf{r}\sim\mathcal{N}(\mathbf{0},\mathbf{S}) \) under a correct measurement model. Show that the gate statistic \( \gamma = \mathbf{r}^\top \mathbf{S}^{-1}\mathbf{r} \) follows a chi-square law and explain why the threshold \( \chi^2_{m,1-\alpha} \) gives false-rejection rate approximately \( \alpha \).

Solution: Let \( \mathbf{u}=\mathbf{S}^{-1/2}\mathbf{r} \). Then \( \mathbf{u}\sim\mathcal{N}(\mathbf{0},\mathbf{I}_m) \) and

\[ \gamma = \mathbf{r}^\top \mathbf{S}^{-1}\mathbf{r} = \mathbf{u}^\top \mathbf{u} = \sum_{i=1}^{m}u_i^2 \]

By definition, the sum of squares of \( m \) independent standard normal variables is \( \chi_m^2 \). Therefore

\[ \Pr\!\left(\gamma \le \chi^2_{m,1-\alpha}\right) = 1-\alpha \]

so a correct measurement is rejected with probability approximately \( \alpha \). In practice, model mismatch and correlated errors can alter this rate, which is why field calibration of gate thresholds remains necessary.

This lesson translated outdoor AMR theory into two integrated deployment scenarios. We derived mission-level cost functions, coverage-time formulas, deadline chance constraints, stopping-distance speed limits, and estimator gating logic, then implemented the ideas across five programming environments. The main takeaway is that agricultural and delivery robots share the same mathematical backbone, but differ in objective weights, uncertainty models, and operational constraints.