Yaw Stability Control of Unmanned Emergency Supplies Transportation Vehicle Considering Two-Layer Model Predictive Control

Abstract

The transportation of emergency supplies is characterized by real-time, urgent, and non-contact, which constitute the basic guarantee for emergency rescue and disposal. To improve the yaw stability of the four-wheel-drive unmanned emergency supplies transportation vehicle (ESTV) during operation, a two-layer model predictive controller (MPC) method based on a Kalman filter is proposed in this paper. Firstly, the dynamics model of the ESTV is established. Secondly, the improved Sage–Husa adaptive extended Kalman filter (SHAEKF) is used to decrease the impact of noise on the ESTV system. Thirdly, a two-layer MPC is designed for the yaw stability control of the ESTV. The upper-layer controller solves the yaw moment and the front wheel steering angle of the ESTV. The lower-layer controller optimizes the torque distribution of the four tires of the ESTV to ensure the self-stabilization of the ESTV operation. Finally, analysis and verification are carried out. The simulation results have verified that the improved SHAEKF can decrease the state estimation error by more than 78% and achieve the noise reduction of the ESTV state. Under extreme conditions of high velocity and low adhesion, the average relative error is within 6.77%. The proposed control method can effectively prevent the instability of the ESTV and maintain good yaw stability.

1. Introduction

Emergency supplies transportation plays an important role in production disaster relief, medical assistance, and dangerous situation disposal, and it has important application value for protecting social property and people’s safety [1]. The unmanned emergency supplies transportation vehicle (ESTV) has a high energy utilization rate, which can assist or replace human work and reduce labor input. Therefore, the attention gradually increased [2]. In the context of artificial intelligence, ESTV has been widely used in all aspects of society. With the continuous improvement and development of the performance of the ESTV, stability control under the regular operating conditions of good roads is easy to achieve, but due to uncertain factors such as the ground adhesion coefficient, especially the stability under complex road conditions such as low adhesion after snow and ice, still faces great challenges [3]. Therefore, ensuring the safety and stability of ESTV under various complex road conditions is an important research direction for vehicle dynamics control [4].

Yaw stability control is of important significance for ensuring vehicle safety [5]. To promote the rapid development of four-wheel-drive vehicles, many scholars have carried out related research on improving their yaw stability, such as PID control [6,7], sliding mode control [8,9], adaptive control [10,11], neural network control [12,13], model predictive control (MPC) [14,15], and other methods. Among them, the MPC can make local optimization adjustments at each time step and obtain optimal control input. It is an effective method to deal with multivariable and constrained problems and can compensate for the uncertainties caused by model error, disturbances, and other factors in real time, with good dynamic control performance [16].

The yaw stability control system can be divided into integral stability controllers and hierarchical stability controllers according to the system structure. The integral structure control strategy combines the yaw stability controller design with the drive torque distribution, solves the dynamic coupling between the vehicle and the actuator, and directly optimizes the control input of the system with good control performance [17]. Qu et al. [18] used an extended state observer to estimate the disturbance after linearization, and the LMPC controller was used for yaw stability control to provide the front wheel steering angle and drive torque for four wheels. Wang et al. [19] proposed a coordination control strategy with a two-layer controller. The upper controller used adaptive backstepping technology to obtain the expected additional yaw moment, and the lower controller used the quadratic programming method to calculate the torque distribution between the four wheels so that the tire adhesion could be fully utilized. Chowdhri et al. [20] calculates the control action of four wheels considering reaction time and vehicle limitations with a g–g diagram and Kamm’s circle constraints. Since the integrated control of a vehicle is a highly nonlinear control problem, the computational burden of stability control based on a nonlinear predictive controller is large, and meeting the real-time requirements of a fast dynamic system of a vehicle becomes a control challenge. The hierarchical stability controller treats the upper-layer yaw stability control separately from the lower-layer optimal distribution of the drive torque. Liu et al. [21] constructed a coordinated yaw and roll motion controller based on data-driven multi-MPC in the upper-layer controller, and the lower-layer controller combined the tire slip rate and the vertical load transfer amount and used the quadratic programming algorithm to transform the fusion expected yaw moment into the optimal drive torque of each wheel. Zhang et al. [15] designed the upper supervisor controller to calculate the compensation torque required when the vehicle turns and used Gaussian process regression to reduce the error. The lower-layer controller distributes the torque to the wheels in the form of braking torque. This hierarchical structure reduces the system dimensionality of the centralized optimization strategy and computational burden [9].

The ESTV is inevitably affected by environmental factors in actual work. To obtain the required signal, suppress noise, and improve the accuracy of measurement information, it is particularly important to use filtering methods to remove noise [22]. Kalman filtering (KF) is a filtering algorithm that filters out various mixed noise disturbances from signal observation and estimates the required signal, which is widely used in intelligent vehicle fields, such as positioning [23], parameter identification [24,25], target tracking [26,27], etc. Both the KF [28,29] and the extended Kalman filter (EKF) [30,31] are based on the premise that the statistical characteristics of the process noise and measurement noise of the system are known. However, vehicles are subject to sensor measurement errors, and it can be difficult to obtain statistical information about the measurement noise in real operation, which is sometimes time-variable and uncertain. To overcome these shortcomings, an adaptive Kalman filter (AKF) is widely used. Hajiyev et al. [32] proposed a robust AKF algorithm with multiple adaptive factors. This algorithm combines the adaptive principle and adaptively adjusts for sensor/actuator failures, which can give accurate estimation in the case of sensor or actuator faults. Bahraini [33] proposed an algorithm to increase the estimation accuracy of UKF-SLAM by adjusting the scale parameter and designed an unscented KF with adaptive selection of the scaling parameter to improve the estimation accuracy of SLAM. If UKF is used on a model with a large state vector, numerical stability problems may occur. Mosconi et al. [34] considered the influence factors such as tire condition and road characteristics and composed an EKF state estimator with 14 states to provide the estimation of road slope and banking angle, which can accurately identify the physical characteristics of tire/road interaction in real time. This method relies on accurate vehicle models and detailed parameters. Wang et al. [35] proposed an improved Sage–Husa adaptive Kalman filter (SHAKF) scheme combined with fuzzy clustering method, which can change the Kalman filter structure through the adaptive mechanism, and the system is well estimated. The construction of AKF is of great significance for increasing the accuracy and stability of filtering.

