[22] ApchiDK&TH

7/28/2019 [22] ApchiDK&TH

1/13

1

MOBILE ROBOT LOCALIZATION USING FUZZY NEURAL NET-WORK BASED EXTENDED KALMAN FILTER

Thuan Hoang Tran, Manh Duong Phung, Thi Thanh Van Nguyen, Quang VinhTran

University of Engineering and Technology, Vietnam National University, Hanoi

144 Xuan Thuy, Cau Giay, Hanoi, Vietnam

Corresponding author: [email protected]

ABSTRACT

This paper proposes a novel approach to improve the performance of the extended Kalman

filter (EKF) for the problem of mobile robot localization. A fuzzy logic system is employed tocontinuously adjust the noise covariance matrices of the filter. A neural network is implemented

to regulate the membership functions of the antecedent and consequent parts of the fuzzy rules.The aim is to gain the accuracy and avoid the divergence of the EKF when the noise covariancematrices are fixed or wrongly determined. Simulations and experiments have been conducted.The results show that the proposed filter is better than the EKF in localizing the mobile robot.

Keywords: fuzzy logic, neural network, extended kalman filter, localization, mobile robotI.INTRODUCTION

Localization, that is the determination of the robots position and orientation from sensor da-ta, is a fundamental problem in mobile robotics. In order to complete given tasks, the robot need

to know its own pose at each sampling time to make the path planning and motion control. Thedifficulty is that there always exist two kinds of error in the robot system: the systematic and nonsystematic errors [1]. The systematic error is caused by the imperfectness of robot mechanics

such as the limitation of encoder resolution, the ansymmetry of robot chassis, and the inequalityand misalignment of driven wheels. The non systematic error, on the other hand, is often randomand unknown. It is caused by uncertainty elements such as the slippage of wheels or the uneven-ness of floor during the robot operation. Filtering these errors to extract the actual pose of therobot is the final goal of localization algorithms. Various approaches have been proposed withtheir strengths and weaknesses.

In [2], one linear and two rotary encoders are used to measure the relative distance and bear-ing between two wheels. The result is then injected to the conventional dead reckoning methodto improve the accuracy. Another approach is the Monte Carlo method that is able to localize themobile robot without knowledge of its starting location. This method is more accurate, faster,

and less memory intensive than grid based methods [3]. A survey of Bayesian filter applied toreal world estimation was done by authors of university of Washington [4]. This research showsthat the Bayesian filter technique is a powerful statistical tool to perform the multi-sensor fusion,

estimate the system state and manage the measurement uncertainties. Its drawback is the expen-sive computation.

A less computation but effective method is the extended Kalman filter (EKF) [5]. It esti-mates the robot position under the scenaio that both sensor and system data are subjected to ze-

ro-mean Gaussian white noises [5, 6]. In this method, the choice of noise covariance matrices

7/28/2019 [22] ApchiDK&TH

2/13

2

greatly affects the estimate accuracy. Due to random essence of errors, these matrices changeaccording to the time of operation and therefore are difficult to determine. In practice, these ma-trices are often assumed to be fixed and chosen through off-line processes. However, this simpli-

fication may cause the EKF to diverge in some cases.

In this paper, a fuzzy system is implemented to online adjust the noise covariance matricesat each Kalman step. The membership functions of the fuzzy system are regulated by a neuralnetwork. The incorporation of fuzzy logic and neural network into the EKF creates a new opti-mal filter called the fuzzy neural network based EKF (FNN-EKF). This filter enhances the accu-

racy and convergence of the EKF for the problem of localization in unknown indoor environ-ment. This combined with previous results in path planning and obstacle avoidance of the au-thors group constitute a quite complete set of mechanisms for the autonomous operation of amobile robot [7-9].

The paper is organized as follows. Details of the robot model and EKF implementation aredescribed in section II. Section III presents the use of fuzzy logic and neural network to over-come the weakness of the EKF in localization. Simulations and experiments are presented insection IV. An evaluation of the system with respect to suggestions of possible future develop-

ments is mentioned in section V.

II.MOBILEROBOTLOCALIZATIONUSINGEXTENDEDKALMANFILTERAs mentioned in previous section, the EKF is considered as one of the most effective method

