2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

Simulation of Mobile Robot Navigation with Sensor

Fusion on an Uneven Path
D.M.G.A.I.N. Sumanarathna

isurasumanarathna@gmail.com

H.G.N. Sandamali

hgnsandamali@gmail.com

I.A.S.R. Senevirathna

srs.ruwantha@gmail.com

M. Branesh Pillai

pillaibranesh@elect.mrt.ac.lk

K.L.U. Sirisena

fwmlaksith@yahoo.com

A.M. Harsha S. Abeykoon

harsha@elect.mrt.ac.lk

Department of Electrical Engineering, University of Moratuwa, Katubedda, Moratuwa, Sri Lanka.

Abstract- This paper describes the navigation of a two-wheel
drive mobile robot along a predefined path under uneven road
conditions where it cannot solely rely on encoders, GPS or an
accelerometer individually. There are conditions when low
friction or slippery ground surfaces such as sandy paths and
pits cause one or both encoders to halt or rotate less as the robot
moving forward. Areas covered with clouds, trees or structures
can block GPS signals. Sudden pickups and halts give false
information from accelerometers. Therefore Kalman filter
based sensor fusion algorithm is implemented in order to get
the best position estimation for the mobile robot using above
sensor outputs. The Special feature of this algorithm is that it
includes a simple method to overcome the effects of encoder
errors due to the slipping of wheels of the mobile robot, which
does not require complex computations to additional
measurement units to directly measure the slipping of the
wheels of the robot. Finally the validity of the proposed
algorithm is demonstrated via simulation.
Keywords: robot navigation; sensor fusion; encoder slipping;
relative
localization
information; absolute
localization
information; uneven path

I. INTRODUCTION
The task of the mobile robot is to move from a given point to
a destination along a given course. The localization and
positioning in a reference frame is necessary to navigate the
mobile robot in the given environment. Three sensors,
Encoder, GPS and Accelerometer are used to localize and
decide the next move to reach the waypoints.
But each sensor mentioned above has unique weaknesses of
its own. They may be inherent errors of the sensors or errors
caused by external factors.
Encoders are the most accurate sensor used in the
research. Using Encoder outputs, kinematics model and dead
reckoning current position and attitude angle of the mobile
robot can be estimated. (For dead reckoning, successive
velocity, angular displacement feedback and the previously
calculated position and attitude angle are also needed).This
method is popular because of its simplicity. But problems
arise when the encoder outputs are erroneous due to some
environmental factors such as slippery ground surfaces and
uneven road conditions. When the robot is moving long
distances errors are also accumulated and creates a
considerable error. These reasons make encoder outputs
faulty. So to get precise estimation errors of the encoders
should be re-corrected.

978-1-4799-2397-7/14/$31.00 2014 IEEE

GPS and accelerometer also have higher errors. Due to

cloudy skies, large trees and structures GPS signal can be
blocked and geometry of satellites also causes its error vary
over time. But it is useful in navigating the robot for long
distances. Accelerometer measures the acceleration of the
robot and by double integration position of the robot can be
estimated. But then error is also integrated twice.
Due to this reasons each sensor cannot be used individually.
Therefore the sensory information are fused using a Kalman
filter based sensor fusion algorithm to locate the mobile
robot.
Slipping of the wheels of the mobile robot and uneven road
conditions has great impact to the precision of position
estimation. Therefore Many Research have been done on
mobile robot navigation with slip estimation, mobile robot
navigation using sensor fusion and even on mobile robot
navigation and slip estimation using sensor fusion [1]. This
involves complicated calculations related to slip rates. Wheel
soil interaction models and Terramechanics models are more
relevant in rover navigation under special soil conditions
such as extra-planetary explorations. But For small scale
mobile robot navigation purposes (like this study), those
calculations and soil condition investigations may not be
useful. Another research proposes estimating the wheel slip
to provide the accurate information to the controller [3].
Here, wheel slip and tire radius errors are lumped together as
an effective radius and estimated. However due to the radius
estimation changes with the speed; it causes some
degradation of the solution especially when the speed is low
as in our case. There are different methods proposed to
estimate the slip for controlling its effect on navigation of the
mobile robot [2], which does involve different models, tests
and bothersome calculations [4] which is not beneficial for
systems like ours.
In this paper, it is not the objective to calculate the slip or
controlling the slip. Instead multiple Kalman filter [7]
algorithm is used such that weight on Encoder measurements
is switched from higher to lower level automatically when
slipping is detected. This method avoids unnecessary
calculations and models while providing a precise
positioning of the mobile robot.

