Subscribe Now Subscribe Today
Research Article
 

Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots



Iraj Hassanzadeh and Mehdi Abedinpour Fallah
 
Facebook Twitter Digg Reddit Linkedin StumbleUpon E-mail
ABSTRACT

In this study, we present the local reconstruction of differential-drive mobile robots position and orientation with an accurate odometry calibration. Starting from the encoders readings and assuming an absolute measurement available, Augmented Extended and Unscented Kalman Filters (AEUKF) are proposed to localize the vehicle while estimating a proper set of odometric parameters. In order to compare their estimation performances explicitly, both observers are designed for the same mobile robot model and run with the equal covariance matrices under the identical initial conditions. In the simulation results, it is shown that Augmented Unscented Kalman Filter (AUKF) outperforms the Augmented Extended Kalman Filter (AEKF).

Services
Related Articles in ASCI
Similar Articles in this Journal
Search in Google Scholar
View Citation
Report Citation

 
  How to cite this article:

Iraj Hassanzadeh and Mehdi Abedinpour Fallah, 2008. Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots. Journal of Applied Sciences, 8: 2901-2906.

DOI: 10.3923/jas.2008.2901.2906

URL: https://scialert.net/abstract/?doi=jas.2008.2901.2906
 

INTRODUCTION

The problem of finding and tracking the mobile robot`s pose is called localization and can be global or local. During the past few years many suggestions have been made to address this problem. One of the first methods introduced and still used in many projects, is odometry. It is the reconstruction of the mobile robot configuration, i.e., position and orientation, resorting to the encoders` measurements at the wheels. Starting from a known configuration, the current position and orientation of the robot is obtained by time integration of the vehicle`s velocity corresponding to the commanded wheels` velocity (Antonelli and Chiaverini, 2007). Its advantages are the low cost of the sensors, good accuracy for short distances and compatibility with other positioning methods. The estimation of the odometric parameters is known in the literature as odometry calibration.

