Disclosure of Invention
The invention aims to solve the problems of low estimation precision and low response speed in the prior art.
The technical scheme adopted by the invention for solving the technical problems is that a road information estimation method combining vehicle formation with unscented Kalman filtering is provided, and comprises the following steps:
designing a state equation and a measurement equation of a road information estimation vehicle system;
The following vehicle receives the road information estimated by the front vehicle by vehicle-to-vehicle communication, and decides whether to update the road information estimated by the following vehicle by using the road information estimated by the front vehicle according to a judging module in the unscented Kalman filter;
and solving a state equation and a measurement equation by using an unscented Kalman filter to estimate the road information, wherein the road information is estimated at the last moment in the road information estimation process.
Preferably, the design road information estimation state equation and measurement equation include the following steps:
Using a 7-degree-of-freedom vehicle model that takes into account the influence of the road gradient as a model for estimating a vehicle system of road information, a state space equation of the vehicle system is expressed as:
Wherein x represents a state variable, u represents a control input variable, z represents a measurement variable, w and v represent process noise and measurement noise respectively, w and v both satisfy Gaussian distribution, and k-1 of subscripts both represent time;
the state equation is constructed based on the state space equation as follows:
Where v x is the vehicle longitudinal speed, Representing v x derivative of time, m representing vehicle mass, θ representing road grade, β representing centroid slip angle,Representing the derivative of β with respect to time; X and Y respectively represent longitudinal coordinates and transverse coordinates of the vehicle, mu is road adhesion coefficient, subscripts fl, fr, rl and rr respectively represent front left wheel, front right wheel, rear left wheel and rear right wheel, Δt is a time interval, and w k-1 is state noise.
The measurement equation is constructed based on the state equation as follows:
Wherein ax and ay are longitudinal acceleration and transverse acceleration respectively, r is yaw rate, fx and Fy are longitudinal force and transverse force of the tire respectively, subscripts fl, fr, rl and rr respectively represent front left wheel, front right wheel, rear left wheel and rear right wheel, delta represents front wheel rotation angle, lf and lr respectively represent distances from the center of mass of the vehicle to the front axle and the rear axle, k1 and k2 are longitudinal rigidity and cornering rigidity of the tire respectively, and v represents measurement noise.
Preferably, the determining module in the unscented kalman filter determines whether to update the road information estimated by itself by using the road information estimated by the preceding vehicle, and includes the following steps:
The judging module adopts a linear fitting algorithm to carry out linear fitting on the data in the sliding window, compares the data change rate in the sliding window at each moment with a set threshold value, carries out 0 or 1 assignment on phi according to the comparison result, and is expressed as:
Wherein, gamma represents the data change rate in the sliding window, W j represents the last data in the window, W 1 is the first data in the window, j represents the number of data in the window; Representing a set threshold value;
The following vehicle updates its own estimated road information with the road information estimated by the preceding vehicle only at phi=1.
Preferably, the road information estimated by the preceding vehicle includes only a road gradient and a road surface adhesion coefficient.
Preferably, the following vehicle updates its own estimated road information with the road information estimated by the preceding vehicle only at Φ=1, expressed as:
Wherein K k represents a kalman gain matrix; representing a predicted mean of the state vector, including road grade and road surface adhesion coefficients; representing the predicted mean of the observed variables, the upper left hand corner n represents the fleet member sequence.
Preferably, the solving the state equation and the measurement equation by using the unscented kalman filter includes the following steps:
Receiving longitudinal acceleration, transverse acceleration, wheel rotating speed, engine torque, front wheel rotating angle delta and tire transverse and longitudinal force calculated by a Dugoff tire model, wherein the longitudinal acceleration, the transverse acceleration, the wheel rotating speed, the engine torque and the front wheel rotating angle delta are acquired by a sensor;
Solving a state equation and a measurement equation based on the received parameters, and calculating to obtain a road surface attachment coefficient mu, a road gradient theta, a centroid slip angle beta, a vehicle mass m and a vehicle position X, Y;
calculating a road curvature rho based on the vehicle position X, Y;
The road information estimation result including the road surface attachment coefficient μ, the road gradient θ, and the road curvature ρ is output.
Preferably, the calculating the road curvature ζ based on the vehicle position X, Y includes:
The estimated vehicle abscissa and ordinate adopts a mode of calculating the circle curvature by three points, and the road curvature is calculated as follows:
Wherein xi represents curvature, x and y represent the abscissa and ordinate of the vehicle respectively, S Δ represents the area of a triangle formed by three points, when the area is smaller than a set threshold value, the three points are considered to be collinear, the curvature is zero, a Δ,bΔ,cΔ represents the three-side length of the triangle respectively, The value range is 0-1 for the filter coefficient;
The road curvature is filtered using a first order low pass filtering algorithm, expressed as:
where y (t) is the filtering result and u (t) is the value of xi.
The invention also provides a road information estimation device combining vehicle formation with unscented Kalman filtering, which comprises:
the equation building module is used for designing a state equation and a measurement equation of the road information estimation vehicle system;
The updating judging module is used for receiving road information estimated by a front vehicle by using vehicle-to-vehicle communication of a following vehicle and determining whether to update the road information estimated by the following vehicle by using the road information estimated by the front vehicle according to the judging module in the unscented Kalman filter;
and the information estimation module is used for solving a state equation and a measurement equation by using the unscented Kalman filter so as to estimate the road information, and the road information estimated at the last moment is used in the road information estimation process.
The invention has the advantages that the judging module is added in the unscented Kalman filter of the following vehicle, so as to judge whether the estimation result of the road gradient and the road surface attachment coefficient transmitted by the preceding vehicle through vehicle-to-vehicle communication is received, the estimation speed of the road surface attachment coefficient and the road gradient of the new road by the following vehicle is accelerated, the judging module can also avoid the error that the vehicle-to-vehicle communication data is lost and cannot be calculated, and the unscented Kalman filter is used for carrying out joint estimation on the road gradient, the road surface attachment coefficient and the road curvature.
The present invention will be described in further detail with reference to the drawings and examples, but the present invention is not limited to the examples.
Detailed Description
Referring to fig. 1 and 2, a method step diagram and a vehicle formation schematic diagram according to an embodiment of the present invention are shown, including the following steps:
S101, designing a state equation and a measurement equation of a road information estimation vehicle system;
S102, the following vehicle receives road information estimated by the front vehicle by vehicle-to-vehicle communication, and decides whether to update the road information estimated by itself by using the road information estimated by the front vehicle according to a judging module in the unscented Kalman filter;
S103, solving a state equation and a measurement equation by using an unscented Kalman filter to estimate the road information, wherein the road information is estimated at the last moment in the road information estimation process.
Specifically, the embodiment of the invention is mainly characterized in that (1) a road information estimation state equation and a measurement equation are designed, and (2) a judgment module of a tracking Kalman filter of a following vehicle is designed. (3) The following vehicle receives road information estimated by the preceding vehicle using vehicle-to-vehicle communication.
(1) The road information estimation state equation and the measurement equation are designed as follows:
the invention uses a 7-degree-of-freedom vehicle model that takes into account the road gradient effect, and the state space equation of the system is as follows:
Where k represents time, x represents a state variable, u represents a control input variable, w and v represent process noise and measurement noise, respectively, and satisfy a gaussian distribution.
The state equation is as follows:
The measurement equation is as follows:
v x is the vehicle longitudinal speed, m is the vehicle mass, θ is the road grade, β is the centroid slip angle, X and Y are the vehicle longitudinal and lateral coordinates, μ is the road adhesion coefficient, a x,ay is the longitudinal and lateral acceleration, r is the yaw rate, fxij, fyij is the longitudinal and lateral forces of the wheel, i is the front or rear wheel, j is the left or right wheel, δ is the front wheel angle, lf, lr are the distance of the vehicle centroid from the front and rear axles, k 1 and k 2 are the longitudinal and lateral stiffness of the tire, Δt is a time interval, respectively.
The estimated abscissa and ordinate adopt a mode of calculating the circular curvature by three points, the road curvature is calculated, and a first-order low-pass filtering algorithm is used for filtering in order to avoid high-frequency oscillation. The formula is as follows:
Wherein x and y respectively represent the abscissa and the ordinate, S Δ represents the area of a triangle formed by three points, when the area is smaller than a set threshold value, the three points are considered to be collinear, the curvature is zero, a Δ,bΔ,cΔ respectively represents the three-side length of the triangle, and xi represents the curvature. The value range is 0-1, y (t) is the filtering result, and u (t) is the sampling value.
(2) And a judgment module of the unscented Kalman filter of the following vehicle is designed. The judging module is composed of a sliding window algorithm and a linear fitting algorithm. Fitting the data in the sliding window, wherein the specific formula is as follows:
w represents the data in the window, j represents the number of data contained in the window; Indicating the threshold set, this example uses 0.05. The sliding window algorithm and the linear fitting algorithm can effectively avoid the error which cannot be calculated and is caused by the loss of the data packet generated by vehicle-to-vehicle communication.
Judging whether the following vehicle receives the estimated information of the preceding vehicle according to the condition, wherein a system update equation of the following vehicle is as follows:
Wherein n in the upper left corner represents a fleet member sequence. Phi represents a set condition for receiving the preceding vehicle estimation information, 1 represents satisfaction, and 0 represents non-satisfaction.
Specifically, the preceding vehicle transmits the estimation result of the road information to the following vehicle at the moment, but only when phi is 1, the following vehicle receives the estimation result of the preceding vehicle once. The value of phi is assigned by a judgment module of a designed unscented Kalman filter of the following vehicle, and the judgment module is formed by using a sliding window algorithm and a linear fitting algorithm. The linear fitting algorithm carries out linear fitting on the data in the sliding window, and compares the change rate of the data in the sliding window at each moment with a set threshold value so as to assign phi. The judging module can also effectively avoid the error that the data is lost and cannot be calculated due to the vehicle-to-vehicle communication by using the two algorithms.
(3) The following vehicle receives road information estimated by the preceding vehicle using vehicle-to-vehicle communication. In the joint estimation, only the road gradient and the road adhesion coefficient affecting the braking distance are aimed, so that the road estimation information transmitted by the vehicle-to-vehicle communication only comprises the two estimation results of the preceding vehicle.
Specifically, each time unit of the front vehicle transmits road adhesion coefficient and road gradient estimation results to a following vehicle behind the front vehicle through vehicle-to-vehicle communication, and the following vehicle judges whether to receive the front vehicle estimation results according to instructions given by a judging module of an unscented Kalman filter of the following vehicle. The two algorithms used by the judging module can effectively avoid the error which cannot be calculated due to the loss of data of vehicle-to-vehicle communication. After the following vehicle receives the estimated result of the front vehicle, the following vehicle uses the result to estimate the next road information by using the unscented Kalman filter.
Referring to fig. 3, a device structure diagram of an embodiment of the present invention includes:
the equation establishment module 301 designs a state equation and a measurement equation of the road information estimation vehicle system;
The update judging module 302 receives the road information estimated by the front vehicle by the following vehicle using vehicle-to-vehicle communication, and decides whether to update the road information estimated by itself by the road information estimated by the front vehicle according to the judging module in the unscented kalman filter;
the information estimation module 303 solves the state equation and the measurement equation by using the unscented kalman filter to estimate the road information, and the road information is estimated by using the road information estimated at the last time.
Therefore, the road information estimation method based on the vehicle formation combined unscented Kalman filter provided by the invention can estimate the road adhesion coefficient, the road curvature and the road gradient simultaneously without adding additional equipment, and improves the estimation speed and the estimation precision of the road adhesion coefficient and the road gradient.
The foregoing is only illustrative of the present invention and is not to be construed as limiting thereof, but rather as various modifications, equivalent arrangements, improvements, etc., within the spirit and principles of the present invention.