Hierarchical multiloop MPC scheme for robot manipulators with nonlinear disturbance observer

: This paper addresses the robust enhancement problem in the control of robot manipulators. A new hierarchical multiloop model predictive control (MPC) scheme is proposed by combining an inverse dynamics-based feedback linearization and a nonlinear disturbance observer (NDO) based uncertainty compensation. By employing inverse dynamics-based feedback linearization, the multi-link robot manipulator was decoupled to reduce the computational burden compared with the traditional MPC method. Moreover, an NDO was introduced into the input torque signal to compensate and correct the errors from external disturbances and uncertainties, aiming to enhance the robustness of the proposed controller. The feasibility of the proposed hierarchical multiloop MPC scheme was verified and validated via simulation of a 3-DOF robot manipulator. Results demonstrate that the proposed controller provides comparative accuracy and robustness and extends the existing state-of-the-art algorithms for the trajectory tracking problem of robot manipulators with disturbances.


Introduction
Designed to grasp and operate objects for industrial application, robot manipulators have been drawing increasing attention in the last decades [1]. The motion of robot manipulators is implemented by the embedded controllers, such as open loop controller and PID controller [2,3]. However, open loop controller or PID controller can hardly guarantee a high-quality performance of the robot manipulator under inevitable dynamic uncertainties and external disturbances. To deal with the problems, increasing efforts have been made and many robust control strategies have attained, for example, active disturbance rejection control [4], artificial neural network controller [5,6], fuzzy logical controller [7], fuzzy adaptive controller [8,9], sliding mode controller [10,11], integral sliding mode controller [12], linear matrix inequality scheme [13], et al.
As robot manipulators always confront various uncertainties during the working process, operators used to be forbidden to enter the working space of robot manipulators for safety purposes. In recent years, robot manipulators have been extensively concerned in not only industrial assembly but also agricultural engineering, medical surgery, public hygiene [14], and other fields [15,16]. Consequently, the expanding application scenarios require the robot manipulator to have high precision and response speed and, in the meantime, special attention to the way it achieves critical tasks, especially in some scenarios requiring compliant contact, obstacle avoidance, and close robot-human interaction [17]. For these scenarios, it is essential to constrain the input control variables and the state and output variables, bringing severe challenges to satisfying all the dynamic constraints via a classical control scheme.
To meet the demands of the complex constrained dynamics systems, optimal control theories are gradually initialed and developed [18]. Model predictive control (MPC) is a typical optimal control theory method [19,20]. Besides certain robustness, one significant feature of MPC is that it can consider constraints of input and output variables and state variables simultaneously. In this virtue, MPCs have effectively served many actual industrial applications, for example, unmanned aerial vehicles [21] and ground vehicles [22,23]. Meanwhile, MPCs are also considered to be a promising control technique for applications in the field of robot manipulators. Guechi et al. discussed an MPC for a 2-DoF (degree of freedom) robot manipulator and compared it with a linear quadratic (LQ) controller [24]. Wilson et al. put forward a simplified Nonlinear MPC for a 2-DoF vertical robot manipulator [25]. Best et al. presented an MPC scheme for a soft humanoid robot [26]. Carron et al. introduced a Gaussian process-based MPC scheme for the offset-free tracking of a robot manipulator [27]. It is noted that compared with classical control methods, MPC methods generate the optimal control sequence by predicting the future evolution of the objective variables [28]. Thus, the online optimization process of MPC will result in a remarkable increase in the calculation time of the manipulator controller. Especially for multi-link robot manipulators of solid coupling and nonlinear characteristics, the MPC control method often requires magnificent computational ability, which will significantly increase the overall cost of the manipulator controller. What is worse, the applications of robot manipulators in actual working circumstances are inevitably affected by workspace constraints, modeling uncertainties, and external disturbances. Although MPC has certain robustness, excessive uncertainties and disturbances still might cause the failure of MPC. These negative factors seriously limit the application of MPC controllers in robot manipulator systems [29].
Due to the possibility of excessive dynamics uncertainties and external disturbances, recent investigations have been dedicated to exploiting robust tube MPC methods capable of meeting the demands of the system constraints even in these critical conditions [30,31]. In their work, the worst possible uncertainties are taken into account to complete the binding dynamic constraints, which adds massive extra computational cost. To address this problem, Incremona et al. [32] developed a hierarchical multiloop scheme for robot manipulators with inverse dynamics, MPC, and integral sliding mode module, using inverse dynamics to decouple the robot manipulator and the integral sliding mode controller to reject the uncertainties of the unmatched dynamics model of the robot manipulators, which improves the robustness of the MPC while lessening the burden of online calculation. At the same time, however, this method implies the shortcomings of the chattering problem.
Although disturbance observers can provide a promising alternative way to estimating and rejecting the adversarial disturbances on dynamics systems [33,34], past research seldom discusses the knowledge of the application of Nonlinear disturbance observer (NDO) in the hierarchical multiloop MPC scheme for robot manipulators. This motivated our attempt to propose a novel robust hierarchical multiloop MPC scheme combining inverse dynamics, NDO, and MPC together in this article (see Figure 1). Specifically, the inverse dynamics controller is designed to decouple the n-link robot manipulator model into n single-link ones. The MPC is to ensure the optimal strategy of the manipulator system in terms of the evolution of state and output variable and input torque constraints. The NDO is used to compensate for the inevitable unknown frictions and external disturbances to eventually enhance the robust performance of the proposed controller at a low online calculation cost.

