Automatic driving behavior decision robust optimization method guided by long-period historical trajectory

Abstract

The invention discloses an automatic driving behavior decision robust optimization method guided by a long-period historical track, and belongs to the technical field of automatic driving. The method comprises the following steps: setting the number of historical decision trajectory windows, storing historical decision trajectory points of an automatic driving vehicle, and constructing a historical decision trajectory probability distribution model; obtaining a track point corresponding to the current decision, performing coordinate transformation on the track point corresponding to the current decision to obtain a space coordinate under a unified coordinate system, and constructing a probability distribution model of the track point corresponding to the current decision; calculating decision similarity measurement guided by historical decisions; and designing a behavior decision objective function according to decision similarity measurement to solve a robust optimization problem. According to the method, the problems of decision jitter and strategy abrupt change caused by dependence on single-frame observation or short-term trajectory information in a traditional method are avoided, the decision robustness, stability and reliability of an automatic driving system in a dynamic complex traffic environment are improved, and the risk of collision with surrounding traffic participants is reduced.

Description

Technical Field

[0001] This invention relates to a robust optimization method for autonomous driving behavior decision-making guided by long-term historical trajectories, belonging to the field of autonomous driving technology. Background Technology

[0002] In the architecture of autonomous driving systems, the behavior decision-making module, as a key link connecting environmental perception and vehicle control, directly determines the safety performance and operational reliability of autonomous vehicles through its decision robustness. This module needs to generate driving strategies in real time based on environmental perception data, and achieve adaptive responses to dynamic traffic environments by planning specific behaviors such as acceleration, deceleration, and lane changes.

[0003] Existing autonomous driving behavior decision-making methods have significant technical shortcomings:

[0004] Its decision-making is based solely on the Euclidean distance quantization results of single-frame observation data or short-term historical trajectory information.

[0005] While such methods can achieve basic decision-making functions in ideal scenarios where the perception module output is stable, they have serious limitations in real-world complex road environments. Due to the susceptibility of the upstream perception module to interference from obstructions, sensor noise, and accuracy limitations, false detections or missed detections of targets frequently occur. Furthermore, traditional methods lack an effective mechanism for utilizing long-term historical decision trajectories, making it impossible to establish the ability to identify decision deviation trends or correct perception errors through trajectory correlation analysis.

[0006] The aforementioned defects directly lead to unstable output from the decision-making module: decision jitter and policy abrupt changes are prone to occur during continuous decision-making, significantly increasing the risk of collisions between autonomous vehicles and surrounding road users. This instability not only seriously threatens road traffic safety but also leads to a decline in driving efficiency, becoming a key technical bottleneck restricting the commercialization of autonomous driving technology. Summary of the Invention

[0007] To address the problems existing in the background technology, the present invention provides a robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectories.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: a robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectories, the method comprising the following steps:

[0009] S1: Historical trajectory information storage and probability distribution modeling;

[0010] S2: Modeling the probability distribution of the current decision trajectory;

[0011] S3: Calculation of decision similarity measurement guided by historical decision-making;

[0012] S4: Robust optimization of behavioral decisions.

[0013] Furthermore, step S1 includes the following steps:

[0014] S101: Set the number of historical decision trajectory windows ;

[0015] S102: Stores historical decision trajectory points of autonomous vehicles.

[0016] (1)

[0017] In formula (1):

[0018] It is a set of historical decision-making trajectory points;

[0019] For the current moment The first Two-dimensional coordinate trajectory points at each instant;

[0020] S103: Constructing a probability distribution model of historical decision trajectories using kernel density estimation. :

[0021] (2)

[0022] In formula (2):

[0023] The number of windows representing historical decision-making trajectories;

[0024] For bandwidth parameters;

[0025] Let be the trajectory points whose probability density is to be estimated.

[0026] Furthermore, step S2 includes the following steps:

[0027] S201: Obtain the trajectory point corresponding to the current decision:

[0028] (3)

[0029] In formula (3):

[0030] This is the set of trajectory points corresponding to the current decision.

[0031] For the current moment The last Two-dimensional coordinate trajectory points at each instant;

[0032] S202: Obtain the core parameters of the spatiotemporal coordinate system of historical and current trajectories, including: time offset. Spatial rotation angle and spatial translation vector ;

[0033] S203: Transform the coordinates of the trajectory points corresponding to the current decision generated in S201 to obtain spatial coordinates in a unified coordinate system. :

