Target vehicle trajectory prediction method based on double-layer lane intention inference

Through the extended Kalman filtering and path similarity scoring model, the short-term vehicle trajectory prediction combined with interactive multi-model and five-degree polynomial curve method is solved, and the problem of insufficient adaptability of vehicle trajectory prediction in the prior art is achieved, achieving more accurate and flexible Trajectory prediction, suitable for complex traffic scenarios.

CN119068693AActive Publication Date: 2024-12-03BEIHANG UNIV
View PDF 7 Cites 0 Cited by

Patent Information

Application Number
CN202411205254.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-30
Publication Date
2024-12-03
Estimated Expiration
2044-08-30

AI Technical Summary

Technical Problem

The real-time effect adaptability of vehicle trajectory prediction in the prior art in real traffic environment is limited, and it is difficult to effectively deal with complex and changeable actual traffic scenarios.

Method used

The short-term prediction trajectory of the target vehicle is obtained based on the extended Kalman filtering and path similarity scoring model, and the lane selection intention prediction model is constructed in combination with interactive multi-models, and the prediction trajectory is generated using the five-degree polynomial curve method.

Benefits of technology

It improves the accuracy and adaptability of vehicle trajectory prediction, can better handle the variability and uncertainty of vehicle behavior, and the generated trajectory is smooth, conforms to the actual vehicle driving characteristics, and adapts to different driving and road conditions.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119068693A_ABST
    Figure CN119068693A_ABST
Patent Text Reader

Abstract

The invention relates to the technical field of driverless cars in a structured road intersection area, in particular to a target vehicle trajectory prediction method based on double-layer lane intention inference, which comprises the following steps: acquiring a short-term prediction trajectory of a target vehicle based on extended Kalman filtering; analyzing the short-term prediction trajectory based on a path similarity scoring model, and obtaining a road prediction value selected when the target vehicle enters the intersection; based on the lateral deviation of the target vehicle, constructing a lane selection intention prediction model based on an interactive multi-model, and obtaining a lane prediction value by the lane selection intention prediction model based on the road prediction value; based on the lane prediction value, generating a prediction track of the target vehicle by adopting a quintic polynomial curve method; according to the invention, high-precision prediction can be carried out on driving tracks of other driving vehicles in an intersection scene.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of driverless vehicles in a structured road intersection area, and in particular to a target vehicle trajectory prediction method based on double-layer lane intention inference. Background Art

[0002] The realization of unmanned driving has a significant effect on improving traffic safety, improving traffic efficiency and optimizing travel experience. Among them, the accurate prediction of the driving trajectory of surrounding vehicles helps the autonomous driving system to identify potential traffic conflicts in a timely manner and effectively plan safe driving routes. It is a key link in realizing the commercialization and widespread application of unmanned driving technology, and is of great significance to accelerating the development of intelligent connected vehicles and promoting the industrial application of autonomous driving technology. The target vehicle trajectory prediction algorithm aims to predict the driving trajectory of other manned vehicles on structured roads, especially in urban intersections, and provide accurate predicted trajectory information for the traffic decision-making and obstacle avoidance technology of unmanned vehicles. Similar technologies are commonly found in vehicle trajectory prediction and vehicle driving intention prediction related technologies in scenarios such as urban road intersections, highways, and closed parks.

[0003] The Chinese invention patent application with publication number CN116933005A discloses a vehicle trajectory prediction method based on a graph convolutional neural network in a networked environment, the Chinese invention patent application with publication number CN116758727A discloses a target vehicle trajectory prediction method for a multi-lane scene, and the Chinese invention patent application with publication number CN117408406A discloses a planning-coupled multi-target vehicle trajectory prediction method and system. The similarity of the above technical solutions is that they all use deep learning methods to use the historical trajectory information of the vehicle to solve the problem of vehicle trajectory prediction in dynamic scenes. Whether it is a graph convolutional neural network, a social convolutional pooling network or a long short-term memory neural network (LSTM), it is necessary to first encode the characteristics of the vehicle's historical trajectory, the planned trajectory and the interaction relationship between vehicles. After these features are pre-processed, they are trained and learned based on a large amount of data through a neural network model. These methods are based on the learning of historical trajectory data by neural networks, resulting in certain differences between the real-time prediction effect and the experimental results in a real traffic environment. They are more suitable for specific traffic scenes or road conditions, and may have certain limitations for complex and changeable actual traffic environments. Summary of the invention

[0004] In view of the above problems, the present invention provides a target vehicle trajectory prediction method based on double-layer lane intention inference, which solves the technical problem of limited adaptability of real-time prediction effect under real traffic environment in the prior art.

[0005] The present invention provides a target vehicle trajectory prediction method based on double-layer lane intention inference, comprising the following steps:

[0006] Step S1, based on the extended Kalman filter, obtaining the short-term predicted trajectory of the target vehicle; based on the path similarity scoring model, analyzing the short-term predicted trajectory to obtain the predicted value of the road selected when the target vehicle enters the intersection;

