Hierarchical Incremental MPC for Redundant Robots: A Robust and Singularity-Free Approach

This article presents a model predictive control (MPC) method for redundant robots controlling multiple hierarchical tasks formulated as multilayer constrained optimal control problems (OCPs). The proposed method, named hierarchical incremental MPC (HIMPC), is robust to dynamic uncertainties, untethered from kinematic/algorithmic singularities, and capable of handling input and state constraints such as joint torque and position limits. To this end, we first derive robust incremental systems that approximate uncertain system dynamics without computing complex nonlinear functions or identifying model parameters. Then, the constrained OCPs are cast as quadratic programming problems which result in linear MPC, where dynamically-consistent task priority is achieved by deploying equality constraints and optimal control is attained under input and state constraints. Moreover, hierarchical feasibility and recursive feasibility are theoretically proven. Since the computational complexity of HIMPC drastically decreases compared with nonlinear MPC-based methods, it is implemented under the sampling frequency of 1 kHz for physical experiments with redundant manipulator setups, where robustness (high tracking accuracy and enhanced dynamic consistency), admissibility of multiple constraints, and singularity-avoidance nature are demonstrated and compared with state-of-the-art task-prioritized controllers.

Hierarchical Incremental MPC for Redundant Robots: A Robust and Singularity-Free Approach Yongchao Wang , Yang Liu , Marion Leibold , Martin Buss , Fellow, IEEE, and Jinoh Lee , Senior Member, IEEE Abstract-This article presents a model predictive control (MPC) method for redundant robots controlling multiple hierarchical tasks formulated as multilayer constrained optimal control problems (OCPs).The proposed method, named hierarchical incremental MPC (HIMPC), is robust to dynamic uncertainties, untethered from kinematic/algorithmic singularities, and capable of handling input and state constraints such as joint torque and position limits.To this end, we first derive robust incremental systems that approximate uncertain system dynamics without computing complex nonlinear functions or identifying model parameters.Then, the constrained OCPs are cast as quadratic programming problems which result in linear MPC, where dynamically-consistent task priority is achieved by deploying equality constraints and optimal control is attained under input and state constraints.Moreover, hierarchical feasibility and recursive feasibility are theoretically proven.Since the computational complexity of HIMPC drastically decreases compared with nonlinear MPC-based methods, it is implemented under the sampling frequency of 1 kHz for physical experiments with redundant manipulator setups, where robustness (high tracking accuracy and enhanced dynamic consistency), admissibility of multiple constraints, and singularity-avoidance nature are demonstrated and compared with state-of-the-art task-prioritized controllers.Identity matrix, null matrix, and null vector.

Index
Euclidean norm of vector/matrix x. x Q x Q = x Qx for vector x and Q 0.

•
The estimated or approximated value of •.

I. INTRODUCTION
K INEMATICALLYredundant robotic systems (e.g., high degree-of-freedom robot manipulators and humanoid robots) are capable of executing several operational space tasks simultaneously.For example in robotic welding, the primary task is often to implement accurate trajectory tracking of its welding torch, i.e., end-effector (EE), and to obtain satisfactory welding seam additional tasks such as maintaining the orientation of EE will be performed by deploying degree-of-redundancy of the robot, e.g., the welding torch is expected to be perpendicular to the welding surface or maintain a specific angle.Although the multiple tasks can be controlled by being concatenated into a single task vector, however, if a control conflict occurs among tasks, control performance will be adversely affected.
To ensure conflict resolution, the operational space control was employed to bestow control capability of multiple tasks in a strictly prioritized manner for redundant robots, mostly with the null-space projection method [1], [2], [3], [4], [5], [6].These are called task-prioritized controllers grouped into three categories [6], where either the desired joint velocities [1], [2], or accelerations [3], or forces/torques [4], [5] are computed, respectively.Since most of commercial robots do not allow for a torque-level control interface, the velocity-based scheme is preferred in robotic applications.Nevertheless, for tracking with the second-order dynamics of a rigid body robot system, the acceleration-based and force/torque-based schemes are more appealing owing to the explicit incorporation of accelerations.
In particular, the force/torque-based scheme brings significant advantages in such applications that manipulation in contact with environments is required, and constraints on torques are imposed to guarantee safety.In the last few decades, a variety of force/torque-based methods, such as operational space formulation (OSF) [4], [5], [6], [7], [8], [9], [10], [11], [12], hierarchical quadratic programming (HQP) [13], [14], [15], [16], and more recently model predictive control (MPC) [17], [18], are developed.Nevertheless, the most practical performance criteriarobustness, singularity-avoidance, optimality, and constraint admissibility-have not been simultaneously conquered yet.Concurrently addressing these issues is challenging given the inevitable modeling errors, inherent inversion operation in algorithms and limited computing power of systems.Accordingly, this article aims at developing a task-prioritized control scheme taking into account all aforementioned criteria.

A. Related Works 1) OSF Schemes:
The OSF [4] relying on null-space projections [19] has been widely used and became a popular tool for task-prioritized control since it has been introduced to robotics in late 80s.It offers dynamically-decoupled control of multiple tasks based on a property known as dynamic consistency: Forces/torques for lower priority tasks do not affect the execution of high-priority tasks [5], where model-based feedback linearization is applied on all hierarchical levels and decoupled control behavior of each task is obtained.Recently, the formal null-space stability of hierachical control is theoretically analyzed for a regulation case in [7] and [8] and for a tracking case in [9], respectively, although this is generally known to be difficult to be proven [6], [20].
Nevertheless, both the novel hierarchical controller and the OSF are vulnerable to modeling errors [6] since precisely identified parameters of the mathematical model are crucial to attain feedback linearization in the controller design.In practice, unfortunately, modeling errors are inevitable and moreover, dynamics terms undergo severe changes especially when the robotic system carries/releases unknown payloads, which indeed deteriorate desired control performance.
To deal with modeling errors, adaptive methods [21] and learning techniques [22] are propitiously employed in operational space control, where uncertain/unknown functions are identified or optimal control is found online that maximizes an immediate reward.However, the computational complexity is such high that it is difficult to apply in practice.Moreover, parameters have to be heuristically selected, which also increases the complexity of these methods.As an alternative, time-delay estimation (TDE)-based OSF methods were developed in [10], [11], and [12].TDE [23], [24], [25], [26], [27] is a model-free controller design method, which uses time-delayed input and output signals to estimate system dynamics without a concrete mathematical model, laborious parameter identification, and linearization around equilibrium points of the system.It results in enhanced accuracy in terms of dynamic consistency and control performance along with enhanced computational efficiency.
Another hurdle in the OSF-based methods, typically using null-space projection [4], [5], [6], [7], [8], [9], [10], [11], [12], is the singularity problem, especially the algorithmic one which occurs when tasks conflict significantly; this finally results in unstable behavior.Although a damping factor [28] and a continuous null-space projection [29] are introduced to address the singularity problem, there is a laborious tuning process of parameters and unavoidable tradeoff between accuracy and feasibility of the solution.In addition, physical limitations, such as input and state constraints, are also not considered in all approaches mentioned so far.Constraints are generally used to describe safety requirements and should be considered in safety-critical applications, e.g., close to or in cooperation with humans.
2) HQP Schemes: Considering input and/or state (motion) constraints, HQP is developed in the hierarchically-ordered framework of multiple quadratic programs (QPs) [13], [14], [15], [16], where the task priority is strictly achieved using the hierarchical structure of the QPs [2], [30].In [13], the robustness of HQP was addressed by sliding mode control (SMC).However, since this method is based on the null-space projection to design the sliding variable of hierarchical tasks, a potential risk of singularity exists.
Without using the null-space projection method, the authors in [14] implemented the task hierarchy by solving the cascaded QPs in lexicographic order, while the authors in [15] and [16] propose that equality constraints are imposed to guarantee strict task hierarchy.Thus, in [14], [15], and [16], algorithmic singularity is avoided.However, once the Jacobian matrix becomes singular, Hessian matrices of QPs in [14] and [15] are no longer positive definite, and kinematic singularity occurs.This is because, for QP solvers, numerical weakness increases with the rise of the condition number of the Hessian matrix.When the Hessian matrix is positive semidefinite, undesired (large-value) control signals are computed and the system becomes unstable.Whereas, although the Hessian matrix is always positive definite in [16], the risk of kinematic singularity still remains because inverse calculation of the terms with respect to (w.r.t.) Jacobian matrices is exhibited in the algorithm.
In addition, it is noteworthy that solutions of the HQP are optimal to the current robot configuration, but not w.r.t.global tasks.In other words, the locally optimal controllers might drive the controlled robotic systems to disadvantageous configurations in the context of global tasks [31].Last, the HQP control structure may cause torque peaks and oscillations [3], attributed to its limited (a single step) prediction horizon.
3) MPC Schemes: Model predictive controllers have been proposed to address drawbacks of locally optimal controllers [31], [32].Given that control performance is considered over a finite-time horizon [33], [34], [35], MPC provides a powerful option to fulfill control objectives of multiple tasks while constraints are not violated.Nevertheless, in [31] and [32], the task hierarchy was acquired via a weight-prioritized optimization problem [36].It resulted in a soft task-prioritized inverse kinematics control scheme.When tasks conflict, control performance of the highest-priority task, which is usually the primary or safety-related work, will be adversely compromised.The torque-based task-prioritized MPC schemes were developed independently in [17] and [18].A single convex constrained optimization problem was constructed in [17] where the task hierarchy is achieved by a quadratic inequality constraint that tracking errors of high-priority tasks must be less than or equal to that of low-priority tasks.It still results in a soft task hierarchy.Besides, the desired joint position and velocities are calculated using the null-space projection method, resulting again in risk of algorithmic singularity.In addition, predictions are generated based on the nominal model of robotic systems while uncertainties and/or disturbances are not considered.
To improve robustness of MPC, learning-based MPC methods [37], [38] are developed for a single-task control problem, where Gaussian processes (GP) and neural networks (NN) are employed to identify nonlinear models online.Unfortunately, GP and NN learning techniques further increase the computational complexity of MPC.In [39], the SMC scheme is combined with MPC to address modeling errors and external disturbances with better computational efficiency for a single task.However, when multiple tasks are considered, task hierarchy will be adversely affected by the compensating SMC method.To guarantee task hierarchy, the task-prioritized sliding variable for each layer is designed, and then an operational space model predictive SMC was developed in [18].Nevertheless, the task-prioritized sliding variable is designed using the null-space projection method, which means it still suffers from the risk of algorithmic singularity.Besides, the aforementioned SMC-based methods request the nominal mathematical model to be fairly identified.In addition, when they are applied in task-space control schemes, due to the introduced pseudocontrol law, constraints imposed on joint positions, velocities, and torques are nonlinear and nonconvex, and thus the considered optimal control problems (OCPs) are general nonlinear programming (NLP) problems whose convexity is not guaranteed.In the nonconvex programming problem, the solution is easy to fall into local optimum and is computationally demanding in general.
The permitted sampling period of robotic systems is usually down to hundredths or even thousandths of a second, and thus with the general NLP solvers, such as IPOPT [40] and LOQO [41], it is difficult to guarantee real-time control, finally causing feedback delays.The most popular and commonly used method is sequential quadratic programming (SQP) [42], [43], [44], where a series of approximated QP problems is solved sequentially until the solution converges.Owing to the efficient SQP, the computation time is down to the order of milliseconds or tens of milliseconds [42], [43], [44].However, due to demands for high-precision in robotics systems, the fast control frequency, e.g., 1 kHz, is generally suggested since the control accuracy is in line with the sampling rate [10], [11], [12], [25], [26], [27], [45], [46], [47], [48].To this end, the approximated algorithm which trades control performance for speed, such as real-time iteration (RTI) [49] and feasibility-driven differential dynamic programming (FDDP) [50], are developed to avoid solving NLP iteratively.In RTI, a single convex QP that locally approximates the original optimization problem is solved per feedback step [46].The gap contraction of FDDP is equivalent to direct multiple-shooting formulations [50].However, only equality constraints can be considered in FDDP.Besides, applications of RTI and FDDP rely on accurate models of controlled plants and solver stability problems arise in the presence of reference changes and/or large external disturbances [51].In contrast to NLP solvers, QP solvers which are running at microseconds deliver solutions in a more reliable and efficient way [52], [53].Therefore, it is appealing that the task-prioritized MPC scheme can be formulated into a linear form whose OCP can be cast as a QP.

