Multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding

Abstract

The invention discloses a multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding, and the method comprises the steps: sampling candidate lanes based on the position of a target obstacle and a high-precision map, and carrying out the clustering and filtering of the candidate lanes; inputting the information of the target obstacle and the information of the candidate lane into a feature fusion module and a candidate lane scoring module to obtain scores of the candidate lane; speed embedding is carried out on the candidate lanes to obtain candidate lane information containing different longitudinal behaviors, and the candidate lane information is sent to a trajectory regression module to obtain a predicted trajectory; sending the predicted trajectory into a trajectory scoring module to obtain the confidence coefficient of the multi-modal trajectory; the steps of self-vehicle trajectory planning are roughly the same as those of trajectory prediction; and the predicted trajectory of the target obstacle and the planned trajectory of the vehicle are sent to a Cost scoring module for comprehensive scoring. According to the method, the predicted and planned trajectory is obtained based on candidate lanes and speed embedding, so that the model has the characteristics of low delay and high interpretability while the prediction and planning precision of the model is higher.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving trajectory prediction and trajectory planning, and in particular to a multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding. Background Art

[0002] Trajectory prediction and trajectory planning tasks have very important applications in the field of autonomous driving or advanced driver assistance. Early work often regarded trajectory prediction and trajectory planning as two separate independent modules and constructed them without coupling them together, such as (Müller M, Dosovitskiy A, Ghanem B, et al. Driving Policy Transfer via Modularity and Abstraction[J].arXiv, 2018.). Later research works gradually coupled the two tasks of trajectory prediction and trajectory planning together, such as (Song H, Ding W, Chen Y, et al. PiP: Planning-informed Trajectory Prediction for Autonomous Driving [A]. See: 2020, 12366: 598-614.), (Huang Z, Karkus P, Ivanovic B, et al. DTPP: Differentiable Joint Conditional Prediction and Cost Evaluation for Tree Policy Planning in Autonomous Driving [J]. arXiv, 2024.), (Cheng J, Chen Y, Chen Q. PLUTO: Pushing the Limit of Imitation Learning-based Planning for Autonomous Driving [J]. arXiv, 2024.).

[0003] For trajectory prediction tasks, the earliest method was to use physical models to simulate and predict possible human behaviors, such as the AEB (Autonomous Emergency Brake) system (Kaempchen N, Schiele B, Dietmayer K. Situation Assessment of an Autonomous Emergency Brake for Arbitrary Vehicle-to-Vehicle Collision Scenarios [J]. IEEE Transactions on Intelligent Transportation Systems, 2009, 10 (4): 678-687.) The physical model was used to consider the possible trajectories and brake triggering time between vehicles to minimize collisions. However, an obvious defect is that the model is not general enough and the generalization ability is insufficient. In addition, early models are all dedicated to deterministic trajectory prediction tasks (Deterministic Trajectory Prediction, DTP), such as (Dendorfer P, Elflein S, Leal-Taixé L. MG-GAN: A Multi-Generator Model Preventing Out-of-Distribution Samples in Pedestrian TrajectoryPrediction[A].2021:13158-13167.), which does not reflect the multimodality of human intentions in real scenarios. Therefore, in order to solve this problem, research on multimodal trajectory prediction has been paid attention to in recent years, and machine learning (Learning-Based) methods have emerged. Advanced methods such as attention mechanism, convolutional pooling, and graph neural network have been applied to prediction and planning tasks and achieved good results.

[0004] This method refers to many of the latest technical research. In terms of feature fusion BackBone, the SIMPL model (Zhang L, Li P, Liu S, et al. SIMPL: A Simple and Efficient Multi-agent Motion Prediction Baseline for Autonomous Driving [J]. arXiv, 2024.) is closely related to this method. It uses an instance-centric approach to adjust the coordinate system of the input features, and uses RPE to encode the relative position features between elements, and finally sends them to the Symmetric Fusion Transformer for feature fusion.

[0005] The methods that are closely related to this method in terms of trajectory prediction and trajectory planning decoding are (Zhao H, Gao J, Lan T, et al. TNT: Target-drive N Trajectory Prediction [J]..) and (Afshar S, Deo N, Bhagat A, et al. PBP: Path-based Trajectory Prediction for Autonomous Driving [J]. arXiv, 2024.). Early models mostly used direct regression of trajectory points to obtain output trajectories. Although the method is simple, the trajectory often does not conform to road constraints or violates kinematics. TNT uses the 2D target points sampled on the road as a priori conditions for subsequent complete trajectory generation, but a single 2D target point has a weak induced bias for predicting the entire trajectory. The predicted complete trajectory is not smooth enough, and because there is no encoding of lateral and longitudinal information, it is poorly interpretable. PBP uses road information that has stronger features than target point information, but the essential approach is only to sample the starting point and midpoint of the road, the features are still weak, and there is no guidance information for longitudinal behavior. In terms of the cost part, the method that is closely related to this method is (Huang Z, Karkus P, Ivanovic B, et al. DTPP: Differentiable Joint Conditional Prediction and Cost Evaluation for Tree Policy Planning in Autonomous Driving[J]. arXiv, 2024.), but it does not consider the multimodality of the predicted trajectory. Summary of the invention

