Path Planning in Complex Environments with Superquadrics and Voronoi-Based Orientation

Path planning in narrow passages is a challenging problem in various applications. Traditional planning algorithms often struggle in complex environments such as mazes and traps, where navigating through narrow entrances requires precise orientation control. Additionally, perception errors can lead to collisions if the planned path is too close to obstacles. Maintaining a safe distance from obstacles can enhance navigation safety. In this work, we present a novel approach that combines superquadrics (SQ) representation and Voronoi diagrams to solve the narrow passage problem. The key contributions are: $i)$ an efficient and generalizable path planning framework, integrating SQ with Voronoi diagrams to provide a computationally efficient and differentiable mathematical formulation, enabling flexible shape fitting for various robots and obstacles. ii) improved passage feasibility, achieved by expanding SQ's minor axis to eliminate impassable narrow gaps while maintaining feasible paths along the major axis, which aligns with the Voronoi hyperplane and follows maximum clearance paths. iii) enhanced robustness in complex environments, by integrating multiple connected obstacles into the same Voronoi region, handling trap scenarios. We validate our framework through a 2D object retrieval task and 3D drone simulation, demonstrating that our approach outperforms classical planners and a cutting-edge drone planner in various key aspects. The video can be found at https://youtu.be/kbvQRxRACvQ