2.1.1 Coverage Planners

This section presents the algorithms employed for the discovery of portable objects. While a comprehensive technical analysis of these algorithms is beyond the scope of this work, they are described with sufficient detail to enable understanding of their application within our framework. For in-depth technical details and formal derivations, the reader is referred to the original publications cited herein. To demonstrate the different navigators, a costmap of the Pulse Breakout room is used. This map was obtained during testing of the coverage methods. Furthermore Nav2 Macenski et al., 2020 is used for low level motion planning and execution as well as real-time control and costmap generation.

Preprocessing¶

Before the map can be used to plan a path, a common preprocessing algorithm can be abstracted. Coverage path planners generally use either a binary map of the free space, (free=1, occupied=0) or the polygons constructed from the boundaries of the free space.

The input to the algorithm can be any 2D image displaying the free and occupied pixels. In this case the input will be the costmap directly. First the costmap is thresholded to create a binary costmap, which is the first output.

Then using this binary costmap, the contours Figure 3 are extracted.

Morphology-Based Skeleton Path Planner¶

The first implemented method, based on the work of Becoy et al., 2025, Niu et al., 2025, derives a coverage path from the morphological skeleton of the free space. See the algorithm below.

Input: Occupancy grid $\mathcal{M}$, robot start position $\mathbf{p}_0 \in \mathbb{R}^2$
Output: Ordered waypoint segments $\mathcal{P} = [P_1, P_2, \ldots, P_k]$

1. $B \leftarrow$ threshold($\mathcal{M}$, lethal_threshold)

2. Extract contours from $B$, group by containment into polygon groups $G_{poly}$

3. For each polygon group $g \in G_{poly}$:

   1. $S \leftarrow$ medialAxis($B$)     (skimage skeletonize)

   2. $W \leftarrow$ skeletonToWorldCoords($S$)

   3. $G_{graph} \leftarrow$ buildKDTreeGraph($W$, radius $= r_{path}$)

   4. bridgeDisconnectedComponents($G_{graph}$)

   5. $L \leftarrow$ findLeafNodes($G_{graph}$)     (nodes with degree 1)

   6. $v \leftarrow$ nearestLeaf($\mathbf{p}_0$, $L$)

   7. $\mathcal{Q} \leftarrow [v]$, $\quad V_{visited} \leftarrow \emptyset$

   8. While $V_{visited} \neq L$:

      1. $V_{visited} \leftarrow V_{visited} \cup \{v\}$

      2. $r \leftarrow$ nearestLeafByGraphDistance($v$, $L \setminus V_{visited}$, $G_{graph}$)

      3. $\pi \leftarrow$ shortestPath($v$, $r$, $G_{graph}$)

      4. append $\pi$ to $\mathcal{Q}$

      5. $v \leftarrow r$

   9. $P_i \leftarrow [W[u] \;\forall\, u \in \mathcal{Q}]$

   10. append $P_i$ to $\mathcal{P}$, set $\mathbf{p}_0 \leftarrow P_i[\text{last}]$

4. sanitiseWaypoints($\mathcal{P}$, $\mathcal{M}$)

5. Return $\mathcal{P}$

The free space is extracted using the methods discussed in the previous section. The polygons are used and converted to a binary map as to not produce too many branches for the robot to navigate through. This map is then skeletonized to extract waypoints, see Figure 4.

Once the waypoints are obtained, a graph is constructed and leaf nodes are extracted. In the graph leaf nodes are defined by their single connection to the rest of the skeleton, these are seen as endpoints of the path.

Finally, starting from the closest leaf node to the robot, the full path is planned by recursively tracing the graph from the current leaf node to the nearest unvisited leaf node. Use the slider below to visualize the full path.

Grid Based Spanning Tree Planner¶

Secondly, a grid based spanning tree algorithm is used to plan a full coverage path over the map. This planner is based on the work of Gabriely & Rimon, 2001.

Input: Occupancy grid $\mathcal{M}$, robot start position $\mathbf{p}_0 \in \mathbb{R}^2$, scale $s = 0.06$
Output: Ordered waypoint segments $\mathcal{P} = [P_1, P_2, \ldots, P_k]$

1. $B \leftarrow$ threshold($\mathcal{M}$, lethal_threshold)

2. $B_{down} \leftarrow$ resize($B$, scale $= s$, interpolation $=$ NEAREST)

3. $G \leftarrow$ gridGraph($B_{down}$)     (4-connected)

4. Remove all edges $(u, v)$ where $B_{down}[u] = 0$ or $B_{down}[v] = 0$

5. $T \leftarrow \emptyset$

6. For each connected component $\mathcal{C} \in G$:

   1. $T \leftarrow T \cup$ DFS_tree($\mathcal{C}$)

7. For each edge $(u, v) \in T$:     (edge subdivision)

   1. $m \leftarrow \frac{u + v}{2}$

   2. replace $(u, v)$ with edges $(u, m)$ and $(m, v)$

8. $P_{perim} \leftarrow \mathbf{0}^{H \times W}$, $\quad r \leftarrow \lfloor \frac{1}{4s} \rfloor$

9. For each node $p \in T$:

   1. $p_{img} \leftarrow \frac{p + 0.5}{s}$

   2. drawFilledRect($P_{perim}$, centre $= p_{img}$, halfsize $= r$)

10. $\mathcal{C} \leftarrow$ findContours($P_{perim}$, RETR_EXTERNAL)

11. For each contour $c \in \mathcal{C}$:

    1. $W \leftarrow$ pixelToWorld($c$)

    2. If $|W| < 2$: skip

    3. $idx \leftarrow \arg\min_i \|W[i] - \mathbf{p}_0\|_2$

    4. $P_i \leftarrow [W[idx], \ldots, W[-1], W[0], \ldots, W[idx-1]]$

    5. append $P_i$ to $\mathcal{P}$, set $\mathbf{p}_0 \leftarrow P_i[\text{last}]$

12. sanitiseWaypoints($\mathcal{P}$, $\mathcal{M}$)

13. Return $\mathcal{P}$

As opposed to the Skeleton planner, this planner makes use of the binary costmap directly. Using this binary costmap, it is further discretized to a grid which is the fraction of the resolution of the original costmap. Figure 7.

A degree first search (DFS) is conducted over the free cells, from which sparse waypoints are created.

A black image, of the same resolution as the original costmap is then created and white squares, with half the cell width, are placed in the center of each cell and in between each cell. Waypoints are created from the contours of this tree. These waypoints represent a circumnavigation around the DFS tree.

A segment from this circumnavigation is then removed from the path

A lab cleanup robot must not blindly follow waypoints. If it does it might collide with obstacles near the map boundary or collide with moving objects.
Nav2 implements several low level real-time trajectory planners that plan inside the live updating costmap Macenski et al., 2023. They handle how the waypoints are followed and how much influence the map ought to have on the generated trajectory. The system described in this section implements an MPPI controller Williams et al., 2017 due to its superior performance in highly dynamic environments, like labs where students or people frequently walk Elbouhy et al., 2024. Other planners that were considered include DWB (Dynamic Window Approach Behavior-based) Thrun et al., 1997 and RPP (Regulated Pure Pursuit) Macenski et al., 2023.