[0006] The purpose of the present invention is to provide an integrated multi-modal trajectory prediction and planning method based on candidate lanes and speed embedding in order to address the deficiencies of the prior art.

[0007] The object of the present invention is achieved through the following technical solution: a multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding, comprising the following steps:

[0008] (1) Based on the location of the target obstacle and the high-precision map, sample the candidate lanes that it may go to in the future, then perform hierarchical clustering on the candidate lanes to filter out redundant candidate lanes;

[0009] (2) The historical information of the target obstacle, the historical information of other surrounding intelligent agents, the element information of the surrounding high-precision map, and the candidate lane information obtained in step (1) are input into the feature fusion module and the candidate lane scoring module to obtain the score of the candidate lane, which represents the possibility of the target obstacle moving to the lane in the future;

[0010] (3) Embed the candidate lanes obtained in step (1) with their speeds to obtain candidate lane information containing different longitudinal behaviors;

[0011] (4) Sending the candidate lane information containing different longitudinal behaviors obtained in step (3) into the trajectory regression module to obtain the predicted trajectory of the target obstacle;

[0012] (5) Sending the predicted trajectory obtained in step (4) to the trajectory scoring module to obtain the confidence of the multimodal predicted trajectory of the target obstacle;

[0013] (6) Changing the object from the target obstacle to the autonomous vehicle, further filtering the candidate lanes of the autonomous vehicle according to its own intention based on step (1); executing steps (1) to (5) to obtain the planned trajectory of the autonomous vehicle;

[0014] (7) The predicted trajectory of the target obstacle and the planned trajectory of the autonomous vehicle are sent to a learnable cost scoring module for comprehensive scoring of the planned trajectory.

[0015] Furthermore, the step (1) includes the following sub-steps:

[0016] (1.1) According to the current position of the target obstacle vehicle, call the map API interface to obtain all the original lane information map_paths within a certain range around the target obstacle vehicle (raw) ;

[0017] (1.2) Using the map_paths obtained in step (1.1) (raw) , use the depth-first algorithm to search from near to far based on the vehicle's current position and orientation to obtain candidate lanes map_paths (ref_all) , and then perform equally spaced sequence point sampling to obtain all candidate lane point sequences path_cords (ref_all) ;

[0018] (1.3) Using all candidate lane point sequences path_cords obtained in step (1.2) (ref_all) , use the KD-Tree data structure to record each candidate lane point sequence, and then use the point-to-line projection to calculate the distance between each two candidate lane point sequences to construct the distance matrix M path ;

[0019] (1.4) Using the distance matrix M obtained in step (1.3) path And all candidate lane point sequences path_cords obtained in step (1.2) (ref_all) , use hierarchical clustering to filter candidate lanes with high similarity to obtain the final candidate lane point sequence path_cords of the target obstacle (ref) , and then extract the direction vector and splice it to get the candidate lane feature candidate_refpath of the target obstacle.

[0020] Furthermore, the step (2) includes the following sub-steps:

[0021] (2.1) Sample the features agent_feats of the target obstacle and other agents around it for 2 seconds, where agent_feats contains the following features: coordinates xy, speed magnitude v, speed direction v_yaw, vehicle width w, vehicle length l and vehicle type type, the sampling frequency is 10Hz, and the coordinate system is the own coordinate system;

[0022] (2.2) Sampling map element features map_feats around the target obstacle, where map_feats contains the following features: coordinate xy, direction vector d_xy and lane type type. The interval of sampling points is 10m, the coordinate system is the own coordinate system, and a maximum of 20 points are sampled for each ordinary road or intersection;

[0023] (2.3) The number of target obstacles is denoted as N tar , the number of other agents around is recorded as N sur , the number of map elements is recorded as N map , the total number of instances is recorded as N instance , then N instance =N tar +N sur +N map , calculate the pairwise relative position information RPE between the target obstacle, other surrounding agents, and all map elements, including the relative orientation angle γ i→j , relative heading angle β i→j and relative distance |dist i→j |, the dimension of RPE is [N instance ,N instance ,5];

[0024] (2.4) The agent_feats obtained in step (2.1), the map_feats obtained in step (2.2), and the RPE obtained in step (2.3) are sent to the feature fusion module, and then the fused agent_feats features and the target obstacle candidate lane feature candidate_refpath obtained in step (1.4) are sent to the candidate lane scoring module to obtain the score Prob for the candidate lane. refpath .

[0025] Furthermore, the step (3) includes the following sub-steps:

[0026] (3.1) Predefine the learnable velocity vector parameter V embed , which includes three longitudinal velocity behaviors, namely acceleration V acc , uniform speed V cons and deceleration V dec ;

