Method for calculating probability distribution of driving track of automatic driving vehicle

Abstract

The invention discloses an automatic driving vehicle driving track probability distribution calculation method, which comprises the following steps of S1, acquiring driving data of a vehicle, including a control signal and a vehicle positioning track; s2, a double-layer optimization algorithm is adopted to iteratively train the dynamic model and the observation model, the inner layer optimizes parameters of the dynamic model based on a maximum marginal likelihood function, the outer layer optimizes parameters of a filtering algorithm based on a long-term trajectory prediction result, observation noise is removed, and the vehicle state and the observation model are estimated; and S3, using the trained dynamic model and observation model, combining the planning model and the control model, and iteratively calculating the prediction result of the vehicle track and the probability distribution thereof. According to the method for calculating the probability distribution of the driving track of the automatic driving vehicle, under the condition that the vehicle state cannot be directly observed, the vehicle state is effectively estimated through a double-layer optimization algorithm, model parameters are optimized, and therefore the accuracy of prediction of the probability distribution of the driving track of the vehicle is improved.

Description

Technical Field

[0001] The present invention relates to the field of vehicle trajectory prediction, and in particular to a method for estimating the probability distribution of the driving trajectory of an autonomous vehicle. Background Art

[0002] When autonomous vehicles operate in dynamic environments, their state responses are subject to uncertainty due to the system's own dynamic changes or external random interference. Therefore, autonomous driving system design must fully consider the impact of uncertainty on vehicle operation. Predicting the probability distribution of the vehicle's trajectory and determining its actual position can help estimate collision risk, endpoint error, and other factors in advance, facilitating proactive route adjustments to avoid adverse outcomes and effectively improving system safety and reliability.

[0003] Common trajectory prediction methods mostly focus on single trajectory predictions based on historical data, ignoring the uncertainty of vehicle system state responses and failing to predict the probability distribution of driving trajectories. Furthermore, because the vehicle's true state cannot be directly observed, existing trajectory prediction methods based on uncertainty iteration lack the ability to quantify the impact of observation noise on uncertainty predictions, often resulting in inaccurate iterative predictions of the final trajectory probability distribution. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings and deficiencies of the above-mentioned prior art and to provide a method for estimating the probability distribution of the driving trajectory of an autonomous driving vehicle.

[0005] When the real vehicle state data cannot be directly observed, the present invention effectively estimates the vehicle state and optimizes the model parameters through a two-layer optimization algorithm, thereby improving the accuracy of the vehicle trajectory probability distribution prediction.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for estimating the probability distribution of a driving trajectory of an autonomous vehicle comprises the following steps:

[0008] S1, collecting vehicle driving data; the driving data includes vehicle control signals and vehicle positioning tracks;

[0009] S2 uses a two-layer optimization algorithm to iteratively train the dynamics model and observation model. The inner layer optimizes the dynamics model parameters based on the maximum marginal likelihood function, while the outer layer optimizes the filtering algorithm parameters based on long-term trajectory prediction results to remove observation noise and estimate the vehicle state and observation model.

[0010] S3 uses the trained dynamics model and observation model, combined with the planning model and control model, to iteratively calculate the prediction results of the vehicle trajectory and its probability distribution.

[0011] In step S1, the vehicle control signal includes the throttle opening, the brake pedal opening and the steering wheel angle; the vehicle positioning trajectory includes the position and heading angle in the absolute coordinate system.

[0012] Step S2 includes the following sub-steps:

[0013] S21, selecting initial filtering algorithm parameters;

[0014] S22, the filtering algorithm pre-processes the driving data to estimate the vehicle state and observation model;

[0015] S23, inner layer: training the dynamics model and optimizing the model parameters based on maximizing the marginal likelihood function;

[0016] S24, outer layer: using the model trained in step S22 and step S23, to make multi-step predictions for long-term trajectories, and using an intelligent optimization algorithm to update the filtering algorithm parameters according to the prediction results;

