In Chapter 17 so far, we treated exploration as choosing actions (frontiers / viewpoints) to maximize \( \Delta \)-entropy reduction or other information objectives. Real AMRs also face budgets:

• Energy budget (battery): traction power, compute/sensing power, plus safety reserve.
• Time budget: mission deadline, time-to-return, or “must dock by time \(T\)”.

We will model exploration as choosing a sequence of decision points \( \pi = (a_1,a_2,\dots,a_N) \) (e.g., next-best-views), where each action yields expected information gain but consumes energy and time. The key constraint is that exploration must remain feasible: the robot must be able to reach the next goal and still return to a terminal set (home/dock/safe zone) with a reserve.

flowchart TD
  S["Start: belief map + budgets"] --> O["Observe and update map"]
  O --> F["Compute candidate targets (frontiers / views)"]
  F --> E["Estimate utility: IG - lambda*cost"]
  E --> C["Check feasibility: can reach target and return with reserve?"]
  C -->|yes| G["Choose best feasible target"]
  C -->|no| R["Switch to return-to-home policy"]
  G --> M["Move + local avoidance (previous chapters)"]
  M --> O
  R --> H["Navigate to home/dock"]
  H --> X["Stop/Recharge/End"]
        

The loop above is a receding-horizon policy: we plan one target at a time using the current belief and remaining budgets. This fits the AMR pipeline: mapping uncertainty and reachable space change as the robot senses and discovers obstacles.

Let remaining battery energy be \( E(t) \) (Joules) and remaining mission time be \( T(t) \) (seconds). A minimal continuous model is:

\[ \dot{E}(t) = -P(t), \qquad \dot{T}(t) = -1, \qquad E(t)\ge 0,\; T(t)\ge 0 \]

The total power splits into traction and base loads: \( P(t)=P_{\text{drive} }(v(t),\kappa(t),\text{terrain}) + P_{\text{base} } \), where \( v(t) \) is speed and \( \kappa(t) \) curvature (related to turning losses and slip).

For waypoint-to-waypoint planning, we usually discretize into motion segments \( i=1,\dots,N \). If segment \( i \) has length \( \ell_i \) and average speed \( v_i \), then time \( \Delta t_i = \ell_i/v_i \) and energy is approximately:

\[ \Delta E_i \approx \left(P_{\text{drive} }(v_i) + P_{\text{base} }\right)\Delta t_i = \left(P_{\text{drive} }(v_i) + P_{\text{base} }\right)\frac{\ell_i}{v_i} \]

In many indoor AMR stacks, a practical abstraction is cost-per-meter (or cost-per-grid-step):

\[ \Delta E_i \approx c_E \,\ell_i + c_0\,\Delta t_i, \qquad \Delta T_i = \Delta t_i \]

where \( c_E \) captures traction cost per distance and \( c_0 \) captures base loads. This lesson focuses on decision-making given such costs, not on electrochemistry.

Safety reserve. We enforce a reserve energy \( E_{\text{res} } \) so the robot never plans down to zero: \( E(t)\ge E_{\text{res} } \). Reserve accounts for localization failures, detours, and braking/stopping.

Let \( \mathcal{M} \) be the map random variable (occupancy grid or features). The belief at time \( t \) has entropy \( H(\mathcal{M}\mid \mathcal{D}_t) \) where \( \mathcal{D}_t \) denotes collected sensor data. For a candidate action/goal \( a \) (e.g., a viewpoint), define its expected information gain:

\[ \operatorname{IG}(a \mid \mathcal{D}_t) = \mathbb{E}\!\left[ H(\mathcal{M}\mid \mathcal{D}_t) - H(\mathcal{M}\mid \mathcal{D}_t \cup Z(a)) \right] \]

where \( Z(a) \) is the random future observation obtained after executing action \( a \). If we choose a sequence (policy) \( \pi \), then the “total gain” is \( \sum_k \operatorname{IG}(a_k\mid \mathcal{D}_{t_k}) \), but note that gains are computed under changing beliefs.

A standard constrained formulation is:

\[ \max_{\pi}\;\; \mathbb{E}\!\left[\sum_{k=1}^{N} \operatorname{IG}(a_k \mid \mathcal{D}_{t_k})\right] \quad \text{s.t.}\quad \sum_{k=1}^{N} C_E(a_k) \le B_E,\;\; \sum_{k=1}^{N} C_T(a_k) \le B_T \]

where \( C_E(a) \) is expected energy cost (including travel and sensing) and \( C_T(a) \) is time cost. In practice, we also impose a terminal feasibility condition (Section 5): each chosen goal must still allow return-to-home with reserve.

Lagrangian relaxation (practical selection rule). Introduce multipliers \( \lambda_E \ge 0 \) and \( \lambda_T \ge 0 \) and optimize the relaxed objective:

\[ \max_{\pi}\;\; \mathbb{E}\!\left[\sum_{k=1}^{N} \Big(\operatorname{IG}(a_k\mid \mathcal{D}_{t_k}) - \lambda_E C_E(a_k) - \lambda_T C_T(a_k)\Big)\right] \]

This converts “hard budgets” into a weighted trade-off, which is exactly what many real-time explorers do: choose the action with largest \( \operatorname{IG} - \lambda \times \text{cost} \) among feasible candidates, and update budgets after execution. The multipliers can be fixed (engineering choice) or adapted online (Section 6).

When viewpoints overlap in what they can observe, information gain exhibits diminishing returns: the more you have already observed, the less a new nearby viewpoint adds. This is captured by submodularity.

Consider a simplified setting where each candidate viewpoint \( a \) reveals a set of map cells (or features) \( \mathcal{V}(a) \). Define a “coverage gain” set function:

\[ f(S) = \left|\bigcup_{a\in S}\mathcal{V}(a)\right| \]

Claim (submodularity of coverage). For any \( A \subseteq B \) and any action \( a \notin B \), the marginal gain satisfies: \( f(A\cup\{a\}) - f(A) \ge f(B\cup\{a\}) - f(B) \). Intuitively, if you already cover more (set \( B \)), adding \( a \) yields fewer new cells.

With costs \( c(a) > 0 \) and budget \( \sum_{a\in S} c(a)\le B \) (a knapsack constraint), the exact optimum is generally NP-hard. However, a classic result shows that greedy selection can be near-optimal for monotone submodular objectives.

Theorem (classical approximation, sketch). If \( f \) is monotone submodular and \( f(\emptyset)=0 \), then a greedy algorithm that repeatedly picks the item with maximum “marginal gain per cost” achieves a constant-factor approximation (commonly stated near \( 1-1/e \) with appropriate variants and bookkeeping).

Sketch of the key inequality (diminishing returns argument). Let \( S_k \) be greedy set after cost \( C_k \). Under submodularity, the best remaining marginal gain per unit cost is at least the average remaining gain of an optimal set normalized by remaining budget, implying:

\[ f(S_{k+1}) - f(S_k) \ge \frac{c(a_{k+1})}{B}\Big(f(S^\star) - f(S_k)\Big) \]

Rearranging and iterating yields an exponential decay of the gap \( f(S^\star)-f(S_k) \), leading to the familiar \( 1-e^{-C_k/B} \) style bound, and therefore a constant-factor guarantee when \( C_k \) approaches \( B \). In exploration, this theory justifies a greedy “value-per-cost” rule for selecting viewpoints/frontiers.

Unlike pure coverage objectives, real AMRs must ensure they can reach a terminal set (dock/home/safe zone) from wherever they decide to go. Let \( x \) be current pose, \( g \) a candidate target (frontier/view), and \( h \) be home/dock. Let \( d(x,y) \) be shortest-path distance on the current traversability map. With an energy-per-meter model \( c_E \) and base term \( c_0 \), define the travel energy estimate:

\[ \widehat{C}_E(x \rightarrow y) \approx c_E\, d(x,y) + c_0\,\widehat{t}(x,y) \]

The return-feasibility constraint for choosing target \( g \) is:

\[ E_{\text{rem} } - \widehat{C}_E(x \rightarrow g) - \widehat{C}_E(g \rightarrow h) \ge E_{\text{res} } \]

Similarly for time:

\[ T_{\text{rem} } - \widehat{C}_T(x \rightarrow g) - \widehat{C}_T(g \rightarrow h) \ge 0 \]

Important engineering note. Because the map is uncertain, \( d(\cdot,\cdot) \) is itself uncertain. Robust planners use inflated costs (risk-aware) or conservative bounds (e.g., add a detour factor \( \alpha > 1 \)): \( \widehat{C}_E \leftarrow \alpha\,\widehat{C}_E \).

stateDiagram-v2
  [*] --> Explore
  Explore --> Explore: "feasible target exists"
  Explore --> Return: "no feasible target"
  Explore --> Return: "budget near threshold"
  Return --> Dock: "arrive home/dock"
  Dock --> [*]
        

The state structure above is minimal but critical: exploration is always subordinate to safety and mission constraints.

In Section 3 we introduced Lagrangian weights \( \lambda_E,\lambda_T \). A fixed choice works, but we can also adapt them based on remaining budgets. A simple proportional rule is:

\[ \lambda_E(t) = \lambda_{E,0}\left(\frac{E_{\max} }{E_{\text{rem} }(t)}\right), \qquad \lambda_T(t) = \lambda_{T,0}\left(\frac{T_{\max} }{T_{\text{rem} }(t)}\right) \]

As budgets shrink, the multipliers grow, discouraging long detours for small gains. More principled approaches use dual ascent (treating \( \lambda \) as dual variables) by nudging them upward whenever measured consumption violates planned constraints.

Receding-horizon target selection. Let candidate set be \( \mathcal{G}_t \) (frontiers / viewpoints). For each \( g\in\mathcal{G}_t \) we compute:

\[ U(g) = \operatorname{IG}(g\mid \mathcal{D}_t) - \lambda_E(t)\widehat{C}_E(x\rightarrow g) - \lambda_T(t)\widehat{C}_T(x\rightarrow g) \]

and choose \( g^\star = \arg\max U(g) \) among those satisfying the return-feasibility inequalities from Section 5. Then execute navigation to \( g^\star \) using the already-taught local planners (DWA/TEB/etc.), update the map, and repeat.

This is the core “budget-aware exploration” pattern implemented in the code labs below.

This implementation is self-contained (no ROS). In a ROS2 system, you would replace:

• Map belief updates with OccupancyGrid / OctoMap updates (Chapter 9).
• Frontier extraction with your exploration package or custom frontier node.
• Travel cost estimates with Nav2 global planner path length.
• Execution with action clients to Nav2 (NavigateToPose).

Code: Chapter17_Lesson4.py

# Chapter17_Lesson4.py
# Exploration Under Limited Battery/Time (Budget-Aware Frontier Exploration)
# Author: course material generator
#
# This script demonstrates a simple, self-contained budget-aware exploration loop:
# - A probabilistic occupancy grid with unknown cells
# - Frontier detection
# - Utility = expected information gain - lambda * travel_cost
# - Safety constraint: must have enough budget to reach goal and return home + reserve
#
# No ROS required; "integration points" are noted in comments.

from __future__ import annotations
from dataclasses import dataclass
from typing import List, Tuple, Optional, Dict
import heapq
import math
import random

random.seed(7)

Cell = Tuple[int, int]

def clamp(x: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, x))

def entropy_bern(p: float) -> float:
    """Binary entropy H(p) in nats."""
    p = clamp(p, 1e-9, 1 - 1e-9)
    return -(p * math.log(p) + (1 - p) * math.log(1 - p))

@dataclass
class Budget:
    # energy in Joules (or arbitrary units)
    E: float
    # time in seconds (or arbitrary units)
    T: float

@dataclass
class RobotParams:
    # cost per grid step (move energy) and per step time
    E_step: float = 1.0
    T_step: float = 1.0
    # idle/base costs per step
    E_base: float = 0.10
    # reserve to guarantee safe stop/return
    E_reserve: float = 10.0

@dataclass
class SensorParams:
    # sensing radius (Chebyshev radius in grid)
    r: int = 3
    # observation quality: new occupancy probability when observed obstacle
    p_occ_obs: float = 0.95
    p_free_obs: float = 0.05

class OccupancyGrid:
    """
    Probability grid p_occ in [0,1].
    Unknown cells are initialized to 0.5.
    """
    def __init__(self, w: int, h: int):
        self.w, self.h = w, h
        self.p = [[0.5 for _ in range(w)] for _ in range(h)]

    def inb(self, c: Cell) -> bool:
        x, y = c
        return 0 <= x < self.w and 0 <= y < self.h

    def get(self, c: Cell) -> float:
        x, y = c
        return self.p[y][x]

    def set(self, c: Cell, p_occ: float) -> None:
        x, y = c
        self.p[y][x] = clamp(p_occ, 0.0, 1.0)

    def is_free(self, c: Cell, thr: float = 0.65) -> bool:
        return self.get(c) < thr

    def is_occupied(self, c: Cell, thr: float = 0.65) -> bool:
        return self.get(c) > thr

    def neighbors4(self, c: Cell) -> List[Cell]:
        x, y = c
        cand = [(x+1,y),(x-1,y),(x,y+1),(x,y-1)]
        return [q for q in cand if self.inb(q)]

def astar(grid: OccupancyGrid, s: Cell, g: Cell, occ_thr: float = 0.65) -> Optional[List[Cell]]:
    """A* on 4-neighborhood; treats p_occ > occ_thr as blocked."""
    if not grid.inb(s) or not grid.inb(g):
        return None
    if not grid.is_free(s, occ_thr) or not grid.is_free(g, occ_thr):
        return None

    def h(c: Cell) -> float:
        return abs(c[0]-g[0]) + abs(c[1]-g[1])

    openpq: List[Tuple[float, Cell]] = []
    heapq.heappush(openpq, (h(s), s))
    gscore: Dict[Cell, float] = {s: 0.0}
    parent: Dict[Cell, Cell] = {}

    while openpq:
        _, cur = heapq.heappop(openpq)
        if cur == g:
            # reconstruct
            path = [cur]
            while cur in parent:
                cur = parent[cur]
                path.append(cur)
            path.reverse()
            return path

        for nb in grid.neighbors4(cur):
            if not grid.is_free(nb, occ_thr):
                continue
            tentative = gscore[cur] + 1.0
            if nb not in gscore or tentative < gscore[nb]:
                gscore[nb] = tentative
                parent[nb] = cur
                heapq.heappush(openpq, (tentative + h(nb), nb))
    return None

def detect_frontiers(grid: OccupancyGrid, free_thr: float = 0.35) -> List[Cell]:
    """
    A frontier cell: free (p_occ < free_thr) and has a 4-neighbor unknown-ish (close to 0.5).
    """
    frontiers = []
    for y in range(grid.h):
        for x in range(grid.w):
            c = (x, y)
            p = grid.get(c)
            if p >= free_thr:
                continue
            # unknown neighbor?
            for nb in grid.neighbors4(c):
                pnb = grid.get(nb)
                if abs(pnb - 0.5) < 0.15:
                    frontiers.append(c)
                    break
    return frontiers