[0027] (3.2) Use the target obstacle candidate lane feature candidate_refpath obtained in step (1.4) and the three speed behavior embedding vectors in step (3.1) to accelerate V acc , uniform speed V cons and deceleration V dec Perform dimension splicing to obtain candidate lane information refpath containing different longitudinal behaviors vel .

[0028] Furthermore, the step (4) includes the following sub-steps:

[0029] (4.1) Using the candidate lane information refpath containing different longitudinal behaviors of the target obstacle obtained in step (3.2) vel The agent_feats obtained after the feature fusion in step (2.4) is sent to the trajectory regression module to output the Bezier control point CP of the target obstacle's trajectory in the next 5 seconds bez ;

[0030] (4.2) Using the Bezier control point CP of the target obstacle's trajectory in the next 5 seconds obtained in step (4.1) bez Multiply it with the constant matrix to get the trajectory Traj for the next 5 seconds pred , the output frequency is 10Hz.

[0031] Furthermore, the step (5) is specifically as follows:

[0032] The agent_feats obtained after the feature fusion in step (2.4) and the predicted trajectory Traj in step (4.2) predSend it to the trajectory scoring module to obtain the multimodal predicted trajectory Traj of the target obstacle pred The confidence level Prob pred .

[0033] Furthermore, the step (6) includes the following sub-steps:

[0034] (6.1) Change the object from the target obstacle to the self-driving car, based on the map_paths obtained in step (1.2) (ref_all) Filter based on the actual future intention of the vehicle and only keep adjacent candidate lanes that match the future intention;

[0035] (6.2) Execute steps (1) to (5) to obtain the planned trajectory Traj of the autonomous vehicle ego_plan .

[0036] Furthermore, the step (7) includes the following sub-steps:

[0037] (7.1) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5) pred , the planned trajectory Traj of the autonomous vehicle in step (6.2) ego_plan The agent_feats obtained after the feature fusion in step (2.4) is sent to the Cost scoring module;

[0038] (7.2) First, feature extraction is performed using the CP in step (4.1) bez Combined with the differentiable property of Bessel control points, the kinematic parameters ego_traj_features of the planned trajectory of the autonomous driving vehicle are obtained. The features of a single planned trajectory are recorded as c e ; Contains the following features: speed Vec ego , Acceleration Acc ego , Jerk ego and the lateral acceleration a y ;

[0039] (7.3) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the autonomous vehicle planning trajectory Traj in step (6.2) ego_plan Calculate the multimodal interaction features traj_interact_features between trajectories. The interaction features of a single planned trajectory are recorded as c i ;

[0040] (7.4) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the self-driving vehicle planning trajectory Traj in step (6.2) ego_plan , calculate the collision features collision_features between trajectories, and the collision features of a single planned trajectory are recorded as c co , the calculation formula is as follows:

[0041] c co =∑exp(-0.1|Traj pred -Traj ego_plan | 2 )Prob pred

[0042] (7.5) Use ego_traj_features in step (7.2), traj_interact_features in step (7.3), and collision_features in step (7.4) and multiply them with the corresponding weights of the weight module to get the final score Cost_Plan. The score calculation formula for a single planning trajectory is as follows:

[0043]

[0044] where t F is the time length for future prediction and planning, t0 represents the current moment, j = [e, i, co], ω j is the output weight of the weight module, c j is the output of the feature extraction module, and t is the future prediction time.

[0045] The beneficial effect of the present invention is that the prediction and planning accuracy of the model is improved through the integrated method of multimodal trajectory prediction and planning based on candidate lanes and speed embedding, while ensuring low model reasoning delay, further increasing interpretability, and the basic framework will provide an efficient construction idea for subsequent work. At the same time, the algorithm has weights and features that are different from the manually designed weights and features of the traditional scoring module. This module uses a learnable comprehensive scoring module to help the planned trajectory be more comfortable, safe, and kinematically reasonable. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is the overall framework diagram of this method;

[0047] Figure 2 is a clustering diagram of candidate lanes. Figure 2(a) in the figure is the candidate lane before clustering. Figure 2 (b) in the figure is the candidate lane after clustering. Figure 2 (c) in the figure is the hierarchical clustering process;

[0048] Figure 3 This is a statistical graph of the inference time consumption of this method under different numbers of instances;

[0049] Figure 4 This is a schematic diagram of the results of applying this method to a real port dataset. Figure 4 (a) Figure 4 (b) in the figure are schematic diagrams of outputting the predicted trajectory of the target obstacle in a complex intersection scenario (the blue vehicle is the target obstacle);

[0050] Figure 5 This is a schematic diagram of the results of applying this method to a real port dataset. Figure 5 (a) Figure 5 (b) in the figure are schematic diagrams of outputting the planned trajectory of the autonomous driving car in complex intersection scenarios and straight-following scenarios (the red vehicle is the autonomous driving car). DETAILED DESCRIPTION