[0007] Step S2: constructing a lane selection intention prediction model based on an interactive multi-model based on the lateral offset of the target vehicle, wherein the lane selection intention prediction model obtains a lane prediction value based on the road prediction value;

[0008] Step S3: Based on the lane prediction value, a predicted trajectory of the target vehicle is generated using a quintic polynomial curve method.

[0009] Preferably, step S1 specifically includes:

[0010] Step S1-1, searching for a set of candidate lanes that the target vehicle can reach;

[0011] Step S1-2, predicting the short-term local state quantity of the target vehicle, and predicting the short-term predicted trajectory of the target vehicle based on the extended Kalman filter;

[0012] Step S1-3, establishing a path similarity scoring model to evaluate the similarity between the short-term predicted trajectory of the target vehicle and each lane in the candidate lane set;

[0013] Step S1-4: determining the road with the highest probability selected by the target vehicle based on the similarity as the road prediction value.

[0014] Preferably, step S1-1 includes: according to the real-time status information of the target vehicle, searching for a set of candidate lanes for the target vehicle from the high-definition map by a depth-first search algorithm, wherein the set of candidate lanes includes multiple candidate lanes, representing all possible choices of lanes in other roads to which the target vehicle can drive at the intersection.

[0015] Preferably, step S1-2 includes: step S1-2-1, predicting the short-term local state of the target vehicle; step S1-2-2, predicting the short-term trajectory based on the extended Kalman filter, using the following methods: (1) establishing a process model, (2) establishing a measurement model, (3) performing prediction of the extended Kalman filter, and (4) updating the extended Kalman filter.

[0016] Preferably, step S1-3 specifically includes: calculating the similarity score between the short-term predicted trajectory of the target vehicle and each lane in the candidate lane set by:

[0017]

[0018] in, is the i-th lane and the short-term predicted trajectory The similarity score between them, k1 and k2 are the weighted coefficients of position similarity and yaw angle similarity, and They are position similarity and yaw angle similarity;

[0019] Location Similarity The calculation method is:

[0020]

[0021] in, is the mean of the Euclidean distance, α is the index of the short-term predicted trajectory point, x α ,y α is the coordinate of the short-term prediction trajectory point, x′ α , y′ α is the position coordinate of the nearest point of the short-term predicted trajectory point, m is the width of the short-term future time window, and L is the mapping threshold parameter;

[0022] Yaw angle similarity The calculation method is:

[0023]

[0024] in, is the average difference of yaw angle, α is the index of short-term predicted trajectory point, θ α is the short-term predicted trajectory point yaw angle, θ′ α It is the heading in the lane corresponding to the short-term predicted trajectory point.

[0025] Preferably, step S1-4 comprises: using a softmax function to sort the candidate lane set The similarity score Normalize to obtain the probability of the target vehicle choosing different lanes

[0026]

[0027] Among them, n3 is the total number of all candidate lanes;

[0028] By summing the probabilities of each lane in the lane group of the same road, the probability of the vehicle selecting each road is obtained, and the road with the maximum probability is used as the road prediction value.

[0029] Preferably, step S2 specifically includes:

[0030] Step S2-1, constructing a lane selection intention prediction model based on an interactive multi-model, and obtaining a lane with the highest probability within the lane prediction value as a target lane based on the lateral offset and lateral offset speed of the target vehicle;

[0031] Step S2-2: Calculate the preview distance, and obtain the anchor point of the center line of the target lane based on the preview distance as the lane prediction value.

[0032] Preferably, step S2-1 specifically includes: (1) constructing a state transition probability matrix, (2) performing interactive part calculations of the interactive multi-model, (3) performing filtering part calculations of the interactive multi-model, (4) performing lane model probability updates of the interactive multi-model, and (5) selecting a lane with the highest posterior lane model probability among the lanes inside the road prediction value as the target lane.

[0033] Preferably, step S2-2 specifically includes:

[0034] Calculate the preview distance dis:

[0035] dis=v k T predict

[0036] Where T predict is the preview time, and the centerline anchor point on the target lane is queried based on the preview distance to obtain the state of the anchor point P T (x T ,y T ,θ T ) as the lane prediction value, where x T g T ,θ T are the horizontal and vertical coordinates and yaw angle of the anchor point respectively.

[0037] Preferably, the following quintic polynomial trajectory curve interpolation formula is established:

[0038] x(t)=a5t 5 +a4t 4 +a3t 3 +a2t 2 +a1t+a0

[0039] y(t)=b5t 5 +b4t 4 +b3t 3 +b2t 2 +b1t+b0

[0040] Where t is time, a0, ..., a5, b0, ..., b5 are polynomial trajectory parameters, x(t) and y(t) are trajectory curve coordinates; the constraints of the starting point and end point of the quintic polynomial trajectory curve are:

[0041] x(0)=x k y(0)=y k

[0042] x′(0)=v k cosθ k y′(0)=v k sinθ k

[0043] x″(0)=a k cosθ k y″(0)=a k sinθ k

[0044] x(T predict )=x T y(T predict )=y T

