Intelligent car humanization speed and trajectory planning method and device

Abstract

The present application relates to a kind of mountainous highway intelligent car anthropomorphism speed and trajectory planning method and device, wherein the method comprises the following steps: obtaining vehicle operating data, road geometry data and environmental data;Data preprocessing;With road geometry data and environmental data as explanatory variable, based on simultaneous equations model, establish theoretical vehicle operating speed and trajectory model;Based on the result of vehicle operating speed and trajectory model, establish anthropomorphism motion planning basic rule;Anthropomorphism motion planning model is constructed;Anthropomorphism motion planning model is corrected, and the behavior that due to driver manually drives vehicle and leads to deviation out of lane is avoided;The output of corrected anthropomorphism motion planning model is used as a kind of soft constraint, for intelligent car motion control module reference, realize anthropomorphism speed and trajectory planning.Compared with prior art, the complexity of driver driving behavior in mountainous highway is considered, more in line with the motion expectation of intelligent car driver.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle motion planning, and in particular to an anthropomorphic speed and trajectory planning method and device for intelligent vehicles on mountain highways. Background Technology

[0002] Free-flow traffic environments on highways, where there are typically no other vehicles affecting the driving process, are an important application scenario for intelligent vehicles. In such scenarios, road alignment and environment are the main factors influencing the driver's speed and trajectory behavior. Especially on mountainous highways with numerous combinations of alignments, a speed and trajectory planning method that balances comfort and safety is needed, namely motion planning, for the motion control module to reference.

[0003] Existing intelligent vehicle motion planning methods have not fully addressed the challenges of complex road sections on mountainous highways. Current methods consider driver comfort requirements by incorporating comfort constraints, such as lateral acceleration, longitudinal acceleration, jerk, and steering wheel angle, into the cost function of the planning problem. However, these methods lack an understanding of human driver speed and trajectory behavior habits from a traffic engineering perspective. Summary of the Invention

[0004] The purpose of this invention is to provide an intelligent vehicle anthropomorphic speed and trajectory planning method and device for mountainous highways. By collecting driving data, road geometry data, and environmental data from drivers manually operating the vehicle in a free-flow traffic environment, a simultaneous equation model is established with road geometry and environmental variables as exogenous independent variables and speed and trajectory variables as endogenous independent variables. The vehicle speed and trajectory model results are used as the basis for the anthropomorphic motion planning method. Road geometry and environmental variables are used as inputs, and the speed and trajectory results predicted by the simultaneous equation model are subjected to safety correction. Finally, the speed and trajectory results are used as the output of the anthropomorphic planning method, resulting in intelligent vehicle speed and trajectory planning results that conform to driver behavior habits.

[0005] The objective of this invention can be achieved through the following technical solutions:

[0006] A human-like speed and trajectory planning method for intelligent vehicles on mountain highways includes the following steps:

[0007] Step 1) Obtain vehicle operation data, road geometry data, and environmental data for mountain highways;

[0008] Step 2) Data preprocessing;

[0009] Step 3) Using road geometry data and environmental data as explanatory variables, establish a theoretical vehicle speed and trajectory model based on a simultaneous equation model;

[0010] Step 4) Based on the results of the vehicle speed and trajectory model, establish basic rules for anthropomorphic motion planning;

[0011] Step 5) Construct an anthropomorphic motion planning model based on the results of the vehicle speed and trajectory model and the basic rules of anthropomorphic motion planning;

[0012] Step 6) Correct the anthropomorphic motion planning model to avoid lane departure caused by the driver manually driving the vehicle;

[0013] Step 7) Output the corrected anthropomorphic motion planning model as a soft constraint for the intelligent vehicle motion control module to realize anthropomorphic speed and trajectory planning of the intelligent vehicle.

[0014] The vehicle operation data includes speed and trajectory.

[0015] The road data includes road cross-sections, longitudinal sections, and planar geometric data.

[0016] The environmental data includes weather, lighting, road conditions, etc.

[0017] Step 2) includes the following steps:

[0018] Step 2-1) Remove vehicle operation data that is affected by factors other than road geometry and road environment;

[0019] Step 2-2) Define three types of vehicle deviation behavior: deviating to the outside of the curve, deviating to the inside of the curve, and staying within the lane. Determine the trajectory deviation safety threshold TD0 based on vehicle and road characteristics. If the vehicle trajectory deviation reaches TD0, it indicates that there is a safety hazard.

[0020] Steps 2-3) Match vehicle operation data, road geometry data, and environmental data to the same spatial coordinates. Based on the road centerline, collect the raw data at pre-configured distance intervals to obtain the vehicle's speed, trajectory offset, and the average values ​​of road geometry data and environmental data within each distance interval.

[0021] The vehicle speed and trajectory model is established based on different combinations of alignment types and vehicle offset behavior to calibrate the coefficients of significant variables in different application scenarios. The vehicle speed and trajectory model is expressed as follows:

[0022]

[0023]

[0024] Among them, SpeedCrnt j TD represents the predicted velocity at point j. j Indicates the predicted lateral trajectory offset, AvgSpeedUp j X represents the average speed of the upstream road segment, where upstream represents the segment just passed and downstream represents the segment about to be entered; these are endogenous variables. i,j The variables represent the road geometry and environmental variables at point j, including variables of the current point and upstream and downstream road segments, which are exogenous variables; m and n represent the number of significant variables; α and β represent model coefficients, and μ1 and μ2 represent model error terms.

[0025] The basic rules of the anthropomorphic motion planning include:

[0026] Intelligent vehicles use the output results of vehicle speed and trajectory models as a reference to determine the basic speed and trajectory values ​​for motion planning.

[0027] The trajectory is adjusted based on the vehicle's speed and the output of the trajectory model to ensure that the vehicle does not deviate from the lane, i.e., TD. j <TD0, and adjust the speed simultaneously based on the driver's behavior habits shown in the model results.

[0028] Step 5) includes the following steps:

[0029] Step 5-1) Determine the scene where the intelligent vehicle is located, including horizontal curve uphill, horizontal curve downhill, horizontal curve concave curve, and horizontal curve convex curve;

[0030] Step 5-2) Determine the predicted vehicle offset direction on the combined alignment in the scenario. Based on the data analysis in step 2), determine the offset direction of the intelligent vehicle with the highest probability under the combined alignment type.

[0031] Step 5-3) Based on the scene and the determined offset direction, construct an anthropomorphic motion planning model based on the vehicle's running speed and trajectory model. The input of the anthropomorphic motion planning model is the road geometric variables and environmental variables at point j, and the output of the model is the speed and lateral trajectory offset predicted by the model at point j.

[0032] Step 6) includes the following steps:

[0033] Step 6-1) Based on the anthropomorphic motion planning model, determine the significant variables of speed and their coefficients, and define two types: Type I: speed and trajectory are positively correlated; Type II: speed and trajectory are not correlated or are negatively correlated.

[0034] Step 6-2) If the anthropomorphic motion planning model of the scene is type I, and at the same time for TD... AV,j and vAV,j The correction is performed, and the correction objectives are as follows:

[0035] TD AV,j =TD correct,j <TD Human,j

[0036] TD correct ≤TD0

[0037] v AV,j =v correct,j <v Human,j

[0038] Among them, TD AV,j TD represents the lateral trajectory of the intelligent vehicle at point j. Human,j TD represents the trajectory of manual driving at point j. correct,j This indicates a correction trajectory that does not exceed the threshold; v AV,j v represents the speed at point j of the intelligent vehicle. Human,j v represents the speed at which the vehicle is manually driven at point j. correct,j Indicates the corrected speed reference value; the corrected TD AV,j Less than or equal to TD0; v correct,j Based on the vehicle's operating speed and trajectory model, β1 or β2, and TD correct,j With TD Human,j Determining the difference:

[0039] ΔTD j =TD Human,j -TD correct,j =β·ΔV j

[0040] ΔV j =V Human,j -V correct,j

[0041]

[0042] β is either β1 or β2 depending on the significance of the variables in the model;