[0051] The core technology of the present invention is to sample candidate lanes based on the information perceived around the vehicle and map information, and obtain an accurate multimodal prediction trajectory of the target obstacle and a comfortable, safe and kinematically reasonable multimodal planning trajectory of the autonomous driving vehicle (self-vehicle) by speed embedding.

[0052] The present invention proposes an integrated method for multimodal trajectory prediction and planning based on candidate lanes and speed embedding. The method obtains the multimodal prediction trajectory of the target obstacles around the self-driving vehicle and the multimodal planning trajectory of the self-driving vehicle based on the historical information of the target obstacle, the historical information of the autonomous driving vehicle (self-driving vehicle) and the high-precision map output. The candidate lane will be used as a priori information to assist in trajectory generation. First, all candidate lanes are obtained according to the position and orientation of the target obstacle or the self-driving vehicle, and then hierarchical clustering is used in combination with the KD-Tree data structure for efficient filtering to obtain fewer candidate lanes with less redundancy. According to the three longitudinal behavior modes (acceleration, uniform speed and deceleration), the candidate lanes are feature expanded and then sent to the trajectory regression module. The model will learn the trajectories of three different longitudinal behaviors of each candidate lane based on these three features, and finally sent to the trajectory scoring module to output the confidence of the multimodal trajectory. The final Cost scoring module will comprehensively score the multimodal prediction trajectory of the target obstacle output by the model and the multimodal planning trajectory of the self-driving vehicle, taking into account the kinematic characteristics, interaction characteristics and collision characteristics of the self-driving vehicle planning path.

[0053] See also Figure 1 , the present invention proposes a multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding, comprising the following steps:

[0054] (1) Based on the location of the target obstacle and the high-precision map, sample the candidate lanes that it may go to in the future, then perform hierarchical clustering on the candidate lanes and filter out redundant candidate lanes; this includes the following sub-steps:

[0055] (1.1) According to the current position of the target obstacle vehicle, call the map API interface to obtain all the original lane information (including ordinary roads and forked intersections) within a certain range around the target obstacle vehicle map_paths (raw) .

[0056] Specifically, the maximum range of the API interface to search for road objects is 100m, and the virtual lanes are filtered.

[0057] (1.2) Using the map_paths obtained in step (1.1) (raw) , use the depth-first algorithm to search from near to far based on the vehicle's current position and orientation to obtain candidate lanes map_paths (ref_all) , and then perform equally spaced sequence point sampling to obtain all candidate lane point sequences path_cords (ref_all) .

[0058] Specifically, using the map_paths obtained in step (1.1) (raw) Use the depth first search algorithm (DFS) to search for possible lanes in the future layer by layer along the current direction of the vehicle and based on the topological structure of the lanes until the cumulative search distance of the candidate lanes reaches 60m, and get map_paths (ref_all) At this time, the candidate lane information is still the data structure provided by the map API, and the coordinate point information is not involved. Then, the searched candidate lanes (candidate lanes with inclusion relationships) are initially filtered and then the coordinate point sampling is performed to obtain the coordinate point sequence of each candidate lane. For ordinary lanes in the candidate lanes, the lane centerline is directly sampled at equal intervals; for forked intersections, a curve is regressed using Bezier control points based on the entry and exit lanes of the intersection, and then equal-interval sampling is performed to finally obtain path_cords (ref_all) .

[0059] (1.3) Using all candidate lane point sequences path_cords obtained in step (1.2) (ref_all), use the KD-Tree data structure to record each candidate lane point sequence, and then use the point-to-line projection to calculate the distance between each two candidate lane point sequences to construct the distance matrix M path .

[0060] Specifically, define the candidate lane trajectory point sequence traj (i) and candidate lane trajectory point sequence traj (j) The distance between dist(traj (i) ,traj (j) )for:

[0061]

[0062] in, For traj (i) Proj is the projection operation from point to line segment, and the KD-Tree data structure is used to find the shortest distance between points.

[0063] In this way, the distances between all candidate lane trajectory point sequences can be obtained, thereby defining the distance matrix RDM (Refpath Dist Matrix):

[0064] RDM[i,j]=dist(traj (i) ,traj (j) )

[0065] (1.4) Using the distance matrix M obtained in step (1.3) path And all candidate lane point sequences path_cords obtained in step (1.2) (ref_all) , use hierarchical clustering to filter candidate lanes with high similarity to obtain the final candidate lane point sequence path_cords of the target obstacle (ref) , and then extract the direction vector and splice it to get the candidate lane feature candidate_refpath of the target obstacle.

[0066] Specifically, hierarchical clustering uses the average-linkage algorithm, and the distance formula between clusters is:

[0067]

[0068] Among them, C1 and C2 are clusters, which represent the set of candidate lane trajectory point sequences, dist(traj (i) ,traj (j) ) is obtained from the RDM in step (1.3).

[0069] After clustering, 20 fixed and equally spaced points are sampled for the candidate lanes. At the same time, 20 direction vectors are obtained by subtracting the points to form the feature information candidate_refpath of the candidate lane, with a dimension of [agent_num, refpath_num, 40, 2].

