Automatic driving vehicle trajectory planning method based on interactive vehicle trajectory prediction

Abstract

The invention discloses an automatic driving vehicle trajectory planning method based on interactive vehicle trajectory prediction, and relates to the field of vehicle decision planning. The method comprises the following steps: firstly, planning a driving path for a vehicle by using a quintic polynomial curve and collision remaining time based on an interactive vehicle trajectory prediction model; using an SSA-AN trajectory prediction model considering vehicle driving parameters, shape and size and space influence to accurately predict future driving trajectories of vehicles around the main vehicle, calculating potential collision between the main vehicle and the interactive vehicle according to a collision judgment model, planning a reasonable longitudinal driving speed for the main vehicle, and then obtaining a vehicle planning trajectory; and then a transverse controller based on MPC is adopted to control the transverse direction of the vehicle, and a longitudinal controller based on PID is adopted to control the longitudinal speed of the vehicle.

Description

Technical Field

[0001] The present invention proposes a trajectory planning method for autonomous vehicles based on interactive vehicle trajectory prediction. In particular, it relates to a trajectory decision-making method for vehicles in complex driving scenarios such as urban roads. Background Art

[0002] With the development of science and technology, the occupancy rate of autonomous vehicles in cities is getting higher and higher. However, the driving environment on urban roads is changeable and the driving uncertainty is high. Therefore, it is of great significance to plan a safe, comfortable and efficient driving trajectory for autonomous vehicles in complex driving environments.

[0003] Vehicle trajectory prediction aims to let the vehicle know how to move next. If the trajectory is accurately predicted, the vehicle can reasonably and effectively infer the future movements of surrounding vehicles and then make the best decision. The decision-making and trajectory planning technologies of autonomous vehicles are based on environmental perception technologies. Currently, there are still the following several difficulties in autonomous driving in complex traffic scenarios:

[0004] (1) Multi-modal characteristics of prediction results: Different predicted trajectories may be obtained using the same historical trajectory.

[0005] (2) Interpretability and reliability of prediction results: Taking traffic rules as part of the input data to output safer and more reliable data is very important and challenging for the autonomous driving system.

[0006] (3) When existing vehicle trajectory planning methods based on trajectory prediction consider the future driving trajectories of interactive vehicles, kinematic models are mostly used to predict the driving trajectories of surrounding vehicles, and the spatial influence between other vehicles in the scenario is less considered to predict the trajectories of interactive vehicles.

[0007] (4) Sensors generally can only obtain intuitive data such as vehicle speed, position, and direction, and abstract data such as the driving intention of the vehicle or driver cannot be directly obtained. Summary of the Invention

[0008] Aiming at the deficiencies in the prior art, the present invention provides a trajectory planning method for autonomous vehicles based on interactive vehicle trajectory prediction. It can solve the problem that the existing methods do not fully consider the spatial influence between other vehicles, that is, the influence of driving parameters and shape and size on vehicle trajectory prediction.

[0009] To achieve the above object, the present invention provides the following solutions:

[0010] A trajectory planning method for autonomous vehicles based on interactive vehicle trajectory prediction, comprising:

[0011] (1) The trajectory prediction algorithm SSA-GAN model is described as follows:

[0012] Obtain the historical driving parameter sequence of the vehicle in the dataset, which consists of position coordinates, driving speed, acceleration, vehicle length, and vehicle width.

[0013] The trajectory generator composed of the physically guided FCL neural network layer and the LSTM network layer obtains the feature vectors S i,n , H i,n . Input the historical time driving parameter sequence into the FCL neural network layer and then into the LSTM network layer for further encoding to obtain the vehicle historical trajectory feature vector H i,n .

[0014] S i,n = FCL(frame i , id n , x i,n , y i,n , v i,n , a i,n , L l,n , L w,n ; W fcl )

[0015] H i,n = LSTM(H i-1,n , S i,n ; W en ) i=(1, 2,..., t obs )

[0016] Where: frame i , id n , x i,n , y i,n are the driving frame number, special number, and position coordinates of the vehicle; v i,n , a i,n are the driving speed and acceleration of the vehicle; L l,n , L w,n are the vehicle length and width; FCL is a fully connected neural network layer using the ReLU activation function; W fcl , W en are the weight functions of the corresponding network layers.

