DSpace at VNU: Mobile robot localization using fuzzy neural network based extended Kalman filter

Mobile Robot Localization Using Fuzzy Neural Network Based Extended Kalman Filter Thi Thanh Van Nguyen, Manh Duong Phung, Thuan Hoang Tran, Quang Vinh Tran University of Engineering and

Mobile Robot Localization Using Fuzzy Neural

Network Based Extended Kalman Filter Thi Thanh Van Nguyen, Manh Duong Phung, Thuan Hoang Tran, Quang Vinh Tran University of Engineering and Technology, Vietnam National University, Hanoi

144 Xuan Thuy, Cau Giay, Hanoi, Vietnam

vanntt@vnu.edu.vn

Abstract—Localization is fundamental to autonomous

operation of the mobile robot In this paper, a new optimal filter

namely fuzzy neural network based extended Kalman filter

(FNN-EKF) is introduced to improve the localization of a mobile

robot in unknown environment The filter is a combination

between a normal extended Kalman filter (EKF) installed on a

differential-drive wheeled mobile robot and an online adjustment

of the process noise covariance matrix Q and the measurement

noise covariance matrix R The adjustment is performed by fuzzy

system and the purpose is to overcome the divergence of the EKF

when the matrices Q and R are fixed or wrongly determined The

membership functions of the antecedent and consequent parts of

fuzzy if-then rules in the fuzzy system are tuned by neural

network Integrating neural network into the fuzzy system called

the fuzzy neural network is to gain the accuracy while reducing

the time and cost in designing the membership functions

Simulating experiments have been conducted and results show

that the FNN-EKF is more accurate than the EKF in localizing

the mobile robot An evaluation of the system with respect to

suggestions of possible future developments is also mentioned in

the paper

Index Terms— fuzzy neural network, extended kalman filter,

localization, mobile robot

I INTRODUCTION Localization is a major problem in mobile robotic In

order to autonomously complete given tasks, the robot needs

to know its position and orientation in operating environment

As a result, various approaches have been proposed to localize

the mobile robot [1- 4] For example, the Monte Carlo method

is able to determine the robot position without knowledge of

its starting location This method is faster, more accurate and

less memory intensive than grid based methods [1] A survey

of Bayesian filter applying to real world estimation was done

in [2] This research showed that the Bayesian filter technique

is a statistical tool to help to perform the multi-sensor fusion,

estimate the system state and manage the measurement

uncertainties

Among various localization methods, the EKF is

considered as one of the most popular and effective

approaches due to its ability to reject unnecessary and false

data [5-7] This filter uses the sensor data and the systematic

information to estimate the robot’s position in a scenario that

the system is affected by Gauss white noise [8,9] The

efficiency of the EKF depends on the accuracy in determining

the process noise covariance matrix Q and the measurement

noise covariance matrix R In practice, it is often assumed that

these matrices are fixed and calculated through off-line processes while they actually change according to the operation time due to random errors This makes the EKF diverge in some cases The fuzzy logic can be employed to overcome this disadvantage [10] Fuzzy logic rules enable an online adjustment of the matrices Q and R during the operation of the EKF and consequently increase the performance as well as prevent the divergence of the filter Nevertheless, the fuzzy system usually requires a lot of time to design and tune its membership functions and rules A technique which can fix these disadvantages is neural network [11] It can be used to tune the membership functions of fuzzy systems so that the development time and cost are reduced while improving the system performance

In this paper, the fuzzy system is used to continuously adjust the noise covariance matrices Q and R at each Kalman step This ensures the relevance and convergence of the EKF algorithm The membership functions of the antecedent and consequent parts of fuzzy if-then rules are tuned by neural network The combination between fuzzy logic and neural network creates the fuzzy neural network The incorporation

of fuzzy neural network into the EKF creates a new optimal filter called the fuzzy neural network based EKF (FNN-EKF) This filter is implemented in the two-wheeled, differential-drive mobile robot model for the problem of localization in unknown indoor environment

The paper is divided into five parts as follows Part II describes the model of two-wheeled, differential-drive mobile robot and the implementation of the EKF for this kind of robot The use of fuzzy neural network to overcome the weakness of the EKF in localization problem is presented in Part III Simulation installation and results are presented in the Part IV Finally, an evaluation of the system with respect to suggestions

of possible future developments is mentioned in Part V

II MOBILE ROBOT LOCALIZATION WITH EKF The two-wheeled, differential-drive mobile robot with non-slipping and pure rolling is considered in this research Figure 1 shows the coordinate system of the robot, where (XG, YG) is the global coordinate system and (XR, YR) is the local coordinate system relative to the robot chassis With this type

of mobile robot, the dead reckoning is often used to determine the relative position of the robot in the work space