[0045] x′(T predict )=v T cosθ T y′(T predict )=v T cosθ T

[0046] x″(T predict )=0 y″(T predict )=0

[0047] Among them, x k ,y k ,θ k , v k , a k are the position coordinates, yaw angle, longitudinal velocity and longitudinal acceleration of the target vehicle at time k, respectively. T y T ,θ T , v T , a T are the position coordinates, yaw angle, longitudinal velocity and longitudinal acceleration of the lane prediction value, respectively, and v T =v k , a T =a k , x′(·) and x″(·) are the first-order derivative and second-order derivative of the horizontal coordinate of the trajectory curve, respectively; y′(·) and y″(·) are the first-order derivative and second-order derivative of the vertical coordinate of the trajectory curve, respectively;

[0048] Based on the starting point and end point constraints, the fifth-order polynomial trajectory parameters are solved, and the interpolation result of the fifth-order polynomial trajectory curve is used as the predicted trajectory of the target vehicle.

[0049] Compared with the prior art, the present invention has at least the following beneficial effects:

[0050] (1) The present invention can more accurately predict the short-term trajectory and lane selection intention of the vehicle by combining the dynamic characteristics and real-time driving status of the target vehicle. The introduction of the prediction and path similarity scoring model based on vehicle dynamics can improve the prediction accuracy of the actual driving path of the vehicle, thereby providing more reliable data support for subsequent traffic management and autonomous driving system decision-making.

[0051] (2) The lane selection intention prediction model constructed by the present invention using the interactive multiple model method can effectively handle the variability and uncertainty of vehicle behavior. The interactive multiple model adapts to the changes in the target vehicle behavior by combining the estimates of multiple models, so that the prediction results can be more flexible to adapt to the complex situations in actual driving.

[0052] (3) The present invention utilizes the quintic polynomial curve method to generate the predicted trajectory of the target vehicle, which not only ensures the smoothness of the trajectory and reduces the unsafe factors caused by sudden changes in the predicted trajectory, but also makes the generated trajectory more consistent with the actual vehicle driving characteristics and can adapt to different driving and road conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The drawings are only for the purpose of illustrating particular embodiments and are not to be construed as limiting the invention.

[0054] Figure 1 A flow chart of a target vehicle trajectory prediction method based on double-layer lane intention inference disclosed in an embodiment of the present invention;

[0055] Figure 2 Obtain candidate lanes and road schematic diagrams for the target vehicle disclosed in the embodiment of the present invention;

[0056] Figure 3 A kinematic diagram of a target vehicle disclosed in an embodiment of the present invention;

[0057] Figure 4 A schematic diagram of a target vehicle path similarity scoring result disclosed in an embodiment of the present invention;

[0058] Figure 5 A schematic diagram of the lane selection intention prediction of a target vehicle on a road disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0059] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. In addition, the present invention can also be implemented in other ways different from those described herein, and therefore, the protection scope of the present invention is not limited by the specific embodiments disclosed below.

[0060] The present invention provides a target vehicle trajectory prediction algorithm based on double-layer lane intention inference. It mainly relies on the lane and vehicle posture information at the structured road intersection, uses the dynamics-based accurate short-term prediction trajectory as input, and infers the road selection intention of the vehicle when entering the intersection through the designed road similarity evaluation model. Subsequently, based on the interactive multi-model framework based on the multi-lane model, the specific lane selection intention of the vehicle on different roads is inferred. Finally, the quintic polynomial method is used to quickly generate the trajectory for the lane with the highest prediction probability.

[0061] In order to illustrate the effectiveness of the method proposed in the present invention, the above technical solution of the present invention is described in detail below through a specific embodiment. Figure 1 As shown, a target vehicle trajectory prediction method based on double-layer lane intention inference is disclosed, comprising the following steps:

[0062] Step S1, based on the extended Kalman filter, obtaining the short-term predicted trajectory of the target vehicle; based on the path similarity scoring model, analyzing the short-term predicted trajectory to obtain the predicted value of the road selected when the target vehicle enters the intersection;

[0063] Step S1-1, search for a set of candidate lanes that the target vehicle can reach. Figure 2 As shown in the figure, when a vehicle reaches an intersection, it will go straight, turn right or turn left to reach the corresponding road, and each road has multiple lanes. In this step, according to the real-time status information of the target vehicle, the depth-first search algorithm searches for the candidate lane set of the target vehicle from the high-definition map. The candidate lane set contains multiple candidate lanes, which represent all possible lanes that the target vehicle can choose to drive to other roads at the intersection. The set of roads is represented as follows:

[0064]

[0065] in, is the set of all roads, n1 is the total number of all roads, For the road, for The set of all lanes on the road, n2 is The total number of lanes on the road, is the set of candidate lanes, and n3 is the total number of all candidate lanes.

[0066] Then, according to the difference in traffic flow at the intersection, the candidate lane set The lanes are divided into several clear groups, corresponding to semantic actions such as left turn, right turn, and straight ahead.

