Frontier-based exploration converts mapping progress into navigation goals. Using the current occupancy map, the robot identifies a set of \( \text{frontiers} \): boundaries between \( \text{known-free} \) space and \( \text{unknown} \) space. It then selects a frontier goal, navigates to it with the existing navigation stack, and updates the map with new sensor information. Repeating this loop expands the known region until no reachable frontiers remain.

flowchart TD
  A["Inputs: occupancy grid + robot pose"] --> B["Detect frontier cells"]
  B --> C["Cluster frontiers (connected components)"]
  C --> D["Compute goal candidates (centroid / best free cell)"]
  D --> E["Score candidates: cost vs expected gain"]
  E --> F["Send selected goal to local+global navigation"]
  F --> G["Arrive / fail / timeout"]
  G --> H["Update map with new sensor data"]
  H --> B
        

The algorithm is attractive because it is simple, online, and compatible with a standard AMR navigation stack (global planner + local planner + costmaps). Its main limitations are (i) local minima due to greedy selection, (ii) sensitivity to map noise near unknown/free boundaries, and (iii) potential oscillations if goals are not stabilized.

Let the environment be discretized into a grid graph \( G=(\mathcal{C},\mathcal{E}) \), where each cell \( c\in\mathcal{C} \) has a belief \( p_t(c)=\Pr(m(c)=1\mid z_{1:t},u_{1:t}) \) of being occupied at time \( t \). (Occupancy grid mapping and log-odds updates were covered earlier; here we only use the resulting discrete state.)

We use a thresholding rule to assign a discrete label \( s_t(c)\in\{\text{FREE},\text{OCC},\text{UNK}\} \):

\[ s_t(c)=\begin{cases} \text{OCC} & \text{if } p_t(c)\ge p_{\text{occ} } \\ \text{FREE} & \text{if } p_t(c)\le p_{\text{free} } \\ \text{UNK} & \text{otherwise} \end{cases} \]

where \( 0 < p_{\text{free} } < p_{\text{occ} } < 1 \). Let \( \mathcal{N}_4(c) \) be the 4-neighborhood (von Neumann) and \( \mathcal{N}_8(c) \) the 8-neighborhood (Moore).

Definition (frontier cell). A cell \( c \) is a frontier cell at time \( t \) if it is known free and adjacent to at least one unknown cell:

\[ \mathcal{F}_t = \left\{ c\in\mathcal{C}\; \middle|\; s_t(c)=\text{FREE} \;\wedge\; \exists c'\in\mathcal{N}_4(c)\text{ such that } s_t(c')=\text{UNK} \right\}. \]

Using \( \mathcal{N}_4 \) for detection is common because it reduces diagonal “touching” artifacts. For clustering, \( \mathcal{N}_8 \) is often preferred to keep diagonally connected boundaries in the same cluster.

Lemma (frontier set as a boundary). Let \( \mathcal{K}_t = \{c\in\mathcal{C}\mid s_t(c)\ne \text{UNK}\} \) be the set of known cells, and \( \mathcal{F}_t \) defined above. Then every frontier cell lies on the boundary between known-free cells and unknown cells, and if \( \mathcal{F}_t=\emptyset \) then there is no unknown cell adjacent to any known-free cell.

Proof. If \( c\in\mathcal{F}_t \), then by definition \( s_t(c)=\text{FREE} \) and there exists \( c'\in\mathcal{N}_4(c) \) with \( s_t(c')=\text{UNK} \), hence \( c \) is adjacent to unknown and is therefore on a known/unknown boundary. Conversely, if \( \mathcal{F}_t=\emptyset \), then no known-free cell has an unknown neighbor, i.e., the known-free region has no boundary contact with unknown space. \( \blacksquare \)

In practice, additional filters are used: (i) remove frontier cells near obstacles using a clearance threshold, (ii) discard very small frontier clusters (often noise), and (iii) enforce reachability with the current free-space connectivity.

Frontier cells typically form long, thin boundaries. Treating each cell as a candidate goal is inefficient and unstable. Instead, we compute connected components under \( \mathcal{N}_8 \) adjacency: \( \{\mathcal{F}_t^{(1)},\dots,\mathcal{F}_t^{(K)}\} \), where \( \mathcal{F}_t=\bigsqcup_{k=1}^K \mathcal{F}_t^{(k)} \).

For cluster \( \mathcal{F}_t^{(k)} \), define its geometric centroid in world coordinates: if each cell \( c \) has center position \( \mathbf{x}(c)\in\mathbb{R}^2 \), then

\[ \boldsymbol{\mu}_k = \frac{1}{\left|\mathcal{F}_t^{(k)}\right|} \sum_{c\in \mathcal{F}_t^{(k)} } \mathbf{x}(c). \]

However, the centroid can fall (i) inside unknown space, (ii) inside obstacles (due to discretization), or (iii) on an unreachable pocket. A robust goal-extraction method is:

\[ \mathbf{g}_k = \arg\min_{\mathbf{x}(c)} \left\{ \left\|\mathbf{x}(c)-\boldsymbol{\mu}_k\right\| \; \middle|\; s_t(c)=\text{FREE},\; c\in\mathcal{R}_t,\; \delta_t(c)\ge \delta_{\min} \right\}, \]

where \( \mathcal{R}_t \) is the set of free cells reachable from the robot in the current map, \( \delta_t(c) \) is obstacle clearance (e.g., distance transform on occupied cells), and \( \delta_{\min} \) is a safety margin consistent with robot radius + inflation.

flowchart TD
  A["Frontier cells"] --> B["Connected components (8-neighborhood)"]
  B --> C["For each cluster: centroid (mean position)"]
  C --> D["Project to reachable FREE cell with clearance"]
  D --> E["Compute path cost from robot"]
  E --> F["Score: alpha*cost - beta*cluster_size"]
  F --> G["Pick best cluster goal"]
        

This keeps goals stable (cluster-level) and safe (clearance constraint), and avoids commanding the robot into unknown cells.

