A trajectory planning method for autonomous driving vehicles considering surrounding vehicle uncertainty

Abstract

The present invention is applicable to the field of autonomous driving technology, and provides a method for trajectory planning of an autonomous driving vehicle that takes into account the uncertainty of surrounding vehicles. Based on the calculation of the n-step probability reachable set of surrounding vehicles and the model predictive control method, the present invention enables the autonomous driving system to predict the possible motion trajectories of other vehicles in advance, thereby more effectively avoiding potential collision risks and maintaining a smooth trajectory in the face of unforeseen situations. The present invention can improve the safety and robustness of the autonomous driving system, provide better adaptability and flexibility, optimize the driving experience, and improve overall traffic efficiency. The present invention utilizes a combination of reachable sets and model predictive control, introduces a predicted n-step probability reachable set when considering obstacle vehicles, and by predicting future states and optimizing control strategies, ensures driving safety and comfort while considering uncertainty, and improves system performance and stability.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a trajectory planning method for an autonomous driving vehicle taking into account the uncertainty of surrounding vehicles. Background Art

[0002] In recent years, the number of motor vehicles and cars in China has increased year by year. The increase in the number of vehicles has led to more complicated road traffic conditions, and the introduction of autonomous driving is expected to reduce the occurrence of traffic accidents and improve road safety. The intelligent vehicle driving system is mainly composed of a perception system, a trajectory planning system, and a control system. The trajectory planning system is a key technology that affects the safety, stability, and comfort of vehicle driving. It mainly focuses on how to generate a driving trajectory that conforms to the vehicle's kinematics, and how to track the predetermined trajectory smoothly and comfortably. At the same time, in a real environment, changes in the vehicle's own state and the state of the surrounding vehicles will affect the vehicle's motion trajectory and control effect. How to quickly plan local trajectories and output control quantities is of great significance to intelligent driving cars. Therefore, how to predict the surrounding vehicle trajectory and design a trajectory planner so that the vehicle can plan a safe and comfortable trajectory based on the vehicle's feedback status information and surrounding vehicle information is the main planning problem. For the prediction of the surrounding vehicle trajectory and trajectory planning, there are mainly the following problems:

[0003] 1) A common method for predicting the future state of traffic conditions around a target vehicle is deterministic prediction, which assumes that other vehicles around the target vehicle maintain their current motion within a limited time frame. However, this method ignores the probabilities of all possible movements of surrounding vehicles and therefore cannot correctly identify unexpected driving situations. Trajectory planning methods that use deterministic predictions will result in the generated trajectory being unable to predict the lateral position changes caused by the uncertainty of surrounding vehicles, greatly affecting the safety and comfort of trajectory planning.

[0004] 2) Planning schemes that consider uncertainty often use robust predictive control. In order to ensure driving safety and stability, the planned trajectory is too conservative. Conservative control strategies often mean that the vehicle needs to operate in a less efficient operating range to ensure driving stability and safety, which will greatly increase time and energy costs. In response to the above problems, the present invention proposes a trajectory planning method for an autonomous driving vehicle that considers the uncertainty of the surrounding vehicle. Summary of the invention

[0005] The purpose of the present invention is to provide a trajectory planning method for an autonomous driving vehicle taking into account the uncertainty of surrounding vehicles, aiming to solve the problems raised in the above-mentioned background technology.

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

[0007] A method for trajectory planning of an autonomous driving vehicle considering surrounding vehicle uncertainty comprises the following steps:

[0008] Step 1: Target vehicle uncertainty prediction: Use vehicle kinematics to predict the future trajectory of the target vehicle, and express the uncertainty of the surrounding vehicle through an n-step probabilistic reachable set;

[0009] Step 2: Design of nonlinear model prediction trajectory planner: Build a vehicle model, construct a cost function and consider physical constraints and safety boundary constraints, and convert probabilistic constraints into deterministic constraints to obtain the final nonlinear planning problem, and obtain the planned future trajectory through the trajectory planner.

[0010] Furthermore, in the step of predicting the future trajectory of the target vehicle using vehicle kinematics, the nominal kinematic model of the target vehicle is described as:

[0011] ;

[0012] in and are the longitudinal position derivative and lateral position derivative of the target vehicle, respectively. and are the longitudinal position and lateral position of the target vehicle, respectively. and are the longitudinal speed and lateral speed of the target vehicle, respectively. is the yaw angle of the target vehicle;

[0013] The kinematic model is discretized to obtain the nominal discrete kinematic model for trajectory prediction, which is defined as:

[0014] ;

[0015] in and The target vehicles are k The nominal longitudinal position and nominal lateral position at time +1, and The target vehicles are k The nominal longitudinal position and nominal lateral position at the moment, and The target vehicles are k The nominal longitudinal velocity and nominal lateral velocity at time, For the target vehicle k The nominal yaw angle at time, T s is the sampling time.

[0016] Furthermore, in the step of expressing the uncertainty of the surrounding vehicle through the n-step probabilistic reachable set, assuming that the uncertainty only affects the lateral position of the vehicle, we get:

[0017] ;

[0018] in and The target vehicles are k +1 vertical and horizontal positions at the moment, and The target vehicles are k The vertical and horizontal positions at the moment, and The target vehicles are k The longitudinal and lateral velocities at the moment, For the target vehicle k The yaw angle at the moment, w ( k ) is a random disturbance;

[0019] Assuming random disturbance w ( k ) belongs to Q Independent and identically distributed, Q Assuming that w ( k ), the error is expressed as follows:

[0020] ;

[0021] in e ( k +1) is the lateral displacement k +1 moment error, e ( k ) is the lateral displacement k The error at the moment is used to obtain the predicted position of the obstacle vehicle through the n-step probability reachable set method;

[0022] For the following formula, n Sets ≥ 0 is called the probability level p The probability of a set reachable in n steps is:

[0023] ;

[0024] in For random variables falling within the set probability.

[0025] Assume that k The uncertainty disturbance at time remains unchanged and still belongs to Q The independent and identically distributed set of disturbance probabilities in the prediction time domain can be reached by using iterative definition:

[0026] ;

[0027] satisfy ;in Indicated in k time e The i-step probability reachable set; reach is a set operation function, which means that the reachable set at the current moment is calculated from the reachable set at the previous moment and the perturbation; for k time e of i -1-step probability reachable set, for k Predicted at all times k + i -1 time disturbance, N p The prediction time domain for the trajectory planner;

[0028] exist k The horizontal and vertical positions and uncertainty information of the target vehicle at the moment are expressed as follows:

[0029] ;

[0030] in , The probability reachable set of the lateral displacement of the obstacle vehicle is obtained:

[0031] ;

[0032] in is the probability reachable set of the nominal lateral displacement of the obstacle vehicle; is the perturbation probability reachable set, is the Minkowski sum, which is defined as follows:

[0033] ;

[0034] in a + b Representing a collection A An element in the set B The vector sum of an element in ;

[0035] The i-step probability reachable set of the target vehicle is expressed as:

[0036] ;

[0037] The dynamic equations for the mean and variance are as follows:

[0038] ;

[0039] ;

[0040] in for k The mean error at +1 time, for k The mean of the time error, for k The mean of the time disturbance, for k The variance of the error at +1, for k The variance of the time error, for k The variance of the moment disturbance;

[0041] Based on the Chebyshev boundary formula, k The i-step probability reachable set at the moment is expressed as:

[0042] ;

[0043] in c Indicates the probability level is 1 / c 2 The reachable set boundary of ;

[0044] Assuming that the lateral motion of the target vehicle follows a Gaussian distribution, within a 95% confidence interval k The i-step probability of reaching the set boundary at the moment is expressed as ,in For k Predicted at all times k + i The variance of the time error, For k Predicted at all times k + i The mean of the time errors.

[0045] Further, the vehicle model includes a vehicle kinematics model, a vehicle three-degree-of-freedom model and a tire brush model;

[0046] The vehicle kinematic model is described as:

[0047] ;

[0048] in is the vehicle longitudinal position derivative, is the vehicle lateral position derivative, X and Y Represent the longitudinal and lateral positions of the vehicle, respectively. v x and v y denote the longitudinal velocity and lateral velocity of the vehicle, respectively. represents the yaw angle of the vehicle;

[0049] The vehicle three-degree-of-freedom model takes into account the longitudinal, lateral and yaw motions of the vehicle. Based on Newton's second law of motion and the torque balance law, the longitudinal velocity, lateral velocity and yaw motion equations in the vehicle coordinate system are obtained as follows:

[0050] ;

[0051] in represents the vehicle lateral velocity derivative, represents the vehicle longitudinal velocity derivative, represents the vehicle's yaw rate derivative, ω r represents the vehicle's yaw rate, F yf and F yr They represent the resultant lateral forces of the tires on the front axle and the rear axle of the vehicle, δ represents the front wheel turning angle of the vehicle, m Indicates the vehicle mass, a x represents the longitudinal acceleration, l f and l r Represent the distance from the vehicle's center of mass to the front and rear axles, respectively. I z Indicates that the vehicle is around z The moment of inertia of the shaft;

[0052] The tire brush model models the vehicle tires, and the tire lateral forces on the front and rear wheels are calculated using the following equations:

[0053] ;

[0054] in, F y is the lateral force of the tire, C α represents the tire lateral stiffness, α Indicates the tire slip angle, µ represents the road adhesion coefficient, F z Indicates the vertical load on the vehicle tire;

[0055] When the vehicle is turning, the tire side slip angles of the front and rear wheels α f and α r The calculation formula is as follows:

[0056] ;

[0057] In the tire brush model, the vertical loads on the vehicle tires at the front and rear wheels F zf and F zr Calculated by the following formula:

[0058] ;

[0059] in g represents the acceleration due to gravity, h cg Indicates the height of the vehicle's center of gravity. L Indicates the distance from the front axle to the rear axle of the vehicle.

[0060] Furthermore, the design steps of the trajectory planner are as follows:

