Content uploaded by Rodrigo A. Carrasco
Author content
All content in this area was uploaded by Rodrigo A. Carrasco on Mar 13, 2018
Content may be subject to copyright.
NONLINEAR STATE ESTIMATION IN MOBILE ROBOTS USING A FUZZY OBSERVER
†Rodrigo Carrasco, †Aldo Cipriano, ‡Ricardo Carelli
†Departamento de Ingeniería Eléctrica
Pontificia Universidad Católica de Chile, Chile
{rax; aciprian}@ing.puc.cl
‡ Instituto de Automática
Universidad Nacional de San Juan, Argentina
rcarelli@inaut.unsj.edu.ar
Abstract: The performance of model based fault detection and isolation systems can be
improved by designing more accurate estimation methods. This work presents a novel
implementation of a nonlinear Kalman filter based on the Takagi–Sugeno (TS) fuzzy
structure, for a mobile robot. First, a TS model is derived from the robot kinematic
equations, which is optimized through genetic algorithms to obtain an accurate model.
Based on this model, several linear Kalman filters are combined using fuzzy logic,
designing a nonlinear state estimator. Finally, the resulting fuzzy nonlinear observer is
compared with the conventional Extended Kalman Filter, showing an improvement in
performance and robustness. Copyright © 2005 IFAC
Keywords: Fuzzy modelling, Kalman filters, Mobile robots, State estimation, Robotics.
1. INTRODUCTION
Over the last decade the use of mobile robots has
increased, especially in dangerous tasks such as
space exploration, land mines extraction, and rescue
operations, keeping human operators away from
harm. Mainly due to the dynamic and complex
environments in which mobile robots work, a large
number of different faults may appear, reducing the
capabilities or even disabling the robot. Recent
studies show that the mean time between failures is
less than 20 hours for field robots, after which the
robots must be repaired, consuming time and
resources (Carlson, 2004). This means that when
faults are taken into account, the advantages of using
mobile robots are cut back, as repairs are not always
possible, like in space exploration, or is very
dangerous to retrieve the robot to do them. These
facts imply that accurate fault detection and isolation
(FDI) systems are required, increasing the reliability
of the robots and reducing the costs associated to
fault appearance.
The use of system models and system observers is
one of the most effective methods for detecting and
isolating faults (Basseville and Nikiforov, 1998). As
state estimation becomes more accurate, the
performance of the associated FDI system increases,
detecting faults sooner and reducing confusions
during the isolation process.
The Kalman filter is an optimal state estimator for
linear systems which uses the model of the system
and the measurements to minimize the error in the
estimation (Kalman, 1960), making it an excellent
tool for implementing a model based FDI system.
This filter can be extended to nonlinear systems, such
as mobile robots as shown in (Larsen, et al., 1999),
by using a linearization of the process on each
operating point. Although the Extended Kalman
Filter (EKF) gives a good estimation of the state
vector of the system, it is very difficult to tune it
accurately, as it is very sensitive to modelling errors
and noise estimation. The main problem is that the
noise covariance matrices needed must be precisely
determined to get good results, which is seldom
achieved in nonlinear systems.
To solve the problems related with nonlinearities,
several authors have used fuzzy logic to tune the
EKF dynamically, determining the covariance
matrices needed as the operating point changes
(Sasiadek and Hartana, 2002; Wang and Goh, 1999).
In this work, a novel implementation of a nonlinear
Kalman filter based on the Takagi-Sugeno fuzzy
structure is described. First, a kinematic and dynamic
model of a mobile robot is presented. Next, a TS
model of the robot kinematics is constructed,
followed by the design of the fuzzy Kalman filter.
Finally, the fuzzy filter is compared with the EKF
through simulations, showing improvements in the
accuracy and robustness of the estimation.
2. ROBOT MODEL
A simplified model of a mobile robot is presented on
figure 1. The body is considered a circular disc of
radius b, with two independent wheels of radius r
each. The angular velocity of the right wheel is
defined as
ω
1, whereas the left one is
ω
2. The posture
of the robot is defined as a three element vector,
containing the coordinates of the position of the
robot with respect to some reference point and the
heading angle: X=[x y
ϕ
]T. The equations that
describe the behaviour of the robot can be divided
into two sets, one for the kinematics and one for the
dynamics, which includes the model of the motors
used (Angeles, 1997).
2.1. Dynamic Equations
To simplify the simulation problem, very often only
the kinematic equations are used to model the robot,
disregarding the robot dynamics. In this work, the
dynamic equations of the robot are also modelled,
which helps to analyze the effect they have both on
the fuzzy observer and the EKF. These equations
relate the torque applied to the wheels by the motors,
τ
, with the acceleration the robot acquires,
ω
:
(
)
F+=M
ω
ωτ
(1)
On equation 1, M represents the inertia matrix and
F( ) is a function that depends on the speed of the
wheels, representing the effects of friction. The DC
motor equations are also included in this model to
determine the torque each motor applies with a given
input voltage, vi. As described on figure 2, a PID
controller is used to control the voltages on each
motor, achieving the reference angular velocity on
each wheel.
Fig. 2. PID control loop and robot kinematics.
Fig. 1. Mobile robot model and basic parameters.
2.2. Kinematic Equations
Using simple geometric relations, the linear speed of
the robot at time step k can be obtained as equation 2
shows:
(
)
1, 2,
2
r
kk
V
ωω
=+
k
(2)
The kinematic equations of the robot relate the
angular velocity of each wheel with the variation in
the posture vector, which is used as state vector
through this work:
()
()
()
1
1
11,
2
cos
sin
kkk k k
kkk k k
r
kkk k k
b
xxx VT
yyy VT
T
ϕ
ϕ
ϕϕϕ ω ω
−
−
−
∆= − =∆
∆= − =∆
∆=− =∆ −
2,
(3)
(4)
(5)
3. TAKAGI-SUGENO MODEL GENERATION
The objective of this work is to achieve a more
accurate model for using in FDI systems on mobile
robots. The first step to achieve this is to design an
ideal model that does not consider the effects of noise
on the process and the measurements. As the
nonlinearities of this system are only on the robot
kinematics, the Takagi–Sugeno fuzzy structure is
used to model these equations, without considering
the dynamic components.
3.1. Model Formulation
The fuzzy structure can be used to fuse together
multiple linear models of a system, obtaining a
nonlinear model of the process (Lin and Lee, 1996).
As there is no single operating point in mobile
robots, the whole spectrum of possibilities must be
covered. To achieve this, equations 3 and 4 are
linearized in five different heading angles: 0, -π/2,
π/2, π and, -π. Although π and -π are actually the
same angle, both linearizations are needed as the
linearization about π will only be valid for values
near to it. If it is evaluated near -π the approximation
given is very bad, compromising the accuracy of the
whole model. Each linear model will have the
following structure, where
ϕ
n is the linearization
angle:
(
)()( )
() ()( )
,
,
cos sin
sin cos
kn k n n k n
kn k n n k n
xVT
yVT
ϕϕϕϕ
ϕϕϕϕ
∆=∆ − −
⎡
⎤
⎣
⎦
∆=∆ + −
⎡
⎤
⎣
⎦ (6)
(7)
As these two equations are independent, two
different fuzzy structures are used to fuse together
the linear models, one for
∆
x and one for
∆
y, both
using
ϕ
as input for the fuzzyfication, as figure 3
shows. The output of each structure is given by the
lineal combination of all the linear models, Ln,
within:
Fig. 3. Takagi–Sugeno nonlinear model.
Fig. 4. X position membership functions.
Fig. 5. Y position membership functions.
5
,
1
5
,
1
kn
n
kn
n
kn
kn
x
x
yy
µ
λ
=
=
∆= ∆
∆= ∆
∑
∑ (8)
(9)
The values for the coefficients
µ
n and
λ
n are
determined by the membership function (MF)
associated to the linear model n. As there are five
linearization angles, there are five MF for each
variable: Centre (C) associated to
ϕ
1
=0, Left (L)
associated to
ϕ
2
=π/2, Right (R) for
ϕ
3
=-π/2, Back 1
(B1) associated to
ϕ
4
=-π, and Back 2 (B2) associated
to
ϕ
5
=π.
3.2. Model Optimization
The nonlinearities in the TS model are given by the
morphology of the MF associated to each linear
model, thus the accuracy of the model can be
optimized by modifying each MF. One method that
has proved to return good results is genetic
optimization, which can be used to modify the
morphology of each MF to improve the model.
Genetic optimization is based on the same principles
as evolution theories, where a group of possible
solutions, called parents, are combined together
resulting in new child solutions that tend to be better
in solving a certain problem (Lin and Lee, 1996).
One recent development in genetic programming
(Jung, 2003) uses the same method as bee colonies,
where only the best member of the parent population,
also called the queen, is used to create the new
population, reducing the computational needs and
complexity of the optimization.
Several assumptions can help in reducing of the
number of free variables in the optimization process.
First, as each linearization gives an exact
approximation of the nonlinear function at the
linearization point, the degree of membership of the
associated MF at that point must be 1, whereas the
other MF must be 0. Another consideration is that
due to the geometric characteristics of the problem,
the MF can be assumed to be symmetric about the
linearization point. Taking these considerations into
account, each MF is coded as a 7 element vector,
where each element contains the degree of
membership or height of the MF at constant intervals
in the angle variable. The values in between are
obtained through a cubic interpolation of the
neighbours.
Fig. 6. Comparison between the initial and optimized
fuzzy models.
The optimization is made by comparing the
performance of the TS model using each individual
in the population, with the result of the exact
nonlinear model of the robot, given a certain input
vector U. Then, the best individual is selected to
produce the next generation of solutions.
Using an initial population of 30 individuals with
triangular MF, the system is optimized for 100
generations resulting in the MF of figures 4 for the x
variable, and figure 5 for the y variable. Figure 6
shows a comparison between the estimation given by
the model using the initial triangular MF, and the
optimized model.
The simulation of the exact nonlinear model and the
TS model for several different trajectories shows that
the fuzzy model is extremely accurate.
4. FUZZY KALMAN FILTER DESIGN
When noise is taken into account, the difference
between the estimation of the previous model and the
real posture of the robot increases over time, so some
correction method is needed. Due to the fact that
process disturbances and measurement errors can be
characterized as Gaussian white noise, Kalman filters
can be used to reduce the deviation caused by these
disturbances.
4.1. Kalman Filter
As the TS nonlinear model is based on several linear
models, conventional linear tools can be used over
each of them. In this case, a Kalman filter can be
implemented for each model, obtaining an optimal
estimation of the state vector for each of them, and
thus reducing the effect of noise over them.
11
2,
kk k
kkk
−−
=++
=+
AB
C
XX U
YX
1,k
ε
ε
(10)
Equation 10 represents a linear system, where Xk is
the state vector, Uk the input, and Yk the
measurement vector, at time k. The process and
measurement Gaussian noise at time k are denoted
by
ε
1 and
ε
2 respectively.
The Kalman filter is an iterative algorithm that uses
both the system equations and actual measurements
to correct the prior estimation. First, the state and
measurement vectors, and the covariance matrix, P,
are estimated using the system equations, as shown
in (11), where Q represents the process noise
covariance matrix:
11
1
ˆ
kk
kk
T
kk
−
−−
−−
−
−
=+
=
=+
AB
C
AAQ
k
X
XU
YX
PP
(11)
Once this estimation is made, the Kalman gain is
calculated using R, the measurement covariance
matrix:
()
1
TT
kk k
−
−−
=+CCC RKP P (12)
Finally, using this gain and the measurement vector
Yk, the estimation is updated:
(
)
()
ˆkkkkk
kkk
−−
−
=+ −
=−
IC
X
XKYY
PKP (13)
4.2. Fuzzy Observer
As described on figure 7, for each of the linear
models Ln, a Kalman filter KFn is implemented,
which gives an optimal estimation for that model,
reducing the effect of the process and measurement
noise. Also, each linear model has its own process
noise covariance matrix Qn, which in the case of
linear systems can be easily determined to obtain the
best possible performance. Then, each KFn is
specifically tuned with these matrices. This is one of
the main advantages of this method, compared with
the EKF, where generally it is very difficult to
determine the covariance matrices needed to achieve
a good performance, resulting in a significant
reduction of the tuning process.
Fig. 7. Takagy-Sugeno based fuzzy observer.
In the same way in which the nonlinear TS fuzzy
model fuses together the different linear models, this
structure can be used to fuse the estimation given by
each Kalman filter, resulting in a fuzzy nonlinear
observer.
For each time step k, the previous state estimation
-1
ˆk
X
is used together with the input vector Uk-1 and
measurement vector Yk, to obtain an optimal
estimation from each linear Kalman filter KFn. The
different estimations for the heading angle
ϕ
k,n, which
are almost the same on each model, are averaged
together to determine the best estimation ˆk
ϕ
. This
value is used as an input to the fuzzy structure to fuse
together the estimations for x and y given by each
Kalman filter. The same membership functions
determined through the optimization of the nonlinear
TS model of the robot kinematics are used to fuse the
estimations and obtain ˆk
x
and . The covariance
matrix Pk is obtained as a linear combination between
the different covariance matrices. The relative weight
for each covariance matrix is given by the MF
associated to the corresponding model, using the
membership functions for the x variable for models 1,
4, and 5 as they are more related to movements on
the X axis, whereas for models 2 and 3 the weights
are obtained from the y related membership
functions.
ˆk
y
5. RESULTS
To analyze the robustness and performance of the
fuzzy observer, the estimation error is compared with
the one obtained from an EKF for several different
conditions. The EKF is tuned to obtain the best
possible estimation given a certain trajectory,
whereas the different linear Kalman filters in the
fuzzy observer are tuned using the linear equations,
without considering the trajectory.
Due to the fact that these are statistical tools that are
used to reduce the effect of noise, for each different
situation 1000 simulations are made choosing
random seeds for the noise generation. This allows a
more suitable comparison between the two
estimations. On each simulation it is assumed that
every 0.1 [s] the mobile robot can measure the
heading angle (with a magnetic compass for
example) and the position (with a GPS or radio
beacons).
5.1. Basic Comparison
The basic comparison considers that the value for the
standard deviation of both the angle measurement
Gaussian noise and process Gaussian noise is 10% of
the maximum possible value. The standard deviation
for the position measurement noise is considered to
be 1 [m]. One of the simulation results is shown on
figures 8 and 9. On both figures it can be observed
that the fuzzy observer is more accurate than the
EKF, even though the tuning for the fuzzy observer
is faster and simpler. For these simulations, the
average RMS error for the EKF was 0.018 [m],
whereas the fuzzy observer presents an average error
of 0.006 [m].
The average error of the fuzzy observer in the 1000
simulations was more than 3 times smaller than the
error of the EKF. It was also observed that only in
2.6% of the simulations done, the EKF outperformed
the fuzzy observer.
Fig. 8. Robot localization using the EKF and the
fuzzy nonlinear observer.
5.2. Effect of the Robot Dynamics
The effect that the dynamic equations have on both
methods must be also checked, as none of them
include the robot dynamics in their structure. To
achieve this, instead of using U, the reference angular
velocities, as input vector for each method, the actual
applied angular velocities are used, eliminating the
effect of the robot dynamics. Through this test it is
observed that both methods improve their estimation,
but the improvement of the EKF is higher. This
means that the effect of the robot dynamics has less
effect on the fuzzy observer compared with the EKF,
which needs more accurate equations to achieve a
good performance.
5.3. Sensitivity To Measurement and Process Noise
For this test, the standard deviation for the
measurements and the process noise were modified
by a 30% up and down, analyzing the effect on both
methods.
Fig. 9. Estimation error for both methods.
When the measurement noise is incremented, the
estimation error increases in both methods, but it
affects less the output of the fuzzy observer. The
same happens when the process noise is augmented.
5.4. Sensitivity To Noise Estimation
More important than the sensitivity towards the noise
is the sensitivity towards the noise estimation. In
most cases it is very difficult to estimate the noise
level on the measurements and it is even more
difficult to do it on the process. This makes important
to analyze the effect that a wrong estimation has on
both methods.
For these tests the measurements and process noise
standard deviation is estimated to be between a 30%
more and 30% less than the actual level used in the
exact robot model. The results show that when the
noise level is estimated incorrectly the estimation
error increases for both methods, but in average the
improvement of the fuzzy observer over the EKF
increases.
It is important to mention that considering all the
different tests made, the fuzzy observer gives an
estimation error that is at least 50% smaller than the
one given by the EKF, whereas the EKF
outperformed the fuzzy method in less than a 4% of
the simulations.
6. CONCLUSIONS
Through this work a novel nonlinear fuzzy observer
is presented. This observer is based on several linear
Kalman filters and a Takagi–Sugeno fuzzy model
which is used to combine the different estimations,
resulting in a more accurate observer for the posture
of the robot, compared with the conventional
Extended Kalman Filter.
The simulations also show several other interesting
advantages of the fuzzy observer. First and more
important, as the fuzzy observer is based on linear
models, the covariance matrices needed are easily
determined, implying that less tuning is required to
achieve a good performance, compared with the
EKF, which requires more work.
Also, the fuzzy observer is more robust that the EKF
as the effect of not considering the robot dynamics
and errors in the estimation of the process and
measurement noise affects more the performance of
the EKF than the performance of the fuzzy observer.
Based on this new nonlinear observer, less noise
contaminated residuals can be obtained, which will
be used to design a more accurate fault detection and
isolation system for mobile robots.
ACKNOWLEDGEMENTS
This work was supported by FONDECYT project no
1050684, CYTED Iberoamerican Network of
Robotics, and DIPUC Direction of Research and
Graduate Studies, Pontificia Universidad Católica de
Chile.
REFERENCES
Angeles, J. (1997). Fundamentals of Robotic
Mechanical Systems: Theory, Methods, and
Algorithms, Springer.
Basseville, M. and I.V. Nikiforov (1998). Detection
of Abrupt Change–Theory and Application (2nd
Ed.) Online: http://www.irisa.fr/sigma2/kniga/
Carlson, J., R.R. Murphy and A. Nelson (2004).
Follow-up Analysis of Mobile Robot Failures.
In: Proc. of the 2004 IEEE International
Conference on Robotics & Automation, 4987-
4994.
Jung, S.H. (2003). Queen-Bee Evolution for Genetic
Algorithms. IEEE Electronic Letters, 39, 575-
76.
Kalman, R.E. (1960). A New Approach to Linear
Filtering and Prediction Problems. ASME
Journal of Basic Engineering, 86, 35-45.
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. In: Proc. of the 1999
IEEE International Conference on Control
Applications, 2, 1021-1026.
Lin, C.T. and C.S.G. Lee (1996). Neural Fuzzy
Systems: A Neuro-Fuzzy Synergism to Intelligent
Systems, Prentice Hall.
Sasiadek, J.Z. and P. Hartana (2002). Adaptive
Fuzzy Logic System for Sensor Fusion in Dead-
reckoning Mobile Robot Navigation. In: Proc. of
the 15th IFAC Triennial World Congress.
Wang, H. and C.T. Goh (1999). Fuzzy Logic Kalman
Filter Estimation for 2-wheel Steerable Vehicles.
In: Proc. of the 1999 International Conference
on Intelligent Robots and Systems, 1, 88-93.