[0043] Step 6-3) If the anthropomorphic motion planning model of the scene is Type II, only for TD AV,j Perform calibration, and the calibrated TD AV,j Less than or equal to TD0, v AV,j Keep with v Human,j The same applies, and no correction based on the model is required.

[0044] A humanoid speed and trajectory planning device for intelligent vehicles on mountain highways includes a memory, a processor, and a program stored in the memory. When the processor executes the program, it implements the method described above.

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

[0046] (1) By establishing a simultaneous equation model, this invention takes speed and trajectory as endogenous variables, which can take into account the internal influence of the two vehicle operation indicators, improve the accuracy of the driver speed and trajectory behavior model, and make the results more reliable and practical than independent modeling.

[0047] (2) This invention establishes vehicle speed and trajectory models in different scenarios of mountain highways as the basis for anthropomorphic motion planning methods. Compared with other motion planning methods that set comfort constraints, it learns human behavior of adjusting vehicle movement according to the upstream and downstream alignment and environment of the road. This can improve the motion planning strategy of intelligent vehicles, enabling intelligent vehicles to pass through complex combined alignment road sections smoothly. It can better meet the speed and trajectory expectations of drivers riding in intelligent vehicles under autonomous driving, and meet the safety and comfort expectations of riding. Attached Figure Description

[0048] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0049] Figure 2 This is a schematic diagram of the horizontal and vertical geometry of a sample road segment in one embodiment.

[0050] Figure 3 This is a schematic diagram of the anthropomorphic speed planning result in one embodiment. Detailed Implementation

[0051] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0052] This embodiment provides a human-like speed and trajectory planning method for intelligent vehicles on mountain highways, such as... Figure 1 As shown, it includes the following steps:

[0053] Step 1) The driver manually drives the intelligent vehicle on a mountain highway in a free-flow environment, using the intelligent vehicle's equipment to acquire vehicle operation data (including speed and trajectory), road geometry data (including road cross-section, longitudinal section, and planar geometry data), and environmental data (including weather, lighting, road surface conditions, etc.). The trajectory is the distance the vehicle's centerline deviates from the lane centerline.

[0054] Step 2) Data preprocessing.

[0055] Step 2-1) This invention focuses on the speed and trajectory expected by the driver in a free-flow environment. Changes in speed and trajectory due to other vehicles, lane changes when entering or leaving the main line, etc. are not considered. Therefore, data affected by factors other than road geometry and road environment are excluded from vehicle operation data.

[0056] Step 2-2) Define three types of vehicle deviation behavior: deviation to the outside of the curve (IDCF), deviation to the inside of the curve (ADCF), and keeping in the lane (In-lane). Determine the trajectory deviation safety threshold TD0 based on vehicle and road characteristics. If the vehicle trajectory deviation reaches TD0, it indicates a safety hazard, such as the vehicle edge crossing the lane line.

[0057] Steps 2-3) Match vehicle operation data, road geometry data, and environmental data to the same spatial coordinates. Based on the road centerline, collect the original data at 5m intervals to obtain the vehicle's speed, trajectory offset, and the average value of road geometry data and environmental data within each 5m interval.

[0058] Step 3) Using road geometry data and environmental data as explanatory variables, a theoretical vehicle speed and trajectory model is established based on a simultaneous equation model, as shown in formulas (1) and (2). Mountain highways have complex alignment conditions, making it difficult to establish a unified driver behavior model. It is necessary to establish models based on different combined alignment types and vehicle deviation behaviors to calibrate the coefficients of significant variables in different application scenarios.

[0059]

[0060]

[0061] Among them, SpeedCrnt j TD represents the predicted velocity at point j. j Indicates the predicted lateral trajectory offset, AvgSpeedUp j X represents the average speed of the upstream road segment, where upstream represents the segment just passed and downstream represents the segment about to be entered; these are endogenous variables. i,j The variables represent the road geometry and environmental variables at point j, including variables of the current point and upstream and downstream road segments, which are exogenous variables; m and n represent the number of significant variables; α and β represent model coefficients, and μ1 and μ2 represent model error terms.