[0061] Select vertical speed v x , lateral speed v y , yaw angle , yaw rate ω r , longitudinal displacement in geodetic coordinate system X and lateral displacement Y As a state variable, the state of the trajectory planner is set to x ,Right now:

[0062] ;

[0063] Select front wheel angle δ and longitudinal moment F x As the control quantity of the trajectory planner, that is:

[0064] ;

[0065] in u is the control quantity of the planner;

[0066] The output is the planned trajectory information, then:

[0067] ;

[0068] in y is the planned trajectory information, v xe is the planned longitudinal speed information, X e is the planned longitudinal position information, Y e is the planned horizontal position information, is the planned yaw angle information;

[0069] The vehicle model of the system in continuous time is obtained through the vehicle kinematic model and dynamic model as follows:

[0070] ;

[0071] The vehicle model in continuous time is discretized by forward Euler discretization, and the sampling time is T s ,exist k At this moment, the corresponding discrete vehicle model is as follows:

[0072] ;

[0073] In order to make the target vehicle track the expected longitudinal velocity and the global path when there are no obstacles, the control objective is described as:

[0074] ;

[0075] ;

[0076] Considering the change of the control quantity in the optimization objective, the following formula is designed:

[0077] ;

[0078] in for k Predicted at all times k + i The longitudinal velocity at time, is the reference speed, for k Predicted at all times k + i The yaw angle at the moment, is the reference yaw angle, for k Predicted at all times k + i The yaw rate at time, is the reference yaw rate, for k Predicted at all times k + i The horizontal position at the moment, is the reference lateral position, and They are k Predicted at all times k + i Moment and k + i- The front wheel turning angle at time 1, and They are kPredicted at all times k + i Moment and k + i- Longitudinal moment at time 1;

[0079] According to the vehicle's desired reference path, reference speed, and output control constraint, the objective function is set as:

[0080] ;

[0081] in, Q 1, Q 2, Q 3, Q 4 are A 1, A 2, A 3, A 4 is a weighted coefficient vector, which controls the weight of different control targets in the objective function. A 1, A 2, A 3, A 4, A 5 is each control target, C 1 is the weighting coefficient of the control quantity change;

[0082] During the vehicle trajectory planning process, the vehicle position always complies with the constraints of the upper and lower boundaries of the road, namely:

[0083] ;

[0084] in Y min and Y max They are the upper and lower boundary constraints of the road respectively;

[0085] According to the actual requirements of the system, the longitudinal moment F x and front wheel angle δ The values ​​of should be between the upper and lower limits of the safety constraint, as shown in the following formula:

[0086] ;

[0087] in For k Always k+i time u predictions, u min and u max are the upper and lower bounds of the safety constraint, u min and umax They are defined as:

[0088] ;

[0089] in δ min and δ max are the upper and lower limits of the front wheel turning angle, F xmin and F xmax are the upper and lower limits of longitudinal moment respectively;

[0090] The obstacle avoidance constraints that should be met when encountering an obstacle vehicle are as follows:

[0091] ;

[0092] in , k 1 and k 2 are the obstacle avoidance coefficients of two different obstacle vehicles, r is the constraint radius of the obstacle vehicle, for k Predicted at all times k + i The longitudinal position of the vehicle at the time, for k Predicted at all times k + i The longitudinal position of the obstacle vehicle at time for k Predicted at all times k + i The lateral position of the vehicle at the time, for k Predicted at all times k + i The lateral position of the obstacle vehicle at the moment.

[0093] Compared with the prior art, the present invention has the following beneficial effects:

[0094] 1. Based on the calculation and model prediction control method of the n-step probabilistic reachable set of surrounding vehicles, the automatic driving system can predict the possible movement trajectory of other vehicles in advance, thereby more effectively avoiding potential collision risks and maintaining a smooth trajectory in the face of unforeseen situations. The present invention can improve the safety and robustness of the automatic driving system, provide better adaptability and flexibility, optimize the driving experience, and improve overall traffic efficiency.

[0095] 2. The present invention combines reachable sets with model predictive control and introduces predicted n-step probabilistic reachable sets when considering obstacle vehicles. By predicting future states and optimizing control strategies, it not only ensures driving safety and comfort while considering uncertainty, but also improves system performance and stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] Figure 1 It is a flow chart of the present invention.

[0097] Figure 2 It is a framework diagram of the present invention.

[0098] Figure 3 This is the trajectory planning curve of the vehicle at t=18s in the experimental verification of the present invention.

[0099] Figure 4 This is the trajectory planning curve of the vehicle at t=18.5s in the experimental verification of the present invention.

[0100] Figure 5 This is the trajectory planning curve of the vehicle at t=19s in the experimental verification of the present invention.

[0101] Figure 6 This is the trajectory planning curve of the vehicle at t=19.5s in the experimental verification of the present invention.

[0102] Figure 7 This is the trajectory planning curve of the vehicle at t=20s in the experimental verification of the present invention. DETAILED DESCRIPTION

[0103] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with 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.

