A dynamic obstacle trajectory prediction and planning method for autonomous driving

Abstract

This invention discloses a dynamic obstacle trajectory prediction and planning method for autonomous driving, relating to the field of autonomous driving technology. It includes: acquiring historical movement states and map information of obstacles; obtaining a safe boundary trajectory based on the physical movement capability limits through reachability analysis, which does not consider driving intentions and traffic rules; using a closed-loop trained behavior reference trajectory generator, generating behavior reference trajectories with driving intention categories and driving preference parameters as conditions; the generator being trained by forming a closed loop with a planner and evaluator, aiming to maximize the planning utility score; using the safe boundary trajectory as a hard constraint, constructing a soft cost field by weighting the behavior reference trajectories according to intention probabilities; and minimizing a composite cost function containing driving efficiency objectives and soft cost terms under the hard constraints to solve for the vehicle's driving trajectory. This invention solves the problem of inconsistency between prediction training objectives and planning safety requirements.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically, to a dynamic obstacle trajectory prediction and planning method for autonomous driving. Background Technology

[0002] For autonomous vehicles to navigate safely in dynamic environments, it is essential to predict the future trajectories of surrounding traffic participants and plan their own paths accordingly. Currently, the commonly used approach is a sequential architecture of "prediction first, planning later," where the prediction module first outputs an estimate of the future trajectories of obstacles, and the planning module then generates the vehicle's trajectory based on this estimate. For example, Chinese patent CN118894134A proposes an autonomous vehicle trajectory planning method that considers the uncertainty of surrounding vehicles, using the n-step probability reachability set of surrounding vehicles to express motion uncertainty and combining it with model predictive control for trajectory planning. Chinese patent CN119099654A proposes a trajectory prediction method based on hierarchical progressive interaction and target lane segments, generating multiple candidate predicted trajectories by hierarchically modeling local and global interaction relationships.

[0003] The existing technologies described above have two shortcomings. First, the prediction module aims to minimize the deviation between the predicted trajectory and the actual trajectory, pursuing statistically significant prediction accuracy. However, the planning module actually requires complete motion information that can define hazard boundaries, resulting in inconsistent objective functions. This leads to the predictor's output of high-probability trajectories potentially missing low-probability but safety-related critical motion patterns, making it difficult for the planner to reliably balance safety and efficiency when complete hazard boundary information is lacking. Second, the establishment of safety constraints in existing methods is highly dependent on the accuracy of intent reasoning. If the intent judgment is flawed, the safety constraints may fail. Therefore, a dynamic obstacle trajectory prediction and planning method for autonomous driving is proposed to address the above problems. Summary of the Invention

[0004] To overcome the aforementioned deficiencies of the prior art, embodiments of the present invention provide a dynamic obstacle trajectory prediction and planning method for autonomous driving, in order to solve the problems of inconsistency between the training objectives of the prediction module and the safety requirements of the planning module, as well as the problem of safety constraints relying excessively on the accuracy of intent reasoning in the prior art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: A dynamic obstacle trajectory prediction and planning method for autonomous driving includes the following steps: S1: Obtain the historical motion status of dynamic obstacles around the vehicle and road map information to provide basic data for obstacle motion pattern analysis and scene constraints in subsequent steps; S2, for each of the dynamic obstacles, based on its current motion state and the preset physical motion capability limit, obtain all possible reachable spatial ranges in the future time period through reachability analysis, and extract an envelope trajectory with the maximum intrusion depth for the vehicle's expected path from the spatial range as a safety boundary trajectory. The reachability analysis is constrained only by the physical mobility limit and does not consider the driving intentions of obstacles or traffic rules. Therefore, there is no dependency between the generation process of the safe boundary trajectory and the accuracy of intention inference; the boundary of the safe drivable area it delineates is guaranteed by physical laws and will not fail due to errors in intention judgment. S3 utilizes a behavior reference trajectory generator trained in a closed loop, taking the historical state and scene information of obstacles as input, and the driving intention category and driving preference parameters as conditions to generate a set of behavior reference trajectories; and uses an intention reasoning network to output the posterior probability of each driving intention category as the intention probability based on the historical state and scene information. Different intention probabilities reflect the probability of the corresponding movement pattern occurring. The generator training process is as follows: The generator, planner, and evaluator are connected in series to form a closed loop. The generator outputs a sample reference trajectory. The planner generates a vehicle path based on this sample reference trajectory and the safety boundary trajectory. The evaluator evaluates the vehicle path to obtain a utility score. The generator parameters are updated with the goal of maximizing this utility score, combined with diversity and feasibility constraints. Through this closed-loop training, the generator no longer aims solely at fitting the real trajectory, but gradually learns to produce reference trajectories that help improve planning utility. S4. During the planning phase, the safety boundary trajectory is used as a hard constraint, requiring the planned trajectory of the vehicle to maintain a preset safe distance from it in time and space, thereby ensuring that the vehicle does not intrude into the motion envelope of the obstacle under any physically possible worst-case scenario. The behavioral reference trajectories are weighted according to their corresponding intent probabilities to construct a soft cost field, which serves as the soft cost term in the planning and optimization objective. Through weighting, areas with higher intent probabilities generate greater cost value in the soft cost field, thus providing stronger avoidance guidance for the vehicle's trajectory. Areas with lower probability of intent generate lower cost and provide weaker guidance for vehicle trajectory avoidance. S5, under the condition of satisfying the hard constraints, the future driving trajectory of the vehicle is solved by minimizing the composite cost function, which includes the driving efficiency objective and the soft cost term. The hard constraints limit the safe feasible region, and the soft cost term guides the trajectory to avoid high-probability conflict areas within the feasible region. The two work together to enable the vehicle to achieve a balance between safety and traffic efficiency.

