A Method and Device for Cooperative Planning of Path and Speed of an Autonomous Vehicle

Abstract

The present invention belongs to the field of autonomous vehicles, and particularly relates to a method and device for collaborative path and speed planning of an autonomous vehicle, including: constructing a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire, and using it as the motion limit boundary of the vehicle; adopting the quasi-steady state equilibrium method to traverse the steady state equilibrium states of the vehicle under different throttle openings and braking pressures, record the lateral acceleration and longitudinal acceleration at each moment, and calculate the improved G-G diagram through vector calculation; based on the obtained improved G-G diagram, adopting the forward-backward Euler method to establish an optimal speed model; adopting the road segmentation method to establish a trajectory optimization model under the shortest travel time; based on the road safety boundary and vehicle dynamics constraints, combining the optimal speed model and the trajectory optimization model, and using quadratic programming to construct an optimal path and speed collaborative planning model, and solving to obtain the optimal path and speed planning.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous vehicles, and particularly relates to a method and device for collaborative path and speed planning of autonomous vehicles. Background Art

[0002] With the rapid development of autonomous driving technology, the collaborative path and speed planning of autonomous vehicles has become one of the key technologies for achieving safe, efficient, and comfortable driving. Path planning aims to plan an optimal or feasible driving path for a vehicle according to constraints such as road environment and traffic rules; while speed planning is to reasonably set the driving speed of the vehicle on different road sections based on factors such as the vehicle's own dynamic performance and traffic conditions on the basis of path planning, so as to ensure the safety and driving efficiency of the vehicle. The collaborative path and speed planning is an important part of realizing the intelligent and autonomous decision-making of autonomous vehicles.

[0003] However, there are multiple problems with current research methods. Existing methods mostly simply decouple the path planning problem into two parts: path planning and vehicle speed planning in a hierarchical manner, ignoring the associated effects of the two, which easily leads to problems such as sub-optimal paths and unreasonable vehicle speed planning, thereby affecting driving efficiency and safety; they also mostly use the point mass model to evaluate the vehicle's limit state, ignoring the influence of the dynamic load differences of the four tires on the wheel adhesion, and the simplified model cannot accurately reflect the actual dynamic characteristics of the vehicle under different working conditions, resulting in deviations in the evaluation of the vehicle's dynamic limit performance, which will cause drivers or autonomous driving systems to overestimate the vehicle's capabilities and plan vehicle speeds beyond the actual execution capabilities of the vehicle, thereby increasing the risk of vehicle instability and safety accidents; there is also the use of a unified standard to evaluate the dynamic limit performance of different vehicles, ignoring the differences in the force states of vehicles with different drive forms, restricting the vehicle's movement efficiency and safety, and resulting in insufficient evaluation capabilities for high-performance vehicles; existing methods also mostly focus on the continuity of the planned speed, ignoring the impact of sudden changes in vehicle acceleration on reachability, which easily leads to the vehicle being unable to accurately track the planned speed during actual driving, and the occurrence of speed fluctuations and sudden changes in acceleration, thereby affecting the ride comfort and driving stability of the vehicle. Summary of the Invention

[0004] The present invention aims to provide a method and device for collaborative path and speed planning of autonomous vehicles. By accurately evaluating the dynamic limit performance of the vehicle, constructing an improved G-G diagram to obtain the lateral maximum acceleration and longitudinal maximum acceleration for constraints, then calculating the optimal speed model, and constructing a trajectory optimization model with the shortest travel time based on the optimal speed model, the two are coupled to construct an optimal path and speed collaborative planning model to obtain the optimal path and speed planning, so as to achieve the purpose of reducing the vehicle passing time, improving the passing efficiency and safety, and further realizing high-precision tracking control, and enhancing the driving efficiency and safety of autonomous vehicles.

[0005] A method for collaborative planning of path and speed of an autonomous vehicle, comprising the following steps:

[0006] Build a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire, and use it as the motion limit boundary of the vehicle;

[0007] Adopt the quasi-steady state equilibrium method to traverse the steady state equilibrium states of the vehicle under different throttle openings and braking pressures, record the lateral acceleration and longitudinal acceleration at each moment, and calculate the improved G-G diagram through vector calculation;

[0008] Based on the obtained improved G-G diagram, use the forward-backward Euler method to establish an optimal speed model;

[0009] Adopt the road segmentation method to establish a trajectory optimization model under the shortest travel time;

[0010] Based on the road safety boundary and vehicle dynamics constraints, combine the optimal speed model and the trajectory optimization model, and use quadratic programming to construct an optimal path and speed collaborative planning model, and solve to obtain the optimal path and speed planning.

[0011] By accurately evaluating the dynamic limit performance of the vehicle, constructing an improved G-G diagram to obtain the lateral maximum acceleration and longitudinal maximum acceleration for constraints, then calculating the optimal speed model, and constructing a trajectory optimization model under the shortest travel time based on the optimal speed model, the two are coupled to construct an optimal path and speed collaborative planning model to obtain the optimal path and speed planning, so as to achieve the purpose of reducing the vehicle passing time, improving the passing efficiency and safety, and further realizing high-precision tracking control, and enhancing the driving efficiency and safety of the autonomous vehicle.