[0104] The specific implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0105] like Figure 1 and Figure 2 As shown, a method for trajectory planning of an autonomous driving vehicle considering the uncertainty of surrounding vehicles is provided in one embodiment of the present invention, comprising the following steps:

[0106] Step 1: Predict target vehicle uncertainty: Use vehicle kinematics to predict the future trajectory of the target vehicle, and express the uncertainty of the surrounding vehicle through an n-step probabilistic reachable set. The n-step probabilistic reachable set of the surrounding vehicle uncertainty plays a key role in dealing with the dynamic uncertainty environment faced by autonomous driving vehicles. Predicting the probabilistic reachable set of obstacle vehicles using the Chebyshev boundary formula probabilistic prediction method can help plan safe trajectories, and perform effective trajectory planning and collision avoidance while ensuring safety and robustness.

[0107] Step 2, nonlinear model prediction trajectory planner design: build a vehicle model, construct a cost function and consider physical constraints and safety boundary constraints, and convert the probability constraints into deterministic constraints to obtain the final nonlinear planning problem, and obtain the planned future trajectory through the trajectory planner. The vehicle trajectory planner adopts the model predictive control method, and provides the controller with the motion state trajectory for a period of time in the future under the conditions of considering constraints such as vehicle obstacle avoidance and road boundaries, so that the controller can respond to environmental changes in advance and make corresponding control actions.

[0108] In the embodiment of the present invention, the target vehicle uncertainty prediction module and the trajectory planner module are built in the Matlab-Simulink environment, and the controlled vehicle and the target vehicle are built in the CarSim simulation software.

[0109] The control goal of the present invention is to plan a trajectory that meets the constraints of road boundaries and target vehicle safety distance on the basis of considering the uncertainty of the surrounding vehicles, so that the generated trajectory has higher comfort and safety. The uncertainty of the surrounding vehicles is to obtain the state information of the target vehicle through the perception module, predict the future trajectory of the target vehicle based on the kinematics of the target vehicle, form a probabilistic reachable set, and complete the planning of the vehicle's driving trajectory under the consideration of the probabilistic reachable set. The vehicle trajectory planner considers the road boundary constraints and the vehicle safety distance constraints, and introduces the probabilistic reachable set generated by the uncertainty of the surrounding vehicles into the vehicle safety distance constraints. Therefore, an optimized collision-free trajectory can be obtained under an uncertain environment.

[0110] As a preferred embodiment of the present invention, the specific process of step 1 is as follows:

[0111] In order to predict the trajectory and uncertainty of the target vehicle, a trajectory prediction method based on the vehicle kinematic model is adopted. The vehicle kinematic model is to ignore the vertical motion of the vehicle and not consider its lateral sliding when the vehicle is driving at a low speed on a flat road with small curvature. It studies the change law of the vehicle coordinates in the spatial geometry caused by the vehicle's movement speed and time change. The nominal kinematic model of the target vehicle under the assumed conditions (the nominal model refers to the model without disturbance) is described as:

[0112] ;

[0113] in and are the longitudinal position derivative and lateral position derivative of the target vehicle, respectively. and are the longitudinal position and lateral position of the target vehicle, respectively. and are the longitudinal speed and lateral speed of the target vehicle, respectively. is the yaw angle of the target vehicle;

[0114] The kinematic model is discretized to obtain the nominal discrete kinematic model for trajectory prediction, which is defined as:

[0115] ;

[0116] in and The target vehicles are k The nominal longitudinal position and nominal lateral position at time +1, and The target vehicles are k The nominal longitudinal position and nominal lateral position at the moment, and The target vehicles are k The nominal longitudinal velocity and nominal lateral velocity at time, For the target vehicle k The nominal yaw angle at time, T s is the sampling time.

[0117] Assuming that the uncertainty only affects the lateral position of the vehicle, we obtain:

[0118] ;

[0119] in and The target vehicles are k +1 vertical and horizontal positions at the moment, and The target vehicles are k The vertical and horizontal positions at the moment, and The target vehicles are k The longitudinal and lateral velocities at the moment, For the target vehicle k The yaw angle at the moment, w ( k ) is a random disturbance;

[0120] Assuming random disturbance w (k ) belongs to Q Independent and identically distributed, Q Assuming that w ( k ), the error is expressed as follows:

[0121] ;

[0122] in e ( k +1) is the lateral displacement k +1 moment error, e ( k ) is the lateral displacement k The error at the moment is used to obtain the predicted position of the obstacle vehicle through the n-step probability reachable set method;

[0123] For the following formula, n Sets ≥ 0 is called the probability level p The probability of a set reachable in n steps is:

[0124] ;

[0125] in For random variables falling within the set probability;

[0126] Assume that k The uncertainty disturbance at time remains unchanged and still belongs to Q The independent and identically distributed set of disturbance probabilities in the prediction time domain can be reached by using iterative definition:

[0127] ;

[0128] satisfy ;in Indicated in k time e The i-step probability reachable set; reach is a set operation function, which means that the reachable set at the current moment is calculated from the reachable set at the previous moment and the perturbation; for k time e of i -1 step probability reachable set, for k Predicted at all times k + i -1 time disturbance, N p The prediction time domain for the trajectory planner;

