Optimal Structure Synthesis for Environment Augmenting Robots

TL;DR

This paper introduces an optimal planning algorithm for environment-augmenting robots to automatically build structures that enable traversal of complex environments, balancing optimality and practical runtime.

Contribution

It presents a complete, optimal branch-and-bound algorithm for structure synthesis in environment augmentation, addressing an NP-Hard problem with real-world applicability.

Findings

Algorithm solves 3D indoor maps in about one minute

Structures enable robots to traverse entire environments

Proves the problem is NP-Hard

Abstract

Building structures can allow a robot to surmount large obstacles, expanding the set of areas it can reach. This paper presents a planning algorithm to automatically determine what structures a construction-capable robot must build in order to traverse its entire environment. Given an environment, a set of building blocks, and a robot capable of building structures, we seek a optimal set of structures (using a minimum number of building blocks) that could be built to make the entire environment traversable with respect to the robot's movement capabilities. We show that this problem is NP-Hard, and present a complete, optimal algorithm that solves it using a branch-and-bound strategy. The algorithm runs in exponential time in the worst case, but solves typical problems with practical speed. In hardware experiments, we show that the algorithm solves 3D maps of real indoor environments in about one minute, and that the structures selected by the algorithm allow a robot to traverse the entire environment. An accompanying video is available online at https://youtu.be/B9WM557NP44.

Tables

Table 1. TABLE I: Runtime Performance

Environment	(A)	(B)	(C)	(D)	(E)
Size (square, px)	151	151	72	240	115
Structures	634	148	474	3302	1275
Regions	4	4	9	5	6
Branches Explored	33	1	13	1	1
Potential Conflicts	16196	476	12974	33908	18206
CPF	14.0%	16.3%	7.6%	3.3%	4.7%
Structure Time (s)	9.834	2.25	3.188	61.824	9.507
BB-MST Time (s)	6.642	0.024	9.588	0.552	0.14
Total Time (s)	20.604	6.329	13.828	86.848	15.708
Blocks used	17	46	40	13	15

Equations

$$\text{cliffs}(\mathcal{E}(v))=$$

$$\text{steep}(\mathcal{E}(v))=$$

$$Z_i=$$

$$Z_{i+1}=$$

$$|Z_{i+1}-Z_i|<$$

$$h_{max}(i{)}_j=$$

$$h_{min}(i{)}_j=$$

Full text

I Introduction

Augmenting the environment to make a task easier is a familiar human experience: to reach objects on a shelf, we use a stepstool, and at a larger scale, we construct bridges to cross wide rivers. In contrast most robotic planning strategies search for behaviors the robot can execute within the constraints of its environment, without considering how those constraints could be changed. Well-established work in construction robotics provides evidence that robots have the ability to build large structures [1, 2, 3], indicating that they should, similarly to humans, be able to augment their environment for simpler task execution.

Consider a scenario where a small robot must move through an environment filled with objects larger than itself. The robot is unable to cross over large objects or gaps. As a result, entire portions of the environment may be inaccessible. The planning problem can be posed: Given an environment, a robot, and a supply of building blocks, can we find a set of structures that could be added to the environment to make it fully accessible to the robot (i.e. there exists a navigable path between any pair of points)? Furthermore, since structure-building is a time-consuming process, can we find such a set of structures which uses a minimum number of building blocks? We refer to this as the optimal structure synthesis problem.

This paper presents a mathematical formalism for optimal structure synthesis, shows that the problem is NP-Hard, and presents a complete, optimal algorithm that will find a minimum-cost solution to any instance of the problem, if a solution exists. The algorithm solves practical problems efficiently using a branch-and-bound strategy, typically exploring a tiny fraction of the exponentially-large solution space before finding the optimal solution. In our experiments, we show that the algorithm finds optimal solutions in about one minute for 3D maps of real indoor environments, and demonstrate that the structures selected by the algorithm do indeed allow the robot to access every region of the environments.

II Related Work

The fields of collective construction robotics and modular robotics offer examples of systems that build and traverse structures. Petersen et al. present Termes [1], a collective construction robot system that creates structures using blocks co-designed with the robot. Werfel et al. present algorithms for environmentally-adaptive construction: a team of robots senses obstacles and builds around them, modifying the goal structure if needed to leave room for immovable obstacles [2]. Other work allows robots to adaptively reinforce the structure they are building to bear the load imposed on it [4]. Terada and Murata [5] present a lattice-style modular system with two parts, structure modules and an assembler robot. Like many lattice-style modular systems, the assembler robot can only move on the structure modules, and not in an unstructured environment. M-blocks [6] form structures out of robot cubes which rotate over the structure, and can reconfigure between arbitrary 3D shapes, except those containing certain inadmissible sub-configurations [7]. Other related work in manipulation planning allows robots to carry out multi-step procedures to assemble furniture [8] or rearrange clutter surrounding a primary manipulation task [9].

There is also some work showing that robots can deploy structures to enhance their ability to move through an environment. Napp et al. present a distributed algorithm for adaptive ramp building with amorphous materials [10]. Using local information, the algorithm controls one or more robots to deposit amorphous material (like foam or sandbags) on their environment to make a goal point accessible [11]. Our work addresses a similar, complementary problem: assuming a map of the entire environment, we generate a globally optimal set of structures making every point accessible.