[0012] Further, the process of building a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire and using it as the motion limit boundary of the vehicle specifically includes the following steps:

[0013] Build a vehicle dynamics model and a tire dynamics model;

[0014] Consider the transfer of the vehicle load in the lateral and forward directions, and calculate the vertical force characteristics of the four tires;

[0015] Use the adhesion coefficient between the tire and the road surface to calculate the ultimate adhesion force, obtain the dynamic force limit of the tire, and use it as the motion limit boundary of the vehicle.

[0016] Further, the expression of the vehicle dynamics model is:

[0017]

[0018] where m represents the vehicle mass; v x represents the longitudinal velocity; represents the longitudinal acceleration; v y represents the lateral velocity; represents the lateral acceleration; γ represents the yaw rate; F x21 represents the longitudinal force of the rear left wheel; F x22 represents the longitudinal force of the rear right wheel; F y11 represents the lateral force of the front left wheel; F y12 represents the lateral force of the front right wheel; F y21 represents the lateral force of the rear left wheel; F y22 represents the lateral force of the rear right wheel; δ f represents the front wheel steering angle; I z represents the moment of inertia about the Z-axis; I x represents the moment of inertia about the X-axis; a represents the front half wheelbase; b represents the rear half wheelbase; m s represents the body mass; represents the body roll angle; h g represents the center of mass height; represents the lateral damping; represents the roll stiffness; represents the first derivative; represents the second derivative;

[0019] The expression of the tire dynamics model is:

[0020]

[0021] where F x represents the tire longitudinal force; F y represents the tire lateral force; C x represents the tire longitudinal stiffness; C y represents the tire cornering stiffness; K represents the longitudinal slip ratio; α represents the side slip angle; F represents the tire longitudinal and lateral combined force, and its expression is:

[0022]

[0023] where μ represents the road surface adhesion coefficient; F z represents the vertical load of the tire, which includes the vertical load F z11 of the front left wheel, the vertical load F z12 of the front right wheel, the vertical load F z21 of the rear left wheel, and the vertical load F z22 of the rear right wheel, that is, the expressions of the vertical loads of each tire are:

[0024]

[0025] wherein, l represents the wheelbase; B f represents the front track; B r represents the rear track; M roll represents the overturning moment rotating about the X-axis.

[0026] Further, by adopting the quasi-steady state equilibrium method, the steady state equilibrium states of the vehicle under different throttle openings and braking pressures are traversed, the lateral acceleration and longitudinal acceleration at each moment are recorded, and the process of obtaining the improved G-G diagram by vector calculation specifically includes the following steps:

[0027] Set the vehicle to drive straight;

[0028] By adopting the quasi-steady state equilibrium method, different throttle pedal openings are traversed, the vehicle speed at each moment is recorded, and the maximum speed is solved;

[0029] The expression of the maximum speed is:

[0030] f v =-min(X v );

