Hierarchical multiloop MPC scheme for robot manipulators with nonlinear disturbance observer

1.   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.

2.   Dynamics model of robot manipulators

The n-link robot manipulator can be denoted by the dynamics equation ^[35]

where $\boldsymbol{q}\in {\mathfrak{R}}^{\mathrm{n}}$ is the vector of generalized joint coordinates, M$\left(\boldsymbol{q}\right)\in {\mathfrak{R}}^{\mathrm{n}}$ is the inertia matrix, $\boldsymbol{C}\left({\bf{q}}, \dot{{\bf{q}}}\right)\in {\mathfrak{R}}^{\mathrm{n}}$ is the vector of centrifugal and Coriolis torques, $\boldsymbol{G}\left(\boldsymbol{q}\right)\in {\mathfrak{R}}^{\mathrm{n}}$ is the gravity vector, and $\boldsymbol{\tau }\in {\mathfrak{R}}^{\mathrm{n}}$ is the input torques, $\boldsymbol{d}$ is the lumped interferences composed of unknown frictions, external disturbances, and sources of dynamics mismatch, etc.

3.   The proposed hierarchical multiloop MPC scheme

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

3.1.   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

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. ${\boldsymbol{\hat d}}$ signifies the estimation of the uncertainties.

By substituting Eq (4) into Eq (1) and sorting it out, we can get

3.2.   Nonlinear disturbance observer (NDO)

For robot manipulators, we can assume the NDO has the following form ^[36]

where $\widehat{\boldsymbol{d}}$ is the estimated value of d; $\boldsymbol{L}\left(\boldsymbol{q}, \dot{\boldsymbol{q}}\right)$ is a nonlinear function to be designed.

Substitute Eq (1) into Eq (6) yields

Design another auxiliary vector as

where $\boldsymbol{z}\in {\mathfrak{R}}^{\mathrm{n}}$ and $\boldsymbol{p}\left(\boldsymbol{q}, \dot{\boldsymbol{q}}\right)$ 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

Assumed that the change in disturbance relative to the observer is slow, i.e., the rate of change is zero, one can get

Then we have

Substitution of Eq (9) and Eq (13) into Eq (16) yields

At last, the observation error equation is obtained as

By properly designing the nonlinear function $\boldsymbol{L}\left(\boldsymbol{q}, \dot{\boldsymbol{q}}\right)$, the estimated interference value ${\boldsymbol{\hat d}}$ can be exponentially approximated to the true value of the interference d.

3.3.   Model predictive control (MPC)

Substituting Eq (14) into Eq (5), one can obtain

where ${\bf{ {\boldsymbol{ \eta }} }}$ 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 ${\boldsymbol{x}}_{i} = {[{x}_{i}^{1}, {x}_{i}^{2}]}^{\mathrm{T}} = {[{q}_{i}, {\dot{q}}_{i}]}^{\mathrm{T}}$, then we have the single input single output decoupled robot systems, as follows

For convenience, rewritten Eq (20) in matrix form as

where $\boldsymbol{A} = \left[\begin{array}{cc}0 & 1\\ 0 & 0\end{array}\right], \boldsymbol{B} = \left[\begin{array}{c}0\\ 1\end{array}\right].$

Substituting Eq (19) into Eq (21), we can get

The discrete form of Eq (22) can be expressed as

where ${\boldsymbol{w}}_{d}\left(k\right)$ represents the $\boldsymbol{B}{\boldsymbol{M}\left(\boldsymbol{q}\right)}^{-1}\boldsymbol{w}\left(t\right)$ 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

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 v_i(k) are also required. Thus the objective function can be defined by

