# KC-TSS: An Algorithm for Heterogeneous Robot Teams Performing Resilient
Target Search

Minkyu Kim1,2, Ryan Gupta1,3, and Luis Sentis1,3 The authors are with the
1Human Centered Robotics Lab, the 2Department of Mechanical Engineering, and
the 3Department of Aerospace Engineering at the University of Texas at Austin

###### Abstract

This paper proposes KC-TSS: K-Clustered-Traveling Salesman Based Search, a
failure resilient path planning algorithm for heterogeneous robot teams
performing target search in human environments. We separate the sample path
generation problem into Heterogeneous Clustering and multiple Traveling
Salesman Problems. This allows us to provide high quality candidate paths
(i.e. minimal backtracking, overlap) to an Information-Theoretic utility
function for each agent. First, we generate waypoint candidates from map
knowledge and a target prediction model. All of these candidates are clustered
according to the number of agents and their ability to cover space, or
_coverage competency_. Each agent solves a Traveling Salesman Problem (TSP)
instance over their assigned cluster and then candidates are fed to a utility
function for path selection. We perform extensive Gazebo simulations and
preliminary deployment of real robots in indoor search and simulated rescue
scenarios with static targets. We compare our proposed method against a state-
of-the-art algorithm and show that ours is able to outperform it in mission
time. Our method provides resilience in the event of single or multi teammate
failure by recomputing global team plans online.

##  I Introduction

This study attempts to solve the problem of target search with multiple
heterogeneous robots by generating informative global paths and executing
search plans. We interpret the target search problem as a variant of Coverage
Path Planning (CPP) or exploration in which a team must additionally detect a
missing or injured person or object in situations where mission speed is
critical. In traditional CPP a robot or a team of robots must find the optimal
path such that the sensor footprint, or FoV, passes over the entire region,
referred to as the search polygon. Exploration strategy is typically to find
informative next view-point or path using sampling based approaches. We take
heterogeneous to mean different sizes of Field of View (FoV) and movement
speeds. The target may be considered as static and may be given with some
prior target knowledge (e.g. a search and rescue mission searching near
buildings while looking for people) and we assume static map information is
known a priori. When the target is static the proposed search algorithm
simplifies to a CPP algorithm due to the uniform target prediction model over
unseen space in the basic problem setting.

Figure 1: Sampling paths along the TSP trajectory to calculate the Expected
Information Gain. The three agents each have two candidate paths, A and B.
Shaded rectangles denote the FoV of each agent at points along the path. If
Agent 1 (red) selects path B then Agent 2 (blue) will choose path A to reduce
overlap and increase $IG$.

CPP, exploration and target search are receiving significant attention from
the robotics community due to the relevance in many important real-world
scenarios including map building [1, 2, 3, 4], cleaning/covering indoor areas
[5, 6], security, surveillance and information gathering [7, 8, 9, 10],
reconnaissance or search and rescue [11, 12, 13].

The goal of the presented algorithm is to generate high quality search plans
for robot teams that attempt to utilize each team member to its full capacity.
We define a metric, _coverage competency_ , which captures each member of the
team’s ability to effectively cover ground during search. Our algorithm uses
coverage competency to assign a search region to each agent through
heterogeneous clustering (i.e. the spread of the waypoints reflects that
agent’s FoV size and movement speed). We also provide a new metric, Waypoint
Allocation Factor (WAF), for measuring how evenly the waypoints were divided
amongst agents based on their coverage competency score.

We demonstrate that our algorithm has proportional complexity in n and
validate with n=50 agents. Further, we demonstrate that our algorithm is
robust to failure of team members. If one or more agents fail, the remaining
member(s) will complete the search. Because the server has access to robot
plans over the entire search time via the TSP solution and current robot
state, when one or more teammates fail, a re-planning step is invoked and
nearby agents are assigned the failed members waypoints, as demonstrated in
Fig. 2.

Overall, we present an algorithm for heterogeneous robot teams capable of
handling dynamic and static targets. Our contributions are summarized as
follows:

  * •

In the static target case, our method is equivalent to CPP (uniform target
prediction) and we show it to outperform the state-of-the-art CPP method [7]
in mission completion time

  * •

We introduce a new parameter and metric for characterizing the success of
heterogeneous teaming. Namely, _coverage competency_ , for quantifying agents’
search skill and the metric, Waypoint Allocation Factor (WAF), for assessing
the effectiveness of this parameter

  * •

Our algorithm can generate online mission plans for n robotic agents and
monitors team progress to determine when re-planning is required.

  * •

We validate our framework using both extensive Gazebo simulations of multiple
robots and with a three robot team in a real-world environment

This paper is organized as follows. Section II discusses related works. In
section III we formally introduce our problem and describe our methods. In
section IV we experimentally validate our methods in simulation finally
section V concludes our work.

##  II Background and Related Work

For decades CPP has received great attention for its relevance in navigation
tasks. Early solutions offer an offline planner given a static map. A
traditional method is to decompose the coverage region into cells using
Boustrophedon decomposition [14]. We employ a similar cell decomposition to
generate the set of waypoints for complete coverage. A common strategy in this
form of the problem is to break down the map and perform simple back-and-forth
behaviors. One alternative to such behavior is to generate the set of points
to visit and solve a Traveling Salesman Problem [6, 11]. [15] instead find the
optimal order of visitation using mixed integer linear programming.

There are also a number of geometric approaches [5, 17], which typically give
global guarantees with respect to distance traveled during execution. However,
such methods often make assumptions about perfect omni-directional sensing.
Traditional methods for path planning are insufficient, however, when the goal
is to operate in an online fashion to handle robot failure or dynamic
environments.

Sampling based approaches are popular in mapping and exploration [2, 3, 4, 9,
18]. Such methods account for sensor noise and environment complexity. Early
work used frontiers [19] to explore spaces. Frontiers are the boundaries
between explored and unexplored regions of the search polygon. The critical
factor in information-theoretic based strategies is how to generate subsequent
observation paths; that is, to provide quality candidates to the utility
function. Early work [2] uses a laser range scanner to map a previously
unknown small environment. In [3, 4] they leverage a utility function based on
Cauchy-Schwartz quadratic mutual information to more efficiently generate
plans to map 3D spaces. [9] proposes three planning structures for information
gathering missions like signal monitoring. [18] focuses on mission speed with
MAVs by taking a hybrid approach to frontier and sampling based exploration
strategies.

