Embedding nonlinear optimization in RRT* for optimal kinodynamic planning

Some of the latest developments in motion planning methods have addressed the merging of optimal control with sampling-based approaches, to handle the problem of optimal kinodynamic motion planning for complex robot systems in cluttered environments. These include embedding the Linear Quadratic Regulator method in an RRT* context, or solving the kinematic problem with an RRT algorithm first and then feeding the solution to an NLP solver. An alternative approach is presented here, in which NLP is embedded in an RRT* context from the start. The resulting methodological features are illustrated with numerical examples. These include problems in which differential constraints play a fundamental role.