A Nonlinear Model Predictive Control Scheme for Cooperative Manipulation with Singularity and Collision Avoidance

TL;DR

This paper presents a nonlinear model predictive control approach for cooperative robotic manipulation that ensures obstacle avoidance, singularity avoidance, and input saturation compliance, with proven feasibility and convergence.

Contribution

It introduces a novel NMPC scheme for cooperative manipulation that guarantees collision, singularity avoidance, and input constraints in a bounded workspace.

Findings

Successful simulation validation of the proposed control scheme.

Guarantees on collision and singularity avoidance in cooperative manipulation.

Convergence and feasibility of the NMPC method are theoretically proven.

Abstract

This paper addresses the problem of cooperative transportation of an object rigidly grasped by $N$ robotic agents. In particular, we propose a Nonlinear Model Predictive Control (NMPC) scheme that guarantees the navigation of the object to a desired pose in a bounded workspace with obstacles, while complying with certain input saturations of the agents. Moreover, the proposed methodology ensures that the agents do not collide with each other or with the workspace obstacles as well as that they do not pass through singular configurations. The feasibility and convergence analysis of the NMPC are explicitly provided. Finally, simulation results illustrate the validity and efficiency of the proposed method.

Equations

$${\mathcal{O}}_z$$

$$=\left\{p\in {\mathbb{R}}^3:(p-c_z{)}^{\top }P(p-c_z)\le 1\right\},$$

$$J_{B_i}({\eta }_{B_i})=\left[\begin{array}{ccc}1& 0& \sin ({\theta }_{B_i})\\ 0& \cos ({\varphi }_{B_i})& -\cos ({\theta }_{B_i})\sin ({\varphi }_{B_i})\\ 0& \sin ({\varphi }_{B_i})& \cos ({\theta }_{B_i})\cos ({\varphi }_{B_i})\end{array}\right].$$

$$p_{E_i}(q_i)$$

$${\eta }_{E_i}(q_i)$$

$$v_i(q_i,{\dot{q}}_i)$$

$$=\left[\begin{array}{c}{\dot{p}}_{B_i}-S(R_{B_i}{k}_{p_i}){\omega }_{B_i}+R_{B_i}\frac{\partial k_{p_i}}{\partial {\alpha }_i}\\ {\omega }_{B_i}+R_{B_i}{J}_{A_i}{\dot{\alpha }}_i\end{array}\right],$$

$$v_i(q_i,{\dot{q}}_i)=\left[\begin{array}{c}{\dot{p}}_{E_i}(q_i,{\dot{q}}_i)\\ {\omega }_{E_i}(q_i,{\dot{q}}_i)\end{array}\right]=J_i(q_i){\dot{q}}_i,$$

$$J_i(q_i)=\left[\begin{array}{ccc}{I}_3& -S(R_{B_i}({\eta }_{B_i})k_{p_i}({\alpha }_i))J_{B_i}({\eta }_{B_i})& R_{B_i}({\eta }_{B_i})\frac{\partial k_{p_i}({\alpha }_i)}{\partial {\alpha }_i}\\ 0_{3\times 3}& J_{B_i}({\eta }_{B_i})& R_{B_i}({\eta }_{B_i})J_{A_i}(q_i)\end{array}\right].$$

$${\mathcal{Q}}_i=\{q_i\in {\mathbb{R}}^{n_i}:\det (J_i^{\top }{J}_i)=0\},i\in \mathcal{N}.$$

$${\tilde{\mathcal{Q}}}_i=\{q_i\in {\mathbb{R}}^{n_i}:|\det (J_i^{\top }{J}_i)|\ge \epsilon >0\},i\in \mathcal{N},$$

$$B_i(q_i){\ddot{q}}_i+N_i(q_i,{\dot{q}}_i){\dot{q}}_i+g_{q_i}(q_i)={\tau }_i-J_i^{\top }{\lambda }_i,$$

$$M_i(q_i){\dot{v}}_i+C_i(q_i,{\dot{q}}_i)v_i+g_i(q_i)=u_i-{\lambda }_i,$$

$$M_i(q_i)$$

$$C_i(q_i,{\dot{q}}_i)J_i(q_i){\dot{q}}_i$$

$$g_i(q_i)$$

$${\dot{x}}_O(t)$$

$${\lambda }_O$$

$$J_{O_{r,\theta }}(x_O)=\left[\begin{array}{ccc}1& 0& \sin ({\theta }_O)\\ 0& \cos ({\varphi }_O)& -\cos ({\theta }_O)\sin ({\varphi }_O)\\ 0& \sin ({\varphi }_O)& \cos ({\theta }_O)\cos ({\varphi }_O)\end{array}\right],$$

$$p_{E_i}(q_i(t))$$

$$=p_O(t)+R_{E_i}(t)p_{E_i/O}^{E_i},$$

$${\eta }_{E_i}(q_i(t))$$

$$v_i(q_i,{\dot{q}}_i(t))=J_{O_i}(q_i)v_O(t),$$

$${\dot{v}}_i(t)=J_{O_i}(q_i){\dot{v}}_O(t)+{\dot{J}}_{O_i}(q_i)v_O(t),$$

$$J_{{\scriptscriptstyle O_{i}}}(q_{i})=\left[\begin{array}[]{cc}I_{3}&S(p_{{\scriptscriptstyle O/E_{i}}}(q_{i}))\\ 0_{{\scriptscriptstyle 3\times 3}}&I_{3}\end{array}\right],$$

$${\lambda }_O=G^{\top }(q)\bar{\lambda },$$

$$\bar{\lambda }=u-\bar{M}(q)G(q){\dot{v}}_O-(\bar{M}(q)\dot{G}(q,\dot{q})+\bar{C}(q,\dot{q})G(q))v_O-\bar{g}(q),$$

$$\tilde{M}(q){\dot{v}}_O+\tilde{C}(q,\dot{q})v_O+\tilde{g}(q)=G^{\top }(q)u,$$

$$\tilde{M}(q)$$

$$\tilde{C}(q,\dot{q})$$

$$\tilde{g}(q)$$

$$S_{i,O}(q)$$

$$S_{i,A}(q)$$

$$S_O(x_O)$$

$$\dot{x}=f(x,u)=\left[\begin{array}{c}{f}_1(x,u)\\ f_2(x,u)\\ f_3(x,u)\end{array}\right],x(0)=x_0,$$

$$f_1(x,u)$$

Full text

I Introduction

Over the last years, multi-agent systems have gained a significant amount of attention, due to the advantages they offer with respect to single-agent setups. In the case of robotic manipulation and object transportation, difficult tasks involving heavy payloads as well as challenging maneuvers necessitate the employment of multiple robots. Fig. 1 depicts a system of two robotic mobile manipulators (KUKA youBots), each comprising of a moving base and a robotic arm of 5 Degrees of Freedom (DOF).

