CN110989625B - A vehicle path tracking control method - Google Patents

Abstract

The invention discloses a vehicle path tracking control method. The method includes: S1, obtaining a new reference path with more dense path points according to the existing reference path points; S2, obtaining vehicle state information; S3, in the new reference path Find the nearest waypoint on the above; S4, take the nearest waypoint as the starting point, search for N preview points in front of the vehicle on the new reference path; S5, construct the prediction model, objective function and system constraints, according to the current measurement information and Prediction model, predict the future dynamics of the vehicle, solve the optimization problem that satisfies the objective function and constraint conditions online, and obtain the optimal control sequence composed of the expected front wheel steering angles corresponding to the N preview points; S6, according to the optimal control sequence , the vehicle is controlled until the next sampling time arrives, and when the next observation time arrives, steps S2 to S5 are repeated. The method provided by the invention has high tracking accuracy, and at the same time, it can also ensure the comfort in the control process, and does not produce a sudden change of the control quantity.

Description

A vehicle path tracking control method

technical field

The invention relates to the technical field of vehicle control, in particular to a vehicle path tracking control method.

Background technique

Unmanned vehicle path tracking control refers to controlling the steering angle of the vehicle through the underlying controller of the unmanned vehicle, so that the vehicle always travels along the desired path. Therefore, the path tracking ability of unmanned vehicles is an important guarantee for the safe driving of unmanned vehicles. The goal of path-following control is to reduce the deviation of the vehicle from the reference path while driving, and to ensure the smooth steering of the vehicle.

At present, the main path tracking control methods for unmanned vehicles mainly include fuzzy control, predictive control and model predictive control.

For example, the patent application with publication number CN107942663A proposes a fuzzy PID control method, which uses the fuzzy control algorithm to establish a fuzzy control rule table, and uses the PID control method to obtain proportional coefficients, differential coefficients and integral coefficients, and calculates the steering controller. control value. The control method provided in this patent document combines fuzzy control with PID control, but this method does not use an accurate physical model and does not add vehicle constraints, so it is difficult to achieve good tracking results in complex situations.

Another example: the patent application with the publication number CN109318905A proposes two control methods, one of which uses the kinematic constraints of the vehicle to establish a lateral control preview kinematic model of the vehicle, and selects a certain distance from the vehicle ahead of the vehicle to preview. The distance of the way point is used as the preview point, and then combined with the PID control method to control the vehicle laterally. However, this method can only select one preview point, which can only make the deviation error of the path near the selected single preview point smaller, but cannot take into account the tracking error of the entire path.

Another example: the patent application with the patent publication number CN108973769A proposes a control method based on model cybernetics (MPC, Model Predictive Control), and constructs a dynamic model of vehicle path tracking, which obtains the optimal control sequence by solving the model predictive controller. . However, this method only uses the deviation of the current vehicle and the path as the input condition, and does not make full use of the position and direction information of the entire path.

To sum up, none of the methods provided in the prior art can solve the problem of vehicle path tracking control on a large curvature path, so there is a need for a method that can fully utilize path information to improve path tracking accuracy.

SUMMARY OF THE INVENTION

An object of the present invention is to provide a vehicle path tracking control method to overcome or at least alleviate at least one of the above-mentioned drawbacks of the prior art.

In order to achieve the above object, the present invention provides a vehicle path tracking control method, which includes:

S1: Interpolate the existing reference path points to obtain a new reference path with more dense path points;

S2, obtain vehicle status information;

S3, traverse all the path points p _i (x _i , y _i ) on the new reference path obtained by S1, and find the nearest path point p ₀ ;

S4, on the new reference path obtained in S1, take the nearest path point p ₀ found in S3 as the starting point, and search for N preview points ahead of the vehicle according to the vehicle motion speed v(t) obtained in S2;