Let the robot pose be \( \mathbf{x}_r(t) \). For each candidate goal \( \mathbf{g}_k \), define a travel cost based on the shortest path length over currently free cells: \( L_t(\mathbf{x}_r,\mathbf{g}_k) \). On grids with uniform cost, BFS computes this exactly; with costs/inflation, Dijkstra or A* is used.

Frontier-based exploration uses a greedy objective of the form \( \text{gain} - \text{cost} \). A simple, effective rule is:

\[ k^\star = \arg\min_{k\in\{1,\dots,K\} } J_t(k), \quad J_t(k) = \alpha\,L_t(\mathbf{x}_r,\mathbf{g}_k) - \beta\,\left|\mathcal{F}_t^{(k)}\right|. \]

Here \( \alpha > 0 \) sets how strongly we penalize travel, while \( \beta > 0 \) rewards larger frontiers (a proxy for “expected new area”). More refined utilities approximate expected map expansion based on sensor range and visibility, but those belong to entropy/information-gain methods (next lesson).

Reachability constraint. If \( L_t(\mathbf{x}_r,\mathbf{g}_k)=+\infty \), the goal is unreachable under the current map connectivity and should be ignored or deferred.

Theorem (finite termination under monotone discovery). Assume: (i) the grid has a finite number of cells, (ii) the robot can reach any selected goal \( \mathbf{g}_{k^\star} \) that is reachable in the current map, and (iii) upon reaching a frontier goal, at least one previously unknown cell becomes known (FREE or OCC), i.e., \( |\mathcal{K}_{t+1}| \ge |\mathcal{K}_t| + 1 \). Then the frontier-based exploration loop terminates after finitely many successful goal executions with \( \mathcal{F}_t=\emptyset \) (no adjacent unknown to known-free).

Proof. By assumption (iii), \( |\mathcal{K}_t| \) strictly increases by at least 1 after each successful goal execution. Because the grid is finite, \( |\mathcal{K}_t| \le |\mathcal{C}| \), so only finitely many strict increases are possible. Therefore, after finitely many steps no further unknown-to-known conversions can occur from reachable frontier motion. At that point, there cannot exist a known-free cell adjacent to an unknown cell; otherwise moving to that boundary would reveal at least one unknown cell (contradicting maximality). Hence \( \mathcal{F}_t=\emptyset \). \( \blacksquare \)

Practical note. Failures (local planner oscillation, stuck, timeouts) violate assumption (ii). AMR stacks handle this using recovery behaviors (already covered in Chapter 14) and by blacklisting repeatedly failing goals.

Frontier exploration is usually implemented as a node that subscribes to: \( \text{(i) occupancy grid} \), \( \text{(ii) robot pose} \), and publishes a goal to the navigation action server. In ROS2/Nav2, a typical pipeline is:

• Map source: SLAM toolbox / mapping node publishes occupancy grid.
• Exploration node: computes \( \mathcal{F}_t \), clusters, selects \( \mathbf{g}_{k^\star} \).
• Nav2: NavigateToPose action handles global planning + local control + recovery.
• Feedback: if goal fails, pick a new frontier or trigger recovery.

Simulink implementation idea. You can implement the exploration loop with Robotics System Toolbox blocks: (1) Occupancy grid input, (2) MATLAB Function block for frontier detection/clustering/scoring, (3) Path planning (grid planner) block, (4) Controller block (Pure Pursuit / local planner), (5) Map update from simulated LiDAR. The provided MATLAB code can be placed inside a MATLAB Function block (after adapting array I/O).

The following reference codes implement the core operations: frontier detection, clustering, reachable goal selection, and a simple utility score. They use a common encoding (unknown = -1, free = 0, occupied = 100), consistent with many AMR stacks.

6.1) Python (NumPy)

File: Chapter17_Lesson1.py

"""
Autonomous Mobile Robots — Chapter 17, Lesson 1: Frontier-Based Exploration
File: Chapter17_Lesson1.py

This script demonstrates a self-contained frontier-based exploration loop on a 2D occupancy grid.
Grid encoding (common in ROS nav_msgs/OccupancyGrid):
  -1 = unknown, 0 = free, 100 = occupied

Core steps:
  1) detect frontier cells
  2) cluster frontiers
  3) score clusters by a distance-vs-size utility
  4) choose a reachable goal near the best cluster
  5) (toy) update map to simulate sensing at the goal

NOTE: This is a teaching reference implementation; integrate with ROS2/Nav2 by replacing
the toy "simulate_sensor_update" with real mapping updates and path planning calls.
"""
from __future__ import annotations

from collections import deque
from dataclasses import dataclass
from typing import List, Tuple, Optional, Set

import numpy as np


Coord = Tuple[int, int]  # (row, col)


@dataclass
class FrontierCluster:
    cells: List[Coord]

    @property
    def size(self) -> int:
        return len(self.cells)

    def centroid(self) -> Tuple[float, float]:
        rs = [c[0] for c in self.cells]
        cs = [c[1] for c in self.cells]
        return (float(np.mean(rs)), float(np.mean(cs)))


def in_bounds(grid: np.ndarray, rc: Coord) -> bool:
    r, c = rc
    return 0 <= r < grid.shape[0] and 0 <= c < grid.shape[1]


def neighbors8(grid: np.ndarray, rc: Coord) -> List[Coord]:
    r, c = rc
    nbrs = []
    for dr in (-1, 0, 1):
        for dc in (-1, 0, 1):
            if dr == 0 and dc == 0:
                continue
            rr, cc = r + dr, c + dc
            if 0 <= rr < grid.shape[0] and 0 <= cc < grid.shape[1]:
                nbrs.append((rr, cc))
    return nbrs


def neighbors4(grid: np.ndarray, rc: Coord) -> List[Coord]:
    r, c = rc
    nbrs = []
    for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)):
        rr, cc = r + dr, c + dc
        if 0 <= rr < grid.shape[0] and 0 <= cc < grid.shape[1]:
            nbrs.append((rr, cc))
    return nbrs


def is_free(v: int) -> bool:
    return v == 0


def is_unknown(v: int) -> bool:
    return v == -1