B. Method and Contributions
In this article, a task-prioritized control scheme is proposed for robotic systems modeled by Euler-Lagrange equations and we call it hierarchical incremental model predictive control (HIMPC).To improve robustness against uncertainties and disturbances, we first develop a robust incremental model exploiting the TDE method.Then, the HIMPC is formulated with multilevel constrained OCPs sequentially ordered in accordance with task priorities.The state predictions are generated from the discretized incremental model.Task hierarchy is fulfilled through equality constraints on control signals for lower-priority tasks, based on the dynamic consistency principle.Moreover, each constrained OCP of the HIMPC is cast to a QP.The hierarchical feasibility and recursive feasibility of the HIMPC are verified theoretically.
The contributions of this article are summarized as follows.1) Robust: Robustness of the controller in terms of tracking accuracy and also dynamic consistency is enhanced.Different from existing task-prioritized MPC [17], [18] and optimization-based methods [2], [13], [14], [15], [16], the nominal mathematical model of the robotic system will not be required for the proposed controller.We will approximate both system dynamics and equations of motion of tasks using TDE.This model-free nature improves robustness against uncertainties and disturbances.Besides, TDE will allow that task priority constraints are set independent of the accurate inertia matrix.2) Computationally Efficient: Relying on the TDE method will not only improve robustness of the proposed method, but also simplify nonlinear equations to linear ones.The proposed HIMPC is essentially a linear MPC and each constrained OCP can be cast to a QP.Compared with the nonlinear task-prioritized MPC schemes developed in [17] and [18], the proposed HIMPC decreases computational complexity dramatically, which makes it possible to allow for real-time control in milliseconds.3) Singularity-Free: The null-space projection idea will not be employed in this approach, and task hierarchy is achieved by imposing equality constraints on input signals for lower-priority tasks.Since no inverse calculation is involved in these equality constraint equations, algorithmic singularity is avoided.Moreover, the inverse calculation of terms w.r.t.Jacobian matrices is not required to formulate the constrained OCPs, and the Hessian matrix of each constrained OCP is verified to be positive definite.Thus, the proposed HIMPC is also a kinematically singularity-free method.

C. Organization
The rest of this article is organized as follows.In Section II, the robotic system dynamics and control objective are introduced.In Section III, equations of motion of tasks and system dynamics Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
are approximated by incremental systems using TDE, which do not need dynamic model parameters.Further, the accuracy of this approximation method is analyzed.Section IV presents the HIMPC.The hierarchical and recursive feasibility of HIMPC is analyzed.In Sections V and VI, the effectiveness of the proposed method is verified by simulation and experimental results.Finally, Section VII concludes this article.

II. PRELIMINARIES
This section is devoted to giving the problem statement in terms of a control objective.Therefore, at first, the robotic system dynamics and then, the tasks are introduced.

A. Robot Dynamics
The dynamics of an n-link robotic system is given by an Euler-Lagrange equation [45] M(q)q + C(q, q) q + G(q) + F( q) + τ d = τ (1) where q, q, q ∈ R n represent the vectors of position, velocity, and acceleration, respectively.M(q) ∈ R n×n is the symmetric inertia matrix, C(q, q) ∈ R n×n is the Coriolis/centrifugal matrix, G(q) ∈ R n contains the gravitational terms exerting on the robotic system, F( q) ∈ R n denotes viscous friction, τ ∈ R n is the vector of torque supplied by the actuators, and τ d ∈ R n denotes the unknown disturbance.
The uniform positive definiteness property of M(q) is introduced in the following.Later, we will need this property to determine design parameters (see Remark 4).
Property 1 (Positive Definite Inertia Matrix [45], [47]): The inertia matrix M(q) is uniformly positive definite and there exist constants μ 1 , μ 2 ∈ R >0 such that each eigenvalue of M(q), denoted by λ i (M(q)), satisfies the following: Note that due to inevitable modeling errors, the inertia matrix M(q), and other dynamics terms C(q, q), G(q), and F( q) are considered to be unknown/uncertain in this article.Besides, to guarantee the system is controllable, the unknown disturbance τ d is assumed to be bounded [54].

B. Task Hierarchy, Tasks, and Control Objective
A control task hierarchy including r ∈ I >0 levels is introduced and it is assumed that the ith task has a lower priority level than all previous (i − 1) tasks [9].Especially, the first and rth tasks have the highest and lowest priority level, respectively.The task-space coordinate x i ∈ R m i of the ith task is where f i (q) : R n → R m i is the task mapping, which is known exactly and continuously differentiable.With the continuous differentiable task mapping f i (q), the Jacobian J i (q) ∈ R m i ×n corresponding to x i is defined as Since f i (q) is exactly known in general, all Jacobians are regarded as known functions without uncertainties.In this article, task i is defined to make x i track a desired reference trajectory x id and achieve the following target motion dynamics in free space [10]: where xi := x i − x id is the tracking error, and K V i 0 and K P i 0 denote damping and stiffness matrices, respectively.Nonsmooth reference trajectories can cause damage to mechanical systems due to sharp actuator torque changes.Thus, x id is assumed to be bounded and smooth.
Assumption 1 (Bounded and Smooth Reference Trajectory, [55]): The reference trajectory x id is bounded and smooth, satisfying 0 Note that exact values of x i , x i , ẋi , ẋi , ẍi , and ẍi in Assumption 1 are not required for the controller design.
We aim to design a strict task-prioritized controller which is robust yet singularity-free while physical constraints of the robot manipulator, such as constraints on joint positions, velocities, and torques, are satisfied.The following box constraints are considered: where • min , • max denote specific lower/upper bounds of •.
Remark 1 (Delete an Assumption of Non-Singular Task Space): In literature, it is common to avoid singularities by restricting allowed tasks, and one assumption is made that goal tasks are designed not to include singularities.However, this assumption delimits the feasible task space of the robotic system.In this article, to ensure sufficient task space for the robotic system and to guarantee safety, we will strive for designing a singularity-free controller.Thus, the assumption on nonsingular task space is removed.