[0017] Step S25: repeat steps S22-S24 until the trajectory prediction result converges.

[0018] In step S2, the outer iteration of step S2 uses an intelligent optimization algorithm to optimize the filtering algorithm parameters, and the intelligent optimization algorithm includes a genetic algorithm and a particle swarm algorithm.

[0019] In step S2, the filtering algorithm specifically refers to filtering the observation data to remove noise components and estimate the vehicle state, including Kalman filtering and moving average filtering.

[0020] In step S2, the dynamic model expresses the conversion relationship between the vehicle state and control signal at the current moment and the vehicle state at the next moment, and uses a probabilistic model to learn its uncertainty, including Gaussian process regression and probabilistic integrated neural network.

[0021] In step S2, the observation model expresses the conversion relationship from the current vehicle state to the observation data. The observation data is equal to the superposition of the vehicle state and the observation noise. The variance term of the observation noise can be obtained by statistically comparing the uncertainty before and after data filtering.

[0022] Step S3 includes the following sub-steps:

[0023] S31, the planning model inputs the probability distribution of the vehicle's current / initial observations and outputs the preview state;

[0024] S32, the control model inputs the probability distribution of the vehicle's current / initial observation quantity and the preview state quantity, and calculates the probability distribution of the control quantity;

[0025] S33, the dynamics model inputs the probability distribution of the current / initial state and the probability distribution of the control variable, and outputs the probability distribution of the state at the next moment;

[0026] S34, the observation model inputs the probability distribution of the state at the next moment and outputs the probability distribution of the observation at the next moment;

[0027] S35, repeating steps S31 to S34 to implement uncertainty iterative prediction of state quantities until trajectory estimation is completed.

[0028] In step S3, the planning model is an algorithm for generating and optimizing the vehicle driving path in the autonomous driving system.

[0029] In step S3, the control model is an algorithm for controlling vehicle behavior in an autonomous driving system, including PID control, model predictive control (MPC), and linear quadratic regulator (LQR).

[0030] Compared with the prior art, the present invention has the following advantages and effects:

[0031] When absolute positioning technology is available, the inference method described in the present invention can be used to predict the probability distribution of the driving trajectory of an autonomous driving vehicle; when the actual vehicle status data cannot be directly observed, a two-layer optimization algorithm is used to estimate the vehicle status and optimize the model parameters.

[0032] Compared with existing trajectory probability distribution prediction methods, the present invention provides an accurate estimation of vehicle status, effectively reduces the cumulative error caused by noise in long-term trajectory prediction, and improves the accuracy of the vehicle's future driving trajectory probability distribution prediction. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a flowchart of the overall process of the present invention.

[0034] Figure 2 This is the trajectory probability distribution iterative prediction process of the present invention. DETAILED DESCRIPTION

[0035] The present invention is described in further detail below with reference to specific embodiments.

[0036] like Figure 1 As shown, the present invention provides a method for estimating the probability distribution of the driving trajectory of an autonomous vehicle. This can be achieved by the following steps:

[0037] Step S1: collecting vehicle driving data, including control signals and vehicle positioning trajectory.

[0038] Specifically, the driving data of this embodiment includes a vehicle control signal and a vehicle positioning trajectory, wherein the vehicle control signal includes the throttle opening uth , brake pedal opening u br and steering wheel angle δ sw ,The vehicle positioning trajectory includes the position x, y and the heading angle yaw in the absolute coordinate system.

[0039] In step S2, a two-layer optimization algorithm is used to iteratively train the dynamics model and the observation model. The inner layer optimizes the dynamics model parameters based on the maximum marginal likelihood function, and the outer layer optimizes the filtering algorithm parameters based on the long-term trajectory prediction results to remove observation noise and estimate the vehicle state and observation model.

[0040] Specifically, the filtering algorithm in this embodiment filters the observation data to remove noise components and estimate the vehicle state, including but not limited to Kalman filtering and moving average filtering.