[0006] Furthermore, in step S2, the physical motion capability limit includes the maximum longitudinal acceleration of the obstacle under road surface adhesion conditions. Maximum longitudinal deceleration Maximum lateral acceleration and maximum steering angular velocity .

[0007] The aforementioned limits are determined by the vehicle's physical properties and the road surface adhesion conditions, constituting the boundary of the physically achievable range of movement of the obstacle. Any movement exceeding this boundary is physically impossible.

[0008] Furthermore, in step S2, the reachability analysis employs a numerical integration method. Specifically, it involves using the current motion state of the obstacle as the initial value, sampling the longitudinal acceleration and steering angular velocity in the prediction time domain, and using a kinematic model to iteratively calculate the state evolution trajectory corresponding to each sampled control input. The endpoints of all trajectories constitute the spatial range. This method, through dense sampling and frame-by-frame integration within the control input space, can obtain highly accurate reachability calculation results.

[0009] Further, in step S2, the reachability analysis adopts a geometric analysis method, specifically: the motion of the obstacle is decoupled into longitudinal motion along the lane direction and lateral motion perpendicular to the lane direction, and the longitudinal displacement interval and lateral displacement interval are calculated using the uniform acceleration motion formula, respectively. After combining them into a rectangular area, the parts occupied by static obstacles and those exceeding the road boundary are removed to obtain the spatial range.

[0010] This method decouples two-dimensional motion into a combination of two one-dimensional motions, and achieves higher computational efficiency by replacing frame-by-frame integration with analytical calculation.

[0011] Furthermore, in step S2, the envelope trajectory is extracted in the following way: Determine the expected path of the vehicle, calculate the projection offset of all possible positions in the spatial range on the normal of the expected path at each future time, take the position point corresponding to the maximum offset towards the vehicle as the envelope point, and connect the envelope points along the time axis to form the envelope trajectory.

[0012] This envelope trajectory corresponds to the most intrusive movement of the obstacle to the vehicle among all possible physical maneuvers. As long as the vehicle does not enter this envelope trajectory, it can avoid collision under the guarantee of the laws of physics.

[0013] Furthermore, in step S3, the driving intention category is selected from yielding, cutting in, constant speed cruising, and lane changing, covering typical driving behavior patterns in intersection interaction scenarios; The driving preference parameters are a set of low-dimensional vectors, with each dimension corresponding to a quantified value of conservatism, courtesy, and speed preference. By adjusting the driving preference parameters, the overall behavioral tendency of the generated trajectory can be changed without altering the generator network parameters.

[0014] Further, in step S3, the training process of the generator includes: In the first stage, the generator is pre-trained by imitating real trajectories, so that the generator initially has the ability to output physically reasonable trajectories. In the second stage, the generator, planner, and evaluator are connected in a closed loop. The generator parameters are updated with the utility score as the feedback signal. The training loss function of the generator is composed of the negative value of the utility score, the diversity constraint term, and the feasibility constraint term.

[0015] The diversity constraint term forces the generated trajectories to maintain a preset spatial separation degree under different driving intention categories, so that the trajectories corresponding to each intention are dispersed and cover different potential motion patterns; the feasibility constraint term constrains the generated trajectory to meet kinematic constraints, ensuring that the generated trajectory is physically executable. In the third stage, training continues using the utility score as feedback, while perturbing the input obstacle history state. By introducing input perturbation during training, the generator's robustness to perceived noise is enhanced.

[0016] Furthermore, the utility score obtained by the evaluator for the autonomous vehicle route is calculated by weighting at least two of the following: safety score, traffic efficiency score, and comfort score. This utility score comprehensively measures the quality of the planned route across multiple dimensions and serves as a feedback signal to guide the generator to update in a direction that improves the planning utility.