def detect_frontiers(grid: np.ndarray) -> List[Coord]:
    """
    Frontier cell definition:
      cell is FREE and has at least one UNKNOWN neighbor (4-neighborhood is typical).
    """
    frontiers: List[Coord] = []
    H, W = grid.shape
    for r in range(H):
        for c in range(W):
            if not is_free(int(grid[r, c])):
                continue
            for rr, cc in neighbors4(grid, (r, c)):
                if is_unknown(int(grid[rr, cc])):
                    frontiers.append((r, c))
                    break
    return frontiers


def cluster_frontiers(grid: np.ndarray, frontier_cells: List[Coord]) -> List[FrontierCluster]:
    """
    Cluster frontier cells using 8-connectivity.
    """
    frontier_set: Set[Coord] = set(frontier_cells)
    visited: Set[Coord] = set()
    clusters: List[FrontierCluster] = []

    for cell in frontier_cells:
        if cell in visited:
            continue
        if cell not in frontier_set:
            continue
        q = deque([cell])
        visited.add(cell)
        comp: List[Coord] = []

        while q:
            u = q.popleft()
            comp.append(u)
            for v in neighbors8(grid, u):
                if v in frontier_set and v not in visited:
                    visited.add(v)
                    q.append(v)

        clusters.append(FrontierCluster(comp))

    clusters.sort(key=lambda cl: cl.size, reverse=True)
    return clusters


def bfs_shortest_path_length(grid: np.ndarray, start: Coord) -> np.ndarray:
    """
    Shortest path length from start to all reachable FREE cells using BFS (4-neighborhood).
    Obstacles and unknown cells are treated as blocked.
    """
    H, W = grid.shape
    dist = np.full((H, W), np.inf, dtype=float)
    if not is_free(int(grid[start[0], start[1]])):
        return dist

    q = deque([start])
    dist[start[0], start[1]] = 0.0

    while q:
        u = q.popleft()
        du = dist[u[0], u[1]]
        for v in neighbors4(grid, u):
            if not is_free(int(grid[v[0], v[1]])):
                continue
            if dist[v[0], v[1]] == np.inf:
                dist[v[0], v[1]] = du + 1.0
                q.append(v)
    return dist


def choose_goal_near_cluster(
    grid: np.ndarray,
    dist_from_robot: np.ndarray,
    cluster: FrontierCluster,
    max_search_radius: int = 6,
) -> Optional[Coord]:
    """
    Choose a reachable FREE goal near the cluster centroid by searching a window around it.
    """
    cr, cc = cluster.centroid()
    r0, c0 = int(round(cr)), int(round(cc))

    best: Optional[Coord] = None
    best_d = np.inf

    for rad in range(0, max_search_radius + 1):
        for r in range(r0 - rad, r0 + rad + 1):
            for c in range(c0 - rad, c0 + rad + 1):
                if not in_bounds(grid, (r, c)):
                    continue
                if not is_free(int(grid[r, c])):
                    continue
                d = dist_from_robot[r, c]
                if not np.isfinite(d):
                    continue
                if d < best_d:
                    best_d = d
                    best = (r, c)
        if best is not None:
            return best
    return None


def score_cluster(distance: float, size: int, alpha: float = 1.0, beta: float = 3.0) -> float:
    """
    J = alpha * distance - beta * size (lower is better)
    """
    return alpha * float(distance) - beta * float(size)


def select_best_frontier_goal(
    grid: np.ndarray,
    robot: Coord,
    alpha: float = 1.0,
    beta: float = 3.0,
    min_cluster_size: int = 5,
):
    frontier_cells = detect_frontiers(grid)
    if not frontier_cells:
        return None, []

    clusters = cluster_frontiers(grid, frontier_cells)
    clusters = [cl for cl in clusters if cl.size >= min_cluster_size]
    if not clusters:
        return None, []

    dist = bfs_shortest_path_length(grid, robot)

    best_goal = None
    best_cost = np.inf

    for cl in clusters:
        goal = choose_goal_near_cluster(grid, dist, cl)
        if goal is None:
            continue
        d = dist[goal[0], goal[1]]
        J = score_cluster(d, cl.size, alpha=alpha, beta=beta)
        if J < best_cost:
            best_cost = J
            best_goal = goal

    return best_goal, clusters


def simulate_sensor_update(grid: np.ndarray, center: Coord, radius: int = 4) -> None:
    """
    Toy sensing: reveal unknown cells in a square neighborhood around center.
    """
    r0, c0 = center
    H, W = grid.shape
    for r in range(max(0, r0 - radius), min(H, r0 + radius + 1)):
        for c in range(max(0, c0 - radius), min(W, c0 + radius + 1)):
            if int(grid[r, c]) == -1:
                grid[r, c] = 0


def demo() -> None:
    H, W = 30, 40
    grid = np.full((H, W), -1, dtype=int)

    grid[5:15, 5:18] = 0
    grid[5:15, 12] = 100
    grid[10, 12:30] = 100
    grid[18:25, 25:35] = 0

    robot = (8, 8)

    for step in range(1, 9):
        goal, clusters = select_best_frontier_goal(grid, robot, alpha=1.0, beta=2.5, min_cluster_size=6)
        print(f"[step {step}] robot={robot} clusters={len(clusters)} goal={goal}")
        if goal is None:
            print("No frontier goal found. Exploration may be complete (reachable area).")
            break
        robot = goal
        simulate_sensor_update(grid, robot, radius=4)

    print("Remaining unknown cells:", int(np.sum(grid == -1)))


if __name__ == "__main__":
    demo()
      

Exercise file: Chapter17_Lesson1_Ex1.py

"""
Autonomous Mobile Robots — Chapter 17, Lesson 1 (Exercise 1)
File: Chapter17_Lesson1_Ex1.py

Exercise: Implement frontier clustering (8-connectivity).
"""
from __future__ import annotations

from collections import deque
from typing import List, Tuple, Set

import numpy as np

Coord = Tuple[int, int]