III. ROBUST INCREMENTAL SYSTEMS
In this section and the following Section IV, a task-prioritized control scheme is developed.We aim at taking into account optimality, input and state constraints, strict task hierarchy, and robustness, simultaneously.To achieve optimality and address input and state constraints, the control scheme will be developed in the framework of MPC.Considering the unknown system dynamics and also mitigating the computational complexity of the algorithm, the TDE method is used to accurately approximate equations of motion of tasks and system dynamics to avoid online identification or other time-consuming operations.Besides, the accuracy of the resulting TDE-based incremental systems is analyzed.

A. Derivation of Two Incremental Systems
In this subsection, uncertain equations of motion of tasks and system dynamics will be approximated using TDE, and incremental systems are obtained, which are used to generate state predictions.
As stated in the control objectives in Section II-B, we aim to impose constraints on torques.Thus, the torque is selected as the control variable in the proposed task-prioritized controller.First, the equations of motion of tasks are derived with the torques being the inputs.With (1) and ( 3), the equation of motion of the ith task is obtained as follows: which includes uncertain system dynamics terms and unknown disturbance.To deal with these uncertainties and reduce dependency on the concrete mathematical model, the TDE technique [23], [24], [25], [26], [27] is employed to approximate the equation of motion (5), which involves the following two steps: Step 1. Separating Uncertain Terms from Known Terms: It is observed that uncertainties in the equations of motion ( 5) originate from the uncertain system dynamics (1).Thus, we will investigate (1) first.Introducing a diagonal positive definite matrix M, (1) is transformed into 6), the equation of motion ( 5) is rewritten as where H i is also uncertain/unknown because of the lumped uncertain nonlinear function H q in (6).
Step 2. Using the Time-Delayed Signals/Functions to Approximate the Unknown Terms: The value of H i at the time t is approximated by that of H i at (t − L) for a sufficiently small delay time L [25], [26], [27] We abbreviate ẍi,0 := (ẍ i ) (t−L) , τ 0 := τ (t−L) , and J i,0 = (J i ) (t−L) for simplicity.From ( 7) and ( 8), one obtains the TDE of H i , i.e., (H i ) (t−L) , as follows: Comparing ( 7) with ( 9), we see that the input matrices J i M−1 and J i,0 M−1 are different.We will unify these input matrices using the continuity property.In practice, a digital system can be regarded as a continuous system when the sampling rate is faster than 30 times the system bandwidth [56].Thus, according to the continuity property, the variations in the Jacobian J i during a short time period are negligible.That is, formally, for a sufficiently small L With (10), we then have an alternative TDE of H i , i.e., Ĥi , as follows: Finally, we obtain the incremental version of ( 5) with the combination of ( 7) and ( 11) where Δτ := τ − τ 0 is the incremental control signal, and ε x := H i − Ĥi is the TDE error in ith task space which is considered as a disturbance to the incremental system (12).Note that only current values of ẍi and τ are used in (11) to get the approximation while complex and uncertain functions, such as M(q), C(q, q), G(q), and F(q), are not required.To guarantee the approximation accuracy, we also assume state measurement errors are sufficiently small.In accordance with the formal TDE implementation [10], [11], [12], [23], [24], [25], [26], [27], the sampling period T s is selected as L, which guarantees the delay time is sufficiently small.Then according to the continuity property, ε x can be sufficiently small when we select a sufficiently small delay time L. For ε x = 0, the nominal incremental system for ith task is obtained as In what follows, the Euler method is used to obtain a discretetime version of ( 13), which is used to generate predictions of x i .Since the sampling period is sufficiently small, the discretization error is ignored.Thus, we have where We define a stack variable x i (k + 1) := col(x i (k + 1), xi (k)), and rewrite ( 14) as a canonical linear equation with It is noteworthy that the approximated equation of motion of the ith task ( 13) is general and it is also suitable for posture tasks which are normally to regulate specific joint positions.Similarly, the approximated system dynamics can also be obtained, which will be used to predict joint space behavior (q and q) to be able to evaluate constraints in joint space during predictions.In analogy to the procedures in Step 1 and Step 2 with the Jacobian matrix J := I, the following incremental system is obtained: where ε q := (H q ) (t−L) − H q is the TDE error in joint space.
Let ε q = 0 from ( 16), one can obtain the nominal incremental system in the joint-space coordinate as follows: Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
Then, similar to (15), let q(k + 1) := col(q(k + 1), q(k)), q := col(q, q) and again a canonical linear equation is obtained with In brief, using an auxiliary matrix M, we found two discretetime incremental systems as approximations of equations of motion and system dynamics (cf.( 15) and ( 18)).We will use ( 15) and ( 18) to generate state predictions over the prediction horizon.
Remark 2 (The Differences between the TDE Formulation in this Article and that in [10]): In [10], the TDE approximation was formulated by using conventional null-space projection, where task Jacobians are assumed to be full rank, i.e., the considered task space is nonsingular.Whereas, in the proposed formulation as seen in ( 13), inversion of terms involving Jacobian matrices is not required and the assumption on nonsingular task space is relaxed.
Remark 3 (Why Including Ji q in the Lumped Function H i ): It is noteworthy that the inclusion of Ji q in the lumped function H i in ( 7) enables a canonical linear system approximation (cf.( 15)) plugged into the linear MPC framework as introduced in Section IV.In fact, since time derivatives of the Jacobian ( Ji ) can be computed from the exact kinematic parameter values and mapping function f i (q), it is not necessary to include the term Ji q in the lumped function H i .However, this requests a nonlinear MPC framework which is computationally so demanding that real-time execution is impossible.In addition, the proposed linear formulation will allow a more indepth and accessible theoretical analysis of the controller performance.Moreover, owing to its continuity property, the lumped H i including Ji q term can be accurately approximated by time-delayed signals with a sufficiently high sampling rate; this also saves additional computation of Ji q.
Remark 4 (How to Determine M): In existing TDE-based control schemes [10], [11], [12], [25], [26], [27], the stability condition I − M −1 M ≤ 1 needs to be fulfilled.To avoid performance deterioration by inappropriate M, this stability condition is also required to be satisfied in this article.For a specific robotic system, the principle to determine the diagonal matrix M such that This also implies that the sufficient condition I − M −1 M < 1 can be achieved by a small positive Mi although exact expressions and eigenvalues of M are unknown [27].From the nature of TDE, too small M results in large TDE errors while too large M causes noisy responses, and vice versa.Accordingly, the constant M := diag( M1 , . . ., Mn ) is selected in a manual tuning process [10]: 1) begin with a sufficiently small positive Mi to guarantee stability; and 2) increase Mi until tracking performance is satisfactory or the closed-loop system almost shows a noisy response.In practice, a wide range of M can be selected [23], [24], [25], [26], [27], since the error (M − M) will be compensated by time-delayed signals (cf.Section III-B).