[0031] X v =[v x ,K e ; T ;

[0032] wherein, f v represents the objective function of the maximum speed; X v represents the optimization variable of the maximum speed; K e represents the throttle pedal opening; * T represents the transpose;

[0033] At the current speed, by adopting the quasi-steady state equilibrium method, different braking pressures are traversed, the longitudinal acceleration at each moment is recorded, and the maximum longitudinal acceleration is solved;

[0034] The expression of the maximum longitudinal acceleration at the current speed is:

[0035]

[0036] wherein, represents the objective function of the maximum longitudinal acceleration; represents the optimization variable of the maximum longitudinal acceleration; T b represents the braking torque of the brake pedal;

[0037] At the current speed and longitudinal acceleration, by adopting the quasi-steady state equilibrium method, different throttle pedal openings and braking pressures are traversed, the lateral acceleration at each moment is recorded, and the maximum lateral acceleration is solved;

[0038] The expression for the maximum lateral acceleration at the current speed and longitudinal acceleration is as follows:

[0039]

[0040] In the formula, represents the objective function of the maximum longitudinal acceleration; represents the optimization variable of the maximum longitudinal acceleration;

[0041] The maximum longitudinal acceleration and the maximum lateral acceleration are subjected to vector calculation, and a G-G diagram is generated using mapping software to limit the maximum lateral acceleration and the maximum longitudinal acceleration of the vehicle.

[0042] Furthermore, based on the obtained improved G-G diagram, the process of establishing the optimal speed model using the forward-backward Euler method specifically includes the following steps:

[0043] According to the constraint of the tire adhesion limit, calculate the maximum passing speed of the vehicle passing through a curve under different curvatures; the maximum passing speed includes the maximum longitudinal speed and the maximum lateral speed;

[0044] The relational expression for the maximum passing speed is:

[0045]

[0046] In the formula, v xmax represents the maximum longitudinal speed; represents the maximum lateral speed; ρ represents the curvature;

[0047] The functional expression of the longitudinal speed and the path is:

[0048]

[0049] In the formula, v x represents the longitudinal speed; ds represents the differential element of the path; represents the longitudinal acceleration, represents the maximum longitudinal acceleration at the current vehicle speed;

[0050] Based on the differential idea, combined with the G-G diagram and the constraint of the maximum longitudinal acceleration, construct the functional expression of the longitudinal speed and the path;

[0051] Taking the maximum acceleration change rate of the power system as the iteration gain, connect the speed and acceleration coupling points of the bidirectional iteration to establish the optimal speed model;

[0052] The expression of the optimal speed model is:

[0053]

[0054] In the formula, Represents the current feedforward longitudinal acceleration, v x_forward Represents the feedforward longitudinal velocity, Represents the next feedforward longitudinal velocity; Represents the previous feedforward longitudinal acceleration; ΔS represents the differential unit of the path length; jerk represents the acceleration change rate.

[0055] Furthermore, the process of establishing the trajectory optimization model under the shortest travel time by using the road segmentation method specifically includes:

[0056] Using the road segmentation method, divide the planned path into multiple sampling points;

[0057] The expression of the sampling point is:

[0058]

[0059] In the formula, Represents the sampling point on the planned path, j = 1, 2, …, n; h j Represents the abscissa of the jth sampling point; r j Represents the ordinate of the jth sampling point; Represents the abscissa vector unit; Represents the ordinate vector unit; h u,j Represents the abscissa of the jth sampling point on the upper boundary u; r u,j Represents the ordinate of the jth sampling point on the upper boundary u; h d,j Represents the abscissa of the jth sampling point on the lower boundary d; r d,j Represents the ordinate of the jth sampling point on the lower boundary d; Δh j Represents the path width in the abscissa direction of the current path point; Δr j Represents the path width in the ordinate direction of the current path point; α j Represents the road constraint coefficient, α j ∈(0, 1);

[0060] Use a fifth-degree polynomial curve to smoothly connect multiple sampling points to obtain the planned path;

[0061] Combine the optimal speed model to construct the trajectory optimization model under the shortest travel time;

[0062] The expression of the trajectory optimization model under the shortest travel time is:

[0063]

[0064] In the formula, min(·) represents the minimum value, T srepresents the travel time; Δs represents the distance between adjacent path sampling points; v x (j) represents the longitudinal speed of the j-th sampling point.

[0065] Furthermore, the expression of the optimal path and speed collaborative planning model is:

[0066]

[0067] s.t X(t) ∈ X.;

[0068] U(t) ∈ U;

[0069] g[X(t), U(t)] ≤ 0;

[0070] q[X(t), U(t)] = 0;

[0071] In the formula, S ∑ represents the total path length; ds represents the differential unit of the path length; dt represents the differential unit of time; X represents the state quantity; U represents the control quantity; g[X(t), U(t)] represents the soft constraint related to the state quantity at time t and the control quantity at time t; q[X(t), U(t)] represents the hard constraint related to the state quantity at time t and the control quantity at time t.

[0072] An apparatus for an autonomous vehicle path and speed collaborative planning method, comprising:

[0073] A constraint condition determination module, which is used to determine the motion limit boundary of the vehicle, the maximum lateral acceleration and the maximum longitudinal acceleration, the road safety boundary and the vehicle dynamics constraint;

[0074] A model construction module, which is used to construct a vehicle dynamics model, a tire dynamics model, an optimal speed model, a trajectory optimization model, and an optimal path and speed collaborative planning model.

[0075] An electronic device, the electronic device includes a memory and a processor, the memory stores a computer program, and the processor implements the method as described above when executing the computer program.

[0076] A computer-readable storage medium, the computer-readable storage medium stores a computer program, and the computer program implements the method as described above when executed by a processor.

[0077] The beneficial effects of the present invention are:

[0078] The present invention accurately evaluates the dynamic limit performance of a vehicle, constructs an improved G-G diagram to obtain the maximum lateral acceleration and the maximum longitudinal acceleration for constraint, then calculates an optimal speed model, constructs a trajectory optimization model under the shortest travel time based on the optimal speed model, couples the two to construct an optimal path and speed collaborative planning model to obtain the optimal path and speed planning, so as to achieve the purpose of reducing the vehicle passing time, improving the passing efficiency and safety, and further realizing high-precision tracking control, and enhancing the driving efficiency and safety of the autonomous vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 is a schematic flow chart of the present invention;

[0080] Figure 2 is a schematic diagram of the forward-backward Euler method;

[0081] Figure 3 is a schematic model diagram of the road segmentation method;

[0082] Figure 4 is a schematic diagram of the influence of the acceleration change rate on the optimal speed model;

[0083] Figure 5 is a schematic diagram of the comparison of the trajectory optimization models under different methods;

[0084] Figure 6 is a schematic diagram of the comparison of the path curvatures under different methods;

[0085] Figure 7 is a schematic diagram of the comparison of the planned vehicle speed results under different methods;

[0086] Figure 8 is a schematic diagram of the comparison of the speed control results under different methods;

[0087] Figure 9 is a schematic diagram of the comparison of the trajectory error results under different methods;

[0088] Figure 10 is a schematic structural diagram of the device in the present invention;

[0089] Figure 11 is a schematic structural diagram of the computer device in the present invention.

[0090] REFERENCE SIGNS:

[0091] 1. Memory; 2. Processor. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0092] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative work belong to the scope of protection of the present invention.

[0093] It should be noted that the following describes various aspects of embodiments within the scope of the appended claims. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is illustrative only. Based on this disclosure, those skilled in the art should understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement a device and / or practice a method. Additionally, this device and / or this method can be implemented using other structures and / or functionality in addition to one or more of the aspects described herein.