388

2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

II.

KINEMATICS OF WHEELED MOBILE ROBOT (WMR)

Y(m)

= Jaco+ .

W
P0 (x,y)

= Jaco+ .

As shown in Figure 1 the position of the robot (P0)

which is the middle point of the line connecting the axis of
two wheels is defined using the world coordinate system. It
consists of, X coordinate (x) which is the distance the robot
platform has travelled in X direction, the Y coordinate (y)
which is the distance the robot platform has travelled in Y
direction and the attitude angle () which is the angle formed
between the perpendicular to the mobile robots axis and the
x axis. Therefore, if the angle of rotation of left and right
wheels (l,r respectively) and the pose vector of robot
platform in the surface is described in equations (1) and (2).

(1)
(2)

Then the direct kinematic equation can be represented in

equation (3),
.

Where Jaco represent the Jacobean matrix calculated as in

equation (4),

(4)

A. Dead Reckoning

The current position and attitude angle of the WMR can be

estimated using dead reckoning. For calculation It requires
the velocity, angular displacement feedbacks and the
previously calculated position and attitude angle data.
The x and y positions are estimated as follows [5].
,

(9)
(10)

Where, Vp is the velocity of the WMR in world coordinate

system and
is the sampling time. The subscript k indicates
the respective values at kth sampling moment and the
subscript k-1 denotes the values at the pervious occasion of
kth sampling moment.
The attitude angle is calculated using the encoder outputs
from the equation (11),
(11)
B. GPS Data Conversion

While encoders and the three axis accelerometer provide the

relative localization information, GPS module provides
absolute localization information. To convert GPS latitude
and longitude data to useful world coordinate system,
following equations are used in the research.
.

However, the inverse kinematic equation is also required to

convert the world co-ordinate system data to joint coordinate data in order to plan the robot motion and the robot
control. Since Jacobean matrix is not a quadratic matrix, the
is used. The kinematic
pseudo inverse matrix Jaco+
equation has to be differentiated to calculate the inverse
kinematic equations.

(8)

= Jaco+ .

(3)

(7)

By neglecting the inverse Jacobean derivative,

The velocity and displacement instructions are given to

the WMR with respect to a world coordinate system. But the
angular velocities and displacements of the driven wheels of
the WMR, measured by the encoders are in joint co-ordinate
system. Therefore encoder outputs are converted to the world
coordinate system data using the following kinematic model.

(6)

X(m)

= [l , r ] T

Where Jaco+ represents the inverse Jacobean matrix,

Figure 1. The kinematics model in world coordinate system

X = [x, y, ]

(5)

(12)

(13)

Here, (b2-b1), (a2-a1), R and b1 represent latitude difference

between two positions, longitude difference between the
same two points, earth radius and latitude of initial position
respectively.

389

2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

Figure 2. Control block diagram of the mobile robot

(16)

C. Accelerometer Data Conversion

The raw output of the accelerometer is the acceleration in X,
Y and Z directions. But in this case only the acceleration in
X and Y directions are required ( , ). These values are
subject to noise and therefore an Exponential Moving
Average Low Pass Filter is included for noise reduction. (As
in equation (14) and (15) a smoothing factor is introduced
where 0<<1). raw, raw are the raw readings of the
accelerometer and filtered_old and filtered_old are the previously
filtered values of , .
filtered_new

= (1-)*

filtered_old

+ *

raw

filtered_new = (1-)* filtered_old + * raw

(14)
(15)

The filtered values are then processed using quaternion

theorem and still they are a combination of both static and
dynamic acceleration [6]. Dynamic components are
calculated from respective quaternion components and
integrated twice to get position estimation. is the Yaw of
Euler angles.
III.

(17)
Equation (16) is the process model and equation (17) is the
measurement model.

Where Xk, Xk-1are current and previous values of x,y and ,