[0017] The physically guided vehicle spatial influence field function f based on the improved two-dimensional normal distribution is established IR , that is, the spatial attention module,

[0018] D = λ(L l + L W )(1.566v 6.687 × 10 -14 + 0.3345

[0019]

[0020] Among them, (x0, y0) is the position coordinate of the field source center vehicle; (x, y) is the position coordinate point of other vehicles; D is the vehicle risk; σ is the spatial influence field distribution factor; λ is the risk influence factor; μ and γ are the acceleration and speed influence factors.

[0021] The physical guidance vehicle spatial influence field function f IR Get t obs At time t, vehicle n receives the set of spatial influence forces from other vehicles in the scene Normalized by the softmax function as the spatial attention weight factor of other vehicles Multiply with the spatial feature vector H of the vehicle i,n To obtain the vehicle spatial feature vector This vector is used to characterize the spatial influence relationship between vehicles.

[0022]

[0023] Among them: σ is the softmax function; W σ Is the weight parameter of the corresponding function.

[0024] (2) The main vehicle path planning method is as follows:

[0025] Use a fifth-degree polynomial curve to plan a driving trajectory to avoid static obstacles in the environment.

[0026]

[0027] Where x and y are the position coordinates of the vehicle in the Cartesian coordinate system; a1, a2,... a5, b1, b2,... b5 are undetermined coefficients. Let the starting time of the obstacle avoidance trajectory be t0, and the vehicle position, speed, and acceleration at the start be The end time of the collision avoidance trajectory is t1, and the parameters at the end are Then the boundary conditions of the fifth-degree polynomial planning path can be obtained.

[0028]

[0029] Solve the undetermined coefficients of the fifth-degree polynomial by combining the fifth-degree polynomial with the boundary conditions, and then obtain the vehicle planning path. Set the obstacle avoidance process time equal to the time to collision (TTC) between the autonomous vehicle and the stationary obstacle:

[0030] TTC = L d × 3.6 / v x

[0031] Among them, TTC is the remaining collision time between the host vehicle and the static obstacle, and L d (m) is the longitudinal distance between the host vehicle and the static obstacle ahead, and v x (km / h) is the longitudinal speed of the vehicle.

[0032] (3) The host vehicle speed planning method is as follows:

[0033] a) Use a rectangular box to describe the vehicle's shape, and in order to increase the driving safety of the vehicle, use an extended rectangular model to depict the collision avoidance plane model of the vehicle.

[0034] Among them: L and W are the true length and width of the vehicle; L' and W' are the length and width of the extended rectangle; L s and W s are the increased longitudinal and lateral safety distances.

[0035] b) Based on the Cartesian coordinate system, take a set of dense discrete path points D id on the vehicle's planned path, take the four fixed points A', B', C', D' of the vehicle's extended rectangle, and A, B, C, D are the coordinate points after the vehicle's steering angle θ. The coordinate rotation formula of the physical quantity is obtained

[0036]

[0037] Calculate the Euclidean distances from the interactive vehicles A, B, C, D to each point in the discrete path point set D id at the current moment t, and obtain the path point index id A 、id B 、id C 、id D 。

[0038] The Euclidean distance is defined as dis A 、dis B 、dis C 、dis D ,

[0039] min{dis A 、dis B 、dis C 、dis D} < d asfe

[0040] When min{dis A 、dis B 、dis C 、dis D} is less than the safety distance threshold d safe a collision occurs between the vehicles.

[0041] Solve the S-T graph of the host vehicle. The vertical axis S represents the longitudinal length of the path planned by the path planning model, and the horizontal axis T represents the vehicle planning time.

[0042] In the S-T graph, the length occupied by the obstacle is the longitudinal length S between the path points corresponding to the maximum and minimum indices in minmax{dis A , dis B , dis C , dis D}. By traversing the predicted time nodes t ∈ (t1,...t t ), the dynamic obstacle area Z pred can be obtained. i .

[0043] c) Perform rasterization processing on the S-T graph;

[0044] Among them, T max is the speed planning time, S max is the longitudinal distance length of the vehicle planning path within the speed planning period, ΔS is the discrete distance step, and ΔT is the discrete time step.

