Research on Operation Trajectory Tracking Control of Loader Working Mechanisms

Abstract

Autonomous shovel digging of loaders is the key technology to realise automation and intelligent operation. The effective tracking control for the target operation trajectory is one of its core parts. Proportional–integral–derivative (PID) and other control methods without system models have issues, such as large overshoot amplitudes and jitter phenomena under system constraints. Given that model predictive control (MPC) effectively deals with system constraints to ensure smooth operation, this paper introduces MPC into motion control for the loader’s working mechanism and proposes a trajectory tracking control method based on nonlinear model predictive control (NMPC). This study shows that, under the same system constraints for different target operation trajectories, the designed controller achieves better tracking performance than conventional PID and sliding-mode control (SMC) controllers in handling system constraints and ensuring smoothness. It is also found that the tracking performance decreases as the dig insertion depth increases. Therefore, trajectories with larger dig insertion depths are not recommended as viable operation trajectories. This study provides an important foundation and new insights for improving the control performance of the loader’s working mechanism.

1. Introduction

Automation and intelligence have become the mainstream development trend in the field of mechanical equipment [1]. Loaders are important mechanical equipment, whose main task is to load and transport bulk materials (e.g., sand, gravel, and soil) [2]. The operation cycle of a loader is mainly divided into four phases: shovelling, transporting, unloading, and levelling, which are all completed by the working mechanism [3]. At present, the shovel-digging operation still requires manual control, so autonomous shovel digging is of great practical significance for the realisation of automated operation [4]. In particular, the trajectory planning and tracking control of the shovel-digging operation is a key foundational aspect, i.e., the actual trajectory of the shovel bucket in the material not only affects the output of the shovel bucket in a single cycle [5] but also influences the energy consumption of the shovel-digging operation [6,7]. Therefore, effective tracking control of the operation trajectory is of great value for improving output and reducing energy consumption [8,9].

At present, the literature on trajectory tracking control for the loader’s working mechanism is relatively limited. The working mechanism of the excavator and other construction machinery has similar structural characteristics to the working mechanism of the loader [10,11], and its trajectory tracking control method is similarly relevant to the trajectory tracking control of the loader’s working mechanism. Therefore, research on the working mechanism of the loader and excavator can be considered relevant. For example, Gong [12] established a kinematic and dynamic model of the loader’s working mechanism in joint space and designed a shovel-digging trajectory tracking controller by calculating the moment method, which achieved effective tracking control of the working mechanism on the target trajectory. Zhang [13] established a two-degree-of-freedom dynamic model of the hydraulic excavator’s working mechanism and proposed a method for PID parameter setting through experimental testing, ultimately achieving effective tracking control of the set trajectory in a simulation environment. He [14] designed a trajectory controller for the excavator’s working mechanism based on the incremental theory of trajectory control and carried out a horizontal straight digging experiment on an excavator under a simulation environment comprising a trajectory controller and VR (Virtual Reality) software, ultimately achieving a good control effect by adjusting the control parameters. Yan [15] established a kinematic model of the excavator’s working mechanism for the problem of the accurate positioning of the bucket and designed a single-neuron adaptive PID controller, achieving the online optimisation of the controller parameters based on the quadratic performance index. Du [16] proposed a control method combining a radial basis function (RBF) neural network with a traditional PID controller to solve the problem of the automatic trajectory control of a single-bucket hydraulic excavator, which improved the control performance and adaptability of the system. Chen [17] proposed a PID trajectory control method based on dynamics compensation through dynamic analysis and the actual working characteristics of the working mechanism of an explosive charging vehicle to achieve tracking control of its operation trajectory. Feng [18] proposed a new adaptive sliding-mode control method based on an RBF neural network, i.e., by introducing a nonlinear term in the sliding mode, an adaptive terminal sliding-mode controller was designed for the excavator’s working mechanism, improving the dynamic performance and convergence speed of the controller. Liu [19] added a rapidly switchable electro-hydraulic proportional control model to the original control system of the working mechanism of a skid-steer loader and designed a fuzzy self-tuning PID controller, which effectively achieved tracking of its operation trajectory. Zhang [20] took into account the nonlinear, time-varying, and other factors of the excavator and proposed a trajectory tracking control strategy based on a fuzzy multiparameter self-tuning PID following the variable-domain concept, which achieved effective tracking of the planned trajectory. Azulay [21] proposed a deep reinforcement learning data-driven controller for unmanned ground vehicles with a customised bucket mechanism, which achieved operational control of the working mechanism by learning the cyclic operation data of the bucket.

In the above works, the main methods used are computational torque control [12], conventional PID control [13], trajectory incremental recursive control [14], neural network PID control [15,16], dynamics-compensated PID control [17], neural network adaptive sliding-mode control [18], fuzzy self-tuning PID control [19,20,22], data-driven iterative learning control [21,23,24], and so on. Among these control methods, PID control has the advantages of a simple structure and mature technology. It is the most widely used method for industrial applications and is often used as the basis for emerging intelligent control algorithms [25,26]. However, most of the above control methods, including PID, do not use a system model (or use a nominal model) for control, making it difficult to effectively deal with system constraints in the design of the control law. This results in large overshoot amplitudes, chattering phenomena, and other shortcomings under system constraints [27,28]. Thus, these shortcomings increase the burden and energy consumption of the drive device, introducing potential safety hazards in the shovel-digging operation.

In recent years, model predictive control (MPC), an emerging modern control method, has been applied in a variety of fields [29,30]. MPC takes an established system model as a predictive model and, when solving the feasible range of control inputs, can add constraints to explicitly handle system constraints, ultimately finding a set of optimal control inputs closest to the reference state by optimising the objective function [31]. So, MPC is an optimal control method based on a system model [32]. The literature [33,34,35] has shown that MPC has significant advantages in handling system constraints and making the system run smoothly compared with control methods that do not require a system model. However, most motion control methods for the working mechanism of heavy construction machinery, such as loaders, still use traditional techniques, and emerging control methods are rarely applied. In particular, the MPC method is rarely applied. But, as its advantages have been gradually highlighted, it has been used in the motion control of multi-arm robots and the driving control of loaders [36,37]. MPC is mainly divided into linear model predictive control (LMPC) and nonlinear model predictive control (NMPC). NMPC does not require linearisation and has higher accuracy than LMPC; therefore, it is more suitable for strongly nonlinear systems [38]. Currently, NMPC is widely used in the fields of unmanned driving and intelligent manufacturing [39,40,41,42]. In terms of trajectory tracking control based on NMPC, there are two main design ideas [38]: one is the standard NMPC method, which uses a discretised kinematic or dynamic model as the prediction model, and the other is the nonlinear error MPC (NEMPC) method, which uses a discretised nonlinear error model as the prediction model. The literature [38] states that the typical accuracy of NMPC is significantly better than that of NEMPC in the presence of large curvature mutations in the reference trajectory, and this conclusion applies to different motion platforms. In addition, our in-depth study of the mathematical model of the loader’s working mechanism confirms that the mechanism exhibits motion characteristics such as strong nonlinearity and a high degree of coupling [11,43]. Thus, a large amount of model details is lost after linearisation, leading to a significant decrease in accuracy. Additionally, there are large curvature mutations in the trajectory of the loader’s working mechanism when it is in operation. Therefore, the standard NMPC method may be more suitable for the trajectory control of the complex mechanism. In the following, NMPC refers exclusively to standard NMPC.

In summary, in the research of the operation trajectory tracking control of the loader’s working mechanism, PID, as a typical control method that does not require a system model, has difficulty effectively handling system constraints, resulting in large overshooting amplitudes and chattering phenomena under system constraints. But MPC, as a control method based on a system model, has a significant advantage in its ability to handle system constraints to make the system run smoothly. Therefore, this paper introduces the MPC method in the motion control of the loader’s working mechanism. Since the mechanism exhibits strong nonlinearity and a high degree of coupling and NMPC is more suitable for handling nonlinear systems, we propose a trajectory tracking control method based on NMPC for the working mechanism for the first time. First, the kinematic model of the working mechanism in the joint space is established using the Denavit–Hartenberg method (abbreviated as the “D–H method”), and then the kinematic model in the drive space is given through a mapping relationship between the joint space and drive space. Second, the target trajectory and system constraints are given, and then the kinematic model is used as the prediction model to design the trajectory tracking controller for the working mechanism based on NMPC, with conventional PID and SMC controllers used for comparison. Finally, a co-simulation platform is built for verification and analysis. It is shown that the controller designed in this paper outperforms conventional PID and SMC controllers in handling system constraints and ensuring smooth operation.

2. Modelling

2.1. Drive Space and Drive Parameters

