Observer-Based Approximate Affine Nonlinear Model Predictive Controller for Hydraulic Robotic Excavators with Constraints

Abstract

Given the highly nonlinear and strongly constrained nature of the electro-hydraulic system, we proposed an observer-based approximate nonlinear model predictive controller (ANMPC) for the trajectory tracking control of robotic excavators. A nonlinear non-affine state space equation with identified parameters is employed to describe the dynamics of the electro-hydraulic system. Then, to mitigate the plant-model mismatch caused by the first-order linearization, an approximate affine nonlinear state space model is utilized to represent the explicit relationship between the output and input and an ANMPC is designed based on the approximate nonlinear model. Meanwhile, the Extended Kalman Filter was introduced for state observation to deal with the unmeasurable velocity information and heavy measurement noises. Comparative experiments are conducted on a 1.7-ton hydraulic robotic excavator, where ANMPC and linear model predictive control are used to track a typical excavation trajectory. The experimental results provide evidence of convincing trajectory tracking performance.

1. Introduction

Robotic excavators are a type of multi-joint device driven by hydraulic systems, which are widely used in earth-moving fields such as energy development, road construction, and infrastructure construction. However, the operator must have skilled techniques to control the multiple joints of the excavator to improve operational efficiency [1]. In addition, in some harsh construction environments, the operator’s safety may be threatened. As a consequence, it is very difficult for manual operation to meet the requirements of excavation operations. In this regard, the automation of excavators has attracted a great deal of interest, which can help reduce human-associated costs and improve construction efficiency and safety [2,3].

Excavator automation can be viewed as the control of a kind of robot with a bucket as an end effector, and trajectory tracking control is the key to automated operations [4]. However, since robotic excavators typically use hydraulic systems as their power source, achieving high-performance trajectory tracking remains challenging due to the nonlinear and multi-constrained features [5]. Many control methods, such as the improved PID control [6,7,8], self-tuning pressure-feedback control [9], adaptive robust control [10], intelligent control [11,12], and so on, have been proposed to deal with the nonlinearity, time-variance, and parameter uncertainty of hydraulic robotic excavators. However, they find it difficult to meet the multi-constraint features of electro-hydraulic systems [13]. In practical applications, hydraulic actuators face various constraints, such as control input constraints, speed constraints, and position constraints. A control strategy that neglects physical constraints may degrade the control performance and even lead to system instability [14,15].

Due to the presence of constraints in hydraulic systems, the MPC method has become an increasingly popular control strategy. As an optimal control technique, MPC can actively handle system constraints during the control process, showing great potential in solving complex system control problems with strong constraints [16]. In [17], a hybrid control method consisting of linear model predictive control (MPC) and proportion-integration control (PIC) was proposed for force control of electro-hydraulic servo systems, where the MPC in the outer loop provided references to the PIC in the inner loop. For the trajectory tracking control of a robotic excavator, a linear MPC was designed in [18,19] with simulation and experimental verification. In [20], a gain-scheduled MPC was proposed for the dynamic performance enhancement of an excavator, but only the boom joint was considered. All aforementioned MPC-based controllers are based on linear models, because nonlinear MPC has the limitation of heavy online computation burden, requiring extensive computational resources and longer solving times, making it difficult to apply in practical robotic excavators [21]. Nevertheless, due to the presence of dead zones, saturation, nonlinear friction, and the compressibility of hydraulic oil, hydraulic systems have complex nonlinear dynamics [13]. Consequently, linear models based on the first-order linearization of the nonlinear system using Jacobian calculations will introduce large errors, resulting in control performance degradation [22].

On the other hand, the MPC control method adopts the concept of full-state feedback control, which means that all state information (i.e., position signals, velocity signals, and pressure signals) must be known during the control process [23]. However, for most hydraulic excavators, not all system states can be directly measured due to physical limitations or the high cost of implementing enough sensors. In addition, the heavy noise in the measurement may cause inaccurate state information. Based on the above considerations, state observers have received considerable attention, such as the Extended State Observer [24], Sliding Mode Observer [25], and State-Dependent-Riccati-Equation filter [26]. Among these observers, the Extended Kalman Filter (EKF) has been widely used as a state estimator in nonlinear systems, providing a simple and effective solution to the state estimation problem of complex nonlinear systems [27,28]. Within this context, only pressure and displacement sensors are installed for the robotic excavator. Therefore, we design an EKF for the electro-hydraulic system to estimate the unknown velocity information while reducing the measurement error of displacement and pressure signals.

Motivated by the above challenges, an observer-based approximate nonlinear model predictive controller is proposed for a hydraulic robotic excavator in this study. The main contributions of this paper are as follows.

(1)

Parameter identification was performed to identify the key parameters in the nonlinear non-affine model of the hydraulic excavator using measured data. Then, the nonlinear non-affine model is approximated to an affine nonlinear state-space model, which can reduce unmodeled errors and more accurately reflect the system’s dynamic features compared to the conventional first-order linearization models.

(2)

An approximated nonlinear model predictive control (ANMPC) is designed based on the approximate affine nonlinear state-space model, which can still be solved by quadratic programming (QP), with the same computation burden as a linear MPC, avoiding solving the nonlinear MPC problem. As only pressure and displacement sensors are installed for the robotic excavator, we design an EKF observer for the electro-hydraulic system to estimate the unknown velocity information while reducing the measurement error of displacement and pressure signals.