[0034] (4)

[0035] In equation (4):

[0036] These are the original two-dimensional coordinates of the trajectory point corresponding to the current decision;

[0037] The two-dimensional coordinates of the trajectory point corresponding to the current decision after coordinate system transformation;

[0038] S204: Construct a probability distribution model of the trajectory points corresponding to the current decision using the kernel density estimation method. :

[0039] (5)

[0040] In equation (5):

[0041] This represents the number of trajectory points corresponding to the decision made at that time.

[0042] The trajectory point output by the current decision.

[0043] Furthermore, the calculation formula described in S3 is as follows:

[0044] (6)

[0045] In formula (6):

[0046] Historical decision trajectory probability distribution model Probability distribution model of trajectory points corresponding to the current decision A unified format;

[0047] This is the kernel function.

[0048] Furthermore, step S4 includes the following steps:

[0049] S401: Design the behavioral decision objective function based on decision similarity measurement. :

[0050] (7)

[0051] In equation (7):

[0052] All are weighted hyperparameters;

[0053] For the sake of efficiency;

[0054] For the sake of security;

[0055] For the cost of navigation;

[0056] The cost of achieving consensus in decision-making;

[0057] S402: Solving a robust optimization problem:

[0058] (8)

[0059] In equation (8):

[0060] The first planned for the current moment A two-dimensional trajectory point;

[0061] Maximum speed constraint for autonomous vehicles;

[0062] Maximum acceleration constraint for autonomous vehicles.

[0063] Furthermore, the efficiency cost described in S401 The calculation formula is as follows:

[0064] (9)

[0065] In equation (9):

[0066] , as well as All are weighted hyperparameters;

[0067] Current speed of the vehicle Vehicle reference speed The speed difference between them;

[0068] Current speed of the vehicle relative to the vehicle in front Speed ​​overshoot;

[0069] For the vehicle ahead Vehicle reference speed The speed difference between them;

[0070] This represents the number of trajectory points corresponding to the current decision.

[0071] Furthermore, the security cost described in S401 The calculation formula is as follows:

[0072] (10)

[0073] In formula (10):

[0074] For the cost of collision;

[0075] and All are Boolean values;

[0076] as well as All are weighted hyperparameters;

[0077] This is within the safe speed range.

[0078] Furthermore, the navigation cost described in S401 The calculation formula is as follows:

[0079] (11)

[0080] In equation (11):

[0081] as well as All are weighted hyperparameters;

[0082] Indicates the vehicle's current location Reference Path The distance between them;

[0083] It is a Boolean value.

[0084] Compared with the prior art, the beneficial effects of the present invention are:

[0085] This invention effectively identifies decision deviation trends caused by false detections or missed detections in the perception module or singularities in numerical solutions by constructing a probability distribution model of long-term historical decision trajectories and measuring its similarity to the probability distribution of the current decision trajectory. This avoids the decision jitter and policy mutation problems caused by traditional methods that rely on single-frame observations or short-term trajectory information. At the same time, it uses the difference in the probability distribution of historical trajectories as the core constraint of the robust optimization objective function, and combines a weighted combination of efficiency cost, safety cost, and navigation cost to optimize driving efficiency and path following accuracy while ensuring driving safety. This improves the decision robustness, stability, and reliability of the autonomous driving system in dynamic and complex traffic environments, reduces the risk of collisions with surrounding traffic participants, and provides key technical support for the commercialization of autonomous driving technology. Attached Figure Description

[0086] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0087] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0088] A robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectories, the method comprising the following steps:

[0089] S1: Historical trajectory information storage and probability distribution modeling;

[0090] S2: Modeling the probability distribution of the current decision trajectory;

[0091] S3: Calculation of decision similarity measurement guided by historical decision-making;

[0092] S4: Robust optimization of behavioral decisions.

[0093] Furthermore, step S1 includes the following steps:

[0094] S101: Set the number of historical decision trajectory windows based on onboard computing resource requirements. ;

[0095] S102: Establish a cache to store historical decision trajectory points of autonomous vehicles:

[0096] (1)

[0097] In formula (1):

[0098] It is a set of historical decision-making trajectory points;

[0099] For the current moment The first Two-dimensional coordinate trajectory points at each instant;

[0100] S103: Construct a probability distribution model of historical decision trajectories using kernel density estimation (KDE). :

[0101] (2)

[0102] In formula (2):

[0103] This refers to the number of historical decision trajectory windows, i.e., the number of samples participating in probability distribution modeling.

[0104] For bandwidth parameters;

[0105] Let be the trajectory points whose probability density is to be estimated.

[0106] Furthermore, step S2 includes the following steps:

[0107] S201: Obtain the trajectory points corresponding to the current decision using decision programming methods:

[0108] (3)

[0109] In formula (3):

[0110] This is the set of trajectory points corresponding to the current decision.

[0111] For the current moment The last Two-dimensional coordinate trajectory points at each instant;

[0112] S202: Core parameters of the spatiotemporal coordinate system obtained by the autonomous vehicle's own perception module, comparing historical and current trajectories, including: time offset. (Directly obtained from the decision frequency of the aforementioned decision-making method), spatial rotation angle (Obtained from the vehicle's own steering wheel angle) and spatial translation vector (Obtained from the vehicle's own positioning system);

[0113] S203: Transform the coordinates of the trajectory points corresponding to the current decision generated in S201 to obtain spatial coordinates in a unified coordinate system. :

[0114] (4)

[0115] In equation (4):

[0116] These are the original two-dimensional coordinates of the trajectory point corresponding to the current decision;

[0117] The two-dimensional coordinates of the trajectory point corresponding to the current decision after coordinate system transformation;

[0118] S204: Construct a probability distribution model of the trajectory points corresponding to the current decision using the kernel density estimation method. :

[0119] (5)

[0120] In equation (5):

[0121] This represents the number of trajectory points corresponding to the decision made at that time.

[0122] The trajectory point output by the current decision.

[0123] Furthermore, based on the historical decision trajectory probability distribution model Probability distribution model of trajectory points corresponding to the current decision The Jensen-Shannon divergence (JS divergence) between two probability distribution models is calculated as a similarity measure for decision guidance; the calculation formula described in S3 is as follows:

[0124] (6)

[0125] In formula (6):

[0126] Historical decision trajectory probability distribution model Probability distribution model of trajectory points corresponding to the current decision A unified format;

[0127] The kernel function is a Gaussian kernel, which is used in this method.

[0128] Furthermore, step S4 includes the following steps:

[0129] S401: Design the behavioral decision objective function based on decision similarity measurement. :

[0130] (7)

[0131] In equation (7):

[0132] All are weighted hyperparameters;

[0133] For the sake of efficiency;

[0134] For the sake of security;

[0135] For the cost of navigation;

[0136] The cost of achieving consensus in decision-making;

[0137] S402: Solve robust optimization problems using common nonlinear optimization methods (gradient descent, Gauss-Newton method, etc.).

[0138] (8)

[0139] In equation (8):

[0140] The first planned for the current moment A two-dimensional trajectory point;

[0141] Maximum speed constraint for autonomous vehicles;

[0142] Maximum acceleration constraint for autonomous vehicles.

[0143] Furthermore, the efficiency cost described in S401 The calculation considers both the vehicle's reference speed and the relative speeds of other road users in the surrounding traffic environment. The formula is as follows:

[0144] (9)

[0145] In equation (9):

[0146] , as well as All are weighted hyperparameters;

[0147] Current speed of the vehicle Vehicle reference speed The speed difference between them;

[0148] Current speed of the vehicle relative to the vehicle in front Speed ​​overshoot;

[0149] For the vehicle ahead Vehicle reference speed The speed difference between lanes indicates the efficiency of the current lane;

[0150] This represents the number of trajectory points corresponding to the current decision.

[0151] Furthermore, this invention evaluates the simulated trajectory of the vehicle after executing the decision sequence, as well as the trajectories of all other traffic participants. If a collision occurs between trajectories, the corresponding decision sequence is directly marked as a failure, and the safety cost is set to a maximum value. If there is no collision, it is also necessary to determine whether the vehicle trajectory poses a safety threat. When a safety threat occurs, the safety cost is calculated based on the safe speed range obtained from the actual situation. The safety cost described in S401... The calculation formula is as follows:

[0152] (10)

[0153] In formula (10):

[0154] The cost of the collision is a very large value;

[0155] and All are Boolean values, indicating whether a collision occurred in the vehicle's trajectory and whether there was a safety threat;

[0156] as well as All are weighted hyperparameters;

[0157] This is within the safe speed range.

[0158] Furthermore, this invention considers lane departure behavior in behavioral decision-making. Simultaneously, to improve decision consistency, strategies with similar implications to decisions made in the previous decision cycle are rewarded. Therefore, the navigation cost described in S401... The calculation formula is as follows:

[0159] (11)

[0160] In equation (11):

[0161] as well as All are weighted hyperparameters;

[0162] Indicates the vehicle's current location Reference Path The distance between them;

[0163] This is a Boolean value indicating whether the behavior of the current decision sequence is consistent with that of the previous decision cycle.

[0164] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of the equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0165] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectories, characterized in that: The method includes the following steps: S1: Historical trajectory information storage and probability distribution modeling; S2: Modeling the probability distribution of the current decision trajectory; S3: Calculation of decision similarity measurement guided by historical decision-making; S4: Robust optimization of behavioral decisions.

2. The robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectory as described in claim 1, characterized in that: S1 includes the following steps: S101: Set the number of historical decision trajectory windows ; S102: Stores historical decision trajectory points of autonomous vehicles. (1) In formula (1): It is a set of historical decision-making trajectory points; For the current moment The first Two-dimensional coordinate trajectory points at each instant; S103: Constructing a probability distribution model of historical decision trajectories using kernel density estimation. : (2) In formula (2): The number of windows representing historical decision-making trajectories; For bandwidth parameters; Let be the trajectory points whose probability density is to be estimated.

3. The robust optimization method for autonomous driving behavior decision-making guided by long-period historical trajectory as described in claim 2, characterized in that: S2 includes the following steps: S201: Obtain the trajectory point corresponding to the current decision: (3) In formula (3): This is the set of trajectory points corresponding to the current decision. For the current moment The last Two-dimensional coordinate trajectory points at each instant; S202: Obtain the core parameters of the spatiotemporal coordinate system of historical and current trajectories, including: time offset. Spatial rotation angle and spatial translation vector ; S203: Transform the coordinates of the trajectory points corresponding to the current decision generated in S201 to obtain spatial coordinates in a unified coordinate system. : (4) In equation (4): These are the original two-dimensional coordinates of the trajectory point corresponding to the current decision; The two-dimensional coordinates of the trajectory point corresponding to the current decision after coordinate system transformation; S204: Construct a probability distribution model of the trajectory points corresponding to the current decision using the kernel density estimation method. : (5) In equation (5): This represents the number of trajectory points corresponding to the decision made at that time. The trajectory point output by the current decision.

4. The robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectory as described in claim 3, characterized in that: The calculation formula described in S3 is as follows: (6) In equation (6): Historical decision trajectory probability distribution model Probability distribution model of trajectory points corresponding to the current decision A unified format; This is the kernel function.

5. The robust optimization method for autonomous driving behavior decision-making guided by long-period historical trajectory as described in claim 4, characterized in that: S4 includes the following steps: S401: Design the behavioral decision objective function based on decision similarity measurement. : (7) In equation (7): All are weighted hyperparameters; For the sake of efficiency; For the sake of security; For the cost of navigation; The cost of achieving consensus in decision-making; S402: Solving a robust optimization problem: (8) In equation (8): The first planned for the current moment A two-dimensional trajectory point; Maximum speed constraint for autonomous vehicles; Maximum acceleration constraint for autonomous vehicles.

6. The robust optimization method for autonomous driving behavior decision-making guided by long-cycle historical trajectory as described in claim 5, characterized in that: The efficiency cost described in S401 The calculation formula is as follows: (9) In equation (9): , as well as All are weighted hyperparameters; Current speed of the vehicle Vehicle reference speed The speed difference between them; Current speed of the vehicle Relative to the vehicle in front Speed ​​overshoot; For the vehicle ahead Vehicle reference speed The speed difference between them; This represents the number of trajectory points corresponding to the current decision.

7. The robust optimization method for autonomous driving behavior decision-making guided by long-period historical trajectory as described in claim 6, characterized in that: Security Costs as Described in S401 The calculation formula is as follows: (10) In formula (10): For the cost of collision; and All are Boolean values; as well as All are weighted hyperparameters; This is within the safe speed range.

8. The robust optimization method for autonomous driving behavior decision-making guided by long-period historical trajectory as described in claim 7, characterized in that: Navigation cost as described in S401 The calculation formula is as follows: (11) In equation (11): as well as All are weighted hyperparameters; Indicates the vehicle's current location Reference Path The distance between them; It is a Boolean value.

Images