[0017] Furthermore, in step S4, the safe distance is determined by the traffic flow density and interaction intensity of the current scene through a preset mapping relationship. The traffic flow density reflects the congestion level of the scene, and the interaction intensity reflects the severity of potential conflicts between the vehicle and obstacles. The safe distance is adaptively adjusted accordingly, so that the strictness of the safety constraints is adapted to the danger level of the scene. The soft cost field is constructed as follows: each behavioral reference trajectory is discretized into spatiotemporal sampling points, and a Gaussian potential field is generated with each sampling point as the center. The amplitude of the potential field is determined by the intent probability corresponding to the trajectory. The potential fields of all trajectories are superimposed to form the soft cost field. The trajectory with the higher the intent probability, the larger the amplitude of the potential field generated around it. The lower the probability of an intention, the smaller the potential field amplitude generated around it.

[0018] Furthermore, in step S5, the driving efficiency target includes at least one of travel time, path smoothness, and speed maintenance, seeking efficient passage within the feasible space defined by safety constraints; The weight coefficient of the soft cost term is adjusted synchronously with the safety distance. When the safety distance increases, the soft cost weight increases accordingly, and when the safety distance decreases, the soft cost weight decreases accordingly, so that the strength of safety constraints and efficiency guidance remains coordinated.

[0019] The technical effects and advantages of this invention are as follows: First, the generation of the safe boundary trajectory in this method relies solely on the physical movement limits of obstacles, completely disregarding driving intentions and traffic rules. The analysis results are directly guaranteed by physical laws. This safe boundary trajectory provides the planner with a hard constraint that is completely decoupled from the accuracy of intention inference. Even if the intention inference network makes a judgment error, the vehicle can still drive within a physically safe area, and the safety baseline is not shaken by inaccurate intention guesses.

[0020] Second, the behavior reference trajectory generator employs a closed-loop training mechanism, connecting the generator, planner, and evaluator in series to form a loop. Within the feasible region defined by the safety boundary trajectory, the planner generates a vehicle path based on the sample reference trajectory output by the generator. The evaluator performs a multi-dimensional utility assessment of this path and uses the utility score as a feedback signal for backpropagation to update the generator parameters. This training method shifts the generator's focus from simply fitting the real trajectory to improving planning utility. The output reference trajectory proactively provides useful information for planning decisions, thus resolving the inconsistency between the prediction training objective and the planning application objective.

[0021] Third, during the planning phase, the safety boundary trajectory is transformed into an inviolable hard constraint, defining an absolutely safe driving boundary. The behavioral reference trajectory, weighted according to intent probability, constructs a soft cost field, forming a risk gradient within the safety boundary to guide the vehicle's trajectory towards low-risk areas. Hard constraints and the soft cost field work in a layered and coordinated manner; the former ensures no collisions occur, while the latter optimizes traffic efficiency. Simultaneously, both the safety distance and the soft cost weights are adaptively adjusted based on the current traffic flow density and interaction intensity, relaxing constraints in sparse traffic to improve efficiency and tightening constraints in dense interactions to enhance safety, achieving a dynamic balance between safety and efficiency. Attached Figure Description

[0022] Figure 1 This is a schematic diagram of the prediction process of the planning method of the present invention; Figure 2 This is a schematic diagram of the dual-track output structure of the safety boundary trajectory and behavior reference trajectory of the present invention. Detailed Implementation

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

[0024] Example 1 As attached Figures 1 to 2 The method shown is a dynamic obstacle trajectory prediction and planning method for autonomous driving, which is applicable to highly interactive driving scenarios such as unsignaled intersections and highway ramp merging.

[0025] This method reconstructs the predicted output into two parallel channels: a safety boundary trajectory and a behavioral reference trajectory. The safety boundary trajectory is derived based on the physical limits of motion capabilities and is completely decoupled from the guessing of driving intentions, providing the planner with absolute safety hard constraints. The behavioral reference trajectory is generated through a closed-loop training mechanism that uses planning utility as a feedback signal. It actively serves planning decisions and provides efficiency optimization references for the planner.

[0026] The following provides a detailed explanation of each step.

[0027] S1 acquires the historical motion status of dynamic obstacles around the vehicle and road map information.

[0028] The vehicle acquires the motion state sequence of surrounding dynamic obstacles over a historical period through its onboard perception system. The motion state sequence of each obstacle includes a position sequence, a velocity sequence, and a heading angle sequence recorded at a sampling frequency of 10Hz. The historical period is 3 seconds, and each obstacle contains 30 sampling frames. Each frame records the obstacle's x-coordinate, y-coordinate, resultant velocity, and heading angle in the vehicle's coordinate system.

[0029] Simultaneously, road map information is obtained from high-precision maps, including lane boundaries, lane center lines, stop lines, and geometric descriptions of intersection areas. The road map information is then converted into a rasterized local map feature map in the vehicle coordinate system, with a raster resolution of 0.2m and a coverage area extending 50m outwards from the vehicle to form a 500×500 raster map.

