Methods for Direct Optimal Control of Multibody Systems

Licentiate thesis, 2016

In this thesis we present practical tools and techniques to numerically solve optimal control problems involving multibody systems. The driving application for the thesis has been developing motion planning tools for industrial robots and digital humans. For solution of constrained optimal control problems in real world applications, direct methods are by far the most widespread, and successfully used techniques. In direct methods, the optimal control problem is transcribed into a nonlinear program, which can be solved using standard nonlinear programming algorithms. Evaluation of the equations of motion, and derivatives thereof, are the most expensive operations in using direct methods for optimal control of multibody systems. We propose two methods to reduce this cost when using variational integrators to discretize the dynamics. First, we present efficient algorithms for computation of the residual of the constrained discrete Euler-Lagrange equations of motion for tree structured, rigid multibody systems. In particular, we present new recursive formulas for computing sensitivities of the kinetic energy. This enables us to solve the inverse dynamics problem of the discrete system with linear computational complexity. The resulting algorithms are easy to implement, and can naturally be applied to a very broad class of multibody systems by imposing constraints on the coordinates by means of Lagrange multipliers. Second, we show how to speed up the derivative computations by exploiting separability of the discrete equations of motion. Exploiting the structure of the dynamics is particularly important when applying the direct optimal control methods to digital humans, where the state space is substantially larger than for an industrial robot. Another problem addressed in the thesis is incorporation of collision avoidance in direct optimal control methods. While conceptually straightforward, in general, collision avoidance results in a large number of constraints, or non-smooth constraint functions. To avoid these issues, we propose a new scheme based on iterative refinement of a collision free trajectory, and approximation of obstacles in configuration space.