Sage Journals: Discover world-class research

Abstract

Extended Kalman filter is well-known as a popular solution to the simultaneous localization and mapping problem for mobile robot platforms or vehicles. In this article, the development of a neuro-fuzzy-based adaptive extended Kalman filter technique is presented. The objective is to estimate the proper values of the R matrix at each step. We design an adaptive neuro-fuzzy extended Kalman filter to minimize the difference between the actual and theoretical covariance matrices of the innovation consequence. The parameters of the adaptive neuro-fuzzy extended Kalman filter is then trained offline using a particle swarm optimization technique. With this approach, the advantages of high-dimensional search space can be exploited and more effective training can be achieved. In the experiments, the mobile robot navigation with a number of landmarks under two benchmark situations is evaluated. The results have demonstrated that the proposed adaptive neuro-fuzzy extended Kalman filter technique provides the improvement in both performance efficiency and computational cost.

Keywords

Extended Kalman filter simultaneous localization and mapping neuro-fuzzy assistance partial swarm optimization

Introduction

The problem of simultaneous localization and mapping (SLAM) has been investigated for many decades, and it is still a challenging issue for mobile robotics research till now. An SLAM-based robot is to construct a mobile platform which is able to localize its position in the environment and build the environment map at the same time.¹ This idea has attracted many attentions to the researchers and developers of autonomous mobile robots.

In this work, we divide the SLAM problems into two categories: relative SLAM and absolute SLAM. In the relative SLAM, the robot utilizes the odometry data provided by the built-in encoders of the robot wheels to localize itself and construct the environment map. This approach is also known as dead reckoning,² and its main disadvantage is the inevitable error accumulation over time, which generally leads significant degradation of localization and mapping results. On the other hand, the absolute SLAM uses the robot’s external sensors to obtain the environment measurements as the inputs to the localization and mapping algorithms. It is also known as beacon or landmark-based SLAM.³ The major drawback of this approach is that its accuracy is significantly influenced by the observed features and the reliability of the sensor measurements. Inconsistent feature observations or noisy sensor readings can both lead to wrong inferences of the SLAM system.

To cope with these problems, this work investigates the integration of both the absolute and relative methods based on the extended Kalman filter (EKF).^4,5 The EKF is well-known as a method for multi-sensor fusion. It collaboratively combines the information from multiple sensors and provides more reliable and accurate measurement outputs. In this article, the data obtained from the odometer and the range-finder are fused through the EKF.

In the literature, some previous works using EKF for robot localization and mapping have been proposed.^6,7,8 However, designing effective EKF requires the priori knowledge of the process and measurement covariance matrix Q andR, respectively. In most application scenarios, these parameters are unknown and usually set as fixed values during execution. This fixed value setting usually results in poor estimations of the process and measurement noise and consequently leads to a significant degradation of EKF performance. The classical full EKF-based SLAM approach has another serious issue: The computational complexity becomes very high if the number of features is significant. Under this circumstance, both the total robot state vectors and covariance matrices become very large and difficult to handle. One effective solution to deal with this issue is to employ adaptive algorithms for EKF SLAM.^{9,10,11,12,13} Specifically, the studies of the literature^14,15 have shown that the approach of artificial intelligence-assisted EKF for the SLAM problems is far more superior than the conventional approaches (e.g. unscented filter, square root unscented filter) and others (e.g. particle filter). Their conclusions and results have also greatly motivated this work for a further investigation.

In this article, SLAM with neuro-fuzzy-assisted EKF technique is proposed. In particular, adaptive neuro-fuzzy is employed to derive the matrix R while maintaining the known and fixed Q.¹⁶ A stochastic and random search-based metaheuristic optimization procedure, particle swarm optimization (PSO),^17,18 is employed to learn the free parameters of the neuro-fuzzy model. The main contribution of this work is the design and implementation of an adaptive neuro-fuzzy EKF (ANFEKF) technique for SLAM systems. The ANFEKF algorithm improves the SLAM performance in larger scale environments where the conventional EKF approaches cannot provide satisfactory results.

There are many related studies on this topic available in the literature, and most of them are published recently or still ongoing project developments. For example, Ivanov et al. present a mobile robot path planning technique with continuous laser scanning. A technical vision system using the dynamic triangulation is adopted for 3-D coordinate measurement.¹⁹ So here comes the question: Why need another study on the topic of adaptive neuro-fuzzy-based mobile robot localization techniques? The reason of the present work is to offer an intuitive solution to the SLAM problem in terms of three layers. First, our method is mainly based on the optimal state estimation approach, the EKF.^12,20,21 Second, the artificial intelligence approach, or to be more specifically, adaptive neuro-fuzzy,^22,23 is applied to optimize the basic EKF performance. Finally, the heuristic search approach,²⁴ PSO, is employed to assist the adaptive neuro-fuzzy technique by improving the learning process. Although a few work have addressed some of the above issues, none of them offers all three layers of solutions presented in this article.

Related work

