Nonlinear model predictive control with set terminal constraint for safe robot motion planning via speed and separation monitoring

This paper proposes a methodology for safely planning the motion of a robot manipulator sharing its workspace with a human operator. The motion of the robot is continuously re-planned via nonlinear model predictive control (NMPC), imposing the so-called speed and separation monitoring (SSM) condition to guarantee human safety. Contrary to previous works in the field, the NMPC algorithm is designed with an ellipsoidal terminal constraint, to enlarge the domain of attraction compared to the case in which a point terminal constraint was imposed. This is a very important aspect in real-world applications, allowing the robot to plan its motion from initial configurations that are relatively far from the goal point. Theoretical results are proved on recursive feasibility and closed-loop stability for both cases of NMPC with point and set terminal constraints, under the simplifying assumption of a static human. The effectiveness of the proposed approach is verified via numerical evaluation of the domain of attraction and with experiments on a UR5 manipulator.