[0094] In addition, in the following description, specific details are provided to facilitate a thorough understanding of the examples. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0095] Embodiment 1

[0096] Figure 1 Shown is a method for collaborative path and speed planning of an autonomous vehicle. By accurately evaluating the dynamic limit performance of the vehicle and constructing an improved G - G diagram to obtain constraints on the maximum lateral acceleration and maximum longitudinal acceleration, and then calculating an optimal speed model, and based on the optimal speed model, constructing a trajectory optimization model under the shortest travel time. The two are coupled to construct an optimal path and speed collaborative planning model to obtain the optimal path and speed planning, so as to achieve the purpose of reducing the vehicle passing time, improving the passing efficiency and safety, and further realizing high - precision tracking control, thus enhancing the driving efficiency and safety of the autonomous vehicle. The specific steps are as follows:

[0097] S1: Construct a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire, and use it as the motion limit boundary of the vehicle;

[0098] S11: Construct a vehicle dynamics model and a tire dynamics model;

[0099] S12: Consider the transfer of the vehicle's load in the lateral and forward directions, and calculate the vertical force characteristics of the four tires;

[0100] S13: Calculate the ultimate adhesion force using the adhesion coefficient between the tire and the road surface to obtain the dynamic force limit of the tire.

[0101] Take it as the motion limit boundary of the vehicle.

[0102] Among them, the expression of the vehicle dynamics model is:

[0103]

[0104] In the formula, m represents the total vehicle mass; v x represents the longitudinal speed; represents the longitudinal acceleration; v y represents the lateral speed; represents the lateral acceleration; γ represents the yaw angular velocity; F x21 represents the longitudinal force of the rear left wheel; F x22 represents the longitudinal force of the rear right wheel; F y11 represents the lateral force of the front left wheel; F y12 represents the lateral force of the front right wheel; F y21 represents the lateral force of the rear left wheel; F y22 represents the lateral force of the rear right wheel; δ f represents the front wheel steering angle; I z represents the moment of inertia about the Z axis; I x represents the moment of inertia about the X axis; a represents the front half wheelbase; b represents the rear half wheelbase; m s represents the body mass; represents the body roll angle; h g represents the center of mass height; represents the lateral damping; represents the roll stiffness; represents the first derivative; represents the second derivative;

[0105] Among them, the expression of the tire dynamics model is:

[0106]

[0107] In the formula, F x represents the tire longitudinal force; F y represents the tire lateral force; C x represents the tire longitudinal stiffness; C y represents the tire cornering stiffness; K represents the longitudinal slip ratio; α represents the slip angle; F represents the combined longitudinal and lateral force of the tire, and its expression is:

[0108]

[0109] In the formula, μ represents the road surface adhesion coefficient; F zRepresents the vertical load of the tire, which includes the vertical load F of the front left wheel z11 , the vertical load F of the front right wheel z12 , the vertical load F of the rear left wheel z21 , the vertical load F of the rear right wheel z22 , that is, the expression of the vertical load of each tire is:

[0110]

[0111] In the formula, l represents the wheelbase; B f represents the front track; B r represents the rear track; M roll represents the overturning moment rotating around the X axis.

[0112] S2: Adopt the quasi-steady state equilibrium method, traverse the steady state equilibrium states of the vehicle under different throttle openings and braking pressures, record the lateral acceleration and longitudinal acceleration at each moment, and calculate the improved G-G diagram vectorially;

[0113] S21: Set the vehicle to drive straight;

[0114] S22: Adopt the quasi-steady state equilibrium method, traverse different throttle pedal openings, record the vehicle speed at each moment, and solve for the maximum speed;

[0115] Among them, the expression of the maximum speed is:

[0116] f v =-min(X v );

