A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

A Survey on Aerial Swarm Robotics

The use of aerial swarms to solve real-world problems has been increasing steadily, accompanied by falling prices and improving performance of communication, sensing, and processing hardware. The commoditization of hardware has reduced unit costs, thereby lowering the barriers to entry to the field of aerial swarm robotics. A key enabling technology for swarms is the family of algorithms that allow the individual members of the swarm to communicate and allocate tasks amongst themselves, plan their trajectories, and coordinate their flight in such a way that the overall objectives of the swarm are achieved efficiently. These algorithms, often organized in a hierarchical fashion, endow the swarm with autonomy at every level, and the role of a human operator can be reduced, in principle, to interactions at a higher level without direct intervention. This technology depends on the clever and innovative application of theoretical tools from control and estimation. This paper reviews the state of the art of these theoretical tools, specifically focusing on how they have been developed for, and applied to, aerial swarms. Aerial swarms differ from swarms of ground-based vehicles in two respects: they operate in a three-dimensional space and the dynamics of individual vehicles adds an extra layer of complexity. We review dynamic modeling and conditions for stability and controllability that are essential in order to achieve cooperative flight and distributed sensing. The main sections of this paper focus on major results covering trajectory generation, task allocation, adversarial control, distributed sensing, monitoring, and mapping. Wherever possible, we indicate how the physics and subsystem technologies of aerial robots are brought to bear on these individual areas

Game-Theoretic Safety Assurance for Human-Centered Robotic Systems

In order for autonomous systems like robots, drones, and self-driving cars to be reliably introduced into our society, they must have the ability to actively account for safety during their operation. While safety analysis has traditionally been conducted offline for controlled environments like cages on factory floors, the much higher complexity of open, human-populated spaces like our homes, cities, and roads makes it unviable to rely on common design-time assumptions, since these may be violated once the system is deployed. Instead, the next generation of robotic technologies will need to reason about safety online, constructing high-confidence assurances informed by ongoing observations of the environment and other agents, in spite of models of them being necessarily fallible.This dissertation aims to lay down the necessary foundations to enable autonomous systems to ensure their own safety in complex, changing, and uncertain environments, by explicitly reasoning about the gap between their models and the real world. It first introduces a suite of novel robust optimal control formulations and algorithmic tools that permit tractable safety analysis in time-varying, multi-agent systems, as well as safe real-time robotic navigation in partially unknown environments; these approaches are demonstrated on large-scale unmanned air traffic simulation and physical quadrotor platforms. After this, it draws on Bayesian machine learning methods to translate model-based guarantees into high-confidence assurances, monitoring the reliability of predictive models in light of changing evidence about the physical system and surrounding agents. This principle is first applied to a general safety framework allowing the use of learning-based control (e.g. reinforcement learning) for safety-critical robotic systems such as drones, and then combined with insights from cognitive science and dynamic game theory to enable safe human-centered navigation and interaction; these techniques are showcased on physical quadrotors—flying in unmodeled wind and among human pedestrians—and simulated highway driving. The dissertation ends with a discussion of challenges and opportunities ahead, including the bridging of safety analysis and reinforcement learning and the need to ``close the loop'' around learning and adaptation in order to deploy increasingly advanced autonomous systems with confidence