Viability and Feasibility of Constrained Kinematic Control of Manipulators

Abstract

Recent advances in planning and control of robot manipulators make an increasing use of optimization-based techniques, such as model predictive control. In this framework, ensuring the feasibility of the online optimal control problem is a key issue. In the case of manipulators with bounded joint positions, velocities, and accelerations, feasibility can be guaranteed by limiting the set of admissible velocities and positions to a viable set. However, this results in the imposition of nonlinear optimization constraints. In this paper, we analyze the feasibility of the optimal control problem and we propose a method to construct a viable convex polyhedral that ensures feasibility of the optimal control problem by means of a given number of linear constraints. Experimental and numerical results on an industrial manipulator show the validity of the proposed approach.

1. Introduction

Robotic systems typically present kinematic and/or dynamic limitations. For example, joint positions are generally bounded within an available range of motion, while actuators implicitly present velocity and acceleration/torque limits. Further constraints can be due to the environment where the robot has to operate (e.g., partial occupation of the robot workspace) or to safety reasons, which may determine velocity and acceleration limitations. Including such constraints in the development of planning and control methods is of utter importance, as their violation might lead to unrealizable motions (with consequent significant errors in the execution of the tasks) or to safety issues (for example, in the case workspace limits are not respected).

Constrained methods are widely diffused, for example, in the case of offline trajectory planning. The general approach consists in the formulation of a constrained optimization problem that should optimize a given objective, such as minimum execution time [1] or minimum energy consumption [2]. This is also the case of global redundancy resolution methods [3], where the robot has to perform an assigned task, and the redundancy can be exploited to optimize a desired objective, such as maximum manipulability [4], dexterity [5], or maximum joint range availability [6]. However, all these methods are typically performed offline as they need the prior knowledge of the task to be performed, and they lead to heavy computational burdens that do not permit their online execution. For this reason, they are not able to handle online trajectory generation and re-planning techniques and this is a relevant limitation, considering recent robotic applications such as collaborative robotics and robots in unstructured environments. To handle the robot constraints in online planning and control methods, optimization-based techniques are becoming more and more widespread as the increasing computing power of modern processors allows their real-time implementation with small sampling periods.

Quadratic programming, for example, is intensively exploited in the resolution of the Inverse Kinematic (IK) problem of redundant manipulators [7,8,9,10,11]. In brief, a manipulator is termed kinematically redundant with respect to a task when it has a number of degrees of freedom greater than the one of the desired task. The IK problem does not present a unique solution, as the transformation between the joint and the task space results in an undetermined system. Such problem is usually addressed at differential level in order to exploit the linear relation between joint and task velocities given by the Jacobian of the robot. This can be formulated as a chain of Quadratic Programs (QP) with decreasing priorities: the tasks are written as cost functions and the robot limits are represented as linear constraints. The possibility of including robot constraints in the QP represents a great advantage with respect to other typical methods (such as the unconstrained projection of secondary tasks in the null space of the Jacobian [12,13]). Further developments in this field explicitly face the problem of possible saturations and task deformations by introducing a scaling variable into the QP to slow down the execution of the tasks if needed [14].

A further improvement of these methods is represented by the use of Model Predictive Control (MPC) techniques, which aim at overcoming issues related to the fact that the previous methods only consider the current state of the robot. In fact, MPC-based methods are able to take into account the future evolution of the system, the tasks, and the constraints, improving the behavior of the robot in terms of task satisfaction and smoothness [15,16]. MPC is also applied to constrained motion planning [17,18] or low-level control applications [19].

Disregarding the particular field of application, a fundamental issue to address when using optimization-based control techniques is the feasibility of the online optimization problem. In fact, feasibility must be ensured in any possible state of the system. Otherwise, a solution to the problem might not exist, and this could cause the algorithm to stop (unless specific but sub-optimal strategies are activated when infeasibility occurs).

Feasibility of the online optimization problem is strictly related to the concept of set viability [20]. A set is said to be viable if, given an initial state within such set, the state trajectory can be kept within the set by means of a proper and realizable input function. In other words, keeping the state within a viable set ensures feasibility for all future time instants. On the contrary, if the state exits the viable set, infeasibility of the control problem will surely occur at a certain time in the future. This is particularly relevant in the case of state-constrained systems, as infeasibility might occur if no viability conditions are added to the control problem but only state and input limits are taken into account.

For robot manipulator control, this issue is common to any control strategy that takes into account both position and acceleration/torque constraints. In this case, the simplistic imposition of box constraints on the position and the acceleration/torque of the robot does not ensure the existence of a feasible solution to the optimization problem. For example, if a robot joint approaches its position limit with high velocity, the position bound will be exceeded due to the bounded admissible deceleration. Few works addressed this issue by means of manually-tuned heuristic strategies that aimed at reducing the velocity of the robot when it approached its position limits [21,22,23]. Many other control strategies did not take into account position bounds, assuming that the reference trajectories were implicitly feasible [24,25]. Notice that the viability guarantee does not ensure the feasibility of the reference trajectory, which could be only verified offline when the whole trajectory is known a priori. Such approach does not apply to online methods, which should be able to handle online trajectory generation and re-planning. In these cases, we can only ensure the feasibility of the optimal control control, which typically aims at minimizing the deviation of the performed motion with respect to the reference trajectory. This means that, although the desired motion might be infeasible, the optimal control problem remains feasible. In this case, a deformation with respect to the nominal trajectory could arise.

