Guaranteed Global Optimal Path Planning Through Convex Segmentation and Homotopy Guidance

Abstract

Trajectory planning and optimization in three-dimensional space remains fundamental challenges in high-dimensional motion planning. This paper presents ConvexSegTopoChainOpt (Csto), a trajectory optimization framework that integrates convex decomposition and topological guidance. The proposed method partitions obstacle space into a sequence of connected, collision-free convex polyhedral via geometric decomposition. Homotopy vectors are introduced to enforce topological consistency, ensuring that the trajectory traverses a continuous chain of convex regions. A global convex optimization problem is formulated using an auxiliary variable relaxation strategy, enabling efficient computation of the op-timal trajectory. Csto has been implemented and validated on the Kinova Jaco2 robotic platform, demonstrating its effectiveness in physically realistic experimental settings.