def expected_info_gain(grid: OccupancyGrid, pose: Cell, sensor: SensorParams) -> float:
    """
    Approximate IG as entropy reduction over cells in sensor range, assuming observation makes
    p_occ closer to {p_occ_obs or p_free_obs}. In a real system, this would integrate
    measurement model over rays (LiDAR) or pixels (vision).
    """
    x0, y0 = pose
    ig = 0.0
    for dy in range(-sensor.r, sensor.r+1):
        for dx in range(-sensor.r, sensor.r+1):
            c = (x0+dx, y0+dy)
            if not grid.inb(c):
                continue
            p0 = grid.get(c)
            H0 = entropy_bern(p0)
            # expected posterior entropy (simple symmetric mixture proxy)
            # posterior becomes more "certain" than prior
            p_post1 = clamp(sensor.p_occ_obs, 0.0, 1.0)
            p_post0 = clamp(sensor.p_free_obs, 0.0, 1.0)
            H_post = 0.5 * entropy_bern(p_post1) + 0.5 * entropy_bern(p_post0)
            # only count if cell is not already certain
            w_unc = clamp(1.0 - abs(p0 - 0.5) * 2.0, 0.0, 1.0)
            ig += w_unc * max(0.0, H0 - H_post)
    return ig

def apply_observation(grid: OccupancyGrid, truth: OccupancyGrid, pose: Cell, sensor: SensorParams) -> None:
    """Simulate observation by revealing truth in sensor neighborhood with noisy posterior."""
    x0, y0 = pose
    for dy in range(-sensor.r, sensor.r+1):
        for dx in range(-sensor.r, sensor.r+1):
            c = (x0+dx, y0+dy)
            if not grid.inb(c):
                continue
            # truth is binary obstacle vs free by thresholding its probability
            is_occ = truth.is_occupied(c, thr=0.5)
            grid.set(c, sensor.p_occ_obs if is_occ else sensor.p_free_obs)

def pick_goal_budgeted(
    grid: OccupancyGrid,
    frontiers: List[Cell],
    cur: Cell,
    home: Cell,
    budget: Budget,
    rp: RobotParams,
    sensor: SensorParams,
    lam: float = 0.25,
    occ_thr: float = 0.65,
    max_candidates: int = 80
) -> Optional[Tuple[Cell, List[Cell], float]]:
    """
    Choose frontier maximizing U = IG(goal) - lam * travel_cost
    subject to energy/time feasibility INCLUDING return-to-home.
    """
    if not frontiers:
        return None

    # subsample for speed
    if len(frontiers) > max_candidates:
        frontiers = random.sample(frontiers, max_candidates)

    best = None
    bestU = -1e18
    best_path = None

    for g in frontiers:
        path = astar(grid, cur, g, occ_thr=occ_thr)
        if path is None:
            continue
        # cost to reach g
        steps = max(0, len(path) - 1)
        E_go = steps * (rp.E_step + rp.E_base)
        T_go = steps * rp.T_step

        # return cost (shortest path from g to home)
        path_back = astar(grid, g, home, occ_thr=occ_thr)
        if path_back is None:
            # if we cannot guarantee return on current map, treat as infeasible
            continue
        steps_back = max(0, len(path_back) - 1)
        E_back = steps_back * (rp.E_step + rp.E_base)
        T_back = steps_back * rp.T_step

        # feasibility with reserve
        if budget.E - (E_go + E_back) < rp.E_reserve:
            continue
        if budget.T - (T_go + T_back) < 0:
            continue

        ig = expected_info_gain(grid, g, sensor)
        U = ig - lam * (steps)  # travel cost proxy
        if U > bestU:
            bestU = U
            best = g
            best_path = path

    if best is None:
        return None
    return best, best_path, bestU

def run_demo():
    # grid and truth
    W, H = 35, 25
    belief = OccupancyGrid(W, H)
    truth = OccupancyGrid(W, H)

    # create synthetic obstacles in truth (maze-ish)
    for y in range(H):
        for x in range(W):
            truth.set((x,y), 0.05)  # mostly free

    # borders
    for x in range(W):
        truth.set((x,0), 0.95); truth.set((x,H-1), 0.95)
    for y in range(H):
        truth.set((0,y), 0.95); truth.set((W-1,y), 0.95)

    # random rectangles
    rng = random.Random(3)
    for _ in range(8):
        x0 = rng.randint(3, W-10)
        y0 = rng.randint(3, H-8)
        ww = rng.randint(3, 7)
        hh = rng.randint(2, 5)
        for y in range(y0, min(H-1, y0+hh)):
            for x in range(x0, min(W-1, x0+ww)):
                truth.set((x,y), 0.95)

    # start/home
    home = (2, 2)
    cur = home

    rp = RobotParams(E_step=1.2, T_step=1.0, E_base=0.10, E_reserve=15.0)
    sensor = SensorParams(r=3)

    # budgets
    budget = Budget(E=160.0, T=140.0)

    # initial observation
    apply_observation(belief, truth, cur, sensor)

    explored_steps = 0
    goals_reached = 0

    while True:
        frontiers = detect_frontiers(belief)
        if not frontiers:
            print("No frontiers left. Exploration complete.")
            break

        choice = pick_goal_budgeted(
            belief, frontiers, cur, home, budget, rp, sensor,
            lam=0.35, occ_thr=0.65
        )
        if choice is None:
            print("No feasible frontier under remaining budget. Returning home.")
            break

        goal, path, U = choice
        # execute path step-by-step, update budgets and sensing
        for step in path[1:]:
            # consume
            budget.E -= (rp.E_step + rp.E_base)
            budget.T -= rp.T_step
            explored_steps += 1
            cur = step
            apply_observation(belief, truth, cur, sensor)

            if budget.E < rp.E_reserve or budget.T <= 0:
                print("Budget exhausted mid-path. Returning home (if possible).")
                break
        goals_reached += 1

        # stop if too low to continue safely
        if budget.E < rp.E_reserve or budget.T <= 0:
            break

        if goals_reached >= 40:
            print("Stop: reached demo goal limit.")
            break

    # return home (best effort)
    back = astar(belief, cur, home, occ_thr=0.65)
    if back is not None:
        for step in back[1:]:
            budget.E -= (rp.E_step + rp.E_base)
            budget.T -= rp.T_step
            cur = step

    print(f"Final pose: {cur}, steps: {explored_steps}, goals: {goals_reached}")
    print(f"Remaining budget: E={budget.E:.2f}, T={budget.T:.2f}")

if __name__ == "__main__":
    run_demo()
      

Exercise Code: Chapter17_Lesson4_Ex1.py

# Chapter17_Lesson4_Ex1.py
# Exercise: Greedy knapsack selection for monotone submodular set function
# We illustrate the (1-1/e) style idea using a toy coverage objective.

from __future__ import annotations
from typing import List, Set, Tuple
import math

def greedy_submodular_knapsack(
    items: List[Tuple[str, Set[int], float]],
    B: float
) -> List[str]:
    """
    items: list of (name, covered_elements, cost)
    objective: f(S) = |union covered|
    choose S with sum(cost) <= B maximizing f(S)
    greedy by marginal gain per cost.
    """
    chosen = []
    covered: Set[int] = set()
    remaining = items[:]
    budget = B

    while True:
        best = None
        best_ratio = -1.0
        for name, elems, cost in remaining:
            if cost > budget:
                continue
            marg = len((covered | elems)) - len(covered)
            ratio = marg / cost if cost > 0 else float("inf")
            if ratio > best_ratio and marg > 0:
                best_ratio = ratio
                best = (name, elems, cost)

        if best is None:
            break

        name, elems, cost = best
        chosen.append(name)
        covered |= elems
        budget -= cost
        remaining = [it for it in remaining if it[0] != name]

    return chosen

