A hierarchical method for stochastic motion planning in uncertain environments

This paper considers the problem of stochastic motion planning in uncertain environments, and extends existing chance constrained optimal control solutions. Due to the imperfect knowledge of the system state caused by motion uncertainty, sensor noise and environment uncertainty, the system constraints cannot be guaranteed to be satisfied and consequently must be considered probabilistically. To account for the uncertainty, the constraints are formulated as convex constraints on a random variable, known as chance constraints, with the violation probability of all the constraints guaranteed to be below a threshold. Standard chance constrained stochastic motion planning methods do not incorporate environmental sensing which typically leads to overly-conservative solutions. To address this, a novel hierarchical framework is proposed that consists of two main steps: an expected shortest path problem on an uncertain graph and a chance constrained motion planning problem. The first successful, real-time experimental demonstration of chance constrained control with uncertain constraint parameters and variables is also presented for a quadrotor equipped with a Kinect sensor navigating through an uncertain, cluttered 3D environment.