Early works related to cooperative manipulation develop control architectures where the robotic agents communicate and share information with each other as well as completely decentralized schemes, where each agent uses only local information or observers, avoiding potential communication delays [1, 2, 3, 4, 5, 6, 7]. Impedance and force/motion control constitutes the most common methodology used in the related literature [1, 8, 9, 10, 11, 12, 13, 14, 15, 16]. However, most of the aforementioned works employ force/torque sensors to acquire knowledge of the manipulator-object contact forces/torques, which, however, may result to performance decline due to sensor noise or mounting difficulties. Recent technological advances allow to manipulator grippers to grasp rigidly certain objects (see e.g., [17]), which, as shown in this work, can render the use of force/torque sensors unnecessary.

Furthermore, in manipulation tasks, such as pose/force or trajectory tracking, collision with obstacles of the environment has been dealt with only by exploiting the extra degrees of freedom that appear in over-actuated robotic agents. Potential field-based algorithms may suffer from local minima and navigation functions [18] cannot be extended to multi-agent second order dynamical systems in a trivial way. Moreover, these methods usually result in high control input values near obstacles that need to be avoided, which might conflict the saturation of the actual motor inputs.

Another important property that concerns robotic manipulators is the singularities of the Jacobian matrix, which maps the joint velocities of the agent to a $6$D vector of generalized velocities. Such singular kinematic configurations, that indicate directions towards which the agent cannot move, must be always avoided, especially when dealing with task-space control in the end-effector [19]. In the same vein, representation singularities can also occur in the mapping from coordinate rates to angular velocities of a rigid body.

In this work, we aim to address the problem of cooperative manipulation of an object in a bounded workspace with obstacles. In particular, given $N$ agents that rigidly grasp an object, we design control inputs for the navigation of the object to a final pose, while avoiding inter-agent collisions as well as collisions with obstacles. Moreover, we take into account constraints that emanate from control input saturation as well kinematic and representation singularities.

For the design of a stabilizing feedback control law for each robot, such that the desired specifications are met, while satisfying constraints on the controls and the states, one would ideally look for a closed loop solution for the feedback law satisfying the constraints while optimizing the performance. However, typically the optimal feedback law cannot be found analytically, even in the unconstrained case, since it involves the solution of the corresponding Hamilton-Jacobi-Bellman partial differential equations. One approach to circumvent this problem is the repeated solution of an open-loop optimal control problem for a given state. The first part of the resulting open-loop input signal is implemented and the whole process is repeated. Control approaches using this strategy are referred to as Nonlinear Model Predictive Control (NMPC) (see e.g. [20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34]) which we aim to use in this work for the problem of the constraint cooperative manipulation of an object which is rigidly grasped by $N$ agents. To the best of the authors’ knowledge, this problem has not been addressed in the related literature.

The remainder of the paper is structured as follows. Section II provides preliminary background. The system dynamics and the formal problem statement are given in Section III. Section IV discusses the technical details of the solution and Section V is devoted to a simulation example. Finally, conclusions and future work are discussed in Section VI.

II Notation and Preliminaries

The set of positive integers is denoted as $\mathbb{N}$ and the real $n$-coordinate space, with $n\in\mathbb{N}$, as $\mathbb{R}^{n}$; $\mathbb{R}^{n}_{\geq 0}$ and $\mathbb{R}^{n}_{>0}$ are the sets of real $n$-vectors with all elements nonnegative and positive, respectively. The notation $\mathbb{R}^{n\times n}_{\geq 0}$ and $\mathbb{R}^{n\times n}_{>0}$, with $n\in\mathbb{N}$, stands for positive semi-definite and positive definite matrices, respectively. Moreover, $\lVert x\rVert$ is the Euclidean norm of a vector $x\in\mathbb{R}^{n}$. Given a set $S$, we denote by $|S|$ its cardinality and by $S^{N}=S\times\dots\times S$ its $N$-fold Cartesian product. Given the sets $S_{1},S_{2}$, the set difference and the Minkowski addition are denoted by $\backslash,\oplus$, respectively, and are defined by $S_{1}\backslash S_{2}=\{s:s\in S_{1}\ \text{and}\ s_{2}\notin S_{2}\}$ and $S_{1}\oplus S_{2}=\{s_{1}+s_{2}:s_{1}\in S_{1},s_{2}\in S_{2}\}$, respectively. The $n\times n$ identity matrix and the $n\times m$ matrix with zero entries, are denoted by $I_{n}$, $0_{n\times m}$ and $\mathbbm{1}_{n}$, respectively, with $n,m\in\mathbb{N}$. The largest singular value of matrix $A\in\mathbb{R}^{n\times m}$ is denoted as $\sigma_{\max}(A)$.

The vector connecting the origins of coordinate frames $\{A\}$ and $\{B$} expressed in frame $\{C\}$ coordinates in $3$-D space is denoted as $p^{\scriptscriptstyle C}_{\scriptscriptstyle B/A}=[x_{\scriptscriptstyle B/A},y_{\scriptscriptstyle B/A},z_{\scriptscriptstyle B/A}]^{\top}\in\mathbb{R}^{3}$. Given $a\in\mathbb{R}^{3},S(a)$ is the skew-symmetric matrix defined according to $S(a)b=a\times b$. We further denote as $\eta_{\scriptscriptstyle A/B}=[\phi_{\scriptscriptstyle A/B},\theta_{\scriptscriptstyle A/B},\psi_{\scriptscriptstyle A/B}]^{\top}\in\mathbb{T}^{3}\subseteq\mathbb{R}^{3}$ the $x$-$y$-$z$ Euler angles representing the orientation of frame $\{A\}$ with respect to frame $\{B\}$, with $\phi_{\scriptscriptstyle A/B},\psi_{\scriptscriptstyle A/B}\in[-\pi,\pi]$ and $\theta_{\scriptscriptstyle A/B}\in[-\tfrac{\pi}{2},\tfrac{\pi}{2}]$, where $\mathbb{T}^{3}$ is the $3$-D torus; Moreover, $R^{\scriptscriptstyle B}_{\scriptscriptstyle A}\in SO(3)$ is the rotation matrix associated with the same orientation and $SO(3)$ is the $3$-D rotation group. The angular velocity of frame $\{B\}$ with respect to $\{A\}$, expressed in frame $\{C\}$ coordinates, is denoted as $\omega^{\scriptscriptstyle C}_{{\scriptscriptstyle B/A}}\in\mathbb{R}^{3}$ and it holds that $\dot{R}^{\scriptscriptstyle B}_{\scriptscriptstyle A}=S(\omega^{\scriptscriptstyle A}_{\scriptscriptstyle B/A})R^{\scriptscriptstyle B}_{\scriptscriptstyle A}$. We further define the sets $\mathbb{M}=\mathbb{R}^{3}\times\mathbb{T}^{3}$, $\mathcal{N}=\{1,\dots,N\}$. We define also the set

[TABLE]