[0062] Step 4) Based on the results of the vehicle speed and trajectory model, establish the basic rules for anthropomorphic motion planning:

[0063] Intelligent vehicles use the output results of vehicle speed and trajectory models as a reference to determine the basic speed and trajectory values ​​for motion planning.

[0064] The trajectory is adjusted based on the vehicle's speed and the output of the trajectory model to ensure that the vehicle does not deviate from the lane, i.e., TD. j <TD0, and adjust the speed simultaneously based on the driver's behavior habits shown in the model results.

[0065] Step 5) Construct an anthropomorphic motion planning model based on the results of the vehicle speed and trajectory model and the basic rules of anthropomorphic motion planning.

[0066] Step 5-1) Determine the scene where the intelligent vehicle is located. The scene includes horizontal curve uphill, horizontal curve downhill, horizontal curve concave curve, and horizontal curve convex curve.

[0067] Step 5-2) Determine the predicted vehicle offset direction on the combined alignment in the scene. Based on the data analysis in step 2), determine the offset direction of the intelligent vehicle with the highest probability under the combined alignment type.

[0068] Step 5-3) Based on the scene and the determined offset direction, construct an anthropomorphic motion planning model based on the vehicle's running speed and trajectory model. The input of the anthropomorphic motion planning model is the road geometric variables and environmental variables at point j, and the output of the model is the speed and lateral trajectory offset predicted by the model at point j.

[0069] Step 6) Since drivers manually drive the vehicle and may deviate from the lane, intelligent vehicles should avoid such behavior. Therefore, the anthropomorphic motion planning model is corrected.

[0070] Step 6-1) Based on the anthropomorphic motion planning model, determine the significant speed variables and their coefficients, and define two types: Type I: speed is positively correlated with trajectory; Type II: speed is not correlated with trajectory or is negatively correlated.

[0071] Step 6-2) If the anthropomorphic motion planning model of the scene is type I, and at the same time for TD... AV,j and v AV,j The correction is performed, and the correction targets are shown in equations (3)-(5):

[0072] TD AV,j =TD correct,j <TD Human,j (3)

[0073] TD correct ≤TD0 (4)

[0074] v AV,j =v correct,j <v Human,j (5)

[0075] Among them, TD AV,j TD represents the lateral trajectory of the intelligent vehicle at point j. Human,j TD represents the trajectory of manual driving at point j. correct,j This indicates a correction trajectory that does not exceed the threshold; v AV,j v represents the speed at point j of the intelligent vehicle. Human,j v represents the speed at which the vehicle is manually driven at point j. correct,j Indicates the corrected speed reference value; the corrected TD AV,j Less than or equal to TD0; v correct,j Based on the vehicle's operating speed and trajectory model, β1 or β2, according to TD correct,j With TD Human,j The difference, calculate v correct,j With v Human,j The corresponding difference is used to derive the corrected v. AV,j The specific calculation steps are shown in equations (6)-(8) below:

[0076] ΔTD j =TD Human,j -TD correct,j =β·ΔV j (6)

[0077] ΔV j =V Human,j -V correct,j (7)

[0078]

[0079] SpeedCrnt j With AvgSpeedUp j Since the correlation is high, the model results should not all be significant. Therefore, β should be either β1 or β2 depending on the significance of the variables in the model.

[0080] Step 6-3) If the anthropomorphic motion planning model of the scene is Type II, only for TD AV,j Perform calibration, and the calibrated TD AV,j Less than or equal to TD0, v AV,j Keep with v Human,j The same applies, and no correction based on the model is required.

[0081] Step 7) Output the corrected anthropomorphic motion planning model as a soft constraint for the intelligent vehicle motion control module to realize anthropomorphic speed and trajectory planning of the intelligent vehicle.

[0082] This embodiment utilizes 24 kilometers of real road geometry data, environmental data, and vehicle operation data from the Yongji Expressway in Hunan Province, China, to test the present invention.

