A relative motion parameter estimation method for intelligent vehicle platooning driving evaluation

By constructing a nonlinear dynamic model and using particle filtering algorithms, the relative motion state between vehicles in the intelligent vehicle formation is directly estimated, which solves the problem of insufficient estimation accuracy in the prior art and improves the accuracy and safety of formation operation.

CN114879700BActive Publication Date: 2024-12-03SOUTHEAST UNIV
View PDF 4 Cites 0 Cited by

Patent Information

Application Number
CN202210697067.X
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-06-20
Publication Date
2024-12-03
Estimated Expiration
2042-06-20

AI Technical Summary

Technical Problem

In the prior art, in the intelligent vehicle formation driving evaluation, it is difficult to accurately estimate the relative motion state parameters between vehicles, which affects the safety and efficiency of formation operation.

Method used

A nonlinear dynamic model is constructed and a particle filtering algorithm is used to directly filter and estimate parameters such as the relative velocity, distance, and relative yaw angular velocity between the formation vehicles, and recursively calculate the relative motion state through the particle filtering algorithm.

Benefits of technology

The accuracy of estimating the motion state of the formation vehicle is improved, and the relative motion state parameters between adjacent vehicles can be accurately calculated, which enhances the reliability and safety of formation operation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114879700B_ABST
    Figure CN114879700B_ABST
Patent Text Reader

Abstract

The present invention discloses a relative motion parameter estimation method for intelligent vehicle platoon driving evaluation. First, a nonlinear dynamic model is constructed according to the motion characteristics of the vehicle platoon driving process. Then, a particle filter algorithm is used to accurately estimate the motion state parameters such as the relative speed, relative distance, relative yaw angular velocity, and relative yaw angle of the platoon vehicles. The method proposed in the present invention can achieve comprehensive and accurate measurement of vehicle motion state parameters, and directly estimate the relative motion state of platoon vehicles, thereby improving the estimation accuracy of parameters required for intelligent vehicle platoon driving evaluation to a certain extent.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of road testing and evaluation of intelligent driving vehicles, and in particular relates to a relative motion parameter estimation method for intelligent vehicle platoon driving evaluation. Background Art

[0002] With the continuous increase in the total mileage of highways and the number of cars in my country, the current traffic control capacity cannot meet the current situation of the rapidly expanding traffic volume. In addition, my country's traffic conditions are complex and traffic accidents occur frequently, causing casualties and seriously affecting economic development and social stability. At its core, improper human operation is the direct cause of most traffic accidents. In this context, intelligent driving technology uses electronic information technology to assist car driving behavior and reduce unsafe operations of drivers, which can reduce the occurrence of traffic accidents and is a hot topic of research today.

[0003] Intelligent vehicle platooning is an important research direction in vehicle-road cooperative systems and an important part of intelligent driving technology. Through platoon control, road utilization can be greatly improved, traffic congestion can be effectively alleviated, and vehicles can be actively controlled to avoid collisions, improve traffic safety, and reduce the occurrence of accidents. Accurate and reliable platoon driving evaluation is the key to ensuring the normal operation of vehicle platoons. For the vehicle motion state parameters required for the evaluation, most of them first estimate the motion state of each vehicle in the queue, and then calculate the relative motion state between vehicles. For example, the invention patent "A method for evaluating lane change performance of intelligent vehicle formations" (patent number: 202010436610.1) estimates the motion state parameters of a single vehicle through an improved unscented Kalman filter algorithm, and then calculates the relative distance, relative speed and other motion states in subsequent evaluation indicators. The relative motion parameter estimation method for intelligent vehicle platoon driving evaluation proposed in the present invention aims to directly filter and estimate the relative motion state parameters between platoon vehicles. Summary of the invention

[0004] The purpose of the present invention is to propose a relative motion parameter estimation method for intelligent vehicle platoon driving evaluation. First, based on the motion characteristics of vehicles in platoon driving, a nonlinear dynamic model of the platoon vehicle driving process is constructed. Then, based on the dynamic model of the vehicle driving process, a particle filter algorithm is used to filter and estimate the relative motion state variables such as the relative speed, distance, and relative yaw rate of the platoon vehicles. The specific steps include:

[0005] Step 1: Construct a dynamic model of the platoon vehicle driving process