as the set of an ellipsoid in 3D, where $c_{z}\in\mathbb{R}^{3}$ is the center of the ellipsoid, $\beta_{1,z},\beta_{2,z},\beta_{3,z}\in\mathbb{R}_{>0}$ the lengths of its three semi-axes and $z\geq 1$ is an index term. The eigenvector of matrix $P\in\mathbb{R}^{3}$ define the principal axes of the ellipsoid, and the eigenvalues of $P$ are: $\beta_{1,z}^{-2},\beta_{2,z}^{-2}$ and $\beta_{3,z}^{-2}$. For notational brevity, when a coordinate frame corresponds to an inertial frame of reference $\left\{I\right\}$, we will omit its explicit notation (e.g., $p_{\scriptscriptstyle B}=p^{\scriptscriptstyle I}_{\scriptscriptstyle B/I},\omega_{\scriptscriptstyle B}=\omega^{\scriptscriptstyle I}_{\scriptscriptstyle B/I},R_{\scriptscriptstyle A}=R^{\scriptscriptstyle I}_{\scriptscriptstyle A}$, etc.). Finally, all vector and matrix differentiations will be with respect to an inertial frame $\{I\}$, unless otherwise stated.

Definition 1**.**

([35]) A continuous function $f:[0,\alpha]\to\mathbb{R}_{\geq 0},\alpha\in\mathbb{R}_{>0}$ is said to belong to class $\mathcal{K}$, if is strictly increasing and $f(0)=0$.

Lemma 1**.**

([36]) Let $\gamma$ be a continuous, positive definite function and $x$ be an absolutely continuous function on $\mathbb{R}$. If the following holds:

• •

$\|x(\cdot)\|<\infty,\|\dot{x}(\cdot)\|<\infty$,

• •

$\displaystyle\lim_{t\to\infty}\int_{0}^{t}\gamma(x(s))ds<\infty$.

Then, it holds that: $\lim_{t\to\infty}\|x(t)\|=0$.

III Problem Formulation

Consider a bounded and convex workspace $\mathcal{W}\subseteq\mathbb{R}^{3}$ consisting of $N$ robotic agents rigidly grasping an object, as shown in Fig. 2, and $Z$ obstacles described by the ellipsoids $\mathcal{O}_{z},z\in\mathcal{Z}=\{1,\dots,Z\}$. The free space is denoted as $\mathcal{W}_{\text{free}}=\mathcal{W}\backslash\bigcup_{z\in\mathcal{Z}}\mathcal{O}_{z}$. The agents are considered to be fully actuated and they consist of a base that is able to move around the workspace (e.g., mobile or aerial vehicle) and a robotic arm. The reference frames corresponding to the $i$-th end-effector and the object’s center of mass are denoted with $\left\{E_{i}\right\}$ and $\left\{O\right\}$, respectively, whereas $\left\{I\right\}$ corresponds to an inertial reference frame. The rigidity of the grasps implies that the agents can exert any forces/torques along every direction to the object. We consider that each agent $i$ knows the position and velocity only of its own state as well as its own and the object’s geometric parameters. Moreover, no interaction force/torque measurements or on-line communication is required.

III-A System model

III-A1 Robotic Agents

We denote by $q_{i}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{n_{i}}$ the joint space variables of agent $i\in\mathcal{N}$, with $n_{i}=n_{\alpha_{i}}+6$, $q_{i}(t)=[p^{\top}_{\scriptscriptstyle B_{i}}(t),\eta^{\top}_{\scriptscriptstyle B_{i}}(t),\alpha^{\top}_{i}(t)]^{\top}$, where $p_{\scriptscriptstyle B_{i}}=[x_{\scriptscriptstyle B_{i}},y_{\scriptscriptstyle B_{i}},z_{\scriptscriptstyle B_{i}}]^{\top}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{3},\eta_{\scriptscriptstyle B_{i}}=[\phi_{\scriptscriptstyle B_{i}},\theta_{\scriptscriptstyle B_{i}},\psi_{\scriptscriptstyle B_{i}}]^{\top}:\mathbb{R}_{\geq 0}\to\mathbb{T}^{3}\subseteq\mathbb{R}^{3}$ is the position and Euler-angle orientation of the agent’s base, and $\alpha_{i}:\mathbb{R}_{\geq 0}\to\mathbb{R}^{n_{\alpha_{i}}},n_{\alpha_{i}}>0$, are the degrees of freedom of the robotic arm. The overall joint space configuration vector is denoted as $q=[q^{\top}_{1},\dots,q^{\top}_{N}]^{\top}\in\mathbb{R}^{n}$, with $n=\sum_{i\in\mathcal{N}}n_{i}$. In addition, we denote as $p_{\scriptscriptstyle E_{i}}:\mathbb{R}^{n_{i}}\to\mathbb{R}^{3},\eta_{\scriptscriptstyle E_{i}}:\mathbb{R}^{n_{i}}\to\mathbb{T}^{3}\subseteq\mathbb{R}^{3}$ the position and Euler-angle orientation of agent $i$’s end-effector. Let also $v_{i}:\mathbb{R}^{n_{i}}\times\mathbb{R}^{n_{i}}\rightarrow\mathbb{R}^{6}$ denote the velocity of agent $i$’s end-effector, with $v_{i}(q_{i},\dot{q}_{i})=[\dot{p}^{\top}_{\scriptscriptstyle E_{i}},\omega^{\top}_{\scriptscriptstyle E_{i}}]^{\top}$, whereas $\dot{p}_{\scriptscriptstyle B_{i}},\omega_{\scriptscriptstyle B_{i}}:\mathbb{R}^{n_{i}}\times\mathbb{R}^{n_{i}}\to\mathbb{R}^{3}$ are the linear and angular velocity, respectively, of the agent’s base.

We consider that each agent $i\in\mathcal{N}$ has access to its own state $q_{i}$ as well as $\dot{p}^{\scriptscriptstyle B_{i}}_{\scriptscriptstyle B_{i}},\omega^{\scriptscriptstyle B_{i}}_{\scriptscriptstyle B_{i}}$, and $\dot{\alpha}_{i}$ via on-board sensors. Then, $\dot{p}_{\scriptscriptstyle B_{i}},\omega_{\scriptscriptstyle B_{i}}$ can be obtained via $\dot{p}_{\scriptscriptstyle B_{i}}=R_{B_{i}}(\eta_{B_{i}})\dot{p}^{\scriptscriptstyle B_{i}}_{\scriptscriptstyle B_{i}}$, $\omega_{\scriptscriptstyle B_{i}}=R_{\scriptscriptstyle B_{i}}(\eta_{\scriptscriptstyle B_{i}})\omega^{\scriptscriptstyle B_{i}}_{\scriptscriptstyle B_{i}}$, where $R_{\scriptscriptstyle B_{i}}:\mathbb{T}^{3}\to SO(3)$ is the rotation matrix of the agent $i$’s base. Moreover, $\dot{\eta}_{\scriptscriptstyle B_{i}}$ is related to $\omega_{\scriptscriptstyle B_{i}}$ via $\omega_{\scriptscriptstyle B_{i}}=J_{\scriptscriptstyle B_{i}}(\eta_{\scriptscriptstyle B_{i}})\dot{\eta}_{\scriptscriptstyle B_{i}}$, where $J_{\scriptscriptstyle B_{i}}:\mathbb{T}^{3}\to\mathbb{R}^{3\times 3}$, with

