Vehicle state and parameter estimation based on adaptive robust unscented particle filter

1. Introduction

When analyzing the active safety of a vehicle, it is particularly important to obtain the state parameters during the driving process of the vehicle. The algorithms used to estimate the key state parameters of automobiles mainly include Kalman filter algorithm, particle filter algorithm, sliding mode observer algorithm, robust observer algorithm and Romberg observer algorithm. The sliding mode observer algorithm depends on the accuracy and performance of the sensor, otherwise it is prone to chattering. The robust observer algorithm is prone to underestimation bias in some cases, which causes the algorithm to diverge. The calculation process of the Romberg observer algorithm is complex, and it is difficult to meet the real-time requirements of the vehicle estimation algorithm.

A vehicle is a strong nonlinear system, but the Kalman filter can only process linear systems. Based on the standard Kalman filter algorithm, the Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) are developed to handle nonlinear systems. The extended Kalman filter algorithm linearizes the nonlinear system by performing Taylor expansion of the mathematical model of the nonlinear system at the optimal point, and solving the Jacobian matrix of the nonlinear function. When this algorithm is used to linearize the nonlinear system, only the first-order system is retained, and the second-order or higher-order components are discarded, so there is a certain estimation bias. At the same time, when the estimated target system has strong nonlinearity, its calculation amount is too large and the Jacobian matrix is complicated to solve, so the divergence is easy to occur. For each sigma point obtained, its mean and variance are ensured the same as the original data, and brought into the nonlinear system for unscented transformation making it close to Gaussian distribution through weighted summation of samples. Based on the characteristics of Gaussian distribution, the algorithm can be accurate to the third-order mean and covariance, and the operation is simple, and the obtained system is stable.

The problem of vehicle state estimation has been widely studied. A brief review is presented in what follows.

Lenzo et al.[1] proposed an Extended Kalman Filter (EKF) method to estimate the vehicle states as well as tyre-road coefficient of friction. Li et al. [2] proposed a new situation-sensitive method to improve the vehicle detection at nighttime. In order to estimate vehicle states and parameter with high precision, Zhu et al. [3] presented a modified particle filter. Wang et al. [4] proposed a novel adaptive fault-tolerant extended Kalman filter to estimate vehicle state in case of partial loss of sensor data. In order to improve the driving dynamics and the driving safety, Henning et al.[5] presented an integrated lateral dynamics control concept for a over-actuated vehicle. Zhao et al. [6] presented a state estimation method to enable fully autonomous flight in outdoor environments. Alatorre et al. [7] proposed an algorithm that merges the concepts of least squares method and sliding mode observer for the estimation of the vehicle mass. Heidfeld et al. [8] proposed an Unscented KALMAN Filter (UKF) for simultaneous state and parameter estimation. Badini et al. [9] presented a simple parameter independent speed estimation algorithm for vector-controlled permanent magnet synchronous motor (PMSM) drive. Kulikov et al. [10] resolved the lack of square-root implementations by means of hyperbolic QR transforms applied for yielding J-orthogonal square roots. Malikov [11] solved the problem of differential equations with periodic using the quadratic Lyapunov function. Takikawa1 et al. [12] used a global navigation satellite system (GNSS) Doppler for accurate vehicular trajectory estimation. Gao et al. [13, 14] presented a new methodology of distributed state fusion for multisensory nonlinear systems by using the sparse-grid quadrature filter.

In the literatures proposed above, some researchers did not implement the algorithms in onboard real-time systems. And some researchers missed the diverse traffic conditions in daytime and nighttime when estimating states of the vehicle. At the same time, more vehicle parameters including the yaw inertia of the vehicle hadn’t been estimated in a lot of literatures. And the estimation error covariance as well as the closed-loop operation of the state estimator with a driving dynamic hadn’t been investigated. For some researchers, the usability of the proposed model should be improved.