[0006] First, in order to accurately describe the movement characteristics of intelligent vehicles in platooning, a platoon of four vehicles is formed. It is assumed that the first intelligent vehicle in the platoon is the pilot vehicle, which is driven in manual mode and drives according to the planned safe operation route. The remaining intelligent vehicles are followers and follow the pilot vehicle autonomously at a certain distance.

[0007] Then, the dynamic model of the platoon vehicle is simplified and equivalent to a two-wheeled single vehicle model, XY is the geodetic coordinate system (the X axis is defined as the east direction and the Y axis is defined as the north direction), xy is the vehicle body coordinate system, x represents the longitudinal direction of the vehicle, y represents the lateral direction of the vehicle, and the direction perpendicular to the XY coordinate system and the plane where the xy coordinate system is located pointing to the sky is defined as the Z axis direction, which conforms to the right-hand screw rule and makes the following assumptions:

[0008] (1) The factors of road pitch and roll are not considered;

[0009] (2) The relative yaw angle between the vehicle and the road is very small;

[0010] (3) The front wheel steering angle is very small;

[0011] Based on the above requirements and assumptions, the dynamic model of each platoon vehicle is obtained, considering the lateral, longitudinal and yaw motion of the vehicle, and assuming that the forces on the two wheels of the front axle of the vehicle are the same, the forces on the two wheels of the rear axle are also the same, the left and right front wheels are merged into one point, namely point A; the left and right rear wheels are merged into one point, namely point B; point C is the center of mass of the vehicle, with coordinates (X, Y); the force equations of the vehicle in the longitudinal, lateral and yaw degrees of freedom are as follows:

[0012]

[0013] In the formula, the superscripts “·” and “··” represent the first-order differential and the second-order differential, respectively, and a x is the longitudinal acceleration of the vehicle, F yf 、F yr They represent the longitudinal force and lateral force on the front and rear wheels respectively. represents the yaw angle of the vehicle, m is the mass of the vehicle, I z represents the moment of inertia around the Z axis, l f , l r Respectively represent the distances from the front and rear axles to the center of mass;

[0014] The tire slip angle is defined as the angle between the tire plane direction and the tire velocity vector direction, that is, the tire slip angle a f =δ-θ vf , where θ vf represents the angle between the velocity vector of the front wheel of the vehicle and the longitudinal axis of the vehicle, δ represents the steering angle of the front wheel, and the side slip angle of the rear wheel is represented by a r =-θ vr, where θ vr It represents the angle between the velocity vector of the rear wheel of the vehicle and the longitudinal axis of the vehicle. When the tire slip angle is small, it is proportional to the lateral force of the front and rear wheels, that is,

[0015]

[0016] In the formula, C αf and C αr is the proportional constant representing the cornering stiffness of the front and rear wheels;

[0017] θ vf and θ vr The calculation satisfies the following relationship:

[0018]

[0019] Formula (3) can be simplified by the small angle approximation method to obtain:

[0020]

[0021] Combining the transformation relationship between the earth coordinate system and the vehicle coordinate system, we get:

[0022]

[0023] The dynamic model of the pilot vehicle is described as:

[0024]

[0025] Step 2: Vehicle relative motion state estimation based on particle filtering

[0026] Sub-step 1: Establish a mathematical model of the vehicle platooning system

[0027] (1) Establishing the system state equation

[0028] First, based on the dynamic model of the leading vehicle, the dynamic model of each following vehicle is recursively derived and defined as The subscript i represents the position of the vehicle in the queue, i = 1....4; and defines the relative variable

[0029] Secondly, we take the first and second vehicles in the platoon (hereinafter referred to as vehicle 1 and vehicle 2) as examples to illustrate the establishment of the system state equation and the selection of state variables. Superscript T Represents matrix transpose;

[0030] According to the dynamic equation described by formula (6), the system state equation model between vehicle 1 and vehicle 2 is established:

[0031] X=f(X,U,W,γ) (7)

[0032] Where f(.) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U = [a i-x , δ i , F i_yf , F i_yr ] T , where the subscript i represents the position of the vehicle in the queue, and a x It can be measured by the inertial measurement unit. There is a certain proportional relationship between the front wheel angle δ and the steering wheel angle ε ( ρ is the transmission ratio of the steering system), the steering wheel angle ε can be obtained through the steering wheel angle sensor, and the lateral force F of the tire yf and F yr It can be determined by the tire linear model, W is the system noise matrix, γ is the input noise matrix;