for mobile robot localization. This section first presents the kinematic model of the robot. The

implementation of the EKF is then described and discussed.

A. Robot modelThe mobile robot considered in this research is the two-wheeled, differential-drive mobile ro-

bot with non-slipping and pure rolling. Figure 1 shows the coordinate system of the robot, where(XG, YG) is the global coordinate system and (X R, YR) is the local coordinate system relative tothe robot chassis. With this kind of mobile robot, the dead reckoning is often used to determine

the relative position of the robot in the work space. It estimates the robots position and orienta-

tion based on encoder data.

From figure 1, the conversion factor that translates encoder pulses into linear wheel dis-

placement is given by:

RCm

nCe

(1)

Figure 1: Pose and parameters of mobile robot

7/28/2019 [22] ApchiDK&TH

3/13

3

whereR is the wheel diameter, n is the gear ratio of the reduction gear between the motor andthe drive wheel, andCe is the encoder resolution (in pulses per revolution).

LetNL,i andNR,ibe the number of pulses counted by encoders of the left and right wheels attime i, respectively. The displacement of each wheel is then given by:

, , , ,S C N S C N m mL i L i R i R i

(2)

These can be translated to the linear incremental displacement of the robots centerSand ori-entation angleas:

, ,

2

, ,

S SL i R i

Si

S SR i L i

i L

(3)

whereL is the distance between two wheels. The coordinates of the robot in the global coordi-

nate frame then can be update by:

* os( )1 1*sin( )

1 1

1

x x S cii ii iy y Si i ii i

i ii

(4)

From equation (4), it is able to determine the robots position and orientation if we know thenumber of encoder pulses in each sampling period. In another word, equation (4) is the localiza-

tion equation of the dead reckonging method. Nevertheless, equation (4) does not include errorsappeared in the system. As analyzed in section I, errors are unavoidable and may downgrade thesystem performance if appropriate compensation is not investigated. For this reason, the robot

model is rewritten in state-space representation with the appearance of disturbances as follows.

( , , )1 1 1

fi i i i x x u w (5)

( , )hi i i x v (6)

where i i iT

x y x is the state vector described the instantaneous position and orientation of

the robot,, ,

TS SL i R i

u is the input, zi is the measurments, fandh are the system func-

tions, wi and vi are random variables described the process and measurement noises. Thesenoises are assumed to be zero-mean, independent, white, and Gaussian with the process noise

covariance matrix Q and the measurement noise covariance matrix R : (0, )i iw N Q ,

(0, )i iv N R . The state-space presentation (5) - (6) is basis for the implementation of the EKF.

B. Implementation of the EKF for mobile robot localizationAs x is the state vector described the robot postion and orientation, the problem of localizationbecomes the problem of state estimation. An optimal solution is the Kalman filter [12]. Its im-plementation is performed through two steps: prediction and correction, as follows:

1. Prediction step with time update equations:

7/28/2019 [22] ApchiDK&TH

4/13

4

( , ,0)1 1

fi i i x x u (7)

1 1T T

i i i i ii i P A P A W Q W (8)

where i

x : the prior state estimate at step i given knowledge of the process prior to step i-1

iP : the covariance matrix of the state prediction error

iP: the covariance matrix of the corresponding estimation error

A : the Jacobian matrix of partial derivatesftox

W : the Jacobian matrix of partial derivatesfto w

Q : the input noise covariance matrix

2. Correction step with measurement update equations:1

( )T T T

i i i i i i i i i K P H H P H V R V (9)

( ( ,0))hi i i i i x x K z x (10)

( )i i i i

P I K H P (11)

where ix : the posterior state estimate at step i given measurement zi

iK: the Kalman gain

R : the covariance matrix of measurement noise

H : the Jacobian matrix of partial derivates h tox

V : the Jacobian matrix of partial derivates h to v

Equations (7) - (11) show that the noise covariance matrices Q andR need be accurately de-

termined in order to ensure the efficient operation of the EFK. In addition, these matrices should

be changed at each step of the EKF. The dynamic determination of Q andR however is often not

easy to perform, especially during the operating session of the mobile robot. In practice,