[0117] X v =[v x ,K e T ;

[0118] In the formula, f v represents the objective function of the maximum speed; X v represents the optimization variable of the maximum speed; K e represents the throttle pedal

[0119] opening; * T represents the transpose;

[0120] S23: At the current speed, adopt the quasi-steady state equilibrium method, traverse different braking pressures, record the longitudinal acceleration at each moment, and solve for the maximum longitudinal acceleration;

[0121] Among them, the expression of the maximum longitudinal acceleration at the current speed is:

[0122]

[0123] In the formula,​ The objective function representing the maximum longitudinal acceleration; The optimization variable representing the maximum longitudinal acceleration;

[0124] T b Represents the braking torque of the brake pedal;

[0125] S24: At the current speed and longitudinal acceleration, adopt the quasi-steady state equilibrium method, traverse different throttle pedal openings and braking pressures, record the lateral acceleration at each moment, and solve for the maximum lateral acceleration;

[0126] Among them, the expression for the maximum lateral acceleration at the current speed and longitudinal acceleration is:

[0127]

[0128] In the formula, The objective function representing the maximum longitudinal acceleration; The optimization variable representing the maximum longitudinal acceleration;

[0129] S25: Perform vector calculation on the maximum longitudinal acceleration and the maximum lateral acceleration, and use mapping software to generate a G-G diagram for restricting the maximum lateral acceleration and the maximum longitudinal acceleration of the vehicle.

[0130] S3: Based on the obtained improved G-G diagram, adopt the forward-backward Euler method to establish an optimal speed model;

[0131] S31: According to the constraint of the tire adhesion limit, calculate the maximum passing speed of the vehicle passing through a curve under different curvatures; the maximum passing speed includes the maximum longitudinal speed and the maximum lateral speed;

[0132] Among them, the relationship formula for the maximum passing speed is:

[0133]

[0134] In the formula, v xmax Represents the maximum longitudinal speed; Represents the maximum lateral speed; ρ represents the curvature;

[0135] The functional expression of the longitudinal speed and the path is:

[0136]

[0137] In the formula, v x Represents the longitudinal speed; ds represents the differential element of the path; Represents the longitudinal acceleration, Represents the maximum longitudinal acceleration at the current vehicle speed;

[0138] S32: Based on the differential idea, combined with the G - G diagram and the constraint of the maximum longitudinal acceleration, construct the functional expression of the longitudinal velocity and the path;

[0139] S33: Adopt the forward - backward Euler method, use the maximum acceleration change rate of the dynamic system as the iteration gain, connect the speed and acceleration coupling points of the bidirectional iteration, and establish the optimal speed model;

[0140] Figure 2 The figure shows the schematic diagram of the forward - backward Euler method; O is the driving / regulating demarcation point, and the j - point represents the static limit speed extreme value of the j - th sampling point, and j + 1 represents the static limit speed extreme value of the (j + 1) - th sampling point.

[0141] Among them, the expression of the optimal speed model is:

[0142]

[0143] In the formula, represents the current feed - forward longitudinal acceleration, v x_forward represents the feed - forward longitudinal velocity, represents the next feed - forward longitudinal velocity; represents the previous feed - forward longitudinal acceleration; ΔS represents the differential unit of the path length; jerk represents the acceleration change rate.

[0144] S4: Adopt the road segmentation method to establish the trajectory optimization model under the shortest travel time;

[0145] S41: Adopt the road segmentation method to divide the planned path into multiple sampling points;

[0146] Among them, the expression of the sampling point is:

[0147]

[0148] In the formula, represents the sampling point on the planned path, j = 1, 2, …, n; h j represents the abscissa of the j - th sampling point; r j

[0149] represents the ordinate of the j - th sampling point; represents the abscissa vector unit; represents the ordinate vector unit; h u,j represents the abscissa of the j - th sampling point on the upper boundary u; r u,j represents the ordinate of the j - th sampling point on the upper boundary u; h d,j represents the abscissa of the j - th sampling point on the lower boundary d; r d,jrepresents the ordinate of the j-th sampling point on the lower boundary d; Δh j represents the path width in the horizontal coordinate direction of the current path point; Δr j represents the path width in the vertical coordinate direction of the current path point; α j represents the road constraint coefficient, α j ∈(0, 1);

[0150] Figure 3 Schematic diagram of the road segmentation method shown.

[0151] S42: Smoothly connect multiple sampling points using a fifth-degree polynomial curve to obtain the planned path;

[0152] S43: Combine the optimal speed model under the selected sampling points to construct a trajectory optimization model for the shortest travel time;

[0153] Among them, the expression of the trajectory optimization model for the shortest travel time is:

[0154]

[0155] In the formula, min(·) represents the minimum value, T s represents the travel time; Δs represents the distance between adjacent path sampling points; v x (j) represents the longitudinal speed of the j-th sampling point.

[0156] S5: Based on the road safety boundary and vehicle dynamics constraints, combine the optimal speed model and the trajectory optimization model, and use quadratic programming to construct an optimal path and speed collaborative planning model, and solve to obtain the optimal path and speed planning.

[0157] The expression of the optimal path and speed collaborative planning model is:

[0158]

[0159] s.t X(t) ∈ X.;

[0160] U(t) ∈ U;

[0161] g[X(t), U(t)] ≤ 0;

[0162] q[X(t), U(t)] = 0;

[0163] In the formula, S ∑ represents the total path length; ds represents the path length differential unit; dt represents the time differential unit; X represents the state quantity; U represents the control quantity; g[X(t), U(t)] represents the soft constraint related to the state quantity and the control quantity; q[X(t), U(t)] represents the hard constraint related to the state quantity and the control quantity.

[0164] Example 2

[0165] In this embodiment, a method for collaborative planning of an autonomous vehicle's path and speed is provided.

[0166] In this embodiment, the vehicle mass m = 1270 kg; the wheelbase l = 2.91 m; the front-to-rear wheelbase ratio a:b = 1:1; the engine power is 150 kw; the front-to-rear braking force ratio is 5:3; and the road surface adhesion coefficient μ = 0.8.

[0167] In this embodiment, the methods of the central path, the shortest path, the optimal curvature path, the optimal path, and the collaborative planning path are used to compare and evaluate each step of the method described in this application, that is:

[0168] (1) Under the standard double lane change condition, a comparative experiment is carried out using the combined simulation platform of Carsim and Simulink to evaluate the influence degree of the acceleration change rate. As Figure 4 shown, the results show that: using the optimal speed model described in this application, that is, the maximum speed control error obtained considering the acceleration change rate is 0.18 m / s, and the root mean square error is 0.08 m / s; when not considering the acceleration change rate, the speed directly switches from rapid acceleration to rapid deceleration, the acceleration change rate is too large and even there is an acceleration mutation, and the maximum speed control error obtained is 0.93 m / s, and the root mean square error is 0.39 m / s.

[0169] (2) Under the standard double lane change condition, comparative experiments are carried out using different methods. As Figure 5 shown, the results clearly show the differences between the collaborative planning path and the paths planned by other methods.

[0170] (3) Under the standard double lane change condition, comparative experiments are carried out based on the path curvature using different methods. As Figure 6 shown, the results show that: using the method described in this application, the curvature change is smooth, and the maximum curvature is the smallest compared with other paths, which is beneficial to the realization of higher speeds; while in other methods, the curvature change of the central path is large, the maximum curvature and the average curvature are both the largest, and the average curvature of the curvature optimal path is the smallest, and there are individual large curvature sections.

[0171] (4) Compare the planned vehicle speeds under different methods. As Figure 7 shown, the results show that: the average expected speed of the central path is 14.00 m / s, the average expected speed of the shortest path is 28.84 m / s, the average expected speed of the optimal curvature is 31.91 m / s, the average expected speed of the optimal path is 32.25 m / s, and the average expected speed of the optimal path and speed collaborative planning method is 33.3 m / s. That is, using the method described in this application has a higher average vehicle speed and higher driving efficiency.

[0172] (5) Compare the speed control results under different methods, such as Figure 8 As shown, the results show that: the maximum control error of the central path is 0.31 m / s, the root mean square error is 0.13 m / s, and the travel time is 8.58 s; the maximum control error of the shortest path is 0.27 m / s, the root mean square error is 0.10 m / s, and the travel time is 4.16 s; the maximum control error of the optimal curvature is 0.26 m / s, the root mean square error is 0.10 m / s, and the travel time is 3.76 s; the maximum control error of the optimal path is 0.34 m / s, the root mean square error is 0.13 m / s, and the travel time is 3.72 s; the maximum control error of the collaborative planning method of the optimal path and speed is 0.01 m / s, the root mean square error is 0.01 m / s, and the travel time is 3.60 s. That is, the method described in this application can also maintain a high speed control accuracy.

[0173] (6) Using the method described in this application, such as Figure 9 As shown, the results show that: the maximum trajectory tracking error of the central path is 0.08 m, the root mean square error is 0.02 m; the maximum trajectory tracking error of the shortest path is 0.05 m, the root mean square error is 0.02 m; the maximum trajectory tracking error of the optimal curvature is 0.06 m, the root mean square error is 0.02 m; the maximum trajectory tracking error of the optimal path is 0.09 m, the root mean square error is 0.04 m; the maximum trajectory tracking error of the collaborative planning method of the optimal path and speed is 0.04 m, the root mean square error is 0.01 m. That is, the method described in this application can also maintain a high trajectory control accuracy.

[0174] Example 2

[0175] As Figure 10 shown, the present invention also provides an apparatus for collaborative path and speed planning of an autonomous vehicle, including a constraint condition determination module and a model construction module.

[0176] Among them, the constraint condition determination module is used to determine the motion limit boundary of the vehicle, the maximum lateral acceleration and the maximum longitudinal acceleration, the road safety boundary, and the vehicle dynamics constraints;

[0177] Among them, the model construction module is used to construct a vehicle dynamics model, a tire dynamics model, an optimal speed model, a trajectory optimization model, and a collaborative path and speed planning model of the optimal path.

[0178] Example 3

[0179] Based on the same inventive concept, an embodiment of the present application also provides a computer device, including a memory 1 and a processor 2, as Figure 11As shown, the memory 1 stores a computer program, and when the processor 2 executes the computer program, the method described in any one of the above is implemented.

[0180] Among them, the memory 1 includes at least one type of readable storage medium, and the readable storage medium includes flash memory, hard disk, multimedia card, card-type memory (such as SD or DX memory, etc.), magnetic memory, magnetic disk, optical disc, etc. In some embodiments, the memory 1 can be an internal storage unit of the autonomous vehicle path and speed collaborative planning device, such as a hard disk. In other embodiments, the memory 1 can also be an external storage device of the autonomous vehicle path and speed collaborative planning device, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, Flash Card, etc. Further, the memory 1 can also include both the internal storage unit and the external storage device of the autonomous vehicle path and speed collaborative planning device. The memory 1 can not only be used to store application software installed in the autonomous vehicle path and speed collaborative planning device and various types of data, such as the code of the autonomous vehicle path and speed collaborative planning device program, etc., but also be used to temporarily store data that has been output or will be output.

[0181] In some embodiments, the processor 2 can be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other data processing chips, and is used to run the program code stored in the memory 1 or process data, such as executing the autonomous vehicle path and speed collaborative planning device program, etc.

[0182] The disclosed embodiments of the present invention also provide a computer-readable storage medium, on which a computer program is stored, and when the computer program is run by a processor, the steps of the method described in the above method embodiments are executed. Among them, the storage medium can be a volatile or non-volatile computer-readable storage medium.

[0183] The computer program product of the application page content refreshing method provided by the disclosed embodiments of the present invention includes a computer-readable storage medium storing program code, and the instructions included in the program code can be used to execute the steps of the method described in the above method embodiments. For details, refer to the above method embodiments and will not be elaborated here.

[0184] The disclosed embodiments of the present invention also provide a computer program, which, when executed by a processor, implements any one of the methods in the foregoing embodiments. The computer program product can be specifically implemented in the form of hardware, software, or a combination thereof. In an alternative embodiment, the computer program product is specifically embodied as a computer storage medium. In another alternative embodiment, the computer program product is specifically embodied as a software product, such as a Software Development Kit (SDK), and so on.

[0185] It can be understood that the same or similar parts in the above embodiments can be referred to each other, and the content not detailed in some embodiments can be seen in the same or similar content in other embodiments.

[0186] It should be noted that in the description of the present invention, the terms "first", "second", etc. are only used for descriptive purposes and cannot be construed as indicating or implying relative importance. In addition, in the description of the present invention, unless otherwise specified, the meaning of "a plurality of" refers to at least two.

[0187] Any process or method description in a flowchart or described in any other way herein can be understood to represent a module, segment, or portion of code including one or more executable instructions for implementing a specific logical function or process. The scope of the preferred embodiments of the present invention includes additional implementations, where the functions can be executed in a substantially simultaneous manner or in a reverse order according to the functions involved, rather than in the order shown or discussed. This should be understood by those skilled in the technical field of the embodiments of the present invention.

[0188] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one or a combination of the following well-known technologies in the art can be used: discrete logic circuits with logic gate circuits for implementing logical functions on data signals, application-specific integrated circuits with appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0189] Those of ordinary skill in the technical field of the present invention can understand that all or part of the steps carried by the methods in the above embodiments can be completed by instructing relevant hardware through a program. The program can be stored in a computer-readable storage medium, and when the program is executed, it includes one or a combination of the steps of the method embodiments.

