Research for Nonlinear Model Predictive Controls to Laterally Control Unmanned Vehicle Trajectory Tracking

Featured Application

This work is used for the unmanned driving of vehicles and other devices.

Abstract

The autonomous driving is rapid developing recently and model predictive controls (MPCs) have been widely used in unmanned vehicle trajectory tracking. MPCs are advantageous because of their predictive modeling, rolling optimization, and feedback correction. In recent years, most studies on unmanned vehicle trajectory tracking have used only linear model predictive controls to solve MPC algorithm shortcomings in real time. Previous studies have not investigated problems under conditions where speeds are too fast or trajectory curvatures change rapidly, because of the poor accuracy of approximate linearization. A nonlinear model predictive control optimization algorithm based on the collocation method is proposed, which can reduce calculation load. The algorithm aims to reduce trajectory tracking errors while ensuring real-time performance. Monte Carlo simulations of the uncertain systems are carried out to analyze the robustness of the algorithm. Hardware-in-the-loop simulation and actual vehicle experiments were also conducted. Experiment results show that under i7-8700, the calculation time is less than 100 ms, and the mean square error of the lateral deviation is maintained at ${10}^{-3} {\mathrm{m}}^2$, which proves the proposed algorithm can meet the requirement of real time and accuracy in some particular situations. The unmanned vehicle trajectory tracking method provided in this article can meet the needs of real-time control.

1. Introduction

The autonomous driving of unmanned vehicles is in a golden age of development. Trajectory tracking plays a pivotal role in autonomous driving, and tracking controllers are fundamental to the driving performance of unmanned vehicles. In recent years, the number of autonomous controllers has increased. The most widely used of these controllers are proportional-integral-derivative (PID) controllers, linear quadratic regulators (LQR), and model predictive control (MPC) controllers [1,2,3,4,5].

A PID controller is a negative feedback controller based on error compensation. It does not require complex modeling of the control object, and adjust the controller parameters to obtain a good tracking effect [6,7]. However, the parameters of the PID controller need to be determined through a large number of simulations, and have to be adjusted dynamically under different working conditions.

An LQR is based on state feedback, which can reduce steady-state lateral errors during curve driving [8]. Additionally, LQRs can perform real-time computations to handle emergency situations [9]. However, this depends on an accurate control model and requires a curvature continuity trajectory.

An MPC has the basic characteristics of a predictive model, rolling optimization, and feedback correction. It is especially suitable for situations where accurate mathematical models cannot be established and there are multiple constraints [10]. MPCs are more applicable than LQRs, which rely too much on models. In real driving processes, it is necessary to predict driving actions over a future horizon according to the surrounding environment, with complicated road and traffic conditions, even if the vehicle is driving manually. Neither PID nor LQR can realize this. However, MPC could realize the predictions needed for real-time controls with its basic characteristics of predicting several time steps in the future. Linear model predictive controls (LMPCs) was proposed and applied due to its simple linearization model and fast calculation speed [11]. Researchers establish kinematic models for different control object, and use LMPC to obtain good trajectory tracking effects [12,13]. In [14], a multilayer control system that includes three path tracking controllers was proposed based on LMPC, which can actively adjust the longitudinal velocity and use suitable path tracking controller to improve the real-time performance. In [15], a LMPC is designed to linearize the nonlinear vehicle model locally at each sampling point to reduce the computational complexity. In addition, many MPC algorithms for autonomous driving use linear predictive models based on approximate linearization [16]. A linear time-varying predictive model considering cornering characteristics was designed and optimized in [17]. Gallep et al. directly used a simplified vehicle linear kinematics model and verified the algorithm by a single-shift simulation [18]. However, since the control objects are generally nonlinear complex systems, the accuracy of approximate linearization is poor, and more and more system nonlinear factors are added to the consideration of MPCs [19]. Zietkiewicz et al. compared the control effects of nonlinear model predictive controls (NMPCs) and LMPCs, and the result shows that the LMPC algorithm are not satisfactory at high speeds or when target trajectory curvatures change rapidly [20]. Kayacan et al. compared results for LMPCs and NMPCs in simulations, showing that LMPC lateral errors fluctuated greatly at different speeds and that the performance of the LMPCs was lower than that of the NMPCs [21].