Multi-sensor fusion is a term referring to the synergistic combination of sensory data from multiple sensors to provide more reliable and accurate information.²⁵ In most applications, the sensor noise is inevitable and might cause incorrect system response and state estimation. Thus, multi-sensor fusion techniques are employed to integrate the sensory readings and result in more reliable information. In the past decades, Kalman filter is one of the most important methods for sensor fusion.

In principle, Kalman filter performs as a prediction–correction operator. Particularly, the state estimation is carried out by two main steps, the prediction and correction steps. The first (prediction) step is also known as an update step, a priori state estimation is calculated. In the second (correction) step, the measurements from sensor devices are taken into account to compute the posterior state and used for the current state estimation. While the normal Kalman filter is employed intentionally to handle linear problems, the EKF adopted in our approach is designed to deal with nonlinear models.

Artificial intelligence is one of the common approaches employed to optimize the Kalman filter, specifically the model of noise covariance. Recently, the use of fuzzy inference systems has increased significantly. The applications vary from consumer products such as washing machines, cameras, camcorders to industrial process control such as medical instrumentation and decision support systems. Generally, a fuzzy inference system is designed to map the input space to the output space, and the primary mechanism is using a list of if–then statements called rules. The rules work with the variables described by words rather than numbers.

In practical applications, fuzzy inference systems are often employed to build the fuzzy controller system. There are two typical designs for fuzzy controllers, Mamdani type and Sugeno type. While Mamdani type is popular due to its simple design and less experience requirement, the Sugeno type is preferred by experts because it is able to deal with more complex situations and cost less, even with the requirement of more experience in controller design. The design of fuzzy controller system can be divided into three main steps:

Fuzzification: In this step, the determination of discourse and fuziliers is accomplished.

Inference mechanism: This step includes two rule base and inference engine, which mutually interacts with each other. In other words, it is simply the process of documenting the rule set via a set of if–then expressions.

Defuzzification: This is a process to produce quantifiable results in a crisp inference system, which will then be implemented for controlling the system.

Neural networks belong to a class of artificial intelligence and are often used to solve classification or pattern recognition problem that cannot be handled by simple or straightforward algorithmic methods. Generally, neural network structures are inspired by biological neuron systems. The neuron networks can be trained to perform a particular function by adjusting the value of connection (weighting) between elements. The system that has ability of self-adjusting its structure according to the given information flowing through the network is called an adaptive system, and the self-adjusting ability is called learning.

The neural network is constructed from a number of layers, which are arranged by the interconnected neurons. Primarily, one typical structure often includes three layers. The first layer is called the input layer and the last layer is the output layer. The layer lies between the input and output layers is called a “hidden layer”. Once a network is well trained, it can perform complex functions such as pattern recognition and identification. Additionally, the learning ability of neural networks can integrate with other approaches to make the system adaptive and robust. In this work, the combination of fuzzy inference system and neural network is proposed.

As the advantages of fuzzy inference systems to deal with linguistic expression and neural networks with the self-learning ability, Jang combined these two techniques and proposed an adaptive neuro-fuzzy inference system (ANFIS).²⁶ The basic idea of the adaptive neural fuzzy inference system is to use a fuzzy system to represent the knowledge in an interpretable manner and has the learning ability derived from a neural network that can adjust the membership function (MF) parameters and linguistic rules directly from data to enhance the system performance. Different from the typical three-layered neural network, a typical ANFIS architecture is the model of Takagi Sugeno fuzzy inference system²⁷ which includes five layers.

For the ANFIS with one input x and one output f, there are only two rules used in the Takagi Sugeno model as follows:

Rule 1: If $x i s A_{1} a n d y i s B_{1}$ , then $f_{1} = p_{1} x + q_{1} x + r_{1}$ .

Rule 2: If $x i s A_{2} a n d y i s B_{2}$ , then $f_{2} = p_{2} x + q_{2} x + r_{2}$ .

where

A_{1}, A_{2}

and

B_{1}, B_{2}

are the MFs of the inputs x and y, while

p_{1}, q_{1}, r_{1}

and

p_{2}, q_{2}, r_{2}

are linear parameters of the Takagi Sugeno fuzzy inference model.

Approach

The proposed SLAM technique is the neuro-fuzzy-assisted EKF and adopted for wheeled robots. We first briefly introduce the robot kinematics and EKF-based SLAM, followed by our approach with neuro-fuzzy-assisted EKF for the SLAM problem.

Robot kinematics modeling and EKF

The wheeled mobile platform is a well-known class of vehicles and typical ground robots. A commonly used model for four-wheeled car-like robots can be represented by

(\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{ϕ} \end{matrix}) = f (X) = (\begin{matrix} (v + ζ_{v}) cos (ϕ + [γ + ζ_{γ}]) \\ (v + ζ_{v}) sin (ϕ + [γ + ζ_{γ}]) \\ \frac{(v + ζ_{v})}{B} sin (γ + ζ_{v}) \end{matrix})