[0129] Therefore kThe horizontal and vertical positions and uncertainty information of the target vehicle at the moment are expressed as follows:

[0130] ;

[0131] in , From this, we can get the probability reachable set of the lateral displacement of the obstacle vehicle:

[0132] ;

[0133] in is the probability reachable set of the nominal lateral displacement of the obstacle vehicle; is the perturbation probability reachable set, is the Minkowski sum, which is defined as follows:

[0134] ;

[0135] in a + b Representing a collection A An element in the set B The vector sum of an element in ;

[0136] The i-step probability reachable set of the target vehicle is expressed as:

[0137] ;

[0138] The common method to construct a probabilistic reachable set is to calculate the mean and variance at each moment, and then use the Chebyshev boundary formula to calculate the probabilistic reachable set. The dynamic equations of the mean and variance are as follows:

[0139] ;

[0140] ;

[0141] in for k The mean error at +1 time, for k The mean of the time error, for k The mean of the time disturbance, for k The variance of the error at +1, for k The variance of the time error, for k The variance of the moment disturbance;

[0142] Based on the Chebyshev boundary formula, k The i-step probability reachable set at the moment is expressed as:

[0143] ;

[0144] in c Indicates the probability level is 1 / c 2 The reachable set boundary of ;

[0145] Chebyshev's inequality is applicable to any random variable with any distribution. For special probability distributions, such as Gaussian distribution, the corresponding probability reachable set can use its distribution characteristics to obtain the probability boundary. Assuming that the lateral motion of the target vehicle follows Gaussian distribution, within the 95% confidence interval k The i-step probability at time can be expressed as ,in For k Predicted at all times k + i The variance of the time error, For k Predicted at all times k + i The mean of the time errors.

[0146] As a preferred embodiment of the present invention, the specific process of step 2 is as follows:

[0147] (1) Establishment of vehicle model;

[0148] The vehicle model plays an important role in controller design. The feasibility curve generated by the planning needs to meet the kinematic and dynamic characteristics of the vehicle itself. At the same time, the output control quantity must also meet the actuator constraints and safe driving constraints of the vehicle. The vehicle model includes the vehicle kinematic model, the vehicle three-degree-of-freedom model and the tire brush model;

[0149] The vehicle kinematic model under the assumed conditions is described as:

[0150] ;

[0151] in is the vehicle longitudinal position derivative, is the vehicle lateral position derivative, X and Y Represent the longitudinal and lateral positions of the vehicle, respectively. v x and v y denote the longitudinal velocity and lateral velocity of the vehicle, respectively. represents the yaw angle of the vehicle;

[0152] The vehicle three-degree-of-freedom model considers the longitudinal, lateral and yaw motions of the vehicle, and assumes that the vehicle body and chassis are rigidly connected, the body has no roll and pitch motion above the suspension, and the body roll and pitch motion caused by the longitudinal deformation of the tires are ignored. Based on Newton's second law of motion and the torque balance law, the longitudinal speed, lateral speed and yaw motion equations in the vehicle coordinate system are obtained as follows:

[0153] ;

[0154] in represents the vehicle lateral velocity derivative, represents the vehicle longitudinal velocity derivative, represents the vehicle's yaw rate derivative, ω r represents the yaw rate of the vehicle, F yf and F yr They represent the resultant lateral forces of the tires on the front axle and the rear axle of the vehicle, respectively. δ represents the front wheel turning angle of the vehicle, m Indicates the vehicle mass, a x represents the longitudinal acceleration, l f and l r Represent the distance from the vehicle's center of mass to the front and rear axles, respectively. I z Indicates that the vehicle is around z The moment of inertia of the shaft;

[0155] The tire is the only part of the vehicle that contacts the road. The friction between the tire and the road directly affects the actual driving conditions of the vehicle, such as acceleration and deceleration, as well as uphill and downhill driving. Its dynamic characteristics have a very important influence on the driving comfort and smoothness of the vehicle, and its adhesion characteristics with the road will also directly affect the driving force, braking force and steering characteristics of the vehicle. Because the tire model has a high degree of complexity and its dynamics has a more significant nonlinear characteristic. Therefore, the use of a suitable tire model has a very important influence on vehicle dynamics modeling. The present invention uses a tire brush model to model the vehicle tire, and the tire lateral force of the front and rear wheels is calculated by the following formula:

[0156] ;

[0157] in, F y is the lateral force of the tire, C α represents the tire lateral stiffness, α Indicates the tire slip angle, µrepresents the road adhesion coefficient, F z Indicates the vertical load on the vehicle tire;

[0158] When a vehicle turns, the front and rear wheels will generate side slip angles as they turn, which will have a great impact on the lateral force of the tires. α f and α r The calculation formula is as follows:

[0159] ;

[0160] In the tire brush model, the vertical loads on the vehicle tires at the front and rear wheels F zf and F zr Calculated by the following formula:

[0161] ;

