Stochastic Motion Planning Using Successive Convexification and Probabilistic Occupancy Functions

Abstract

We propose a method for real-time motion planning in stochastic, dynamic environments via a receding horizon framework that exploits computationally efficient algorithms for forward stochastic reachability analysis and non-convex optimization. Our method constructs a dynamically feasible trajectory for a robot, modeled as an LTI dynamical system, while ensuring 1) a desired probabilistic collision-avoidance guarantee is achieved, 2) state and control constraints are satisfied, and 3) a convex performance objective is minimized. We first compute “keep-out” regions at each time instant to assure a probabilistic collision avoidance guarantee. These keep-out regions are convex and compact, and can be tightly overapproximated by ellipsoids which may be computed in a grid-free, recursion-free, and sample-free manner. The regions are constraints in a non-convex optimization problem, solved via successive convexification. This algorithm can uses interior point methods for real-time implementation. We present numerical simulations to demonstrate the efficacy of the approach.