[0033] Assume that the sampling period is T, and discretize formula (7) to obtain the system state equation at time k:

[0034] X(k)=f{X(k-1), U(k-1), W(k-1), γ(k-1)} (8)

[0035] In the formula, the system process noise W = [w 1 , w 2 , w 3 , w 4 , w 5 , w 6 ] T , the Gaussian white noise covariance matrix corresponding to W(k-1) is Input Noise And the noise component is hidden in the four external inputs of the system, and the nonlinear state transfer equation f(.) is expressed as:

[0036]

[0037] in,

[0038]

[0039]

[0040]

[0041]

[0042]

[0043]

[0044] Finally, based on formula (9), the system state equation between vehicle 1 and vehicle 2 is established, and the system state equation between vehicle i and vehicle i+1 (where i≥2) in the queue is recursively derived;

[0045] (2) Establishing the observation equation

[0046] Each vehicle participating in the vehicle formation driving evaluation is equipped with a dual-mode satellite navigation system receiver and an inertial measurement unit. The relative yaw angular velocity ΔZ of the two adjacent vehicles obtained by the inertial measurement unit is i_ω (ΔZ i_ω =Z i+1_ω -Z i_ω , Z i_ω represents the yaw rate of the i-th vehicle in the queue), the longitudinal velocity difference between two adjacent vehicles in the queue (in v i_x is the longitudinal speed of the i-th vehicle in the queue obtained by the satellite navigation system), and represents the distance Z between the centroids of two adjacent vehicles in the queue in the geodetic coordinate system. i_d , as the observed quantity of the system, that is in X p_i and Y p_i is the east and north position of the i-th vehicle in the formation queue that can be collected by the satellite navigation system. The observation equation is expressed as (also taking vehicle 1 and vehicle 2 as examples):

[0047] Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (10)

[0048] In the formula, V is the observation noise, and its corresponding measurement noise covariance matrix in Determined by the statistical characteristics of the position measurement noise of the satellite navigation system, and It can be determined by the statistical characteristics of the speed and yaw rate measurement noise of the inertial measurement unit;

[0049] Similarly, the observation equation between vehicle i and vehicle i+1 (where i≥2) is established based on formula (10);

[0050] Sub-step 2: Establish the recursive process of particle filter algorithm

[0051] (1) Initialize particles

[0052] At time k, the particle set {(X j (k-1), η j (k-1))|j=1,...,N}, where ηj (k-1) = 1 / N, η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is ​​Φ, and the average value is set to be the state estimate at the previous moment, that is:

[0053]

[0054] In the formula, represents the estimated value of the state at time k-1;

[0055] (2) Prediction

[0056] When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1), η j (k-1))|j=1,...,N} to obtain a new particle swarm

[0057] (3) Update particle weights and perform normalization

[0058]

[0059] In the formula, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It can be expressed as:

[0060]

[0061] Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix;

[0062] According to the observation value at the current moment, use formula (12) to update the particle weight value and perform normalization to obtain the new weight value The specific steps are:

[0063] First, the particle importance weights are updated and the prior probability distribution is selected as the importance density function, that is, The weight formula can be simplified to

[0064] Then, the weight normalization is performed, that is,

[0065] (4) State Estimation

[0066] Using the weighted particles obtained in (3) To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is,

[0067]

[0068] (5) Resampling

[0069] According to the size of the particle weight value, the particle set Resample to obtain a new particle group {(X j (k), η j (k))|i=1,...,N}, where, η j (k) = 1 / N, and return to repeat (1) for the next iteration;

[0070] After the particle filter recursive calculation composed of the above formulas (11), (12) and (13), the output state estimation is the longitudinal speed difference, lateral speed difference, coordinate difference of the center of mass of the two adjacent vehicles in the vehicle formation queue at time k, relative yaw angular velocity and relative yaw angle information;

[0071] In summary, when estimating the relative motion parameters of the intelligent vehicle platoon driving evaluation, first, based on the motion characteristics of the vehicles in the platoon driving in "step one", a nonlinear dynamic model of the platoon vehicle driving process is constructed. Then, according to "step two", on the basis of constructing the mathematical model of the vehicle platoon system, the particle filter algorithm is used to recursively estimate the relative motion state parameters such as the relative speed, distance, and relative yaw rate between adjacent vehicles in the platoon.