def neighbors8(grid: np.ndarray, rc: Coord) -> List[Coord]:
    r, c = rc
    out = []
    for dr in (-1, 0, 1):
        for dc in (-1, 0, 1):
            if dr == 0 and dc == 0:
                continue
            rr, cc = r + dr, c + dc
            if 0 <= rr < grid.shape[0] and 0 <= cc < grid.shape[1]:
                out.append((rr, cc))
    return out


def cluster_frontiers_exercise(grid: np.ndarray, frontier_cells: List[Coord]) -> List[List[Coord]]:
    """
    TODO:
      - Use BFS/DFS to group frontier_cells into connected components under 8-connectivity.
      - Return a list of clusters, each a list of cells.
    """
    frontier_set: Set[Coord] = set(frontier_cells)
    visited: Set[Coord] = set()
    clusters: List[List[Coord]] = []

    # TODO: implement
    raise NotImplementedError("Implement me!")


if __name__ == "__main__":
    grid = np.zeros((6, 6), dtype=int)
    frontiers = [(1, 1), (1, 2), (2, 2), (4, 4)]
    print(cluster_frontiers_exercise(grid, frontiers))
      

6.2) C++ (self-contained)

File: Chapter17_Lesson1.cpp

/*
Autonomous Mobile Robots — Chapter 17, Lesson 1: Frontier-Based Exploration
File: Chapter17_Lesson1.cpp
*/

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

using Coord = std::pair<int,int>; // (row, col)

static inline bool in_bounds(int H, int W, int r, int c) {
  return (0 <= r && r < H && 0 <= c && c < W);
}

static inline bool is_free(int v) { return v == 0; }
static inline bool is_unknown(int v) { return v == -1; }

static std::vector<Coord> neighbors4(int H, int W, Coord u) {
  const int r = u.first, c = u.second;
  std::vector<Coord> out;
  const int dr[4] = {-1, 1, 0, 0};
  const int dc[4] = {0, 0, -1, 1};
  for (int k = 0; k < 4; ++k) {
    int rr = r + dr[k], cc = c + dc[k];
    if (in_bounds(H, W, rr, cc)) out.push_back({rr, cc});
  }
  return out;
}

static std::vector<Coord> neighbors8(int H, int W, Coord u) {
  const int r = u.first, c = u.second;
  std::vector<Coord> out;
  for (int dr = -1; dr <= 1; ++dr) {
    for (int dc = -1; dc <= 1; ++dc) {
      if (dr == 0 && dc == 0) continue;
      int rr = r + dr, cc = c + dc;
      if (in_bounds(H, W, rr, cc)) out.push_back({rr, cc});
    }
  }
  return out;
}

static std::vector<Coord> detect_frontiers(const std::vector<std::vector<int>>& grid) {
  const int H = (int)grid.size();
  const int W = (int)grid[0].size();
  std::vector<Coord> frontiers;
  for (int r = 0; r < H; ++r) {
    for (int c = 0; c < W; ++c) {
      if (!is_free(grid[r][c])) continue;
      for (auto v : neighbors4(H, W, {r,c})) {
        if (is_unknown(grid[v.first][v.second])) { frontiers.push_back({r,c}); break; }
      }
    }
  }
  return frontiers;
}

static std::vector<std::vector<Coord>> cluster_frontiers(const std::vector<std::vector<int>>& grid,
                                                         const std::vector<Coord>& frontier_cells) {
  const int H = (int)grid.size();
  const int W = (int)grid[0].size();

  std::set<Coord> frontier_set(frontier_cells.begin(), frontier_cells.end());
  std::set<Coord> visited;
  std::vector<std::vector<Coord>> clusters;

  for (auto cell : frontier_cells) {
    if (visited.count(cell)) continue;
    if (!frontier_set.count(cell)) continue;

    std::queue<Coord> q;
    q.push(cell);
    visited.insert(cell);

    std::vector<Coord> comp;
    while (!q.empty()) {
      Coord u = q.front(); q.pop();
      comp.push_back(u);
      for (auto v : neighbors8(H, W, u)) {
        if (frontier_set.count(v) && !visited.count(v)) {
          visited.insert(v);
          q.push(v);
        }
      }
    }
    clusters.push_back(comp);
  }

  std::sort(clusters.begin(), clusters.end(),
            [](const std::vector<Coord>& a, const std::vector<Coord>& b){ return a.size() > b.size(); });
  return clusters;
}

static std::vector<std::vector<double>> bfs_dist_free(const std::vector<std::vector<int>>& grid, Coord start) {
  const int H = (int)grid.size();
  const int W = (int)grid[0].size();
  const double INF = std::numeric_limits<double>::infinity();

  std::vector<std::vector<double>> dist(H, std::vector<double>(W, INF));
  if (!is_free(grid[start.first][start.second])) return dist;

  std::queue<Coord> q;
  q.push(start);
  dist[start.first][start.second] = 0.0;

  while (!q.empty()) {
    Coord u = q.front(); q.pop();
    double du = dist[u.first][u.second];
    for (auto v : neighbors4(H, W, u)) {
      if (!is_free(grid[v.first][v.second])) continue;
      if (!std::isfinite(dist[v.first][v.second])) {
        dist[v.first][v.second] = du + 1.0;
        q.push(v);
      }
    }
  }
  return dist;
}

static Coord rounded_centroid(const std::vector<Coord>& comp) {
  double sr = 0.0, sc = 0.0;
  for (auto c : comp) { sr += c.first; sc += c.second; }
  double cr = sr / std::max(1.0, (double)comp.size());
  double cc = sc / std::max(1.0, (double)comp.size());
  return {(int)std::lround(cr), (int)std::lround(cc)};
}