[0030] The historical motion state sequences of all obstacles are arranged in chronological order to form a historical trajectory tensor. The historical trajectory tensor and local map feature map output in this step serve as the basic inputs for subsequent steps.

[0031] S2, for each dynamic obstacle, based on its current motion state and preset physical motion capability limits, obtains all possible reachable spatial ranges within a future time period through reachability analysis, and extracts an envelope trajectory with the maximum intrusion depth for the vehicle's expected path from this spatial range as the safety boundary trajectory. The reachability analysis is constrained only by physical motion capability limits and does not consider the driver's intentions or traffic rules regarding the obstacles.

[0032] First, determine the physical movement limits of each obstacle. This is based on the current road surface adhesion coefficient. Calculate the maximum longitudinal acceleration of the obstacle under road surface adhesion conditions. Maximum longitudinal deceleration Maximum lateral acceleration and maximum steering angular velocity . It was estimated by the vehicle based on weather and road conditions.

[0033] Under dry asphalt pavement Take 0.8, at this time , , . Depend on Divide by the current speed of the obstacle To obtain, that is .when When it is below 1 m / s, The limit is set to a preset upper limit of 1.0 rad / s to prevent the steering angular velocity from being too high at low speeds.

[0034] Next, reachability analysis is performed to obtain the obstacles in the future prediction time domain. The set of all possible reachable spatial locations within the area. Take 5 seconds. There are two options for reachability analysis.

[0035] The first method is numerical integration. Using the current motion state of the obstacle as the initial value, ... Inner Numerical integration is performed for the step size. The control input is longitudinal acceleration. and steering angular velocity , exist Sampling is performed uniformly within the interval according to a preset step size. exist Samples are taken uniformly within the interval at a preset step size. Control input is applied to each sampling group. The position of the obstacle at each time step is calculated iteratively using a single-vehicle kinematics model.

[0036] Let the position of the obstacle at the current moment be... Speed ​​is , heading angle is .

[0037] go through Then, update the status: , , , .

[0038] A complete state evolution trajectory is formed by iterating frame by frame, and the trajectory endpoints corresponding to all sampled control inputs constitute the spatial position set. This method has high calculation accuracy and can accurately characterize the motion capability boundaries of obstacles.

[0039] The second approach is a geometric analysis method. This decouples the obstacle's motion into longitudinal motion along the lane direction and lateral motion perpendicular to the lane direction. In the longitudinal direction, the longitudinal motion is treated as uniformly accelerated motion, and the current longitudinal velocity of the obstacle is obtained. ,Right now Projection in the tangential direction of the lane centerline.

[0040] Calculate the maximum forward displacement using the displacement formula for uniformly accelerated motion and maximum backward displacement : , .

[0041] In the lateral direction, assuming the initial lateral velocity is 0, with... Calculate the maximum lateral displacement on both sides : .

[0042] The longitudinal and lateral displacement intervals are combined to obtain a rectangular region. Then, the portions of this rectangular region occupied by static obstacles and those extending beyond the road boundaries are removed. The corrected region is the set of spatial locations. This method has high computational efficiency and is suitable for deployment scenarios with stringent real-time requirements.

[0043] Finally, the envelope trajectory is extracted from the obtained spatial location set. First, the expected driving path of the vehicle in the future time period is determined, taking the centerline of the vehicle's current lane as the path, and then based on the vehicle's current speed. Extend forward distance .

[0044] In every future moment Calculate the projected offset of all possible positions in the spatial location set at that moment along the expected path normal, and take the position point corresponding to the maximum offset towards the vehicle as the envelope point at that moment. Connect each envelope point sequentially along the time axis to form the safety boundary trajectory of the obstacle.

[0045] The safe boundary trajectories of all obstacles constitute a set of safe boundary trajectories, each of which is a deterministic spatiotemporal curve. Regardless of any physically possible manipulation of the obstacle, its range of motion is necessarily limited by this envelope trajectory. The vehicle only needs to plan its driving path outside this envelope trajectory to ensure that it does not collide with the obstacle.

[0046] S3 utilizes a closed-loop trained behavior reference trajectory generator, taking the historical state of obstacles and scene information as input, and the driving intention category and driving preference parameters as conditions to generate a set of behavior reference trajectories. Simultaneously, an intention inference network outputs the posterior probability of each driving intention category based on historical state and scene information as the intention probability, which is then used for subsequent weighting.

[0047] The purpose of this step is to provide the planner with a diverse set of behavioral reference trajectories that are practically helpful for decision-making. The behavioral reference trajectory generator is implemented using a conditional generation model, which requires two key conditional inputs.

[0048] The first conditional input is the driving intention category, selected from four typical driving modes: yielding, cutting in, cruise control, and lane changing. Yielding means the obstacle slows down to allow the vehicle to pass the conflict area first; cutting in means the obstacle accelerates to pass first; cruise control means the obstacle maintains its current speed and heading; and lane changing means the obstacle moves laterally into the adjacent lane.

