Stochastic Ensemble Simulation motion planning in stochastic dynamic environments

Motion planning in stochastic dynamic environments is difficult due to the need for constant plan adjustment caused by the uncertainty of the environment. There are many motion planning problems, including flight coordination and autonomous vehicles, that require an algorithm to predict obstacle motion and plan safely. In this paper, we propose Stochastic Ensemble Simulation (SES)-based planning, a novel framework to efficiently predict and produce safe trajectories in the presence of stochastic obstacles. The stochastic obstacles can be introduced in several ways including stochastic motion or position/speed uncertainty. SES-based planning works by first predicting an obstacle´s future position offline through an ensemble of Monte Carlo simulations. These runs simulate the stochastic obstacle dynamics and store the simulation results in temporal snapshots of predicted positions. An online planner then uses this output to identify a predicted collision-free direct path to the goal. If the direct path is not expected to be collision-free, a more expensive tree-based planner is used. Our experiments show SES-based planning outperforms other methods that have high planning success in environments with 900 stochastically moving obstacles. Furthermore, our method plans trajectories with an 80% success rate for a 7 DOF robot in an environment with 250 stochastic moving obstacles and 50 obstacles with speed/position uncertainty. This complex problem is currently beyond the capability of several comparison methods.