This work presents an efficient method to solve a class of continuous-time, continuous-space stochastic optimal control problems in the context of motion planning in a cluttered environment. The method builds upon a path integral representation of the stochastic optimal control problem that allows for computation of the optimal solution through some type of sampling process. As this sampling process often leads to a local minimum especially when the state space is highly non-convex due to the obstacle field, we present an efficient method to alleviate this issue of local optima by devising a topology-embedded sampling-based planning algorithm. Combined with a receding-horizon scheme in execution of the optimal control solution, the proposed method can generate a globally optimal, dynamically feasible and collision-free trajectory. An illustrative numerical example is presented to demonstrate the applicability and validity of the proposed approach.
View on arXiv