Resilience by Reconfiguration: Exploiting Heterogeneity in Robot Teams

TL;DR

This paper introduces a reconfiguration method for heterogeneous robot teams to maintain resource availability after failures, optimizing communication and formation using integer semi-definite programming and simulated annealing.

Contribution

It presents a novel approach combining integer semi-definite programming and simulated annealing for rapid reconfiguration of robot teams after resource failures.

Findings

Method achieves reconfiguration in a few seconds.

Validated in simulation and real-robot experiments.

Maintains high resource availability despite failures.

Abstract

We propose a method to maintain high resource in a networked heterogeneous multi-robot system to resource failures. In our model, resources such as and computation are available on robots. The robots engaged in a joint task using these pooled resources. In our model, a resource on a particular robot becomes unavailable e.g., a sensor ceases to function due to a failure), the system reconfigures so that the robot continues to have to this resource by communicating with other robots. Specifically, we consider the problem of selecting edges to be in the system's communication graph after a resource has occurred. We define a metric that allows us to characterize the quality of the resource distribution in the represented by the communication graph. Upon a resource becoming unavailable due to failure, we reconfigure network so that the resource distribution is brought as to the ideal resource distribution as possible without a big change in the communication cost. Our approach uses integer semi-definite programming to achieve this goal. We also provide a simulated annealing method to compute a formation that satisfies the inter-robot distances imposed by the topology, along with other constraints. Our method can compute a communication topology, spatial formation, and formation change motion planning in a few seconds. We validate our method in simulation and real-robot experiments with a team of seven quadrotors.

Figures

Equations

$$\mathbf{L}=Diag(\mathbf{A}\cdot 1^{|\mathcal{V}|})-\mathbf{A}.$$

$$\mathbf{L}\cdot 1^{|\mathcal{V}|}$$

$$Trace(\mathbf{L})$$

