Research on the Model Predictive Trajectory Tracking Control of Unmanned Ground Tracked Vehicles

Abstract

This article summarizes the research significance and the development status of the unmanned ground tracked vehicles (UGTVs). According to the speed and steering principle of the UGTVs in plane motion, the kinematic state space equation of the vehicle is established. Based on the model predictive control (MPC), the UGTVs trajectory tracking controller is also established. After introducing the overall control system solution, based on the vehicle model, the track speed on both sides is used as the control input to solve the predicted output of the system. After constraining the control amount, the model prediction controller is established by using S function. Several simulations with different preset speeds are performed under linear conditions, and the numerical simulation with different prediction time domains and control time domains are performed under continuous curves. The test validates the effectiveness of the controller, and the effects of speed and time domain parameters on the deviation are analyzed based on the results. Based on satellite positioning technology, trajectory tracking tests of the UGTVs are carried out. After analyzing the positioning principle and common errors, the real-time kinematic technology is used to improve the positioning accuracy of the vehicle prototype. The hardware of the test platform includes a data acquisition system, a control execution system and a walking execution system, and the software part includes data processing and optimization control. The trajectory tracking experiments under different driving conditions are conducted, and the results show that the navigation tracking system can achieve good tracking performance.

1. Introduction

Tracked mine vehicles are usually large in volume. When the running track is deviated, the adjustment is difficult and takes a long time. In addition, its operation relies heavily on the experience level and proficiency of drivers. Statistics show that more than 80% of mine equipment accidents are caused by personnel’s operation errors [1]. Therefore, it is very important to adopt efficient and stable tracking control methods in UGTVs and equip them with high-precision positioning equipment to ensure their engineering work.

In UGTVs control, the control that enables the UGTVs to reach the target position from any initial position is called position control. The path that the UGTVs passes through is not required in position control. If the UGTVs are required to follow a specific path in position control, such continuous control is called path tracking control. In the field of trajectory tracking, relevant scholars have experimented with UGTVs, aircraft, underwater vessels and other directions. Liu K. et al. proposed a composite controller scheme that can overcome external disturbances; an observer based on adaptive sliding mode control was used to predict disturbances in real time, and a neurodynamic model was used to generate smooth signals to overcome disturbances [2]. A. Pedro Aguiar et al. proposed a trajectory tracking method for an unmanned ground vehicle (UGV) that is not accurate enough for parametric modeling [3]. The tracking error was reduced by combining adaptive switching supervisory control and nonlinear tracking control based on Lyapunov. Simulation experiments in 2D and 3D verified that the algorithm has a good effect. Lisa Fiorentini et al. designed the state feedback controller by re-establishing the internal dynamics control model and combining small gain parameters and adaptive control, so as to realize the progressive tracking of a specific type of aircraft under model uncertainty [4]. Jian Xu et al. proposed an adaptive dynamic sliding mode control method for the trajectory tracking of an unmanned underwater vehicle (UUV) to improve the robustness under disturbance [5]. JunZhang Li et al. designed an adaptive output feedback trajectory tracking control for surface vessels [6]. By considering the coupling effect of multiple degrees of freedom directional forces and the nonlinear characteristics of damping, accurate tracking of vessels under high-speed conditions could be achieved. Hang et al. designed a linear variable parameter controller for path tracking, and the Weighted Least Squares (WLS) algorithm was used to allocate the required torque of the wheels on both sides to improve tracking performance [7]. Ting et al. used a biaxial strapdown accelerometer to estimate the position heading of the UGTV and the angular velocity of its geometric center, the improved PID (proportional–integral–derivative) control was used to track the trajectory [8]. Zhao et al. proposed a six-parameter slip parameter estimation method, and then established a trajectory tracking controller based on the slip kinematics model, which was verified under off-road conditions [9]. Liu et al. developed a robust fault-tolerant tracking control scheme for a quadrotor Unmanned Aerial Vehicle (UAV). The scheme was based on a fixed-time perturbation observer, enabling the UAV to track a predefined path effectively, even in the presence of model uncertainties, external perturbations, actuator failures, and input delays [10].

Predictive control was first proposed in the late 1970s. It uses impulse response or step response model as the prediction model, which belongs to the heuristic control algorithm based on model prediction. Shell first applied it to control the refining process, and in the subsequent development, model algorithm control (MAC), dynamic matrix control (DMC), generalized predictive control (GPC), state space model predictive control (SSPC) and generalized predictive pole assignment control (GPP) appeared successively. Compared with other control methods, MPC has many control advantages, such as being able to efficiently handle multi-input and multi-output control systems, dealing with systems in which input and output interact, effectively handling multi-constraint situations, and having predictive ability, etc. Today, MPC is used in chemical, metallurgical, papermaking, cement and other industrial processes [11,12].