[TABLE]

The pose of the $i$th end-effector can be computed via

[TABLE]

where $k_{p_{i}}:\mathbb{R}^{n_{\alpha_{i}}}\to\mathbb{R}^{3},k_{\eta_{i}}:\mathbb{T}^{3}\times\mathbb{R}^{n_{\alpha_{i}}}\to\mathbb{T}^{3}$ are the forward kinematics of the robotic arm [19]. Then, $v_{i}$ can be computed as

[TABLE]

where $J_{\scriptscriptstyle A_{i}}:\mathbb{R}^{n_{\alpha_{i}}}\to\mathbb{R}^{3\times n_{\alpha_{i}}}$ is the angular Jacobian of the robotic arm with respect to the agent’s base. The differential kinematics (1) can be written as

[TABLE]

where $J_{i}:\mathbb{R}^{n_{i}}\to\mathbb{R}^{6\times n_{i}}$ is the agent Jacobian matrix, with

[TABLE]

Remark 1**.**

Note that $J_{\scriptscriptstyle B_{i}}$ becomes singular at representation singularities, when $\theta_{\scriptscriptstyle B_{i}}=\pm\tfrac{\pi}{2}$ and $J_{i}$ becomes singular at kinematic singularities defined by the set

[TABLE]

In the following, we will aim at guaranteeing that $q_{i}$ will always be in the closed set:

[TABLE]

for a small positive constant $\varepsilon$.

The joint-space dynamics for agent $i\in\mathcal{N}$ can be computed using the Lagrangian formulation:

[TABLE]

where $B_{i}:\mathbb{R}^{n_{i}}\rightarrow\mathbb{R}^{n_{i}\times n_{i}}$ is the joint-space positive definite inertia matrix, $N_{i}:\mathbb{R}^{n_{i}}\times\mathbb{R}^{n_{i}}\rightarrow\mathbb{R}^{n_{i}\times n_{i}}$ represents the joint-space Coriolis matrix, $g_{q_{i}}:\mathbb{R}^{n_{i}}\rightarrow\mathbb{R}^{n_{i}}$ is the joint-space gravity vector, $\lambda_{i}\in\mathbb{R}^{6}$ is the generalized force vector that agent $i$ exerts on the object and $\tau_{i}\in\mathbb{R}^{n_{i}}$ is the vector of generalized joint-space inputs, with $\tau_{i}=[\lambda^{\top}_{\scriptscriptstyle B_{i}},\tau^{\top}_{\alpha_{i}}]^{\top}$, where $\lambda_{\scriptscriptstyle B_{i}}=[f^{\top}_{\scriptscriptstyle B_{i}},\mu^{\top}_{\scriptscriptstyle B_{i}}]^{\top}\in\mathbb{R}^{6}$ is the generalized force vector on the center of mass of the agent’s base and $\tau_{\alpha_{i}}\in\mathbb{R}^{n_{\alpha_{i}}}$ is the torque inputs of the robotic arms’ joints. By inverting (3) and using (2) and its derivative, we can obtain the task-space agent dynamics [19]:

[TABLE]

with the corresponding task-space terms:

[TABLE]

The task-space input wrench $u_{i}$ can be translated to the joint space inputs $\tau_{i}\in\mathbb{R}^{{n}_{i}}$ via $\tau_{i}=J_{i}^{\top}(q_{i})u_{i}+(I_{{{n_{i}}}}-J^{\top}_{i}(q_{i})\bar{J}^{\top}_{i}(q_{i}))\tau_{i_{0}}$, where $\bar{J}_{i}$ is a generalized inverse of $J_{i}$ [19]. The term $\tau_{i_{0}}$ concerns over-actuated agents and does not contribute to end-effector forces.

We define by $\mathcal{A}_{i}(q_{i})\triangleq\mathcal{O}_{i},i\in\mathcal{N}$, the ellipsoid that bounds the $i$ th agent’s volume with the corresponding centers $c_{i}$ and semi-axes $\beta_{i,1},\beta_{i,2},\beta_{i,3}$, i.e., the workspace of the arm of agent $i$ [19] enlarged so that it includes the $i$th base. Note that $\mathcal{A}_{i}$ depends on $q_{i}$ and can be explicitly found.

III-A2 Object Dynamics

Regarding the object, we denote as $x_{\scriptscriptstyle O}:\mathbb{R}_{\geq 0}\rightarrow\mathbb{M}$, $v_{\scriptscriptstyle O}:\mathbb{R}_{\geq 0}$ $\rightarrow\mathbb{R}^{6}$ the pose and velocity of the object’s center of mass, with $x_{\scriptscriptstyle O}(t)=[p^{\top}_{\scriptscriptstyle O}(t),\eta^{\top}_{\scriptscriptstyle O}(t)]^{\top}$, $p_{\scriptscriptstyle O}(t)=[x_{\scriptscriptstyle O}(t),y_{\scriptscriptstyle O}(t),z_{\scriptscriptstyle O}(t)]^{\top}$, $\eta_{\scriptscriptstyle O}(t)=[\phi_{\scriptscriptstyle O}(t),\theta_{\scriptscriptstyle O}(t),\psi_{\scriptscriptstyle O}(t)]^{\top}$ and $v_{\scriptscriptstyle O}(t)=[\dot{p}^{\top}_{\scriptscriptstyle O}(t),\omega^{\top}_{\scriptscriptstyle O}(t)]^{\top}$. The second order dynamics of the object are given by:

[TABLE]

where $M_{\scriptscriptstyle O}:\mathbb{M}\rightarrow\mathbb{R}^{6\times 6}$ is the positive definite inertia matrix, $C_{{\scriptscriptstyle O}}:\mathbb{M}\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6\times 6}$ is the Coriolis matrix, $g_{\scriptscriptstyle O}:\mathbb{M}\rightarrow\mathbb{R}^{6}$ is the gravity vector, which are derived from the Newton-Euler formulation. In addition, $J_{\scriptscriptstyle O_{r}}:\mathbb{M}\rightarrow\mathbb{R}^{6\times 6}$ is the object representation Jacobian $J_{\scriptscriptstyle O_{r}}(x_{\scriptscriptstyle O})=\text{diag}\{I_{3},J_{\scriptscriptstyle O_{r,\theta}}(x_{\scriptscriptstyle O})\}$, with

[TABLE]

which is singular when $\theta_{\scriptscriptstyle O}=\pm\tfrac{\pi}{2}$. Finally, $\lambda_{\scriptscriptstyle O}\in\mathbb{R}^{6}$ is the force vector acting on the object’s center of mass. Also, similarly to the robotic agents, we define by $\mathcal{C}_{\scriptscriptstyle O}(x_{\scriptscriptstyle O})\triangleq\mathcal{O}_{\scriptscriptstyle O}$, as the bounding ellipsoid of the object.