[0070] (2) The historical 2-second information of the target obstacle, the information of other surrounding intelligent agents, the element information of the surrounding high-precision map, and the candidate lane information obtained in step (1) are input into the feature fusion module and the candidate lane scoring module to obtain the score of the candidate lane, which represents the possibility of the target obstacle moving to the lane in the future; specifically, the following sub-steps are included:

[0071] (2.1) Sample the historical 2-second features of the target obstacle and other agents around it, agent_feats, with a dimension of [all_n, 20, 13], where agent_feats contains the following features: coordinates xy, speed magnitude v, speed direction v_yaw, vehicle width w, vehicle length l, vehicle type type, and the sampling frequency is 10Hz (i.e., a total of 20 frames are sampled). If the number of historical feature frames of the obstacle is less than 20 frames, the flag variable agent_mask is used to mark the validity of different frames.

[0072] (2.2) Sample the map element features map_feats around the target obstacle, with a dimension of [map_element_num, 20, 5], where map_feats contains the following features: coordinate xy, direction vector d_xy and road type type. The interval between sampling points is 10m, and the coordinate system is its own coordinate system. A maximum of 20 points are sampled for each ordinary road or intersection. If there are less than 20 points, the flag variable map_mask is used to mark the validity of the sampling points.

[0073] The coordinate transformation method for agent_feats and map_feats is similar to that in (Zhang L, Li P, Liu S, et al. SIMPL: A Simple and Efficient Multi-agent Motion Prediction Baseline for Autonomous Driving [J]. arXiv, 2024.), that is, the instance-centric encoding method is adopted. For the agent, the coordinates and orientation of the observation point are used as the new coordinate system; for map elements, the centroid and orientation of all sampling points are used as the new coordinate system.

[0074] (2.3) The number of target obstacles is denoted as N tar , the number of other agents around is Nsur , the number of map elements is recorded as N map , the total number of instances is recorded as N instance , then N instance =N tar +N sur +N map . Calculate the relative position information RPE between the target obstacle, other surrounding agents, and all map elements, including the relative orientation angle γ i→j , relative heading angle β i→j and relative distance |dist i→j |, T is the transposition operation, and the dimension of RPE is [N instance ,N instance ,5], an element in RPE can be expressed as:

[0075]

[0076] (2.4) Send agent_feats, map_feats, agent_mask, map_mask and RPE to the Symmetric Fusion Transformer feature fusion module (Zhang L, Li P, Liu S, et al. SIMPL: A Simple and Efficient Multi-agent Motion Prediction Baseline for Autonomous Driving [J]. arXiv, 2024.), which uses the Attention mechanism to fuse the multi-modal vector encoding features. The agent_feats after the fusion feature is obtained and the candidate_refpath obtained in step (1) are sent to the candidate lane scoring module Refpath Prob Layer to obtain the score Prob for the candidate lane. refpath ,This module scores the candidate lanes to obtain the confidence (lateral behavior information) of different future target lanes.

[0077] (3) Embed the candidate lanes obtained in step (1) to obtain candidate lane information Refpath containing different longitudinal behaviors vel , the dimension of candidate lanes changes from [agent_num, refpath_num, 40, 2] of candidate_refpath to [agent_num, 3*refpath_num, 40*2+D vel_emb ], representing the longitudinal behaviors of acceleration, uniform speed and deceleration respectively. It includes the following sub-steps:

[0078] (3.1) Predefine the learnable velocity vector parameter V embed , which includes three longitudinal velocity behaviors, namely acceleration V acc , uniform speed V cons and deceleration V dec ;

[0079] (3.2) Use the target obstacle candidate lane feature candidate_refpath obtained in step (1.4) and the three speed behavior embedding vectors in step (3.1) to accelerate V acc , uniform speed V cons and deceleration V dec Perform dimension splicing to obtain candidate lane information refpath containing different longitudinal behaviors vel .

[0080] (4) The candidate lane information containing different longitudinal behaviors is sent to the trajectory regression module to obtain the predicted trajectory for the next 5 seconds; specifically, the following sub-steps are included:

[0081] (4.1)Use refpath vel The agent_feats after feature fusion is sent to the trajectory regression module TrajRegressor, which outputs the Bessel control point CP of the target obstacle's trajectory in the next 5 seconds bez , its dimension is [agent_num, 3*refpath_num, (n_order+1)*2], where n_order is the order of the Bezier curve. According to the definition of the Bezier curve, the number of control points of the Bezier curve is its order plus one, that is, n_order+1. The function of the trajectory regression module is to use the abstract agent_feats feature as input and obtain specific two-dimensional predicted trajectory points after deep neural network calculation.

[0082] (4.2) Using the Bezier control point CP of the target obstacle's trajectory in the next 5 seconds obtained in step (4.1) bez , and the constant matrix is ​​multiplied and decoded to obtain the trajectory P(t) for the next 5 seconds. The formula is:

[0083]