if __name__ == "__main__":
    # toy environment elements 0..19 to "cover"
    items = [
        ("A", {0,1,2,3,4,5}, 3.0),
        ("B", {4,5,6,7,8}, 2.0),
        ("C", {9,10,11,12}, 2.5),
        ("D", {12,13,14,15,16}, 3.0),
        ("E", {16,17,18,19}, 1.8),
        ("F", {2,3,8,9,17}, 2.2),
    ]
    B = 7.0
    S = greedy_submodular_knapsack(items, B)
    print("Budget:", B)
    print("Chosen:", S)
      

In ROS2 C++, the same selection loop would live in a node that subscribes to the map and publishes goals. Relevant libraries: rclcpp, Nav2 costmap interfaces, and TF2 for transforms. The code below remains standalone for learning clarity.

Code: Chapter17_Lesson4.cpp

// Chapter17_Lesson4.cpp
// Exploration Under Limited Battery/Time (Budget-Aware Frontier Exploration)
// Self-contained demo without ROS.
// Build: g++ -O2 -std=c++17 Chapter17_Lesson4.cpp -o demo
#include <bits/stdc++.h>
using namespace std;

struct Budget { double E, T; };
struct RobotParams { double E_step=1.2, T_step=1.0, E_base=0.10, E_reserve=15.0; };
struct SensorParams { int r=3; };

static inline double clampd(double x,double lo,double hi){ return max(lo,min(hi,x)); }

static double entropyBern(double p){
    p = clampd(p, 1e-9, 1.0-1e-9);
    return -(p*log(p) + (1.0-p)*log(1.0-p));
}

struct Grid {
    int W,H;
    vector<double> p; // row-major p_occ
    Grid(int w,int h):W(w),H(h),p(w*h,0.5){}
    bool inb(int x,int y) const { return 0<=x && x<W && 0<=y && y<H; }
    double get(int x,int y) const { return p[y*W+x]; }
    void setv(int x,int y,double v){ p[y*W+x]=clampd(v,0.0,1.0); }
    bool isFree(int x,int y,double thr=0.65) const { return get(x,y) < thr; }
    bool isOcc(int x,int y,double thr=0.65) const { return get(x,y) > thr; }
};

using Cell = pair<int,int>;

static vector<Cell> neigh4(const Grid& g, Cell c){
    int x=c.first, y=c.second;
    vector<Cell> cand={ {x+1,y},{x-1,y},{x,y+1},{x,y-1} };
    vector<Cell> out;
    for(auto &q:cand) if(g.inb(q.first,q.second)) out.push_back(q);
    return out;
}

static vector<Cell> astar(const Grid& g, Cell s, Cell goal, double occ_thr=0.65){
    if(!g.inb(s.first,s.second) || !g.inb(goal.first,goal.second)) return {};
    if(!g.isFree(s.first,s.second,occ_thr) || !g.isFree(goal.first,goal.second,occ_thr)) return {};
    auto h = [&](Cell c){ return abs(c.first-goal.first)+abs(c.second-goal.second); };

    unordered_map<long long,double> gscore;
    unordered_map<long long, long long> parent;
    auto key=[&](Cell c)->long long{ return (long long)c.second*g.W + c.first; };

    struct Node{ double f; Cell c; };
    struct Cmp{ bool operator()(const Node& a,const Node& b) const { return a.f>b.f; } };
    priority_queue<Node, vector<Node>, Cmp> pq;
    pq.push({(double)h(s), s});
    gscore[key(s)] = 0.0;

    while(!pq.empty()){
        auto cur = pq.top().c; pq.pop();
        if(cur==goal){
            vector<Cell> path; path.push_back(cur);
            long long k=key(cur);
            while(parent.count(k)){
                k = parent[k];
                path.push_back({(int)(k%g.W), (int)(k/g.W)});
            }
            reverse(path.begin(), path.end());
            return path;
        }
        long long kc=key(cur);
        double gc=gscore[kc];
        for(auto nb: neigh4(g,cur)){
            if(!g.isFree(nb.first, nb.second, occ_thr)) continue;
            long long kn=key(nb);
            double tent = gc + 1.0;
            if(!gscore.count(kn) || tent < gscore[kn]){
                gscore[kn]=tent;
                parent[kn]=kc;
                pq.push({tent + (double)h(nb), nb});
            }
        }
    }
    return {};
}

static vector<Cell> detectFrontiers(const Grid& g, double free_thr=0.35){
    vector<Cell> fr;
    for(int y=0;y<g.H;y++){
        for(int x=0;x<g.W;x++){
            double p=g.get(x,y);
            if(p >= free_thr) continue;
            for(auto nb: neigh4(g,{x,y})){
                double pn=g.get(nb.first, nb.second);
                if(fabs(pn-0.5) < 0.15){ fr.push_back({x,y}); break; }
            }
        }
    }
    return fr;
}

static double expectedIG(const Grid& belief, Cell pose, const SensorParams& sp){
    double ig=0.0;
    for(int dy=-sp.r; dy<=sp.r; dy++){
        for(int dx=-sp.r; dx<=sp.r; dx++){
            int x=pose.first+dx, y=pose.second+dy;
            if(!belief.inb(x,y)) continue;
            double p0 = belief.get(x,y);
            double H0 = entropyBern(p0);
            // simple proxy posterior entropy
            double Hpost = 0.5*entropyBern(0.95) + 0.5*entropyBern(0.05);
            double w = clampd(1.0 - fabs(p0-0.5)*2.0, 0.0, 1.0);
            ig += w * max(0.0, H0 - Hpost);
        }
    }
    return ig;
}

static void applyObs(Grid& belief, const Grid& truth, Cell pose, const SensorParams& sp){
    for(int dy=-sp.r; dy<=sp.r; dy++){
        for(int dx=-sp.r; dx<=sp.r; dx++){
            int x=pose.first+dx, y=pose.second+dy;
            if(!belief.inb(x,y)) continue;
            bool isOcc = truth.isOcc(x,y,0.5);
            belief.setv(x,y, isOcc ? 0.95 : 0.05);
        }
    }
}

static bool pickGoalBudgeted(
    const Grid& belief,
    const vector<Cell>& frontiers,
    Cell cur, Cell home,
    const Budget& budget,
    const RobotParams& rp,
    const SensorParams& sp,
    double lam,
    Cell& bestGoal,
    vector<Cell>& bestPath
){
    double bestU = -1e18;
    bool found=false;
    // subsample if huge
    vector<Cell> cand=frontiers;
    if((int)cand.size()>80){
        std::mt19937 rng(7);
        shuffle(cand.begin(), cand.end(), rng);
        cand.resize(80);
    }

    for(auto g: cand){
        auto path = astar(belief, cur, g, 0.65);
        if(path.empty()) continue;
        int steps = max(0, (int)path.size()-1);
        double Ego = steps*(rp.E_step + rp.E_base);
        double Tgo = steps*rp.T_step;

        auto back = astar(belief, g, home, 0.65);
        if(back.empty()) continue;
        int bsteps = max(0, (int)back.size()-1);
        double Eback = bsteps*(rp.E_step + rp.E_base);
        double Tback = bsteps*rp.T_step;

        if(budget.E - (Ego+Eback) < rp.E_reserve) continue;
        if(budget.T - (Tgo+Tback) < 0) continue;

        double ig = expectedIG(belief, g, sp);
        double U = ig - lam*steps;
        if(U > bestU){
            bestU=U; bestGoal=g; bestPath=path; found=true;
        }
    }
    return found;
}