In the current research, most of the observation noise covariance matrix is set to a fixed value. However, in the actual driving process of the vehicle, the process noise and observation noise are randomly generated. The unscented particle filter (UPF) algorithm uses the unscented Kalman filter method to generate the proposal density function, so that the prior probability peak value and the likelihood function peak value have good agreement reducing particle degradation. However, the accuracy is affected by the uncertainty of the system noise. And the lack of an adaptive adjustment mechanism makes it impossible to adjust the filter gain and related parameters in real time. In order to better estimate the state parameters of the vehicle in real-time and effectively, the adaptive robust unscented particle filter (ARUPF) method, which can adjust filter parameters in real time and has better adaptability to interference noise is proposed.

The ARUPF algorithm absorbs the advantages of robust estimation, robust adaptive filtering and particle filtering fully. It combines robust estimation principle and the UPF algorithm through equivalent weight function and adaptive factor. The state model information and measurement model information can be controlled by selecting appropriate weight function and adaptive factor, suppressing the influence of abnormal interference. The ARUPF algorithm overcomes the shortcomings of using only a single filtering algorithm. The algorithm uses the important density function selected from the three important steps of UT transformation, equivalent weight and adaptive factor adjustment to perform importance sampling, reducing the degree of particle degradation, and having high filtering accuracy.

2. Mathematical model of vehicle dynamics

2.1. 3-DOF vehicle model

The vehicle state estimation model is established based on a 3-DOF vehicle model. The 3-DOF vehicle model is shown in Fig. 1. $xoy$ is the vehicle coordinate system, and the origin of the vehicle coordinate system coincides with the center of mass of the vehicle.

Fig. 13-DOF vehicle model

The paper adopts a simplified estimation model, and establishes a nonlinear 3-DOF vehicle model including longitudinal and lateral as well as yaw motion. And it is assumed that:

(1) The center of mass of the vehicle model coincides with the origin of the vehicle coordinate system;

(2) The suspension has no effect on the vertical movement of the vehicle;

(3) The vehicle has no degree of freedom in the pitch and roll directions;

(4) The impact of the longitudinal rolling resistance is ignored for the state parameter estimation.

In Fig. 1, $a$ and $b$ are the distances of front and rear axles from the center of gravity respectively; $t_f$ and $t_r$ are the tracks of the front and rear wheels respectively; ${\alpha }_{fl,fr}$ are the side slip angles of the left and right front wheels; ${\alpha }_{rl,rr}$ are the side slip angles of the left and right rear wheels; ${F_{x\_}}_{fl,fr,rl,rr}$ are the longitudinal forces of the left front, right front, left rear, and right rear wheels; ${F_{y\_}}_{fl,fr,rl,rr}$ are the lateral forces of the left front, right front, left rear, and right rear wheels; ${\delta }_{fl,fr}$ are the wheel angles of the left and right front wheels.

The dynamic equation of the 3-DOF vehicle model is as follows:

1

$\left\{\begin{array}{l}{a}_x=\dot{u}-vr,\\ a_y=\dot{v}+ur,\\ I_z\dot{r}=\sum M,\end{array}\right.$

where, $u$ and $v$ are the longitudinal and the lateral speed; $r$ is the yaw rate; $a_x$ and $a_y$ the longitudinal and lateral acceleration; $M$ is the yaw moment; $I_z$ is the moment of inertia around the z axis of the vehicle.

The side slip angle of the center of mass is:

2

$\beta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{v}{u}\right).$

According to the kinetic equation, the calculation formulas for other parameters are as follows:

3

$\begin{array}{l}M=a(F_{x\_fl}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fl}+F_{y\_fl}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fl})-\frac{t_f}{2}(F_{x\_fl}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fl}-F_{y\_fl}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fl})\\ +a(F_{x\_fr}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fr}+F_{y\_fr}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fl})+\frac{t_f}{2}(F_{x\_fr}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fr}-F_{y\_fr}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fr})\\ -b{F}_{x\_rl}-\frac{t_f}{2}{F}_{x\_rl}-b{F}_{x\_rr}+\frac{t_f}{2}{F}_{x\_rr}\end{array}$