[0084] Among them, P i is the Bessel control point, t is the time, is the ith nth order Bernstein polynomial, and the formula is:

[0085]

[0086] in, is the number of combinations.

[0087] The output frequency of the trajectory regression module is 10 Hz, that is, the predicted trajectory of 5 seconds corresponds to 50 trajectory points, denoted as Traj pred , the dimension is [agent_num,3*refpath_num,50,2].

[0088] (5) Agent_feats and predicted trajectory Traj after feature fusion pred Send it to the trajectory scoring module TrajProb Layer to obtain the multimodal predicted trajectory Traj of the target obstacle pred The confidence level Prob pred , thus reflecting the uncertainty of the target obstacle’s intention, and the dimension is [agent_num,3*refpath_num].

[0089] (6) The steps of trajectory planning for an autonomous vehicle (ego vehicle) are the same as steps (1) to (5) for obstacle trajectory prediction, except that the object is changed from “target obstacle” to “ego vehicle”. Secondly, the candidate lanes of the ego vehicle will be further filtered based on its own intention, i.e., the map_paths obtained in step (1.2) (ref_all) The vehicle will be filtered according to its actual future intention, and only the adjacent candidate lanes that match the future intention will be retained; that is, only the forward lane of the vehicle and its left and right lanes will be retained, and finally the planned trajectory Traj of the vehicle will be obtained. ego_plan .

[0090] (7) The predicted trajectory of the target obstacle and the planned trajectory of the ego vehicle are sent to a learnable cost scoring module for comprehensive scoring of the planned trajectory. This includes the following sub-steps:

[0091] (7.1) The target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred , the ego-vehicle planning trajectory Traj in step (6.2) ego_plan The agent_feats obtained after the feature fusion in step (2.4) is sent to the Cost scoring module. The Cost module consists of two parts, namely the feature extraction module Feature Extractor and the learnable weight assignment module Weight Decoder. The feature extraction module first extracts complex kinematic features, interaction features and collision features, and the weight assignment module assigns weights to the features to obtain the final comprehensive score of the planned trajectory.

[0092] (7.2) First, feature extraction is performed, using the CP in step (4.1) bezCombined with the differentiable property of Bezier control points, the formula is:

[0093]

[0094] in, is the first-order derivative of the Bessel function.

[0095] Find the kinematic parameters of the planned trajectory of the ego vehicle ego_traj_features (the features of a single planned trajectory are denoted by c e ), contains the following features: speed Vec ego , Acceleration Acc ego , Jerk ego and the lateral acceleration a y , the formula is:

[0096]

[0097] Among them, v is the speed, R is the turning radius, and κ is the turning curvature.

[0098] (7.3) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the self-driving vehicle planning trajectory Traj in step (6.2) ego_plan Calculate the multimodal interaction features traj_interact_features between trajectories (the interaction features of a single planning trajectory are recorded as c i ), contains the following features relative angle relative_yaw, relative speed relative_velocity and relative position relative_pos, and then uses the probability Prob of the predicted trajectory pred Weighted.

[0099] (7.4) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the self-driving vehicle planning trajectory Traj in step (6.2) ego_plan , calculate the collision features collision_features between trajectories (the collision features of a single planning trajectory are recorded as c co ), the calculation formula is as follows:

[0100] c co =∑exp(-0.1|Traj pred -Traj ego_plan | 2 )Probpred

[0101] (7.5) Using ego_traj_features in step (7.2), traj_interact_features in step (7.3), and collision_features in step (7.4), multiply them with the corresponding weights of the weight module WeightDecoder to obtain the final score Cost_Plan. The calculation formula for a single planning trajectory is as follows:

[0102]

[0103] where t F is the time length for future prediction and planning, t0 represents the current moment, j = [e, i, co], ω j is the output weight of the weight module, c j is the output of the feature extraction module, and t is the future prediction time. Here, t F =5.0. During training, this module uses the maximum entropy IRL method for inverse reinforcement learning.

[0104] Implementation Examples

[0105] The implementation example of the present invention is implemented on a server equipped with 7 Nvidia 1080Ti graphics cards (video memory 11G), and tested on the Feibu Port dataset, where the schematic diagram of candidate lane clustering is shown in FIG. Figure 2 As shown, Figure 2 (a) in the figure is the candidate lane before clustering. Figure 2 (b) in the figure is the candidate lane after clustering. Figure 2 (c) in the figure is the hierarchical clustering process. After experimental testing, the prediction accuracy of each item is shown in Table 1, and the ablation experiment is shown in Table 2, which shows the effectiveness of this method and the necessity of the speed embedding module, planning module and candidate lane scoring module; the planning indicators are shown in Table 3, and the inference speed is shown in Table 4. Figure 3 The test on the port dataset is shown in Figure 4 (a) Figure 4 (b) and Figure 5 (a) Figure 5 As shown in (b), the red vehicle is the self-driving car (ego car) and the blue vehicle is the target obstacle.