From the perspective of the types of UGV, the MPC can be applied to cars, trucks, tractor-trailer, UGTVs, underwater vehicles and other vehicle types. From the perspective of application scope, it includes steering control, trajectory tracking control, multi-vehicle coordination control, lane change control and other fields. Jesus Felez et al. applied MPC to the control of an unmanned articulated truck driving on an electric motorized highway [13]. After establishing the electric powertrain model of the vehicle, simulation tests such as adaptive cruise control, trajectory tracking and overtaking control under safety conditions were carried out. Hyunsik Nam et al. built a vehicle steering system model that included interference, actuator bandwidth and motor voltage. Taking tracking error as input, Hyunsik Nam calculated the steering angle within the constraint of voltage range and carried out avoidance steering control of a UGV [14]. Chao Shen et al. applied MPC based on Lyapunov to a UUV to enhance trajectory tracking performance on the basis of ensuring actuator saturation and thrust allocation [15]. Jianqiang W et al. used MPC to design a control scheme to reduce longitudinal collisions by coordinating the braking of multiple vehicles [16]. Xu Wang et al. proposed an adaptive Min-Max MPC when dealing with the track–trailer path tracking problem with skidding considered [17]. Compared with sliding mode control, this method had better robustness and accuracy. Zhang et al. proposed an MPC controller that integrated local trajectory planning and trajectory tracking and discussed the influence of parameters such as the error weighting matrix on the tracking effect [18]. Ming Yue et al. proposed a coordinated control method for tractor–trailer trajectory tracking under vehicle kinematic constraints and dynamic constraints, applied MPC to vehicle attitude control and applied sliding mode control to speed control [19]. The simulation results showed that this coordinated control method had a better control effect. Liu et al. introduced an anti-saturation adaptive fixed-time sliding mode controller for second-order nonlinear systems subject to saturation constraints [20]. They also developed a novel non-singular fast time sliding mode surface. The proposed method was then compared to quadrotor UAV attitude stabilization through simulations and experiments. The results demonstrated the method’s capability to achieve faster convergence rates and excellent control performance.

UGTVs encounter numerous challenges in trajectory tracking compared to other traveling systems such as wheeled vehicles, UUVs, and UAVs. These challenges encompass difficulties in measuring track–ground slip, complex nonlinear dynamics and the effects of uncertain external disturbances. Conventional control methods struggle to precisely track the desired trajectory due to these factors. To tackle these issues, it is crucial to develop and optimize tailored control methods for UGTVs by integrating sensor technologies and advanced control strategies, thereby enhancing trajectory tracking accuracy and robustness.

This paper presents a trajectory tracking control method for UGTVs, which effectively mitigates the adverse effects of track slippage and handles the complexity of trajectory tracking control in the presence of dynamic challenges. The proposed method achieves accurate tracking of the preset trajectory for UGTVs. Based on the walking theory and motion state space equation of UGTVs, we establish a trajectory tracking control model for UGTVs using the MPC. The predictive output of the system is obtained by taking the track velocity on both sides as the control input, and the model predictive controller is established by using S function after constraining the control quantity. Simulations of linear operating conditions with different speeds and numerical simulation of continuous curves with different predictive time domain and control time domain were carried out to verify the effectiveness of the controller and analyze the influence of speed and time domain parameters on the deviation. Based on RTK positioning technology, a trajectory tracking test of a UGTV was carried out. The hardware of the test platform included a data acquisition system, control execution system and walking execution system, and the software included a data processing and optimization control part. The trajectory tracking tests of UGTVs under different driving conditions show that the model predictive tracking system can achieve a better tracking effect.

2. Kinematics Model of UGTVs

When analyzing the plane motion e of a UGTV, two coordinate systems are established, which are global coordinate system XOY and vehicle local coordinate system X_CO_CY_C, respectively. The origin O of the global coordinate system is fixed at a point on the Earth, and the position and direction of the coordinate system do not change during the UGTV’s movement. The origin of the vehicle local coordinate system coincides with the centroid position of the UGTV, and the positive direction of O_CX_C is the forward direction of the UGTV, while the position and direction of the coordinate system vary with the movement of the UGTV.

At the initial moment, the geometric center O_C of the UGTV is located at point O of the global coordinate system with a difference (x₀, y₀, φ₀). At the moment t, the track arrives at the position shown in Figure 1. Based on the figure, we can formulate the equations representing the planar motion state of the UGTV.

$$\{\begin{array}{l}x(t)=\frac{1}{2}{\displaystyle {\int }_0^t(v_R+v_L)\cos \phi dt+x_0}\hfill \\ y(t)=\frac{1}{2}{\displaystyle {\int }_0^t(v_R+v_L)\sin \phi dt+y_0}\hfill \\ \phi (t)=\frac{1}{B+b}{\displaystyle {\int }_0^t(v_R-v_L)dt+{\phi }_0}\hfill \end{array}$$

(1)

In the formula, x(t) and y(t) represent the real-time position coordinates of the centroid of the UGTV, φ(t) represents the rotation angle between the local coordinate system and the global coordinate system, and the anticlockwise direction is positive. The parameter B represents the width of the vehicle body, b represents the width of the track plate, w_C represents the angular velocity of the vehicle’s mass center and v_R and v_L are the right and left track travel speeds, respectively. x₀, y₀, and φ₀ are constants. x₀, y₀, and φ₀ are 0 if O_C coincides with the point O. Take the derivative of the above formula:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}\cos \phi & \frac{1}{2}\cos \phi \\ \frac{1}{2}\sin \phi & \frac{1}{2}\sin \phi \\ \frac{1}{B+b}& -\frac{1}{B+b}\end{array}\right]\left[\begin{array}{c}{v}_R\\ v_L\end{array}\right]$$

(2)

In the formula, $\dot{x}$ and $\dot{y}$, respectively, represent the linear velocity of the vehicle in the x-coordinate direction and y-coordinate direction in the global coordinate system, and $\dot{\phi }$ represents the rotational angular velocity.

In trajectory tracking research, the given reference trajectory is generally described by the motion trajectory of the given reference vehicle. Each reference point on it satisfies the above kinematic equation, with r representing the reference quantity, and its general form is:

$${\dot{x}}_r=f(x_r,u_r)$$

(3)

where x_r = [x_r, y_r, φ_r]^T is the state vector; u_r = [v_Rr, v_Lr] is the input vector.

Since the above is a nonlinear system, it needs to be linearized in order to apply it to MPC, so Equation (3) is expanded by Taylor series and the higher-order term is ignored.

$${\dot{x}}_r=f(x_r,u_r)+{\frac{\partial f(x,u)}{\partial x}|}_{\begin{array}{l}x=x_r\\ u=u_r\end{array}}(x-x_r)+{\frac{\partial f(x,u)}{\partial u}|}_{\begin{array}{l}x=x_r\\ u=u_r\end{array}}(u-u_r)$$

