Fast nonlinear Model Predictive Control for unified trajectory optimization and tracking

This paper presents a framework for real-time, full-state feedback, unconstrained, nonlinear model predictive control that combines trajectory optimization and tracking control in a single, unified approach. The proposed method uses an iterative optimal control algorithm, namely Sequential Linear Quadratic (SLQ), in a Model Predictive Control (MPC) setting to solve the underlying nonlinear control problem and simultaneously derive the optimal feedforward and feedback terms. Our customized solver can generate trajectories of multiple seconds within only a few milliseconds. The performance of the approach is validated on two different hardware platforms, an AscTec Firefly hexacopter and the ball balancing robot Rezero. In contrast to similar approaches, we perform experiments that require leveraging the full system dynamics.