A Trajectory Tracking Method for High-Speed Autonomous Vehicles Based on Model Predictive Control

Abstract

The present invention relates to a trajectory tracking method for a high-speed autonomous vehicle based on predictive control, specifically including: establishing a vehicle model; establishing a vehicle convex polytope model including tire cornering stiffness and longitudinal speed based on the vehicle model; the autonomous vehicle performs trajectory planning, fits the reference trajectory based on the Bezier curve, and calculates the road curvature in real time; constructing a robust objective function expressing trajectory tracking error and stability constraints based on the vehicle convex polytope model; constructing a discrete neural network model to solve the objective function, obtaining the optimal control quantity, and inputting it to the vehicle for execution. The above-mentioned trajectory tracking method for a high-speed autonomous vehicle based on fast robust model predictive control improves the solution efficiency of the control algorithm, realizes that the autonomous vehicle can maintain a good trajectory tracking effect during high-speed driving, and ensures the stable driving of the vehicle.

Description

Technical Field

[0001] The present invention relates to an automobile trajectory tracking method in the field of autonomous vehicle research, and particularly to a trajectory tracking method for high-speed autonomous vehicles based on fast robust model predictive control. Background Art

[0002] The deep integration of new-generation information and communication technologies and advanced manufacturing technologies has made digitalization, networking, and intelligence the main future development trends in the automotive industry. Compared with traditional vehicles, autonomous vehicles have significant advantages in reducing traffic accidents, improving travel efficiency and safety. Currently, autonomous vehicles can basically drive at low speeds in simple road environments. How to enable autonomous vehicles to drive safely, stably, at high speeds, and avoid dangers in complex road environments is an important goal for the future development of autonomous vehicle technology.

[0003] Different from low-speed operating conditions, when autonomous vehicles drive in complex operating conditions such as high-speed conditions, the requirements for the accuracy of vehicle models and the uncertainty of vehicle dynamics systems are more stringent, and the real-time performance of control algorithms is also difficult to meet the driving requirements of autonomous vehicles in high-speed conditions. In addition, the influence of road factors such as road curvature and road surface adhesion conditions also increases the difficulty of trajectory tracking control. This makes the stability trajectory tracking control method for autonomous vehicles driving in complex operating conditions extremely challenging. Summary of the Invention

[0004] To solve the technical problem of poor driving stability of autonomous vehicles in complex operating conditions such as high-speed conditions, the present invention provides a trajectory tracking method for high-speed autonomous vehicles based on fast robust model predictive control.

[0005] The present invention is implemented by the following scheme. A trajectory tracking method for high-speed autonomous vehicles based on fast robust model predictive control includes the following steps:

[0006] Step 1: Establish a vehicle model

[0007]

[0008] Among them, ξ(t) represents the state vector at time t, u1(t) represents the front-wheel steering angle input vector at time t, and u2(t) represents the disturbance input vector at time t; A(t), B(t), and C(t) are all Jacobian matrices, which are respectively:

[0009]