As shown in Figure 1, the reversing six-linkage working mechanism system of the loader comprises five rigid bodies and two driving operating devices, which are regarded as being planar and articulated with each other [11]. Among them, component 1 is the front vehicle body, component 2 is the boom, component 3 is the bucket, component 4 is the rotating rod, and component 5 is the two-linkage. Driving operation device I is the lift hydraulic cylinder, which drives the boom up and down, and driving operation device II is the tilt hydraulic cylinder, which drives the bucket’s rotational movement. When the working mechanism operates, the working surface of the vehicle body is usually regarded as a plane, and the movement of the vehicle body (component 1) is regarded as a straight-line planar motion. As the working mechanism has three degrees of freedom in the plane, operators can control it in practice by adjusting the movement of the vehicle body and driving the operation devices (lift and tilt cylinders). Therefore, the kinematic model of the loader’s working mechanism must be established in the drive space for practical significance and value. First, the drive space is established as follows:

$${\mathbb{R}}_{drive}=\left\{s_v\left(t\right),l_l\left(t\right),l_t\left(t\right)\right\}$$

(1)

In Equation (1), t denotes the time variable; $s_v\left(t\right)$ denotes the front vehicle body displacement; $l_l\left(t\right)$ denotes the lift cylinder length and $l_t\left(t\right)$ denotes the tilt cylinder length, which are the drive space parameters; and ${\mathbb{R}}_{drive}$ denotes the set of drive space parameters, consisting of $s_v\left(t\right)$, $l_l\left(t\right)$, and $l_t\left(t\right)$. Here, the three parameters $s_v\left(t\right)$, $l_l\left(t\right)$, and $l_t\left(t\right)$ are independent of each other and are regarded as generalised coordinates in the drive space.

Due to the complex structural coupling of the working mechanism system (abbreviated as “mechanism system”), it is difficult to establish the kinematic model in the drive space directly. Therefore, we draw on the theory of robotics to first establish the kinematic model in the joint space, then seek the mapping relationship between the joint space and the drive space, and finally obtain the kinematic model in the drive space.

2.2. Kinematic Modelling

Considering the design of the controllers, the kinematic model of the mechanism system in the joint space is first established using the D–H method, which includes the positive and inverse kinematic models at the position level and the positive kinematic model at the velocity level. Then, the explicit derivation of analytical geometry is used to derive the positive and inverse kinematic mapping relationships between the joint space and the drive space, and finally, the kinematic model of the mechanism system in the drive space is obtained. Among these, the positive kinematic model at the velocity level in the drive space is used as the prediction model in the subsequent design of the NMPC controller, while the inverse kinematic model at the position level in the drive space is used as the nonlinear decoupling model in the subsequent design of the PID and SMC controllers.

2.2.1. Kinematic Model Based on the D-H Method

As shown in Figure 2, the Cartesian coordinate system $O_i{x}_i{y}_i{z}_i$ ($i=0,1,2,3,4^{\prime }$) of the mechanism system in the joint space is established. For convenience in modelling, three mutually independent parameters, namely the displacement $s_v$ of the front vehicle body (component 1) relative to the working surface, the rotation angle ${\theta }_2$ of the boom (component 2) relative to the vehicle body, and the rotation angle ${\theta }_3$ of the bucket (component 3) relative to the boom, are chosen as the generalized coordinates of the mechanism system in the joint space. The end point P (i.e., bucket tip) of the mechanism system is usually used as the target point for operation trajectory tracking control of the working mechanism. Therefore, kinematic modelling is carried out with point P as the research object.

(1) Kinematic model at the position level

First, build the positive kinematic model. The D–H parameters of the mechanism system are shown in

Table 1. D–H parameters of the mechanism system.

i	${\mathit{\alpha }}_{\mathit{i}-1}/\left(\mathbf{rad}\right)$	${\mathit{\ell }}_{\mathit{i}-1}/\left(\mathbf{m}\right)$	${\mathit{b}}_{\mathit{i}}/\left(\mathbf{m}\right)$	${\mathbf{\Theta }}_{\mathit{i}}/\left(\mathbf{rad}\right)$
1	0	$s_v$	0	$\pi /2$
2	0	$l_1$	0	${\theta }_2-\pi /2$
3	0	$l_2$	0	${\theta }_3$
4	0	$l_3$	0	0

.

In Table 1, ${\alpha }_{i-1}$ denotes the rotation angle from axis $z_{i-1}$ to axis $z_i$ around axis $x_{i-1}$; ${\ell }_{i-1}$ denotes the distance from axis $z_{i-1}$ to the $z_i$ along axis $x_{i-1}$; $b_i$ denotes the distance from axis $x_{i-1}$ to axis $x_i$ along axis $z_i$; and ${\mathrm{\Theta }}_i$ denotes the rotation angle (i.e., the rotation angle of the joint i) from axis $x_{i-1}$ to axis $x_i$ around axis $z_i$.

The corresponding chi-square transformation matrix is as follows:

$${\mathbf{T}}_i^{i-1}=\left[\begin{array}{cccc}c{\mathrm{\Theta }}_i& -s{\mathrm{\Theta }}_i& 0& {\ell }_{i-1}\\ s{\mathrm{\Theta }}_{i}c{\alpha }_{i-1}& c{\mathrm{\Theta }}_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{b}_i\\ s{\mathrm{\Theta }}_{i}s{\alpha }_{i-1}& c{\mathrm{\Theta }}_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{b}_i\\ 0& 0& 0& 1\end{array}\right]$$

(2)

In Equation (2), ${\mathbf{T}}_i^{i-1}$ denotes the D–H transformation matrix from system $O_{i-1}{x}_{i-1}{y}_{i-1}{z}_{i-1}$ to system $O_i{x}_i{y}_i{z}_i$, and $s{\mathrm{\Theta }}_i$ ($c{\mathrm{\Theta }}_i$) and $s{\alpha }_{i-1}$ ($c{\alpha }_{i-1}$) denote $sin{\mathrm{\Theta }}_i$ ($cos{\mathrm{\Theta }}_i$) and $sin{\alpha }_{i-1}$ ($cos{\alpha }_{i-1}$), respectively.

The D–H transformation matrices for systems $O_0{x}_0{y}_0{z}_0$ to $O_{4^{\prime }}{x}_{4^{\prime }}{y}_{4^{\prime }}{z}_{4^{\prime }}$ are then obtained as follows:

$${\mathbf{T}}_{4^{\prime }}^0={\mathbf{T}}_1^0{\mathbf{T}}_2^1{\mathbf{T}}_3^2{\mathbf{T}}_{4^{\prime }}^3=\left[\begin{array}{cccc}{c}_{23}& -s_{23}& 0& s_v+l_2{c}_2+l_3{c}_{23}\\ s_{23}& c_{23}& 0& l_1+l_2{s}_2+l_3{s}_{23}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(3)

In Equation (3), $s_2$ ($c_2$) and $s_{23}$ ($c_{23}$) denote $sin{\theta }_2$ ($cos{\theta }_2$) and $sin\left({\theta }_2+{\theta }_3\right)$ ($cos\left({\theta }_2+{\theta }_3\right)$), respectively.

Therefore, the position matrix ${\mathbf{S}}_P$ of point P in system $O_0{x}_0{y}_0{z}_0$ is as follows:

$${\mathbf{S}}_P=\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\\ {\alpha }_P\\ {\beta }_P\\ {\theta }_P\end{array}\right]=\left[\begin{array}{c}{s}_v+l_2{c}_2+l_3{c}_{23}\\ l_1+l_2{s}_2+l_3{s}_{23}\\ 0\\ 0\\ 0\\ {\theta }_2+{\theta }_3\end{array}\right]$$

(4)

In Equation (4), $x_P$, $y_P$, $z_P$, and ${\alpha }_P$, ${\beta }_P$, ${\theta }_P$ denote the position coordinates and attitude angles of point P in system $O_0{x}_0{y}_0{z}_0$, respectively.

Next, build the inverse kinematic model. The bucket attitude angle ${\theta }_P$ is considered to be known when the mechanism operates in the plane, and the following relationship is obtained from Equation (4):