(3)

Experiments were conducted on a 1.7-ton hydraulic excavator, where three joints of the hydraulic excavator were simultaneously controlled to execute a typical excavation trajectory. The results demonstrate that the proposed observer-based ANMPC outperforms the conventional LMPC in trajectory tracking control during excavation.

The rest of this article is organized as follows. The working principle of electro-hydraulic excavator is introduced in Section 2, a non-affine nonlinear mathematical model of the electro-hydraulic excavator is established, and a parameter identification is performed to obtain the unknown parameters. In Section 3, an approximate nonlinear model predictive controller with an EKF for the electro-hydraulic excavator is designed. Section 4 presents the trajectory tracking control results based on a 1.7-ton robotic excavator. The conclusions of this study are given in Section 5.

2. System Description and Modeling

2.1. Electro-Hydraulic System Description

A 1.7-ton electro-hydraulic robotic excavator is used in this study, and the flow coupling between different hydraulic cylinders is ignored. As the boom, stick, and bucket are theoretically the same, we establish an electro-hydraulic control system model using the bucket as an example. As shown in Figure 1, the electro-hydraulic control system primarily consists of a controller, a pilot valve, a main valve, and hydraulic cylinders. The working principle is as follows. According to the reference trajectory and the actual position, the controller generates a control signal, which is amplified and used to control the pilot valve. According to the given control signal, the pilot valve will push the spool of the main valve to move to the left or right, connecting the corresponding pipeline with the cavity of the hydraulic cylinder. Then, the piston rod will extend or retract under the push of hydraulic oil, thus realizing the movement of the cylinder. The actual position of the cylinder during movement will be fed back to the controller through a displacement sensor, to form a closed control loop.

Figure 1. Structure of the electro-hydraulic control system.

2.2. Kinematics

The kinematic model of the excavator robot plays a crucial role in determining the relationship between joint angles and end-effector positions, which is essential for trajectory planning and control. The D-H coordinate system of the robotic excavator is established as shown in Figure 2. The D-H coordinate parameters are shown in Table 1.

Figure 2. D-H coordinates.

Table 1. The parameters of D-H coordinates.

Joint i	${\mathit{a}}_{\mathit{i}} \mathbf{\left(}\mathbf{m}\mathbf{\right)}$	${\mathit{d}}_{\mathit{i}} \mathbf{\left(}\mathbf{m}\mathbf{\right)}$	${\mathit{\alpha }}_{\mathit{i}} \mathbf{(}\mathbf{°}\mathbf{)}$	${\mathit{\theta }}_{\mathit{i}} \mathbf{(}\mathbf{°}\mathbf{)}$
1 (Rotation)	0.65	0.82	90	−180 to 180
2 (Boom)	1.8	0	0	−56.1 to 65.5
3 (Arm)	0.95	0	0	−30.0 to −154.0
4 (Bucket)	0.535	0	0	−44.5 to −118.5