[0072] Advantages and significant effects of the present invention:

[0073] (1) The relative motion parameter estimation method for intelligent vehicle platoon driving evaluation proposed in the present invention directly considers the relative motion state parameters between two adjacent vehicles in the platoon, which can improve the estimation accuracy of the actual motion state of the platoon vehicles to a certain extent;

[0074] (2) The present invention adopts a particle filter algorithm for recursive inference for the state estimation of the nonlinear system model, and can accurately infer the relative motion state parameters between adjacent vehicles in the queue. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 Schematic diagram of technical route;

[0076] Figure 2 Vehicle dynamics model diagram;

[0077] Figure 3 It is the flow chart of particle filter algorithm. DETAILED DESCRIPTION

[0078] With the continuous increase in the total mileage of highways and the number of cars in my country, the current traffic control capacity cannot meet the current situation of the rapidly expanding traffic volume. In addition, my country's traffic conditions are complex and traffic accidents occur frequently, causing casualties and seriously affecting economic development and social stability. At its core, improper human operation is the direct cause of most traffic accidents. In this context, intelligent driving technology uses electronic information technology to assist car driving behavior and reduce unsafe operations of drivers, which can reduce the occurrence of traffic accidents and is a hot topic of research today.

[0079] Intelligent vehicle platooning is an important research direction in vehicle-road cooperative systems and an important part of intelligent driving technology. Through platoon control, road utilization can be greatly improved, traffic congestion can be effectively alleviated, and vehicles can be actively controlled to avoid collisions, improve traffic safety, and reduce the occurrence of accidents. Accurate and reliable platoon driving evaluation is the key to ensuring the normal operation of vehicle platoons. For the vehicle motion state parameters required for the evaluation, most of them first estimate the motion state of each vehicle in the queue, and then calculate the relative motion state between vehicles. For example, the invention patent "A method for evaluating lane change performance of intelligent vehicle formations" (patent number: 202010436610.1) estimates the motion state parameters of a single vehicle through an improved unscented Kalman filter algorithm, and then calculates the relative distance, relative speed and other motion states in subsequent evaluation indicators. The relative motion parameter estimation method for intelligent vehicle platoon driving evaluation proposed in the present invention aims to directly filter and estimate the relative motion state parameters between platoon vehicles.

[0080] The purpose of the present invention is to propose a relative motion parameter estimation method for intelligent vehicle platoon driving evaluation. First, based on the motion characteristics of vehicles in platoon driving, a nonlinear dynamic model of the platoon vehicle driving process is constructed. Then, on the basis of constructing a mathematical model of the vehicle platoon system, a particle filter algorithm is used to filter and estimate the relative motion state variables such as the relative speed, distance, and relative yaw rate of the platoon vehicles. The specific steps include:

[0081] Step 1: Construct a dynamic model of the platoon vehicle driving process

[0082] First, in order to accurately describe the movement characteristics of intelligent vehicles in platooning, a platoon of four vehicles is formed. It is assumed that the first intelligent vehicle in the platoon is the pilot vehicle, which is driven in manual mode and drives according to the planned safe operation route. The remaining intelligent vehicles are followers and follow the pilot vehicle autonomously at a certain distance.

[0083] Then, the dynamic model of the platoon vehicles is simplified to be equivalent to a two-wheeled single vehicle model, as follows: Figure 1As shown in the figure, XY is the geodetic coordinate system (the X axis is specified as the east direction and the Y axis is the north direction), xy is the vehicle coordinate system, x represents the longitudinal direction of the vehicle, y represents the lateral direction of the vehicle, and the direction perpendicular to the XY coordinate system and the plane where the xy coordinate system is located pointing to the sky is specified as the Z axis direction, which conforms to the right-hand screw rule and makes the following assumptions:

[0084] (1) The factors of road pitch and roll are not considered;

[0085] (2) The relative yaw angle between the vehicle and the road is very small;

[0086] (3) The front wheel steering angle is very small;