[0162] in g represents the acceleration due to gravity, h cg Indicates the height of the vehicle's center of gravity. L Indicates the distance from the front axle to the rear axle of the vehicle.

[0163] (2) Design of trajectory planner;

[0164] Select vertical speed v x , lateral speed v y , yaw angle , yaw rate ω r , longitudinal displacement in geodetic coordinate system X and lateral displacement Y As a state variable, the state of the trajectory planner is set to x ,Right now:

[0165] ;

[0166] Select front wheel angle δ and longitudinal moment F x As the control quantity of the trajectory planner, that is:

[0167] ;

[0168] in u is the control quantity of the planner;

[0169] The output is the planned trajectory information, then:

[0170] ;

[0171] in y is the planned trajectory information, v xe is the planned longitudinal speed information, X e is the planned longitudinal position information, Y e is the planned horizontal position information, is the planned yaw angle information;

[0172] The vehicle model of the system in continuous time is obtained through the vehicle kinematic model and dynamic model as follows:

[0173] ;

[0174] When designing the controller, the continuous model needs to be discretized. The present invention discretizes the vehicle model in continuous time by forward Euler discretization. The sampling time is T s ,exist k At this moment, the corresponding discrete vehicle model is as follows:

[0175] ;

[0176] In order to make the target vehicle track the expected longitudinal velocity and the global path when there are no obstacles, the control objective is described as:

[0177] ;

[0178] ;

[0179] In order to make the output control amount smoother, it is also necessary to consider the change of the control amount in the optimization target and design the following formula:

[0180] ;

[0181] in for k Predicted at all times k + i The longitudinal velocity at time, is the reference speed, for k Predicted at all times k + i The yaw angle at the moment, is the reference yaw angle, for k Predicted at all times k + iThe yaw rate at time, is the reference yaw rate, for k Predicted at all times k + i The horizontal position at the moment, is the reference lateral position, and They are k Predicted at all times k + i Moment and k + i- The front wheel turning angle at time 1, and They are k Predicted at all times k + i Moment and k + i- Longitudinal moment at time 1;

[0182] According to the vehicle's desired reference path, reference speed, and output control constraint, the objective function is set as:

[0183] ;

[0184] in, Q 1, Q 2, Q 3, Q 4 are A 1, A 2, A 3, A 4 is a weighted coefficient vector, which controls the weight of different control targets in the objective function. A 1, A 2, A 3, A 4, A 5 is each control target, C 1 is the weighting coefficient of the control quantity change;

[0185] During the vehicle trajectory planning process, the vehicle position always complies with the constraints of the upper and lower boundaries of the road, namely:

[0186] ;

[0187] in Y min and Y max They are the upper and lower boundary constraints of the road respectively;

[0188] According to the actual requirements of the system, the control quantity should comply with the conventional safety constraints of the system, as shown in the following formula, that is, the longitudinal torqueF x and front wheel angle δ The values ​​of should be between the upper and lower limits of the safety constraints:

[0189] ;

[0190] in For k Always k+i time u predictions, u min and u max are the upper and lower bounds of the safety constraint, u min and u max They are defined as:

[0191] ;

[0192] in δ min and δ max are the upper and lower limits of the front wheel turning angle, F xmin and F xmax are the upper and lower limits of longitudinal moment respectively;

[0193] The obstacle avoidance constraints that should be met when encountering an obstacle vehicle are as follows:

[0194] ;

[0195] From the reachable set, we know , k 1 and k 2 are the obstacle avoidance coefficients of two different obstacle vehicles, r is the constraint radius of the obstacle vehicle, for k Predicted at all times k + i The longitudinal position of the vehicle at the time, for k Predicted at all times k + i The longitudinal position of the obstacle vehicle at time for k Predicted at all times k + i The lateral position of the vehicle at the time, for k Predicted at all times k + i The lateral position of the obstacle vehicle at the moment.

[0196] Experimental Example 1: In order to verify the effectiveness of the trajectory planner proposed in the present invention, the following experiment was designed for verification.

[0197] The sampling time of the trajectory planner in the simulation environment is 100ms, and the prediction time domain and control time domain of the trajectory planner are 40. In the structured road, the lane width is 3.5m, the centerline of lane one is located on the X axis, and the centerline of lane two is at Y=3.5m. The initial position of the vehicle is at the origin of the inertial coordinate system, and it moves forward in the positive direction of the X axis at an initial speed of 20m / s. The upper expected speed is 20m / s, the road adhesion coefficient is set to 0.8, and the reference trajectory is the X axis. The longitudinal position of the obstacle vehicle is 100m in the positive direction of the X axis, and the lateral position is located at the centerline of lane one, that is, (100,0) in the inertial coordinate system, and moves forward in the positive direction of the X axis at a constant speed of 15m / s. The vehicle maintains the reference trajectory and performs obstacle avoidance overtaking after detecting the obstacle vehicle in front, and continues to drive along the reference trajectory after overtaking.