[0045] The selection of the feasible state points at each moment in the S-T graph satisfies the following constraints:

[0046]

[0047] Among them: The vehicle is not allowed to reverse; S' and S" are the vehicle driving speed and acceleration; v max is the maximum speed allowed for the vehicle to drive in the road scene; a max , a min are the maximum and minimum accelerations of the vehicle.

[0048] The dynamic programming cost function DP of the S-T graph speed curve cost is defined as

[0049]

[0050]

[0051] Among them: S' is the vehicle driving speed; v ref is the expected driving speed of the vehicle; S''' is the jerk of the vehicle when driving. represents the distance degree between the vehicle and the obstacle; represents the vehicle acceleration and jerk cost, comfort cost; represents the difference between the vehicle planned speed and the expected speed, the vehicle form efficiency cost; are the weight parameters of each cost.

[0052] For Perform normalization processing,

[0053]

[0054] c) Replace the optimal discrete points in the discrete speed planning curve obtained by dynamic programming with a fifth-degree polynomial curve.

[0055] Fifth-degree polynomial expression:

[0056]

[0057] Where: S represents the longitudinal distance traveled by the vehicle; v represents the traveling speed; acc represents the traveling acceleration; jerk represents the traveling jerk; j represents the number of fifth-degree polynomials, j = T max / ΔT.

[0058] The numerical optimization cost function of the optimal speed curve is:

[0059]

[0060] The first and second terms are the comfort cost functions during numerical optimization solution, and the third term is the cost function of the error between the numerical optimization result and the dynamic programming result during solution.

[0061] The solution of this cost function can be converted into a quadratic programming solution problem. When solving quadratic programming, multiple segments of fifth-degree polynomial curves need to satisfy the following equality and inequality constraints:

[0062] a) Equality constraints on the position, speed, and acceleration at the starting time T0 of the planning. The position, speed, and acceleration at the starting point of the vehicle are 0, v init , a init .

[0063]

[0064] b) Equality constraints on the position, speed, acceleration, and at the connection point of adjacent fifth-degree polynomial curves.

[0065]

[0066] c) Each segment of the fifth-degree polynomial should satisfy the constraints of upper and lower limits such as obstacles, speed, and acceleration.

[0067]

[0068] Where: S lb , S ub are the upper and lower boundaries of the convex space where the dynamic programming results are located.

[0069] The numerical optimization result is the optimal planned speed curve that satisfies the vehicle dynamics constraints and has continuous position, speed, and acceleration at each node.

[0070] (4) The trajectory tracking controller is designed as follows:

[0071] a) Design of the lateral controller based on MPC

[0072] According to Newton's second law, the two-degree-of-freedom dynamics model of the vehicle is:

[0073] ∑F y = ma y = F yf cosδ f + F yr

[0074]

[0075] Where: Among them, F y is the component force acting on the vehicle body along the y-axis of the vehicle coordinate system; m is the vehicle mass; a y is the lateral acceleration of the vehicle; F yf , F yz are the lateral forces acting on the front and rear wheels; δ f is the front wheel steering angle; M z is the moment of the vehicle about the : axis; I z is the moment of inertia of the vehicle about the z-axis; is the yaw angular acceleration of the vehicle; a, b are the distances from the vehicle center of mass position to the front and rear axles.

[0076] The front wheel steering angle δ f →0, so cos(δ f )≈1,

[0077] Simplifying the above formula gives:

[0078] ma y = F yf + F yr = C f α f + C r α r

[0079]

[0080] According to the small-angle assumption of the vehicle, there is a linear relationship between the tire side force and the side slip angle:

[0081] F yf = C f α f

[0082] F yr= C r α r

[0083] where: C f , C r is the cornering stiffness of the front and rear wheels; α f , α r is the cornering angle of the front and rear wheels.

[0084] Taking as the state quantity obtained by the model, δ f as the control quantity,

[0085] Based on the vehicle coordinate system:

[0086]

[0087] Coordinate transformation between the vehicle coordinate system and the Cartesian coordinate system:

[0088]

[0089] where: v x , v y are the longitudinal and lateral speeds of the vehicle in the vehicle coordinate system; ψ is the angle between the driving direction of the vehicle and the X-axis of the Cartesian coordinate system.

[0090] The vehicle non-dynamic model is:

[0091]

[0092] Defining the control quantity u dy = [δ f ,

[0093] Performing Taylor expansion and neglecting the high-order terms, the vehicle linear time-varying model can be obtained:

[0094]

[0095] where A dy (t), B dy are the Jacobian matrices of f with respect to ξ and u; ξ0(t), u0(t) are the reference state quantity and control quantity.

[0096] Discretizing the linear time-varying model using the forward Euler method gives:

[0097]

[0098] A dy (k) = I + T s A dy (t), B dy (k) = T s Bdy (t)

[0099] The optimization objective function of the design model predictive control is as follows:

[0100]

[0101] Where: N p is the prediction horizon; N c is the control horizon; η dy is the predicted output; η dy,r is the reference output; Δu dy is the input control increment; Q and R are the weight matrices of the output deviation and the input control quantity; ε is the relaxation factor; ρ is the weight parameter of the relaxation factor.

[0102] b) Design of the longitudinal controller based on the PID control algorithm:

[0103] The expression of the PID control algorithm:

[0104]

[0105] Where: where e rr (t) represents the difference between the actual value and the expected value; K p is the proportional adjustment coefficient; T i is the integral adjustment coefficient; T d is the differential adjustment coefficient; E(t) is the linear combination of the error e rr (t) after proportional, integral, and differential adjustments. Description of the Drawings

[0106] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0107] Figure 1 is the framework diagram of the present invention;

[0108] Figure 2 is the SSA-GAN network structure of the present invention;

[0109] Figure 3 is the vehicle path planning diagram provided by the example of the present invention;

[0110] Figure 4 is the vehicle extended rectangular model of the present invention;

[0111] Figure 5 is the discrete speed planning curve of the present invention;

[0112] Figure 6 is the vehicle collision judgment model of the present invention;

[0113] Figure 7 is the schematic diagram of dynamic programming of the present invention;

[0114] Figure 8 is the S-T diagram of the present invention;

[0115] Figure 9 is the two-degree-of-freedom dynamic model of the vehicle of the present invention Detailed implementation manners

[0116] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

[0117] As Figure 1 shown, the present invention proposes a trajectory planning method and system for autonomous driving vehicles based on the prediction of interactive vehicle trajectories. Specifically, it includes the following steps:

[0118] Step (1), based on the social generative adversarial network (S-GAN), the present invention combines the driving characteristics of vehicles in the road environment, considers the driving parameters, shape and size of interactive vehicles to establish a spatial influence field for vehicles to characterize the spatial influence relationship between vehicles, and proposes a vehicle trajectory prediction model SSA-GAN based on a spatial attention mechanism. The SSA-GAN model consists of a trajectory generator, a spatial attention module, a pooling module, a trajectory discriminator, and a loss function module. As Figure 2 shown, the present invention only elaborates on the encoder module and the spatial attention module in the generator.

[0119] Step (1.1), obtain the historical driving parameters of the vehicle from the dataset, including position coordinates (x, y), driving speed (v), acceleration (a), vehicle length (l), and width (w).

[0120] Step (1.2), input the formal parameter sequence of the vehicle during the observation time period t∈(1, ···, t obs ) into the FCL neural network layer to obtain the feature vector S i,n , and input it into the LSTM network layer for encoding to obtain the vehicle historical trajectory feature vector H i,n vector.

[0121] S i,n = FCL(frame i , id n , x i,n , y i,n , v i,n , a i,n , Ll,n , L w,n ; W fcl )

[0122] H i,n = LSTM(H i-1,n , S i,n ; W en ) i = (1, 2,..., t obs )

[0123] where: frame i , id n , x i,n , y i,n are the driving frame number, special number, and position coordinates of the vehicle; v i,n , a i,n are the driving speed and acceleration of the vehicle; L l,n , L w,n are the length and width of the vehicle; FCL is the fully connected neural network layer using the ReLU activation function; W fcl , W en are the weight functions of the corresponding network layers.

[0124] Step (1.3), since it is considered that the vehicle has the following driving characteristics in the real driving environment: a) The spatial influence of the vehicle on surrounding vehicles is greater near and smaller far away; b) Larger-sized vehicles have a greater spatial influence on surrounding vehicles; c) The spatial influence of the vehicle on surrounding vehicles increases with the increase of speed and acceleration.