We build on prior work with the SMORES-EP robot (Figure 1), in which the robot autonomously built and deployed structures to overcome obstacles while completing tasks [12]. This prior system relied on a human-made library of structures: for example, the library entry “height-2 ramp” would have an associated classifier to identify environments of type “height-2 ledge,” indicating that such a ledge can be made traversable by adding the ramp. In contrast, our current paper provides a general formalism to decide what structure the robot should build, and where it should build it.

III Problem Formulation

III-A Problem Summary

Given an environment, a robot, and a set of building blocks, we seek a set of structures, made of building blocks, that could be added to the environment to make it fully accessible by the robot. We present an algorithm that is guaranteed to find a globally optimal solution to this discrete, combinatorial optimization problem. The solution is optimal in the sense that it uses a minimum number of building blocks.

The environment is a discretized height map - a 2d x-y grid with a single height Z at each cell (Fig. 2). The robot moves over the surface of the environment. We abstract the motion of the robot as discrete transitions along the edges of the graph representing the environment. The movement capabilities of the robot are modeled as the traversability criteria, which quantify the largest obstacles and steepest slopes the robot can move over, and consequently determine whether the robot can occupy a node or move along an edge in the environment graph. These criteria are detailed in Definitions 1 and 2. The framework is general to any robot whose motion can be modeled using these criteria.

Building blocks can be assembled by the robot to form structures. Our goal is to determine the shapes and locations of a set of structures that could be added to the environment to make it fully traversable. Section III-C provides a formal definition for structures made of building blocks, and describes the constraints that valid structures must obey.

Our algorithm proceeds in two stages. First, the Waterfall algorithm (Section IV) finds the set of min-cost structures connecting each pair of regions in the environment. Among these, the BB-MST algorithm (Section V) then finds a min-cost tree of structures that spans all regions, while obeying the constraints for structure validity. Together, Waterfall and BB-MST generate the minimum-cost set of structures that makes the environment fully traversable (Theorem 1).

III-B Preliminaries

Consider a ground robot traversing a discretized height-map environment $\mathcal{E}:\mathbb{Z}^{2}\to\mathbb{R}^{+}$. The environment can be represented as a grid graph $G(V,E)$, where $V$ is a set of nodes corresponding to each of the grid cells, and edges $E\subseteq V\times V$ connect neighboring cells. The robot’s movements are treated as discrete transitions along edges. The robot is subject to certain physical limitations, which determine its ability to traverse this graph, defined for both nodes and edges.

The first criterion is on edges, or transitions between nodes. A ground robot is typically not able to traverse a sharp rise or dropoff (or “cliff”). Edges across which the difference in height is above a threshold $\Delta Z_{cliff}$ is thus non-traversable.

Definition 1** (Non-Traversable Edge).**

An edge $e=(v_{i},v_{j})$ is non-traversable if $|\mathcal{E}(v_{i})-\mathcal{E}(v_{j})|>\Delta Z_{cliff}$. We refer to these edges as cliffs.

The second criterion is on the slope of the environment. We find slopes too “steep” by identifying nodes at which the average gradient (within a window of radius $d$ about the node) is higher than a threshold $K_{steep}$. To prevent cliffs from influencing the apparent steepness of nearby nodes (due to the windowed average), we remove the contribution of cliffs from the gradient before identifying steep nodes. Let:

[TABLE]

where $\underset{d}{ma}$ denotes a windowed-moving-average with window-size $d$. In our implementation, $d=8$ cm.

Definition 2** (Non-Traversable Node).**

A node $v\in V$ is non-traversable if $\texttt{steep}(\mathcal{E}(v))>K_{steep}$.

For the robot made of SMORES-EP modules used in our implementation, $\Delta Z_{cliff}=4$ cm and $K_{steep}=1$. These values were experimentally determined by testing the robot’s ability to drive over sloped surfaces and step obstacles.

We define the traversable environment to be $G_{T}$, the subgraph of $G$ containing only traversable nodes and edges. If $G_{T}$ is connected, then the robot is able to reach every traversable grid cell. However, it is possible that $G_{T}$ is not connected. In this case, the environment is split into regions, which are the connected components of $G_{T}$.

Definition 3** (Region).**

A region $r_{i}\in R$ is a connected component in $G_{T}(V,E)$. By definition, a path exists between any two nodes in a single region, and no path exists between any two nodes in different regions.

Fig. 2 shows an environment with a table and chair (left), and the corresponding height map $\mathcal{E}$ (center). Different colors on this map correspond to different regions.

III-C Structures

Structures are composed of building blocks, which the robot is able to carry, place, and drive over, and can be added to the environment can allow the robot to move between regions. Our goal is to select the shapes, positions, and orientations of structures that could be added to the environment to make it globally traversable. We do not attempt to determine what actions the robot could take to actually build the structure - to do so, we could use an existing algorithm for robotic assembly planning (See Section II).

A structure is a line of columns of stacked block and wedge building blocks, shown in Figure 1. Building blocks have a square footprint with side length $L_{B}$; for our building blocks, $L_{B}=8cm$. Importantly, we assume the robot can only move over the structure along a straight line - this is a physical constraint imposed by the building blocks, which have side walls. We identify building blocks with type labels $t\in\{block,wedge_{f},wedge_{b}\}$, with “f” and “b” denoting the two possible orientations (forward and backward) of wedges with respect to the structure. For each type, we define a surface function $s:[0,L_{B}]\to\mathbb{R}^{+}$ describing its shape: $block:~{}s(x)=L_{B}$, $wedge_{f}:~{}s(x)=x$, and $wedge_{b}:~{}s(x)=L_{B}-x$.