int main(){
    int W=35, H=25;
    Grid belief(W,H), truth(W,H);

    // truth: mostly free
    for(int y=0;y<H;y++) for(int x=0;x<W;x++) truth.setv(x,y,0.05);

    // borders
    for(int x=0;x<W;x++){ truth.setv(x,0,0.95); truth.setv(x,H-1,0.95); }
    for(int y=0;y<H;y++){ truth.setv(0,y,0.95); truth.setv(W-1,y,0.95); }

    // random rectangles
    std::mt19937 rng(3);
    auto rint=[&](int a,int b){ std::uniform_int_distribution<int> d(a,b); return d(rng); };
    for(int k=0;k<8;k++){
        int x0=rint(3,W-10), y0=rint(3,H-8);
        int ww=rint(3,7), hh=rint(2,5);
        for(int y=y0;y<min(H-1,y0+hh);y++)
            for(int x=x0;x<min(W-1,x0+ww);x++)
                truth.setv(x,y,0.95);
    }

    Cell home={2,2}, cur=home;

    RobotParams rp;
    SensorParams sp;
    Budget bud{160.0, 140.0};

    applyObs(belief, truth, cur, sp);

    int stepsDone=0, goals=0;

    while(true){
        auto fr = detectFrontiers(belief);
        if(fr.empty()){
            cout << "No frontiers left.\n"; break;
        }
        Cell goal; vector<Cell> path;
        bool ok = pickGoalBudgeted(belief, fr, cur, home, bud, rp, sp, 0.35, goal, path);
        if(!ok){
            cout << "No feasible frontier under budget. Returning.\n";
            break;
        }
        for(size_t i=1;i<path.size();i++){
            bud.E -= (rp.E_step + rp.E_base);
            bud.T -= rp.T_step;
            stepsDone++;
            cur = path[i];
            applyObs(belief, truth, cur, sp);
            if(bud.E < rp.E_reserve || bud.T <= 0) break;
        }
        goals++;
        if(bud.E < rp.E_reserve || bud.T <= 0) break;
        if(goals >= 40) { cout << "Stop: demo goal limit.\n"; break; }
    }

    auto back = astar(belief, cur, home, 0.65);
    if(!back.empty()){
        for(size_t i=1;i<back.size();i++){
            bud.E -= (rp.E_step + rp.E_base);
            bud.T -= rp.T_step;
            cur = back[i];
        }
    }
    cout << "Final pose: ("<<cur.first<<","<<cur.second<<") steps="<<stepsDone<<" goals="<<goals<<"\n";
    cout << "Remaining budget: E="<<bud.E<<" T="<<bud.T<<"\n";
    return 0;
}
      

Java is less common on embedded AMRs today, but the algorithmic structure is identical. If integrating with robotics middleware, typical options include bridging to ROS via network APIs or using Java-based robotics frameworks. The code below demonstrates the full budget-aware loop without middleware.

Code: Chapter17_Lesson4.java

// Chapter17_Lesson4.java
// Exploration Under Limited Battery/Time (Budget-Aware Frontier Exploration)
// Self-contained demo without ROS. Designed for clarity, not maximum performance.
// Compile: javac Chapter17_Lesson4.java
// Run:     java Chapter17_Lesson4
import java.util.*;

public class Chapter17_Lesson4 {

    static class Budget { double E, T; Budget(double E,double T){this.E=E;this.T=T;} }
    static class RobotParams { double EStep=1.2, TStep=1.0, EBase=0.10, EReserve=15.0; }
    static class SensorParams { int r=3; }

    static double clamp(double x,double lo,double hi){ return Math.max(lo, Math.min(hi, x)); }
    static double entropyBern(double p){
        p = clamp(p, 1e-9, 1.0-1e-9);
        return -(p*Math.log(p) + (1.0-p)*Math.log(1.0-p));
    }

    static class Grid {
        int W,H;
        double[] p; // row-major
        Grid(int W,int H){ this.W=W; this.H=H; p=new double[W*H]; Arrays.fill(p, 0.5); }
        boolean inb(int x,int y){ return 0<=x && x<W && 0<=y && y<H; }
        int idx(int x,int y){ return y*W+x; }
        double get(int x,int y){ return p[idx(x,y)]; }
        void set(int x,int y,double v){ p[idx(x,y)] = clamp(v, 0.0, 1.0); }
        boolean isFree(int x,int y,double thr){ return get(x,y) < thr; }
        boolean isOcc(int x,int y,double thr){ return get(x,y) > thr; }
    }

    static class Cell {
        int x,y;
        Cell(int x,int y){ this.x=x; this.y=y; }
        @Override public boolean equals(Object o){
            if(!(o instanceof Cell)) return false;
            Cell c=(Cell)o; return x==c.x && y==c.y;
        }
        @Override public int hashCode(){ return Objects.hash(x,y); }
    }

    static List<Cell> neigh4(Grid g, Cell c){
        int x=c.x,y=c.y;
        int[][] cand={ {x+1,y},{x-1,y},{x,y+1},{x,y-1} };
        ArrayList<Cell> out=new ArrayList<>();
        for(int[] q:cand) if(g.inb(q[0],q[1])) out.add(new Cell(q[0],q[1]));
        return out;
    }

    static List<Cell> astar(Grid g, Cell s, Cell goal, double occThr){
        if(!g.inb(s.x,s.y) || !g.inb(goal.x,goal.y)) return Collections.emptyList();
        if(!g.isFree(s.x,s.y,occThr) || !g.isFree(goal.x,goal.y,occThr)) return Collections.emptyList();

        class Node { double f; Cell c; Node(double f,Cell c){this.f=f;this.c=c;} }
        PriorityQueue<Node> pq=new PriorityQueue<>(Comparator.comparingDouble(a->a.f));
        Map<Cell, Double> gscore=new HashMap<>();
        Map<Cell, Cell> parent=new HashMap<>();

        java.util.function.ToDoubleFunction<Cell> h = (Cell c) -> Math.abs(c.x-goal.x)+Math.abs(c.y-goal.y);

        pq.add(new Node(h.applyAsDouble(s), s));
        gscore.put(s, 0.0);

        while(!pq.isEmpty()){
            Cell cur=pq.poll().c;
            if(cur.equals(goal)){
                ArrayList<Cell> path=new ArrayList<>();
                path.add(cur);
                while(parent.containsKey(cur)){
                    cur = parent.get(cur);
                    path.add(cur);
                }
                Collections.reverse(path);
                return path;
            }
            double gc = gscore.get(cur);
            for(Cell nb: neigh4(g, cur)){
                if(!g.isFree(nb.x, nb.y, occThr)) continue;
                double tent = gc + 1.0;
                if(!gscore.containsKey(nb) || tent < gscore.get(nb)){
                    gscore.put(nb, tent);
                    parent.put(nb, cur);
                    pq.add(new Node(tent + h.applyAsDouble(nb), nb));
                }
            }
        }
        return Collections.emptyList();
    }

    static List<Cell> detectFrontiers(Grid g, double freeThr){
        ArrayList<Cell> fr=new ArrayList<>();
        for(int y=0;y<g.H;y++){
            for(int x=0;x<g.W;x++){
                if(g.get(x,y) >= freeThr) continue;
                Cell c=new Cell(x,y);
                for(Cell nb: neigh4(g,c)){
                    double pn = g.get(nb.x, nb.y);
                    if(Math.abs(pn-0.5) < 0.15){ fr.add(c); break; }
                }
            }
        }
        return fr;
    }