Dynamics model of robot manipulators
The n-link robot manipulator can be denoted by the dynamics equation [35] , where ∈ ℜ is the vector of generalized joint coordinates, M ∈ ℜ is the inertia matrix, , ∈ ℜ is the vector of centrifugal and Coriolis torques, ∈ ℜ is the gravity vector, and ∈ ℜ is the input torques, is the lumped interferences composed of unknown frictions, external disturbances, and sources of dynamics mismatch, etc.

The proposed hierarchical multiloop MPC scheme
This section proposes a hierarchical multiloop MPC scheme containing inverse dynamics control, MPC, and NDO based compensation.

Inverse dynamics based feedback linearization
When the interferences are all known, the control law based on the inverse dynamics scheme can be designed as: , where v is the auxiliary control variable. By substituting Eq (2) into Eq (1), one can obtain (3) However, since the uncertainties are challenging to know totally, the inverse dynamics control law, using an estimation on the uncertainties, can be set to , where the symbol "^" indicates the assessment of the physical quantity which covers. d signifies the estimation of the uncertainties. By substituting Eq (4) into Eq (1) and sorting it out, we can get (5)

Nonlinear disturbance observer (NDO)
For robot manipulators, we can assume the NDO has the following form [36] , , , where is the estimated value of d; , is a nonlinear function to be designed. Substitute Eq (1) into Eq (6) yields , , , Design another auxiliary vector as , where ∈ ℜ and , is the nonlinear function to design, which should satisfy , , By substitution of the derivative of Eq (8) into Eq (9), we can get , Substitute Eq (7) into Eq (10) yields Substitute Eq (8) into Eq (11) yields To sum up, the NDO can be expressed by The observation error of the disturbance observer is defined as (14) Assumed that the change in disturbance relative to the observer is slow, i.e., the rate of change is zero, one can get Substitution of Eq (9) and Eq (13) into Eq (16) At last, the observation error equation is obtained as , 0 By properly designing the nonlinear function , , the estimated interference value d can be exponentially approximated to the true value of the interference d.

Model predictive control (MPC)
Substituting Eq (14) into Eq (5), one can obtain (19) where η is an auxiliary parameter. The inverse dynamics controller is aiming to decouple the n-link robot manipulator model into n single-link ones. For the i th joint of the robot manipulator, let , , , then we have the single input single output decoupled robot systems, as follows (20) For convenience, rewritten Eq (20) in matrix form as where 0 1 0 0 , 0 1 .
Substituting Eq (19) into Eq (21), we can get The discrete form of Eq (22) can be expressed as 1 where represents the term after discretization. Yu et al. [37] have proven that MPC is a near-optimal control algorithm regardless of whether the perturbation is random or adversarial. Therefore, we can simplify the dynamics objective of the MPC loops as 1 The objective function needs to minimize the error between the output trajectory and the desired one in the MPC loops. More generally, extra penalties concerning the input torques vi(k) are also required. Thus the objective function can be defined by where , and | respectively represent the prediction output, reference output, and prediction input for the step during the k th sampling. ‖. ‖ is the Euclidean norm multiplied by the output state weighting Q, ‖. ‖ is the Euclidian norm multiplied by the input torque weighting R, and ‖. ‖ is the Euclidean norm multiplied by the final output state weighting P.
Eventually, the MPC can be formulated by solving the following quadratic constrained optimization problem: where and represent the lower and upper limits of the input torque .

Simulation
For simplicity without loss of generality, this work considers a vertical three-link planar robot manipulator to illustrate the effectiveness of the proposed hierarchical multiloop MPC scheme. The schematic diagram of the three-link planar robot manipulator is shown in Figure 2. And the inertia and structural parameters of the manipulator are listed in Table 1, of which g is the gravity acceleration. For the convenience of expression, the parameters in the simulation process are all set to dimensionless values.  In addition, for the manipulator in the joint space, the working range of each joint can be denoted by where is the lower bound of the working range of joint , and is the upper bound of the working range of joint . For instance, when , , π and , , π , the threelink planar robot and its working space using Robotics Toolbox for MATLAB is shown in Figure 3.
Eventually, the nonlinear function , can be denoted by During the procedure of the simulation experiment, desired joint trajectories should be input to the manipulator system at first. To demonstrate the proposed control scheme, a set of sinusoidal signals are employed into the robot manipulator, as follows 0.2 (31) In addition, in the MPC loop, the initial speeds of the robot are all set to 0 0 0 0, and the initial angles of the robot manipulator are assigned to 0 0 0 1. Besides, the control period is set to 0.01, the prediction horizon is set to 10, and the control horizon is set to 1. Moreover, Q = diag{50,0}, R = 1e-6, P = 2Q, vmin = −100, and vmax = 100. To validate this method, we will verify the trajectory tracking performance of the proposed hierarchical multiloop MPC scheme when the robot manipulator has unknown frictions, external disturbances, and the simultaneous existence of frictions and disturbances. It should be noted that the proposed control scheme and the design of the controllers could be more general, even in the spatial coordinates for robot manipulators of four or more links.