[0083] The driving data of 56 drivers were collected using the eight-degree-of-freedom high-fidelity driving simulation platform of Tongji University. To ensure that the drivers' speed and trajectory behavior were mainly affected by the road geometry, the experiment was set to a uniform sunny daytime, and the participants were instructed to stay in the same lane before the experiment. The raw data was preprocessed in step 2 to obtain the road geometry data and the driving data of the 56 drivers required for modeling. The road geometry data included variables such as the curvature and slope of the current point, and the curvature and slope of the upstream and downstream 300 meters.

[0084] According to step 3) of the present invention, a vehicle speed and trajectory model based on a simultaneous equation model is established, including IDCF and ADCF trajectory models in horizontal curve uphill, horizontal curve downhill, horizontal curve concave curve, and horizontal curve convex curve scenarios, as well as speed models of IDCF, ADCF, and in-lane. The ADCF speed (unit: km / h) and trajectory (unit: m) model results in a horizontal curve uphill are taken as an example.

[0085] v ADCF =104.85-2.84·AvgGrdUp+0.58·MaxCrvChngUp-11.09·Tunnel+9.37·TAvgGrdUp-10.63·TAvgCrvUp-3.59·TMaxCrvChngUp

[0086] TD ADCF =-0.892+0.013·AvgSpeedUp+0.114·Curvature+0.276·CirCrvPerUp+0.024·MaxGrdUp-0.176·Tunnel

[0087] Where AvgGrdUp represents the average gradient of the upstream road segment (unit: %), MaxCrvChngUp represents the maximum curvature difference of the upstream road segment (unit: 1 / km), Tunnel represents the tunnel effect (1 inside the tunnel, 0 outside the tunnel), TAvgGrdUp, TAvgCrvUp, and TMaxCrvChngUp represent the average gradient (unit: %), average curvature (unit: 1 / km), and maximum curvature difference (unit: 1 / km) of the upstream road segment under the tunnel effect, respectively. Curvature represents the curvature at the current location (unit: 1 / km), CirCrvPerUp represents the percentage of circular curves in the upstream road segment (unit: %), and MaxGrdUp represents the maximum gradient of the upstream road segment (unit: %).

[0088] Based on the model results, anthropomorphic speed and trajectory planning is performed. Taking a 1-kilometer road segment consisting of horizontal curves, concave curves, horizontal curves, convex curves, and horizontal curves downhill as an example... Figure 2 This is a schematic diagram of the road alignment for this example section. Taking an intelligent vehicle entering curve 1 using the IDCF type as an example, based on the maximum probability of the offset direction, curves 2 and 3 are ADCF and In-lane, respectively. Based on the vehicle's speed and trajectory model results, the basic anthropomorphic speed and trajectory results are calculated. The maximum speed of the example section is 110.4 km / h, the minimum speed is 94.2 km / h, and the maximum trajectory offset is 0.830 m, exceeding the safety threshold of 0.775 m. Therefore, safety correction is required. According to step 6-2) Type I, the maximum speed is reduced by 5.0 km / h, and the vehicle trajectory is moved to within the safety threshold of curve 1. The corrected speed results are as follows. Figure 3 As shown.

[0089] In summary, the results of this embodiment achieve speed planning that meets the driver's expectations and corrects the trajectory to within a safe threshold. Using the vehicle speed and trajectory planning results in this invention as soft constraints in intelligent vehicle motion control will help intelligent vehicles better meet the driver's expectations when driving on mountain highways.

