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

## TL;DR

This paper introduces a novel method combining homotopy techniques with numerical optimal control to efficiently compute locally optimal motion plans in complex, non-convex environments, surpassing traditional sampling-based planners.

## Contribution

It extends numerical optimal control methods to non-convex motion planning problems using a homotopy approach integrated with SQP, enabling solutions where previous methods struggled.

## Key findings

- Outperforms benchmark sampling-based planners in 2D and 3D environments
- Successfully computes locally optimal solutions in challenging non-convex spaces
- Demonstrates practical applicability in complex motion planning scenarios

## Abstract

This paper presents a systematic approach for computing local solutions to motion planning problems in non-convex 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 perturbs, or preferably relaxes, the original problem to an easily solved problem. 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 a state-of-the-art open-source optimizing sampled-based planner commonly used as benchmark.