Optimal Estimation and Control of Large Collaborative Swarms using Random Finite Set Theory

Abstract

Controlling large swarms of robotic agents presents many challenges including, but not limited to, computational complexity due to a large number of agents, uncertainty in the functionality of each agent in the swarm, and uncertainty in the swarm's configuration. The contributions of this work is to form the Random Finite Set (RFS) control for large collaborative swarms, decentralize RFS control for individual agents, and form RFS control using other multi-agent RFS filters. The state representation of the large swarms with an unknown number of agents is generalized as an RFS where an RFS is a collection of agent states with no ordering between individual agents that can randomly change through time. The novelty of this idea is to generalize the notion of distance using RFS-based distance measures and "close-the-loop" between an estimating and controlling a swarm RFS. Specifically, multi-target estimation is determined using the Gaussian Mixture Probability Hypothesis Density (GM-PHD) filter which processes measurements from an unknown number of agents with defined spawn, birth, and death rates. RFS control is then compared for each distributional distance-based cost studied including the Cauchy-Schwarz, L2^2, and a modified L2^2 divergence using a model predictive control (MPC) based Quasi-Newton optimization. Next, RFS control and estimation is extended to MPC via iterative linear quadratic regulator (a variant of differential dynamic programming) for spacecraft swarms. The swarm is estimated in both cardinality (number of agents) and state using the GM-PHD filter which provides the estimates for RFS control. RFS control through ILQR approximates a quadratic value function from the distributional distance-based cost (i.e. the modified L2^2 divergence) to find an optimal control solution. This results in an implicit proof for RFS control of large collaborative swarms. The RFS control formulation assumes that the topology underlying the swarm control is complete and uses the complete graph in a centralized manner. To generalize the control topology in a localized or decentralized manner, sparse LQR is used to sparsify the RFS control gain matrix obtained using ILQR. This allows agents to use information of agents near each other (localized topology) or only the agent's own information (decentralized topology) to make a control decision. Sparsity and performance for decentralized RFS control are compared for different degrees of localization in feedback control gains which show that the stability and performance compared to centralized control do not degrade significantly in providing RFS control for large collaborative swarms. The GM-PHD filter is the most basic RFS-based filters used for estimation. Other RFS-based filters can improve the estimate or provide additional tracking information for RFS control by using either the Cardinalized Probability Hypothesis Density (CPHD) filter or the Generalized labeled Multi-Bernoulli (GLMB) filter, respectively. The CPHD filter generalizes the GM-PHD filter by jointly propagating a generalized cardinality distribution as well as the RFS to produce better estimates at high cardinality. The GLMB filter incorporates labels into the RFS, thus the GLMB filter is able to track individual trajectories of agents through time. Both these filters are propagated in feedback with RFS control for the spacecraft relative motion problem. Specifically, the MPC-based ILQR is implemented to provide swarm control in a centralized manner. By using the CPHD and GLMB filters, the cardinality and state estimates become more accurate for RFS control for large collaborative swarms.