The differential-drive mobile robots` pose estimation is affected mainly by odometry errors which can be separated into systematic and nonsystematic. Regarding the systematic errors, there are two dominant error sources, unequal wheel diameters and uncertainty about the effective wheel base. In the study by Borenstein and Freg (1996), a calibration technique called, UMBmark has been developed to calibrate for systematic errors of a differential drive mobile robot. Martinelli et al. (2007) and Larsen et al. (1999) introduced an Augmented Kalman Filter (AKF) which simultaneously estimates the robot configuration and the parameters characterizing the systematic odometry errors.

Within the significant toolbox of mathematical tools that can be used for stochastic estimation from noisy sensor measurements, one of the most well known and often used tools is the Kalman filter. As an extension to the same idea, the Extended Kalman Filter (EKF) is used if the dynamic of the system and/or the output dynamic is nonlinear. It is based on linearization about the current estimation error mean and covariance. Although it is straightforward and simple, it has drawbacks too (Julier and Uhlmann, 2002). The Unscented Kalman Filter (UKF) is the newest revision of the Kalman Filter, proposed to overcome these flaws. It does not need the linearization for a nonlinear function and is more accurate and simpler than the EKF applied to nonlinear systems (Julier and Uhlmann, 2004; Wan and van der Merwe, 2000).

In this study, we present the local reconstruction of differential-drive mobile robots position and orientation with an accurate odometry calibration. To fuse the absolute measurement`s data and the odometry from the wheels` encoders we design the augmented extended and unscented Kalman filters and compare their performances under the same covariance matrices and initial conditions.

MATERIALS AND METHODS

Kinematic model: Kinematics refers to the evolution of the position and velocity of a mechanical system, without reference to its mass and inertia. Let us consider a two-wheeled differential drive mobile robot operating on a planar surface as shown in Fig. 1, where x and y denote the position of the center of the axle of the robot with respect to the global coordinate frame, θ denotes the heading of the vehicle with respect to the x axis in the global coordinate frame. There are two main wheels, each of which is attached to its own motor. A third wheel-a caster, is placed in the front to passively roll along while preventing the robot from falling over. The differential equations that describe the kinematics of the robot are written as:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(1)

where, the absolute velocity of the robot body (v) and the angular speed (ω) are assumed to be inputs.

The motion of the left and right wheels is characterized by angular velocities ωL and ωR, respectively. The body-fixed components v and ω of the robot velocity are related to these angular velocities by:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(2)

In which d denotes the distance between the two wheels, while γR and γL are the radii of the right and left wheels, respectively (Antonelli and Chiaverini, 2006).

Augmented Kalman filter: Let us define Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots as the robot configuration;

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(3)

The distance between the wheels or the wheel radiuses may not be precisely known, which lead to systematic errors and worsen the performance of the Kalman filter. Making a complex model of the two-wheeled differential drive mobile robots will not reduce this problem but in fact make it worse, as this model not only will contain the same errors, but most likely also introduce further inaccuracies (Larsen et al., 1999).

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 1: Schematic diagram of a two-wheeled differential drive mobile robot

To remedy this problem, we adopt the same Augmented Kalman Filter (AKF) introduced by Martinelli et al. (2007) and Larsen et al. (1999). This filter estimates the augmented state containing the robot configuration and the systematic parameters as:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(4)

Extended Kalman filter: The Kalman filter addresses the general problem of trying to estimate the state x ε Rn of a discrete-time controlled process that is governed by a linear stochastic difference equation. As an extension to the same idea, the EKF is used if the dynamic of the system and/or the output dynamic is nonlinear. EKF is based on linearization about the current estimation error mean and covariance.

Definitions: Let us assume that the process has a state vector x ε Rn and a control vector u and is governed by the non-linear stochastic difference equation:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(5)

with a measurement z ε Rm that is

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(6)

the random variables wk and vk represent the process and measurement noise, respectively. They are assumed to be independent of each other, white and with normal probability distributions with covariance matrices Q and R. It can be shown that the time update equations of EKF is:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(7)

where, Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots is the a priori state estimate. These time update equations project the state and covariance estimate (Pk) from the previous time step k–1 to the current time step k. And the measurement update equations of the EKF are:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(8)

where, A, W, H and V are Jacobian matrices and K is the correction gain vector. These measurement update equations correct the state and covariance estimate with the measurement zk. The design process of this filter is explained next.

EKF implementation: The discrete model of the system can be derived by converting the differential Eq. 1 to their corresponding backward difference equations. So, the dynamical equation for the augmented state Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots is given by:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(9)

where, the subscript k denotes the k-th time sample and T is the sampling period.

The Jacobian matrix at each iteration can be derived as:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(10)

With

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots

Suppose that the position and orientation of the robot are measured directly. Hence,

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(11)

where, vk represents the measurement noise and the output matrix H is constant and defined as:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(12)

where, I3 is the 3x3 identity matrix and O3 is a null 3x3 matrix.

Due to the recursive nature of the EKF algorithm, the state vector needs to be initialized in startup. The initial robot configuration is taken as the first measured value. Also, the nominal values of d, γR and γL are chosen as their initial estimations. Here, the following initial conditions are selected randomly for the state vector:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(13)

We add uncertainty to the initial conditions by selecting

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(14)

and it choose the process noise and measurement noise as:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(15)

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(16)

Thus, it is developed all the necessary elements of the augmented EKF for the two-wheeled differential drive mobile robot.

Unscented Kalman filter: The basic premise behind the UKF is employing a Gaussian distribution approximation of a nonlinear function rather than the approximation. Instead of linearizing using Jacobian matrices, the UKF uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points and it has 3rd order (Taylor series expansion) accuracy for Gaussian error distribution for any non-linear system (Wan and Van der Merwe, 2000), while EKF uses linearizing Jacobian matrix, which is a first order approximation. The UKF is claimed to have obvious advantages over EKF (Julier and Uhlmann, 2004). A brief overview of the UKF algorithm here.

Definitions: The Unscented Transformation (UT) is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation. The L dimensional random variable x with mean Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots and covariance Px is approximated by 2L+1 weighted points given by:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(17)

These sigma points are propagated through the nonlinear function,

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(18)

from which the mean and covariance of the transformed probability can be approximated

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(19)

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(20)

with weights Wi given by:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(21)

where, λ = α2(L+κ)–L is a scaling parameter. The constant α determines the spread of the sigma points around Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots and is usually set to small positive values less than one (typically in the range 0.001 to 1) whereas, κ is the secondary scaling parameter usually set to zero or 3–L and the constant Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots is used to incorporate prior knowledge of the distribution of x (for Gaussian distributions, β = 2 is optimal). The scale parameters may be tuned to match the specific problem; some guidelines to choose them are provided in (Julier and Uhlmann, 2004).

The UKF can be implemented using UT by expanding the state space to include the noise component: Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots. The UKF algorithm can be summarized as follows:

Initialization

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(22)

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(23)

Iteration for each time step

Calculate sigma-points

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(24)

Time update

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(25)

Measurement update

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(26)

Where:

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
(27)

UKF implementation: We initialize it in the same way as the EKF, using the same randomly selected values for the augmented state vector and covariance matrices. It is also need to set the tuning parameters α, β and κ. The optimum values for coefficients α and β are chosen as 1 and 1.8, respectively. Also, κ is set to zero. These optimum values are chosen such that they provide the best estimates for robot position and orientation for all experiments (Wan and Van der Merwe, 2000).

RESULTS AND DISCUSSION

Here, presents simulation results using our developed package by MATLAB®. Figure 2 shows the trajectory estimations of augmented EKF and UKF for the two-wheeled differential drive mobile robot.

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 2: Trajectory estimations of augmented EKF and UKF

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 3: Illustration of using AEUKF to estimate the position x

To evaluate the performances of AEUKF, we plotted the estimated and actual states for the available measurements in Fig. 3-5.

Figure 3-5 reveal that UKF outperforms the EKF. So, the claim in (Julier and Uhlmann, 2004) and (Wan and Van der Merwe, 2000) that the UKF is better than the EKF is verified in this specific application. Figure 6 shows the error comparison for these filters.

Also, Fig. 7 shows the values of estimated parameters via the augmented filters. It can be concluded that the AUKF has less variations around the nominal values than the AEKF and can be trusted more.

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 4: Illustration of using AEUKF to estimate the position y

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 5: Illustration of using AEUKF to estimate the orientation

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 6: Error comparison for the augmented EKF and UKF

Image for - Design of Augmented Extended and Unscented Kalman Filters for Differential-Drive Mobile Robots
Fig. 7: The estimated and by the augmented filters

CONCLUSIONS

In this study, AEKF and AUKF are proposed to localize a differential-drive mobile robot while estimating a proper set of odometric parameters. Comparing the simulations results in different scenarios show the performance of the AUKF is consistently better than the AEKF. Moreover, both filters provide estimations for the systematic parameters around their nominal values. In the future we will conduct some experiments using the Khepera II mobile robot, manufactured by K-Team, to verify the simulations of this study. Furthermore, utilizing the AEUKF in the simultaneous localization and mapping (SLAM) problem would be our future challenging task, where the odometry calibration could play a vital role especially in solving the data association problem.

ACKNOWLEDGMENT

This research is part of a research supported by a research grant of the University of Tabriz.

REFERENCES
1:  Antonelli, G. and S. Chiaverini, 2006. Linear estimation of the odometric parameters for differential-drive mobile robots. Proceedings of the RSJ International Conference on Intelligent Robots and Systems, October 9-15, 2006, Beijing, China, pp: 3287-3292.

2:  Antonelli, G. and S. Chiaverini, 2007. Linear estimation of the physical odometric parameters for differential-drive mobile robots. Springer Netherlands. Auton. Robots, 23: 59-68.
CrossRef  |  

3:  Borensein, J. and L. Freg, 1996. Measurement and correction of systematic odometry errors in mobile robots. IEEE Trans. Robot. Automat., 12: 869-880.
CrossRef  |  INSPEC

4:  Julier, S.J. and J.K. Uhlmann, 2002. Reduced sigma point filters for the propagation of means and covariances through nonlinear transformations. Proceedings of the American Control Conference, May 8-10, 2002, Anchorage AK, USA., pp: 887-892.

5:  Julier, S.J. and J.K. Uhlmann, 2004. Unscented filtering and nonlinear estimation. Proc. IEEE, 92: 401-422.
CrossRef  |  

6:  Larsen, T.D., K.L. Hansen, N.A. Andersen and O. Ravn, 1999. Design of Kalman filters for mobile robots; evaluation of the kinematic and odometric approach. IEEE. Int. Conf. Control Applications, 2: 1021-1026.
CrossRef  |  INSPEC

7:  Martinelli, A., N. Tomatis and R. Siegwart, 2007. Simultaneous localization and odometry self calibration for mobile robot. Springer Netherlands. Auton. Robots, 22: 75-85.
CrossRef  |  

8:  Wan, E.A. and R. Van Der Merwe, 2000. The unscented kalman filter for nonlinear estimation. Proceedings of the Symposium 2000 on Adaptive Systems for Signal Processing, Communication and Control, Octiber 1-4, 2000, Lake Louise, Alberta, Canada, pp: 153-158.

©  2021 Science Alert. All Rights Reserved