[7] addresses the problem of scalable CPP by efficiently generating path plans
with up to 150 agents in non-convex areas. They propose the robot team
conducts auction and conflict resolution steps to determine the region of
space they will cover. We benchmark our method against this path planner,
which is state-of-the-art in terms of coverage completion time I. Our method
features agent autonomy for search execution, a target prediction model, and
automatic re-planning; however, when the target is static the path planning
algorithm is equivalently a CPP problem, and therefore we use [7] as a state-
of-the-art comparison in terms of mission time and computation time (see Table
I).

There are an abundance of real-time algorithms for multi-robot teams in
coverage and exploration tasks. Groups have emphasized resilience to robot
failure [20, 21, 22], management of energy [23, 24, 25] or communications [26,
27, 28] constraints, and heterogeneous teaming [29, 30]. [24] also considers
an information gathering mission, however their future work lists increasing
to n agent systems. [25] performs search and exploration with UAVs, which are
limited in both communication and battery life. They use a state machine to
help team members decide between exploring, meeting, sacrificing and relaying.
They leverage a frontier based method. In [26] they consider a model of
communication strength between the agents and a central control PC and
cleverly use serial connection of the robots to maximize their exploration
area. Instead of constraining the team to constant connectivity, [28] proposes
a method of periodic communications at fixed intervals to update the full
team. In [13] an algorithm for solving the Multi-robot Efficient Search Path
Planning (MESPP) problem for finding a non-adversarial moving target. This
method provides theoretical bounds on search performance however it only
scales up to five agents and does not provide resilience to robot failure.

Our method, KC-TSS, builds on previous work [30] where the team was not robust
to agent failure because it did not provide complete search plans at any given
moment. As a result of both this fact and the greedy planner, some of the
generated paths were overlapping and lacked efficiency. The additional
clustering and TSP steps allow us to provide better candidate paths to the
utility function and improve team cooperation.

Algorithm 1 Multi-Agent Search()

$M,\mathbb{\mu},SR$

Entropy Map, Robot Pose, Coverage Competency

$P^{*}=\\{p^{*}_{1},p^{*}_{2},\cdots,p^{*}_{N}\\}$ (A set of paths)

$\mathcal{W}\leftarrow ExtractionUnknownRegions()$

$\mathbb{C}=HeterogenousClustering(\mathcal{W})$

for $n\leftarrow 1$ to $N$ do $\triangleright$ for each cluser in $\mathbb{C}$

$r_{n}=TravelingSalesmanProblem(c_{n})$

end for

for $n\leftarrow 1$ to $N$ do $\triangleright$ for each agent

$\hat{p_{n}}=SamplingPaths(r_{n})$

for $k\leftarrow 1$ to $|\hat{p_{n}}|$ do

$U(p_{k})=IG(p_{k})-c(p_{k})$ $\triangleright$ Equation (7)

end for

$p^{*}=p_{k}\leftarrow\operatorname*{arg\,max}{U(p_{k})}$ $\triangleright$ Get
the best path

$P^{*}.append(p^{*})$

end for

return $P^{*}$

##  III Methods

In this section we define a cooperative multi-agent target search algorithm
for solving the problem setting described above. Given a search map (entropy
map), a search polygon, the number of robots and initial robot position, this
method first uses cell decomposition to generate a set of waypoints, which, if
visited, provide complete map coverage over the unknown (high entropy)
regions. Then we perform a weighted clustering over all points and assign sub-
regions (clusters) to each agent based on their coverage competency. The next
step solves instances of the single agent TSP in parallel to maintain the
computational efficiency needed for generating global paths online if re-
planning is necessary. The result is two high quality candidate paths for each
agent, that is, along both directions of the TSP solution (see Fig. 1). The
final step is for these candidates for each agent to be considered by the IG
based utility function (see Eq. 6). This IG is computed as entropy reduction
along the path, where entropy is modeled by the target prediction model and
unseen regions. Because of the computational efficiency and global knowledge
of this algorithm, re-planning is performed in event of agent failure or if
some agents finish their assigned waypoints.

###  III-A Overall Framework

Our search, presented in Algorithm 1:

  1. 1.

Update search map using Bayesian filtering,

  2. 2.

Use Algorithm 2 to Generate waypoints from map,

  3. 3.

Heterogeneous Clustering using _coverage competency_ , See Algorithm 3,

  4. 4.

Solve Traveling Salesman Problem for each agent,

  5. 5.

Select optimal path using information-theoretic approach

  6. 6.

If Re-plan conditions are met, repeat steps 2) - 5).

###  III-B Target Estimation: Bayesian Filtering

We use Bayesian Inference to recursively estimate target state $x$ through
sequential observations $y$. Bayesian inference is commonly used to estimate
target state in a probabilistic manner. This inference model aims to predict
the posterior distribution of target position at time $k$, namely, $p(x_{k})$.
Bayesian filtering uses a prediction stage and a correction stage with
incoming sensing information. Assuming that the prior distribution
$p(x_{k-1})$ is available at time $k-1$, the prediction step attempts to
estimate $P(x_{k}|y^{1:n}_{1:k-1})$ from previous observations as follows.

$p(x_{k}|y^{1:n}_{1:k-1})=\int
p(x_{k}|x_{k-1})p(x_{k-1}|y^{1:n}_{1:k-1})dx_{k-1},$ (1)

where $p(x_{k}|x_{k-1})$ is the target’s motion model based on a first order
Markov process. Then, when the measurement $y^{1:n}_{k}$ is available, the
estimated state can be updated as

$p(x_{k}|y^{1:n}_{k})=\frac{p(y^{1:n}_{k}|x_{k})p(x_{k}|y^{1:n}_{1:k-1})}{p(y^{1:n}_{k}|y^{1:n}_{1:k-1})}$
(2)