Based on this, this paper combines the improved SHAEKF method and the two-layer MPC method to design the yaw stability control strategy, which can solve the safety and stability problem of the four-wheel-drive ESTV during operation. It has the following three advantages:

• A two-layer control structure is adopted to decouple the dynamics between the yaw control system and the driving system. Compared with the integral control structure of yaw stability and torque distribution, the system dimension of the overall optimization strategy is reduced, the computational burden is reduced, and the computational efficiency of multi-objective optimization control with constraints is improved, which is easier to implement.
• To improve the accuracy and stability of filtering, an improved SHAEKF method that can dynamically adjust the statistical characteristics of noise in a timely manner is used to optimize the state variables, which improves the accuracy of the state variables.
• The upper-layer yaw stability controller uses MPC to solve the expected additional yaw moment and the front wheel steering angle. The lower-layer torque distribution controller uses the rolling time domain optimization method to track the additional yaw moment required for the upper-layer yaw stability.

The rest of this paper is organized as follows. In Section 2, the dynamics model and tire model of the ESTV are established. In Section 3, the improved SHAEKF algorithm is used to optimize the state of the ESTV. A two-layer MPC yaw stability controller is designed in Section 4. The effectiveness of the improved SHAEKF and the yaw stability control effect are simulated and analyzed in Section 5. Section 6 gives the conclusion.

2. Vehicle Dynamics Model

In this section, the two vehicle dynamics models are established to ensure the yaw stability of the ESTV. A two-degrees-of-freedom (2DOF) reference model is established to obtain the reference yaw rate ${\gamma }^{\ast }$ and the reference sideslip angle ${\beta }^{\ast }$ of the vehicle. The 7DOF dynamics model, which can comprehensively respond to the vehicle characteristics, is established considering the relationship between the actual dynamic parameters of the vehicle and its yaw stability, and then the tire model is introduced.

The prediction model needs to be simplified as much as possible on the basis of accurately reflecting the dynamic process of the ESTV to reduce the computing burden of the control algorithm. Therefore, the following idealized assumptions are made for the ESTV:

• Assume that the vehicle is operating on a flat road, and ignore the vehicle’s vertical movement.
• Assume that the suspension system and the vehicle are rigid.
• Assume that only pure lateral deflection tire characteristics are considered.
• Assume that the lateral load transfer of the wheel is not considered.
• Assume that the effect of lateral and longitudinal aerodynamics on the yaw characteristics of the vehicle is ignored.

2.1. Dynamics Model

The ideal reference model is required to accurately reflect the expected steering process of the vehicle, both to make the system have the same steering flexibility as a traditional vehicle and to ensure that the vehicle has a good posture. The 2DOF model, including the lateral and yaw motion of the ESTV, is shown in Figure 1.

In the vehicle control system, $\beta$ and $\gamma$ represent the 2DOF of the simplified ESTV dynamics model, and ${\delta }_{\mathrm{f}}$ is assumed to be small, that is $cos{\delta }_{\mathrm{f}}\approx 1$, $sin{\delta }_{\mathrm{f}}\approx {\delta }_{\mathrm{f}}$. The 2DOF dynamics equation is:

$$\left\{\begin{array}{c}{\dot{\beta }}^{\ast }=\frac{F_{y\mathrm{f}}+F_{y\mathrm{r}}}{m{v}_x}-\gamma \hfill \\ {\dot{\gamma }}^{\ast }=\frac{l_{\mathrm{f}}{F}_{y\mathrm{f}}-l_{\mathrm{r}}{F}_{y\mathrm{r}}+M_x}{I_z}\hfill \end{array}\right..$$

(1)

Figure 2 shows the 7DOF dynamics model used to verify the effectiveness of the controller design, including the longitudinal motion, lateral motion, yaw motion, and four-wheel rotation of the vehicle [36], which can truly express the dynamic characteristics of the ESTV.

2.2. Tire Model

Given the structural characteristics of the tire, the dynamic performance is nonlinear. Selecting a tire model that is realistic and easy to use is the key to establishing a vehicle dynamics model. The tire longitudinal and lateral force model in this paper is a semi-empirical tire model based on the magic formula. The magic formula tire model is a relatively mature vehicle stability control model that considers the interactions between longitudinal and lateral force. In general, the magic formula is expressed as [37]:

$$F_{c\mathrm{i}\mathrm{j}}=Dsin\{Carctan[B{\alpha }_{\mathrm{ij}}-E(B{\alpha }_{\mathrm{ij}}-arctan\left(B{\alpha }_{\mathrm{ij}}\right))]\},$$

(2)

where ${\alpha }_{\mathrm{ij}}$ is the slip angle of the tire.

However, the magic model is too cumbersome to apply to controller design. Therefore, the model is considered to be simplified, and the cubic polynomial is obtained by Taylor expansion, such as in Equation (3):

$$\left\{\begin{array}{c}{F}_{y\mathrm{f}\mathrm{j}}=-2C_{\mathrm{f}}\left(1-K_a{\alpha }_{\mathrm{fj}}^2\right){\alpha }_{\mathrm{fj}}\hfill \\ F_{y\mathrm{r}\mathrm{j}}=-2C_{\mathrm{r}}\left(1-K_b{\alpha }_{\mathrm{rj}}^2\right){\alpha }_{\mathrm{rj}}\hfill \end{array}\right.,$$

(3)

where $K_a$ and $K_b$ are the fitting coefficients obtained from the tire magic formula. The dynamics relationship between $\beta$ and ${\alpha }_{\mathrm{ij}}$, ${\alpha }_{\mathrm{ij}}$ is calculated as follows:

$$\left\{\begin{array}{c}{\alpha }_{\mathrm{fj}}=\beta +\frac{l_{\mathrm{f}}\gamma }{v_x}-{\delta }_{\mathrm{f}}\hfill \\ {\alpha }_{\mathrm{rj}}=\beta -\frac{l_{\mathrm{r}}\gamma }{v_x}\hfill \end{array}\right..$$

(4)

The longitudinal force of the tire is:

$$\left\{\begin{array}{c}{F}_{x\mathrm{f}\mathrm{j}}=-2C_{l\mathrm{f}\mathrm{j}}{s}_{\mathrm{fj}}\hfill \\ F_{x\mathrm{r}\mathrm{j}}=-2C_{l\mathrm{r}\mathrm{j}}{s}_{\mathrm{rj}}\hfill \end{array}\right..$$

(5)

Tire slip rate is:

$$s_{\mathrm{ij}}=\frac{{\omega }_{\mathrm{ij}}{R}_{\omega }-v_x}{v_x}.$$

(6)

Under the driving conditions, the tire force analysis is shown in Figure 3.

The rotation dynamic equation of a single tire is:

$$\begin{array}{cc}{I}_{\omega }{\dot{\omega }}_{\mathrm{ij}}=T_{\mathrm{ij}}-F_{x\mathrm{i}\mathrm{j}}{R}_{\omega }& \end{array}(\mathrm{i}=\mathrm{f},\mathrm{r},\mathrm{j}=\mathrm{l},\mathrm{r}).$$

(7)

The dynamics equation for the tire slip rate is [9]:

$${\dot{s}}_{\mathrm{ij}}=(-\frac{R_{\omega }^2}{I_{\omega }{v}_x}-4\frac{s_{\mathrm{ij}}+1}{m{v}_x})C_{l\mathrm{i}\mathrm{j}}{s}_{\mathrm{ij}}+\frac{R_{\omega }}{I_{\omega }{v}_x}{T}_{\mathrm{ij}}.$$

(8)

Ignoring the tire nonlinear characteristics, the 2DOF reference model is obtained as follows:

$$\left\{\begin{array}{c}{\dot{\beta }}^{\ast }=\frac{2C_{\mathrm{f}}({\delta }_{\mathrm{f}}-{\beta }^{\ast }-\frac{l_{\mathrm{f}}{\gamma }^{\ast }}{v_x})+2C_{\mathrm{r}}(-{\beta }^{\ast }+\frac{l_{\mathrm{r}}{\gamma }^{\ast }}{v_x})}{m{v}_x}-{\gamma }^{\ast }\\ {\dot{\gamma }}^{\ast }=\frac{2l_{\mathrm{f}}{C}_{\mathrm{f}}({{\delta }_{\mathrm{f}}}^{\ast }-{\beta }^{\ast }-\frac{l_{\mathrm{f}}{\gamma }^{\ast }}{v_x})-2l_{\mathrm{r}}{C}_{\mathrm{r}}(-{\beta }^{\ast }+\frac{l_{\mathrm{r}}{\gamma }^{\ast }}{v_x})+M_x}{I_z}\end{array}\right..$$

(9)

Since $\beta$ and $\gamma$ are closely related to the yaw stability of the vehicle, they are selected as the state variables $\mathit{x}={\left[\begin{array}{cc}\beta & \gamma \end{array}\right]}^{\mathrm{T}}$, while ${\delta }_{\mathrm{f}}$ and $M_x$ are selected as the control inputs $\mathit{u}={\left[\begin{array}{cc}{\delta }_{\mathrm{f}}& M_x\end{array}\right]}^{\mathrm{T}}$, and Equation (9) is written into the standard state space form as follows:

$$\left\{\begin{array}{c}\dot{\mathit{x}}=\mathit{Ax}+\mathit{Bu}\hfill \\ \mathit{y}=\mathit{Cx}\hfill \end{array}\right.,$$

(10)

where

$$\mathit{A}=\left[\begin{array}{cc}-\frac{2C_{\mathrm{f}}+2C_{\mathrm{r}}}{m{v}_x}& \frac{2l_{\mathrm{r}}{C}_{\mathrm{r}}-2l_{\mathrm{f}}{C}_{\mathrm{f}}}{m{v}_x^2}-1\\ \frac{2l_{\mathrm{r}}{C}_{\mathrm{r}}-2l_{\mathrm{f}}{C}_{\mathrm{f}}}{I_z}& -\frac{2l_f^2{C}_{\mathrm{f}}+2l_r^2{C}_{\mathrm{r}}}{I_z{v}_x}\end{array}\right],\mathit{B}=\left[\begin{array}{cc}\frac{2C_{\mathrm{f}}}{m{v}_x}& 0\\ \frac{2l_{\mathrm{f}}{C}_{\mathrm{f}}}{I_z}& \frac{1}{I_z}\end{array}\right],\mathit{C}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right].$$

3. Improved SHAEKF

In the actual transportation of supplies, the operating conditions of the ESTV are complex and will be easily influenced by environmental factors. For example, factors including uphill and downhill, uneven road surface, and so on, lead to the continuous jitter of the vehicle during operation, and the detection accuracy of the sensor will be affected. In order to obtain more accurate and credible vehicle state information, it is necessary to filter and estimate the sensor data to reduce the influence of noise on state variables and improve the accuracy of measurement information.

In practice, the statistical characteristics of noise are unobtainable, so SHAEKF with simple principles and good real-time performance is used to solve this problem. The SHAKF is suitable for the state estimation problem with time-varying statistical characteristics of noise. It can predict and rectify the system process noise and measurement noise in a timely manner while using measurement data for recursive filtering when the statistical characteristics of the noise are unknown, so as to achieve the purpose of suppressing filter divergence and improving filtering accuracy. The SHAEKF algorithm can use the estimated parameters to estimate the state parameters of the EKF and obtain the state estimation value of the system [38], which improves the robustness of AKF to measurement noise.

Since the SHAKF algorithm cannot estimate both the system noise covariance matrix and the measurement noise covariance matrix [39], the measurement noise with large uncertainty is adaptively adjusted. The improved SHAEKF algorithm is used to correct the noise characteristics of the ESTV in real time to obtain better estimation accuracy. The state equation and the measurement equation of the system with noise are established by combining the ESTV dynamics model as:

$$\left\{\begin{array}{l}\mathit{x}(k+1)=\mathit{A}\left(k\right)\mathit{x}\left(k\right)+\mathit{B}\left(k\right)\mathit{u}\left(k\right)+\mathit{w}\left(k\right)\\ \mathit{y}(k+1)=\mathit{H}(k+1)\mathit{x}(k+1)+\mathit{v}(k+1)\end{array}\right.,$$

(11)

where $\mathit{w}\left(k\right)$ is process noise, and $\mathit{v}\left(k\right)$ is measurement noise.

The prediction state vector ${\widehat{\mathit{x}}}_{k+1,k}$ and the prediction error covariance matrix ${\mathit{P}}_{k+1,k}$ are:

$${\widehat{\mathit{x}}}_{k+1,k}={\mathit{A}}_k{\widehat{\mathit{x}}}_k+{\mathit{B}}_k{\mathit{u}}_k$$

(12)

$${\mathit{P}}_{k+1,k}={\mathit{A}}_k{\mathit{P}}_{k,k}{\mathit{A}}_k^{\mathrm{T}}+{\widehat{\mathit{Q}}}_k,$$

(13)

where ${\widehat{\mathit{x}}}_k$ is the state estimation vector, ${\mathit{A}}_k$ is the Jacobian matrix after the partial derivative of the state equation for x, and ${\mathit{Q}}_k$ is the process noise covariance matrix.

The Kalman measurement update includes the gain Equation (14), filter Equation (15), and error covariance update Equation (16):

$${\mathit{K}}_{k+1}={\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}({\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}+{\widehat{\mathit{R}}}_{k+1}{)}^{-1}$$

(14)

$${\widehat{\mathit{x}}}_{k+1}={\widehat{\mathit{x}}}_{k+1,k}+{\mathit{K}}_{k+1}{\mathit{\epsilon }}_{k+1}$$

(15)

$${\mathit{P}}_{k+1}=(\mathit{I}-{\mathit{K}}_{k+1}{\mathit{H}}_{k+1}){\mathit{P}}_{k+1,k},$$

(16)

where ${\mathit{K}}_{k+1}$ is the Kalman gain, ${\mathit{H}}_{k+1}$ is the Jacobian matrix after the partial derivative of the measurement equation to x, ${\mathit{R}}_{k+1}$ is the measurement noise covariance matrix, and ${\mathit{\epsilon }}_{k+1}$ is the residual. The residual ${\mathit{\epsilon }}_{k+1}$ is:

$${\mathit{\epsilon }}_{k+1}={\mathit{y}}_{k+1}-{\mathit{H}}_{k+1}{\widehat{\mathit{x}}}_{k+1,k}-{\widehat{\mathit{r}}}_{k+1},$$

(17)

where ${\mathit{y}}_{k+1}$ is the observation variable, and ${\widehat{\mathit{r}}}_{k+1}$ is the measurement noise variance.

Finally, the estimation of the noise and the equation for estimating the mean and covariance matrix of the noise are:

$${\widehat{\mathit{r}}}_{k+1}=(1-d_k){\widehat{\mathit{r}}}_k+d_k({\mathit{y}}_{k+1}-{\mathit{H}}_{k+1}{\widehat{\mathit{x}}}_{k+1,k})$$

(18)

$$\begin{array}{cc}\hfill {\widehat{\mathit{R}}}_{k+1}=& (1-d_k){\widehat{\mathit{R}}}_k+d_k({\mathit{\epsilon }}_{k+1}{\mathit{\epsilon }}_{k+1}^{\mathrm{T}}-{\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}),\hfill \end{array}$$

(19)

where $d_k$ is the weighting coefficient. The weighting coefficient $d_k$ is:

$$d_k=\frac{1-b}{1-b^k},$$

(20)

where b is the forgetting factor with a value range of [0.95, 0.995].

When there is a convergence trend in the filtering process, the error covariance ${\mathit{P}}_{k+1}$ will decrease, and ${\mathit{H}}_{k+1}$ is a fixed value; then, ${\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}$ will gradually decrease until it tends to 0. Therefore, the effect of ${\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}$ on the measurement noise covariance estimation can be ignored [40]. Therefore, ${\widehat{\mathit{R}}}_{k+1}$ of Equation (19) is replaced by:

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k{\mathit{\epsilon }}_{k+1}{\mathit{\epsilon }}_{k+1}^{\mathrm{T}}.$$

(21)

The flowchart of the improved SHAEKF is shown in Figure 4.

Therefore, the two state variables of the system can be estimated to decrease the interference of noise and ensure the high accuracy of the calculation of ${\delta }_{\mathrm{f}}$ and $M_x$ of the vehicle.

4. Control Strategy

The $\beta$ and $\gamma$ of the vehicle are two important variables to describe the posture of the vehicle, which are closely related to vehicle stability. When the ESTV is steering, the operation stability of the vehicle is reflected by the $\beta$, and the $\gamma$ reflects the steady-state steering characteristics and the vehicle manipulation stability. The control structure of the ESTV yaw stabilization system is shown in Figure 5.

As shown in the control structure diagram, the required ${\beta }^{\ast }$ and ${\gamma }^{\ast }$ are determined by the vehicle reference 2DOF model. The upper-layer yaw stability controller calculates ${\delta }_{\mathrm{f}}$ and $M_x$ and takes the yaw moment generated by the tire longitudinal force as the control target of the lower-layer torque distribution controller.

The upper-layer MPC controller calculates the optimal control inputs so that $\beta$ and $\gamma$ of the ESTV achieve the expected value as much as possible in the current operating state of the ESTV. The prediction model of yaw stability control is:

$$\left\{\begin{array}{c}\mathit{x}(k+1\left|t\right.)={\mathit{A}}_k\mathit{x}\left(k\left|t\right.\right)+{\mathit{B}}_k\mathit{u}\left(k\left|t\right.\right)\hfill \\ \mathit{y}\left(k\left|t\right.\right)={\mathit{C}}_k\mathit{x}\left(k\left|t\right.\right)\hfill \end{array}\right..$$

(22)

The controller is solved for the optimal input in each finite time domain using the quadratic programming method [41]. In MPC, $N_{\mathrm{p}}$ is the predictive time domain and $N_{\mathrm{p}}\ge 1$, while $N_{\mathrm{c}}$ is the control time domain and $1\le N_{\mathrm{c}}\le N_{\mathrm{p}}$. The predicted control output sequence is [17]:

$${\mathit{Y}}_p(k+1|k)={\left[\begin{array}{c}\mathit{y}(k+1|k)\\ \mathit{y}(k+2|k)\\ ⋮\\ \mathit{y}(k+N_{\mathrm{p}}|k)\end{array}\right]}_{N_{\mathrm{p}}\times 1}.$$

(23)

According to the 2DOF reference model of the ESTV, the control output reference sequence is:

$$\mathit{R}(k+1|k)={\left[\begin{array}{c}\mathit{r}(k+1|k)\\ \mathit{r}(k+2|k)\\ ⋮\\ \mathit{r}(k+N_{\mathrm{p}}|k)\end{array}\right]}_{N_{\mathrm{p}}\times 1}.$$

(24)

The control input increment sequence in $N_{\mathrm{p}}$ at time k is as follows:

$$\Delta \mathit{U}\left(k\right)={\left[\begin{array}{c}\Delta \mathit{u}\left(k\right)\\ \Delta \mathit{u}(k+1)\\ ⋮\\ \Delta \mathit{u}\left(k+N_{\mathrm{c}}-1\right)\end{array}\right]}_{N_{\mathrm{c}}\times 1}.$$

(25)

The upper-layer yaw stability controller obtains ${\delta }_{\mathrm{f}}$ and $M_x$ by tracking and calculating the reference state of the vehicle. The lower-layer control strategy adopts the torque optimization controller based on MPC to realize the stable operation of the vehicle by reasonably distributing the drive torque of four tires.

The yaw moment generated by the longitudinal force of the four tires is:

$$M_x=\frac{d}{2}·(-F_{x\mathrm{f}\mathrm{l}}+F_{x\mathrm{f}\mathrm{r}}-F_{x\mathrm{r}\mathrm{l}}+F_{x\mathrm{r}\mathrm{r}}).$$

(26)

The vertical load of the vehicle’s four tires is:

$$\left\{\begin{array}{c}{F}_{z\mathrm{f}\mathrm{l}}=\frac{l_{\mathrm{r}}}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}mg-\frac{m{a}_{x}h}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}-\frac{m{a}_{y}h{l}_{\mathrm{r}}}{d\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}\\ F_{z\mathrm{f}\mathrm{r}}=\frac{l_{\mathrm{r}}}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}mg-\frac{m{a}_{x}h}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}+\frac{m{a}_{y}h{l}_{\mathrm{r}}}{d\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}\\ F_{z\mathrm{r}\mathrm{l}}=\frac{l_{\mathrm{f}}}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}mg+\frac{m{a}_{x}h}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}-\frac{m{a}_{y}h{l}_{\mathrm{f}}}{d\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}\\ F_{z\mathrm{r}\mathrm{r}}=\frac{l_{\mathrm{f}}}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}mg+\frac{m{a}_{x}h}{2\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}+\frac{m{a}_{y}h{l}_{\mathrm{f}}}{d\left(l_{\mathrm{f}}+l_{\mathrm{r}}\right)}\end{array}\right..$$

(27)

When the vehicle operates in conditions with a low adhesion coefficient and fluctuating roads, the vertical load transfer of each wheel plays an important role in the lateral stability of the vehicle. In the controller design, the tire vertical load transfer variable factor needs to be added to coordinate the drive torque distribution of each wheel. The vertical load transfer weight coefficients of the four wheels are [21]:

$$\left\{\begin{array}{c}{\xi }_{\mathrm{fl}}=\frac{F_{z\mathrm{f}\mathrm{l}}}{F_{z\mathrm{f}\mathrm{l}}+F_{z\mathrm{f}\mathrm{r}}+F_{z\mathrm{r}\mathrm{l}}+F_{z\mathrm{r}\mathrm{r}}}\\ {\xi }_{\mathrm{fr}}=\frac{F_{z\mathrm{f}\mathrm{r}}}{F_{z\mathrm{f}\mathrm{l}}+F_{z\mathrm{f}\mathrm{r}}+F_{z\mathrm{r}\mathrm{l}}+F_{z\mathrm{r}\mathrm{r}}}\\ {\xi }_{\mathrm{rl}}=\frac{F_{z\mathrm{r}\mathrm{l}}}{F_{z\mathrm{f}\mathrm{l}}+F_{z\mathrm{f}\mathrm{r}}+F_{z\mathrm{r}\mathrm{l}}+F_{z\mathrm{r}\mathrm{r}}}\\ {\xi }_{\mathrm{rr}}=\frac{F_{z\mathrm{r}\mathrm{r}}}{F_{z\mathrm{f}\mathrm{l}}+F_{z\mathrm{f}\mathrm{r}}+F_{z\mathrm{r}\mathrm{l}}+F_{z\mathrm{r}\mathrm{r}}}\end{array}\right..$$

(28)

The lower-layer controller state variable is the tire slip ratio ${\mathit{x}}_{\mathrm{L}}={\left[\begin{array}{cccc}{s}_{\mathrm{fl}}& s_{\mathrm{fr}}& s_{\mathrm{rl}}& s_{\mathrm{rr}}\end{array}\right]}^{\mathrm{T}}$, the control input is the drive torque of four tires ${\mathit{u}}_{\mathrm{L}}={\left[\begin{array}{cccc}{T}_{\mathrm{fl}}& T_{\mathrm{fr}}& T_{\mathrm{rl}}& T_{\mathrm{rr}}\end{array}\right]}^{\mathrm{T}}$, and the output is ${\mathit{y}}_{\mathrm{L}}=M_x$. Convert Equation (8) into the state space equation as:

$$\left\{\begin{array}{c}{\dot{\mathit{x}}}_{\mathrm{L}}=(-\frac{R_{\omega }^2}{I_{\omega }{v}_x}-\frac{({\xi }_{\mathrm{fl}}+{\xi }_{\mathrm{fr}}+{\xi }_{\mathrm{rl}}+{\xi }_{\mathrm{rr}})(\mathit{x}+1)}{{\xi }_{\mathrm{ij}}m{v}_x}){\mathit{C}}_{l\mathrm{i}\mathrm{j}}{\mathit{x}}_{\mathrm{L}}+\frac{R_{\omega }}{I_{\omega }{v}_x}{\mathit{u}}_{\mathrm{L}}\hfill \\ {\mathit{y}}_{\mathrm{L}}=\frac{d}{2}\left[\begin{array}{cccc}-{\xi }_{\mathrm{fl}}{C}_{l\mathrm{f}\mathrm{l}}& {\xi }_{\mathrm{fr}}{C}_{l\mathrm{f}\mathrm{r}}& -{\xi }_{\mathrm{rl}}{C}_{l\mathrm{r}\mathrm{l}}& {\xi }_{\mathrm{rr}}{C}_{l\mathrm{r}\mathrm{r}}\end{array}\right]{\mathit{x}}_{\mathrm{L}}\hfill \end{array}\right..$$

(29)

The state Equation (29) is discretized:

$$\left\{\begin{array}{c}{\mathit{x}}_{\mathrm{L}}(k+1)=\mathit{\varphi }{\mathit{x}}_{\mathrm{L}}\left(k\right)+\mathit{D}{\mathit{u}}_{\mathrm{L}}\left(k\right)\hfill \\ {\mathit{y}}_{\mathrm{L}}\left(k\right)={\mathit{C}}_l{\mathit{x}}_{\mathrm{L}}\left(k\right)\hfill \end{array}\right..$$

(30)

The upper-layer controller ensures that the ESTV has good stability during operation so that $\widehat{\beta }$ and $\widehat{\gamma }$ estimated by the SHAEKF can quickly track ${\beta }^{\ast }$ and ${\gamma }^{\ast }$. To ensure that the ESTV accurately tracks the reference trajectory, ${\mathit{Y}}_p$ should closely track $\mathit{R}$ to maintain the tracking accuracy of the system. The fluctuation of the system control variables affects the smoothness of the vehicle’s operation. Therefore, we need to consider the stability of control input variables. The lower-layer controller is required not only to track $M_x$ but also to minimize energy consumption while ensuring the stability of the ESTV. The objective function is:

$$\begin{array}{cc}\hfill {\mathit{J}}_1& =\sum _{i=1}^{N_{\mathrm{p}}}\left[{\mathbf{\Gamma }}_{\mathrm{y}1}{\left(\widehat{\beta }(k+i\mid k)-{\beta }^{\ast }\left(k\right)\right)}^2\right.+\left.{\mathbf{\Gamma }}_{\mathrm{y}2}{\left(\widehat{\gamma }(k+i\mid k)-{\gamma }^{\ast }\left(k\right)\right)}^2\right]\hfill \\ \hfill & +\sum _{i=0}^{N_{\mathrm{c}}-1}[{\mathbf{\Gamma }}_{\Delta 1}(\Delta {\delta }_{\mathrm{f}}^2(k+i\mid k))+\left({\mathbf{\Gamma }}_{\Delta 2}\right(\Delta T_{\mathrm{fl}}^2(k+i\mid k)+\Delta T_{\mathrm{fr}}^2(k+i\mid k)\hfill \\ \hfill & +\Delta T_{\mathrm{rl}}^2(k+i\mid k)+\Delta T_{\mathrm{rr}}^2(k+i\mid k)\left)\right]\hfill \\ \hfill & +{\mathbf{\Gamma }}_{\mathrm{y}}{∥\mathit{Y}(k+1\mid k)-M_x(k+1)∥}^2+{\mathbf{\Gamma }}_{\mathrm{u}}{∥{\mathit{U}}_{\mathrm{L}}\left(k\right)∥}^2,\hfill \end{array}$$

(31)

where ${\mathit{U}}_{\mathrm{L}}\left(k\right)$ is the control variables sequence, and $\mathit{Y}(k+1\mid k)$ is the $N_{\mathrm{p}}$-step prediction output at moment k, and ${\mathbf{\Gamma }}_{\mathrm{y}}$, ${\mathbf{\Gamma }}_{\Delta }$, and ${\mathbf{\Gamma }}_{\mathrm{u}}$ are the weight coefficient matrices of the system output, the controlled input variables, and the inputs, respectively. To achieve the expected control performance during the actual operation of the vehicle, the capacity range constraint of the actuator should be considered in the design of the MPC [20]. Due to the saturation characteristics of the actuator, ${\delta }_{\mathrm{f}}$ and the drive torque of the system are constrained by physical conditions. To ensure the yaw stability while avoiding the operational stability of the vehicle caused by tire slip, the following boundary constraints are imposed:

$$\begin{array}{cc}\hfill & -{{\delta }_{\mathrm{f}}}_{max}\le {\delta }_{\mathrm{f}}(k+i\mid k)\le {{\delta }_{\mathrm{f}}}_{max},\hfill \\ \hfill & -T_{max}\le T_{\mathrm{ij}}(k+i\mid k)\le T_{max}\hfill & \hfill \\ \hfill & \begin{array}{c}-s_{max}\le s_{\mathrm{i}\mathrm{j}}(k+i\mid k)\le s_{max}\end{array}(\mathrm{i}=\mathrm{f},\mathrm{r},\mathrm{j}=\mathrm{l},\mathrm{r}).\hfill \end{array}$$

(32)

At the same time, the sum of each drive torque should be equal to the total torque required:

$$\sum T_{\mathrm{ij}}=T_t,$$

(33)

where $i=1,2,\cdots ,N_{\mathrm{p}}$. Therefore, the objective function expression of the controller’s k-th control period is:

$$\begin{array}{cc}\hfill & \begin{array}{cc}\min & {\mathit{J}}_k={\displaystyle \sum _{i=1}^{N_{\mathrm{p}}}}{\mathit{Y}}_{\mathrm{e}}^{\mathrm{T}}\left(k\right){\mathbf{\Gamma }}_{\mathrm{y}}{\mathit{Y}}_{\mathrm{e}}\left(k\right)+{\displaystyle \sum _{i=0}^{N_{\mathrm{c}}-1}}\left[\Delta {\mathit{U}}^{\mathrm{T}}\left(k\right){\mathbf{\Gamma }}_{\Delta }\Delta \mathit{U}\left(k\right)+{\mathit{U}}_{\mathrm{L}}^{\mathrm{T}}\left(k\right){\mathbf{\Gamma }}_{\mathrm{u}}{\mathit{U}}_{\mathrm{L}}\left(k\right)\right]\end{array}\hfill \\ \hfill & \begin{array}{cc}s.t.& \begin{array}{cc}\hfill -{{\delta }_{\mathrm{f}}}_{max}& \le {\delta }_{\mathrm{f}}\left(k+i\left|k\right.\right)\le {{\delta }_{\mathrm{f}}}_{max}\hfill \\ \hfill -s_{max}& \le s_{\mathrm{ij}}\left(k+i\left|k\right.\right)\le s_{max}\hfill \\ \hfill -T_{max}& \le T_{\mathrm{ij}}\left(k+i\left|k\right.\right)\le T_{max}\hfill \\ \hfill & \sum T_{\mathrm{ij}}=T_t\hfill \end{array}\end{array}.\hfill \end{array}$$

(34)

5. Analysis and Validation

In this paper, a MATLAB/Simulink 2018a simulation model is built to simulate and verify the estimation effect of the improved SHAEKF and the two-layer MPC controller. To verify the performance of the designed yaw stability controller, the stability is simulated and tested under multiple operating conditions. The ESTV model used in this experiment is a 7DOF dynamics model, and the controller prediction model adopts a 2DOF model. The parameters of the ESTV defined by simulation are shown in

Table 1. ESTV parameters.

Definition	Value
Vehicle mass m	930 kg
Moment of inertia $I_z$	1372 $\mathrm{kg}·{\mathrm{m}}^2$
Distance from the center of gravity to front axle $l_{\mathrm{f}}$	0.986 m
Distance from the center of gravity to rear axle $l_{\mathrm{r}}$	1.253 m
Wheel tread d	1.14 m
Tire rolling radius $R_{\omega }$	0.26 m

, and the controller parameters are shown in

Table 2. Controller parameters.

Definition	Controller Value
Predictive time domain $N_{\mathrm{p}}$	15
Control time domain $N_{\mathrm{c}}$	5
Sampling time T	0.05 s
Weight coefficient matrix ${\mathbf{\Gamma }}_{\mathrm{y}}$	$\mathrm{diag}\left(100,\hspace{0.17em}200\right)$
Weight coefficient matrix ${\mathbf{\Gamma }}_{\Delta }$	60
Weight coefficient matrix ${\mathbf{\Gamma }}_{\mathrm{u}}$	100

, where the ${\delta }_{\mathrm{f}}$ constraint is 0.1 rad, the maximum driving torque of a single wheel is 100 Nm, and the boundary value of the safe range of tire slip rate is 0.08.

5.1. Simulation of Improved SHAEKF

To verify the effectiveness of SHAEKF, Gaussian white noise is added. The trajectories of the vehicle before and after the improved SHAEKF are shown in Figure 6. Figure 7 shows the state error before and after the KF, AKF, and the improved SHAEKF. The comparison between the measured values and the estimated values of the state variables are shown in

Table 3. Measured values and estimated values.

State Variables	Original Data	KF	AKF	SHAEKF
${\gamma }_{\mathrm{e}}$ maximum value	0.1659 m	0.1249 m	0.0968 m	0.0847 m
${\beta }_{\mathrm{e}}$ maximum value	0.2666 m	0.1618 m	0.1016 m	0.0882 m
$\left|{\gamma }_{\mathrm{e}}\right|$ average value	0.0038 m	0.0027 m	0.0017 m	0.0013 m
$\left|{\beta }_{\mathrm{e}}\right|$ average value	0.0049 m	0.0034 m	0.0022 m	0.0011 m
${\gamma }_{\mathrm{e}}$ variance	$1.7210\times {10}^{-4}$${\mathrm{m}}^2$	$9.0315\times {10}^{-5}$${\mathrm{m}}^2$	$4.6964\times {10}^{-5}$${\mathrm{m}}^2$	$2.6667\times {10}^{-5}$${\mathrm{m}}^2$
${\beta }_{\mathrm{e}}$ variance	$2.2990\times {10}^{-4}$${\mathrm{m}}^2$	$8.7293\times {10}^{-5}$${\mathrm{m}}^2$	$3.7698\times {10}^{-5}$${\mathrm{m}}^2$	$2.6103\times {10}^{-5}$${\mathrm{m}}^2$

. The ${\gamma }_{\mathrm{e}}$ and ${\beta }_{\mathrm{e}}$ of the original data represent the difference between the measured value and the reference value, and the ${\gamma }_{\mathrm{e}}$ and ${\beta }_{\mathrm{e}}$ after filtering represent the difference between the estimated value and the reference value.

According to Figure 6 and Figure 7, it can be seen that the improved SHAEKF is closer to the reference trajectory without the prior knowledge of the measurement noise, and the average value of the absolute value of the state variables estimation error is reduced by more than 78% compared with the measured value. It can be seen that the improved SHAEKF reduces the influence of noise on the ESTV.

5.2. Double Shift Trajectory Simulation

5.2.1. Comparison of Different Control Methods

To verify the effectiveness of the two-layer MPC control strategy in improving the operation stability of ESTV, a comparative simulation is conducted to verify the two-layer MPC control strategy, no control, and traditional MPC control. The velocity is set to 15 m/s, and the road adhesion coefficient is set to 0.4. The simulation results are shown in Figure 8, Figure 9 and Figure 10.

Figure 8, Figure 9 and Figure 10 show the trajectory tracking ability of the ESTV under three control methods. From Figure 8, we can see that the uncontrolled vehicle has a large sideslip, the error between the operation trajectory and the reference trajectory is too large, and the vehicle is out of control. The vehicle based on traditional MPC can basically ensure safe operation, but there is also a large overshoot at the corner. Based on the two-layer MPC control strategy, the accuracy of the ESTV tracking is significantly improved, it has a strong ability to correct the turning attitude, and there is basically no excessive sideslip and tail flick. Figure 9 is the comparison of the ESTV’s $\gamma$ and $\beta$ under three control methods. Compared with the traditional MPC, the $\gamma$ and $\beta$ of the ESTV based on the two-layer MPC control strategy are smoother, which can ensure the stable operation of the ESTV and significantly improve the vehicle stability. The uncontrolled vehicle has completely exceeded the stable range under this experimental condition, and there is a risk of instability. From Figure 10, we can see that, compared with the traditional MPC, ${\delta }_{\mathrm{f}}$ based on the two-layer MPC control strategy is smaller and smoother, which avoids excessive steering and ensures the stability of transporting supplies.

5.2.2. Robustness of the Control System to Road Adhesion Conditions

When the ESTV operates on roads with different adhesion conditions (such as dry roads or wet and slippery roads), the dynamic parameters of the vehicle itself, such as tire cornering stiffness, will change, and the lateral force provided by the ground will also be insufficient. The adhesion coefficient of the vehicle is $\mu$ = 0.7∼1 on the dry driving road and about $\mu =0.4$ on the slippery road. Therefore, two road conditions with $\mu$ of 0.4 and 0.8 are adopted for experiments. The vehicle operating velocity is set to 15 m/s. The simulation results are shown in Figure 11, Figure 12, Figure 13 and Figure 14.

Figure 11 shows the trajectory of the ESTV, and it reflects that the controller can operate along the reference trajectory on the road with different adhesion conditions, but the tracking error of ESTV is smaller when the adhesion condition is good. The ground cannot provide sufficient lateral force, and the yaw angle will have a large deviation when the vehicle is turning under the condition of poor road adhesion; however, the controller can correct the deviation in a timely manner, and finally the error tends to 0. Figure 12 is the yaw rate and sideslip angle of the vehicle. It reflects that the $\gamma$ and $\beta$ will increase when the ESTV operates on a road with adhesion conditions that are poor but still within a certain range, reflecting that the controller can maintain the stability of the ESTV under low adhesion conditions. Figure 13 shows the front wheel steering angle of ESTV, and it can be seen that, when the road adhesion condition is poor, ${\delta }_{\mathrm{f}}$ of the ESTV is still within the constraint range, which ensures the normal operation of the actuator. Figure 14 is the drive torque of the vehicle, and it can be seen that there is no drive torque when there is no steering signal input. The MPC torque distribution control strategy can effectively maintain the yaw moment of the upper-layer yaw stability, and each motor torque is within the constraint, ensuring that the ESTV can operate smoothly and safely while reducing energy loss.

5.2.3. Robustness of the Control System to Operating Velocity

The control algorithm needs to determine different control parameters for different operating velocities, while the MPC has strong robustness to the change in operating velocity. In this section, different operating velocities are used to achieve stable control of the ESTV under the same conditions, and the robustness of the controller to different operating velocities is analyzed.

The simulation experiments are conducted at the velocities of 10 m/s, 15 m/s, and 20 m/s, respectively, and the road adhesion coefficient is 0.8. The simulation results are shown in Figure 15, Figure 16, Figure 17, Figure 18 and Figure 19.

Figure 15 is the trajectory of the vehicle at three different operating velocities. The simulation results verify that the vehicle, operating at different velocities, has good trajectory tracking performance, reflecting the robustness to velocity. Figure 16 shows the ESTV’s yaw rate and sideslip angle. It reflects that the ESTV, operating at different velocities, has good yaw stability under the same control parameters, reflecting that the two-layer MPC controller has strong robustness to velocity. Figure 17 is the front-wheel steering angle, and it can be seen that the increment of the control variable is different at different velocities, but always within the constraint range. Figure 18 is the drive torque of the two front wheels of the ESTV, and Figure 19 is the drive torque of the two rear wheels of the ESTV. It reflects that the higher the ESTV velocity, the greater the drive force obtained by the wheels. The vehicle can flexibly obtain the optimal value of the drive torque during operation, which improves the mobility and yaw stability of the vehicle. The designed controller can maintain good stability at different operating velocities.

6. Conclusions

Aiming at the problem of yaw stability control of an ESTV, the improved SHAEKF algorithm and two-layer MPC method are adopted to realize the yaw stability control of a four-wheel-drive ESTV. The improved SHAEKF is used to obtain the estimated state of the system under external disturbances and measurement noise, which improves the accuracy of the state variables. The two-layer MPC is used to realize the yaw stability control of the ESTV. The upper-layer yaw stability controller solves the additional yaw moment and the front wheel steering angle of ESTV, and the lower-layer torque distribution controller optimizes the torque distribution of the four tires of the vehicle. It improves the control accuracy, ensures the stability of the ESTV, and reduces the computational burden of the controller. The simulation experiments verify that the improved SHAEKF can effectively estimate the state variables, and the yaw stability controller based on the two-layer MPC can prevent instability and maintain the stability of the vehicle.

The control method proposed in this paper has a positive effect on improving the stability of the emergency supplies transportation vehicle, but it still needs to be improved. This paper does not consider its inherent complex nonlinear characteristics and only considers the different road adhesion conditions and different velocities for the operation of the ESTV. Further, it does not consider the influence of road inclination and curvature on the yaw stability control. Therefore, in the future work, we will continue to improve the controller design to further expand its application range.