[0010] C(t) = [0 0 0 0 0 -v x T , ​

[0011] In the formula, C αf represents the cornering stiffness of the front tire, and C αr represents the cornering stiffness of the rear tire. l f is the distance from the vehicle's center of mass to the front axle, and l r is the distance from the vehicle's center of mass to the rear axle. v x is the longitudinal speed at the vehicle's center of mass, m is the vehicle mass, and I z is the moment of inertia of the vehicle about the Z-axis;

[0012] Step Two: Based on the vehicle model Establish a discrete vehicle convex polytope model ξ:

[0013] ξ(t + 1) = A'(t)ξ(t) + B'(t)u1(t) + C'(t)u2(t)

[0014] where u1(t) is the front wheel steering angle δ f changing with time t, that is, u1 = δ f , and u2(t) is the road curvature k2 changing with time t, that is, u2 = κ2. A1(t), B1(t), and C1(t) are all Jacobian matrices changing with time t, respectively: I is the identity matrix;

[0015] Step Three: Calculate the road curvature k2:

[0016]

[0017] where x′ = x c - x a , y′ = y c - y a , x″ = x c + x a - 2x b , y″ = y c + y a - 2y b , η = (x′) 2 +(y′) 2 , (x a , y a ), (x b , y b ), (x c , y c ) are three interpolation points of each fitting section of the road;

[0018] Step Four: Construct a robustness objective function for expressing the trajectory tracking error Δ u1(t) and the stability constraint min

[0019]

[0020] |u1(k+i|k)|≤u 1,max ,

[0021] |Δu1(k+i|k)|≤Δu 1,max ,

[0022]

[0023] |β(k)|≤β ss (k),

[0024] e y,min (k)-d s ≤e y (k)≤e y,max (k)-d s ,

[0025] k = 1, 2, …, N c

[0026] where χ p (k+i|k) is the predicted value of the control output, χ ref (k+i|k) is the reference value of the control output, (k+i|k) represents the value predicted at the (k + i)-th moment based on the information at the k-th sampling moment, Q and R are weight matrices, N p is the prediction horizon, N c is the control horizon, ρ is the weight coefficient, ε is the weight factor, Δu1(k) = u1(k) - u1(k - 1);

[0027] Step 5: Construct a discrete neural network model to solve the objective function, obtain the optimal control quantity, and input it to the vehicle for execution;

[0028] where constructing a discrete neural network model to solve the objective function includes:

[0029] Converting the objective function into a quadratic programming problem:

[0030]

[0031] s.t.l 0 ≤x op ≤h 0

[0032] l 1 ≤Wx op ≤h 1 ,

[0033] In the formula, x op represents the optimal solution quantity, H = Θ TQΘ + R, f = Θ T Q(Ψξ(k) + ΥU2 - χ ref ), where l and h are stability constraint conditions; W represents the state extraction matrix;

[0034] The projection equation of the discrete neural network can be expressed as:

[0035] y(k + 1) = y(k) + μΛ{G Z [Ny(k) - (Dy(k) + K)] - My(k)},

[0036] x(k) = [I n 0 n×n 0 n×m y(k),

[0037] where y(k) is the equilibrium point of the projection equation at time k, x(k) is the optimal control quantity of the objective function at time k, and μ is the scaling factor;

[0038]

[0039] I n and I m are identity matrices.

[0040] As a further improvement of the above solution, the method for establishing the vehicle model includes the following steps:

[0041] Establish a vehicle dynamics model, expressed as:

[0042]

[0043] where v y is the lateral velocity at the vehicle's center of mass, is the yaw rate, β is the sideslip angle at the center of mass, and δ f is the front wheel steering angle;

[0044] Establish a vehicle tracking error model, expressed as:

[0045]

[0046] where the lateral position error e y is the distance between the projection points of the center of the vehicle's rear axle on the road center line, and the heading error is the angle between the tangent of the road center line and the road ground coordinate system, and κ is the curvature of the reference path;

[0047] Establish the vehicle model according to the vehicle dynamics model and the vehicle tracking error model.

[0048] As a further improvement of the above solution, the method for establishing the discrete vehicle convex polyhedron model includes the following steps:

[0049] Construct a vehicle convex polyhedron model with 2 vertices that suppresses the non-linear characteristics of the tire cornering stiffness and the vehicle longitudinal speed. The time-varying variables of the parameter matrix at the vertices can be expressed as: 3 Vertices of the vehicle convex polyhedron model, the time-varying variables of the parameter matrix at the vertices can be expressed as:

[0050]

[0051] v x,min and v x,max are the minimum and maximum values of the longitudinal speed at the vehicle's center of mass, respectively;

[0052] C αf,min and C αf,max are the minimum and maximum values of the tire cornering stiffness of the front wheels, respectively;

[0053] C αr,min and C αr,max are the minimum and maximum values of the tire cornering stiffness of the rear wheels, respectively;

[0054] The non-linear parameters in the vehicle model are linearly combined with the parameter values at the convex polyhedron vertices as:

[0055]

[0056] In the formula, i2 is the number of vertices of the convex polyhedron, and are correction coefficients, where

[0057]

[0058] m = 1, 2, n = 1, 2, j = 1, 2,

[0059] Replace the tire cornering stiffness and the vehicle longitudinal speed in the Jacobian matrix of the vehicle model to obtain the convex polyhedron state space matrix (A i2 (t), B i2 (t), C i2 (t)) of the i2-th convex polyhedron vertex, i2 = 2 3 ;

[0060] Discretize the state space model at the convex polyhedron vertices using the first-order difference quotient method to obtain the discrete vehicle convex polyhedron model.

[0061] As a further improvement of the above solution, the method for calculating the road curvature k includes the following steps:

[0062] The reference trajectory is represented by fitting with a third-order Bezier curve as:

[0063] q(τ i1 ) = (1 - τ i1 ) 3 P0 + 3τ i1 (1 - τ i1 ) 2 P1 + 3τ i1 2 (1 - τ i1 ) 2 P2 + τ i1 3 P3,

[0064] where q(τ i1 ) is the cubic Bezier curve fitting of the parameter τ at the i1 - th interpolation point of the reference trajectory, P i1 is the k1 - th control point of the reference trajectory. By taking values of the parameter τ k1 within [0, 1], any number of interpolation points are generated between the first control point and the last control point; i1

[0065] Calculate the road curvature according to the interpolation points of each fitting section.

[0066] As a further improvement of the above - mentioned solution, the trajectory tracking method for high - speed autonomous vehicles further includes the steps of constructing a prediction model of the objective function and constraint conditions:

[0067] Construct a prediction model according to the discrete vehicle convex polyhedron model;

[0068] Establish a stability control boundary composed of yaw rate and sideslip angle of the center of mass, and perform stability constraints on the yaw rate and sideslip angle of the center of mass;

[0069] Establish a boundary of the feasible road area and perform constraints on the lateral displacement error;

[0070] Perform constraints on the control input quantity.

[0071] Preferably, the prediction model includes:

[0072] Construct a new state vector ξ(k|t) = [ξ(k) u1(k - 1)] T , and obtain a new state - space equation according to the discrete vehicle convex polyhedron model:

[0073]

[0074] where Δu1(k) = u1(k) - u1(k - 1), I is the identity matrix;

[0075] ​Perform state prediction according to the new state space equation to obtain the prediction model at future moments:

[0076] Y = Ψξ(k) + ΘΔU1 + ΥU2,

[0077] where Y = [ξ(k + 1) ξ(k + 2) … ξ(k + N c ) … ξ(k + N p )] T ,

[0078]

[0079]

[0080] ΔU1 = [Δu1(k) Δu1(k + 1) … Δu1(k + N c )] T ,

[0081] U2 = [u2(k) u2(k + 1) … u2(k + N c )] T .

[0082] More preferably, the stability control boundary of the yaw rate is;

[0083]

[0084] where the threshold of the rear wheel side slip angle α r,ss is [-α r,lim , α r,lim ;

[0085] The stability control boundary of the center of mass side slip angle is:

[0086]

[0087] |β(k)| ≤ β ss (k).

[0088] Furthermore, the boundary of the feasible road area is expressed as:

[0089] e y,min (k) - d s ≤ e y (k) ≤ e y,max (k) - d s ,

[0090] where e y,min (k), e y,min (k) are the minimum lateral position error and the maximum lateral position error varying with time respectively, and d s is the safety distance defined according to the vehicle body size.

[0091] Among them, the control input quantity constraint condition is:

[0092] |u1(k+i|k)|≤u 1,max ,

[0093] |Δu1(k+i|k)|≤Δu 1,max ,

[0094] i = 1, 2, …, N c 。

[0095] As a further improvement of the above solution, a discrete neural network model is constructed to solve the objective function, including:

[0096] The objective function is transformed into a quadratic programming problem:

[0097]

[0098] s.t.l 0 ≤x op ≤h 0

[0099] l 1 ≤Wx op ≤h 1 ,

[0100] In the formula, H = Θ T QΘ + R, f = Θ T Q(Ψξ(k) + ΥU2 - χ ref ), l and h are stability constraint conditions;

[0101] The discrete neural network projection equation can be expressed as:

[0102] y(k + 1) = y(k) + μΛ{G Z [Ny(k) - (Dy(k) + K)] - My(k)},

[0103] x(k) = [I n 0 n×n 0 n×m y(k),

[0104] In the formula, y(k) is the equilibrium point of the projection equation, x(k) is the optimal control quantity of the objective function, μ is the scaling factor,

[0105]

[0106] I n and I m are identity matrices.

[0107] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0108] 1. For the above-mentioned high-speed autonomous vehicle trajectory tracking method based on fast robust model predictive control, a vehicle convex polytope model including tire cornering stiffness and vehicle longitudinal speed is constructed, and a discrete neural network model is constructed, which improves the robustness of the control algorithm, effectively suppresses the influence of vehicle dynamics system uncertainty on trajectory tracking performance and stability, and realizes that when an autonomous vehicle is driving, especially when driving in complex road environments such as high speed and ice and snow roads, it can maintain a good trajectory tracking effect and ensure the stable driving of the vehicle;

[0109] 2. The method for constructing a discrete neural network to solve the model predictive control objective function proposed by the present invention improves the solution efficiency of the control algorithm, reduces the requirements of the control algorithm for system hardware, and reduces the occupation of computing resources, providing a new way for real-time solving of complex model predictive control. BRIEF DESCRIPTION OF THE DRAWINGS

[0110] Figure 1 is a flowchart of a high-speed autonomous vehicle trajectory tracking method based on fast robust model predictive control for an embodiment;

[0111] Figure 2 is a vehicle dynamics model diagram;

[0112] Figure 3 is a vehicle tracking error model diagram;

[0113] Figure 4 is an overall strategy diagram of stability path tracking control;

[0114] Figure 5 is a feasible road area envelope boundary diagram. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0115] In order to make the objectives, technical solutions and advantages of the present invention clearer, the following further describes the present invention in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0116] This embodiment is as Figure 1 shown. The high-speed autonomous vehicle trajectory tracking method based on fast robust model predictive control includes the following steps:

[0117] Step S110, establish a vehicle model of an autonomous vehicle including a vehicle dynamics model and a tracking error model.

[0118] In this embodiment, a vehicle body coordinate system xyz is established at the vehicle's center of gravity. The origin of the coordinate system coincides with the vehicle's center of mass. The x-axis is parallel to the ground and coincides with the vehicle's longitudinal driving direction. The y-axis is parallel to the ground and coincides with the vehicle's lateral driving direction. The z-axis is perpendicular to the ground. The vehicle's single-track yaw dynamics model is as follows Figure 2 As shown, considering that trajectory tracking is mainly lateral motion control and assuming that the vehicle's longitudinal speed remains unchanged, focusing on the vehicle's motion along the y-axis and rotation around the z-axis, the vehicle's yaw dynamics model is obtained as follows:

[0119]

[0120] In the formula, m is the vehicle mass, v y is the longitudinal speed of the center of mass in the vehicle body coordinate system, I z is the moment of inertia of the vehicle about the z-axis, l f and l r are the wheelbase of the vehicle's center of mass line and the rear axle respectively. is the yaw angular velocity, β is the sideslip angle, F yf and F yr are the resultant forces of the lateral forces of the tires acting on the front and rear axles of the vehicle respectively;

[0121]

[0122] Among them, the tire sideslip force is expressed as:

[0123]

[0124] In the formula, C αf and C αr are the linear sideslip stiffnesses of the front and rear tires; among them, the tire sideslip angle is expressed as:

[0125]

[0126] Combining Formula 1 - Formula 6, the vehicle's single-track yaw dynamics model is obtained as follows:

[0127]

[0128] In the formula, m is the vehicle mass, v x , v y are the longitudinal speed and lateral speed of the vehicle's center of mass respectively, is the yaw angular velocity, β is the center-of-mass sideslip angle, I z is the moment of inertia of the vehicle body about the Z-axis, l f , l r are the distances from the vehicle's center of mass to the front and rear axles respectively, C αf , C αf are the tire sideslip stiffnesses, δ f is the front wheel steering angle.

[0129] In this embodiment, as Figure 3 shown in the vehicle tracking error model, when an autonomous vehicle is driving in a complex road environment, the road curvature will affect the accuracy of trajectory tracking control, resulting in a large tracking error, and even affect the vehicle stability; based on the geometric relationship between the vehicle position and the road, a vehicle tracking error model is established:

[0130]

[0131] where e y is the distance between the center of the rear axle of the vehicle and the projection point on its road center line, is the heading deviation, that is, the difference between the vehicle yaw angle and the desired yaw angle at the current reference trajectory point.

[0132] By combining Equation (7) and Equation (8), let be the state variable, u1 = δ f be the control variable, u2 = κ be the disturbance input, and linearize to obtain the vehicle model:

[0133]

[0134] where A(t), B(t) and C(t) are Jacobian matrices, where,

[0135]

[0136] C(t) = [0 0 0 0 0 -v x T .

[0137] Step S120, based on the vehicle model established in step S110, establish a vehicle convex polytope model including the tire cornering stiffness and the longitudinal speed;

[0138] In this embodiment, since the tire cornering stiffness will change with factors such as vehicle vertical load, tire wear, and road adhesion conditions, the change ranges of the front and rear tire cornering stiffness are set as [C αf,min , C αf,max and [C αr,min , C αr,max ; and there is a time-varying parameter v x in Equation (9). Usually, the longitudinal speed is bounded, and the change range of v x is set as [v x,min , v x,max ; adopt a convex polytope model with 2 3 vertices to cover all possible selected parameter variables and suppress the non-linear characteristics of the vehicle longitudinal speed and the tire cornering stiffness [1 / v x ​C αf C αr ;

[0139] Among them, the time-varying variables of the time-varying parameter matrix at the vertices can be expressed as:

[0140]

[0141] The non-linear parameters in the vehicle model are linearly combined with the parameter values at the vertices of the convex polytope, as shown in the following formula:

[0142]

[0143] In the formula, and are correction coefficients.

[0144] Replace the tire cornering stiffness and vehicle longitudinal speed in the Jacobian matrix of the vehicle model to obtain the convex polytope state space matrix (A i , B i , C i ), i = 2 3 ;

[0145] Use the first-order difference quotient method to discretize the state space model at the vertices. When the sampling time is small, discard the high-order terms to obtain:

[0146]

[0147] Convert Equation (9) into a discrete vehicle convex polytope linear time-varying model, as shown below:

[0148] ξ(t + 1) = A(t)ξ(t) + B(t)u1(t) + C(t)u2(t) (13)

[0149] Among them,

[0150] The discretized vehicle dynamics model can be expressed as a polytope model. For non-negative constants, γ i (i = 1, 2,..., 8) and A, B, C can be expressed as:

[0151]

[0152] Step S130, the autonomous vehicle performs trajectory planning, uses a Bezier curve to fit the reference trajectory, and calculates the road curvature in real time;

[0153] In this embodiment, since the road curvature has a great influence on the trajectory tracking effect of the autonomous vehicle when driving on a high-speed or complex road environment, and even affects the vehicle's handling stability, the reference trajectory obtained by the vehicle trajectory planning system is fitted with a cubic Bezier curve, which can be expressed as:

[0154] q(ω i ) = (1 - ω i ) 3 P0 + 3ω i (1 - ω i ) 2 P1 + 3ω i 2 (1 - ω i ) 2 P2 + ω i 3 P3 (15)

[0155] Wherein, q(ω i ) is the interpolation point at parameter ω i , P k is the k-th control point. By taking values of parameter ω i within [0, 1], any number of interpolation points can be generated between the first control point and the last control point; the calculation equations for the intermediate control points P1 and P2 are:

[0156]

[0157] Wherein,

[0158]

[0159] According to the control time domain, the curve is segmented and fitted. During each iteration of curve fitting, the positions of the intermediate control points of each segment of the Bezier curve are solved. According to Equation (15), the interpolation points corresponding to the original path points can be obtained. Finally, the road curvature is calculated based on the interpolation points of each fitted section as follows:

[0160]

[0161] Wherein, x′ = x c - x a , y′ = y c - y a , x″ = x c + x a - 2x b , y″ = y c + y a - 2y b , η = (x′) 2 + (y′) 2 ,

[0162] (x a , y a ), (x b , y b ) and (x c , yc ) is the interpolation point for each fitting section.

[0163] Step S140: Based on the vehicle convex polyhedron model constructed in step S120, construct a robust objective function for trajectory tracking error and stability constraints.

[0164] In this embodiment, since the autonomous vehicle needs to consider the influence of factors such as the robustness of the control system, driving safety, and comfort during the process of following the reference trajectory; therefore, on the basis of the vehicle convex polyhedron model constructed in step S120, considering factors such as robustness and safety, and adding a feedback correction module, a robust model predictive control trajectory tracking controller is designed as Figure 4 shown, to ensure the robustness and stability of the autonomous vehicle during high-speed driving; a robust objective function for trajectory tracking error and stability constraints is constructed.

[0165] Among them, according to the discretized vehicle convex polyhedron dynamics model formula (13), the control increment is used as the control quantity, and a new state vector ξ(k|t) = [ξ(k) u1(k - 1)] T , to obtain the state space expression:

[0166]

[0167] In the formula, Δu1(k) = u1(k) - u1(k - 1), I is the identity matrix; the prediction horizon is N p , and the control horizon is N c ;

[0168] Among them, according to formula (18), prediction is performed at the sampling time k, and the system prediction model is obtained as follows:

[0169] Y = Ψξ(k) + ΘΔU1 + ΥU2 (19)

[0170] Y = Ψξ(k) + ΘΔU1 + ΥU2,

[0171] In the formula, Y = [ξ(k + 1) ξ(k + 2) … ξ(k + N c ) … ξ(k + N p )] T ,

[0172]

[0173]

[0174] ΔU1 = [Δu1(k) Δu1(k + 1) … Δu1(k + N c )] T ,

[0175] U2 = [u2(k) u2(k + 1) … u2(k + N c )] T ;

[0176] Among them, to enable the system to track the desired trajectory quickly and smoothly; therefore, an objective function in the following form is designed:

[0177]

[0178] In the formula, χ p (k + i|k) is the predicted value of the control output, χ ref (k + i|k) is the reference value of the control output, and Q and R are weight matrices.

[0179] Among them, the model predictive control can consider the influence of various factors on the trajectory tracking control, and by constraining variables such as the state quantity and control quantity of the prediction model, solve the constrained optimization problem by rolling; the stability control boundary includes:

[0180] The stability control boundary of the yaw rate:

[0181]

[0182] The stability control boundary of the sideslip angle of the center of mass:

[0183]

[0184] Considering the vehicle shape and the width of the road, the tracking accuracy is improved through a combination of a series of lateral deviation thresholds. Among them, as Figure 5 shown, the envelope boundary of the feasible road area can be expressed as:

[0185] e y,min (k) - d s ≤ e y (k) ≤ e y,max (k) - d s (23)

[0186] In the formula, d s = (R d - d w ) / 2 is the safety distance defined according to the vehicle body size, R d is the road width, and d w is the vehicle body width.

[0187] Among them, after establishing the objective function and constraint conditions, the objective function is transformed into an optimization problem, which can be expressed as:

[0188]

[0189] In the formula, a relaxation factor is added to prevent the situation where there is no optimal solution within the specified calculation time. ρ is the weight coefficient, and ε is the relaxation factor.

[0190] Step S150: Construct a discrete neural network model to solve the objective function, obtain the optimal control quantity, and input it to the vehicle for execution.

[0191] In this embodiment, since the neural network has the characteristics of natural parallelism, self - adaptability, and less resource occupancy, it provides a new way to solve large - scale quadratic programming problems in real time. By constructing a discrete neural network model to solve the objective function, the optimal control quantity is obtained and input to the vehicle for execution.

[0192] Equation (24) is transformed into the following quadratic programming form:

[0193]

[0194] In the formula, x op is the optimal solution Δu(k), l and h are the stability constraint conditions.

[0195] Perform an identical transformation on the constraint conditions in Equation (25) to obtain:

[0196]

[0197] The Lagrangian function of Equation (26) is expressed as:

[0198]

[0199] In the formula, is the Lagrange multiplier.

[0200] According to the saddle - point theorem, if x * is the global optimal solution of the optimization problem, then and only then when there exist u * and η * , (x * , u * , η * ) satisfy the following inequalities:

[0201] L(x * , u, η * ) ≤ L(x * , u * , η * ) ≤ L(x, u * , η) (29)

[0202] Based on the projection theorem, the inequality is equivalent to:

[0203] η *= G Z (η * - u * )(30)

[0204] Hx + f - E T u * = 0 (31)

[0205] In the formula, is a projection algorithm defined by P, represents the Euclidean norm,

[0206] The projection equation system is expressed as:

[0207]

[0208] Substituting various coefficient matrices, the projection equation is obtained as follows:

[0209] My = G Z [Ny - (Dy + K)] (33)

[0210] In the formula, y = [x * v * w * T , u * = [(v * ) T (w * ) T T ,

[0211]

[0212] 0 n×n , 0 n×m , 0 m×n and 0 m×m are zero matrices, I n and I m are identity matrices.

[0213] The projection neural network dynamic equation is expressed as:

[0214]

[0215] If y(t) is the equilibrium point of the projection equation, then x(t) = [I n 0 n×n 0 n×m y(t) is the optimal solution of the quadratic programming problem,

[0216] Continuous neural networks are difficult to be implemented in hardware. Discretizing the neural network gives:​​

[0217] y(k + 1)=y(k)+μΛ{G Z [Ny(k)-(Dy(k)+K)]-My(k)} (35)

[0218] Then the output equation is as follows:

[0219] x(k)=[I n 0 n×n 0 n×m y(k) (36)

[0220] In the formula,

[0221] where λ max (P) is the maximum eigenvalue of matrix P, ||Λ|| 2 is the square of the two - norm of Λ, and

[0222] When the present invention realizes the driving of an autonomous vehicle, especially when driving in complex road environments such as highways, icy and snowy roads, it can maintain a good trajectory tracking effect and ensure the stable driving of the vehicle.

[0223] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A trajectory tracking method for a high-speed autonomous vehicle based on fast robust model predictive control, characterized in that It includes the following steps: Step 1: Establish a vehicle model Among them, ξ(t) represents the state vector at time t, u1(t) represents the front wheel steering angle input vector at time t, and u2(t) represents the disturbance input vector at time t; A(t), B(t), and C(t) are all Jacobian matrices, which are respectively: C(t) = [0 0 0 0 0 -v x T ,​ where C αf represents the cornering stiffness of the front tire, C αr represents the cornering stiffness of the rear tire, l f is the distance from the vehicle's center of mass to the front axle, l r is the distance from the vehicle's center of mass to the rear axle, v x is the longitudinal speed at the vehicle's center of mass, m is the vehicle mass, I z is the moment of inertia of the vehicle about the Z-axis; Step 2: Based on the vehicle model Establish a discrete vehicle convex polyhedron model ξ: ξ(t + 1) = A'(t)ξ(t) + B'(t)u1(t) + C'(t)u2(t) Among them, the value of u1(t) is the front-wheel steering angle δ that changes with time t f , that is, u1 = δ f , the value of u2(t) is the road curvature κ2 that changes with time t, that is, u2 = κ2, and A'(t), B'(t), and C'(t) are the derivatives of A(t), B(t), and C(t) respectively: I is the identity matrix, and γ i is the i-th correction coefficient; Step 3: Calculate the road curvature κ2: where x' = x c -x a , y' = y c -y a , x'' = x c +x a -2x b , y'' = y c +y a -2y b , η = (x')[[]] 2 +(y')[[]] 2 , (x[[[]] a ,y[[[]] a ), (x[[[]] b ,y[[[]] b ), (x[[[]] c ,y[[[]] c ) are three of the interpolation points for each fitted section of the road; Step 4. Construct a robust objective function for expressing the trajectory tracking error based on the vehicle convex polytope model Δ u1(t) and the stability constraint min |u1(k + i|k)| ≤ u 1,max , |Δu1(k+i|k)|≤Δu 1,max , |β(k)| ≤ β ss (k), e y,min (k)-d s ≤e y (k)≤e y,max (k)-d s , k = 1, 2, …, N c Among them, χ p (k + i|k) is the predicted value of the control output, χ ref (k + i|k) is the reference value of the control output, (k + i|k) represents the value predicted for the (k + i)-th sampling time based on the information at the k-th sampling time, Q and R are weight matrices, N p is the prediction horizon, N c is the control horizon, ρ is the weight coefficient, ε is the weight factor, Δu1(k) = u1(k) - u1(k - 1), e y,min (k) is the minimum lateral position error varying with time, d s is the safety distance defined according to the vehicle body size, e y (k) is the boundary of the feasible road area, e y,max (k) is the maximum lateral position error varying with time; Step 5: Construct a discrete neural network model to solve the objective function, obtain the optimal control quantity, and input it to the vehicle for execution.

2. The trajectory tracking method for a high-speed autonomous vehicle based on fast and robust model predictive control according to claim 1, wherein, Vehicle model The method for establishing the vehicle model includes the following steps: Establish a vehicle dynamics model, expressed as: where v y is the lateral velocity at the vehicle's center of mass, is the yaw rate, β is the sideslip angle at the center of mass, and δ f is the front wheel steering angle; Establish a vehicle tracking error model, expressed as: where the lateral position error e y is the distance between the projection points of the center of the vehicle's rear axle on the road center line, and the heading error is the angle between the tangent of the road center line and the road ground coordinate system, and κ is the road curvature of the reference path; Establish the vehicle model based on the vehicle dynamics model and the vehicle tracking error model.

3. The method for trajectory tracking of a high-speed autonomous vehicle based on fast and robust model predictive control according to claim 1, characterized in that, The method for establishing the discrete vehicle convex polytope model includes the following steps: Construct a vehicle convex polytope model with 2 3 vertices that suppress the nonlinear characteristics of the tire cornering stiffness and the vehicle longitudinal speed. The time-varying variables of the parameter matrix at the vertices are expressed as: v x,min and v x,max are the minimum and maximum values of the longitudinal speed at the vehicle's center of mass, respectively; C αf,min and C αf,max are the minimum and maximum values of the cornering stiffness of the front wheels respectively; C αr,min and C αr,max are the minimum and maximum values of the cornering stiffness of the rear wheels respectively; The non-linear parameters in the vehicle model are linearly combined with the parameter values at the vertices of the convex polytope as: In the formula, and are correction factors, where m = 1, 2, n = 1, 2, j = 1, 2, Replace the tire cornering stiffness and vehicle longitudinal speed in the Jacobian matrix of the vehicle model to obtain the convex polytope state space matrix (A i2 (t), B i2 (t), C i2 (t)) of the i2-th convex polytope vertex, where i2 = 2 3 ; i2 is the number of convex polytope vertices; Use the first-order difference quotient method to discretize the state space model at the vertices of the convex polytope to obtain a discrete vehicle convex polytope model.

4. The trajectory tracking method of a high-speed autonomous vehicle based on fast robust model predictive control according to claim 2, wherein The calculation method of the road curvature κ includes the following steps: The reference path is fitted with a third-order Bezier curve and expressed as: q(τ i1 ) = (1 - τ i1 ) 3 P0 + 3τ i1 (1 - τ i1 ) 2 P1 + 3τ i1 2 (1 - τ i1 ) 2 P2 + τ i1 3 P3, where q(τ i1 ) is the parameter τ at the i1-th interpolation point of the reference path i1 of the third-order Bessel curve fitting, P k1 is the k1-th control point of the reference path, by taking values of the parameter τ i1 in [0, 1], any number of interpolation points are generated between the first control point and the last control point; Calculate the road curvature according to the interpolation points of each fitting section.

5. The trajectory tracking method for a high-speed autonomous vehicle based on fast and robust model predictive control according to claim 1, characterized in that, The high-speed autonomous vehicle trajectory tracking method further includes the steps of constructing a prediction model of the objective function and constraint conditions: Construct a prediction model according to the discrete vehicle convex polytope model; Establish a stability control boundary composed of yaw rate and sideslip angle of the center of mass, and perform stability constraints on the yaw rate and sideslip angle of the center of mass; Establish a feasible road area boundary to constrain the lateral displacement error; Constrain the control input quantity.

6. The method for trajectory tracking of a high-speed autonomous vehicle based on fast robust model predictive control according to claim 5, characterized in that The prediction model includes: Construct a new state vector ξ(k|t) = [ξ(k) u1(k - 1)] T , and obtain a new state - space equation according to the discrete vehicle convex polyhedron model: wherein, Δu1(k) = u1(k) - u1(k - 1), and I is the identity matrix; Perform state prediction according to the new state space equation to obtain the prediction model at future times: Y = Ψξ(k) + ΘΔU1 + ΥU2, where Y = [ξ(k + 1) ξ(k + 2) … ξ(k + N c ) … ξ(k + N p )] T , ΔU1 = [Δu1(k) Δu1(k + 1) … Δu1(k + N c )] T , U2 = [u2(k) u2(k + 1) … u2(k + N c )] T 。 7. The method for trajectory tracking of a high-speed autonomous vehicle based on fast and robust model predictive control according to claim 6, wherein The stability control boundary of the yaw rate is; where the rear wheel sideslip angle is α r,ss The threshold is [-α r,lim , α r,lim ; The stability control boundary of the sideslip angle of the center of mass is: |β(k)| ≤ β ss (k).

8. The trajectory tracking method for a high-speed autonomous vehicle based on fast robust model predictive control according to claim 7, characterized in that The boundary e of the feasible road area y (k) is expressed as: e y,min (k)-d s ≤e y (k)≤e y,max (k)-d s 。 9. The high-speed autonomous vehicle trajectory tracking method based on fast robust model predictive control according to claim 8, characterized in that The constraint conditions of the control input quantity are: |u1(k + i|k)| ≤ u 1,max , |Δu1(k + i|k)| ≤ Δu 1,max , i = 1, 2, …, N c 。 10. The method for trajectory tracking of a high-speed autonomous vehicle based on fast and robust model predictive control according to claim 8, characterized in that Constructing a discrete neural network model to solve the objective function includes: The objective function is transformed into a quadratic programming problem: s.t.l 0 ≤ x op ≤ h 0 l 1 ≤Wx op ≤h 1 , where H = Θ T QΘ + R, f = Θ T Q(Ψξ(k) + ΥU2 - χ ref ), l and h are stability constraint conditions; The discrete neural network projection equation can be expressed as: y(k + 1) = y(k) + μΛ{G Z [Ny(k) - (Dy(k) + K)] - My(k)}, x(k) = [I n 0 n×n 0 n×m y(k), In the formula, y(k) is the equilibrium point of the projection equation, x(k) is the optimal control quantity of the objective function, and μ is the scaling factor. I n and I m is the identity matrix.

Images