Uk is the control input and Wk-1, Vk represent the process and
measurement noise respectively. Zk is the actual output by
the measurement sensor. A, B and H are transition, control
and observation matrices.
In the research therefore encoder is chosen as the process
sensor while the GPS and three axis accelerometer are the
measurement sensors.
Kalman filter is recursive process of two steps.
Time update (Prediction or priori step)

SENSOR FUSION ALGORITHM

The sensor fusion algorithm used in the research is derived

from the Kalman filter. Mobile robot is a dynamic system
hence it can be represented in a state-space model. The
kalman filter addresses the problem of estimating the state of
a discrete time controlled process that is governed by the
models below.

X = [x, y, ]T

Here

(18)
(19)

Measurement update (Correction or posterior step)

|
|
|

390

(20)

|
|

|
|

(21)
(22)

2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

Here,
is the prior estimate, | is the prior error
|
covariance, | , | are posterior state and covariance and
is the Kalman gain.
and
are the system noise
covariance and the measurement noise covariance which
were found in empirically.
.
In the research, the authors have used multiple Kalman
filtering. There Kalman filter based two algorithms are
applied, switching from one algorithm to other occurs
according to the behavior of the encoder outputs.
A. Algorithm I
When the robot is moving on the predefined path it uses the
information derived from the three sensors Encoder, GPS
and 3-Axis Accelerometer. Encoder measurements are highly
affected by the uneven road conditions and slipping of the
WMR. GPS has a higher noise and high probability for being
unavailable. (GPS signal lost) 3-Axis accelerometer gives the
measurement with the highest noise.

C. Switching Algorithm
Algorithm I and II are the inputs to the switching algorithm.
The intention here is to select the best position estimation
from either Algorithm I or II which is very close to the actual
position.
Generally, for a flat surface where slipping is negligible, the
commanded position of the mobile robot and the encoder
measurement outputs are approximately the same. A range
for difference between commanded position and the encoder
output can be defined for such a surface using practical data
gained from multiple tests.
As long as the difference between commanded position and
the encoder measurement lies in that practically obtained
range, it can be assumed that encoder is accurate and
Algorithm I can be utilized to get the position data to
navigate the robot. When the encoder measurement exceeds
the above range, Algorithm II is chosen by the Switching
Algorithm.

When the WMR is moving on a flat surface, Encoder

measurement is very accurate. For such conditions
Algorithm 1 can be used to estimate the robot positioning on
the path. Here, encoder measurement is fused with GPS
measurement using Kalman filter and Kalman output is
directed to the switching algorithm. However this method
has been used by some other authors also.

Figure 5. Sensor Fusion Algorithm

IV.

Figure 3. Algorithm I

B. Algorithm II

SIMULATION AND RESULTS

Matlab is used for the simulation. [9]

This algorithm is a new concept to find the robot positioning

over time along the path when the encoder measurement is
not available or reliable. Here, first Encoder and GPS data is
fused and that output is fused again with the accelerometer
data using Kalman Filter to get a reliable positioning
estimation especially in slipping of WMR or uneven road
conditions

As Kalman filter is used for this simulation several

assumptions are made. They are; this is a linear system and
all the noises (process and measurement noise) are Gaussian.
Encoder is a comparably a very low noise sensor. Its
resolution is 1024 PPR. It is used in quadrature
multiplication mode such that we get 4096 PPR. But in
slipping situations and other uneven road conditions this
noise is increased.
The typical error of the GPS is taken as 3 to 4 meters.
GPS has a considerable noise but not as large as the
accelerometer.

Figure 4. Algorithm II

391

2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

A. When sensory data is available

B. When sensory data is not available

1.

1.

Raw measurement from GPS and Encoder

Figure 6. Raw measurements from GPS and Encoder

Figure 6 is plotted using raw GPS and Encoder data. As

Encoders noise is low it is very close to the actual path.
GPS output with high noise is randomly deviated from the
actual path as shown in the figure.
2.

Figure 8. Raw measurements from GPS and Encoder

Figure 8 is plotted considering scenarios like when one of the

sensors or both sensors would not be available or reliable. i.e.
when the wheels of the mobile robot is slipping (Encoder is
no more reliable) and GPS signal is lost.
2.

Kalman output of the GPS and Encoder Data

Figure 7. Kalman output of the GPS and Encoder Data

Figure 7 shows the output of the Algorithm I (shown in

figure 3). Therefore it can be concluded that Algorithm I
output in this case is a sufficiently accurate estimation of the
actual path.

