A Probabilistic Particle Control Approach to Optimal, Robust Predictive Control

Abstract

Autonomous vehicles need to be able to plan trajectories to a specified goal that avoid obstacles, and are robust to the inherent uncertainty in the problem. This uncertainty arises due to uncertain state estimation, disturbances and modeling errors. Previous solutions to the robust path planning problem solved this problem using a finite horizon optimal stochastic control approach. This approach finds the optimal path subject to chance constraints, which ensure that the probability of collision with obstacles is below a given threshold. This approach is limited to problems where all uncertain distributions are Gaussian, and typically result in highly conservative plans. In many cases, however, the Gaussian assumption is invalid; for example in the case of localization, the belief state about a vehicle’s position can consist of highly non-Gaussian, even multimodal, distributions. In this paper we present a novel method for finite horizon stochastic control of dynamic systems subject to chance constraints. The method approximates the distribution of the system state using a finite number of particles. By expressing these particles in terms of the control variables, we are able to approximate the original stochastic control problem as a deterministic one; furthermore the approximation becomes exact as the number of particles tends to infinity. For a general class of chance constrained problems with linear system dynamics, we show that the approximate problem can be solved using efficient Mixed-Integer Linear Programming techniques. We apply the new method to aircraft control in turbulence, and show simulation results that demonstrate the efficacy of the approach.