Viewed from above, the footprint of a structure in the $xy$ plane is a linear array of $n$ contiguous squares called structure cells (Fig. 3, left); sitting atop each structure cell is a column of stacked blocks which comprise the structure. Structures are not locked to the grid - rather, the build point $\mathbf{b}$ defines the position of the first structure cell in the plane, and the orientation vector $\mathbf{\hat{u}}$ defines the line along which all cells lie, and the direction along which the robot may move.

Definition 4** (Structure).**

A structure $T=\left\langle\mathbf{b},\mathbf{\hat{u}},C\right\rangle$ has build point $\mathbf{b}\in\mathbb{R}^{2}$, orientation vector $\mathbf{\hat{u}}\in\mathbb{R}^{2}:|\mathbf{\hat{u}}|=1$, and cells $C=\{c_{1}\ldots c_{n}\}$. Each cell $c_{i}=\left\langle\mathbf{\lambda}_{i},0pt_{i},t_{i}\right\rangle,~{}i\in\{1\ldots n\}$ has corners $\mathbf{\lambda}_{i}\in\mathbb{R}^{4\times 2}$, column height $h_{i}\in\mathbb{Z}^{+}$, and surface block type $t_{i}$.

The height and surface block type of a cell respectively specify the number of blocks stacked on that cell and the type of building block at the top of the stack. To fully define a structure, it is sufficient to provide $\mathbf{b}$, $\mathbf{\hat{u}}$, and lists of cell heights $\left\{{h}_{1},{h}_{2}\ldots,{h}_{n}\right\}$ and surface block types $\left\{{t}_{1},{t}_{2}\ldots,{t}_{n}\right\}$; assuming $L_{B}$ is known, cell corners $\mathbf{\lambda}$ can be computed from $\mathbf{b}$ and $\mathbf{\hat{u}}$. The cost of a structure is the number of blocks it contains, $\texttt{cost}(T)=\sum_{i=1\ldots n}0pt_{i}$.

For a structure to be considered valid, the environment surface between each of its cells must be suitable to support a column of blocks. For our structures, we define buildability in terms of the flatness of the underlying environment surface. Letting $\mathcal{E}(c_{i})=\texttt{median}(\mathcal{E}(v))~{}\forall~{}v\in c_{i}$, cell $c_{i}$ is buildable iff $\mathcal{E}(v)-\mathcal{E}(c_{i})<\alpha L_{B}~{}\forall~{}v\in c_{i}$. In our implementation, $\alpha=0.4$.

For a structure to be useful, it must be both buildable and traversable. Since each block is individually traversable, we determine traversability of a structure by evaluating the cliff condition between neighboring cells $c_{i}$ and $c_{i+1}$:

[TABLE]

The boundary is traversable if Equation 5 is satisfied.

In addition to moving between blocks on the structure, the robot must also be able to transition from the structure to the ground surface at both ends, so we require the first and last cell of traversable structures to have zero height and exempt them from the buildability condition. Additionally, when moving to the first or last structure cell involves crossing a region boundary (e.g. moving from the structure to the top of a cliff), the region boundary must be flat, and its surface normal must align with the structure orientation $\mathbf{\hat{u}},$ so that structure presses flat up against the cliff (Fig 3).

Definition 5** (Valid Structure).**

A structure $T$ is valid if: $~{}\forall~{}c_{i},c_{i+1}\in C$, $(c_{i},c_{i+1})$ is traversable, and $\forall c_{i}\in\{c_{2}\ldots c_{n-1}\}$, $c_{i}$ is buildable.

III-D Conflicts

Introducing a valid structure whose endpoint cells are in different regions creates a path between them. Our goal, then, is to synthesize a set of structures $\mathcal{T}=\left\{{T}_{1},{T}_{2}\ldots,{T}_{k}\right\}$ on a world graph with regions $R$ such that there is a path between every pair of regions. Because structures occupy physical space, certain combinations of them can result in two kinds of conflicts: (1) collision between two structures and (2) splitting regions with structures.

Definition 6** (Structure-Structure Conflict).**

A pair of structures conflict if any of their cells intersect.

Definition 7** (Region-Structure Conflict).**

*Consider region $r_{c}$ and set of structures $\mathcal{T}_{c}=\left\{{T}_{1},{T}_{2}\ldots,{T}_{k}\right\}$. Let $V_{r_{c}}$ be the set of nodes in region $r_{c}$. Let $V_{\mathcal{T}_{c}}$ be the set of nodes in the cells of $\mathcal{T}_{c}$. Let $G_{r_{c}}$ be the subgraph of $G(V,E)$ induced by $V_{r_{c}}\setminus V_{\mathcal{T}_{c}}$. There is a region-structure conflict between $r_{c}$ and $\mathcal{T}_{c}$ if $G_{r_{c}}$ is disconnected. *

Structure-structure conflicts create physically impossible conditions and are thus never allowed. Region-structure conflict occur when one or more structures split a region into multiple disconnected pieces, requiring each piece to be treated as a new region.