[0198] At t=18s, Figure 3 As shown, the vehicle path planning curve starts to change lanes to the left; Figure 4 , Figure 5 and Figure 6 They are the path planning curves during the lane change process; at t=20s Figure 7 As shown in the figure, the vehicle path planning curve starts to change lanes to the right, so that the vehicle returns to the center line of lane 1. Due to the increase in lateral disturbance, the target obstacle vehicle gradually deviates from the center line of lane 1 during driving. At this time, the vehicle can still plan a relatively smooth and safe trajectory, which is sufficient to verify the safety and effectiveness of the present invention.

[0199] The above are only preferred embodiments of the present invention. It should be pointed out that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention. These should also be regarded as the protection scope of the present invention. These will not affect the effect of the implementation of the present invention and the practicality of the patent.

Claims

1. A trajectory planning method for autonomous vehicles considering surrounding vehicle uncertainty, It is characterized in that The following steps are involved: Step 1: Target vehicle uncertainty prediction: Use vehicle kinematics to predict the future trajectory of the target vehicle, and express the uncertainty of the surrounding vehicle through an n-step probabilistic reachable set; Step 2: Design of nonlinear model prediction trajectory planner: Build a vehicle model, construct a cost function, consider physical constraints and safety boundary constraints, and transform probabilistic constraints into deterministic constraints to obtain the final nonlinear planning problem. The planned future trajectory is obtained through the trajectory planner. In the step of predicting the future trajectory of the target vehicle using vehicle kinematics, the nominal kinematic model of the target vehicle is described as: ； in and are the longitudinal position derivative and lateral position derivative of the target vehicle, respectively. and are the longitudinal position and lateral position of the target vehicle, respectively. and are the longitudinal speed and lateral speed of the target vehicle, respectively. is the yaw angle of the target vehicle; The kinematic model is discretized to obtain the nominal discrete kinematic model for trajectory prediction, which is defined as: ； in and The target vehicles are k The nominal longitudinal position and nominal lateral position at time +1, and The target vehicles are k The nominal longitudinal position and nominal lateral position at the moment, and The target vehicles are k The nominal longitudinal velocity and nominal lateral velocity at time, For the target vehicle k The nominal yaw angle at time, T s is the sampling time; In the step of expressing the uncertainty of the surrounding vehicle through the n-step probabilistic reachable set, assuming that the uncertainty only affects the lateral position of the vehicle, we get: ； in and The target vehicles are k +1 vertical and horizontal positions at the moment, and The target vehicles are k The vertical and horizontal positions at the moment, and The target vehicles are k The longitudinal and lateral velocities at the moment, For the target vehicle k The yaw angle at the moment, w ( k ) is a random disturbance; Assuming random disturbance w ( k ) belongs to Q Independent and identically distributed, Q Assuming that w ( k ), the error is expressed as follows: ； in e ( k +1) is the lateral displacement k +1 moment error, e ( k ) is the lateral displacement k The error at the moment is used to obtain the predicted position of the obstacle vehicle through the n-step probability reachable set method; For the following formula, n Sets ≥ 0 is called the probability level p The probability of a set reachable in n steps is: ； in For random variables falling within the set probability; Assume that k The uncertainty disturbance at time remains unchanged and still belongs to Q The independent and identically distributed set of disturbance probabilities in the prediction time domain can be reached by using iterative definition: ； satisfy ;in Indicated in k time e The i-step probability reachable set; reach is a set operation function, which means that the reachable set at the current moment is calculated from the reachable set at the previous moment and the perturbation; for k time e of i -1-step probability reachable set, for k Predicted at all times k + i -1 time disturbance, N p The prediction time domain for the trajectory planner; exist k The horizontal and vertical positions and uncertainty information of the target vehicle at the moment are expressed as follows: ； in , The probability reachable set of the lateral displacement of the obstacle vehicle is obtained: ； in is the probability reachable set of the nominal lateral displacement of the obstacle vehicle; is the perturbation probability reachable set, is the Minkowski sum, which is defined as follows: ； in a + b Representing a collection A An element in the set B The vector sum of an element in ; The i-step probability reachable set of the target vehicle is expressed as: ； The dynamic equations for the mean and variance are as follows: ； ； in for k The mean error at +1 time, for k The mean of the time errors, for k The mean of the time disturbance, for k The variance of the error at +1, for k The variance of the time error, for k The variance of the moment disturbance; Based on the Chebyshev boundary formula, k The i-step probability reachable set at the moment is expressed as: ； in c Indicates the probability level is 1 / c 2 The reachable set boundary of ; Assuming that the lateral motion of the target vehicle follows a Gaussian distribution, within a 95% confidence interval k The i-step probability at the moment can be expressed as ,in For k Predicted at all times k + i The variance of the time error, For k Predicted at all times k + i The mean of the time errors.