S5, construct the prediction model and the objective function shown in the following formula (5), predict the future dynamics of the vehicle according to the current vehicle state information and the prediction model obtained in S2, solve the optimization problem online that satisfies the objective function and the constraints, and obtain S4 The expected front wheel steering angle corresponding to each of the N preview points obtained from the search, and the desired front wheel steering angle corresponding to each of the N preview points constitutes the optimal formula (20). control sequence;

In formula (5):

Q is the weight coefficient;

Δδ(k+t|t) is the difference between the steering angle of the front wheels of the vehicle at the time k+1+t predicted at the time t and the steering angle of the front wheels of the vehicle at the last predicted time k+t;

e(k+t|t) is the distance between the vehicle rear axle center at time k+t predicted at time t and the tangent line of the kth preview point searched at time t, which is expressed as formula (6):

In formula (6):

x(k+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+t predicted at time t;

y(k+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+t at time t;

x _p (k) is the abscissa of the k-th preview point obtained by the search in S4;

y _p (k) is the ordinate of the k-th preview point obtained by searching in S4;

θ _p (k) is the heading angle of the k-th preview point obtained by the search in S4;

表示在t时刻的k+t时刻的前轮转向角；In formula (20),

Represents the steering angle of the front wheel at time k+t at time t;

控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点。S6, use

The vehicle is controlled until the next sampling time arrives, and when the next observation time t+1 arrives, steps S2 to S5 are repeated, and the problem is refreshed using the new vehicle state provided by S2, and this cycle is repeated until the end of the path is reached.

Further, "searching for N preview points ahead of the vehicle" in S4 specifically includes the following methods:

Taking the nearest path point p ₀ as the starting point, and taking v(t)ΔT as the search distance, search for N path points ahead of the vehicle along the new reference path as the preview points (x _p (k), y _p (k) )), k=1, 2, ..., N, ΔT is a discrete time step, when the search reaches the end point of the new reference path, the search is stopped.

Further, "searching for N preview points ahead of the vehicle" in S4 specifically includes the following methods:

Taking the nearest path point p ₀ as the starting point, and taking v(t)ΔT as the search distance, search for N path points ahead of the vehicle along the new reference path as the preview points (x _p (k), y _p (k) )), k=1, 2, ..., N, ΔT is a discrete time step, when the search reaches the set maximum number of prediction steps N, the search is stopped.

Further, S5 solves the optimization problem shown in the following equation (13) in each control cycle:

In formula (13):

minJ(k) is the minimum value of the objective function provided by equation (5);