The solution complexity of NMPCs is closely related to the dimensions of the equations. Even if a simple dynamic model is used, multiple time steps need to be predicted, and the computational load will be large [22]. Thus, NMPCs remain challenging to use in real-time operations. But with the development of integrated chips and optimized methods, which have accelerated computing speeds, an increasing number of researchers are considering NMPCs for autonomous trajectory tracking. In [23,24], researchers compared NMPC autonomous steering to manual steering in simulations of lane change and return maneuvers, and verified the advantage of NMPCs. In [25] a method of varying discrete steps is adopted in NMPCs to compatible with path tracking and obstacle avoidance better. In [26], the vehicle model of a NMPC is formulated by approximating the magic tire model with a set of piecewise linear functions to reduce the computational burden in simulations. In [27,28], the optimization problem solution of NMPCs is calculated by the use of sequential quadratic programming (SQP) to improve solution efficiency. In addition to improve the calculation speed, literature [29] applies convex quadratic cone programming optimization numerical calculation method to the nonlinear coupling model, and there are methods such as “warm start” [30] and reduction of optimization process variables by assuming that the control variable is unchanged in a certain period of time [31]. In most of these studies, however, only simulations were performed, and calculation times were largely ignored.

NMPCs have been also widely used in other fields, which may provide some inspirations to our research. Researchers proposed applying a particle swarm optimization to the MPC algorithm for a quadcopter in trajectory tracking which produced fast solutions in [32]. A NMPC approach is applied to a sugar precipitation for Chinese medicine solution which significantly improves precipitation efficiency [33]. A Min–Max selector algorithm and MPC algorithm are compared for aircraft engine fuel control in [34] and the MPC algorithm provides faster response. A NMPC scheme for the control of brushless direct current motor is presented in [35] and improves energy saving by 19%.

To solve the above problems, an NMPC based on the collocation method is proposed to the lateral control of unmanned vehicle trajectory tracking. The specific objective of this research is to ensure the accuracy of the NMPC algorithm and improve its real-time performance. The Runge-Kutta (RK) formula used in the collocation method in this paper is currently the main method for solving differential equations and has been widely used in various fields. The calculation process can be significantly simplified so that the computing time will be greatly reduced under the conditions where the initial value and derivative of the equation are given. The two-degrees-of-freedom kinematics equations for the vehicle, which are used by the MPC, also fit well with the requirements of the RK formula. And this paper uses Monte Carlo method to analyze the robustness of the algorithm which is a very mature sampling technique and can ensure the reliability of the experiment.

This paper is organized as follows. The second section introduces the theory of the NMPC. In the third section, a second-order RK formula is used to discretize the linear equations transforms the linear equations into a non-linear programming problem with constraints and performs Monte Carlo simulations. The fourth section describes the verification experiment. The results indicate that this method requires a low amount of calculations, with the calculation time for each solution step being within 100 ms. The fifth section presents the conclusions.

2. Nonlinear Model Predictive Control

2.1. Principles of Model Predictive Control

2.1.1. Predictive Model

The vehicle model is the basis of the MPC trajectory tracking controller. The model predicts future states from the current state information provided by the system, plus future steering control inputs. A detailed mathematical model will be introduced in Part B.

2.1.2. Rolling Optimization

At each sampling time, a control sequence that minimizes the objective function is calculated using the trajectory tracking controller. Only the first steering angle of the sequence will be applied to the CAN bus. At the next sampling time, a new sequence will be recalculated.

2.1.3. Feedback Correction

