ASAP-MPC: An Asynchronous Update Scheme for Online Motion Planning with Nonlinear Model Predictive Control

This paper presents a Nonlinear Model Predictive Control (NMPC) scheme targeted at motion planning for mechatronic motion systems, such as drones and mobile platforms. NMPC-based motion planning typically requires low computation times to be able to provide control inputs at the required rate for system stability, disturbance rejection, and overall performance. Although there exist various ways in literature to reduce the solution times in NMPC, such times may not be low enough to allow real-time implementations. This paper presents ASAP-MPC, an approach to handle varying, sometimes restrictively large, solution times with an asynchronous update scheme, always allowing for full convergence and real-time execution. The NMPC algorithm is combined with a linear state feedback controller tracking the optimised trajectories for improved robustness against possible disturbances and plant-model mismatch. ASAP-MPC seamlessly merges trajectories, resulting from subsequent NMPC solutions, providing a smooth and continuous overall trajectory for the motion system. This frameworks applicability to embedded applications is shown on two different experiment setups where a state-of-the-art method fails: a quadcopter flying through a cluttered environment in hardware-in-the-loop simulation and a scale model truck-trailer manoeuvring in a structured lab environment.

FATROP : A Fast Constrained Optimal Control Problem Solver for Robot Trajectory Optimization and Control

Trajectory optimization is a powerful tool for robot motion planning and control. State-of-the-art general-purpose nonlinear programming solvers are versatile, handle constraints in an effective way and provide a high numerical robustness, but they are slow because they do not fully exploit the optimal control problem structure at hand. Existing structure-exploiting solvers are fast but they often lack techniques to deal with nonlinearity or rely on penalty methods to enforce (equality or inequality) path constraints. This works presents FATROP: a trajectory optimization solver that is fast and benefits from the salient features of general-purpose nonlinear optimization solvers. The speed-up is mainly achieved through the use of a specialized linear solver, based on a Riccati recursion that is generalized to also support stagewise equality constraints. To demonstrate the algorithm's potential, it is benchmarked on a set of robot problems that are challenging from a numerical perspective, including problems with a minimum-time objective and no-collision constraints. The solver is shown to solve problems for trajectory generation of a quadrotor, a robot manipulator and a truck-trailer problem in a few tens of milliseconds. The algorithm's C++-code implementation accompanies this work as open source software, released under the GNU Lesser General Public License (LGPL). This software framework may encourage and enable the robotics community to use trajectory optimization in more challenging applications.