[0087] The kinematic model constructs the vehicle model through geometric relationships, while the dynamic model constructs the vehicle model through force analysis of the vehicle in motion. Compared with the vehicle's kinematic model, the dynamic model can better reflect the dynamic changes in the vehicle formation tracking process;

[0088] Based on the above requirements and assumptions, the dynamic model of each platoon vehicle is obtained, considering the lateral, longitudinal and yaw motion of the vehicle, and assuming that the forces on the two wheels of the front axle of the vehicle are the same, the forces on the two wheels of the rear axle are also the same, the left and right front wheels are merged into one point, that is, point A, the left and right rear wheels are merged into one point, that is, point B, and point C is the center of mass of the vehicle with coordinates (X, Y); the force equations of the vehicle in the longitudinal, lateral and yaw degrees of freedom are as follows:

[0089]

[0090] In the formula, the superscripts “·” and “··” represent the first-order differential and the second-order differential, respectively, and a x is the longitudinal acceleration of the vehicle, F yf 、F yr They represent the longitudinal force and lateral force on the front and rear wheels respectively. represents the yaw angle of the vehicle, m is the mass of the vehicle, I z represents the moment of inertia around the Z axis, l f , l r Respectively represent the distances from the front and rear axles to the center of mass;

[0091] The tire slip angle is defined as the angle between the tire plane direction and the tire velocity vector direction, that is, the tire slip angle a f =δ-θ vf , where θ vf represents the angle between the vehicle's velocity vector and the vehicle's longitudinal axis, δ represents the front wheel steering angle, and the rear wheel slip angle is represented by a r =-θ vr , where θ vrIt represents the angle between the velocity vector of the rear wheel of the vehicle and the longitudinal axis of the vehicle. When the tire slip angle is small, it is proportional to the lateral force of the front and rear wheels, that is,

[0092]

[0093] In the formula, C αf and C αr is the proportional constant representing the cornering stiffness of the front and rear wheels;

[0094] θ vf and θ vr The calculation satisfies the following relationship:

[0095]

[0096] Formula (3) can be simplified by the small angle approximation method;

[0097]

[0098] Combining the transformation relationship between the earth coordinate system and the vehicle coordinate system, we get:

[0099]

[0100] The dynamic model of the pilot vehicle can be described as:

[0101]

[0102] Step 2: Vehicle relative motion state estimation based on particle filtering

[0103] The filtering recursive estimation method is used to estimate the motion state of the vehicle formation during driving. Because the system state equation described in step 2 is a nonlinear equation, a particle filter is used to obtain a comprehensive and accurate state estimation using relatively few observations.

[0104] Sub-step 1: Establish a mathematical model of the vehicle platooning system

[0105] (1) Establishing the system state equation

[0106] First, based on the dynamic model of the leading vehicle, the dynamic model of each following vehicle is recursively derived and defined as The subscript i represents the position of the vehicle in the queue, i = 1....4; and defines the relative variable

[0107] Secondly, we take the first and second vehicles in the platoon (hereinafter referred to as vehicle 1 and vehicle 2) as examples to illustrate the establishment of the system state equation and the selection of state variables. The superscript T indicates matrix transpose;

[0108] According to the dynamic equation described by formula (6), the system state equation model between vehicle 1 and vehicle 2 is established:

[0109] X=f(X,U,W,γ) (7)

[0110] Where f(.) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U = [a i_x , δ i , F i_yf , F i_yr ] T , where the subscript i represents the position of the vehicle in the queue, and a x It can be measured by the inertial measurement unit. There is a certain proportional relationship between the front wheel angle δ and the steering wheel angle ε ( ρ is the transmission ratio of the steering system), the steering wheel angle ε can be obtained through the steering wheel angle sensor, and the lateral force F of the tire yf and F yr It can be determined by the tire linear model, W is the system noise matrix, γ is the input noise matrix;

[0111] Assume that the sampling period is T, and discretize formula (7) to obtain the system state equation at time k:

[0112] X(k)=f{X(k-1), U(k-1), W(k-1), γ(k-1)} (8)

[0113] In the formula, the system process noise W = [w 1 , w 2 , w 3 , w 4 , w 5 , w 6 ] T , the Gaussian white noise covariance matrix corresponding to W(k-1) is Input Noise And the noise component is hidden in the four external inputs of the system, and the nonlinear state transfer equation f(.) is expressed as:

[0114]

[0115] in,

[0116]

[0117]

[0118]

[0119]

[0120]

[0121]

[0122] Finally, based on formula (9), the system state equation between vehicle 1 and vehicle 2 is established, and the system state equation between vehicle i and vehicle i+1 (where i≥2) in the queue is recursively derived;

[0123] (2) Establishing the observation equation

[0124] Each vehicle participating in the platoon is equipped with a dual-mode satellite navigation system receiver and an inertial measurement unit. The relative yaw angular velocity ΔZ of the two adjacent vehicles obtained by the inertial measurement unit is i_ω (ΔZ i_ω =Z i+1_ω -Z i_ω , Z i_ω represents the yaw rate of the i-th vehicle in the queue), the longitudinal velocity difference between two adjacent vehicles in the queue (in v i_x is the longitudinal speed of the i-th vehicle in the queue obtained by the satellite navigation system), and represents the distance Z between the centroids of two adjacent vehicles in the queue in the geodetic coordinate system. i_d , as the observed quantity of the system, that is in X p_i and Y p_i is the east and north position of the i-th vehicle in the formation queue that can be collected by the satellite navigation system. The observation equation can be expressed as (also taking vehicle 1 and vehicle 2 as an example):

[0125] Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (10)

[0126] In the formula, V is the observation noise, and its corresponding measurement noise covariance matrix in Determined by the statistical characteristics of the position measurement noise of the satellite navigation system, and It can be determined by the statistical characteristics of the speed and yaw rate measurement noise of the inertial measurement unit;

[0127] Similarly, the observation equation between vehicle i and vehicle i+1 (where i≥2) is established based on formula (10);

[0128] Sub-step 2: Establish the recursive process of particle filter algorithm

[0129] (1) Initialize particles

[0130] At time k, the particle set {(X j (k-1), η j (k-1))|j=1,...,N}, where η j (k-1) = 1 / N, η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is ​​Φ, and the average value is set to be the state estimate at the previous moment, that is:

[0131]

[0132] In the formula, represents the estimated value of the state at time k-1;

[0133] (2) Prediction

[0134] When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1), η j (k-1))|j=1,...,N} to obtain a new particle swarm

[0135] (3) Update particle weights and perform normalization

[0136]

[0137] In the formula, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It can be expressed as:

[0138]

[0139] Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix;

[0140] According to the observation value at the current moment, use formula (12) to update the particle weight value and perform normalization to obtain the new weight value The specific steps are:

[0141] First, the particle importance weights are updated and the prior probability distribution is selected as the importance density function, that is, The weight formula can be simplified to

[0142] Then, the weight normalization is performed, that is,

[0143] (4) State Estimation

[0144] Using the weighted particles obtained in (3) To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is,

[0145]

[0146] (5) Resampling

[0147] According to the size of the particle weight value, the particle set Resample to obtain a new particle group {(X j (k), η j (k))|i=1,...,N}, where, η j (k) = 1 / N, and return to repeat (1) for the next iteration;

[0148] After the particle filter recursive calculation composed of the above formulas (11), (12) and (13), the output state estimation is the longitudinal speed difference, lateral speed difference, coordinate difference of the center of mass of the two adjacent vehicles in the vehicle formation queue at time k, relative yaw angular velocity and relative yaw angle information;

[0149] In summary, when estimating the relative motion parameters of the intelligent vehicle platoon driving evaluation, first, based on the motion characteristics of the vehicles in the platoon driving in "step one", a nonlinear dynamic model of the platoon vehicle driving process is constructed. Then, according to "step two", on the basis of constructing the mathematical model of the vehicle platoon system, the particle filter algorithm is used to recursively estimate the relative motion state parameters such as the relative speed, distance, and relative yaw rate between adjacent vehicles in the platoon.

[0150] The relative motion parameter estimation method for intelligent vehicle platoon driving evaluation proposed in the present invention directly considers the relative motion state parameters between two adjacent vehicles in the platoon, which can improve the calculation accuracy of the actual motion state of the platoon vehicles to a certain extent; the particle filter algorithm is used for recursive deduction of the nonlinear system, which can accurately deduce the relative motion state parameters between the vehicles in the platoon. The advantages of the present invention are significant.

Claims