static Coord choose_goal_near_centroid(const std::vector<std::vector<int>>& grid,
                                      const std::vector<std::vector<double>>& dist,
                                      const std::vector<Coord>& comp,
                                      int max_radius = 6) {
  const int H = (int)grid.size();
  const int W = (int)grid[0].size();
  const double INF = std::numeric_limits<double>::infinity();

  Coord c0 = rounded_centroid(comp);
  Coord best = {-1, -1};
  double best_d = INF;

  for (int rad = 0; rad <= max_radius; ++rad) {
    for (int r = c0.first - rad; r <= c0.first + rad; ++r) {
      for (int c = c0.second - rad; c <= c0.second + rad; ++c) {
        if (!in_bounds(H, W, r, c)) continue;
        if (!is_free(grid[r][c])) continue;
        double d = dist[r][c];
        if (!std::isfinite(d)) continue;
        if (d < best_d) { best_d = d; best = {r,c}; }
      }
    }
    if (best.first != -1) return best;
  }
  return best;
}

static double score_cluster(double distance, int size, double alpha = 1.0, double beta = 2.5) {
  return alpha * distance - beta * (double)size;
}

int main() {
  const int H = 30, W = 40;
  std::vector<std::vector<int>> grid(H, std::vector<int>(W, -1));

  for (int r = 5; r < 15; ++r) for (int c = 5; c < 18; ++c) grid[r][c] = 0;
  for (int r = 5; r < 15; ++r) grid[r][12] = 100;
  for (int c = 12; c < 30; ++c) grid[10][c] = 100;
  for (int r = 18; r < 25; ++r) for (int c = 25; c < 35; ++c) grid[r][c] = 0;

  Coord robot = {8, 8};

  auto frontiers = detect_frontiers(grid);
  auto clusters  = cluster_frontiers(grid, frontiers);
  auto dist = bfs_dist_free(grid, robot);

  Coord best_goal = {-1,-1};
  double best_cost = std::numeric_limits<double>::infinity();

  for (auto& comp : clusters) {
    if ((int)comp.size() < 6) continue;
    Coord goal = choose_goal_near_centroid(grid, dist, comp, 6);
    if (goal.first == -1) continue;
    double d = dist[goal.first][goal.second];
    double J = score_cluster(d, (int)comp.size(), 1.0, 2.5);
    if (J < best_cost) { best_cost = J; best_goal = goal; }
  }

  std::cout << "robot=(" << robot.first << "," << robot.second << ")\n";
  std::cout << "frontier_clusters=" << clusters.size() << "\n";
  std::cout << "best_goal=(" << best_goal.first << "," << best_goal.second << "), cost=" << best_cost << "\n";
  return 0;
}
      

6.3) Java (self-contained)

File: Chapter17_Lesson1.java

/*
Autonomous Mobile Robots — Chapter 17, Lesson 1: Frontier-Based Exploration
File: Chapter17_Lesson1.java
*/

import java.util.*;

public class Chapter17_Lesson1 {

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

    static boolean inBounds(int H, int W, int r, int c) {
        return 0 <= r && r < H && 0 <= c && c < W;
    }
    static boolean isFree(int v) { return v == 0; }
    static boolean isUnknown(int v) { return v == -1; }

    static List<Coord> neighbors4(int H, int W, Coord u) {
        int[] dr = {-1, 1, 0, 0};
        int[] dc = {0, 0, -1, 1};
        List<Coord> out = new ArrayList<>();
        for (int k = 0; k < 4; k++) {
            int rr = u.r + dr[k], cc = u.c + dc[k];
            if (inBounds(H, W, rr, cc)) out.add(new Coord(rr, cc));
        }
        return out;
    }

    static List<Coord> neighbors8(int H, int W, Coord u) {
        List<Coord> out = new ArrayList<>();
        for (int dr = -1; dr <= 1; dr++) {
            for (int dc = -1; dc <= 1; dc++) {
                if (dr == 0 && dc == 0) continue;
                int rr = u.r + dr, cc = u.c + dc;
                if (inBounds(H, W, rr, cc)) out.add(new Coord(rr, cc));
            }
        }
        return out;
    }

    static List<Coord> detectFrontiers(int[][] grid) {
        int H = grid.length, W = grid[0].length;
        List<Coord> frontiers = new ArrayList<>();
        for (int r = 0; r < H; r++) {
            for (int c = 0; c < W; c++) {
                if (!isFree(grid[r][c])) continue;
                for (Coord v : neighbors4(H, W, new Coord(r, c))) {
                    if (isUnknown(grid[v.r][v.c])) { frontiers.add(new Coord(r, c)); break; }
                }
            }
        }
        return frontiers;
    }

    static List<List<Coord>> clusterFrontiers(int[][] grid, List<Coord> frontierCells) {
        int H = grid.length, W = grid[0].length;
        HashSet<Coord> frontierSet = new HashSet<>(frontierCells);
        HashSet<Coord> visited = new HashSet<>();
        List<List<Coord>> clusters = new ArrayList<>();

        for (Coord cell : frontierCells) {
            if (visited.contains(cell)) continue;
            if (!frontierSet.contains(cell)) continue;

            ArrayDeque<Coord> q = new ArrayDeque<>();
            q.add(cell);
            visited.add(cell);

            List<Coord> comp = new ArrayList<>();
            while (!q.isEmpty()) {
                Coord u = q.removeFirst();
                comp.add(u);
                for (Coord v : neighbors8(H, W, u)) {
                    if (frontierSet.contains(v) && !visited.contains(v)) {
                        visited.add(v);
                        q.addLast(v);
                    }
                }
            }
            clusters.add(comp);
        }

        clusters.sort((a, b) -> Integer.compare(b.size(), a.size()));
        return clusters;
    }

    static double[][] bfsDistFree(int[][] grid, Coord start) {
        int H = grid.length, W = grid[0].length;
        double INF = Double.POSITIVE_INFINITY;
        double[][] dist = new double[H][W];
        for (int r = 0; r < H; r++) Arrays.fill(dist[r], INF);
        if (!isFree(grid[start.r][start.c])) return dist;

        ArrayDeque<Coord> q = new ArrayDeque<>();
        q.add(start);
        dist[start.r][start.c] = 0.0;

        while (!q.isEmpty()) {
            Coord u = q.removeFirst();
            double du = dist[u.r][u.c];
            for (Coord v : neighbors4(H, W, u)) {
                if (!isFree(grid[v.r][v.c])) continue;
                if (Double.isInfinite(dist[v.r][v.c])) {
                    dist[v.r][v.c] = du + 1.0;
                    q.addLast(v);
                }
            }
        }
        return dist;
    }