Figure 1: The pose and parameters of mobile robot

The conversion factor that translates encoder pulses into

linear wheel displacement is

Cm=nCeπR (1)

where R is the wheel diameter, n is gear ratio of the reduction

gear between the motor and the drive wheel, and Ce is the

encoder resolution Suppose that at time i, the encoders of left

and right wheels count the pulse increments of N L,i and N R,i

respectively The displacement of each wheel is then

calculated as:

These can be translated to the linear incremental displacement

of the robot’s center ∆S and orientation angle ∆θ

,

2

where L is the distance between two wheels

The coordinates of the robot at time i in the global

coordinate frame can be then updated by:

1

θ θ

θ θ

θ θ θ

= − + Δ

(4)

The equation (4) does not consider errors appeared in the

system while in fact errors are unavoidable and affect the

system performance The EKF is used to reduce the effect of

the errors to the system The installation of the EKF for the

mobile robot is summarized as follows

Letx=⎡⎣x i y i θi⎤⎦T , x∈ℜnbe the state variable

describing the instantaneous position and direction of the

robot, = Δ⎡ S L i, ΔS R i, ⎤T

u be the input The non-linear

stochastic different equation of the robot’s kinematic model is

described as:

xi = f(xi−1,ui−1,wi−1) (5)

and the measurement equation is written as:

zi =h( ,x v i i) (6)

where the random variables wi∼N(0,Qi) and vi ∼N(0,Ri)

present the process and measurement noises They are

assumed to be independent of each other, white and normal probability distributions with the process noise covariance matrix Q and the measurement noise covariance matrix R The EKF is implemented in two phases: the prediction and correction as follows:

1 Prediction step with time update equations

ˆi− = f ˆi−1, i−1,0)

x x u (7)

1 T 1 T

P A P A W Q W (8) where xˆi−: the priori state estimate at step i given knowledge

of the process priori to step i-1

i−

P : the covariance matrix of the state prediction error

i

P : the covariance matrix of the corresponding estimation error

A : the Jacobian matrix of partial derivates f to x

W : the Jacobian matrix of partial derivates f to w

Q : the input noise covariance matrix

2 Correction step with measurement update equations

1

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

ˆi =ˆi−+ i i( −h(ˆi−,0))

x x K z x (10) ( )

P I K H P (11) where ˆxi : the posteriori state estimate at step i given measurement zi

i

K : the Kalman gain

R : the covariance matrix of measurement noise

H : the Jacobian matrix of partial derivates h to x

V : the Jacobian matrix of partial derivates h to v

Equations (7) to (11) show that the efficient operation of the EFK depends on the accuracy of the noise covariance matrices Q and R It is not easy to perform the dynamic determination of Q and R, especially during the operating session of the mobile robot In practice, Q and R are usually assumed to be fixed and their values are often predetermined before the execution of the EKF Though this approach is simple, it has some drawbacks Firstly, the fixed matrices Q and R are not able to reflect the variation of noise Secondly, the covariance error P and the Kalman gain K would quickly stabilize and remain constant This is equivalent to the convergence of the EKF to some certain degree of accuracy And finally, the fixation of Q and R in some cases may break the stability operation of the EKF and cause the system to be halted The fuzzy logic can be employed to overcome these Fuzzy logic rules enable an online adjustment of the matrices Q and R during the operation of the EKF and consequently increase the performance as well as prevent the divergence of the filter

III.THE FUZZY NEURAL NETWORK BASED EKF This section presents the use of fuzzy logic to overcome disadvantages of the EKF A fuzzy neural network which is computational equivalent to fuzzy system is used to tune

membership functions of the antecedent and consequent parts

of fuzzy if-then rules

A ‘On-line’ adjustment of covariance matrices Q and R

The residual ri =zi −h xˆi−,0) in (10) is the difference

between the absolute and predictive measurements This

amount is also the consequence of measurement zi with the

weighting gain Ki, Ki i(z −h(xˆi−,0)) The EKF uses

i

r as a correction coefficient to correct the priori estimate ˆxi− The

fuzzy inference system employs ri as a reference factor to

adjust the covariance matrices Q and R The adjusting process

of R is performed as follows

Assume that the system operates with a predetermined

and fixed Q Calculating the theoretical covariance

T

i = i i i− + i

S H P H R and the actual covariance

1

ˆ

o

i

j j

j j

N

= r r

C where N is the size of moving estimation

window, the difference between the theoretical covariance and

the actual covariance is then defined byDi =Si −Cˆi

The fuzzy inference system with the input Di and the

outputΔRi defines three general rules as:

1 If 0Di ≅ ( implying that Siand ˆCiperfectly

matched) then maintain ΔRi