where $(\dot{x} \dot{y} \dot{ϕ})$ is the dynamic model of the robot states with $(x, y)$ being the Cartesian coordinates and ϕ the robot orientation with respect to the global coordinate system, respectively. Let $u = (v, γ)$ be the control input consisting of two parameters, the velocity v and the steering angle γ. The covariance matrix of the process noise is given by $ζ = (ζ_{v}, ζ_{γ})$ with the noise variances $ζ_{v}$ and $ζ_{γ}$ of the control input $u = (v, γ)$ . B is the baseline of the vehicle or robot. Figure 1 illustrates more intuitive view of the concept of mobile robot kinematics.

Figure 1.

The localization of a mobile robot based on observed landmark features.

A mobile robot uses exteroceptive sensors to perceive the features of the real-world environment. In this work, the sonars are adopted as the exteroceptive sensors for the robot to provide the measurements of the range $r_{i}$ and the bearing $θ_{i}$ of an observed feature $(x_{i}, y_{i})$ . Mathematically, it can be written as

(\begin{matrix} r_{i} \\ θ_{i} \end{matrix}) = h (X) = (\begin{matrix} \sqrt{{(x - x_{i})}^{2} + {(y - y_{i})}^{2}} + ω_{r} \\ {tan}^{- 1} \frac{y - y_{i}}{x - x_{i}} - ϕ + ω_{θ} \end{matrix})

where the noise variances $ω_{r}$ and $ω_{θ}$ are with the range and bearing measurements, respectively, and $W = (ω_{r}, ω_{θ})$ is the covariance matrix of the noise observation.

Kalman filter was developed in 1960 to deal with the so-called linear-quadratic problem using a recursive solution.²⁸ It is a problem for the estimation of the current state of a linear dynamic system (or state of process) perturbed by white noise. To predict (or estimate) the current state of the process, Kalman filter works in a recursive way by employing the current measurement and the priori state of the process. Since Kalman filter works recursively until resulting in an optimal state estimation, it is considered as a very powerful tool in minimizing the state estimation error.

EKF is an extended version of Kalman filter which aims to deal with nonlinear models. The main difference between EKF and Kalman filter is the additional linearization model in the prediction step and the calculation of partial derivatives of the state variables. The process and measurement noises are assumed to be zero mean Gaussian distributions with independent error covariance in EKF. The disadvantage is that if the estimate process and measurement noises are not precisely modeled, the filter would diverge and lead to the system inconsistency. Practically, the well-defined models of error covariance are seldom achieved and the values are often manually set. It may take a lot of time with trial and error to derive the expected model. Therefore, the artificial intelligence approach is employed to assist EKF to model the system, especially for the noise covariance.

The general nonlinear system and measurement form can be described by

\begin{array}{l} x_{k + 1} = f (x_{k}, u_{k}) + w_{k} \\ z_{k} = h (x_{k}) + v_{k} \end{array}

where $x_{k}$ , $z_{k}$ , and $u_{k}$ are the $n \times 1$ process state vector, the $m \times 1$ measurement vector, and the control input, respectively. The random variables $w_{k}$ and $v_{k}$ represent the Gaussian white noise and the measurement noise, respectively. $P_{k}$ , $Q_{k}$ , and $R_{k}$ represent the covariance matrices for $x_{k}$ , $w_{k}$ , and $v_{k}$ , respectively.

There are two main steps in the EKF algorithm: time update (prediction) and measurement update (correction):

Prediction update: Calculate the projections of the state and the error covariance estimates from the sampling instants k to k + 1 given by

\begin{array}{l} {\hat{x}}_{k + 1}^{-} = f (x_{k}, u_{k}) & ​ \\ P_{k + 1}^{-} = F_{k} P_{k} F_{k}^{T} + G_{u} Q_{k} G_{u}^{T} & ​ \end{array}

where

F_{k} = \frac{\partial f}{\partial X} = (\begin{matrix} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial ϕ} \\ \frac{\partial f_{2}}{\partial x} & \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial ϕ} \\ \frac{\partial f_{3}}{\partial x} & \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial ϕ} \end{matrix})

and

G_{u} = \frac{\partial f}{\partial u} = (\begin{matrix} \frac{\partial f_{1}}{\partial u_{1}} & \frac{\partial f_{1}}{\partial u_{2}} \\ \frac{\partial f_{2}}{\partial u_{1}} & \frac{\partial f_{2}}{\partial u_{2}} \\ \frac{\partial f_{3}}{\partial u_{1}} & \frac{\partial f_{3}}{\partial u_{2}} \end{matrix})

Measurement update: When the measurement $z_{k}$ becomes available, EKF computes the Kalman gain matrix and incorporates with the measurement innovation to derive the estimated state $x_{k}$ , followed by the state error covariance matrix update. The general scheme for the measurement update step is given by

K_{k} = P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} + R_{k})}^{- 1}

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - h ({\hat{x}}_{k}^{-}))

P_{k} = (I - K_{k} H_{k}) P_{k}^{-}