III-E Problem Statement

Consider an input environment $\mathcal{E}$, represented as grid-graph $G(V,E)$, with regions $R$. Our objective is to find a min-cost, conflict-free set of structures which make the entire environment traversable. Let $G_{R}(R,\mathcal{T})$ be a graph in which nodes represent regions and edges represent valid structures connecting regions. We seek a set of structures $\mathcal{T}^{*}=\left\{{T}_{1},{T}_{2}\ldots,{T}_{k}\right\}$ such that $G_{R}(R,\mathcal{T}^{*})$ is fully connected and conflict-free, and $\texttt{cost}(\mathcal{T}^{*})=\sum_{T_{i}\in\mathcal{T}^{*}}\texttt{cost}(T_{i})$ is minimized.

IV Waterfall Algorithm - Generating All Useful Structures

To solve this problem, our first step is to generate a set $\mathcal{T}$ of potentially useful structures. A valid structure may be placed at any position and orientation in the environment, but we observe that it is only useful if it connects two different regions, so we ought to look for structures that bridge the boundaries between regions. Regions are bounded by sloped areas (where nodes are removed) and sharp cliffs (where edges are removed), but because sloped areas violate the conditions for buildability, we consider only cliffs as candidate structure locations. We define the set of build points $B=\left\{{\mathbf{b}}_{1},{\mathbf{b}}_{2}\ldots,{\mathbf{b}}_{n}\right\}$ to be the coordinates of the cliff edges. At each build point, we attempt to synthesize a structure connecting the top of the cliff to another region.

For the robot to move from the cliff surface onto the structure, the structure’s orientation $\mathbf{\hat{u}}$ should be nearly perpendicular to the cliff surface (Definition 5). In our implementation, the cliff surface normal at $\mathbf{b}$ is estimated by selecting all nodes with a radius of $L_{B}$ of $\mathbf{b}$ and training a linear classifier (specifically an SVM) using node coordinates as features and node regions as labels; the resulting classification plane provides a suitable estimate of the surface normal. The error rate of the classifier provides a measure of surface flatness: if more than 10% of training points are incorrectly classified, the boundary is considered non-flat and the build point is rejected.

IV-A Algorithm

Given build point $\mathbf{b}$ and orientation $\mathbf{\hat{u}}$, we synthesize valid structures by solving the constraint satisfaction problem imposed by the conditions for structure validity (Definition 5). Using parameters $L_{B}=8cm$ and $\Delta Z_{cliff}=4cm=L_{B}/2$ for our robot and building blocks, the constraint imposed by Equations 3-5 imposes upper and lower bounds on the cell heights $0pt_{i}$ (measured in number of blocks) as a function of the bounds on its neighbors:

[TABLE]

The brackets $\lceil x\rceil$ and $\lfloor x\rfloor$ denote the ceiling and floor functions, which round $x$ up or down to the nearest integer (respectively). The above equations enforce the constraints imposed on $c_{i}$ by $c_{j}$, where $j=i\pm 1$. Fig. 3 illustrates these upper and lower bounds for an example structure.

The $\operatorname{\textsc{Waterfall}}$ algorithm (Algorithm 1) solves for structures by propagating these bounds outwards from $\mathbf{b}$. It begins by placing the first structure cell at the top of the cliff, assigning it height $h=0$. It then marches cells into the lower region, outwards from $\mathbf{b}$ in the direction of $\mathbf{\hat{u}}$ (Fig. 3). For each new cell, it calculates $0pt_{min}$ and $0pt_{max}$, propagating the constraints forward. If it encounters a cell that is unbuildable, or for which $0pt_{max}<0$, it returns False (no structure can be built here). If it finds a cell for which $0pt_{min}<0$, it records this cell as the endpoint of the structure, and assigns it height $0pt=0$.

The algorithm then marches back to $\mathbf{b}$, assigning cell heights. At each cell $c_{i}$, it checks the $0pt_{min}$ constraint in the backwards direction (imposed on $i$ by $i+1$), and assigns $0pt_{i}$ to be the maximum of the lower-bounds from enforcing the constraint in both directions.

The algorithm performs a final pass to assign terminators. At each cell $i$, it considers the difference in height $Z$ between its neighbors $i+1$ and $i-1$. If magnitude of the difference is less than $L_{B}$/2, the terminator is a block (flat); otherwise, the terminator is a wedge oriented so that its slope matches the sign of $\Delta Z$.

IV-B Correctness Guarantees

Lemma 1** (Optimality).**

$\operatorname{\textsc{Waterfall}}$* finds the cheapest valid structure (if it exists) that can be built at $\mathbf{b}$ which will connect the region at the top of the cliff to another region.*

Proof.

Valid structures must begin and end with zero-height cells (i.e. at the environment surface). In its first pass, $\operatorname{\textsc{Waterfall}}$ finds the closest cell to $\mathbf{b}$ at which a valid structure could terminate, and selects this cell as the end of the structure. At each cell between the terminators, $\operatorname{\textsc{Waterfall}}$ computes the lower bound on structure height imposed by propagating the traversability constraints forward from the start cell and backwards from the end cell, and selects the minimum structure height satisfying both constraints. ∎

Lemma 2** (Completeness).**

