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

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.