where H is the linearization matrix (or Jacobian matrix) of the measurement and calculated by $H = \partial h / \partial X$ and ${\hat{x}}_{k}$ is an estimate of the state vector $x_{k}$ . The discrepancy between the predicted and observed measurements is commonly referred to as the measurement innovation or the residual and denoted by $r_{k} = z_{k} - h ({\hat{x}}_{k}^{-})$ . A major disadvantage of EKF is that if the estimated process and measurement noise are not precisely modeled, the filter would diverge and lead to a system inconsistence. Practically, well-defined error covariance models are seldom achieved, so the values are set manually. It may take a lot of time with trial and error before getting an expected model. Thus, in this article, the artificial intelligence approach is employed to assist the EKF to model the system, especially for the noise covariance.

Localization with EKF

In the conventional localization framework, it is assumed that the environment map is given, that is, the environment features are known to the robot in advance. The EKF estimates the robot position by integrating the information acquired from odometers, map, and exteroceptive sensors such as rangefinder or sonar. The algorithm usually consists of several steps: pose prediction, observation, measurement prediction, matching, and estimation.

Pose prediction: The robot pose at time k + 1 is predicted based on the previous position and orientation (at time k) and its movement with the control input $u_{k}$ and is given by

\begin{array}{l} {\hat{x}}_{k}^{-} = f (x_{k}, u_{k}) & ​ \\ P_{k + 1}^{-} = F_{k} P_{k} F_{k}^{T} + G_{u} Q_{k} G_{u}^{T} & ​ \end{array}

where $F_{k} = \partial f / \partial X$ and $G_{u} = \partial f / \partial u$ .

Actual observation: In the second step, the robot obtains the sensor measurements $z_{k + 1}$ at time k + 1.

Observation prediction: From the predicted robot pose ${\hat{x}}_{k + 1}^{-}$ and the currently available map, a predicted measurement ${\hat{z}}_{k}$ is generated by

{\hat{z}}_{k} = (\begin{matrix} \sqrt{{(\hat{x} - x_{i})}^{2} + {(\hat{y} - y_{i})}^{2}} \\ {tan}^{- 1} \frac{\hat{y} - y_{i}}{\hat{x} - x_{i}} - \hat{ϕ} \end{matrix})

The state error between the actual and predicted measurements is $v_{k} = z_{k} - {\hat{z}}_{k}$ , where $v_{k}$ is the innovation sequence (or residual) with the covariance $S_{k} = H_{k} P_{k}^{-} H_{k}^{T} + R_{k}$ , and $H_{k} = \partial h / \partial X$

Matching: In the matching procedure, an assignment is processed from measurements to the landmarks and then stored in the map.

Estimation: The estimation of the state is given by

\begin{array}{l} \hat{x} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - h ({\hat{x}}_{k}^{-})) \\ K_{k} = P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} + R_{k})}^{- 1} \\ P_{k} = (I - K_{k} H_{k}) P_{k}^{-} \end{array}

where $K_{k}$ is the Kalman gain and $P_{k}$ is the new state covariance matrix.

Neuro-fuzzy-assisted EKF for localization

Localization with ANFIS

There are several factors affecting the estimation of Kalman filter, and among them the noise covariance is the most important to be considered. These process and measurement noise covariance matrices are denoted by Q and R, respectively. Their initial values greatly affect the overall performance of the filter because they contribute to the Kalman gain directly. R and Q simply provide the system with more trust on the prediction or measurement equations.

Considering a larger Q is equivalent to considering a larger uncertainty in the state equations. That is, it trusts the equations less, which effectively indicates that the filter should correct more with the measurement update. Similarly, considering a larger R is equivalent to considering a larger uncertainty in the measurement. This is in terms equivalent to trusting the measurement less. Thus, the filter should correct less with the measurement update.

Figure 2 illustrates the performance of the filter for various R values. While the measurement noise covariance R is increased from 0.5 to 0.9, the measurement is trusted less so that the filter is affected less by the noisy measurement. Consequently, the filter converges to the actual position slower. If R is decreased from 0.5 to 0.1, the filter trusts the measurement more. It converges to the actual position faster but oscillates more frequently due to the noise. From the figure, it is easy to draw the conclusion that the performance of EKF highly depends on the predefined model. To derive the well-defined model for Q and R, trial and error is a common way, but it is often time-consuming and may not yet be optimal. Therefore, the artificial intelligence approaches are preferred to quickly find the optimal model for Q and R.

Figure 2.

The EKF performance for various R values (from top to bottom): (a) $R = 0.5$ , (b) $R = 0.1$ , and (c) $R = 0.9$ . EKF: extended Kalman filter.

To obtain a well-defined model for Q and R, a common approach is by trial and error. However, it is time-consuming and the result might not be the optimal model. This work proposes an innovation adaptive estimation (IAE) scheme for the EKF integrated with ANFIS to adjust the value of R (with Q fixed). ANFIS is able to identify the optimal model for R quickly and prevent the EKF from diverging.

Localization based on ANFIS EKF