[0049] The second input condition is the driving preference parameter, which is a three-dimensional vector. The three dimensions correspond to the quantified values ​​of conservatism, courtesy, and speed preference, respectively, and the values ​​of each dimension are all in the range of [0, 1]. The higher the conservatism, the more cautious the obstacle behavior; the higher the courtesy, the more the obstacle is inclined to avoid the vehicle; and the higher the speed preference, the more the obstacle is inclined to drive at high speed.

[0050] During the inference phase, the driving intention category is output by the intention inference network. The intention inference network adopts a graph attention network structure, taking the historical trajectory tensors of all obstacles and local map feature maps as inputs. Each obstacle is regarded as a node in the graph, and the interaction weights between nodes are adaptively calculated through the attention mechanism. The output is the posterior probability distribution of each obstacle belonging to the four driving intention categories, and the sum of the probabilities is 1.

[0051] Driving preference parameters are set by the autonomous vehicle decision-making module based on the current driving mode. In efficiency-first mode, the speed preference parameter is set to a higher value (e.g., 0.8) and the conservatism parameter is set to a lower value (e.g., 0.2); in safety-first mode, the conservatism parameter is set to a higher value (e.g., 0.8) and the speed preference parameter is set to a lower value (e.g., 0.2).

[0052] The specific structure of the behavior reference trajectory generator can adopt the conditional diffusion model. The conditional diffusion model consists of a forward diffusion process and a backward denoising process: The forward diffusion process gradually adds Gaussian noise to the true future trajectory, and after several steps the trajectory degenerates into pure noise.

[0053] The inverse denoising process uses the obstacle's historical trajectory, map features, driving intention category, and driving preference parameters as conditions. Starting from pure noise, it gradually removes noise and ultimately reconstructs a future trajectory that matches the conditions. The generator outputs multiple behavioral reference trajectories for each obstacle, with each trajectory consisting of a series of location points arranged chronologically within the prediction time period.

[0054] As an alternative approach, the generator can also employ a conditional variational autoencoder, which maps the input to a latent spatial distribution, and then the decoder samples from this distribution and combines it with conditional information to reconstruct the future trajectory. This method is typically faster in training and inference than the diffusion model, making it suitable for deployment scenarios with limited computing resources.

[0055] The generator training process is carried out in three progressive stages.

[0056] The first stage is pre-training. Historical obstacle trajectories and their corresponding future real-world trajectories are extracted from a real-world driving dataset as training sample pairs, with the future real-world trajectories serving as the supervision signal. The training loss function is the sum of the mean squared errors between the corresponding position points of the generated trajectory and the real trajectory. This stage enables the generator to initially possess the ability to output physically plausible trajectories, providing good parameter initialization for subsequent closed-loop training.

[0057] The second stage is closed-loop utility training. Before starting this stage, the intention reasoning network is independently pre-trained on the same dataset, using manually labeled driving intention categories as supervision labels and cross-entropy loss for training, enabling it to output reliable posterior probabilities of intentions. After entering closed-loop training, the parameters of the intention reasoning network remain fixed and no longer participate in gradient updates.

[0058] The closed-loop training circuit connects the generator, planner, and evaluator modules in series to work together. A complete closed-loop training iteration proceeds in the following order.

[0059] The first step is for the generator to receive the historical state of obstacles and map features from the training samples, and output multiple behavioral reference trajectories under the given driving intention category and driving preference parameters.

[0060] The second step is to feed these behavioral reference trajectories, along with the set of safety boundary trajectories produced in step S2, into the planner.

[0061] The third step involves the planner performing hierarchical planning. First, the safety boundary trajectories are transformed into hard constraints: at every future time point in the prediction time domain, the Euclidean distance between the planned trajectory position of the vehicle and the position of each safety boundary trajectory at the same time point must not be less than the safety distance. Training phase Take a fixed value of 1.5m.

[0062] The behavioral reference trajectories are then weighted according to the posterior probabilities given by the intention reasoning network to construct a soft cost field in the spatiotemporal space. Trajectories with higher intention probabilities generate higher cost values ​​in the space.

[0063] Then, within the feasible space defined by hard constraints, the future driving path of the vehicle is generated by solving a constrained optimization problem with driving efficiency as the objective and soft cost field as the penalty term.

[0064] The planner is implemented using model predictive control. In each control cycle, it solves a finite-time open-loop optimization problem starting from the current state, executes only the first step of the optimization result control variable, and repeats this process in the next cycle.

[0065] The fourth step involves the evaluator receiving the vehicle path output by the planner, performing a comprehensive evaluation from multiple dimensions, and calculating a scalar utility score. : ; in The safety score is derived by mapping the minimum collision time between the vehicle's planned path and the safety boundary trajectories of each obstacle. A shorter minimum collision time results in a higher score. The lower.

