Estimation of Skid-Steered Wheeled Vehicle States Using STUKF with Adaptive Noise Adjustment

Abstract

Skid-steered wheeled vehicles are commonly adopted in outdoor environments with the benefits of mobility and flexible structure. However, different from Ackerman turning vehicles, skid-steered vehicles do not possess geometric constraint but only dynamic constraint when steered, which leads to motion control and state estimation problems for skid-steered vehicles. The controlling accuracy of a skid-steered vehicle depends largely on feedback state information from sensors and an observer. In this study, a 3-DOF dynamic model using a Brush nonlinear tire model is built, first, to model a 6 × 6 skid-steered wheeled vehicle in flat ground driving conditions. Then, an observer using the unscented Kalman filter with a strong tracking algorithm and adaptive noise matrix adjustment (AN-STUKF) is established to estimate vehicle motion states based on the 3-DOF dynamic model. Finally, the experiment is carried out in three different driving conditions to verify the accuracy and stability of the proposed method. The results show that the AN-STUKF method possesses better accuracy and tracking rate than the traditional UKF, and the phenomenon of ICRs shifting forward of the skid-steered wheeled vehicle is also verified.

1. Introduction

Unmanned ground vehicles have been developed for carrying out different particular tasks, especially for rescues and in the military. Among different configurations of unmanned ground vehicles, skid-steered vehicles have been widely used because of the following advantages [1,2]:

(1) Skid-steered vehicles have great mobility such as the ability to steer pivotally, and the minimum steering radius of Ackerman steering vehicles is constrained by the ‘Ackermann principle’.

(2) Skid-steered vehicles are steered by the velocity difference between the left-side wheels and the right-side wheels, and therefore an explicit mechanical system for steering can be omitted, which is a great advantage to the overall layout.

Because of the abovementioned advantages, skid-steered wheeled vehicles have become a hot research area in agriculture, engineering, and the military. The AMX10RC light tank [3], CMU’s Crusher [4], IAI’s RoBattle [5], and ARGO’s XTR [6] all adopt the skid-steered configuration and perform advantageous maneuverability and passing ability. However, as compared with an Ackerman turning vehicle, a skid-steered vehicle lacks a geometric constraint because the tires cannot deflect. This limit leads to the tire lateral slipping trend, which results in the tires frequently working in nonlinear conditions. Additionally, skid-steered wheeled vehicles usually work in off-road environments for their special applications, and therefore the tire-road contact tends to be complex. All these elements result in poorer tracing ability and stability of skid-steered wheeled vehicles. High nonlinearity and instability are also significant challenges to state estimation for skid-steered vehicles.

To meet the requirements of a control system, the vehicle moving states are important information for vehicle control. In view of the special conditions the skid-steered vehicle frequently works in, for example, the complex electromagnetic environment on a battlefield, the estimation algorithm is based on the local sensors (encoder, IMU, etc.) instead of GNSS and other sensors.

The focus of this study is to develop an observer to estimate motion states of a skid-steered wheeled vehicle including velocity, yaw rate, and slip parameters. Firstly, a 3-DOF dynamic model using the Brush nonlinear tire model is developed to model the skid-steered wheeled vehicle dynamic characteristics. A kinetic model based on wheel rotation rate and slip rate is also developed to calculate the sideslip angle of each tire and longitudinal slip rate. Then, based on the dynamic model, the UKF observer is built to estimate the vehicle motion states. In addition, a strong tracking algorithm and adaptive noise matrix adjustment algorithm are developed to build the AN-STUKF observer in order to improve the stability, accuracy, and tracking speed. Finally, the experiment is conducted in three different driving conditions to verify the effectiveness of the AN-STUKF. The dynamic characteristics shown in the experiment are also analyzed in this study.

This paper is organized as follows: In Section 2, the relative works in recent years are reviewed; in Section 3, the dynamic and kinetic model is developed to describe the vehicle motion characteristics; in Section 4, we present the UKF observer with an adaptive noise matrix adjustment and a strong tracking algorithm; in Section 5, the experimental verification tests are presented; and in Section 6, we state the conclusions and future studies.

2. Related Works

There are few studies on skid-steered wheeled vehicle estimation. In this section, the skid-steered wheeled vehicle dynamics and kinetic modeling and the traditional Ackerman estimation methods are discussed as the reference for the state estimation for skid-steered wheeled vehicles.

2.1. Modeling Skid-Steered Wheeled Vehicle

The dynamic and kinetic methods are both adopted to model skid-steered wheeled vehicle behaviors.

Suresh Golconda [7] analyzed the advantages and disadvantages of skid-steering and investigated the turning controlling strategy and PID control method. Tran, T. H. [8] used the methodology of terramechanics to analyze tire-road contact characteristics for predicting the moving trace of a vehicle, but it was difficult for real-time computation. Kim [1,9,10,11] developed an 18-DOF dynamic model based on a 6 × 6, armed suspension vehicle to study vehicle state estimation, driving torque distribution, slip rate control, and vehicle motion control. Caldwell [12] used a tire model considering coulomb friction for optimization and predicted the turning performance for a skid-steered wheeled vehicle. Zhang Yu [13,14] established a 2-DOF dynamic model for a skid-steered wheeled vehicle by referring to the 2-DOF bicycle model for a traditional Ackerman turning vehicle and analyzed stability for the lateral dynamics of a skid-steered wheeled vehicle. He presented the phenomenon of ICRs shifting forward as a significant element that leads to instability and calculated the critical speed. Similarly, Ossama Mokhiamar [15] used a 2-DOF model to analyze vehicle response and used direct torque control to compute needed driving torque on each wheel. Sedat Dogru [16] analyzed the friction coefficient in the model to describe how the friction force changes with the radius and the location of mass center.

Some researchers believe that a dynamic model is too complex to compute in real time. They tend to use a kinetic model to describe vehicle motion. The instantaneous center of rotation (ICRs) is a widely used method to model skid-steered vehicle kinetics. Martinez [17] and Wong, J. Y. [18] analyzed the kinetic characteristics of tracked vehicles and demonstrated that the ICRs of track-road contact surface and the ICR of a vehicle is on a line parallel with the y-axis. It is easy to prove that the skid-steered wheeled vehicle possesses the same characteristics [19]. Pazderski and Kozłowski [20] developed a kinetic model using a rotating instantaneous center method. Tarokh and McDermott [21,22] used DH (Denavit–Hartenberg) transformation and presented a modeling method for a 3D gravamen road which was applied on the Rocky 7 Mars rover. Seegmiller and Kelly [23] established an enhanced 3D wheeled moving robot using space vector algebra and presented the slip rate predicting algorithm based on inertial measurements.