[0067] Step S1-2, predicting the short-term trajectory of the target vehicle based on the kinematic model, mainly includes short-term local state quantity prediction and short-term trajectory prediction based on the extended Kalman filter.

[0068] Step S1-2-1: Prediction of short-term local state quantity. Figure 3 When the target vehicle moves, the position coordinates of the target vehicle are (x k ,y k ), the yaw angle is the angle θ rotating counterclockwise with respect to the positive direction of the x-axis k , the longitudinal velocity is v k , the longitudinal acceleration is a k , the yaw angular velocity is γ k , the yaw angular acceleration is

[0069] According to the longitudinal velocity and yaw rate of the target vehicle in the short-term historical time window, the moving average method is used to predict the longitudinal velocity and yaw rate of the target vehicle in the short-term future time window:

[0070]

[0071] in, is the longitudinal velocity and yaw angular velocity at time k+m in the future time window, is the longitudinal velocity and yaw rate at time k+1 in the future time window, m is the width of the short-term future time window, n is the width of the short-term historical time window, a and b are parameter indexes, and w a is the weight coefficient, v k-a+1 , γ k-a+1 are the longitudinal velocity and yaw angular velocity at time k-a+1 in the short-term historical time window.

[0072] In some embodiments, the short-term history time window is set to 1 second (n=10), the short-term future time window is set to 1 second (m=10), and the sampling period T is 0.1 second.

[0073] Step S1-2-2, predicting short-term trajectory based on extended Kalman filtering. Construct a state estimation model, take the longitudinal velocity and yaw rate of the short-term future time window as observation values, perform extended Kalman filtering on other state quantities, and obtain other state quantities (xk ,y k ,θ k , v k , γ k ), where based on (x k ,y k ,θ k ) can represent the short-term future trajectory of the target vehicle The extended Kalman filter includes four parts: establishing a process model, establishing a measurement model, predicting, and updating:

[0074] (1) Establish process model:

[0075] x k+1 =f(x k )+q k

[0076] =[f1,f2,f3,f4,f5]+q k

[0077] Among them, x k =(x k ,y k ,θ k , v k , γ k ) is the state vector of the target vehicle, q k ~N(0,Q k ) is the process noise, which has a mean of zero and a power spectral density of the process noise matrix Q k The normal distribution of k =diag{0,0,q dθ ,q dv ,q dγ}, q dθ represents the noise intensity of the yaw angle process, q dv represents the longitudinal velocity process noise intensity, q dγ Indicates the noise intensity of the yaw rate process.

[0078] f(x k ) is the state transfer equation based on vehicle kinematics and is calculated as:

[0079] f1:x k+1 =x k +(v k cosθ k )T

[0080] f2:y k+1 =y k +(v k sinθ k )T

[0081] f3:θk+1 =θ k +γ k T

[0082] f4:v k+1 =v k

[0083] f5: γ k+1 =γ k

[0084] Where T is the sampling period.

[0085] (2) Establishing the measurement model:

[0086]

[0087] Where H is the measurement matrix, r k ~N(0,R k ) is the measurement noise, which has a mean of zero and a noise covariance matrix of R k The normal distribution, R k =diag{r v , r γ}, r v represents the longitudinal velocity measurement noise intensity, r γ Indicates the yaw rate measurement noise intensity.

[0088] The measurement model takes into account that the estimates of velocity and yaw rate become increasingly inaccurate over time, and therefore assumes that the variance of velocity and yaw rate also increases linearly.

[0089] (3) Perform the prediction step f of the extended Kalman filter ekf,predict , the calculation process is:

[0090]

[0091] in, is the prior estimated state at time k, is the posterior estimated state at time k-1, f(·) is the state transfer equation, is the prior covariance matrix at time k, is the posterior covariance matrix at time k-1, is the Jacobian matrix at time k-1, Q k-1 is the process noise matrix at time k-1.

[0092] (4) Perform the update step f of the extended Kalman filter ekf,update , the calculation process is:

[0093]

[0094] Among them, K k is the Kalman gain at time k.

[0095] Repeat the update steps of the extended Kalman filter to finally obtain the short-term prediction trajectory

[0096] Step S1-3, establish a path similarity scoring model. In order to determine the semantic behavior of the predicted target vehicle, such as left turn, right turn and straight, the present invention infers the candidate lane set according to the lane similarity scoring model Similarity scores for all lanes in :

[0097]

[0098] in, is the i-th lane and the short-term predicted trajectory The similarity score between them, k1 and k2 are the weighting coefficients of position similarity and yaw angle similarity, k1+k2=1; and They are position similarity and yaw angle similarity respectively.

[0099] is the position similarity between the path and the short-term prediction trajectory, by calculating m short-term prediction trajectories The mean Euclidean distance between the point on the lane and the nearest point on the lane is calculated, and the result is mapped from [0, L] and [L, ∞] to [0, 1] to obtain the position similarity, which is calculated as follows:

[0100]