A formal viability guarantee was given in [26], which proposed an invariance control scheme for constrained robot manipulators. In particular, viability conditions were derived in order not to exceed workspace limitations with bounded joint accelerations. In [14], a local redundancy resolution technique was proposed: to respect a given deceleration limit in the breaking phase, the position-velocity state region was limited by imposing that the robot was able to stop within the given joint limit (in fact, limiting the state region to a viable set). This approach was similarly adopted in [27] in the context of robust constrained motion planning. Recently, viability for joint-constrained manipulators was explicitly addressed in [28], with a particular focus on the discrete-time implementation of the viability constraints.

All the above-mentioned approaches were based on the derivation of an analytical viability condition for a double integral system with bounded input and output. The resulting condition is quadratic in the velocity state. However, as all the above-mentioned works refer to local/feedback methods, such condition could be easily linearized around the current system state, resulting in variable box constraints on the control actions. However, such linearization is not possible for predictive strategies. In this case, a linear approximation of the viability set should be obtained. For example, Faroni et al. [29] gives a sufficient condition to approximate the quadratic viable set with a linear constraint and maintaining the viability property. Although the approximation technique is easy to implement and does not remarkably increase the computational complexity of the problem, the resulting allowed state region could result to be significantly smaller than the original one.

In this paper, we propose a method to derive a viable convex polyhedron for a robotic system with bounded joint positions, velocities, and accelerations. In particular, a simple optimization problem is set up to determine the maximum polyhedron that approximates the original viable set. The original quadratic condition is approximated by a polyhedron with a given number of sides, by maximizing the area of the allowed velocity-position state-region. Moreover, viability of the resulting set is ensured by imposing that, for all points on the polyhedron boundary, there exists a realizable input action that keeps the next state within the polyhedron itself, which is also demonstrated to be convex and can be therefore rewritten as a linear inequality constraint in optimization-based algorithms (such as linear MPC techniques). The paper shows that, by increasing the number of sides, the polyhedron gives a better approximation of the original viable set, as expected. This means that a larger admissible state region can be exploited by the controller when the resulting constraints are included in the optimization problem. Consequently, the controller can obtain smaller tracking errors when the desired task requires the robot joint to get close to the maximal viable set boundary. In particular, numerical results show the enlargement of the admissible state region as the number of sides increases. Such improvement is more and more evident as the maximum acceleration values gets smaller. Finally, experimental results on a Universal Robots UR10 manipulator demonstrates the validity of the proposed approach. In particular, an MPC algorithm is applied to the tracking problem of a given joint-space trajectory and the results with different viability conditions are compared. Firstly, an experimental example shows that the use of an invariance condition is of vital importance to ensure both the feasibility of the online optimization problem and the satisfaction of the manipulator limits. Secondly, the results show that the viable sets obtained by means of the proposed method permits to obtain smaller (or null) tracking errors in the case the required motion is close to the viability boundary, increasing the performance of the MPC controller.

The paper is organized as follows. Section 2 introduces the concept of set viability applied to constrained kinematic control of robot manipulators. Section 3 illustrates the proposed method for the computation of the optimal viable polyhedron and gives viability and convexity proves. Numerical results demonstrate the effectiveness of the proposed approach in Section 4, while experimental results on a Universal Robot UR10 manipulator are shown in Section 5. Section 6 concludes the paper.

2. Feasibility of the Constrained Kinematic Control Problem

Consider a generic robot joint and denote with q, $\dot{q}$, $\ddot{q}$ its position, velocity, and acceleration, respectively. The limits on q, $\dot{q}$, and $\ddot{q}$ can be therefore expressed as:

$$\begin{array}{c}\hfill q_{\min }\le q\le q_{\max },\end{array}$$

(1)

$$\begin{array}{c}\hfill {\dot{q}}_{\min }\le \dot{q}\le {\dot{q}}_{\max },\end{array}$$

(2)

$$\begin{array}{c}\hfill {\ddot{q}}_{\min }\le \ddot{q}\le {\ddot{q}}_{\max },\end{array}$$

(3)

where $q_{\min }$, ${\dot{q}}_{\min }$, ${\ddot{q}}_{\min }$, and $q_{\max }$, ${\dot{q}}_{\max }$, ${\ddot{q}}_{\max }$ are the minimum and maximum joint position, velocity, and acceleration, respectively.