[0125] Establish a vehicle spatial influence field function based on the improved two-dimensional normal distribution. The specific formula is as follows:

[0126]

[0127] where, (μx, μ y ) is the position coordinate of the field source center vehicle, (x, y) is the position coordinate point of other vehicles, d is the vehicle risk, σ is the spatial influence field distribution factor, α is the risk influence factor, and β is the acceleration and speed influence factor. Through normalization by the softmax function, the spatial attention weight factor of other vehicles is obtained, which is multiplied by the vehicle spatial feature vector to obtain the vehicle spatial feature vector.

[0128] Step (2), for the host vehicle path planning, use a fifth-degree polynomial curve to plan a driving trajectory to avoid static obstacles in the environment. The specific formula is as follows:

[0129] y(t) = b0 + b1t + b2t 2 + b3t 3 + b4t 4 + b5t 5

[0130] Among them, a0, a1, a2, a3, a4, and a5 are undetermined coefficients. Assume that the starting time of the obstacle avoidance trajectory is 0, and the vehicle position, speed, and acceleration are (x0, y0), The end time of the collision avoidance trajectory is t, and the parameters at the end are (x t , y t ). Solve for the undetermined coefficients of the fifth-degree polynomial by combining the boundary conditions to obtain the vehicle's planned path. Figure 3 Describes the planned path curve graph of the autonomous vehicle when the stationary obstacle vehicle is at 35 m and the driving speed of the autonomous vehicle is 36 km / h.

[0131] Step (3), due to the uncertainty of the motion trajectory of the dynamic obstacle, the driving trajectory may collide with the planned path of the host vehicle at a certain time. If the local path planning method is used to avoid the dynamic obstacle, it is easy to cause the vehicle to have extreme maneuvers and incomplete collision avoidance, and it is easy to have lateral instability when the vehicle speed is high.

[0132] Step (3.1), use a rectangular box to describe the vehicle shape, and extend the rectangular model to depict the collision avoidance plane model of the vehicle. The specific formula is as follows:

[0133] L ext = L + 2D safe , W ext = W + D safe

[0134] Among them, L and W are the true length and width of the vehicle, L ext and W ext are the length and width of the extended rectangle, and D safe is the longitudinal and lateral safety distance. Considering the characteristic that the longitudinal danger degree of the vehicle in the real driving environment is greater than the lateral one, set D safe to be 15 cm, as Figure 4 shown.

[0135] Step (3.2), based on the Cartesian coordinate system, take a dense discrete path point set on the vehicle's planned path, take the four vertices of the vehicle's extended rectangle, calculate the Euclidean distance from the interacting vehicle to each point in the discrete path point set at the current time t, and define that a collision occurs between vehicles when the Euclidean distance is less than the safety distance threshold.

[0136] Step (3.2.1), Figure 5 The position of the interacting vehicle in the upper right corner in idSet, take the four fixed points A', B', C', D' of the vehicle's extended rectangle. A, B, C, D are the coordinate points after the vehicle rotates the heading angle θ. The physical quantity-guided coordinate rotation formula gives:

[0137]

[0138] Step (3.2.2), calculate the Euclidean distances from the interactive vehicles A, B, C, D at the current moment t to each point in the discrete path point set D id to obtain the path point index id with the smallest Euclidean distance from points A, B, C, D A 、id B 、id C 、id D .

[0139] The Euclidean distance magnitude is defined as dis A 、dis B 、dis C 、dis D ,

[0140] min{dis A 、dis B 、dis C 、dis D} < d asfe

[0141] When min{dis A 、dis B 、dis C 、dis D} is less than the safety distance threshold d safe a collision occurs between the vehicles.

[0142] Step (3.3), the S-T diagram describes the future motion relationship between the ego vehicle and the dynamic obstacle vehicle, intuitively reflecting the potential collision between the vehicles, as Figure 6 shown. The dynamic programming algorithm first rasterizes the S-T diagram, as Figure 7 shown. First, calculate the cost values of all feasible candidate state points from the initial moment to the moment T1, and then calculate the cost values of the feasible state points at the moment T1 to the feasible candidate state points at the moment T2, and iterate in turn until the moment T max The cost value is obtained by the defined dynamic programming cost function DP cost Calculate the set of cost values of all feasible state points at the moment T max to the discrete velocity curve at the initial moment K is the number of feasible discrete velocity curves, and select the discrete velocity curve with the smallest cost value as the result of dynamic programming. As Figure 8As shown by the center line and the dotted-dashed line, where the dotted line is the deceleration collision avoidance speed planning curve, that is, decelerating and following to avoid collisions with dynamic obstacles, and the dotted-dashed line is the acceleration collision avoidance speed planning curve.