III-A3 Coupled Dynamics

Consider $N$ robotic agents rigidly grasping an object. Then, the coupled system object-agents behaves like a closed-chain robot and we can express the object’s pose and velocity as a function of $q_{i}$ and $\dot{q}_{i}$, $\forall i\in\mathcal{N}$. In view of Fig. 2, we have that

[TABLE]

$\forall i\in\mathcal{N}$, where $p^{\scriptscriptstyle E_{i}}_{{\scriptscriptstyle E_{i}/O}}$ represents the constant distance and $\eta_{\scriptscriptstyle E_{i}/O}$ the relative orientation offset between the $i$th agent’s end-effector and the object’s center of mass, which are considered known. The grasp rigidity implies that $\omega_{\scriptscriptstyle E_{i}}$ $=\omega_{\scriptscriptstyle O}$, $\forall i\in\mathcal{N}$. Therefore, by differentiating (6a), we obtain

[TABLE]

which, by time differentiation, yields

[TABLE]

where $J_{\scriptscriptstyle O_{i}}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{6\times 6}$ is a smooth mapping representing the Jacobian from the object to the $i$-th agent:

[TABLE]

and is always full rank due to the grasp rigidity.

Remark 2**.**

Since the geometric object parameters $p^{\scriptscriptstyle E_{i}}_{\scriptscriptstyle E_{i}/O}$ and $\eta_{\scriptscriptstyle E_{i}/O}$ are known, each agent can compute $p_{\scriptscriptstyle O},\eta_{\scriptscriptstyle O}$ and $v_{\scriptscriptstyle O}$ simply by inverting (6) and (7), respectively, without employing any sensory data. In the same vein, all agents can also compute the object’s bounding ellipsoid $\mathcal{C}_{\scriptscriptstyle O}$, which depends on $q$.

The Kineto-statics duality [19] along with the grasp rigidity suggest that the force $\lambda_{\scriptscriptstyle O}$ acting on the object center of mass and the generalized forces $\lambda_{i},i\in\mathcal{N}$, exerted by the agents at the contact points are related through

[TABLE]

where $\bar{\lambda}=[\lambda^{\top}_{1},\cdots,\lambda^{\top}_{N}]^{\top}\in\mathbb{R}^{6N}$ and $G:\mathbb{R}^{{n}}\rightarrow\mathbb{R}^{6N\times 6}$ is the grasp matrix, with $G(q)=[J_{{\scriptscriptstyle O_{1}}}^{\top},\cdots,J_{{\scriptscriptstyle O_{N}}}^{\top}]^{\top}$.

Next, we substitute (7) and (8) in (4) and we obtain in vector form after rearranging terms:

[TABLE]

where we have used the stack forms $\bar{M}=\text{diag}\{\left[M_{i}\right]_{i\in\mathcal{N}}\}$, $\bar{C}=\text{diag}\{\left[C_{i}\right]_{i\in\mathcal{N}}\}$, $\bar{g}=[g_{1}^{\top},\dots,g_{N}^{\top}]^{\top}$, and $u=[u_{1}^{\top},\dots,u_{N}^{\top}]^{\top}$. By substituting (10) and (5) in (9) and by noticing from (6) that $x_{\scriptscriptstyle O}$ depends on $q$ owing to the grasp rigidity, we obtain the coupled dynamics:

[TABLE]

where:

[TABLE]

Remark 3**.**

Note that the agents dynamics under consideration hold for generic robotic agents comprising of a moving base and a robotic arm. Hence, the considered framework can be applied for mobile, aerial, or underwater manipulators.

We can now formulate the problem considered in this work:

Problem 1**.**

Consider $N$ robotic agents rigidly grasping an object, governed by the coupled dynamics (11). Given the desired pose $x_{\scriptscriptstyle O,\text{des}}$, design the control input $u:\mathbb{R}_{\geq 0}\to\mathbb{R}^{6N}$ such that $\lim\limits_{t\to\infty}x_{\scriptscriptstyle O}(t)=x_{\scriptscriptstyle O,\text{des}}$, while ensuring the satisfaction of the following collision avoidance and singularity properties:

$\mathcal{A}_{i}(q_{i})\cap\mathcal{O}_{z}=\emptyset,\forall i\in\mathcal{N},z\in\mathcal{Z}$, 2. 2.

$\mathcal{C}_{\scriptscriptstyle O}(x_{\scriptscriptstyle O})\cap\mathcal{O}_{z}=\emptyset,\forall z\in\mathcal{Z}$, 3. 3.

$\mathcal{A}_{i}(q_{i})\cap\mathcal{A}_{j}(q_{j})=\emptyset,\forall i,j\in\mathcal{N},i\neq j$, 4. 4.

$-\tfrac{\pi}{2}<-\bar{\theta}\leq\theta_{\scriptscriptstyle O}\leq\bar{\theta}<\tfrac{\pi}{2}$, 5. 5.

$-\tfrac{\pi}{2}<-\bar{\theta}\leq\theta_{\scriptscriptstyle B_{i}}\leq\bar{\theta}<\tfrac{\pi}{2}$, 6. 6.

$q_{i}\in\widetilde{\mathcal{Q}}_{i}$.

for a $0<\bar{\theta}<\frac{\pi}{2}$, as well as the input and velocity magnitude and input constraints: $\lvert\tau_{i_{k}}\rvert\leq\bar{\tau}_{i},\lvert\dot{q}_{i_{k}}\rvert\leq\bar{\dot{q}}_{i},\forall k\in\{1,\dots,n_{i}\},i\in\mathcal{N}$, for some positive constants $\bar{\tau}_{i},\bar{\dot{q}}_{i},i\in\mathcal{N}$.

The aforementioned constraints correspond to the following specifications:

• •

$1)$ stands for collision avoidance between the agents and the obstacles.

• •

$2)$ stands for collision avoidance between the object and the obstacles.

• •

$3)$ stands for collision avoidance between the agents.

• •

$4)$ stands for representation singularity avoidance of the object.

• •

$5)$ stands for representation singularity avoidance of the agents’ bases.

• •

$6)$ stands for kinematic singularity avoidance of the agents.

In order to solve the aforementioned problem, we need the following reasonable assumption regarding the workspace:

Assumption 1**.**

(Problem Feasibility Assumption) The distance between any pair of obstacles is sufficiently large such that the coupled system object-agents can navigate among them without collisions.

We also define the following sets for every $i\in\mathcal{N}$:

[TABLE]

associated with the desired collision-avoidance properties.

IV Problem Solution

In this section, a systematic solution to Problem 1 is introduced. Our overall approach builds on designing a Nonlinear Model Predictive control scheme the system of the manipulators and the object. Nonlinear Model Predictive Control (see e.g. [20, 21, 22, 23, 24, 25, 26, 27, 28]) have been proven suitable for dealing with nonlinearities and state and input constraints.

The coupled agents-object nonlinear dynamics can be written in compact form as follows:

[TABLE]

where $x=[x_{\scriptscriptstyle O}^{\top},v_{\scriptscriptstyle O}^{\top},q^{\top}]^{\top}\in\mathbb{R}^{n+12},u\in\mathbb{R}^{6N}$ and

[TABLE]

where we have also used that:

[TABLE]

The expression for $f_{3}(x,u)$ is derived by employing (8) and (2). Note that $f$ is locally Lipschitz continuous in its domain since it is continuously differentiable in its domain. Next, we define the respective errors:

[TABLE]

where $q_{\text{des}}=[q_{1,\text{des}},\dots,q_{N,\text{des}}]^{\top}$ is appropriately chosen such that $x_{\scriptscriptstyle O}(t)=x_{\scriptscriptstyle O,\text{des}},\forall t\text{ s.t. }q(t)=q_{\text{des}}$ (see (6)), and $\dot{x}_{\scriptscriptstyle O,\text{des}}=\dot{q}_{\text{des}}=0$. The error dynamics are then $\dot{e}(t)=f(x(t),u(t))$, which can be appropriately transformed to be written as:

[TABLE]

where $f_{e}(t)\triangleq f(e(t)+x_{\text{des}},u(t)$. By ignoring over-actuated input terms, we have that $\tau_{i}=J_{i}^{\top}(q_{i})u_{i}$, which becomes

[TABLE]

where we have employed the property $\sigma_{\min}(J^{\top}_{i})\lVert u_{i}\rVert\leq\lVert J^{\top}_{i}u_{i}\rVert$, with $\sigma_{\min}(J^{\top}_{i})$ denoting the minimum singular value of $J^{\top}_{i}$, which is strictly positive, if the constraint $q_{i}\in\widetilde{\mathcal{Q}}_{i}$ is always satisfied. Hence, the constraint $\lvert\tau_{i_{k}}\rvert\leq\bar{\tau}_{i}$ is equivalent to

[TABLE]

Let us now define the following set $U\subseteq\mathbb{R}^{6N}$:

[TABLE]

as the set that captures the control input constraints of the error dynamics system (16). Define also the set $X\subseteq\mathbb{R}^{n+12}$:

[TABLE]

The set $X$ captures all the state constraint of the system dynamics (13). In view of (15), we define the set $E\subseteq\mathbb{R}^{n+12}$ as:

[TABLE]

as the set that captures all the constraints of the error dynamics system (16).

The problem in hand is the design of a control input $u(t)\in U$ such that $\lim_{t\to\infty}\|e(t)\|=0$ while ensuring $e(t)\in E,\forall t\in\mathbb{R}_{\geq 0}$. In order to solve the aforementioned problem, we propose a Nonlinear Model Predictive scheme, that is presented hereafter.

Consider a sequence of sampling times $\{t_{i}\}_{i\geq 0}$ with a constant sampling period $0<h<T_{p}$, where is $T_{p}$ is the prediction horizon, such that:

[TABLE]

In the sampling-data NMPC, a finite-horizon open-loop optimal control problem (OCP) is solved at discrete sampling time instants $t_{i}$ based on the current state error information $e(t_{i})$. The solution is an optimal control signal $\hat{u}(t)$, for $t\in[t_{i},t_{i}+T_{p}]$. For more details, the reader is referred to [21]. The open-loop input signal applied in between the sampling instants is given by the solution of the following Optimal Control Problem (OCP):

[TABLE]

where the hat $\hat{\cdot}$ denotes the predicted variables (internal to the controller), i.e. $\hat{e}(\cdot)$ is the solution of (21b) driven by the control input $\hat{u}(\cdot):[t_{i},t_{i}+T_{p}]\to\mathcal{U}$ with initial condition $e(t_{i})$. Note that the predicted values are not necessarily the same with the actual closed-loop values (see [21]). The term $F:E\times U\to\mathbb{R}_{\geq 0}$, is the running cost, and is chosen as:

[TABLE]

The terms $V:E\to\mathbb{R}_{>0}$ and $\mathcal{E}_{f}$ are the terminal penalty cost and terminal set, respectively, and are used to enforce the stability of the system (see Section 4.2). The terminal cost is given by $V(e)=e^{\top}Pe$. The terms $Q\in\mathbb{R}^{(n+12)\times(n+12)}_{\geq 0}$, $P\in\mathbb{R}^{(n+12)\times(n+12)}_{>0}$ and $R\in\mathbb{R}^{6N\times 6N}_{>0}$ are chosen as:

[TABLE]

where $\widetilde{q}_{i}\in\mathbb{R}_{\geq 0},\widetilde{p}_{i}\in\mathbb{R}_{>0},\forall i\in\{1,\dots,n+12\}$ and $\widetilde{r}_{j}\in\mathbb{R}_{>0},\forall j\in\{1,\dots,6N\}$ are constant weights.

Lemma 2**.**

There exist functions $\alpha_{1}$, $\alpha_{2}\in\mathcal{K}_{\infty}$ such that:

[TABLE]

for every $z\triangleq\left[e^{\top},u^{\top}\right]^{\top}\in\mathcal{E}\times\mathcal{U}$.

Proof.

The proof can be found in Appendix A. ∎

The solution of the OCP (21a)-(21d) at time $t_{i}$ provides an optimal control input denoted by $\hat{u}^{\star}(t;e(t_{i}))$, for $t\in[t_{i},t_{i}+T_{p}]$. It defines the open-loop input that is applied to the system until the next sampling instant $t_{i+1}$:

[TABLE]

The corresponding optimal value function is given by:

[TABLE]

where $J(\cdot)$ as is given in (21a). The control input $u(t;e(t_{i}))$ is a feedback, since it is recalculated at each sampling instant using the new state information. The solution of (16) starting at time $t_{1}$ from an initial condition $e(t_{1})$, applying a control input $u:[t_{1},t_{2}]\to\mathcal{U}$ is denoted by $e(s;u(\cdot),e(t_{1})),s\in[t_{1},t_{2}]$. The predicted state of the system (16) at time $t_{i}+s,s>0$ is denoted by $\hat{e}(t_{i}+s;u(\cdot),e(t_{i}))$ and it is based on the measurement of the state $e(t_{i})$ at time $t_{i}$, when a control input $u(\cdot;e(t_{i}))$ is applied to the system (16) for the time period $[t_{i},t_{i}+s]$. Thus, it holds that:

[TABLE]

We define an admissible control input as:

Definition 2**.**

A control input $u:[0,T_{p}]\to\mathbb{R}^{6N}$ for a state $e_{0}$ is called admissible, if all the following hold:

$u(\cdot)$ is piecewise continuous; 2. 2.

$u(s)\in U,\forall\ s\in[0,T_{p}]$; 3. 3.

$e(s;u(\cdot),e_{0})\in E,\forall\ s\in[0,T_{p}]$; 4. 4.

$e(T_{p};u(\cdot),e_{0})\in\mathcal{E}_{f}$;

Lemma 3**.**

The terminal penalty function $V(\cdot)$ is Lipschitz continues in $\mathcal{E}_{f}$, with Lipschitz constant $L_{V}=2\varepsilon_{0}\sigma_{\max}(P)$, for all $e(t)\in\mathcal{E}_{f}$.

Proof.

The proof can be found in Appendix B. ∎

Through the following theorem, we guarantee the stability of the system which is the solution to Problem 1.

Theorem 1**.**

Consider the Assumptions 1,2. Suppose also that:

The OCP (21a)-(21d) is feasible for the initial time $t=0$. 2. 2.

The terminal set $\mathcal{E}_{f}\subseteq E$ is closed, with $0_{n+12}\in\mathcal{E}_{f}$. 3. 3.

The terminal set $\mathcal{E}_{f}$ is chosen such that there exists an admissible control input $u_{f}:[0,h]\to\mathcal{U}$ such that for all $e(s)\in\mathcal{E}_{f}$ it holds that:

1. (a)

$e(s)\in\mathcal{E}_{f},\forall\ s\in[0,h]$. 2. (b)

$\displaystyle\frac{\partial V}{\partial{e}}f_{e}(e(s),u_{f}(s))+F(e(s),u_{f}(s))\leq 0,\forall\ s\in[0,h].$**

Then, the closed loop trajectories of the system (16), converges to the set $\mathcal{E}_{f}$, as $t\to\infty$.

Proof.

As usual in predictive control the proof consists of two parts: in the first part it is established that initial feasibility implies feasibility afterwards. Based on this result it is then shown that the error $e(t)$ converges to the terminal set $\mathcal{E}_{f}$. The feasibility analysis can be found in Appendix C. The convergence analysis can be found in D. ∎

V Simulation Results

To demonstrate the efficiency of the proposed control protocol, we consider two simulation scenarios.

Scenario 1: Consider $N=2$ ground vehicles equipped with $2$ DOF manipulators, rigidly grasping an object with $n_{1}=n_{2}=4,n=n_{1}+n_{2}=8$. From (13) we have that $x=[x_{\scriptscriptstyle O}^{\top},v_{\scriptscriptstyle O}^{\top},q^{\top}]^{\top}\in\mathbb{R}^{16},u\in\mathbb{R}^{8}$, with $x_{\scriptscriptstyle O}=[p_{\scriptscriptstyle O}^{\top},\phi_{\scriptscriptstyle O}]^{\top}\in\mathbb{R}^{4}$, $v_{\scriptscriptstyle O}=[\dot{p}_{\scriptscriptstyle O}^{\top},\omega_{\scriptscriptstyle x_{O}}]^{\top}\in\mathbb{R}^{4}$, $p_{\scriptscriptstyle O}=[x_{\scriptscriptstyle O},y_{\scriptscriptstyle O},z_{\scriptscriptstyle O}]^{\top}\in\mathbb{R}^{3},q=[q_{1}^{\top},q_{2}^{\top}]^{\top}\in\mathbb{R}^{8}$, $q_{i}=[p_{\scriptscriptstyle B_{i}}^{\top},\alpha_{i}^{\top}]^{\top}\in\mathbb{R}^{4}$, $p_{\scriptscriptstyle B_{i}}=[x_{\scriptscriptstyle B_{i}},y_{\scriptscriptstyle B_{i}}]^{\top}\in\mathbb{R}^{2}$, $\alpha_{i}=[\alpha_{i_{1}},\alpha_{i_{2}}]^{\top}\in\mathbb{R}^{2},i\in\{1,2\}$. The manipulators become singular when $\sin(\alpha_{i_{1}})=0\},i\in\{1,2\}$, thus the state constraints for the manipulators are set to:

[TABLE]

We also consider the input constraints:

[TABLE]

The initial conditions are set to:

[TABLE]

The desired goal states are set to:

[TABLE]

We set an obstacle between the initial and the desired pose of the object. the obstacle is spherical with center $[5,5,1]$ and radius $2$. The sampling time is $h=0.1\sec$, the horizon is set to $T_{p}=0.3\sec$, and the total simulation time is $80\sec$; The matrices $P,Q,R$ are set to:

[TABLE]

The simulation results are depicted in Fig. 3- Fig. 8, which shows that the states of the agents as well as the states of the object converge to the desired ones while guaranteeing that the obstacle is avoided and all state and input constraints are met.

Scenario 2: Consider $N=3$ ground vehicles equipped with $2$ DOF manipulators, rigidly grasping an object with $n_{1}=n_{2}=n_{3}=4,n=n_{1}+n_{2}+n_{3}=12$. From (13) we have that $x=[x_{\scriptscriptstyle O}^{\top},v_{\scriptscriptstyle O}^{\top},q^{\top}]^{\top}\in\mathbb{R}^{20}$, $u\in\mathbb{R}^{12}$, with $x_{\scriptscriptstyle O}=[p_{\scriptscriptstyle O}^{\top},\phi_{\scriptscriptstyle O}]^{\top}\in\mathbb{R}^{4}$, $v_{\scriptscriptstyle O}=[\dot{p}_{\scriptscriptstyle O}^{\top},\omega_{\scriptscriptstyle x_{O}}]^{\top}\in\mathbb{R}^{4}$, $p_{\scriptscriptstyle O}=[x_{\scriptscriptstyle O},y_{\scriptscriptstyle O},z_{\scriptscriptstyle O}]^{\top}\in\mathbb{R}^{3}$, $q=[q_{1}^{\top},q_{2}^{\top},q_{3}^{\top}]^{\top}\in\mathbb{R}^{12}$, $q_{i}=[p_{\scriptscriptstyle B_{i}}^{\top},\alpha_{i}^{\top}]^{\top}\in\mathbb{R}^{4}$, $p_{\scriptscriptstyle B_{i}}=[x_{\scriptscriptstyle B_{i}},y_{\scriptscriptstyle B_{i}}]^{\top}\in\mathbb{R}^{2}$, $\alpha_{i}=[\alpha_{i_{1}},\alpha_{i_{2}}]^{\top}\in\mathbb{R}^{2},i\in\{1,2\}$. The manipulators become singular when $\sin(\alpha_{i_{1}})=0\},i\in\{1,2,3\}$, thus the state constraints for the manipulators are set to:

[TABLE]

We also consider the input constraints:

[TABLE]

The initial conditions are set to:

[TABLE]

The desired goal states are set to:

[TABLE]

The sampling time is $h=0.1\sec$, the horizon is set to $T_{p}=0.5\sec$, and the total simulation time is $100\sec$; The matrices $P,Q,R$ are set to:

[TABLE]

The simulation results are depicted in Fig. 9- Fig. 16, which shows that the states of the agents as well as the states of the object converge to the desired ones while guaranteeing that all state and input constraints are met. The simulation scenarios were carried out by using the NMPC toolbox given in [25] and they took $23500\sec$, $45547\sec$ for Scenario $1$ and Scenario $2$, respectively, in MATLAB Environment on a desktop with $8$ cores, $3.60$ GHz SPU and $16$GB of RAM.