Let $R_{k}$ and $Q_{k}$ represent the covariance matrices of the measurement and process noise, respectively. To make EKF adaptive, there are two different methods. One is to simultaneously adjust R and Q and the other is to assign one of these $Q / R$ as a fixed value and adjust the remaining. Thus, we fix Q and adjust R by innovation-based adaptive estimation algorithm to adapt the EKF. The basic idea of IAE is to let the true values of $C_{k}$ consistent with the theoretical values of $S_{k}$ . The theoretical covariance $S_{k}$ is derived from EKF, and the true covariance $C_{k}$ is approximated by averaging its sampling covariance $r_{k}$ within a sliding window of size N, that is,

{\hat{C}}_{k} = \frac{1}{N} \sum_{i = k - N + 1}^{k} (r_{i}^{Τ} r_{i})

where $i_{k}$ is the k-th sample in the estimation window.

In principle, the purpose of the IAE approach can be considered as the minimization of mismatches between $S_{k}$ and $C_{k}$ as much as possible. We denote the mismatch by degree of mismatch (DOM) and define it as

{DOM}_{k} = S_{k} - {\hat{C}}_{k}

To reduce DOM, the values of $S_{k}$ are adjusted by changing R. That is, the adaption of the element $(i, i)$ of R is assigned based on its correspondence $(i, i)$ of DOM by employing ANFIS. Here, the general rules used for the adaption of R are given as follows:

If ${DOM}_{k} (i, i) = 0$ , then maintain $R_{k}$ unchanged.

If ${DOM}_{k} (i, i) > 0$ , then decrease $R_{k}$ .

If ${DOM}_{k} (i, i) < 0$ , then increase $R_{k}$ .

The ANFIS architecture

The ANFIS model in this article is designed as single input and single output. The input to ANFIS is ${DOM}_{k}$ and the output is the adjustment factor $Δ R$ . The covariance matrix R is updated by

R_{k} = Δ R_{k} R_{0}

where $Δ R_{k}$ is the ANFIS output and $R_{0}$ is the initial noise constant matrix. Since ${DOM}_{k}$ and $R_{k}$ are both $2 \times 2$ matrices, and the two diagonal elements of these matrices correspond to the error variances of the range and bearing of the measurements.

The ANFIS model used to adjust $Δ R_{k}$ is constructed as a five-layer network architecture with one input and one output as shown in Figure 3. Let $u_{i}^{l}$ and $o_{i}^{l}$ denote the input and the output of the i-th node in the l-th layer, respectively. For each layer, the function is presented below.

Figure 3.

The five-layered network of the proposed ANFIS structure. ANFIS: adaptive neuro-fuzzy inference system.

Layer 1: The input layer. In the input layer, the node directly passes the input value to the following layer. The input–output relation is given by

O^{1} = u^{1} = {DOM}_{k}

Layer 2: The input MF layer. In this layer, the input variables are fuzzified by employing five MFs, large negative, negative, zero, positive, and large positive. The output of i-th MF is given by

o_{i j}^{2} = μ_{i j} (u^{2}) = exp {\frac{- (u_{i j}^{2} - m_{i j}^{2})}{δ_{i j}^{2}}}

where $m_{i j}$ and $δ_{i j}$ are the mean and width of the Gaussian MFs, the subscript ij indicates the j-th term of the i-th element. There are two free parameters in each node, which are adjustable.

Layer 3: The rule layer. The nodes in this layer are rule nodes. They perform a fuzzy AND operation (or product inference) for the calculation of the firing strength by

o_{l}^{3} = \prod_{i} u_{i}^{3}

Layer 4: The normalization layer. The node in this layer performs the normalization of firing strength from layer 3 and is given by

o_{l}^{4} = \frac{u_{l}^{4}}{\sum_{l = 1}^{n} u_{l}^{4}}

Layer 5: The output layer. The output layer performs the scaling for the defuzzified output by the relation

Δ R_{i} = \sum_{l = 1}^{N} u_{l}^{5} w_{l}

Training the neuro-fuzzy model with PSO

The objective of the training algorithm is to adjust the free parameters of the network so as to minimize the cost function. In this work, the cost function is given by

E = \frac{1}{2} e_{k}^{2}

where $e_{k} = S_{k} - {\hat{C}}_{k} = {DOM}_{k}$ .

There exist many ways to train our ANFIS system. The most conventional approach is through the backward propagation. However, it is easily stuck in local optima and thus is not suitable for highly nonlinear models. To deal with this problem, the PSO algorithm is adopted for more effective training. Using the PSO training algorithm, the weighting factors and the network are adjusted so that the value of the cost function given previously is less than a desired threshold after a number of iterations for training.

One input is used for the ANFIS system. We define the degree of mismatch as DOM and derive one output $Δ R$ as the adjustable factor of the measurement covariance matrix R. Using five MFs for each input gives us 10 parameters since each function is associated with two parameters. There are five output rules used, and each outputs a singleton (using the Sugeno model), so there are a total of 15 parameters to be optimized. In other words, there are one input neuron, five hidden layer neurons, and one output neuron in our network. PSO employs the configuration of the 15-dimensional particles, including the scale factors for antecedent parameters mN, δN, δZ, mP, mLP, δP, δLN, mZ, and δLP and the scale factors for consequent parameters W1–W5. During the training time, both the parameters of antecedent and consequent are adjusted at the same time by the corresponding scale factor.

