I Introduction and Related work
Target tracking using a robot team is of interest in many civilian and military applications. Consequently, this has led to a burgeoning interest in studying target tracking problems using robots [1, 2, 3, 4]
. Even though each robot has a limited view of the environment, multiple robots could perform the target tracking task collaboratively by exchanging information among themselves. A necessary condition for this to succeed is the existence of an underlying communication network. Collaborative distributed tracking avoids the need for a central data fusion center which combines the data collected on the robots to estimate the state of the target. A common approach for multi-robot target tracking in a distributed manner is through a distributed Kalman filter. We follow this method in this paper.
We envision a scenario in which a team of robots tracks and estimates the state of a target while running a distributed Kalman filter on board. For simplicity we assume there is a single sensor (e.g. a camera) on each robot that is used to obtain target information. The robot team is monitored by a base station which intervenes in the team’s activities only if a robot in the team experiences a deterioration in sensing quality. We use the trace of the sensor’s measurement noise covariance matrix as the measure of sensing quality. Our focus is on the strategies to mitigate the effect of a sensor quality deterioration on a robot (and hence on the team’s tracking performance) by appropriately modifying the topology of the underlying communication network and target state measurement fusion weights (Figure Figure 1). In a control theoretic sense, the state estimation error of the DKF depends upon the observability of the underlying communication network . Understanding and quantifying the effect of network topology on a network’s observability properties [6, 7, 8] is well studied. These findings form the basis of our proposed strategy to employ communication network topology reconfiguration to mitigate the effect of sensor quality deterioration in target tracking performance. Although, we restrict ourselves to single target tracking in this paper, our technique can be applied to multi-target tracking assuming that robots can identify and discern individual targets.
, we propose a two-stage strategy to alleviate the impact of a robot’s sensor quality deterioration on the team’s performance. Our framework is in two stages. The first stage updates the team’s communication network topology such that the new topology is adjacent to the original. It also computes an optimal set of new measurement fusion weights. Specifically, we optimizes a monotonic function of the trace of the covariance matrix of the target state estimation error. The second stage generates a set of coordinates implementing communication network topology while maximizing the coverage of the robots over the target’s domain. The framework considered here only accounts for sensor quality deterioration and not for the situation where the sensor fails completely. However, a near-complete sensor failure can be modeled as a sensor with very high measurement noise covariance. We also assume that the sensor measurement noise is always characterized by a zero mean probability distribution. Finally, we assume that robots can estimate their sensor quality. Since there exist techniques for sensor fault detection and degradation estimation [11, 12], this assumption does not impose undue restrictions on the applicability of our method.
Stage one of our method uses mixed integer semi-definite programs (MISDPs) to formulate and solve the problem of fabricating a communication graph topology and associated information fusion weights that improves the tracking performance of the team. We consider two MISDPs: agent-centric configuration generation (ACCG) and team-centric configuration generation (TCCG). The first MISDP(ACCG) maximizes the trace of the inverse of target state estimation error covariance matrix (TSEECM) associated with the robot that experienced the sensor quality degradation. The second one (team-centric) minimizes the average of the trace of TSEECM over all the robots. Our procedure in stage two solves an optimization problem to compute the coordinates of the robots that maximize the coverage of the robots over the target’s configuration space under communication constraints dictated by the graph topology computed in stage one. Although, resilience in multi-robot systems is well studied , the idea of resilience through reconfiguration during task execution is recent. This paper introduces the idea of resilience by reconfiguration into multi-robot target tracking.
Ii Notation and Preliminaries
For any positive integer , denotes the set .
denotes the standard Euclidean 2-norm and the induced 2-norm for vectors and matrices.is the Frobenius norm of the matrix . is the trace of matrix . and are the vector of ones and zeros with appropriate dimensions. We drop the superscripts whenever the dimensions of the vectors or matrices are clear from the context. We use to denote a vector of zeros with one as its entry. For any vector , is the matrix with along its diagonal. Conversely, yields the vector containing the diagonal elements of the matrix . or is the transpose of . denotes the space of symmetric positive semi-definite matrices. A weighted undirected graph with non negative edge weights is defined using the triplet , where is the weighted adjacency matrix of the graph. is the edge complement of . A matrix is doubly stochastic if its rows and columns sum to unity . Mathematically, and 15], Lemma 2.9).
Iii Problem Formulation
We consider a team of n robots whose labels belong to . The team is tasked with tracking a moving target of interest for a time period of epochs. It is assumed that the moving target is confined to a compact Euclidean space . We refer to the robot team that tracks this moving target as the tracker team and the robots as trackers. The robot with label is indicated as . Let denotes the triplet position vector of , then the set contains the positions of all trackers. Also, represents the position vector of the target. We assume that the trackers are equipped with localization capabilities which enable them to localize with reasonable accuracy in the environment. We consider the scenario where the tracker team performs distributed target tracking, thus trackers need to communicate among themselves during the tracking task.
Let the dynamic undirected graph model the communication network of the tracker team at the time step(). Note that we use time step, time and epoch interchangeably in this paper. The node set is isomorphic to the tracker team label set . An edge is included in the edge set if communicates with at time . We denote the communication range of trackers as . The neighbor set of node in is defined as . The interaction between nodes in a graph can also be represented using an unweighted adjacency matrix. The adjacency matrix of is denoted by .
Iii-a Distributed Kalman filter for tracking
We describe the distributed Kalman filter computation employed by the robots to collectively track a target maneuvering in an environment according to some known dynamics. The main advantage of employing a distributed (as opposed to a centralized) Kalman filter is that it eliminates the need for a central data fusion center [16, 2, 5, 17]. We follow the formulation in [18, 5]. The target dynamics are described by the standard linear state space equation
where and are the state and the input vectors of the target respectively. and are the state transition matrix and input matrix of appropriate dimensions respectively.
is the zero mean normally distributed random vector with the covariance matrix(). Each tracker can obtain measurements about the state of the target. The sensing region of is a disc of radius centered around the robot. The measurement model of in its sensing region is where and are the measurement vector and output matrix of , respectively. Also, is a zero mean Gaussian sensing noise vector with a covariance matrix modeling the sensor noise characteristics of .
The distributed Kalman filter algorithm consists primarily of two steps: 1) individual update and 2) consensus update. In the individual update step, the robots that receive a measurement estimate the state of the tracked agent using the standard Kalman filter equations. Mathematically, if received a measurement then it makes an estimate about the state of the tracked target using the following equations.
Local innovation or measurement update:
The local target state estimate of is fused with the local target state estimate of other trackers in the tracker team in a distributed fashion via a consensus protocol. This distributed information fusion is performed in the consensus step of the distributed Kalman filter. Each tracker in the tracker team computes the information vector and the associated information matrix prior to the consensus step. Once these quantities are computed, each robot in the tracker team exchanges information with its neighbors. Ultimately this results in consensus on a refined state estimate of the tracked target. The robots exchange information as follows:
where is the entry of a doubly stochastic matrix with the same structure as the unweighted adjacency matrix () of except for the diagonal elements. Specifically, is non-zero along its diagonal and its off-diagonal elements are non-zero if and only if the corresponding elements of are unity. In theory, information vectors and matrices of the robots converge to a common quantity only when tends to infinity. However, it is known that this consensus protocol has an exponential rate of convergence . As a result, a reasonable level of consensus on the information vector and matrix can be achieved by propagating Equation 8 and Equation 7 for a sufficient number of consensus steps . We note that, the consensus update is assumed to happen at a much faster time scale compared to Equation 1. Since the steps consensus process results in a fusion of all the measurements obtained by the team, the posteriori estimate of the target’s state vector and the posteriori estimation error covariance matrix can be determined using and respectively.
Iii-B Tracking under Sensor Quality Deterioration
As mentioned in Section I, we consider the problem of mitigating the effect of sensor quality deterioration on target tracking performance through appropriate reconfiguration of the tracker team. Next we give a precise definition of tracker team reconfiguration and sensor quality deterioration.
We term the tuple as the configuration of the tracker team at the time step and denote it by . is a doubly stochastic matrix whose elements are used to perform the information fusion computations outlined in Equation 8 and Equation 7 for the consensus step. During tracking operation for a time period , let detrimental events occur independently to random trackers in the tracker team. We assume that each event results in some sensor quality deterioration. At time , we say that ’s sensor quality is deteriorated if the trace of the measurement noise covariance matrix associated with the sensor has increased with respect . In other words, if , then sensor quality deteriorated at time . Recall that, we assume the sensor is unbiased even after its quality deteriorates. Consider a sequence set , where indicate the time step when the sensor fault occurred. We specify that is the configuration of the tracker team before the detrimental event occurred. We now formally define the problems studied in this paper. The first problem (Problem 1) deals with reconfiguration of the tracker team such that target tracking performance is optimal in some reasonable sense. The second problem addresses the issue of realizing the graph topology in 3D space while maximizing the tracker team’s coverage over the .
Configuration generation or reconfiguration: Given that experienced sensor quality deterioration at some time , is the sensor noise covariance matrix immediately after the deterioration event and is the tracker configuration prior to the event determine a new configuration such that,
is a connected graph,
, where is the number of edges that may be modified in to obtain and
tracking performance is optimized.
Formation synthesis: Given a tracker team configuration , generate coordinates that best realize the given configuration and maximize the tracker team’s coverage over , subject to constraints. We defer the details of this problem to Section IV-B.
Graph connectivity is an essential requirement for any distributed computation over a network and thus is enforced in Problem 1 . The second condition enables the user to control the communication load on the generated configuration by tuning the parameter . Finally, the third condition ensures good tracking performance.
We now describe our strategies for solving Problem 1 and Problem 2. A base station monitors the activities of the tracker team. Only when sensor deterioration occurs, the base station directs the tracker team to reconfigure to a new formation which it computes using the available information. Following the framework in our previous work , the base station uses a two-step procedure to compute a new configuration and generate a set of robot coordinates which realize the computed configuration. As in , we refer to these steps as configuration generation and formation synthesis. The base station decision making process is shown in Figure 2. The configuration generation and formation synthesis steps are solutions to Problem 1 and Problem 2.
Iv-a Configuration Generation
We propose two strategies for tackling the configuration generation problem. As mentioned earlier, we allude to these strategies as agent-centric configuration generation (ACCG) and team-centric configuration generation (TCCG). Both these strategies amount to solving a mixed integer semi-definite program. Network topology design problems are often addressed by posing them as MISDPs [19, 20]. In the agent-centric configuration method, the idea is to find a new configuration such that the trace of the information matrix (inverse of TSEECM) associated with the deteriorated tracker in the tracker team is maximized. The team-centric method minimizes the average of the trace of TSEECMs associated with all the trackers. Since the optimization over the covariance matrices resulting from consensus steps is a very hard problem to solve, we use the TSEECM obtained after one step consensus to formulate and solve the MISDP for both strategies. Before we describe the details of the MISDP formulations, we state a theorem (see for a proof, omitted here for brevity) which serves as the basis for the connectivity inequality constraint in MISDPs.
If a graph containing self loops at every node is equipped with a weighted adjacency matrix which is doubly stochastic then any graph isomorphic to this graph with or without self loops is connected if and only if
The following MISDP models our agent-centric configuration generation approach:
The decision variables and model the doubly stochastic matrix used for the consensus protocol and the adjacency matrix of the generate configuration respectively. Constraint 11 and Constraint 15 to Constraint 18 ensures that is a doubly stochastic matrix that is structurally equivalent to . In the light of Theorem 1, Constraint 12 enforces the generated configuration to possess a connected graph. Finally, Constraint 19 encodes the second condition in Problem 1 into the MISDP. If represents the label of the robot whose sensor quality deteriorated at , then with some simple algebraic manipulation it is easy to see that Equation 10 is equal to or .
Consider the following MISDP formulation encoding the team-centric configuration generation strategy which minimizes , where .
Where is a block diagonal matrix with along its diagonal and results in the Kronecker product  between
and the identity matrix of the equal size.Constraint 21 is essentially Equation 7 for written compactly as a single equation for the whole tracker team. Therefore, should match the information matrix . The following lemma (see for a proof, omitted here for brevity) proves that minimizing Equation 20 minimizes .
The is an upper bound on
Iv-B Formation Synthesis
We now describe a procedure to assign a physical location to each robot which maximizes the non-overlapping coverage of the space. We also impose constraints so that connected robot pairs remain within communication distance of each other, and that the distance between all robot pairs exceeds to ensure that no two robots collide. An additional constraint is added to ensure that robots which were tracking the target at have the target within their sensing range after reconfiguration.
We define coverage of the space as the total area covered by all robot sensing regions minus the overlapping area covered by two or more robot sensing regions. This produces the following constrained optimization problem:
where is the radius of the circular field of vision of tracker, , and are the minimum and maximum extents of an axis-aligned bounding box, with the operator applied elementwise in Equation 25. is the labels of the subset of trackers tracking the target at .
We solve the formation synthesis optimization problem Equation 22 - Equation 26 following the approach described in  by simply updating the objective to Equation 22 and incorporating the new constraint Equation 26.
To validate our approach, we conducted multiple simulation experiments of a robot team tracking a target agent following a circular trajectory. We modeled the target trajectory as a Dubins car . In our simulation, the target’s state vector describes its position, orientation and velocity at each time step k. The parameters for the linearized discrete dynamics (Equation 1) of the target are
and input vector . We held and constant throughout the simulation. This resulted in the target following a circular path.
For the distributed Kalman filter, we initiate the same and for each robot in the tracker team. We used for the consensus step. Parameters chosen for the configuration generation and formation synthesis problems were , , , and , with a bounding box of , , and .
The target dynamics and distributed Kalman filter were implemented in Python. For the agent-centric approach, the MISDP problem was solved using Python with PICOS as the optimization problem modeling interface and MOSEK as the semi-definite programming solver. For the team-centric approach, the MISDP problem was solved in MATLAB with YALMIP as the modeling interface. In both approaches, the simulated annealing technique for formation synthesis was implemented in Python.
To simulate deteriorating sensor quality for a robot i, we modified its covariance matrix by adding a random positive definite matrix. We generated various deterioration event sequences for robot teams of where a random robot was chosen at every f time step of the simulation to experience sensor deterioration. Figure 3 shows an overhead view of a single simulation trial. The true trajectory of the target and the estimated trajectory of the target by each tracker in the team are shown. Figure 4 shows screenshots from a separate trial showing different deterioration instances before and after reconfiguration. A video of this trial is included in the supplementary material.
For comparison of the ACCG and TCCG, we simulated 20 deterioration event sequences for each size robot team.
We also compared both ACCG and TCCG against a greedy strategy in which a single edge is added connecting the robot whose sensing quality has deteriorated to the robot with lowest at the time of the deterioration event. Each trial was initialized with a line graph.
To quantify performance, we plotted the maximum and maximum estimation error of the robot team for each strategy at each time step over all trials. In our plots is the covariance matrix of a tracker after completing consensus steps of the distributed Kalman filter, or . This differs from , which we optimized in the MISDP formulations. Figure 5 shows the results of these simulations.
While the greedy strategy appears to perform best in the early steps of the simulation, TCCG outperforms both ACCG and greedy strategies for larger teams as the simulation progresses and sensor quality deteriorates. Interestingly, TCCG performed poorly even compared to the simple greedy approach for all team sizes.
To simulate a worst case scenario, we additionally ran simulations in which the same robot experienced sensor deterioration at each time step. Under these conditions, TCCG outperforms ACCG and the greedy strategy at most time steps, as seen in Figure 6, for all simulated team sizes.
We describe a novel method that enables a team of robots engaged in tracking a target to reconfigure themselves in response to deterioration in the sensing quality in one of the team members. The reconfigured team alleviates the effect of sensor quality deterioration on team performance. We proposed two methods (both based on optimizing MISDPs) to generate the new configuration. The methods (agent-centric) and the other method (team-centric) were validated in simulation and compared to each other. They were also compared to a greedy strategy. The team-centric approach outperforms both the agent-centric and greedy approaches. We are currently working on validating our approach on our multi-robot testbed . We are also extending our approach to handle multi-robot multi-target tracking scenarios where the number of targets are unknown.
-  K. Hausman, J. Müller, A. Hariharan, N. Ayanian, and G. S. Sukhatme, “Cooperative multi-robot control for target tracking with onboard sensing,” The International Journal of Robotics Research, vol. 34, no. 13, pp. 1660–1677, 2015.
-  R. Olfati-Saber and P. Jalalkamali, “Collaborative target tracking using distributed kalman filtering on mobile sensor networks,” in Proceedings of the 2011 American Control Conference. IEEE, 2011, pp. 1100–1105.
-  R. K. Williams and G. S. Sukhatme, “Observability in topology-constrained multi-robot target tracking,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), May 2015, pp. 1795–1801.
-  P. Dames, P. Tokekar, and V. Kumar, “Detecting, localizing, and tracking an unknown number of moving targets using a team of mobile robots,” The International Journal of Robotics Research, vol. 36, no. 13-14, pp. 1540–1553, 2017.
-  Q. Liu, Z. Wang, X. He, and D. Zhou, “On kalman-consensus filtering with random link failures over sensor networks,” IEEE Transactions on Automatic Control, vol. 63, no. 8, pp. 2701–2708, 2017.
-  F. Pasqualetti, S. Zampieri, and F. Bullo, “Controllability metrics, limitations and algorithms for complex networks,” in 2014 American Control Conference, June 2014, pp. 3287–3292.
-  D. Leitold, Á. Vathy-Fogarassy, and J. Abonyi, “Controllability and observability in complex networks–the effect of connection types,” Scientific reports, vol. 7, no. 1, p. 151, 2017.
-  R. K. Ramachandran and S. Berman, “The effect of communication topology on scalar field estimation by large networks with partially accessible measurements,” in 2017 American Control Conference (ACC), May 2017, pp. 3886–3893.
-  R. K. Ramachandran, J. A. Preiss, and G. S. Sukhatme, “Resilience by reconfiguration: Exploiting heterogeneity in robot teams,” in IEEE/RSJ Int’l. Conf. on Intelligent Robots and Systems (IROS), 2019, to appear. [Online]. Available: https://arxiv.org/abs/1903.04856
-  A. B. Sharma, L. Golubchik, and R. Govindan, “Sensor faults: Detection methods and prevalence in real-world datasets,” ACM Transactions on Sensor Networks (TOSN), vol. 6, no. 3, p. 23, 2010.
-  S. Arosh, S. Nayak, S. Duttagupta, et al., “Fitness function based sensor degradation estimation using h infinity filter,” Procedia Computer Science, vol. 58, pp. 172–177, 2015.
-  L. Jiang, D. Djurdjanovic, J. Ni, and J. Lee, “Sensor degradation detection in linear systems,” in Engineering Asset Management. Springer, 2006, pp. 1252–1260.
-  L. Guerrero-Bonilla, A. Prorok, and V. Kumar, “Formations for resilient robot teams,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 841–848, April 2017.
-  R. A. Horn and C. R. Johnson, Eds., Matrix Analysis. New York, NY, USA: Cambridge University Press, 1986.
-  F. Bullo, Lectures on Network Systems, 1st ed. Kindle Direct Publishing, 2019, with contributions by J. Cortes, F. Dorfler, and S. Martinez. [Online]. Available: http://motion.me.ucsb.edu/book-lns
-  Olfati-Saber, “Distributed kalman filtering for sensor networks,” in Decision and Control, 2007 46th IEEE Conference on, 2007.
-  F. Morbidi and G. L. Mariottini, “On active target tracking and cooperative localization for multiple aerial vehicles,” in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2011, pp. 2229–2234.
-  G. Battistelli, L. Chisci, G. Mugnai, A. Farina, and A. Graziano, “Consensus-based algorithms for distributed filtering,” in 2012 IEEE 51st IEEE Conference on Decision and Control (CDC). IEEE, 2012, pp. 794–799.
-  M. Rafiee and A. M. Bayen, “Optimal network topology design in multi-agent systems for efficient average consensus,” in 49th IEEE Conference on Decision and Control. IEEE, 2010, pp. 3877–3883.
-  D. Xue, A. Gusrialdi, and S. Hirche, “A distributed strategy for near-optimal network topology design,” in 21st International Symposium on Mathematical Theory of Networked and Systems (MTNS 2014), 2014.
-  S. M. LaValle, Planning Algorithms. New York, NY, USA: Cambridge University Press, 2006.
-  J. A. Preiss*, W. Hönig*, G. S. Sukhatme, and N. Ayanian, “Crazyswarm: A large nano-quadcopter swarm,” in IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 3299–3304.