2 If 0Di > ( implying that Siis greater than ˆCi ) then

decreaseΔRi

3 If 0Di < (implying that Siis smaller ˆCi) then

increaseΔRi

Each element in the main diagonal of R is then adjusted

as

( , ) 1( , )

i j j = i− j j + ΔRi

R R (11)

where ΔRi is an adjusting factor added or subtracted from the

elements (j,j) of the matrix R at each time interval

The adjustment of the process noise covariance Q is

similar to above steps for R except that the theoretical

covariance matrix of ri is replaced by:

)

S H (A P A Q H R (12)

B The fuzzy neural network

In this part, the installation of neural network into the fuzzy

inference system is presented The membership functions of

the antecedent and consequent parts of fuzzy if-then rules are

tuned based on given training set instead of choosing

manually

The input/output language variables of the fuzzy system

are chosen as below:

- Di: Negative (N), Zero (Z), Positive (P)

- ΔRi : Decrease (D), Maintain (M), Increase (I) Rules for the fuzzy system are defined so that the difference between the theoretical covariance and the actual covariance of the residual is smallest

1 If Diis Z then ΔRi is M

2 If Diis P then ΔRi is D

3 If Diis N then ΔRi is I where P(a1,b1), N(a2,b2), D(a3,b3) and I(a4,b4) have sigmoid membership functions, M(c1,σ1) and Z(c2,σ2) have gauss membership functions given by

1 ( )

1 e a x b i i

+

2 2

2

( )

i i

x c

− −

= (13) where parameters ai, bi, ci, and σi are distinct values for each membership function

The fuzzy neural network with five layers is shown in the figure 2

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 N( ),x 2 Z( ),x 3 P( )x

α = α = α = Layer 3: The normalization of the firing levels is indicated The normalized firing level of the corresponding rule is

1

3

α α α α α α α α α

Layer 4: The product of the normalized firing level and the individual rule output of the corresponding rule is the output

of each neuron They are determined respectively as below

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

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

z=β z +β z +β z (16) With given the crisp training set{( ,D1 ΔR1),(D2,ΔR2) (D K,ΔR K)}, we define the measure

of error for the k-th training pattern as

2

1

E ( ) , 1

2

k = z k− ΔR k k= K (17)

Figure 2: The fuzzy neural network

where zk is the computed output form of the fuzzy system

corresponding to the input pattern Dk and the desired output

k

R

Δ The hybrid neural network learns the shape parameters

ai, bi, ci, and σi of each membership function by using the

steepest descent method They are described as:

E

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

i

a

η∂

∂ E

b (t +1) = b (t) - k

i

b

η∂

∂ (18) E

c (t +1) = c (t) - k

i

c

η∂

∂ E (t +1) = (t) - k

i

σ σ η

σ

∂

∂ where η>0 is the learning constant and t is the number of

the adjustments From these parameters, the new membership

functions or learned membership functions are use for the

fuzzy inference system

IV.SIMULATION

In this section, the simulation is presented to evaluate the

efficiency of the FNN-EKF for mobile robot localization The

robot mentioned in part II is employed with following

parameters: the wheel’s diameter R =0.05m, the distance

between the wheels L = 0.6m, the gear ratio of the reduction

gear between the motor and the drive wheel n = 1 and the

encoder resolution Ce = 500 These parameters are extracted

from a real mobile robot built by the author’s research group

[12] The random errors are modeled as being proportional to

the displacement of the robot’s center ∆S i and orientation ∆θ i

at step i The input noise covariance matrix Q is defined

as

2 0

2

0

S

σ

σ θ

=

Δ

⎣ Δ ⎦

Q In case the deviation of the

displacement of the robot’s center ∆S i is σ∆S = 10 cm and the

deviation of the orientation angle ∆θ i is σ∆θ = 50, the process

noise covariance matrix Q and the initial measurement noise

covariance matrix R are:

0.01 0

0 0.0076

0.01 0 0

0 0.01 0

0 0 0.018

=

The simulation program creates an arbitrary path of the

robot called “The theory path” Due to random errors, the robot

actually follows another path called “The true path” Sensor

system updates the robot state by making measurements to

create “The measurement path” The comparison among these

paths is shown in fig.3 The goal of the filter is to estimate the

path that is most close to the true path The EKF and

FNN-EKF are executed at the same time The results are shown in

the fig.4 The figures only show 100 sampling points for the

convenience of view and comparison

0.8 1 1.2 1.4 1.6 1.8 2 2.2

T ime(100ms)

T heory

T rue Measurement

Figure 3: The theory, the true and the measurement path

0.8 1 1.2 1.4 1.6 1.8 2 2.2

T ime(100ms)

T rue EKF FNN-EKF

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

0.2 0.4 0.6 0.8

1

N Z P

Figure 5: The initial membership functions of Di

-0.20 0 0.2 0.4 0.6

0.2

0.4

0.6

0.8

1

D M I

Figure 6: The initial membership functions of ∆Ri

0.2

0.4

0.6

0.8

1

N Z P

Figure 7: The learned membership functions of Di

0.2

0.4

0.6

0.8

1

D M I

Figure 8: The learned membership functions of ∆Ri

-0.1 -0.08 -0.06 -0.04 -0.02 0 0.02 0.04 0.06

T ime(100ms)

EKF FNN-EKF

Figure 9: The deviation between the estimated and the true path in X

direction

-0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2

T ime(100ms)

EKF FNN-EKF

Figure 10: The deviation between the estimated and the true path in Y

direction

-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3

T ime(100ms)

EKF FNN-EKF

Figure 11: The deviation between the estimated and the true path in

orientation

100 120 140 160 180 200 -0.15

-0.1

-0.05

0 0.05

0.1

0.15

0.2

T ime(100ms)

Initial FNN-EKF FNN-EKF

Figure 12: Comparison between the initial EKF and the tuned

FNN-EKF

The figures 5 and 6 are the initial shape of the

membership functions of the FNN They are chosen suitably

to the altering limitation of D i and RiΔ With given crisp

training set{( ,D1 ΔR1),(D2,ΔR2) (D K,ΔR K)}, where K = 967,

the FNN learns the shape parameters of the membership

functions and has results: a1 = -0.1073, b1 = -17.0995, a2 =

0.1515, b2 =13.8108, a3 = -0.06552, b3 = -143, a4 = 0.1030,

b4 = 24.2000, σ1 = 0.1779, c1 = 0.0571, σ2 = 0.0620, and c2 =

0.0467 From these parameters, the new learned membership

functions are shown in the figure 7 and 8

The comparative result of paths estimated by the EKF and

FNN-EKF is shown in the figure 4 It shows that the estimated

path estimated by the FNN-EKF is better than the EKF due to

the smaller deviation in compare to the true path The deviation

between the estimated value of the EKF and the FNN-EKF and

the true path in the X, Y direction and the orientation angle is

shown in the figure 9, 10, and 11 respectively A comparison

between the initial FNN-EKF and the tuned FNN-EKF in the

figure 12 indicates the efficiency of the neural network in

tuning the membership functions of the fuzzy system

V.CONCLUSIONS This research proposes the fuzzy neural network based

extended Kalman filter for the localization problem of mobile

robot This filter estimates the path that is most approximate to

the real path of robot motion The simulation results

demonstrate the improvement of the FNN-EKF compared to

the EKF The good localization result from this research can be

used as observation data for the controller to control the robot

motion in navigation tasks In future work, the presented research will be installed in a real mobile robot in order to enhance the accuracy and efficiency of autonomous operation

REFERENCES [1] F Dellaert, D Fox, W Burgard, “Monte Carlo localization for mobile robots”, Proceedings of the 1999 IEEE International Conference on Robotics and Automation, Volume 2, pp

1322-1328

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

[3] 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

[4] Borenstein, J, 1994, “The CLAPPER: a Dual drive mobile robot with internal correction of dead reckoning errors” Proceedings

of the 1994 IEEE International Conference on Robotics and Automation, San Diego, May 8-13, pp 3085-3090

[5] Sasisdek, J.Z “Sensor data fusion using Kalman filter”, Proceedings of the Third International Conference on Information Fusion, 2000, WED5/19 - WED5/25 vol.2

[6] Georgiev A “Localization methods for a mobile robot in urban environments” IEEE Transactions on Robotics, Vol 20, Issue 5, page 851-864

[7] Reina.G “Adaptive Kalman Filtering for GPS-based Mobile Robot Localization”, IEEE International Workshop on Safety, Security and Rescue Robotics 2007, SSRR, pages 1-6

[8] R.E Kalman, “A New Approach to Linear Filtering and Prediction Problems”, Transactions of the ASME–Journal of Basic Engineering, 82 (Series D): 35-45, 1960

[9] Greg Welch, Gary Bishop, “Introduction to the Kalman filter”, Siggraph 2001, Course 8

[10] 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

[11] Robert Fuler, “Neural Fuzzy System”, Abo Akedemi University,

1995

[12] T.T Hoang, P.M Duong, N.T.T.Van, D.A.Viet and T.Q Vinh,

“Development of an EKF-based Localization Algorithm Using Compass Sensor and LRF”, The 12th International Conference

on Control, Automation, Robotics and Vision, ICARCV, Guangzhou - China, 2012