The steps of the PSO learning algorithm are given as follows:

Initialization: A random vector in the search space is used to initialize a particle.

Updating fitness position: The velocity and the position of a particle is adjusted based on the best local and global fitness location.

Convergence: The above steps repeat iteratively till the stopping condition is achieved.

Figures 4 and 5 depict the influence of the learning algorithm to the ANFIS structure. Figure 4 shows the initial ANFIS input–output structure with its corresponding parameters as tabulated in Table 1. A Gaussian distribution is used for the input MF type, and the output type is Singleton. Likewise, Figure 5 and Table 2 show the trained ANFIS input–output structure as well as the corresponding parameters.

Figure 4.

The initial ANFIS input–output (left and right) structure with the parameters shown in Table 1. ANFIS: adaptive neuro-fuzzy inference system.

Figure 5.

The trained ANFIS input–output (left and right) structure with the parameters shown in Table 2. ANFIS: adaptive neuro-fuzzy inference system.

Table 1.

The membership functions of initial ANFIS for the input (DOM) and the weight for the output ( $Δ R$ ).

Parameter		LN	N	Z	P	LP
Input	Center	−1	−0.5	0	0.5	1
Input	Width	0.2123	9.2123	0.2123	0.2123	0.2123
Output	Weight	1	1	1	1	1

ANFIS: adaptive neuro-fuzzy inference system; LN: large negative; N: negative; Z: zero; P: positive; LP: large positive.

Table 2.

The membership functions of trained ANFIS for the input (DOM) and the weight for the output ( $Δ R$ ).

Parameter		LN	N	Z	P	LP
Input	Center	−0.976	−0.373	−0.7647	0.752	0.878
Input	Width	0.6649	0.27	0.126	0.5016	0.2087
Output	Weight	0.95	0.4	8	1.3	0.77

ANFIS: adaptive neuro-fuzzy inference system; LN: large negative; N: negative; Z: zero; P: positive; LP: large positive.

Simulation and implementation

The proposed technique is evaluated with simulation and implemented for real experiments.

Experimental results

In the simulation, we evaluate our algorithm by performing two experiments with different number of landmarks, the first one with 10 landmarks and the second one with 30 landmarks. The initial robot position is set at $(x, y, θ) = (190, 25, 0)$ , and the moving speed of robot is set as 1 m/s with a fixed steering angle of 0.5°. The robot is equipped with a range–bearing sensor with the maximum range of 100 m and 60° of frontal field-of-view. To simulate the real implementation, the control and measurement noise is added using a random distribution with zero mean and 1 m variance.

We compare the results from the conventional EKF-based SLAM and the ANFEKF. In the conventional EKF-based SLAM, the matrices $Q_{k}$ and $R_{k}$ are fixed throughout the experiments, while in the ANFEKF-based SLAM, $Q_{k}$ is constant but $R_{k}$ is adjusted by the ANFIS system. Figures 6 and 7 show the robot’s motion trajectories and landmark positions. The blue crosses represent the observed landmarks, while the red crosses signify all of the known landmarks in the environment. The red dash curves represent the estimated robot trajectories over time, while the black contours denote the ground truth motion trajectories.

Figure 6.

Ten landmarks are placed in the environment. Left: the conventional EKF-based SLAM. Right: ANEKF-based SLAM.

Figure 7.

Thirty landmarks are placed in the environment. Left: the conventional EKF-based SLAM. Right: ANEKF-based SLAM.

The errors of the robot state estimates for both approaches are shown in Figures 8 and 9. It can be seen that the estimation error of AFEKF is approximately zero for 30 landmarks, and the error of the conventional method is much higher. This clearly demonstrates that ANFEKF is more accurate and outperforms the conventional EKF-based SLAM. Figure 10 shows the results from the adjustable scaling factor nR at each sample state in the simulation. The figure indicates that the value of the scaling factor nR varies considerably from 0 to 3. With this adjustment of the scaling factor, the error covariance will always keep close to the actual error to prevent the divergence of the EKF, and thus keep the EKF performance consistently.

Figure 8.

The estimation error of the robot state in the environment with 10 landmarks. (a), (b) and (c) are x, y and angle versus time, respectively.

Figure 9.

The estimation error of the robot state in the environment with 30 landmarks. (a), (b) and (c) are x, y and angle versus time, respectively.

Figure 10.

The results of the scaling factors nR in the environment of 10 landmarks (left) and 30 landmarks (right).

Implementation and real experiments