x(k+1+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+1+t predicted at time t;

y(k+1+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+1+t at time t;

为在t时刻预测到的k+t时刻的车身航向角；

is the body heading angle at time k+t predicted at time t;

为在t时刻预测到的k+1+t时刻的车身航向角；

is the body heading angle at time k+1+t predicted at time t;

v(t) is the vehicle speed obtained by S2;

Δt is the time of each prediction step;

δ(k+t|t) is the steering angle of the front wheel predicted at time k+t at time t;

δ(k+1+t|t) is the front wheel steering angle predicted at time k+t+1 at time t;

L is the wheelbase of the vehicle;

k is the kth prediction step;

_Δmax is the maximum variation of the steering angle of the front wheels of the vehicle in adjacent moments;

_δmax is the maximum steering angle of the front wheel of the vehicle.

Further, before the optimization problem shown in equation (13) is solved, the specific calculation method of the initial value of the steering angle of the front wheel in the prediction time domain includes:

S51. Use the vehicle state information obtained in step S2, take the same step size ΔT as in step S4, and use the nearest path point p ₀ obtained in step S3 and step S4 and the first N-1 preview points searched as Stanley The method corresponds to the nearest path point at the moment (x _p (k), y _p (k)), k=1, 2, ..., N;

_S52 . _{Calculate the} lateral _{deviation e fa} ( t):

In formula (14), θ _p (0) is the heading angle of the path point p ₀ ;

S53. Calculate the angle error θ _e (t) of the heading angle of the vehicle body and the heading angle of the nearest waypoint at the current time t:

为车身航向角，θ_p(0)为路径点p₀的航向角；In formula (15),

is the heading angle of the vehicle body, and θ _p (0) is the heading angle of the waypoint p ₀ ;

S54. Calculate the expected steering angle δ′(t) of the front wheel at the current time t:

In formula (16):

K is the weight coefficient;

θ _e (t) is the angular error part of the desired steering angle δ′(t);

前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：S55. δ′(t) obtained by formula (16) and known vehicle rear axle center coordinates (x _r (t), y _r (t)), speed v, body heading angle

Front wheel steering angle δ(t) information and vehicle kinematics formulas (17) to (19) to obtain the vehicle state information at the next moment t+ΔT:

S56. Update the vehicle state with the vehicle state information obtained from equations (17) to (19), take the time t+ΔT as the current time, and select the first preview point (x _p (1), y _p (1) ) as the nearest path point, repeat steps S52 to S55, and calculate the expected steering angle δ′(t+k*ΔT) corresponding to each preview point after that, and use it as the δ( k+t|t) iteration initial value.

Further, the method of "closest path point p ₀ " in S3 includes:

Use formula (1) to calculate the square D _i of the distance between the path point p _i (x _i , y _i ) and the vehicle rear axle center (x _r (t), y _r (t)) at the current time t, and find the distance The path point with the smallest square D _i is denoted as the nearest path point p ₀ :

D _i =(x _r (t)-x _i ) ² +(y _r (t)-y _i ) ² (1).

Further, S1 adopts the cubic spline interpolation method, which specifically includes the following steps:

S11, select a path point on the reference path, and calculate the rotation angle of the coordinate system according to the yaw angle information of the path point adjacent to the selected path point in the geodetic coordinate system;

S12, the geodetic coordinate system is rotated, and the rotation direction angle is the coordinate system rotation direction angle calculated in step S11, and the new coordinates and the heading angle of the two described path points in the rotated S11 are calculated;

S13, according to the new coordinates and heading angles of the two path points in S11 calculated in S12, calculate and obtain a fitting cubic spline interpolation curve expression between the two path points;

S14, the abscissas of the cubic spline interpolation curve expression obtained in S13 are discretized at equal intervals to obtain high-density interpolation path points, and then obtain a new reference path. Among them, the first derivative of each interpolation path point is the heading angle information of this point;

S15, rotate the coordinate system of the interpolation path point in S14 to restore it to the geodetic coordinate system, and obtain the coordinates and heading angle information of the original adjacent path points and the interpolated path points in the geodetic coordinate system, wherein: the rotation direction is the same as that of S12. The rotation direction is opposite, and the size of the rotation angle is the rotation direction angle of the coordinate system calculated by S11.

Due to adopting the above technical solutions, the present invention has the following advantages: 1. When a path point is selected as a preview point, a parseable path is not required, and even discrete path points and non-analyzable paths can be well Find the preview point, which is more in line with the actual working conditions in engineering applications. 2. At the same time, in terms of the number of selected preview points, compared with the traditional control method based on preview, only one preview point can be selected. This method adopts the method of multi-point preview. A single preview point can predict longer steps and distances, and can take into account the tracking error of the entire path, thereby improving the tracking accuracy, and at the same time ensuring the comfort in the control process, and will not produce sudden changes in the control amount . 3. The combination of preview control and model predictive control can take into account the uncertainty caused by model mismatch, time-varying, interference, etc., and make up for it in time, always base the latest optimization on the actual basis, and maintain the actual the optimum.

Description of drawings

1 is a schematic flowchart of a vehicle path tracking control method provided by an embodiment of the present invention;

Fig. 2 is a schematic flowchart of the cubic spline interpolation method provided by the step of reference path point interpolation in Fig. 1;

3 is a schematic diagram of path tracking provided by an embodiment of the present invention;

4 is a schematic diagram of the principle of obtaining the closest way point provided by the step of calculating the closest way point in FIG. 1;

FIG. 5 is a schematic structural diagram of the path tracking controller provided in the step of calculating the closest path point in FIG. 1 .

Detailed ways

The present invention will be described in detail below with reference to the accompanying drawings and embodiments.

As shown in FIG. 1 , the vehicle path tracking control method provided by the embodiment of the present invention includes the following steps:

S1: Interpolate the existing reference path points to obtain a new reference path with more dense path points. Wherein, the "reference path" can be obtained, for example, by recording the path with a GPS system. However, the obtained reference path may be discrete sparse path points. In order to obtain more dense path points to ensure tracking accuracy, this embodiment uses a cubic spline interpolation method to compare the starting path points on the reference path with the target path. All the waypoints between the points are interpolated to obtain the coordinates (x, y) and heading angle _θp of each waypoint on the reference path from the starting point to the target point in the geodetic coordinate system (as shown in Figure 2). ). The cubic spline interpolation method specifically includes the following steps:

S11, select a waypoint on the reference path, and calculate the rotation angle of the coordinate system according to the yaw angle information of the waypoint adjacent to the selected waypoint in the geodetic coordinate system.

S12, the geodetic coordinate system is rotated, and the rotation direction angle is the coordinate system rotation direction angle calculated in step S11, and the new coordinates and heading angles of the two path points in the rotated S11 are calculated.

S13, according to the new coordinates and heading angles of the two path points in S11 calculated in S12, calculate and obtain the fitting cubic spline interpolation curve expression f(x)=a(xx between the two path points ′ _i-1 ) ³ +b(xx′ _i-1 ) ² +c(xx′ _i-1 )+d, where i-1 and i represent the serial numbers of the two path points in S11, respectively, x' _i-1 represents the abscissa of the i-1th point in the rotated coordinate system, a, b, c, d are the cubic splines between the two path points in S11 in the rotated coordinate system The coefficient of the interpolation curve expression is the parameter to be calculated in S13.

S14, the abscissas of the cubic spline interpolation curve expression obtained in S13 are discretized at equal intervals to obtain high-density interpolation path points, and then obtain a new reference path. Among them, the first derivative of each interpolation path point is the heading angle information of the point.

S15, rotate the coordinate system of the interpolation path point in S14 to restore it to the geodetic coordinate system, and obtain the coordinates and heading angle information of the original adjacent path points and the interpolated path points in the geodetic coordinate system, wherein: the rotation direction is the same as that of S12. The rotation direction is opposite, and the size of the rotation angle is the rotation direction angle of the coordinate system calculated by S11.

和前轮转向角δ(t)。S2, obtain vehicle state information. Figure 3 shows the position and reference path of the vehicle. As shown in Figure 3, the coordinates of the center of the rear axle of the vehicle at the current observation time t are obtained through the GPS sensor, inertial measurement unit (IMU) and other sensors installed on the vehicle. (x _r (t), y _r (t)) and velocity v(t), body heading angle

and the front wheel steering angle δ(t).

S3, calculate the closest path point to the center of the rear axle of the vehicle on the new reference path obtained in S1.

As shown in Figure 4, traverse all the waypoints p _i ( _xi , y _i ) on the new reference path, and use formula (1) to calculate the path point p _i ( _xi , y _i ) and the back of the vehicle at the current time t The distance square D _i of the axis center (x _r (t), y _r (t)), and find the path point with the smallest distance square D _i , which is recorded as the nearest path point p ₀ :

D _i =(x _r (t)-x _i ) ² +(y _r (t)-y _i ) ² (1).

S4, on the new reference path obtained in S1, taking the nearest path point p ₀ found in S3 as the starting point, and according to the vehicle motion speed v(t) obtained in S2, search for N preview points ahead of the vehicle. Here, "toward the front of the vehicle" refers to the front of the vehicle.

Among them: "Searching for N preview points ahead of the vehicle" in S4 specifically includes the following methods:

Taking the nearest waypoint p ₀ as the starting point, and at the same time, taking v(t)ΔT as the search distance, search for N waypoints ahead of the vehicle as preview points (x _p (k), y _p (k)), k =1,2,...,N. That is, the distance between the first preview point (x _p (1), y _p (1)) and the nearest path point p ₀ along the new reference path is v(t)ΔT, and the second preview point (x The distance between _p (2), y _p (2)) and the first preview point (x _p (1), y _p (1)) along the new reference path is v(t)ΔT, and so on, the i-th The distance between the preview point (x _p (j), y _p (j)) and the i-1th preview point (x _p (j-1), y _p (j-1)) along the new reference path is v(t)ΔT, the Nth preview point (x _p (N), y _p (N)) and the N-1th preview point (x _p (N-1), y _p (N-1) ) along the new reference path is v(t)ΔT, where ΔT is the discrete time step.

In the above embodiment, when the search reaches the end point of the new reference path, the search is stopped. Of course, the search operation can also be controlled according to the set number of predicted steps, for example: when the search reaches the set maximum number of predicted steps N, the search is stopped.

S5, build a model predictive controller. The basic schematic diagram of the model predictive controller is shown in Figure 5. The model predictive controller includes a predictive model, an objective function and system constraints. According to the current vehicle state information and the predictive model, it predicts the future dynamics of the vehicle and solves it online. The optimization problem that satisfies the objective function and constraints, obtains the optimal control sequence of the steering angle of the front wheel corresponding to each of the N preview points (x _p (k), y _p (k)) obtained by the S4 search .

According to the vehicle kinematics formula, the prediction model is set as formula (2) to formula (4):

In formula (2) to formula (4):

x(k+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+t predicted at time t;

x(k+1+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+1+t predicted at time t;

y(k+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+t at time t;

y(k+1+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+1+t at time t;

为在t时刻预测到的k+t时刻的车身航向角；

is the body heading angle at time k+t predicted at time t;

为在t时刻预测到的k+1+t时刻的车身航向角；

is the body heading angle at time k+1+t predicted at time t;

v(t) is the vehicle speed obtained by S2;

Δt is the time of each prediction step;

δ(k+t|t) is the steering angle of the front wheel of the vehicle at time k+t predicted at time t;

L is the wheelbase of the vehicle;

k is the kth prediction step.

According to the requirements of vehicle tracking accuracy and stability, the objective function is set to formula (5):

In formula (5):

Q is a weight coefficient, which needs to be obtained according to actual experiments and simulations. In this embodiment, Q=150 is taken.

e(k+t|t) is the distance between the center of the rear axle of the vehicle at time k+t predicted at time t and the tangent line of the k-th preview point searched at time t, that is, the distance e _fa shown in Figure 3, This distance represents the lateral deviation between the vehicle and the reference path. In the optimization process, e(k+t|t) should be minimized as much as possible to ensure the accuracy of vehicle tracking. The calculation formula of e(k+t|t) is formula (6):

In formula (6):

x(k+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+t predicted at time t;

y(k+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+t at time t;

x _p (k) is the abscissa of the k-th preview point obtained by the search in S4;

y _p (k) is the ordinate of the k-th preview point obtained by searching in S4;

θ _p (k) is the heading angle of the k-th preview point obtained by the search in S4.

Δδ(k+t|t) is the difference between the steering angle of the front wheels of the vehicle at time k+1+t predicted at time t and the steering angle of the front wheels of the vehicle at the previous predicted time k+t. This target is introduced for the purpose of Under the condition of ensuring the tracking accuracy, the variation range of the steering angle of the front wheel is limited, so as to prevent the steering wheel from shaking greatly and ensure the stability of the tracking. The calculation formula of Δδ(k+t|t) is formula (7):

Δδ(k+t|t)=δ(k+1+t|t)-δ(k+t|t) (7)

In formula (7):

δ(k+t|t) is the steering angle of the front wheel predicted at time k+t at time t;

δ(k+1+t|t) is the front wheel steering angle predicted at time k+t+1 at time t.

Since the vehicle is a mechanical system, there are some structural constraints, and the steering angle of the steering wheel cannot exceed the maximum allowable steering angle. At the same time, the vehicle as a vehicle needs to ensure the comfort of the passengers, and the steering angle change rate of the steering wheel should not be too large. In view of this , the feasible state and control variable space constraints of equation (6) are set as equations (8) to (12):

_-Δmax ≤δ(k+1+t|t)-δ(k+t|t) _≤Δmax (11)

-δ _max ≤δ(k+t|t)≤δ _max (12)

In equations (8) to (12), _Δmax is the maximum change in the steering angle of the front wheel in adjacent moments, and _δmax is the maximum steering angle of the front wheel of the vehicle, which is a limitation of the mechanical structure.

Solve the optimization problem in each control cycle:

According to the objective function and constraints established in the previous steps, the constrained optimization problem that the model predictive controller needs to solve in each cycle is as follows:

In equation (13), minJ(k) is the minimum value of the objective function provided by equation (5).

Before solving the optimization problem, use the Stanley method to calculate the initial value of the front wheel steering angle in the prediction time domain through the vehicle kinematics model simulation. The specific calculation method is as follows:

S51. Use the vehicle state information obtained in step S2, take the same step size ΔT as in step S4, and use the nearest path point p ₀ obtained in step S3 and step S4 and the first N-1 preview points searched as Stanley The method corresponds to the nearest waypoint at the moment (x _p (k), y _p (k)), k=1, 2, . . . , N.

_S52 . _{Calculate the} lateral _{deviation e fa} ( t):

In formula (14), θ _p (0) is the heading angle of the path point p ₀ .

S53. Calculate the angle error θ _e (t) of the heading angle of the vehicle body and the heading angle of the nearest waypoint at the current time t:

为车身航向角，θ_p(0)为路径点p₀的航向角。In formula (15),

is the body heading angle, and θ _p (0) is the heading angle of the waypoint p ₀ .

S54. Calculate the expected steering angle δ′(t) of the front wheel at the current time t:

In formula (16):

K is a weight coefficient, and the weight coefficient needs to be adjusted according to different situations. The specific method is to obtain a better weight coefficient by testing and simulation. In this embodiment, K=1;

θ _e (t) is the angular error part of the desired steering angle δ′(t), which is calculated as shown in formula (15);

前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：S55. δ′(t) obtained by formula (16) and known vehicle rear axle center coordinates (x _r (t), y _r (t)), speed v(t), body heading angle

Front wheel steering angle δ(t) information and vehicle kinematics formulas (17) to (19) to obtain the vehicle state information at the next moment t+ΔT:

S56. Update the vehicle state with the vehicle state information obtained from equations (17) to (19), take the time t+ΔT as the current time, and select the first preview point (x _p (1), y _p (1) ) as the nearest path point, repeat steps S52 to S55, and calculate the expected steering angle δ′(t+k*ΔT) corresponding to each preview point after that, and use it as the δ( k+t|t) iteration initial value.

The reference of the initial value can improve the convergence speed in the solution process and the success rate of obtaining the optimal solution, and avoid the problem from falling into local optimum.

According to the initial value of the steering angle of the front wheel in the predicted time domain obtained by solving steps S51 to S56, the optimization problem provided by the formula (13) is solved, and then each of the N preview points (x _p (k) is obtained. ), y _p (k)) corresponding to the control amount of the front wheel steering angle, that is, the expected front wheel steering angle corresponding to each of the N preview points (x _p (k), y _p (k)) , the expected front wheel steering angle corresponding to each of the N preview points constitutes the optimal control sequence represented by formula (20):

表示在t时刻预测的k+t时刻的前轮转向角。In formula (20),

Indicates the front wheel steering angle at time k+t predicted at time t.

In the above optimal control sequence, the first element is used as the control amount of this step in N steps.

控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点。S6, use

The vehicle is controlled until the next sampling time arrives, and when the next observation time t+1 arrives, steps S2 to S5 are repeated, and the problem is refreshed using the new vehicle state provided by S2, and this cycle is repeated until the end of the path is reached.

Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them. Those of ordinary skill in the art should understand that: the technical solutions described in the foregoing embodiments can be modified, or some technical features thereof can be equivalently replaced; these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the various aspects of the present invention. The spirit and scope of the technical solutions of the embodiments.

Claims (6)

1. a vehicle path tracking control method, is characterized in that, comprises: S1: Interpolate the existing reference path points to obtain a new reference path with more dense path points; S2, obtain vehicle status information; S3, traverse all the path points p _i (x _i , y _i ) on the new reference path obtained by S1, and find the nearest path point p ₀ ; S4, on the new reference path obtained in S1, taking the nearest path point p ₀ found in S3 as the starting point, and searching for N previews ahead of the vehicle according to the vehicle motion speed v(t) in the vehicle state information obtained in S2 point; S5, construct the prediction model represented by equations (2) to (4) and the objective function shown in the following equation (5), predict the future dynamics of the vehicle according to the current vehicle state information and the prediction model obtained in S2, and solve the online solution to meet the requirements Describe the optimization problem of the objective function and constraints, obtain the expected front wheel steering angle corresponding to each of the N preview points obtained by the S4 search, and the expected front wheel corresponding to each of the N preview points. The steering angle constitutes the optimal control sequence represented by formula (20);

In formula (5): Q is the weight coefficient; Δδ(k+t|t) is the difference between the steering angle of the front wheels of the vehicle at the time k+1+t predicted at the time t and the steering angle of the front wheels of the vehicle at the previous predicted time k+t; e(k+t|t) is the distance between the vehicle rear axle center at time k+t predicted at time t and the tangent line of the kth preview point searched at time t, which is expressed as formula (6):

In formula (6): x(k+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+t predicted at time t; y(k+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+t at time t; x _p (k) is the abscissa of the k-th preview point obtained by the search in S4; y _p (k) is the ordinate of the k-th preview point obtained by searching in S4; θ _p (k) is the heading angle of the k-th preview point obtained by the search in S4;

In formula (20),

Represents the steering angle of the front wheel predicted at time k+t at time t; The optimization problem shown in Equation (13) below is solved in each control cycle:

In formula (13): minJ(k) is the minimum value of the objective function provided by equation (5); x(k+1+t|t) is the abscissa of the center point of the rear axle of the vehicle at time k+1+t predicted at time t; y(k+1+t|t) is the ordinate of the center point of the rear axle of the vehicle predicted at time k+1+t at time t;

is the body heading angle at time k+t predicted at time t;

is the body heading angle at time k+1+t predicted at time t; v(t) is the vehicle speed obtained by S2; Δt is the time of each prediction step; δ(k+t|t) is the steering angle of the front wheel predicted at time k+t at time t; δ(k+1+t|t) is the front wheel steering angle predicted at time k+t+1 at time t; L is the wheelbase of the vehicle; k is the kth prediction step; _Δmax is the maximum variation of the steering angle of the front wheels of the vehicle in adjacent moments; _δmax is the maximum steering angle of the front wheel of the vehicle; S6, use

The vehicle is controlled until the next sampling time arrives, and when the next observation time t+1 arrives, steps S2 to S5 are repeated, and the problem is refreshed using the new vehicle state provided by S2, and the cycle is repeated until the end of the path is reached. 2. The vehicle path tracking control method according to claim 1, wherein the "searching for N preview points ahead of the vehicle" in S4 specifically comprises the following methods: Taking the nearest path point p ₀ as the starting point, and taking v(t)ΔT as the search distance, search for N path points ahead of the vehicle along the new reference path as the preview points (x _p (k), y _p (k) )), k=1, 2, ..., N, ΔT is a discrete time step, when the search reaches the end point of the new reference path, the search is stopped. 3. The vehicle path tracking control method according to claim 1, wherein the "searching for N preview points ahead of the vehicle" in S4 specifically comprises the following methods: Taking the nearest path point p ₀ as the starting point, and taking v(t)ΔT as the search distance, search for N path points ahead of the vehicle along the new reference path as the preview points (x _p (k), y _p (k) )), k=1, 2, ..., N, ΔT is a discrete time step, when the search reaches the set maximum number of prediction steps N, the search is stopped. 4. The vehicle path tracking control method according to claim 2 or 3, characterized in that, before the optimization problem shown in equation (13) is solved, the specific calculation method of the initial value of the front wheel steering angle solution iteration in the prediction time domain include: S51. Use the vehicle state information obtained in step S2, take the same step size ΔT as in step S4, and use the nearest path point p ₀ obtained in step S3 and step S4 and the first N-1 preview points searched as Stanley The method corresponds to the nearest path point at the moment (x _p (k), y _p (k)), k=0, 2, ..., N-1; _S52 . _{Calculate the} lateral _{deviation e fa} ( t):

In formula (14), θ _p (0) is the heading angle of the path point p ₀ ; S53. Calculate the angle error θ _e (t) of the heading angle of the vehicle body and the heading angle of the nearest waypoint at the current time t:

In formula (15),

is the heading angle of the vehicle body, and θ _p (0) is the heading angle of the waypoint p ₀ ; S54. Calculate the expected steering angle δ′(t) of the front wheel at the current time t:

In formula (16): K is the weight coefficient; θ _e (t) is the angular error part of the desired steering angle δ′(t); S55. δ′(t) obtained by formula (16) and known vehicle rear axle center coordinates (x _r (t), y _r (t)), speed v, body heading angle

Front wheel steering angle δ(t) information and vehicle kinematics formulas (17) to (19) to obtain the vehicle state information at the next moment t+ΔT:

S56. Update the vehicle state with the vehicle state information obtained from equations (17) to (19), take the time t+ΔT as the current time, and select the first preview point (x _p (1), y _p (1) ) as the nearest path point, repeat steps S52 to S55, and calculate the expected steering angle δ′(t+k*ΔT) corresponding to each preview point after that, and use it as the δ( k+t|t) iteration initial value. 5. The vehicle path tracking control method according to claim 1, 2 or 3, wherein the method for "the closest path point p ₀ " in S3 comprises: Use formula (1) to calculate the square D _i of the distance between the path point p _i (x _i , y _i ) and the vehicle rear axle center (x _r (t), y _r (t)) at the current time t, and find the distance The path point with the smallest square D _i is denoted as the nearest path point p ₀ : D _i =(x _r (t)-x _i ) ² +(y _r (t)-y _i ) ² (1). 6. The vehicle path tracking control method according to claim 1, 2 or 3, wherein S1 adopts a cubic spline interpolation method, which specifically comprises the following steps: S11, select a path point on the reference path, and calculate the rotation angle of the coordinate system according to the yaw angle information of the path point adjacent to the selected path point in the geodetic coordinate system; S12, the geodetic coordinate system is rotated, and the rotation direction angle is the coordinate system rotation direction angle calculated in step S11, and the new coordinates and the heading angle of the two described path points in the rotated S11 are calculated; S13, according to the new coordinates and the heading angle of the two path points in S11 calculated in S12, calculate the fitting cubic spline interpolation curve expression between the two path points; S14, the abscissa of the cubic spline interpolation curve expression obtained in S13 is discretized at equal intervals to obtain high-density interpolation path points, and then a new reference path is obtained, wherein the first derivative of each interpolation path point is the heading of the point corner information; S15, rotate the coordinate system of the interpolation path point in S14 to restore it to the geodetic coordinate system, and obtain the coordinates and heading angle information of the original adjacent path points and the interpolated path points in the geodetic coordinate system, wherein: the rotation direction is the same as that of S12. The rotation direction is opposite, and the size of the rotation angle is the rotation direction angle of the coordinate system calculated by S11.

Images