Enhanced Probabilistic Collision Detection for Motion Planning Under Sensing Uncertainty

TL;DR

This paper introduces an advanced probabilistic collision detection method that uses superquadrics and accounts for both position and orientation errors, significantly improving accuracy and efficiency in robot motion planning under sensing uncertainty.

Contribution

It presents a novel PCD approach that incorporates orientation errors and uses superquadrics for better shape approximation, enhancing robustness and performance.

Findings

Achieves collision detection accuracy twice that of previous methods.

Reduces path length by 30% and planning time by 37%.

Lower collision probability in simulation to 2% when considering orientation errors.

Abstract

Probabilistic collision detection (PCD) is essential in motion planning for robots operating in unstructured environments, where considering sensing uncertainty helps prevent damage. Existing PCD methods mainly used simplified geometric models and addressed only position estimation errors. This paper presents an enhanced PCD method with two key advancements: (a) using superquadrics for more accurate shape approximation and (b) accounting for both position and orientation estimation errors to improve robustness under sensing uncertainty. Our method first computes an enlarged surface for each object that encapsulates its observed rotated copies, thereby addressing the orientation estimation errors. Then, the collision probability under the position estimation errors is formulated as a chance-constraint problem that is solved with a tight upper bound. Both the two steps leverage the recently developed normal parameterization of superquadric surfaces. Results show that our PCD method is twice as close to the Monte-Carlo sampled baseline as the best existing PCD method and reduces path length by 30% and planning time by 37%, respectively. A Real2Sim2Real pipeline further validates the importance of considering orientation estimation errors, showing that the collision probability of executing the planned path in simulation is only 2%, compared to 9% and 29% when considering only position estimation errors or no errors at all.