$$\left\{\begin{array}{c}{x}_P=s_v+l_2{c}_2+l_3{c}_{23}\hfill \\ y_P=l_1+l_2{s}_2+l_3{s}_{23}\hfill \\ {\theta }_P={\theta }_2+{\theta }_3\hfill \end{array}\right.$$

(5)

In Equation (5), ${\theta }_2\in \left(-\pi /2,\pi /2\right)$, $x_P-l_3{c}_P>s_v$. Then, Equation (5) yields the following positional inverse solution:

$$\left\{\begin{array}{c}{s}_v=x_P-l_3{c}_P-{\left[{l_2}^2-{\left(y_P-l_1-l_3{s}_P\right)}^2\right]}^{1/2}\hfill \\ {\theta }_2=si{n}^{-1}\left[\left(y_P-l_1-l_3{s}_P\right)/l_2\right]\hfill \\ {\theta }_3={\theta }_P-{\theta }_2\hfill \end{array}\right.$$

(6)

In Equation (6), $s_P$ and $c_P$ denote $sin{\theta }_P$ and $cos{\theta }_P$, respectively.

(2) Kinematic model at the velocity level

Construct the positive kinematic model. Figure 3 shows that for the joint space,

$$\left\{\begin{array}{c}\mathrm{\Theta }={\left[{\mathrm{\Theta }}_1,{\mathrm{\Theta }}_2,\cdots ,{\mathrm{\Theta }}_n\right]}^{\mathrm{T}}\hfill \\ \mathbf{J}\left(\mathrm{\Theta }\right)=\left[{\mathbf{J}}_1,{\mathbf{J}}_2,\cdots ,{\mathbf{J}}_n\right]\hfill \\ {\mathbf{J}}_i=\left[\begin{array}{c}{\mathbf{k}}_i\times {\mathbf{r}}_i\\ {\mathbf{k}}_i\end{array}\right]\hfill \\ {\mathbf{r}}_i={\mathbf{a}}_i+{\mathbf{a}}_{i+1}+\cdots +{\mathbf{a}}_n\hfill \end{array}\right.,i=1,2,\cdots ,n$$

(7)

In Equation (7), n denotes the number of joints; ${\mathrm{\Theta }}_i$ denotes the rotation angle of joint i; $\mathrm{\Theta }$ denotes the column vector consisting of ${\mathrm{\Theta }}_i$; $\mathbf{J}\left(\mathrm{\Theta }\right)$ denotes the Jacobian matrix in the joint space; ${\mathbf{J}}_i$ denotes the column vector element of $\mathbf{J}\left(\mathrm{\Theta }\right)$; ${\mathbf{k}}_i$ denotes the unit vector on the axis of rotation joint i (the direction is determined by the right-hand rule); ${\mathbf{r}}_i$ denotes the displacement vector of joint i to end point P; ${\mathbf{a}}_i$ ($i=1,2,\cdots ,n-1$) denotes the displacement vector of joint i to joint $i+1$; and ${\mathbf{a}}_i$ ($i=n$) denotes the displacement vector of joint i to end point P.

Then, the velocity of point P in system $O_0{x}_0{y}_0{z}_0$ is

$${\dot{\mathbf{S}}}_P=\left[\begin{array}{c}{\mathbf{v}}_P\\ {\omega }_P\end{array}\right]=\mathbf{J}\left(\mathrm{\Theta }\right)\dot{\mathrm{\Theta }}$$

(8)

where

$$\left[\begin{array}{c}{\mathbf{v}}_P\\ {\omega }_P\end{array}\right]={\left[v_{Px},v_{Py},v_{Pz},{\omega }_{Px},{\omega }_{Py},{\omega }_{Pz}\right]}^{\mathrm{T}}$$

(9)

In Equation (9), ${\mathbf{v}}_P$ and ${\omega }_P$, respectively, denote the velocity and angular velocity vectors of point P; and $v_{Px}$, $v_{Py}$, $v_{Pz}$ and ${\omega }_{Px}$, ${\omega }_{Py}$, ${\omega }_{Pz}$, respectively, denote the velocity and angular velocity components of point P in system $O_0{x}_0{y}_0{z}_0$.

Finally, by combining Table 1 and Figure 2, the following kinematic relations are obtained by relating Equations (7)–(9):

$$\left[\begin{array}{c}{v}_{Px}\\ v_{Py}\\ v_{Pz}\\ {\omega }_{Px}\\ {\omega }_{Py}\\ {\omega }_{Pz}\end{array}\right]=\left[\begin{array}{ccc}\begin{array}{c}1\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}& \begin{array}{c}-l_2{s}_2-l_3{s}_{23}\\ l_2{c}_2+l_3{c}_{23}\\ 0\\ 0\\ 0\\ 1\end{array}& \begin{array}{c}-l_3{s}_{23}\\ l_3{c}_{23}\\ 0\\ 0\\ 0\\ 1\end{array}\end{array}\right]\left[\begin{array}{c}{\dot{s}}_v\\ {\dot{\theta }}_2\\ {\dot{\theta }}_3\end{array}\right]$$

(10)

2.2.2. Mapping of the Joint Space to the Drive Space

In the previous subsection, the kinematic model of the mechanism system in the joint space was established, and the specific mapping relationship between the motion parameters of end point P and the joint space parameters ($s_v$, ${\theta }_2$ and ${\theta }_3$) was given. In this section, based on the explicit derivation method of analytical geometry, the motion-mapping relations between the joint space parameters (${\theta }_2$ and ${\theta }_3$) and the drive space parameters ($l_l$ and $l_t$) are established, obtaining the kinematic model of the mechanism in the drive space.

Combined with Figure 2, in order to make the mathematical notation uniform and neat, the following conventions are used: the symbol $l_{AB}$ denotes the length from point A to point B, and the symbol ${\gamma }_{ABC}$ denotes the angle $\angle ABC$. Since the design dimensions of the components of the mechanism system are known, the motion-mapping relationship between the joint space and the drive space is derived as follows.

First, construct the positive kinematic mapping relations. The mapping of the joint space parameter ${\theta }_2$ is

$$\left\{\begin{array}{c}{\gamma }_{A_0{O}_2{A}_1}={cos}^{-1}\left[\left({l_{O_2{A}_0}}^2+{l_{O_2{A}_1}}^2-{l_l}^2\right)/\left(2l_{O_2{A}_0}{l}_{O_2{A}_1}\right)\right]\hfill \\ {\dot{\gamma }}_{A_0{O}_2{A}_1}={\displaystyle \frac{\mathrm{d}{\gamma }_{A_0{O}_2{A}_1}}{\mathrm{d}{l}_l}}{\dot{l}}_l\hfill \end{array}\right.$$

(11)

$$\left\{\begin{array}{c}{\theta }_2=\pi /2-{\gamma }_{A_0{O}_2{X}_1}+{\gamma }_{A_0{O}_2{A}_1}-{\gamma }_{O_4{O}_2{A}_1}-{\gamma }_{O_3{O}_2{O}_4}\hfill \\ {\dot{\theta }}_2={\displaystyle \frac{\mathrm{d}{\theta }_2}{\mathrm{d}{\gamma }_{A_0{O}_2{A}_1}}}{\dot{\gamma }}_{A_0{O}_2{A}_1}\hfill \end{array}\right.$$

(12)

The mapping of the joint space parameter ${\theta }_3$ is

$$\left\{\begin{array}{c}{\gamma }_{O_4{O}_2{B}_0}={\theta }_2+{\gamma }_{O_3{O}_2{O}_4}+{\gamma }_{B_0{O}_2{X}_1}-\pi /2\hfill \\ {\dot{\gamma }}_{O_4{O}_2{B}_0}={\displaystyle \frac{\mathrm{d}{\gamma }_{O_4{O}_2{B}_0}}{\mathrm{d}{\theta }_2}}{\dot{\theta }}_2\hfill \end{array}\right.$$

(13)

Since the velocity derivation is the same for each transfer variable, only the position mapping is given next:

$$\left\{\begin{array}{c}{l}_{O_4{B}_0}={\left({l_{O_2{O}_4}}^2+{l_{O_2{B}_0}}^2-2l_{O_2{O}_4}{l}_{O_2{B}_0}cos{\gamma }_{O_4{O}_2{B}_0}\right)}^{1/2}\hfill \\ {\gamma }_{O_2{O}_4{B}_0}={sin}^{-1}\left[l_{O_2{B}_0}sin{\gamma }_{O_4{O}_2{B}_0}/\left(l_{O_4{B}_0}\right)\right]\hfill \\ {\gamma }_{B_0{O}_4{B}_1}={cos}^{-1}\left[\left({l_{O_4{B}_0}}^2+{l_{O_4{B}_1}}^2-{l_t}^2\right)/\left(2l_{O_4{B}_0}{l}_{O_4{B}_1}\right)\right]\hfill \\ {\gamma }_{O_2{O}_4{B}_1}={\gamma }_{B_0{O}_4{B}_1}-{\gamma }_{O_2{O}_4{B}_0}\hfill \\ {\gamma }_{O_3{O}_4{O}_5}={\gamma }_{O_2{O}_4{O}_3}+{\gamma }_{O_5{O}_4{B}_1}+{\gamma }_{O_2{O}_4{B}_1}-2\pi \hfill \\ l_{O_3{O}_5}={\left({l_{O_3{O}_4}}^2+{l_{O_4{O}_5}}^2-2l_{O_3{O}_4}{l}_{O_4{O}_5}cos{\gamma }_{O_3{O}_4{O}_5}\right)}^{1/2}\hfill \\ {\gamma }_{O_5{O}_3{O}_6}={cos}^{-1}\left[\left({l_{O_3{O}_5}}^2+{l_{O_3{O}_6}}^2-{l_{O_5{O}_6}}^2\right)/\left(2l_{O_3{O}_5}{l}_{O_3{O}_6}\right)\right]\hfill \\ {\gamma }_{O_4{O}_3{O}_5}={sin}^{-1}\left[l_{O_4{O}_5}sin{\gamma }_{O_3{O}_4{O}_5}/\left(l_{O_3{O}_5}\right)\right]\hfill \\ {\gamma }_{O_4{O}_3{O}_6}={\gamma }_{O_5{O}_3{O}_6}-{\gamma }_{O_4{O}_3{O}_5}\hfill \end{array}\right.$$

(14)

finally obtaining

$$\left\{\begin{array}{c}{\theta }_3=\pi -{\gamma }_{O_4{O}_3{O}_6}-{\gamma }_{O_6{O}_{3}P}-{\gamma }_{O_2{O}_3{O}_4}\hfill \\ {\dot{\theta }}_3={\displaystyle \frac{\mathrm{d}{\theta }_3}{\mathrm{d}{\gamma }_{O_4{O}_3{O}_6}}}{\dot{\gamma }}_{O_4{O}_3{O}_6}\hfill \end{array}\right.$$

(15)

Second, construct the inverse kinematic mapping relations. Since the derivative method is the same as above, it is not difficult to obtain the mapping of the drive space parameters $l_l$ and $l_t$. Considering the limited space, it is not repeated here. Eventually, the following form of the motion-mapping relation is obtained by relating Equations (4) and (6) to Equations (10) and (15):

$$\left\{\begin{array}{c}{\mathbf{T}}_P={\mathrm{\Phi }}_k\left(s_v,l_l,l_t,{\dot{s}}_v,{\dot{l}}_l,{\dot{l}}_t\right)\hfill \\ {\mathbf{L}}_{drive}={{\mathrm{\Phi }}_k}^{-1}\left(x_P,y_P,{\theta }_P,v_{Px},v_{Py},{\omega }_{Pz}\right)\hfill \end{array}\right.$$

(16)

where

$$\left\{\begin{array}{c}{\mathbf{T}}_P={\left[x_P,y_P,{\theta }_P,v_{Px},v_{Py},{\omega }_{Pz}\right]}^{\mathrm{T}}\hfill \\ {\mathbf{L}}_{drive}={\left[s_v,l_l,l_t,{\dot{s}}_v,{\dot{l}}_l,{\dot{l}}_t\right]}^{\mathrm{T}}\hfill \end{array}\right.$$

(17)

In Equation (16), ${\mathrm{\Phi }}_k$ denotes the motion-mapping function matrix and ${{\mathrm{\Phi }}_k}^{-1}$ denotes its inverse function matrix.

In summary, through the modelling method in this section, the kinematic model of the end point of the mechanism system in the drive space is established, which provides a basis for the subsequent design of the tracking controllers for the operation trajectory of the working mechanism.

3. Trajectories and Controllers

3.1. Operation Trajectories

As shown in Figure 4, we previously proposed a trajectory planning method based on the minimum shovel energy consumption by studying the bucket resistance during the operation of the working mechanism and designing an operation trajectory with a three-stage strategy [7]. In this study, we take this type of trajectory as the target operation trajectory (abbreviated as “target trajectory”) to complete the trajectory tracking controller design.

Combined with Figure 4, the operation process along the target trajectory can be described as follows: first, the bucket is laid flat and the bucket tip is moved from the initial position 0 to the starting point (position 1) of the shovel-digging operation; second, the bucket tip is inserted into the pile of material from position 1 and digs flatly to position 2; third, the bucket is inwardly turned over at a uniform speed and elevated to position 3; fourth, the bucket continues to be inwardly turned over at a uniform speed and elevated vertically to position 4; and finally, the bucket maintains a constant attitude and detaches from the pile of material from position 4 and continues to be elevated to position 5. Specifically, the target trajectory can be described by the following segmented function:

$$\left\{\begin{array}{c}\left\{\begin{array}{c}{x}_{Pr}=v_{Pr}·\left(t-t^{\left(0\right)}\right)+x_{Pr}^{\left(0\right)}\hfill \\ y_{Pr}=y_{Pr}^{\left(0\right)}\hfill \\ {\theta }_{Pr}={\theta }_{Pr}^{\left(0\right)}\hfill \end{array}\right.,t^{\left(0\right)}\le t<t^{\left(2\right)}\hfill \\ \left\{\begin{array}{c}{x}_{Pr}=v_{Pr}cos\rho ·\left(t-t^{\left(2\right)}\right)+x_{Pr}^{\left(2\right)}\hfill \\ y_{Pr}=v_{Pr}sin\rho ·\left(t-t^{\left(2\right)}\right)+y_{Pr}^{\left(2\right)}\hfill \\ {\theta }_{Pr}=\left({\theta }_{Pr}^{\left(3\right)}-{\theta }_{Pr}^{\left(2\right)}\right)·\left(t-t^{\left(2\right)}\right)/\left(t^{\left(3\right)}-t^{\left(2\right)}\right)+{\theta }_{Pr}^{\left(2\right)}\hfill \end{array}\right.,t^{\left(2\right)}\le t<t^{\left(3\right)}\hfill \\ \left\{\begin{array}{c}{x}_{Pr}=x_{Pr}^{\left(3\right)}\hfill \\ y_{Pr}=v_{Pr}·\left(t-t^{\left(3\right)}\right)+y_{Pr}^{\left(3\right)}\hfill \\ {\theta }_{Pr}=\left({\theta }_{Pr}^{\left(4\right)}-{\theta }_{Pr}^{\left(3\right)}\right)·\left(t-t^{\left(3\right)}\right)/\left(t^{\left(4\right)}-t^{\left(3\right)}\right)+{\theta }_{Pr}^{\left(3\right)}\hfill \end{array}\right.,t^{\left(3\right)}\le t<t^{\left(4\right)}\hfill \\ \left\{\begin{array}{c}{x}_{Pr}=x_{Pr}^{\left(4\right)}\hfill \\ y_{Pr}=v_{Pr}·\left(t-t^{\left(4\right)}\right)+y_{Pr}^{\left(4\right)}\hfill \\ {\theta }_{Pr}={\theta }_{Pr}^{\left(4\right)}\hfill \end{array}\right.,t^{\left(4\right)}\le t\le t^{\left(5\right)}\hfill \end{array}\right.$$

(18)

In Equation (18), $t^{\left(i\right)}$ ($i=0,\cdots ,5$) denotes the arrival time of point P at position i; $x_{Pr}$, $y_{Pr}$, and ${\theta }_{Pr}$ denote the position parameters of point P at any time t in system $O_0{x}_0{y}_0{z}_0$; $x_{Pr}^{\left(i\right)}$, $y_{Pr}^{\left(i\right)}$, and ${\theta }_{Pr}^{\left(i\right)}$ ($i=0,\cdots ,5$) denote the positions of point P at position i; $v_{Pr}$ denotes the target velocity of point P; and $\rho$ denotes the angle of repose of the material pile.

From Equation (18), it can be seen that this type of target trajectory can be described as a two-dimensional sequence composed of the bucket-tip position information and the bucket rotation angle (abbreviated as “bucket angle”) information, which is described and illustrated in [6]. It is briefly described as follows: in practice, the rated digging volume V (i.e., bucket-digging volume or weight) and the angle of repose $\rho$ are considered to be known, so the position information of the trajectory in the material pile is uniquely characterised by the dig insertion depth $s_1$ in Figure 4 (see [6] for specific proofs); the rated digging initial angle ${\theta }_{Pr}^{\left(0\right)}$ and the termination angle ${\theta }_{Pr}^{\left(5\right)}$ are considered to be known, so the bucket angle information can be uniquely characterised by the bucket intermediate angle ${\theta }_{Pr}^{\left(3\right)}$ (the bucket angle at position 3). Thus, any sequence of target trajectories in the target trajectory space can be characterised by the parameters $s_1$ and ${\theta }_{Pr}^{\left(3\right)}$.

In fact, the whole shovel-digging process in practical experience needs to follow the following principles [6]: ① steady movement of the vehicle body; ② steady lifting of the arm; and ③ steady rotation of the bucket. In addition, it is also necessary to consider the motion performance of the vehicle body and the driving hydraulic cylinder unit during the shovel-digging operation. Therefore, the design of the trajectory tracking controllers needs to consider the following system constraints:

$$\left\{\begin{array}{c}{v}_{vmin}\le {\dot{s}}_v\le v_{vmax},a_{vmin}\le {\ddot{s}}_v\le a_{vmax}\hfill \\ v_{lmin}\le {\dot{l}}_l\le v_{lmax},a_{lmin}\le {\ddot{l}}_l\le a_{lmax}\hfill \\ v_{tmin}\le {\dot{l}}_t\le v_{tmax},a_{tmin}\le {\ddot{l}}_t\le a_{tmax}\hfill \end{array}\right.$$

(19)

In Equation (19), $v_{vmax}$ ($v_{vmin}$), $v_{lmax}$ ($v_{lmin}$), and $v_{tmax}$ ($v_{tmin}$) denote the upper (lower) limit values of the movement velocity of the vehicle body, the piston rod of the lift cylinder, and the piston rod of the tilt cylinder, respectively, during the shovel-digging operation; and $a_{vmax}$ ($a_{vmin}$), $a_{lmax}$ ($a_{lmin}$), and $a_{tmax}$ ($a_{tmin}$) denote the upper (lower) limit values of the movement acceleration of the vehicle body, the piston rod of the lift cylinder, and the piston rod of the bucket cylinder, respectively, during the shovel-digging operation.

3.2. Controllers

3.2.1. NMPC Controller

Due to the working mechanism’s strong nonlinearity and high degree of coupling [11,43], and given that NMPC is more suitable for handling nonlinear systems [38], this paper adopts a nonlinear model predictive controller (NMPC controller).

The control principle of the NMPC controller in the operation trajectory tracking control of the working mechanism is shown in Figure 5. First, the prediction model predicts the motion state of the controlled mechanism at the next time step based on the current state of the controlled mechanism and the future control inputs within the constraints of the system and computes the deviation of its position information from the target trajectory information, which is then incorporated into the objective function. Then, the objective function iteratively computes the optimal control input based on the prediction model according to the size of the predicted time domain and the control time domain. Finally, the optimal control input is used as the control input for the actuator to control the motion of the mechanism.

The design of the NMPC controller focuses on the establishment of the prediction model and the design of the objective function. The prediction model is used to predict the future state of the bucket position based on the current position of the working mechanism and the future control inputs, so this paper establishes the prediction model using the kinematic model of the working mechanism. The kinematic model of the working mechanism (Equation (16)) can be written in the following general expression form:

$$\dot{\xi }=f\left(\xi ,\mathbf{u}\right)$$

(20)

In Equation (20), f denotes the state-space mapping function; $\xi$ denotes the state vector; and $\mathbf{u}$ denotes the control input of the system (i.e., the control input of the controlled system). The specific expressions are as follows:

$$\left\{\begin{array}{c}\xi ={\left[x_P,y_P,{\theta }_P\right]}^{\mathrm{T}}\hfill \\ \mathbf{u}={\left[{\dot{s}}_v,{\dot{l}}_l,{\dot{l}}_t\right]}^{\mathrm{T}}\hfill \end{array}\right.$$

(21)

From Equations (20) and (21), it follows that the state-space mapping function f is the function mapping form of the velocity-level kinematic model of the working mechanism in the state space, in which the control input $\mathbf{u}$ of the controlled system is a control sequence jointly composed of the vehicle body velocity ${\dot{s}}_v$, the lift/tilt cylinder piston rod velocity (${\dot{l}}_l$ and ${\dot{l}}_t$) in the drive space, i.e., by coordinating the cylinder piston rod velocity and the vehicle body velocity to enable the working mechanism to track the target trajectory. Therefore, the Euler method is used to discretise the above kinematic model, and the expression of the prediction model is as follows:

$$\xi \left(\left.t+1\right|t\right)=\xi \left(\left.t\right|t\right)+Tf\left(\xi \left(\left.t\right|t\right)\right.,\left.\mathbf{u}\left(\left.t\right|t\right)\right)$$

(22)

In Equation (22), T denotes the control period of the controller. Let the current time be t, the prediction time domain be $N_p$, and the control time domain be $N_c$. Then, in the prediction time domain, the position state of the working mechanism at any time is

$$\left\{\begin{array}{c}\xi \left(\left.t+1\right|t\right)=\xi \left(\left.t\right|t\right)+Tf\left(\xi \left(\left.t\right|t\right)\right.,\left.\mathbf{u}\left(\left.t\right|t\right)\right)\hfill \\ ⋮\hfill \\ \xi \left(\left.t+N_c+1\right|t\right)=\xi \left(\left.t+N_c\right|t\right)+Tf\left(\xi \left(\left.t+N_c\right|t\right)\right.,\left.\mathbf{u}\left(\left.t+N_c\right|t\right)\right)\hfill \\ ⋮\hfill \\ \xi \left(\left.t+N_p\right|t\right)=\xi \left(\left.t+N_p-1\right|t\right)+Tf\left(\xi \left(\left.t+N_p-1\right|t\right)\right.,\left.\mathbf{u}\left(\left.t+N_c\right|t\right)\right)\hfill \end{array}\right.$$

(23)

The position deviation of the working mechanism from the target trajectory within the prediction time domain is represented by the error vector $\mathbf{e}$, which is obtained from the prediction model as follows:

$$\mathbf{e}\left(\left.t+i\right|t\right)=\xi \left(\left.t+i\right|t\right)-{\xi }_r\left(\left.t+i\right|t\right),i=1,2,\cdots ,N_p$$

(24)

In Equation (24), ${\xi }_r$ represents the position state information of the tracking point of the target trajectory. The selection of the tracking point primarily depends on the position of the working mechanism at the current time and the given target velocity. Assuming that the function of the target trajectory is expressed as $\phi \left(x_{Pr},y_{Pr},{\theta }_{Pr}\right)$ (Equation (18)), the 0-th tracking point at time t is expressed as follows:

$$d\left(\xi \left(\left.t\right|t\right),{\xi }_r\left(\left.t\right|t\right)\right)=d_{min}\left(\xi \left(\left.t\right|t\right),\phi \left(x_{Pr},y_{Pr},{\theta }_{Pr}\right)\right)$$

(25)

In Equation (25), d denotes the function that computes the distance. The remaining tracking points in the prediction time domain are expressed as follows:

$$r_g\left({\xi }_r\left(\left.t+i\right|t\right),{\xi }_r\left(\left.t+i-1\right|t\right)\right)=T{v}_r,i=1,2,\cdots ,N_p$$

(26)

In Equation (26), $r_g$ denotes the arc length of the target trajectory between two tracking points and $v_r$ denotes the given target velocity.

Further, based on the prediction model established above, the objective function $\mathrm{\Gamma }$ is expressed as follows:

$$\mathrm{\Gamma }=\sum _{i=1}^{N_p}{∥\mathbf{e}\left(\left.t+i\right|t\right)∥}_{{\mathbf{Q}}_1}^2+\sum _{i=1}^{N_c}{∥\Delta \mathbf{u}\left(\left.t+i\right|t\right)∥}_{{\mathbf{Q}}_2}^2+{∥\epsilon ∥}_{{\mathbf{Q}}_3}^2$$

(27)

In Equation (27), the first term is the error penalty term, which reflects the controller’s ability to track the target trajectory; the second term reflects the requirement for smooth changes in the control inputs; $\epsilon$ denotes the relaxation factor; and ${\mathbf{Q}}_1$, ${\mathbf{Q}}_2$, and ${\mathbf{Q}}_3$ denote the matrices of the weight coefficients. In practice, the weight matrices (${\mathbf{Q}}_1$, ${\mathbf{Q}}_2$, and ${\mathbf{Q}}_3$) can be adjusted based on different operational requirements to achieve different control effects. For instance, increasing the value of each term of ${\mathbf{Q}}_1$ can improve the tracking accuracy, or increasing the value of each term of ${\mathbf{Q}}_2$ can improve the control smoothness.

The purpose of the objective function is to enable the mechanism to track the target trajectory as quickly and smoothly as possible. In this case, $\Delta \mathbf{u}\left(\left.t+i\right|t\right)$ is the increment of the control input, and its expression is as follows:

$$\Delta \mathbf{u}\left(\left.t+i\right|t\right)=\mathbf{u}\left(\left.t+i\right|t\right)-\mathbf{u}\left(\left.t+i-1\right|t\right),i=1,2,\cdots ,N_p$$

(28)

At this point, the trajectory tracking control problem for the working mechanism can be transformed into a multi-constraint quadratic programming problem, which takes the following form:

$$\left\{\begin{array}{c}\underset{\mathbf{u}\left(t\right),\epsilon }{min}\mathrm{\Gamma }={\displaystyle \sum _{i=1}^{N_p}}{∥\mathbf{e}\left(\left.t+i\right|t\right)∥}_{{\mathbf{Q}}_1}^2+{\displaystyle \sum _{i=1}^{N_c}}{∥\Delta \mathbf{u}\left(\left.t+i\right|t\right)∥}_{{\mathbf{Q}}_2}^2+{∥\epsilon ∥}_{{\mathbf{Q}}_3}^2\hfill \\ \mathrm{s}.\mathrm{t}.\left\{\begin{array}{c}{\mathbf{u}}_{min}\le \mathbf{u}\le {\mathbf{u}}_{max}\hfill \\ {\mathbf{a}}_{min}T\le \Delta \mathbf{u}\le {\mathbf{a}}_{max}T\hfill \end{array}\right.\hfill \end{array}\right.$$

(29)

where

$$\left\{\begin{array}{c}{\mathbf{u}}_{min}={\left[v_{vmin},v_{lmin},v_{tmin}\right]}^{\mathrm{T}}\hfill \\ {\mathbf{u}}_{max}={\left[v_{vmax},v_{lmax},v_{tmax}\right]}^{\mathrm{T}}\hfill \\ {\mathbf{a}}_{min}={\left[a_{vmin},a_{lmin},a_{tmin}\right]}^{\mathrm{T}}\hfill \\ {\mathbf{a}}_{max}={\left[a_{vmax},a_{lmax},a_{tmax}\right]}^{\mathrm{T}}\hfill \end{array}\right.$$

(30)

Finally, the optimal control sequence is obtained by numerically solving Equation (29), with the first element serving as the control input for the controlled system at the next time step.

3.2.2. PID Controller

PID, as a typical control method that does not require a system model, has been widely used in industrial applications due to its simple structure and mature technology. For this reason, this paper uses the conventional PID control method to compare and analyse the performance of the controller. Given that trajectory tracking control involves multi-driver cooperation and that PID has difficulty directly handling multiple inputs and multiple outputs, along with the presence of strong nonlinearities and other kinematic characteristics of the working mechanism, this subsection presents an inverse kinematic decoupled PID controller [44] based on the inverse kinematic model of the working mechanism described in Section 2.2.

The control quantity $\mathbf{u}\left(t\right)$ from the PID controller at time t in the discrete control system is given by

$$\left\{\begin{array}{c}\mathbf{u}\left(t\right)=K_P\left(\mathbf{e}\left(t\right)+{\displaystyle \frac{T}{T_I}}{\displaystyle \sum _{i=0}^t}\mathbf{e}\left(i\right)+{\displaystyle \frac{T_D}{T}}\left[\mathbf{e}\left(t\right)-\mathbf{e}\left(t-1\right)\right]\right)\hfill \\ \mathrm{s}.\mathrm{t}.\left\{\begin{array}{c}{\mathbf{u}}_{min}\le \mathbf{u}\le {\mathbf{u}}_{max}\hfill \\ {\mathbf{a}}_{min}T\le \Delta \mathbf{u}\le {\mathbf{a}}_{max}T\hfill \end{array}\right.\hfill \end{array}\right.$$

(31)

In Equation (31), $\mathbf{u}\left(t\right)$ denotes the control input from the controller (i.e., the control input of the controlled system); $\mathbf{e}\left(t\right)$ denotes the error vector; $K_P$ denotes the proportional gain; $T_I$ denotes the integral time constant; and $T_D$ denotes the derivative time constant. The error vector $\mathbf{e}\left(t\right)$ is expressed as follows:

$$\mathbf{e}=\left[\begin{array}{c}{s}_{ve}\\ l_{le}\\ l_{te}\end{array}\right]=\left[\begin{array}{c}{s}_{vr}-s_v\\ l_{lr}-l_l\\ l_{tr}-l_t\end{array}\right]$$

(32)

In Equation (32), $s_{vr}$ and $s_{ve}$, respectively, denote the target displacement and displacement error of the vehicle body; $l_{lr}$ and $l_{le}$, respectively, denote the target length and length error of the lift cylinder; and $l_{tr}$ and $l_{te}$, respectively, denote the target length and length error of the tilt cylinder.

By combining Equations (16) and (17), the following relationship holds:

$$\left\{\begin{array}{c}{\mathbf{L}}_{drive}={{\mathrm{\Phi }}_k}^{-1}\left(x_{Pr},y_{Pr},{\theta }_{Pr}\right)\hfill \\ {\mathbf{L}}_{drive}={\left[s_{vr},l_{lr},l_{tr}\right]}^{\mathrm{T}}\hfill \end{array}\right.$$

(33)

3.2.3. SMC Controller

SMC, as a variable structure control method with simple algorithms, fast response times, and insensitivity to parameter variations [45], has been widely adopted in many fields, including robotics, unmanned vehicles, and aerospace [46,47]. In order to more comprehensively compare and analyse the performance of the controllers, this paper uses the conventional SMC method, which adopts a reaching law-based SMC controller [48]. Similar to the above controllers, it aims to enable the working mechanism to track the target trajectory by controlling the piston rod velocity of the driving cylinders and the movement velocity of the vehicle body.

First, the sliding-mode function is defined. The sliding-mode function in the discrete control system is expressed in the following form at time t:

$$\mathbf{s}\left(t\right)=c\mathbf{e}\left(t\right),c>0$$

(34)

In Equation (34), $\mathbf{s}\left(t\right)$ denotes the sliding-mode function and c denotes the adjustment parameter.

According to Equations (32) and (34), the sliding-mode function takes the following form:

$$\mathbf{s}\left(t+1\right)=c\mathbf{e}\left(t+1\right)=c\left[\mathbf{e}\left(t\right)+T\mathbf{u}\left(t\right)-\Delta {\mathbf{L}}_{drive,r}\left(t+1\right)\right]$$

(35)

where

$$\Delta {\mathbf{L}}_{drive,r}\left(t+1\right)={\mathbf{L}}_{drive,r}\left(t+1\right)-{\mathbf{L}}_{drive,r}\left(t\right)$$

(36)

Second, choose the reaching law. The discrete reaching law uses the commonly used exponential reaching law [48] as follows:

$$\mathbf{s}\left(t+1\right)-\mathbf{s}\left(t\right)=-\mu T\mathrm{sgn}\left(\mathbf{s}\left(t\right)\right)-kT\mathbf{s}\left(t\right),\mu >0,k>0$$

(37)

In Equation (37), $\mu$ and k denote the adjustment parameters and $\mathrm{sgn}$ denotes the sign function, which is given by

$$\mathrm{sgn}\left(s\right)=\left\{\begin{array}{c}1,s>0\\ 0,s=0\\ -1,s<0\end{array}\right.$$

(38)

Finally, by combining Equations (35) and (37), the control input $\mathbf{u}\left(t\right)$ from the SMC controller at time t in the discrete control system is given by

$$\left\{\begin{array}{c}\mathbf{u}\left(t\right)=c^{-1}\left[-\mu \mathrm{sgn}\left(\mathbf{s}\left(t\right)\right)-k\mathbf{s}\left(t\right)\right]+T^{-1}\Delta {\mathbf{L}}_{drive,r}\left(t+1\right)\hfill \\ \mathrm{s}.\mathrm{t}.\left\{\begin{array}{c}{\mathbf{u}}_{min}\le \mathbf{u}\le {\mathbf{u}}_{max}\hfill \\ {\mathbf{a}}_{min}T\le \Delta \mathbf{u}\le {\mathbf{a}}_{max}T\hfill \end{array}\right.\hfill \end{array}\right.$$

(39)

In summary, this section presents the design of the NMPC controller for the loader’s working mechanism operation and introduces the conventional PID and SMC controllers. The tracking performance of the controllers is analysed in the next section.

4. Validation and Analysis

4.1. Co-Simulation Platform

Figure 6 shows the co-simulation platform for the operation trajectory tracking control of the loader’s working mechanism. The simulation environment is based on a desktop host with the Windows 10 Professional operating system, an Intel(R) Core(TM) i7-9800X processor, and 32G of RAM. The platform consists of two major parts: the controller module and the prototype module. Among these, the controller module is written based on the S-function of MATLAB/Simulink (2020a version), which contains the target trajectory information and the controller program. Its role is to receive point P’s motion information, the driving cylinders’ length information, and the vehicle body’s displacement information from the prototype model in the prototype module in real time, and then compute the control inputs, such as the vehicle body’s velocity and the cylinders’ velocity, before inputting the control commands into the prototype module. The prototype module is based on the MSC.ADAMS (2016 version), including the prototype data interaction interface and the prototype model. Its role is to receive the control inputs from the controller module through the prototype data interaction interface in real time and input them into the drivers of the prototype model to make the prototype move. At the same time, the state information (e.g., point P’s movement information) is fed back to the controller module in real time. The specific constraints and geometric parameters of the prototype model in the prototype module are described in [43].

The main control parameters in the control module are shown in

Table 2. Main control parameters of the control module.

Controllers	$\mathit{\rho }/\left(°\right)$	$\mathit{T}/\left(\mathbf{s}\right)$	${\mathit{v}}_{\mathit{r}}/\left(\mathbf{m}·{\mathbf{s}}^{-1}\right)$	${\mathit{N}}_{\mathit{p}}$	${\mathit{N}}_{\mathit{c}}$	${\mathit{K}}_{\mathit{P}}$	${\mathit{T}}_{\mathit{I}}$	${\mathit{T}}_{\mathit{D}}$	c	$\mathit{\mu }$	k
NMPC	40	0.05	0.5	10	2	-	-	-	-	-	-
PID	40	0.05	0.5	-	-	10	100	0.005	-	-	-
SMC	40	0.05	0.5	-	-	-	-	-	5	0.15	7

. The target trajectories and constraint parameters under different experimental groups are shown in

Table 3. Relevant parameters of the different experimental groups.

Group	${\mathit{s}}_1/\left(\mathbf{m}\right)$	${\mathit{\theta }}_{\mathbf{Pr}}^{\left(0\right)}/\left(°\right)$	${\mathit{\theta }}_{\mathbf{Pr}}^{\left(3\right)}/\left(°\right)$	${\mathit{\theta }}_{\mathbf{Pr}}^{\left(5\right)}/\left(°\right)$	${\mathit{u}}_{min}/\left(\mathbf{m}·{\mathbf{s}}^{-1}\right)$	${\mathit{u}}_{max}/\left(\mathbf{m}·{\mathbf{s}}^{-1}\right)$	${\mathit{a}}_{min}/\left(\mathbf{m}·{\mathbf{s}}^{-2}\right)$	${\mathit{a}}_{max}/\left(\mathbf{m}·{\mathbf{s}}^{-2}\right)$
1	1.3	0	25	44	-	-	-	-
2	1.3	0	25	44	${\left[0,0,0\right]}^{\mathrm{T}}$	${\left[0.5,0.2,0.2\right]}^{\mathrm{T}}$	${\left[-0.5,-0.5,-0.5\right]}^{\mathrm{T}}$	${\left[0.5,0.5,0.5\right]}^{\mathrm{T}}$
3	1.0	0	35	44	${\left[0,0,0\right]}^{\mathrm{T}}$	${\left[0.5,0.2,0.2\right]}^{\mathrm{T}}$	${\left[-0.5,-0.5,-0.5\right]}^{\mathrm{T}}$	${\left[0.5,0.5,0.5\right]}^{\mathrm{T}}$

. Among these, Group 1 and Group 2 share the same target trajectories, with the difference being in the presence or absence of the system constraints. Group 2 and Group 3 share the same system constraints, but they differ in their target trajectories. In addition, it should be noted that the control time domain $N_c$ is closely related to real-time performance, so a shorter control time domain (1 or 2) is usually chosen to maintain real-time operation [49]. When the control time domain is determined, there exists an optimal prediction time domain $N_p$. If the prediction time domain $N_p$ is too large, it will act too early, increasing the distance from the target point, whereas if the prediction time domain $N_p$ is too small, it will act too late, leading to overshooting, oscillation, and even out-of-control problems [50]. Therefore, in practice, the appropriate control parameters can be selected according to the system characteristics.

4.2. Experiments and Analyses

Figure 7 shows the target trajectory and the tracking effect of point P under the different controllers in Group 1. It can be seen that, in the ideal case without system constraints, since the control inputs from the PID controller, the SMC controller, and the NMPC controller are not constrained by the system constraints, they can achieve accurate tracking of the target trajectory, which verifies the appropriateness of the controller parameters. In addition, compared with the PID and SMC control methods, the NMPC control method has the property of early prediction, so it appears to steer in advance at positions 2 and 3.

Figure 8, Figure 9, and Figure 10 show the tracking effect, the errors, and the control inputs of point P under the different controllers in Group 2, respectively.

Figure 8 shows that, under system constraints, point P’s trajectory under the PID controller has a large deviation from position 2, which is especially pronounced between position 3 and position 5, exhibiting significant overshooting and a tendency to diverge. Figure 9 shows the changes in the displacement error and the bucket angle error. It can be seen that, under the influence of system constraints, point P’s trajectory has a large displacement error from position 2, especially between position 3 and position 5, and the maximum absolute value of point P’s displacement error is 0.184 m. The bucket angle also has a large error and fluctuation from position 2, and the maximum absolute value of the bucket angle error is 3.09°. Additionally, the errors are dispersed in the back section of the shovel-digging operation. As discussed in Section 3.2.2, the main reason for this phenomenon is that PID, as a control method that does not require a system model, relies on the error state model to design the control law by adjusting the corresponding parameter to achieve control. However, it is difficult to account for system constraints, making it difficult to handle system constraint limitations effectively. Therefore, from position 2 onward, the PID controller exhibits poor tracking performance due to system constraints. In addition, affected by the principles and characteristics of PID, Figure 10 shows that the control inputs under the PID controller exhibit more pronounced jitter.

In Group 2, the SMC controller has a similar effect as the PID controller. Under the system constraints, the maximum absolute value of point P’s displacement error in the SMC controller is 0.166 m, and the maximum absolute value of the bucket angle error is 2.88°. As discussed in Section 3.2.3, designing the control law for SMC can cause the system state to converge from outside the hyperplane to the switching hyperplane by controlling the sliding mode of the control system. Once the system reaches the switching hyperplane, the control action ensures that the system moves along the switching hyperplane to reach the origin, achieving sliding-mode control. Its control characteristics are very similar to those of the PID controller; for example, both are mainly based on the present state of the error and do not consider the system state in the future. Additionally, they do not effectively account for system constraints, so under system constraints, large overshoot will occur when the system state changes. However, the jitter phenomenon under the SMC controller is significantly reduced compared with the PID controller because, although the control law of SMC includes a switching function that causes system jitter, the exponential function reaching law is adopted, which effectively decreases the jitter amplitude of the controller.

However, Figure 8 shows that point P’s trajectory under the NMPC controller has good tracking performance throughout the process under the system constraints. Figure 9 shows that the maximum absolute value of point P’s displacement error is only 0.052 m, and the maximum absolute value of the bucket angle error is 2.58°, both of which occur at position 2, and the errors do not diverge throughout the process. As discussed in Section 3.2.1, the main reasons for these phenomena are that NMPC, as an optimal control method that requires a system model, first uses the established system model as the prediction model when designing the control law, then incorporates system constraints into the established objective function to handle them explicitly, and finally determines a set of optimal control inputs (see Equation (29)) through the solution of the multi-constraint quadratic programming problem. The control inputs not only meet the system constraints and are optimal but also have the characteristics of advanced prediction control. Although these characteristics lead to a certain sacrifice in accuracy when there is a sudden change in the curvature of the target trajectory, this sacrifice can be balanced to a certain extent by smoothness, and the magnitude of this sacrifice is smaller compared to errors such as overshooting in methods like PID. This can be observed in the maximum absolute value of the displacement error. The maximum displacement error under the NMPC controller is mainly due to this sacrifice, whereas in the PID controller, it is caused by overshooting, over-constraints, etc. The displacement error in NMPC is smaller than in PID. Therefore, compared to control methods that do not require a system model, NMPC shows better tracking performance under the system constraint limitations. In addition, Figure 10 shows that the control inputs under the NMPC controller are smoother. This is because the NMPC controller adopts an objective function based on the prediction model (see Equation (27)). The second term in the objective function represents the incremental form of the control inputs, which enforces a smoothness requirement. So, the smoothness of the control inputs improves when solving for the optimal control inputs.

Figure 11, Figure 12, and Figure 13 show the tracking effect, the errors, and the control inputs of point P under the different controllers in Group 3, respectively.

Similar to the effect in Group 2, and affected by the limitations of the PID control principle, Figure 11 shows that under the system constraints, point P’s trajectory under the PID controller deviates significantly from position 3, and a large overshooting phenomenon occurs. Figure 12 shows that, under the influence of the system constraints, point P exhibits a large displacement error from position 3, with a maximum absolute value of 0.164 m. The bucket angle exhibits more obvious fluctuations from position 2, and even diverges in the rear section of the shovel-digging operation, with a maximum absolute bucket angle error of 3.99°. In addition, Figure 13 shows that the control inputs under the PID controller exhibit more pronounced jitter.

In Group 3, the effect of the SMC controller is almost the same as that of the PID controller. It is also affected by the SMC control characteristics. Under the system constraints, the maximum absolute value of point P’s displacement error is 0.170 m, and the maximum absolute value of the bucket angle error is 3.89°. Meanwhile, the jitter phenomenon under the SMC controller is reduced compared with the PID controller.

Similarly, benefiting from the advantages of the NMPC control principle, Figure 11 shows that under the system constraints, point P’s trajectory under the NMPC controller exhibits good tracking performance throughout the process. Figure 12 shows that the maximum absolute value of point P’s displacement error is 0.040 m, and the maximum absolute value of the bucket angle error is 1.95°, both of which occur at position 2, and the errors do not diverge throughout the process. In addition, Figure 13 shows that the control inputs under the NMPC controller are relatively smooth.

In summary, the results of the co-simulation experiments show that under the same system constraints, for different target trajectories, compared with the PID and SMC controllers, the NMPC controller exhibits better control performance compared with the PID and SMC controllers. Specifically, the maximum absolute value of point P’s displacement error is reduced by at least 71% and 68%, respectively; the maximum absolute value of the bucket angle error is reduced by at least 16% and 10%, respectively; and the NMPC controller exhibits smoother control performance. Therefore, the operation trajectory tracking controller based on the NMPC method for the working mechanism exhibits better performance in handling system constraints and ensuring smoothness than the conventional PID and SMC methods.

Next, in order to ensure the realisation of the trajectories in the shovel-digging trajectory space, based on the above research, the NMPC controller (method) is used to perform a comprehensive tracking test for the trajectories.

4.3. Comprehensive Tracking Test

Based on the previous research, in this section, the NMPC controller is used to carry out a comprehensive tracking test on the trajectories in the shovel-digging trajectory space, and its results are analysed and investigated.

4.3.1. Trajectory Tracking Test

As discussed in Section 3.1, the trajectories in the shovel-digging trajectory space should be tracked and tested as comprehensively as possible to verify the performance of the controller. The trajectory sequence ranges are defined as follows: the dig insertion depth $s_1$ ranges from 0.4 m to 1.5 m, at intervals of 0.1 m, and the bucket intermediate angle ${\theta }_{Pr}^{\left(3\right)}$ ranges from 10$°$ to 40$°$, at intervals of 10$°$. The specific parameters of the trajectory sequence are shown in

Table 4. Parameters of the trajectory sequence in the shovel-digging trajectory space.

j	1		2		3		4
$\mathit{i}$	$s_1/\left(\mathrm{m}\right)$	${\theta }_{Pr}^{\left(3\right)}/\left(°\right)$	$s_1/\left(\mathrm{m}\right)$	${\theta }_{Pr}^{\left(3\right)}/\left(°\right)$	$s_1/\left(\mathrm{m}\right)$	${\theta }_{Pr}^{\left(3\right)}/\left(°\right)$	$s_1/\left(\mathrm{m}\right)$	${\theta }_{Pr}^{\left(3\right)}/\left(°\right)$
1	0.4	10	0.4	20	0.4	30	0.4	40
2	0.5	10	0.5	20	0.5	30	0.5	40
3	0.6	10	0.6	20	0.6	30	0.6	40
4	0.7	10	0.7	20	0.7	30	0.7	40
5	0.8	10	0.8	20	0.8	30	0.8	40
6	0.9	10	0.9	20	0.9	30	0.9	40
7	1.0	10	1.0	20	1.0	30	1.0	40
8	1.1	10	1.1	20	1.1	30	1.1	40
9	1.2	10	1.2	20	1.2	30	1.2	40
10	1.3	10	1.3	20	1.3	30	1.3	40
11	1.4	10	1.4	20	1.4	30	1.4	40

. There are 44 trajectory groups, each represented by (i, j).

Next, Figure 14 shows all test trajectories under the NMPC controller.

4.3.2. Tracking Results and Analyses

Table 5. Tracking errors relative to target trajectories.

j	1		2		3		4
$\mathit{i}$	Maximum displacement error /(mm)	Maximum angel error/(°)	Maximum displacement error /(mm)	Maximum angel error/(°)	Maximum displacement error /(mm)	Maximum angel error/(°)	Maximum displacement error /(mm)	Maximum angel error/(°)
1	48.58	2.28	48.60	1.26	48.60	0.60	48.61	1.38
2	48.58	2.46	48.59	1.35	48.61	0.87	48.63	1.74
3	48.59	2.25	48.60	1.08	48.62	0.90	48.66	1.86
4	48.59	1.89	48.61	0.66	48.63	0.99	48.73	2.04
5	48.59	1.41	48.62	0.51	48.65	1.17	49.31	2.28
6	48.60	0.90	48.63	0.48	48.90	1.41	50.69	2.64
7	48.61	0.54	48.66	0.63	49.25	1.74	51.49	3.24
8	48.62	0.45	48.92	0.93	49.37	2.25	52.26	3.81
9	48.64	0.51	49.33	1.35	50.18	2.88	53.30	4.53
10	48.90	0.69	50.12	1.80	51.23	3.48	54.08	5.16
11	49.36	0.93	51.38	2.34	53.42	4.08	56.08	5.79

shows the tracking errors under the NMPC controller, which are mainly characterised by the maximum absolute value of point P’s displacement error (abbreviated as “maximum displacement error” in the following text) and the maximum absolute value of the bucket angle error (abbreviated as “maximum angle error” in the following text) throughout the shovel-digging process. To facilitate the analysis, Figure 15 and Figure 16, respectively, show the maximum displacement errors and the maximum angle errors for all tracking trajectories under the NMPC controller.

By connecting the different dig insertion depth groups under the same bucket angle in Figure 15, it can be seen that under the same bucket angle, as the dig insertion depth increases, the maximum displacement error increases, especially when the dig insertion depth is more than approximately 1.1 m (i.e., i = 8), the maximum displacement error increases rapidly with the increase in the dig insertion depth. Combined with the tracking trajectory effect diagram in the previous subsection, it can be seen that the maximum displacement error occurs in the third stage of the shovel-digging operation, i.e., the phenomenon of overshooting occurs. It can be surmised from Section 4.2 that this phenomenon is mainly due to the deviation of the multi-driver devices of the working mechanism, which are affected by the system driving constraints. In addition, Figure 15 shows that at the same dig insertion depth, as the bucket angle increases, the maximum displacement error tends to increase, which mainly occurs in the second stage of the shovel-digging operation. But the deviation is not significant, which is in line with the characteristics of the controller.

Similarly, by connecting the different dig insertion depth groups under the same bucket angle in Figure 16, it can be seen that when the dig insertion depth is more than approximately 1.1 m (i.e., i = 8), the maximum angle error increases rapidly with the increase in the dig insertion depth under the same bucket angle. This phenomenon can be surmised to be a deviation of the controller, mainly affected by the system driving constraints. However, when the dig insertion depth is less than 1.1 m, the maximum angle error increases with the increase in the dig insertion depth at a larger bucket angle, while at a smaller bucket angle, the maximum angle error changes in the opposite direction. Combined with the kinematic model of the working mechanism in Section 2.2, it is surmised that this phenomenon is mainly due to the deviation of the controller, which is affected by the system’s geometrical constraints. The deviation is so small that it can be ignored.

In summary, the maximum displacement error and the maximum angle error under the NMPC controller do not exceed 60 mm and 6°, respectively, which verifies the good control performance of the working mechanism tracking controller based on the NMPC method designed in this paper. In addition, as the dig insertion depth increases, the tracking performance of the controller decreases (e.g., displacement and angle overshooting). Considering that in practice, as the bucket inserts into the material at a greater depth, the resistance increases approximately linearly. At the same time, the overshooting increases the dig insertion depth, which also leads to an increase in resistance. Therefore, it is not recommended to use a trajectory with a larger dig insertion depth as a viable shovel-digging trajectory.

5. Conclusions

Aiming at the difficulty of handling system constraints using a control method that does not require a system model (represented by PID), resulting in large overshooting amplitudes and jitter under system constraints, this paper introduced the MPC concept into the motion control of the loader’s working mechanism for the first time. Then, by combining the characteristics of the working mechanism with the advantages of NMPC, we proposed an operation trajectory tracking control method and designed an NMPC-based controller for the working mechanism. Finally, we compared it with conventional PID and SMC controllers. The main conclusions drawn are as follows:

(1) Co-simulation analyses show that under the same system constraints, for different target trajectories, the designed controller exhibits better control performance compared to the PID and SMC controllers. Specifically, the maximum absolute value of the bucket-tip displacement error under the designed controller does not exceed 0.052 m, which is at least 71% and 68% lower than that of the PID and SMC controllers, respectively. The maximum absolute value of the bucket angle error under the designed controller does not exceed 2.58°, which is at least 16% and 10% lower than that of the PID and SMC controllers, respectively. The designed controller has smoother control performance. Therefore, compared with the conventional PID and SMC methods, the trajectory tracking control for the working mechanism based on the NMPC method exhibits better performance in terms of handling system constraints and ensuring smoothness, mainly due to the advantages of the NMPC control principle. Of course, NMPC control also has inherent limitations, mainly the real-time problem caused by the computational complexity. The literature [49,50] elaborates on the study of NMPC real-time performance: relative to the prediction time domain, its real-time performance is more affected by the control time domain. Since the loader’s operational movement is relatively slow, in practice, choosing a smaller control time domain can ensure real-time performance, while adjusting the prediction time domain can ensure tracking performance.

(2) Through comprehensive tracking tests of shovel-digging trajectories, it was found that the tracking performance of the controller decreases with the increase in the dig insertion depth (e.g., displacement and angle overshooting), and considering that, in practice, as the bucket inserts deeper into the material, the resistance increases approximately linearly. At the same time, the overshooting increases the dig insertion depth, which also leads to an increase in resistance. Therefore, it is not recommended to take the trajectory with a larger dig insertion depth as the reasonable shovel-digging trajectory in practical operation. Moreover, the target trajectory in this study is a fixed empirical trajectory, but real-world operations may require dynamic trajectory adjustments (e.g., due to material distribution changes or unexpected obstacles). Therefore, it is necessary to translate operator experience (operating style modelling) into adaptive rules for trajectory parameters [51]. For instance, historical operation data could be used to learn different operators’ preferences for digging depth, enabling dynamic adjustments of controller parameters. So, in future work, the uncertainties (e.g., the fuzziness and dynamic adaptability in human decision making) in a human operator’s behaviour can be incorporated into the design of the controller to enhance human–machine collaboration [52].

(3) The research in this paper is a preliminary achievement. It not only provides an important foundation and new insights for improving the control performance of the loader’s working mechanism but also provides a research reference for multi-arm machinery to realise safe and low-consumption automated operation. Specifically, the kinematic model of multi-arm machinery should first be created using the kinematic modelling method (Section 2), then, the prediction model and the discrete control volume (which depend on the specific object to set the system constraints) can be created using the NMPC method (Section 3.2.1), and finally, the NMPC-based controller for multi-arm machinery can be designed. However, due to the complexity and diversity of nonlinear systems, there are still a lot of problems to be explored and solved in the theoretical research and industrial application of NMPC in terms of stability, robustness, and optimisation [53]. In the future, it will be necessary to further develop and promote the existing research results of NMPC to meet the application requirements and characteristics of mechanical equipment, thereby improving control performance in practical applications.