# CONCERTS: Coverage Competency-Based Target Search for Heterogeneous Robot Teams
## 4 Methods
### Path Selection  
After constructing and assigning a cluster of waypoints to each robot, we solve an instance of the Traveling Salesman Problem (TSP) separately for each agent in parallel using the Genetic Algorithm for solving TSP [14]. The TSP provides the optimal waypoint visitation order with a starting and ending point at the cluster's centroid (i.e., the robot's initial position). This optimal visitation order, however, is computed for each agent inde pendently and therefore does not consider the effects of the team. Instead of simply using the TSP solution for each agent, we consider that there are two possible directions along the solution. These two directions are the sample paths we supply to the information-theoretic utility function (see Figure 2). This approach provides better samples to the utility function than may be generated by traditional or frontier-based methods. Next, the utility function considers the sample paths for each team member sequentially and therefore ensures that the team will choose paths that minimize overlap and maximize the new area searched. By combining the TSP with the information-theoretic utility function, we have optimized each agent's individual path within their assigned cluster while also allowing the final path selection to consider the full team effort.  
We use an A* planner to generate obstacle-free paths and use B-spline to obtain the spline function of it (parameterization). Then we re-parameterize them with respect to the agent's moving speed using a set of sampled points along the path. This re-parameterization normalizes the process and ensures that the horizon is equivalent for each robot, regardless of movement speed and FoV size. Using this approach, we compute the information gain, denoted as \(IG(s)\), with the following equation:  
\[\begin{split} IG(s)&\approx\sum_{i=1}^{N_{\text{s} }}H(FOV(s^{i}))\\ &=\sum_{i=1}^{N_{\text{s}}}\left[\sum_{j=1}^{N_{\text{s}}}\left[m _{i,j}log(m_{i,j})+(1-m_{i,j})log(1-m_{i,j})\right]\right]\end{split} \tag{8}\]  
where \(s^{i}\) denotes the \(i\)-th sampled point and \(m\) is a random variable representing occupancy, while \(N_{\text{s}}\) and \(N_{\text{g}}\) are the number of sampling points and the number of grid 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_{i}|y^{1},y^{2},\dots,y^{n})]=\sum_{i}^{n}(IG(\text{s}_ {i})-c(\text{s}_{i})) \tag{9}\]  
where \(c(s_{i})\) denotes the traveling cost (traveling distance) to move along the path \(s_{i}\).  
Figure 2: Sampling paths along the TSP trajectory to calculate the Expected information gain. The three agents each have two candidate paths, A and B. The stars represent robot initial position, shaded rectangles denote the FoV of each agent at points along the path and the arrows represent the robot path from its initial position. If Agent 1 (red) selects path B, then Agent 2 (blue) will choose path A to reduce overlap and increase \(IG\). It can be seen that Agent 2 in blue has a larger FoV and a higher coverage competency, and therefore covers a larger region than Agent 3 in green.