Varying-Radius Tunnel-Following NMPC for Robot Manipulators Using Sequential Convex Quadratic Programming

The tunnel-following nonlinear model predictive control (NMPC) scheme for robot manipulators allows the definition of tasks where deviations from a given path reference are allowed but upper-bounded by a user-defined parameter, which for a position tunnel represents the radius of the tunnel. The underlying optimal control problem (OCP) in this scheme can be efficiently solved by using the sequential convex quadratic programming (SCQP) method. Up to now, this scheme has been implemented with constant tunnel radii, although several tasks, such as human-robot collaboration or pick-and-place tasks, would benefit from variable radii throughout task execution. The SCQP method is however not able to exploit the structure of varying-radius tunnel constraints, which can lead to unstable iterations of the SQP method. In this work, we propose a reformulation of the tunnel constraints to overcome this issue, allowing the use of the SCQP method to efficiently solve the underlying OCP. We also provide insight into an efficient implementation of the SCQP method using the lin operator and prove the main theorem underlying such operator. Simulation results of a task involving a varying-radius tunnel are presented to support the applicability of the proposed methods.