《NONLINEAR ESTIMATION METHODS FOR AUTONOMOUS TRACKED VEHICLE WITH SLIP.doc》由会员分享，可在线阅读，更多相关《NONLINEAR ESTIMATION METHODS FOR AUTONOMOUS TRACKED VEHICLE WITH SLIP.doc（4页珍藏版）》请在三一文库上搜索。
1、NONLINEAR ESTIMATION METHODS FOR AUTONOMOUS TRACKED VEHICLE WITH SLIP*Abstract: In order to achieve precise, robust autonomous guidance and control of a tracked vehicle, a kinematic model with longitudinal and lateral slip is established. Four different nonlinear filters are used to estimate both st
2、ate vector and time-varying parameter vector of the created model jointly. The first filter is the well-known extended Kalman filter. The second filter is an unscented version of the Kalman filter. The third one is a particle filter using the unscented Kalman filter to generate the importance propos
3、al distribution. The last one is a novel and guaranteed filter that uses a linear set-membership estimator and can give an ellipsoid set in which the true state lies. The four different approaches have different complexities, behavior and advantages that are surveyed and compared. Key words: Tracked
4、 vehicle Nonlinear estimation Kalman filter Partis filter Set-membership filter0 INTRODUCTIONTracked vehicles are currently ucsu :n many urban reconnaissance and rescue mission scenarios11, autonomous planetary explorations, snd other applications where terrain conditions are difficult to predict. T
5、Viey re better suited for such tasks than wheeled vehicles due to the larger contact area of tracks that provides them with better stability and traction at various ground conditions3.The general kinematic model of tracked vehicles without slip is similar to common wheeled robots, in which the slip
6、is considered as disturbance or noise to the model4. However, the slip in both longitudinal and lateral directions appears very often in practice, and has a critical influence on the performance of vehicles. In order to improve the accuracy of the localization and control the robot more efficiently,
7、 it is important to establish the slip model in which the slip is regarded as time-varying parameters. Currently some works have covered this question56, but few consider the slip in both directions. As the slip parameters can also be incorporated into the controller, they may be estimated with the
8、pose of the vehicle simultaneously. To solve this problem of nonlinear joint estimation, some nonlinear filters can be employed.The classical method is the extended Kalman filter (EKF)71. The EKF is an approximate extension of Kalman filtering techniques to nonlinear systems by linearizing the state
9、 dynamics at each step of the time. However, it does require that the state dynamics is sufficiently differentiable (i.e. the Jacobian exists), and the linear approximation can lead to poor representations of the nonlinear functions and probability distributions of interest. As a result, this filter
10、 may diverge.Recently, a new variant of the Kalman filter, unscented Kalman filter (UKF) 9!, which approximate the nonlinearity to a higher order, has been proposed. The filter is founded based on the intuition that it is easier to approximate a Gaussian distribution than arbitrary nonlinear functio
11、ns. It may result in a more accurate filter than EKF. UKF can generate much better estimates of the covariance of the states. Because the above two filters are based on the Kalman filter, they cant get rid of the assumption of Gaussian distributions.The popular solution strategy for the general filt
12、ering problem is to use sequential Monte Carlo methods, also known as particle filters10. These methods allow for a complete representation of the posterior distribution of the states, so they can deal with any nonlinear dynamic. However, in order to get a good estimation, many particles are needed.
13、 A new particle filter that uses UKF to generate the importance proposal distribution has been proposed1. This novel filter, which can be called asUPF, tai greatly decicase the number of particles but still guarantee the estimation accuracy.The stochastic algorithms mentioned above often need some p
14、rior knowledge of noise distributions, and are not proper for those applications that require guaranteed uncertainty bounds such as many robust and optimized control customization approaches. The set-membership filters based on unknown-butbounded noise assumption have more advantages under these con
15、ditions1213. For nonlinear systems, an extended version of linear set-membership filter was proposed to guarantee nonlinear estimation14. This extended set-membership filter (ESMF) linearizes the nonlinear dynamics about the current estimate in a way similar to the EKF. The remaining terms are bound
16、ed and incorporated into the algorithm as additions to the process or sensor noise bounds. This allows the solution to be guaranteed for nonlinear systems so long as the bound on the nonlinear term is guaranteed. ESMF is compatible with many robust control methods that require hard bounds, and plann
17、ing algorithms that require guaranteed uncertainty information. It is also computationally efficient for on-line recursive implementation.The rest of the paper is organized as follows: In section 1, the kinematic model with slip is established and the problem of nonlinear joint estimation is describ
18、ed; In section 2, algorithms of four filters are described; The simulation results are showed and compared in section 3; In section 4, conclusions are drawn and future work is proposed.1 KINEMATIC MODEL1.1 Kinematic modelingIn this section, a general kinematic model with slip of tracked vehicles wil
19、l be developed. The slip is described by three time-varying parameters. Fig. 1 shows the platform of the vehicle undergoing general plane motion.In order to describe the motion of the tracked vehicle, a fixed reference frame F(xw, yw) and a moving frame F2(xm ym) attached to the vehicle body with or
20、igin at the geometric center Om of the vehicle are defined. The motion of the vehicle is composed of the translation velocity vc=(vx vy)T and the rotational velocity fflfc=d yr/fa, where vx and vy stand for the projection of the vc to axes of the frame F2, and y is the yaw angle. vy is termed with t
21、he side-slipping velocity. / is the instantaneous centre of rotation. Because of the sideslip, it often shifts ahead 0mby a mount d. An angle a exists between the line connecting / to Om and the perpendicular of/to x axis of frame F2.In the frame F2, a suitable model with slip can be written as In t
22、he simulation and experiments, X, Y and if are the measurements.1.2 Statement of joint estimation problemIn joint estimation151, states and parameters are estimated simultaneously using a same filter. It is often used to solve the state feedback control with uncertain parameters, or the modeling of
23、the parameters with noise and states that cant be measured directly. Because of the incorporation of the states and the parameters, more accurate results may be made using this approach. In the localization of the tracked vehicle with slip, the pose and the slip parameters should be estimated at the
24、 same time. A new state vector is defined as a combination of the old state and parameter vector, x, = X Y yi ir ctt. In this augmented state, the dynamic of the slip parameters are often unknown. They can be rewritten as e R is the additive process noise which drives the model.2 NONLINEAR FILTERS F
25、OR ESTIMATIONThis section will introduce four different nonlinear filters: extended Kalman filter, unscented Kalman filter, particle filtewith UKF proposal, and extended set-membership filter. The algorithms will be described with respect to the following general nonlinear systemwhere wk and v*+i ar
26、e process and measurement noise with covariance Qt and Rt+U respectively. The initial state vector is defined as *0-2.1Extended Kalman Alter (EKF)The EKF is a minimum mean-square-error (MMSE) estimator based on the Taylor series expansion of the nonlinear functions around the estimated states. Using
27、 only the linear expansion terms, it is easy to derive the following update equations for the Gaussian approximation to the poster distribution of the states.Given the estimation x, and the covariance Pt at time k,A=0,1, , then at time k+, the EKF algorithm can be summarized as follows.Note that the
28、 nonlinear system is linearized only when the covariance and filter gain coefficients are computed. For the state estimation propagation, the original nonlinear model is used.2.2Unscented Kalman Alter (UKF)Unlike EKF, the UKF does not approximate the nonlinear process and observation models. Instead
29、, it uses the true nonlinear models and approximates the distribution of the state random variable. The UKF, which does not need to compute the Jacobian, uses the so-called unscented transform (UT) and sigma points to propagate all of them through models. As a result, the UKF often leads to more acc
30、urate estimations than EKF. The UKF algorithm is given below.(1) Initialize withwhere L is the dimension of the augmented states; a controls the size of the sigma point distribution and should be ideally a small number to avoid sampling non-local effects when the nonlinearities are strong; And ft is
31、 a non-negative weighting term, which can be used to acknowledge the information of the higher order moments of the distribution. For a Gaussian prior the optimal choice is ft = 2 , and to guarantee positive semi-definiteness of the covariance matrix, k 0 is chosen. The rest parameters are defined a
32、s Note that the above equations are for the most general form of the UKF. For some special cases such as the model is linear, the algorithm can be simplified significantly.23 Unscented particle filter (UPF)The advantage of particle filter compared to Kalman-based filter is that there is no Gaussian
33、approximation assumption. However, there are still some other problems. Too many particles will increase the time cost greatly, while fewer particles will result in a bad estimation. Moreover, the particles generated on each step dont take into account the measurements. To solve these problems, a ne
34、w particle filter with UKF proposal has been developed recently nl. This filter uses UKF to generate the important proposal distribution. In comparison with general particle filter, UPF tends to generate more accurate estimation with fewer particles. It is more suitable for online estimation. The al
35、gorithm is described as follows.(1) Initialization2) Select step: Multiply/suppress panicles with high/low importance weights to obtain random psrticlos3) Take some optional steps to improve the performance, such as MCMC sveb.(3) Output ths founc estimationsFor the degeneration problem, re-sampling
36、or other optional step- s.iould be required in the above algorithm.2.4 Extended set-membership filter (ESMF)All above stochastic nonlinear filters are based on Bayesian theory, and some prior information on the distribution of the noises must be known ahead. Unfortunately, nothing but the hard bound
37、s of the noises often can be assumed. This kind of noise is called unknown-but-bounded noise. In addition, current robust control and coordination approaches, including on-line optimizing algorithms, require bounded uncertainty information in their formulations in order to guarantee closed loop stab
38、ility and performances. This leads to the use of a kind of method known asset-membership or set-valued estimation. For a general class of nonlinear systems, an extended version of the linear set-membership filter has been developed li4 This approach is to cast the nonlinear dynamics in a way that al
39、lows the advantage of the linear SMF framework can be taken. Specially, the nonlinear dynamics is linearized about the current estimation; The remaining terms are then bounded using interval analysis and incorporated into the process noise or measurement noises. Then a linear SMF algorithm is used a
40、nd an ellipsoid estimation set is made. The true value is guaranteed to lie in the ellipsoid.An ellipsoid set can be defined as and the two ellipsoid bounds of the two noisesthen at time k+, the ESMF algorithm can be summarized in the following steps.(1) Calculate the state interval based on the ell
41、ipsoid extrema where the superscript i,j stand for the (i,j) elements of a matrix.(2)Find the maximum interval for the Lagrange remainderestimate both states and slip parameters simultaneously. The using interval analysispurpose here is to compare filter performances and find out weakness of the dif
42、ferent approaches. The platform used in studys a tracked vehicle with two arms (Fig. 2).(3)Calculate the ellipsoid bounding the linearization errorThe observation model should be dealt in the same way as attaining the incorporated measurement noise(5) Calculate the predicted situ; ellipsoid using th
43、e linear SMF filter step(6) Calculate the updated state ellipsoid using the linear SMF filter stepSpecial attention should be given to the two following problems: The values of three parameters pQ, p pk can be optimized at each time step in an effort to find the smallest bounding ellipsoid. Some cri
44、terions such as maximizing the volume of ellipsoid or minimizing the trace of the bounding matrix can be used. When (5*0, the state uncertainty ellipsoid is not defined. This will occur when the basic algorithm assumption is not satisfied. Thus, it is a good check for the health of the algorithm.3 S
45、IMULATIONIn this section, the four filters mentioned above will be applied to the kinematic model of the tracked vehicle with slip to The sensors equipped on the vehicle include 2 accelerometeis. 2-DOF electrolytic tilt sensor, a pair of cameras, a gyroscope and a GPS. The vehicle has the ability of
46、 striding over some obstacles, and can localize itself in both 2D and 3D environments, although only planar motion is considered in the study.In the model, the angular velocities of the two wheels that drive the two main tracks are considered as input variables. The constants in the model are chosen
47、 as follows: The width of the two main tracks is 6=0.65 m; And the radius of two wheels is r=0.35 m. The three states about the vehicles pose are measurements. The total time of the simulation is chosen as f = 20 s, and the time-step is 0.1 s. The noises used in the model, including the process nois
48、e and the measurement noise, are assumed to have two forms: One is the Gaussian noise with zero mean value and 2% variance of the nominal values; The other is the ellipsoid bounded noise whose bound is the same as the variance of above Gaussian noise. In fact, the points selected in the ellipsoid of
49、 the latter one are uniformly distributed. In order to verify the efficiency of filters, some sudden changes are assumed to occur in three slip parameters in the course of moverment.In order to achieve satisfactory result, proper tuning of the filters is essential. The values in Q, and /fj+i for Kalman based filters should be determined carefully. For UKF, the three constant filter parameters are cho