[0106] In Tables 1 and 2, Ours is the method of the present invention, TNT is the model structure adopted by (Zhao H, Gao J, Lan T, et al. TNT: Target-driveN Trajectory Prediction[J]..), and SIMPL is the model structure adopted by (Zhang L, Li P, Liu S, et al. SIMPL: A Simple and Efficient Multi-agent Motion Prediction Baseline for Autonomous Driving[J]. arXiv, 2024.). The meanings of the evaluation indicators in the table are as follows: minADE (min Average Displacement Error) is the minimum average displacement error, min FDE (min Final Error) is the minimum final position displacement error, the final position in this article is the position at the 5th second in the future, minAHE (min Average Heading Error) is the minimum average heading error, min FHE (min Final Heading Error) is the minimum final position heading error, MR (Missing Rate) is the missed detection rate, where dis=2 means that if the final predicted position deviates from the true position by more than 2m, it is recorded as a missed detection, k=i means taking the first i predicted trajectories with the highest scores, param is the parameter quantity, W / o vel embedding (without velocity embedding) is the model after removing the velocity embedding module, W / o plan module (without planning module) is the model after removing the ego-vehicle planning module, and W / o refpath prob layer is the model after removing the candidate lane scoring module.

[0107] In Table 3, min_RMS_Jerk is the root mean square of the minimum jerk, min_Curvature is the minimum mean curvature, min_Lat_Acc is the minimum lateral acceleration, Risk_Rate is the collision risk index, k=i is the first i trajectories with the highest scores, and W / o Cost represents the structure after removing the Cost module of this article.

[0108] Table 1

[0109]

[0110] Table 2

[0111]

[0112] Table 3

[0113]

Claims

1. A multi-modal trajectory prediction and planning integrated method based on candidate lanes and speed embedding, characterized in that: The steps include: (1) Based on the location of the target obstacle and the high-precision map, sample the candidate lanes that it may go to in the future, then perform hierarchical clustering on the candidate lanes to filter out redundant candidate lanes; (2) The historical information of the target obstacle, the historical information of other surrounding intelligent agents, the element information of the surrounding high-precision map, and the candidate lane information obtained in step (1) are input into the feature fusion module and the candidate lane scoring module to obtain the score of the candidate lane, which represents the possibility of the target obstacle moving to the lane in the future; (3) Embed the candidate lanes obtained in step (1) with their speeds to obtain candidate lane information containing different longitudinal behaviors; (4) The candidate lane information containing different longitudinal behaviors obtained in step (3) is sent to the trajectory regression module to obtain the predicted trajectory of the target obstacle; (5) Sending the predicted trajectory obtained in step (4) to the trajectory scoring module to obtain the confidence of the multimodal predicted trajectory of the target obstacle; (6) Changing the object from the target obstacle to the autonomous vehicle, further filtering the candidate lanes of the autonomous vehicle according to its own intention based on step (1); executing steps (1) to (5) to obtain the planned trajectory of the autonomous vehicle; (7) The predicted trajectory of the target obstacle and the planned trajectory of the autonomous vehicle are sent to a learnable cost scoring module for comprehensive scoring of the planned trajectory.

2. The method for integrated multimodal trajectory prediction and planning based on candidate lane and speed embedding according to claim 1, characterized in that: The step (1) comprises the following sub-steps: (1.1) According to the current position of the target obstacle vehicle, call the map API interface to obtain all the original lane information map_paths within a certain range around the target obstacle vehicle (raw) ; (1.2) Using the map_paths obtained in step (1.1) (raw) , use the depth-first algorithm to search from near to far based on the vehicle's current position and orientation to obtain candidate lanes map_paths (ref_all) , and then perform equally spaced sequence point sampling to obtain all candidate lane point sequences path_cords (ref_all) ; (1.3) Using all candidate lane point sequences path_cords obtained in step (1.2) (ref_all) , use the KD-Tree data structure to record each candidate lane point sequence, and then use the point-to-line projection to calculate the distance between each two candidate lane point sequences to construct the distance matrix M path ; (1.4) Using the distance matrix M obtained in step (1.3) path And all candidate lane point sequences path_cords obtained in step (1.2) (ref_all) , use hierarchical clustering to filter candidate lanes with high similarity to obtain the final candidate lane point sequence path_cords of the target obstacle (ref) , and then extract the direction vector and splice it to get the candidate lane feature candidate_refpath of the target obstacle.