We can derive the forward kinematics equations to calculate the end-effector pose $[px,py,pz,\psi ]$ from the joint angles $[{\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4]$, which is presented as Equation (1):

$$\left\{\begin{array}{l}px=\cos ({\theta }_1)[a_4\cos ({\theta }_2+{\theta }_3+{\theta }_4)+a_3\cos ({\theta }_2+{\theta }_3)+a_2\cos ({\theta }_2)+a_1]\\ py=\sin ({\theta }_1)[a_4\cos ({\theta }_2+{\theta }_3+{\theta }_4)+a_3\cos ({\theta }_2+{\theta }_3)+a_2\cos ({\theta }_2)+a_1]\\ pz=a_4\sin ({\theta }_2+{\theta }_3+{\theta }_4)+a_3\sin ({\theta }_2+{\theta }_3)+a_2\sin ({\theta }_2)+d_1\\ \psi ={\theta }_2+{\theta }_3+{\theta }_4\end{array}\right.$$

(1)

The inverse kinematics equations can also be derived to calculate the joint angles required to achieve a desired end-effector position and orientation. The angles of each joint ${\theta }_1-{\theta }_4$ can be calculated by Equation (2):

$$\left\{\begin{array}{l}{\theta }_1=\mathrm{atan}(\frac{py}{px})\\ {\theta }_2=\alpha +\beta +\gamma \\ {\theta }_3=\pi -\mathrm{acos}(\frac{a_2{}^2+a_3{}^2-C{Q}^2}{2a_2{a}_3})\\ {\theta }_4=\psi -{\theta }_2-{\theta }_3\end{array}\right.$$

(2)

where $\alpha$ is the angle between CF and the horizontal line, $\beta$ is the angle between CQ and the horizontal line, and $\gamma$ is the angle between CV and the horizontal line.

2.3. Mathematical Model of EHS

Usually, the response speed of the pilot valve is much faster than that of the main valve, so the dynamic characteristics of the pilot valve can be ignored without significantly reducing control performance [7,8]. Therefore, this process can be regarded as a pure proportional stage:

$$x_v=k_{v}I$$

(3)

where $x_v$ is the spool displacement of the main valve, $I$ is the control current, and $k_v$ is a coefficient.

The following equation expresses the orifice flow equation in the main valve:

$$Q_L=C_d\omega x_v\sqrt{\frac{P_s-sgn(x_v)P_L}{\rho }}$$

(4)

where $Q_L$ is the load flow, $P_L$ is the load pressure, $C_d$ is the discharge coefficient, $\omega$ is the spool valve area gradient, $\rho$ is the fluid mass density, $P_s$ is the supply pressure, and $sgn(\ast )$ is the symbolic function.

Ignoring the external leakage of the hydraulic cylinder, the continuity equation of the hydraulic cylinder flow can be expressed as:

$$A\dot{y}+C_t{P}_L+\frac{V_t}{4{\beta }_e}{\dot{P}}_L=Q_L$$

(5)

where $A$ is the cross-sectional area of the cylinder, $y$ is the displacement of the piston rod, $V_t$ is the total actuator volume, $C_t$ is the coefficient of leakage, and ${\beta }_e$ is the effective bulk modulus.

The force balance equation of the hydraulic cylinder is:

$$P_{L}A=m\ddot{y}+b\dot{y}+ky+F_f$$

(6)

where $m$ is the equivalent mass, $b$ is the viscous damping coefficient, $k$ is the equivalent spring stiffness, and $F_f$ is the friction force. In our study, the coulomb viscous model is selected to describe the frictional force acting on hydraulic cylinders:

$$F_f=F_c+F_v\dot{d}$$

(7)

where $F_c$ is the stiction force and $F_v$ is the dynamic friction coefficient.

Combining the above equations of each sub-system, selecting the state vector $[x_1;x_2;x_3]=[y;\dot{y};P_L]$ and the control input $u=I$, the model of an electro-hydraulic system can be represented by the following three non-affine nonlinear state space equations:

$$\begin{array}{l}\hspace{0.17em}{\dot{x}}_1=x_2\\ {\dot{x}}_2=-\frac{k}{m}{x}_1-\frac{b}{m}{x}_2+\frac{A}{m}{x}_3-\frac{F_{c}sgn(x_2)+F_v{x}_2}{m}\\ {\dot{x}}_3=-\frac{4A{\beta }_e}{V_t}{x}_2-\frac{4C_t{\beta }_e}{V_t}{x}_3+\frac{4C_d{\beta }_e\omega k_v}{V_t\sqrt{\rho }}\sqrt{P_s-sgn(u)x_3}\cdot u\end{array}$$

(8)

Equation (8) can be rewritten as:

$$\begin{array}{l}\hspace{0.17em}{\dot{x}}_1=x_2\\ {\dot{x}}_2=-a_1{x}_1-a_2{x}_2+a_3{x}_3-a_{4}sgn(x_2)\\ {\dot{x}}_3=-a_5{x}_2-a_6{x}_3+a_7\sqrt{P_s-sgn(u)x_3}\cdot u\end{array}$$

(9)

where, $a_1=\frac{k}{m}$, $a_2=\frac{b+F_v}{m}$, $a_3=\frac{A}{m}$, $a_4=\frac{F_c}{m}$, $a_5=\frac{4A{\beta }_e}{V_t}$, $a_6=\frac{4C_t{\beta }_e}{V_t}$, and $a_7=\frac{4C_d{\beta }_e\omega k_v}{V_t\sqrt{\rho }}$.

We selected the sampling time as $T_s$ and the system output as $y(k)=x_1(k)$. Then, the discrete state space equation model can be obtained through forward difference discretization:

$$\begin{array}{l}\hspace{0.17em}{x}_1(k+1)=T_s{x}_2+x_1(k)\\ x_2(k+1)=\left(-a_1{x}_1(k)-a_2{x}_2(k)+a_3{x}_3(k)-a_{4}sgn(x_2(k))\right)T_s+x_2(k)\\ x_3(k+1)=\left(-a_5{x}_2(k)-a_6{x}_3(k)+a_7\sqrt{P_s-sgn(u(k))x_3(k)}\cdot u(k)\right)T_s+x_3(k)\end{array}$$

(10)

2.4. Model Validation

To estimate the model parameters, a pseudo-random binary signal (PRBS) with a range of [−200 mA, 200 mA] is selected as the excitation for parameter identification, as shown in Figure 3a. We construct nonlinear grey-box modeling of EHS using the System Identification Tool in MATLAB. The settings of grey-box parameter identification [17] are as follows: The solver was the Runge-Kutta 45 solver with an adaptive step size, the search method was the Trust-Region Reflective Newton method of nonlinear least squares, the cost function was set as the sum of squared error between the measured output and simulated output, and the absolute error and relative error tolerances were 1 × 10⁻⁶ and 1 × 10⁻⁵, respectively. The parameter identification results are given in Table 2. Under this set of parameters, model outputs fit 98.48% of actual system outputs and the final prediction of the error is 1.57 × 10⁻⁷, and the comparison between the model response and the actual data is shown in Figure 3b. Thus, we ensure that the results of parameter identification are reasonable.

Figure 3. Input and response of system identification: (a) Input signal; (b) response.

Table 2. The estimated parameters.

Parameters	${\mathit{a}}_1$	${\mathit{a}}_2$	${\mathit{a}}_3$	${\mathit{a}}_4$	${\mathit{a}}_5$	${\mathit{a}}_6$	${\mathit{a}}_7$
Estimated Values	0	133.75	1.65 × 10−6	12.5	1.05 × 1010	81.33	1.047 × 104

To verify the estimated mathematical model, we tested the real system with mixed sine signals of different frequencies, as given in Equation (11). Subsequently, we compared the experimental results with the model output. Figure 4b shows that the response of the model agreed well with the experimental data, indicating that the identified model can accurately predict the behavior of the real system.

$$u(t)=\left\{\begin{array}{c}80+20[\sin (4\pi t)+\sin (2.6\pi t)+\sin (2\pi t)+\sin (1.6\pi t)+\sin (0.8\pi t)],t\le 15\\ -120-30[\sin (2\pi t)+\sin (1.3\pi t)+\sin (\pi t)+\sin (0.8\pi t)+\sin (0.4\pi t)],t>15\end{array}\right.$$

(11)

Figure 4. Comparison between model and real system: (a) Input signal; (b) response.

As an example, the identification results of the bucket are listed in Table 2. Similarly, we can obtain the parameters for the boom and arm, as shown in Table 3.

Table 3. The estimated parameters of boom and arm.

Parameters	${\mathit{a}}_1$	${\mathit{a}}_2$	${\mathit{a}}_3$	${\mathit{a}}_4$	${\mathit{a}}_5$	${\mathit{a}}_6$	${\mathit{a}}_7$
Boom	0	53.3	2.5 × 10−5	5.0	1.05 × 1010	63.16	8.21 × 103
Arm	0	89.17	3.67 × 10−5	8.33	1.05 × 1010	71.77	9.33 × 103

3. Approximate Nonlinear Model Predictive Controller Design

The design of an MPC controller requires a model to predict the future state and obtain the optimal control input by solving an optimization problem in real-time [21]. Therefore, accurate modeling of a system is crucial for an MPC controller. The electro-hydraulic system is highly nonlinear, but the design of the NMPC controller will cause difficulties in solving optimization problems and bring a large computational burden. Therefore, linearization methods are often used to obtain the linear expression of the model, and an LMPC will be designed based on it.

Usually, the linearization method involves computing the Taylor expansion of the model at the equilibrium point and ignoring the higher-order terms. However, the first-order linearization introduces unmodeled errors, thus the linearized model cannot accurately reflect the dynamic characteristics of the system. When the equilibrium is improperly selected, it can lead to a decrease in controller performance or even cause the control system to diverge. Therefore, to reduce the plant-model mismatch caused by linearization, this study uses an approximate method to represent the explicit relationship between the output and input.

3.1. Approximation Affine Nonlinear State Space Model

Due to the nonlinear relationship between the input and output, it is difficult to derive the relationship between the output and the input and design a predictive controller. Therefore, an approximate model is first derived for System (9) to ensure a simplified input–output relationship. For the non-affine nonlinear model $i$, where $i=boom,arm,bucket$ in this study, the Taylor expansion of $x_i(k+1)$ with respect to $u_i(k)$ around $u_i(k-1)$ is shown as:

$$\begin{array}{l}{x}_i(k+1)=f_i[x_i(k),u_i(k-1)]+\frac{\partial f_i[x_i(k),u_i(k-1)]}{\partial u_i(k)}(u_i(k)-u_i(k-1))+{\epsilon }_i(k)\\ \hspace{1em}\hspace{1em}\hspace{1em}=f_i[x_i(k),u_i(k-1)]+g_i[x_i(k),u_i(k-1)]\mathsf{\Delta }{u}_i(k)+{\epsilon }_i(k)\end{array}$$

(12)

where ${\epsilon }_i(k)=\frac{{\partial }^2{f}_i[x_i(k),{\mu }_i(k)]}{\partial u_i^2(k)}\mathsf{\Delta }{u}_i^2(k)$ and ${\mu }_i(k)={\eta }_i{u}_i(k)+(1-{\eta }_i)u_i(k-1)$ with ${\eta }_i$ being a positive constant.

Since only the derivative expansion of the control input is performed, the nonlinear characteristics of the state variables are retained. Subsequently, the approximate model may not miss too many characteristics of the system [27,29].

3.2. ANMPC Design

Ignoring ${\epsilon }_i(k)$ in Equation (12), an approximate affine nonlinear system can be represented by state space Equation (13):

$$\begin{array}{l}{x}_i(k+1)=f_i[x_i(k),u_i(k-1)]+g_i[x_i(k),u_i(k-1)]\mathsf{\Delta }{u}_i(k)\\ y_i(k)=C{x}_i(k)\end{array}$$

(13)

with constraints:

$$\begin{array}{c}{u}_{i,}{}_{min}\le u_i(k)\le u_{i,}{}_{max}\\ \mathsf{\Delta }{u}_{i,}{}_{min}\le \mathsf{\Delta }{u}_i(k)\le \mathsf{\Delta }{u}_{i,}{}_{max}\\ y_{i,}{}_{min}\le y_i(k)\le y_{i,}{}_{max}\end{array}$$

where $x_i(k)\in {\Re }^{n_x}$ is the state vector, $u_i(k)\in {\Re }^{n_u}$ is the input vector, $y_i(k)\in {\Re }^{n_y}$ is the output vector, $\mathsf{\Delta }{u}_i(k)$ is the input increment, $f_i(\ast )$ and $g_i(\ast )$ are nonlinear functions, and $u_{i,}{}_{min}\le u_{i,}{}_{max}$, $\mathsf{\Delta }{u}_{i,}{}_{min}\le \mathsf{\Delta }{u}_{i,}{}_{max}$, and $y_{i,}{}_{min}\le y_{i,}{}_{max}$ are vectors of lower and upper bounds and $C=[1,0,1]$.

Note that there are two main reasons why we ignore the second-order term ${\epsilon }_i(k)$. Firstly, the output of a physical system in practice cannot change too fast within a small time interval due to the “inertia” of the system [26,28], which means that $\mathsf{\Delta }{u}_i(k)$ is limited. Secondly, when the reference trajectories to be tracked are constant, the approximation error ${\epsilon }_i(k)$ will approach zero because $\mathsf{\Delta }{u}_i(k)\to 0$ in the steady state. In addition, the mechanism of a receding horizon and repeating optimization gives MPC inherent robustness.

The MPC controller is an iterative optimization technique, and at each sampling time k, the optimal control input is obtained by solving the optimization problem online based on the current system states and references, and the first element of the control sequence will be applied to the system. The optimal control input is obtained by continuously solving the following optimization problem:

$$\underset{\mathsf{\Delta }{u}_i(k)}{\min }{J}_i(k)={\displaystyle \sum _{j=1}^{N_p}{‖r_i(k+j|k)-y_i(k+j|k)‖}_Q^2}+{\displaystyle \sum _{j=0}^{N_m-1}{‖\mathsf{\Delta }{u}_i(k)‖}_W^2}$$

(14)

where $r_i(k+j|k)$ is the reference, $N_p$ and $N_m$ are the prediction horizon and the control horizon, respectively, $N_m\le N_p$, Q, and W are weighting matrices for two optimization terms, and ${‖\ast ‖}^2$ denotes the Euclidean norms of vectors.

The first term in Equation (14) represents the error between the predicted output and the reference, while the second term considers the change in the control input. According to the approximate affine non-linear Model (13), we have:

$$\left\{\begin{array}{l}\hspace{0.17em}{x}_i(\left.k+1\right|k)=f_i[x_i(k|k-1),u_i(k-1)]+g_i[x(k|k-1),u_i(k-1)]\mathsf{\Delta }{u}_i(k)\\ x_i(\left.k+2\right|k)=f_i[x_i(k+1|k-1),u_i(k-1)]+g_i[x(k+1|k-1),u_i(k-1)](\mathsf{\Delta }{u}_i(k)+\mathsf{\Delta }{u}_i(k+1))\\ ⋮\\ x_i(\left.k+N_p\right|k)=f_i[x_i(k+N_p-1|k-1),u_i(k-1)]+g_i[x(k+N_p-1|k-1),u_i(k-1)](\mathsf{\Delta }{u}_i(k)+\dots \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} +\mathsf{\Delta }{u}_i(k+N_m-1))\end{array}\right.$$

(15)

Define the following vectors:

$$Y_i=[y_i(\left.k+1\right|k) ,y_i(\left.k+2\right|k) ,\cdots y_i(\left.k+N_p\right|k)]\prime \in {\Re }^{N_p{n}_y}$$

(16)

$$R_i=[r_i(\left.k+1\right|k) ,r_i(\left.k+2\right|k) ,\cdots r_i(\left.k+N_p\right|k)]\prime \in {\Re }^{N_p{n}_y}$$

(17)

$$\mathsf{\Delta }{U}_i=[\mathsf{\Delta }{u}_i(k) ,\mathsf{\Delta }{u}_i(k+1), \cdots \mathsf{\Delta }{u}_i(k+N_m-1)]\prime \in {\Re }^{N_m{n}_u}$$

(18)

$$X_i=[x_i(\left.k+1\right|k), x_i(\left.k+2\right|k) ,\cdots x_i(\left.k+N_p\right|k)]\prime \in {\Re }^{N_p{n}_x}$$

(19)

Transforming recursive Equation (14) into matrix form, we have:

$$Y_i=\overline{C}{X}_i=\overline{C}(F_i+G_i\mathsf{\Delta }{U}_i)$$

(20)

where:

$$\overline{C}=\left[\begin{array}{ccc}C& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & C\end{array}\right]\in {\Re }^{N_p{n}_y\times N_p{n}_x}$$

$$F_i=\left[\begin{array}{c}{f}_i[x(k),u(k-1)]\\ f_i[x\left.(k+1\right|k),u(k-1)]\\ ⋮\\ f_i[x\left.(k+p-1\right|k),u(k-1)]\end{array}\right]\in {\Re }^{N_p{n}_x}$$

$$G_i=\left[\begin{array}{cccc}{g}_i[x(k|k-1),u_i(k-1)]& 0& \cdots & 0\\ g_i[x(k|k-2),u_i(k-1)]& g_i[x(k+1|k-1),u_i(k-1)]& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ g_i[x\left.(k+N_p-1\right|k-1),u_i(k-1)]& g_i[x\left.(k+N_p-1\right|k-1),u_i(k-1)]& \cdots & g_i[x\left.(k+N_p-1\right|k-1),u_i(k-1)]\end{array}\right]\in {\Re }^{N_p{n}_x\times N_m{n}_u}$$

Hence, the original optimization Problem (14) becomes:

$$\begin{array}{l}\underset{\mathsf{\Delta }{U}_i}{\min }{J}_i(k)={‖R_i-Y_i‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\\ \hspace{1em}\hspace{1em}\hspace{1em} ={‖R_i-\overline{C}{F}_i-\overline{C}{G}_i\mathsf{\Delta }{U}_i‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\end{array}$$

(21)

$$\begin{array}{l}\hspace{0.17em}s.t.\\ u_{i,}{}_{min}\le u_i(k-1)+\mathsf{\Delta }{U}_i\le u_{i,}{}_{max}\\ \mathsf{\Delta }{u}_{i,}{}_{min}\le \mathsf{\Delta }{U}_i\le \mathsf{\Delta }{u}_{i,}{}_{max}\\ y_{i,}{}_{min}\le \overline{C}(F_i+G_i\mathsf{\Delta }{U}_i)\le y_{i,}{}_{max}\end{array}$$

The above optimization problem is a standard quadratic programming problem, if the weight matrices W and Q are positive definite, then the objective Function (21) is strictly convex, and the feasible region defined by the constraints is a closed convex set. Therefore, the solution to Equation (21) is unique and satisfies the Karush–Kauhn–Tucker (KKT) conditions [30,31].

3.3. EKF-Based ANMPC Design

For the MPC method, the full-state feedback control concept is adopted, which means that all state information (i.e., position signal, velocity signal, and pressure signal) should be available in electro-hydraulic system control. However, in practical systems, due to limitations in installation conditions and cost, not all state information can be obtained through sensors directly. In this study, the displacement and pressure signals of hydraulic cylinders can be obtained through sensors. In terms of the velocity signal, the Extended Kalman Filter (EKF) is introduced as a state observer, which is one of the most popular state estimation techniques primarily developed for nonlinear systems [32]. In addition, the EKF is capable of correcting the position and pressure signals to reduce noise, which further improves the accuracy of the state estimation.

For the nonlinear System (10), the EKF of model i is implemented as follows:

$$\begin{array}{l}{\widehat{x}}_i(k+1)=f_i({\widehat{x}}_i(k),u_i(k))+K_i(k)(y_i(k+1)-H{f}_i({\widehat{x}}_i(k),u_i(k)))\\ K_i(k+1)=P_i(k+1)H^T{(H{P}_i(k)H^T+R_{F,}{}_i(k))}^{-1}\\ P_i(k+1)=F_i(k)(P_i(k)-P_i(k)H^T{(H{P}_i(k)H^T+R_i(k))}^{-1}H{P}_i(k))F_i{(k)}^T+Q_{F,}{}_i(k)\end{array}$$

(22)

where $F_i(k)=\frac{\partial f_i(x_i(k),u_i(k))}{\partial x_i(k)}\left|{}_{x_i(k)={\widehat{x}}_i(k)}\right.$, $H$ represents the measurement matrix, $Q_{F,i}(k)$ and $R_{F,i}(k)$ are the noise matrix of model and observation, and the choice of $Q_{F,i}(k)$ and $R_{F,i}(k)$ can be found in [27,32]. The stability problem of the Kalman filter can be found in [28].

We defined the observer error of EKO as ${\xi }_i(k)=x_i(k)-{\widehat{x}}_i(k)$, then:

$$\begin{array}{l}{\xi }_i(k+1)=x_i(k+1)-{\widehat{x}}_i(k+1)\\ \hspace{1em}\hspace{1em}\hspace{1em} =(I-K_i(k+1)H)\frac{\partial f_i({\eta }_i(k),u_i(k))}{\partial x_i(k)}\end{array}$$

(23)

where ${\eta }_i(k)\in [x_i(k),{\widehat{x}}_i(k)]$. According to [32], ${\xi }_i{(k)}^T{p}_i{(k+1)}^{-1}{\xi }_i(k)$ is a decreasing sequence and thus ${\xi }_i(k)\to 0$ as $k\to \infty$.

After introducing the Kalman state observer, Equation (14) can be represented by:

$$\left\{\begin{array}{l} {\widehat{x}}_i(\left.k+1\right|k)=f_i[{\widehat{x}}_i(k|k-1),u_i(k-1)]+g_i[\widehat{x}(k|k-1),u_i(k-1)]\mathsf{\Delta }{u}_i(k)\\ {\widehat{x}}_i(\left.k+2\right|k)=f_i[{\widehat{x}}_i(k+1|k-1),u_i(k-1)]+g_i[\widehat{x}(k+1|k-1),u_i(k-1)](\mathsf{\Delta }{u}_i(k)+\mathsf{\Delta }{u}_i(k+1))\\ ⋮\\ {\widehat{x}}_i(\left.k+N_p\right|k)=f_i[{\widehat{x}}_i(k+N_p-1|k-1),u_i(k-1)]+g_i[\widehat{x}(k+N_p-1|k-1),u_i(k-1)](\mathsf{\Delta }{u}_i(k)+\dots \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \hspace{1em}+\mathsf{\Delta }{u}_i(k+N_m-1))\end{array}\right.$$

(24)

Then, Equations (19) and (20) become:

$${\widehat{X}}_i=[{\widehat{x}}_i(\left.k+1\right|k), {\widehat{x}}_i(\left.k+2\right|k) ,\cdots {\widehat{x}}_i(\left.k+N_p\right|k)]\prime \in {\Re }^{N_p{n}_x}$$

(25)

$$Y_i=\overline{C}{\widehat{X}}_i=\overline{C}({\widehat{F}}_i+{\widehat{G}}_i\mathsf{\Delta }{U}_i)$$

(26)

where:

$${\widehat{F}}_i=\left[\begin{array}{c}{f}_i[\widehat{x}(k),u(k-1)]\\ f_i[\widehat{x}\left.(k+1\right|k),u(k-1)]\\ ⋮\\ f_i[\widehat{x}\left.(k+p-1\right|k),u(k-1)]\end{array}\right]\in {\Re }^{N_p{n}_x}$$

$${\widehat{G}}_i=\left[\begin{array}{cccc}{g}_i[\widehat{x}(k|k-1),u_i(k-1)]& 0& \cdots & 0\\ g_i[\widehat{x}(k|k-2),u_i(k-1)]& g_i[\widehat{x}(k+1|k-1),u_i(k-1)]& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ g_i[\widehat{x}\left.(k+N_p-1\right|k-1),u_i(k-1)]& g_i[\widehat{x}\left.(k+N_p-1\right|k-1),u_i(k-1)]& \cdots & g_i[\widehat{x}\left.(k+N_p-1\right|k-1),u_i(k-1)]\end{array}\right]\in {\Re }^{N_p{n}_x\times N_m{n}_u}$$

Thus, the optimization problem Equation (21) can be rewritten as:

$$\begin{array}{l}\underset{\mathsf{\Delta }{U}_i}{\min }{J}_i(k)={‖R_i-Y_i‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\\ \hspace{1em}\hspace{1em}\hspace{1em} ={‖R_i-\overline{C}{\widehat{F}}_i-\overline{C}{\widehat{G}}_i\mathsf{\Delta }{U}_i‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\end{array}$$

(27)

$$\begin{array}{l}\hspace{0.17em}s.t.\\ u_{i,}{}_{min}\le u_i(k-1)+\mathsf{\Delta }{U}_i\le u_{i,}{}_{max}\\ \mathsf{\Delta }{u}_{i,}{}_{min}\le \mathsf{\Delta }{U}_i\le \mathsf{\Delta }{u}_{i,}{}_{max}\\ y_{i,}{}_{min}\le \overline{C}(F_i+G_i\mathsf{\Delta }{U}_i)\le y_{i,}{}_{max}\end{array}$$

In summary, for the electro-hydraulic control system of a single joint i, the overall ANMPC framework is shown in Figure 5.

Figure 5. Structure of ANMPC.

4. Experiment

4.1. Experiment Platform

Figure 6 shows the experimental platform, which was modified from a 1.7-ton electro-hydraulic excavator. Wire displacement sensors are installed on the hydraulic cylinders to measure the piston rod displacement of the boom, arm, and bucket, and then the position of the bucket tip can be determined by the kinematics of the robot. In addition, six pressure sensors were installed on the joints of the corresponding cylinders to measure the pressure in the rod and non-rod chambers.

Figure 6. Experiment platform.

The entire control system is divided into the slave part and the master part, which are connected via the CAN bus. The slave part uses a PLC controller primarily for data acquisition and hydraulic system control. The master part uses a computer, and all planning and control algorithms are deployed on the master. Based on the MATLAB GUI environment, the master realizes data reception, display, and the implementation of trajectory planning and trajectory tracking control algorithms.

4.2. Controller Settings

In the experiment, the sampling time of the control system was uniformly set to 0.01 s, which is short enough for the control and the acquisition of data. In order to perform excavation tasks, it is necessary to simultaneously control the boom, stick, and bucket, and the parameters of the three controllers are listed in Table 4.

Table 4. Controller parameters.

Parameters	Boom	Arm	Bucket
[$N_p$, $N_m$]	[10, 2]	[10, 2]	[10, 2]
[$u_{min}$, $u_{max}$] (mA)	[−1000, 1000]	[−1000, 1000]	[−1000, 1000]
[$\mathsf{\Delta }{u}_{min}$, $\mathsf{\Delta }{u}_{max}$] (mA)	[−100, 100]	[−100, 100]	[−100, 100]
[$y_{min}$, $y_{max}$] (m)	[0, 0.38]	[0, 0.445]	[0, 0.352]
[${\dot{y}}_{min}$, ${\dot{y}}_{max}$] (m/s)	[−0.3, 0.3]	[−0.5, 0.5]	[−0.5, 0.5]
[$p_{min}$, $p_{max}$] (MPa)	[−15, 15]	[−12, 12]	[−12, 12]
Dead zone (mA)	[300, 300]	[220, 220]	[200, 200]

At the same time, we designed a linear MPC controller for comparison. Unlike ANMPC, the LMPC linearizes system Model (10) at the equilibrium point $(x_i{}_{,0},u_i{}_{,0})$:

$$x_i(k+1)=f_i(x_{i,0},u_{i,0})+\frac{\partial f_i(x_{i,0},u_{i,0})}{\partial x_i}(x_i(k)-x_{i,0})+\frac{\partial f_i(x_{i,0},u_{i,0})}{\partial u_i}(u_i(k)-u_{i,0})=A_i{x}_i(k)+B_i{u}_i(k)$$

(28)

where $A_i=\frac{\partial f_i(x_{i,0},u_{i,0})}{\partial x_i}$, $B=\frac{\partial f(x_0,u_0)}{\partial x}$.

Based on Equation (28), the LMPC can be obtained as:

$$\begin{array}{l}\underset{\mathsf{\Delta }{U}_i}{\min }{J}_i(k)={‖R_i-Y_i‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\\ \hspace{1em}\hspace{1em}\hspace{1em} ={‖R_i-S_i{x}_i(k)-V_i{u}_i(k-1)-M_i\mathsf{\Delta }{u}_i(k)‖}_Q^2+{‖\mathsf{\Delta }{U}_i‖}_W^2\end{array}$$

(29)

$$\begin{array}{l}\hspace{0.17em}s.t.\\ \hspace{0.17em}\mathsf{\Delta }{U}_{i,\min }\le \mathsf{\Delta }{U}_i\le \mathsf{\Delta }{U}_{i,\max }\\ U_{i,\min }\le u_i(k-1)+\mathsf{\Delta }{U}_i\le U_{i,\max }\\ \hspace{0.17em}{Y}_{i,\min }\le Y_i\le Y_{i,\max }\end{array}$$

where the definitions of S_i, V_i, and M_i are referenced in [30,31]. The parameter settings of the LMPC were the same as those of ANMPC in our experiment.

4.3. Trajectory Planning

In the experiment, the excavator did not rotate, so only motion in the X-Z plane was considered. To verify the effectiveness of the proposed controller, a typical excavation trajectory was used as the reference trajectory. A typical excavation process includes penetration, cutting, and loading [33], and the trajectory of the bucket tip is shown in Figure 7. By solving the inverse kinematics, the trajectory of the bucket tip is converted into the trajectory of each joint, as shown in Figure 8.

Figure 7. Desired bucket tip trajectory: (a) In X-Z plane; (b) X-axis; (c) Z-axis; (d) bucket angle.

Figure 8. Desired joint trajectory: (a) Boom; (b) arm; (c) bucket.

4.4. Experiment Results

The LMPC and ANMPC were used to control the hydraulic cylinders to move along the planned trajectory, and the actual trajectories of each joint were obtained through sensors. In the experiment, the tracking performance of each controller was analyzed both in joint space and Cartesian space.

In the joint space, the trajectory tracking control results of the boom, arm, and bucket are given in Figure 9, Figure 10 and Figure 11, respectively. When using the LMPC, the maximum tracking errors of each joint are 4.11 × 10⁻³ m, 3.89 × 10⁻³ m, and 2.65 × 10⁻³ m, respectively, while the maximum trajectory tracking errors of each joint are 1.92 × 10⁻³ m, 1.72 × 10⁻³ m, and 2.06 × 10⁻³ m, respectively, when using the ANMPC.

Figure 9. Tracking result and errors of boom: (a) Trajectory; (b) errors.

Figure 10. Tracking result and errors of Arm: (a) Trajectory; (b) errors.

Figure 11. Tracking result and errors of bucket: (a) Trajectory; (b) errors.

In the Cartesian space, the bucket tip trajectories when using different controllers are shown in Figure 12a, and the position errors are shown in Figure 12b. The maximum errors of the LMPC and ANMPC are 87.5 × 10⁻³ m and 23.1 × 10⁻³ m, respectively. In addition, the tracking performance of the bucket tip in the X-axis, Z-axis, and bucket angle is given in Figure 13, Figure 14 and Figure 15, respectively.

Figure 12. Bucket tip trajectory and errors: (a) Trajectory; (b) errors.

Figure 13. Bucket tip in X-axis and errors: (a) Trajectory; (b) errors.

Figure 14. Bucket tip in Z-axis and errors: (a) Trajectory; (b) errors.

Figure 15. Bucket angle and errors: (a) Angle trajectory; (b) errors.

The tracking errors of different controllers when tracking a typical excavation trajectory are shown in Table 5. Due to the less unmodeled error of the approximate affine nonlinear model, the proposed ANMPC has a significant advantage over the LMPC in terms of trajectory tracking control performance of the robotic excavator.

Table 5. Tracking errors of the experiment.

Errors (10−3 m)		Joint Space			Cartesian Space
		Boom	Arm	Bucket	Distance	X-Axis	Z-Axis	Angle
LMPC	Max	4.1	3.9	26.4	87.5	25.6	63.9	0.0659
	RMSE	2.3	2.1	6.7	31.4	19.7	24.5	0.0425
ANMPC	Max	1.9	1.7	2.1	23.1	13.8	18.5	0.02
	RMSE	1.0	0.9	1.2	11.6	5.6	10.1	0.0083

5. Conclusions

In this study, a novel observer-based ANMPC was proposed for the trajectory tracking of a hydraulic robotic excavator. Firstly, considering the highly nonlinear characteristics of the electro-hydraulic system, a non-affine nonlinear system model was established, and the parameters of the model were obtained through a parameter identification method. Then, in order to reduce the plant-model mismatch caused by the conventional first-order linearization, we used an approximation method to obtain the explicit linear relationship between the output and input, and ANMPC is designed based on the approximate affine nonlinear model. Compared with the linearized model, the approximate affine nonlinear state space model can better reflect the dynamic characteristics of the system and predict the future state more accurately. At the same time, ANMPC avoids the difficulties in solving nonlinear MPC problems for non-affine nonlinear systems due to the slow solving speed and insufficient computing resources. Trajectory tracking control experiments were designed on a 1.7-ton excavator platform, where ANMPC and LMPC were used to control the robot to perform a typical excavation trajectory, and the three joints of the robot were controlled simultaneously in the experiment. The experimental results showed that ANMPC had better trajectory tracking control performance than LMPC.

Given the significant external disturbances that robotic excavators face during operation, such as soil resistance, our future work will focus on suppressing disturbances to further improve the performance of the controller and promote its application in engineering. Moreover, combining autonomous navigation and 3D reconstruction to achieve fully autonomous operation is a promising research direction.