[0066] Traffic efficiency is scored based on the ratio of the vehicle's average speed to a preset desired speed; the closer the ratio is to 1, the better. The higher.

[0067] The comfort score is obtained by normalizing the reciprocal of the root mean square value of the rate of change of vehicle acceleration; the smaller the rate of change, the better. The higher.

[0068] In scenarios where security is the primary objective, Set it to 0.5. Set it to 0.3. Set it to 0.2; if efficiency is a greater priority, then... Increase to 0.5, Reduced to 0.3.

[0069] Step 5: Utilize utility scores Calculate the training loss function of the generator : ; First item middle The larger the value, the smaller the value of this item, driving the generator to update parameters in a direction that improves planning efficiency.

[0070] Second item As a diversity constraint, calculate the average Euclidean distance between each pair of generated behavioral reference trajectories under different driving intention categories. .

[0071] when Less than the preset separation distance threshold hour, Pick ,otherwise Set to 0. This constraint forces the generated trajectories to maintain sufficient spatial separation under different intentional conditions. Typically, a value of 1.0m to 3.0m is used, with smaller values ​​used for narrow urban roads and larger values ​​used for highways.

[0072] Third item As a feasibility constraint, the acceleration of the generated trajectory is checked at each sampling point to see if it exceeds the limit. Does the steering angular velocity exceed the limit? For any quantity exceeding the limit, the square of the result is accumulated as a penalty to ensure that the generated trajectory is physically executable. , , These are the weighting coefficients. Set to 1.0. and Adjust within the range of 0.1 to 0.5 to minimize... To achieve this, the network parameters of the generator are updated via backpropagation.

[0073] Through repeated iterative training, the generator learns to produce reference trajectories that enable the planner to obtain efficient scores. Its output is no longer a simple imitation of the real trajectory, but actively serves the planning decision.

[0074] The third stage is disturbance-resistant training. After the second stage training has converged, random noise is superimposed onto the obstacle history states input to the generator. Each element in the noise independently follows a mean of 0 and a standard deviation of [value missing]. Gaussian distribution, Use 0.05 to 0.20 times the standard deviation of each dimension of the historical state. Continue using the loss function. The generator parameters are updated to improve the generator's robustness to perceived noise, enabling it to stably output a reference trajectory that is beneficial to planning even if there are certain errors in perception in a real deployment environment.

[0075] After the three stages of training described above, the behavior reference trajectory generator outputs a set of behavior reference trajectories during inference, with each trajectory corresponding to a driving intention category. Due to the continuous effect of the diversity constraint during training, the trajectories generated under different intention categories maintain a preset separation distance in space, collectively covering multiple possible movement patterns of obstacles, and providing rich decision-making reference information for subsequent planning steps.

[0076] S4. In the planning phase, the safety boundary trajectory is used as a hard constraint, requiring the planned trajectory of the vehicle to maintain a preset safe distance from it in time and space; the behavioral reference trajectory is weighted according to the corresponding intention probability to construct a soft cost field, which is used as a soft cost term in the planning optimization objective.

[0077] First, we address the hard constraints. All the safe boundary trajectories obtained in step S2 are transformed into hard constraints that the planner must satisfy: At every future moment within the prediction time domain The Euclidean distance between the location of the vehicle's planned trajectory and the location of each safety boundary trajectory at the same time must not be less than the safety distance. . Based on the traffic flow density of the current scenario and interaction strength Determined through a pre-set two-dimensional lookup table.

[0078] Defined as the ratio of the number of obstacles within a 50m radius around the vehicle to the area of ​​the road. It is used to measure the degree of spatiotemporal overlap between the vehicle's expected path and the safety boundary trajectories of each obstacle.

[0079] The calculation method is as follows: the expected path of the vehicle is unfolded along the time axis to form a spatiotemporal corridor, and each safety boundary trajectory is unfolded along the time axis to form a spatiotemporal volume. The size of the intersection of the two is calculated and normalized to the interval [0, 1]. The higher the value, the more serious the potential conflict.

[0080] Two-dimensional lookup table will and The scenarios are divided into three ranges: low, medium, and high, and then paired to form nine different scenarios. In one example, the low... And low In this case Take 1.2m, height And high In this case Take 2.5m, middle And in In this case Take 1.8m as the reference, and set the reference height in different increments between 1.2m and 2.5m for other cases.

[0081] The corresponding soft cost weighting coefficient In low And low In this case, take 0.1, high And high In this case, take 0.8, in the middle And in In this case, the value is set to 0.4; in other cases, it is set in increments between 0.1 and 0.8.

[0082] Based on real-time calculations at runtime and Given the range it falls into, look up the table to obtain the appropriate table. and .