In the real experiment, the implementation on an Aria P3-DX robot is carried out to demonstrate the AFEKF-based SLAM approach. The odometry information is provided by the measurement from wheel encoders. Since it commonly accumulates errors for long distance travel, the external sensors are required for more accurate and reliable motion estimation. One way to reduce the angular position error is to adopt a laser range scanner.²⁹ In our approach, on-board sensors are employed for external sensing, and the sensor readings are fused with the odometric information acquired from the internal sensor to derive the robot position estimates. Figure 11 illustrates an ARIA mobile robot platform and the placement of sonar sensors. There are 16 sonars installed in different directions, with the maximum distance reading of about 4–5 m.

Figure 11.

The Aria mobile robot used in the experiments and the placement of sonar sensors.

The environment for the experimental setup is as shown in Figure 12. The mobile robot starts from the left corner (as indicated by a blue rectangle in the figure) and moves to its destination at the right side (another blue rectangle) while localizing itself simultaneously and building the environment map. There are three landmarks (indicated by red circles) shown in the figure. The mobile robot utilizes the odometric information from the encoders to localize its position and integrate with the detected landmarks to relocate its position more accurately.

Figure 12.

The real-world environment for the experiments.

Figure 13 shows the performance of both the conventional EKF-based SLAM and our ANFEKF-based SLAM using three landmarks in the real environment. The errors of the robot motion in $x, y, θ$ are shown in Figure 14. It can be seen clearly in the figure that the results from the ANFEKF-based approach is more consistent than the results from the conventional EKF-based method. In addition, the adjustable results of scaling factor nR at each sample state are shown in Figure 15. The complete map when the mobile robot reaches the target is shown in Figure 16. The black dash curve denotes the robot position throughout the run time. The black circles indicate the observed landmarks, while the red dots represent the environment map. As shown in the experimental results, the effectiveness of the proposed technique is validated with the robot navigation map.

Figure 13.

Left: conventional EKF-based SLAM. Right: ANEKF-based SLAM with three landmarks in the real environment.

Figure 14.

The estimation error of the robot state in the real-world environment with three landmarks. (a), (b) and (c) are x, y and angle versus time, respectively.

Figure 15.

The results of the scaling factors nR in the real-world environment with three landmarks.

Figure 16.

The complete map build by the mobile robot in the real-world environment.

Conclusion

This work proposed an ANFIS for SLAM based on EKF. In a conventional EKF technique, it is assumed that the priori knowledge is well-known. However, this assumption is usually biased and a significant degradation of the performance might occur in practical applications. By adapting the R matrix, the proposed ANFEKF is able to deal with the incorrect knowledge issue. Compared with the conventional approaches, it has the main advantage of consistency. The experiments with simulation and real implementation on a mobile robot are carried out to evaluate the performance of the proposed technique. The localization results have demonstrated that our ANFEKF performs more accurately than the conventional EKF.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the Ministry of Science and Technology of Taiwan under grant MOST 106-2221-E-194-004 and the Advanced Institute of Manufacturing with High-tech Innovations (AIM-HI) from The Featured Areas Research Center Program within the framework of the Higher Education Sprout Project by the Ministry of Education (MOE) in Taiwan is gratefully acknowledged.

ORCID iD

Huei-Yung Lin

References

Neira

Davison

Leonard

. Guest editorial special issue on visual SLAM. IEEE Trans on Robot 2008; 24(5): 929–931.

Ojeda

Borenstein

. Personal dead-reckoning system for GPS-denied environments. In: IEEE international workshop on safety, security and rescue robotics, 2007. SSRR 2007., Rome, Italy, 27–29 September 2007, pp. 1–6. IEEE.

Betke

Gurvits

. Mobile robot localization using landmarks. IEEE Trans Robot Autom 1997; 13(2): 251–263.

Moore

Stouch

. A generalized extended Kalman filter implementation for the robot operating system. In: Menegatti

Michael

Berns

Yamaguchi

(eds) Intelligent autonomous systems 13. Cham: Springer International Publishing, 2016. ISBN 978-3-319-08338-4, pp. 335–348.

Huang

Dissanayake

. Convergence and consistency analysis for extended Kalman filter based SLAM. IEEE Trans Robot 2007; 23(5): 1036–1049.

Jetto

Longhi

Vitali

. Localization of a wheeled mobile robot by sensor data fusion based on a fuzzy logic adapted Kalman filter. Control Eng Pract 1999; 7(6): 763–771.

Sasiadek

Wang

Zeremba

. Fuzzy adaptive Kalman filtering for INS/GPS data fusion. In: Proceedings of the 2000 IEEE International Symposium on Intelligent Control, 2000, Rio Patras, Greece, 19 July 2000, pp. 181–186. IEEE.

Chatterjee

Matsuno

. Improving EKF-based solutions for SLAM problems in mobile robots employing neuro-fuzzy supervision. In: 2006 third international IEEE conference on intelligent systems, London, UK, 4–6 September 2006, pp. 683–689. IEEE.

Havangi

Nekoui

Teshnehlab

. Adaptive neuro-fuzzy extended Kalman filtering for robot localization. In: Proceedings of 14th international power electronics and motion control conference EPE-PEMC 2010, Ohrid, Macedonia, 6–8 September 2010, pp. T5–T130. IEEE.

10.

Jiang