where $\boldsymbol{e}\left(k+j|k\right) = \boldsymbol{y}\left(k+j|k\right)-\boldsymbol{r}\left(k+j|k\right)$, $\boldsymbol{x}(k+j|k)$, $\boldsymbol{r}(k+j|k)$, and $\boldsymbol{v}(k+j|k)$ respectively represent the prediction output, reference output, and prediction input for the $(k+j)$ step during the k^th sampling. ${‖.‖}_{Q}^{2}$ is the Euclidean norm multiplied by the output state weighting Q, ${‖.‖}_{R}^{2}$ is the Euclidian norm multiplied by the input torque weighting R, and ${‖.‖}_{P}^{2}$ 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 ${\boldsymbol{v}}_{min}$ and ${\boldsymbol{v}}_{max}$ represent the lower and upper limits of the input torque ${\boldsymbol{v}}_{k}$.

4.   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 ${lb}_{i}$ is the lower bound of the working range of joint $i$, and ${ub}_{i}$ is the upper bound of the working range of joint $i$. For instance, when $lb = [-\frac{\mathrm{\pi }}{2}, -\frac{\mathrm{\pi }}{2}, -\frac{3}{4}\mathrm{\pi }]$ and $ub = [\frac{\mathrm{\pi }}{2}, \frac{\mathrm{\pi }}{2}, \frac{3}{4}\mathrm{\pi }]$, the three-link planar robot and its working space using Robotics Toolbox for MATLAB is shown in Figure 3.

For the 3-DOF manipulator, the nonlinear vector function in the NDO can be designed as follows

Substitute Eq (28) into Eq (9) yields

Eventually, the nonlinear function $\boldsymbol{L}\left(\boldsymbol{q}, \dot{\boldsymbol{q}}\right)$ 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

In addition, in the MPC loop, the initial speeds of the robot are all set to ${\dot{q}}_{1}\left(0\right) = {\dot{q}}_{2}\left(0\right) = {\dot{q}}_{3}\left(0\right) = 0$, and the initial angles of the robot manipulator are assigned to ${q}_{1}\left(0\right) = {q}_{2}\left(0\right) = {q}_{3}\left(0\right) = 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, v_min = −100, and v_max = 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.

5.   Results and discussion

5.1.   Robot manipulator with unknown frictions

Since the frictions of robot system are always related to joint speeds, we can employ the Coulomb-viscous friction model for validation purpose. When the interferences consist only of the Coulomb-viscous frictions, it can be denoted by

where a_ri and b_ri are the viscous and Coulomb friction coefficient, respectively. And ${a}_{r1}$ = 10, ${a}_{r2}$ = 8, ${a}_{r3}$ = 5, ${b}_{r1}$ = 3.0, ${b}_{r2}$ = 2.4, ${b}_{r3}$ = 1.8 for the simulation; $\mathrm{s}\mathrm{g}\mathrm{n}(.)$ is the sign function (i.e., when x > 0, $\mathrm{s}\mathrm{g}\mathrm{n}\left(x\right)$ = 1; if x = 0, $\mathrm{s}\mathrm{g}\mathrm{n}\left(x\right)$ = 0; when x < 0, $\mathrm{s}\mathrm{g}\mathrm{n}\left(x\right)$ = −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.

5.2.   Robot manipulator with external disturbances

Since the external disturbance ${\boldsymbol{\tau }}_{d}$ 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 ${a}_{di}$ determines the size of the disturbance, ${b}_{di}$ signifies the center of the disturbance, ${\sigma }_{i}$ mainly represents the time range of disturbance. And ${a}_{d1}$ = 10, ${a}_{d2}$ = 8, ${a}_{d3}$ = 0.3, ${b}_{d1}$ = 1, ${b}_{d2}$ = 2, ${b}_{d3}$ = 3, ${\sigma }_{1}$ = ${\sigma }_{2}$ = ${\sigma }_{3}$ = 0.3 for the simulation.

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.

5.3.   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

where ${\boldsymbol{F}}_{rd}$ is the sum of ${\boldsymbol{F}}_{r}$ and ${\boldsymbol{\tau }}_{d}$, 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.

5.4.   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.

6.   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 multi-link 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.

Acknowledgments

This work is financially supported by the National Natural Science Foundation of China (No. 51875266).

Conflict of interest

The authors declare that there is no conflict of interest.