[0041] Specifically, the dynamic model in this embodiment is expressed using a Gaussian process regression model. Gaussian process regression can naturally express three types of uncertainty in the system: system noise, observation noise, and model uncertainty. The model parameters can be solved by maximizing the marginal likelihood function. The dynamic model of this embodiment expresses the transformation relationship between the current vehicle state and control signal to the next vehicle state, which can be expressed as:

[0042] Y t = f (X t-1 ,u t ) +ε (1)

[0043] Where, X t-1 ={x t-1 ,y t-1 ,yaw t-1} is the vehicle state at the previous moment, u t ={u th,t ,u br,t ,δ sw,t} is the control signal at the current moment, the system noise ε~N(0,∑ ε ).

[0044] The observation model of this embodiment expresses the conversion relationship from the current vehicle state to observation data. The observation data is equal to the superposition of the vehicle state and observation noise:

[0045] Y t = X t +ν (2)

[0046] Where Y t ={x obs,t ,y obs,t ,yaw obs,t} is the observation data at the current moment, and the observation noise ν~N(0,∑ ν), the size of its variance term can be obtained by statistically comparing the uncertainty before and after data filtering.

[0047] Specifically, in this embodiment, the outer iteration of the two-layer optimization algorithm uses a genetic algorithm to optimize the filtering algorithm parameters, selects the trajectory multi-step prediction error as the fitness function, performs genetic operations such as selection, crossover, and mutation, and iteratively optimizes the filtering algorithm parameters with an advantageous fitness function, that is, the filtering algorithm parameters with a smaller trajectory multi-step prediction error.

[0048] Specifically, the dual-layer optimization algorithm in this embodiment iteratively trains the dynamic model and the observation model. Figure 2 As shown, it includes the following sub-steps:

[0049] Step S201, select initial filtering algorithm parameters

[0050] Step S202 , the filtering algorithm pre-processes the driving data to estimate the vehicle state X and the observation model;

[0051] Step S203, inner layer: training the dynamics model. Given the vehicle state X and the control signal u, the hyperparameters of the Gaussian process can be optimized based on maximizing the marginal likelihood function.

[0052] Step S204, outer layer: use the model trained in S202 and S203 to make multi-step predictions of long-term trajectories, and use a genetic algorithm to update the filtering algorithm parameters based on the prediction results;

[0053] Step S205: repeat steps S202-S204 until the trajectory prediction result converges.

[0054] In step S3, the trained dynamics model and observation model are used in combination with the planning model and the control model to iteratively calculate the prediction result of the vehicle trajectory and its probability distribution.

[0055] Specifically, the planning model in this embodiment is the algorithm used to generate and optimize the vehicle's driving path in the autonomous driving system. The control model is the algorithm used to control the vehicle's behavior in the autonomous driving system, including but not limited to PID control, model predictive control (MPC), and linear quadratic regulator (LQR).

[0056] Furthermore, in this embodiment, iteratively calculating the prediction result of the vehicle trajectory and its probability distribution includes the following sub-steps:

[0057] Step S301: The planning model inputs the probability distribution of the vehicle's current / initial observations and outputs the preview state.

[0058] Step S302: The control model inputs the probability distribution of the vehicle's current / initial observation quantity and the preview state quantity to calculate the probability distribution of the control quantity;

[0059] Step S303: The dynamics model inputs the probability distribution of the current / initial state and the probability distribution of the control variable, and outputs the probability distribution of the state at the next moment;

[0060] Step S304: The observation model inputs the probability distribution of the state at the next moment and outputs the probability distribution of the observation quantity at the next moment;

[0061] Step S305 , repeating steps S301 to S304 to implement uncertainty iterative prediction of state quantities until trajectory estimation is completed.