    static double expectedIG(Grid belief, Cell pose, SensorParams sp){
        double ig=0.0;
        for(int dy=-sp.r; dy<=sp.r; dy++){
            for(int dx=-sp.r; dx<=sp.r; dx++){
                int x=pose.x+dx, y=pose.y+dy;
                if(!belief.inb(x,y)) continue;
                double p0=belief.get(x,y);
                double H0=entropyBern(p0);
                double Hpost=0.5*entropyBern(0.95) + 0.5*entropyBern(0.05);
                double w=clamp(1.0 - Math.abs(p0-0.5)*2.0, 0.0, 1.0);
                ig += w * Math.max(0.0, H0 - Hpost);
            }
        }
        return ig;
    }

    static void applyObs(Grid belief, Grid truth, Cell pose, SensorParams sp){
        for(int dy=-sp.r; dy<=sp.r; dy++){
            for(int dx=-sp.r; dx<=sp.r; dx++){
                int x=pose.x+dx, y=pose.y+dy;
                if(!belief.inb(x,y)) continue;
                boolean occ = truth.isOcc(x,y,0.5);
                belief.set(x,y, occ ? 0.95 : 0.05);
            }
        }
    }

    static class Choice { Cell goal; List<Cell> path; double U; Choice(Cell g,List<Cell> p,double U){this.goal=g;this.path=p;this.U=U;} }

    static Choice pickGoalBudgeted(
        Grid belief, List<Cell> frontiers, Cell cur, Cell home,
        Budget bud, RobotParams rp, SensorParams sp, double lam, double occThr
    ){
        if(frontiers.isEmpty()) return null;

        // subsample
        if(frontiers.size() > 80){
            Collections.shuffle(frontiers, new Random(7));
            frontiers = frontiers.subList(0, 80);
        }

        double bestU=-1e18;
        Choice best=null;

        for(Cell g: frontiers){
            List<Cell> path = astar(belief, cur, g, occThr);
            if(path.isEmpty()) continue;
            int steps = Math.max(0, path.size()-1);
            double Ego = steps*(rp.EStep + rp.EBase);
            double Tgo = steps*rp.TStep;

            List<Cell> back = astar(belief, g, home, occThr);
            if(back.isEmpty()) continue;
            int bsteps = Math.max(0, back.size()-1);
            double Eback = bsteps*(rp.EStep + rp.EBase);
            double Tback = bsteps*rp.TStep;

            if(bud.E - (Ego+Eback) < rp.EReserve) continue;
            if(bud.T - (Tgo+Tback) < 0) continue;

            double ig = expectedIG(belief, g, sp);
            double U = ig - lam*steps;
            if(U > bestU){
                bestU=U;
                best = new Choice(g, path, U);
            }
        }
        return best;
    }

    public static void main(String[] args){
        int W=35, H=25;
        Grid belief=new Grid(W,H);
        Grid truth=new Grid(W,H);

        // truth: mostly free
        for(int y=0;y<H;y++) for(int x=0;x<W;x++) truth.set(x,y,0.05);

        // borders
        for(int x=0;x<W;x++){ truth.set(x,0,0.95); truth.set(x,H-1,0.95); }
        for(int y=0;y<H;y++){ truth.set(0,y,0.95); truth.set(W-1,y,0.95); }

        // random rectangles
        Random rng=new Random(3);
        for(int k=0;k<8;k++){
            int x0=3 + rng.nextInt(W-10-3+1);
            int y0=3 + rng.nextInt(H-8-3+1);
            int ww=3 + rng.nextInt(7-3+1);
            int hh=2 + rng.nextInt(5-2+1);
            for(int y=y0;y<Math.min(H-1, y0+hh); y++)
                for(int x=x0;x<Math.min(W-1, x0+ww); x++)
                    truth.set(x,y,0.95);
        }

        Cell home=new Cell(2,2);
        Cell cur=home;

        RobotParams rp=new RobotParams();
        SensorParams sp=new SensorParams();
        Budget bud=new Budget(160.0, 140.0);

        applyObs(belief, truth, cur, sp);

        int stepsDone=0, goals=0;

        while(true){
            List<Cell> fr = detectFrontiers(belief, 0.35);
            if(fr.isEmpty()){
                System.out.println("No frontiers left.");
                break;
            }
            Choice ch = pickGoalBudgeted(belief, fr, cur, home, bud, rp, sp, 0.35, 0.65);
            if(ch == null){
                System.out.println("No feasible frontier under budget. Returning.");
                break;
            }
            for(int i=1;i<ch.path.size();i++){
                bud.E -= (rp.EStep + rp.EBase);
                bud.T -= rp.TStep;
                stepsDone++;
                cur = ch.path.get(i);
                applyObs(belief, truth, cur, sp);
                if(bud.E < rp.EReserve || bud.T <= 0) break;
            }
            goals++;
            if(bud.E < rp.EReserve || bud.T <= 0) break;
            if(goals >= 40){ System.out.println("Stop: demo goal limit."); break; }
        }

        List<Cell> back = astar(belief, cur, home, 0.65);
        if(!back.isEmpty()){
            for(int i=1;i<back.size();i++){
                bud.E -= (rp.EStep + rp.EBase);
                bud.T -= rp.TStep;
                cur = back.get(i);
            }
        }

        System.out.printf("Final pose: (%d,%d) steps=%d goals=%d%n", cur.x, cur.y, stepsDone, goals);
        System.out.printf("Remaining budget: E=%.2f T=%.2f%n", bud.E, bud.T);
    }
}
      

MATLAB is excellent for quickly validating exploration logic and budget controllers. For Simulink: implement the selection policy in a MATLAB Function block and manage Explore/Return logic using Stateflow. If you have Robotics System Toolbox, you can also connect to ROS/ROS2 and replace the internal map with live occupancy grids.

Code: Chapter17_Lesson4.m

% Chapter17_Lesson4.m
% Exploration Under Limited Battery/Time (Budget-Aware Frontier Exploration)
% Self-contained MATLAB demo (no toolboxes required).
%
% Notes for Simulink:
% - You can implement the "budget update" and "goal selection" blocks in Simulink
%   using MATLAB Function blocks, then connect a discrete-time integrator for
%   time and energy and a state machine (Stateflow) for Explore/Return states.

clear; clc; rng(7);

W = 35; H = 25;
belief = 0.5 * ones(H, W);        % p_occ belief
truth  = 0.05 * ones(H, W);       % synthetic truth (mostly free)

% Borders as obstacles
truth(1,:) = 0.95; truth(end,:) = 0.95;
truth(:,1) = 0.95; truth(:,end) = 0.95;

% Random rectangles
rng(3);
for k=1:8
    x0 = randi([4, W-10]); y0 = randi([4, H-8]);
    ww = randi([3, 7]);    hh = randi([2, 5]);
    x1 = min(W-1, x0+ww);  y1 = min(H-1, y0+hh);
    truth(y0:y1, x0:x1) = 0.95;
end

home = [3, 3];   % [x,y] with 1-based indexing
cur  = home;

% Robot/budget params
E_step = 1.2; E_base = 0.10; T_step = 1.0;
E_reserve = 15.0;
E = 160.0; T = 140.0;

% Sensor params
r = 3;
p_occ_obs = 0.95; p_free_obs = 0.05;

% Initial observation
belief = apply_observation(belief, truth, cur, r, p_occ_obs, p_free_obs);

goals = 0; stepsDone = 0;