4

$a_x=\frac{1}{m}\left(F_{x_{fl}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fl}-F_{y_{fl}}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fl}+F_{x_{fr}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fr}-F_{y_{fr}}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fr}+F_{x_{rl}}+F_{x_{rr}}\right),$

5

$a_y=\frac{1}{m}\left(F_{x_{fl}}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fl}+F_{y_{fl}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fl}+F_{x_{fr}}\mathrm{s}\mathrm{i}\mathrm{n}{\delta }_{fr}+F_{y_{fr}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{fr}+F_{x_{rl}}+F_{x_{rr}}\right),$

6

$\left\{\begin{array}{l}{\alpha }_{fl,fr}={\delta }_{fl,fr}-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{g}\frac{v+ar}{u\pm \frac{t_f}{2}r},\\ {\alpha }_{rl,rr}=-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{g}\frac{v-br}{u\pm \frac{t_r}{2}r},\end{array}\right.$

7

$\left\{\begin{array}{l}{v}_{fl,fr}=\sqrt{{\left(u\mp \frac{t_f}{2}r\right)}^2+(v+ar{)}^2},\\ v_{rl,rr}=\sqrt{{\left(u\mp \frac{t_r}{2}r\right)}^2+(v-br{)}^2},\end{array}\right.$

8

$\left\{\begin{array}{l}{F_{z\_}}_{fl,fr}=\left(\frac{1}{2}mg\pm m{a}_y\frac{h}{t_f}\right)b-\frac{1}{2}m{a}_{x}h,\\ {F_{z\_}}_{rl,rr}=\left(\frac{1}{2}mg\pm m{a}_y\frac{h}{t_r}\right)b+\frac{1}{2}m{a}_{x}h,\end{array}\right.$

where, $m$ is the vehicle mass; $h$ is the height of the center of mass; $R_e$ is the rolling radius of the wheels; $l$ is the wheelbase; $v_{fl,fr}$ is the center speed of the left and right front wheels; $v_{rl,rr}$ is the center speed of the left and right rear wheels; ${F_{z\_}}_{fl,fr}$ is the vertical load of the left and right front wheels; ${F_{z\_}}_{rl,rr}$ is the vertical load of the left and right rear wheels.

3. The ARUPF Algorithm

The traditional particle filter algorithm has the defect of particle degradation in the iterative process, resulting in waste of computing resources and low accuracy of estimation results. In order to solve the above problems, the filtering algorithm is often optimized by increasing the number of particles, re-sampling, and selecting a reasonable proposal density function. Increasing the number of particles can effectively alleviate the particle degradation, but it increases the computational workload of the system. The re-sampling method can increase the diversity of particles and avoid particle degradation. The adaptive robust unscented particle filter algorithm uses the unscented transformation algorithm to calculate the mean and covariance of each particle and establish a reasonable proposal density function combining the robust filter estimation algorithm to automatically adjust the gain matrix and system variance making the distribution of sample points fit better with the maximum likelihood function. Unscented particle filter algorithm is easy to implement in engineering and can effectively reduce the workload of the system. The specific methods are as follows:

1) Initialization, $k=$0.

Drawing initial state particles from the prior distribution:

where ${\stackrel{-}{x}}_0^{\left(i\right)}$ and $p_0^{\left(i\right)}$ are the mathematical expectation and variance of the initial particle respectively, ${\stackrel{-}{x}}_0^{\left(i\right)a}$ and $p_0^{\left(i\right)a}$ are the mathematical expectation and variance of the initial Sigma point, respectively; $Q$ and $R$ are the covariance matrix and the observation covariance matrix of the system respectively.

2) Importance sampling.

Calculating the mean and variance using the unscented Kalman algorithm.

(1) Extracting the set of Sigma points:

where $x_{k-1}^{\left(i\right)a}$ and $p_{k-1}^{\left(i\right)a}$ are the mathematical expectation and variance of the extracted particles respectively; $n_a$ and $\lambda$ are the state dimension and scaling factor respectively.

(2) One-step prediction for the Sigma point set:

where $x_{k\left|k-1\right.}^{\left(i\right)a}$, ${\stackrel{-}{x}}_{k\left|k-1\right.}^{\left(i\right)}$ and $p_{k\left|k-1\right.}^{\left(i\right)}$ are state value, mathematical expectation and variance of the Sigma particle after one-step prediction respectively; $Z_{k\left|k-1\right.}^{\left(i\right)}$ and ${\stackrel{-}{Z}}_{k\left|k-1\right.}^{\left(i\right)}$ are the observed value and the observed mean value obtained by inputting the observation equation for the Sigma point after one-step prediction respectively; $W_j^m$ and $W_j^c$ are the calculation weights of the mean and the covariance corresponding to Sigma respectively.

(3) Integrating the observation data to update the mean, Kalman gain and covariance of the Sigma point set:

where $P_{{\stackrel{-}{Z}}_k,{\stackrel{-}{Z}}_k}$ and $P_{x_k,Z_k}$ are the observed covariance and system variance obtained by weighted calculation respectively; $K_k$, ${\stackrel{-}{x}}_k^{\left(i\right)}$ and ${\widehat{P}}_K^{\left(i\right)}$ are the system gain matrix, state value and variance after state update respectively.

3) ARUPF algorithm.