    static Coord roundedCentroid(List<Coord> comp) {
        double sr = 0.0, sc = 0.0;
        for (Coord c : comp) { sr += c.r; sc += c.c; }
        double cr = sr / Math.max(1.0, (double)comp.size());
        double cc = sc / Math.max(1.0, (double)comp.size());
        return new Coord((int)Math.round(cr), (int)Math.round(cc));
    }

    static Coord chooseGoalNearCentroid(int[][] grid, double[][] dist, List<Coord> comp, int maxRadius) {
        int H = grid.length, W = grid[0].length;
        Coord c0 = roundedCentroid(comp);
        Coord best = null;
        double bestD = Double.POSITIVE_INFINITY;

        for (int rad = 0; rad <= maxRadius; rad++) {
            for (int r = c0.r - rad; r <= c0.r + rad; r++) {
                for (int c = c0.c - rad; c <= c0.c + rad; c++) {
                    if (!inBounds(H, W, r, c)) continue;
                    if (!isFree(grid[r][c])) continue;
                    double d = dist[r][c];
                    if (Double.isInfinite(d)) continue;
                    if (d < bestD) { bestD = d; best = new Coord(r, c); }
                }
            }
            if (best != null) return best;
        }
        return best;
    }

    static double scoreCluster(double distance, int size, double alpha, double beta) {
        return alpha * distance - beta * (double)size;
    }

    public static void main(String[] args) {
        int H = 30, W = 40;
        int[][] grid = new int[H][W];
        for (int r = 0; r < H; r++) Arrays.fill(grid[r], -1);

        for (int r = 5; r < 15; r++) for (int c = 5; c < 18; c++) grid[r][c] = 0;
        for (int r = 5; r < 15; r++) grid[r][12] = 100;
        for (int c = 12; c < 30; c++) grid[10][c] = 100;
        for (int r = 18; r < 25; r++) for (int c = 25; c < 35; c++) grid[r][c] = 0;

        Coord robot = new Coord(8, 8);

        List<Coord> frontiers = detectFrontiers(grid);
        List<List<Coord>> clusters = clusterFrontiers(grid, frontiers);
        double[][] dist = bfsDistFree(grid, robot);

        Coord bestGoal = null;
        double bestCost = Double.POSITIVE_INFINITY;

        for (List<Coord> comp : clusters) {
            if (comp.size() < 6) continue;
            Coord goal = chooseGoalNearCentroid(grid, dist, comp, 6);
            if (goal == null) continue;
            double d = dist[goal.r][goal.c];
            double J = scoreCluster(d, comp.size(), 1.0, 2.5);
            if (J < bestCost) { bestCost = J; bestGoal = goal; }
        }

        System.out.println("robot=(" + robot.r + "," + robot.c + ")");
        System.out.println("frontier_clusters=" + clusters.size());
        if (bestGoal != null) {
            System.out.println("best_goal=(" + bestGoal.r + "," + bestGoal.c + "), cost=" + bestCost);
        } else {
            System.out.println("No reachable frontier goal found.");
        }
    }
}
      

6.4) MATLAB / Simulink (matrix-based reference)

File: Chapter17_Lesson1.m

% Autonomous Mobile Robots — Chapter 17, Lesson 1: Frontier-Based Exploration
% File: Chapter17_Lesson1.m

function Chapter17_Lesson1()
    H = 30; W = 40;
    grid = -1 * ones(H, W);

    grid(6:15, 6:18) = 0;
    grid(6:15, 13) = 100;
    grid(11, 13:30) = 100;
    grid(19:25, 26:35) = 0;

    robot = [9, 9];

    frontierCells = detect_frontiers(grid);
    clusters = cluster_frontiers(grid, frontierCells);
    dist = bfs_dist_free(grid, robot);

    minClusterSize = 6;
    bestGoal = [];
    bestCost = inf;

    for k = 1:numel(clusters)
        comp = clusters{k};
        if size(comp,1) < minClusterSize
            continue;
        end
        goal = choose_goal_near_centroid(grid, dist, comp, 6);
        if isempty(goal)
            continue;
        end
        d = dist(goal(1), goal(2));
        J = score_cluster(d, size(comp,1), 1.0, 2.5);
        if J < bestCost
            bestCost = J;
            bestGoal = goal;
        end
    end

    disp(['robot=(' num2str(robot(1)) ',' num2str(robot(2)) ')']);
    disp(['frontier_clusters=' num2str(numel(clusters))]);
    if ~isempty(bestGoal)
        disp(['best_goal=(' num2str(bestGoal(1)) ',' num2str(bestGoal(2)) '), cost=' num2str(bestCost)]);
    else
        disp('No reachable frontier goal found.');
    end
end

function frontiers = detect_frontiers(grid)
    [H, W] = size(grid);
    frontiers = zeros(0,2);
    for r = 1:H
        for c = 1:W
            if grid(r,c) ~= 0
                continue;
            end
            nbrs = neighbors4(H, W, [r,c]);
            for i = 1:size(nbrs,1)
                rr = nbrs(i,1); cc = nbrs(i,2);
                if grid(rr,cc) == -1
                    frontiers(end+1,:) = [r,c]; %#ok<AGROW>
                    break;
                end
            end
        end
    end
end