[0190] In addition, each functional unit in various embodiments of the present invention may be integrated into a processing module, or each unit may exist physically alone, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. When the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0191] The above-mentioned storage medium may be a read-only memory, a magnetic disk, an optical disc, etc.

[0192] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0193] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for collaborative planning of the path and speed of an autonomous vehicle, characterized in that, It includes the following steps: Construct a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire, and use it as the motion limit boundary of the vehicle; Adopt the quasi-steady state equilibrium method to traverse the steady state equilibrium states of the vehicle under different throttle openings and braking pressures, record the lateral acceleration and longitudinal acceleration at each moment, and calculate the improved G-G diagram through vector calculation; Based on the obtained improved G-G diagram, adopt the forward-backward Euler method to establish an optimal speed model; Adopt the road segmentation method to establish a trajectory optimization model under the shortest travel time; Based on the road safety boundary and vehicle dynamics constraints, combine the optimal speed model and the trajectory optimization model, and use quadratic programming to construct an optimal path and speed collaborative planning model, and solve to obtain the optimal path and speed planning; Based on the obtained improved G-G diagram, adopt the forward-backward Euler method to establish an optimal speed model, including: According to the constraint of the tire adhesion limit, calculate the maximum passing speed of the vehicle passing through a curve under different curvatures; the maximum passing speed includes the maximum longitudinal speed and the maximum lateral speed; Based on the differential idea, combine the G-G diagram and the constraint of the maximum longitudinal acceleration to construct a function expression of the longitudinal speed and the path; Adopt the forward-backward Euler method, use the maximum acceleration change rate of the dynamic system as the iteration gain, connect the speed and acceleration coupling points of the two-way iteration, and establish an optimal speed model; The expression of the optimal path and speed collaborative planning model is: ； ； ； ； ； In the formula, represents the total degree of the path; represents the differential unit of the path length; represents the differential unit of time; represents the state quantity; represents the control quantity; represents the soft constraint related to the state quantity at time and the control quantity at time represents the hard constraint related to the state quantity at time and the control quantity at time 2. The method for collaborative path and speed planning of an autonomous vehicle according to claim 1, characterized in that, The process of constructing a vehicle dynamics model and a tire dynamics model to obtain the dynamic force limit of the tire and using it as the motion limit boundary of the vehicle specifically includes the following steps: Construct a vehicle dynamics model and a tire dynamics model; Consider the transfer of the vehicle's load in the lateral and forward directions, and calculate the vertical force characteristics of the four tires; Use the adhesion coefficient between the tire and the road surface to calculate the limit adhesion force, obtain the dynamic force limit of the tire, and use it as the motion limit boundary of the vehicle.