2.2. Vehicle Motion States Estimation

The estimation of vehicle motion states is a hot topic of current studies. Even though the relative research mainly focuses on a traditional Ackerman turning vehicle, the estimation algorithm for the Ackerman turning vehicle is still useful for a skid-steered wheeled vehicle.

The Kalman filter (KF) is the most widely used observer for estimating vehicle states; however, the traditional KF is only adopted in linear driving conditions. Thus, nonlinear KF improved algorithms have been adopted for estimating vehicle states. Chen, T. [24] established the Luenberger observer for a priori estimates, and then used EKF for a posteriori observer based on multi-information fusion. Reina, G. [25] used EKF to estimate vehicle states based on a vehicle dynamic model and Cordeiro, R. [26] used EKF to estimate tire force of three axes. Liu [27] combined the auxiliary particle filtering (APF) and iterated expanded Kalman filtering (IEKF) to estimate the road adhesion coefficient and sideslip angle of tires. Wang, Z. [28] developed an adaptive UKF to estimate vehicle states on different roads. Jin [29] established a cubature Kalman filter to estimate kinetic states for a distributed electric vehicle based on the 3-DOF dynamic model and Dugoff tire model. Chen [30] used a UKF-based adaptive variable structural observer with dynamic correction (AUKF) for vehicle body sideslip angle. Liu, F. [31] developed a multi-innovative unscented Kalman filter (MI-UKF) based on an ICR kinetic model of a skid-steered robot. As compared with EKF based on Taylor expansion to realize linear approximation, the UKF performed more accurately in high nonlinear work conditions [32]. To improve the performance of the traditional UKF, some improved algorithms for UKF have been proposed. The strong tracking algorithm [30,33] was proposed to solve the problem that the UKF prediction matrix may not be positive in some conditions. Considering that a variable environment leads to a variable noise matrix, an adaptive variance matrix adjustment was proposed to improve the accuracy and stability of the UKF [34,35,36].

The Kalman filter is based on a Gaussian assumption. To process non-Gaussian conditions, the partial filter (PF) based on importance sampling and Monte Carlo assumption was proposed and was applied in vehicle state estimation and location [37,38]. The main problem in the PF is particle degradation [39]. The unscented partial filter (UPF) establishes particle swarm using unscented transformation similar to the UKF to avoid particle degradation [40]. Chu [41] and Chen [42] estimated electric vehicle state using the UPF. Zhu [43] built a modified PF which performed better than the UPF in the sensor fault mode. Although the UPF performs better than the UKF, in some studies in the literature, in high-nonlinear and non-Gaussian conditions, the larger amount of computation than the UKF limits its application on real vehicles. The UPF has not been widely applied in real vehicle state estimation [44,45] and the reliability of the UPF has not been efficiently verified in real vehicle state estimation as compared with the UKF algorithm.

3. System Modeling

This section covers the dynamic and kinetic models for a 6-wheel skid-steered vehicle. The modeling will be discussed in two parts: kinetic model and dynamic model.

To simplify the modeling, the following assumptions are proposed:

(1) The vehicle is driven on flat ground; thus, all 6 wheels maintain contact with the ground.

(2) The 3 wheels on the same side of the vehicle rotate at the same speed.

(3) The CM of the vehicle is located on the geometric center of the vehicle’s body.

(4) The suspension is ignored in the observer model, and quantities originating from the rolling/pitching movements are omitted.

(5) The quantities in the inertial and non-inertial systems are the same.

3.1. Motion Analysis

To model a skid-steered vehicle maneuvering on the ground, two coordinate frames are introduced, as shown in Figure 1. The XOY-coordinate is the geodetic coordinate system, and the XOY-coordinate is the vehicle reference coordinate whose origin is located on the geometric center. In Figure 1, ICR_v denotes the vehicle instantaneous rotation center, ICR_l and ICR_r denote the instantaneous rotation center of tire slip velocity on the left and right sides, respectively.

In the vehicle reference coordinate, the velocity and yaw rate are expressed as:

$$\left\{\begin{array}{l}\dot{x}=\frac{R}{2}\left[{\omega }_l\left(1-i_l\right)+{\omega }_r\left(1-i_r\right)\right]\\ \dot{y}=\frac{R}{2}\left[{\omega }_l\left(1-i_l\right)+{\omega }_r\left(1-i_r\right)\right]\tan \beta =\dot{x}\tan \beta \\ \dot{\varphi }=\frac{R\left[-{\omega }_l\left(1-i_l\right)+{\omega }_r\left(1-i_r\right)\right]}{B}\end{array}\right.$$

(1)

where $\dot{x}$ and $\dot{y}$ denote longitudinal and lateral velocity on the vehicle coordinate, respectively; ${\omega }_l$ and ${\omega }_r$ denote right and left side rotating velocity, respectively; B denotes the width of the vehicle; R is the wheel radius; β is the sideslip angle of the vehicle body; $i_l$ and $i_r$ denote the slip rate of the wheels on each side, respectively, which is expressed as:

$${\mathit{i}}_{\mathit{i}}=\frac{{\mathit{Rw}}_{\mathit{i}}{-\mathit{v}}_{\mathit{xi}}}{{\mathit{Rw}}_{\mathit{i}}}$$

(2)

On the basis of the assumption (4), the deformation of suspension is ignored. Thus, the same side wheels are at the same longitudinal velocity. The longitudinal velocity of each side is expressed as:

$$u_{x,ij}=\left\{\begin{array}{ll}\dot{x}-\frac{B}{2}\dot{\varphi }& j=l\\ \dot{x}+\frac{B}{2}\dot{\varphi }& j=r\end{array}\right.$$

(3)

Then, the longitudinal slip rate is expressed as:

$$\left\{\begin{array}{l}{i}_l=\frac{R{\omega }_l-\left(\dot{x}-\frac{B}{2}\dot{\varphi }\right)}{R{\omega }_l}\\ i_r=\frac{R{\omega }_r-\left(\dot{x}+\frac{B}{2}\dot{\varphi }\right)}{R{\omega }_r}\end{array}\right.$$

(4)

It is easy to learn that the tires on the same axis are on the same lateral velocity as follows:

$$u_{y,ij}=\left\{\begin{array}{ll}\dot{y}+a\dot{\varphi }& i=f\\ \dot{y}+b\dot{\varphi }& i=m\\ \dot{y}+c\dot{\varphi }& i=r\end{array}\right.$$

(5)

where a, b, and c denote the distance from each axis to the CM. Then, the tire sideslip angle on each wheel is expressed as:

$${\alpha }_{ij}=\frac{u_{y,j}}{u_{x,ij}}=\arctan \left(\frac{\dot{y}+{\alpha }_1\dot{\varphi }}{\dot{x}+{\alpha }_2\frac{B}{2}\dot{\varphi }}\right)=\arctan \left(\frac{\tan \beta +{\alpha }_1\frac{\dot{\varphi }}{\dot{x}}}{1+{\alpha }_2\frac{B}{2}\frac{\dot{\varphi }}{\dot{x}}}\right)$$

(6)

where the coefficient ${\alpha }_1$ and ${\alpha }_2$ are defined as:

$${\mathsf{\alpha }}_1=\left\{\begin{array}{ll}\mathit{a}& \mathit{i}=\mathit{f}\\ \mathit{b}& \mathit{i}=\mathit{m}\\ \mathit{c}& \mathit{i}=\mathit{r}\end{array},\hspace{1em}{\mathsf{\alpha }}_2=\left\{\begin{array}{ll}-1& \mathit{j}=\mathit{l}\\ 1& \mathit{j}=\mathit{r}\end{array}\right.\right.$$

(7)

3.2. Dynamic Model

In this study, a 3-DOF dynamic model is built to describe the vehicle dynamic behavior, as shown in Figure 2.

The coordinate is the same as the coordinate in the kinetic model in Figure 2. The 3-DOF dynamic model is set up as (8):

$$\left\{\begin{array}{l}m(\ddot{x}-\dot{y}\dot{\varphi })={\displaystyle \sum _{i=f,m,r}{\displaystyle \sum _{j=l,r}{F}_{x,ij}}}\\ m(\ddot{y}+\dot{x}\dot{\varphi })={\displaystyle \sum _{i=f,m,r}{\displaystyle \sum _{j=l,r}{F}_{y,ij}}}\\ I_z\ddot{\varphi }=\frac{B}{2}{\displaystyle \sum _{i=f,m,r}\left(F_{x,ir}-F_{x,il}\right)}+{\displaystyle \sum _{j=l,r}\left(a{F}_{y,f}+b{F}_{y,mj}+c{F}_{y,rj}\right)}\\ I_w{\dot{\omega }}_{ij}=T_{ij}-R{F}_{x,ij}-R{\omega }_{ij}\hspace{1em}i=f,m,r;j=l,r\end{array}\right.$$

(8)

where $F_{x,ij}$ and $F_{y,ij}$ denote the longitudinal and lateral force, respectively, on each tire; $I_z$ denotes the moment of inertia about the z-axis of the vehicle; and $I_w$ denotes the moment of inertia of the wheel. The tire force is calculated in the next section.

3.2.1. Tire Model

In this study, a modified brush tire model which considers combined slip, both lateral and longitudinal [46], is used to model the tire force in the vehicle dynamic model. The tire longitudinal and lateral force can be calculated as (9):

$$\begin{array}{l}\kappa =\frac{R\omega -u_x}{u_x}\\ \alpha =\arctan \left(\frac{u_y}{u_x}\right)\\ f=\sqrt{C_x^2{\left(\frac{\kappa }{1+\kappa }\right)}^2+C_{\alpha }^2{\left(\frac{\tan \alpha }{1+\kappa }\right)}^2}\\ F=\left\{\begin{array}{cc}f-\frac{1}{3\mu F_z}{f}^2+\frac{1}{27{\mu }^2{F}_z^2}{f}^3& f\le 3\mu F_z\\ \mu F_z& f>3\mu F_z\end{array}\right.\\ F=\left\{\begin{array}{cc}f-\frac{1}{3\mu F_z}{f}^2+\frac{1}{27{\mu }^2{F}_z^2}{f}^3& f\le 3\mu F_z\\ \mu F_z& f>3\mu F_z\end{array}\right.\\ F_x=\frac{C_x\kappa }{(1+\kappa )}\frac{F}{f}\\ F_y=-\frac{C_{\alpha }\tan \alpha }{(1+\kappa )}\frac{F}{f}\end{array}$$

(9)

where μ denotes the friction coefficient between road and tire; $C_x$ and $C_{\alpha }$ denote the longitudinal and lateral tire stiffness, respectively; κ and α denote longitudinal and sideslip rate on each tire, respectively. The longitudinal and lateral velocity are calculated from (3) and (5).

Figure 3 shows the tire longitudinal and lateral forces when the tire load is 5000 N and the road-tire adhesion coefficient is 0.8.

3.2.2. Tire Vertical Force Estimation

The tire force calculation for a multi-axis vehicle is a statically indeterminate problem. Although, in assumption (4), the suspension is omitted and the rolling/pitching movements are also omitted in the dynamic model proposed in Section 3.2, to calculate the tire vertical force for tire model (9), an assumption is proposed, i.e., the vertical force is proportional to the suspension deformation.

This assumption is only used in the tire vertical force calculation, and the suspension deformation is a virtual displacement and would not appear in the final calculating results of vertical force. The equations of force are expressed as:

$$\left\{\begin{array}{l}{F}_{zf}+F_{zm}+F_{zr}=G\cos \theta \\ m{a}_{x}h+G\sin \theta h+G\cos \theta a-F_{zm}(a-b)-F_{zr}(a-c)=0\\ \frac{{\mathsf{\Delta }}_{zr}-{\mathsf{\Delta }}_{zf}}{{\mathsf{\Delta }}_{zm}-{\mathsf{\Delta }}_{zf}}=\frac{a-c}{a-b}\\ F_{zi}=k_{si}{\mathsf{\Delta }}_{zi}\hspace{1em}i=f,m,r\end{array}\right.$$

(10)

where $F_{zf},F_{zm},F_{zr}$ denote the vertical force on the front, middle, and rear axis, respectively; h denotes the height of mass center; ${\mathsf{\Delta }}_{zi}$ denotes the suspension deformation on each axis; $k_{si}$ denotes the suspension stiffness, which would be divided out in the calculating procedure, and therefore the value would not influence the result. The tire vertical force on the longitudinal direction is as (11):

$$\left\{\begin{array}{l}{F}_{zf}=\frac{\begin{array}{l}G\cos (\theta )\left(-a(b+c)+b^2+c^2\right)\\ +Gh\sin (\theta )(-2a+b+c)+hm{a}_x(-2a+b+c)\end{array}}{2\left(a^2-a(b+c)+b^2-bc+c^2\right)}\\ F_{zm}=\frac{\begin{array}{l}G\cos (\theta )\left(a^2-ab+c(c-b)\right)\\ +Gh\sin (\theta )(a-2b+c)+hm{a}_x(a-2b+c)\end{array}}{2\left(a^2-a(b+c)+b^2-bc+c^2\right)}\\ F_{zr}=\frac{\begin{array}{l}G\cos (\theta )\left(a^2-ac+b(b-c)\right)\\ +Gh\sin (\theta )(a+b-2c)+hm{a}_x(a+b-2c)\end{array}}{2\left(a^2-a(b+c)+b^2-bc+c^2\right)}\end{array}\right.$$

(11)

Taking lateral acceleration into account:

$$\mathsf{\Delta }{F}_{zr}=-\mathsf{\Delta }{F}_{zl}=\frac{m{a}_{y}h}{B}$$

(12)

where $\mathsf{\Delta }{F}_{zr},\mathsf{\Delta }{F}_{zl}$ is the vertical force variation of the right and left side caused by lateral acceleration.

Then, combining (11) and (12), the vertical tire force is calculated by:

$$F_{zij}=\left\{\begin{array}{c}\frac{F_{zi}}{2}-\frac{m{a}_{y}h}{3B} i=f,m,r j=l\\ \frac{F_{zi}}{2}+\frac{m{a}_{y}h}{3B}\hspace{1em}i=f,m,r j=r\end{array}\right.$$

(13)

The simulation result in TruckSim is shown in Figure 4. From 8 s, the vehicle begins to accelerate, the load on the rear axis increases, while the load on front axis decreases. From 30 s, the vehicle turns, the load on right side increases and the load on left side decrease. The continuous line denotes the tire load simulated from TruckSim, while the dashed line denotes the load calculated by (13). The simulation result shows that the vertical force calculation method shown in (13) satisfies the accuracy requirements at different driving conditions.

4. Observer Design

In this section, a UKF-based observer is proposed to estimate the skid-steered vehicle maneuvering state.

4.1. Unscented Kalman Filter

For a given discrete system:

$$\left\{\begin{array}{l}{x}_{k+1}=f\left(x_k,u_k,w_k\right)\\ y_k=h\left(x_k,u_k,v_k\right)\end{array}\right.$$

(14)

The UKF computational procedure is described in the following steps.

Step 1 Initiate the mean value and variance matrix.

$$\begin{array}{c}{\widehat{x}}_0^{+}=E\left(x_0\right)\\ P_0^{+}=E\left[\left(x_0-{\widehat{x}}_0^{+}\right){\left(x_0-{\widehat{x}}_0^{+}\right)}^T\right]\end{array}$$

(15)

where ${\widehat{x}}_0^{+}$ is the posteriori estimation value of the expected mean value and $P_0^{+}$ is the posteriori estimation of variance matrix when the computing step k = 0.

Step 2 Time update.

(a) The sigma points are chosen by:

$$\begin{array}{c}{\widehat{x}}_{k-1}^{(i)}={\widehat{x}}_{k-1}^{+}+{\tilde{x}}^{(i)},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=0,\dots ,2n\\ {\tilde{x}}^{(i)}=0,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=0\\ {\tilde{x}}^{(i)}={\left(\sqrt{(n+k)P_{k-1}^{+}}\right)}_i^T,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=1,\dots ,n\\ {\tilde{x}}^{(n+i)}=-{\left(\sqrt{(n+k)P_{k-1}^{+}}\right)}_i^T,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=1,\dots ,n\end{array}$$

(16)

(b) Use the system function f() transforms the sigma points:

$${\widehat{x}}_k^{(i)}=f\left({\widehat{x}}_{k-1}^{(i)},u_k,t_k\right)$$

(17)

(c) Compute the a priori estimation value of the expected mean value:

$${\widehat{x}}_k^{-}=\sum _{2n}^{i=0}{W}_m^{\left(i\right)}{\widehat{x}}_k^{\left(i\right)}$$

(18)

where $W^{(i)}$ represents the unscented transform weighted average coefficient.

$$\begin{array}{c}{W}_m^{(0)}=\frac{t}{n+t}\\ W_c^{(0)}=\frac{t}{n+t}+\left(1-a^2+b\right)\\ W_m^{(i)}=W_c^{(i)}=\frac{1}{2(n+t)},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} (i=1,\dots ,2n)\end{array}$$

(19)

where $\tau ={\alpha }^2\left(n+\kappa \right)-n$; $\kappa$ is the adjust coefficient to decrease the high-order error, $n+\kappa \ne 0$; α is usually chosen as a small positive value, it reflects the distribution of sigma points at the ${\widehat{\mathit{x}}}_{\mathit{k}}$. The smaller the α value, the smaller the influence of the nonlinear high order of the function; β describes the system distribution information and β = 2 when the system is a Gaussian distribution.

(d) Compute the a priori estimation of the variance matrix:

$$P_k^{-}={\displaystyle \sum }_{2n}^{i=0}{W}_c^{\left(i\right)}\left({\widehat{x}}_k^{\left(i\right)}-{\widehat{x}}_k^{-}\right){\left({\widehat{x}}_k^{\left(i\right)}-{\widehat{x}}_k^{\top }\right)}^T+Q_{k-1}$$

(20)

Step 3 Measurement update.

(a) Refresh the sigma points:

$$\begin{array}{c}{\widehat{x}}_k^{(t)}={\widehat{x}}_k^{-}+{\tilde{x}}^{(i)},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=0,\dots ,2n\\ {\tilde{x}}^{(i)}=0,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=0\\ {\tilde{x}}^{(i)}={\left(\sqrt{(n+k)P_k^{-}}\right)}_i^T,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=1,\dots ,n\\ {\tilde{x}}^{(n+i)}=-{\left(\sqrt{(n+k)P_k^{-}}\right)}_i^T,\hspace{0.33em}\hspace{0.33em}\hspace{0.33em} i=1,\dots ,n\end{array}$$

(21)

(b) Transform sigma points using measurement function h():

$${\widehat{y}}_k=\sum _{2n+1}^{i=1}{W}_m^{\left(i\right)}{\widehat{y}}_k^{\left(i\right)}$$

(22)

(c) Compute the estimation of measurement:

$${\widehat{y}}_k=\sum _{2n+1}^{i=1}{W}_m^{\left(i\right)}{\widehat{y}}_k^{\left(i\right)}$$

(23)

(d) Compute the measurement estimation variance:

$$P_y=\sum _{2n}^{i=0}{W}_c^{\left(i\right)}\left({\widehat{y}}_k^{\left(i\right)}-{\widehat{y}}_k\right){\left({\widehat{y}}_k^{\left(i\right)}-{\widehat{y}}_k\right)}^T+R_k$$

(24)

(e) Compute the covariance between ${\widehat{x}}_k^{-}$ and ${\widehat{y}}_k$:

$$P_{xy}=\sum _{2n}^{i=0}{W}_c^{\left(i\right)}\left({\widehat{x}}_k^{\left(i\right)}-{\widehat{x}}_k^{-}\right){\left({\widehat{y}}_k^{\left(i\right)}-{\widehat{y}}_k\right)}^T$$

(25)

(f) Compute the a posteriori estimation of expected mean value and variance matrix:

$$\begin{array}{c}{K}_k=P_{xy}{P}_y^{-1}\\ {\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left(y_k-{\widehat{y}}_k\right)\\ P_k^{+}=P_k^{-}-K_k{P}_y{K}_k^T\end{array}$$

(26)

4.2. Strong Tracking UKF

Equations (16) and (21) show the UKF computing procedure for computing Cholesky decomposition, which requires the variance matrix to be positive, but in some cases, the matrix, $p_k^{+}$, cannot be kept as a positive-definite matrix, then, the algorithm would be terminated. To improve the astringency of the algorithm, in this study, the strong tracking algorithm is introduced.

The strong tracking UKF (STUKF) uses the residual of state estimation value to correct the a posteriori variance matrix. Define innovation as:

$$Y_{err}=y_k-{\widehat{y}}_k$$

(27)

And innovation Y_err satisfies:

$$Y_{\mathrm{e}\mathrm{r}\mathrm{r}}^{\mathrm{T}}{Y}_{\mathrm{e}\mathrm{r}\mathrm{r}}\le S\cdot trE\left(Y_{\mathrm{e}\mathrm{r}\mathrm{r}}{Y}_{\mathrm{e}\mathrm{r}\mathrm{r}}^{\mathrm{T}}\right)$$

(28)

where $S\ge 1$ represents the threshold coefficient.

The STUKF computing procedure is as follows:

Step 1 Calculate residual and residual variance matrix.

$$\begin{array}{c}{Y}_{\mathrm{e}\mathrm{r}\mathrm{r}}=y_k-{\widehat{y}}_k\\ C_{0,k}=\left\{\begin{array}{cc}{Y}_{\mathrm{e}\mathrm{r}\mathrm{r}}{Y}_{\mathrm{e}\mathrm{r}\mathrm{r}}^{\mathrm{T}}& k=1\\ \frac{\beta C_{0,k}+Y_{\mathrm{e}\mathrm{r}\mathrm{r}}{Y}_{\mathrm{e}\mathrm{m}}^{\mathrm{T}}}{1+\beta },& k>1\end{array}\right.\end{array}$$

(29)

where β is the forgotten factor ($\beta <1$) is to decrease the influence of early information to enhance the tracking ability.

Step 2 Calculate the suboptimal fading factor at the current time.

$$\begin{array}{c}{\rho }_k=\left\{\begin{array}{cc}{\rho }_0& {\rho }_0\ge 1\\ 1& {\rho }_0<1\end{array}\right.\\ {\rho }_0=\frac{N_k}{M_k}\\ N_k=tr{\left(C_{0,k}-R\right)}^T\\ M_k=tr\left({\displaystyle \sum _{2n}^{i=0}}{W}^{\left(i\right)}\left({\widehat{y}}_k^{\left(i\right)}-{\widehat{y}}_k\right){\left({\widehat{y}}_k^{\left(i\right)}-{\widehat{y}}_k\right)}^T\right)\end{array}$$

(30)

where tr() denotes the trace of the matrix.

Step 3 Update the variance matrix.

$$P_k^{-}={\rho }_k\sum _{2n}^{i=0}{W}^{\left(i\right)}\left({\widehat{x}}_k^{\left(i\right)}-{\widehat{x}}_k^{-}\right){\left({\widehat{x}}_k^{\left(i\right)}-{\widehat{x}}_k^{-}\right)}^T+Q_{k-1}$$

(31)

The next step is the same as the traditional UKF.

4.3. Adaptive Noise Matrix Adjustment

The variance matrix in the UKF is hard to get. The traditional variance matrix in the UKF is a fixed value depending on experience.

A prominent difference between a skid-steered wheeled vehicle and an Ackerman steered vehicle is that when a skid-steered vehicle is turned, the sideslip angle of the tires is considerably more than that of an Ackerman vehicle. Therefore, the nonlinearity of the skid-steered wheeled vehicle is larger than that of an Ackerman vehicle, especially when the vehicle turns. When the sideslip angle of the tires increases, the inaccuracy of tire model also increases. To decrease the influence of nonlinearity, an adaptive variance matrix algorithm is proposed.

The sideslip angle of the tires is regarded as the main factor which is an incremental impact of variance matrix for a skid-steered wheeled vehicle. Equation (6) shows the lateral velocity and yaw rate are positively correlated to the sideslip and longitudinal velocity is negatively correlated to the sideslip.

The authors of [35] and [30] proposed a linear variance matrix adaptive algorithm and based on this algorithm, an adaptive variance matrix algorithm is proposed for a skid-steered wheeled vehicle. In this algorithm, the major factor influent on variance matrix is regarded as the sideslip rate of the tires.

The variance matrixes are shown as (32):

$$Q=\left[\begin{array}{ccc}{q}_{11}& & \\ & q_{22}& \\ & & q_{33}\end{array}\right]\hspace{1em}R=\left[\begin{array}{ccc}{r}_{11}& & \\ & r_{22}& \\ & & r_{33}\end{array}\right]$$

(32)

where

$$\left\{\begin{array}{ll}{q}_{11}=& q_{a1}\left|a_x\right|+q_{a2}\left|a_y\right|+q_{a3}\left|{\displaystyle \sum _{i,j}{S}_{ij}}\right|\\ & +q_{a4}\left|{\displaystyle \sum _{i,j}{\alpha }_{ij}}\right|+q_{a5}\left|\gamma \right|+q_{a0}\\ q_{22}=& q_{b1}\left|a_x\right|+q_{b2}\left|a_y\right|+q_{b3}\left|{\displaystyle \sum _{i,j}{S}_{ij}}\right|\\ & +q_{b4}\left|{\displaystyle \sum _{i,j}{\alpha }_{ij}}\right|+q_{b5}\left|\gamma \right|+q_{b0}\\ q_{33}=& q_{c1}\left|a_x\right|+q_{c2}\left|a_y\right|+q_{c3}\left|{\displaystyle \sum _{i,j}{S}_{ij}}\right|\\ & +q_{c4}\left|{\displaystyle \sum _{i,j}{\alpha }_{ij}}\right|+q_{c5}\left|\gamma \right|+q_{c0}\end{array}\right.$$

(33)

$$\left\{\begin{array}{l}{r}_{11}=K_{r1}\left[r_{a1}\left(d_{a1}-\left|a_x\right|\right)+r_{a3}\left(d_{a3}-\left|\gamma \right|\right)\right]+r_a\\ r_{22}=K_{r1}\left[r_{b2}\left(d_{b2}-\left|a_y\right|\right)+r_{b3}\left(d_{b3}-\left|\gamma \right|\right)\right]+r_b\\ r_{33}=r_c\end{array}\right.$$

(34)

The parameters K, q, and r are calibrated by the experimental data. Equation (33) expresses that the wide range maneuver would increase the nonlinearity, which will result in the increasing variance of the system. Equation (34) expresses that the large measurement data would decrease the relative error of measurement.

4.4. Observer Design

The dynamic model of the vehicle is described as (8). Define the observed object as (35):

$$\left\{\begin{array}{l}\dot{x}=f(x,u)+w\\ y=h(x,u)+v\end{array}\right.$$

(35)

where the state vector is $x={[v_x v_y \dot{\varphi }]}^T$, the measurement vector is $y={[a_x{a}_y\dot{f}]}^T$, the controlling input is $u={[{\omega }_l {\omega }_r]}^T$, $w={[q_1 q_2 q_3]}^T$ is the process noise vector, and $v={[r_1 r_2 r_3]}^T$ is the measurement noise vector.

Discretize dynamic model (8):

$$\left\{\begin{array}{l}{x}_1(k+1)=\left(\left({\displaystyle \sum _{\begin{array}{l}j=l,r\\ i=f,m,r\end{array}}{F}_{x,ij}\left(k\right)}\right)/m+x_2(k)x_3(k)\right)\Delta T+x_1(k)\\ x_2(k+1)=\left(\left({\displaystyle \sum _{\begin{array}{l}j=l,r\\ i=f,m,r\end{array}}{F}_{y,ij}\left(k\right)}\right)/m-x_1(k)x_3(k)\right)\Delta T+x_2(k)\\ x_3(k+1)=\left(\begin{array}{l}\frac{B}{2}{\displaystyle \sum _{i=f,m,r}\left(F_{x,ir}-F_{x,il}\right)}\\ +{\displaystyle \sum _{j=l,r}\left(a{F}_{y,j,i}+b{F}_{y,mj}+c{F}_{y,rj}\right)}\end{array}\right)\Delta T/I_z+x_3(k)\end{array}\right.$$

(36)

where F_x,ij and F_y,ij is the function of x and u, respectively, which is calculated by (9), $F_{y,ij}(k)=F_{y,ij}(x(k),u(k))$ and $F_{x,ij}(k)=F_{x,ij}(x(k),u(k))$; $\Delta T$ is the time interval.

The discretization for measurement function is the same as (37):

$$\left\{\begin{array}{l}{y}_1\left(k+1\right)=\frac{{\displaystyle \sum _{i=f,m,r}}{\displaystyle \sum _{j=l,r}}{F}_{x,ij}\left(k+1\right)}{m}\\ y_2\left(k+1\right)=\frac{{\displaystyle \sum _{i=f,m,r}}{\displaystyle \sum _{j=l,r}}{F}_{y,ij}\left(k+1\right)}{m}\\ y_3\left(k+1\right)=x_3\left(k+1\right)\end{array}\right.$$

(37)

The discrete system model is:

$$\left\{\begin{array}{l}x(k+1)=f(x(k),u(k))+w(k)\\ y(k+1)=h(x(k+1),u(k))+v(k)\end{array}\right.$$

(38)

The road friction factor and suspension stiffness are identified in a traditional algorithm and are not described in this study. The observer structure is shown in Figure 5.

5. Experiment Results and Analysis

In this section, a test experiment is set up to verify the AN-STUKF proposed in this study. First, the experimental platform designed by our library and the sensors set up on the vehicle are introduced. Then, two driving conditions are set up to verify the accuracy of the proposed estimation algorithm. The estimating results are analyzed to explain the skid-steered wheeled vehicle dynamic characteristics. Figure 6 shows the verification test experiment vehicle.

5.1. Experimental Platform Introduction

The experimental platform is a skid-steered wheeled vehicle with a three-axis rock arms unmanned vehicle. Two driving motors are installed on the two side of the vehicle body to drive the three wheels on the same side by transmission mechanism. The rotation speed is given by an encoder on the motor and torque is given by a motor controller. The wheel rotation on the same side is the same, due to the transmission structure. The rotation rate and torque on a wheel is calculated by:

$$\begin{array}{l}{\omega }_l=\frac{2\pi n_{motor\_l}}{60i},{\omega }_r=\frac{2\pi n_{motor\_r}}{60i}\\ {\displaystyle \sum _{i=fl,ml,rl}{T}_i=i{T}_{motor\_l}},{\displaystyle \sum _{i=fr,mr,rr}{T}_i=i{T}_{motor\_r}}\end{array}$$

(39)

where ${\omega }_l,{\omega }_r,T_l,T_r$, respectively, denote the rotation rate and torque output by the left and right motor and i is the transmission rate from the motor to the wheel.

The IMU located on the geometric center provides the acceleration and rotation rate on the three axes. On the basis of the assumption proposed in Section 3, the center of mass is located on the geometric center. The location of the IMU is to decrease the bias caused by the installation site. The double antenna is set up on the axle wire of the vehicle. The RTK-GNSS provides accurate longitudinal and lateral velocity, as well as position and direction information. A binocular camera is located on the front of the vehicle to gather environment information. An 8-line LIDAR is located on the front and a 32-line LIDAR is located on the top of the vehicle to realize obstacle detection. However, environmental information from the camera and LIDAR is not used in this study. The sensors used in the proposed estimation algorithm include the IMU, GNSS, and encoder. The vehicle parameters are shown in

Table 1. Vehicle intrinsic parameters.

Parameters	Symbols	Value
Vehicle mass	m (kg)	3000
Length of the vehicle	L (m)	4.7
Width of the vehicle	B (m)	2.060
Length of the arm	l (m)	0.7
Distance between 1th and 2nd axis	a (m)	1.836
Distance between 2nd and 3rd axis	b (m)	1.036
Distance between 2nd and CM	x (m)	0.244
Distance between each axises	c (m)	1.400
Height of CM	H (m)	0.600
rotational inertia round z-axis	Iz (kg·m2)	6390
Transmission rate	i	27.13
Longitudinal stiffness of tire	Cx (N)	31300
Lateral stiffness of tire	Ca (N)	40200
Width of the tire	Bw (m)	0.210
Radius of the tire	R (m)	0.375

.

5.2. Staright Driving Condition

In this experimental condition, the test vehicle accelerates from 0 to 30 km/h. In the straight driving condition, only the longitudinal state is discussed in this study. The experimental data and estimation results are shown in Figure 7.

In Figure 7, the AN-STUKF (blue line) shows better tracking rate than the traditional UKF (red line) when the vehicle accelerates. After the vehicle longitudinal velocity reaches steady state, the AN-STUKF and the traditional UKF both remain steady near the true state given by RTK-GNSS. The results of this experiment verify the accuracy of AN-STUKF and the strong traceability of the strong tracking algorithm. When the vehicle drives steadily, the UKF and AN-STUKF possess almost similar performance of estimation accuracy and tracking rate.

Figure 7 also provides the longitudinal slip rate of the two side wheels. The longitudinal slip rate is calculated by (4). When a vehicle begins to accelerate, the longitudinal rate increases. The longitudinal slip rate remains at almost zero when the longitudinal velocity is steady.

The skid-steered vehicle performs with almost no difference as compared with an Ackerman turned vehicle when it drives straight. In the following section, the feasibility of the proposed algorithm, when the skid-steered vehicle turns, is verified.

5.3. Turning Driving Condition at Low Speed

In this study, we emphatically discuss the vehicle state estimation in turning conditions. In this experimental condition, the test vehicle turns at a low speed (about 5 km/h) and turns at a 0.2 rad/s. The vehicle moving trace is shown in Figure 8. Figure 8 shows that the turn radius is about 7 m. The vehicle moves in a counterclockwise direction.

Figure 9 shows the longitudinal state of the vehicle when it turns. The two estimation algorithms show almost no difference when the longitudinal velocity changes slightly. The longitudinal slip rate of the right side is positive becuse the right left is the outside wheels which are in the skidding condition. Similarly, the left side is the inside wheels which are in the slipping condition. When a skid-steered wheeled vehicle turns, the speed difference between the wheels on the sides would lead to a large tire slip rate even though the vehicle is moving on the ground with a high adhesion coefficient.

The lateral velocity, yaw rate, and tire sideslip angle estimation results are shown in Figure 10. The two estimation algorithms perform almost similarly when observing yaw rate, because yaw rate is the measurement value. In this study, the yaw rate calculated by the encoders’ data is also compared with the observer result which is the cyan line shown in Figure 10. The calculated result by the encoder is larger than the real values because it ignores the longitudinal slip rate. The result calculated by encodes verifies that simple longitudinal velocity and yaw rate calculating method only using an encoder is not accurate enough when the vehicle driving is in strong nonlinear conditions.

The lateral velocity estimation result by AN-STUKF is more accurate than the results estimated by the traditional UKF. The AN-STUKF shows higher accuracy than the traditional UKF when the vehicle drives in strong nonlinear conditions.

The sideslip angle of tires is calculated by (6). As shown in Figure 11, all the sideslip angles are negative, which expresses that all the tire cornering directions are negative to the y-axis. The sideslip angles indicate that the middle and rear axes are obviously in a nonlinear condition. The tires on the rear axis are out of the sideslip limit, which is usually about 30 degrees [47]. A large sideslip angle indicates that the tires are in a severe lateral slip condition. This driving condition should be avoided. One reason is that lateral sideslip of tire would damage the tire. Another reason is that a large sideslip angle would require the tire to work in a severe nonlinear condition, which creates difficulty in control. Figure 11 shows the tire marks of a skid-steered wheeled vehicle driving in turning and straight conditions. The tire is obviously skidding when the vehicle turns.

The sideslip angle of vehicle body is calculated by:

$$\beta =\arctan \left(\frac{\dot{y}}{\dot{x}}\right)$$

(40)

The sideslip angles of the vehicle body estimation results are shown in Figure 12. The AN-STUKF performs better than the traditional UKF in sideslip angle observing, especially in nonlinear conditions. The sideslip angle of a skid-steered wheeled vehicle when it turns at a small radius would become quite large, which would cause a negative effect on vehicle stability control.

Table 2. Error analyses for small radius turning.

Parameters	Max Absolute Error (Rad)	Max Relative Error	Mean Absolute Error (Rad)	Mean Relative Error	Standard Deviation
UKF	0.3619	89.57%	0.0530	49.47%	0.0732
AN-STUKF	0.1305	46.11%	0.0201	17.24%	0.0453

shows the error analyses of the small radius turning driving condition for the two estimations. The relative error used the data from 20 to 40 s to avoid the influence of values close to zero. The proposed AN-STUKF performs much better than the traditional UKF in all error evaluation indexes.

Here, we define the instantaneous center of rotation (ICRs) as (41):

$$\left\{\begin{array}{l}{x}_{ICRv}=-\frac{\dot{y}}{\dot{\varphi }}\\ y_{ICRv}=\frac{\dot{x}}{\dot{\varphi }}\end{array}\right.$$

(41)

where x_ICRv and y_ICRv are defined in Figure 1, which are expressed as the coordinate values of vehicle instantaneous center of rotation on the x-label and y-label, respectively.

The data from 20 to 40 s is chosen to calculate the ICRs shown in Figure 13 to analysis the vehicle kinetic characteristics, because, in this time period, the vehicle is moving at a relatively high rotating rate. The ICRx (whose average value is about 3 m) is larger than the distance between the first axis ($a=1.836\mathrm{m}$, which is shown in Table 1) and the CM of the vehicle, which marks that the ICR of the vehicle is ahead of the first axis, as shown in Figure 14. The ICRy shows the ICR of the vehicle is located on the left side of the vehicle, which performs similarly with the rotating direction.

The ICRs shifting forward is an important phenomenon in skid-steered wheeled vehicle turning. The authors of [14,48] discuss the phenomenon of ICRs shifting forward in large radius turning conditions. The ICRs moving forward reflect the increasing of sideslip angle of the vehicle body, which leads to the instability of the vehicle. The ICRs moving forward also reflects that all the lateral tire force point to the direction of the rotating center. When the ICRs are between the first and rear axis, the sideslip angle of the first axis tire is still positive. This phenomenon reflects that the vehicle needs large lateral tire force to satisfy the lateral motion.

5.4. Turning Driving Condition at High Speed

In this experimental test, the vehicle drives at a S-shaped trace at the speed of 30 km/h. The trace is shown in Figure 15. The vehicle turns in a large radius.

Figure 16 shows the longitudinal state of the vehicle when it turns. The two estimation algorithms show almost no difference when the longitudinal velocity changes slightly. The longitudinal slip rate is relatively small, and the symbol of left and right slip rate alternately changes. This reflects that the two-side tires alternately are skidding and slipping. The estimation results correspond to the vehicle trace.

The lateral velocity, yaw rate, and tire sideslip angle estimation results are shown in Figure 17. The two estimation algorithms perform almost similarly when observing yaw rate because yaw rate is the measurement value.

The estimated lateral velocity from AN-STUKF performs with better accuracy than the traditional UKF, in most time periods. However, sometimes the traditional UKF performs better than AN-STUKF. The possible reason is that the linear adjustment algorithm of variance matrix cannot satisfy all conditions.

The sideslip angle of tires in the large radius condition is relatively small as compared with the small radius condition.

The sideslip angle of the vehicle body estimating result shown in Figure 18 shows similar characteristics as that of the lateral velocity estimating result. The sideslip angle is relatively small, so the vehicle condition is regarded to be steady.

Table 3. Error analyses for S-shape trace.

Parameters	Max Absolute Error	Max Relative Error	Mean Absolute Error	Mean Relative Error	Standard Deviation
UKF	0.0278	145.71%	0.0090	34.05%	0.0108
AN-STUKF	0.0285	119.35%	0.0085	25.63%	0.0105

shows the error analyses in small radius turning driving condition for the two estimations. The relative error used the data from 20 to 25 s to avoid the influence of values close to zero. The proposed AN-STUKF performance does not have much advantage as compared with the traditional UKF in all error evaluation indexes when the vehicle does not drive in high-nonlinear conditions.

The ICRs shown in Figure 19 in this condition show abnormity when the yaw rate approaches zero, because the effect of error increases when the real state value is small. Thus, in this section, we cut out the data from 4 to 10 s to analyze the ICRs. The ICRx estimating result shows that, even though the turning radius is small, the ICRs would show the phenomenon of shifting forward when the speed is high enough. The experiment verifies the critical speed of ICRs moving forward proposed in [13].

6. Conclusions and Future Works

In this study, we present an AN-STUKF estimating algorithm to observe the state of the skid-steered wheeled vehicle. A 3-DOF dynamic model is built to describe the vehicle dynamic characteristics, in which the Brush nonlinear tire model is used to estimate the tire and road contact information. The strong tracking algorithm is used to enhance the stability of the UKF algorithm. The adaptive variance adjustment is used to increase the accuracy when the vehicle drives in strong nonlinear conditions. In this observer, longitudinal velocity, lateral velocity, and yaw rate of the vehicle are estimated. Then, based on these estimated states, the sideslip angle of vehicle body and instantaneous center of rotation (ICR) are calculated to reflect the vehicle motion characteristics.

In the experimental test, two driving conditions are considered to verify the accuracy of the proposed estimating algorithm. The accelerating straight driving condition verifies that the AN-STUKF traces faster than the traditional UKF. The experimental test in turning condition at small radius verifies that the AN-STUKF performs better than the traditional UKF when the vehicle drives in high nonlinear conditions. Then, the sideslip angle of vehicle body and the instantaneous center of rotation are computed by the estimation results. These two states’ estimation results reflect that the instability increases in small radius turning conditions.

However, even though the AN-STUKF performs some improvement in estimation accuracy and tracing rate, the application of this estimation algorithm still requires investigation. The parameters of the adaptive variance matrix adjustment require heavy workload to tune according to the experimental data and the linear method cannot satisfy all driving conditions. The computational burden also needs to be considered and this is an important reason why the linear method is used in the noise matrix adjustment instead of a more accurate exponent method. The accuracy of the used nonlinear tire model also needs to be verified. The kinetic model would be considered into the estimation model to improve the robustness of the observer.