[0062] The inference method described in the present invention effectively estimates the vehicle state and optimizes model parameters through a two-layer optimization algorithm when the vehicle's actual state cannot be directly observed. This effectively reduces the cumulative error caused by noise in long-term trajectory prediction and improves the accuracy of the probability distribution prediction of the vehicle's future driving trajectory.

[0063] As described above, the present invention can be implemented well.

[0064] The implementation methods of the present invention are not limited to the above-mentioned embodiments. Any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A method for estimating the probability distribution of an autonomous vehicle's driving trajectory, characterized by: The following steps are involved: S1, collecting vehicle driving data; the driving data includes vehicle control signals and vehicle positioning tracks; S2 uses a two-layer optimization algorithm to iteratively train the dynamics model and observation model. The inner layer optimizes the dynamics model parameters based on the maximum marginal likelihood function, while the outer layer optimizes the filtering algorithm parameters based on long-term trajectory prediction results to remove observation noise and estimate the vehicle state and observation model. S3 uses the trained dynamics model and observation model, combined with the planning model and control model, to iteratively calculate the prediction results of the vehicle trajectory and its probability distribution.

2. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: In step S1, the vehicle control signal includes the throttle opening, the brake pedal opening and the steering wheel angle; the vehicle positioning trajectory includes the position and heading angle in the absolute coordinate system.

3. The method for estimating the probability distribution of the driving trajectory of an autonomous driving vehicle according to claim 1, characterized in that: Step S2 includes the following sub-steps: S21, selecting initial filtering algorithm parameters; S22, the filtering algorithm pre-processes the driving data to estimate the vehicle state and observation model; S23, inner layer: training the dynamics model and optimizing the model parameters based on maximizing the marginal likelihood function; S24, outer layer: using the model trained in step S22 and step S23, to make multi-step predictions for long-term trajectories, and using an intelligent optimization algorithm to update the filtering algorithm parameters according to the prediction results; Step S25: repeat steps S22-S24 until the trajectory prediction result converges.

4. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: In step S2, the outer iteration of step S2 uses an intelligent optimization algorithm to optimize the filtering algorithm parameters, and the intelligent optimization algorithm includes a genetic algorithm and a particle swarm algorithm.

5. The method for estimating the probability distribution of the driving trajectory of an autonomous driving vehicle according to claim 1, characterized in that: In step S2, the filtering algorithm specifically refers to filtering the observation data to remove noise components and estimate the vehicle state.

6. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: In step S2, the dynamic model expresses the conversion relationship between the vehicle state and control signal at the current moment and the vehicle state at the next moment, and uses a probability model to learn its uncertainty.

7. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: In step S2, the observation model expresses the conversion relationship from the current vehicle state to the observation data. The observation data is equal to the superposition of the vehicle state and the observation noise. The variance term of the observation noise can be obtained by statistically comparing the uncertainty before and after data filtering.

8. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: Step S3 includes the following sub-steps: S31, the planning model inputs the probability distribution of the vehicle's current / initial observations and outputs the preview state; S32, the control model inputs the probability distribution of the vehicle's current / initial observation quantity and the preview state quantity, and calculates the probability distribution of the control quantity; S33, the dynamics model inputs the probability distribution of the current / initial state and the probability distribution of the control variable, and outputs the probability distribution of the state at the next moment; S34, the observation model inputs the probability distribution of the state at the next moment and outputs the probability distribution of the observation at the next moment; S35, repeat steps S31-S34 to implement uncertainty iterative prediction of state quantities until trajectory estimation is completed.

9. The method for estimating the probability distribution of the driving trajectory of an autonomous driving vehicle according to claim 1, characterized in that: In step S3, the planning model is an algorithm for generating and optimizing the vehicle driving path in the autonomous driving system.

10. The method for estimating the probability distribution of the driving trajectory of an autonomous vehicle according to claim 1, characterized in that: In step S3, the control model is an algorithm for controlling vehicle behavior in an autonomous driving system.

Images