Homotopy Method for Optimal Motion Planning With Homotopy Class Constraints

Optimal motion planning is an essential task within the field of control theory. Therein, the key task is to synthesize optimal system trajectories that pass through cluttered environments while respecting given homotopy class constraints, which is critical in many topology-restricted applications such as search and rescue. In this letter, we introduce a novel optimal motion planning technique with 2-dimensional homotopy class constraints for general dynamical systems. We first initialize an optimal system trajectory regardless of obstacles and homotopy class constraints, and design an auxiliary obstacle trajectory for each obstacle such that the system trajectory belongs to the desired homotopy class regarding these auxiliary obstacle trajectories. During the procedure of deforming the auxiliary obstacle trajectory to the original counterparts, we propose a homotopy method based on nonlinear programming (NLP) such that the synthesized optimal system trajectories fulfill the aforementioned homotopy class constraints. The proposed method is validated with numerical results on two classic nonlinear systems with planar static and moving obstacles.