[0101] in, is the mean of the Euclidean distance, α is the index of the short-term predicted trajectory point, x α ,y α is the coordinate of the short-term prediction trajectory point, x′ α , y′ α is the position coordinate of the nearest point of the short-term predicted trajectory point, m is the width of the short-term future time window, and L is the mapping threshold parameter.

[0102] is the yaw similarity between the path and the short-term predicted trajectory, by calculating m short-term predicted trajectories The average difference between the points on the lane and the heading in the lane, and the result is mapped from [0, 2π) to [0, 1] to obtain the position similarity, which is calculated as:

[0103]

[0104] in, is the average difference in yaw angle, α is the index of the short-term predicted trajectory point, θα is the yaw angle of the short-term predicted trajectory point, and θ′α is the heading in the lane corresponding to the short-term predicted trajectory point.

[0105] Step S1-4: Classify the candidate lane set into different roads and calculate the road with the highest probability. Use the softmax function to classify the candidate lane set into different roads and calculate the road with the highest probability. The similarity score Normalize to obtain the probability of the target vehicle choosing different lanes

[0106]

[0107] Finally, by summing the probabilities of each lane in the lane group of the same road, the probability of the vehicle selecting each road is obtained, and the road with the maximum probability is used as the road prediction value.

[0108] In the above manner, the scheme of the present invention searches and determines the set of candidate lanes that the target vehicle can reach, which provides a possible range of choices for subsequent predictions. The short-term trajectory of the target vehicle is predicted based on vehicle dynamics. This process takes into account the current state and possible actions of the vehicle to predict its movement trend in the short term. A path similarity scoring model is established to evaluate the degree of match between each lane and the predicted trajectory of the target vehicle by calculating the similarity scores of all candidate lanes. Finally, the candidate lane set is classified into different roads and the road with the highest probability is calculated to determine the road that the target vehicle is most likely to choose. This provides a basis for understanding the general driving intention of the target vehicle.

[0109] Step S2: constructing a lane selection intention prediction model based on an interactive multi-model based on the lateral offset of the target vehicle, wherein the lane selection intention prediction model obtains a lane prediction value based on the road prediction value;

[0110] Step S2-1, considering the lateral offset and lateral offset speed of the target vehicle, construct a lane selection intention prediction model based on IMM (interactive multiple models). According to the results of the previous steps, the intention of the target vehicle to select different roads can be determined, but there are usually multiple lanes in the road with different probabilities. Therefore, it is necessary to infer the lane intention of the target vehicle more carefully. The present invention selects the lateral offset l of the target vehicle from the center line of the lane at time k k and lateral speed As the state input, an interactive multi-model framework is constructed based on the target multi-lane model, which mainly includes the following steps: constructing the state transition probability matrix (TPM), the interaction part of IMM, the filtering part of IMM, and the lane model probability update of IMM.

[0111] (1) Construct the state transition probability matrix (TPM):

[0112] Based on the Markov assumption, lane intention inference within the road is conceptualized as a lane change prediction problem. By considering the real-time measured lateral deviation velocity To derive the time-varying transition probability matrix (TPM):

[0113]

[0114] Wherein, the subscripts i and j represent the lane numbers. n′ is the total number of lanes, and p ij represents the probability of lane i at time k-1 switching to lane j at time k, which is constructed as Related time-varying functions:

[0115]

[0116] in, The probability density function is The prior transition probability calculated. The prior transition probability of each row of the prior TPM Normalization is required. is the cumulative value of the probability density function of lane i switching to lane j. In the present invention, left lane change, following lane change and right lane change are all modeled as normal distributions:

[0117]

[0118] Among them, ρ ij and are the mean and covariance of the lateral offset velocity of the target vehicle switching from lane i to lane j.

[0119] (2) Interaction part of IMM:

[0120] In the interactive part of the IMM algorithm, the posterior lane model probability based on the k-1 moment Lane lateral deviation and the covariance of the probability model for choosing lane j The mixing probability of lane i to lane j at the current kth moment Prior Lane Model Probability Mixed state of lateral deviation of j lane and the mixed covariance of the lateral offset Perform update calculations:

[0121]

[0122] in, and are the posterior lane model probability, lane lateral offset and covariance of the j-lane probability model obtained from the k-1 moment, is the mixing probability, is the prior lane model probability, is the mixed state of the lateral offset of lane j, is the mixed covariance of the lateral offset of lane j, It is the mixed state of the lateral offset of lane i.

[0123] (3) Filtering part of IMM:

[0124] Then, for each lane model All perform standard linear Kalman filtering, and the calculation process is:

[0125]

[0126] Among them, KF prediction (·) and KF update (·) are the prediction and update steps of the standard Kalman filter, A is the transfer matrix, Q is the process noise matrix, is the mixed covariance of the lateral offset of lane i, and are the prior and posterior lane lateral offsets at time k, is the mean lateral offset of existing lanes. and are the prior and posterior covariance of lane i, respectively.

[0127] (4) IMM lane model probability update:

[0128] Finally, according to the measured residual of the filter of the i-th lane model at time k and covariance Update the posterior lane model probability The calculation method is:

[0129]

[0130] in, is the likelihood of the filter measurement for the i-th lane model, is the measured residual of the filter of the i-th lane model at time k, is the covariance of the filter of the i-th lane model at time k, l k is the lateral displacement of the target vehicle from the lane centerline at time k, and R is the noise matrix.

[0131] Finally, the lane with the highest posterior lane model probability is selected as the target lane from the lanes within the road prediction value in step S1.

[0132] Step S2-2, calculate the preview distance, and obtain the anchor point of the maximum probability lane centerline based on the assumption that the target vehicle is moving at a constant speed. Select the lane with the highest posterior lane model probability from the lanes within the road prediction value in step S1 as the target lane, assuming that the longitudinal speed v k Constant, calculate the preview distance dis according to the formula:

[0133] dis=v k T predict

[0134] Where T predict is the preview time. Then, use this distance as input to query the centerline anchor point on the target lane and obtain the state P of the anchor point T (x T y T ,θ T ), where x T , g T ,θ T are the horizontal and vertical coordinates and yaw angle of the anchor point respectively.

[0135] Through the above-mentioned method, after determining the road that the target vehicle may choose, the present invention further refines it to the lane level and predicts the specific lane selection intention of the vehicle on the road. Taking into account the lateral deviation and lateral deviation speed of the target vehicle, a lane selection intention prediction model based on an interactive multi-model (IMM) is constructed. The IMM model can handle the diversity and uncertainty of vehicle behavior and improve the accuracy of the prediction. By obtaining the anchor point of the most approximate lane centerline based on the assumption that the target vehicle is traveling at a constant speed, the lane that the target vehicle is most likely to choose is further determined.

[0136] Step S3, based on the fifth-order polynomial curve method, the predicted trajectory with the highest probability of the target vehicle is generated. The real-time state of the target vehicle at time k is used as the initial state start k (x k ,y k ,θ k , v k , a k ), the target point of the target vehicle on the reference path, that is, the state of the target lane anchor point with the highest probability obtained in the previous step, is taken as the end state target (x T , g T ,θ T , v T =v k , a T =a k ), establish the following quintic polynomial trajectory curve interpolation method:

[0137] x(t)=a5t 5 +a4t4 +a3t 3 +a2t 2 +a1t+a0

[0138] y(t)=b5t 5 +b4t 4 +b3t 3 +b2t 2 +b1t+b0

[0139] Where t is time, a0, ..., a5, b0, ..., b5 are polynomial coefficients, and x(t) and y(t) are trajectory curve coordinates. The constraints of the starting point and end point of the quintic polynomial trajectory curve are:

[0140] x(0)=x k y(0)=y k

[0141] x′(0)=v k cosθ k y′(0)=v k sinθ k

[0142] x″(0)=a k cosθ k y″(0)=a k sinθ k

[0143] x(T predict )=x T y(T predict )=y T

[0144] x′(T predict )=v T cosθ T y′(T predict )=v T cosθ T

[0145] x″(T predict )=0 y″(T predict )=0

[0146] Among them, x′(·) and x″(·) are the first-order derivative and second-order derivative of the horizontal coordinate of the trajectory curve, respectively, and y′(·) and y″(·) are the first-order derivative and second-order derivative of the vertical coordinate of the trajectory curve, respectively.

[0147] Thus, the parameters of the quintic polynomial trajectory are solved and the result is used as the target vehicle trajectory prediction.

[0148] Through the above method, the present invention uses the fifth-order polynomial curve method to generate the predicted trajectory with the highest probability of the target vehicle on the basis of the first two steps. The fifth-order polynomial curve can provide a smooth trajectory, while taking into account the current position, speed, acceleration and other states of the vehicle, to generate a predicted trajectory that conforms to the vehicle dynamics characteristics, and provide accurate and practical target vehicle behavior and trajectory information, which provides an important basis for the decision-making, path planning and collision avoidance functions of the autonomous driving system.

[0149] Although the specific embodiments of the present invention depict various actions or steps in a specific order, this should be understood as requiring such actions or steps to be performed in the specific order shown or in a sequential order, or requiring all illustrated actions or steps to be performed to obtain the desired results. Under certain circumstances, multitasking and parallel processing may be advantageous. Similarly, although several specific implementation details are included in the above discussion, these should not be interpreted as limiting the scope of the present disclosure. Certain features described in the context of a separate embodiment can also be implemented in a single implementation in combination. On the contrary, the various features described in the context of a single implementation can also be implemented in multiple implementations individually or in any suitable sub-combination. The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or replacements that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0150] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A target vehicle trajectory prediction method based on double-layer lane intention inference, characterized in that: The steps include: Step S1, based on the extended Kalman filter, obtaining the short-term predicted trajectory of the target vehicle; based on the path similarity scoring model, analyzing the short-term predicted trajectory to obtain the predicted value of the road selected when the target vehicle enters the intersection; Step S2: constructing a lane selection intention prediction model based on an interactive multi-model based on the lateral offset of the target vehicle, wherein the lane selection intention prediction model obtains a lane prediction value based on the road prediction value; Step S3: Based on the lane prediction value, a predicted trajectory of the target vehicle is generated using a quintic polynomial curve method.

2. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 1 is characterized in that: Step S1 specifically includes: Step S1-1, searching for a set of candidate lanes that the target vehicle can reach; Step S1-2, predicting the short-term local state quantity of the target vehicle, and predicting the short-term predicted trajectory of the target vehicle based on the extended Kalman filter; Step S1-3, establishing a path similarity scoring model to evaluate the similarity between the short-term predicted trajectory of the target vehicle and each lane in the candidate lane set; Step S1-4: determining the road with the highest probability selected by the target vehicle based on the similarity as the road prediction value.

3. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 2 is characterized in that: Step S1-1 includes: According to the real-time status information of the target vehicle, the depth-first search algorithm searches the HD map to obtain the candidate lane set for the target vehicle. The candidate lane set contains multiple candidate lanes, indicating all possible choices for the target vehicle at the intersection to drive to lanes in other roads.

4. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 3 is characterized in that: Step S1-2 includes: Step S1-2-1: predict the short-term local state of the target vehicle in the following manner: in, is the longitudinal velocity and yaw angular velocity at time k+m in the future time window, is the longitudinal velocity and yaw rate at time k+1 in the future time window, m is the width of the short-term future time window, n is the width of the short-term historical time window, a and b are parameter indexes, and w a is the weight coefficient, v k-a+1 , γ k-a+1 are the longitudinal velocity and yaw rate at time k-a+1 in the short-term historical time window; Step S1-2-2: Predict the short-term trajectory based on the extended Kalman filter in the following way: (1) Establish process model: x k+1 =f(x k )+q k =[f1,f2,f3,f4,f5]+q k Among them, f(x k ) is the state transfer equation of vehicle kinematics, and the calculation method is: f1:x k+1 =x k +(v k cosθ k )T f2:y k+1 =and k +(v k sinθ k )T f3:θ k+1 =θ k +g k T f4:v k+1 =v k f5:c k+1 =c k Among them, x k =(x k ,y k ,θ k , v k , γ k ) is the state vector of the target vehicle at time k, (x k ,y k ) is the position coordinate of the target vehicle, θ k is the yaw angle, v k is the longitudinal velocity, γ k is the yaw angular velocity, q k ~N(0,Q k ) is the process noise, which has a mean of zero and a power spectral density of the process noise matrix Q k The normal distribution, Q k =diag{0,0,q dθ ,q dv ,q dγ }, q dθ represents the noise intensity of the yaw angle process, q dv represents the longitudinal velocity process noise intensity, q dγ represents the noise intensity of the yaw angular velocity process, and T is the sampling period; (2) Establishing the measurement model: Among them, z k is the state vector measurement value of the target vehicle at time k, H is the measurement matrix, r k ~N(0,R k ) is the measurement noise, which has a mean of zero and a noise covariance matrix of R k The normal distribution, R k =diag{r v , r γ }, r v represents the longitudinal velocity measurement noise intensity, r γ represents the yaw rate measurement noise intensity; (3) Perform prediction of the extended Kalman filter, and the calculation process is as follows: in, is the prior estimated state at time k, is the posterior estimated state at time k-1, f(·) is the state transfer equation, is the prior covariance matrix at time k, is the posterior covariance matrix at time k-1, is the Jacobian matrix at time k-1, Q k-1 is the process noise matrix at time k-1; (4) Update the extended Kalman filter, and the calculation process is: Among them, K k is the Kalman gain at time k; Repeat the update of the extended Kalman filter until all trajectories in the future time window are completed, and finally obtain the short-term predicted trajectory 5. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 4 is characterized in that: Step S1-3 specifically includes: The similarity scores between the short-term predicted trajectory of the target vehicle and each lane in the candidate lane set are calculated in the following way: in, is the i-th lane and the short-term predicted trajectory The similarity score between them, k1 and k2 are the weighted coefficients of position similarity and yaw angle similarity, and They are position similarity and yaw angle similarity; Location Similarity The calculation method is: in, is the mean of the Euclidean distance, α is the index of the short-term predicted trajectory point, x α ,y α is the coordinate of the short-term prediction trajectory point, x′ α , y′ α is the position coordinate of the nearest point of the short-term predicted trajectory point, m is the width of the short-term future time window, and L is the mapping threshold parameter; Yaw angle similarity The calculation method is: in, is the average difference of yaw angle, α is the index of short-term predicted trajectory point, θ α is the short-term predicted trajectory point yaw angle, θ′ a It is the heading in the lane corresponding to the short-term predicted trajectory point.

6. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 5 is characterized in that: Step S1-4 includes: Use the softmax function to select the candidate lane set The similarity score Normalize to obtain the probability of the target vehicle choosing different lanes Among them, n3 is the total number of all candidate lanes; By summing the probabilities of each lane in the lane group of the same road, the probability of the vehicle selecting each road is obtained, and the road with the maximum probability is used as the road prediction value.