[0143] The selection of the feasible state points at each moment in the S-T diagram satisfies the following constraints:

[0144]

[0145] Among them: The vehicle is not allowed to reverse; S' and S" are the vehicle's driving speed and acceleration; v max is the maximum speed allowed for the vehicle to drive in the road scenario; a max , a min is the maximum and minimum acceleration of the vehicle's driving.

[0146] The dynamic programming cost function DP of the speed curve in the S-T diagram cost is defined as

[0147]

[0148] Among them: Among them: S' is the vehicle's driving speed; v ref is the expected driving speed of the vehicle; S''' is the jerk of the vehicle when driving. represents the distance degree between the vehicle and the obstacle; represents the cost of the vehicle's acceleration and jerk, the comfort cost; represents the difference between the vehicle's planned speed and the expected speed, the form efficiency cost of the vehicle; are the weight parameters of each cost.

[0149] For carry out normalization processing,

[0150]

[0151] Step (3.4), the discrete speed planning curve obtained by dynamic programming is connected by multiple straight lines, and the speed and acceleration at the nodes of adjacent straight lines are not continuous, which does not meet the vehicle dynamics constraints. Therefore, use a five-segment fifth-degree polynomial curve to replace the straight line to connect the optimal discrete points of dynamic programming, and solve the undetermined coefficients of the multi-segment fifth-degree polynomial by numerical optimization methods.

[0152] Use the fifth-degree polynomial curve to replace the optimal discrete points in the discrete speed planning curve obtained by dynamic programming. The expression of the fifth-degree polynomial is as follows:

[0153]

[0154] Where: S represents the longitudinal distance of the vehicle form; v represents the driving speed; acc represents the driving acceleration; jerk represents the driving jerk; j represents the number of fifth-order polynomials, and j = T max / ΔT.

[0155] The first and second terms are the comfort cost functions during numerical optimization, and the third term is the cost function for the error between the numerical optimization result and the dynamic programming result during the solution. The solution of this cost function can be converted into a quadratic programming problem and solved using the quadprog toolbox in MATLAB. When solving the quadratic programming, the multi-segment fifth-order polynomial curves need to satisfy the following equality and inequality constraints.

[0156] a) Equality constraints for the position, speed, and acceleration at the starting moment T0 of the planning. The position, speed, and acceleration at the starting point of the vehicle are 0, v init , a init .

[0157]

[0158] b) Equality constraints for the position, speed, acceleration, and acceleration at the connection point between adjacent fifth-order polynomial curves.

[0159]

[0160] c) Each segment of the fifth-order polynomial should satisfy the constraints of the upper and lower limits of obstacles, speed, acceleration, etc.

[0161]

[0162] Where: S lb , S ub are the upper and lower boundaries of the convex space where the dynamic programming result is located.

[0163] The numerical optimization result is the optimal planned speed curve that satisfies the vehicle dynamics constraints and is continuous in terms of position, speed, and acceleration at each node.

[0164] Step (4), decouple the vehicle control into lateral control and longitudinal control, design a lateral controller based on MPC and a longitudinal controller based on the PID control algorithm. The lateral controller is used for the vehicle to track the planned path; the longitudinal controller is used for the vehicle to track the planned speed. The path tracking of the vehicle is to control the steering wheel angle of the vehicle to track the planned path. In this paper, a simplified two-degree-of-freedom dynamics model of the vehicle is used, and the model is as Figure 9 shown.

[0165] Step (4.1) According to Newton's second law, the two-degree-of-freedom dynamics model of the vehicle is obtained:

[0166] ∑F y = may = F yf cosδ f + F yr

[0167]

[0168] where: F y is the component force acting on the vehicle body along the y-axis of the vehicle coordinate system; m is the vehicle mass; a y is the vehicle lateral acceleration; F yf , F yz are the lateral forces acting on the front and rear wheels; δ f is the front wheel steering angle; M z is the moment of the vehicle about the : axis; I z is the moment of inertia of the vehicle about the z-axis; is the vehicle yaw angular acceleration; a, b are the distances from the vehicle center of mass position to the front and rear axles. According to the small angle assumption of the vehicle, the relationship between the tire side force and the side slip angle is linear:

[0169] F yf = C f α f

[0170] F yr = C r α r

[0171] where: C f and C r are the side slip stiffnesses of the front and rear wheels; α f and α r are the side slip angles of the front and rear wheels.

[0172] The front wheel steering angle δ f →0, so cos(δ f )≈1,

[0173] Simplifying the above equation gives:

[0174] ma y = F yf + F yr = C f α f + C r α r

[0175]

[0176] According to the small angle assumption of the vehicle, the relationship between the tire side force and the side slip angle is linear:

[0177] F yf = C f αf

[0178] F yr = C r α r

[0179] where: C f , C r is the cornering stiffness of the front and rear wheels; α f , α r is the slip angle of the front and rear wheels.

[0180] Taking as the state variables obtained from the model, and δ f as the control variable, the following state equation is obtained:

[0181] Based on the vehicle coordinate system:

[0182]

[0183] The above formula is established in the vehicle coordinate system. However, the planned path of the vehicle is obtained based on the Cartesian coordinate system. Therefore, it is necessary to perform a coordinate transformation between the vehicle coordinate system and the Cartesian coordinate system. The transformation formula for the coordinate transformation between the vehicle coordinate system and the Cartesian coordinate system is:

[0184]

[0185] where: v x , v y are the longitudinal and lateral velocities of the vehicle in the vehicle coordinate system; ψ is the angle between the driving direction of the vehicle and the X-axis of the Cartesian coordinate system.

[0186] Step (4.2), Design of the lateral controller based on MPC

[0187] Step (4.2.1), Linear time-varying model. The non-dynamic model of the vehicle is:

[0188]

[0189] Define the control variable u dy = [δ f ,

[0190] Performing a Taylor expansion and neglecting the high-order terms, the linear time-varying model of the vehicle can be obtained:

[0191]

[0192] where A dy (t), B dy are the Jacobian matrices of f with respect to ξ and u; ξ0(t), u0(t) are the reference state variables and control variables.

[0193] The linear time-varying model is discretized using the forward Euler method to obtain:

[0194]

[0195] A dy (k) = I + T s A dy (t), B dy (k) = T s B dy (t)

[0196] where: T s is the sampling period; I is the identity matrix.

[0197] Step (4.2.2), establishing the constraint conditions. The constraints mainly include control quantity constraints, control increment constraints, and vehicle dynamic constraints, including center of mass sideslip angle constraints, vehicle adhesion condition constraints, and tire sideslip angle constraints.

[0198] a) Center of mass sideslip angle β constraint.

[0199] -12° < β < 12°

[0200] b) Vehicle adhesion condition constraint.

[0201]

[0202] where: a x is the longitudinal acceleration of the vehicle; a y is the lateral acceleration of the vehicle; μ is the ground adhesion coefficient.

[0203] c) Front wheel sideslip angle α f constraint.

[0204] -12° < α f < 12°

[0205] Step (4.2.3), solving the objective function. The designed model predictive control optimization objective function is:

[0206]

[0207] where: N p is the prediction horizon; N c is the control horizon; η dy is the predicted output; η dy,r is the reference output; Δu dy is the input control increment; Q, R are the weight matrices of the output quantity deviation and the input control quantity; ε is the relaxation factor; ρ is the weight parameter of the relaxation factor.

[0208] Step (4.3), Design of Longitudinal Controller Based on PID Control Algorithm:

[0209] Expression of PID Control Algorithm:

[0210]

[0211] where: e rr (t) represents the difference between the actual value and the expected value; K p is the proportional adjustment coefficient; T i is the integral adjustment coefficient; T d is the differential adjustment coefficient; E(t) is the linear combination of the error e rr (t) after proportional, integral, and differential adjustments. Proportional control is used to amplify the deviation value to adjust the system, but it will cause the system to become unstable; integral control is used to eliminate the steady-state error, but it has a certain time lag; differential control eliminates the deviation in advance through the rate of change of the deviation value and has predictability.

[0212] The described embodiments are the preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Without departing from the substantial content of the present invention, any obvious improvements, substitutions, or variations that those skilled in the art can make all fall within the protection scope of the present invention.

Claims

1. A method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction, characterized in that: include: Obtain the position coordinates of the autonomous driving vehicle based on the Cartesian coordinate system, obtain the longitudinal distance to the preceding vehicle, and plan the path of the autonomous driving vehicle based on a quintic polynomial and the remaining collision time; The vehicle spatial influence field function is determined based on the historical driving parameter sequence of the vehicle, which consists of position coordinates, driving speed, acceleration, vehicle length, and vehicle width; the vehicle spatial feature vector and vehicle spatial vector are determined based on the social generative adversarial network (Social-GAN); based on the vehicle spatial influence field function set and the vehicle spatial vector, the attention mechanism is obtained to predict the trajectory of the interactive vehicle; ST diagram to the main vehicle based on the main vehicle path planning, interactive vehicle prediction trajectory and collision judgment model; Based on the dynamic programming cost function of the ST graph speed curve and the numerical optimization cost function method of the optimal speed curve, the ST graph is solved to obtain the optimal speed curve, and the safe and comfortable optimal speed curve that meets the vehicle dynamics constraints is obtained.

2. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 1, characterized in that: The remaining collision time is: TTC=L d ×3.6 / v x Among them, TTC is the remaining time between the main vehicle and the static obstacle, L d (m) is the longitudinal distance between the main vehicle and the static obstacle in front, v x (km / h) is the longitudinal speed of the vehicle.

3. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 1, characterized in that: The spatial feature vector of the vehicle is: S i,n =FCL(frame i ,id n ,x i,n ,y i,n ,v i,n ,a i,n ,L l,n ,L w,n ;W fcl ) H i,n =LSTM(H i-1,n ,S i,n ;W en )i=(1,2,...,t obs ) Where: frame i , id n , x i,n ,y i,n is the vehicle's running frame number, special number and position coordinates; v i,n , a i,n is the vehicle speed and acceleration; L l,n , L w,n are the length and width of the vehicle; FCL is the fully connected neural network layer using the ReLU activation function; W fcl , W en is the weight function of the corresponding network layer.

4. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 3, characterized in that: The vehicle space influence field function is: Among them, (x0, y0) is the position coordinate of the vehicle at the center of the source; (x, y) is the position coordinate point of other vehicles; D is the vehicle danger level; σ is the spatial influence field distribution factor.

5. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 4, characterized in that: The vehicle space influence vector is: Where: σ is the softmax function; W σ is the weight parameter of the corresponding function. is the vehicle space influence field function f IR Get obs At the moment, vehicle n is affected by the spatial influence of other vehicles in the scene Normalized by the softmax function as the spatial attention weight factor of other vehicles, for and the vehicle's spatial feature vector H i,n The vehicle space feature vector is obtained by multiplication, which is used to characterize the spatial influence relationship between vehicles.

6. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 1, characterized in that: The dynamic programming cost function of the ST graph speed curve is: Where: S' is the vehicle speed; v ref is the expected speed of the vehicle; S'' is the jerk of the vehicle when it is running; Indicates the distance between the vehicle and the obstacle; Indicates the cost of vehicle acceleration and impact, and comfort cost; Represents the difference between the planned speed and the expected speed of the vehicle, and the formal efficiency cost of the vehicle; is the weight parameter of each cost.

7. The method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction according to claim 1, characterized in that: The numerical optimization cost function of the optimal speed curve is: The first and second terms are the comfort cost functions when solving numerical optimization, and the third term is the cost function of the error between the numerical optimization result and the dynamic programming result when solving; The body measurement sensor is arranged on the surfaces of the left and right handhold positions of the steering wheel rim and has a symmetrical structure.

8. A method for autonomous driving vehicle trajectory planning based on interactive vehicle trajectory prediction, characterized in that: The vehicle is provided with an autonomous driving vehicle trajectory planning method based on interactive vehicle trajectory prediction as described in any one of claims 2-8.

Images