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
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:
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
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
as the robot configuration;
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).
||Schematic diagram of a two-wheeled differential drive
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:
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
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:
with a measurement z ε Rm that is
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:
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:
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
is given by:
where, the subscript k denotes the k-th time sample and T is the sampling
The Jacobian matrix at each iteration can be derived as:
Suppose that the position and orientation of the robot are measured directly.
where, vk represents the measurement noise and the output
matrix H is constant and defined as:
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
We add uncertainty to the initial conditions by selecting
and it choose the process noise and measurement noise as:
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
and covariance Px is approximated by 2L+1 weighted points given
These sigma points are propagated through the nonlinear function,
from which the mean and covariance of the transformed probability can
with weights Wi given by:
where, λ = α2(L+κ)–L is a scaling parameter.
The constant α determines the spread of the sigma points around
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
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: .
The UKF algorithm can be summarized as follows:
Iteration for each time step
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.
||Trajectory estimations of augmented EKF and UKF
||Illustration of using AEUKF to estimate the position
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
||Illustration of using AEUKF to estimate the position
||Illustration of using AEUKF to estimate the orientation
||Error comparison for the augmented EKF and UKF
||The estimated and by the augmented filters
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.
This research is part of a research supported by a research grant of
the University of Tabriz.