where $p(y^{1:n}_{k}|y^{1:n}_{1:k-1})=\int
p(y^{1:n}_{k}|x_{k})p(x_{k}|y^{1:n}_{k-1})dx_{k}$ and $p(y^{i:n}_{k}|x_{k})$
is a sensing model for multi agent system, which can also be decomposed to
each agent’s sensing model $p(y^{i}|x)$. For the correction stage, the
measurement of all agents are used to modify the prior estimate, leading to
the target belief. If a static target is assumed the target prediction can be
described as $p(x_{k}|x_{k-1})=\mathcal{N}(x_{k-1};x_{k},\Sigma)$, only
containing a noise term with the previous target state. If it is a target
motion model assumed to have some constant velocity, we can represent the
model as $p(x_{k}|x_{k-1})=\mathcal{N}(x_{k-1};x_{k}+V\Delta,\Sigma)$.

###  III-C Target Prediction

The action plan our algorithm generates incorporates information from the
target prediction model. The prediction model is made based on various prior
knowledge or experience. For example, in a rescue mission, it might be useful
to take advantage of the fact that people are more likely to be near collapsed
buildings.In this paper, those are called context $c$ and can be used to
estimate a target location. A Gaussian Mixture Model (GMM) can be constructed
using a finite number of contexts at time $k$ using the following equation

$\begin{split}p(x_{k}|c_{k})&=\sum^{N}_{i=1}p(x_{k}|c^{i}_{k})p(c^{i}_{k})\\\
&=\sum^{N}_{i=1}\pi_{i}G(x|\mu_{i},\Sigma_{i})\end{split}$ (3)

where $\pi_{i}$, $\mu_{i}$ and $\Sigma_{i}$ are a mixing coefficient, mean
vector, and covariance for $i$-th distribution, respectively. We use a
particle filter to implement this prediction model.

###  III-D Search Map

The search map is updated using sensor data of each robot at each instance.
The exploration begins under the assumption that the target exists in the
search region. We use a 2D occupancy grid map representation in which the
search region is discretized into cells. More details of the search map cells
are presented in [30]. A cell’s occupancy status is used to measure the
uncertainty of the target over the total search space (i.e the entropy map).
Entropy is defined here as

$H(M_{t})=-\sum_{i=1}^{N}(m^{i}_{t}\log(m^{i}_{t})+(1-m_{t}^{i})log(1-m_{t}^{i}))$
(4)

where $m^{i}_{t}$ is the occupancy variable at time step $t$ and $N$ denotes
the total number of cells. The search map is also maintained in this way.
Furthermore, it can be filtered such that the degree of entropy increases over
time in previously searched regions to account for a target prediction model.

Algorithm 2 ExtractionUnknownRegions()

$M,pos$ $\triangleright$ (Entropy Map, Current Pose)

$\mathcal{W}=\\{w_{1},w_{2},\cdots,w_{n}\\}$ NewUnknownSet

$queue_{m}\leftarrow\emptyset$

$\mathcal{W}\leftarrow\emptyset$

$\text{Initialize}\;Visited[M_{m=0.5}]=False$

$c_{0}\approx M_{m=0.5}$ take the unknown cell

$Enqueue(queue_{m},c_{0})$

$Visited[c_{0}]=True$

while $queue_{m}$ is not empty() do

$c_{0}\leftarrow DEQUEUE(queue_{m})$

for $n_{c}\leftarrow neighborhood(c_{0})$ do $\triangleright$ neighborcells

if $IsNewUnknown(n_{c})$ and $visited[cell]==False$ then

$w,Visited,l_{c}\leftarrow buildNewUnknowns(n_{c})$

$\mathcal{W}.append(w)$

$Enqueue(queue_{m},l_{c})$

else if $Visited[n_{c}]==False$ then

$Enqueue(queue_{m},n_{c})$

$Visited[n_{c}]=True$

end if

end for

end while

return $\mathcal{W}(\text{NewUnknownSet})$

###  III-E Waypoint Generation

Under the premise that we are given static map information a priori, the
search map (entropy map) can be used to identify unexplored areas. We generate
waypoints that divide these unexplored portions of the search polygon
according to the unit FoV size (i.e. those points which have a 0.5 value). The
resulting set of waypoints ensure complete coverage if visited. Our proposed
algorithm generates this set using sensor range as shown in Algorithm 2.

Algorithm 3 HeterogeneousClustering()

$\mathcal{W},\mathcal{\mu}_{1:n},SR_{1:n}=\\{\eta_{1},\dots,\eta_{n}\\}(\text{normalized})$

(Waypoints, Centroids(Robot Poses), Sensing Capabilities)

A Partition $\mathbb{C}=\\{C_{1},C_{2},\cdots,C_{n}\\}$

$cost^{-}=0$

$\text{Initiallize }\mathcal{\mu}$

repeat

$\hat{C_{i}}=\\{w_{j}:\eta_{i}d(w_{j},\mu_{i})\leq\eta_{h}d(w_{j},\mu_{h})\;\text{for
all }h=1,\dots,n\\}$

$\triangleright$ assign all datapoints to the nearest cluster

$\mu=\frac{1}{|C_{i}|}\sum_{w_{j}\in C_{i}}w_{j}$ $\triangleright$ update
centroids if required

$cost=\sum_{j=1}^{n}\sum_{w\in C_{j}}||\eta_{j}d(w,\mu_{j})||$

$\Delta cost=|cost-cost^{-}|$

if $\Delta cost<\epsilon$ then

$\mathbb{C}\leftarrow\hat{C}$

$break$

end if

$cost^{-}\leftarrow cost$

until $MAXLOOP$

return $\mathbb{C}$

Figure 2: Scenario with 6 agents. All paths are described in different colors
(a) Agent E (cyan) failure. (b) Re-planning step: Because the server has
access to all robot plans, the waypoints assigned to the agent E can be
reassigned to nearby agents. Ellipses represent the assignment of those
waypoints (c) KC-TSS guarantees complete coverage by solving a new TSP
instance.