Q andR are often assumed to be fixed. Their values are determined before the execution of the

EKF. This approach has several drawbacks. Firstly, fixed matrices Q andR do not reflect the

variation of noises. Secondly, the covariance errorP and the Kalman gain K will quickly reachthe steady state. This is equivalent to the convergence of the EKF to certain degree accuracy.

Finally, the fixation ofQ andR may break the stability operation of the EKF and cause the sys-

tem to be halted in some cases. To overcome these, fuzzy logic [13, 14] can be employed to en-able an online adjustment (not the determination) of Q andR . In addition, the effeciciency of

fuzzy rules can be futher improved by using neural network to regulate its membership func-

tions.

III. IMPROVEMENTEKFBYFUZZYNEURALNETWORKSYSTEMThis section first presents the basis idea to adjust the covariance matrices Q andR. Details

of implementation this idea using fuzzy logic and neural network are then described.

A. Concept of noise covariance matrices adjustmentFrom equation (10), let ( ,0)hi i i

r z x be the residual between the actual and the pre-

dicted measurements. This residual, gained by K, is the correction factor to form the posterior

7/28/2019 [22] ApchiDK&TH

5/13

5

estimate ix

from the prior estimate i

x . It also reflects the accuracy of the estimation value. A

small value ofri implies good estimation as the predicted measurement is closed to the actual

measurement. As mentioned in section II.B, if the covariance matrices Q and R are fixed, the

Kalman gain K will quickly stabilize and remain constant. It means that the EKF will converge

to certain accuracy degree. In order to improve this, we can adjust Q andR so that ir is reduced.

Assume that the system operates with a predetermined and fixedQ , the adjusting process of

R to reduceir is performed as follows:

- Determining the residuali

r and computing its present covariance:

Ti i i i i

H P H R (12)

- The average covariance of ir through a number of steps from the past to present is givenby:

1

0

i Ti j jj jN

C r r (13)

wherej0 = i-N+1 and N is the number of past periods.

- The difference between the present and average covariances ofi

r is given by:

i i i D S C (14)

This difference is used as the reference to adjustiR .

- Leti

R be an adjustment factor.i

R is changed through each step as follows:

1. If 0i D then maintain iR 2. If 0i D then decrease Ri 3. If 0i D then increase Ri

-

R is then adjusted as:i i iR R R (15)

The adjustment of covariance matrix Q is similar to steps for R except that the present co-

variance matrix ofi

r is replaced by:

)T T

