Simulation Toolset for Localization and Control of Swarming Vehicles using Random Finite Set Theory
Vaughn A. Weirens, Chuck S. Hisamoto, Suneel I. Sheikh, ASTER Labs Inc.
Swarming vehicle applications represent a rapidly growing focal area in guidance, navigation, and control robotics systems research. To accurately simulate the behavior and performance of the individual swarm elements and the swarm collective in application-driven scenarios is highly complex and complicated due to the large number of objects, uncertainties in functionality and communication, and limited knowledge of how to accurately model the objects for a specific environment. Furthermore, it can be increasingly difficult when these problems are scaled, varying in size from swarms as small as two units, to thousands or tens of thousands of units. Additionally, communication methods cannot be assumed to be optimal or consistent amongst these various swarm elements, and can be limited not only be the system’s transmission capabilities, but also by the local environment through which the swarm moves or operates. Thus, a decentralized estimation, localization, and control approach must be developed to properly monitor and control swarms of varying scale with inherent variations of communication capabilities among elements. Random Finite Set (RFS) theory can assist with the estimation of a model of the swarm, and model predictive control can then be applied to create an efficient control solution, regardless of the number of units involved. RFS theory addresses the decentralized formation problem by approximating swarms as units of collective swarm elements. Each unit can represent anywhere from a single swarm element to thousands of swarm elements. By approximating the swarm as a whole in this way, simulations and calculations of large magnitudes can be significantly reduced in complexity, as the technique features a continuously-adjustable scale of granularity to model the estimation problem, thus enabling complex behavior and complex distributed decision making capabilities. Significantly reduced design overhead and implementation costs have been demonstrated, as this probabilistic approach to swarm guidance enables faster convergence, reducing computational loading. The swarm dynamics and operation can be estimated using a Gaussian Mixture Probability Hypothesis Density (GM-PHD) filter, which can handle non-Gaussian errors in the vehicle state and non-Gaussian element birth and spawn models. The benefit of using a PHD filter is that the number of units being tracked does not need to be specified, as the filter can estimate this number in real time, while also rejecting background measurements, so as not to use these in tracking. Model predictive control can solve the optimal control problem using Behavioral Distribution Control (BDC) modeled as a Gaussian mixture distribution. A key advantage of this system in employing model predictive control is that it can handle non-linearities in the objective function. It also uses Gaussian Process to initialize the starting point, and a finite horizon. Using these methods of RFS estimation, and applying the GM-PHD filter and model predictive control approaches, a simulation toolset has been developed to provide a development environment which allows for the testing, performance evaluation, and visualization of a swarm of robotic vehicles In particular the toolset’s foundations in using probabilistic methods for localization and control means that swarm elements with limited communication capabilities and other resource limitations such as “Chipsats” and FemotSats can be directly supported. The new SWARM toolset has been developed using a MATLAB graphical user interface to create a user-oriented simulation environment that provides a simple, efficient development platform to simulate and approximate these normally highly complex estimation and control problems. The SWARM toolset initializes state parameters for the swarm, including approximate centroid location, destination, and approximate velocity, and then outputs a trajectory of swarm elements for inspection. The precise nature of these input parameters depends on the environment the swarm is operating within and the nature of the task or challenge the swarm is presented with solving. The SWARM toolset has applications in a variety of real world environments, including ground based roving vehicle systems, Unmanned Aerial Vehicles (UAVs) and aerial flight systems, seafaring vessels or undersea rovers, and spacecraft. While initially focusing on ground based vehicular navigation and small-satellite swarms, the toolset is currently being adapted to contain a variety of additional environments and vehicle parameters, which will include ground, seafaring, flight, and space, in and around planetary bodies other than Earth. The ground based examples utilize a simple two-dimensional dynamic model optimized with a Linear Quadratic Regulator, which solves the optimal control problem and then applies the optimized trajectory to minimize costs and loss. Aerial examples allow for motion in three dimensions, including a variety of formations and maneuvers including barrel rolls, and arc flight formation. Satellite swarm examples utilize the Clohessy-Wiltshire Equations to simulate the relative element dynamics while the swarm performs maneuvers in near circular orbits. Furthermore, this toolset is being developed with the purpose of supporting hardware demonstrations of swarm technology. These hardware demonstrations will deploy the developed toolset in conjunction with a scalable, variable-environment using two separate swarm robotic platforms, including small-sized Kilobots and the wheeled robot platform Jasmine. Swarm formations directly supported by this swarm toolset include formation motion examples, where the swarm is instructed to move to, and maintain a desired shape as the elements perform a task, as well as object detection and avoidance, where the swarm discovers an object and seeks to observe or avoid it. These simulations have direct applications to future missions in a variety of environments, formation motion being essential in almost all environments, due to the need to coordinate motion throughout a swarm. Additionally, object detection and avoidance is a useful application for avoiding rocks or canyons in the case of a rover, or circumnavigating around asteroids in the case of a swarm of satellites. This paper will present the development and capabilities of utilizing Random Finite Set statistics and model predictive control theory to efficiently operate swarms of vehicles at a high-level, and present methods to implement these theories within the new SWARM robotics simulation and control toolset. The paper will describe the capabilities enhancements these theories provide to multiple applications, both terrestrial and in space, including common and practical applications on Earth.