2. The method for trajectory planning of an autonomous driving vehicle considering surrounding vehicle uncertainty according to claim 1, It is characterized in that The vehicle model includes a vehicle kinematics model, a vehicle three-degree-of-freedom model and a tire brush model; The vehicle kinematic model is described as: ； in is the vehicle longitudinal position derivative, is the vehicle lateral position derivative, X and Y Represent the longitudinal and lateral positions of the vehicle, respectively. v x and v y denote the longitudinal velocity and lateral velocity of the vehicle, respectively. represents the yaw angle of the vehicle; The vehicle three-degree-of-freedom model takes into account the longitudinal, lateral and yaw motions of the vehicle. Based on Newton's second law of motion and the torque balance law, the longitudinal velocity, lateral velocity and yaw motion equations in the vehicle coordinate system are obtained as follows: ； in represents the vehicle lateral velocity derivative, represents the vehicle longitudinal velocity derivative, represents the vehicle's yaw rate derivative, ω r represents the vehicle's yaw rate, F yf and F yr They represent the resultant lateral forces of the tires on the front axle and the rear axle of the vehicle, respectively. δ represents the front wheel turning angle of the vehicle, m Indicates the vehicle mass, a x represents the longitudinal acceleration, l f and l r Represent the distance from the vehicle's center of mass to the front and rear axles, respectively. I z Indicates that the vehicle is around z The moment of inertia of the shaft; The tire brush model models the vehicle tires, and the tire lateral forces on the front and rear wheels are calculated using the following equations: ； in, F y is the lateral force of the tire, C α represents the tire lateral stiffness, α Indicates the tire slip angle, µ represents the road adhesion coefficient, F z Indicates the vertical load on the vehicle tire; When the vehicle is turning, the tire side slip angles of the front and rear wheels α f and α r The calculation formula is as follows: ； In the tire brush model, the vertical loads on the vehicle tires at the front and rear wheels F zf and F zr Calculated by the following formula: ； in g represents the acceleration due to gravity, h cg Indicates the height of the vehicle's center of gravity. L Indicates the distance from the front axle to the rear axle of the vehicle.

3. The automatic driving vehicle trajectory planning method considering surrounding vehicle uncertainty according to claim 2, It is characterized in that The design steps of the trajectory planner are as follows: Select vertical speed v x , lateral speed v y , yaw angle , yaw rate ω r , longitudinal displacement in geodetic coordinate system X and lateral displacement Y As a state variable, the state of the trajectory planner is set to x ,Right now: ; Select front wheel angle δ and longitudinal moment F x As the control quantity of the trajectory planner, that is: ; in u is the control quantity of the planner; The output is the planned trajectory information, then: ; in y is the planned trajectory information, v xe is the planned longitudinal speed information, X e is the planned longitudinal position information, Y e is the planned horizontal position information, is the planned yaw angle information; The vehicle model of the system in continuous time is obtained through the vehicle kinematic model and dynamic model as follows: ； The vehicle model in continuous time is discretized by forward Euler discretization, and the sampling time is T s ,exist k At this moment, the corresponding discrete vehicle model is as follows: ； In order to make the target vehicle track the expected longitudinal velocity and the global path when there are no obstacles, the control objective is described as: ； ； Considering the change of the control quantity in the optimization objective, the following formula is designed: ； in for k Predicted at all times k + i The longitudinal velocity at time, is the reference speed, for k Predicted at all times k + i The yaw angle at the moment, is the reference yaw angle, for k Predicted at all times k + i The yaw angular velocity at time, is the reference yaw rate, for k Predicted at all times k + i The horizontal position at the moment, is the reference lateral position, and They are k Predicted at all times k + i Moment and k + i- The front wheel turning angle at time 1, and They are k Predicted at all times k + i Moment and k + i- Longitudinal moment at time 1; According to the vehicle's desired reference path, reference speed, and output control constraint, the objective function is set as: ； in, Q 1 , Q 2 , Q 3 , Q 4 They are A 1 , A 2 , A 3 , A 4 The weighted coefficient vector is used to control the weight of different control targets in the objective function. A 1 , A 2 , A 3 , A 4 , A 5 For each control target, C 1 is the weighting coefficient of the control quantity change; During the vehicle trajectory planning process, the vehicle position always complies with the constraints of the upper and lower boundaries of the road, namely: ； in Y min and Y max They are the upper and lower boundary constraints of the road respectively; According to the actual requirements of the system, the longitudinal moment F x and front wheel angle δ The values ​​of should be between the upper and lower limits of the safety constraint, as shown in the following formula: ； in For k Always k+i time u predictions, u min and u max are the upper and lower bounds of the safety constraint, u min and u max They are defined as: ； in δ min and δ max are the upper and lower limits of the front wheel turning angle, F xmin and F xmax are the upper and lower limits of longitudinal moment respectively; The obstacle avoidance constraints that should be met when encountering an obstacle vehicle are as follows: ； in , k 1 and k 2 are the obstacle avoidance coefficients of two different obstacle vehicles, r is the constraint radius of the obstacle vehicle, for k Predicted at all times k + i The longitudinal position of the vehicle at the time, for k Predicted at all times k + i The longitudinal position of the obstacle vehicle at time for k Predicted at all times k + i The lateral position of the vehicle at the time, for k Predicted at all times k + i The lateral position of the obstacle vehicle at the moment.

Images