while true
    frontiers = detect_frontiers(belief, 0.35);
    if isempty(frontiers)
        disp("No frontiers left."); break;
    end

    [goal, path] = pick_goal_budgeted(belief, frontiers, cur, home, ...
        E, T, E_step, E_base, T_step, E_reserve, r, 0.35);

    if isempty(goal)
        disp("No feasible frontier under budget. Returning."); break;
    end

    % Execute path (skip first cell)
    for i = 2:size(path,1)
        E = E - (E_step + E_base);
        T = T - T_step;
        stepsDone = stepsDone + 1;
        cur = path(i,:);
        belief = apply_observation(belief, truth, cur, r, p_occ_obs, p_free_obs);
        if (E < E_reserve) || (T <= 0)
            disp("Budget exhausted mid-path."); break;
        end
    end

    goals = goals + 1;
    if (E < E_reserve) || (T <= 0), break; end
    if goals >= 40, disp("Stop: demo goal limit."); break; end
end

fprintf("Final pose: (%d,%d) steps=%d goals=%d\n", cur(1), cur(2), stepsDone, goals);
fprintf("Remaining budget: E=%.2f T=%.2f\n", E, T);

% --------------------------- Helper functions ---------------------------

function frontiers = detect_frontiers(belief, free_thr)
    [H,W] = size(belief);
    frontiers = [];
    for y=1:H
        for x=1:W
            if belief(y,x) >= free_thr, continue; end
            nbs = neighbors4([x,y], W, H);
            for k=1:size(nbs,1)
                xn=nbs(k,1); yn=nbs(k,2);
                if abs(belief(yn,xn) - 0.5) < 0.15
                    frontiers = [frontiers; x,y]; %#ok<AGROW>
                    break;
                end
            end
        end
    end
end

function nbs = neighbors4(c, W, H)
    x=c(1); y=c(2);
    cand = [x+1,y; x-1,y; x,y+1; x,y-1];
    keep = cand(:,1)>=1 & cand(:,1)<=W & cand(:,2)>=1 & cand(:,2)<=H;
    nbs = cand(keep,:);
end

function belief = apply_observation(belief, truth, pose, r, p_occ_obs, p_free_obs)
    [H,W] = size(belief);
    x0=pose(1); y0=pose(2);
    for dy=-r:r
        for dx=-r:r
            x=x0+dx; y=y0+dy;
            if x<1 || x>W || y<1 || y>H, continue; end
            isOcc = truth(y,x) > 0.5;
            belief(y,x) = (isOcc)*p_occ_obs + (~isOcc)*p_free_obs;
        end
    end
end

function ig = expected_info_gain(belief, pose, r)
    % nats, approximate entropy reduction proxy
    x0=pose(1); y0=pose(2);
    [H,W] = size(belief);
    ig = 0.0;
    Hpost = 0.5*bern_entropy(0.95) + 0.5*bern_entropy(0.05);
    for dy=-r:r
        for dx=-r:r
            x=x0+dx; y=y0+dy;
            if x<1 || x>W || y<1 || y>H, continue; end
            p0 = belief(y,x);
            H0 = bern_entropy(p0);
            w = max(0.0, min(1.0, 1.0 - abs(p0-0.5)*2.0));
            ig = ig + w * max(0.0, H0 - Hpost);
        end
    end
end

function H = bern_entropy(p)
    p = max(1e-9, min(1-1e-9, p));
    H = -(p*log(p) + (1-p)*log(1-p));
end

function [goal, bestPath] = pick_goal_budgeted(belief, frontiers, cur, home, ...
        E, T, E_step, E_base, T_step, E_reserve, r, lam)

    goal = []; bestPath = [];
    bestU = -1e18;

    % subsample
    if size(frontiers,1) > 80
        idx = randperm(size(frontiers,1), 80);
        frontiers = frontiers(idx,:);
    end

    for k=1:size(frontiers,1)
        g = frontiers(k,:);
        path = astar_grid(belief, cur, g, 0.65);
        if isempty(path), continue; end

        steps = size(path,1) - 1;
        Ego = steps*(E_step + E_base);
        Tgo = steps*T_step;

        back = astar_grid(belief, g, home, 0.65);
        if isempty(back), continue; end
        bsteps = size(back,1) - 1;
        Eback = bsteps*(E_step + E_base);
        Tback = bsteps*T_step;

        if (E - (Ego+Eback) < E_reserve), continue; end
        if (T - (Tgo+Tback) < 0), continue; end

        ig = expected_info_gain(belief, g, r);
        U = ig - lam*steps;

        if U > bestU
            bestU = U;
            goal = g;
            bestPath = path;
        end
    end
end

function path = astar_grid(belief, s, g, occThr)
    % 4-neighbor A*; blocks if belief(y,x) > occThr
    [H,W] = size(belief);
    if any(s<1) || s(1)>W || s(2)>H || any(g<1) || g(1)>W || g(2)>H
        path = []; return;
    end
    if belief(s(2),s(1)) > occThr || belief(g(2),g(1)) > occThr
        path = []; return;
    end

    % maps (use linear indices)
    idx = @(x,y) (y-1)*W + x;
    pos = @(k) [mod(k-1,W)+1, floor((k-1)/W)+1];

    open = java.util.PriorityQueue();
    gscore = containers.Map('KeyType','int32','ValueType','double');
    parent = containers.Map('KeyType','int32','ValueType','int32');

    hs = abs(s(1)-g(1)) + abs(s(2)-g(2));
    open.add({hs, idx(s(1),s(2))});
    gscore(idx(s(1),s(2))) = 0.0;

    visited = containers.Map('KeyType','int32','ValueType','logical');

    while ~open.isEmpty()
        node = open.remove();
        curk = node{2};
        if isKey(visited, curk), continue; end
        visited(curk) = true;

        c = pos(curk);
        if all(c==g)
            % reconstruct
            path = c;
            while isKey(parent, curk)
                curk = parent(curk);
                path = [pos(curk); path]; %#ok<AGROW>
            end
            return;
        end

        nbs = neighbors4(c, W, H);
        for i=1:size(nbs,1)
            nb = nbs(i,:);
            if belief(nb(2),nb(1)) > occThr, continue; end
            nbk = idx(nb(1),nb(2));
            tent = gscore(curk) + 1.0;
            if ~isKey(gscore, nbk) || tent < gscore(nbk)
                gscore(nbk) = tent;
                parent(nbk) = curk;
                f = tent + abs(nb(1)-g(1)) + abs(nb(2)-g(2));
                open.add({f, nbk});
            end
        end
    end
    path = [];
end
      

Mathematica is useful for symbolic reasoning about the Lagrangian relaxation and for experimenting with submodular selection rules. The following notebook-style Wolfram Language code (saved as .nb) includes (i) a toy Lagrangian maximization and (ii) a greedy knapsack selection for coverage.

Code: Chapter17_Lesson4.nb

(* Chapter17_Lesson4.nb
   Exploration Under Limited Battery/Time (Budget-Aware Exploration)
   Wolfram Language code (can be pasted into a Mathematica notebook).
*)

ClearAll["Global`*"];

(* 1) Utility / constrained optimization view *)
(* Maximize expected info gain G(x) subject to energy/time cost constraints. *)
(* Use a Lagrangian relaxation: maximize  G(x) - lambda_E C_E(x) - lambda_T C_T(x). *)

G[x_] := Exp[-0.25 (x - 6)^2] + 0.5 Exp[-0.2 (x - 14)^2];   (* toy "information" peaks *)
CE[x_] := 1.2 x + 0.10 x;                                  (* linearized energy cost *)
CT[x_] := x;                                               (* time cost *)

lambdaE = 0.05;
lambdaT = 0.02;