. A novel SLAM algorithm with adaptive Kalman filter. In: International conference on advanced robotics and mechatronics (ICARM), >Macau, China, 18–20 August 2016, pp. 107–111. IEEE.

11.

Jwo

Chang

. Application of optimization technique for GPS navigation Kalman filter adaptation. In: Huang

Wunsch

Levine

. (eds) Advanced intelligent computing theories and applications. With aspects of theoretical and methodological issues, 2008, pp. 227–234. Berlin, Heidelberg: Springer.

12.

Mehra

. On the identification of variances and adaptive Kalman filtering. IEEE Trans Automat Control 1970; 15(2): 175–184.

13.

Chatterjee

Matsuno

. A neuro-fuzzy assisted extended Kalman filter-based approach for simultaneous localization and mapping (SLAM) problems. IEEE Trans Fuzzy Syst 2007; 15(5): 984–997.

14.

Ankışhan

Efe

. Adaptive neuro-fuzzy supported Kalman filter approach for simultaneous localization and mapping. In: 2011 IEEE 19th signal processing and communications applications conference (SIU), Antalya, Turkey, 20–22 April 2011, pp. 266–270. IEEE.

15.

Havangi

Nekoui

Taghirad

, et al. SLAM based on intelligent unscented Kalman filter. In: 2011 second international conference on control, instrumentation and automation (ICCIA), Shiraz, Iran, 27–29 December 2011, pp. 877–882. IEEE.

16.

Pothal

Parhi

. Navigation of multiple mobile robots in a highly clutter terrains using adaptive neuro-fuzzy inference system. Robot Auton Syst 2015; 72: 48–58.

17.

Gong

Zhou

, et al. Genetic learning particle swarm optimization. IEEE Trans Cybern 2016; 46(10): 2277–2290.

18.

Eberhart

Shi

. Particle swarm optimization: developments, applications and resources. In: Proceedings of the 2001 Congress on Evolutionary Computation (IEEE Cat. No.01TH8546), vol. 1, Seoul, South Korea, 27–30 May 2001, pp. 81–86. IEEE. DOI: 10.1109/CEC.2001.934374.

19.

Ivanov

Lindner

Sergiyenko

, et al. Mobile robot path planning using continuous laser scanning. In: Rivas-Lopez

Sergiyenko

Flores-Fuentes

Rodríguez-Quiñonez

(eds) Optoelectronics in Machine Vision-Based Theories and Applications. Pennsylvania: IGI Global, 2019, pp. 338–372.

20.

Fitzgerald

. Divergence of the Kalman filter. IEEE Trans Autom Control 1971; 16(6): 736–747.

21.

Grewal

Andrews

. Kalman filtering. Theory and practice. Prentice Hall: Englewood Cliffs, 1993.

22.

Cong

Kim

Lim

. Hybrid extended Kalman filter-based localization with a highly accurate odometry model of a mobile robot. In: International conference on control, automation and systems, 2008. ICCAS 2008, Seoul, South Korea, 14–17 October 2008, pp. 738–743. IEEE.

23.

Koprinkova-Hristova

Alexiev

. Neuro-fuzzy tuning of Kalman filter. In: 2016 IEEE eighth international conference on intelligent systems (IS), Sofia, Bulgaria, 4–6 September 2016, pp. 651–657. IEEE.

24.

Chatterjee

Matsuno

. A Geese PSO tuned fuzzy supervisor for EKF based solutions of simultaneous localization and mapping (SLAM) problems in mobile robots. Expert Syst Appl 2010; 37(8): 5542–5548.

25.

Sun

Deng

. Multi-sensor optimal information fusion Kalman filter. Automatica 2004; 40(6): 1017–1023.

26.

Jang

. ANFIS: adaptive-network-based fuzzy inference system. IEEE Trans Syst Man Cybern B Cybern 1993; 23(3): 665–685.

27.

Bergsten

Palm

Driankov

. Observers for Takagi-Sugeno fuzzy systems. IEEE Trans Syst Man Cybern Part B Cybern 2002; 32(1): 114–121.

28.

Chui

Chen

. Kalman filtering. Berlin: Springer, 2017.

29.

Reyes-Garcia

Lindner

Rivas-López

, et al. Reduction of angular position error of a machine vision system using the digital controller lm629. In: IECON 2018 - 44th Annual Conference of the IEEE Industrial Electronics Society, Washington, DC, 21–23 October 2018, pp. 3200–3205. IEEE. DOI: 10.1109/IECON.2018.8592803.

Incorporating neuro-fuzzy with extended Kalman filter for simultaneous localization and mapping

Abstract

Keywords

Introduction

Related work

Approach

Robot kinematics modeling and EKF

Localization with EKF

Neuro-fuzzy-assisted EKF for localization

Localization with ANFIS

Localization based on ANFIS EKF

The ANFIS architecture

Training the neuro-fuzzy model with PSO

Simulation and implementation

Experimental results

Implementation and real experiments

Conclusion

Footnotes

Declaration of conflicting interests

Funding

ORCID iD

References