Motion Planning with Homotopy Class Constraints via the Auxiliary Energy Reduction Technique

We introduce the so-called Auxiliary Energy Reduction (AER) technique, which is a gradient-based approach to solving motion planning problems with homotopy class constraints for system models with full-scale nonholonomic dynamics. The hallmark of our approach is that we first introduce virtual control terms to the original system dynamics that ensure that any preset state trajectory is dynamically feasible with respect to the new extended system. We then gradually shift the contribution of the artificial inputs to the actual original inputs by solving a sequence of associated quadratic programs. When the contribution of the artificial inputs has been fully removed, the preset trajectory will have been deformed to a trajectory of the same homotopy class that is now also feasible with respect to the original system. The practicality of our method is demonstrated in simulation examples for the Brockett integrator, the unicycle, and a 12-dimensional nonlinear quadcopter model.