function clusters = cluster_frontiers(~, frontierCells)
    [H, W] = deal(0,0); %#ok<ASGLU>
    if isempty(frontierCells)
        clusters = {};
        return;
    end

    frontierSet = containers.Map();
    for i = 1:size(frontierCells,1)
        key = sprintf('%d,%d', frontierCells(i,1), frontierCells(i,2));
        frontierSet(key) = true;
    end

    visited = containers.Map();
    clusters = {};

    for i = 1:size(frontierCells,1)
        cell = frontierCells(i,:);
        key0 = sprintf('%d,%d', cell(1), cell(2));
        if isKey(visited, key0); continue; end
        if ~isKey(frontierSet, key0); continue; end

        q = cell;
        visited(key0) = true;
        comp = zeros(0,2);

        while ~isempty(q)
            u = q(1,:);
            q(1,:) = [];
            comp(end+1,:) = u; %#ok<AGROW>
            nbrs = neighbors8(size(grid,1), size(grid,2), u); %#ok<UNRCH>
            for j = 1:size(nbrs,1)
                v = nbrs(j,:);
                key = sprintf('%d,%d', v(1), v(2));
                if isKey(frontierSet, key) && ~isKey(visited, key)
                    visited(key) = true;
                    q(end+1,:) = v; %#ok<AGROW>
                end
            end
        end

        clusters{end+1} = comp; %#ok<AGROW>
    end

    sizes = cellfun(@(x) size(x,1), clusters);
    [~, idx] = sort(sizes, 'descend');
    clusters = clusters(idx);
end

function dist = bfs_dist_free(grid, start)
    [H, W] = size(grid);
    dist = inf(H, W);
    if grid(start(1), start(2)) ~= 0
        return;
    end

    q = start;
    dist(start(1), start(2)) = 0;

    while ~isempty(q)
        u = q(1,:);
        q(1,:) = [];
        du = dist(u(1), u(2));
        nbrs = neighbors4(H, W, u);
        for i = 1:size(nbrs,1)
            v = nbrs(i,:);
            if grid(v(1), v(2)) ~= 0
                continue;
            end
            if isinf(dist(v(1), v(2)))
                dist(v(1), v(2)) = du + 1;
                q(end+1,:) = v; %#ok<AGROW>
            end
        end
    end
end

function goal = choose_goal_near_centroid(grid, dist, comp, maxRad)
    [H, W] = size(grid);
    cr = mean(comp(:,1));
    cc = mean(comp(:,2));
    r0 = round(cr); c0 = round(cc);

    goal = [];
    bestD = inf;

    for rad = 0:maxRad
        for r = (r0-rad):(r0+rad)
            for c = (c0-rad):(c0+rad)
                if r < 1 || r > H || c < 1 || c > W
                    continue;
                end
                if grid(r,c) ~= 0
                    continue;
                end
                d = dist(r,c);
                if ~isfinite(d)
                    continue;
                end
                if d < bestD
                    bestD = d;
                    goal = [r,c];
                end
            end
        end
        if ~isempty(goal)
            return;
        end
    end
end

function J = score_cluster(distance, sz, alpha, beta)
    J = alpha * distance - beta * sz;
end

function nbrs = neighbors4(H, W, u)
    r = u(1); c = u(2);
    cand = [r-1 c; r+1 c; r c-1; r c+1];
    mask = cand(:,1) >= 1 & cand(:,1) <= H & cand(:,2) >= 1 & cand(:,2) <= W;
    nbrs = cand(mask,:);
end

function nbrs = neighbors8(H, W, u)
    r = u(1); c = u(2);
    nbrs = zeros(0,2);
    for dr = -1:1
        for dc = -1:1
            if dr == 0 && dc == 0; continue; end
            rr = r + dr; cc = c + dc;
            if rr >= 1 && rr <= H && cc >= 1 && cc <= W
                nbrs(end+1,:) = [rr cc]; %#ok<AGROW>
            end
        end
    end
end
      

Simulink tip: Put the frontier selection logic (detect + cluster + score) inside a MATLAB Function block. Provide the occupancy grid as a 2D matrix input, and output a goal cell index or goal pose. The navigation/control part can reuse your existing path tracking + local planner setup from Chapters 14–16.

6.5) Wolfram Mathematica (Wolfram Language)

File: Chapter17_Lesson1.nb

(* Autonomous Mobile Robots — Chapter 17, Lesson 1: Frontier-Based Exploration *)
(* File: Chapter17_Lesson1.nb *)

ClearAll["Global`*"];

H = 30; W = 40;
grid = ConstantArray[-1, {H, W}];

Do[grid[[r, c]] = 0, {r, 6, 15}, {c, 6, 18}];
Do[grid[[r, 13]] = 100, {r, 6, 15}];
Do[grid[[11, c]] = 100, {c, 13, 30}];
Do[grid[[r, c]] = 0, {r, 19, 25}, {c, 26, 35}];

robot = {9, 9};

inBounds[{r_, c_}] := 1 <= r <= H && 1 <= c <= W;
neighbors4[{r_, c_}] := Select[{ {r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1} }, inBounds];
neighbors8[{r_, c_}] := Select[
  DeleteCases[Flatten[Table[{r + dr, c + dc}, {dr, -1, 1}, {dc, -1, 1}], 1], {r, c}],
  inBounds
];

isFree[v_] := v === 0;
isUnknown[v_] := v === -1;