For the purposes of minimal structure construction, we need not consider any structures that could be built at $\mathbf{b}$ other than the one produced by $\operatorname{\textsc{Waterfall}}$.

Proof.

To show this, consider if it were not the case. Say a longer structure $T_{L}$ (connecting $\mathbf{b}$ to a different region) is required as part of the minimum spanning tree of structures. This structure would cut through multiple regions. Based on the buildability conditions, at each point on a region boundary that this structure crosses through there is another potential build point, with its own potential minimal structure identified by the $\operatorname{\textsc{Waterfall}}$ algorithm in the $\mathbf{\hat{u}}$ direction. All of these minimal structures must cost less than $T_{L}$, and their footprints are fully contained within the footprint of $T_{L}$. Call the set of these structures $\mathcal{T}_{L}$.

Now consider if $T_{L}$ were removed from the minimum spanning tree. The environment would then be separated into two separate components. At least one of the minimal structures in $\mathcal{T}_{L}$ must bridge these components. If this structure is added back to the minimum spanning tree, then the environment is again fully traversable, and the new set of structures costs less than the original. So the original spanning tree of structures was not minimal. We therefore only need to search through the set of structures discovered through $\operatorname{\textsc{Waterfall}}$. ∎

IV-C Runtime and Generalization

The structure synthesis problem can be formulated as an integer linear program (ILP) – the variables (cell heights) are integer valued, while the constraints (traversability) and objective (number of blocks used) are linear. Well-established algorithms can solve general ILP problems, however ILP is known to be NP-Complete. For the particular case of our building blocks, we are able to solve this ILP efficiently using the $\operatorname{\textsc{Waterfall}}$ algorithm, which synthesizes structures with $N$ cells in $O(N)$ time. For other kinds of building blocks, the $\operatorname{\textsc{Waterfall}}$ algorithm could be modified, or a general ILP solver could be used.

V BB-MST Algorithm – Solving for the Minimum Spanning Tree of Structures

Once the useful structures are identified, then these structures can be used to make the environment traversable. Given the set $\mathcal{T}$ of structures generated by running $\operatorname{\textsc{Waterfall}}$ at every build point, form graph $G_{R}(R,\mathcal{T})$ with nodes as regions and edges as structures connecting regions, and assign edge weights equal to the cost of their structures. We seek $M^{*}_{R,\mathcal{T}}$, a conflict-free min-cost spanning tree of $G_{R}(r,\mathcal{T})$. We refer to this problem as struct-MST.

V-A NP-Hardness of struct-MST

Kruskal’s algorithm computes minimum-spanning trees of graphs in $O(|E|\log|V|)$ time. However, structure-structure and region-structure conflicts (Section III-D) impose constraints that make struct-MST a much more difficult problem to solve efficiently. Structure-structure conflicts create pairwise negative disjunctive constraints between edges in $G_{R}$, that is, pairs of edges that cannot both be present in the solution. These constraints may be represented in terms of a conflict graph with vertices as edges in the original graph, and edges as constraints. It has been shown that deciding the existence of a spanning tree (as well as finding the min-cost spanning tree) of a graph is strongly $\mathcal{NP}$-hard under negative disjunctive constraints, unless the conflict graph has a maximum path length less than two [13]. This is not the case for struct-MST (because any structure could conflict with multiple others), making struct-MST at least $\mathcal{NP}$-hard.

V-B Algorithm

We present $\operatorname{\textsc{BB-MST}}$, a branch-and-bound algorithm to solve struct-MST. Branch-and-bound is a general algorithm for solving combinatorial optimization problems, and has been employed as a practical technique to solve many $\mathcal{NP}$-hard problems [14].

Algorithm 2 provides pseudocode for $\operatorname{\textsc{BB-MST}}$. Each time $\operatorname{\textsc{BB-MST}}$ is called, it uses Kruskal’s algorithm to solve for $M_{R,\mathcal{T}}$, the MST of the regions and structures passed in as arguments, and compares it to the best-yet solution $M^{*}$. Regardless of whether $M_{R,\mathcal{T}}$ is a valid solution, if $\texttt{cost}(M_{R,\mathcal{T}})\geq\texttt{cost}(M^{*})$, $\operatorname{\textsc{BB-MST}}$ returns $M^{*}$, terminating search of the branch. If $M_{R,\mathcal{T}}$ is conflict-free and $\texttt{cost}(M_{R,\mathcal{T}})<\texttt{cost}(M^{*})$, $M^{*}$ is updated and $M_{R,\mathcal{T}}$ is returned.

If $M_{R,\mathcal{T}}$ includes a conflict, $\operatorname{\textsc{BB-MST}}$ recursively branches, solving two or more child problems in which some of the conflicting edges or nodes have been removed, and returns the cheapest of the child solutions. Structure-structure and region-structure conflicts are handled as follows:

V-B1 Structure-Structure Conflicts

If there is a conflicting pair of structures $\{T_{1},T_{2}\}$ in $M_{R,\mathcal{T}}$, form two child problems, removing one conflicting structure from each: $M_{1}=\operatorname{\textsc{BB-MST}}(R,\mathcal{T}\setminus T_{1})$ and $M_{2}=\operatorname{\textsc{BB-MST}}(R,\mathcal{T}\setminus T_{2})$. Return the cheaper of the two solutions.