1. A relative motion parameter estimation method for intelligent vehicle platoon driving evaluation, characterized by: Firstly, based on the motion characteristics of vehicles in platoon, a nonlinear dynamic model of the intelligent vehicle driving process is constructed; then, the particle filter algorithm is used to filter and estimate the relative speed, relative distance, relative yaw rate and relative yaw angle of the vehicles in the platoon; the specific steps include: Step 1: Construct a dynamic model of the platoon vehicle driving process First, in order to accurately describe the movement characteristics of intelligent vehicles in platooning, a platoon of four vehicles is formed. It is assumed that the first intelligent vehicle in the platoon is the pilot vehicle, which is driven in manual mode and drives according to the planned safe operation route. The remaining intelligent vehicles are followers and follow the pilot vehicle autonomously at a certain distance. Then, the dynamic model of the platoon vehicle is simplified and equivalent to a two-wheeled single vehicle model. XY is defined as the geodetic coordinate system, where the X axis is the east direction and the Y axis is the north direction; xy is the vehicle body coordinate system, where x represents the longitudinal direction of the vehicle and y represents the lateral direction of the vehicle. The direction perpendicular to the XY coordinate system and the plane where the xy coordinate system is located pointing to the sky is defined as the Z axis direction, which conforms to the right-hand screw rule and makes the following assumptions: (1) The factors of road pitch and roll are not considered; (2) The relative yaw angle between the vehicle and the road is very small; (3) The front wheel steering angle is very small; Based on the above requirements and assumptions, the dynamic model of each platoon vehicle is obtained, considering the lateral, longitudinal and yaw motion of the vehicle, and assuming that the forces on the two wheels of the front axle of the vehicle are the same, the forces on the two wheels of the rear axle are also the same, the left and right front wheels are merged into one point, that is, point A, the left and right rear wheels are merged into one point, that is, point B, and point C is the center of mass of the vehicle with coordinates (X, Y); the force equations of the vehicle in the longitudinal, lateral and yaw degrees of freedom are as follows: In the formula, the superscripts "·" and "··" represent the first-order differential and the second-order differential, respectively, and a x is the longitudinal acceleration of the vehicle, F yf 、F yr They represent the longitudinal force and lateral force on the front and rear wheels respectively. represents the yaw angle of the vehicle, m is the mass of the vehicle, I z represents the moment of inertia around the Z axis, l f , l r Respectively represent the distances from the front and rear axles to the center of mass; The tire slip angle is defined as the angle between the tire plane direction and the tire velocity vector direction, that is, the tire slip angle a f =δ-θ vf , where θ vf represents the angle between the velocity vector of the front wheel of the vehicle and the longitudinal axis of the vehicle, δ represents the steering angle of the front wheel, and the side slip angle of the rear wheel is represented by a r =-θ vr , where θ vr It represents the angle between the velocity vector of the rear wheel of the vehicle and the longitudinal axis of the vehicle. When the tire slip angle is small, it is proportional to the lateral force of the front and rear wheels, that is, In the formula, C αf and C αr is the proportional constant representing the cornering stiffness of the front and rear wheels; θ vf and θ vr The calculation satisfies the following relationship: Formula (3) can be simplified by the small angle approximation method to: Combining the transformation relationship between the earth coordinate system and the vehicle coordinate system, we get: The dynamic model of the pilot vehicle is described as: Step 2: Vehicle relative motion state estimation based on particle filtering Sub-step 1: Establish a mathematical model of the vehicle platooning system (1) Establishing the system state equation First, based on the dynamic model of the leading vehicle, the dynamic model of each following vehicle is recursively derived and defined as The subscript i represents the position of the vehicle in the queue, i = 1…4; and defines the relative variable Secondly, the establishment of the system state equation between the first and second vehicles in the platoon is used to illustrate the process of selecting state variables. Superscript T Represents matrix transpose; According to the dynamic equation described by formula (6), the system state equation model between the first vehicle and the second vehicle is established: X=f(X,U,W,γ) (7) Where f(·) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U = [a i_x ,δ i ,F i_yf ,F i_yr ] T , where the subscript i represents the position of the vehicle in the queue, and a x Measured by the inertial measurement unit, there is a certain proportional relationship between the front wheel angle δ and the steering wheel angle ε, that is, ρ is the transmission ratio of the steering system, the steering wheel angle ε is obtained through the steering wheel angle sensor, and the lateral force F of the tire yf and F yr Determined by the tire linear model, W is the system noise matrix, and γ is the input noise matrix; Assume that the sampling period is T, and discretize formula (7) to obtain the system state equation at time k: X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)} (8) Where, the system process noise W = [w1,w2,w3,w4,w5,w6] T , the Gaussian white noise covariance matrix corresponding to W(k-1) is Input Noise And the noise component is hidden in the four external inputs of the system, and the nonlinear state transfer equation f(·) is expressed as: in, Finally, based on the system state equation between the first and second vehicles established in formula (9), the system state equation between the i-th vehicle and the i+1-th vehicle in the queue is recursively derived, where i ≥ 2; (2) Establishing the observation equation Each vehicle participating in the platoon is equipped with a dual-mode satellite navigation system receiver and an inertial measurement unit. The relative yaw angular velocity ΔZ of the two adjacent vehicles obtained by the inertial measurement unit is i_ω , the longitudinal speed difference between two adjacent vehicles in the queue and the distance Z between the centroids of two adjacent vehicles in the queue in the geodetic coordinate system i_d As the observed quantity of the system, where ΔZ i_ω =Z i+1_ω -Z i_ω , Z i_ω represents the yaw rate of the oth vehicle in the queue, v i_x is the longitudinal speed of the i-th vehicle in the queue obtained by the satellite navigation system, X p_i and Y p_i is the east and north position of the i-th car in the formation queue collected by the satellite navigation system. Similarly, the system observation equation between the first and second cars is expressed as: Z(k)=h{X(k),V(k)}=h[X(k)]+V(k) (10) In the formula, V is the observation noise, and its corresponding measurement noise covariance matrix in Determined by the statistical characteristics of the position measurement noise of the satellite navigation system, and Determined by the statistical characteristics of the velocity and yaw rate measurement noise of the inertial measurement unit; Similarly, the observation equation between the i-th vehicle and the i+1-th vehicle is established based on formula (10), where i ≥ 2; Sub-step 2: Establish the recursive process of particle filter algorithm (1) Initialize particles At time k, the particle set {(X j (k-1),η j (k-1))|j=1,…,N}, where η j (k-1) = 1 / N, η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is ​​Φ, and the average value is set to be the state estimate at the previous moment, that is: In the formula, represents the estimated value of the state at time k-1; (2) Prediction When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1),η j (k-1))|j=1,…,N} to obtain a new particle swarm (3) Update particle weights and perform normalization In the formula, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It is expressed as: Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix; According to the observation value at the current moment, use formula (12) to update the particle weight value and perform normalization to obtain the new weight value The specific steps are: First, the particle importance weights are updated and the prior probability distribution is selected as the importance density function, that is, The weight formula is simplified to Then, the weight normalization is performed, that is, (4) State Estimation Using the weighted particles obtained in (3) To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is, (5) Resampling According to the size of the particle weight value, the particle set Resample to obtain a new particle group {(X j (k),η j (k))|i=1,...,N}, where, η j (k) = 1 / N, and return to repeat (1) for the next iteration; After the particle filter recursive calculation composed of the above formulas (11), (12) and (13), the output state estimation It is the longitudinal speed difference, lateral speed difference, coordinate difference of the center of mass of two adjacent vehicles in the vehicle formation driving queue at time k, relative yaw angular velocity and relative yaw angle information; In summary, when estimating the relative motion parameters of the intelligent vehicle platoon driving evaluation, first, based on the motion characteristics of the vehicles in the platoon driving in "step one", a nonlinear dynamic model of the platoon vehicle driving process is constructed; then, according to "step two", on the basis of constructing the mathematical model of the vehicle platoon system, the particle filter algorithm is used to recursively estimate the relative speed, relative distance, relative yaw angular velocity, and relative yaw angle between adjacent vehicles in the platoon.

Citation Information

Patent Citations

  • Sensor network distribution type consistency particle filter based on Gaussian mixture model

    CN104467742A

  • Performance evaluation method for self-adaptive cruise control system

    CN111595592A

  • Intelligent vehicle formation lane changing performance evaluation method

    CN111780981A

  • Method for testing and evaluating driving performance of intelligent vehicle queue

    CN112629883A