(4)

By subtracting the above equation from Equation (4), the following can be obtained.

$$\dot{\tilde{x}}=\dot{x}-{\dot{x}}_r=\left[\begin{array}{ccc}0& 0& -\frac{1}{2}\sin {\phi }_r(v_{Rr}+v_{Lr})\\ 0& 0& \frac{1}{2}\cos {\phi }_r(v_{Rr}+v_{Lr})\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}\cos {\phi }_r& \frac{1}{2}\cos {\phi }_r\\ \frac{1}{2}\sin {\phi }_r& \frac{1}{2}\sin {\phi }_r\\ \frac{1}{B+b}& -\frac{1}{B+b}\end{array}\right]\left[\begin{array}{c}{v}_R-v_{Rr}\\ v_L-v_{Lr}\end{array}\right]$$

(5)

Since the above equation is a continuous time system, in order to apply it to MPC, discretization is still required after linearization.

$$\dot{\tilde{x}}(k+1)=A_1(k)\tilde{x}(k)+B_1(k)\tilde{u}(k)$$

(6)

3. Trajectory Tracking Control Based on Model Prediction

3.1. Trajectory Tracking Control System Scheme

The schematic diagram of vehicle tracking is shown in Figure 2. A curve path is preset in the space environment and the position and heading of the ideal virtual moving vehicle is set at time t₁, t₂ and t₃. The current moment is t₀, and the current pose of the UGTV is (x₀, y₀, φ₀). If the trajectory tracking task is completed, at time t₁ the vehicle needs to reach the position shown in the figure at time t₁ and maintain the same heading, as well as t₂ and t₃ moments in the same way. Therefore, the trajectory tracking problem requires the vehicle to follow a time-dependent trajectory.

To enable the UGTV to track the planned trajectory and reach the target position on time, the forward speed and heading of the vehicle need to be controlled. Since the UGTV moves forward or turns by adjusting the speed difference between the tracks on both sides, the longitudinal speed and heading of the vehicle are adjusted by controlling the left and right track speed. The process of trajectory tracking control system for UGTVs is shown in Figure 3.

The trajectory tracking control system of the UGTV based on RTK is divided into four parts: trajectory preset, MPC controller, UGTV and RTK satellite positioning system. The trajectory preset is used to set the reference trajectory in advance and discretize it in the system before the UGTV walks, so as to provide an ideal position and pose information reference for the trajectory tracking of the UGTV at each time step. The MPC controller is the core of the trajectory tracking control system. By comparing the deviation between the current pose and the ideal pose, the objective function that can reduce the deviation between the vehicle and the target trajectory can be solved many times to obtain the optimal control sequence and output the first control quantity. The intelligent UGTV part executes the optimal control quantity obtained in each time step. In kinematic control tracking, the trajectory is tracked by changing the left and right track speed of the UGTV. In dynamic control tracking, the trajectory is tracked by adjusting the drive motor frequency. RTK satellite positioning system is used for real-time acquisition of the UGTV position and heading information and output the current time data. The control system circulates and makes the vehicle quickly track the target position through timely adjusting the speed of each track until the whole track is tracked.

3.2. Design of Trajectory Tracking Controller Based on Model Prediction

3.2.1. Model Predictive Control Principle

At each sampling moment, model predictive control sets the current system state as the initial state. By calculating the deviation between the current state and the target state, the optimal control sequence containing multiple components is optimized and solved, and then the first component in the sequence is executed. The principle is shown in Figure 4.

The vertical axis is the current moment k of the system, m is the predicted time domain length of the system, the gray line on the left is the executed control quantity of the system and the gray curve represents the past output state of the system. At time k, the deviation between the current system state and the target state is compared, and the optimal control sequence is calculated by means of the system prediction model and the optimization algorithm, as shown in the red line, and at this point, the predicted output of the system is shown in the green curve. The MPC controller applies the first component of the control sequence to the system and omits the remaining control components. After reaching the next sampling time k + 1, the system recalculates the deviation between the current system state and the target state, performs the optimization calculation again and obtains the optimal control sequence at this time, after which u(k + 1) is applied to the system. This is repeated until the control process is completed.

3.2.2. Establishment of Prediction Model

The state space model of a linear steady discrete time system is as follows:

$$\{\begin{array}{l}x(k+1)=Ax(k)+Bu(k)\\ y(k)=Cx(k)\end{array}$$

(7)

Among them, x(k) is an n-dimensional state vector (x₁, x₂, …, x_n)^T, y(k) is the m-dimensional output vector (y₁, y₂, …, y_m)^T, u(k) is the r-dimension input vector (u₁, u₂, …, u_k)^T, A is the state matrix, B is the input matrix and C is the output matrix. Suppose that the system is without disturbance all state vectors in the system are measurable; the recurrence method is used to solve the state space-based predictive control problem. Take $\hat{x}(k+i|k)$ as the estimated value of x at time k + i at time k and $y(k+i|k)$ as the estimated value of y at time k + i at time k, while Hp is the time domain length of system prediction and H_c is the time domain length of system control:

$$\widehat{x}(k+j|k)=A^{j}x(k)+\left[A^{j-1}\begin{array}{ccc}{A}^{j-2}& \cdots & I\end{array}\right]B\left[\begin{array}{c}\widehat{u}(k|k)\\ \widehat{u}(k+1|k)\\ ⋮\\ \widehat{u}(k+j-1|k)\end{array}\right],j=1,\dots ,H_p$$

(8)

At the current time k, u(k) is an unknown quantity, and u(k − 1) is a known quantity; thus, take

$$\Delta \widehat{u}(k+i|k)=\widehat{u}(k+i|k)-\widehat{u}(k+i-1|k)$$

(9)

Therefore, it can be obtained that:

$$\begin{array}{l}\widehat{u}(k|k)=\Delta \widehat{u}(k|k)+u(k-1)\\ \widehat{u}(k+1|k)=\Delta \widehat{u}(k+1|k)+\Delta \widehat{u}(k|k)+u(k-1)\\ \cdots \\ \widehat{u}(k+H_c-1|k)=\Delta \widehat{u}(k+H_c-1|k)+\cdots +\Delta \widehat{u}(k|k)+u(k-1)\end{array}$$

(10)

Therefore, the above equation is substituted into Equation (8), and H_c can be obtained.

$$\begin{array}{ll}\hfill \widehat{x}(k+H_c|k)=& A^{H_c}x(k)+(A^{H_c-1}+\cdots +A+I)B\Delta \widehat{u}(k|k)\hfill \\ & +\cdots +B\Delta \widehat{u}(k+H_c-1|k)+(A^{H_c-1}+\cdots +A+I)Bu(k-1)\hfill \end{array}$$

(11)

When H_c, the control input does not change, i.e., $u(k+i|k)=u(k+-1\left|k\right)$, then:

$$\begin{array}{ll}\hfill \widehat{x}(k+H_c+1|k)=& A^{H_c+1}x(k)+(A^{H_c}+\cdots +A+I)B\Delta \widehat{u}(k|k)+\cdots +\hfill \\ & (A+I)B\Delta \widehat{u}(k+H_c-1|k)+(A^{H_c}+\cdots +A+I)Bu(k-1)\hfill \end{array}$$

(12)

$$\begin{array}{ll}\hfill \widehat{x}(k+H_p|k)=& A^{H_p}x(k)+(A^{H_p-1}+\cdots +A+I)B\Delta \widehat{u}(k|k)\hfill \\ & +\cdots +(A^{H_p-H_c}+\cdots +I)B\Delta \widehat{u}(k+H_c-1|k)\hfill \\ & +(A^{H_p-1}+\cdots +A+I)Bu(k-1)\hfill \end{array}$$

(13)

Therefore, in the prediction time domain H_p, the general prediction equation of state vector is:

$$\begin{array}{ll}\hfill \left[\begin{array}{c}\widehat{x}(k+1|k)\\ ⋮\\ \widehat{x}(k+H_c|k)\\ ⋮\\ \widehat{x}(k+H_p|k)\end{array}\right]=& \left[\begin{array}{c}A\\ ⋮\\ A^{H_c}\\ ⋮\\ A^{H_p}\end{array}\right]x(k)+\left[\begin{array}{c}B\\ ⋮\\ {\displaystyle \sum _{i=0}^{{}^{H_c-1}}{A}^i}B\\ ⋮\\ {\displaystyle \sum _{i=0}^{{}^{H_p-1}}{A}^i}B\end{array}\right]u(k-1)+\hfill \\ & \left[\begin{array}{ccc}B& \cdots & 0\\ ⋮& \cdots & ⋮\\ {\displaystyle \sum _{i=0}^{{}^{H_c-1}}{A}^i}B& \cdots & B\\ ⋮& \cdots & ⋮\\ {\displaystyle \sum _{i=0}^{{}^{H_p-1}}{A}^i}B& \cdots & {\displaystyle \sum _{i=0}^{{}^{H_p-H_c}}{A}^i}B\end{array}\right]\left[\begin{array}{c}\Delta \widehat{u}(k|k)\\ ⋮\\ \Delta \widehat{u}(k+H_c-1|k)\\ ⋮\\ \Delta \widehat{u}(k+H_c-1|k)\end{array}\right]\hfill \end{array}$$

(14)

The total prediction equation of output variable is:

$$\left[\begin{array}{c}\widehat{y}(k+1|k)\\ \widehat{y}(k+2|k)\\ ⋮\\ \widehat{y}(k+H_p|k)\end{array}\right]=\left[\begin{array}{cccc}C& 0& \cdots & 0\\ 0& C& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & C\end{array}\right]\left[\begin{array}{c}\widehat{x}(k+1|k)\\ \widehat{x}(k+2|k)\\ ⋮\\ \widehat{x}(k+H_p|k)\end{array}\right]$$

(15)

The linear error model of the vehicle helps to simplify the calculation of the predicted output; thus, define new extended state vectors:

$$\xi (k)=\left[\begin{array}{c}x(k)\\ u(k-1)\end{array}\right]$$

(16)

Substituting into Equation (7), it can be obtained that:

$$\{\begin{array}{l}\xi (k+1)=\left[\begin{array}{c}x(k+1)\\ u(k)\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& I\end{array}\right]\xi (k)+\left[\begin{array}{c}B\\ I\end{array}\right]\Delta u(k)\hfill \\ y(k)=\left[\begin{array}{cc}C& 0\end{array}\right]\xi (k)\hfill \end{array}$$

(17)

At this time, in the prediction time domain H_p, the prediction equation of the system is:

$$\{\begin{array}{l}\widehat{\xi }(k+H_p|k)={\tilde{A}}^{H_p}\xi (k)+{\tilde{A}}^{H_p-1}\tilde{B}\Delta \widehat{u}(k|k)+\cdots {\tilde{A}}^{H_p-H_c-1}\tilde{B}\Delta \widehat{u}(k+H_c|k)\hfill \\ \widehat{y}(k+H_p|k)=\tilde{C}\widehat{\xi }(k+H_p|k)\hfill \end{array}$$

(18)

Among them is:

$$\tilde{A}=\left[\begin{array}{cc}A& B\\ 0& I\end{array}\right],\tilde{B}=\left[\begin{array}{c}B\\ I\end{array}\right],\tilde{C}=\left[\begin{array}{cc}C& 0\end{array}\right]$$

(19)

In order to observe the relationship between variables easily, the matrix expression of the predicted output of the system is:

$$Y(k)=\Psi \xi (k)+\Theta \Delta U(k)$$

(20)

The insides are:

$$Y(k)=\left[\begin{array}{c}\widehat{y}(k+1|k)\\ \widehat{y}(k+2|k)\\ ⋮\\ \widehat{y}(k+H_c|k)\\ ⋮\\ \widehat{y}(k+H_p|k)\end{array}\right],\Psi =\left[\begin{array}{c}\tilde{C}\tilde{A}\\ \tilde{C}{\tilde{A}}^2\\ ⋮\\ \tilde{C}{\tilde{A}}^{H_c}\\ ⋮\\ \tilde{C}{\tilde{A}}^{H_p}\end{array}\right],\Delta U(k)=\left[\begin{array}{c}\Delta \widehat{u}(k|k)\\ \Delta \widehat{u}(k+1|k)\\ ⋮\\ \Delta \widehat{u}(k+H_c|k)\end{array}\right]$$

(21)

$$\Theta =\left[\begin{array}{cccc}\tilde{C}\tilde{B}& 0& \cdots & 0\\ \tilde{C}\tilde{A}\tilde{B}& \tilde{C}\tilde{B}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{N_c-1}\tilde{B}& \tilde{C}{\tilde{A}}^{N_c-2}\tilde{B}& \cdots & \tilde{C}\tilde{B}\\ ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{N_p-1}\tilde{B}& \tilde{C}{\tilde{A}}^{N_p-2}\tilde{B}& \cdots & \tilde{C}{\tilde{A}}^{N_p-N_c-1}\tilde{B}\end{array}\right]$$

(22)

3.2.3. The Objective Function Design

In MPC, the system can predict the state quantity of k + 1 time at k time, as shown in Equation (17), in which the state quantity at k time is known and the control increment is unknown. A series of optimal control increments satisfying the constraint conditions need to be obtained by solving the optimization objective function equation at each time. The objective function of design optimization is as follows:

$$J(k)={{\displaystyle \sum _{i=1}^{H_p}\Vert \widehat{y}(k+i|k)-y_{ref}(k+i|k)\Vert }}_Q^2+{{\displaystyle \sum _{i=0}^{H_c-1}\Vert \Delta \widehat{u}(k+i|k)\Vert }}_R^2+\rho {\epsilon }^2$$

(23)

The first term in the formula is the norm of the error between the predicted output of the system and the reference trajectory, which represents the ability to track the reference trajectory. The second term is used for the system’s constraints on the control increment to prevent the control amount from changing too drastically. Q is the error weight matrix of the system. An element in the matrix indicates how much the system attaches importance to tracking errors at different times. By modifying the elements in the matrix, the tracking errors at different times can be weighted. R is the control weight matrix of the system, which has a certain degree of control over the stability of the system. Let:

$$Y_{ref}(k)=\left[\begin{array}{c}{y}_{ref}(k+1|k)\\ ⋮\\ y_{ref}(k+H_p|k)\end{array}\right],Q_D=\left[\begin{array}{cccc}Q& 0& \cdots & 0\\ 0& Q& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & Q\end{array}\right],R_D=\left[\begin{array}{cccc}R& 0& \cdots & 0\\ 0& R& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & R\end{array}\right]$$

(24)

and the deviation of the system output is:

$$E(k)=\Psi \xi (k)-Y_{ref}(k)$$

(25)

Then, the objective function can be rewritten as:

$$J(k)={[E(k)+\Theta \Delta U(k)]}^T{Q}_D[E(k)+\Theta \Delta U(k)]+\Delta U{(k)}^T{R}_D\Delta U(k)+\rho {\epsilon }^2$$

(26)

The above formula is the general form of the performance index function. In the process of solving, it is easy to solve by computer programming. It is often transformed into a standard quadratic form through matrix operations, that is, into a quadratic programming (QP) problem. Putting Equation (20) into the above formula, we can obtain:

$$J(k)=\frac{1}{2}{\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]}^{T}H\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]+G\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]+P$$