i i i i i i i i S H (A P A Q H R (16)

B. Fuzzy system implementationThe fuzzy system to execute equation (15) is implemented through three steps as follows.

Step 1: Define input/output language variablesThe input/output language variables of the fuzzy system are chosen as follows:

( , )j jiD : Negative (N), Zero (Z), Positive (P)

Ri : Decrease (D), Maintain (M), Increase (I)

Step 2: Define membership functionsWe define four sigmoid-shape membership functions P(a1,b1), N(a2,b2), D(a3,b3) and I(a4,b4)

and two gauss-shape membership functions M(c1,1) and Z(c2,2) as follows:

7/28/2019 [22] ApchiDK&TH

6/13

6

( )

1( )

1 e i ia x bsigmoid x

2

2

( )

2( )

i

i

x c

gauss x e

(17)

where parameters ai, bi, ci, and i are distinct values for each membership function.

Step 3: Design fuzzy rulesRules for the fuzzy system are defined so that the difference between the present and aver-age covariances of the residual is reduced as follows:

1. IfiD is Z then Ri is M

2. IfiD is P then Ri is D

3. Ifi

D is N then Ri is I

Step 4: DefuzzificationThe defuzzification is accomplished by centroid method.

( )

( )

i i

i

x x

x

(18)

wherexi is the Ith domain value and(xi) is the truth membership value for that domain point.

Among steps, the definition of membership functions requires manual customization. It istherefore time consuming and often do not adapt to the change of system parameters. In order to

overcome this limitation, a neural network is implemented to regulate membership functions.

C. The fuzzy neural network systemThe objective of the neural network is to tune parameters a i, bi, ci, and i so that the member-

ship functions are fitter with the system. We design a neural network with five layers as shown

in the figure 2. Details of each layer are as follows:

Figure 2: The fuzzy neural network

Layer 1: The output of the node is the degree of the variable with respect to fuzzy set N, Z and P.Layer 2: The firing level of each rule is computed by

1 2 3N( ), Z( ), P( )x x x (19)

Layer 3: The normalization of the firing levels is indicated. The normalized firing level of the

corresponding rule is

1

31 22 3

1 2 3 1 2 3 1 2 3

, ,

(20)

Layer 4: The product of the normalized firing level and the individual rule output of the corre-sponding rule is the output of each neuron. They are determined respectively as below

7/28/2019 [22] ApchiDK&TH

7/13

7

1 1

1 1 1

1 1 2 2 2 2 3 3 3 3I ( ), M ( ), D ( )z z z

(21)

Layer 5: The overall output system is the sum of all incoming signals

1 1 2 2 3 3z z z z (22)

With given the crisp training set 1 1 2 2( , ), ( , )...( , )K KD R D R D R

, we define the measure oferror for the k-th training pattern as

21E ( ) , 1...2

k k kz R k K

(23)

where zk is the computed output form of the fuzzy system corresponding to the input pattern kD

and the desired outputk

R . The hybrid neural network learns the shape parameters a i, bi, c i, and

i of each membership function by using the steepest descent method. They are described as:

Ea (t + 1) = a (t) - k

i i

ia

E

b (t +1) = b (t) - ki i

ib

(24)

Ec (t + 1) = c (t) - ki i

ic

E

(t +1) = (t) - ki ii

where >0 is the learning constant and tis the number of the adjustments.Parameters extracted from equation (21) are used to construct new membership functions orlearned membership functions for the fuzzy inference system.

IV. SIMULATIONSA. Simulation setup

Simulations are conducted to evaluate the effeciency of the proposed algorithm. In each

evaluation, a Monte Carlo simulation with 100 times is executed to compare the performance of

the EKF and the FNN-EKF. Parameters for simulations are extracted from a real mobile robotbuilt by the authors research group at the universitys laboratory [15].

The mobile robot has following parameters: the wheels diameter R is 0.05m, the distancebetween the wheels L is 0.6m, the gear ratio of the reduction gear between the motor and thedrive wheel n equals to 1, and the encoder resolution C e is 500. The input noise covariance ma-

trix Q is defined as propotional to the deviations of the displacement, S, and the orientation,

, of the robot:

20

20

S

Q =

The covariance of measurement noise is chosen to be:

0.01 0 0

0 0.01 0

0 0 0.018

R

This is based on the value of the real robot system described in next section.

7/28/2019 [22] ApchiDK&TH

8/13

8

Initial membership functions P(a1,b1), N(a2,b2), D(a3,b3), I(a4,b4), M(c1,1) and Z(c2,2) arechosen with parameters as follows: a1 = -0.16, b1 = -17, a2 = 0.1, b2 =13, a3 = 0, b3 = -100, a4 =0.1, b4 = 23, 1 = 0, c1 = 0.03, 2 = 0.03 and c2 = 0.03. After the learning process of neural net-

work, these parameters are as follows: a1 = -0.1073, b1 = -17.0995, a2 = 0.1515, b2 =13.8108, a3= -0.06552, b

3= -143, a

4= 0.1030, b

4= 24.2000,

1= 0.1779, c

1= 0.0571,

2= 0.0620 and c

2=

0.0467. Figure 3 and figure 4 show the initial and learned shapes of the membership functions D,

M and I of Ri , respectively.

-0.2 0 0.2 0.4 0.60

0.2

0.4

0.6

0.8

1

D

M

I

-0.2 0 0.2 0.4 0.60

0.2

0.4

0.6

0.8

1

D

M

I

Figure 3: The initial membership functions of Ri Figure 4: The learned membership functions of Ri

B. Simulation resultsIn the first comparison, the initial deviations of the displacement and the orientation of the

robot are chosen to be small: S = 2cm, = 10. The trajectory of the robot in this simulation is

shown in figure 5.

0 5 10 15

0

2

4

6

8

10

12

14

16

S

T

X(m)

Y(m)

5 10 15 20 25 30

4

6

8

10

12

14

16

18

S

T

X(m)

Y(m)

Figure 5: The trajectory of the robot Figure 6: The trajectory of the robot

in the frist case in the second case

7/28/2019 [22] ApchiDK&TH

9/13

9

Figure 7 shows the comparative result of paths estimated by the EKF and FNN-EKF. Notingthat only 100 sample points are shown for the convenience of view and comparison. Figure 8-10show the root mean square error (RMSE) of estimations in the X, Y, and direction. It indicates

that the FNN-EKF estimation introduces smaller error than the EKF.

100 120 140 160 180 200

0.8

1

1.2

1.4

1.6

1.8

2

2.2

Time(100ms)

X(m)

True

EKF

FNN-EKF

100 120 140 160 180 200

0.02

0.03

0.04

0.05

0.06

0.07

0.08

Time(100ms)

RMSEX(m)

EKF

FNN-EKF

Figure 7: Comparison between the EKF,the FNN-EKF and the true path

Figure 8: The RMSE of estimationsin X direction

100 120 140 160 180 2000.02

0.04

0.06

0.08

0.1

0.12

Time(100ms)

RM

SEY(m)

EKF

FNN-EKF

Figure 9: The RMSE of estimations

in X direction

100 120 140 160 180 2000

0.05

0.1

0.15

0.2

Time(100ms)

RM

SE(radian)

EKF

FNN-EKF


in X direction

In the second Monte Carlo simulation, the initial deviations of the displacement and the ori-

entation of the robot are chosen to be l: S = 5cm, = 30. The trajectory of the robot in this

simulation is shown in figure 6. Figure 11-14 show the results. They also indicate that the FNN-

EKF estimation is better than than the EKF.

7/28/2019 [22] ApchiDK&TH

10/13

10

400 420 440 460 480 5008.5

9

9.5

10

10.5

11

11.5

Time(100ms)

X(m)

True

EKF

FNN-EKF

400 420 440 460 480 5000

0.05

0.1

0.15

0.2

0.25

Time(100ms)

RMSEX(m)

EKF

FNN-EKF

Figure 11: Comparison between the EKF,

the FNN-EKF and the true path


in X direction

400 420 440 460 480 5000

0.02

0.04

0.06

0.08

0.1

0.12

Time(100ms)

RMSEY(m)

EKF

FNN-EKF


in X direction

400 420 440 460 480 5000

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Time(100ms)

RMSE(radian)

EKF

FNN-EKF


in X direction

V. EXPERIMENTSA. Experiment setup

Experiments were conducted in a real mobile robot developed by the authors research group(figure 15). The robot is a two wheeled, differential-drive mobile robot. Its wheel diameter is0.05m and the distance between two drive wheels is 0.6m. The speed stability of motors con-

trolled by the PID algorithm is 5%. The sampling time t is 375ms. The operation environmentis flat-floor constructed by cement with several wooden plates surrounded (figure 16). Details ofthe robot system can be refered from our previous work [15]. The initial value of matries Q and

R is similar to the second simulation case.

7/28/2019 [22] ApchiDK&TH

11/13

11

Figure 15: A real mobile robot at laboratory. Figure 16. A operation environment of robot

B. Experiment resultsIn experiments, the robot is controlled to follow a round path. Figure 17 shows the true path

and the paths estimated by the EKF and the FNN-EKF. Some parts of trajectory are extracted forthe convenience of view as shown in figure 18-20. Due to the asynchronization between experi-

mental values measured on the floor and the values estimated by the filters, it is not possible tocompute the RMSE in experiments. Nevertheless, it is able to conclude from figure 18-20 thatthe FNN-EKF path is closer to the true path than the EKF.

-1 0 1 2 3 4-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

X(m)

Y(m)

TrueEKF

FNN-EKF

2.5 2.6 2.7 2.8 2.9 3

-0.3

-0.2

-0.1

0

0.1

X(m)

Y(m)

True

EKF

FNN-EKF

Figure 17: The trajectory of robot Figure 18: The first corner on the trajectory

7/28/2019 [22] ApchiDK&TH

12/13

12

2.5 2.6 2.7 2.8 2.9 3

-2.5

-2.4

-2.3

-2.2

-2.1

X(m)

Y(m)

True

EKF

FNN-EKF

0 0.5 1-3.2

-3

-2.8

-2.6

-2.4

-2.2

X(m)

Y(m)

True

EKF

FNN-EKF

Figure 19: A line on the trajectory Figure 20: The third corner on the trajectory

VI.CONCLUSIONSThis paper proposes a novel filter called FNN-EKF for the problem of localization. This fil-

ter employs a fuzzy system and a neural network to regulate the noise covariance matrices sothat the estimate is converged and more accurate. A number of simulations and experimentshave been conducted and the results demonstrate the improvement of the FNN-EKF compared to

the EKF. The good localization result in this research can be used as observation data for thecontroller to control the robot motion in various navigation tasks.

REFERENCES[1] Jonh Borenstein and Liqiang Feng, Measurement and Correction of Systematic Odometry Error inMobile Robots, IEEE Transactions on Robotics and Automation, Vol 12, No5, October 1996.

[2] Borenstein, J, 1994, The CLAPPER: a Dual drive mobile robot with internal correction of deadreckoning errors. Proceedings of the 1994 IEEE International Conference on Robotics and Automation,San Diego, May 8-13, pp 3085-3090.

[3] F Dellaert, D Fox, W Burgard, Monte Carlo localization for mobile robots, Proceedings of the 1999IEEE International Conference on Robotics and Automation, Volume 2, pp 1322-1328.

[4] V Fox, J Hightower, L Liao, D Schulz, Bayesian Filtering for Localization Estimation, Volume 2,Issue 3, page 24-33 2003, Pervasive Computing IEEE.

[5] Grewal M.S & Andrew A.P, Kalman Filter Theory and Practice Using Matlab, Jonh Wiley andSons, Inc, 2001

[6] Greg Welch, Gary Bishop, Introduction to the Kalman filter, Siggraph 2001, Course 8.[7] Manh Duong Phung, Thanh Van Thi Nguyen, and Tran Quang Vinh, Control of an Internet-basedRobot System Using Fuzzy Logic, Conference on Integrated Circuits and Devices in Vietnam (ICDV)2011, Hanoi, Vietnam

[8]Nguyen Thi Thanh Van, Phung Manh Duong, Pham Dinh Tuan, and Tran Quang Vinh,Development of a Fuzzy-based Patrol Robot Using in Building Automation System, Journal ofComputer Science and Cybernetics, vol.27 no.1, 2011, p83-93

7/28/2019 [22] ApchiDK&TH

13/13

13

[9] Manh Duong Phung, Thanh Van Thi Nguyen, Cong Hoang Quach, Quang Vinh Tran, Developmentof a Tele-guidance System with Fuzzy-based Secondary Controller, The 11th International Conference

on Control, Automation, Robotics and Vision, ICARCV, Singapore 2010.

[10]J.L. Crowly, World modeling and position estimation for a mobile robot using ultrasonic ranging,in Proc. IEEE Int Conf Robot Automation, 1989, pp 674-680

[11]T. Skordata, P. Puget, R.Zigman and N.Ayache, Building 3-D edgelines tracked in an imagesequence, in Pro Intell, Autonomous Systems-2, Amsterdam, 1989, pp 907-919

[12]R.E Kalman, A New Approach to Linear Filtering and Prediction Problems, Transactions of theASMEJournal of Basic Engineering, 82 (Series D): 35-45, 1960.

[13]L Zadeh, Fuzzy Logic, Computer Journals & Magazines, Vol 45 Issue 7, 1988.[14]P.J Escamilla-Ambrosio, N.Mort, Development of a fuzzy logic-based adaptive kalman filter,Proc.European Control Conference, Porto, Portugal, September, pp. 1768-1773. Escamilla, PJ and N.Mort 2003.

[15]T.T. Hoang, P.M Duong, N.T.T.Van, D.A.Viet and T.Q. Vinh, Development of an EKF-basedLocalization Algorithm Using Compass Sensor and LRF, The 12th International Conference on Control,

Automation, Robotics and Vision, ICARCV, Guangzhou - China, 2012.

Documents

[22] ApchiDK&TH