V-B2 Region-Structure Conflicts

Region-structure conflicts are somewhat more complex. Let $\mathcal{T}(M_{R,\mathcal{T}})$ be the set of edges of $M_{R,\mathcal{T}}$. $\forall(T,r)\in\mathcal{T}(M_{R,\mathcal{T}})\times R$, check whether $T$ has more than one structure cell containing a boundary node of $r$; if so, $T$ might split $r$. Let $r_{c}$ be one such region, and $\mathcal{T}_{c}=\left\{{T}_{1},{T}_{2}\ldots,{T}_{k}\right\}$ be the set of structures meeting this condition. Let $V_{\mathcal{T}_{c}}$ be the set of nodes in the grid-graph $G(V,E)$ occupied by $\mathcal{T}_{c}$, $V_{r_{c}}$ be the set of nodes in $r_{c}$, and $G_{r_{c}}$ be the subgraph of $G(V,E)$ induced by $V_{r_{c}}\setminus V_{\mathcal{T}_{c}}$. Compute $R_{r_{c}}$, the set of connected components of $G_{r_{c}}$: if $R_{r_{c}}$ has more than one element, $r_{c}$ has been split and is in conflict with $\mathcal{T}_{c}$.

We handle this by forming $k+1$ branches, where $k$ is the number of structures in $\mathcal{T}_{c}$. In each of the first $k$ branches, we remove one conflicting structure: $M_{i}=\operatorname{\textsc{BB-MST}}(R,\mathcal{T}\setminus T_{i})~{}\forall~{}T_{i}\in\mathcal{T}_{c}$. In the final branch, we keep all conflicting structures, but split the region $r_{c}$ into multiple sub-regions: Letting $R_{split}=(R\setminus r_{c})\cup R_{r_{c}}$, we have $M_{split}=\operatorname{\textsc{BB-MST}}(R_{split},\mathcal{T})$. We return the cheapest of all solutions $\left\{M_{1},M_{2},\ldots,M_{k},M_{split}\right\}$.

V-C Correctness Guarantees

Lemma 3** (Child Bounding).**

Let $M_{R,\mathcal{T}}$ (returned by Kruskal’s algorithm, Algorithm 2 line 7) contain one or more conflicts. $\texttt{cost}(M_{R,\mathcal{T}})$ is a lower bound on the cost of the solution to any child problem formed by branching on a conflict in $M_{R,\mathcal{T}}$.

Proof.

Whenever $\operatorname{\textsc{BB-MST}}$ branches, it either eliminates one structure, or it splits one region into multiple regions. Consider the case where structure $T$ has been eliminated. It is clear that $\texttt{cost}(M_{R,\mathcal{T}})\leq\texttt{cost}(M_{R,\mathcal{T}\setminus T})$: $M_{R,\mathcal{T}\setminus T}$ is optimal with respect to $\mathcal{T}\setminus T$, so making $T$ available could only decrease cost. Consider the case where region $r_{c}\in R$ has been split into two sub-regions $r_{1},r_{2}$, resulting in a new set of regions $R_{split}=(R\setminus r_{c})\cup\{r_{1},r_{2}\}$. $M_{R_{split},\mathcal{T}}$ is the MST which spans $R_{split}$. Form a new graph $\mathcal{M}$ identical to $M_{R_{split},\mathcal{T}}$ except that nodes $r_{1},r_{2}$ have been merged together to form node $r_{c}$. We may remove one edge from this graph to form a tree, which we will denote $M^{\prime}$. By construction, $\texttt{cost}(M_{R_{split},\mathcal{T}})=\texttt{cost}(\mathcal{M})\geq\texttt{cost}(M^{\prime})$. $M^{\prime}$ spans the same set of nodes as $M_{R,\mathcal{T}}$, but $M_{R,\mathcal{T}}$ is the MST, so $\texttt{cost}(M_{R,\mathcal{T}})\leq\texttt{cost}(M^{\prime})\leq\texttt{cost}(M_{R_{split},\mathcal{T}})$. ∎

Lemma 4**.**

$\operatorname{\textsc{BB-MST}}(R,\mathcal{T})$* returns the best-yet set of spanning structures that connects the regions in the environment.*

Proof.

We prove by induction that each recursive call of $\operatorname{\textsc{BB-MST}}(R,\mathcal{T})$ returns either $M^{*}_{R,\mathcal{T}}$ or $M^{*}$ (the best-yet solution), whichever is cheaper.

V-C1 Base Case

There are three conditions under which $\operatorname{\textsc{BB-MST}}$ returns without branching. (1) Null: $G_{R}$ is disconnected, so no spanning tree can be found and $\operatorname{\textsc{BB-MST}}$ returns $\emptyset$. (2) Shortcut: $\texttt{cost}(M_{R,\mathcal{T}})\geq\texttt{cost}(M^{*})$, so $\operatorname{\textsc{BB-MST}}$ returns $M^{*}$; by Lemma 3, this branch cannot contain a solution cheaper than the current best-yet solution $M^{*}$, so we stop exploring it. (3) Success: $\texttt{cost}(M_{R,\mathcal{T}})<\texttt{cost}(M^{*})$ and $M_{R,\mathcal{T}}$ is conflict-free, so we set $M^{*}\leftarrow M_{R,\mathcal{T}}$ and return $M_{R,\mathcal{T}}$. In this case, it is clear that $M_{R,\mathcal{T}}=M^{*}_{R,\mathcal{T}}$ since Kruskal’s algorithm produces a min-cost spanning tree, which is explicitly verified as conflict-free.