7. The target vehicle trajectory prediction method based on double-layer lane intention inference according to any one of claims 1 to 6, characterized in that: Step S2 specifically includes: Step S2-1, constructing a lane selection intention prediction model based on an interactive multi-model, and obtaining a lane with the highest probability within the lane prediction value as a target lane based on the lateral offset and lateral offset speed of the target vehicle; Step S2-2: Calculate the preview distance, and obtain the anchor point of the center line of the target lane based on the preview distance as the lane prediction value.

8. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 7 is characterized in that: Step S2-1 specifically includes: (1) Construct the state transition probability matrix: Wherein, the subscripts i and j represent the lane numbers, n′ is the total number of lanes, and p ij It represents the probability of lane i at time k-1 switching to lane j at time k, which is calculated as follows; in, The probability density function is The calculated prior transition probability, is the cumulative value of the probability density function of lane i switching to lane j, ρ ij and is the mean and covariance of the lateral offset velocity of the target vehicle switching from lane i to lane j; (2) The interactive part of the interactive multi-model performs the following update calculations: in, and are the posterior lane model probability, lane lateral offset and covariance of the j-lane probability model obtained from the k-1 moment, is the mixing probability, is the prior lane model probability, is the mixed state of the lateral offset of lane j, is the mixed covariance of the lateral offset of lane j, is the mixed state of the lateral offset of lane i; (3) The filtering part of the interactive multi-model, for each lane model All perform standard linear Kalman filtering, and the calculation process is: Among them, KF prediction (·) and KF update (·) are the prediction and update steps of the standard Kalman filter, A is the transfer matrix, Q is the process noise matrix, is the mixed covariance of the lateral offset of lane i, and are the prior and posterior lane lateral offsets at time k, is the mean lateral deviation of existing lanes, and are the prior and posterior covariance of lane i, respectively; (4) The lane model probability of the interactive multi-model is updated by the following calculation: in, is the likelihood of the filter measurement for the i-th lane model, is the measured residual of the filter of the i-th lane model at time k, is the covariance of the filter of the i-th lane model at time k, l k is the lateral displacement of the target vehicle from the lane centerline at time k, and R is the noise matrix; (5) Selecting a lane with the highest posterior lane model probability from among the lanes within the road prediction value as the target lane.

9. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 8 is characterized in that: Step S2-2 specifically includes: Calculate the preview distance dis: dis=v k T predict Where T predict is the preview time, and the centerline anchor point on the target lane is queried based on the preview distance to obtain the state of the anchor point P T (x T ,y T ,θ T ) as the lane prediction value, where x T ,y T ,θ T are the horizontal and vertical coordinates and yaw angle of the anchor point respectively.

10. The target vehicle trajectory prediction method based on double-layer lane intention inference according to claim 9 is characterized in that: Step S3 specifically includes: The following quintic polynomial trajectory curve interpolation formula is established: x(t)=a5t 5 +a4t 4 +a3t 3 +a2t 2 +a1t+a0 <h2 style=";text-align:left;direction:ltr">y(t) = b5t<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> +b4t<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> +b3t<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +b2t<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +b1t+b0 Where t is time, a0, ..., a5, b0, ..., b5 are polynomial trajectory parameters, x(t) and y(t) are trajectory curve coordinates; the constraints of the starting point and end point of the quintic polynomial trajectory curve are: x(0)=x k y(0)=y k x′(0)=v k cosθ k y′(0)=v k sinθ k x″(0)=a k cosθ k y″(0)=a k sinθ k x(T predict )=x T y(T predict )=y T x′(T predict )=v T cosθ T y′(T predict )=v T cosθ T x″(T predict )=0 y″(T predict )=0 Among them, x k ,y k ,θ k , v k , a k are the position coordinates, yaw angle, longitudinal velocity and longitudinal acceleration of the target vehicle at time k, respectively. T ,y T ,θ T , v T , a T are the position coordinates, yaw angle, longitudinal velocity and longitudinal acceleration of the lane prediction value, respectively, and v T =v k , a T =a k , x′(·) and x″(·) are the first-order derivative and second-order derivative of the horizontal coordinate of the trajectory curve, respectively; y′(·) and y″(·) are the first-order derivative and second-order derivative of the vertical coordinate of the trajectory curve, respectively; Based on the starting point and end point constraints, the fifth-order polynomial trajectory parameters are solved, and the interpolation result of the fifth-order polynomial trajectory curve is used as the predicted trajectory of the target vehicle.

Citation Information

Patent Citations

  • Target vehicle track prediction method for multi-lane scene

    CN116758727A

  • Vehicle track prediction method based on graph convolutional neural network in network connection environment

    CN116933005A

  • Planning-coupled multi-target vehicle trajectory prediction method and system

    CN117408406A

  • Adjacent vehicle interactive trajectory prediction method

    CN117208009A

  • Strip mine area target vehicle trajectory prediction method based on similarity matching

    CN117407724A