[0083] Next, the soft cost field is processed. All behavioral reference trajectories generated in step S3 are weighted according to their corresponding intention posterior probabilities to construct a soft cost field covering the prediction time domain. .

[0084] The construction process is as follows: For each behavioral reference trajectory, it is discretized into a series of spatiotemporal sampling points along the time axis at 0.1s intervals. A two-dimensional Gaussian potential field is generated on the spatial plane with each sampling point as the center. This potential field has a maximum value at the center and smoothly decays to 0 with increasing distance from the center. The amplitude of the Gaussian potential field is set as the posterior probability value of the intent corresponding to the trajectory; the higher the probability, the larger the amplitude.

[0085] The spatial extent of the potential field is determined by the standard deviation parameter. control, The distance traveled is taken as 0.10 to 0.30 times the estimated travel distance of the vehicle within the prediction time domain. For example, when the estimated travel distance is 50m, Between 5m and 15m.

[0086] By superimposing the Gaussian potential fields generated by the reference trajectories of all obstacles in space one by one, a spatiotemporally continuous soft cost field is obtained. . The meaning is: the vehicle incurs a higher cost when traveling in areas traversed by trajectories with high intent probability, a lower cost when traveling in areas traversed by trajectories with low intent probability, and zero cost when traveling in areas without any behavioral reference trajectories.

[0087] S5. Under the condition of satisfying hard constraints, the future driving trajectory of the vehicle is solved by minimizing the composite cost function, which includes a driving efficiency objective and a soft cost term.

[0088] First, construct the composite cost function. : ; in The driving efficiency target is composed of three weighted sub-items: The passage time item is measured by the deviation between the time it takes for a vehicle to pass through the intersection and the theoretical shortest passage time, encouraging vehicles to pass through as quickly as possible; The path smoothness term is measured by the integral of the square of the curvature of the vehicle trajectory over time, which encourages smooth trajectories and reduces unnecessary steering. The speed maintenance term uses the integral of the square of the difference between the vehicle's actual speed and the preset desired speed over time as a metric to encourage the vehicle to maintain a speed close to the desired speed.

[0089] The weighting coefficients for each sub-item can be set according to driving style. For example, the weight for travel time can be 0.4, the weight for path smoothness can be 0.3, and the weight for speed maintenance can be 0.3. The weighting coefficients for the soft cost term are... Determined synchronously using the same two-dimensional lookup table.

[0090] exist low and Low-end scenarios Smaller The impact on planning is relatively weak, and autonomous driving behavior tends to be more efficient and aggressive. exist High and High-resolution scenes Larger The impact on planning is significant, and drivers are becoming more cautious and cooperative, actively avoiding high-probability conflict areas.

[0091] Under the condition of satisfying hard constraints, solve for... The goal is to find the vehicle's trajectory sequence that minimizes the minimum value. Hard constraints ensure that the vehicle will never enter the restricted area defined by the safe boundary trajectory. There are two possible solution methods.

[0092] The first approach uses the interior-point method for numerical optimization. It uses the position and velocity at each sampling time point in the vehicle trajectory sequence as optimization variables, and the safety distance requirement as an inequality constraint. The objective function is used to obtain the optimal trajectory sequence through iterative solution. When two adjacent iterations... The change in value is less than a preset threshold (e.g.) Stop iteration when ).

[0093] The second approach parameterizes the vehicle trajectory as a quintic spline curve, using spline control points as optimization variables, and employs a gradient descent method to iteratively search for the optimal control point positions. Each iteration follows... The control point position is updated using the negative gradient direction of the control point coordinates. When the gradient magnitude is less than a preset threshold (e.g., ...), the control point position is updated. When convergence is reached, the iteration stops.

[0094] The final obtained vehicle trajectory sequence is directly sent to the vehicle control system for execution.

[0095] Through the complete process described in S1 to S5, safety constraints are established on a safety boundary trajectory derived from the limits of physical movement capabilities, completely decoupled from the accuracy of intent reasoning. Even if the intent reasoning network makes a mistake, the safety boundary trajectory still provides absolute safety assurance. The generation of the behavioral reference trajectory is achieved through closed-loop training driven by planning utility feedback, enabling the generator to actively output reference information beneficial to planning decisions. Both work collaboratively through hierarchical hard constraints and soft cost fields to optimize traffic efficiency above the safety baseline.

[0096] Those skilled in the art should understand that the above-described embodiments depict various optional implementation methods. Reachability analysis can be performed using numerical integration or geometric analysis methods depending on computational accuracy and real-time requirements. The behavioral reference trajectory generator can employ a conditional diffusion model or a conditional variational autoencoder. The planning solution can employ interior point methods or gradient-based spline optimization methods. Substitutions and combinations of these implementation methods, as long as they utilize the technical concept of dual-track output of the safety boundary trajectory and the behavioral reference trajectory, and drive reference trajectory generation with planning utility feedback, do not depart from the scope of protection of this invention.