V-C2 Induction Step

When $M_{R,\mathcal{T}}$ has a conflict, $\operatorname{\textsc{BB-MST}}$ forms two or more child branches and returns the cheapest solution among them. Assuming recursive calls of $\operatorname{\textsc{BB-MST}}(R,\mathcal{T})$ return $M^{*}_{R,\mathcal{T}}$ for the set of regions and structures passed down to them, we prove that the optimal solution to the parent problem must be the cheapest of its children.

Let $M_{R,\mathcal{T}}$ contain structure-structure conflict set $\{T_{1},T_{2}\}$. Since $\{T_{1},T_{2}\}$ cannot be in the solution, we know $M^{*}_{R,\mathcal{T}}$ must be a subset of either $\mathcal{T}\setminus T_{1}$ or $\mathcal{T}\setminus T_{2}$, since $\mathcal{P}(\mathcal{T}\setminus T_{1})\bigcup\mathcal{P}(\mathcal{T}\setminus T_{2})$ is the set of all subsets of $\mathcal{T}$ that do not contain $\{T_{1},T_{2}\}$. Therefore, $M^{*}_{R,\mathcal{T}}$ must be the cheaper of $M^{*}_{R,\mathcal{T}\setminus T_{1}}$ and $M^{*}_{R,\mathcal{T}\setminus T_{2}}$.

By similar reasoning, let $M_{R,\mathcal{T}}$ contain a region-structure conflict set $\{r_{c},\mathcal{T}_{c}\}$. We know that either $M^{*}_{R,\mathcal{T}}=M^{*}_{R,\mathcal{T}\setminus T_{c}}$ for some $T_{c}\in\mathcal{T}_{c}$, or $M^{*}_{R,\mathcal{T}}=M^{*}_{R_{split},\mathcal{T}}$, since in each of these cases a single member of the conflict set has been removed. $\operatorname{\textsc{BB-MST}}$ returns the cheapest of these options by comparing $M_{edges}$ and $M_{split}$, returned by $\operatorname{\textsc{branchEdges}}$ and $\operatorname{\textsc{branchRegion}}$. ∎

Theorem 1**.**

A combination of $\operatorname{\textsc{BB-MST}}$ and $\operatorname{\textsc{Waterfall}}$ generates the minimum cost set of structures that makes the environment fully traversable.

Proof.

According to Lemma 1, the set of structures produced using the $\operatorname{\textsc{Waterfall}}$ algorithm contains all the minimal structures needed to connect each region to its neighbors. Lemma 2 states that no other structures are needed to produce a minimal spanning set. The $\operatorname{\textsc{BB-MST}}$ algorithm searches through all possible subsets of structures and, by Lemma 4, outputs the best-yet spanning set. Therefore, if the algorithm $\operatorname{\textsc{BB-MST}}$ is allowed to run to completion using the full set of structures provided by $\operatorname{\textsc{Waterfall}}$, it will produce the minimum cost spanning set. ∎

V-D Runtime

$\operatorname{\textsc{BB-MST}}$ resolves conflicts by recursively exploring each possible conflict-free subset of the conflict set, so in the worst case it will explore an exponential number of branches before finding a solution. This is to be expected: struct-MST is NP-Hard, and $\operatorname{\textsc{BB-MST}}$ solves the problem exactly.

In practice, $\operatorname{\textsc{BB-MST}}$ typically prunes many branches and explores a tiny fraction of this space. Additionally, once a feasible solution is found, $\operatorname{\textsc{BB-MST}}$ has an anytime property: it can be terminated at any time and return the best-yet feasible solution. To identify that no conflict-free spanning solutions exists, the algorithm must explore each branch until $G_{R}$ becomes disconnected, which can be very time-consuming.

It is worth noting that relaxing the optimality requirement of struct-MST would not improve the worst-case runtime, because deciding the existence of a (non-minimal) spanning tree of structures is also $\mathcal{NP}$-hard.

VI Results

VI-A Examples and Experiments

Our Python implementation can solve for optimal sets of structures for the SMORES-EP robot and building blocks given a height-field representation of an environment. The implementation accepts CAD models and 3D maps of real environments as inputs, and we show that it can solve for optimal sets of structures in real-world indoor environments.

VI-A1 CAD Example – Table-and-Chair

Figure 4 shows optimal solutions for two configurations of the table-and-chair CAD example environment from Figure 3. In Example (A) -“Chair Pulled Out”, the optimal solution uses one large ramp from the floor to the chair seat, and two smaller structures connecting the seat to the tabletop and tabletop to chair top. Notice the position the structure connecting the tabletop to the chair top: the structure crosses over the chair seat, and has been placed at the far left edge of the chair seat to avoid creating a region-structure conflict (which would have required a fourth structure). In Example B, the chair has been pushed in further, and there is no longer enough space to place structures on the chair seat. The algorithm is forced to select a more expensive solution using three large ramps from the floor.

VI-A2 Real-World Experiments

Figure 5 shows solutions scanned from two indoor environments. The video accompanying this paper shows the SMORES-EP robot moving through these environments using structures placed in the locations selected by the algorithm.