Robot manipulator with unknown frictions
Since the frictions of robot system are always related to joint speeds, we can employ the Coulombviscous friction model for validation purpose. When the interferences consist only of the Coulombviscous frictions, it can be denoted by sgn (32) where ari and bri are the viscous and Coulomb friction coefficient, respectively. And = 10, = 8, = 5, = 3.0, = 2.4, = 1.8 for the simulation; sgn . is the sign function (i.e., when x > 0, sgn = 1; if x = 0, sgn = 0; when x < 0, sgn = −1). When only unknown frictions exist in the robot manipulator, the time evolution of the unknown frictions and the observed values for each joint is demonstrated in Figure 4. It can be seen that the unknown friction torques start with a relatively large amplitude in the initial stage and then gradually decreases and are stably maintained in a small amplitude interval. In detail, for joints 1 and 2, at about one second, the estimated values of the NDO approximate the actual values of the unknown frictions; for joint 3, the observed value matches the unknown friction at the very beginning because the preset friction of the joint is less. It is worth mentioning that in this case, the system cannot converge if without the compensation of the NDO. When only unknown friction torque is there in the robot manipulator, the time evolution of the trajectory tracking curve for each joint is shown in Figure 5. It can be seen that each joint of the robot manipulator can track the reference trajectory from an initial position in less than half a second.

Robot manipulator with external disturbances
Since the external disturbance is always a time-dependent variables, when the interferences consist only of external disturbances, we assume that the interferences can be denoted by Gaussian function, as follows where determines the size of the disturbance, signifies the center of the disturbance, mainly represents the time range of disturbance. And  When only external disturbances exist in the robot manipulator, the time evolution of the unknown external disturbances and the observed values for each joint is demonstrated in Figure 6. Overall, the estimated values of the NDO approximate the actual values of the unknown external disturbances well. Specifically, for joint 1, it can be seen that the amplitude peak of the observed value is smaller than the actual disturbance and a small side peak followed. For joint 2, a slight valley before the peak occurs in the estimated values of the NDO, but it does not exist in the actual disturbance. For joint 3, although there are still some mismatches due to the small disturbance value, the overall matching degree between the estimated values of the NDO and the actual disturbance of the robot system is good. When there is only unknown external interference in the manipulator, the time evolution of the external disturbances and the observed values for each joint is demonstrated in Figure 7. It can be seen that each joint of the manipulator can also track the reference trajectory from an initial position in a limited time.

Robot manipulator with both unknown frictions and external disturbances
Furthermore, when both unknown frictions and external disturbances affect the robotic manipulator system, we can assume that they are a combination of frictions and disturbances, as follows (34) where is the sum of and , and the parameters of them are the same as the subsection above. When both unknown frictions and external disturbances are in the robot manipulator simultaneously, the time evolution of the total interferences and the observed values for each joint is demonstrated in Figure 8. It can be seen that, on the whole, the NDO can compensate for the total unknown interferences well, enhancing the robustness of the proposed controller. When both unknown frictions and external disturbances affect the robot manipulator simultaneously, the time evolution of the trajectory tracking curve for each joint is illustrated in Figure 9. It can be seen that each joint of the robot manipulator can quickly track the reference trajectory from an initial position.

Robot manipulator motion in Cartesian space
In addition, an example of the motion performance of the robot manipulator in Cartesian space is examined. At the initial position, the end of the third link is set to [1.2, 0.5], and the angle between the third link and the x-axis is -90 degrees. At the final position, the end of the third link is set to [2.423, 2.234], and the third link is parallel to the x-axis. Then the motion trajectory of the robot manipulator in Cartesian space is shown in Figure 10. It can be seen from Figure 10 that the robot manipulator can still reach the preset position in Cartesian space well even under the condition of unknown frictions and external disturbances. In brief, enhanced robust control can be achieved via the proposed hierarchical multiloop MPC scheme by rejecting the unknown interferences with the nonlinear disturbance observer.

Conclusions
This paper proposes an enhanced robust hierarchical multiloop control strategy by utilizing the inverse dynamics, NDO, and MPC to realize the optimal control of the trajectory tracking of robot manipulators under interferences. Specifically, the loop of the inverse dynamics decouples the multilink robot manipulator system to several single link dynamics systems, aiming to reduce the computational burden compared with the traditional MPC method. The NDO loop can compensate and correct the errors caused by external interferences in the robot manipulator to enhance the proposed controller's robustness.
The workability of the proposed hierarchical multiloop MPC scheme is validated by the simulation of a 3-DOF robot manipulator under three types of common interferences, i.e., unknown frictions, external disturbances, and both frictions and disturbances, respectively. At length, the simulation demonstrates the enhanced robustness and accuracy of the proposed controller.