detectFrontiers[g_] := Module[{front = {} },
  Do[
    If[isFree[g[[r, c]]] && AnyTrue[neighbors4[{r, c}], isUnknown[g[[#[[1]], #[[2]]]]] &],
      AppendTo[front, {r, c}]
    ],
    {r, 1, H}, {c, 1, W}
  ];
  front
];

clusterFrontiers[g_, frontierCells_] := Module[{frontSet, visited = <||>, clusters = {}, q, comp, u},
  frontSet = AssociationThread[frontierCells -> ConstantArray[True, Length[frontierCells]]];
  Do[
    If[KeyExistsQ[visited, cell] || !KeyExistsQ[frontSet, cell], Continue[]];
    q = {cell};
    visited[cell] = True;
    comp = {};
    While[Length[q] > 0,
      u = First[q]; q = Rest[q];
      AppendTo[comp, u];
      Do[
        If[KeyExistsQ[frontSet, v] && !KeyExistsQ[visited, v],
          visited[v] = True;
          AppendTo[q, v];
        ],
        {v, neighbors8[u]}
      ];
    ];
    AppendTo[clusters, comp];
    ,
    {cell, frontierCells}
  ];
  Reverse@SortBy[clusters, Length]
];

bfsDistFree[g_, start_] := Module[{dist, q, u, du, v},
  dist = ConstantArray[Infinity, {H, W}];
  If[!isFree[g[[start[[1]], start[[2]]]]], Return[dist]];
  q = {start};
  dist[[start[[1]], start[[2]]]] = 0;
  While[Length[q] > 0,
    u = First[q]; q = Rest[q];
    du = dist[[u[[1]], u[[2]]]];
    Do[
      If[isFree[g[[v[[1]], v[[2]]]]] && dist[[v[[1]], v[[2]]]] === Infinity,
        dist[[v[[1]], v[[2]]]] = du + 1;
        q = Append[q, v];
      ],
      {v, neighbors4[u]}
    ];
  ];
  dist
];

centroid[comp_] := Mean[comp];
chooseGoal[g_, dist_, comp_, maxRad_: 6] := Module[{c0, best = {}, bestD = Infinity, r0, c1, rad, r, c, d},
  c0 = Round[centroid[comp]];
  r0 = c0[[1]]; c1 = c0[[2]];
  For[rad = 0, rad <= maxRad, rad++,
    For[r = r0 - rad, r <= r0 + rad, r++,
      For[c = c1 - rad, c <= c1 + rad, c++,
        If[!inBounds[{r, c}], Continue[]];
        If[!isFree[g[[r, c]]], Continue[]];
        d = dist[[r, c]];
        If[NumberQ[d] && d < bestD, bestD = d; best = {r, c};];
      ];
    ];
    If[best =!= {}, Return[best]];
  ];
  best
];

score[distance_, size_, alpha_: 1.0, beta_: 2.5] := alpha*distance - beta*size;

frontiers = detectFrontiers[grid];
clusters = clusterFrontiers[grid, frontiers];
dist = bfsDistFree[grid, robot];

bestGoal = {};
bestCost = Infinity;

Do[
  If[Length[comp] < 6, Continue[]];
  goal = chooseGoal[grid, dist, comp, 6];
  If[goal === {}, Continue[]];
  d = dist[[goal[[1]], goal[[2]]]];
  J = score[d, Length[comp], 1.0, 2.5];
  If[J < bestCost, bestCost = J; bestGoal = goal;];
  ,
  {comp, clusters}
];

Print["robot=", robot];
Print["frontier_clusters=", Length[clusters]];
Print["best_goal=", bestGoal, ", cost=", bestCost];
      

Problem 1 (Frontier characterization): Let \( \mathcal{F}_t \) be the set of frontier cells defined with \( \mathcal{N}_4 \). Show that \( \mathcal{F}_t=\emptyset \) implies that every unknown cell is at graph distance at least 2 (in 4-neighborhood) from any known-free cell.

Solution: If \( \mathcal{F}_t=\emptyset \), then no known-free cell has an unknown neighbor in \( \mathcal{N}_4 \). Therefore, for any known-free cell \( c \), all neighbors in \( \mathcal{N}_4(c) \) are not unknown. Hence any unknown cell cannot be at distance 1 from any known-free cell. The minimum 4-neighborhood distance must be at least 2.

Problem 2 (Complexity): Consider an \( H\times W \) grid. Give the time complexity (big-O) of (i) frontier detection by scanning all cells and checking 4 neighbors, and (ii) clustering with BFS over the frontier set using 8-neighborhood adjacency.

Solution: (i) Scanning all cells is \( O(HW) \), and checking a constant number (4) neighbors keeps it \( O(HW) \). (ii) BFS visits each frontier cell once and checks up to 8 neighbors, so clustering is \( O(|\mathcal{F}_t|) \), which is at most \( O(HW) \).

Problem 3 (Termination condition): Assume a finite grid and that whenever the robot reaches a selected frontier goal, at least one unknown cell becomes known. Prove that the exploration loop cannot execute an infinite number of successful goals.

Solution: Let \( |\mathcal{K}_t| \) be the number of known cells. By assumption, each successful goal increases \( |\mathcal{K}_t| \) by at least 1. Since \( |\mathcal{K}_t| \le |\mathcal{C}| \) and \( |\mathcal{C}| \) is finite, only finitely many increments are possible. Therefore, the number of successful goals is finite.

Problem 4 (Utility scaling): In \( J_t(k)=\alpha L_t(\mathbf{x}_r,\mathbf{g}_k)-\beta|\mathcal{F}_t^{(k)}| \), suppose distances are measured in meters and \( |\mathcal{F}_t^{(k)}| \) is a count of cells. Provide a principled way to choose units/scales so that the two terms are comparable.

Solution: Let grid resolution be \( \rho \) meters per cell. A frontier of size \( n \) cells roughly corresponds to boundary length \( \approx n\rho \) meters, and a crude expected explored area can be related to sensor range (next lesson). To make terms comparable, choose \( \beta \) in “meters per cell” units, e.g., set \( \beta = \lambda \rho \) where \( \lambda \) is dimensionless. Then both terms scale in meters: \( \alpha L \) (meters) and \( \beta n \) (meters).

Problem 5 (Reachability filter): Let \( \mathcal{R}_t \) be the set of free cells reachable from the robot via 4-neighborhood connectivity on free cells. Show that if a frontier cluster has no cell in \( \mathcal{R}_t \), then no safe goal adjacent to that cluster can be reached without first changing the map.

Solution: If the cluster has no cell in \( \mathcal{R}_t \), then every frontier cell is disconnected (in free space) from the robot in the current map. Any goal selected as a free cell “near” the cluster that is connected to the cluster through free space must lie in the same disconnected component, hence also outside \( \mathcal{R}_t \). Therefore, it is unreachable unless the map changes (e.g., an unknown corridor becomes known free and connects components).

We defined frontier cells rigorously in occupancy grids, clustered them to form stable exploration targets, and derived a practical selection rule balancing shortest-path travel cost and a proxy for map expansion. Under finite-map and monotone discovery assumptions, we proved that the loop terminates after finitely many successful goals. In the next lesson, we replace the frontier-size proxy with explicit information-theoretic measures (entropy reduction).