The ARUPF algorithm is based on robust estimation filtering theory, which controls the abnormal situation of the observed value of the dynamic model, and constructs an adaptive factor to control the error of the dynamic model. If ${\stackrel{-}{P}}_i$ is set as the weight matrix of the state matrix ${\stackrel{-}{x}}_k^{\left(i\right)}$, the equivalent weight matrix is $\stackrel{-}{P}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}({\stackrel{-}{P}}_1,{\stackrel{-}{P}}_2,\cdots ,{\stackrel{-}{P}}_k)$. The principle for using the IGG (Institute of Geodesy & Geophysics) method to generate the equivalent weight function is as follows:

where $V_K=A_k{\stackrel{-}{x}}_k^{\left(i\right)}-{\stackrel{-}{Z}}_{k\left|k-1\right.}^{\left(i\right)}$ is the detect residual values for sensors; the value range of the adjustment factor are $K_0\in \left(\mathrm{1,1.5}\right)$ and $K_g\in \left(\mathrm{3,8}\right)$.

It is set that the sensor perception matrix is $A_k$. The system state vector is updated according to the weight matrix obtaining the system state solution vector of the adaptive robust Kalman filter:

where ${\alpha }_k$ is the adaptive factor; $tr$ is the matrix trace operator; the ranges of the regulators $c_0$ and $c_1$ are $c_0\in \left(\mathrm{1,1.5}\right)$, $c_1\in \left(\mathrm{3,8}\right)$.

In the above formula, the weight matrix ${\stackrel{-}{P}}_k$ is obtained by judging the residual error; the adaptive factor ${\alpha }_k$ is obtained by the difference operation between the state estimated value and the predicted value. The two parameters are used to adjust the Kalman gain, the sampled particle mean and the particle weight at the same time and update the particle and normalize weights:

where $K_k^{*}$ is the Kalman gain computed by the robustness algorithm; ${\stackrel{-}{x}}_k^{\left(i\right)*}$ is the State sample mean; ${\widehat{P}}_K^{\left(i\right)*}$ is the sample variance; $W_k^{\left(i\right)*}$ is the updated particle weight value.

Using the resampling algorithm, the particle set is eliminated and copied based on the normalized weight, and the weight is reset to the new particle. When the prediction model has excessive abnormal interference, the adaptive factor ${\alpha }_k$ is decreased, which can weaken the influence of the interference. When there is a large disturbance in the observation model, the abnormal influence caused by the disturbance can be reduced by adjusting the weight matrix ${\stackrel{-}{P}}_k$. The adaptive robust unscented particle filter algorithm can effectively solve the problem of gross error and abnormal state of the system observation, and establish a reasonable particle filter which effectively solves the problem of particle degradation.

