CN110532590A - A kind of vehicle state estimation method based on adaptive volume particle filter - Google Patents
A kind of vehicle state estimation method based on adaptive volume particle filter Download PDFInfo
- Publication number
- CN110532590A CN110532590A CN201910627592.2A CN201910627592A CN110532590A CN 110532590 A CN110532590 A CN 110532590A CN 201910627592 A CN201910627592 A CN 201910627592A CN 110532590 A CN110532590 A CN 110532590A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- state
- algorithm
- equation
- volume
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 239000002245 particle Substances 0.000 title claims abstract description 63
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 30
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 34
- 238000005259 measurement Methods 0.000 claims description 29
- 239000011159 matrix material Substances 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000012952 Resampling Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 4
- 238000013016 damping Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000001172 regenerating effect Effects 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 2
- 238000004088 simulation Methods 0.000 abstract description 9
- 238000013461 design Methods 0.000 abstract description 4
- 230000006870 function Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000001052 transient effect Effects 0.000 description 5
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 239000010426 asphalt Substances 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010835 comparative analysis Methods 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010304 firing Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005312 nonlinear dynamic Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Computational Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Operations Research (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
The invention discloses a kind of vehicle state estimation methods for being based on adaptive volume particle filter (ACPF), comprising: is primarily based on unstable state Dynamic tire model, constructs high-dimensional non-linear eight degrees of freedom vehicle dynamic model；Secondly the importance density function of elementary particle filtering algorithm is updated using adaptive volume Kalman filtering algorithm, and adaptive volume particle filter algorithm design is completed with this；Using onboard sensor information, realize with ACPF algorithm to the key stato variables high-precision online observation such as angle of heel, side slip angle of vehicle.It finally builds Simulink-Carsim union simulation platform and has carried out the verifying of algorithm, the results showed that the algorithm state estimated accuracy is higher than tradition without mark particle filter (UPF) algorithm, and algorithm operation efficiency is higher than UPF algorithm.
Description
Technical Field
The invention relates to a vehicle state estimation method based on adaptive volume particle filtering (ACPF), and belongs to the technical field of vehicle monitoring.
Background
With the rise and the firing of intelligent vehicles, the requirements for controlling the entire vehicle based on vehicle dynamics become higher and higher, wherein accurate recognition of the state of the vehicle itself is inevitably involved. When a driver drives a car, the driver can perceptively know some state variables of the car, such as the roll angle of the car body, but the intelligent car controller cannot do the same, so that the cheap car-mounted sensor is used for obtaining the known state information to estimate some unknown states of the car, and the visualization of the data of necessary state parameters required by dynamic control is particularly important.
At present, the main methods for estimating the vehicle state include kalman filtering, particle filtering, fuzzy logic method, neural network method, synovial observer method, Luenberger observer method, robust observer method, Extended Kalman Filtering (EKF), Unscented Kalman Filtering (UKF), volumetric kalman filtering (CKF), Unscented Particle Filtering (UPF) and the like which are developed on the basis of the classic Kalman Filtering (KF) algorithm and the basic Particle Filtering (PF) algorithm. However, there are various problems: in the fuzzy logic method, scale factors and quantization factors are difficult to determine, the neural network method depends on the performance of a sensor and a processor, the buffeting phenomenon exists when the state estimation is carried out by the sliding film observer method, the Luenberger observer method has underestimation deviation under certain conditions, the EKF needs to carry out first-order Taylor expansion on a state equation, the estimation precision is reduced, the UKF and the CKF do not need to calculate a Jacobian matrix of a nonlinear function, the estimation precision is improved, but the divergence phenomenon possibly occurs in the non-Gaussian system state estimation; particle filtering shows better performance in processing nonlinear non-Gaussian system state estimation, but particle degradation is easy to occur due to the difficulty in selecting an importance density function.
Disclosure of Invention
The purpose of the invention is as follows: in order to overcome the defects in the prior art, the invention provides a vehicle state estimation method based on adaptive volume particle filtering, which designs a particle filtering importance density function through adaptive volume Kalman filtering (ACKF), introduces the latest observation data of a sensor, and forms adaptive volume particle filtering (ACPF), thereby approaching the posterior probability density of the system state more under the condition of considering the nonlinearity and non-Gaussian characteristic of the system and effectively improving the estimation precision of the system state.
The technical scheme is as follows: in order to achieve the purpose, the invention adopts the technical scheme that:
a vehicle state estimation method based on adaptive volume particle filtering comprises the following steps:
step 1, establishing a vehicle dynamics model by applying a Dalnberg principle according to a state variable to be estimated and information obtained by a vehicle-mounted sensor, and converting the model into a state equation and a measurement equation;
step 2, algorithm initialization: determining a volume point and a corresponding weight according to a spherical radial criterion, and extracting particles and a covariance matrix from an initial state probability density distribution function;
step 3, designing a particle filter importance density sampling function by adopting ACKF, introducing latest acquired data of a sensor, and generating a predicted particle set and a corresponding variance;
step 4, regenerating particles, calculating importance weight and normalizing the weight;
step 5, resampling the particle set according to the weight normalization result;
step 6, calculating a state estimation value and an error covariance matrix of the particle filter at the current moment;
step 7, judging whether the state estimation is finished or not, if so, outputting an estimation result and quitting the state estimation; if not, outputting the estimation result, inputting the estimation result and the latest observation data into the step 3, and continuing to carry out state estimation.
On the basis of the CKF algorithm, the invention introduces the adaptive factor to improve the influence of observation abnormity, forms the adaptive volume Kalman (ACKF) to design the importance density function of the PF algorithm, and introduces the latest observation data to form the adaptive volume particle filter (ACPF) to carry out state estimation.
On the basis of a Doguff tire model, a dynamic tire model is introduced to establish an unsteady state dynamic tire model, an eight-degree-of-freedom vehicle model including a side roll, a yaw, a longitudinal direction and a lateral direction is established, an ACPF algorithm is selected to estimate the vehicle state, and parameters of the longitudinal vehicle speed, the lateral vehicle speed, the yaw angular speed, the roll angular speed and the centroid roll angle are observed.
Further, the vehicle-mounted sensors include wheel speed sensors, a longitudinal acceleration sensor, a lateral acceleration sensor, a roll angle speed sensor, a yaw rate sensor, and a steering wheel angle sensor.
Further, the state equation and the measurement equation of the vehicle system model are in the form of:
wherein,for the state variable to be estimated, u (t) ═ δ, wfl,wfr,wrl,wrr]' is an input variable, z (t) ═ ax,ay,p,wr]' is an observation variable, the observation variable and an input variable are information obtained by a vehicle-mounted sensor, and w (t), v (t) are process noise and measurement noise respectively;
further, the equation of state f (x (t), u (t)) is:
the measurement equation h (x (t), u (t)) is:
in the formula, x1＝Vx，x2＝Vy，x3＝wr，x4＝p，
∑Fy＝Fxflsinδfl+Fxfrsinδfr+Fyflcosδfl+Fyfrcosδfr+Fyrl+Fyrr
∑Fx＝Fxflcosδfl+Fxfrcosδfr-Fyflsinδfl-Fyfrsinδfr+Fxrl+Fxrr
Wherein m is the total mass of the vehicle, msFor sprung mass of the vehicle, VxIs the longitudinal speed of the vehicle mass center, VyIs the vehicle centroid lateral velocity, wrThe centroid yaw rate, e the roll arm height,is the centroid roll angle, p is the centroid roll angle velocity, delta is the steering wheel angle, Sigma FxFor the resultant longitudinal force of vehicle tyres, ∑ FyFor resultant force in the lateral direction of vehicle tyre, axIs the longitudinal acceleration of the vehicle centroid, ayFor the vehicle centroid lateral acceleration, i ═ f, r denote front and rear wheels, respectively, j ═ l, r denote left and right wheels, respectively, and w representsijAs the wheel speed, deltaijIs the tire slip angle, FxijIs a tire longitudinal force, FyijFor tire side force, ∑ MxFor vehicle roll moment, [ sigma ] MzYaw moment for vehicle, IxxsMoment of inertia about the x-axis for sprung mass of the vehicle, IxzsIs the product of rotational inertia, I, of sprung mass of the vehicle about the x and z axeszzThe yaw moment of inertia of the vehicle, g the acceleration of gravity,in order to provide the roll rigidity of the vehicle,for vehicle roll damping, a is the distance of the vehicle center of mass from the front axle, b is the distance of the vehicle center of mass from the rear axle, dfFor the front axle track of the vehicle, drIs the rear axle track of the vehicle.
Further, in step 2, the algorithm initialization process specifically includes: inputting fixed parameter information, initial values of quantities to be estimated, covariance matrixes of process noise and measurement noise, error covariance matrixes, particle numbers, filter parameters and sampling time in a state equation and a measurement equation;
then, 2M volume points and corresponding weights are generated according to the spherical radial criterion, that is:
wherein M is the dimension of the state equation, ξnIs the nth volume point, ωcnRepresents the nth weight, [ I ]]nRepresenting a set of volume points [ I]Column n, using the three-order volume principle, volume point set [ I]Comprises the following steps: [ I ] of]＝[I1，-I1]，I1Is a 5 × 5 unit matrix;
from initial state probability distribution density p (x)0) Extracting particles
In the formula (I), the compound is shown in the specification,as the initial particles, the particles are,is the mean value of the initial particles,initial particle error covariance.
Further, the step 3 is divided into a time updating process and a measurement updating process, and the specific steps are as follows:
3.1 time update:
for error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
calculate the Cubature point conducted by the equation of state:
updating the state prediction value and the error covariance prediction value:
wherein, time k is 1,2,3,is an estimate of the state at time k-1, uk-1Input quantity, w, obtained by the sensor for the moment k-1k-1Is the process noise at time k-1.
3.2 measurement update:
for the updated error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
the Cubature point conducted by the measurement equation is calculated:
updating the measurement one-step prediction:
updating the prediction value of the covariance of the measurement error:
calculating the cross covariance:
calculating a volume Kalman filtering gain matrix:
calculating a state estimation value after volume Kalman filtering:
calculating an error covariance estimate after volumetric Kalman filtering:
wherein alpha iskFor the adaptive factor, the calculation equation is:
in the formula,c0∈(1.0～1.5)，c1∈(3.0～8.0)
further, in the step 5, the resampling method selects residual resampling compatible with the calculated amount and performance.
Has the advantages that: compared with the prior art, the vehicle state estimation method based on the adaptive volume particle filtering has the following advantages that:
1. the state parameters in the whole vehicle control process are estimated through the information acquired by the vehicle-mounted cheap sensor, the information directly acquired by the sensor is filtered as much as possible, the information which cannot be directly acquired by the sensor is efficiently estimated, the estimation precision is high, and the whole vehicle control requirement is met;
2. introducing adaptive factors to improve the influence of observation abnormity, designing a particle filter importance density function through adaptive volume Kalman filtering (ACKF), introducing the latest observation data of a sensor to form adaptive volume particle filtering (ACPF), so that the posterior probability density of the system state is more approximated under the condition of considering both the nonlinearity and the non-Gaussian characteristic of the system, and the estimation precision of the system state is improved;
3. under the same accuracy estimation requirement, the ACPF algorithm running time is shorter than the UPF algorithm.
Drawings
FIG. 1 is a flow chart of an adaptive volumetric particle filtering (ACPF) algorithm in accordance with the present invention;
FIGS. 2a and 2b are a top view and a front view, respectively, of a vehicle dynamics model in accordance with the present invention;
FIG. 3 is a diagram of a state estimation simulink simulation architecture in an embodiment of the present invention;
FIGS. 4a and 4b are a lateral acceleration diagram and a rotational speed diagram of each wheel, respectively, for simulated operating conditions in an embodiment of the present invention;
FIGS. 5a and 5b are a longitudinal velocity estimation result and a longitudinal velocity estimation absolute error map of a simulation condition according to an embodiment of the present invention, respectively;
FIGS. 6a and 6b are a lateral velocity estimation result and a lateral velocity estimation absolute error map of a simulation condition according to an embodiment of the present invention, respectively;
FIGS. 7a and 7b are a centroid slip angle estimation result and a centroid slip angle estimation absolute error diagram of a simulation condition in an embodiment of the present invention, respectively;
FIGS. 8a and 8b are a diagram of the yaw-rate estimation result and the absolute error of the yaw-rate estimation, respectively, for a simulated operating condition in an embodiment of the present invention;
FIGS. 9a and 9b are respectively a vehicle body roll angle speed estimation result and a vehicle body roll angle speed estimation absolute error map of a simulation condition in an embodiment of the present invention;
FIGS. 10a and 10b are respectively a vehicle body roll angle estimation result and a vehicle body roll angle estimation absolute error map of a simulation condition in an embodiment of the present invention.
Detailed Description
The vehicle state estimation method according to the present invention will be described in detail below with reference to the drawings and embodiments.
As shown in fig. 2a and 2b, an eight degree of freedom vehicle dynamics model is first built.
The longitudinal kinematic equation is:
the lateral kinetic equation is:
the roll motion dynamics equation is:
the yaw motion dynamic equation is as follows:
wherein m is the total mass of the vehicle, msFor sprung mass of the vehicle, VxIs the longitudinal speed of the vehicle mass center, VyIs the vehicle centroid lateral velocity, wrThe centroid yaw rate, e the roll arm height,is the centroid roll angle, p is the centroid roll angle velocity, delta is the steering wheel angle, Sigma FxFor the resultant longitudinal force of vehicle tyres, ∑ FyFor resultant force in the lateral direction of vehicle tyre, axIs the longitudinal acceleration of the vehicle centroid, ayIs the vehicle centroid lateral acceleration; i ═ f, r denote front and rear wheels, respectively, j ═ l, r denote left and right wheels, respectively, and w representsijAs the wheel speed, deltaijIs the tire slip angle, FxijIs a tire longitudinal force, FyijFor tire side force, ∑ MxFor vehicle roll moment, [ sigma ] MzYaw moment for vehicle, IxxsMoment of inertia about the x-axis for sprung mass of the vehicle, IxzsIs the product of rotational inertia, I, of sprung mass of the vehicle about the x and z axeszzThe yaw moment of inertia of the vehicle, g the acceleration of gravity,in order to provide the roll rigidity of the vehicle,for vehicle roll damping, a is the distance of the vehicle center of mass from the front axle, b is the distance of the vehicle center of mass from the rear axle, dfFor the front axle track of the vehicle, drIs the rear axle track of the vehicle.
At present, widely used non-linear tire models include theoretical models, semi-empirical models and empirical models, and the theoretical tire models based on advanced analysis are adopted in the text: the Doguff tire model, namely:
in the formula, Cxij、CyijThe longitudinal and lateral cornering stiffness of the wheel.
The Doguff tire model established in the formula is more suitable for a steady-state tire, and in order to better describe the nonlinear characteristics of the tire, the dynamic tire model relaxation time constant tau is introduced on the basis of the steady-state tire modelij_lagTo illustrate the hysteresis effect of the non-linear tire force, so as to obtain the non-steady-state dynamic tire model, namely:
in the formula, σijIs a relaxation factor; fxij_stat，Fyij_statCalculated as the steady state tire force by the douff tire model.
The calculation formula of other parameters is as follows:
centroid slip angle:
forward speed of each wheel:
tire slip angle:
tire vertical force:
wherein,
the slip ratio is defined as:
in the formula, VijAs the advancing speed of the wheel, musf、musrThe front and rear unsprung masses of the vehicle, h is the height of the center of mass of the whole vehicle, and Lfs、LrsRespectively the distance h from the center of mass of the sprung mass to the front and rear axesrf、hrrRespectively the front and rear roll center height, huf、hurRespectively, front and rear unsprung mass center of mass height, wijR is the wheel rotational angular velocity and R is the wheel effective radius.
The conversion into the form of the equation of state and the equation of measurement according to the established kinetic model is as follows
Obtaining f (x) (t), u (t) and h (x (t), u (t)) as follows:
wherein, Sigma FxSum Σ FyE and Σ M as shown in the kinetic equationxAs follows:
wherein,is a state variable to be estimated; u (t) ═ δ, wfl,wfr,wrl,wrr]' is an input variable; z (t) ═ ax,ay,p,wr]' is an observation variable, and information of the observation variable and an input variable is obtained by a vehicle-mounted sensor; w (t), v (t) are process noise and measurement noise, respectively.
Starting to perform state estimation according to the established state equation, measurement equation and information obtained by the vehicle-mounted sensor, as shown in fig. 1:
when k is 0:
2M (M is the dimension of the state equation, here 5) volume points and corresponding weights are generated according to the spherical radial criterion, i.e.:
wherein ξnIs the nth volume point, ωcnRepresents the nth weight, [ I ]]nRepresenting a set of volume points [ I]Column n, using the third order volume principle, volume point set [ I ] in this text]Comprises the following steps: [ I ] of]＝[I1，-I1]，I1Is a 5 x 5 identity matrix.
From initial state probability distribution density p (x)0) Extracting particlesAnd is
When k is 1,2, ·, then:
first, the ACKF is used to design the importance sampling density function, yielding a set of predicted particles and corresponding variances (N is the number of particles), which comprises time updating and measurement updating, and comprises the following steps:
and (3) time updating:
for error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
calculate the Cubature point conducted by the equation of state:
updating the state one-step prediction value and the error covariance prediction value:
time k is 1,2,3, ·,is an estimate of the state at time k-1, uk-1Input quantity, w, obtained by the sensor for the moment k-1k-1For process noise at time k-1。
Measurement updating:
for the updated error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
the Cubature point conducted by the measurement equation is calculated:
updating the measurement one-step prediction:
updating the prediction value of the covariance of the measurement error:
calculating the cross covariance:
calculating a volume Kalman filtering gain matrix:
calculating a state estimation value after volume Kalman filtering:
calculating an error covariance estimate after volumetric Kalman filtering:
wherein alpha iskFor the adaptive factor, the calculation equation is:
in the formula,c0∈(1.0～1.5)，c1∈(3.0～8.0)
resampling and calculating the estimate:
and (3) regenerating particles:
calculating an importance weight:
weight normalization:
resampling the particle set according to the normalization result to obtain the particle set with equal weight value of 1/N
And calculating a state estimation value and an error covariance estimation value of the particle filter:
at this moment, the algorithm outputs a state estimation value and judges whether the algorithm is finished, if not, the state estimation value and the error covariance estimation value are input into the algorithm for continuous estimation; if so, the algorithm ends.
The correctness of the state estimation method is verified by combining a specific embodiment, an MATLAB/Simulink and Carsim combined simulation platform is built, the structure is shown in fig. 3, the parameters of the whole vehicle are input, the initial vehicle speed is 120km/h, an asphalt pavement with a high adhesion coefficient is selected, the state estimation results of an adaptive volume particle filter (ACPF) algorithm and an Unscented Particle Filter (UPF) algorithm are compared under the double-moving-wire working condition, the simulation working condition is shown in fig. 4, and the estimation results and the comparative analysis results are shown in fig. 5 to 10.
As can be seen from FIG. 4a, the maximum lateral acceleration is 6.07m/s2At the moment, the tire is in a nonlinear working area, and the whole vehicle is in a strong nonlinear state; it can be seen from fig. 4b that the vehicle starts to enter the double-shift line condition from the time point of 1.2s, the rotating speeds of the left and right wheels of the automobile start to deviate, and the double-shift line condition is finished at the time point of 7.6 s. The starting time of the rotational angular speeds of the front and rear wheels is the same, but the simulated vehicle is a front-driving vehicle, so the rotational speeds of the wheels on the two sides of the same axle are the same after the initialization of 0.3s and before the double-line-shifting working condition and after the double-line-shifting working condition, and the rotational speed of the wheel on the front axle is slightly larger than that of the wheel on the rear axle.
Fig. 5 to 7 are absolute error graphs of the estimation values and the estimation values of the longitudinal vehicle speed, the lateral vehicle speed and the centroid slip angle relative to the reference values by the ACPF algorithm and the UPF algorithm, respectively. From the estimated value graph, it can be seen that the two estimation algorithms can stably track the reference value from the beginning, and from the absolute error graph of the estimated value relative to the reference value, it can be seen that the maximum transient deviations of the ACPF algorithm to the estimated values of the longitudinal speed, the lateral speed and the centroid slip angle are respectively reduced by 0.11m/s2、0.05m/s20.09. The absolute error trends of fig. 6 and fig. 7 are substantially the same, because the centroid slip angle is calculated from the longitudinal velocity and the lateral velocity, the longitudinal velocity estimation of the two algorithms is substantially consistent, and the accuracy of the ACPF algorithm for estimating the lateral velocity is improved by 3%, so that the accuracy of the ACPF for estimating the centroid slip angle is also improved by 3%.
Fig. 8 to 10 are absolute error maps of estimated values and estimated values of yaw rate, roll rate, and roll angle with respect to reference values by the ACPF algorithm and the UPF algorithm, respectively. It can be seen from the estimated value map that both estimation algorithms can stably track the reference value all the time (the three-line coincidence degree is high). As can be seen from the error analysis chart in FIG. 8, the maximum transient deviation of the ACPF estimate is reduced by about 0.3/s from the maximum transient error of the UPF estimate, and the estimation accuracy is improved by one order of magnitude. As can be seen from the error analysis graphs in fig. 9 and 10, compared with the UPF algorithm, the maximum transient deviation of the ACPF algorithm in the estimation of the roll angle speed and the roll angle is respectively improved by 0.025 °/s and 0.003 °, although the maximum transient deviation is improved, the relative errors of the ACPF algorithm in the estimation are respectively 2.831% and 2.8392%, and the estimation accuracy is still high.
The result analysis graph shows that the ACPF algorithm can well estimate the vehicle state, although the deviation still exists, the deviation is within the allowable range, and the main reason is that the established nonlinear vehicle dynamic model still cannot completely reflect the real motion situation of the vehicle although the dimension is higher; the state estimation of the ACPF algorithm with the fusion of the adaptive volume Kalman filtering and the particle filtering has better applicability to a nonlinear vehicle system model.
Under the condition of the same particle number, the ACPF operation time is 17.62s, the UPF operation time is 19.49s, the running computer CPU is i5-3210M, and the running memory is 8G. Comparing the importance density functions of the two algorithms, it can be seen that the ACKF needs to select 2M points for nonlinear propagation, while the UKF needs to select 2M +1 points for nonlinear propagation, so that the running time of the ACPF algorithm is shorter than that of the UPF algorithm under the same condition.
According to the method, the states of the mass center, the side deflection angle, the side inclination angle and the like of the vehicle are estimated based on the adaptive volume particle filter, and the result shows that the adaptive volume particle filter can stably track each state parameter of the vehicle. The ACPF algorithm based on the vehicle nonlinear dynamic model is a current high-efficiency vehicle state estimation algorithm, and a feasible method is provided for accurate perception of the vehicle state.
The above description is only of the preferred embodiments of the present invention, and it should be noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the invention and these are intended to be within the scope of the invention.
Claims (6)
1. A vehicle state estimation method based on adaptive volume particle filtering is characterized by comprising the following steps:
step 1, establishing a vehicle dynamic model according to a state variable to be estimated and information obtained through a vehicle-mounted sensor, and converting the vehicle dynamic model into a state equation and a measurement equation;
step 2, algorithm initialization: determining a volume point and a corresponding weight according to a spherical radial criterion, and extracting particles and a covariance matrix from an initial state probability density distribution function;
step 3, designing a particle filter importance density sampling function by adopting ACKF, importing the latest observation data of the sensor, and generating a prediction particle set and a corresponding variance;
step 4, regenerating particles, calculating importance weight and normalizing the weight;
step 5, resampling the particle set according to the weight normalization result;
step 6, calculating a state estimation value and an error covariance matrix of the particle filter at the current moment;
and 7, judging whether the state estimation is finished or not, outputting an estimation result and quitting the state estimation if the state estimation is finished, otherwise, outputting the estimation result, inputting the estimation result and the latest observation data into the step 3, and continuing to perform the state estimation.
2. The adaptive volume particle filter-based vehicle state estimation method according to claim 1, wherein the on-vehicle sensors include wheel speed sensors, longitudinal acceleration sensors, lateral acceleration sensors, roll angle speed sensors, and yaw rate sensors, and steering wheel angle sensors.
3. The adaptive volumetric particle filter based vehicle state estimation method of claim 2, wherein the state equation and the metrology equation of the vehicle system model are in the form of:
wherein ,for the state variable to be estimated, u (t) ═ δ, wfl,wfr,wrl,wrr]' is an input variable, z (t) ═ ax,ay,p,wr]The method comprises the steps that' observation variables and input variables are acquired through a vehicle-mounted sensor, and w (t) and v (t) are process noise and measurement noise respectively;
further, the equation of state f (x (t), u (t)) is:
the measurement equation h (x (t), u (t)) is:
in the formula,x1＝Vx，x2＝Vy，x3＝wr，x4＝p，
∑Mz＝(Fxrr-Fxrl)dr/2-b(Fyrl+Fyrr)+a(Fxflsinδfl+Fxfrsinδfr+Fyflcosδfl
+Fyfrcosδfr)+(Fxfrcosδfr-Fxflcosδfl+Fyflsinδfl-Fyfrsinδfr)df/2
∑Fy＝Fxflsinδfl+Fxfrsinδfr+Fyflcosδfl+Fyfrcosδfr+Fyrl+Fyrr
∑Fx＝Fxflcosδfl+Fxfrcosδfr-Fyflsinδfl-Fyfrsinδfr+Fxrl+Fxrr
Wherein m is the total mass of the vehicle, msFor sprung mass of the vehicle, VxIs the longitudinal speed of the vehicle mass center, VyIs the vehicle centroid lateral velocity, wrThe centroid yaw rate, e the roll arm height,is the centroid roll angle, p is the centroid roll angle velocity, delta is the steering wheel angle, Sigma FxFor the resultant longitudinal force of vehicle tyres, ∑ FyFor resultant force in the lateral direction of vehicle tyre, axIs the longitudinal acceleration of the vehicle centroid, ayIs the vehicle centroid lateral acceleration; i ═ f, r denote front and rear wheels, respectively, j ═ l, r denote left and right wheels, respectively, and w representsijAs the wheel speed, deltaijIs the tire slip angle, FxijIs a tire longitudinal force, FyijFor tire side force, ∑ MxFor vehicle roll moment, [ sigma ] MzYaw moment for vehicle, IxxsMoment of inertia about the x-axis for sprung mass of the vehicle, IxzsIs the product of rotational inertia, I, of sprung mass of the vehicle about the x and z axeszzThe yaw moment of inertia of the vehicle, g the acceleration of gravity,in order to provide the roll rigidity of the vehicle,for vehicle roll damping, a is the distance of the vehicle center of mass from the front axle, b is the distance of the vehicle center of mass from the rear axle, dfFor the front axle track of the vehicle, drIs the rear axle track of the vehicle.
4. The method according to claim 3, wherein in the step 2, the algorithm initialization process specifically comprises: inputting fixed parameter information, initial values of quantities to be estimated, covariance matrixes of process noise and measurement noise, error covariance matrixes, particle numbers, filter parameters and sampling time in a state equation and a measurement equation;
then, 2M volume points and corresponding weights are generated according to the spherical radial criterion, that is:
wherein M is the dimension of the state equation, ξnIs the nth volume point, ωcnRepresents the nth weight, [ I ]]nRepresenting a set of volume points [ I]Column n, using the three-order volume principle, volume point set [ I]Comprises the following steps: [ I ] of]＝[I1，-I1]，I1Is a 5 × 5 unit matrix;
from initial state probability distribution density p (x)0) Extracting particles(i ═ 1,2, ·, N are population numbers), and:
in the formula,as the initial particles, the particles are,is the mean value of the initial particles,initial particle error covariance.
5. The adaptive volume particle filter-based vehicle state estimation method according to claim 4, wherein the step 3 is divided into a time update process and a measurement update process, and specifically comprises:
3.1 time update:
for error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
calculate the Cubature point conducted by the equation of state:
updating the state prediction value and the error covariance prediction value:
wherein, time k is 1,2,3,is an estimate of the state at time k-1, uk-1Input variable, w, obtained by the sensor for time k-1k-1Process noise at time k-1;
3.2 measurement update:
for the updated error covariance matrixAnd (3) decomposing:
calculate the Cubature point:
the Cubature point conducted by the measurement equation is calculated:
updating the measurement one-step prediction:
updating the prediction value of the covariance of the measurement error:
calculating the cross covariance:
calculating a volume Kalman filtering gain matrix:
calculating a state estimation value after volume Kalman filtering:
calculating an error covariance estimate after volumetric Kalman filtering:
wherein αkFor the adaptive factor, the calculation equation is:
in the formula,c0∈(1.0～1.5)，c1∈(3.0～8.0)。
6. the adaptive volume particle filter-based vehicle state estimation method according to claim 5, wherein the resampling method in step 5 selects residual resampling.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910627592.2A CN110532590B (en) | 2019-07-12 | 2019-07-12 | Vehicle state estimation method based on self-adaptive volume particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910627592.2A CN110532590B (en) | 2019-07-12 | 2019-07-12 | Vehicle state estimation method based on self-adaptive volume particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110532590A true CN110532590A (en) | 2019-12-03 |
| CN110532590B CN110532590B (en) | 2023-06-13 |
Family
ID=68659748
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910627592.2A Active CN110532590B (en) | 2019-07-12 | 2019-07-12 | Vehicle state estimation method based on self-adaptive volume particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110532590B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110884499A (en) * | 2019-12-19 | 2020-03-17 | 北京理工大学 | Method and system for determining vehicle mass center slip angle |
| CN112287289A (en) * | 2020-10-14 | 2021-01-29 | 南京航空航天大学 | Vehicle nonlinear state fusion estimation method for cloud control intelligent chassis |
| CN112699924A (en) * | 2020-12-22 | 2021-04-23 | 安徽卡思普智能科技有限公司 | Method for identifying lateral stability of vehicle |
| CN112874529A (en) * | 2021-02-05 | 2021-06-01 | 北京理工大学 | Vehicle mass center slip angle estimation method and system based on event trigger state estimation |
| CN113075715A (en) * | 2020-11-16 | 2021-07-06 | 中移(上海)信息通信科技有限公司 | Positioning method, device, equipment and storage medium |
| WO2021248641A1 (en) * | 2020-06-10 | 2021-12-16 | 北京理工大学 | Multi-sensor information fusion-based model adaptive lateral velocity estimation method |
| CN113886957A (en) * | 2021-09-30 | 2022-01-04 | 中科测试(深圳)有限责任公司 | Vehicle dynamic parameter estimation method |
| CN116992697A (en) * | 2023-09-26 | 2023-11-03 | 华东交通大学 | Intelligent electric vehicle running state information estimation method |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108162976A (en) * | 2017-12-21 | 2018-06-15 | 江苏大学 | A kind of vehicle running state method of estimation based on sparse grid quadrature Kalman filtering |
| CN108802692A (en) * | 2018-05-25 | 2018-11-13 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter |
-
2019
- 2019-07-12 CN CN201910627592.2A patent/CN110532590B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108162976A (en) * | 2017-12-21 | 2018-06-15 | 江苏大学 | A kind of vehicle running state method of estimation based on sparse grid quadrature Kalman filtering |
| CN108802692A (en) * | 2018-05-25 | 2018-11-13 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter |
Non-Patent Citations (1)
| Title |
|---|
| 汪龚等: "基于递推最小二乘法与模糊自适应扩展卡尔曼滤波相结合的车辆状态估计", 《中国机械工程》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110884499A (en) * | 2019-12-19 | 2020-03-17 | 北京理工大学 | Method and system for determining vehicle mass center slip angle |
| CN110884499B (en) * | 2019-12-19 | 2021-03-19 | 北京理工大学 | Method and system for determining vehicle mass center slip angle |
| WO2021248641A1 (en) * | 2020-06-10 | 2021-12-16 | 北京理工大学 | Multi-sensor information fusion-based model adaptive lateral velocity estimation method |
| CN112287289A (en) * | 2020-10-14 | 2021-01-29 | 南京航空航天大学 | Vehicle nonlinear state fusion estimation method for cloud control intelligent chassis |
| CN113075715A (en) * | 2020-11-16 | 2021-07-06 | 中移(上海)信息通信科技有限公司 | Positioning method, device, equipment and storage medium |
| CN112699924A (en) * | 2020-12-22 | 2021-04-23 | 安徽卡思普智能科技有限公司 | Method for identifying lateral stability of vehicle |
| CN112874529A (en) * | 2021-02-05 | 2021-06-01 | 北京理工大学 | Vehicle mass center slip angle estimation method and system based on event trigger state estimation |
| CN112874529B (en) * | 2021-02-05 | 2022-07-08 | 北京理工大学 | Vehicle mass center slip angle estimation method and system based on event trigger state estimation |
| CN113886957A (en) * | 2021-09-30 | 2022-01-04 | 中科测试(深圳)有限责任公司 | Vehicle dynamic parameter estimation method |
| CN116992697A (en) * | 2023-09-26 | 2023-11-03 | 华东交通大学 | Intelligent electric vehicle running state information estimation method |
| CN116992697B (en) * | 2023-09-26 | 2023-12-15 | 华东交通大学 | Intelligent electric vehicle running state information estimation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110532590B (en) | 2023-06-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110532590B (en) | Vehicle state estimation method based on self-adaptive volume particle filtering | |
| JP6815519B2 (en) | Systems and methods for calibrating vehicle tires | |
| CN111845775B (en) | Joint estimation method for driving state and inertia parameters of distributed driving electric automobile | |
| EP2927065B1 (en) | Road surface friction and surface type estimation system and method | |
| EP2927066B1 (en) | Model-based longitudinal stiffness estimation system and method | |
| EP3115765B1 (en) | Tire sensor-based vehicle state estimation system and method | |
| CN112660112B (en) | Vehicle side-tipping state and side-tipping prediction method and system | |
| CN113460056B (en) | Vehicle road surface adhesion coefficient estimation method based on Kalman filtering and least square method | |
| JP2019513613A (en) | Method, system and non-transitory computer readable memory for controlling a vehicle | |
| CN111332277A (en) | Vehicle lateral stability control method based on stable region under limit working condition | |
| CN106250591A (en) | A kind of motoring condition method of estimation considering to roll impact | |
| CN111547059A (en) | Distributed driving electric automobile inertia parameter estimation method | |
| Chen et al. | Longitudinal-lateral-cooperative estimation algorithm for vehicle dynamics states based on adaptive-square-root-cubature-Kalman-filter and similarity-principle | |
| Li et al. | A variable structure adaptive extended Kalman filter for vehicle slip angle estimation | |
| Pi et al. | Design and evaluation of sideslip angle observer for vehicle stability control | |
| CN111796522B (en) | Vehicle state estimation method | |
| CN111688715B (en) | Centroid slip angle observation method of four-wheel drive electric vehicle based on fusion technology | |
| CN114715158B (en) | Road surface adhesion coefficient measuring device and method based on road surface texture characteristics | |
| CN103279675A (en) | Method for estimating tire-road adhesion coefficients and tire slip angles | |
| Liu et al. | Vehicle state estimation based on unscented Kalman filtering and a genetic-particle swarm algorithm | |
| CN114103967A (en) | Method for estimating mass center slip angle and tire lateral force of four-wheel independent drive electric vehicle | |
| CN111942399A (en) | Vehicle speed estimation method and system based on unscented Kalman filtering | |
| CN113203422B (en) | Freight car state inertia parameter joint estimation method based on size measurement device | |
| Liu et al. | Vehicle state estimation based on unscented kalman filtering and a genetic algorithm | |
| Khaknejad et al. | Identification of vehicle parameters using modified least square method in ADAMS/Car |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |