Probabilistically-sound and asymptotically-optimal algorithm for stochastic control with trajectory constraints

In this paper, we consider a class of stochastic optimal control problems with trajectory constraints. As a special case, we can constrain the probability that a system enters undesirable regions to remain below a certain threshold. We extend the incremental Markov Decision Process (iMDP) algorithm, which is a new computationally-efficient and asymptotically-optimal sampling-based tool for stochastic optimal control, to approximate arbitrarily well an optimal feedback policy of the constrained problem. We show that with probability one, in the presence of trajectory constraints, the sequence of policies returned from the algorithm is both probabilistically sound and asymptotically optimal. We demonstrate the proposed algorithm on motion planning and control problems subject to bounded collision probability in uncertain cluttered environments.