4. Numerical simulations and experimental verification

4.1. Numerical simulations

A Volkswagen vehicle is verified by a simulation test on the virtual prototype software ADAMS. And the real vehicle used for test is the same with the virtual vehicle model in Adams. That is to say, the parameters of the vehicle dynamics model in ADAMS are the same as our test vehicle shown in Table 1.

ADAMS/Car adopts a top-down modeling sequence. That is to say, the vehicle model and the system assembly model are built on the basis of each subsystem model. And each subsystem model needs to be established by templates, so the establishment of each template is the key steps to establish the vehicle model. The template establishment process is as follows:

(1) Simplification of the physical model: According to the relative motion relationship between the various parts of the subsystem, defining the “Topological Structure” of the parts, integrating the parts, and defining the parts without motion relationship as a “Gene-ralPart”.

(2) Determining “Hard Point”: The hard point is the geometric positioning point at the key connection point of the part, and the determination of the hard point is to determine the geometric coordinates of the key point of the part. To determine the hard point is to determine the geometric coordinates of the key points of the part in the subsystem coordinate system, and the hard point can be modified in the vehicle model and template state.

Table 1Simulation parameters

Parameter	Value
$m$ (kg)	1558
$I_z$ (kg∙m2)	2538
$a$ (m)	1.48
$b$ (m)	1.08
$h$ (m)	0.432
$t_f$ (m)	1.51
$t_r$ (m)	1.55
$r_e$ (m)	0.32

(3) Determining the parameters of the part: Calculating or measuring the mass of the integrated part, the position of the center of mass, and the moment of inertia around the three axes of the center of mass coordinate system. It should be noted that the directions of the three coordinate axes of the center of mass coordinate system must be parallel to the directions of the three axes of the system coordinate system.

(4) Creating the “Geometry” of the part: Building the geometric model of the part on the basis of the hard point. Since the dynamic parameters of the part have been determined, the shape of the geometric model has no influence on the dynamic simulation results. But in the kinematic analysis, the outer contour of the part is directly related to the motion check of the part. And the intuitiveness of the model is considered. And considering the intuitiveness of the model, the geometry of the part should be as close to the actual structure as possible.

(5) Defining “Constrain”: Defining the type of constraint according to the motion relationship between parts. The components are connected by constraints to form the subsystem structure model. Defining constraints is the key to correct modeling and is directly related to the rationality of the degrees of freedom of the system.

(6) Defining “Mount”: Defining assembly commands at the connections between subsystems and subsystems or external models.

(7) Defining “Subsystem”: Transferring the model built in Template to the standard mode and defining each subsystem model preparing for assembling the vehicle model.

Fig. 2Vehicle model in ADAMS

(8) Defining “Assembly”: In the standard mode, each subsystem is assembled into a complete vehicle model. So that the establishment process of the physical model is completed in the ADAMS/Car module. By adding attribute files, the simulation analysis of the vehicle under different working conditions can be carried out to obtain the required results.

The vehicle model can be obtained as shown in Fig. 2.

4.1.1. Sine delay test

The sine delay test is carried out on a dry, flat and clean cement test site. The test vehicle drives at a constant speed of (80±2) km/h. The virtual test value, the experimental measurements, the estimation results of the ARUPF and the UPF algorithm are shown in Fig. 3. The results show that the ARUPF algorithm achieves better estimation results for the longitudinal velocity and the yaw rate, while the estimation results of the UPF algorithm have a certain amplitude fluctuation in the steering wheel holding stage. For the lateral velocity, the ARUPF There is a small deviation between the estimated value of the ARUPF algorithm and the virtual test value at the peak. But the overall estimated result meets the engineering needs. And the estimated result of the UPF algorithm is different from the virtual test value during the steering wheel rotation process. There is a certain magnitude of fluctuation in the steering wheel holding process.