The objective function will include lateral deviations and heading angle deviations, which will be used to correct the steering control sequence and improve the trajectory tracking performance of the controller. This feedback correction provides the MPC controller with a strong ability to resist interference and overcome system uncertainty. The design architecture of the MPC trajectory tracking controller is shown in Figure 1. The controller obtains the initial state firstly, uses the predictive model and the target trajectory to get the objective function secondly, and finally calculate a control sequence to minimize the objective function at each optimization period.

2.2. Mathematical Model of Nonlinear System

2.2.1. System State Equation

A nonlinear system can be expressed using the following equation:

$$\{\begin{array}{c}\dot{X\left(\mathsf{\tau }\right)}= f\left(X\left(\mathsf{\tau }\right),u\left(\mathsf{\tau }\right)\right)\\ X\left(0\right)=X_0\\ Y\left(\mathsf{\tau }\right)=h\left(X\left(\mathsf{\tau }\right), u\left(\tau \right)\right)\end{array}$$

(1)

where $X$ is the system state, $u$ is the system input, $Y$ is the system output,$f\left(X,u\right)$ describes the dynamic system, and$h\left(X,u\right)$ defines the system output.

A future system state can be predicted using a numerical integration algorithm:

$$X\left(t+1\right)=g\left(X\left(t\right),u\left(t\right)\right)$$

(2)

where $g\left(X,u\right)$ is a numerical integration method.

In this paper, a vehicle kinematic model is employed to describe the system state.

2.2.2. Vehicle Kinematic Model

The research goal of this study is to ensure the accuracy and real-time performance of the vehicle while tracking its desired trajectory. To reduce lateral errors and improve control accuracy, it is necessary to establish a suitably simplified kinematic model. The research focuses on the longitudinal, lateral and yaw movement of the vehicle.

Therefore, the modeling is appropriately simplified shown in Figure 2, and the following assumptions were made as follows: Ignoring the effects of the steering system, using the steering angle of the front wheels as the steering input, ignoring the role of the suspension, ignoring the air resistance, and ignoring the impact of the ground tangential force on the tire. For convenience, the center of the vehicle’s rear axis was used as the origin of the vehicle coordinate system.

The simplified model of an intelligent vehicle is as follows:

$$\{\begin{array}{c}\dot{x}=v\mathit{cos}\theta \hfill \\ \dot{y}=v\mathit{sin}\theta \hfill \\ \dot{\theta }=v\mathit{tan}\delta /L\hfill \end{array}$$

(3)

where x is the abscissa of the center of the rear axle of the vehicle, y is the ordinate of the center of the rear axle of the vehicle, θ is the yaw angle of the vehicle, δ is the steering wheel angle, L is the wheelbase of the vehicle, and v is the vehicle speed.

2.2.3. Objective Function

The goal of the MPC algorithm is to calculate a control sequence that can minimize the objective function. The objective function has a similar form to the following equation:

$$J= {{\displaystyle \sum }}_{k=1}^{N}L(X\left(t+k|t\right), Y\left(t+k|t\right), u(t+k|t))$$

(4)

where $X\left(t+k|t\right)$ is a future system state, $Y\left(t+k|t\right)$ is a future system output, and $u\left(t+k|t\right)$ is a future system input.

The objective function of the trajectory tracking controller should reflect the ability of the controller to smoothly track the desired trajectory. At the same time, the objective function is expected to require a low amount of calculation, to avoid affecting the real-time performance of the algorithm. The objective function is expressed as

$$J\left(U\right)=k_1{{\displaystyle \sum }}_{i=1}^n\left[y_i-T(x_i\right){]}^2+k_2{{\displaystyle \sum }}_{i=1}^n\left[{\theta }_i-arctan{T}^{\prime }(x_i\right){]}^2+ k_3{{\displaystyle \sum }}_{i=1}^n{u}_i{}^2$$

(5)

where $T\left(x\right)$ is the selected planning target trajectory, $n$ indicates prediction horizon, $k_1$, $k_2$, and $k_3$ are the weight coefficients of the lateral deviations, yaw angle deviations, and control inputs, respectively. The first two items reflect the system’s ability to track the desired trajectory, and the third item reflects the requirements for stably changing control inputs.

2.2.4. Constraints

In practical applications, there will always be various constraints, including equality and inequality.

The motor power for a real car steering system is limited, the rotation speed of the wheel corner cannot be arbitrarily large, and the wheel angle increment $\mathit{U}$ must be limited for real-world application. At the same time, there are physical limits on the wheel angle of the vehicle, so a wheel angle limit constraint must be added to the prediction model. In summary, the additional constraints are as follows:

$$-\mathsf{\Delta }{\mathit{\delta }}_{max}\le \mathit{U}\le \mathsf{\Delta }{\mathit{\delta }}_{max}$$

(6)

$$-{\mathit{\delta }}_{max}-{\mathit{\delta }}_0\le \mathit{C}\mathit{U}\le {\mathit{\delta }}_{max}-{\mathit{\delta }}_0$$

(7)

where

$$\mathit{U}=\left[\begin{array}{c}\mathsf{\Delta }{\delta }_1\\ \mathsf{\Delta }{\delta }_2\\ ⋮\\ \mathsf{\Delta }{\delta }_{n-1}\\ \mathsf{\Delta }{\delta }_n\end{array}\right]\mathit{C}=\left[\begin{array}{ccccc}1& 0& \cdots & 0& 0\\ 1& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 1& 1& \cdots & 1& 0\\ 1& 1& \cdots & 1& 1\end{array}\right]$$

(8)

$\mathsf{\Delta }{\mathit{\delta }}_{max}$ is the maximum wheel angle increment, and ${\mathit{\delta }}_{max}$ is the maximum wheel angle.

3. Optimal Algorithm for NMPC

3.1. Discretization of Kinematic Model Based on the Collocation Method

The RK method is derived from the Taylor formula and uses a slope to express the differential. It is used to estimate the slopes of several points in the integration interval, and then to calculate a weighted average as the basis for the next point. This produces a higher accuracy numerical integration calculation method. In this study, we used a special form of second-order RK, the improved Euler method:

$$\{\begin{array}{c}{y}_{i+1}=y_i+h\left(\frac{1}{2}{K}_1+\frac{1}{2}{K}_2\right)\\ K_1= f\left(x_i,y_i\right)\\ K_2=f\left(x_i+h, y_i+h{K}_1\right)\end{array}$$

(9)

The vehicle kinematics equation comprises expressions of the derivatives of state parameters, which satisfies the conditions of using a second-order RK. Using Equation (9), the vehicle yaw angle and coordinates in Equation (8) are discretized. Assuming that the discretization period is T and the predicted time domain is n, we obtain

$${\theta }_{i+1}={\theta }_i+\frac{Tv}{2L}\left(\mathit{tan}{\delta }_i+\mathit{tan}{\delta }_{i+1}\right)$$

(10)

$$x_{i+1}=x_i+\frac{Tv}{2}\left(\mathit{cos}{\theta }_i+\mathit{cos}{\theta }_{i+1}\right)$$

(11)

$$y_{i+1}=y_i+\frac{Tv}{2}\left(\mathit{sin}{\theta }_i+\mathit{sin}{\theta }_{i+1}\right)$$

(12)

where $i=0,1,\cdots ,n-1$.

The vehicle’s initial yaw angle ${\theta }_0$, initial wheel rotation angle ${\delta }_0$, initial abscissa $x_0$, current ordinate $y_0$, and control input set $\left\{{\delta }_1,\cdots ,{\delta }_{n-1},{\delta }_n\right\}$ are known. Substituting these parameters into Equation (10), the numerical solution sequence for the vehicle yaw angle $\theta \left(t\right)$ at each node under the second-order RK method $\left\{{\theta }_1,\cdots ,{\theta }_{n-1},{\theta }_n\right\}$ can be obtained. Substituting these parameters into Equations (11) and (12), a numerical solution sequence can also be obtained for the vehicle’s abscissa $x\left(t\right)$ and ordinate $y\left(t\right)$.

The Algorithm 1 is designed according to the above calculation steps. The pseudo-code of the algorithm is as follows:

Algorithm 1: Second-order RK Collocation Method for Intelligent Vehicle Prediction Model.

Input: Control sequence$\left\{{\delta }_1,\cdots ,{\delta }_{n-1},{\delta }_n\right\}$;

Initial state value:$\left\{{\theta }_0,x_0,y_0\right\}$

Output: state quantity matrix$\left\{\mathit{\theta },\mathit{X},\mathit{Y}\right\}$

1: calculate $\frac{Tv}{2}$, $\frac{Tv}{\left(2L\right)}$, $\mathit{tan}{\delta }_0$;

2: for $i=1:n$

3: calculate $\mathit{tan}{\delta }_i$;

4: calculate ${\theta }_i={\theta }_{i-1}+\left(\frac{Tv}{\left(2L\right)}\right)\times \left(\mathit{tan}{\delta }_{i-1}+\mathit{tan}{\theta }_i\right)$

5: end for

6: calculate $\mathit{sin}{\theta }_0$ and $\mathit{cos}{\theta }_0$

7: for $i=1:n$

8: calculate $\mathit{sin}{\theta }_i$ and $\mathit{cos}{\theta }_i$

9: calculate $x_i=x_{i-1}+\left(\frac{Tv}{2}\right)\times \left(\mathit{cos}{\theta }_{i-1}+\mathit{cos}{\theta }_i\right)$

10: calculate $y_i=y_{i-1}+\left(\frac{Tv}{2}\right)\times \left(\mathit{sin}{\theta }_{i-1}+\mathit{sin}{\theta }_i\right)$

11: end for

3.2. Solving Nonlinear Programming Problems

The collocation method is used to discretize the optimal control problems into nonlinear programming problems. When solving nonlinear programming problems, a solver named SNOPT based on a sequential quadratic programming (SQP) algorithm is used to perform successive approximations through a series of quadratic programming, which can efficiently solve large-scale nonlinear optimization problems.

3.3. Monte Carlo Simulations

Monte Carlo method is also called the random simulation method, is a powerful tool for system analysis and design. The principle is to randomly sample the variables that affect the performance index of the control system. The random sampling of each variable will generate a performance index value, and the probability distribution of the performance index can be obtained by repeating n times (such as 100).

Trajectory tracking algorithm always needs a vehicle model, but because the physical parameters, such as mass and tire lateral stiffness, cannot be accurately measured, we can only use a nominal value. However, when the algorithm is applied to a real vehicle, the actual value of vehicle parameters may differ from the nominal value selected in the algorithm, which will inevitably affect the control effect of the algorithm. For the above problems, Simulink blocks provided by Robust Control Toolbox are used to analyze the robustness of the algorithm. A vehicle model is established in Simulink, artificially limited the probability distribution of vehicle parameters. uncertain state space block and Matlab functions are used to generate random parameters conforming to the probability distribution. Finally, the algorithm is applied to the vehicle model. The robustness of the algorithm is analyzed by a large number of repeated simulation experiments. In this experiment, the experimental vehicle was asked to follow a dashed line, and one of the results is as shown in the Figure 3. The red line is the target trajectory and the black line is the actual trajectory. The mean square error of lateral position deviation was used as the evaluation index, and the experiment was repeated 100 times. As can be seen from the Figure 4, although the physical parameters change according to the probability distribution, the mean square deviation remains in a relatively stable state at about 1.0–1.3 $\times {10}^{-3}{\mathrm{m}}^2$. Therefore, the algorithm is excellent in robustness and has good prospects in practical applications.

4. Verification Experiment Results and Analysis

4.1. Hardware-In-The-Loop Experiment Platform Architecture

The hardware-in-the-loop (HiL) system used the simulation software Prescan and robot operating system (ROS). Prescan running on the host was used to build a simulation environment run in real-time. Prescan is widely used in physical simulation application platforms for automotive advanced assisted driving and autonomous driving systems. The tested machine employs a robot operating system, which has powerful functions, numerous interfaces, and strong portability, and has become a key tool for intelligent vehicle development.

An interface diagram of the HiL platform is shown in Figure 5. Prescan creates a simulated vehicle and simulates the characteristics of the controlled vehicle. The state variables X, Y, and θ are transmitted to the tested machine through the CAN bus. The software for the intelligent vehicle MPC trajectory tracking controller for is developed in C++. Real-time communication between the host and tested machine was carried out through the ROS bus and CAN bus.

For the hardware-in-the-loop experiment, an intelligent vehicle in Prescan was selected as the experiment object, and a two-degrees-of-freedom kinematic model was employed in the vehicle configuration. The vehicle kinematic model includes an engine, transmission, chassis, shift logic, and switching between automatic and manual shifting. Vehicle parameters like mass and wheelbase can be modified in the object configuration dialog. In this experiment, we selected only steering, throttle, and brake press as the inputs and set auto gear shifting to 1. The output of the vehicle kinematic model will be published on the Prescan bus.

4.2. HiLExperiment Configuration

The vehicle mass was M = 1104 kg and the vehicle wheelbase was L = 2.48 m. One of the difficulties we overcame was poor performance under conditions where curvatures changed rapidly or speeds were high. Therefore, we take the lane change and return maneuvers as an item. As shown in Figure 6, the solid red line is the target trajectory. Considering the adaptability of the algorithm at different speeds, the vehicle should be set to a total of nine speeds between 10–90 km/h.

The central processing unit of the trajectory tracking controller is an Intel Core i7-8700. The operating system is Ubuntu 16.04.4. The compiler uses release mode optimize program running efficiency. The main parameters of the NMPC tracker in the experiment are as follows: Discretization period$T=0.2\mathrm{s},$ prediction time horizon $n=25$, lateral deviation weight coefficient$k_1=1$, yaw angle deviation weight coefficient $k_2=500$, control input weighting coefficient $k_3=1000$, rotation wheel angle constraint${\delta }_{max}=0.6rad$, and rotation wheel angle control increment constraint$\mathsf{\Delta }{\delta }_{max}=0.04\mathrm{rad}$.

4.3. HiLExperiment Results

The actual trajectory is shown as the black dashed line in Figure 6, for a vehicle tracking the target trajectory at a speed of 30 km/h. The tracking lateral deviation mean square error is 8.7814 $\times {10}^{-4}{\mathrm{m}}^2$, which shows that the vehicle can track the target trajectory well. The mean calculation time is 6.7 ms. It is obvious from Figure 7 that the computing time for each step was within 100 ms, which illustrates that a real-time solution of the non-linear programming problem was realized under conditions of ensuring accuracy.

The lateral deviations at speeds of 10, 20, 30, and 60 km/h are shown in Figure 8. As vehicle speeds increase, the amplitude of the lateral deviation fluctuates gradually, and remains in a small range, which illustrates the stability of the algorithm at different speeds. The AB and CD segments had large curvatures relative to the overall path. When the vehicle entered these segments, lateral deviation increased slightly, but still fell within a low range, verifying the accuracy of the algorithm. Throughout the experiment, a total of nine speeds between 10–90 km/h were selected to complete trajectory tracking. The mean square errors for tests at different speeds were used as the evaluation index for trajectory tracking, as indicated by the solid line in Figure 9. As vehicle speeds increased, the steady-state mean square error values slowly increased. At low speeds between 0 and 30 km/h, the mean squared error values remained at approximately ${10}^{-4}{\mathrm{m}}^2$. At medium and high speeds, the mean squared error values remained at approximately ${10}^{-3} {\mathrm{m}}^2$. The steady-state mean square error values for different vehicle speeds reflect the algorithm’s good stability and trajectory tracking ability. During actual automatic driving, large deviations may cause accidents. The mean square error cannot be used to effectively evaluate special situations, so we take the max deviation as another evaluation index. It can be seen from Figure 9 that the maximum deviation points are less than 0.6 m which guarantees the driving safety. The maximum deviation points can be divided into two trend lines. This is referred to as an overlap for a nonlinear system, and is a potential subject for future research.

In order to compare the effects with other algorithms and prove the superiority of the MPC algorithm, we use the LQR algorithm to follow a similar target trajectory, and the effect is shown in Figure 10. The speed is set to be 30 km/h. It is clear that the vehicle cannot follow the target trajectory very well while using LQR algorithm. It is because LQR does not have a predictive model so that there will be a lag during the following process which causes a larger lateral deviation than MPC, as can be seen from Figure 11. Comparing the effects of MPC and LQR, it is convinced that MPC can guarantee the safety to some extent.

4.4. Real Vehicle Experiment

The NMPC algorithm proposed was also applied to an autonomous vehicle, which won third prize in the 2019 Intelligent Connected Vehicle Competition organized by the Guangzhou municipal government. The vehicle was developed by the South China University of Technology and is shown in Figure 12. Figure 13 shows the technical architecture which can be divided into three parts: Software, hardware, and vehicle. The software architecture significantly impacted control efficacy. The localization module and perception module could send data to the planning module to generate a local target trajectory. Ignoring changes in speed for the time being, lateral controls play a vital role in the control module. The NMPC algorithm proposed above was applied to the lateral control module. Finally, the first steering angle of the solution sequence was sent to the CAN bus.

Results and Analysis

There is a scene of entering a parking lot through continuous right-angle turns in the competition, as shown in Figure 14a. Figure 14b,c show the reference trajectory and the curvature of the trajectory. It can be seen that the curvature of the trajectory increases rapidly when making a right-angle turn. In order to follow the actual driving rules and guarantee the safety, the turning speed is limited to 8 km/h.

The result can be seen from Figure 14b that the vehicle can follow the reference trajectory during the whole process. Although there is slight deviation around 0.4 m at the lager turning rate, it is also within an allowable range, which meets the requirements of unmanned vehicle control. This also fully proves the algorithm’s ability to follow the trajectory with large curvature.

5. Conclusions

In this paper, a suitable discretization method was used to solve a real-time NMPC problem. Then, a SQP method was used for the solution. Monte Carlo simulations were performed to analyze the robustness, which is rare in autonomous vehicle trajectory tracking. The algorithm was verified with both a HiL simulation platform and a real vehicle, and the calculation accuracy and speed were well balanced. The experimental results showed that the algorithm could effectively solve trajectory tracking problems. The mean square error was maintained at approximately ${10}^{-3}{\mathrm{m}}^2$ and the calculation time was 100 ms. It can be concluded that this method can guarantee the accuracy and real-time performance of an algorithm during a driving process, which is of great significance in practical vehicle applications. However, there are still some aspects which can be improved in this paper. The vehicle model in Monte Carlo simulation can be more complex a small number of parameters were random sampled with the limitations of experimental conditions. And the application research and development are based on the three-degree-of-freedom nonlinear kinematics model. In fact, the vehicle is a high-degree-of-freedom nonlinear system. In the future, it can be extended to a dynamic vehicle model with higher degrees of freedom to explore the performance of algorithmic path tracking combined with vehicle dynamics. The experiment item is the lane change and return maneuvers, which is classic in a stability test, and we will test more complicated conditions on the basis of this experiment. And in the real vehicle experiment, the speed was kept at about 50 km/h considering the safety for all participants in the competition. More complex conditions and higher speeds will be tested in the future, and the overlap for a nonlinear system is a potential subject. In this paper, a feasible method and some problems are presented which may inspire other researchers and promote the development of unmanned vehicle trajectory tracking.