Chapter 17: Exploration and Active Mapping
Lesson 5: Lab: Autonomous Exploration in Unknown Map
This lab integrates the exploration concepts from Lessons 1–4 into a complete single-robot autonomy loop: build an occupancy grid online, detect frontiers, score candidate goals using information gain and travel cost, dispatch goals to a local navigation stack, and evaluate exploration performance with quantitative metrics. The emphasis is on active mapping: choosing actions that reduce map uncertainty fastest while respecting safety and time/battery constraints.
1. Lab Outcomes and Setup
1.1 Learning outcomes
- Implement an online exploration manager that selects navigation goals from the evolving occupancy grid.
- Compute frontier sets and cluster them into candidate goal regions.
- Define and tune a goal score that trades off information gain, travel cost, and risk.
- Define termination conditions and logging metrics (coverage, entropy, travel, collisions/timeouts).
1.2 Prerequisites (from previous chapters)
- Occupancy grids and log-odds mapping (Chapter 9).
- Scan-based mapping / SLAM interfaces (Chapters 10–12).
- Navigation stack interfaces and goal dispatching (Chapter 14).
- Frontier exploration, information gain, and next-best-view ideas (Chapter 17, Lessons 1–3).
- Time/battery constraints and safe termination (Lesson 4).
1.3 Deliverables
- A working exploration loop in simulation (grid-world demo provided; ROS2 integration optional).
- A short report: plots of explored fraction and entropy vs time, plus a discussion of tuning.
- Answer the problems at the end (with derivations).
2. Mathematical Foundations for Autonomous Exploration
2.1 Occupancy grid belief and entropy
Let the map be a grid of cells indexed by \( i \in \{1,\dots,N\} \). The occupancy belief at time \( t \) is \( p_t(i) = \Pr(\text{cell } i \text{ is occupied} \mid z_{1:t}, u_{1:t}) \). In the log-odds form, \( \ell_t(i) = \log\frac{p_t(i)}{1-p_t(i)} \) and \( p_t(i) = \sigma(\ell_t(i)) \) with sigmoid \( \sigma(\cdot) \).
For a Bernoulli cell, the Shannon entropy (in nats) is \( H(p) = -p\log p - (1-p)\log(1-p) \). Total map entropy is \( \mathcal{H}(t)=\sum_{i=1}^{N} H(p_t(i)) \). In exploration, a common objective is to maximize entropy reduction \( \Delta\mathcal{H} = \mathcal{H}(t) - \mathcal{H}(t+1) \).
\[ H(p) = -p\log p - (1-p)\log(1-p),\qquad \mathcal{H}(t)=\sum_{i=1}^{N} H\big(p_t(i)\big). \]
2.2 Frontier definition (operational)
Using a 3-class discretization of belief, define a cell as unknown if \( p_t(i) \in (p_{\text{free} }, p_{\text{occ} }) \), free if \( p_t(i) \le p_{\text{free} } \), and occupied if \( p_t(i) \ge p_{\text{occ} } \). A frontier cell is an unknown cell adjacent (4-neighborhood) to at least one free cell. This captures the boundary where moving the robot can reveal new space.
\[ i \in \mathcal{F}_t \;\Longleftrightarrow\; \big(i \text{ is unknown}\big)\;\wedge\; \exists j\in\mathcal{N}_4(i):\; p_t(j) \le p_{\text{free} }. \]
2.3 Information gain for a candidate goal
For a candidate goal pose \( g \), define the set of cells expected to become observed (e.g., within sensor range and line-of-sight) as \( \mathcal{V}(g) \). The expected information gain is the expected entropy reduction:
\[ \operatorname{IG}(g) = \mathbb{E}\Big[\mathcal{H}(t) - \mathcal{H}(t+1)\;\Big|\; g\Big] = \sum_{i\in\mathcal{V}(g)} \mathbb{E}\Big[ H\big(p_t(i)\big) - H\big(p_{t+1}(i)\big) \Big]. \]
A widely used lab heuristic (also used in the provided code) is: if a cell is currently unknown, it contributes approximately \( \log 2 \) nats (since \( H(0.5)=\log 2 \)) and becomes near-certain after observation. Then \( \operatorname{IG}(g) \approx |\mathcal{V}_{\text{unk} }(g)|\log 2 \), where \( \mathcal{V}_{\text{unk} }(g) \subseteq \mathcal{V}(g) \) are currently unknown cells.
\[ \operatorname{IG}(g) \approx \sum_{i\in\mathcal{V}(g)} \mathbf{1}[p_t(i) \approx 0.5]\;\log 2 = |\mathcal{V}_{\text{unk} }(g)|\log 2. \]
2.4 Goal scoring with cost and risk
Let \( c(g) \) be an estimated travel cost (e.g., shortest-path length on the current grid) and \( \rho(g) \) a risk proxy (e.g., average occupancy probability along the planned path). A standard linear scalarization is:
\[ J(g)= w_{\mathrm{IG} }\,\operatorname{IG}(g) - w_c\,c(g) - w_{\rho}\,\rho(g), \qquad g^\star = \arg\max_{g\in\mathcal{G}_t} J(g). \]
In this lab, \( \mathcal{G}_t \) is obtained by clustering frontier cells and using cluster centroids as candidate goals. The weights \( w_{\mathrm{IG} }, w_c, w_{\rho} \) are tuned experimentally.
2.5 Time/battery feasibility constraint (single robot)
Let remaining battery/time budget be \( B \) (seconds), translational speed limit \( v_{\max} \), and let \( d(\cdot,\cdot) \) be path length. A conservative feasibility condition for choosing \( g \) is:
\[ \frac{d(x_t,g)}{v_{\max} } + \frac{d(g,x_{\text{home} })}{v_{\max} } \;\le\; B. \]
In simulation, you can set \( x_{\text{home} }=x_0 \) (start pose) and enforce this by rejecting any candidate with \( d(x_t,g) + d(g,x_0) > v_{\max} B \).
3. End-to-End Exploration Loop (Specification)
3.1 Core loop
The exploration manager repeatedly (i) updates the map from new sensor data, (ii) extracts frontiers, (iii) scores candidate goals, (iv) dispatches the best goal to the navigator, and (v) monitors progress and safety.
flowchart TD
A["Start: pose x0, empty map"] --> B["Sense + update occupancy belief"]
B --> C["Classify cells: free / occupied / unknown"]
C --> D["Detect frontier cells (unknown adjacent to free)"]
D --> E["Cluster frontiers -> candidate goals"]
E --> F["For each goal: estimate IG, cost, risk"]
F --> G["Pick goal: maximize score J = wIG*IG - wC*cost - wR*risk"]
G --> H["Send goal to navigator (local planner handles obstacles)"]
H --> I["Monitor: reached? stuck? collision? battery/time?"]
I -->|continue| B
I -->|stop: no frontier OR budget exhausted| Z["Return / terminate + report metrics"]
3.2 Termination and recovery (lab rules)
- No frontiers: \( \mathcal{F}_t = \emptyset \).
- Budget exhausted: time/battery constraint violated for all candidates.
- Stuck detection: navigation fails to make progress for \( T_{\text{stall} } \) seconds (then re-plan goal).
- Safety: if predicted collision risk exceeds a threshold, reject that goal.
3.3 Practical tuning parameters
- Frontier clustering minimum size (filters spurious single cells).
- Unknown-cell penalty in travel cost (discourage moving through unknown).
- Sensor range used for \( \mathcal{V}(g) \) in IG approximation.
- Weight ratio \( w_{\mathrm{IG} }:w_c:w_{\rho} \) (dominates behavior).
4. Lab Procedure
4.1 Option A: Self-contained grid-world (provided code)
- Run the provided Python simulator to generate a map online and explore until termination.
- Repeat for at least 10 random seeds and report mean/variance of metrics.
- Perform weight sweeps: scale \( w_c \) and \( w_{\rho} \) to study the exploration–safety tradeoff.
4.2 Option B: ROS2 mapping + navigation integration (recommended if available)
Use a standard mapping node (e.g., slam_toolbox) that publishes
/map as an occupancy grid and a navigation stack (e.g.,
Nav2) that accepts goal poses (e.g.,
/navigate_to_pose action). Your exploration manager
subscribes to the map and robot pose, computes the next goal, and sends
it to Nav2.
-
Input topics:
/map,/tf(or/amcl_pose//odom). - Output: navigation action goal (PoseStamped), plus logs/diagnostics.
- Keep the same score structure \( J(g) \); only replace the simulator cost function with the nav costmap path length.
4.3 Safety checklist
- Inflate obstacles in the costmap; treat unknown as lethal if the platform requires conservative behavior.
- Set maximum goal distance and maximum navigation time per goal.
- Always maintain a feasible “return-to-home” option under remaining budget.
5. Metrics and Evaluation
5.1 Coverage / known fraction
Let \( \mathbb{I}_t(i) \) indicate whether cell \( i \) is known (classified as free or occupied). The known fraction is:
\[ \mathrm{KnownFrac}(t) = \frac{1}{N}\sum_{i=1}^{N} \mathbb{I}_t(i). \]
5.2 Entropy reduction rate
Plot \( \mathcal{H}(t) \) and compute the average slope \( (\mathcal{H}(0)-\mathcal{H}(T))/T \). Faster reduction indicates better active mapping.
5.3 Path efficiency and safety
- Travel distance: total steps (or meters) traveled.
- Collision / near-collision: count events where robot enters unsafe cells or exceeds risk threshold.
- Goal success rate: fraction of goals reached within timeout.
flowchart TD
A["Run (seed s)"] --> B["Log: KnownFrac(t), Entropy(t), Travel(t)"]
B --> C["Compute summary: final known, final entropy, travel, stalls"]
C --> D["Repeat over seeds"]
D --> E["Report mean +/- std"]
E --> F["Compare weight settings (wIG, wC, wR)"]
6. Code Implementations (Downloadable Files Included)
The following implementations use the same conceptual components: belief update, frontier detection, goal scoring, and motion/execution. The simulator versions are self-contained and suitable for grading.
6.1 Python (self-contained lab simulator)
File: Chapter17_Lesson5.py
import math
import random
from dataclasses import dataclass
from typing import List, Tuple, Optional, Dict
# ============================================================
# Chapter17_Lesson5.py
# Lab: Autonomous Exploration in Unknown Map (single-robot)
# Self-contained grid-world simulator with:
# - Occupancy-grid belief (log-odds)
# - Frontier detection
# - Goal scoring: J = wIG*IG - wC*cost - wR*risk
# - A* planning on current belief map
# - Simple execution and logging of entropy + coverage
#
# NOTE: This is an educational simulator, not a full ROS stack.
# ============================================================
# -----------------------------
# Utilities
# -----------------------------
def sigmoid(x: float) -> float:
return 1.0 / (1.0 + math.exp(-x))
def clamp(x: float, lo: float, hi: float) -> float:
return max(lo, min(hi, x))
def bernoulli_entropy(p: float) -> float:
# H(p) = -p log p -(1-p) log(1-p), nats; handle boundaries
eps = 1e-12
p = clamp(p, eps, 1.0 - eps)
return -p * math.log(p) - (1.0 - p) * math.log(1.0 - p)
# 4-neighborhood moves
MOVES_4 = [(1,0), (-1,0), (0,1), (0,-1)]
@dataclass
class Params:
W: int = 40
H: int = 30
p_occ_true: float = 0.18 # density of true obstacles
sensor_range: int = 6 # Manhattan range for "visibility" model
max_steps: int = 1200
# Belief classification thresholds
p_free: float = 0.35
p_occ: float = 0.65
# Log-odds mapping parameters (simple inverse sensor model)
l0: float = 0.0 # prior log-odds (p=0.5)
l_free: float = -0.85 # increment when sensed free
l_occ: float = 0.85 # increment when sensed occupied
l_min: float = -4.0
l_max: float = 4.0
# Scoring weights
w_ig: float = 1.0
w_cost: float = 0.20
w_risk: float = 0.60
# Risk model
risk_unknown_penalty: float = 0.25 # penalty per unknown cell on path
risk_occ_weight: float = 1.0 # scales occupancy probability along path
# Frontier clustering
min_frontier_cluster: int = 4
cluster_radius: int = 2 # simple radius-based clustering (grid units)
# Budget constraint (time/battery)
budget_steps: int = 1200 # treat each executed step as 1 unit budget
return_home: bool = True
# Stuck detection / goal timeout
goal_timeout: int = 120
stall_window: int = 40
min_progress_in_window: int = 5
class GridWorld:
def __init__(self, prm: Params, seed: int = 0):
self.prm = prm
self.rng = random.Random(seed)
self.true_occ = [[0 for _ in range(prm.W)] for __ in range(prm.H)]
self._generate_world()
def _generate_world(self):
# Generate obstacles with a random field, but keep boundaries blocked
for y in range(self.prm.H):
for x in range(self.prm.W):
if x == 0 or y == 0 or x == self.prm.W - 1 or y == self.prm.H - 1:
self.true_occ[y][x] = 1
else:
self.true_occ[y][x] = 1 if self.rng.random() < self.prm.p_occ_true else 0
# Carve a small free region around the start
sx, sy = self.start()
for dy in range(-2, 3):
for dx in range(-2, 3):
xx, yy = sx + dx, sy + dy
if 0 <= xx < self.prm.W and 0 <= yy < self.prm.H:
self.true_occ[yy][xx] = 0
def start(self) -> Tuple[int, int]:
return (2, 2)
def is_occ_true(self, x: int, y: int) -> bool:
return self.true_occ[y][x] == 1
def in_bounds(self, x: int, y: int) -> bool:
return 0 <= x < self.prm.W and 0 <= y < self.prm.H
def sense(self, rx: int, ry: int) -> List[Tuple[int, int, int]]:
"""
A simplified sensor: returns cell observations in Manhattan ball of radius R.
Each observation is (x,y,occ_true) for cells within range.
In practice, LiDAR would provide ray endpoints; this is a lab abstraction.
"""
obs = []
R = self.prm.sensor_range
for dy in range(-R, R + 1):
for dx in range(-R, R + 1):
if abs(dx) + abs(dy) <= R:
x, y = rx + dx, ry + dy
if self.in_bounds(x, y):
obs.append((x, y, 1 if self.is_occ_true(x, y) else 0))
return obs
class BeliefMap:
def __init__(self, prm: Params):
self.prm = prm
self.logodds = [[prm.l0 for _ in range(prm.W)] for __ in range(prm.H)]
def p(self, x: int, y: int) -> float:
return sigmoid(self.logodds[y][x])
def classify(self, x: int, y: int) -> int:
"""
0 unknown, 1 free, 2 occupied
"""
p = self.p(x, y)
if p <= self.prm.p_free:
return 1
if p >= self.prm.p_occ:
return 2
return 0
def update_from_observations(self, obs: List[Tuple[int, int, int]]):
# Simple additive log-odds update with clamping
for x, y, occ in obs:
if occ == 1:
self.logodds[y][x] = clamp(self.logodds[y][x] + self.prm.l_occ, self.prm.l_min, self.prm.l_max)
else:
self.logodds[y][x] = clamp(self.logodds[y][x] + self.prm.l_free, self.prm.l_min, self.prm.l_max)
def total_entropy(self) -> float:
s = 0.0
for y in range(self.prm.H):
for x in range(self.prm.W):
s += bernoulli_entropy(self.p(x, y))
return s
def known_fraction(self) -> float:
known = 0
total = self.prm.W * self.prm.H
for y in range(self.prm.H):
for x in range(self.prm.W):
c = self.classify(x, y)
if c != 0:
known += 1
return known / float(total)
# -----------------------------
# Frontier detection + clustering
# -----------------------------
def frontiers(bm: BeliefMap) -> List[Tuple[int, int]]:
prm = bm.prm
F = []
for y in range(1, prm.H - 1):
for x in range(1, prm.W - 1):
if bm.classify(x, y) != 0:
continue
# unknown; check if adjacent to free
for dx, dy in MOVES_4:
xx, yy = x + dx, y + dy
if bm.classify(xx, yy) == 1:
F.append((x, y))
break
return F
def cluster_frontiers(F: List[Tuple[int, int]], radius: int, min_size: int) -> List[List[Tuple[int, int]]]:
"""
Very simple clustering: greedy "balls" under L_infty distance.
Intended for teaching; replace with DBSCAN in more advanced labs.
"""
unused = set(F)
clusters = []
while unused:
seed = next(iter(unused))
queue = [seed]
unused.remove(seed)
cl = [seed]
while queue:
x0, y0 = queue.pop()
# collect points within radius in L_infty
near = []
for (x, y) in list(unused):
if max(abs(x - x0), abs(y - y0)) <= radius:
near.append((x, y))
for p in near:
unused.remove(p)
queue.append(p)
cl.append(p)
if len(cl) >= min_size:
clusters.append(cl)
return clusters
def cluster_centroid(cluster: List[Tuple[int,int]]) -> Tuple[int,int]:
sx = sum(p[0] for p in cluster)
sy = sum(p[1] for p in cluster)
n = len(cluster)
return (int(round(sx / n)), int(round(sy / n)))
# -----------------------------
# Planning (A*)
# -----------------------------
def astar_path(bm: BeliefMap, start: Tuple[int,int], goal: Tuple[int,int]) -> Optional[List[Tuple[int,int]]]:
prm = bm.prm
sx, sy = start
gx, gy = goal
def h(x: int, y: int) -> float:
return abs(x - gx) + abs(y - gy)
# cost depends on belief classification:
# - occupied: forbidden
# - free: cost 1
# - unknown: cost 1 + penalty (discourage cutting through unknown)
def step_cost(x: int, y: int) -> float:
c = bm.classify(x, y)
if c == 2:
return float("inf")
if c == 0:
return 1.0 + 1.0 # penalty for unknown
return 1.0
open_set = {start}
came: Dict[Tuple[int,int], Tuple[int,int]] = {}
gscore: Dict[Tuple[int,int], float] = {start: 0.0}
fscore: Dict[Tuple[int,int], float] = {start: h(sx, sy)}
def best_in_open():
return min(open_set, key=lambda p: fscore.get(p, float("inf")))
while open_set:
cur = best_in_open()
if cur == goal:
# reconstruct
path = [cur]
while cur in came:
cur = came[cur]
path.append(cur)
path.reverse()
return path
open_set.remove(cur)
cx, cy = cur
for dx, dy in MOVES_4:
nx, ny = cx + dx, cy + dy
if not (0 <= nx < prm.W and 0 <= ny < prm.H):
continue
sc = step_cost(nx, ny)
if sc == float("inf"):
continue
tentative = gscore[cur] + sc
if tentative < gscore.get((nx, ny), float("inf")):
came[(nx, ny)] = cur
gscore[(nx, ny)] = tentative
fscore[(nx, ny)] = tentative + h(nx, ny)
open_set.add((nx, ny))
return None
# -----------------------------
# Scoring: IG, cost, risk
# -----------------------------
def visible_unknown_cells(bm: BeliefMap, g: Tuple[int,int]) -> List[Tuple[int,int]]:
gx, gy = g
R = bm.prm.sensor_range
unk = []
for dy in range(-R, R + 1):
for dx in range(-R, R + 1):
if abs(dx) + abs(dy) <= R:
x, y = gx + dx, gy + dy
if 0 <= x < bm.prm.W and 0 <= y < bm.prm.H:
if bm.classify(x, y) == 0:
unk.append((x, y))
return unk
def info_gain_heuristic(bm: BeliefMap, g: Tuple[int,int]) -> float:
# IG ~ |unknown visible| * log(2)
unk = visible_unknown_cells(bm, g)
return len(unk) * math.log(2.0)
def path_cost(bm: BeliefMap, path: List[Tuple[int,int]]) -> float:
# sum of unit costs along path (excluding start)
cost = 0.0
for (x, y) in path[1:]:
c = bm.classify(x, y)
if c == 1:
cost += 1.0
elif c == 0:
cost += 2.0
else:
return float("inf")
return cost
def path_risk(bm: BeliefMap, path: List[Tuple[int,int]]) -> float:
# risk proxy: average occupancy probability + unknown penalty
if len(path) <= 1:
return 0.0
s = 0.0
unk = 0
for (x, y) in path[1:]:
s += bm.p(x, y) * bm.prm.risk_occ_weight
if bm.classify(x, y) == 0:
unk += 1
return (s / (len(path) - 1)) + unk * bm.prm.risk_unknown_penalty
def score_goal(bm: BeliefMap, start: Tuple[int,int], goal: Tuple[int,int]) -> Tuple[float, Optional[List[Tuple[int,int]]], Dict[str,float]]:
path = astar_path(bm, start, goal)
if path is None:
return (-float("inf"), None, {"ig": 0.0, "cost": float("inf"), "risk": float("inf")})
ig = info_gain_heuristic(bm, goal)
c = path_cost(bm, path)
r = path_risk(bm, path)
J = bm.prm.w_ig * ig - bm.prm.w_cost * c - bm.prm.w_risk * r
return (J, path, {"ig": ig, "cost": c, "risk": r})
# -----------------------------
# Main exploration loop
# -----------------------------
def explore(seed: int = 0, prm: Optional[Params] = None) -> Dict[str, object]:
if prm is None:
prm = Params()
world = GridWorld(prm, seed=seed)
bm = BeliefMap(prm)
rx, ry = world.start()
home = (rx, ry)
# initial sense
bm.update_from_observations(world.sense(rx, ry))
logs = {
"entropy": [],
"known_frac": [],
"travel": [],
"goals": [],
"goal_details": [],
}
budget_left = prm.budget_steps
step = 0
total_travel = 0
# For stuck detection
recent_positions: List[Tuple[int,int]] = []
current_goal: Optional[Tuple[int,int]] = None
current_path: Optional[List[Tuple[int,int]]] = None
steps_on_goal = 0
while step < prm.max_steps and budget_left > 0:
# Log metrics
logs["entropy"].append(bm.total_entropy())
logs["known_frac"].append(bm.known_fraction())
logs["travel"].append(total_travel)
# Update stuck window
recent_positions.append((rx, ry))
if len(recent_positions) > prm.stall_window:
recent_positions.pop(0)
# If no goal or goal invalid/timeout/stuck: pick a new goal
pick_new_goal = False
if current_goal is None or current_path is None or steps_on_goal >= prm.goal_timeout:
pick_new_goal = True
else:
# If the next cell became occupied in belief, replan
if len(current_path) > 1:
nx, ny = current_path[1]
if bm.classify(nx, ny) == 2:
pick_new_goal = True
# Stuck detection: insufficient distinct positions in window
if len(recent_positions) == prm.stall_window:
distinct = len(set(recent_positions))
if distinct <= prm.min_progress_in_window:
pick_new_goal = True
if pick_new_goal:
steps_on_goal = 0
F = frontiers(bm)
if len(F) == 0:
break # no frontiers
clusters = cluster_frontiers(F, prm.cluster_radius, prm.min_frontier_cluster)
if len(clusters) == 0:
# fall back to raw frontier points if clustering filtered everything
candidates = F[:]
else:
candidates = [cluster_centroid(cl) for cl in clusters]
# Score candidates
best = (-float("inf"), None, None, None)
for g in candidates:
# Budget feasibility (return-to-home)
if prm.return_home:
# use Manhattan proxy on grid (lower bound) to reject far goals quickly
d1 = abs(rx - g[0]) + abs(ry - g[1])
d2 = abs(g[0] - home[0]) + abs(g[1] - home[1])
if d1 + d2 > budget_left:
continue
J, path, details = score_goal(bm, (rx, ry), g)
if path is None:
continue
if J > best[0]:
best = (J, g, path, details)
if best[1] is None:
break # no feasible goals
current_goal = best[1]
current_path = best[2]
logs["goals"].append(current_goal)
logs["goal_details"].append(best[3])
# Execute one step along the path
if current_path is None or len(current_path) < 2:
current_goal = None
current_path = None
continue
nx, ny = current_path[1]
# If true obstacle, collision; mark as occupied in belief and replan
if world.is_occ_true(nx, ny):
bm.update_from_observations([(nx, ny, 1)])
current_goal = None
current_path = None
else:
# move
rx, ry = nx, ny
total_travel += 1
budget_left -= 1
steps_on_goal += 1
# sense and update
bm.update_from_observations(world.sense(rx, ry))
# drop the executed step from the path
current_path = current_path[1:]
step += 1
# final logs
logs["entropy"].append(bm.total_entropy())
logs["known_frac"].append(bm.known_fraction())
logs["travel"].append(total_travel)
return {
"seed": seed,
"final_known_frac": logs["known_frac"][-1],
"final_entropy": logs["entropy"][-1],
"total_travel": total_travel,
"steps": step,
"logs": logs,
}
def run_sweep():
# A small experiment: multiple seeds, report mean/std
seeds = list(range(10))
results = []
for s in seeds:
res = explore(seed=s)
results.append(res)
def mean(xs):
return sum(xs) / max(1, len(xs))
def std(xs):
m = mean(xs)
return math.sqrt(mean([(x - m) ** 2 for x in xs]))
kf = [r["final_known_frac"] for r in results]
ent = [r["final_entropy"] for r in results]
trav = [r["total_travel"] for r in results]
print("=== Exploration Sweep (10 seeds) ===")
print(f"Final Known Fraction: mean={mean(kf):.3f}, std={std(kf):.3f}")
print(f"Final Entropy: mean={mean(ent):.3f}, std={std(ent):.3f}")
print(f"Total Travel: mean={mean(trav):.1f}, std={std(trav):.1f}")
# Show one run's logs (seed 0) as a simple table preview
r0 = results[0]["logs"]
print("\nseed=0 sample logs (t, known_frac, entropy, travel):")
for t in range(0, len(r0["entropy"]), max(1, len(r0["entropy"]) // 10)):
print(f"t={t:4d} known={r0['known_frac'][t]:.3f} H={r0['entropy'][t]:.2f} travel={r0['travel'][t]}")
if __name__ == "__main__":
run_sweep()
File: Chapter17_Lesson5_Ex1.py (exercise: frontier clustering)
# ============================================================
# Chapter17_Lesson5_Ex1.py
# Exercise 1: Replace the greedy radius-based clustering with DBSCAN-like logic
# (still self-contained, no external libraries required).
#
# Task:
# 1) Implement a simple DBSCAN on frontier points using L_infty distance.
# 2) Compare number of clusters and exploration performance.
# ============================================================
from typing import List, Tuple, Dict, Set
Point = Tuple[int, int]
def linf(p: Point, q: Point) -> int:
return max(abs(p[0] - q[0]), abs(p[1] - q[1]))
def region_query(points: List[Point], p: Point, eps: int) -> List[Point]:
return [q for q in points if linf(p, q) <= eps]
def dbscan(points: List[Point], eps: int, min_pts: int) -> List[List[Point]]:
"""
Minimal DBSCAN for educational use:
- Uses L_infty distance on integer grid points.
- Returns clusters; noise points are discarded.
"""
visited: Set[Point] = set()
assigned: Dict[Point, int] = {}
clusters: List[List[Point]] = []
for p in points:
if p in visited:
continue
visited.add(p)
neigh = region_query(points, p, eps)
if len(neigh) < min_pts:
continue # noise
# start new cluster
cid = len(clusters)
clusters.append([])
clusters[cid].append(p)
assigned[p] = cid
# expand
queue = list(neigh)
while queue:
q = queue.pop()
if q not in visited:
visited.add(q)
qneigh = region_query(points, q, eps)
if len(qneigh) >= min_pts:
queue.extend(qneigh)
if q not in assigned:
assigned[q] = cid
clusters[cid].append(q)
return clusters
if __name__ == "__main__":
# Small sanity test
pts = [(0,0),(1,0),(0,1),(10,10),(11,10),(30,30)]
cl = dbscan(pts, eps=1, min_pts=2)
print("Clusters:", cl)
6.2 C++ (C++17 self-contained simulator)
File: Chapter17_Lesson5.cpp
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <random>
#include <unordered_map>
#include <unordered_set>
#include <limits>
#include <algorithm>
#include <tuple>
//
// Chapter17_Lesson5.cpp
// Lab: Autonomous Exploration in Unknown Map (single-robot)
// Self-contained grid-world simulator with:
// - log-odds occupancy belief
// - frontier detection + simple clustering
// - A* planning on belief
// - goal scoring: J = wIG*IG - wC*cost - wR*risk
//
// Build (example):
// g++ -std=c++17 -O2 Chapter17_Lesson5.cpp -o explore
//
static inline double sigmoid(double x){
return 1.0 / (1.0 + std::exp(-x));
}
static inline double clamp(double x, double lo, double hi){
return std::max(lo, std::min(hi, x));
}
static inline double bern_entropy(double p){
const double eps = 1e-12;
p = clamp(p, eps, 1.0 - eps);
return -p*std::log(p) - (1.0-p)*std::log(1.0-p);
}
struct Params{
int W = 40;
int H = 30;
double p_occ_true = 0.18;
int sensor_range = 6;
int max_steps = 1200;
double p_free = 0.35;
double p_occ = 0.65;
double l0 = 0.0;
double l_free= -0.85;
double l_occ = 0.85;
double l_min = -4.0;
double l_max = 4.0;
double w_ig = 1.0;
double w_cost = 0.20;
double w_risk = 0.60;
double risk_unknown_penalty = 0.25;
double risk_occ_weight = 1.0;
int min_frontier_cluster = 4;
int cluster_radius = 2;
int budget_steps = 1200;
bool return_home = true;
int goal_timeout = 120;
int stall_window = 40;
int min_progress_in_window = 5;
};
struct World{
Params prm;
std::mt19937 rng;
std::vector<std::vector<int>> occ;
World(const Params& p, int seed): prm(p), rng(seed), occ(p.H, std::vector<int>(p.W,0)) {
generate();
}
std::pair<int,int> start() const { return {2,2}; }
bool in_bounds(int x,int y) const {
return (0 <= x && x < prm.W && 0 <= y && y < prm.H);
}
bool is_occ_true(int x,int y) const { return occ[y][x]==1; }
void generate(){
std::uniform_real_distribution<double> U(0.0,1.0);
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
if(x==0 || y==0 || x==prm.W-1 || y==prm.H-1){
occ[y][x]=1;
}else{
occ[y][x] = (U(rng) < prm.p_occ_true) ? 1 : 0;
}
}
}
auto [sx,sy] = start();
for(int dy=-2;dy<=2;dy++){
for(int dx=-2;dx<=2;dx++){
int xx=sx+dx, yy=sy+dy;
if(in_bounds(xx,yy)) occ[yy][xx]=0;
}
}
}
std::vector<std::tuple<int,int,int>> sense(int rx,int ry) const {
std::vector<std::tuple<int,int,int>> obs;
int R=prm.sensor_range;
for(int dy=-R;dy<=R;dy++){
for(int dx=-R;dx<=R;dx++){
if(std::abs(dx)+std::abs(dy) <= R){
int x=rx+dx, y=ry+dy;
if(in_bounds(x,y)){
obs.emplace_back(x,y, is_occ_true(x,y)?1:0);
}
}
}
}
return obs;
}
};
struct Belief{
Params prm;
std::vector<std::vector<double>> l;
Belief(const Params& p): prm(p), l(p.H, std::vector<double>(p.W, p.l0)) {}
double p(int x,int y) const { return sigmoid(l[y][x]); }
int classify(int x,int y) const {
double pp = p(x,y);
if(pp <= prm.p_free) return 1;
if(pp >= prm.p_occ) return 2;
return 0;
}
void update(const std::vector<std::tuple<int,int,int>>& obs){
for(const auto& t: obs){
int x,y,occ;
std::tie(x,y,occ)=t;
if(occ==1) l[y][x]=clamp(l[y][x]+prm.l_occ, prm.l_min, prm.l_max);
else l[y][x]=clamp(l[y][x]+prm.l_free, prm.l_min, prm.l_max);
}
}
double total_entropy() const {
double s=0.0;
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
s += bern_entropy(p(x,y));
}
}
return s;
}
double known_fraction() const {
int known=0;
int total=prm.W*prm.H;
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
if(classify(x,y)!=0) known++;
}
}
return double(known)/double(total);
}
};
static const int MOVES4[4][2] = { {1,0},{-1,0},{0,1},{0,-1} };
std::vector<std::pair<int,int>> frontiers(const Belief& bm){
std::vector<std::pair<int,int>> F;
for(int y=1;y<bm.prm.H-1;y++){
for(int x=1;x<bm.prm.W-1;x++){
if(bm.classify(x,y)!=0) continue; // must be unknown
for(int k=0;k<4;k++){
int xx=x+MOVES4[k][0], yy=y+MOVES4[k][1];
if(bm.classify(xx,yy)==1){
F.emplace_back(x,y);
break;
}
}
}
}
return F;
}
std::vector<std::vector<std::pair<int,int>>> cluster_frontiers(
const std::vector<std::pair<int,int>>& F, int radius, int min_size
){
// Greedy L_infty balls
std::unordered_set<long long> unused;
auto key = [](int x,int y){ return ( (long long)y<<32 ) ^ (unsigned int)x; };
for(auto& p: F) unused.insert(key(p.first,p.second));
std::vector<std::vector<std::pair<int,int>>> clusters;
while(!unused.empty()){
long long k0 = *unused.begin();
int x0 = (int)(k0 & 0xffffffffu);
int y0 = (int)(k0 >> 32);
std::vector<std::pair<int,int>> q;
q.emplace_back(x0,y0);
unused.erase(k0);
std::vector<std::pair<int,int>> cl;
cl.emplace_back(x0,y0);
while(!q.empty()){
auto cur=q.back(); q.pop_back();
int cx=cur.first, cy=cur.second;
std::vector<long long> to_add;
for(auto it=unused.begin(); it!=unused.end(); ++it){
long long kk=*it;
int x=(int)(kk & 0xffffffffu);
int y=(int)(kk >> 32);
if(std::max(std::abs(x-cx), std::abs(y-cy)) <= radius){
to_add.push_back(kk);
}
}
for(long long kk: to_add){
unused.erase(kk);
int x=(int)(kk & 0xffffffffu);
int y=(int)(kk >> 32);
q.emplace_back(x,y);
cl.emplace_back(x,y);
}
}
if((int)cl.size() >= min_size) clusters.push_back(cl);
}
return clusters;
}
std::pair<int,int> centroid(const std::vector<std::pair<int,int>>& cl){
double sx=0, sy=0;
for(auto& p: cl){ sx+=p.first; sy+=p.second; }
int n=(int)cl.size();
return { (int)std::lround(sx/n), (int)std::lround(sy/n) };
}
struct Node{
int x,y;
double f;
};
struct NodeCmp{
bool operator()(const Node& a, const Node& b) const {
return a.f > b.f;
}
};
std::vector<std::pair<int,int>> astar(const Belief& bm, std::pair<int,int> s, std::pair<int,int> g){
int W=bm.prm.W, H=bm.prm.H;
auto h = [&](int x,int y){
return double(std::abs(x-g.first) + std::abs(y-g.second));
};
auto step_cost = [&](int x,int y){
int c=bm.classify(x,y);
if(c==2) return std::numeric_limits<double>::infinity();
if(c==0) return 2.0; // unknown penalty
return 1.0;
};
std::priority_queue<Node, std::vector<Node>, NodeCmp> pq;
std::vector<std::vector<double>> gscore(H, std::vector<double>(W, std::numeric_limits<double>::infinity()));
std::vector<std::vector<std::pair<int,int>>> came(H, std::vector<std::pair<int,int>>(W, {-1,-1}));
std::vector<std::vector<bool>> inopen(H, std::vector<bool>(W,false));
gscore[s.second][s.first]=0.0;
pq.push({s.first,s.second, h(s.first,s.second)});
inopen[s.second][s.first]=true;
while(!pq.empty()){
Node cur = pq.top(); pq.pop();
int cx=cur.x, cy=cur.y;
if(cx==g.first && cy==g.second){
// reconstruct
std::vector<std::pair<int,int>> path;
int x=cx,y=cy;
path.push_back({x,y});
while(!(x==s.first && y==s.second)){
auto p=came[y][x];
x=p.first; y=p.second;
if(x<0) break;
path.push_back({x,y});
}
std::reverse(path.begin(), path.end());
return path;
}
for(int k=0;k<4;k++){
int nx=cx+MOVES4[k][0], ny=cy+MOVES4[k][1];
if(nx<0||nx>=W||ny<0||ny>=H) continue;
double sc=step_cost(nx,ny);
if(!std::isfinite(sc)) continue;
double tentative=gscore[cy][cx]+sc;
if(tentative < gscore[ny][nx]){
gscore[ny][nx]=tentative;
came[ny][nx]={cx,cy};
double f=tentative+h(nx,ny);
pq.push({nx,ny,f});
inopen[ny][nx]=true;
}
}
}
return {}; // failure
}
int visible_unknown_count(const Belief& bm, std::pair<int,int> g){
int gx=g.first, gy=g.second;
int R=bm.prm.sensor_range;
int cnt=0;
for(int dy=-R;dy<=R;dy++){
for(int dx=-R;dx<=R;dx++){
if(std::abs(dx)+std::abs(dy) <= R){
int x=gx+dx, y=gy+dy;
if(0<=x && x<bm.prm.W && 0<=y && y<bm.prm.H){
if(bm.classify(x,y)==0) cnt++;
}
}
}
}
return cnt;
}
double path_cost(const Belief& bm, const std::vector<std::pair<int,int>>& path){
if(path.size()<2) return 0.0;
double c=0.0;
for(size_t i=1;i<path.size();i++){
int x=path[i].first, y=path[i].second;
int cls=bm.classify(x,y);
if(cls==2) return std::numeric_limits<double>::infinity();
if(cls==0) c+=2.0;
else c+=1.0;
}
return c;
}
double path_risk(const Belief& bm, const std::vector<std::pair<int,int>>& path){
if(path.size()<2) return 0.0;
double s=0.0;
int unk=0;
for(size_t i=1;i<path.size();i++){
int x=path[i].first, y=path[i].second;
s += bm.p(x,y)*bm.prm.risk_occ_weight;
if(bm.classify(x,y)==0) unk++;
}
double avg = s / double(path.size()-1);
return avg + unk*bm.prm.risk_unknown_penalty;
}
struct GoalEval{
double J;
double ig;
double cost;
double risk;
std::pair<int,int> goal;
std::vector<std::pair<int,int>> path;
};
GoalEval score_goal(const Belief& bm, std::pair<int,int> start, std::pair<int,int> goal){
GoalEval ge;
ge.goal=goal;
ge.path = astar(bm, start, goal);
if(ge.path.empty()){
ge.J = -std::numeric_limits<double>::infinity();
ge.ig=ge.cost=ge.risk=0.0;
return ge;
}
ge.ig = visible_unknown_count(bm, goal) * std::log(2.0);
ge.cost = path_cost(bm, ge.path);
ge.risk = path_risk(bm, ge.path);
ge.J = bm.prm.w_ig*ge.ig - bm.prm.w_cost*ge.cost - bm.prm.w_risk*ge.risk;
return ge;
}
int main(){
Params prm;
const int seeds=10;
double sum_known=0, sum_known2=0;
double sum_ent=0, sum_ent2=0;
double sum_trav=0, sum_trav2=0;
for(int s=0;s<seeds;s++){
World world(prm, s);
Belief bm(prm);
auto [rx,ry]=world.start();
auto home=world.start();
bm.update(world.sense(rx,ry));
int budget=prm.budget_steps;
int step=0;
int travel=0;
std::vector<std::pair<int,int>> recent;
std::pair<int,int> current_goal = {-1,-1};
std::vector<std::pair<int,int>> current_path;
int steps_on_goal=0;
while(step < prm.max_steps && budget > 0){
recent.push_back({rx,ry});
if((int)recent.size() > prm.stall_window) recent.erase(recent.begin());
bool pick=false;
if(current_goal.first<0 || current_path.size()<2 || steps_on_goal>=prm.goal_timeout) pick=true;
if(!pick && current_path.size()>1){
int nx=current_path[1].first, ny=current_path[1].second;
if(bm.classify(nx,ny)==2) pick=true;
}
if(!pick && (int)recent.size()==prm.stall_window){
std::unordered_set<long long> S;
for(auto& p: recent){
long long kk = ((long long)p.second<<32) ^ (unsigned int)p.first;
S.insert(kk);
}
if((int)S.size() <= prm.min_progress_in_window) pick=true;
}
if(pick){
steps_on_goal=0;
auto F = frontiers(bm);
if(F.empty()) break;
auto clusters = cluster_frontiers(F, prm.cluster_radius, prm.min_frontier_cluster);
std::vector<std::pair<int,int>> cand;
if(clusters.empty()){
cand = F;
}else{
for(auto& cl: clusters) cand.push_back(centroid(cl));
}
GoalEval best; best.J = -std::numeric_limits<double>::infinity();
bool found=false;
for(auto& g: cand){
if(prm.return_home){
int d1=std::abs(rx-g.first)+std::abs(ry-g.second);
int d2=std::abs(g.first-home.first)+std::abs(g.second-home.second);
if(d1+d2 > budget) continue;
}
auto ge = score_goal(bm, {rx,ry}, g);
if(ge.path.empty()) continue;
if(ge.J > best.J){
best=ge;
found=true;
}
}
if(!found) break;
current_goal = best.goal;
current_path = best.path;
}
if(current_path.size() < 2){
current_goal={-1,-1};
current_path.clear();
continue;
}
int nx=current_path[1].first, ny=current_path[1].second;
if(world.is_occ_true(nx,ny)){
bm.update({std::make_tuple(nx,ny,1)});
current_goal={-1,-1};
current_path.clear();
}else{
rx=nx; ry=ny;
travel += 1;
budget -= 1;
steps_on_goal += 1;
bm.update(world.sense(rx,ry));
current_path.erase(current_path.begin());
}
step++;
}
double kf = bm.known_fraction();
double ent = bm.total_entropy();
sum_known += kf; sum_known2 += kf*kf;
sum_ent += ent; sum_ent2 += ent*ent;
sum_trav += travel; sum_trav2 += travel*travel;
}
auto mean = [](double s, int n){ return s/double(n); };
auto stdev = [&](double s, double s2, int n){
double m = mean(s,n);
double v = mean(s2,n) - m*m;
return std::sqrt(std::max(0.0, v));
};
std::cout << "=== Exploration Sweep (" << seeds << " seeds) ===\n";
std::cout << "Final Known Fraction: mean=" << mean(sum_known,seeds)
<< ", std=" << stdev(sum_known,sum_known2,seeds) << "\n";
std::cout << "Final Entropy: mean=" << mean(sum_ent,seeds)
<< ", std=" << stdev(sum_ent,sum_ent2,seeds) << "\n";
std::cout << "Total Travel: mean=" << mean(sum_trav,seeds)
<< ", std=" << stdev(sum_trav,sum_trav2,seeds) << "\n";
return 0;
}
6.3 Java (self-contained simulator)
File: Chapter17_Lesson5.java
import java.util.*;
import java.util.stream.Collectors;
//
// Chapter17_Lesson5.java
// Lab: Autonomous Exploration in Unknown Map (single-robot)
// Self-contained grid-world simulator with:
// - log-odds occupancy belief
// - frontier detection + simple clustering
// - A* planning on belief
// - goal scoring: J = wIG*IG - wC*cost - wR*risk
//
// Run:
// javac Chapter17_Lesson5.java
// java Chapter17_Lesson5
//
public class Chapter17_Lesson5 {
static class Params {
int W = 40;
int H = 30;
double pOccTrue = 0.18;
int sensorRange = 6;
int maxSteps = 1200;
double pFree = 0.35;
double pOcc = 0.65;
double l0 = 0.0;
double lFree = -0.85;
double lOcc = 0.85;
double lMin = -4.0;
double lMax = 4.0;
double wIG = 1.0;
double wCost = 0.20;
double wRisk = 0.60;
double riskUnknownPenalty = 0.25;
double riskOccWeight = 1.0;
int minFrontierCluster = 4;
int clusterRadius = 2;
int budgetSteps = 1200;
boolean returnHome = true;
int goalTimeout = 120;
int stallWindow = 40;
int minProgressInWindow = 5;
}
static double sigmoid(double x){
return 1.0 / (1.0 + Math.exp(-x));
}
static double clamp(double x, double lo, double hi){
return Math.max(lo, Math.min(hi, x));
}
static double bernEntropy(double p){
double eps=1e-12;
p = clamp(p, eps, 1.0-eps);
return -p*Math.log(p) - (1.0-p)*Math.log(1.0-p);
}
static final int[][] MOVES4 = new int[][]{ {1,0},{-1,0},{0,1},{0,-1} };
static class World {
Params prm;
Random rng;
int[][] occ;
World(Params prm, int seed){
this.prm = prm;
this.rng = new Random(seed);
this.occ = new int[prm.H][prm.W];
generate();
}
int[] start(){ return new int[]{2,2}; }
boolean inBounds(int x,int y){
return 0 <= x && x < prm.W && 0 <= y && y < prm.H;
}
boolean isOccTrue(int x,int y){
return occ[y][x]==1;
}
void generate(){
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
if(x==0 || y==0 || x==prm.W-1 || y==prm.H-1){
occ[y][x]=1;
}else{
occ[y][x] = (rng.nextDouble() < prm.pOccTrue) ? 1 : 0;
}
}
}
int[] st = start();
int sx=st[0], sy=st[1];
for(int dy=-2;dy<=2;dy++){
for(int dx=-2;dx<=2;dx++){
int xx=sx+dx, yy=sy+dy;
if(inBounds(xx,yy)) occ[yy][xx]=0;
}
}
}
List<int[]> sense(int rx,int ry){
List<int[]> obs = new ArrayList<>();
int R=prm.sensorRange;
for(int dy=-R;dy<=R;dy++){
for(int dx=-R;dx<=R;dx++){
if(Math.abs(dx)+Math.abs(dy) <= R){
int x=rx+dx, y=ry+dy;
if(inBounds(x,y)){
obs.add(new int[]{x,y, isOccTrue(x,y)?1:0});
}
}
}
}
return obs;
}
}
static class Belief {
Params prm;
double[][] l;
Belief(Params prm){
this.prm=prm;
this.l = new double[prm.H][prm.W];
for(int y=0;y<prm.H;y++) Arrays.fill(l[y], prm.l0);
}
double p(int x,int y){ return sigmoid(l[y][x]); }
int classify(int x,int y){
double pp=p(x,y);
if(pp <= prm.pFree) return 1;
if(pp >= prm.pOcc) return 2;
return 0;
}
void update(List<int[]> obs){
for(int[] o: obs){
int x=o[0], y=o[1], occ=o[2];
if(occ==1) l[y][x]=clamp(l[y][x]+prm.lOcc, prm.lMin, prm.lMax);
else l[y][x]=clamp(l[y][x]+prm.lFree, prm.lMin, prm.lMax);
}
}
double totalEntropy(){
double s=0.0;
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
s += bernEntropy(p(x,y));
}
}
return s;
}
double knownFraction(){
int known=0;
int total=prm.W*prm.H;
for(int y=0;y<prm.H;y++){
for(int x=0;x<prm.W;x++){
if(classify(x,y)!=0) known++;
}
}
return (double)known/(double)total;
}
}
static List<int[]> frontiers(Belief bm){
List<int[]> F=new ArrayList<>();
for(int y=1;y<bm.prm.H-1;y++){
for(int x=1;x<bm.prm.W-1;x++){
if(bm.classify(x,y)!=0) continue;
for(int[] mv: MOVES4){
int xx=x+mv[0], yy=y+mv[1];
if(bm.classify(xx,yy)==1){
F.add(new int[]{x,y});
break;
}
}
}
}
return F;
}
static List<List<int[]>> clusterFrontiers(List<int[]> F, int radius, int minSize){
// Greedy L_infty balls
Set<Long> unused = new HashSet<>();
for(int[] p: F){
long key = (((long)p[1])<<32) ^ (p[0] & 0xffffffffL);
unused.add(key);
}
List<List<int[]>> clusters = new ArrayList<>();
while(!unused.isEmpty()){
long k0 = unused.iterator().next();
int x0 = (int)(k0 & 0xffffffffL);
int y0 = (int)(k0 >> 32);
unused.remove(k0);
List<int[]> queue = new ArrayList<>();
queue.add(new int[]{x0,y0});
List<int[]> cl = new ArrayList<>();
cl.add(new int[]{x0,y0});
while(!queue.isEmpty()){
int[] cur = queue.remove(queue.size()-1);
int cx=cur[0], cy=cur[1];
List<Long> toAdd = new ArrayList<>();
for(long kk: unused){
int x = (int)(kk & 0xffffffffL);
int y = (int)(kk >> 32);
int d = Math.max(Math.abs(x-cx), Math.abs(y-cy));
if(d <= radius) toAdd.add(kk);
}
for(long kk: toAdd){
unused.remove(kk);
int x = (int)(kk & 0xffffffffL);
int y = (int)(kk >> 32);
queue.add(new int[]{x,y});
cl.add(new int[]{x,y});
}
}
if(cl.size() >= minSize) clusters.add(cl);
}
return clusters;
}
static int[] centroid(List<int[]> cl){
double sx=0, sy=0;
for(int[] p: cl){ sx+=p[0]; sy+=p[1]; }
int n=cl.size();
return new int[]{ (int)Math.round(sx/n), (int)Math.round(sy/n) };
}
static class AStarNode {
int x,y;
double f;
AStarNode(int x,int y,double f){ this.x=x; this.y=y; this.f=f; }
}
static List<int[]> astar(Belief bm, int[] s, int[] g){
int W=bm.prm.W, H=bm.prm.H;
int sx=s[0], sy=s[1], gx=g[0], gy=g[1];
java.util.function.BiFunction<Integer,Integer,Double> h = (x,y) -> (double)(Math.abs(x-gx)+Math.abs(y-gy));
java.util.function.BiFunction<Integer,Integer,Double> stepCost = (x,y) -> {
int c=bm.classify(x,y);
if(c==2) return Double.POSITIVE_INFINITY;
if(c==0) return 2.0;
return 1.0;
};
PriorityQueue<AStarNode> pq = new PriorityQueue<>(Comparator.comparingDouble(n -> n.f));
double[][] gscore = new double[H][W];
int[][][] came = new int[H][W][2];
for(int y=0;y<H;y++){
Arrays.fill(gscore[y], Double.POSITIVE_INFINITY);
for(int x=0;x<W;x++){ came[y][x][0]=-1; came[y][x][1]=-1; }
}
gscore[sy][sx]=0.0;
pq.add(new AStarNode(sx,sy,h.apply(sx,sy)));
while(!pq.isEmpty()){
AStarNode cur=pq.poll();
if(cur.x==gx && cur.y==gy){
List<int[]> path=new ArrayList<>();
int x=gx, y=gy;
path.add(new int[]{x,y});
while(!(x==sx && y==sy)){
int px=came[y][x][0], py=came[y][x][1];
if(px<0) break;
x=px; y=py;
path.add(new int[]{x,y});
}
Collections.reverse(path);
return path;
}
for(int[] mv: MOVES4){
int nx=cur.x+mv[0], ny=cur.y+mv[1];
if(nx<0||nx>=W||ny<0||ny>=H) continue;
double sc=stepCost.apply(nx,ny);
if(!Double.isFinite(sc)) continue;
double tentative=gscore[cur.y][cur.x]+sc;
if(tentative < gscore[ny][nx]){
gscore[ny][nx]=tentative;
came[ny][nx][0]=cur.x;
came[ny][nx][1]=cur.y;
pq.add(new AStarNode(nx,ny,tentative+h.apply(nx,ny)));
}
}
}
return Collections.emptyList();
}
static int visibleUnknownCount(Belief bm, int[] g){
int gx=g[0], gy=g[1];
int R=bm.prm.sensorRange;
int cnt=0;
for(int dy=-R;dy<=R;dy++){
for(int dx=-R;dx<=R;dx++){
if(Math.abs(dx)+Math.abs(dy) <= R){
int x=gx+dx, y=gy+dy;
if(0<=x && x<bm.prm.W && 0<=y && y<bm.prm.H){
if(bm.classify(x,y)==0) cnt++;
}
}
}
}
return cnt;
}
static double pathCost(Belief bm, List<int[]> path){
if(path.size()<2) return 0.0;
double c=0.0;
for(int i=1;i<path.size();i++){
int x=path.get(i)[0], y=path.get(i)[1];
int cls=bm.classify(x,y);
if(cls==2) return Double.POSITIVE_INFINITY;
c += (cls==0) ? 2.0 : 1.0;
}
return c;
}
static double pathRisk(Belief bm, List<int[]> path){
if(path.size()<2) return 0.0;
double s=0.0;
int unk=0;
for(int i=1;i<path.size();i++){
int x=path.get(i)[0], y=path.get(i)[1];
s += bm.p(x,y)*bm.prm.riskOccWeight;
if(bm.classify(x,y)==0) unk++;
}
double avg=s/(path.size()-1);
return avg + unk*bm.prm.riskUnknownPenalty;
}
static class GoalEval {
double J, ig, cost, risk;
int[] goal;
List<int[]> path;
}
static GoalEval scoreGoal(Belief bm, int[] start, int[] goal){
GoalEval ge=new GoalEval();
ge.goal=goal;
ge.path=astar(bm, start, goal);
if(ge.path.isEmpty()){
ge.J = -Double.POSITIVE_INFINITY;
ge.ig=ge.cost=ge.risk=0.0;
return ge;
}
ge.ig = visibleUnknownCount(bm, goal) * Math.log(2.0);
ge.cost = pathCost(bm, ge.path);
ge.risk = pathRisk(bm, ge.path);
ge.J = bm.prm.wIG*ge.ig - bm.prm.wCost*ge.cost - bm.prm.wRisk*ge.risk;
return ge;
}
public static void main(String[] args){
Params prm=new Params();
int seeds=10;
double sumKnown=0, sumKnown2=0;
double sumEnt=0, sumEnt2=0;
double sumTrav=0, sumTrav2=0;
for(int s=0;s<seeds;s++){
World world=new World(prm, s);
Belief bm=new Belief(prm);
int[] st=world.start();
int rx=st[0], ry=st[1];
int[] home=world.start();
bm.update(world.sense(rx,ry));
int budget=prm.budgetSteps;
int step=0;
int travel=0;
List<String> recent=new ArrayList<>();
int[] currentGoal=null;
List<int[]> currentPath=Collections.emptyList();
int stepsOnGoal=0;
while(step < prm.maxSteps && budget > 0){
recent.add(rx+","+ry);
if(recent.size() > prm.stallWindow) recent.remove(0);
boolean pick=false;
if(currentGoal==null || currentPath.size()<2 || stepsOnGoal>=prm.goalTimeout) pick=true;
if(!pick && currentPath.size()>1){
int nx=currentPath.get(1)[0], ny=currentPath.get(1)[1];
if(bm.classify(nx,ny)==2) pick=true;
}
if(!pick && recent.size()==prm.stallWindow){
int distinct=(int)recent.stream().distinct().count();
if(distinct <= prm.minProgressInWindow) pick=true;
}
if(pick){
stepsOnGoal=0;
List<int[]> F=frontiers(bm);
if(F.isEmpty()) break;
List<List<int[]>> clusters = clusterFrontiers(F, prm.clusterRadius, prm.minFrontierCluster);
List<int[]> cand;
if(clusters.isEmpty()){
cand=F;
}else{
cand = clusters.stream().map(Chapter17_Lesson5::centroid).collect(Collectors.toList());
}
GoalEval best=null;
for(int[] g: cand){
if(prm.returnHome){
int d1=Math.abs(rx-g[0])+Math.abs(ry-g[1]);
int d2=Math.abs(g[0]-home[0])+Math.abs(g[1]-home[1]);
if(d1+d2 > budget) continue;
}
GoalEval ge=scoreGoal(bm, new int[]{rx,ry}, g);
if(ge.path.isEmpty()) continue;
if(best==null || ge.J > best.J) best=ge;
}
if(best==null) break;
currentGoal=best.goal;
currentPath=best.path;
}
if(currentPath.size() < 2){
currentGoal=null;
currentPath=Collections.emptyList();
continue;
}
int nx=currentPath.get(1)[0], ny=currentPath.get(1)[1];
if(world.isOccTrue(nx,ny)){
bm.update(Arrays.asList(new int[]{nx,ny,1}));
currentGoal=null;
currentPath=Collections.emptyList();
}else{
rx=nx; ry=ny;
travel += 1;
budget -= 1;
stepsOnGoal += 1;
bm.update(world.sense(rx,ry));
currentPath = currentPath.subList(1, currentPath.size());
}
step++;
}
double kf=bm.knownFraction();
double ent=bm.totalEntropy();
sumKnown += kf; sumKnown2 += kf*kf;
sumEnt += ent; sumEnt2 += ent*ent;
sumTrav += travel; sumTrav2 += travel*travel;
}
double meanKnown=sumKnown/seeds;
double stdKnown=Math.sqrt(Math.max(0.0, sumKnown2/seeds - meanKnown*meanKnown));
double meanEnt=sumEnt/seeds;
double stdEnt=Math.sqrt(Math.max(0.0, sumEnt2/seeds - meanEnt*meanEnt));
double meanTrav=sumTrav/seeds;
double stdTrav=Math.sqrt(Math.max(0.0, sumTrav2/seeds - meanTrav*meanTrav));
System.out.println("=== Exploration Sweep ("+seeds+" seeds) ===");
System.out.printf("Final Known Fraction: mean=%.3f, std=%.3f%n", meanKnown, stdKnown);
System.out.printf("Final Entropy: mean=%.3f, std=%.3f%n", meanEnt, stdEnt);
System.out.printf("Total Travel: mean=%.1f, std=%.1f%n", meanTrav, stdTrav);
}
}
6.4 MATLAB (script + helper functions)
File: Chapter17_Lesson5.m
% ============================================================
% Chapter17_Lesson5.m
% Lab: Autonomous Exploration in Unknown Map (single-robot)
% Self-contained MATLAB grid-world simulator:
% - log-odds occupancy belief
% - frontier detection + clustering (greedy L_inf balls)
% - A* planning on belief (4-neighborhood)
% - goal scoring: J = wIG*IG - wC*cost - wR*risk
%
% Run:
% Chapter17_Lesson5
% ============================================================
function Chapter17_Lesson5()
prm = default_params();
seeds = 10;
kf = zeros(seeds,1);
ent = zeros(seeds,1);
trav = zeros(seeds,1);
for s=1:seeds
res = explore_once(prm, s-1);
kf(s) = res.final_known_frac;
ent(s)= res.final_entropy;
trav(s)=res.total_travel;
end
fprintf('=== Exploration Sweep (%d seeds) ===\n', seeds);
fprintf('Final Known Fraction: mean=%.3f, std=%.3f\n', mean(kf), std(kf));
fprintf('Final Entropy: mean=%.3f, std=%.3f\n', mean(ent), std(ent));
fprintf('Total Travel: mean=%.1f, std=%.1f\n', mean(trav), std(trav));
end
function prm = default_params()
prm.W = 40; prm.H = 30;
prm.p_occ_true = 0.18;
prm.sensor_range = 6;
prm.max_steps = 1200;
prm.p_free = 0.35;
prm.p_occ = 0.65;
prm.l0 = 0.0;
prm.l_free = -0.85;
prm.l_occ = 0.85;
prm.l_min = -4.0;
prm.l_max = 4.0;
prm.w_ig = 1.0;
prm.w_cost = 0.20;
prm.w_risk = 0.60;
prm.risk_unknown_penalty = 0.25;
prm.risk_occ_weight = 1.0;
prm.min_frontier_cluster = 4;
prm.cluster_radius = 2;
prm.budget_steps = 1200;
prm.return_home = true;
prm.goal_timeout = 120;
prm.stall_window = 40;
prm.min_progress_in_window = 5;
end
function res = explore_once(prm, seed)
rng(seed);
% World generation
occ_true = zeros(prm.H, prm.W);
for y=1:prm.H
for x=1:prm.W
if x==1 || y==1 || x==prm.W || y==prm.H
occ_true(y,x)=1;
else
occ_true(y,x) = rand() < prm.p_occ_true;
end
end
end
start = [2,2]; % (x,y)
for dy=-2:2
for dx=-2:2
xx = start(1)+dx; yy=start(2)+dy;
if xx>=1 && xx<=prm.W && yy>=1 && yy<=prm.H
occ_true(yy,xx)=0;
end
end
end
home = start;
% Belief log-odds
L = prm.l0 * ones(prm.H, prm.W);
rx = start(1); ry = start(2);
L = update_from_sense(L, occ_true, rx, ry, prm);
budget = prm.budget_steps;
step = 0;
travel = 0;
recent = zeros(prm.stall_window, 2);
recent_count = 0;
current_goal = [];
current_path = [];
steps_on_goal = 0;
while step < prm.max_steps && budget > 0
recent_count = min(recent_count+1, prm.stall_window);
recent(2:recent_count,:) = recent(1:recent_count-1,:);
recent(1,:) = [rx,ry];
pick = false;
if isempty(current_goal) || size(current_path,1)<2 || steps_on_goal >= prm.goal_timeout
pick = true;
else
nx = current_path(2,1); ny=current_path(2,2);
if classify_cell(L, nx, ny, prm) == 2
pick = true;
end
if recent_count == prm.stall_window
distinct = size(unique(recent,'rows'),1);
if distinct <= prm.min_progress_in_window
pick = true;
end
end
end
if pick
steps_on_goal = 0;
F = find_frontiers(L, prm);
if isempty(F)
break;
end
clusters = cluster_points(F, prm.cluster_radius, prm.min_frontier_cluster);
if isempty(clusters)
cand = F;
else
cand = zeros(numel(clusters),2);
for k=1:numel(clusters)
cand(k,:) = round(mean(clusters{k},1));
end
end
bestJ = -inf;
bestGoal = [];
bestPath = [];
for k=1:size(cand,1)
g = cand(k,:);
if prm.return_home
d1 = abs(rx-g(1)) + abs(ry-g(2));
d2 = abs(g(1)-home(1)) + abs(g(2)-home(2));
if d1+d2 > budget
continue;
end
end
[J, path] = score_goal(L, [rx,ry], g, prm);
if isempty(path)
continue;
end
if J > bestJ
bestJ = J; bestGoal=g; bestPath=path;
end
end
if isempty(bestGoal)
break;
end
current_goal = bestGoal;
current_path = bestPath;
end
if size(current_path,1) < 2
current_goal = []; current_path = [];
continue;
end
nx = current_path(2,1); ny=current_path(2,2);
if occ_true(ny,nx) == 1
% collision: mark occupied and replan
L = update_cell(L, nx, ny, 1, prm);
current_goal = []; current_path = [];
else
rx = nx; ry = ny;
travel = travel + 1;
budget = budget - 1;
steps_on_goal = steps_on_goal + 1;
L = update_from_sense(L, occ_true, rx, ry, prm);
current_path = current_path(2:end,:);
end
step = step + 1;
end
res.final_known_frac = known_fraction(L, prm);
res.final_entropy = total_entropy(L);
res.total_travel = travel;
res.steps = step;
end
function L = update_from_sense(L, occ_true, rx, ry, prm)
R = prm.sensor_range;
for dy=-R:R
for dx=-R:R
if abs(dx)+abs(dy) <= R
x = rx+dx; y=ry+dy;
if x>=1 && x<=prm.W && y>=1 && y<=prm.H
occ = occ_true(y,x);
L = update_cell(L, x, y, occ, prm);
end
end
end
end
end
function L = update_cell(L, x, y, occ, prm)
if occ==1
L(y,x) = clamp(L(y,x)+prm.l_occ, prm.l_min, prm.l_max);
else
L(y,x) = clamp(L(y,x)+prm.l_free, prm.l_min, prm.l_max);
end
end
function v = clamp(x, lo, hi)
v = max(lo, min(hi, x));
end
function p = prob_cell(L, x, y)
p = 1.0/(1.0+exp(-L(y,x)));
end
function cls = classify_cell(L, x, y, prm)
p = prob_cell(L,x,y);
if p <= prm.p_free
cls=1;
elseif p >= prm.p_occ
cls=2;
else
cls=0;
end
end
function F = find_frontiers(L, prm)
F = [];
for y=2:prm.H-1
for x=2:prm.W-1
if classify_cell(L,x,y,prm) ~= 0
continue;
end
for k=1:4
mv = [1 0; -1 0; 0 1; 0 -1];
xx = x + mv(k,1); yy = y + mv(k,2);
if classify_cell(L,xx,yy,prm) == 1
F = [F; x y]; %#ok<AGROW>
break;
end
end
end
end
end
function clusters = cluster_points(P, radius, min_size)
if isempty(P)
clusters = {};
return;
end
unused = true(size(P,1),1);
clusters = {};
while any(unused)
idx = find(unused, 1);
seed = P(idx,:);
unused(idx)=false;
q = seed;
cl = seed;
while ~isempty(q)
cur = q(end,:); q(end,:)=[];
dif = max(abs(P - cur), [], 2);
near_idx = find(unused & dif <= radius);
for j=1:numel(near_idx)
unused(near_idx(j)) = false;
q = [q; P(near_idx(j),:)]; %#ok<AGROW>
cl = [cl; P(near_idx(j),:)]; %#ok<AGROW>
end
end
if size(cl,1) >= min_size
clusters{end+1} = cl; %#ok<AGROW>
end
end
end
function [J, path] = score_goal(L, start, goal, prm)
path = astar(L, start, goal, prm);
if isempty(path)
J = -inf;
return;
end
ig = visible_unknown_count(L, goal, prm) * log(2);
c = path_cost(L, path, prm);
r = path_risk(L, path, prm);
J = prm.w_ig*ig - prm.w_cost*c - prm.w_risk*r;
end
function cnt = visible_unknown_count(L, goal, prm)
gx = goal(1); gy=goal(2);
R = prm.sensor_range;
cnt = 0;
for dy=-R:R
for dx=-R:R
if abs(dx)+abs(dy) <= R
x=gx+dx; y=gy+dy;
if x>=1 && x<=prm.W && y>=1 && y<=prm.H
if classify_cell(L,x,y,prm)==0
cnt = cnt + 1;
end
end
end
end
end
end
function c = path_cost(L, path, prm)
c = 0;
for i=2:size(path,1)
x=path(i,1); y=path(i,2);
cls = classify_cell(L,x,y,prm);
if cls==2
c = inf; return;
elseif cls==0
c = c + 2;
else
c = c + 1;
end
end
end
function r = path_risk(L, path, prm)
if size(path,1)<2
r=0; return;
end
s = 0;
unk = 0;
for i=2:size(path,1)
x=path(i,1); y=path(i,2);
s = s + prob_cell(L,x,y) * prm.risk_occ_weight;
if classify_cell(L,x,y,prm)==0
unk = unk + 1;
end
end
avg = s / (size(path,1)-1);
r = avg + unk * prm.risk_unknown_penalty;
end
function path = astar(L, start, goal, prm)
W=prm.W; H=prm.H;
sx=start(1); sy=start(2);
gx=goal(1); gy=goal(2);
gscore = inf(H,W);
came_x = -ones(H,W);
came_y = -ones(H,W);
gscore(sy,sx)=0;
open = [sx sy manhattan([sx sy],[gx gy])];
while ~isempty(open)
[~,ii] = min(open(:,3));
cur = open(ii,1:2);
open(ii,:) = [];
if cur(1)==gx && cur(2)==gy
path = reconstruct(came_x, came_y, [sx sy], [gx gy]);
return;
end
for k=1:4
mv = [1 0; -1 0; 0 1; 0 -1];
nx = cur(1)+mv(k,1);
ny = cur(2)+mv(k,2);
if nx<1 || nx>W || ny<1 || ny>H
continue;
end
cls = classify_cell(L,nx,ny,prm);
if cls==2
continue;
end
sc = (cls==0)*2 + (cls~=0)*1;
tentative = gscore(cur(2),cur(1)) + sc;
if tentative < gscore(ny,nx)
gscore(ny,nx)=tentative;
came_x(ny,nx)=cur(1);
came_y(ny,nx)=cur(2);
f = tentative + manhattan([nx ny],[gx gy]);
open = [open; nx ny f]; %#ok<AGROW>
end
end
end
path = [];
end
function d = manhattan(a,b)
d = abs(a(1)-b(1)) + abs(a(2)-b(2));
end
function path = reconstruct(came_x, came_y, start, goal)
x=goal(1); y=goal(2);
path = [x y];
while ~(x==start(1) && y==start(2))
px = came_x(y,x);
py = came_y(y,x);
if px<0
path = [];
return;
end
x=px; y=py;
path = [x y; path]; %#ok<AGROW>
end
end
function Htot = total_entropy(L)
[H,W] = size(L);
Htot = 0;
for y=1:H
for x=1:W
p = 1.0/(1.0+exp(-L(y,x)));
p = max(1e-12, min(1-1e-12, p));
Htot = Htot + (-p*log(p) - (1-p)*log(1-p));
end
end
end
function kf = known_fraction(L, prm)
known=0; total=prm.W*prm.H;
for y=1:prm.H
for x=1:prm.W
if classify_cell(L,x,y,prm) ~= 0
known = known + 1;
end
end
end
kf = known/total;
end
6.5 Wolfram Mathematica
File: Chapter17_Lesson5.nb
(* ============================================================
Chapter17_Lesson5.nb
Lab: Autonomous Exploration in Unknown Map (single-robot)
Mathematica / Wolfram Language demonstration:
- log-odds belief map
- frontier detection
- simple A* (grid)
- goal scoring with IG heuristic
============================================================ *)
ClearAll["Global`*"];
sigmoid[x_] := 1/(1 + Exp[-x]);
clamp[x_, lo_, hi_] := Max[lo, Min[hi, x]];
bernoulliEntropy[p_] := Module[{q = clamp[p, 10^-12, 1 - 10^-12]},
-q Log[q] - (1 - q) Log[1 - q]
];
params = <|
"W" -> 40, "H" -> 30, "pOccTrue" -> 0.18, "sensorRange" -> 6, "maxSteps" -> 1200,
"pFree" -> 0.35, "pOcc" -> 0.65,
"l0" -> 0.0, "lFree" -> -0.85, "lOcc" -> 0.85, "lMin" -> -4.0, "lMax" -> 4.0,
"wIG" -> 1.0, "wCost" -> 0.20, "wRisk" -> 0.60,
"riskUnknownPenalty" -> 0.25, "riskOccWeight" -> 1.0,
"minFrontierCluster" -> 4, "clusterRadius" -> 2,
"budgetSteps" -> 1200, "returnHome" -> True
|>;
W = params["W"]; H = params["H"];
startPose = {2, 2};
SeedRandom[0];
(* True world occupancy *)
occTrue = Table[0, {H}, {W}];
Do[
If[x == 1 || y == 1 || x == W || y == H,
occTrue[[y, x]] = 1,
occTrue[[y, x]] = Boole[RandomReal[] < params["pOccTrue"]]
],
{y, 1, H}, {x, 1, W}
];
(* carve around start *)
Do[
With[{xx = startPose[[1]] + dx, yy = startPose[[2]] + dy},
If[1 <= xx <= W && 1 <= yy <= H, occTrue[[yy, xx]] = 0]
],
{dy, -2, 2}, {dx, -2, 2}
];
(* Belief log-odds *)
L = ConstantArray[params["l0"], {H, W}];
pCell[{x_, y_}] := sigmoid[L[[y, x]]];
classify[{x_, y_}] := Module[{p = pCell[{x, y}]},
Which[
p <= params["pFree"], 1, (* free *)
p >= params["pOcc"], 2, (* occupied *)
True, 0 (* unknown *)
]
];
sense[{rx_, ry_}] := Module[{R = params["sensorRange"], obs = {} },
Do[
If[Abs[dx] + Abs[dy] <= R,
With[{x = rx + dx, y = ry + dy},
If[1 <= x <= W && 1 <= y <= H,
AppendTo[obs, {x, y, occTrue[[y, x]]}]
]
]
],
{dy, -R, R}, {dx, -R, R}
];
obs
];
updateCell[{x_, y_}, occ_] := (
If[occ == 1,
L[[y, x]] = clamp[L[[y, x]] + params["lOcc"], params["lMin"], params["lMax"]],
L[[y, x]] = clamp[L[[y, x]] + params["lFree"], params["lMin"], params["lMax"]]
];
);
updateFromObs[obs_] := Scan[(updateCell[{ #[[1]], #[[2]]}, #[[3]]]) &, obs];
totalEntropy[] := Total[Flatten[bernoulliEntropy /@ (pCell /@ Flatten[Table[{x, y}, {y, 1, H}, {x, 1, W}], 1])]];
knownFraction[] := Module[{known = 0},
Do[
If[classify[{x, y}] != 0, known++],
{y, 1, H}, {x, 1, W}
];
known/(W H)
];
frontiers[] := Module[{F = {} },
Do[
If[classify[{x, y}] == 0,
If[AnyTrue[{ {1, 0}, {-1, 0}, {0, 1}, {0, -1} },
(classify[{x + #[[1]], y + #[[2]]}] == 1) &],
AppendTo[F, {x, y}]
]
],
{y, 2, H - 1}, {x, 2, W - 1}
];
F
];
(* A* on belief (unknown penalty) *)
neighbors[{x_, y_}] := Select[{ {x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1} },
(1 <= #[[1]] <= W && 1 <= #[[2]] <= H) &];
stepCost[pos_] := Module[{c = classify[pos]},
Which[c == 2, Infinity, c == 0, 2.0, True, 1.0]
];
heuristic[a_, b_] := Abs[a[[1]] - b[[1]]] + Abs[a[[2]] - b[[2]]];
astar[start_, goal_] := Module[
{open = {start}, came = <||>, gScore = <||>, fScore = <||>, current, best, nbs, tentative},
gScore[start] = 0.0;
fScore[start] = heuristic[start, goal];
While[Length[open] > 0,
best = First@MinimalBy[open, fScore[#] &];
current = best;
If[current === goal,
(* reconstruct *)
Module[{path = {current} },
While[KeyExistsQ[came, current],
current = came[current];
path = Prepend[path, current];
];
Return[path];
]
];
open = DeleteCases[open, current];
nbs = neighbors[current];
Do[
If[stepCost[nb] == Infinity, Continue[]];
tentative = gScore[current] + stepCost[nb];
If[! KeyExistsQ[gScore, nb] || tentative < gScore[nb],
came[nb] = current;
gScore[nb] = tentative;
fScore[nb] = tentative + heuristic[nb, goal];
If[! MemberQ[open, nb], open = Append[open, nb]];
],
{nb, nbs}
];
];
{}
];
visibleUnknownCount[g_] := Module[{R = params["sensorRange"], cnt = 0},
Do[
If[Abs[dx] + Abs[dy] <= R,
With[{x = g[[1]] + dx, y = g[[2]] + dy},
If[1 <= x <= W && 1 <= y <= H,
If[classify[{x, y}] == 0, cnt++]
]
]
],
{dy, -R, R}, {dx, -R, R}
];
cnt
];
pathCost[path_] := Total[stepCost /@ Rest[path]];
pathRisk[path_] := Module[{cells = Rest[path], s = 0.0, unk = 0},
If[Length[cells] == 0, Return[0.0]];
Do[
s += pCell[c] * params["riskOccWeight"];
If[classify[c] == 0, unk++],
{c, cells}
];
(s/Length[cells]) + unk * params["riskUnknownPenalty"]
];
scoreGoal[start_, goal_] := Module[{path = astar[start, goal], ig, c, r, J},
If[path === {} , Return[{ -Infinity, {}, <|"IG" -> 0, "Cost" -> Infinity, "Risk" -> Infinity|> }]];
ig = visibleUnknownCount[goal] * Log[2];
c = pathCost[path];
r = pathRisk[path];
J = params["wIG"]*ig - params["wCost"]*c - params["wRisk"]*r;
{J, path, <|"IG" -> ig, "Cost" -> c, "Risk" -> r|>}
];
(* Run one short exploration demo *)
robot = startPose;
home = startPose;
updateFromObs[sense[robot]];
steps = 0;
budget = params["budgetSteps"];
travel = 0;
entropyLog = {};
knownLog = {};
While[steps < 400 && budget > 0,
AppendTo[entropyLog, totalEntropy[]];
AppendTo[knownLog, knownFraction[]];
F = frontiers[];
If[F === {}, Break[]];
(* candidates: use raw frontier points for simplicity in this WL demo *)
candidates = RandomSample[F, Min[30, Length[F]]];
best = { -Infinity, {}, <||>, {} };
Do[
If[params["returnHome"],
d1 = heuristic[robot, g];
d2 = heuristic[g, home];
If[d1 + d2 > budget, Continue[]];
];
{J, path, det} = scoreGoal[robot, g];
If[J > best[[1]], best = {J, g, det, path}],
{g, candidates}
];
If[best[[2]] === {}, Break[]];
path = best[[4]];
If[Length[path] < 2, Break[]];
next = path[[2]];
If[occTrue[[ next[[2]], next[[1]] ]] == 1,
updateCell[next, 1],
robot = next;
travel++;
budget--;
updateFromObs[sense[robot]];
];
steps++;
];
Print["Final Known Fraction: ", knownLog[[-1]]];
Print["Final Entropy: ", entropyLog[[-1]]];
Print["Travel: ", travel];
Grading tip: require students to report: final known fraction, final entropy, total travel, and a plot of entropy vs time for at least 10 random seeds.
7. Problems and Solutions
Problem 1 (Entropy maximum and uncertainty): Show that for \( p \in (0,1) \), the Bernoulli entropy \( H(p)=-p\log p -(1-p)\log(1-p) \) is maximized at \( p=0.5 \), and the maximum is \( \log 2 \).
Solution: Differentiate: \( H'(p) = -\log p - 1 + \log(1-p) + 1 = \log\frac{1-p}{p} \). Setting \( H'(p)=0 \) gives \( (1-p)/p = 1 \) hence \( p=0.5 \). Second derivative: \( H''(p) = -\frac{1}{p} - \frac{1}{1-p} \), which is negative for \( p\in(0,1) \), so it is a maximum. Finally, \( H(0.5) = -0.5\log 0.5 -0.5\log 0.5 = \log 2 \).
Problem 2 (Heuristic IG derivation): Suppose cells in \( \mathcal{V}_{\text{unk} }(g) \) are unknown with \( p_t(i)=0.5 \), and after a perfect observation become certain: \( p_{t+1}(i) \in \{0,1\} \). Show that the information gain equals \( |\mathcal{V}_{\text{unk} }(g)|\log 2 \).
Solution: For any such cell, \( H(p_t(i))=H(0.5)=\log 2 \) and \( H(p_{t+1}(i))=H(0)=H(1)=0 \). Therefore, \( H(p_t(i)) - H(p_{t+1}(i)) = \log 2 \). Summing over all unknown-visible cells yields \( \operatorname{IG}(g)=\sum_{i\in\mathcal{V}_{\text{unk} }(g)} \log 2 = |\mathcal{V}_{\text{unk} }(g)|\log 2 \).
Problem 3 (Score scaling invariance): Consider the score \( J(g)= w_{\mathrm{IG} }\operatorname{IG}(g) - w_c c(g) - w_{\rho}\rho(g) \). Show that if all weights are multiplied by a constant \( \alpha > 0 \), the maximizing goal \( g^\star \) does not change.
Solution: Define \( \tilde{J}(g)=\alpha J(g) \). For any two candidates \( g_1,g_2 \), \( J(g_1) > J(g_2) \iff \alpha J(g_1) > \alpha J(g_2) \) because \( \alpha > 0 \) preserves inequalities. Hence \( \arg\max_g \tilde{J}(g)=\arg\max_g J(g) \).
Problem 4 (Feasible-goal rejection test): Let remaining time be \( B \) and speed limit be \( v_{\max} \). Show that a sufficient condition to reject a candidate \( g \) is \( d(x_t,g) + d(g,x_0) > v_{\max} B \), where \( x_0 \) is the home pose.
Solution: From the feasibility constraint \( \frac{d(x_t,g)}{v_{\max} } + \frac{d(g,x_0)}{v_{\max} } \le B \), multiply by \( v_{\max} > 0 \) to obtain \( d(x_t,g) + d(g,x_0) \le v_{\max} B \). Therefore, if \( d(x_t,g) + d(g,x_0) > v_{\max} B \), the constraint cannot hold, so rejecting \( g \) is sufficient (conservative).
Problem 5 (Frontier correctness): Prove that if a cell \( i \) is a frontier cell by the definition in Section 2.2, then there exists a path from the robot to the neighborhood of \( i \) that stays within known-free cells if the known-free region is 4-connected and contains the robot pose.
Solution: Since \( i \in \mathcal{F}_t \), there exists a neighbor \( j\in\mathcal{N}_4(i) \) such that \( j \) is known-free. If the known-free set is 4-connected and contains the robot pose \( x_t \), then by definition of 4-connectivity there exists a 4-neighbor path from \( x_t \) to \( j \) that stays entirely in known-free cells. Thus the robot can reach a cell adjacent to the unknown region (the frontier boundary) without traversing unknown or occupied cells.
8. Summary
This lab combined occupancy-grid mapping with active decision-making: extract frontiers, score goals using information gain versus travel and risk, and execute goals through a navigation interface. You also learned how to quantify exploration performance via known-fraction and entropy reduction curves, and how to tune weights to trade off speed of discovery against safety and efficiency.
9. References
- Yamauchi, B. (1997). A frontier-based approach for autonomous exploration. Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation, 146–151.
- Bourgault, F., Makarenko, A., Williams, S.B., Grocholsky, B., & Durrant-Whyte, H.F. (2002). Information based adaptive robotic exploration. IEEE/RSJ International Conference on Intelligent Robots and Systems.
- Sim, R., & Roy, N. (2005). Global A-optimal robot exploration in SLAM. IEEE International Conference on Robotics and Automation, 661–666.
- Carrillo, H., Reid, I., Castellanos, J.A., & Dissanayake, G. (2012). On the comparison of uncertainty criteria for active SLAM. International Journal of Robotics Research, 31(14), 1701–1716.
- Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
- Stachniss, C. (2009). Robot Mapping and Exploration. (Lecture notes / monograph style references.)
- Valencia, R., Morta, M., Andrade-Cetto, J., & Porta, J.M. (2013). Planning reliable paths with pose SLAM. IEEE Transactions on Robotics, 29(4), 1050–1059.
- Burgard, W., Moors, M., Stachniss, C., & Schneider, F. (2005). Coordinated multi-robot exploration. IEEE Transactions on Robotics, 21(3), 376–386. (Background; this lab is single-robot.)
