Improved Kalman-Filter-Based Model-Predictive Control Method for Trajectory Tracking of Automatic Straddle Carriers

Abstract

This paper considers the trajectory tracking problem for a class of automatic straddle carriers (ASCs) with external interferences and the overturning constraints. First, the steering and the dynamics of the ASC are analyzed and the mathematical model of the ASC is established. Then, considering the impact of external interferences on the trajectory tracking accuracy, an improved dynamic Kalman filter is designed in order to obtain the state estimation of the ASC. Based on the obtained state estimation, a model-predictive control method is proposed which takes the anti-overturning constraints into account. In addition, the improved Kalman-filter-based model-predictive control (iKFMPC) algorithm is summarized for the considered ASC to travel smoothly along the given trajectory while meeting the overturning resistance. Finally, simulation analyses demonstrate the effectiveness and superiority of the proposed method.

1. Introduction

With the rise of economic globalization, the volume of cargo transportation in the world’s major ports is constantly rising, along with the demand for the efficiency and safety performance of transportation means. As a type of horizontal transport equipment commonly used in automated terminals, the automatic straddle carrier (ASC) can significantly improve the efficiency compared to unmanned guided vehicles, which has attracted the attention of many experts and scholars [1,2,3]. In [4], a stability criterion was proposed to ensure the ASC does not roll over during the turning process, and a method was designed to determine the critical speed of rollover under a given turning radius. In [5], the problem of determining fleet size in tactical planning was solved, and the fleet size for automatic straddle carrier horizontal container transportation was determined. In the research and development of the ASCs, it is important to ensure the smooth running of the cargo to the destination [6,7,8,9,10]. At the same time, the interference caused by external noise is an inevitable problem for the ASC. Settling this problem represents a key point to ensure the stability of cargo transportation [11,12]. In the actual operation, the ASC is prone to overturn due to its large body, high frame, and high mass center. Therefore, it is of great significance to ensure the stability of transportation of the automated terminals by eliminating the noise and studying the trajectory tracking control algorithm under the premise of ensuring the anti-overturning of the ASC, so as to enable the ASC to travel smoothly along the desired trajectory.

Research on trajectory tracking has been a hot and difficult area in unmanned technology [13]. The authors of [14] investigated actuator failures based on adaptive fault-tolerant tracking control and established an actuator kinematic model to address the actual problem that actuators can cause failures during operation. The adaptive fault-tolerant tracking control method was adopted to adjust the deviation between the current position of the vehicle and the planned trajectory in order to enable the vehicle to track the preset trajectory, which increases the system robustness. However, the control accuracy was insufficient. Reference [15] took advantage of the proportional integral derivative (PID) trajectory tracking control algorithm to select the local trajectory in the complex driving environment for complex trajectory tracking problems of unmanned vehicles. In this way, the optimal reference trajectory was generated and the tracking control could be performed while realizing trajectory planning, which can ensure the trajectory tracking accuracy of unmanned vehicles in a variety of complex scenarios. In practical systems, most of them are nonlinear systems; however, for nonlinear systems, the accuracy of the PID control algorithm is reduced. The authors of [16] established a complex multibody system to ensure trajectory tracking accuracy. As such, the optimal tracking control of nonlinear dynamic systems was adopted to find the optimal control strategy by satisfying certain constraints in [17], which made the system more stable and accurate.

In recent years, the model-predictive control (MPC) method [18,19,20,21] has been widely used in the tracking control problem of unmanned systems since it has good control effect and robustness, which can effectively overcome the uncertainty, nonlinearity, and parallelism of the system and satisfy various constraints [22]. The authors of [23] optimized dynamic systems based on MPC to improve system robustness. Compared to the stability problem without terminal constraints, a model-based predictive control algorithm for stability without terminal constraints was proposed by linearizing the nonlinear system using a linear error model, as the prediction model was proposed in [24]. For the nonlinear systems, in order to deal with the conversion error from nonlinear system to linear system, a real-time iteration was proposed in [25] that can be performed to compensate the conversion error. Through the analysis for the MPC linearized conversion system, it was found that the optimal control sequence can be achieved through the MPC strategy. Therefore, the active steering system of the self-driving car with high control accuracy and stable steering was realized by using the MPC method in [26].

