Kalman Smoothing for Distributed Optimal Feedback Control of Unicycle Formations

Abstract

In many multi-agent control problems, the ability to compute an optimal feedback control is severely limited by the dimension of the state space. In this work, deterministic, nonholonomic agents are tasked with creating and maintaining a formation based on observations of their neighbors, and each agent in the formation independently computes its feedback control from a Hamilton-Jacobi-Bellman (HJB) equation. Since an agent does not have knowledge of its neighbors’ future motion, we assume that the unknown control to be applied by neighbors can be modeled as Brownian motion. The resulting probability distribution of its neighbors’ future trajectory allows the HJB equation to be written as a path integral over the distribution of optimal trajectories. We describe how the path integral approach to stochastic optimal control allows the distributed control problems to be written as independent Kalman smoothing problems over the probability distribution of the connected agents’ future trajectories. Simulations show five unicycles achieving the formation of a regular pentagon.