Fig. 3Comparison of the state variables found with the different algorithms (ARUPF and UPF) for a sine delay test road

a) Longitudinal velocity

b) Absolute error of longitudinal velocity

c) Lateral velocity

d) Absolute error of lateral velocity

e) Yaw rate

f) Absolute error of yaw rate

The ARUPF algorithm has the advantages of robust estimation and unscented particle filtering. After comprehensively selecting the important density function by using unscented transformation, equivalent weight function and adaptive adjustment factor, the importance sampling of particles is carried out. And the sensor perception vector is used reasonably. It effectively suppresses the pollution problem of state parameters and the abnormal disturbance of sensor perception values, reducing the degree of particle degradation in the process of vehicle state estimation and overcoming the shortcomings of a single filtering algorithm. The reasons for this error are as follows: since the mathematical model ignores the suspension characteristics and tire rolling resistance, the tire slip angle and the lateral load transfer of the sprung mass have a certain relationship with the tire slip angle and tire vertical load in ADAMS. However, the observer can effectively estimate the vehicle state such as the longitudinal and lateral velocity, as well as the yaw rate, etc.

4.1.2. Double lane change test

A double lane change test is carried out on a dry, flat and clean cement test site. The test speed is 60 km/h. The test driver make the vehicle pass through the double lane change channel without touching the stakes. The virtual test value, the experimental measurements, the estimation results of the ARUPF and the UPF algorithm are shown in Fig. 4.

Fig. 4Comparison of the state variables found with the different algorithms (ARUPF and UPF) for a double lane change test road

a) Longitudinal velocity

b) Absolute error of longitudinal velocity

c) Lateral velocity

d) Absolute error of lateral velocity

e) Yaw rate

f) Absolute error of yaw rate

This test condition is used to verify the accuracy of the algorithm for estimating the longitudinal and lateral velocity as well as the yaw rate of the vehicle when the vehicle states change rapidly. The results show that the ARUPF algorithm has achieved good results in the estimation of the longitudinal velocity and the yaw rate. But the UPF algorithm has a small fluctuation in the estimation of the longitudinal velocity and the yaw rate. For the lateral velocity, there is a certain magnitude of deviation between the estimated results of the ARUPF algorithm and the virtual test value during the steering wheel rotation, while the estimated results of the UPF algorithm have large fluctuations.

The average absolute error (MAE) and root mean square error (RMSE) are given to verify the estimation accuracy of the proposed algorithm.

Table 2The MAE and RMSE indicators of the two algorithms

Evaluation index	State value	UPF	ARUPF
MAE	$v$ (m/s)	0.316	0.140
	$v$ (m/s)	0.181	0.0475
	$r$ (rad/s)	0.316	0.0180
RMSE	$v$ (m/s)	0.345	0.141
	$v$ (m/s)	0.243	0.0522
	$r$ (rad/s)	0.411	0.0221

From Table 2, it can be seen more intuitively that the estimation accuracy of the ARUPF algorithm is significantly higher than the UPF method.

The ARUPF algorithm has the advantages of robust estimation and unscented particle filtering. After comprehensively selecting the important density function by using unscented transformation, equivalent weight function and adaptive adjustment factor, the importance sampling of particles is carried out. And the sensor perception vector is used reasonably. It effectively suppresses the pollution problem of state parameters and the abnormal disturbance of sensor perception values, reducing the degree of particle degradation in the process of vehicle state estimation and overcoming the shortcomings of a single filtering algorithm. The reasons for this error are as follows: since the mathematical model ignores the suspension characteristics and tire rolling resistance, the tire slip angle and the lateral load transfer of the sprung mass have a certain relationship with the tire slip angle and tire vertical load in ADAMS. However, the observer can effectively estimate the vehicle state such as the longitudinal and lateral velocity, as well as the yaw rate, etc.

4.1.3. Slop input test