B. Analysis of Approximation Accuracy
Although the exact mathematical model of the robotic system is not required, there is a discrepancy between (1) and ( 5), and the nominal incremental systems, ( 13) and ( 17), because of inevitable TDE errors.In this subsection, we will show that the incremental system exhibits a higher approximation accuracy than the nominal nonlinear mathematical model (abbreviated as the nominal model in Section III-B).
First, approximation errors for the nominal model cases are derived.Denote the nominal inertial matrix, Coriolis/centrifugal matrix, gravitational matrix, and viscous matrix as M n , C n , G n , and F n , respectively.In accordance with (1), the following nominal system dynamics ( 19) is used to estimate q and q, where qnom is the approximation for the nominal model case where N n := C n q + G n + F n is the nominal value of the lumped nonlinear dynamics term N (N := C q + G + F).To obtain the approximation error, we recall the real system dynamics (1) and transform it into the following form using M n [10] M n q + Mq + N + τ d = τ (20) where M := M − M n is the modeling error of M. From ( 19) and (20), the system dynamics approximation error γ := qnom − q for the nominal model case is obtained where Ñ := N − N n is the modeling error of N.
If the nominal model is available, the approximated equation of motion of the ith task is obtained according to (5) where xi,nom is the approximation generated by the nominal model.Correspondingly, according to ( 5) and ( 20), the equation of motion of the ith task is rewritten as From ( 22) with ( 23), the equation of motion approximation error β i := ẍi,nom − ẍi is obtained as Next, approximation errors for incremental system cases, ( 13) and ( 17), are derived.Comparing ( 16) with (17), one observes that the system dynamics approximation error for the incremental system case is δ defined as Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
where ε N := N − N (t−L) is the TDE error of N.
Then, comparing ( 12) with ( 13), one observes that the approximation error for the incremental system case is the TDE error ε x .Besides, for a sufficiently small sampling period, it holds that Ji q ≈ Ji q Thus, combining ( 7), ( 9), ( 10), ( 11), (25), and (26) results in 10),( 11) For the TDE approximation (cf.( 25) and ( 27)), we learn that, if the delay time L (sampling period T s ) is sufficiently small, the inertia modeling error (M − M)q and disturbance τ d are compensated by their time-delayed values (M (t−L) − M)q (t−L) and (τ d ) (t−L) effectively, and the smaller the sampling period, the smaller is the TDE error.For the nominal model, there are no terms to compensate for modeling errors and disturbances.Besides, according to the continuity property [56], ε N is small enough, and then its value will be less than that of Ñ, if the sampling rate is sufficiently high.Therefore, for a sufficiently small sampling period, the incremental systems derived using TDE exhibit high approximation accuracy and show strong robustness against modeling errors and disturbances.
So far, we have derived incremental systems which will be used to determine state predictions.Further, we verified their high approximation accuracy if a sufficiently small sampling period is employed.Next, the task-prioritized MPC scheme will be developed with robust incremental systems.
Remark 5 (Effects of the Accuracy of Actuators and Sensors): In practice, there is a difference between the control law τ and the real supplied torque τ r due to motor dynamics and accuracy.In Section II-A, this difference is considered as one part of τ d .From ( 25) and ( 27), we learn that if L is sufficiently small, τ d is compensated by its time-delayed value (τ d ) (t−L) effectively.Hence, the effects of actuator dynamics and accuracy on control performance are limited.Besides, since incremental systems are constructed using time derivatives of states, the effect of the measurement error from the sensor resolution is amplified by numerical derivatives, thus, noisy responses can be triggered if the resolution is coarse.In Section VI, simulations are implemented to quantitatively investigate the allowable measurement resolution for HIMPC.
Remark 6 (Allowable Sampling Periods): The control accuracy is in line with the sampling rate, which is generally up to 1 kHz or even higher for high-precision robotics systems.Moreover, to guarantee the required approximation accuracy of the incremental systems, the sampling period has to be chosen sufficiently small.While there are, unfortunately, no systematic methods to determine the maximum allowable sampling period, one can find in [10], [11], [12], [25], [26], and [27], TDE methods have been successfully demonstrated high-accuracy approximation capability with sampling periods of 1 and 2 ms for robot manipulator control.Accordingly, 1 ms sampling period is considered throughout the article.

IV. HIERARCHICAL INCREMENTAL MPC
In this section, the task-prioritized control scheme, HIMPC, is developed as a series of constrained OCPs, where appropriate constraints are used to enforce the task hierarchy.Finally, the hierarchical and recursive feasibility of the proposed HIMPC is shown.

A. Method
In this subsection, at first, the stage cost for one task is introduced as the basis for developing the series of constrained OCPs.The stage cost is designed to take into account the target motion dynamics (4) as well as controller oscillations, energy efficiency, and actuator protection.Thus, the predicted motion dynamics error and the control signal are considered in the stage cost.
In accordance with (4), the predicted motion dynamics error e( x i,k+j+1|k ) at time k is defined as Here, x i,k+j+1|k := col(x i,k+j+1|k , xi,k+j|k ), xi,k+j|k := is an intermediate variable for time instance (the same hereinafter), N is the length of the prediction horizon, and • k+j|k stands for the predicted variables, in particular, • k|k := •(k), and x i,k+j+1|k is calculated using the discrete-time incremental system (15).That is, for all j ∈ I [0,N −1] x i,k+j+1|k = A 1 x i,k+j|k + Bj 1i Δτ k+j|k (29) with x i,k|k := col(x i,k|k , xi,k−1|k ) and xi,k−1|k := xi (k − 1).In ( 14), the matrix B 1i is derived using the Jacobian J i which is a function of the joint position.Similar to the approach in [31], we simplify calculation by approximating J i and B 1i , and approximations over the prediction horizon, Ĵj i,k and Bj 1i , are obtained using the optimal joint state predictions at time k − 1.The optimal joint state prediction q * k+j|k−1 is calculated by the incremental system (18) with the optimal control input sequence Here, the incremental control signal Δτ is considered in the stage cost, since the equations of motion of tasks and system dynamics are all approximated by incremental systems.Thus, at time k, the following stage cost for task i is defined: Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
where Q i 0 and R i 0 are weighting matrices.Now the stage costs of all tasks are combined and deliver the proposed HIMPC framework where a constrained OCP for each task is introduced, starting with the task 1 as the highest-priority task.The task hierarchy will be achieved by constraints to the OCPs of lower-priority tasks.
where Δτ * 1,k is the optimal control input sequence, q 1,k+j+1|k is the joint state prediction with Δτ 1,k+j|k .Equality constraints (32b) and (32c) are employed to generate predictions x 1,k+j+1|k and q 1,k+j+1|k while constraints (32d)-(32f) are imposed on joint position, velocity, and torques, respectively.In case there is only one task, the first column of Δτ * 1,k is applied to the system directly.For multiple tasks, Δτ * 1,k is just an auxiliary signal to the constrained OCPs of lower-priority tasks while only task r, the lowest priority task, will finally deliver a control input that is applied to the system.
To achieve task hierarchy, relying on the dynamic consistency principle [5] (i.e., high-priority tasks will not be affected by the torques designed for low-priority tasks), the following equality constraints will be imposed on control signals when Problem i Here, Δτ * p,k+j|k and Δτ i,k+j|k are the optimal input and one admissible counterpart for the pth and ith layer constrained OCPs, respectively, and Ĵj p,k is the approximated Jacobian for task p at time k and is calculated using q * k+j|k−1 .From ( 29) and (33), it is observed that the optimal motion of task p will not be disturbed by optimal inputs determined for lower-priority tasks.Here, no null-space projections are used and thus, matrix inversion to determine the prioritized Jacobian is not necessary.Thus, the algorithmic singularity is avoided.
With the task priority constraint (33), the following Problem i for task i q min ≤ q i,k+j+1|k ≤ q max (34d) qmin ≤ qi,k+j+1|k ≤ qmax (34e) where q i,k+j+1|k is the joint state prediction under the action of Δτ i,k+j|k .The difference between Problem 1 and Problem i ) is the series of equality constraints (34g), which allows to enforce the task hierarchy.Because of the approximated Jacobian Ĵj p,k , the equality constraint (34g) is an affine linear system, which is also the basis for analyzing the existence and uniqueness of the solution to Problem i.
The optimal control input sequence Δτ * r,k is obtained after a series of constrained OCPs (from Problem 1 to Problem r) is solved.Δτ * r,k is not only the optimal solution to Problem r, but it also guarantees that optimal motion of tasks 1 to (r − 1) are not affected owing to the equality constraint (34g).Thus, Δτ * r,k is considered as the optimal control input sequence to the proposed HIMPC.Combined with the recent control input τ 0 , the first column of Δτ * r,k is applied to the system, i.e., τ (k + 1) = τ 0 + Δτ * r,k|k .Note that the developed controller is also suitable for other fixed-base robotic systems.Further, it also can be extended to mobile robotic systems when kinematics constraints [44] are considered in the constrained OCP.
As a summary, Fig. 1 visualizes the structure and the control signal flow of the proposed HIMPC.
Remark 7 (Supplementary Notes to the Approximation in ( 29) and ( 33)): With (18) (or (34c)), over the prediction horizon J i can also be updated using the current joint state predictions.However in this case, ( 29) and (33) will be nonlinear equations, and consequently a nonlinear MPC framework is obtained, resulting in higher computational complexity.For simplicity, in this article over the prediction horizon J i and B 1i are approximated, and the approximations Ĵj i,k and Bj 1i are determined using previous joint state predictions q * k+j|k−1 before solving the constrained OCPs.In this way, ( 29) and (33) used to generate predictions of x i and guarantee task hierarchy, which are originally nonlinear equations, are approximated by canonical linear equations.Besides, Δτ * r is the optimal control input sequence to the proposed hierarchical control scheme, and thus Δτ * r,k−1 will be used in (30) to generate q * k+j|k−1 .Remark 8 (Serial Connection of Constrained OCPs): The computational complexity of HIMPC increases as the number of tasks increases.For a task hierarchy with n tasks, there will be n constrained OCPs to be solved.In this article, the constrained OCPs are solved by a QP solver which is possible efficiently.Nevertheless, the computing time increases with an increasing number of tasks because of the serial connection of constrained OCPs.Fortunately, this increase is linear.In [2] and [57], dedicated solvers were developed to speed up the calculation of task-prioritized programming problems.However, the null-space projection idea is adopted and it has the risk of algorithmic singularity.To expedite calculations, one possible alternative is to select shorter prediction horizons for low-priority tasks on the basis of stability.The task hierarchy will be still guaranteed although the optimality of low-priority tasks will deteriorate.

B. Analysis
The developed HIMPC framework is only logical if each constrained OCP is feasible for every time, which means the set D i (1 ≤ i ≤ r) of admissible input trajectories Δτ i,k for Problem i is nonempty.In this subsection, this hierarchical recursive feasibility will be analyzed first.
1) Hierarchical Feasibility: Due to the hierarchical structure and the equality constraint (34g), admissible sets of low-priority constrained OCPs are affected by those of high-priority ones.To analyze the feasibility of this hierarchical MPC, the definition of hierarchical feasibility is introduced.
Definition 1 (Hierarchical Feasibility, [58]): The hierarchical MPC (a series of constrained OCPs ordered hierarchically) admits hierarchical feasibility at time k if the feasibility of the ith constrained OCP implies the feasibility of the (i + 1)th constrained OCP for all i ∈ I [1,r−1] .
To prove the hierarchical feasibility of HIMPC, we first show the existence and uniqueness of solutions to Problem i using convex optimization theory.Notice that Problem i (1 where U i := col(Δτ i,k|k , . . ., Δτ i,k+N −1|k ) and (35b)-(35e) are joint position, velocity, input, and task priority constraints in (34), respectively.The equality constraints generating predictions (34b) and (34c) are omitted here since the predictions of task coordinates x i and joint angles q can be expressed by functions of the input U i and recent states x i (k) and q(k).For detailed expressions of Hessian matrix Q i , gradient vector L i , and other parameters in (35), refer to Appendix A. Note that for i = 1, the task priority constraint (35e) does not exist.
To examine the existence and uniqueness of solutions, the following lemma about the Hessian matrix Q i is given.
Lemma 1: The Hessian matrix Q i is positive definite.Proof: As shown in Appendix A, Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
Then, assuming D i is nonempty, we present a theorem about the properties of the solution to Problem i with the positive definite Hessian matrix Q i .
Theorem 1 (Existence and Uniqueness of the Solution, [36], [59]): Given that D i is nonempty, the solution to Problem i exists and is unique.
Proof: Since D i , the set for the admissible input trajectory Δτ i , is nonempty, the admissible set U i for the argument U i of the QP ( 35) is also nonempty.In the following, we will use the convexity of the QP (35) to verify the existence and uniqueness of the solution to Problem i.
According to (35), the admissible set U i for U i is That is, the admissible set ) and three sublevel sets ) are linear and affine, and inequality constraint functions G i1 , G i2 , and G i3 are all linear and convex.Thus, the admissible set U i is convex.In addition, the cost function 35) is a convex optimization problem and solutions exist.Moreover, since the Hessian matrix Q i is positive definite as shown in Lemma 1, the cost function U i Q i U i + U i L i is thus strictly convex.Therefore, the solution to Problem i is unique given that the admissible set D i is nonempty.
Finally, the hierarchical feasibility of HIMPC is proven.

Theorem 2 (Hierarchical Feasibility of HIMPC):
The proposed HIMPC admits hierarchical feasibility at each time k.
Proof: Assume Problem i is feasible.Then, the admissible set D i is nonempty, and, in accordance withTheorem 1, there exists an optimal control input sequence Δτ * i,k such that the state, input, and task priority constraints (task priority constraints are not required when i = 1) Ĵj Next, the feasibility of Problem (i + 1) will be investigated by inspecting whether Δτ * i,k is an admissible control input sequence also for Problem (i + 1).Substituting Δτ * i,k into Problem (i + 1), obviously the state and input constraints, as well as priority constraints for p ∈ I [1,i−1] are not violated because these constraints are identical to that in Problem i. Besides, the priority constraint when p = i is also fulfilled, i.e., if Hence, no constraints are violated and Δτ * i,k is a feasible control input sequence for Problem (i + 1).We conclude that Problem (i + 1) is feasible if Problem i is feasible, i.e., the proposed HIMPC is hierarchically feasible.
Assume Problem 1 is feasible at time k.By induction, we see that Problems 2 to r are also feasible at time k.Therefore, from Theorem 2, it is concluded that the admissible set In accordance with Theorems 1 and 2, we conclude that the solution to HIMPC exists and is unique if Problem 1 is feasible.In the sequel, recursive feasibility [34], [35] of Problem 1 is demonstrated.
2) Recursive Feasibility: In this part, recursive feasibility of Problem 1 in a local region around the reachable reference trajectory is demonstrated.First, similar to [34], [35], and [45], the definition of the reachable reference trajectory is introduced.
In accordance with the definition of the reachable reference trajectory (Definition 2) and Lemma 4 in [45], we obtain that there exists a constant V max ∈ R >0 such that optimal solutions from x 1 (k) ) is the value function, both Q and Ū are tightened constraint sets, i.e., Q ⊕ C 4n s ⊆ Q, Ū ⊕ C 4n r ⊆ U, s and r are positive scalars.In other words, the reachable reference trajectory can be tracked and lies strictly in tightened constraint sets.
Then, the recursive feasibility of MPC in a local region D around the reachable reference trajectory will be demonstrated in Theorem 3, and the main work is to prove the regionally decreasing property of V N ( x(k), k).
Theorem 3 (Recursive Feasibility): Problem 1 admits recursive feasibility in a local region around the reachable reference trajectory.
Proof: Because of the task-priority constraint (29), the optimal state prediction in task 1, x 1 * 1,k+1|k , will not be disturbed by low-priority tasks, i.e., as the auxiliary value function, and then according to feasibility analysis for MPC without terminal ingredients in [34], [35], and [45], we obtain that for a sufficient long prediction horizon N Besides, according to (12), we have x * 1,k+1|k − x 1 (k + 1) ≤ ¯ with ¯ being a bounded parameter w.r.t. the TDE error.
Next, according to the Lipschitz continuity property of V N ( x(k), k) [45], there exists a constant K ∈ R >0 such that In combination of ( 37) and (38), we obtain the regionally decreasing property of V N ( x 1 (k), k), i.e., where N := K ¯ is the cumulative error bound.
Finally, based on the regionally decreasing property of V N ( x(k), k), the recursive feasibility of Problem 1 will be verified.Assuming the TDE error is sufficiently small In other words, D is a positive invariant set.Therefore, V N ( x 1 (k), k) ≤ V max holds recursively.According to the definition of the reachable reference trajectory, when x 1 ∈ D the input and state constraints are satisfied.In other words, the recursive feasibility of Problem 1 in a local region around the reachable reference trajectory is demonstrated.Remark 9 (How to Determine the Prediction Horizon): For MPC without terminal ingredients, a sufficiently large prediction horizon is required to guarantee feasibility [34], [35], [45].However, in the context of the robotic system, the sampling period is 1 ms.It is challenging to solve OCPs with large prediction horizons in one sampling period.In [34], [35], and [45], the minimal prediction horizon is theoretically derived.Nevertheless, the prediction horizon is normally overestimated.In [35] and [45], it is discussed that in practice a short prediction horizon is sufficient for local stability.Through deriving the required prediction horizon theoretically in [35] and [45], it is concluded that control performance improves with larger prediction horizons.Thus, it provides a guideline to tune the prediction horizon, i.e., considering available computing resources, the prediction horizon is selected as large as possible.

C. Discussion
In this subsection, the advantages of the proposed HIMPC over state-of-the-art task-prioritized controllers are discussed.Besides, limitations of the proposed method are also analyzed.

1) Strengths of HIMPC: Low Requirements for Modeling:
In existing MPC based task-prioritized control schemes [17], [18], state predictions are generated using nominal models.This requires performing time-consuming identification of the mathematical model parameters of the plant.The proposed HIMPC employs incremental systems to obtain state predictions, and thus it is not necessary to identify accurate models.For incremental systems (cf.( 13) and ( 17)), only M is associated with the dynamics model because M is selected such that I − M −1 M < 1.As discussed in Remark 4, I − M −1 M < 1 holds for a sufficiently small positive Mi even though the exact expression of the inertia matrix M is unknown.Therefore, the proposed HIMPC has low requirements for modeling.
Robustness: As shown in Section III-B, the incremental systems show high approximation accuracy and strong robustness against inertia modeling errors and external disturbances.This is because the inertia modeling error and external disturbances are compensated by time-delayed signals.The robustness of this proposed method is also reflected in terms of dynamic consistency.In [16], a nominal M is used to construct equality constraints [similar to (34g)], and it is obvious that dynamic consistency deteriorates when there are inertia modeling errors.In HIMPC, M is replaced by a predetermined M in the task priority constraint (34g).Thus, enhanced dynamic consistency is received.This advantage of HIMPC is also visible in the simulations and experiments presented in Sections V and VI.
Computational Efficiency: The TDE method not only improves robustness of the control scheme, but also simplifies equations of motion of tasks and system dynamics.As shown in ( 15) and ( 18), both, equations of motion and system dynamics, are approximated by linear systems, where online calculation of the complex nonlinear system dynamics terms is not required.As a result, each constrained OCP is a QP problem, and computational complexity will decrease dramatically, allowing for real-time control in milliseconds.In comparison, the MPC frameworks in [17] and [18] are nonlinear since nonlinear equations are employed to generate state predictions.For a general NMPC scheme, heavy computational complexity restricts its real-time application [49].
Singularity Handling: Different from null-space projection based methods [4], [5], [6], [7], [8], [9], [10], [11], [12], we employ equality constraint (34g) to guarantee task hierarchy.No matrix inversion is required and algorithmic singularity is avoided.Besides, also kinematic singularity is avoided.In our approach, the cost function (31) is designed relying on target motion dynamics (4), where inverse calculation of the terms w.r.t.Jacobian matrices is not necessary.In contrast, e.g., in [16], Cartesian forces calculated using the equivalent Cartesian mass matrix (JM −1 J ) −1 are involved in stage costs, and kinematic singularity occurs when the Jacobian matrix loses its rank.Also, different from the approaches in [14] and [15], Hessian matrices of the constrained OCPs are always positive definite (see Lemma 1).This allows to relax the common assumption that singularityfree tasks are defined and gives more flexibility in defining tasks.The simulations and experiments in Sections V and VI will further discuss singularity handling.In [60], kinematic singularity was avoided by adding a manipulability constraint to the constrained OCP.Nevertheless, the constrained OCP in [60] can only be cast to a general NLP problem which is in addition nonconvex because of the introduced nonlinear manipulability constraint.In this article, no constraints are imposed in the considered OCP to avoid kinematic singularity and the constrained OCP is convex and is cast to a QP.