To solve for structures in these environments, 3D occupancy grid maps were created with the Octomap library [15] using point cloud data scanned with a Microsoft Kinect RGB-D sensor. A similar RGB-D sensor can be carried by SMORES-EP to autonomously map its environment [16]. Occupancy grids were converted to height fields (2D grayscale images), which were smoothed (median filter, window of 3 pixels) and segmented (K-means, K=150) to reduce noise before running the algorithm to generate structures. Real environments often contain regions that are too small for the robot to occupy them, even if it could access them (for example, the arms and back of the chair in Figure 5). To account for this, we test whether each region can fit an inscribed 8cm square (the size of one SMORES-EP robot module) anywhere within it, and remove small regions from the set of regions $R$ before running the algorithm.

In Figure 5, Environment (D) consists of a round table, a stack of magazines, an office chair, and a storage container. The algorithm determines that adding one 4-block high ramp structure and three single wedges to this environment will allow the robot to reach every surface large enough to support it. Environment (E) is a staircase. The algorithm determines that six 2-block high ramps can be introduced to make it globally traversable by the robot. The location of each ramp on a given step is effectively random, because solutions that use the same set of structures in different locations have equal cost.

VI-B Runtime Performance

Table I shows metrics for the example environments from Figs 4 and 5. The algorithm generates thousands of structures and solves problems with 4-9 regions in minutes. In many cases, the number of potential conflicts (which determines the worst-case runtime) is in the tens of thousands, but the algorithm explores a tiny fraction of them (less than thirty). The Conflict Pair Fraction (CPF) for each problem is the percentage of pairs of structures which conflict, and provides a measure of problem difficulty. In all examples, a significant fraction of the total time required to reach a solution is spent preprocessing the world (e.g. generating the initial grid graph and identifying cliffs edges and steep areas).

Runtime performance was profiled by generating and solving random environments similar to environment (C) (Fig 4). Each environment is a $3\times 3$ checkerboard with $6L_{B}$-wide squares with randomly selected heights between zero and $3L_{B}$ (in one-pixel increments). Trials were terminated after a timeout of 10 minutes. Of 1118 total trials, 981 found a solution (conflict-free MST), and 137 found that no solution exists. 834 solutions were found in one try, and 16 no-solutions were found in one try. 17 trials timed out, of which 11 found no solution and 6 found a feasible solution. On average, each problem generated 363 structures with 10184 conflicts, and CPF of 7.83%.

VII Discussion and Future Work

This paper presents an algorithm to find min-cost spanning sets of structures allowing a robot to reach every surface of an environment. Finding optimal (as opposed to feasible) solutions to these problems is important - building larger structures takes more time, and in real scenarios the number of available building blocks is always limited. For example, the solution in environment (A) requires 17 blocks, whereas solution (B) (which is also a feasible solution for (A)) requires 46.

Given a build point and direction vector, $\operatorname{\textsc{Waterfall}}$ generates an optimal structure in linear time, allowing thousands of candidate structures to be generated in under a minute. Given a set of candidate structures, $\operatorname{\textsc{BB-MST}}$ will always eventually find a conflict-free solution to a struct-MST problem, if one exists. Because struct-MST is NP-Hard, any algorithm that solves it will have exponential worst-case complexity, and for some problems (especially when no solution exists) $\operatorname{\textsc{BB-MST}}$ will run for an impractically long time before returning. However, in typical problems, $\operatorname{\textsc{BB-MST}}$ explores the solution space efficiently and returns optimal solutions in a few seconds. In many realistic problems, the number of potential conflicts is relatively small compared to the number of potentially useful combinations of structures. For example, the (D)  and (E)  have low CPF values, and in both cases the first MST generated had no conflicts.

Some tasks might require a robot to access only a subset of the regions in an environment. The framework introduced in this paper could easily be extended to solve for min-cost paths of structures (connecting a pair of regions) by calling Dijkstra’s algorithm in place of Kruskal’s algorithm in $\operatorname{\textsc{BB-MST}}$. With slight modification, the framework could also solve for approximately-optimal Steiner trees of structures, to make a selected subset of regions accessible. Solving for min-cost Steiner trees in graphs is $\mathcal{NP}$-hard, but poly-time algorithms can solve the problem approximately [17]. Calling such an algorithm in place of Kruskal’s algorithm would allow $\operatorname{\textsc{BB-MST}}$ to compute Steiner trees, as long as the approximation factor is taken into account when comparing solution costs in the shortcut-return case.

Future work includes taking robot path planning into account when selecting structure locations. For example, in the stairs environment in Figure 5, placing the ramps in a line would allow the robot to move more efficiently through the environment. Optimization of structure positions could be performed as a separate post-processing step after the algorithm selects structures, or information about structure position could be directly incorporated into the cost function for evaluating solutions.

VIII Conclusion

This paper presents an complete, optimal algorithm to generate sets of structures that could be added to an environment to make it globally accessible to a robot. In experiments using real and simulated environments, we demonstrated that the algorithm can synthesize optimal sets of structures with practical speed in realistic settings. This opens up the possibility for a structure-building robot to enter a new environment and quickly determine what structures should be built to enable free movement, enabling tasks that would otherwise be very difficult or impossible for the robot.