3. According to the method for collaborative planning of the path and speed of an autonomous vehicle described in claim 2, characterized in that The expression of the vehicle dynamics model is: ； In the formula, represents the vehicle mass; represents the longitudinal speed; represents the longitudinal acceleration; represents the lateral speed; represents the lateral acceleration; represents the yaw angular velocity; represents the longitudinal force of the rear left wheel; represents the longitudinal force of the rear right wheel; represents the lateral force of the front left wheel; represents the lateral force of the front right wheel; represents the lateral force of the rear left wheel; represents the lateral force of the rear right wheel; represents the front wheel steering angle; represents the moment of inertia about the Z-axis; represents the moment of inertia about the X-axis; represents the front half wheelbase; represents the rear half wheelbase; represents the body mass; represents the body roll angle; represents the center of mass height; represents the lateral damping; represents the roll stiffness; represents the first derivative; represents the second derivative; The expression of the tire dynamics model is: ； In the formula, represents the longitudinal force of the tire; represents the lateral force of the tire; represents the longitudinal stiffness of the tire; represents the cornering stiffness of the tire; represents the longitudinal slip ratio; represents the sideslip angle; ; represents the combined longitudinal and lateral force of the tire, and its expression is: ； Wherein, represents the road surface adhesion coefficient; represents the vertical load of the tire, which includes the vertical load of the front left wheel , the vertical load of the front right wheel , the vertical load of the rear left wheel , and the vertical load of the rear right wheel , that is, the expression of the vertical load of each tire is: ； In the formula, represents the wheelbase; represents the front track; represents the rear track; represents the tipping moment about the X-axis.