However, most of the existing studies on trajectory tracking control are on low-speed unmanned vehicles. In addition, the impact of the shape and size of the vehicles, similar to the ASC, on the designing of the control method is often overlooked. In recent years, some scholars carried out research on the lateral stability of vehicles [27,28]. For example, for the anti-rollover problem of cross-seat monorail vehicles, improving its anti-rollover capability represents the key problem, with the critical side tilt angle being considered to guarantee the stability of the vehicle [29]. Under special circumstances, more constraints need to be taken into account to ensure the vehicle’s operational stability. Thus, based on the virtual prototype, constraints were established for stability dynamics simulation [30]. Given the actual path, the road may not be flat. If the vehicle encounters a slope while driving, the vehicle is more likely to roll over. As such, it is necessary to ensure that the vehicle can be stable even on a slope [31]. However, in the studies on vehicle rollover problems, the barycenter considered by most scholars is relatively low, which is not suitable for the ASC system, because an ASC normally has a large mass and a high barycenter. This is one of the motivations of the current study.

In addition, in the practical port scenario, the harsh environment may lead to external interferences. Filtering is one of the methods used to remove the perturbations [32,33,34,35,36,37,38]. The authors of [39] designed a Kalman filter sample complexity calculation algorithm for unknown systems. A compression sensor based on Kalman filtering was designed to overcome noise issues and increase accuracy in [40]. For the Kalman-based MPC methods, the authors of [41] proposed a conventional model-predictive controller using a static Kalman filter to estimate the state in the trajectory tracking process, which leads to errors in the tracking accuracy. This motivated the study of a dynamic Kalman filter which has two gain matrices that are continuously updated to adapt to the ASC system so as to reduce the tracking error.

As discussed above, this paper investigates the trajectory tracking problems of a class of eight-wheel independently steered ASCs and designs an improved Kalman-filter-based model-predictive control (iKFMPC) algorithm by considering the anti-overturning constraint. The main contributions are concluded as follows:

(1)

Considering the structural characteristics of the ASC, the steering and dynamics of the ASC are analyzed and the mathematical model is established according to Newton’s second law of motion and D’Alembert’s principle.

(2)

An improved dynamic Kalman filter algorithm is proposed to compensate for the process and measurement noise and to provide the system state estimation for the considered ASC.

(3)

Considering the anti-overturning constraint, the objective function is optimized and the iKFMPC algorithm is designed to ensure smooth operation of the ASC and accurate trajectory tracking.

2. Problem Formulation

In this paper, an eight-wheel independently steered ASC is considered, whose dynamical model [40] is shown in Figure 1. $\delta (rad)$ is the wheel turning angle, $\varphi (rad)$ is the lateral tilt angle, x, y, and z are the x, y, and z coordinates, respectively, o is the origin center, $m(kg)$ is the overall vehicle mass, $v(m/s)$ is the vehicle speed, $g$ is the gravitational acceleration, $h(m)$ is the barycenter height, $a_y(m/s^2)$ is the lateral acceleration, $\beta (rad)$ is the sideslip angle, $\gamma (rad)$ is the yaw rate, $B(m)$ is the axle distance, and $L_i(i=1,\dots ,4,m)$ is the distance from the center of the wheel to the turning center of the ASC.

Note that, in this paper, we assume that the considered ASC has a full load. The inertia product $I_{xz}=0$ is obtained by taking into account the structural symmetry of the carrier. The influence of the cab weight and the inertia product of the axes can therefore be ignored. Thus, based on Newton’s second law of motion and D’Alembert’s principle, the following equilibrium equations for the dynamics of the ASC are established and obtained from Figure 1.

$$\{\begin{array}{c}m{a}_y-mh\ddot{\phi }=F_y\\ I_z\dot{\gamma }-I_{xz}\ddot{\phi }=M_z\\ I_{xeq}\ddot{\phi }-I_{xz}\gamma =M_x\\ a_y=v\times (\dot{\beta }+\gamma )\end{array},$$

(1)

where $F_y,M_z,M_x$ satisfy the following equation:

$$\{\begin{array}{c}{F}_y={\displaystyle \sum _{i=1}^4{F}_{li}+{\displaystyle \sum _{i=1}^4{F}_{ri}}}\\ M_z={\displaystyle \sum _{i=1}^2{l}_i(F_{li}+F_{ri})-{\displaystyle \sum _{i=3}^4{l}_i(F_{li}+F_{ri})}}\\ M_x=mhv(\dot{\beta }+\gamma )\cos \varphi +mgh\sin \varphi -C_{\varphi }\dot{\varphi }-K_{\varphi }\varphi \end{array},$$

(2)

where $K_{\varphi }$ is the suspension tire equivalent elasticity coefficient, $C_{\varphi }$ is the suspension tire equivalent damping coefficient, $I_x$ is the rotational inertia of the ASC’s mass around the x-axis, $I_z$ is the rotational inertia of the straddle carrier mass around the z-axis, $F_{li}$ is the left-side tire deflection in row $i$, $F_{ri}$ is the right-side tire deflection in row $i$, $F_y$ is the side slanting force of the y-axis, and $M_x,M_z$ is the torque of the x- and z-axes, respectively.

From the parallel axis theorem, the relationship between the mass of the ASC and rotational inertia of the side tilting axis, as well as the relationship between the mass of the ASC and the rotational inertia of the x-axis, are as follows: $I_{xeq}=I_x+m{h}^2$. Since the side tilt angle is generally small during the turning process, we have $\sin \phi \approx \phi ,\cos \phi =1$. In summary, the model of the considered ASC is further obtained as follows:

$$\{\begin{array}{l}\dot{x}=v\times \cos (\theta +\beta )\hfill \\ \dot{y}=v\times \sin (\theta +\beta )\hfill \\ \dot{\theta }=\gamma \hfill \\ mv(\dot{\beta }+\gamma )-mh\ddot{\phi }+D_1\beta +\frac{D_2}{v}\gamma -U_1=0\hfill \\ I_z\dot{\gamma }+D_2\beta +\frac{D_2}{v}\gamma -U_2=0\hfill \\ I_{xeq}\ddot{\phi }-mv(\dot{\beta }+\gamma )h-mgh\phi +K_{\varphi }\phi +C_{\phi }\dot{\phi }=0\hfill \end{array},$$

(3)

where $x,y$ are the position coordinates of the $X,Y$ axis of the ASC, and $\theta$ is the yaw angle.

We select the state variable as $\tilde{x}={\left[\begin{array}{ccccccc}x& y& \theta & \beta & \gamma & \varphi & \dot{\varphi }\end{array}\right]}^T$ and define the control quantity $\delta ={\left[\begin{array}{ccccccccc}v& {\delta }_{l1}& {\delta }_{l2}& {\delta }_{l3}& {\delta }_{l4}& {\delta }_{r1}& {\delta }_{r2}& {\delta }_{r3}& {\delta }_{r4}\end{array}\right]}^T$ (${\delta }_{li}$ is the left wheel angle of the ASC and ${\delta }_{\mathrm{ri}}$ is the right wheel angle ($i=1,2,3,4$)). As such, Equation (3) can be transformed as follows:

$$\{\begin{array}{l}\dot{\tilde{x}}=A\tilde{x}+B\delta \hfill \\ \tilde{y}=C\tilde{x}\hfill \end{array},$$

(4)

where $\tilde{y}$ is the output and A, B, and C are shown in (5).

$$\begin{gathered} A=\left[\begin{array}{ccccccc}0& 0& 0& -v*\sin ({\theta }_r)& 0& 0& 0\\ 0& 0& 0& v*\cos ({\theta }_r)& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& -\frac{D_1{I}_{xeq}}{mv{I}_x}& -\frac{D_2{I}_{xeq}}{m{v}^2{I}_x}-1& \frac{h(mgh-K_{\phi })}{I_{x}v}& \frac{-h{C}_{\phi }}{I_{x}v}\\ 0& 0& 0& -\frac{D_2}{I_z}& -\frac{D_3}{I_z}& 0& 0\\ 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& -\frac{D_{1}h}{I_x}& -\frac{D_{2}h}{I_{x}v}& \frac{mgh-K_{\phi }}{I_x}& -\frac{C_{\phi }}{I_x}\end{array}\right],C=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right], \\ B=\left[\begin{array}{ccccccccc}\cos ({\theta }_r)& 0& 0& 0& 0& 0& 0& 0& 0\\ \sin ({\theta }_r)& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& \frac{K_{l1}{I}_{xeq}}{mv{I}_x}& \frac{K_{l2}{I}_{xeq}}{mv{I}_x}& \frac{K_{l3}{I}_{xeq}}{mv{I}_x}& \frac{K_{l4}{I}_{xeq}}{mv{I}_x}& \frac{K_{r1}{I}_{xeq}}{mv{I}_x}& \frac{K_{r2}{I}_{xeq}}{mv{I}_x}& \frac{K_{r3}{I}_{xeq}}{mv{I}_x}& \frac{K_{r4}{I}_{xeq}}{mv{I}_x}\\ 0& \frac{K_{l1}{l}_1}{I_z}& \frac{K_{l2}{l}_2}{I_z}& -\frac{K_{l3}{l}_3}{I_z}& -\frac{K_{l4}{l}_4}{I_z}& \frac{K_{r1}{l}_1}{I_z}& \frac{K_{r2}{l}_2}{I_z}& -\frac{K_{r3}{l}_3}{I_z}& -\frac{K_{r4}{l}_4}{I_z}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& \frac{K_{l1}h}{I_x}& \frac{K_{l2}h}{I_x}& \frac{K_{l3}h}{I_x}& \frac{K_{l4}h}{I_x}& \frac{K_{r1}h}{I_x}& \frac{K_{r2}h}{I_x}& \frac{K_{r3}h}{I_x}& \frac{K_{r4}h}{I_x}\end{array}\right], \end{gathered}$$

(5)

In addition, by considering the process noise and the measurement noise, the model of the ASC can be further discretized as follows:

$$\{\begin{array}{l}\tilde{x}(k+1)=A_p\tilde{x}(k)+B_p\delta (k)+\omega (k),\hfill \\ \tilde{y}(k)=C_p\tilde{x}(k)+v(k),\hfill \end{array}$$

(6)

where $\omega (k)$ is the process noise and $v(k)$ is the measurement noise.

$$A_p=e^{A{T}_s},B_p={\displaystyle {\int }_0^{T_s}B{e}^{A\tau }}d\tau ,C_p=C,$$

(7)

where $T_s$ is the sampling period.

Based on the above system model, this paper aims to design a model-predictive controller based on the improved dynamic Kalman filter that regards the side tilt angle as a constraint to address the problem of noise and unstable trajectory tracking caused by sideways tilting. The designed ASC trajectory tracking control scheme is shown in Figure 2, the dashed box of which represents the main part of the proposed model-predictive control scheme. It is mainly composed of the linear error model, constraint condition, and objective function.

Note that, as shown in Figure 2, the improved Kalman filter is designed to compensate for noise, disturbances, and environmental factors in the actual driving of the considered ASC, reducing the impact of noise on the normal driving. In addition, the filtering of the actual measured values of the ASC can effectively avoid measurement noise or measurement fluctuations from acting too frequently on the model-predictive controller, prevent the control sequence obtained by the optimal solution from being too aggressive, thus resulting in larger system overshoot, and further improve the robustness of the automatic straddle carrier system as well as its anti-interference capability [42]. The control limit constraint and the control increment constraint are also taken into account to ensure that the ASC can track the reference trajectory quickly and smoothly. Then, the system constraints can be improved with designed suitable objective function, taking into account the anti-overturning constraint. Finally, the objective function is converted into a standard quadratic form with the constraints being considered to optimize the solution and obtain the optimal control sequence to achieve accurate trajectory tracking.

3. Design of the Improved Kalman Filter

Traditional static Kalman filtering only requires direct observation of the system and uses these observation data to estimate the system state. The state of the ASC is constantly changing, so the improved Kalman filter designed in this article needs to meet this dynamic characteristic. This article continuously updates the $L_k$ and $M_k$ matrices at each sampling time. Based on the state of $k-1$ at the previous time and the current observation value, it predicts and updates $k$ at each time to obtain the state estimation of the current $k$ time.

In the ASC system, not all of the state quantities in the controller are measurable, or the ASC may have measurement noise; thus, state estimation or filtering are required in the control process. By default, conventional MPCs use a static Kalman filter (KF) to estimate the state in the trajectory tracking controller, which leads to errors in tracking accuracy.

In this paper, we suppose that at the time step $k$, $\widehat{\tilde{x}}(k-1)$ is the known state estimation of $\tilde{x}(k-1)$. The following improved Kalman filter is first designed for updating the state estimation $\widehat{\tilde{x}}(k)$ for the true state $\tilde{x}(k)$.

$$\{\begin{array}{l}\widehat{\tilde{x}}(k)=\widehat{\tilde{x}}(k|k-1)+M_{k}e(k),\hfill \\ \widehat{\tilde{x}}(k|k-1)=A_p\widehat{\tilde{x}}(k-1)+B_p\delta (k-1)+L_{k}e(k),\hfill \end{array}$$

(8)

where $M_k$ and $L_k$ are the gain matrices of the filter needing to be designed, and $e(k)$ is the evaluated error, which is defined as

$$e(k)=\tilde{y}(k)-[C_p\widehat{\tilde{x}}(k|k-1)+v(k)].$$

(9)

By defining the state estimation error covariance matrix as

$$P(k)=\mathrm{{\rm E}}\{[\tilde{x}(k)-\widehat{\tilde{x}}(k)]{[\tilde{x}(k)-\widehat{\tilde{x}}(k)]}^T)\}.$$

(10)

and the one-step ahead estate estimation error covariance matrix is

$$P(k|k-1)=\mathrm{{\rm E}}\{[\tilde{x}(k)-\widehat{\tilde{x}}(k|k-1)][\tilde{x}(k)-\widehat{\tilde{x}}{(k|k-1)}^T]\}.$$

(11)

Then, by substituting (6) and (8) into (12), one can have

$$\begin{array}{l}P(k)=\mathrm{{\rm E}}\{(\tilde{x}(k)-\widehat{\tilde{x}}(k))(\tilde{x}(k)-\widehat{\tilde{x}}{(k)}^T\}\\ =\mathrm{{\rm E}}\{(A_p(\tilde{x}(k-1)-\widehat{\tilde{x}}(k-1))-L_k{C}_p(\tilde{x}(k)-\widehat{\tilde{x}}(k|k-1))-L_{k}v(k)+\omega (k))\\ \times (A_p(\tilde{x}(k-1)-\widehat{\tilde{x}}(k-1))-(L_k+M_k)C_p(\tilde{x}(k)-\widehat{\tilde{x}}(k|k-1))-(L_k+M_k)v(k)+\omega (k-1))T\}\\ =A_{p}P(k-1)A_p^T-(L_k+M_k)C_{p}P(k|k-1)C_p{(L_k+M_k)}^T-(L_k+M_k)R_k{(L_k+M_k)}^T+Q_{k-1}\end{array}$$

(12)

where $Q_{k-1}=\mathrm{{\rm E}}\{w(k-1)w{(k-1)}^T\}$. $R_k=\mathrm{{\rm E}}\{v(k)v{(k)}^T\}$ and $Q_{k-1}=\mathrm{{\rm E}}\{w(k-1)w{(k-1)}^T\}$ are the process and measurement noise covariance matrix, respectively.

According to optimal criterion of minimizing the error covariance variance $P_k$, we can obtain the following:

$$\{\begin{array}{c}{L}_k=(A_p{P}_{k|k-1}{C}_p^T+Q_{k-1}){(C_p{P}_{k|k-1}{C}_p^T+R_k)}^{-1},\\ M_k=P_{k|k-1}{C}_p^T{(C_p{P}_{k|k-1}{C}_p^T+R_k)}^{-1},\end{array}$$

(13)

4. Design of iKFMPC

For the discrete state equation of the ASC, the controlled variable $\delta (k)$ is converted to $\Delta \delta (k)$. A new state vector $\xi (k|t)=\left[\begin{array}{cc}\tilde{x}(k)& \delta {(k-1)}^T\end{array}\right]$ is constructed to obtain the new system state space expression as

$$\{\begin{array}{l}\xi (k+1)={\tilde{A}}_k\xi (k)+{\tilde{B}}_k\Delta \delta (k)+w(k),\hfill \\ y(k)={\tilde{C}}_k\xi (k)+v(k),\hfill \end{array}$$

(14)

where

$${\tilde{A}}_k=\left[\begin{array}{cc}{A}_p& B_P\\ 0_{m\times n}& I_m\end{array}\right],{\tilde{B}}_k=\left[\begin{array}{c}{B}_p\\ I_m\end{array}\right],{\tilde{C}}_k=\left[\begin{array}{cc}{C}_p& 0\end{array}\right],$$

(15)

The output of the system at time step $k$ can be obtained by iterative solution for the ASC system as follows:

$$y(k)={\tilde{C}}_k{\displaystyle \prod _{i=1}^{k-1}{\tilde{A}}_i\xi (k)+{\tilde{C}}_k{\displaystyle \prod _{i=1}^{k-1}{\tilde{B}}_i\Delta \delta (k)+{\tilde{C}}_k{\displaystyle \prod _{i=1}^{k-1}(w(k)+v(k)}}}).$$

(16)

In this paper, we assume that the predictive time domain of the system model-predictive controller is $N_p$ and the control time domain is $N_c$. Based on Equation (16), the system output expression in the predictive time domain can be obtained as follows:

$$Y(k)={\mathrm{\Psi }}_k\tilde{\xi }(k)+{\mathrm{\Theta }}_k\Delta \delta (k)+\kappa \varphi (k)+S(k),$$

(17)

where $Y(k)$ is the system output, ${\mathrm{\Psi }}_k,{\mathrm{\Theta }}_k,\kappa$ are the parameter matrices, $\varphi (k)$ is the input deviation, and $S(k)$ is the system prediction deviation value. Details are as follows:

$$\begin{gathered} {\mathrm{\Theta }}_k=\left[\begin{array}{cccc}{\tilde{C}}_{k+1}{\tilde{B}}_k& 0& 0& 0\\ {\tilde{C}}_{k+2}{\tilde{A}}_k{\tilde{B}}_k& {\tilde{C}}_{k+2}{\tilde{B}}_{k+1}& 0& 0\\ ⋮& ⋮& ⋮& ⋮\\ {\tilde{C}}_{k+N_p}{\displaystyle \prod _{i=k+1}^{k+N_p-1}{\tilde{A}}_k{\tilde{B}}_k}& {\tilde{C}}_{k+N_p}{\displaystyle \prod _{i=k+1}^{k+N_p-1}{\tilde{A}}_k{\tilde{B}}_{k+1}}& \cdots & {\tilde{C}}_{k+N_p}{\displaystyle \prod _{i=k+1}^{k+N_p-1}{\tilde{A}}_k{\tilde{B}}_{k+N_p-1}}\end{array}\right], \\ \begin{array}{c}Y(k)=\left[\begin{array}{c}y(k+1)\\ ⋮\\ y(k+N_c)\\ ⋮\\ y(k+N_p)\end{array}\right],\phi (k)=\left[\begin{array}{c}w(k+1)\\ ⋮\\ w(k+N_c)\\ ⋮\\ w(k+N_p)\end{array}\right],S(k)=\left[\begin{array}{c}v(k+1)\\ ⋮\\ v(k+N_p)\end{array}\right],\\ \kappa =\left[\begin{array}{cccc}{\tilde{C}}_{k+1}& 0& 0& 0\\ {\tilde{C}}_{k+2}& {\tilde{C}}_{k+2}& 0& 0\\ ⋮& ⋮& ⋮& ⋮\\ {\tilde{C}}_{k+N_p}& {\tilde{C}}_{k+N_p}& {\tilde{C}}_{k+N_p}& {\tilde{C}}_{k+N_p}\end{array}\right],\mathrm{\Psi }(k)=\left[\begin{array}{c}{\tilde{C}}_{k+1}{\tilde{A}}_k\\ {\tilde{C}}_{k+2}{\tilde{A}}_{k+1}{\tilde{A}}_k\\ ⋮\\ {\tilde{C}}_{k+N_p}{\displaystyle \prod _{i=k}^{k+N_p-1}{\tilde{A}}_i}\end{array}\right].\end{array} \end{gathered}$$

In order to ensure the tracking accuracy and anti-overturning capability of the considered ASC, the appropriate optimized objective function needs to be set with the following specific constraints.

(1)

Considering the system’s ability to follow the desired path of the ASC, the cost function $J_1$ is set, where $R(k)$ is the desired path;

(2)

Considering the constraints on the control increment of the system, the cost function $J_2$ is set;

(3)

Considering the optimal travel distance of the automatic straddle carrier, the cost function $J_3$ is set;

(4)

Considering the constraints on the sideslip angle of the automatic straddle carrier, the cost function $J_4$ is set;

(5)

Considering the tilt angle constraint, the cost function $J_5$ is set.

Details are set as follows:

$$\{\begin{array}{c}{J}_1=\left|\right|Y(k)-R(k)|{|}^2,\\ J_2=\left|\right|\Delta \delta (k)|{|}^2,\\ J_3={\displaystyle \sum _{i=1}^{N_P}(||y(k+i)-y(k+i-1)|{|}^2),}\\ J_4={\displaystyle \sum _{i=1}^{N_P}(||\beta (k+i)|{|}^2}),\\ J_5={\displaystyle \sum _{i=1}^{N_P}(||\varphi (k+i)|{|}^2}),\end{array}$$

(18)

By introducing weighting factors, the multiple objective constraint $J$ is set as below.

$$J={\displaystyle \sum _{i=1}^{N_P}(Z_{d,i}}||y(k+i)-y(k+i-1)|{|}^2)+{\displaystyle \sum _{i=1}^{N_P}(Z_{b,i}||}\beta (k+i)|{|}^2)+Z_c||\Delta \delta (k)|{|}^2+Z_a||Y(k)-R(k)|{|}^2+Z_e||\varphi (k+i)|{|}^2+\rho {\epsilon }^2,$$

(19)

where

$$\begin{array}{l}{Z}_a=diag\{Z_{a,1},Z_{a,2},\cdots ,Z_{a,N_P}\}>0,\hfill \\ Z_c=diag\{Z_{c,1},Z_{c,2},\cdots ,Z_{c,N_P}\}>0,\hfill \\ Z_e=diag\{Z_{e,1},Z_{e,2},\cdots ,Z_{e,N_P}\}>0,\hfill \\ Z_{b,i}>0,Z_{d,i}>0,\end{array}$$

(20)

Note that the weighting factors $Z_{a,i},Z_{b,i},Z_{c,i},Z_{d,i},Z_{e,i}$ are determined by the actual situation [43]; $\rho$ is the weight coefficient and $\epsilon$ is the relaxation factor.

In addition, the ASC system must satisfy the following constraints which are the controlled variable constraint, the side tilt angle constraint, and the control increment constraint.

$$\{\begin{array}{l}{\delta }_{\min }\le \delta (k+i)\le {\delta }_{\max },\hfill \\ {\phi }_{\min }\le \phi (k+i)\le {\phi }_{\max },\hfill \\ \Delta u_{\min }\le \Delta \delta (k+i)\le \Delta u_{\max },\hfill \end{array}$$

(21)

The following optimal problem can be concluded for the solving the trajectory tracking problem of the considered ASC system.

$$\min J(\varphi (k),\delta (k-1),\Delta \delta (k)).$$

(22)

We substitute the prediction equation into Formula (22) to form a standard quadratic programming:

$$\underset{J}{\min }\left\{\frac{1}{2}{\left[\begin{array}{c}\Delta \delta \\ \epsilon \end{array}\right]}^T{Q}_w\left[\begin{array}{c}\Delta \delta \\ \epsilon \end{array}\right]+G\left[\begin{array}{c}\Delta \delta \\ \epsilon \end{array}\right]\right\},$$

(23)

where

$$Q_w=\left(\begin{array}{cc}2({\mathrm{\Theta }}_k^T{Z}_d{\mathrm{\Theta }}_k+{\mathrm{\Gamma }}^T{Z}_b\mathrm{\Gamma }+Z_a+Z_c+Z_e)& 0\\ 0& \rho \end{array}\right),$$

(24)

$$G={\left(\begin{array}{c}2({\mathrm{\Xi }}^T{Z}_d{\mathrm{\Theta }}_k+\Delta {\delta }^T{Z}_{b}M)\\ 0\end{array}\right)}^T.$$

(25)

where $Q_w$ is a positive definite Hessian matrix, $\mathrm{\Xi }$ is the predicted time domain tracking error, $M=K\otimes I_m$, $\otimes$ is Crohneck product, $K$ is an $N_c\times N_c$ dimensional matrix, and $I_m$ is an $m$ dimensional matrix.

As discussed above, the following Algorithm 1 can be summarized:

Algorithm 1:

Step 1: Set $t_0,t_{final}$ as the initial moment and end moment; set the initial state $x,y,\theta ,\beta ,\gamma ,\phi ,\dot{\phi }$; set the predictive time domain $N_p$, the controlled time domain $N_c$ and the reference trajectory point $k_N$; make $k=0$.

Step 2: Based on the improved Kalman filter, the state estimate $\widehat{\tilde{x}}$ is obtained by Equations (8) and (13).

Step 3: Based on the state prediction, $\left[k,k+N_p\right]$ in the future time domain is calculated by Equation (17) to output $Y(k)$.

Step 4: Solve Equation (23) (optimal objective function $J$) by the model-predictive algorithm to obtain the control sequence $U$ in the controlled time domain $\left[k,k+N_c\right]$.

Step 5: The first control increment $\Delta u$ is taken as the actual system input until $k=k_N$, then stop the cycle; otherwise, $k=k+1$. Repeat step 3 to achieve the designed algorithm control function through continuous cycle.

5. Simulation Experiments

In order to verify the effectiveness of the algorithm proposed in this paper, simulation experiments were carried out in MATLAB(R2012a). Straight-line motion and small curvature cosine curve motion of the ASC under the condition of anti-overturning constraint were mainly considered. The number of reference trajectory points N was set to 3000, the sampling time was set as 0.02 s, and the observation time $T$ was $60 s$. The desired speed of the carrier was set to $2 m/s$ and the controllable range of speed was set to $\left[\begin{array}{cc}1.8& 2.2\end{array}\right] m/s$. The desired value of the side tilt angle was 0. The controllable range of the steering angle was set as $\left[\begin{array}{cc}-0.64& 0.64\end{array}\right] rad$. The expected values of the yaw angle and the barycenter sideslip angle were 0, and the expected values of the transverse sway angle speed and the lateral tilt angle speed were also 0. The parameters of the ASC for the simulation experiments in paper were based on [43] and are shown in

Table 1. Parameters of the considered ASC.

Parameters	Numerical Values	Unit	Parameters	Numerical Values	Unit
$N_p$	50	_	$N_c$	50	_
$m$	89,000	$kg$	$I_z$	1,052,500	$kg\cdot m^2$
$C_f$	560,000	$N/rad$	$C_r$	460,000	$N/rad$
$l_1$	3.85	$m$	$l_2$	1.95	$m$

.

In order to show the effectiveness and the advantage of the proposed method for the trajectory tracking of the considered AGV system, the traditional Kalman-based MPC method was used as the comparative experiment. Two modes of operation, which are the linear motion and curvilinear motion, were conducted.

5.1. Linear Motion

When the ASC tracks a straight line, the simulation experimental results under the algorithm designed in this paper are shown in Figure 3.

In Figure 3A is the straight-line trajectory tracking diagram of the considered ASC where the horizontal axis is the x-axis coordinate displacement and the vertical axis is the y-axis coordinate displacement. The blue segment is the given straight-line reference trajectory segment, the black segment is the trajectory segment obtained by the proposed iKFMPC method, and the green one is the trajectory result for the Kalman-based MPC method. The simulation results show that the control method proposed in this paper can track the reference trajectory accurately. In addition, by comparing with the traditional Kalman-based MPC method, the trajectory tracking result of the ASC based on the iKFMPC is more stable and quick with less deviation and fluctuation in a very small range. Thus, the accuracy of the proposed method is verified.

In addition, Figure 3B demonstrates the change of the control inputs of the ASC in the linear trajectory tracking. As can be seen from the simulation experiment image, the speed of the carrier fluctuates slightly at the beginning, and then stabilizes at 2 $m/s$, which meets the setting conditions. As the steering angle of the ASC is generally small during the driving process without overly complicated steering, each group of the independent wheels changes little and is close to each other. From the first group of steering wheels, it can be seen that the wheel steering angle of the ASC meets the requirements of the established conditions.

Further, Figure 3C,D demonstrate the change of states, namely, x, y axis, yaw angle, sideslip angle, yaw rate, tilt angle, and tilt angular velocity. According to Figure 3C,D, with the change of observation time, the tilt angle of the ASC fluctuates around 0 $rad$, which is within the expected range of $\left[\begin{array}{cc}-0.64& 0.64\end{array}\right] rad$. It verifies the effective consideration of the tilt angle constraint. For yaw angle, sideslip angle, yaw rate, and tilt angular velocity, the actual deviation is very small and levels off to 0 after a short period of variation, which is in line with the established conditions and ensures the smooth operation of the carrier.

In summary, the experimental results verify the effectiveness of the proposed method for the considered ASC in the straight-line trajectory tracking.

5.2. Curvilinear Motion

In actual driving, the running trajectory of the ASC normally has certain curvature. In order to further verify the effectiveness of the proposed method, the simulation experiment results are shown in Figure 4 when the driving trajectory is set to the cosine curve $20COS(k\times T/20)-20$ ($k=1,2,\dots ,T$).

In Figure 4A is the curve-line trajectory tracking diagram of the considered ASC where the horizontal axis is the x-axis coordinate displacement and the vertical axis is the y-axis coordinate displacement. The green segment is the given straight-line reference trajectory segment, the black segment is the trajectory segment obtained by the proposed iKFMPC method, and the black one is the trajectory result for the Kalman-based MPC method. The simulation results show that the control method proposed in this paper can track the reference trajectory accurately. In addition, by comparing with the traditional Kalman-based MPC method, the trajectory tracking result of the ASC based on the iKFMPC is more stable with less deviation and fluctuation in a very small range. Thus, the accuracy of the proposed method is verified.

In addition, Figure 4B demonstrates the change of the control inputs of the ASC in the curve-line trajectory tracking. As can be seen from the simulation experiment image, the speed of the carrier fluctuates slightly at the beginning, and then stabilizes at 2 $m/s$, which meets the setting conditions. As the steering angle of the ASC is generally small during the driving process without overly complicated steering, each group of the independent wheels changes little and is close to each other. From the first group of steering wheels, this is consistent with the reference trajectory. It can be seen that the wheel steering angle of the ASC also meets the requirements of the established conditions.

Further, Figure 4C,D demonstrate the change of states, namely, x, y axis, yaw angle, sideslip angle, yaw rate, tilt angle, and tilt angular velocity. According to Figure 4C,D, with the change of observation time, the tilt angle of the ASC fluctuates around 0 $rad$, which is within the expected range of $\left[\begin{array}{cc}-0.64& 0.64\end{array}\right] rad$. It verifies the effective consideration of the tilt angle constraint. For yaw angle, sideslip angle, yaw rate, and tilt angular velocity, the actual deviation is very small and levels off to 0 after a short period of variation, which is in line with the established conditions and ensures the smooth operation of the carrier.

In summary, the experimental results verify the effectiveness of the proposed method for the considered ASC in the curve-line trajectory tracking.

6. Conclusions

This paper designs the trajectory tracking algorithm for a class of ASC subjected to the external interferences and the overturning constraints. The simulation in MATLAB verifies that the ASC can accurately follow the established trajectory and maintain fine accuracy and stability in different trajectories, which verifies the effectiveness of the iKFMPC designed in this paper. This provides a reference for the trajectory tracking research of ASCs.