Fast and Bounded Probabilistic Collision Detection for High-DOF Trajectory Planning in Dynamic Environments

Abstract

We present a novel approach to perform probabilistic collision detection between a high-DOF robot and imperfect obstacle representations in dynamic and uncertain environments. Our formulation is designed for high-DOF robot trajectory planning in dynamic scenes, where the uncertainties are modeled using Gaussian distributions. We present an efficient algorithm to compute collision probabilities between the robot and the obstacles. Furthermore, we present a prediction algorithm for obstacle positions that takes into account spatial and temporal uncertainties and uses that for trajectory optimization. We highlight the performance of our trajectory planning algorithm in challenging simulated and real-world environments with robot arms operating next to dynamically moving human obstacles. Note to Practitioners —This paper suggests a novel trajectory planning approach for dynamic and uncertain environments. Existing planning approaches generally deal with uncertainties by performing collision checks using enlarged bounding shapes for the given confidence levels, which are conservative and tend to compute less optimal trajectories or fail to find feasible trajectories. In this paper, we suggest a new collision probability approximation of a robot and obstacles. Our approach guarantees that the computed probability is an upper bound on the actual probability. We then present a trajectory planning algorithm based on our probabilistic collision detection, and a practical belief space estimation algorithm. In our experimental results, we demonstrate that our approach can compute more efficient trajectories than the prior approaches, while our approach has a similar level of safety. Our experiments assume Gaussian distributions for the environment uncertainties, and we will extend our algorithm to non-Gaussian distributions in future research. Recently, we have extended our approach to general convex polytopes and improved the speed and accuracy of the collision probability computation using bounding volume hierarchies.