Road information estimation method and device based on combination of vehicle formation and unscented Kalman filtering

Abstract

The invention discloses a road information estimation method and device based on combination of vehicle formation and unscented Kalman filtering. The method comprises the following steps: designing a state equation and a measurement equation of a road information estimation vehicle system; the following vehicle receives road information estimated by the preceding vehicle by using vehicle-to-vehicle communication, and determines whether to update road information estimated by the following vehicle by using the road information estimated by the preceding vehicle or not according to a judgment module in the unscented Kalman filter; and solving the state equation and the measurement equation by using an unscented Kalman filter so as to estimate the road information, wherein the road information estimated at the previous moment is utilized in the road information estimation process. The road gradient, the road adhesion coefficient and the road curvature are jointly estimated by using the unscented Kalman filter, and compared with a traditional road information estimation method, three kinds of road information can be estimated at the same time, and the precision of estimating the road adhesion coefficient and the road gradient by the following vehicle is also improved.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle platooning, and in particular to a method and device for estimating road information of vehicle platooning combined with unscented Kalman filtering. Background Art

[0002] In modern transportation systems, vehicle platooning technology has attracted much attention due to its ability to effectively improve road traffic efficiency and reduce energy consumption. Vehicle platooning, especially in intelligent transportation systems, involves arranging autonomous vehicles in small spacing to increase road capacity and reduce exhaust emissions. However, the small spacing between vehicles in a platoon places higher demands on the real-time estimation of road information, as this is directly related to the driving safety of the platoon. Traditional road information estimation techniques, such as using extended Kalman filtering or estimation methods based on physical models, can provide basic estimates of road adhesion coefficient and road slope, but these methods are difficult to meet the requirements of fast response and high-precision estimation in vehicle platooning scenarios with high-speed driving and extremely small vehicle spacing. Especially under complex road conditions, such as sections with variable slopes and sections with variable road adhesion coefficients, the estimation accuracy and response speed of these methods often fail to meet the requirements of practical applications. Summary of the invention

[0003] The purpose of the present invention is to solve the problems of low estimation accuracy and slow response speed in the prior art.

[0004] The technical solution adopted by the present invention to solve the technical problem is: to provide a road information estimation method combining vehicle formation with unscented Kalman filtering, comprising the following steps:

[0005] Design state equations and measurement equations for the road information estimation vehicle system;

[0006] The following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and decides whether to update its own estimated road information using the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter;

[0007] An unscented Kalman filter is used to solve the state equation and the measurement equation to estimate the road information. The road information estimation process utilizes the road information estimated at the last moment.

[0008] Preferably, the designing of the road information estimation state equation and the measurement equation comprises the following steps:

[0009] The 7-DOF vehicle model considering the influence of road slope is used as the model of the road information estimation vehicle system. The state space equation of the vehicle system is expressed as:

[0010]

[0011] Where x represents the state variable, u represents the control input variable; z represents the measured variable; w and v represent the process noise and measurement noise respectively, and both w and v satisfy the Gaussian distribution; the subscripts k and k-1 represent time;

[0012] The state equation is constructed based on the state space equation as follows:

[0013]

[0014] Among them, v x is the vehicle longitudinal velocity, Indicates v x The derivative with respect to time, m represents the vehicle mass, θ represents the road slope, β represents the sideslip angle of the center of mass, represents the derivative of β with respect to time; X and Y represent the longitudinal and lateral coordinates of the vehicle respectively; μ is the road adhesion coefficient, and the subscripts fl, fr, rl and rr represent the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; Δt is a time interval, w k-1 is the state noise.

[0015] The measurement equation based on the state equation is as follows:

[0016]

[0017] Wherein, ax and ay are the longitudinal acceleration and lateral acceleration respectively; r is the yaw angular velocity; Fx and Fy are the longitudinal force and lateral force of the tire respectively; the subscripts fl, fr, rl and rr represent the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; δ represents the front wheel turning angle; lf and lr are the distances from the center of mass of the vehicle to the front axle and the rear axle respectively; k1 and k2 are the longitudinal stiffness and lateral stiffness of the tire respectively; and v represents the measurement noise.

[0018] Preferably, the determining whether to update the estimated road information of the vehicle itself using the estimated road information of the preceding vehicle according to the judgment module in the unscented Kalman filter comprises the following steps:

[0019] The judgment module uses a linear fitting algorithm to perform linear fitting on the data in the sliding window, compares the data change rate in the sliding window at each moment with the set threshold, and assigns φ 0 or 1 according to the comparison result, which is expressed as:

[0020]

[0021] Where γ represents the rate of change of data in the sliding window; W j represents the last data in the window, W1 is the first data in the window, and j represents the number of data in the window; Indicates the set threshold;

[0022] The following vehicle updates its own estimated road information using the road information estimated by the preceding vehicle only when φ=1.

[0023] Preferably, the road information estimated by the preceding vehicle only includes the road slope and the road adhesion coefficient.

[0024] Preferably, the following vehicle updates its own estimated road information using the road information estimated by the preceding vehicle only when φ=1, which is expressed as:

[0025]

[0026] Among them, K k represents the Kalman gain matrix; represents the predicted mean of the state vector, including the road slope and the road adhesion coefficient; Represents the predicted mean of the observed variable, and n in the upper left corner represents the fleet member sequence.

[0027] Preferably, the method of solving the state equation and the measurement equation by using an unscented Kalman filter comprises the following steps:

[0028] Receive the longitudinal acceleration, lateral acceleration, wheel speed, engine torque, front wheel steering angle δ collected by the sensor, and the tire lateral and longitudinal forces calculated by the Dugoff tire model;

[0029] Solve the state equation and measurement equation based on the received parameters to calculate the road adhesion coefficient μ, road slope θ, center of mass sideslip angle β, vehicle mass m and vehicle position X, Y;

[0030] The road curvature ρ is calculated based on the vehicle position X, Y;

[0031] The output includes the road information estimation results of the road adhesion coefficient μ, road slope θ and road curvature ρ.

[0032] Preferably, the calculating of the road curvature ξ based on the vehicle positions X, Y comprises:

[0033] For the estimated vehicle horizontal and vertical coordinates, the road curvature is calculated by using the three-point circular curvature method, which is expressed as:

[0034]

[0035]

[0036] Where ξ represents the curvature, x and y represent the horizontal and vertical coordinates of the vehicle, respectively. Δ Represents the area of ​​a triangle formed by three points. When the area is less than the set threshold, the three points are considered to be collinear and the curvature is zero. Δ , b Δ , c ΔThey represent the lengths of the three sides of the triangle, is the filter coefficient, ranging from 0 to 1;

[0037] The road curvature is filtered using a first-order low-pass filtering algorithm, which is expressed as:

[0038]

[0039] Among them, y(t) is the filtering result, and u(t) is the sampling value of ξ.

[0040] The present invention also provides a road information estimation device combining vehicle formation with unscented Kalman filtering, comprising:

[0041] Equation building module, designing the state equation and measurement equation of the road information estimation vehicle system;

[0042] Update judgment module, the following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and decides whether to update its own estimated road information using the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter;

[0043] The information estimation module uses an unscented Kalman filter to solve the state equation and the measurement equation to estimate the road information. The road information estimation process uses the road information estimated at the last moment.

[0044] The present invention has the following beneficial effects: the present invention adds a judgment module to the unscented Kalman filter of the following vehicle, so as to judge whether to receive the estimation results of the road slope and the road adhesion coefficient transmitted by the leading vehicle through the vehicle-to-vehicle communication, thereby accelerating the estimation speed of the road adhesion coefficient and the road slope of the new road by the following vehicle, and the judgment module can also avoid the error that cannot be calculated due to the loss of vehicle-to-vehicle communication data; and the present invention uses the unscented Kalman filter to jointly estimate the road slope, the road adhesion coefficient and the road curvature. Compared with the traditional road information estimation method, this can not only estimate three kinds of road information at the same time, but also improve the accuracy of the following vehicle in estimating the road adhesion coefficient and the road slope.

[0045] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments, but the present invention is not limited to the embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 A method step diagram of an embodiment of the present invention;

[0047] Figure 2 A schematic diagram of a vehicle formation according to an embodiment of the present invention;

[0048] Figure 3 FIG. 4 is a diagram showing the structure of a device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0049] See also Figure 1 and Figure 2 As shown, it is a method step diagram and a vehicle formation schematic diagram of an embodiment of the present invention, which includes the following steps:

[0050] S101, designing state equations and measurement equations of a road information estimation vehicle system;

[0051] S102, the following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and determines whether to update its own estimated road information using the road information estimated by the preceding vehicle according to a judgment module in the unscented Kalman filter;

[0052] S103, using an unscented Kalman filter to solve the state equation and the measurement equation to estimate the road information, wherein the road information estimation process uses the road information estimated at the last moment.

[0053] Specifically, the embodiments of the present invention focus on: (1) designing the state equation and measurement equation for estimating road information; (2) designing the judgment module of the unscented Kalman filter of the following vehicle; and (3) the following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication.

[0054] (1) Design of road information estimation state equation and measurement equation, as follows:

[0055] The present invention uses a 7-DOF vehicle model that takes into account the influence of road slope, and the state space equation of the system is as follows:

[0056]

[0057] Among them, k represents time, x represents state variable, u represents control input variable, w and v represent process noise and measurement noise respectively, and they satisfy Gaussian distribution.

[0058] The state equation is as follows:

[0059]

[0060] The measurement equation is as follows:

[0061]

[0062] v x is the longitudinal velocity of the vehicle, m is the mass of the vehicle, θ is the road slope, β is the sideslip angle of the center of mass, X and Y are the longitudinal and lateral coordinates of the vehicle, μ is the road adhesion coefficient, and a x , a yare the longitudinal acceleration and lateral acceleration respectively, r is the yaw angular velocity, Fxij, Fyij are the longitudinal and lateral forces of the wheels respectively, i represents the front or rear wheel, j represents the left or right wheel, δ represents the front wheel turning angle, lf and lr are the distances from the center of mass of the vehicle to the front axle and the rear axle respectively, k1 and k2 are the longitudinal and lateral stiffness of the tire respectively, and Δt is a time interval.

[0063] For the estimated horizontal and vertical coordinates, the three-point circular curvature method is used to calculate the road curvature. In order to avoid high-frequency oscillation, a first-order low-pass filter algorithm is used for filtering. The formula is as follows:

[0064]

[0065] Among them, x and y represent the horizontal and vertical coordinates respectively, S Δ Represents the area of ​​a triangle formed by three points. When the area is less than the set threshold, the three points are considered to be collinear and the curvature is zero. Δ , b Δ , c Δ They represent the lengths of the three sides of the triangle, and ξ represents the curvature. is the filtering coefficient, ranging from 0 to 1; y(t) is the filtering result, and u(t) is the sampling value.

[0066] (2) Design of the judgment module of the unscented Kalman filter of the following vehicle. The judgment module consists of a sliding window algorithm and a linear fitting algorithm. The data in the sliding window is fitted, and the specific formula is as follows:

[0067]

[0068] W represents the data in the window, j represents the number of data contained in the window; The sliding window algorithm and the linear fitting algorithm can effectively avoid the incalculable errors caused by the loss of data packets generated by vehicle-to-vehicle communication.

[0069] According to the conditions, it is determined whether the following vehicle receives the estimated information of the preceding vehicle. The system update equation of the following vehicle is:

[0070]

[0071] Among them, n in the upper left corner represents the sequence of team members. φ represents the set condition for receiving the estimation information of the preceding vehicle, 1 means it is satisfied, and 0 means it is not satisfied.

[0072] Specifically, the leading vehicle always transmits its own estimation result of road information to the following vehicle, but the following vehicle receives the estimation result of the leading vehicle only once when φ is 1. The value of φ is assigned by the judgment module of the designed unscented Kalman filter of the following vehicle, which is composed of a sliding window algorithm and a linear fitting algorithm. The linear fitting algorithm linearly fits the data in the sliding window, compares the data change rate in the sliding window at each moment with the set threshold, and then assigns φ. The judgment module uses these two algorithms to effectively avoid errors caused by data loss and inability to calculate due to vehicle-to-vehicle communication.

[0073] (3) The following vehicle receives the road information estimated by the leading vehicle through vehicle-to-vehicle communication. During the joint estimation, only the road slope and road adhesion coefficient that affect the braking distance are considered. Therefore, the road estimation information transmitted by vehicle-to-vehicle communication only contains these two estimation results of the leading vehicle.

[0074] Specifically, the leading vehicle transmits the estimated results of the road adhesion coefficient and road slope to a following vehicle behind it through vehicle-to-vehicle communication every time unit. The following vehicle determines whether to receive the estimation results of the leading vehicle according to the instructions given by the judgment module of its own unscented Kalman filter. The two algorithms used in the judgment module can effectively avoid the data loss of vehicle-to-vehicle communication and the errors that cannot be calculated. After the following vehicle receives the estimation results of the leading vehicle, its own unscented Kalman filter uses this result to estimate the next road information.

[0075] See also Figure 3 FIG. 1 is a diagram showing a structure of a device according to an embodiment of the present invention, comprising:

[0076] An equation building module 301 designs state equations and measurement equations of a road information estimation vehicle system;

[0077] Update judgment module 302, the following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and decides whether to update its own estimated road information using the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter;

[0078] The information estimation module 303 uses an unscented Kalman filter to solve the state equation and the measurement equation to estimate the road information. The road information estimation process uses the road information estimated at the last moment.

[0079] It can be seen that the road information estimation method based on vehicle platooning and unscented Kalman filter proposed by the present invention can simultaneously estimate the road adhesion coefficient, road curvature and road slope without adding additional equipment, and improve the estimation speed and accuracy of the road adhesion coefficient and road slope.

[0080] The above are only preferred embodiments of the present invention and are not intended to limit 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 method for estimating road information by combining vehicle platooning with unscented Kalman filtering, characterized in that: The following steps are involved: Design state equations and measurement equations for the road information estimation vehicle system; The following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and decides whether to update its own estimated road information using the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter; An unscented Kalman filter is used to solve the state equation and the measurement equation to estimate the road information. The road information estimation process utilizes the road information estimated at the last moment.

2. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 1, characterized in that: The design of the road information estimation state equation and the measurement equation comprises the following steps: The 7-DOF vehicle model considering the influence of road slope is used as the model of the road information estimation vehicle system. The state space equation of the vehicle system is expressed as: Where x represents the state variable, u represents the control input variable; z represents the measured variable; w and v represent the process noise and measurement noise respectively, and both w and v satisfy the Gaussian distribution; the subscripts k and k-1 represent time; The state equation is constructed based on the state space equation as follows: Among them, v x is the vehicle longitudinal velocity, Indicates v x The derivative with respect to time, m represents the vehicle mass, θ represents the road slope, β represents the sideslip angle of the center of mass, represents the derivative of β with respect to time; X and Y represent the longitudinal and lateral coordinates of the vehicle respectively; μ is the road adhesion coefficient, and the subscripts fl, fr, rl and rr represent the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; Δt is a time interval, w k-1 is the state noise. The measurement equation based on the state equation is as follows: Wherein, ax and ay are the longitudinal acceleration and lateral acceleration respectively; r is the yaw angular velocity; Fx and Fy are the longitudinal force and lateral force of the tire respectively; the subscripts fl, fr, rl and rr represent the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; δ represents the front wheel turning angle; lf and lr are the distances from the center of mass of the vehicle to the front axle and the rear axle respectively; k1 and k2 are the longitudinal stiffness and lateral stiffness of the tire respectively; and v represents the measurement noise.

3. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 1, characterized in that: The method of determining whether to use the road information estimated by the preceding vehicle to update the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter comprises the following steps: The judgment module uses a linear fitting algorithm to perform linear fitting on the data in the sliding window, compares the data change rate in the sliding window at each moment with the set threshold, and assigns φ 0 or 1 according to the comparison result, which is expressed as: Where γ represents the rate of change of data in the sliding window; W j represents the last data in the window, W1 is the first data in the window, and j represents the number of data in the window; Indicates the set threshold; The following vehicle updates its own estimated road information using the road information estimated by the preceding vehicle only when φ=1.

4. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 3, characterized in that: The road information estimated by the preceding vehicle only includes the road slope and the road adhesion coefficient.

5. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 4, characterized in that: The following vehicle updates its own estimated road information only when φ=1 using the road information estimated by the preceding vehicle, which is expressed as: Among them, K k represents the Kalman gain matrix; represents the predicted mean of the state vector, including the road slope and the road adhesion coefficient; Represents the predicted mean of the observed variable, and n in the upper left corner represents the fleet member sequence.

6. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 1, characterized in that: The method of solving the state equation and the measurement equation by using an unscented Kalman filter comprises the following steps: Receive the longitudinal acceleration, lateral acceleration, wheel speed, engine torque, front wheel steering angle δ collected by the sensor, and the tire lateral and longitudinal forces calculated by the Dugoff tire model; Solve the state equation and measurement equation based on the received parameters to calculate the road adhesion coefficient μ, road slope θ, center of mass sideslip angle β, vehicle mass m and vehicle position X, Y; The road curvature ρ is calculated based on the vehicle position X, Y; The output includes the road information estimation results of the road adhesion coefficient μ, road slope θ and road curvature ρ.

7. The method for estimating road information by combining vehicle platooning with unscented Kalman filtering according to claim 6, characterized in that: The method of calculating the road curvature ξ based on the vehicle positions X and Y includes: For the estimated vehicle horizontal and vertical coordinates, the road curvature is calculated by using the three-point circular curvature method, which is expressed as: Where ξ represents the curvature, x and y represent the horizontal and vertical coordinates of the vehicle, respectively. Δ Represents the area of ​​a triangle formed by three points. When the area is less than the set threshold, the three points are considered to be collinear and the curvature is zero. Δ , b Δ , c Δ They represent the lengths of the three sides of the triangle, is the filter coefficient, ranging from 0 to 1; The road curvature is filtered using a first-order low-pass filtering algorithm, which is expressed as: Among them, y(t) is the filtering result, and u(t) is the sampling value of ξ.

8. A road information estimation device combining vehicle platooning with unscented Kalman filtering, characterized in that: include: Equation building module, designing the state equation and measurement equation of the road information estimation vehicle system; Update judgment module, the following vehicle receives the road information estimated by the preceding vehicle through vehicle-to-vehicle communication, and decides whether to update its own estimated road information using the road information estimated by the preceding vehicle according to the judgment module in the unscented Kalman filter; The information estimation module uses an unscented Kalman filter to solve the state equation and the measurement equation to estimate the road information. The road information estimation process uses the road information estimated at the last moment.

Images