Assume a discrete-time implementation with sampling period T in which the acceleration is considered as constant along each sampling period. At time k, kinematic limits at the next sampling time $k+1$ can be easily written as linear inequalities in the joint acceleration ${\ddot{q}}_k$ as:

$$\begin{array}{c}{q}_{\min }\le q_k+T{\dot{q}}_k+\frac{1}{2}{T}^2{\ddot{q}}_k\le q_{\max },\end{array}$$

(4)

$$\begin{array}{c}{\dot{q}}_{\min }\le {\dot{q}}_k+T{\ddot{q}}_k\le {\dot{q}}_{\max },\end{array}$$

(5)

$$\begin{array}{c}{\ddot{q}}_{\min }\le {\ddot{q}}_k\le {\ddot{q}}_{\max }.\end{array}$$

(6)

However, the simplistic imposition of Equations (4)–(6) may result in an empty admissible set, as no feasible solution might exist that satisfies all constraints at the same time [28].

Indeed, the existence of a solution is guaranteed if Equations (4)–(6) are feasible for all the admissible states of the system. This is strictly correlated to the concept of set viability, as mentioned in Section 1. As each joint is modeled as a double integrator, the viability analysis traces back to the viability of a double integrator system with bounded input and output. By imposing the feasibility of Equations (4)–(6), the maximal viability set for the double integrator can be derived analytically [26]. Intuitively, it can be calculated by imposing that, applying the maximum deceleration ${\ddot{q}}_{\min }$, the joint stops at $q=q_{\max }$ with null velocity ($\dot{q}=0$). The resulting condition (valid for the upper bound) is given by:

$$\left\{\begin{array}{cc}q-\frac{{\dot{q}}^2}{2{\ddot{q}}_{\min }}-q_{\max }\le 0\hfill & \mathrm{if}\dot{q}>0\hfill \\ q-q_{\max }\le 0\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(7)

which expresses a quadratic condition in the system states. An analogous condition for the lower bound can be derived likewise.

Such condition can be easily linearized around the current velocity and position, in the case of local methods, as in [14,27]. The inclusion of Equation (7) in the QP ensures that the states of the system (i.e., the joint velocities and positions) remain within a viable set for which the problem is feasible. More details about the discrete implementation in robotic systems can be found in [28].

The necessity of deriving a linear approximation of the original quadratic equation (Equation (7)) comes from the advantages given by the linear formulation in optimization-based controllers (in particular, the significant decrement of computational time and the ease of implementation of linear MPC with respect to the nonlinear one). However, in the case of predictive strategies, the linearization adopted for local methods is not applicable.

To tackle this issue, Faroni et al. [29] proposed a linear viability condition based on a single constraint for each joint. Firstly, it showed that approximating the quadratic constraint in Equation (7) by means of a straight line passing through the extreme points of the maximal viable set (i.e., $(q_{\max },0)$ and the intersection between Equation (7) and $\dot{q}={\dot{q}}_{\max }$ in Figure 1) leads to a non-viable set. Then, it derives a linear viability condition by imposing that the maximum deceleration along the linear constraint is exactly equal to the maximum admissible deceleration. In this way, the joint states can be always kept within the resulting set by means of a realizable acceleration. Such condition is given by:

$$-\frac{{\ddot{q}}_{\min }}{{\dot{q}}_{\max }}q+\dot{q}+\frac{{\ddot{q}}_{\min }}{{\dot{q}}_{\max }}{q}_{\max }\le 0$$

(8)

for the upper bound, and likewise for the lower one.

Inequalities (Equations (7) and (8)) are graphically represented in Figure 1. Linearity of Equation (8) is a great advantage, as it allows the straightforward inclusion in an optimization problem as a linear constraints. However, it is clear that Equation (8) implies a conservative reduction of the available state-space region compared to Equation (7), and such reduction worsens as the acceleration limits become smaller.

From a practical perspective, the reduction of the admissible state region shows its drawbacks when the robot is required to perform a motion that would violate Equation (8). In fact, the states laying between the linear and the quadratic constraints in Equations (7) and (8) are potentially realizable by the robot, but they are automatically excluded by the controller, with consequent deformation of the desired trajectory. Similarly, for redundant manipulators, shrinking the admissible state region could results in a worse satisfaction of the secondary objectives.

3. Proposed Method

As mentioned in the previous section, viability conditions are fundamental to ensure the feasibility of the control problem. The derivation of linear viability conditions (e.g., Equation (8)) allows their straightforward implementation in an optimization-based control framework, such as MPC algorithms. A strategy to enlarge the admissible region without waving its linearity is therefore proposed. It consists in increasing the number of linear constraints that compose the admissible set. We can therefore state the following problem.

Problem 1.

Given the acceleration, velocity, and configuration limits for each joint, find the maximal viable convex polyhedron determined by a given number of linear inequalities.

We address this problem by converting it into an optimization problem. In particular, for each joint, two optimization problems are set up (one for the upper position bound and one for the lower position bound). Without loss of generality, consider the upper configuration bound for a generic joint (i.e., the first quarter in Figure 1). The optimization variables of the problem are given by the coordinates of the extremes of the segment composing the polyhedron shown in Figure 2. Given a number of segments h, such variables are denoted with the vectors:

$$\begin{array}{c}{p}_x=(p_{x,0},\dots ,p_{x,j},\dots ,p_{x,h}),\hfill \end{array}$$

(9)

$$\begin{array}{c}{p}_y=(p_{y,0},\dots ,p_{y,j},\dots ,p_{y,h}),\hfill \end{array}$$

(10)

whereas a generic point on the polyhedron is denoted with $P_j(p_{x,j},p_{y,j})$.

Now, impose that $(p_{x,0},p_{y,0})=(q_{\mathrm{mean}},{\dot{q}}_{\max })$, where $q_{\mathrm{mean}}=\frac{q_{\max }+q_{\min }}{2}$ and $(p_{x,h},p_{y,h})=(q_{\max },0)$. To maximize the area covered by the polyhedron, a cost function equivalent to the opposite of such area is defined as follows:

$$\psi =\frac{1}{2}\sum _{j=1}^h(p_{x,j-1}-p_{x,j})(p_{y,j-1}+p_{y,j}).$$

(11)

The extremes of the segments are required to lay in the first quarter and have to respect the maximum position and velocity bounds. This results in the following box constraints:

$$\begin{array}{cc}\hfill q_{\mathrm{mean}}\le & p_x\le q_{\max },\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill 0\le & p_y\le {\dot{q}}_{\max }.\hfill \end{array}$$

(13)

Moreover, as a first necessary condition to the convexity of the polyhedron, we impose:

$$\begin{array}{c}{p}_{x,j}-p_{x,j-1}\ge 0\forall j=1,\dots ,h,\hfill \end{array}$$

(14)

$$\begin{array}{c}{p}_{y,j-1}-p_{y,j}\ge 0\forall j=1,\dots ,h.\hfill \end{array}$$

(15)

One last constraint needs to be imposed to ensure that the resulting polyhedron is viable and convex. To this purpose, we resort to a geometrical reasoning. Consider a generic segment $\overline{P_j{P}_{j+1}}$ whose extremes are the points $(p_{x,j},p_{y,j})$ and $(p_{x,j+1},p_{y,j+1})$ and denote with ${\overrightarrow{n}}_j$ the unitary vector normal to the segment and directed toward the inner region of the polyhedron. It results that:

$${\overrightarrow{n}}_j=\left(\frac{p_{y,j+1}-p_{y,j}}{∥\overline{P_j{P}_{j+1}}∥},\frac{p_{x,j}-p_{x,j+1}}{∥\overline{P_j{P}_{j+1}}∥}\right).$$

(16)

Consider then a generic point $P_{\zeta }(p_{x,\zeta },p_{y,\zeta })$ on the segment. By applying the maximum deceleration ${\ddot{q}}_{\min }$ to the system, the states are forced to move on the curve given by:

$$\left\{\begin{array}{c}q=p_{x,\zeta }+p_{y,\zeta }t+\frac{1}{2}{\ddot{q}}_{\min }{t}^2\hfill \\ \dot{q}=p_{y,\zeta }+{\ddot{q}}_{\min }t\hfill \end{array}\right.$$

(17)

The tangent vector to such curve in the $(q,\dot{q})$-plane is given by:

$$\overrightarrow{t}=\left(\frac{dq}{dt},\frac{d\dot{q}}{dt}\right)=\left(p_{y,\zeta }+{\ddot{q}}_{\min }t,{\ddot{q}}_{\min }\right).$$

(18)

The vector $\overrightarrow{t}$ at $t=0$ is therefore the tangent vector in the point $P_{\zeta }$ and results to be:

$$\overrightarrow{t}(p_{x,\zeta },p_{y,\zeta })=(p_{y,\zeta },{\ddot{q}}_{\min }).$$

(19)

Note that $\overrightarrow{t}(p_{x,\zeta },p_{y,\zeta })$ represents the direction of the state movement when the maximum realizable deceleration is applied. The state will therefore be able to remain below the considered segment if the following condition holds:

$${\overrightarrow{n}}_j·\overrightarrow{t}(p_{x,\zeta },p_{y,\zeta })\ge 0\forall (p_{x,\zeta },p_{y,\zeta })\in \overline{P_j{P}_{j+1}}$$

(20)

where $(·)$ denotes the scalar product between two vectors. As

$$\underset{p_{x,\zeta },p_{y,\zeta }}{min}({\overrightarrow{n}}_j·\overrightarrow{t}(p_{x,\zeta },p_{y,\zeta }))={\overrightarrow{n}}_j·\overrightarrow{t}(p_{x,j},p_{y,j}),$$

(21)

condition in Equation (20) can be imposed on the sole extreme point $P_j$ of each segment and becomes:

$$p_{y,j}(p_{y,j+1}-p_{y,j})+{\ddot{q}}_{\min }(p_{x,j}-p_{x,j+1})\ge 0.$$

(22)

Finally, the resulting optimization problem can be written as:

$$\begin{array}{cc}\underset{p_x,p_y}{minimize}\hfill & \psi \hfill \\ subject\; to\hfill & q_{\mathrm{mean}}\le p_x\le q_{\max }\hfill \\ & 0\le p_y\le {\dot{q}}_{\max }\hfill \\ & p_{x,j}-p_{x,j-1}\ge 0\forall j=1,\dots ,h\hfill \\ & p_{y,j-1}-p_{y,j}\ge 0\forall j=1,\dots ,h\hfill \\ & p_{y,j}(p_{y,j+1}-p_{y,j})+{\ddot{q}}_{\min }(p_{x,j}-p_{x,j+1})\ge 0\forall j=1,\dots ,h-1\hfill \\ & p_{x,0}=q_{\mathrm{mean}}\hfill \\ & p_{y,0}={\dot{q}}_{\max }\hfill \\ & p_{x,h}=q_{\max }\hfill \\ & p_{y,h}=0\hfill \end{array}$$

(23)

Following the same reasoning, an analogous optimization problem can be set up for the lower position bound. Note that, if the joint limits are symmetric, the solution for the lower bounds can be obtained by “mirroring” the solution of Equation (23) into the third quarter of the $(q,\dot{q})$-plane. Notice that, as Equation (23) is non-convex, global optimization such as genetic or multi-start algorithms should be adopted to solve the optimization problem (see Section 4).

Denoting with ${\mathcal{P}}_1$ the polyhedron determined by the segments obtained as solution of Equation (23), and with ${\mathcal{P}}_2$ the analogous polyhedron obtained for the lower bounds, the overall polyhedron for the single joint is given by:

$$\mathcal{P}={\mathcal{P}}_1\cup {\mathcal{P}}_2\cup [q_{\min },q_{\mathrm{mean}}]\times [0,{\dot{q}}_{\max }]\cup [q_{\mathrm{mean}},q_{\max }]\times [{\dot{q}}_{\min },0].$$

(24)

Proposition 1.

Polyhedron $\mathcal{P}$ is a viable set with respect to the given joint position, velocity and acceleration limits. Moreover, such polyhedron is convex.

Proof (Viability)

Viability of the set is implicitly ensured by the imposition of Equation (22). ☐

Proof (Convexity)

To prove it by contradiction, denote the solution of Equation (23) with $p_x^{*},p_y^{*}$ and assume that the corresponding polyhedron ${\mathcal{P}}_1^{*}$ (i.e., the part of the polyhedron in the first quarter) is non-convex. Consider then a triple of consequent extremes $\{P_{j-1}$, $P_j$, $P_{j+1}\}$, which causes a non-convexity. Equations (20) and (21) imply:

$$\begin{array}{c}{\overrightarrow{n}}_{j-1}·\overrightarrow{t}(p_{x,j-1}^{*},p_{y,j-1}^{*})\ge 0,\end{array}$$

(25)

$$\begin{array}{c}{\overrightarrow{n}}_j·\overrightarrow{t}(p_{x,j}^{*},p_{y,j}^{*})\ge 0.\end{array}$$

(26)

Consider now the point $P_c$ given by the projection of $P_j$ onto $\overline{P_{j-1}{P}_{j+1}}$ and denote with ${\overrightarrow{n}}_c$ the vector normal to $\overline{P_{j-1}{P}_{j+1}}$. We want to prove that the polyhedron obtained by substituting $P_c$ to $P_j$ in ${\mathcal{P}}_1^{*}$ is a feasible and more efficient solution of Equation (23). First, constraints in Equations (12)–(15) are straightforwardly satisfied. Moreover,

$${\overrightarrow{n}}_c·\overrightarrow{t}(p_{x,j-1}^{*},p_{y,j-1}^{*})\ge {\overrightarrow{n}}_{j-1}·\overrightarrow{t}(p_{x,j-1}^{*},p_{y,j-1}^{*})\ge 0$$

(27)

as ${\overrightarrow{n}}_{j-1}·\overrightarrow{i}\le {\overrightarrow{n}}_c·\overrightarrow{i}\le 0$ and ${\overrightarrow{n}}_c·\overrightarrow{j}\le {\overrightarrow{n}}_{j-1}·\overrightarrow{j}\le 0$ by construction (where $\overrightarrow{i}$ and $\overrightarrow{j}$ denote the unitary vectors directed as the horizontal and the vertical axis, respectively).

Moreover, as ${\ddot{q}}_{\min }$ in Equation (19) is assumed to be negative, the following inequality holds:

$${\overrightarrow{n}}_c·\overrightarrow{t}(p_{x,c},p_{y,c})\ge {\overrightarrow{n}}_c·\overrightarrow{t}(p_{x,j-1}^{*},p_{y,j-1}^{*})\ge 0.$$

(28)

Note that Equations (27) and (28) implies that the new candidate solution satisfies Equation (22) and is therefore a feasible solution to Equation (23). Moreover, the candidate solution is more efficient than the assumed one, as

$$\psi \left(P_0,\dots ,P_{j-1},P_c,P_{j+1},\dots ,P_h\right)\le \psi \left(P_0,\dots ,P_{j-1},P_j,P_{j+1},\dots ,P_h\right),$$

(29)

which means that $p_x^{*},p_y^{*}$ are not the optimal solution of Equation (23), as a feasible and more efficient solution that eliminates such non-convexity exists. Applying this reasoning to any triples of extremes that cause a non-convexity implies that the optimal polyhedron ${\mathcal{P}}_1$ must be convex. The same demonstration can be applied to ${\mathcal{P}}_2$ leading to the same conclusion. ☐

4. Numerical Results

Consider the first joint of the robot manipulator Universal Robot UR10. The joint configuration and velocity limits from the datasheet are given by:

$$q_{\max }=-q_{\min }=3.14\mathrm{rad},{\dot{q}}_{\max }=-{\dot{q}}_{\min }=2.16\mathrm{rad}/\mathrm{s}.$$

(30)

Typically, acceleration limits are not explicitly given in the datasheet. Indeed, the actual dynamic limit of the joint is usually determined by the available joint torque. However, handling viability directly in terms of torque limits is typically avoided due to the complexity of the problem. The common approach consists in estimating a (usually conservative) corresponding acceleration limit for each joint separately. A method for the estimation of such acceleration limit is given, for instance, in [28]. As a first example, assume that the acceleration limits are given by ${\ddot{q}}_{\max }=-{\ddot{q}}_{\min }=6\mathrm{rad}/\mathrm{s}{}^2$.

We solve the optimization problem (Equation (23)) for different values of h to evaluate the resulting polyhedrons as the number of edges grows and to compare the results to the quadratic and the linear viability constraints (Equations (7) and (8), respectively).

Table 1. Inner area of ${\mathcal{P}}_1$ for different values of h in case ${\ddot{q}}_{\max }=-{\ddot{q}}_{\min }=6\mathrm{rad}/\mathrm{s}{}^2$.

Equation (8)	h = 3	h = 4	h = 5	h = 6	h = 7	h = 8	h = 9	h = 10	h = 11	Equation (7)
0.9139	0.9636	0.9773	0.9836	0.9871	0.9895	0.9911	0.9920	0.9923	0.9925	1

shows the values of the inner area of polyhedron ${\mathcal{P}}_1$. The values are normalized with respect to the maximal viable set obtained by means of Equation (7), whose area is therefore equal to one (last column of Table 1). As expected, the smaller area is given by Equation (8) (first column of Table 1), while the extension of the polyhedron obtained by solving Equation (23) grows with the number of edges. In other words, the larger the number of edges, the closer the optimal polyhedron is to the maximal extension (given by Equation (7)). This is clearly shown in Figure 3, which depicts the different viable sets.

An analogous example is performed by using ${\ddot{q}}_{\max }=-{\ddot{q}}_{\min }=3\mathrm{rad}/\mathrm{s}{}^2$. The values of the normalized area of ${\mathcal{P}}_1$ are shown in

Table 2. Inner area of ${\mathcal{P}}_1$ for different values of h in case ${\ddot{q}}_{\max }=-{\ddot{q}}_{\min }=3\mathrm{rad}/\mathrm{s}{}^2$.

Equation (8)	h = 3	h = 4	h = 5	h = 6	h = 7	h = 8	h = 9	h = 10	h = 11	Equation (7)
0.8200	0.9239	0.9525	0.9656	0.9731	0.9731	0.9813	0.9838	0.9857	0.9857	1

and the resulting viable sets are depicted in Figure 4. The results lead to conclusions similar to the ones given by the previous example. However, it is clear that the magnitude of the phenomenon grows as the acceleration limits get smaller.

Of course the use of a larger value of h also gives some drawbacks. First, the computational complexity of Equation (23) rapidly grows with h as the number of variables and constraints is linear in h. As an example, the time needed to solve Equation (23) for $h=1$ and $h=11$ was in the order of 1 second and 15 s, respectively (the computation was performed in Matlab using a multi-start gradient-descent method on a standard laptop mounting a 2.5 GHz Intel Core i5-2520M processor).

Furthermore, the value of h determines the number of linear inequalities describing the polyhedron. In optimization-based control algorithms (such as linear MPC), this affects the computational complexity of the online optimal control problem, which is typically a critical issue in online methods, as mentioned in Section 1.

5. Experimental Results

Experimental were also performed on a six-degrees-of-freedom Universal Robot UR10 manipulator to prove the validity of the proposed approach. For the purposes of the paper, the joint position and velocity limits have been set to slightly conservative values with respect to the ones given in the datasheet. In particular, they are set as:

$$\begin{array}{c}{Q}_{\max }=-Q_{\min }=(3,3,3,3,3,3)\mathrm{rad},\hfill \end{array}$$

(31)

$$\begin{array}{c}{\dot{Q}}_{\max }=-{\dot{Q}}_{\min }=(2,2,3,3,3,3)\mathrm{rad}/\mathrm{s}.\hfill \end{array}$$

(32)

The acceleration limits have been set as ${\ddot{Q}}_{\max }=-{\ddot{Q}}_{\min }=(3,3,3,3,3,3)\mathrm{rad}/\mathrm{s}{}^2$. The experimental platform is shown in Figure 5. The objective of these experiments was the evaluation of the performance of an optimization-based control algorithm when different viability constraints are implemented. To this purpose, a simple MPC scheme was applied to a trajectory following problem. The MPC control scheme is in charge of following a position reference signal in the joint space. The model implemented in the MPC consists of a double integrator for each joint, as typical of robot kinematic control [15,30]. The input action is therefore represented by the joint accelerations, which then feed the low-level controller of the robot. Joint position, velocity, and acceleration limits can be implemented as linear constraints in the MPC (see [15] for details) and the online optimal control problem results to be a QP which minimizes the weighted sum of the tracking error and the control effort, as typical of linear MPC [31]. Notice that the choice of such a simple control scheme is due to the will of highlighting the behavior of a linear MPC controller in the case of different viability conditions. However, the proposed method could be straightforwardly applied to more complex MPC techniques such as [15,16,29].

The MPC algorithm was implemented using a sampling period $T=8$ ms and by setting a predictive and a control horizon $N=20$ sampling periods. The robot trajectory is controlled by means of a ROS-based control architecture. Namely, a position controller runs in a ROS Kinetic Ubuntu 16.04. The controller communicates with the robot by means of a TCP connection. The controller takes the MPC position output as reference and receives the actual joint position. The controller output is the sum of a proportional action, with gain equal to seven, and a feedforward term equal to the MPC velocity output.

We considered a simple trajectory given by a straight line in the joint space, parameterized with respect to the normalized longitudinal length along the path, denoted with $r\in [0,1]$. The trajectory $Q_{\mathrm{des}}\left(t\right)$ is therefore defined as:

$$Q_{\mathrm{des}}\left(t\right)=Q_{\mathrm{start}}+(Q_{\mathrm{end}}-Q_{\mathrm{start}})r\left(t\right),$$

(33)

where $Q_{\mathrm{start}}=(0,-1,0.5,0,0,0.5)$ rad and $Q_{\mathrm{end}}=(2.9,-0.5,-1.5,1,-0.5,2.9)$ rad are the initial and the final points of the trajectory, respectively, and $r:[0,t_{\mathrm{end}}]\to [0,1],t\mapsto r\left(t\right)$ is defined as a timing law with trapezoidal velocity profile, where $t_{\mathrm{end}}$ represents the total time of the trajectory, as depicted in Figure 6.

The control problem the MPC controller has to solve at each cycle k therefore results:

$$\begin{array}{cc}\underset{\ddot{Q}}{minimize}\hfill & \sum _{i=1}^N{∥Q_{\mathrm{des}}(k+j)-Q(k+j)∥}^2+\lambda \sum _{i=0}^{N-1}{∥\ddot{Q}(k+j)∥}^2\hfill \\ subject\; to\hfill & {\dot{Q}}_{\min }\le \dot{Q}(k+j)\le {\dot{Q}}_{\max }\forall j=1,\dots ,N\hfill \\ & {\ddot{Q}}_{\min }\le \ddot{Q}(k+j)\le {\ddot{Q}}_{\max }\forall j=0,\dots ,N-1\hfill \\ & Q(k+1)=Q\left(k\right)+T\dot{Q}\left(k\right)+0.5T^2\ddot{Q}\left(k\right)\hfill \\ & \dot{Q}(k+1)=\dot{Q}\left(k\right)+T\ddot{Q}\left(k\right)\hfill \end{array}$$

(34)

where Q, $\dot{Q}$, and $\ddot{Q}\in {\mathbb{R}}^6$ are the joint position, velocity, and acceleration vectors, respectively, and the controller effort weighting factor $\lambda =1\times {10}^{-6}$ was tuned empirically. Different implementations of the position limits will then be added to the problem in order to evaluate the different behaviors of the system.

As a first example, the total time of the trajectory was chosen as $t_{\mathrm{end}}=2.07$ s and the behavior of the MPC controller was evaluated in three different scenarios:

• No position bounds are implemented.
• Box position constraints are implemented, which means that the following constraints are added to Equation (34):

  $$Q_{\min }\le Q(k+j)\le Q_{\max }\forall j=1,\dots ,N$$

  (35)
• The linear viability condition in Equation (8) is applied, that is, Equation (35), and the following constraint are added to Equation (34):

  $$-\frac{{\ddot{Q}}_{i,\min }}{{\dot{Q}}_{i,\max }}{Q}_i(k+j)+{\dot{Q}}_i(k+j)+\frac{{\ddot{Q}}_{i,\min }}{{\dot{Q}}_{i,\max }}{Q}_{i,\max }\le 0\forall j=1,\dots ,N,\forall i=1,\dots ,6;$$

  (36)

  where the subscript i denotes the ith element of the vector.

The viability constraints are imposed throughout the whole control horizon for the sake of clarity and because the small increment in the computational time (due to the larger number of linear constraints) does not represent a significant issue in the presented experimental tests. However, in the case of linear MPC, imposing such constraints only on the last time instant of the horizon would be enough to ensure feasibility of the control problem and would not give significant differences in terms of control performance. The imposition of the constraint throughout the whole horizon permits to obtain a more reliable prediction of the future trajectory. This can be helpful especially in the case the prediction is utilized in linearized methods such as [15], as a non-reliable prediction might worsen the performance of the method.

Notice that the trajectory is devised in such a way that the desired motions of the first and the sixth joints exceed the maximal viable set, as shown in Figure 7a,b. The figures also show the phase plot of the position/velocity variables computed by the MPC in the three above-mentioned cases. In the case no position bounds are implemented (dashed green line), the joint position obviously does not satisfy the position limit. However, when box position constraints are implemented in the MPC (dash-dotted purple line), infeasibility of the control problem occurs and, since that time, the joint is forced to decelerate with the maximum admissible deceleration, but the position limit cannot be satisfied anyway. This highlights the importance of the viability property. In fact, the implementation of the linear constraints in Equation (8) does ensure the feasibility of the control problem and permits to satisfy the position bound (with a deformation of the original trajectory) (dashed red line). These behaviors are clarified also in Figure 8a,b, where the ideal and the measured joint positions for Joint 1 and Joint 6 are shown.

A second experiment is performed by choosing a total trajectory time $t_{\mathrm{end}}=2.65$ s. In this case, the trajectory drives the first and the third joints in the region between Equations (7) and (8). The behavior of the MPC controller is evaluated in cases where three different viability constraints are implemented:

• The linear viability condition in Equation (8), that is, Equation (36) is added to Equation (34).
• The viable polyhedron obtained by solving Equation (23) with $h=3$, that is, the following constraints are added to Equation (34):

  $$(Q_i(k+j),{\dot{Q}}_i(k+j))\in {\mathcal{P}}_{h=3}^i\forall j=1,\dots ,N,\forall i=1,\dots ,6;$$

  (37)

  where the superscript i denotes the viable polyhedral of the ith joint.
• The viable polyhedron obtained by solving Equation (23) with $h=4$, that is, the following constraints are added to Equation (34):

  $$(Q_i(k+j),{\dot{Q}}_i(k+j))\in {\mathcal{P}}_{h=4}^i\forall j=1,\dots ,N,\forall i=1,\dots ,6.$$

  (38)

Figure 9a,b shows the state trajectory for the first joint in the three different cases. Notice that the position limit is respected and the control problem remains feasible for all cases. However, the reduction of the admissible region determined by Equation (8) gives rise to a significant modification of the original trajectory, although the desired states are always potentially realizable by the robot (as the trajectory does not exceed Equation (7)). An improvement is obtained when the polyhedron with $h=3$ is used.

Notice that the small velocity bumps visible in the figures are due to predictive nature of the controller. In fact, as the control scheme is based on the tracking of a position reference along the predictive horizon, the controller slightly increases the velocity when it realizes the viability constraint will be activated and a deformation of the task will arise (as the state will be forced to follow the constraint). By giving a small acceleration before the activation of the constraint, the controller minimizes the future deviation with respect to the given position reference.

Finally, the use of $h=4$ permits to obtain a viable polyhedron that is large enough to enclose the whole desired trajectory. These behaviors are clarified also in Figure 10a,b, where the ideal and the measured joint positions for Joint 1 and Joint 6 are shown.

6. Conclusions

In this paper, we have proposed a method to compute the maximal viable polyhedron for a robot manipulator with bounded joint positions, velocities, and accelerations. In the proposed approach, each joint is considered separately and two optimization problems are devised: one for the upper bounds and one for the lower ones. Given its number of sides, the resulting polyhedron maximizes the area of the admissible position/velocity region. Moreover, the set is proven to be viable and convex. In this way, it can be easily implemented as linear constraints in optimization-based control methods, such as linear MPC. Numerical results demonstrate that the percentage of area covered by the polyhedrons increases as the number of sides grows. This allows the controller to exploit a larger admissible state region and, thus, gives it a broader margin of maneuver in the case the desired motion drives the robot in proximity of the viability boundary. In this case, a better task following can be achieved, as demonstrated by means of experimental results on a six-degrees-of-freedom manipulator.