U[x_] := G[x] - lambdaE CE[x] - lambdaT CT[x];

sol = NMaximize[{U[x], 0 <= x <= 20}, x]
Plot[{G[x], U[x]}, {x, 0, 20}, PlotLegends -> {"G(x)", "U(x)"}]

(* 2) Submodular "coverage" objective under knapsack: f(S)=|Union coverage| *)
(* Greedy by marginal gain per cost. *)

elements = Range[0, 19];
items = {
   <|"name" -> "A", "cov" -> {0, 1, 2, 3, 4, 5}, "cost" -> 3.0|>,
   <|"name" -> "B", "cov" -> {4, 5, 6, 7, 8}, "cost" -> 2.0|>,
   <|"name" -> "C", "cov" -> {9, 10, 11, 12}, "cost" -> 2.5|>,
   <|"name" -> "D", "cov" -> {12, 13, 14, 15, 16}, "cost" -> 3.0|>,
   <|"name" -> "E", "cov" -> {16, 17, 18, 19}, "cost" -> 1.8|>,
   <|"name" -> "F", "cov" -> {2, 3, 8, 9, 17}, "cost" -> 2.2|>
};

f[S_] := Length[Union @@ (Lookup[S, "cov"])];

GreedyKnapsack[items_, B_] := Module[{S = {}, covered = {}, rem = items, budget = B, best, bestRatio},
  While[True,
    best = None; bestRatio = -Infinity;
    Do[
      If[item["cost"] <= budget,
        With[{marg = Length[Union[covered, item["cov"]]] - Length[covered]},
          If[marg > 0,
            With[{ratio = marg/item["cost"]},
              If[ratio > bestRatio, bestRatio = ratio; best = item]
            ]
          ]
        ]
      ],
      {item, rem}
    ];
    If[best === None, Break[]];
    AppendTo[S, best];
    covered = Union[covered, best["cov"]];
    budget -= best["cost"];
    rem = DeleteCases[rem, best];
  ];
  <|"chosen" -> Lookup[S, "name"], "coveredCount" -> Length[covered], "budgetLeft" -> budget|>
];

GreedyKnapsack[items, 7.0]
      

Problem 1 (Return-feasibility under energy budget): A robot has remaining energy \( E_{\text{rem} }=120 \) (arb. units) and reserve \( E_{\text{res} }=20 \). The estimated energy-per-meter is \( c_E=2 \) and base term is ignored. Current-to-goal distance is \( d(x,g)=15 \) meters and goal-to-home is \( d(g,h)=18 \) meters. Is \( g \) feasible?

Solution: Estimated required travel energy is:

\[ \widehat{C}_E(x\rightarrow g) + \widehat{C}_E(g\rightarrow h) = c_E\big(d(x,g)+d(g,h)\big) = 2(15+18)=66 \]

The feasibility condition is \( E_{\text{rem} } - 66 \ge E_{\text{res} } \), i.e., \( 120-66=54 \ge 20 \). Feasible.

Problem 2 (Lagrangian selection between two frontiers): Two candidate targets have information gains \( \operatorname{IG}_1=9 \), \( \operatorname{IG}_2=7 \). Their travel costs (in steps) are \( c_1=20 \), \( c_2=10 \). Using a utility \( U=\operatorname{IG} - \lambda c \), find the range of \( \lambda \) where target 2 is preferred.

Solution: Prefer 2 when:

\[ 7 - \lambda(10) \ge 9 - \lambda(20) \;\;\Longleftrightarrow\;\; -2 \ge -10\lambda \;\;\Longleftrightarrow\;\; \lambda \ge 0.2 \]

For \( \lambda \ge 0.2 \), cost is weighted strongly enough that the cheaper target wins.

Problem 3 (Submodularity of coverage): Let \( f(S)=\left|\bigcup_{a\in S}\mathcal{V}(a)\right| \). Prove the diminishing returns property: for \( A \subseteq B \) and \( a\notin B \), \( f(A\cup\{a\})-f(A)\ge f(B\cup\{a\})-f(B) \).

Solution: Write the marginal gains as new elements introduced by \( a \):

\[ f(A\cup\{a\})-f(A) = \left|\mathcal{V}(a)\setminus \bigcup_{u\in A}\mathcal{V}(u)\right| \]

\[ f(B\cup\{a\})-f(B) = \left|\mathcal{V}(a)\setminus \bigcup_{u\in B}\mathcal{V}(u)\right| \]

Since \( A \subseteq B \), we have \( \bigcup_{u\in A}\mathcal{V}(u) \subseteq \bigcup_{u\in B}\mathcal{V}(u) \), therefore the set subtracted in the first expression is smaller, and the remaining set is larger: the cardinality cannot decrease when subtracting a smaller set. Hence the inequality holds.

Problem 4 (Greedy gap decay inequality): Assume a monotone submodular \( f \) and a knapsack budget \( B \). Show that if greedy picks \( a_{k+1} \) with cost \( c_{k+1} \), then a typical gap inequality can be written as: \( f(S_{k+1}) - f(S_k) \ge \frac{c_{k+1} }{B}(f(S^\star)-f(S_k)) \) under the “average remaining density” argument.

Solution: Consider an optimal set \( S^\star \) with total cost at most \( B \). By submodularity and monotonicity, the total gain of adding all items in \( S^\star \) to \( S_k \) is:

\[ f(S_k \cup S^\star) - f(S_k) \le \sum_{a\in S^\star}\big(f(S_k\cup\{a\}) - f(S_k)\big) \]

The left-hand side is at least \( f(S^\star)-f(S_k) \) by monotonicity. Hence:

\[ f(S^\star)-f(S_k) \le \sum_{a\in S^\star}\Delta_k(a) \]

where \( \Delta_k(a)=f(S_k\cup\{a\})-f(S_k) \). Divide and multiply by costs and use that \( \sum_{a\in S^\star}c(a)\le B \). There must exist some item in \( S^\star \) whose density satisfies:

\[ \max_{a}\frac{\Delta_k(a)}{c(a)} \ge \frac{f(S^\star)-f(S_k)}{B} \]

Greedy chooses an item with density at least this maximum; thus for the chosen item \( a_{k+1} \): \( \Delta_k(a_{k+1}) \ge \frac{c_{k+1} }{B}(f(S^\star)-f(S_k)) \), proving the stated inequality.

Problem 5 (Triangle-inequality “go/no-go” test on grids): Suppose your planner uses a metric distance \( d \) (shortest path) satisfying triangle inequality. Show that if \( d(x,g) + d(g,h) \le D_{\max} \), then any intermediate point \( y \) on a shortest path from \( x \) to \( g \) also satisfies \( d(y,h) \le D_{\max} - d(x,y) \).

Solution: Let \( y \) lie on a shortest path from \( x \) to \( g \), so \( d(x,g)=d(x,y)+d(y,g) \). Then:

\[ d(y,h) \le d(y,g)+d(g,h) = \big(d(x,g)-d(x,y)\big)+d(g,h) \]

Using the assumption \( d(x,g)+d(g,h)\le D_{\max} \), we get:

\[ d(y,h) \le D_{\max} - d(x,y) \]

This inequality justifies a conservative “keep return margin” rule while executing a path.

We modeled exploration under energy and time budgets, introduced a constrained information-gain objective, and derived practical selection rules via Lagrangian relaxation. We then connected exploration viewpoint selection to submodular maximization under knapsack, explaining why greedy “gain-per-cost” heuristics are effective. Finally, we enforced terminal feasibility (must-return constraint) and implemented a receding-horizon budget-aware frontier explorer in Python, C++, Java, MATLAB, and Mathematica.