3. The integrated multi-modal trajectory prediction and planning method based on candidate lane and speed embedding according to claim 2 is characterized in that: The step (2) comprises the following sub-steps: (2.1) Sample the features agent_feats of the target obstacle and other agents around it for 2 seconds, where agent_feats contains the following features: coordinates xy, speed magnitude v, speed direction v_yaw, vehicle width w, vehicle length l and vehicle type type, the sampling frequency is 10Hz, and the coordinate system is the own coordinate system; (2.2) Sample the map element features map_feats around the target obstacle, where map_feats contains the following features: coordinate xy, direction vector d_xy and lane type type. The interval of the sampling points is 10m, the coordinate system is the own coordinate system, and a maximum of 20 points are sampled for each ordinary road or intersection; (2.3) The number of target obstacles is denoted as N tar , the number of other agents around is recorded as N sur , the number of map elements is recorded as N map , the total number of instances is recorded as N instance , then N instance =N tar +N sur +N map , calculate the relative position information RPE between the target obstacle, other surrounding agents, and all map elements, including the relative orientation angle Y i →j, relative heading angle β i→j and relative distance |dist i→j |, the dimension of RPE is [N instance , N instance , 5]; (2.4) The agent_feats obtained in step (2.1), the map_feats obtained in step (2.2), and the RPE obtained in step (2.3) are sent to the feature fusion module, and then the fused agent_feats features and the target obstacle candidate lane feature candidate_refpath obtained in step (1.4) are sent to the candidate lane scoring module to obtain the score Prob for the candidate lane. refpath .

4. The integrated multi-modal trajectory prediction and planning method based on candidate lane and speed embedding according to claim 3 is characterized in that: The step (3) comprises the following sub-steps: (3.1) Predefine the learnable velocity vector parameter V embed , which includes three longitudinal velocity behaviors, namely acceleration V acc , uniform speed V cons and deceleration V dec ; (3.2) Use the target obstacle candidate lane feature candidate_refpath obtained in step (1.4) and the three speed behavior embedding vectors in step (3.1) to accelerate V acc , uniform speed V cons and deceleration V dec Perform dimension splicing to obtain candidate lane information refpath containing different longitudinal behaviors vel .

5. The integrated multi-modal trajectory prediction and planning method based on candidate lane and speed embedding according to claim 4 is characterized in that: The step (4) comprises the following sub-steps: (4.1) Using the candidate lane information refpath containing different longitudinal behaviors of the target obstacle obtained in step (3.2) vel The agent_feats obtained after the feature fusion in step (2.4) is sent to the trajectory regression module to output the Bezier control point CP of the target obstacle's trajectory in the next 5 seconds bez ; (4.2) Using the Bezier control point CP of the target obstacle's trajectory in the next 5 seconds obtained in step (4.1) bez Multiply it with the constant matrix to get the trajectory Traj for the next 5 seconds pred , the output frequency is 10Hz.

6. The integrated multi-modal trajectory prediction and planning method based on candidate lane and speed embedding according to claim 5 is characterized in that: The step (5) is specifically as follows: The agent_feats obtained after the feature fusion in step (2.4) and the predicted trajectory Traj in step (4.2) pred Send it to the trajectory scoring module to obtain the multimodal predicted trajectory Traj of the target obstacle pred The confidence level Prob pred .

7. The method for integrated multi-modal trajectory prediction and planning based on candidate lane and speed embedding according to claim 6, characterized in that: The step (6) comprises the following sub-steps: (6.1) Change the object from the target obstacle to the self-driving car, based on the map_paths obtained in step (1.2) (ref _all) Filter based on the actual future intention of the vehicle and only keep adjacent candidate lanes that match the future intention; (6.2) Execute steps (1) to (5) to obtain the planned trajectory Traje of the autonomous vehicle go_plan .

8. The method for integrated multi-modal trajectory prediction and planning based on candidate lane and speed embedding according to claim 7, characterized in that: The step (7) comprises the following sub-steps: (7.1) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5) pred , the planned trajectory Traj of the autonomous vehicle in step (6.2) ego_plan The agent_feats obtained after the feature fusion in step (2.4) is sent to the Cost scoring module; (7.2) First, feature extraction is performed using the CP in step (4.1) bez Combined with the differentiable property of Bessel control points, the kinematic parameters ego_traj_features of the planned trajectory of the autonomous driving vehicle are obtained. The features of a single planned trajectory are recorded as c e ; Contains the following features: speed Vec ego , Acceleration Acc ego , Jerk ego and the lateral acceleration a y ; (7.3) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the autonomous vehicle planning trajectory Traje in step (6.2) go_plan Calculate the multimodal interaction features traj_interact_features between trajectories. The interaction features of a single planned trajectory are recorded as c i ; (7.4) Use the target obstacle prediction trajectory Traj obtained in step (4.2) pred , the target obstacle prediction trajectory confidence Prob obtained in step (5.1) pred and the self-driving vehicle planning trajectory Traj in step (6.2) ego_plan , calculate the collision features collision_features between trajectories, and the collision features of a single planned trajectory are recorded as c co , the calculation formula is as follows: c co =∑exp(-0.1|Through pred -Through ego_plan | 2 )Probably pred (7.5) Use ego_traj_features in step (7.2), traj_interact_features in step (7.3), and collision_features in step (7.4) and multiply them with the corresponding weights of the weight module to get the final score Cost_Plan. The score calculation formula for a single planning trajectory is as follows: where t F is the time length for future prediction and planning, t0 represents the current moment, j = [e, i, co], ω j is the output weight of the weight module, c j is the output of the feature extraction module, and t is the future prediction time.

Images