###  III-F Heterogeneous Clustering

We view the waypoint assignment problem in this case as a clustering problem.
The K-means clustering method partitions a set of data into a predefined
number of $K$ subgroups so as to minimize the sum of the distance squared
between each data point and the centroid of assigned cluster. In the search
setting $K$ is equal to the number of team members. Unlike in the conventional
method, which is completely unsupervised learning to determine cluster
centers, we perform clustering based on the known position of the robots. When
computing the nearest neighbors based on robot location, we account for the
robot’s _coverage competency_.

Let $\mathcal{W}=\\{w_{1},w_{2},\cdots,w_{m}\\}$ where $w_{i}$ is $i$-th
waypoint extracted from search map (2) and let $C=\\{c_{1},\cdots,c_{k}\\}$ be
a set of $K$ centroids, which correspond to the location of the agents.
Suppose $K^{\prime}$ ($K^{\prime}\leq K$) centroids are given as a constraint.
The goal is to obtain the remaining number ($K-K^{\prime}$) of centroids and
to assign each point to the nearest centroid such that sum of the metric
$d_{j}(x_{1},x_{2})=\eta_{j}|x_{1}-x_{2}|$ is minimized as follows.

$\min_{c_{K^{\prime}+1},\cdots,c_{K}}\sum_{j=1}^{K}\sum_{w\in
C_{j}}||\eta_{j}d_{j}(w,c_{j})||$ (5)

where $\eta_{j}d_{j}(\cdot,\cdot)$ is the weighted distance, which reflects
the $j$-th robot’s sensing capability using the normalized coefficient
$\eta_{j}$. We define _coverage competency_ $\eta_{j}$ as
$\frac{SR_{min}}{SR_{j}}$ where $SR_{j}$ is the sensing capacity of $j$-th
agent, which is the product of a moving speed and sensing range of agent $j$.
Because we normalize this coefficient with $SR_{min}$, the minimum value of
sensing capacity among all agents, we can use it to consider relative
proximity between centroids and points. In this way we ensure that robots
which are fast or have a large FoV are allocated more waypoints than robots
which are not.

Furthermore, this formulation has the advantage of being flexible in varying
situations. When the robots are evenly distributed in the search region,
computed waypoints will easily be distributed among the agents. However, some
situations arise in which robots start next to one another. If two agents with
competency gaps begin next to each other, then all of the nearby points will
be assigned to the more competent teammate. To avoid this, we compute a new
centroid when any agent is assigned fewer than some minimum defined number of
points. If, on the other hand, two agents of equal skill begin next to each
other we compute the distance to other agents and determine which member is
given the new centroid and in addition those nearby agents are given a new
centroid. As a result of clustering, we receive the waypoints assigned to each
agent (centroid) in $\mathbb{C}$.

###  III-G Traveling Salesman Problem

After constructing and assigning a cluster to each robot we solve an instance
of the Traveling Salesman Problem (TSP) separately for each agent in parallel.
In this study we adopted the Genetic Algorithm for solving TSP [31]. In this
setting the start point is the cluster’s centroid (i.e. the robot’s starting
position). The result is the optimal route along the cluster. While this
generates a set of $k$ optimal paths when considered independently, the
solution does not consider the other agents. To overcome this lack of
consideration of the team’s combined effort, we generate a set of sample paths
for each agent then use the information-theoretic framework using the
aforementioned constraints. We exploit the utility function which maximizes
the acquisition of information for the full team in a limited time while
penalizing traveling costs. In this way we have provided higher quality
samples to the utility function when compared to traditional or frontier-based
methods.

Figure 3: Simulation results with different number of agents in different size
search regions. (a) 3 agent case (40$\times$40) (b) 10 agents (80$\times$80)
(c) 50 agents (200$\times$200)

###  III-H Path Selection