(27)

Among them is:

$$H=\left[\begin{array}{cc}2({\Theta }^T{Q}_D\Theta +R_D)& 0\\ 0& \rho \end{array}\right],G=\left[\begin{array}{cc}2E{(k)}^T{Q}_D\Theta & 0\end{array}\right],P=E{(k)}^T{Q}_{D}E(k)$$

(28)

In trajectory tracking control, while solving the above-mentioned quadratic programming problem, it is necessary to satisfy some constraint conditions of the control quantity, namely the control increment constraint, the control quantity constraint and the output quantity constraint of the system:

$$\begin{array}{l}\Delta u_{\min }(k)\le \Delta u(k)\le \Delta u_{\max }(k),k=0,1,\dots ,N_c-1\\ u_{\min }(k)\le u(k)\le u_{\max }(k),k=0,1,\dots ,N_c-1\\ y_{\min }(k)\le y(k)\le y_{\max }(k),k=0,1,\dots ,N_c-1\end{array}$$

(29)

The trajectory tracking process of MPC is shown in Figure 5.

In each control cycle in the trajectory tracking process, the system needs to solve the following quadratic programming problems:

$$\begin{array}{l}\min J(k)=\frac{1}{2}{\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]}^{T}H\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]+G\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]+P\\ s.t. \Delta U_{\min }(k)\le \Delta U(k)\le \Delta U_{\max }(k)\\ U_{\min }(k)\le u(k-1)+{\displaystyle \sum _{i=0}^k\Delta U(i)}\le U_{\max }(k)\\ Y_{\min }-\epsilon \le \Psi \xi (k)+\Theta \Delta U(k)\le Y_{\max }+\epsilon \\ \epsilon \ge 0\end{array}$$