VI Conclusions and Future Work

In this work we proposed a NMPC scheme for the cooperative transportation of an object rigidly grasped by $N$ robotic agents. The proposed control scheme deals with singularities of the agents, inter-agent collision avoidance as well as collision avoidance between the agents and the object with the workspace obstacles. We proved the feasibility and convergence analysis of the proposed methodology and simulation results verified the efficiency of the approach. Future efforts will be devoted towards including load sharing coefficients, internal force regulation, and complete decentralization of the proposed method. Finally, we will try to decrease the overall complexity and carry out real-time experiments.

Appendix A Proof of Lemma 2

By invoking the fact that:

[TABLE]

we have:

[TABLE]

and:

[TABLE]

where $z=\left[e^{\top},u^{\top}\right]^{\top}$. Thus, we get:

[TABLE]

By defining the $\mathcal{K}_{\infty}$ functions $\alpha_{1}$, $\alpha_{2}:\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$:

[TABLE]

and the parameter $m\in\mathbb{R}_{>0}$ by:

[TABLE]

we get:

[TABLE]

which leads to the conclusion of the proof. ∎

Appendix B Proof of Lemma 2

Proof.

For every $e(t)\in\mathcal{E}_{f}$, the following holds:

[TABLE]

By employing the property that:

[TABLE]

(29) is written as:

[TABLE]

which completes the proof. ∎

Appendix C Feasibility Analysis

Consider any sampling time instant $t_{i}$ for which a solution exists. In between $t_{i}$ and $t_{i+1}$, the optimal control input $\hat{u}^{\star}(s;e(t_{i})),s\in[t_{i},t_{i+1})$ is implemented. According to (25), it holds that:

[TABLE]

The remaining piece of the optimal control input $\hat{u}^{\star}(s;e(t_{i})),s\in[t_{i+1},t_{i}+T_{p}]$ satisfies the state and input constraints $E,U$, respectively. Furthermore,

[TABLE]

and we know from Assumption 2b of Theorem 1 that for all $e(t)\in\mathcal{E}_{f}$, there exists at least one control input $u_{f}(\cdot)$ that renders the set $\mathcal{E}_{f}$ invariant over $h$. Picking any such input, a feasible control input $\bar{u}(\cdot;e(t_{i+1}))$, at time instant $t_{i+1}$, may be the following:

[TABLE]

Thus, from feasibility of $\hat{u}^{\star}(s,e(t_{i}))$ and the fact that $u_{f}(e(t))\in U$, for all $e(t)\in\mathcal{E}_{f}$, it follows that:

[TABLE]

Hence, the feasibility at time $t_{i}$ implies feasibility at time $t_{i+1}$. Therefore, if the OCP (21a) - (21d) is feasible at time $t=0$, it remains feasible for every $t\geq 0$.

Appendix D Convergence Analysis

The second part involves proving convergence of the state $e$ in the terminal set $\mathcal{E}_{f}$. In order to prove this, it must be shown that a proper value function is decreasing along the solution trajectories starting at a sampling time $t_{i}$. Consider the optimal value function $J^{\star}(e(t_{i}))$, as is given in (24). Consider also the cost of the feasible control input, indicated by:

[TABLE]

where $t_{i+1}=t_{i}+h$, as is given in (20). Define:

[TABLE]

$e_{1}(s)$ stands for the predicted state $e$ at time $s$, based on the measurement of the state $e$ at time $t_{i+1}$, while using the feasible control input $\bar{u}(s;e(t_{i+1}))$. Let us also define th the following terms:

[TABLE]

(32), (33) form convenient notations for the readability of the proof hereafter.

By employing (21a), (24) and (31), the difference between the optimal and feasible cost is given by:

[TABLE]

Note that, from (30), the following holds:

[TABLE]

By combining (32), (33) and (35), it yields that:

[TABLE]

which implies that:

[TABLE]

The combination of (36) and (37) implies that:

[TABLE]

which implies that:

[TABLE]

By employing (38), (34) becomes:

[TABLE]

Due to the fact that $t_{i+1}+T_{p}-(t_{i}+T_{p})=t_{i+1}-t_{i}=h$, and the Assumption 2b of Theorem 1 holds for one sampling period $h$, by integrating this inequality from $t_{i}+T_{p}$ to $t_{i+1}+T_{p}$ and we get the following:

[TABLE]

By employing the property $y\leq|y|,\forall y\in\mathbb{R}$, we get:

[TABLE]

By employing Lemma 2, we have that:

[TABLE]

By combining (40) and (41) we get:

[TABLE]

For $s=t_{i}+T_{p}$, (37) gives:

[TABLE]

By combining (43) and (42) we have:

[TABLE]

By combining (39) with (44), the following holds:

[TABLE]

By substituting $e=e_{2}(s),u=u_{2}(s)$ in (28) we get:

[TABLE]

where $z_{2}\triangleq[e_{2},u_{2}]^{\top}$. The latter is equivalent to:

[TABLE]

By combining (45) and (46) we finally get:

[TABLE]

It is clear that the optimal solution at time $t_{i+1}$ i.e., $J^{\star}(e(t_{i+1}))$ will not be worse than the feasible one at the same time i.e. $\bar{J}(e(t_{i+1}))$. Therefore, (47) implies:

[TABLE]

or, by using the fact that $\displaystyle\int_{t_{0}}^{t_{i}}\|z_{2}(s)\|^{2}ds$ $=\displaystyle\sum_{j=0}^{i-1}\int_{t_{j}}^{t_{j+1}}\|z_{2}(s)\|^{2}ds$, equivalently, we obtain:

[TABLE]

By using induction and the fact that $t_{i}=h\cdot i,t_{i+1}=h\cdot(i+1),\forall i\geq 0$, from (20), (49) is written as:

[TABLE]

Since $t_{0}=0$ we obtain:

[TABLE]

which implies that:

[TABLE]

By combining (48), (52), we obtain:

[TABLE]

Therefore, the value function $J^{\star}(e(t_{i}))$ has proven to be non-increasing for all the sampling times. Let us define the function:

[TABLE]

where $s=\max\{t_{i}:t_{i}\leq t\}$. Since $J^{\star}(e(0))$ is bounded, (54) implies that $V(e(t))$ is bounded. Since the signals $e(t),u(t)$ are bounded ($e(t)\in E,u(t)\in U$), according to (16), it holds that $\dot{e}(t)$ is also bounded. From (51) we have that:

[TABLE]

which due to the fact that $s\leq t$, is equivalent to:

[TABLE]

From (55), we get:

[TABLE]

Since $J^{\star}(e(0)),V(e(t))$ has been proven to be bounded, the term $\displaystyle\int_{0}^{t}\|z_{2}(s)\|^{2}ds$ is also bounded. Therefore, by employing Lemma 1, we have that $\|z_{2}(t)\|\to 0$, as $t\to\infty$. The latter implies that:

[TABLE]

and leads to the conclusion of the proof.