Weighting Matrices are Determined without Considering Reference Trajectories:
The weighted control signal in (31) not only allows for energy efficiency and attenuates controller oscillations, but also renders the Hessian matrix positive-definite.The result is similar to regularization [61], which is used to avoid kinematic singularity.We will verify that the incremental control structure exhibits superior control performance in terms Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply. of parameter adjustment.If, as an alternative, the torque τ is considered, and the stage cost is designed: The term τ k+j|k R i is used to achieve the Hessian matrix regularization.However, τ is not always equal to zero even the system is in equilibrium, since nonzero τ is required to maintain the configuration and compensate for gravity.To guarantee tracking accuracy, we need to scale the term τ k+j|k R i to be roughly the same size to the tracking error term e( x i,k+j+1|k ) Q i .Thus, R i should be tuned manually according to the target trajectory [61], and R i should be readjusted when the reference trajectory changes.This is the main limitation of the regularization method.In comparison, the standard cost function for tracking MPC (see (7) in [35]) involves the difference between the control signal and the reference counterpart, and the difference lies inside a small neighborhood around the origin for any reference signal.Thus, R i can be determined without considering reference signals.
Unfortunately, for a redundant robot manipulator, the reference control signal is not determined priorly.This is the main reason for the limitation of the regularization method when it is applied to a redundant robotic system.In this article, the cost function ( 31) is similar to the standard stage cost for the tracking control scheme where the most recent control signal τ 0 is considered as the reference counterpart.Therefore, R i can be determined without considering reference trajectories, owing to the incremental control structure.
2) Limitations of HIMPC-TDE Error: In the constrained OCPs from Section IV-A, equality constraints (cf.(34g)) are imposed on control signals to achieve task hierarchy.However, the TDE error is ignored in the nominal incremental system (13).Consequently, task hierarchy will inevitably deteriorate, which is also visible in simulation and experimental results, see Sections V and VI.In addition, given that the TDE error and measurement noise are ignored in this article, there are state prediction errors.As a result, the system suffers from the risk of state constraint violation.After all, huge deterioration of task hierarchy and strongly violating state constraints are avoided.This is because incremental systems exhibit high approximation accuracy as analyzed in Section III-B.A widely used method to deal with uncertainties and to guarantee strict state constraint satisfaction is tube-based MPC [62].However, in this article, we control task-space variables and impose constraints on joint-space variables.In addition, the cost function does not involve terms w.r.t.joint-space variables since the considered robot manipulator is redundant and the reference signals for joint-space variables cannot be determined priorly.As a result, the optimal initial joint-space variable cannot be obtained, and then tube-based MPC cannot be implemented.A practical alternative is to use the data-driven method to estimate the uncertainty distribution and then employ stochastic MPC [63] to enhance the robustness of MPC in terms of strict state constraint satisfaction.

V. EXPERIMENT ON A THREE-DOF ROBOT MANIPULATOR
The effectiveness of HIMPC is investigated in experiments.It is therefore compared to other state-of-the-art controllers, such as OSF [9], TDE enhanced OSF (TDEOSF) [10], and HQP [16].

A. Experimental Setup
A custom-built three-DoF planar robot manipulator is used for the experimental verification as shown in Fig. 2. See [48] for the system dynamics of the robot manipulator.Note that the identified system dynamics is not used for TDEOSF and HIMPC because they allow for model-free controller design.The manipulator is actuated by three torque-controlled (Maxon) motors with a turn ratio of 1:100.Mounted on each motor, the incremental encoders offer a joint position measurement with a resolution of 1.25×10 −3 deg.Using a peripheral component interconnect communication card, the sensors and actuators are connected with an Intel Core TM (i7 8086K @4.0 GHz) CPU computer.The executable algorithm is created by MATLAB 2017a in Ubuntu 14.04 LTS, using a first-order solver with a sampling rate of 1 kHz.
A task hierarchy with two priority levels is implemented.The task definitions and control gains are shown in Table I.For HQP and HIMPC, qpOASES [52] is used to solve the constrained OCPs.For HIMPC, weighting matrices are Q i = 200I and R i = 10I for i =1,2.The lower (upper) bounds of joint position, velocity, and torques are −150 (+150) • , -100 (+100) •/s , and −4 (+4) N • m.For TDEOSF and HIMPC, M = diag(0.033,0.033, 0.033) is tuned, as addressed in Remark 4. On the premise of ensuring real-time control, we increase the prediction horizon as much as possible to receive satisfactory control performance.In the context of the hardware setup and requirements on safety, Fig. 3. Reference trajectories RT1 and RT2.x d and y d are reference signals for coordinates x and y of the EE in task space, respectively, and the reference geometric curve of the EE is a circle.q 1,d1 and q 1,d2 are reference signals for q 1 .RT1 includes x d , y d , and q 1,d1 , and RT2 involves x d , y d , and q 1,d2 .N = 5 is selected.The effects of the prediction horizon on the system will be investigated by simulations in Section VI.
Two scenarios are implemented to verify the performance of the proposed method w.r.t.control accuracy, optimality, task hierarchy, singularity handling, and constraint handling.

B. Scenario 1: Control Accuracy and Optimality 1) Setting:
The reference trajectory RT1 (see Fig. 3) is used and the initial configuration is q 0 = [0, 90, -90] deg.a) Control Accuracy: OSF, HQP, TDEOSF, and HIMPC are implemented for comparison.The discrepancy between the identified and the real mathematical model is considered as a disturbance.The robustness of tracking performance is analyzed by investigation of control accuracy.b) Optimality (qualitative): We take into account tracking errors and control signals in the cost function (31).Thus, tracking error and control signals are inspected to investigate the optimality of the control methods.We will first compare HIMPC and TDEOSF and analyze how optimality in HIMPC improves performance.Then, we introduce HIQP, i.e., an acronym of hierarchical incremental QP control which denotes HIMPC with the prediction horizon N = 1, and compare this to HIMPC with N = 5 to investigate local optimality of the controller.Note that HIQP is merely implemented in Section V-B.c) Optimality (quantitative): We will calculate the average cost when applying each of the controllers.With the stage cost for the predictions, see (31), we now introduce the average cost (41) for tasks to evaluate the closed-loop performance during a period [0, t s ] (iterations N t = t s /T s ) where t s is the terminal time.
where C i is the average cost for task i.Note that e(x i (k)) in ( 41) is calculated using measurements, not predictions.2) Results: The results are shown in Figs.4-8.