When Sensory data is not accurate (i.e. Encoder is

slipping and GPS signal is lost)

Kalman output when Sensory data is not accurate

(i.e. Encoder is slipping and GPS signal is lost)

Figure 9. Kalman output of the GPS and Encoder Data

Figure 9 shows that when Encoder is accurate, in that case

Encoder and GPS Kalman output is sufficient for the
requirement. But when encoder is not precise (i.e.: slipping
conditions) that Kalman output is no longer reliable.

392

2014 International Conference on Circuit, Power and Computing Technologies [ICCPCT]

V.

Using Algorithm II

CONCLUSION

This paper presents a new method to obtain a more precise

position estimation to navigate the mobile robot precisely
even when slipping conditions or uneven path conditions
affect the accuracy of the encoder data where encoder data
cannot be directly used. Some parameters of soil and lots of
complex calculations are required to correct encoders in
slipping to correct encoders in slipping. But this paper
suggests a rather simple method to avoid the effect of
encoder errors to obtain accurate mobile robot navigation.
For theoretical emphasis a simulation is done using Matlab
software and presented here. In future, practical
implementation of WMR would prove the accuracy of this
sensor fusion algorithm
.
VI.

REFERENCES

Figure 10. Output of the Algorithm II

Algorithm II shown in the figure 4 has been used to plot

figure 10 considering same faulty conditions as in the
previous case. This graph shows that the fault due to the
slipping of the Encoder is successfully overcome. But most
of the time, encoders would be accurate. But at that time
accuracy of Algorithm II is less than the Algorithm I. it is a
disadvantage of this method.
Therefore to avoid this drawback, the switching algorithm is
used to get the most reliable output from both algorithm
outputs.
Final output (after using the switching Algorithm)
Here the weaknesses of both algorithms have been overcome
by automatically switching above two algorithm outputs as
required using the sensor fusion algorithm shown in figure 5.

[1] Damien Lhomme-Desages, Christophe Grand, Faz Ben Amar and

Jean-Claude Guinot, Member, IEEE, Doppler-based Ground Speed
Sensor Fusion and Slip Control for a Wheeled Rover, IEEE/ASME
Transactions On Mechatronics, vol. 14, no. 4, August 2009
[2] Puneet Goel, Stergios I. Roumeliotis and Gaurav S. Sukhatme, Robot
Localization Using Relative and Absolute Position Estimates
[3] Benjamin J. Clark, David M. Bevly, and Shane Farritor, Analysis of
Improvement to Two-Wheel Robot Navigation Using Low-Cost
GPS/INS Aids
[4] Seyr M. and Jakubek S., "Proprioceptive Navigation, Slip Estimation
and Slip Control for Autonomous Wheeled Mobile Robots", Robotics,
Automation and Mechatronics, 2006 IEEE Conference , vol., no., pp.1,6,
Dec. 2006
[5] H.A.N.D. Dayarathna, L.L.G. Prabuddha, K.L.D.N.J. Ariyawansha,
M.K.C.D. Chinthaka, A.M. Harsha S. Abeykoon and M. Branesh Pillai,
Sensorless Contact Position Estimation of a Mobile Robot in Pushing
Motion
[6] Emily L. C. Shepard1,*, Rory P. Wilson1, Lewis G. Halsey2, Flavio
Quintana3, Agustina Gmez Laich3, Adrian C. Gleiss1, Nikolai
Liebsch1, Andrew E. Myers4 and Brad Norman5, Derivation of body
motion via appropriate smoothing of acceleration data, vol. 4: 235241,
December 2008
[7] Louis Drolet1,2, Franois Michaud2 and Jean Ct1, Adaptable Sensor
Fusion Using Multiple Kalman Filters, In proceeding of: Intelligent
Robots and Systems, 2000. (IROS 2000). Proceedings. 2000 IEEE/RSJ
International Conference on, Volume: 2
[8] Greg Welch, Gary Bishop, "An Introduction to the Kalman Filter",
University of North Carolina at Chapel Hill Department of Computer
Science, 2001
[9] M.S.Grewal, A.P. Andrews, "Kalman Filtering - Theory and Practice
Using MATLAB", Wiley, 2001

Figure 11. Final output

393