Claims

1. A dynamic obstacle trajectory prediction and planning method for autonomous driving, characterized in that, Includes the following steps: S1, acquire the historical motion status of dynamic obstacles around the vehicle and road map information; S2, for each of the dynamic obstacles, based on its current motion state and the preset physical motion capability limit, obtain all possible reachable spatial ranges in the future time period through reachability analysis, and extract an envelope trajectory with the maximum intrusion depth for the vehicle's expected path from the spatial range as a safety boundary trajectory. The reachability analysis is constrained only by the physical mobility limit and does not take into account the driving intentions of obstacles and traffic rules. S3. Using a behavior reference trajectory generator trained in a closed loop, a set of behavior reference trajectories is generated with the historical state and scene information of obstacles as input and the driving intention category and driving preference parameters as conditions. The intention reasoning network outputs the posterior probability of each driving intention category as the intention probability based on the historical state and scene information. The training process of the generator is as follows: the generator is connected in series with the planner and the evaluator to form a closed loop. The generator outputs a sample reference trajectory. The planner generates a vehicle path based on the sample reference trajectory and the safety boundary trajectory. The evaluator evaluates the vehicle path to obtain a utility score. The generator parameters are updated with the goal of maximizing the utility score and combining diversity constraints and feasibility constraints. S4. During the planning phase, the safety boundary trajectory is used as a hard constraint, requiring the planned trajectory of the vehicle to maintain a preset safe distance from it in time and space. The behavioral reference trajectories are weighted according to the corresponding intention probabilities to construct a soft cost field, which is used as a soft cost term in the planning and optimization objective. S5. Under the condition of satisfying the hard constraints, the future driving trajectory of the vehicle is solved by minimizing the composite cost function, which includes the driving efficiency objective and the soft cost term.

2. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S2, the physical motion capability limits include the maximum longitudinal acceleration, maximum longitudinal deceleration, maximum lateral acceleration, and maximum steering angular velocity of the obstacle under road surface adhesion conditions.

3. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 2, characterized in that, In step S2, the reachability analysis employs a numerical integration method, specifically: Using the current motion state of the obstacle as the initial value, the longitudinal acceleration and steering angular velocity are sampled in the prediction time domain. The kinematic model is used to iteratively calculate the state evolution trajectory corresponding to each sampled control input. The endpoints of all trajectories constitute the spatial range.

4. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 2, characterized in that, In step S2, the reachability analysis employs a geometric analysis method, specifically: The motion of the obstacle is decoupled into longitudinal motion along the lane direction and lateral motion perpendicular to the lane direction. The longitudinal displacement range and lateral displacement range are calculated using the uniform acceleration motion formula, respectively. After combining them into a rectangular area, the parts occupied by static obstacles and those exceeding the road boundary are removed to obtain the spatial range.

5. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S2, the envelope trajectory is extracted as follows: Determine the expected path of the vehicle, calculate the projection offset of all possible positions in the spatial range on the normal of the expected path at each future time, take the position point corresponding to the maximum offset towards the vehicle as the envelope point, and connect the envelope points along the time axis to form the envelope trajectory.

6. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S3, the driving intention category is selected from yielding, cutting in, constant speed cruising, and changing lanes; The driving preference parameters are a set of low-dimensional vectors, with each dimension's value corresponding to a quantified value of conservatism, courtesy, and speed preference.

7. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S3, the training process of the generator includes: In the first stage, the generator is pre-trained by imitating real trajectories; In the second stage, the generator, planner and evaluator are formed into a closed loop. The generator parameters are updated with the utility score as the feedback signal. The training loss function of the generator is composed of the negative value of the utility score, the diversity constraint term and the feasibility constraint term. Among them, the diversity constraint term forces the generated trajectories to maintain a preset spatial separation degree under different driving intention categories, and the feasibility constraint term constrains the generated trajectory to meet kinematic constraints; In the third stage, while perturbing the input obstacle history state, training continues with the utility score as feedback.

8. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 7, characterized in that, The utility score obtained by the evaluator for evaluating the vehicle path is calculated by weighting at least two of the following: safety score, traffic efficiency score, and comfort score.

9. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S4, the safe distance is determined by the traffic flow density and interaction intensity of the current scene through a preset mapping relationship; The soft cost field is constructed as follows: each behavioral reference trajectory is discretized into spatiotemporal sampling points, and a Gaussian potential field is generated with each sampling point as the center. The magnitude of the potential field is determined by the intention probability corresponding to the trajectory. The potential fields of all trajectories are superimposed to form the soft cost field.

10. The dynamic obstacle trajectory prediction and planning method for autonomous driving according to claim 1, characterized in that, In step S5, the driving efficiency target includes at least one of travel time, path smoothness, and speed maintenance; The weighting coefficient of the soft cost term is adjusted synchronously with the safety distance.

Images