TABLE II EXPERIMENTAL RESULTS OF SCENARIO 1: ROOT-MEAN-SQUARE-ERRORS
(RMSE) OF DIFFERENT CONTROLLERS a) All of the controllers, OSF, HQP, TDEOSF, and HIMPC, allow to complete the two hierarchic tasks, but the tracking accuracy is different.As shown in Fig. 4 and Table II  inevitable as we analyzed in Section I-A.TDE-based methods (TDEOSF and HIMPC) are model-free ones.Thus, they are not sensitive to the model discrepancy and exhibit strong robustness against disturbances.This allows for high tracking precision.b) For further analysis, the tracking errors of TDEOSF and HIMPC are again displayed in Fig. 5, and we conclude that tracking performance of both methods is satisfactory.
The tracking errors of TDEOSF are the smallest.This is because, in HIMPC, the cost function (31) does not only take into account the desired tracking error dynamics, but also the control signal to avoid controller oscillations and to achieve energy efficiency and actuator protection.Thus, the superiority of HIMPC is visible when we in addition look at input trajectories.In Fig. 6, it is observed that in comparison to TDEOSF, control signals of HIMPC are smoother.There are many "glitches" in the control signal trajectories of TDEOSF.This is because the control signal of HIMPC is obtained through solving OCPs.In contrast, for TDEOSF, the control signal is calculated from analytic expressions, focusing on addressing tracking errors, while optimality is not considered.
A final comparison between HIQP and HIMPC analyzes the local optimality of the controller, and experimental results are shown in Figs. 5 and 7.For HIQP and HIMPC, the tracking errors of task 1 are very similar.However, for task 2, the tracking errors of HIQP are larger than those of HIMPC during some periods (e.g., [2 s, 3s]).In addition, we observe that the control signals of HIMPC are smoother than those of HIQP (see rectangular symbols in Fig. 7).This is because control signals of HIMPC are obtained by solving OCPs over a longer prediction horizon, while HIQP only takes into account a one-step-ahead prediction resulting in torque peaks and oscillations, especially for a noisy environment.c) Fig. 8 shows that the average cost of tasks 1-2 is the lowest for HIMPC, which quantitatively verifies the superior optimality.Although the control signals of HIQP are also obtained through solving constrained OCPs, the average cost of task 2 under the action of HIQP is even higher than that of TDEOSF.Due to its one-step prediction, the HIQP controller is locally optimal.Thus, over a time horizon, the average cost of HIQP may be even higher than that of TDEOSF, though optimality is not considered for TDEOSF controller design.
a) Task Hierarchy and Constraint Admissibility: As long as the amplitude of the reference trajectory of task 2 (q 1 ) is small, task 1 and task 2 are nonconflicting.With increasing amplitude, tasks become more and more conflicting.We verify task hierarchy by investigating whether tracking performance of high-priority tasks will be affected when high and low-priority tasks conflict, and the reference trajectory RT2 is employed.RT2 is also used to investigate the capability of HIMPC to guarantee that input and state constraints are not violated.b) Algorithmic Singularity Handling: RT2 is used to demonstrate that algorithmic singularity is avoided in HQP and HIMPC.Null-space projection based methods (OSF and TDEOSF) are likely to have algorithmic singularity problems when tasks conflict.For safety, OSF and TDEOSF are implemented in simulations when RT2 is employed.c) Kinematic Singularity Handling: Moreover, RT3 is used to verify that kinematic singularity is avoided in HIMPC.For RT3, the robot manipulator is straightened and is kinematically singular at the start, end, and intersection between two circles.2) Results: The results are shown in Figs.10-15.a) Tracking errors of HQP and HIMPC are displayed in Fig. 10, and we observe that tracking errors of q 1 (task 2) are large in the time period [3.2 s, 6s].This is because, during this period, tasks conflict.The low-priority task (task 2) has to "sacrifice" itself to guarantee satisfactory tracking performance of the high-priority task.Thus, for HQP and HIMPC, task hierarchy is guaranteed.Besides, because HQP is a model-based method, the tracking error of HQP is higher than that of HIMPC.This further verifies the robustness of HIMPC.Besides, dynamic consistency can be verified by investigating whether tracking errors of the highest-priority task (task 1) are affected when different reference trajectories  (RT1 and RT2) are employed.For the convenience of comparison, tracking errors of task 1 when RT1 and RT2 are employed are summarized in Fig. 11, where one observes that for HQP, tracking performance of task 1 deteriorates when tasks conflict.This is because the nominal M is used in HQP to construct task priority constraints and dynamic consistency is adversely affected by the inertia modeling error.In contrast, for HIMPC, tracking performance of task 1 is nearly not affected no matter whether tasks conflict or not.This is because the TDE method is used to approximate equations of motion of tasks and M is replaced by a predetermined M in the task priority constraints [compare (34g)].Thus, enhanced dynamic consistency of HIMPC is obtained due to the TDE approximation.
In Fig. 12 it is shown that input constraints are not violated.During the period [3.5 s, 5.2s], the input torques of HIMPC are relatively large.This is because here tasks conflict.The control signal not only needs to guarantee the priority of task 1, but it also regulates tracking performance of low-priority tasks as well as possible.As we discussed in Section IV-C, the TDE error and measurement noise are not considered for state predictions in HIMPC, and it results in small prediction errors, see Fig. 13, where Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.joint velocity constraints are slightly violated for short periods.This is common in MPC for uncertain systems with measurement noises.Though, at least we observe in our study that state constraints are not strongly violated.This is because the incremental systems exhibit high approximation accuracy.

TABLE III COMPARISON BETWEEN THE PROPOSED HIMPC AND STATE-OF-THE-ART CONTROLLERS
b) As stated in Section I-A, null-space projection based methods, such as OSF and TDEOSF, are likely to suffer from algorithmic singularity when tasks conflict.For safety, we did numerical simulations to implement OSF and TDEOSF.Consistent with theoretical analysis, the values of J 2|p M −1 J 2|p and J 2|p M−1 J 2|p (J 2|p is the prioritized Jacobian matrix) under the action of OSF and TDEOSF tend to be 0 at around 3.3 s.In other words, J 2|p loses its rank (and is a null vector here).Then, (J 2|p M −1 J 2|p ) −1 and (J 2|p M−1 J 2|p ) −1 , tend to infinity and undesired (large-value) control signals are obtained.As a result, the system will be unstable.Note that in the context of task definitions (see Table I), J 2 =[1, 0, 0] is a constant vector.Thus, at this moment the system is not kinematically singular, but it is attributed to the prioritized Jacobian matrix (null-space projection idea), i.e., the algorithmic singularity occurs.For HQP and HIMPC, equality constraints are employed to achieve task hierarchy, and no prioritized inertia matrices are involved.Thus, algorithmic singularity is avoided in HQP and HIMPC.c) We finally verify that HIMPC can control even if the manipulator passes the kinematically singular configuration in Scenario 2, and RT3 (see Fig. 9) is considered.OSF, TDEOSF, and HQP cannot be implemented because of kinematic singularity in the initial configuration.The experimental results are shown in Figs.14-15.As shown in Fig. 15, tracking accuracy of HIMPC is still high.It verifies that HIMPC is kinematically singularity-free.Note that tracking errors of tasks (especially e y shown in Fig. 15) are slightly larger at the beginning and around 8 s.On the one hand, this is because the control degrees of freedom are partially lost when the robot manipulator is kinematically singular, and on the other hand, the reference trajectory is nonsmooth at 8 s.Thus, this task is challenging and it also evaluates the ability of this method to handle input and state constraints.Nevertheless, HIMPC guarantees that the system passes the singular and also nonsmooth region safely.Finally, we summarize the experimental results in Table III .

VI. SIMULATION OF A SEVEN-DOF KUKA ROBOT
The effectiveness of HIMPC is now in addition validated by simulations for a higher-DoF robot manipulator that allows for a task hierarchy with more than two task levels.

A. Simulation Setup
We simulate a seven-DoF KUKA LBR iiwa 14 R820 [64] using the MATLAB Robotics System Toolbox [65] on a PC with Intel Core TM CPU (i7 9700 K @3.60 GHz).The sampling rate is 1 kHz.A task hierarchy with three priority levels (see Fig. 16) is implemented with task definitions and control gains in the target motion dynamics (4) from Table IV.We introduce the tool center point (TCP) and x, y, and z denote its Cartesian coordinates in the body frame, while o X , o Y , and o Z are its orientation w.r.t. the X−, Y -and Z-axes.The initial configuration is q 0 [0, 60, 0, 60, 0, 0, 0 ] deg.Q i = 200I and R i =10I for i = 1, 2, 3. Constraints for joint position, velocity, and torques are chosen according to [64].The constrained OCPs in HIMPC are solved by qpOASES.According to Remark 4, the diagonal matrix is chosen as M = diag(0.1,0.21, 0.033, 0.042, 0.001, 0.001, 0.0003).
The reference trajectory RT4 is considered in the simulation.The reference curve of TCP is a "circle" with a radius of 10 cm (level 1), i.e., reference signals for Cartesian coordinates x d = 0.2637 − 0.1 sin(0.5πt− 0.5π), y d = −0.1 sin(0.5πt),and z d = 1.096 − 0.1(0.5πt).The orientation of TCP is assigned to be maintained (level 2), i.e., o X = o Y = o Z = 0 • .Moreover, q 1 is commanded to move in a range of 180 • (level 3), i.e., q 1,d = 90 + 90 sin (0.5πt − 0.5π)deg.Three scenarios are considered to verify the feasibility of HIMPC for higher-DoF robotic systems, to investigate effects of the prediction horizon on the system, to verify reasonability of approximation of Jacobians over the prediction horizon, and to investigate effects of the measurement resolution on control performance.
B. Scenario 1: Feasibility for Higher-DoF Robotic Systems 1) Setting: RT4 is designed such that low-priority tasks conflict with high-priority tasks.Task hierarchy is investigated by measuring whether tracking performance of high-priority tasks is affected by low-priority tasks.Also, it is verified that singularities are successfully avoided.The prediction horizon of HIMPC is N = 5 in this scenario.In addition, to further verify robustness of the developed HIMPC against inertia matrix modeling errors, an additional simulation is conducted with a disturbed inertia matrix.Each diagonal element of the inertia matrix is either reduced or increased by 20% of its nominal value, following the order − − + + − + +.
2) Simulation Results of Scenario 1: The results are displayed in Fig. 17.We observe that the low-priority task "sacrifices" itself to guarantee the task hierarchy, while tracking performance of high-priority tasks is almost not affected.For [3.8 s, 4s], tracking errors of level 2 tasks (especially the orientation of TCP w.r.t.Y-axis) slightly increase.It is mainly because joint positions are constrained.
However, for OSF, TDEOSF, and HQP, the system is unstable.It is because, in the initial configuration, the system is kinematically singular, i.e., the Jacobian matrix loses its rank (rank(col(J 1 , J 2 , J 3 )) = 6 < 7).For HIMPC, we see that tracking errors of the level 3 task (q 1 ) are slightly larger in the first 0.2 s.This is because the robot manipulator is kinematically singular and control degrees of freedom of the system are partially lost during this time period.Nevertheless, the system passes the singular region safely.For a disturbed inertia matrix (see Table V), we see that control accuracy (RMSE) and dynamic consistency are nearly not affected.This is in line with the theoretical analysis in Section III-B, i.e., the incremental systems and controller tolerate inertia modeling errors.

C. Scenario 2: Investigating Effects of the Prediction Horizon and Verifying Reasonability of Constraint Simplifications
1) Setting: In order to investigate effects of the length of the prediction horizon on the system, larger prediction horizons (N = 10, 20, 30) are used for HIMPC in this scenario.To further investigate reasonability of constraint simplifications and verify computational efficiency, a nonlinear hierarchical incremental model predictive control (NHIMPC) scheme is introduced, where the Jacobian matrices over the prediction horizon are updated by current joint state predictions, i.e., simplification in ( 29) and ( 33) are suspended in NHIMPC, which is also the only difference between HIMPC and NHIMPC, aiming for objective of comparison simulations.The constrained OCPs in the NHIMPC scheme is solved by an efficient NLP toolbox, CasADi [66], where the SQP method is employed.The computational complexity of the algorithms is investigated by monitoring the computing time for each prediction step.
2) Simulation Results of Scenario 2: For the investigated tasks (see Fig. 16), the Jacobian of task 3 is constant.Thus, the simplifications in ( 29) and (33) affect performance of levels 1 and 2 tasks mostly.Due to space limitations, we will only display tracking errors of x in this scenario.The simulation results are shown in Figs.[18][19] and Tables VI-VII.
As expected from the theory of MPC without terminal ingredients, the tracking performance becomes better with the rise of  19), tracking performance of NHIMPC is even worse.Due to the updated Jacobian, convexity of the constrained OCPs in NHIMPC is not guaranteed.For nonconvex programming problems, it may happen that only a local optimum is found that might be much worse than the global one.In addition, the computing time of the nonconvex programming problems are significantly higher (cf.Table VII).
As shown in Fig. 20, the average computing time of HIMPC increases exponentially with the rise of the prediction horizon, and it is much less than that of NHIMPC.Thus, the simplification in ( 29) not only guarantees convexity of the constrained OCPs, but also decreases computational complexity of the algorithm.It is worth to notice that the average computing time of HIMPC with N = 5 (8.05×10 −3 s) is longer than the sampling period (1 ms).It means the sampling time is to be higher to guarantee the real-time control for the considered seven-DoF manipulator with three task levels in the current controller implementation.To apply HIMPC on higher-DoF systems, yet to keep a small sampling period (e.g., 1 ms) in the same hardware setup, more practical implementation effort will be devoted to developing faster QP solvers [67] and approximation solutions.

D. Scenario 3: Investigating Effects of the Measurement Resolution on Control Performance 1) Setting:
The prediction horizon N = 5, and a series of measurement resolution from 0.5×10 −3 deg to 1×10 −2 °is selected.For a coarse resolution, the noisy response is triggered and control signal chattering will be observed.The noisy response is analyzed by investigating smoothness of control signal curves.The smoothness of curves is measured using the concept of curve smoothness, which is defined as: S = N t k=0 c(k) − c(k) 2 )/N t , where c(k) denotes the control signal for the benchmark, generated with the resolution of 1×10 −5 deg in this simulation.
2) Simulation Results of Scenario 3: The results are shown in Table VIII.The curve smoothness decreases when the resolution is coarser.Because of space limitation, we take τ 1 for example.We learn that when the curve smoothness is greater than 10 N • m, the control signal chattering is observed (see Fig. 21).In the example, to avoid excessive wear of actuators and guarantee great control performance, it is suggested that the encoder measures the joint angle more precisely than 5×10 −3 deg.For instance, one can consider the encoder with a resolution of 16 b attached after the gear, or that with 10 b at the motor shaft with a gear ratio of 100.

VII. CONCLUSION
In this article, a HIMPC is proposed for robot manipulators to execute multiple tasks simultaneously.To reduce dependency on an accurate mathematical model, equations of motion and system dynamics are approximated by incremental systems using TDE.It improves the robustness of the controller in terms of high tracking accuracy and enhanced dynamics consistency.For HIMPC, a series of constrained OCPs is developed, where task hierarchy is achieved by equality constraints, which are set based on the dynamic consistency principle instead of using null-space projections.Besides, inversion of terms that include Jacobians is not needed and all Hessian matrices of the constrained OCPs are positive definite.As a result, the proposed HIMPC is algorithmically and kinematically singularity-free.In addition, HIMPC results in a linear MPC.Compared with nonlinear MPC, the computational complexity of HIMPC dramatically decreases, which enables the controller to run with a sampling time of 1 ms.Finally, simulations and experiments were conducted, and the efficacy of HIMPC is validated.
Future research will be devoted to studying HIMPC with strict state constraints and it will be addressed by the computationally efficient data-driven disturbance estimation algorithm and the stochastic MPC method.

Fig. 1 .
Fig. 1.Control structure of the HIMPC, where "TD" denotes time delay of one sampling period, and Δ τ * i,k := [Δτ * 1,k , . . ., Δτ * i,k ].Problems are solved sequentially following the task priority order, and the optimal control law can only be obtained after all problems are solved.

Fig. 2 .
Fig. 2. Experimental setup of the robot manipulator with hardware and software architectures and its kinematic structure (l 1 = l 2 = l 3 = 0.3 m).

Fig. 9 .Fig. 10 .
Fig.9.Reference trajectory RT3.The reference curve of the EE consists of two tangent circles.At t = 0 s, 8 s, and 16 s, the reference configuration of the planar robot manipulator is straightened.

Fig. 12 .
Fig. 12. Experimental results of Scenario 2: Torque inputs of three joints with the reference trajectory RT2, where b 1,2 denotes the corresponding bounds.

Fig. 13 .
Fig. 13.Experimental results Scenario 2: Joint velocities of three joints with the reference trajectory RT2, where b 1,2 denotes the corresponding bounds.

Fig. 14 .
Fig. 14.Experimental results of Scenario 2: Stroboscopic motion of the robot manipulator using the proposed HIMPC.The reference trajectory is RT3, and the posture is kinematically singular at the 1st, 10th, and 19th points.

Fig. 19 .
Fig. 19.Simulation results of Scenario 2: Tracking errors of TCP positions in X-axis under the action of HIMPC and NHIMPC with N = 10, 20.
, tracking performance of the model-based methods (OSF and HQP) is inferior.This is because of the model discrepancy.Tracking performance of OSF and HQP might still be improved with a more accurate model, though its accurate identification is time consuming and modeling errors are Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.

TABLE V SIMULATION
RESULTS OF SCENARIO 2: RMSE OF HIMPC FOR SYSTEMS WITH NOMINAL AND DISTURBED INERTIA MATRICES

TABLE VI SIMULATION
RESULTS OF SCENARIO 2: RMSE OF HIMPC AND NHIMPC (10 −4 m) VII SIMULATION RESULTS OF SCENARIO 2: COMPUTING TIME OF HIMPC AND NHIMPC WITH N = 5 Fig. 20.Simulation results of Scenario 2: Average computing time of HIMPC per iteration with different prediction horizons.predictionhorizons,seeFig.18.A comparison between HIMPC and NHIMPC is shown in TableVI.Although the Jacobian is updated over the prediction horizon, RMSE of NHIMPC and HINPC are almost equal.Besides, during time periods [0.16 s, 0.2s] and [0.4 s, 0.5s] (see Fig.

TABLE VIII SIMULATION
RESULTS OF SCENARIO 3: CURVE SMOOTHNESS VALUES OF CONTROL SIGNAL CURVES WITH DIFFERENT RESOLUTIONS Fig. 21.Simulation results of Scenario 3: Control signals with the measurement resolution of 5×10 −3 deg.