[0090] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A human-like speed and trajectory planning method for intelligent vehicles on mountain highways, characterized in that, Includes the following steps: Step 1) Obtain vehicle operation data, road geometry data, and environmental data for mountain highways; Step 2) Data preprocessing; Step 3) Using road geometry data and environmental data as explanatory variables, establish a theoretical vehicle speed and trajectory model based on a simultaneous equation model; Step 4) Based on the results of the vehicle speed and trajectory model, establish basic rules for anthropomorphic motion planning; Step 5) Construct an anthropomorphic motion planning model based on the results of the vehicle speed and trajectory model and the basic rules of anthropomorphic motion planning; Step 6) Correct the anthropomorphic motion planning model to avoid lane departure caused by the driver manually driving the vehicle; Step 7) Output the corrected anthropomorphic motion planning model as a soft constraint for the intelligent vehicle motion control module to realize anthropomorphic speed and trajectory planning of the intelligent vehicle; Step 2) includes the following steps: Step 2-1) Remove vehicle operation data that is affected by factors other than road geometry and road environment; Step 2-2) Define three types of vehicle deviation behavior: veering outwards from the curve, veering inwards from the curve, and staying within the lane. Determine the trajectory deviation safety threshold based on vehicle and road characteristics. The vehicle's trajectory deviation reached This indicates a potential safety hazard. Steps 2-3) Match vehicle operation data, road geometry data, and environmental data to the same spatial coordinates. Based on the road centerline, collect the raw data at pre-configured distance intervals to obtain the vehicle's speed, trajectory offset, and the average value of road geometry data and environmental data within each distance interval. The vehicle speed and trajectory model is established based on different combinations of alignment types and vehicle offset behavior to calibrate the coefficients of significant variables in different application scenarios. The vehicle speed and trajectory model is expressed as follows: in, Point Prediction speed at the location, This indicates the predicted lateral trajectory offset. The average speed of the upstream road segment is represented by the segment that has just been passed and the downstream segment that is about to be entered. These are endogenous variables. Point The road geometry variables and environmental variables at the current point, including variables of the current point and upstream and downstream road segments, are exogenous variables; m and n Indicates the number of significant variables; and Represents the model coefficients. and This represents the model error term.

2. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 1, characterized in that, The vehicle operation data includes speed and trajectory.

3. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 1, characterized in that, The road data includes road cross-sections, longitudinal sections, and planar geometric data.

4. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 1, characterized in that, The environmental data includes weather, lighting, and road conditions.

5. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 1, characterized in that, The basic rules of the anthropomorphic motion planning include: Intelligent vehicles use the output results of vehicle speed and trajectory models as a reference to determine the basic speed and trajectory values ​​for motion planning. The trajectory is adjusted based on the vehicle's speed and the output of the trajectory model to ensure that the vehicle does not deviate from the lane. And adjust the speed based on the driver's behavior habits shown in the model results.

6. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 5, characterized in that, Step 5) includes the following steps: Step 5-1) Determine the scene where the intelligent vehicle is located, including horizontal curve uphill, horizontal curve downhill, horizontal curve concave curve, and horizontal curve convex curve; Step 5-2) Determine the predicted vehicle offset direction on the combined alignment in the scenario. Based on the data analysis in step 2), determine the offset direction of the intelligent vehicle with the highest probability under the combined alignment type. Step 5-3) Based on the scene and the determined offset direction, construct a human-like motion planning model based on the vehicle's speed and trajectory model. The input to the human-like motion planning model is a point... The model output is points based on the road geometry and environmental variables at the location. The model predicts the velocity and lateral trajectory offset results.

7. The intelligent vehicle anthropomorphic speed and trajectory planning method for mountain highways according to claim 6, characterized in that, Step 6) includes the following steps: Step 6-1) Based on the anthropomorphic motion planning model, determine the significant variables of speed and their coefficients, and define two types: Type I: speed is positively correlated with trajectory; Type II: speed is not correlated with trajectory or is negatively correlated. Step 6-2) If the anthropomorphic motion planning model of the scene is type I, and at the same time... and The correction is performed with the following objectives: in, Point The lateral trajectory of the intelligent vehicle at that location. Point The trajectory of manual driving, This indicates a correction trajectory that does not exceed the threshold. Indicating intelligent vehicle points The speed at that point, Point At manual driving speed, Indicates the reference value for the corrected speed; after correction Less than or equal to ; Based on vehicle speed and trajectory model or ,as well as and Determining the difference: Based on the significance of the model variables, take or ; Step 6-3) If the anthropomorphic motion planning model of the scene is Type II, only for Perform correction, after correction Less than or equal to , Maintain and The same applies, and no correction based on the model is required.

8. A human-like speed and trajectory planning device for intelligent vehicles on mountain highways, comprising a memory, a processor, and a program stored in the memory, characterized in that, When the processor executes the program, it implements the method as described in any one of claims 1-7.

Images