Combining Homotopy Methods and Numerical Optimal Control to Solve Motion Planning Problems

This paper presents a systematic approach for computing local solutions to motion planning problems in nonconvex environments using numerical optimal control techniques. It extends the range of use of state-of-the-art numerical optimal control tools to problem classes where these tools have previously not been applicable. Today these problems are typically solved using motion planners based on randomized or graph search. The general principle is to define a homotopy that transforms, or preferably relaxes, the original problem to an easily solved problem. In this work, it is shown that by combining a Sequential Quadratic Programming (SQP) method with a homotopy approach that gradually transforms the problem from a relaxed one to the original one, practically relevant locally optimal solutions to the motion planning problem can be computed. The approach is demonstrated in motion planning problems in challenging 2D and 3D environments, where the presented method significantly outperforms both a state-of-the-art numerical optimal control method and a state-of-the-art open-source optimizing sampling-based planner commonly used as benchmark.