Constrained Iterative Nonlinear Optimization for Robot Control Applications

Numerical optimization provides a computational tool widely used to control robotic systems subject to constraints during their motion. However, many of the methods used to solve these problems lack treatment of physical constraints i.e., limitations on the control inputs and constraints on the temporal aspect of the motion, common in practical application. Here we present a computational method to nonlinear dynamic optimization which enables us to take inequality constraints on the control commands and the simultaneously optimized task duration rigorously into account. The presented approach uses state augmentation to reduce the original free time horizon optimization to a fixed time horizon problem. Following this transformation we derive a minimalistic constraint linear-quadratic sub-problem which is iteratively solved to find the solution of the original constrained nonlinear dynamic optimization problem. The proposed approach is used to solve high-dimensional test problems in simulation, a low-dimensional, non-convex robot control problem tested in an feedback control experiment and a high-dimensional, unstable robot planning problem in simulation. These examples indicate high accuracy, demonstrate scalability, and suggest wide range applicability of the proposed approach.