A vehicle intelligent trajectory prediction method based on kinematic constraints
By combining long and short-term memory networks with vehicle kinematics models, predicting the yaw angular velocity and longitudinal velocity of the vehicle, the problem of unreasonable and unexecutable trajectory prediction speed assumption in the prior art is solved, and a more accurate and consistent with vehicle kinematics is achieved. Trajectory prediction.
Patent Information
- Application Number
- CN202211368939.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-11-03
- Publication Date
- 2024-12-27
- Estimated Expiration
- 2042-11-03
AI Technical Summary
Existing vehicle trajectory prediction methods exist in trajectory prediction that is not in line with actual or unsuccessful, and deep learning methods lack complex modeling of models, resulting in inaccurate prediction results.
Combining the long and short-term memory network and vehicle kinematic model, the yaw angular velocity is predicted through the long and short-term memory network, the attenuation acceleration model predicts longitudinal velocity, and the vehicle kinematic model is used to generate trajectories that conform to vehicle kinematics, and state estimation and trajectory prediction are performed through EKF. .
It improves the generalization and executability of trajectory prediction, ensures that the prediction results are in line with the actual motion characteristics of the vehicle, and enhances the adaptability and robustness of the prediction algorithm.
Smart Images
Figure CN115762182B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving vehicle technology, and in particular to the field of autonomous driving vehicle trajectory prediction. Specifically, it relates to a vehicle intelligent trajectory prediction method based on kinematic constraints. Background Art
[0002] Existing research on vehicle trajectory prediction can be roughly divided into two categories. They are shallow learning-based methods and deep learning-based trajectory prediction algorithms. The shallow learning-based method mainly uses a method based on dynamic (kinematic) features combined with a Bayesian filter. This model can predict a predicted trajectory that is consistent with the actual vehicle, but the model assumes a constant speed, which is not in line with the actual situation. The deep learning-based method mainly uses large-scale data sets to learn a reasonable mapping relationship. Its more typical deep learning networks include RNN, long short-term memory network (LSTM), etc. The deep learning method does not require complex modeling of the model, but this method will predict some trajectories that the vehicle cannot execute.
[0003] Based on the limitations of the above two methods, the present invention proposes a vehicle trajectory prediction method that combines a long short-term memory network with a vehicle kinematic model. The method first uses a long short-term memory network to train a large amount of driving data to obtain a network model with high generalization, and uses the model to predict the vehicle's yaw rate. Secondly, a decaying acceleration model is constructed to predict the future longitudinal vehicle speed. Since the vehicle is a variable acceleration motion during driving, the use of a decaying acceleration model can be more consistent with the actual driving process of the vehicle. Finally, the predicted yaw rate and longitudinal speed are combined to generate a trajectory that conforms to the vehicle's kinematics from the vehicle dynamics model. Summary of the invention
[0004] In order to overcome the shortcomings of the prior art, the present invention aims to propose a trajectory prediction algorithm that can be applied to the actual driving process of the vehicle. Specifically, considering the generalization and fault tolerance of the prediction algorithm, and taking into account the executableness of the vehicle, a vehicle trajectory prediction method combining a long short-term memory network and a vehicle kinematic model is adopted. The long short-term memory network can make the predicted vehicle yaw rate more generalized, and the vehicle kinematic model is used to constrain the planned trajectory, so that the predicted trajectory is more accurate while meeting the conditions of vehicle executableness.
[0005] The technical solution adopted by the present invention is: a vehicle intelligent trajectory prediction method based on kinematic constraints, comprising the following steps:
[0006] The first step is to build a long short-term memory network model to predict the vehicle's yaw rate in the future t2 based on the vehicle's lateral error, heading error, and yaw rate in the past t1;
[0007] The second step is to build a decay acceleration model based on the current real-time speed and real-time acceleration, decay the acceleration to 0 within t2, and iterate to find the longitudinal speed within the future t2 as the longitudinal speed measurement value of the vehicle within the future t2;
[0008] The third step is to establish a vehicle kinematic model, using the yaw velocity predicted by the long short-term memory network and the longitudinal velocity output by the attenuation acceleration model as intermediate variables to predict the future trajectory of the vehicle.
[0009] Furthermore, in the first step, the network architecture of the long short-term memory network model includes an input layer, an LSTM network layer, a fully connected layer and a regression layer; the input layer is one layer, and the input dimension is the number of features; the LSTM network layer is set to 250 layers; the fully connected layer is set to 1 layer, and the output dimension is 1, indicating the number of output features.
[0010] Further, the third step includes the following sub-steps:
[0011] (1) Establish the vehicle's state vector:
[0012]
[0013] Among them, x k ,y k ,θ k , v k , γ k , a k , They are global coordinate x, global coordinate y, vehicle heading, longitudinal velocity, yaw velocity, longitudinal acceleration, and yaw acceleration respectively;
[0014] (2) Establishing a process model: Based on the kinematic equation of the vehicle, establish a process model of the relationship between the state vector at the next moment and the current moment in (1).
[0015] x k+1 =f(x k )+q k =[f1, f2, f3, f4, f5, f6, f7]+q k
[0016] Among them, f(x k ) is the state matrix; q k is the process noise, q k ~N(0,Q k ),
[0017] Nonlinear state transition model:
[0018] f1:
[0019] f2:
[0020] f3:
[0021] f4:v k+1 =v k +a k T
[0022] f5:
[0023] f6:a k+1 =a k
[0024] f7: Among them, the sampling period T = 0.05s;
[0025] (3) Obtaining simulated measurement values: The velocity and yaw rate predicted in the first and second steps are used as simulated measurement values for subsequent EKF calculations.
[0026] y k =[v k , γ k ] T
[0027] where γ k 、v k They are the yaw angular velocity obtained by using the long short-term memory neural network in the first step and the longitudinal velocity obtained by using the decay acceleration model in the second step;
[0028] (4) Using nonlinear Kalman filtering, the vehicle state vector x in (1) is k Make predictions and updates:
[0029] Predict the current moment prior state vector and covariance matrix:
[0030]
[0031]
[0032] in, is the a priori estimated state at the current moment, x k-1 is the posterior estimated state at the previous moment, f(·) is the state transfer equation, is the prior covariance matrix at the current moment, F is the state transfer matrix, P k-1 is the posterior covariance matrix of the previous moment, Q k-1 is the process noise at the previous moment;
[0033] Update the current state vector and covariance matrix:
[0034] A. Kalman gain update:
[0035]
[0036]
[0037] Among them, K k is the Kalman gain at the current moment, is the observation matrix, R k is the observation noise covariance matrix at the current moment, R k =diag{0.01,0.001};
[0038] B. State vector update:
[0039]
[0040]
[0041] C. Covariance matrix update:
[0042]
[0043] Among them, P k is the posterior covariance matrix at the current moment, and I is the identity matrix.
[0044] Compared with the prior art, the present invention has the following beneficial effects:
[0045] (1) The long short-term memory network is used as a vehicle state observer to predict the future yaw rate based on the vehicle's historical trajectory. Since the neural network can fully approximate any complex nonlinear relationship, the future yaw rate of the vehicle can be accurately predicted when an accurate vehicle prediction model cannot be established.
[0046] (2) Establishing a decay acceleration model to transform the predicted vehicle's uniform motion into variable acceleration motion within the next 5 seconds can better conform to the actual longitudinal speed change;
[0047] (3) A vehicle kinematic model is established. The yaw rate predicted in the first step and the velocity output by the attenuated acceleration model in the second step are used as input. The future vehicle trajectory is predicted by adding kinematic constraints to the long short-term memory network. This solves the problem that the prediction results of the long short-term memory network do not conform to the vehicle kinematic trajectory, and improves the adaptability and robustness of the prediction algorithm. BRIEF DESCRIPTION OF THE DRAWINGS
[0048] Figure 1It is a flow chart of the vehicle trajectory prediction method combining the long short-term memory network and the vehicle kinematic model.
[0049] Figure 2 It is the process of long short-term memory network predicting yaw rate. DETAILED DESCRIPTION
[0050] The technical solution adopted by the present invention is to use the long short-term memory network and the attenuated acceleration model to predict the intermediate variables yaw rate and longitudinal velocity, and input the intermediate variables into the vehicle kinematic model to predict the vehicle trajectory. A vehicle intelligent trajectory prediction method based on kinematic constraints, such as Figure 1 As shown, the specific steps are as follows:
[0051] The first step is to predict the lateral parameters of the vehicle - the long short-term memory network predicts the vehicle's yaw rate: In order to obtain the accurate yaw rate of the vehicle, the long short-term memory network model is used for prediction; the long short-term memory network learns from a large amount of data, and combined with the historical trajectory of the vehicle, it can predict a relatively accurate yaw rate.
[0052] A long short-term memory network model is established, and the network architecture is set to input layer, LSTM network layer, full connection layer and regression layer. The input layer is one layer, and the input dimension is the number of features; the LSTM network layer is set to 250 layers; the full connection layer is set to 1 layer, and the output dimension is 1, indicating the number of output features.
[0053] The input of the model is: lateral error, heading error, yaw rate, and the output is: yaw rate. That is, based on the lateral error, heading error, and yaw rate of the vehicle in the past 3 seconds, the yaw rate of the vehicle in the next 5 seconds is predicted. Among them, the lateral error is the lateral distance of the key point of the reference path relative to the actual position of the vehicle, which is positive on the left and negative on the right. The heading error is the heading angle deviation of the key point of the reference path relative to the actual position of the vehicle. The process of predicting yaw rate by long short-term memory network is as follows Figure 2 shown.
[0054] The second step is to predict the longitudinal parameters of the vehicle - the attenuated acceleration model predicts the longitudinal speed of the vehicle: the speed of the vehicle is not constant during movement, and the future acceleration is difficult to predict, so the attenuated acceleration model is used to simulate the variable acceleration movement of the vehicle.
[0055] The speed of a vehicle is not constant during its motion, and the future acceleration is difficult to predict, so the decay acceleration model is used to simulate the variable acceleration motion of the vehicle. The input parameters are the current speed and the current acceleration, and the output parameter is the speed within the next 5 seconds.
[0056] The specific process is as follows: based on the current real-time speed and real-time acceleration, a decay acceleration model is constructed, the acceleration decays to 0 within 5 seconds, and the longitudinal speed within the next 5 seconds is iteratively calculated as the longitudinal speed measurement value within the future limited time domain (5 seconds).
[0057] The third step is vehicle trajectory prediction - combining the vehicle kinematic model to predict the future trajectory of the vehicle: the trajectory predicted by the method based solely on the data model sometimes does not conform to the kinematic characteristics of the vehicle. Therefore, a vehicle kinematic model is established, and the results predicted by the long short-term memory network and the output of the attenuated acceleration model are used as intermediate variables. While ensuring the feasibility of the predicted trajectory, it can also prevent the trajectory prediction result from being too large due to network prediction failure.
[0058] The vehicle trajectory in the prediction time domain is predicted based on EKF (Extended Kalman Filter), thereby converting the trajectory prediction problem into a state estimation problem.
[0059] (1) Establish the vehicle's state vector:
[0060]
[0061] Among them, x k ,y k ,θ k , v k , γ k , a k , They are global coordinate x, global coordinate y, vehicle heading, longitudinal velocity, yaw velocity, longitudinal acceleration, and yaw acceleration respectively.
[0062] (2) Establishing a process model: Based on the kinematic equation of the vehicle, establish a process model of the relationship between the state vector at the next moment and the current moment in (1).
[0063] x k+1 =f(x k )+q k =[f1, f2, f3, f4, f5, f6, f7]+q k
[0064] Among them, f(x k ) is the state matrix, the sampling period T = 0.05s; q k is the process noise, q k ~N(0,Q k ),
[0065] Nonlinear state transition model:
[0066] f1:
[0067] f2:
[0068] f3:
[0069] f4:v k+1 =v k +a k T
[0070] f5:
[0071] f6:a k+1 =a k
[0072] f7:
[0073] Among them, the sampling period T = 0.05s.
[0074] (3) Obtaining simulated measurement values: The velocity and yaw rate predicted in the first and second steps are used as simulated measurement values for subsequent EKF calculations.
[0075] y k =[v k , γ k ] T
[0076] where γ k 、v k They are the yaw angular velocity obtained by using the long short-term memory neural network in the first step and the longitudinal velocity obtained by using the decay acceleration model in the second step.
[0077] (4) Using nonlinear Kalman filtering, the vehicle state vector x in (1) is k Make predictions and updates:
[0078] Predict the current moment prior state vector and covariance matrix:
[0079]
[0080]
[0081] in, is the a priori estimated state at the current moment, x k-1 is the posterior estimated state at the previous moment, f(·) is the state transfer equation, is the prior covariance matrix at the current moment, F is the state transfer matrix, P k-1 is the posterior covariance matrix of the previous moment, Q k-1 is the process noise at the previous moment.
[0082] Update the current state vector and covariance matrix:
[0083] A. Kalman gain update:
[0084]
[0085]
[0086] Among them, K k is the Kalman gain at the current moment, is the observation matrix, R k is the observation noise covariance matrix at the current moment, R k =diag{0.01,0.001}.
[0087] B. State vector update:
[0088]
[0089]
[0090] C. Covariance matrix update:
[0091]
[0092] Among them, P k is the posterior covariance matrix at the current moment, and I is the identity matrix.
[0093] The above description is only a specific implementation mode of the present invention and is not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A vehicle intelligent trajectory prediction method based on kinematic constraints, characterized in that: The following steps are involved: The first step is to build a long short-term memory network model to predict the vehicle's yaw rate in the future t2 based on the vehicle's lateral error, heading error, and yaw rate in the past t1; The second step is to build a decay acceleration model based on the current real-time speed and real-time acceleration, decay the acceleration to 0 within t2, and iterate to find the longitudinal speed within the future t2 as the longitudinal speed measurement value of the vehicle within the future t2; The third step is to establish a vehicle kinematic model, using the yaw rate predicted by the long short-term memory network and the longitudinal velocity output by the attenuated acceleration model as intermediate variables to predict the future trajectory of the vehicle. In the first step, the network architecture of the long short-term memory network model includes an input layer, an LSTM network layer, a full connection layer and a regression layer; wherein the input layer is one layer, and the input dimension is the number of features; the LSTM network layer is set to 250 layers; the full connection layer is set to 1 layer, and the output dimension is 1, indicating the number of output features; The third step includes the following sub-steps: (1) Establish the vehicle's state vector: Among them, x k ,y k ,θ k , v k , γ k , a k , They are global coordinate x, global coordinate y, vehicle heading, longitudinal velocity, yaw velocity, longitudinal acceleration, and yaw acceleration respectively; (2) Establishing a process model: Based on the kinematic equation of the vehicle, establish a process model of the relationship between the state vector at the next moment and the current moment in (1). x k+1 =f(x k )+q k =[f1,f2,f3,f4,f5,f6,f7]+q k Among them, f(x k ) is the state matrix; q k is the process noise, q k ~N(0,Q k ), Nonlinear state transition model: f4:v k+1 =v k +a k T f6:a k+1 =a k Among them, the sampling period T = 0.05s; (3) Obtaining simulated measurement values: The velocity and yaw rate predicted in the first and second steps are used as simulated measurement values for subsequent EKF calculations. y k =[v k ,γ k ] T where γ k 、v k They are the yaw angular velocity obtained by using the long short-term memory neural network in the first step and the longitudinal velocity obtained by using the decay acceleration model in the second step; (4) Using nonlinear Kalman filtering, the vehicle state vector x in (1) is k Make predictions and updates: Predict the current moment prior state vector and covariance matrix: in, is the a priori estimated state at the current moment, x k-1 is the posterior estimated state at the previous moment, f(·) is the state transfer equation, is the prior covariance matrix at the current moment, F is the state transfer matrix, P k-1 is the posterior covariance matrix of the previous moment, Q k-1 is the process noise at the previous moment; Update the current state vector and covariance matrix: A. Kalman gain update: Among them, K k is the Kalman gain at the current moment, is the observation matrix, R k is the observation noise covariance matrix at the current moment, R k =diag{0.01,0.001}; B. State vector update: C. Covariance matrix update: Among them, P k is the posterior covariance matrix at the current moment, and I is the identity matrix.