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

TL;DR

The paper tackles path planning through narrow passages where robot orientation is crucial. It introduces a framework that leverages differentiable Superquadrics to expand obstacles and Voronoi diagrams to yield maximum-clearance, orientation-guiding paths in both $2D$ and $3D$. Key contributions include the Minkowski-like obstacle expansion via SQs, a cluster-aware Voronoi construction, a graph-based planner with orientation rules for SE(2) and SE(3), and DMP-based trajectory smoothing. Empirical results in 2D object-retrieval tasks and 3D drone simulations show faster planning and safer trajectories than classical methods and GCOPTER, while also highlighting limitations in highly non-convex environments and outlining avenues for future work, including non-convex Voronoi-region handling and manipulators.

Abstract

Path planning in narrow passages is a challenging problem in various applications. Traditional planning algorithms often face challenges in complex environments like mazes and traps, where narrow entrances require special orientation control for successful navigation. In this work, we present a novel approach that combines superquadrics (SQ) representation and Voronoi diagrams to solve the narrow passage problem in both 2D and 3D environment. Our method utilizes the SQ formulation to expand obstacles, eliminating impassable passages, while Voronoi hyperplane ensures maximum clearance path. Additionally, the hyperplane provides a natural reference for robot orientation, aligning its long axis with the passage direction. 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 by ensuring passable trajectories with maximum clearance.

Figures (6)

• Figure 1: Navigating narrow passages in 2D and 3D: path feasibility, orientation decision, and safety problem due to uncertainty
• Figure 2: An entire process of our algorithm: a): approximate the environment by SQs. b): Grown obstacles with shortest axis length of robot. c): Hyperplane between two SQs and closest point. d): Create Voronoi diagram with hyperplane. e): Utilize graph planning to find edges, then parameterized by DMP.
• Figure 3: Clusters are partitioned using Alg. \ref{['alg: cluster']}, and adjacent SQs are grouped into the same region as Alg. \ref{['alg: Voronoi']}.
• Figure 4: Process for executing the narrow-passage experiment. Step 1: The Kinova robot scans a table crowded with objects. Step 2: Objects are segmented, and the target object is selected as $SQ_r$. Step 3: The remaining obstacles $SQ_o$ are expanded, where the impossible passage are eliminated. Step 4: Trajectory planning is performed with the desired orientation.
• Figure 5: Our method in U-block and T-block scenarios, where closed obstacles are segmented into the same Voronoi region.