Trajectory Planning for Systems with Homotopy Class Constraints

Abstract

There are various applications where homotopy constraints are useful in trajectory generation for mobile robots. In this paper, we present a method to generate an optimal trajectory restricted to a particular homotopy class, which is specified by a given representative trajectory. The optimality is achieved by formulating the trajectory generation problem as a Mixed-Integer Quadratic Program (MIQP). We partition the configuration space into nonoverlapping cells and model each cell in the partition with integer variables and inequality constraints. We associate with any sequence of integer variables a word, so that each trajectory can be mapped to a word. We then construct a set of all words that are homotopically equivalent to a given word. For each word, we fix the integer variables of the MIQP to find the optimal time distribution in each cell, by solving a QP for each iteration, to obtain the locally optimal trajectory in the specified homotopy class. We illustrate an example of minimum acceleration trajectory generation on a plane with different homotopy class constraints.