(30)

After the solution is completed, a control input increment sequence in the control period will be obtained:

$$\Delta U(k)=[\Delta u(k),\Delta u(k+1),\dots ,\Delta u(k+N_c-1)]$$

(31)

The first control input increment is applied to the system and the remaining N_c − 1 increments are discarded:

$$u(k)=u(k-1)+\Delta u(k)$$

(32)

When the state variables and outputs of the new moment are obtained, the above optimization objective function is solved again, and a new control sequence is obtained. At this time, the first control input increment is applied to the system and the above cycle is repeated until the entire reference path is tracked. Thus, the closed-loop rolling optimization of the system is formed.

3.3. Driving Straight Trajectory Tracking Analysis

The MPC is used to carry out trajectory tracking simulation experiments on the kinematics model of the UGTV. The preset track of the UGTV is a straight line, the initial position and attitude of the target track are x and the initial position and attitude of the UGTV are y. The sampling time is set to T = 0.5 s, the simulation time to 30 s, the system prediction time domain length to H_p = 20 and system control time domain length to H_c = 3.

The simulation analysis is carried out under the condition that the centroid linear velocity of the target UGTV is 5 m/s and the centroid angular velocity is 0 rad/s. The speed constraint conditions for actual UGTVs are v_max = 7.5 m/s, v_min = 0. The comparison between the simulated driving trajectory and the actual driving trajectory is shown in Figure 6a,b.

The preset driving trajectory is a horizontal straight line. At the beginning of the simulation, there is an initial deviation of 10 m between the actual crawler and the target crawler in the y-axis direction. After trajectory tracking control, the two trajectories gradually approach. When the UGTV reaches the preset track for the first time, it will deviate from the preset track due to the adjustment of vehicle attitude. After continuous adjustment and optimization, the vehicle can achieve accurate tracking after driving for about 25 m in the x-axis direction.

The position deviation of the UGTV in the y-axis direction keeps decreasing with time; it increases slightly at 2.7 s and then decreases rapidly. The position error in the x-axis direction first increases to a certain extent, then reversely increases after 0.6 s.

As shown in Figure 7, the heading angle deviation of the actual UGTV increased rapidly at the beginning of trajectory tracking, increased to the maximum deviation −0.992 rad at 1.02 s and then continued to decrease, and finally it stabilized at 6.8414 × 10⁻⁷ rad after continuous adjustment. The centroid angular velocity of the UGTV reaches the maximum value of 1.5 rad/s in the counterclockwise direction at 0~0.5 s, and the maximum value of −0.640 rad/s in the clockwise direction at 1.5~2 s, and finally it stabilizes at −5.73464 × 10⁻⁶ rad/s.

The crawler speeds on the left and right sides experience a process of first increasing and then decreasing. The left track reaches the maximum value of 7.5 m/s in 1~1.5 s, and the right track maintains the maximum value of 7.5 m/s in 0~1 s. The change of the linear velocity of the centroid of the UGTV is shown in Figure 8b. The linear velocity of the centroid tends to stabilize after 5 s, and finally it stabilizes at 5.006 m/s at 9.02 s.

From the above simulation results, it can be seen that the MPC can basically make the UGTV walk at a speed of 5 m/s along a predetermined linear trajectory, the centroid angular velocity and centroid linear velocity are the same as the preset values and the accurate tracking is basically realized in about 5 s. After 9 s, accurate and stable trajectory tracking is realized, and the control error is small and finally remains constant. Ensure accurate and stable tracking after 10.5 s. During this period, the overshoot is small, and the linear velocity error remains 0 after tracking.

Using the same preset reference trajectory, simulation duration, sampling time, predicted time domain length and other parameters as above, the simulation analysis of the preset vehicle speed of 3 m/s and 7 m/s is performed, respectively. Comparing the simulation results with the 5 m/s and 1 m/s operating conditions, the comparison results of the driving trajectory, position deviation, heading angle deviation and centroid linear velocity are shown in Figure 9a–d.

From the trajectory tracking effect, under the condition of initial deviation, all four UGTVs with different reference speeds can quickly track the straight-line trajectory. There is almost no overshoot in trajectory tracking under 1 m/s. As the reference speed increases, the overshoot before accurate tracking increases. This is because as the reference speed increases, its requirements for the length of the prediction time domain also increase. If the length of the prediction time domain is extended, the overshoot can be reduced. In terms of the time required for accurate tracking, the accurate tracking can be achieved when the working conditions of 3 m/s, 5 m/s and 7 m/s are about 5 s, and the tracking deviation tends to zero after 7.5 s under a 1 m/s working condition. This is because the driving speed of 1 m/s working condition is too low and it takes a long time to overcome the initial lateral deviation of 10 m.

From the comparison result of the heading angle deviation, as the reference speed decreases, the maximum heading angle deviation value gradually increases. From the comparison chart of centroid linear velocity, it can be seen that the speed of the UGTV under the four working conditions all experience a process of first increasing and then gradually decreasing, and the speed is limited within their respective constraints.

3.4. Tracking Analysis of Driving Trajectory in Continuous Curve

The trajectory of the preset UGTV is a continuous curve, and the reference trajectory equation is as follows:

$$\{\begin{array}{l}x(t)=5+t\\ y(t)=10-t-20\sin (\frac{\pi t}{20})\end{array}$$

(33)

The initial position and posture of the target track are x, and the initial position and posture of the actual UGTV are y, that is, the initial position deviation in the x-axis direction is 5 m, and the position deviation in the y-axis direction is 10 m. The sampling time is set to t = 0.5 s, the simulation time is 40 s and the maximum speed constraint of the actual UGTV is v_max = 6 m/s. When the predicted time domain H_p = 30 and the control time domain H_c = 3, the comparison between the actual UGTV and the preset trajectory is shown in Figure 10.

Due to the deviation in X and Y directions at the beginning of track tracking, there is a large distance between the actual track and the preset track in the early stage of tracking. By continuously adjusting the track speeds on both sides, the UGTV quickly approached the preset trajectory and met the preset trajectory for the first time in 2.2 s. Then the vehicle slightly crossed the preset trajectory, and quickly adjusted and successfully tracked the preset trajectory under the action of the controller.

The driving deviation of the UGTV on the x-axis and y-axis decreases rapidly in the early stage of tracking, and the speed slows down after 2.5 s and gradually reaches 0. The driving deviation of the x-axis and y-axis is 0 for the first time around 9 s. When the UGTV passes the first curve, the tracking effect is better, and the actual driving trajectory basically coincides with the preset driving trajectory, while the position deviation is controlled within 0.2 m within 9~24 s. When the UGTV passes the second curve, the tracking error increases slightly. At this time, the x-axis direction error is controlled within 0.32 m, and the y-axis direction error is controlled within 0.54 m. After the UGTV passes through the curve area, the actual driving trajectory again tends to coincide with the preset driving trajectory.

Figure 11 show the variation curves of heading angle deviation and centroid angular velocity of the UGTV with time. The actual heading angle deviation of the UGTV increases rapidly at the beginning of track tracking, increases to the maximum deviation of −1.88 rad at 0.54 s and then decreases continuously, and the angle deviation is zero for the first time at 2.3 s. In the subsequent trajectory tracking interval from 2.3 s to 40 s, the heading angle deviation fluctuated in the range of 0.08 to −0.13 rad and the maximum value appeared when the UGTV traveled to the vicinity of the second curve. The centroid angular velocity of the UGTV quickly reaches the maximum value of 1.09 rad/s in the counterclockwise direction within 0.5 s after the start of tracking and then reaches the maximum value of 1.23 rad/s in the clockwise direction within 1.5~2.0 s. In the 38 s after this, the centroid angular velocity changes in the range of −0.49~0.40 rad/s.

Initially, the vehicle was driving in a counterclockwise direction. After about 1 s, the speed of the left track was higher than that of the right side for the first time, and the vehicle began to drive in a clockwise direction to adjust its posture. When the UGTV reaches the first curve, the speed of the right crawler is higher than that of the left for the second time, and the vehicle deflects counterclockwise to follow the preset driving trajectory. When driving to the second curve, the track speed on the left is greater than that on the right for the second time, and the vehicle deflects clockwise to follow the preset driving trajectory. Then, the track speeds on both sides tend to be the same, and the vehicle travels straight. The speed variation of the UGTV is shown in Figure 12.

From the above simulation results, it can be seen that the MPC can improve the UGTV’s drive along the preset continuous curve trajectory, and basically achieve accurate tracking in about 10 s. When the vehicle is driving to a curve, the maximum position error does not exceed 0.54 m, and the heading angle deviation does not exceed 0.28 rad.

4. Experimental Study on Trajectory Tracking of UGTVs

4.1. The Test Platform

The overall scheme of the UGTV trajectory tracking test platform is shown in Figure 13, which is composed of hardware equipment and tracking software. The hardware part mainly includes a walking execution system, control execution system and data acquisition system. The data acquisition system mainly includes satellite antennas, positioning receivers and other satellite positioning related equipment; the control system is composed of a computer, NI data acquisition card and inverter; the walking execution system includes a UGTV prototype, an induction motor and a power supply. The hardware part is used for state acquisition and action execution, and the software part is responsible for data processing and control strategies.

The test platform is shown in Figure 14. When carrying out the trajectory tracking test of the UGTV, first fix the satellite antenna of the receiver to the front and back ends of the longitudinal line of the top surface of the crawler walking device, connect the satellite antenna to the mobile station receiver and connect to the computer to measure the real-time test prototype location and speed and heading information. Write a program in LabView to read the serial port data and decode them, calculate the deviation between the current position of the vehicle and the target position, and optimize the calculation through Matlab to obtain the optimal control value at the current sampling moment. LabView sends the control signal command to the data acquisition card, and the NI data acquisition card outputs analog signals to the inverter. After receiving, the DZB 200 inverter adjusts the corresponding frequency according to the corresponding relationship between the signal change range and the power supply frequency, thereby controlling the speed of the crawlers on both sides, making the test prototype travel along a predetermined trajectory.

The data acquisition system includes satellite positioning base stations, mobile stations, measuring antennas and power supplies. The positioning base station uses the Huace Zhonghui i50 small intelligent RTK measurement system, which supports Beidou full constellation positioning and has high measurement accuracy and reliability, which can meet the needs of tracking test. The coordinate heading information of the UGTV is solved by the P3-DT direction-finding receiver based on the two localization antennas as well as the reference station, and its related parameters are listed in

Table 1. Parameters related to RTK hardware devices.

Device		Parameters	Value
Zhonghui i50 Positioning Base Station		Reliability	>99.99%
		RTK positioning accuracy	Plane: ±8 mm; Elevation: ±15 mm
		Operating temperature	−45 °C~+85 °C
P3-DT direction-finding receiver		RTK positioning accuracy	Plane: ±8 mm; Elevation: ±15 mm
		Heading accuracy	<0.09° (2 m baseline); <0.05° (10 m baseline)
		Operating temperature	−40 °C~+75 °C

.

4.2. Test Results and Analysis

4.2.1. Linear Working Conditions Test

In order to verify the performance of the model predictive controller and the effectiveness of the overall navigation tracking system, the crawler test prototype was tracked under the linear working conditions. Before the start of the test, there is an initial lateral deviation of 4 m between the test prototype and the preset trajectory, without initial longitudinal deviation; the initial angular deviation is 0 and the tracking time is 60 s. The test results are shown in Figure 15.

It can be seen from the comparison chart of the actual driving trajectory of the UGTV and the preset trajectory that the crawler prototype can quickly adjust the direction to track the target trajectory after the tracking starts, and it can basically drive along the trajectory without deviation after being overlapped with the target trajectory. The vehicle quickly reduces the y-axis direction deviation after driving and slowly adjusts the driving direction when it is close to the preset trajectory until the deviation area is 0. As for the deviation in the x-axis direction, since the vehicle is slightly ahead of the preset trajectory after the start of the tracking, the deviation value first increases and then decreases. After about 30 s, the vehicle tracks the preset trajectory, and the deviation value fluctuates around 0 in the following time.

In terms of vehicle direction and speed, the track speed on both sides increases rapidly after the start of tracking, and the right track speed is slightly faster than the left to adjust the track direction to around 37°. After about 4 s, the track reduces the adjustment speed and reaches the maximum course value of 50° in about 15 s. During this period, the track prototype travels at the maximum speed to shorten the distance between the vehicle and the target track. In the following 5 s, the vehicle reduces its speed and adjusts its course, during which the x-axis direction deviation is also shortened. After about 10 s, the angular deviation and speed deviation of the vehicle basically return to zero, and the vehicle runs smoothly along the preset trajectory at the reference speed.

4.2.2. Curved Working Conditions Test

The test prototype tracks a circular trajectory to verify the driving control results of the navigation tracking system. The preset radius of circular curve is 4 m and the initial coordinate of track prototype is (0,0). There is no preset deviation in x-axis direction, while the preset deviation in y-axis direction is 1 m, the initial heading angle deviation is 0, the tracking time is 120 s and the reference speed is 0.2 m/s. The test results of driving track and deviation are shown in Figure 16.

After the tracking starts, the crawler prototype quickly adjusts its course and travels in a straight line to quickly approach the preset circular track. After about 15 s, the vehicle slowly accesses the preset trajectory, and follows the trajectory in the next time. From the trajectory comparison chart, it can be seen that the UGTV trajectory tracking test platform has a better effect. The details of the position deviation are shown in Figure 17.

After the start of the test, the vehicle speed increased rapidly, and the distance traveled in the x-axis direction slightly exceeded the preset trajectory, resulting in a reverse increase in the position deviation in the x-axis direction. After about 6 s, the vehicle adjusts the speed of the crawlers on both sides, and the position deviation gradually decreases after reaching the peak. In the y-axis direction, the deviation showed a continuous decreasing trend. The deviation distance was shortened rapidly in the first 12 s. After approaching the preset track, the track prototype adjusts speed and direction, and the deviation decreases slowly. Due to the inevitable existence of positioning error and machine error in the test, the position deviation fluctuates around 0.

According to Figure 18, it can be seen from the deviation of speed and heading angle that UGTVs can quickly increase the speed and change the direction of travel according to the deviation between the vehicle’s position and the target position. After starting, the heading angle deviation of the vehicle increased rapidly, reaching the maximum deviation of 25.7° in about 6 s, and quickly decreased to near 0 in the following 15 s. In terms of speed, the vehicle reached the maximum value of 0.316 m/s in about 4.5 s, gradually decreased after maintaining for 3 s and finally maintained speed at 0.2 m/s from 14 s.

Since curve conditions better reflect the performance of control methods, they are commonly used as the primary basis for evaluating and optimizing these methods. In previous studies [21], the sliding mode control method and the Lyapunov-based backstepping sliding mode control method were employed, and the experimental comparison results are shown in

Table 2. Comparative analysis of test results.

Method	Positional Deviation (m)	Heading Angle Deviation (°)
this article	<0.10 m	<3°
sliding mode control	<0.15 m	<5°
Lyapunov-based backstepping sliding mode control	<0.14 m	<3°

. Based on the contents of Table 2, it is evident that the method proposed in this paper outperforms the sliding mode control method and the Lyapunov-based backstepping sliding mode control method in terms of position deviation and heading angle deviation.

During the test, due to errors in satellite positioning results, lag in vehicle body transmission and certain disturbances in the road surface, the track still has certain fluctuations in position deviation, angular deviation and speed after tracking the preset trajectory. However, on the whole, the UGTV trajectory tracking test platform can drive according to the predetermined trajectory, quickly overcome deviations and has high tracking accuracy and stability.

5. Conclusions

This paper explores the RTK-based UGTV model predictive trajectory tracking control method through simulations and tests. The analysis of velocity and steering principles of UGTV planar motion establishes, discretizes and linearizes the kinematic state space model of UGTVs. Detuning the MPC-based trajectory tracking controller with trajectory speeds on both sides as control inputs and imposing control volume constraints improves the stability of trajectory tracking. Extensive simulations under straight-line and continuous curve conditions verify the tracking effect of the controller. In straight-line conditions at different preset speeds, the time for the UGTV to overcome the initial deviation and track the preset trajectory decreases with increasing vehicle speed. However, the maximum overshoot increases synchronously with the increase of trajectory speed, and the maximum course angle deviation decreases with the increase of preset speed. Under continuous curve conditions, simulations with different prediction and control time domains compare and analyze their effects on the tracking effectiveness. The simulation results on both straight and curved lines demonstrate the controller’s ability to obtain better tracking results.

The trajectory tracking test of the UGTV prototype was successfully carried out using RTK satellite positioning technology. The test platform is equipped with a comprehensive hardware setup, including a data acquisition system, a control execution system and a walking execution system. The software comprises a data processing and optimization control part to support the trajectory tracking tests. These tests were conducted under various driving conditions, providing compelling evidence of the effectiveness and practicability of the RTK navigation tracking system.