2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia 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 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, differentialdrive 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 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 978-1-4673-3143-2/12/$31.00 ©2012 IEEE 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 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 416 2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia 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: Prediction step with time update equations xˆ i− = f (xˆ i −1, ui −1, 0) (7) − T T (8) i = Ai Pi −1Ai + Wi Qi −1Wi where xˆ i− : the priori state estimate at step i given knowledge of the process priori to step i-1 − P : the covariance matrix of the state prediction error i P Figure 1: The pose and parameters of mobile robot The conversion factor that translates encoder pulses into linear wheel displacement is πR Cm = (1) nCe 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 NL,i and NR,i respectively The displacement of each wheel is then calculated as: ΔS L,i = Cm N L,i ΔS R,i = Cm N R,i (2) i : 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 Correction step with measurement update equations − T − T T −1 K = P H (H P H + V R V ) (9) i i i i i i i i i xˆ i = xˆ i− + K i ( z i − h( xˆ i− , 0)) (10) − P = (I − K H ) P (11) i i i i where xˆ i : the posteriori state estimate at step i given P These can be translated to the linear incremental displacement of the robot’s center ∆S and orientation angle ∆θ ΔS L,i + ΔS R,i ΔS R,i − ΔS L,i ΔSi = Δθi = (3) , L 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: xi = x + ΔSi * cos(θ + Δθi ) i −1 i −1 (4) yi = y + ΔSi *sin(θ + Δθi ) i −1 i −1 θi = θi −1 + Δθi 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 T Let x = ⎡⎣ xi yi θi ⎤⎦ , x ∈ ℜn be the state variable describing the instantaneous position and direction of the T ΔS R,i ⎤ be the input The non-linear robot, u = ⎡ΔS L,i ⎣ ⎦ stochastic different equation of the robot’s kinematic model is described as: x = f (x (5) i i −1, ui −1, w i −1 ) and the measurement equation is written as: z = h( x , v ) i i i measurement zi K : the Kalman gain i 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 (6) 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 where the random variables wi ∼ N (0, Qi ) and vi ∼ N (0, R i ) present the process and measurement noises They are 417 2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia membership functions of the antecedent and consequent parts of fuzzy if-then rules - Δ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 If Di is Z then ΔRi is M A ‘On-line’ adjustment of covariance matrices Q and R The residual ri = z i − 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, K i ( z i − h( xˆ i− , 0)) The EKF uses ri as a correction coefficient to correct the priori estimate xˆ i− 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 S =H P H +R and the actual covariance i i i i i i ˆ = ∑ r r T where N is the size of moving estimation C i N j= j j j If Di is P then ΔRi is D If Di is 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 sigmoid ( x) = − ( x − ci )2 gauss ( x) = e 2σ i (13) 1+ e 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 − ( x − bi ) o window, the difference between the theoretical covariance and ˆ the actual covariance is then defined by Di = Si − C i The fuzzy inference system with the input Di and the output ΔRi defines three general rules as: ˆ perfectly If Di ≅ ( implying that Si and C i matched) then maintain ΔRi ˆ ) then If Di > ( implying that Si is greater than C i decrease ΔRi 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 = N( x), α = Z( x), α = P( x) Layer 3: The normalization of the firing levels is indicated The normalized firing level of the corresponding rule is ˆ ) then If Di < (implying that Si is smaller C i increase ΔRi Each element in the main diagonal of R is then adjusted as R i ( j , j ) = R i −1 ( j , j ) + ΔRi (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: T T S = H (A P A + Q ) H + R (12) i i i i i i i i β = α3 α1 α2 , β2 = , β3 = (14) α1 + α + α α1 + α + α α1 + α + α 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 β z1 = β I −1 (α1 ), β z2 = β M −1 (α ), β z3 = β3 D −1 (α ) (15) Layer 5: The overall output system is the sum of all incoming signals z = β1 z1 + β z2 + β z3 (16) With given the crisp training set {( D1 , ΔR1 ), ( D2 , ΔR2 ) ( DK , ΔRK )} , we define the measure 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) of error for the k-th training pattern as E k = ( zk − ΔRk ) , k = K 418 (17) 2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia where zk is the computed output form of the fuzzy system corresponding to the input pattern Dk and the desired output ΔRk 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 i (t +1) = a i (t) -η k ∂ai ci (t +1) = ci (t) -η T heory T rue Measurement 1.8 1.6 X(m) ∂E bi (t +1) = bi (t) - η k ∂bi 2.2 (18) 1.4 1.2 ∂E k ∂ci 0.8 ∂E σ i (t +1) = σ i (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 100 120 140 160 T ime(100ms) 180 200 Figure 3: The theory, the true and the measurement path 2.2 T rue 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 = 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 ∆Si and orientation ∆θi at step i The input noise covariance matrix Q is defined ⎡σ ⎤ ⎢ ΔS ⎥ as Q = ⎢ ⎥ In case the deviation of the σ ⎢⎣ Δθ ⎥⎦ EKF FNN-EKF X(m) 1.8 1.6 1.4 1.2 0.8 100 120 140 160 180 200 T ime(100ms) Figure 4: Comparison between the EKF, the FNN-EKF and the true path displacement of the robot’s center ∆Si 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.01 ⎢ Q=⎢ R= 0.01 ⎥ ⎥ ⎢ ⎥ 0.0076 ⎣ ⎦ ⎢⎣ 0 0.018⎥⎦ N Z 0.8 P 0.6 0.4 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 FNNEKF 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.2 -0.5 0.5 Figure 5: The initial membership functions of Di 419 2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia 0.06 D 0.04 M 0.8 I Deviation X(m) 0.02 0.6 0.4 -0.02 -0.04 -0.06 0.2 EKF FNN-EKF -0.08 -0.2 0.2 0.4 -0.1 100 0.6 Figure 6: The initial membership functions of ∆Ri N 180 200 0.2 Z EKF P 0.15 0.6 FNN-EKF Deviation Y(m) 0.1 0.4 0.2 0.05 -0.05 -0.1 -0.5 0.5 -0.15 100 Figure 7: The learned membership functions of Di M 0.8 120 140 160 T ime(100ms) 180 200 Figure 10: The deviation between the estimated and the true path in Y direction D 0.3 I 0.2 Deviation (radian) 0.6 0.4 0.2 -0.2 140 160 T ime(100ms) Figure 9: The deviation between the estimated and the true path in X direction 0.8 120 0.2 0.4 0.6 0.1 -0.1 -0.2 EKF -0.3 FNN-EKF Figure 8: The learned membership functions of ∆Ri -0.4 100 140 160 180 200 T ime(100ms) Figure 11: The deviation between the estimated and the true path in orientation 420 120 2012 IEEE International Conference on Control System, Computing and Engineering, 23 - 25 Nov 2012, Penang, Malaysia 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 0.2 Initial FNN-EKF Deviation (radian) 0.15 FNN-EKF REFERENCES 0.1 [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 13221328 [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 [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 0.05 -0.05 -0.1 -0.15 100 120 140 160 T ime(100ms) 180 200 Figure 12: Comparison between the initial FNN-EKF and the tuned FNNEKF The figures and are the initial shape of the membership functions of the FNN They are chosen suitably to the altering limitation of Di and ΔRi With given crisp training set {( D1 , ΔR1 ), ( D2 , ΔR2 ) ( DK , ΔRK )} , 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 and The comparative result of paths estimated by the EKF and FNN-EKF is shown in the figure 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 421 ... 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... 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. .. 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 ,