Constructive controllability algorithms for motion planning and optimization

This paper presents novel algorithms for planning feasible and minimum energy paths. The algorithms rely on series expansions to characterize planning problems for polynomial control systems. The resulting inversion problem is solved through an iterative contraction or a power series inversion. While the design methodology is general, our focus is on a class of polynomial control systems for which we provide explicit convergence guarantees. We demonstrate performance and numerical characteristics using one dimensional and six dimensional systems.