A slope input test is carried out on a dry, flat and clean cement test site, the vehicle speed is maintained at 80 km/h. The virtual test value, the experimental measurements, the estimation results of the ARUPF and the UPF algorithm are shown in Fig. 5. The test can make the vehicle gradually enter the limit working condition as the steering wheel angle increases, and the tire gradually transitions from the linear working area to the nonlinear working area, which leads to the increase of the error of the model of the vehicle.

Fig. 5Comparison of the state variables found with the different algorithms (ARUPF and UPF) for a slop input test road

a) Longitudinal velocity

b) Absolute error of longitudinal velocity

c) Lateral velocity

d) Absolute error of lateral velocity

e) Yaw rate

f) Absolute error of yaw rate

It can be seen from Fig. 5 that when the system noise increases, the estimation deviation of the UPF algorithm for the longitudinal lateral speed gradually increases. For the estimation of yaw rate, both the UPF algorithm and the ARUPF algorithm maintain a high estimation accuracy.

The ARUPF algorithm has the advantages of robust estimation and unscented particle filtering. After comprehensively selecting the important density function by using unscented transformation, equivalent weight function and adaptive adjustment factor, the importance sampling of particles is carried out. And the sensor perception vector is used reasonably. It effectively suppresses the pollution problem of state parameters and the abnormal disturbance of sensor perception values, reducing the degree of particle degradation in the process of vehicle state estimation and overcoming the shortcomings of a single filtering algorithm. The reasons for this error are as follows: since the mathematical model ignores the suspension characteristics and tire rolling resistance, the tire slip angle and the lateral load transfer of the sprung mass have a certain relationship with the tire slip angle and tire vertical load in ADAMS. However, the observer can effectively estimate the vehicle state such as the longitudinal and lateral velocity, as well as the yaw rate, etc.

4.2. Experimental verification

According to ISO/TR3888-2004, the real vehicle test on double lane change road is carried out. And the test speed is set as 80 km/h (±3 km/h). A gyroscope is installed on the vehicle to collect the yaw rate and lateral acceleration of the vehicle in real time. The non-contact speed sensor is used to measure the longitudinal and lateral speed of the vehicle. In addition, the steering wheel angle tester is used to measure the steering wheel angle. Fig. 6 is the comparison between the estimated value of ARUPF method of three state variables and the real vehicle test value.

Fig. 6Comparison of the estimated and test values

a) Longitudinal velocity

b) Lateral acceleration

c) Yaw rate

From Fig. 6, it can be seen that although there is a certain error, the estimated value is basically consistent with the experimental value in trend. There is a large difference between the experimental value and the estimated value of the longitudinal velocity, because the estimation of the longitudinal velocity in the model is affected by many parameters such as longitudinal acceleration, lateral velocity and yaw rate. And the tire model used in this paper still has some deviation when simulating the mechanical characteristics of real vehicle tire. In addition, the measurement error and installation position of the sensor are also important reasons for the deviation between the estimated and the test value.

5. Conclusions

Based on the Dugoff tire model, a 3-DOF dynamic model of the vehicle with front wheel steering is established to estimate the longitudinal and lateral speed and the yaw rate of the vehicle. The equivalent weight function is generated by using the IGG method. By adaptively adjusting the weight matrix, the random error caused by nonlinear factors in the process of vehicle sensor detection can be effectively suppressed. And the influence of data distortion caused by interference can be reduced, and the accuracy of vehicle state estimation is improved. Based on the principle of adaptive robust filter and unscented particle filter algorithm, a new state estimation method of vehicle is proposed. The ARUPF method has the advantages of good noise filtering effect and high precision. A simulation platform is used to simulate, analyze and verify the estimation effect of the proposed algorithm. The simulation results show that the vehicle state estimation based on the ARUPF algorithm has the characteristics of high precision, strong anti-interference ability and good stability. It has the advantages of low cost, easy engineering implementation providing a new idea for vehicle state estimation.