4. A method for collaborative planning of an autonomous vehicle's path and speed according to claim 3, characterized in that The process of adopting the quasi-steady state equilibrium method to traverse the steady state equilibrium states of the vehicle under different throttle openings and braking pressures, record the lateral acceleration and longitudinal acceleration at each moment, and calculate the improved G-G diagram through vector calculation specifically includes the following steps: Set the vehicle to drive straight; Adopt the quasi-steady state equilibrium method to traverse different throttle pedal openings, record the vehicle speed at each moment, and solve for the maximum speed; The expression of the maximum speed is: ； ； In the formula, The objective function representing the maximum speed; The optimization variable representing the maximum speed; Indicates the throttle pedal opening; Indicates transpose; At the current speed, adopt the quasi-steady state equilibrium method to traverse different braking pressures, record the longitudinal acceleration at each moment, and solve for the maximum longitudinal acceleration; The expression of the maximum longitudinal acceleration at the current speed is: ； ； In the formula, represents the objective function of the maximum longitudinal acceleration; represents the optimization variable of the maximum longitudinal acceleration; represents the braking torque of the brake pedal; At the current speed and longitudinal acceleration, adopt the quasi-steady state equilibrium method to traverse different throttle pedal openings and braking pressures, record the lateral acceleration at each moment, and solve for the maximum lateral acceleration; The expression of the maximum lateral acceleration at the current speed and longitudinal acceleration is: ； ； In the formula, The objective function representing the maximum longitudinal acceleration; The optimization variable representing the maximum longitudinal acceleration; Perform a vector calculation on the maximum longitudinal acceleration and the maximum lateral acceleration, and use mapping software to generate a G-G diagram for restricting the maximum lateral acceleration and the maximum longitudinal acceleration of the vehicle.

5. A method for collaborative path and speed planning of an autonomous vehicle according to claim 1, wherein The relational expression for the maximum passing speed is: ； In the formula, represents the maximum longitudinal speed; represents the maximum lateral speed; represents the curvature; The functional expression of the longitudinal speed and the path is: ； In the formula, represents the longitudinal speed; represents the differential unit of the path; represents the longitudinal acceleration, , represents the maximum longitudinal acceleration at the current vehicle speed; The expression of the optimal speed model is: ； In the formula, represents the current feedforward longitudinal acceleration, , represents the feedforward longitudinal velocity, , , represents the next feedforward longitudinal velocity; represents the previous feedforward longitudinal acceleration; represents the differential element of the path length; represents the acceleration change rate.

6. A method for collaborative planning of an autonomous vehicle's path and speed according to claim 1, characterized in that, The process of establishing a trajectory optimization model with the shortest travel time by using the road segmentation method specifically includes: Using the road segmentation method, divide the planned path into multiple sampling points; The expression of the sampling point is: ； In the formula, represents the sampling point on the planned path, ; represents the th abscissa of the sampling point; represents the th ordinate of the sampling point; represents the abscissa vector unit; represents the ordinate vector unit; represents the upper boundary at the th abscissa of the sampling point; represents the upper boundary at the th ordinate of the sampling point; represents the lower boundary at the th abscissa of the sampling point; represents the lower boundary at the th ordinate of the sampling point; represents the path width in the abscissa direction of the current path point; represents the path width in the ordinate direction of the current path point; represents the road constraint coefficient, ; Use a fifth-degree polynomial curve to smoothly connect multiple sampling points to obtain the planned path; Combine the optimal speed model to construct a trajectory optimization model with the shortest travel time; The expression of the trajectory optimization model with the shortest travel time is: ； In the formula, represents the minimum value, represents the travel time; represents the distance between adjacent path sampling points; represents the longitudinal velocity of the 7. An apparatus for implementing the method for collaborative path and speed planning of an autonomous vehicle according to claim 1, characterized in that, Including: A constraint condition determination module, which is used to determine the motion limit boundary of the vehicle, the maximum lateral acceleration and the maximum longitudinal acceleration, the road safety boundary, and the vehicle dynamics constraints; A model construction module, which is used to construct a vehicle dynamics model, a tire dynamics model, an optimal speed model, a trajectory optimization model, and an optimal path and speed collaborative planning model.

8. An electronic device, characterized in that, The electronic device includes a memory and a processor, the memory stores a computer program, and when the processor executes the computer program, the method described in any one of claims 1 to 6 is implemented.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, the method described in any one of claims 1 to 6 is implemented.

Images