$$[\bar{\mathbf{A}}{]}_{i,j}=\{\begin{array}{ll}1& \text{if}(i,j)\in \mathcal{E}\text{or}i=j\\ 0& \text{otherwise.}\end{array}$$

$$[\mathbf{D}{]}_{i,j}=\{\begin{array}{ll}\Vert X_i-X_j\Vert & \text{if}(i,j)\in \mathcal{E}\text{or}(j,i)\in \mathcal{E}\\ \infty & \text{otherwise.}\end{array}$$

$$[{\mathbf{\Gamma }}_r{]}_{i,j}=\{\begin{array}{ll}1& i_n\text{has resource}j\in [r]\\ 0& \text{otherwise.}\end{array}$$

$$\mathbf{V}(\bar{\mathbf{A}},\mathbf{\Gamma })=n\cdot 1^{n\times r}-\bar{\mathbf{A}}{\mathbf{\Gamma }}_r.$$

$$\Vert \mathbf{V}(\bar{\mathbf{A}},\mathbf{\Gamma }){\Vert }_{\ast }=\Vert n\cdot 1^{n\times r}-\bar{\mathbf{A}}{\mathbf{\Gamma }}_r{\Vert }_{\ast }.$$

$$\underset{\begin{array}{c}\mathbf{L}\in {\mathcal{S}}_{+}^n,\mu \in {\mathbb{R}}_{>0},\\ \mathbf{\Pi }\in \{0,1{\}}^{n\times n}\end{array}}{\mathrm{minimize}}$$

$$\mathbf{L}\cdot 1^n=0^n$$

$$\frac{1}{n}{1}^{T}1+\mathbf{L}⪰\mu \mathbf{I}$$

$$diag(\mathbf{\Pi })=1^n\forall i\in [n]$$

$$\mathbf{\Pi }={\mathbf{\Pi }}^T$$

$$[\mathbf{L}{]}_{i,i}\ge c_{max}\forall i\in [n]$$

$$[\mathbf{L}{]}_{i,j}\ge c_{min}{\mathbf{\Pi }}_{i,j}\forall (i,j)\in [n{]}^2,i\ne j$$

$$[\mathbf{L}{]}_{i,j}\le c_{max}{\mathbf{\Pi }}_{i,j}\forall (i,j)\in [n{]}^2,i\ne j$$

$$\Vert \mathbf{\Pi }-\bar{\mathbf{A}}[k-1]{\Vert }_F^2\le ne$$

$$\Vert \mathbf{V}(\mathbf{\Pi },{\mathbf{\Gamma }}_{\mathbf{r}}){\Vert }_{\ast }<\Vert \mathbf{V}(\bar{\mathbf{A}}[\mathbf{k}-1],{\mathbf{\Gamma }}_{\mathbf{r}}){\Vert }_{\ast }.$$

$$[\mathbf{D}[k]{]}_{i,j}=\{\begin{array}{ll}\kappa (|[\mathbf{L}{]}_{i,j}|-c_{min})+d_{mc}& \text{if}[\mathbf{L}{]}_{i,j}<0\\ \infty & \text{otherwise},\end{array}$$

$$\mathrm{minimize}$$

$$d_s\le \Vert X_i-X_j{\Vert }_2\le d_{mc}$$

$$d_{mc}\le \Vert X_i-X_j{\Vert }_2$$

$$B^{\min }\le X_i\le B^{\max }$$

$$E(x)=\sum _{(i,j)\in \mathcal{E}}(\Vert X_i-X_j{\Vert }_2-d_{ij}{)}^2+\sum _{(i,j)\in \overline{\mathcal{E}}}{p}_H(d_{mc}-\Vert X_i-X_j{\Vert }_2)$$

$$+\sum _{(i,j)\in \mathcal{E}}\left[p_H(d_s-\Vert X_i-X_j{\Vert }_2)+p_H(\Vert X_i-X_j{\Vert }_2-d_{mc})\right]$$

$$+\sum _{i\in V}\sum _{d\in [3]}{p}_H(X_{i,d}-B_d^{\max })+p_H(B_d^{\min }-X_{i,d}),$$

$$\underset{H\to \infty }{\lim }{p}_H(y)=\{\begin{array}{ll}\infty & :y>0\\ 0& :y<0.\end{array}$$

Full text

I Introduction and Related Work

A heterogeneous multi-robot team, where robots have varied sensing, actuation, communication and computational capabilities, is a promising direction to consider when building a resilient team. Such a team can work together by sharing resources between individual robots to perform complex tasks, thereby being resilient to failures of individual robots. For example, when a particular sensor on a robot fails, it may be able to rely on measurements made by a teammate nearby.

If the team is to perform its task by sharing resources between team members, it is desirable to configure the team with a communication topology such that each robot has access to its teammates’ resources (e.g., sensing, computation) within some neighborhood. Under distance-limited communication, specifying such a topology restricts the feasible set of the robots’ physical locations.

As motivation, consider a multi-target tracking application by a team with two kinds of quadrotors as shown in Figure 1. The smaller quadrotors are equipped with sensors (e.g. cameras) required to sense and identify targets to be tracked. Each large quadrotor has two kinds of computational resource $P1$ and $P2$ (e.g. GPU and CPU). Smaller quadrotors do not have the computational resources to carry out the computation required for tracking the target of interest. They rely on the larger quadrotors to process their sensor data. As shown Figure 1, if resource $P2$ of a quadrotor fails then the communication network must reconfigure so that $P2$ is available to every member in the team in a few hops.

Here, we propose a two-stage method to reduce the impact of a resource failure in a heterogeneous robot team engaged in a task. In response to a resource failure, our method first updates the communication network topology such that the new communication topology is close to the original communication topology and the resource distribution within the network is as close as possible to the maximal resource distribution. An maximal resource distribution is one in which each robot has access to all available resources within a single hop distance. This stage is followed by computing a spatial formation of the robots under which the new topology can be achieved by distance-constrained communications. The framework developed here only accounts for the failure of an individual resource at a time (e.g. a single sensor) on a robot, not for the case where the robot itself completely fails to function (e.g. a quadrotor loses the ability to perform low-level flight control). Thus in our model, when a robot loses its own resources to perform its part in the joint team task, it may still execute its role by relying on the resources of 1-hop neighbors. This leaves open the possibility that in some extreme cases a robot can still help the team perform its overall task e.g. even when all its own sensors needed for the team task have failed but it has access to the relevant sensor information from neighbors. In full generality, we are interested in studying the problem where a robot that has lost a resource needed for performing its part in the team task can access the lost resource from other robots in the team with low communication and physical reconfiguration costs. The 1-hop neighbor case is our first attempt to develop an abstraction to concurrently formalize these ideas. In future work, we plan to study the general case where the access ”distance” (in hops) to resources is larger than 1.

In stage one, we use a mixed integer semi-definite program (MISDP) formulation to solve the problem of constructing a network topology and generating a pairwise distance between communicating robots. The pairwise distances are constructed such that the communicating robots respect a minimum-distance collision constraint, but are within communication range. Our MISDP objective encourages communicating robots to spread out by maximizing the total pairwise distances between communicating robots.

The pairwise distance output from the MISDP forms the input to the second stage of our approach, where we compute physical locations of the robot team. The communication topology and desired pairwise distances dictate an optimization objective and constraints on the physical locations of the robots. Optimizing the multi-robot formation is a highly nonconvex, possibly NP-hard problem. We employ a simulated annealing algorithm with penalty functions to escape bad local minima. It is noteworthy that, in our model, the centralized controller is a monitor for the activities of the robot team. It intervenes to reposition robots only in the event of a robot resource failure. Also, sharing resources with other robots through the centralized controller (e.g., a star network) is unsuitable as, it would increase the communication cost and possibly saturate the communication bandwidth of the centralized controller.

In the literature, the problem of resilience in multi-robot systems has been studied primarily in the context of constructing communication networks that preclude malicious robots in the network from exerting an adverse influence on the network [1, 2]. Researchers have proposed notions of resilience in networks such as r-robustness [3] and p-fraction robustness [3] to characterize the resilience of the network in achieving consensus in the presence non-cooperative or malicious agents, in terms of network connectivity. In [4] algorithms for building resilient robot networks are introduced using this characterization of resilience. Analogous to our work, Hausman et al. [5] also develops a notion for reasoning about the quality of networks neighboring to a given network. Since [5] focuses on target tracking applications, it compares sensing network topologies in terms of their ability to reduce the expected future uncertainty of the target position.

In this work, we take a different approach to resilience by posing it as an abstract resource sharing problem in a heterogeneous robot team. By doing so, we make the following three contributions:

• •

We pose the problem of resilience in a heterogeneous robot team in the context of inter-robot resource sharing. As resources become unavailable on robots due to failures, the system reconfigures to bring resources back into communication range as much as possible. We introduce the task inefficacy matrix – a description of on how far is the resource distribution in a configuration is from the maximal resource distribution – and exploit it to drive network reconfiguration.

• •

We formally pose and solve the configuration generation problem which generates a new communication topology and pairwise distances in response to a failure.

• •

We give a solution to the formation synthesis problem which generates appropriate values for robot positions satisfying the new configuration.

II Preliminaries and Notation

For any positive integer $z\in\mathbb{Z}^{+}$, $[z]$ denotes the set $\{1,2,\cdots,z\}$. We use $\|\cdot\|$ to denote the standard Euclidean 2-norm and the induced 2-norm for vectors and matrices respectively. $\|\mathbf{M}\|_{F}$ denotes the Frobenius norm of a matrix $\mathbf{M}\in\mathbb{R}^{m_{1}\times m_{2}}$, $\|\mathbf{M}\|_{F}\triangleq\sqrt{trace(\mathbf{M}^{T}\mathbf{M})}$. If we arrange the singular values of matrix $\mathbf{M}$ in nondecreasing order, then the $i^{th}$ singular value of $\mathbf{M}$ is denoted by $\sigma_{i}\{\mathbf{M}\}$. The nuclear norm (trace norm) of $\mathbf{M}$, defined as $\sum_{i=1}^{\min\{m_{1},m_{2}\}}\sigma_{i}\{\mathbf{M}\}$, is denoted by $\|\mathbf{M}\|_{*}$. We use $\mathbf{1}^{m_{1}}$ and $\mathbf{1}^{m_{1}\times m_{2}}$ to represent a vector and matrix of ones of appropriate dimensions, respectively. Similarly, $\mathbf{0}^{m_{1}}$ and $\mathbf{0}^{m_{1}\times m_{2}}$ denote a vector and matrix of zeros respectively. For any vector $T\in\mathbb{R}^{m_{1}}$, $Diag(T)$ denotes a matrix with the elements of $T$ along its diagonal. Also, $diag(\mathbf{M})$ outputs a vector which contains the diagonal entries of matrix $\mathbf{M}$ as its elements. $[\mathbf{M}]_{i,j}$ denotes the $i,j$ entry of $\mathbf{M}$. Finally, $\mathcal{S}^{m}_{+}$ denotes the space of $m\times m$ symmetric positive semi-definite matrices. A weighted undirected graph $\mathcal{G}$ is defined by the triplet $(\mathcal{V},\mathcal{E}\subseteq\mathcal{V}\times\mathcal{V},\mathbf{A}\in\mathbb{R}^{|\mathcal{V}|\times|\mathcal{V}|}_{\geq 0})$, where $\mathbf{A}$ is the weighted adjacency matrix of the graph. Also, $\overline{\mathcal{E}}=(\mathcal{V}\times\mathcal{V})\setminus\mathcal{E}$ denotes the edge complement of $\mathcal{G}$. The graph Laplacian matrix $\mathbf{L}$ of $\mathcal{E}$ can be computed as @hyper@itemfalse

[TABLE]

We list a few properties of a graph Laplacian matrix used in this article [6]: @hyper@itemfalse

[TABLE]

III Problem Statement

We consider a team of $n$ heterogeneous robots labeled sequentially as $[n]$, equipped with different types of resources (e.g. sensors, memory, actuators, etc.). The team is assigned with a task of interest. For ease of identification, we denote the robot with label $i$ as $i_{n}$ . Also, let $X_{i}$ denotes the position vector $[x_{i},y_{i},z_{i}]^{T}\in\mathbb{R}^{3}$ of $i_{n}$ and $\{X_{i}\}_{i=1}^{n}$ denote the positions of the robots. A robot is equipped with at most $r$ distinct types of resources. The set $[r]$ contains labels of the resources available within the heterogeneous team.

We assert that all $r$ resources are essential to perform the assigned task. In other words, the team becomes incapable of performing the task of interest if even a single resource among the $r$ resources is unavailable within the team. We assume that each robot can localize itself accurately in the environment. Further, we assume that the robots never lose their localization ability. Finally, we assume that the communication graph (defined next) always has edge connectivity of at least 2, i.e., a single failure of a communication link during the task does not partition the communication graph.

We model the communication network using a dynamic undirected graph ${\mathcal{G}=(\mathcal{V}=[n],\ \mathcal{E}\subseteq\mathcal{V}\times\mathcal{V})}$, where the edges represent pairs of communicating robots. Based on the communication graph $\mathcal{G}$, we define its closed adjacency matrix $\mathbf{\bar{A}}$[7] as follows: @hyper@itemfalse

[TABLE]

In addition, we define the neighborhood distance matrix $\mathbf{D}$, such that, @hyper@itemfalse

[TABLE]

For brevity, we also use $d_{ij}$ to denote $[\mathbf{D}]_{i,j}$. Next we define a resilience metric which is a function of the communication network topology and the resource distribution in the robot team. As in [7], we define a binary matrix $\{0,1\}^{n\times r}$, which we refer to as the resource matrix and denote by $\boldsymbol{\Gamma}_{r}$. The entries of $\boldsymbol{\Gamma}_{r}$ are computed as follows, @hyper@itemfalse

[TABLE]

We term the tuple $(\mathcal{G},\mathbf{D},\boldsymbol{\Gamma}_{r})$ as a configuration of the heterogeneous multi-robot team and denote it by $\mathcal{C}$. We define the task inefficacy matrix $\mathbf{V}$ of a configuration $\mathcal{C}$ as @hyper@itemfalse

[TABLE]

Finally, we define the key quantity used to measure the inability of a heterogeneous multi-robot team with configuration $\mathcal{C}$ to perform the task - the task inefficacy of a heterogeneous multi-robot team. The task inefficacy of a heterogeneous multi-robot team with configuration $\mathcal{C}$ is computed as @hyper@itemfalse

[TABLE]

Next, we examine $\mathbf{V}$ to understand why $\|\mathbf{V}\|_{*}$ is a reasonable metric to quantify the task inefficacy of $\mathcal{C}$. The product matrix $\mathbf{\bar{A}}\cdot\boldsymbol{\Gamma}_{r}$ encodes information regarding the distribution of resources within the heterogeneous multi-robot team. Particularly, $[\mathbf{\bar{A}}\cdot\boldsymbol{\Gamma}_{r}]_{i,j}$ describes the exact number of robots in the neighborhood of robot $i_{n}$, including itself, with resource $j$. This matrix becomes $n\cdot\mathbf{1}^{n\times r}$ if each robot is equipped with every resource and the communication graph is fully connected, resulting in the most task-efficient configuration. The task inefficacy matrix thus describes how far a configuration is from the maximal configuration. Therefore, we minimize a metric which measures the distance between these matrices with a suitable norm. The nuclear norm is a surrogate measure to quantify the rank of a matrix. It is used extensively in the compressed sensing literature to compute low rank matrices [8]. As the robot configuration approaches the maximal configuration the task inefficacy matrix becomes sparser. We use the nuclear norm as a heuristic to measure the sparseness of the task inefficacy matrix. Although in general, low rank matrices need not be sparse, we found that the nuclear norm of the task inefficacy matrix works well for solving our problem and we employ it here. We may explore other matrix norms in future work. When it improves clarity, we will also refer to maximizing the task efficacy instead of minimizing the task inefficacy.

We refer to a resource matrix $\boldsymbol{\Gamma}_{r}$ as a feasible resource matrix, if $\boldsymbol{\Gamma}_{r}^{T}\mathbf{1}^{n}>\mathbf{1}^{r}$, where $>$ is applied elementwise. We term a resource matrix where $\boldsymbol{\Gamma}_{r}^{T}\mathbf{1}^{n}\ngtr\mathbf{1}^{r}$ as infeasible. Within our framework, any heterogeneous robot team having an infeasible resource matrix lacks the ability to perform the overall task. In this paper, a resource failure is a function that consumes a resource matrix (excluding the $n\times r$ matrix of zeros) and generates a resource matrix by setting a random nonzero entry of the consumed resource matrix to zero. A tolerable resource failure maps a feasible resource matrix to another feasible resource matrix, while a catastrophic resource failure maps a feasible resource matrix to an infeasible resource matrix. Since a catastrophic resource failure renders the team incapable of performing the task, we consider resilience only in the context of tolerable resource failures.

Consider a sequence of countably infinite resource failures $\mathcal{F}=[f_{1},f_{2},\cdots f_{\infty}]$ acting on a feasible resource matrix $\boldsymbol{\Gamma}_{r}$ sequentially. Now, let $\mathcal{F}_{n_{f}}=[f_{1},f_{2},\cdots,f_{k},\cdots,f_{n_{f}}]$ be the first $n_{f}\in\mathbb{Z}^{+}$ tolerable resource failures in $\mathcal{F}$, such that $f_{n_{f}+1}\in\mathcal{F}$ is a catastrophic failure. The $k^{th}$ tolerable resource failure in $\mathcal{F}_{n_{f}}$ is denoted as $f_{k}$. We denote $\boldsymbol{\Gamma}_{r}[k]$ as the resultant resource matrix after first $k$ tolerable resource failures in $\mathcal{F}_{n_{f}}$ acted on $\boldsymbol{\Gamma}_{r}$. $\boldsymbol{\Gamma}_{r}[k]$ can be recursively defined as $\boldsymbol{\Gamma}_{r}[k]=f_{k}(\boldsymbol{\Gamma}_{r}[k-1]),\ \boldsymbol{\Gamma}_{r}[0]=\boldsymbol{\Gamma}_{r}$. We term $\mathcal{C}[k-1]$ as the configuration of the heterogeneous multi-robot team before $f_{k}$ occurred.

We formally define the problems addressed in this paper:

Problem III.1

Configuration generation: Given a tolerable resource failure $f_{k}$, an associated feasible resource matrix $\boldsymbol{\Gamma}_{r}[k]$ and a heterogeneous multi-robot team configuration $\mathcal{C}[k-1]$ find a new configuration $\mathcal{C}[k]$ such that,

$\mathcal{G}[k]$* is a connected graph,* 2. 2.

$\|n\cdot\mathbf{1}^{n\times r}-\mathbf{\bar{A}}[k]\boldsymbol{\Gamma}_{r}[k]\|_{*}<\|n\cdot\mathbf{1}^{n\times r}-\mathbf{\bar{A}}[k-1]\boldsymbol{\Gamma}_{r}[k]\|_{*}$** 3. 3.

$\|\mathbf{\bar{A}}[k]-\mathbf{\bar{A}}[k-1]\|_{F}^{2}\leq ne$, where $ne\in\mathbb{Z}^{+}$ is a user-chosen parameter specifying the number of edges to be modified in $\bar{A}[k-1]$ to produce $\bar{A}[k]$ and 4. 4.

the sum of distances between communicating robots is maximized.

Problem III.2

Formation synthesis: Given a heterogeneous multi-robot team configuration $\mathcal{C}[k]$, generate coordinates $X_{1}[k],\dots,X_{n}[k]\in\mathbb{R}^{3}$ for the robots that best realize the given configuration. We describe this problem more precisely in Section IV-B.

The condition of graph connectivity in Problem III.1 is required to ensure cooperative task performance of the heterogeneous multi-robot team. The second condition in Problem III.1 states that the new configuration should improve the task efficacy of the multi-robot system. The third condition ensures that graph topologies $\mathcal{G}[k-1]$ and $\mathcal{G}[k]$ differ from each other in at most $ne$ edges. Although the final condition does not aid in resilience, it is designed to cause robots to spread out in space (this useful e.g. for coverage). The following section describes our solution procedure for these two problems.

IV Procedure

We explain our methods for the solving Problem III.1 and Problem III.2 in this section. When a resource failure occurs, information about the failure is transmitted to a central controller. The central controller checks if the resource failure is tolerable or catastrophic. If catastrophic, it commands the robots to return to their base station as the team no longer retains the necessary resources to perform the assigned task. If the resource failure is tolerable, the central controller uses a two-step procedure to generate a new configuration and an associated set of robot coordinates which realizes the generated configuration in three-dimensional space. Figure 2 gives a schematic illustration of our multi-robot reconfiguration strategy in the event of a tolerable resource failure. We refer to these steps as configuration generation and formation synthesis. Specifically, configuration generation and formation synthesis steps are solutions to Problem III.1 and Problem III.2 respectively. The following subsections describe each step in further detail.

IV-A Configuration generation

We now describe our proposed solution to Problem III.1. We reformulate the problem as a mixed integer semi-definite program (MISDP) and then solve reformulated MISDP. MISDP formulations are extensively used by researchers for graph topology design problems [9, 10]. We first state our MISDP mathematically; in the following discussion we argue that it captures the facets of Problem III.1. A MISDP formulation of Problem III.1 can be written as @hyper@itemfalse

[TABLE]

The decision variable $\mathbf{L}$ is a weighted Laplacian of a graph that has the same topology as $\boldsymbol{\Pi}$ and $\bar{\mathbf{A}}[k]$, and whose weights will be used to compute the desired neighborhood distance matrix $\mathbf{D}[k]$. ${0<c_{min}<|\mathbf{L}_{i,j}|<c_{max}}$ gives the range of nonzero values of the off-diagonal entries of $\mathbf{L}$. Constraint 10 encodes the property of a graph Laplacian described in Equation 2. It is known [11, Proposition 1] that any undirected graph is connected if and only if $\mathbf{L}+\frac{1}{n}\mathbf{1}^{T}\mathbf{1}$ is a positive-definite matrix, therefore Constraint 11 ensures that the graph represented by $\mathbf{L}$ must be connected. Constraint 12 and Constraint 13 models $\boldsymbol{\Pi}$ as the closed adjacency matrix of a graph. Constraint 16 and Constraint 15 force $\mathbf{L}$ and $\boldsymbol{\Pi}$ to model the Laplacian matrix and closed adjacency matrix of graphs with same topology, respectively. In other words, the entries of $\mathbf{L}$ are nonzero if and only if the corresponding entries of $\boldsymbol{\Pi}$ are nonzero. Constraint 17 and Constraint 18 are a direct consequence of the conditions in Problem III.1

The optimization problem described in Equation 9-Equation 18 is a MISDP as it involves an integer matrix ($\boldsymbol{\Pi}$) and a semi-definite matrix ($\mathbf{L}$) as decision variables. Unlike the integer matrix (binary matrix) $\boldsymbol{\Pi}$, whose sole purpose is modeling the closed adjacency matrix of the underlying graph, the Laplacian matrix $\mathbf{L}$ serves two purposes. First, it gives a simple way to incorporate the connectivity constraint of the associated graph in the form of a linear matrix inequality [12]. Second, we use the off-diagonal entries of $\mathbf{L}$ to compute the neighborhood distance matrix $\mathbf{D}[k]$ associated with the new configuration $\mathcal{C}[k]$. The computation of neighborhood distance matrix $\mathbf{D}[k]$ based on a Laplacian matrix $\mathbf{L}$ is performed as follows:

[TABLE]

where $\kappa$ (a constant defined as $\kappa\coloneqq\frac{d_{s}-d_{mc}}{c_{max}-c_{min}}$, $d_{s}\in R_{>0}$) is the minimum safe distance between robots and ${d_{mc}\in\mathbb{R}_{>0}}$ is the minimum distance for non-communicating pairs, such that $d_{mc}>d_{s}$. From Equation 19 it is apparent that the distances between communicating pairs of robots increase as the absolute value of the corresponding Laplacian matrix entries decrease. As a result of Equation 3, maximizing the sum of inter-robot distances between communicating robots can be achieved by minimizing the trace of $\mathbf{L}$. Finally, the constraint described in Equation 14 implies that robots with fewer neighbors should be close to its neighbors. This enables the robots with weak connectivity to remain connected with the communication network, even in presence of small external disturbances (e.g. wind). Thus, solving the MISDP described in this section in turn solves Problem III.1, thereby generating a new configuration $\mathcal{C}[k]=(\mathcal{G}(\mathbf{\bar{A}[k]}=\boldsymbol{\Pi})[k],\mathbf{D}[k],\boldsymbol{\Gamma}_{r}[k])$ for the team. The neighborhood distance matrix $\mathbf{D}[k]$ forms the input to the formation synthesis stage (along with the current formation coordinates to serve as an initial guess in the optimization procedure).

IV-B Formation synthesis

In this subsection, we introduce a procedure to assign a physical location to each robot based on the desired distances $\mathbf{D}$ specified from the procedure of Section IV-A. We drop the resource failure index $[k]$ for brevity. The primary objective is to render $\{X_{i}\}_{i=1}^{n}$ such that they match desired inter-robot distances obtained from the previous step.

It is possible that the desired $d_{i,j}$ impose a geometrically impossible set of pairwise distances. To account for this possibility, we introduce the constraint that each pairwise distance must exceed $d_{mc}$ to ensure that no two robots collide. Additionally, to demonstrate our method in an indoor experimental space, a bounding volume constraint on the whole formation is needed. These considerations produce a constrained optimization problem:

@hyper@itemfalse

[TABLE]

where $B^{\min},\ B^{\max}\in\mathbb{R}^{3}$ are the minimum and maximum extents of an axis-aligned bounding box, with the operator $\leq$ applied elementwise in 23.

The unconstrained version of problem 20 is similar to the graph drawing [13] and multidimensional scaling [14] problems. Graph-drawing variants with constraints similar to 21–23 are considered in [15, 16]. In fact, the feasibility problem composed ]only of the constraints 21 and 23 is a type of sphere-packing problem, a class of problems recognized as highly difficult for centuries. The two-dimensional version was shown to be NP-complete by [17], while some other packing variants are not even established as members of NP. Some interesting reviews of packing problems are found in [18].

Consequently we cannot hope to optimize 20-23 exactly, but in practice, randomized algorithms perform well. Here, we use a simulated annealing approach with constraints approximated by penalty functions.

IV-B1 Simulated annealing

Simulated annealing (SA) is a well-known stochastic global optimization algorithm that can escape poor local minima by occasionally taking random steps that cause an increase in the minimization objective. We propose to use SA for the optimization problem 20 because the set of feasible solutions to sphere-packing problems do not form connected sets in general. Thus, strictly local optimization methods are not appropriate. In this section we briefly recap the algorithmic framework and specify our objective and algorithm hyperparameters for applying SA to solve 20. For additional details on SA refer to [19, 20].

We recap the algorithmic framework of SA in Algorithm 1. $\textsc{Propose}(x)$ generates a new value $x^{\prime}$ in the neighborhood of $x$, $E(x)$ is the objective or energy function, and $T$ is a temperature controlling the probability of taking a step that increases the minimization objective, with $\textsc{Cooling}(T)$ implementing a cooling schedule such that $T$ decreases over time. The implementation of $\textsc{Propose}(x)$ in Algorithm 1 is a generic one applicable to many problems with continuous decision variables. We use exponential cooling ($\textsc{Cooling}(T)=\gamma T$ for $0\ll\gamma<1$), and our stopping criterion is a fixed number of iterations. These are straightforward choices that yielded good solutions in our experiments.

We capture the constraints in the energy function $E(x)$ by adding penalty functions: @hyper@itemfalse

[TABLE]

where $p_{H}$ denotes a penalty function satisfying the property

[TABLE]

The “hardness” parameter $H>0$ increases over iterations, analogous to the decay of $T$. Here we use the exponential penalty $p_{H}(y)=e^{Hy}$. While the literature on penalty function methods typically includes the property that $p_{H}(y)=0$ within the feasible set [22], for example $p_{H}(y)=\max\{0,Ty^{3}\}$, we observed solutions with values of 20 closer to optimal using an exponential penalty. We leave further investigation into this for the future. We emphasize that the method of penalty functions does not guarantee that the solution will be feasible according to the hard constraints 21–23. It is therefore necessary to perform a final feasibility check on the SA output after termination, and possibly restart or fall back on a more computationally expensive exact algorithm. In our experiments, we run SA for 20,000 steps. We choose $\gamma$ such that $T$ decays from $1$ to $10^{-8}$, and the growth constant for $H$ such that $H$ increases from $1$ to $10^{3}$. We let $\delta_{\max}=d_{s}/10$. Note that tunings of $T$ and $H$ are sensitive to the overall scale of the distances $\mathbf{D},\ d_{s},\ d_{mc}$ involved in the problem ($\approx$1\text{,}\mathrm{m}$$ in our experiments).

IV-C Formation change motion planning

To physically realize the formation change induced by a resource failure, we must ensure that the robots can move between formations without collisions. We employ the method of [21] to plan a set of piecewise polynomial trajectories that transition the robots from configuration $X[k]$ to $X[k+1]$ safely. This method is appropriate for quadrotors in three-dimensional environments with obstacles. We emphasize that other multi-robot motion planning algorithms can be used as needed depending on the type of robot, the task, environment map representation, and so on.

V Simulation Results

We conducted various simulation experiments to validate our approach. For each simulation experiment, the MISDP problem ( Section IV-A) was solved using MATLAB with YALMIP [23] as the optimization problem modeling interface and SeDuMi [24] as the semi-definite programming solver. The simulated annealing technique for formation synthesis (Section IV-B) was implemented in MATLAB. All simulation and experimental computations were performed on a 64-bit Ubuntu 16.04 desktop with 3.30GHz Intel Core i7-5820K CPU and 32 GB RAM. Parameters used for the simulations were closely matched with corresponding ones used for the tests with physical robots described in Section VI. In addition, we choose $c_{min}=d_{s}$, $c_{max}=d_{mc}$ and $ne=2$.

As an illustration, Figure 3 depicts screenshots from one simulation trial showing various resource failure instances and the corresponding reconfigurations. This trial was conducted using seven simulated quadrotors, each having three resources at the start of the experiment. As shown in Figure 3(a), the quadrotors’ initial positions are predefined such that their communication graph has a line graph topology. A total of $19$ resource failures were simulated before the resource matrix of the team became infeasible. A 3-bit RGB color code is used to represent resources present in each quadrotor. For example, a white color indicates all three resources (RGB for white is $(1,1,1)$). Similarly, a quadrotor rendered in yellow (RGB $(1,1,0)$) represents the case where the quadrotor possesses all the resources except for the third. A quadrotor rendered in black denotes that it has lost all its resources pertinent to the team task. The multimedia attachment submitted with this manuscript shows the video of this simulation.

In addition, we compared our approach against a strategy in which the robot that suffered from a resource failure connects itself to another random non-communicating robot. We term this the random edge strategy. For comparison, we randomly generated connected graphs with $n$ between $3$ and $30$. A feasible resource matrix with $r$ between $3$ and $20$ was constructed based on a predefined resource percentage $p_{r}$ for each randomly generated graph. For a given $p_{r}$, we construct a feasible resource matrix by randomly selecting $\left\lceil\frac{p_{r}*n*r}{100}\right\rceil$ unique locations from $\mathbf{0}^{n\times r}$ and setting them to $1$, such that, the resultant resource matrix is feasible. We define the edge density of a graph with $n$ vertices as the ratio of the number of edges in the graph to the number of edges in a full connected graph with $n$ vertices. We denote $\Delta_{\mathbf{V}}$ as the difference in task efficacy of the new configuration resulting from the random edge addition strategy and our configuration generation strategy. We generated $1000$ random connected graphs and $1000$ feasible resource matrices, for each $p_{r}\in\{20,50,80\}$. Figure 4 depicts the results from these simulations.

To generate the plots in Figure 4, we divide the interval (0,1] into $50$ bins of equal size. The mean $\Delta_{\mathbf{V}}$ of graphs belonging to each bin were computed. The mean $\Delta_{\mathbf{V}}$ of each bin versus the midpoint of the bin interval were plotted. $\Delta_{\mathbf{V}}>0$ implies that our approach has increased the task efficacy more than the random edge strategy. Our strategy is designed to improve task efficacy of the team, so it is bound to improve this quantity for any configuration of interest. However, from Equation 7 it is also clear that adding a random edge can not hurt task efficacy. Since our MISDP uses constraints to enforce that the task efficacy must be improved but does not explicitly maximize the improvement, it is worthwhile to examine how much is gained in comparison with the random edge strategy. From the scatter plot Figure 4(a), since most points lie above zero, we conclude that our strategy increases the task efficacy of the multi-robot team considerably in comparison to the random edge strategy, even when the resource percentage is low. Again in Figure 4(b), we observe a similar trend in the plot with $p_{r}=50\%$. Finally, in Figure 4(c), we do not see a significant difference between the number of points that lie above and below zero. This is expected behavior, since when resource availability in the system is high, the effect of topology of the communication graph on the task efficacy of the multi-robot team is reduced.

We also performed a set of simulations comparing our strategy with a “hindsight optimized strategy” which uses information from an oracle about all future failures to compute the new configuration. To formalize this idea, we define a new metric hindsight inefficacy($\|\cdot\|^{\sim}$): $\|\mathbf{\bar{A}}[k]\|^{\sim}=\|n\mathbf{1}^{n\times r}-\mathbf{\bar{A}}[k]\Gamma_{r}[k]\|_{*}+\sum_{\tau=k+1}^{h}\|n\mathbf{1}^{n\times r}-\mathbf{\bar{A}}[k]\Gamma_{r}[\tau]\|_{*}$ . The hindsight inefficacy matrix quantifies task inefficacy of a given configuration, if feasible resource matrices associated with a future tolerable resource failure sequence are obtained through an oracle($[\Gamma_{r}[k],\Gamma_{r}[k+1],\cdots,\Gamma_{r}[h]]$). For comparison of both strategies, we performed $30$ tolerable failure sequence simulation trials each with $n\in\{5,10,20\}$. Each trial was initialized with a line graph and $\mathbf{1}^{n\times 6}$ as the initial communication graph and initial resource matrix respectively. We computed a new configuration for each failure in a tolerable failure sequence using both our strategy and the hindsight optimized strategy. The hindsight optimized strategy is same as our MISDP problem without the final constraint (Equation 18), except that it minimizes the hindsight inefficacy instead of the trace of $\mathbf{L}$. Moreover, to quantify the worst case performance, we plotted (Figure 5) the minimum hindsight inefficacy of the hindsight optimized configuration and the maximum hindsight inefficacy of the configuration computed from our strategy over all $30$ trials. Figure 5, illustrates that the performance of our method matches closely with the hindsight optimized strategy as $n$ increases.

VI Experimental Results

We additionally demonstrate our approach using the Crazyswarm multi-quadrotor experimental platform [25] with a team of seven small quadrotors. We execute the same sequence shown in the simulator screenshots in Figure 3. This illustrates that the formation synthesis and trajectory planning stages of our method produce results that satisfy the collision and kinodynamic constraints of a multi-quadrotor team in the real world. In our physical setup, due to programming language interoperability issues we precomputed the sequence of configurations, formations, and multi-robot trajectory plans. However the running time of our method is fast enough for real-time use: in our example problem with seven robots, the average computation time was $0.66\text{\,}\mathrm{s}$ for configuration generation, $2.43\text{\,}\mathrm{s}$ for formation generation, and $0.15\text{\,}\mathrm{s}$ for motion planning, resulting in a total of $3.24\text{\,}\mathrm{s}$. We note that our implementation of simulated annealing neglects opportunities for performance optimization in evaluating $E(x)$ after updating a single coordinate, and we believe a well-tuned implementation could be significantly faster.

VII Conclusions

We described a novel method that enables a heterogeneous team of robots performing a task to reconfigure themselves to a new formation in the event of a robot resource failure. The new formation reduces the impact of resource failure in the team by shifting the robots to a new configuration such that the resultant configuration is closer to an ideal configuration without adding significant communication cost (number of edges) to the network. Our method has two parts. The first (posed as a MISDP) generates the new configuration in the form of a communication graph and the inter-robot distance between the communicating robots. The second part, (optimized with simulated annealing) computes the robots’ spatial coordinates to realize the configuration generated in the previous part in 3D. We validated our strategy through simulations and demonstrated it on multiple quadrotors. Future work involves incorporating explicit models of resources, namely sensing models, actuation models, and internal computation models, into the strategy and decentralizing the strategy.