In this stage, our framework determines which direction the robots will travel
along the given route from TSP (See Fig. 1. Given the routes from TSP,
candidate paths are sampled using several points which are close to robot
positions. An A* planner is used to generate obstacle-free paths and all paths
are re-parameterized with respect to agent’s moving speed. Based on the number
of sampled points along a path, we compute the Information Gain, denoted
$IG(s)$, with the following equation:

$\begin{split}IG(s)\approx\sum_{i=1}^{N_{\text{s}}}H(FOV(\text{s}^{i}))\qquad\qquad\qquad\qquad\qquad\qquad\qquad\qquad\\\
=\sum_{i=1}^{N_{\text{s}}}\left[\sum_{j=1}^{N_{c}}\big{[}m_{i,j}log(m_{i,j})+(1-m_{i,j})log(1-m_{i,j})\big{]}\right]\qquad\quad\\\
\end{split}$ (6)

where $\text{s}^{i}$ denotes the $i$-th sampled point and $m$ is the occupancy
probability, while $N_{\text{s}}$ and $N_{c}$ are the number of sampling
points and the number of cells in the FOV given the sampled points,
respectively. Thus, The $IG(\text{s})$ is calculated by summing over the FOV
regions defined by sampled points through the path. The overall expected
utility, $\mathbb{E}$[$\tilde{U}$], is then computed for the full team as

$\mathbb{E}[\tilde{U}(x_{t}|y^{1},y^{2},\cdots,y^{n})]=\sum_{i}^{n}\left(IG(\text{s}_{i})-c(\text{s}_{i})\right)$
(7)

where $c(s_{i})$ denotes the traveling cost to move along the path $s_{i}$.

###  III-I Re-planning

In the event of robot failure or if some of the agents finish covering their
sub-region without finding the target, it is desirable to re-plan for the
team. When a preset percentage of the team covers their region we generate a
new set of waypoints over the full unexplored search map and perform the full
process again. Similarly, if a robot loses communication with the server or
fails to move for an extended period of time, we perform the same process of
resetting to generating waypoints ( step (2) in section III-A). We explicitly
define re-planning conditions.

  * •

Failure (stop operating or lost communication)

  * •

Another prediction prediction model applied

  * •

All way points assigned are visited by the corresponding agent

These re-planning conditions allow our team to be adaptable to changes in the
environment including robot failure. One re-planning scenario is shown in Fig.
2.

###  III-J WAF

We introduce a new metric called $WAF$, _Waypoint Allocation Factor_ to
evaluate the contribution of the task according to the coverage competency. In
order to compute how evenly the area coverage is allocated, we take the total
swept area of each agent and divide by the _coverage competency_.
Specifically, we apply the following equation,

$WAF=std\left(\lambda\frac{A(|r_{i}|)}{\eta_{i}}\right)$ (8)

where $A(|r_{i}|)$ denotes total swept area for agent $i$, $\eta_{i}$ is the
coverage competency, and $\lambda$ means a normalizing constant. A value close
to zero indicates that the waypoints were evenly distributed among the team
after competency considerations

###  III-K Computational Complexity

Achieving computational efficiency is critical for achieving online planning
as the number of agents increase. We analyze here the computational efficiency
of the proposed algorithm using the following parameters; Number of cells
$N_{g}$ (map size $/$ resolution), the number of trajectories to sample
$N_{t}$, the number of agents $k$, and other parameters for other algorithm.

In detail, the complexity of the waypoint generation algorithm 2 is
$O(N_{g})$. The clustering has a time complexity of $O(ikmd)$ where $i$
denotes the fixed number of iterations (max iteration), $k$ is the number of
clusters which is equal to the number of agents, number of $m$ data and $d$
dimension of the data. It can be computed as $\frac{N_{g}}{kN_{FOV}}$ and
represents the worst case of decomposition (i.e. that the search region is
totally unknown and divided by the product of number of agents and unit cell
size of the FOV). The solution of the TSP with the genetic algorithm is of
order $O(jn_{0}n^{2})$ where $j$ is number of outer iterations of genetic
algorithm and $n_{0}$ is the initial size of population, and $n$ is number of
locations. Sampling based path selection algorithm takes $O(N_{t}\log{N_{t}}$
complexity. Overall, the complexity of our algorithm is
$O(N_{g}+2ik\frac{N_{g}}{kN_{FOV}}+kjn_{0}n^{2}N_{t}\log{N_{t}})$.

##  IV Experiments and Results

###  IV-A Numerical Simulation Results

We present python-based simulation results of our proposed approach to
demonstrate its scalability. Agent state is represented as $s=[x,y,\theta]$
and has two control inputs $u=[v,\omega]$ as per the equations of motion for a
non-holonomic mobile agent. Each agent is equipped with a ray sensor which has
square-type of FoV with limited range. It is assumed that the simulation
environment (search region) and all the static obstacles have a rectangular
shape and obstacles can not be known in advance. To achieve robust collision
avoidance, we use dynamic window approach [32] to generate each agent’s
control input. We tested different initial conditions with varying number of
agents and we show some example scenarios in Fig 3.

Figure 4: Flowchart describing simulation implementation and interactions
between the search server and agents.

Figure 5: (a) Experimental validation for a three robot search in the
Aerospace Building 4th floor. In the top left figure, the yellow regions are
those which have been explored, while the white areas are regions of
uncertainty. The target location is described as a red box and is unknown to
the robots. The red, green and blue markers represent the clusters for each
agent. (b) The completion of the experimental validation with Agent 1
converging to the target of interest. The object recognition is performed
using the well-known YOLO algorithm to detect people (shown in the cyan box).

###  IV-B Comparative Results

We benchmark our method against a state-of-the-art algorithm with the results
shown in Table I. KC-TSS is shown to outperform the offline algorithm SCoPP
[7] when using the same search setting. That is the simulation trials were
performed in the python-based numerical simulation in a region of the same
size using the same number of agents which are given equal skill. We generate
this data using 13 agents in the same size rectangular search polygon as
presented by [7] and we generate this data from 10 comparative trials.

TABLE I: Comparative test Method | Completion Time (s)
---|---
Guastella’s | 3591.8 $\pm$ 0.00
SCoPP | 196.53 $\pm$ 14.53
KC-TSS (ours) | 180.22 $\pm$ 21.23

###  IV-C Effectiveness of Coverage Competency

In simulation we tested six cases. Because of the random distribution of robot
starting place, it will not approach zero because we cannot perfectly account
for coverage skill. However, our results suggest that this value is safely
maintained below 0.4 in all scenarios. It is natural that $WAF$ increases with
the number of agents due to the random initial conditions. The data from these
trials is in Fig. 6.

###  IV-D Resilience to Failure

One significant advantage of our framework is that it can quickly re-plan
navigation behavior when one or more team members fail via loss of
communication or navigational error (e.g. a computer loses internet
connection, a legged robots falls). When faced with one of these events, we
regenerate the set of waypoints over the unexplored region, cluster and
allocate them, and then continue the method as before. Fig. 2 demonstrates
this resilience to failure.

Figure 6: Waypoint Allocation Factor versus number of agents

###  IV-E Gazebo-ROS Simulation

We validate our framework’s ability to transfer to robotic systems in human
environments with high fidelity Gazebo simulations. The first is a 10mx20m
simulation of the Anna Hiss Gymnasium apartment, the second is a 40mx50m
region simulating the outside of a home environment (see Fig. 4 upper right
corner) and the final is a 100mx100m town that has been struck by natural
disaster. We validate the static target search capability in all three
environments. In the town map, the agents search for an injured person and we
use this information to guide the target prediction model by assuming the
injured person will be near a building as opposed to in open space. We also
demonstrate the resilience of the search method by performing re-planning
under agent failure. This paper’s accompanying video features the described
scenarios.

To validate the inclusion of coverage competency, we present II to compare
mission time for the agents when coverage competency is used versus when it is
not used. Additionally, for the trials using coverage competency we include
the $WAF$ score. This table uses teams of $n$ heterogeneous robots and is
computed over 10 trials on each map with varying initial conditions. In all
cases, it was confirmed that the search time was reduced and the WAF value was
also low. There is a trend that the WAF increases slightly as the size of the
map increases. The results clearly demonstrates that the addition of coverage
competency impacts the search speed of the team and that the search area was
more properly distributed.

TABLE II:  Search Time (s) with and without Coverage Competency Map (# robots) | w/o CC | WAF | w CC (proposed) | WAF
---|---|---|---|---
Apartment (2) | 124.8 $\pm$ 13.2 | 0.23 |  83.8 $\pm$ 22.4 | 0.07
Home Outdoor (3) | 72.8 $\pm$ 8.5 | 0.31 |  58.1 $\pm$ 12.4 | 0.12
Disaster (4) | 252.2 $\pm$ 21.2 | 0.37 |  189.8 $\pm$ 29.6 | 0.18

###  IV-F Real Robot Experiment

We implement search for a static target with a heterogeneous three robot team.
This preliminary experiment demonstrates that our proposed method is viable in
real world environments, however, execution of the search required human
intervention due to localization errors. The team is comprised of two Unitree
A1 quadrupeds with different sensor suites and a Toyota HSR. One A1 quadruped
is equipped with a Velodyne VLP-16 3D Lidar and a RealSense D435. The other is
equipped with a RPLidar A3 2D Lidar and a Realsense D435. Both quadrupeds have
Intel NUC Mini PCs onboard which communicate with the robot hardware. The HSR
is equipped with Hokuyo 2D Lidar, RGB-D camera sensing and has an on board
Jetson TK1. The central search server is run on a remote laptop and we use
Robofleet [33] for efficient communication between the server and agents. The
entire framework is implemented in ROS Melodic. At initial planning or a re-
planning step, the search server generates a set of n paths and then sends
those to the agents (see Fig. 4).

The search experiment is performed in the Aerospace Engineering building at
the University of Texas at Austin while looking for a static volleyball and
the total mission time is one minute 14 seconds. The experiment is depicted in
Fig 5.

##  V Concluding Remark

This paper addresses online search for a general heterogeneous multi agent
systems. The mission completion time of our method is compared with a state-
of-the-art algorithm and shown to outperform it. We further validate our
algorithm with extensive simulation results in Gazebo using ROS. Finally, we
demonstrate the efficacy of this proposed method in a real robot experiment.
Overall results validate the effectiveness and robustness of the proposed
algorithm. Several future works remain, however, current efforts are towards
including a target motion model for dynamic targets and performing continuous
coverage.

## References

  * [1] C. Cao, H. Zhu, H. Choset, and J. Zhang, “Tare: A hierarchical framework for efficiently exploring complex 3d environments,” in _Robotics: Science and Systems Conference (RSS), Virtual_ , 2021.
  * [2] F. Amigoni and V. Caglioti, “An information-based exploration strategy for environment mapping with mobile robots,” _Robotics and Autonomous Systems_ , vol. 58, no. 5, pp. 684–699, 2010.
  * [3] B. Charrow, G. Kahn, S. Patil, S. Liu, K. Goldberg, P. Abbeel, N. Michael, and V. Kumar, “Information-theoretic planning with trajectory optimization for dense 3d mapping.” in _Robotics: Science and Systems_ , vol. 11, 2015.
  * [4] B. Charrow, S. Liu, V. Kumar, and N. Michael, “Information-theoretic mapping using cauchy-schwarz quadratic mutual information,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2015, pp. 4791–4798.
  * [5] P. R. Tokekar, “Placement and motion planning algorithms for robotic sensing systems,” Ph.D. dissertation, University of Minnesota, 2014.
  * [6] R. Bähnemann, N. Lawrance, J. J. Chung, M. Pantic, R. Siegwart, and J. Nieto, “Revisiting boustrophedon coverage path planning as a generalized traveling salesman problem,” in _Field and Service Robotics_. Springer, 2021, pp. 277–290.
  * [7] L. Collins, P. Ghassemi, E. T. Esfahani, D. Doermann, K. Dantu, and S. Chowdhury, “Scalable coverage path planning of multi-robot teams for monitoring non-convex areas,” _arXiv preprint arXiv:2103.14709_ , 2021.
  * [8] S. McCammon and G. A. Hollinger, “Topological path planning for autonomous information gathering,” _Autonomous Robots_ , vol. 45, no. 6, pp. 821–842, 2021.
  * [9] G. A. Hollinger and G. S. Sukhatme, “Sampling-based robotic information gathering algorithms,” _The International Journal of Robotics Research_ , vol. 33, no. 9, pp. 1271–1287, 2014.
  * [10] V. Cavinato, T. Eppenberger, D. Youakim, R. Siegwart, and R. Dubé, “Dynamic-aware autonomous exploration in populated environments,” _arXiv preprint arXiv:2104.02696_ , 2021.
  * [11] R. Pěnička, M. Saska, C. Reymann, and S. Lacroix, “Reactive dubins traveling salesman problem for replanning of information gathering by uavs,” in _2017 European Conference on Mobile Robots (ECMR)_. IEEE, 2017, pp. 1–6.
  * [12] S. Kohlbrecher, F. Kunz, D. Koert, C. Rose, P. Manns, K. Daun, J. Schubert, A. Stumpf, and O. v. Stryk, “Towards highly reliable autonomy for urban search and rescue robots,” in _Robot Soccer World Cup_. Springer, 2014, pp. 118–129.
  * [13] G. Hollinger, S. Singh, J. Djugash, and A. Kehagias, “Efficient multi-robot search for a moving target,” _The International Journal of Robotics Research_ , vol. 28, no. 2, pp. 201–219, 2009.
  * [14] H. Choset, “Coverage of known spaces: The boustrophedon cellular decomposition,” _Autonomous Robots_ , vol. 9, no. 3, pp. 247–253, 2000.
  * [15] E. I. Grotli and T. A. Johansen, “Path planning for uavs under communication constraints using splat! and milp,” pp. 265–282, 2012.
  * [16] T. I. F. Erik J. Forsmo, Esten I. Grotli and T. A. Johansen, “Optimal search mission with unmanned aerial vehicles using mixed integer linear programming,” 2013.
  * [17] A. P. Premkumar, K. Yu, and P. Tokekar, “A geometric approach for multi-robot exploration in orthogonal polygons,” in _Algorithmic Foundations of Robotics XII_. Springer, Cham, 2020, pp. 896–911.
  * [18] A. Dai, S. Papatheodorou, N. Funk, D. Tzoumanikas, and S. Leutenegger, “Fast frontier-based information-driven autonomous exploration with an mav,” _arXiv preprint arXiv:2002.04440_ , 2020.
  * [19] B. Yamauchi, “A frontier-based approach for autonomous exploration,” in _Computational Intelligence in Robotics and Automation, 1997. CIRA’97., Proceedings., 1997 IEEE International Symposium on_. IEEE, 1997, pp. 146–151.
  * [20] L. Zhou, V. Tzoumas, G. J. Pappas, and P. Tokekar, “Resilient active target tracking with multiple robots,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 1, pp. 129–136, 2018.
  * [21] I. E. Rabban and P. Tokekar, “Improved resilient coverage maximization with multiple robots,” _arXiv e-prints_ , pp. arXiv–2007, 2020.
  * [22] M. Ishat-E-Rabban and P. Tokekar, “Failure-resilient coverage maximization with multiple robots,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 3894–3901, 2021.
  * [23] K. Yu, J. M. O’Kane, and P. Tokekar, “Coverage of an environment using energy-constrained unmanned aerial vehicles,” in _2019 international conference on robotics and automation (ICRA)_. IEEE, 2019, pp. 3259–3265.
  * [24] X. Cai, B. Schlotfeldt, K. Khosoussi, N. Atanasov, G. J. Pappas, and J. P. How, “Non-monotone energy-aware information gathering for heterogeneous robot teams,” _arXiv preprint arXiv:2101.11093_ , 2021.
  * [25] K. Cesare, R. Skeele, S.-H. Yoo, Y. Zhang, and G. Hollinger, “Multi-uav exploration with limited communication and battery,” in _2015 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2015, pp. 2230–2235.
  * [26] B. Woosley, P. Dasgupta, J. G. Rogers, and J. Twigg, “Multi-robot information driven path planning under communication constraints,” _Autonomous Robots_ , vol. 44, no. 5, pp. 721–737, 2020.
  * [27] G. Shi, I. E. Rabban, L. Zhou, and P. Tokekar, “Communication-aware multi-robot coordination with submodular maximization,” _arXiv preprint arXiv:2011.01476_ , 2020.
  * [28] G. A. Hollinger and S. Singh, “Multirobot coordination with periodic connectivity: Theory and experiments,” _IEEE Transactions on Robotics_ , vol. 28, no. 4, pp. 967–973, 2012.
  * [29] T. Sakamoto, S. Bonardi, and T. Kubota, “A routing framework for heterogeneous multi-robot teams in exploration tasks,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 4, pp. 6662–6669, 2020.
  * [30] M. Kim, R. Gupta, and L. Sentis, “Information-theoretic based target search with multiple agents,” _arXiv preprint arXiv:2107.12715_ , 2021.
  * [31] M. Sedighpour, M. Yousefikhoshbakht, and N. Mahmoodi Darani, “An effective genetic algorithm for solving the multiple traveling salesman problem,” _Journal of Optimization in Industrial Engineering_ , no. 8, pp. 73–79, 2012\.
  * [32] D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance,” _IEEE Robotics & Automation Magazine_, vol. 4, no. 1, pp. 23–33, 1997.
  * [33] K. S. Sikand, L. Zartman, S. Rabiee, and J. Biswas, “Robofleet: Open source communication and management for fleets of autonomous robots,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2021, pp. 406–412.


# Answer to the comment on “On the characteristic polynomial of an effective
Hamiltonian”

Yong Zheng<EMAIL_ADDRESS>School of Physics and Electronics, Qiannan
Normal University for Nationalities, Duyun 558000, China

###### Abstract

In a recent comment by Fernández, it has been argued that our solution method
of an effective Hamiltonian based on the characteristic polynomial [Phys.
Lett. A 443 (2022) 128215] had been developed several years earlier by Fried
and Ezra [J. Chem. Phys. 90 (1989) 6378]. We show here several important
differences between our treatment and the resummation method proposed
previously by Fried and Ezra.

###### keywords:

Characteristic polynomial , Effective Hamiltonian , Resummation method ,
Perturbation calculation

††journal: Physics Letters A

##  1 Introduction

Recently, Fernández has critically pointed out that our solution method of an
effective Hamiltonian based on the characteristic polynomial had been
developed several years earlier by Fried and Ezra in a clearer and more
general way [1]. The resummation method of Fried and Ezra based on the
reconstruction of the effective secular equation [2], which we were unaware of
in our previous study [3], does have some similarities with our method.
However, we show here that Fernández has ignored several important differences
between such two methods.

First of all, Fernández has ignored that: Unlike in Fried and Ezra’s
treatment, for our characteristic polynomial of the effective Hamiltonian
$H_{\mathrm{eff}}(\lambda)$,

$\det[E-H_{\mathrm{eff}}(\lambda)]=\prod_{n=1}^{N}[E-E_{n}^{{}_{P}}(\lambda)]=E^{N}-P_{1}(\lambda)E^{N-1}\\\
+\cdots+(-1)^{N-1}P_{N-1}(\lambda)E+(-1)^{N}P_{N}(\lambda),$ (1)

the coefficients are expressed with the symmetric polynomials of $P$-space
eigenvalues (reproduced here for the convenience of discussion),

$\displaystyle
P_{1}(\lambda)=E_{1}^{{}_{P}}(\lambda)+E_{2}^{{}_{P}}(\lambda)+\cdots+E_{N}^{{}_{P}}(\lambda),$
(2.1) $\displaystyle P_{2}(\lambda)=\sum_{1\leq j_{1}<j_{2}\leq
N}E_{j_{1}}^{{}_{P}}(\lambda)E_{j_{2}}^{{}_{P}}(\lambda),$ (2.2)
$\displaystyle\quad\cdots\cdots$ $\displaystyle
P\\!_{N}(\lambda)=E_{1}^{{}_{P}}(\lambda)E_{2}^{{}_{P}}(\lambda)\cdots
E_{N}^{{}_{P}}(\lambda).$ (2.N)

Introducing these symmetric polynomials and discussing their analytical
property in complex plane of $\lambda$ are one of the key contents in our
study. Actually, without such discussion, the better $\lambda$-expansion
property of the characteristic polynomial $\det[E-H_{\mathrm{eff}}(\lambda)]$
(the so-called secular determinant) in Fried and Ezra’s treatment can only be
inferred from the effective Hamiltonian itself [2], which had been thought to
be rapidly convergent in $\lambda$-expansion. However, as our study has shown,
the analytical properties of $\det[E-H_{\mathrm{eff}}(\lambda)]$ and
$H_{\mathrm{eff}}(\lambda)$ generally are different [3].

Additionally, since the analytical property of the characteristic polynomial
coefficients is unknown in Fried and Ezra’s treatment, the $\lambda$-expansion
can only be carried out by taking $\det[E-H_{\mathrm{eff}}(\lambda)]$ as a
whole, to obtain an approximate characteristic polynomial in a form as, say,
to the order of $\lambda^{K}$,

$\left\\{\prod_{n=1}^{N}\left[E-\sum_{j=0}^{K}E_{n,j}\lambda^{j}\right]\right\\}^{[K]}=E^{N}+\sum_{j=1}^{N}p_{j}(\lambda)E^{N-j},$
(3)

where we have used the symbol $\\{\cdots\\}^{[K]}$ introduced by Fernández,
which means that one has to remove any term with $\lambda^{k}$ if $k>K$ and
retain all the terms with $\lambda^{k}$ if $k\leq K$, to obtain a meaningful
expansion[1, 2]; namely, the coefficients here
$p_{j}(\lambda)\equiv\sum_{k=1}^{K}c_{jk}\lambda^{k}$. This is equivalent to
the expansion of all the $P_{j}(\lambda)$ in our Eq. (1) to the same
$\lambda$-order, i.e,
$P_{j}(\lambda)\approx(-1)^{j}p_{j}(\lambda)=(-1)^{j}\sum_{k=1}^{K}c_{jk}\lambda^{k}$.
From this sense, Fried and Ezra’s treatment seems to be equivalent to ours.
However, their treatment generally can result in energy eigenvalues with an
imaginary part, which are unphysical. This has already been noted by Fried and
Ezra[2].

The trouble of Fried and Ezra’s treatment can be easily understood with the
case of $N=2$, for which the energy eigenvalues $E_{1,2}(\lambda)$ can be
solved with the approximate characteristic polynomial in Eq. (3),

$E_{1,2}(\lambda)=\frac{-p_{1}(\lambda)\pm\sqrt{[p_{1}(\lambda)]^{2}-4p_{2}(\lambda)}}{2}.$
(4)

Since we generally can not ensure that $[p_{1}(\lambda)]^{2}\geq
4p_{2}(\lambda)$, one can expect that energy eigenvalues with an imaginary
part can be obtained when $[p_{1}(\lambda)]^{2}<4p_{2}(\lambda)$ (For example,
when $K=1$, this $\lambda$-inequality is a quadratic one, and can be easily
satisfied by certain $\lambda$ for a general Hamiltonian).

However, in our method, such trouble can be properly overcome. Our treatment
is directly based on the $\lambda$-expansion of the coefficients
$P_{j}(\lambda)$ via Eqs. (2.1)–(2.N), whose analytical property has been
proven. Hence, we can carry out the expansion independently and properly
retain terms to different orders of $\lambda$ for different $P_{j}(\lambda)$,
to avoid problems such as $[p_{1}(\lambda)]^{2}<4p_{2}(\lambda)$ in the $N=2$
case above which possibly appear in Fried and Ezra’s treatment. For example,
for a given value of $\lambda$, we in principle can perform the expansion for
each $P_{j}(\lambda)$ to some individual order, via requiring the additional
correction caused by the next order within a certain precision range we set.
Whether the expansion is directly performed for $P_{j}(\lambda)$ or
$\det[E-H_{\mathrm{eff}}(\lambda)]$ is an important difference between the two
treatments.

Fernández also has made some misleading statements in the comment [1]. For
example, Fried an Ezra’s method was described as a more general one just
because they have not obviously introduced the total state space dimension
$\tilde{N}$ as we have done. However, Fernández has not realized that we have
not made any restrictions on such $\tilde{N}$ in our study. Such $\tilde{N}$
can be very large, since the dimension we really need to deal with is $N$.
Actually, whether $N$ is large is more relevant. In fact, we have constructed
the effective Hamiltonian $\bar{H}_{\mathrm{eff}}(\lambda)$ (see [3]) using
the characteristic-polynomial coefficients $P_{n}(\lambda)$. Our
$\bar{H}_{\mathrm{eff}}(\lambda)$ has been proven to hold a convergence radius
same as $P_{n}(\lambda)$ for $\lambda$-expansion. Hence, one can alternatively
solve for the eigenvalues by diagonalizing $\bar{H}_{\mathrm{eff}}(\lambda)$
in a matrix manner when $N$ is large.

Especially, Fernández has argued that we had overlooked the application of the
method to estimate the exceptional point in the complex plane of $\lambda$
closest to origin. However, he has ignored that such application has already
been performed by us in the last paragraph of Section 3 in [3].

In conclusion, the two treatments of an effective Hamiltonian based on the
characteristic polynomial, the one proposed by Fried and Ezra in [2] and the
one by us [3], have important differences in specific procedures. The
$\lambda$-expansion of the coefficients $P_{j}(\lambda)$ is important and
should be performed carefully to avoid the unphysical complex-energy-
eigenvalue problem. Fernández has ignored these differences between the two
treatments and some misleading statements have been made in the comment [1].

## References

  * [1] F. M. Fernández, Phys. Lett. A 452 (2022) 128456.
  * [2] L. E. Fried and G. S. Ezra, J. Chem. Phys. 90 (1989) 6378.
  * [3] Y. Zheng, Phys. Lett. A 443 (2022) 128215.