A Novel FastSLAM Framework Based on 2D Lidar for Autonomous Mobile Robot

Abstract

The autonomous navigation and environment exploration of mobile robots are carried out on the premise of the ability of environment sensing. Simultaneous localisation and mapping (SLAM) is the key algorithm in perceiving and mapping an environment in real time. FastSLAM has played an increasingly significant role in the SLAM problem. In order to enhance the performance of FastSLAM, a novel framework called IFastSLAM is proposed, based on particle swarm optimisation (PSO). In this framework, an adaptive resampling strategy is proposed that uses the genetic algorithm to increase the diversity of particles, and the principles of fractional differential theory and chaotic optimisation are combined into the algorithm to improve the conventional PSO approach. We observe that the fractional differential approach speeds up the iteration of the algorithm and chaotic optimisation prevents premature convergence. A new idea of a virtual particle is put forward as the global optimisation target for the improved PSO scheme. This approach is more accurate in terms of determining the optimisation target based on the geometric position of the particle, compared to an approach based on the maximum weight value of the particle. The proposed IFastSLAM method is compared with conventional FastSLAM, PSO-FastSLAM, and an adaptive generic FastSLAM algorithm (AGA-FastSLAM). The superiority of IFastSLAM is verified by simulations, experiments with a real-world dataset, and field experiments.

1. Introduction

Owing to the rapid development of autonomous mobile robots, simultaneous localisation and mapping (SLAM) [1] has emerged as a crucial technology in a great many applications, such as self-driving, exploration, and navigation. SLAM can help a robot acquire information on the surrounding environment through a self-positioning process in an unknown environment, and gradually builds an incremental map. The robot performs the SLAM process iteratively in order to eliminate uncertain factors and to estimate the position accurately. Due to the high accuracy and high frequency of lidar, it is very suitable for observation of complex environments. SLAM based on lidar is therefore widely used in practical robot applications. Lidar-based SLAM technologies include 2D lidar SLAM and 3D lidar SLAM. The 3D lidar can identify more details of the environmental space and is suitable for high-precision application scenarios, such as autonomous vehicles, but its application is expensive. The 2D lidar has high measurement accuracy in the two-dimensional plane, which can meet the navigation and positioning requirements of mobile robots. Compared with 3D Lidar, it has lower cost and has the possibility of wide application.

At present, the most widely used SLAM frameworks based on 2D lidar include the approach based on extended Kalman filter (EKF-SLAM) [2,3] and that based on Rao-Blackwellised particle filter (RBPF-SLAM, also known as FastSLAM) [4]. EKF-SLAM ignores the higher-order terms of the Taylor expansion when performing state estimation, resulting in a large uncertainty in the state estimation of the robot. As part of the flow of the algorithm, the covariance matrix must be continuously calculated, which creates increasing computational complexity as the maps expand. In contrast, FastSLAM is a SLAM framework based on a particle filter, which was inspired by early probabilistic experiments by Murphy [5] and Thrun [6]. The particle filter in FastSLAM takes advantage of Monte Carlo sampling technology, which can properly estimate non-linear systems with better state accuracy.

In this paper we focus on the FastSLAM based on 2D lidar. The main drawback of FastSLAM is the particle degeneracy problem, which arises due to insufficient particle diversity during resampling [7]. To overcome the issue of particle degeneracy, researchers have proposed many optimisation algorithms to improve the resampling process in FastSLAM. Cugliari [8] introduced an unscented Kalman filter into FastSLAM and proposed a resampling strategy based on adaptive selection. Liu [9] used an adaptive fading extended Kalman filter to compute the proposal distribution in FastSLAM, and showed that the distribution was closer to the posterior position of the mobile robot and that the degree of particle degradation was reduced. Zhang [10] used a nonlinear adaptive square-root unscented Kalman filter to replace the particle filter in FastSLAM to solve the problem of particle degradation, and found that the accuracy and reliability of the algorithm were improved. While these algorithms have made some progress compared to conventional FastSLAM, they fail to make effective use of the information carried by all particles, and do not use collaborative optimisation methods to solve the issue of particle degeneracy [11].

The genetic algorithm (GA) [12] is a commonly used method of ensuring the diversity of particles before the resampling step. Conventional resampling algorithms suffer from particle degeneracy problems, since higher-weight particles are repeatedly selected and lower-weight particles are deleted after resampling. As a result, the remaining particles cannot closely approximate the true state of the robot [13]. Due to the crossover and mutation steps used, the GA can improve the particle degeneracy problem to a certain extent. Tai-Zhi [14] proposed a new FastSLAM algorithm based on revised genetic resampling and a square-root unscented particle filter, in which a fast Metropolis–Hastings algorithm was used as the mutation operator and was combined with traditional crossover to form a new resampling method. Khairuddin [15] integrated the GA and PSO into FastSLAM, and overcame the particle depletion problem by improving the accuracy of FastSLAM in terms of robot and landmark set position estimation. However, in this approach, the scalar parameters of GA are undetermined, and need to be tuned empirically. To solve these problems, we propose an adaptive genetic resampling algorithm in which the crossover and mutation coefficients are determined adaptively based on the distribution of particle weights. The new resampling algorithm is called AGA-Resampling.

PSO [16] is a group algorithm that optimises a problem by iteratively updating the particle set using local and global optimal values. Global optimal solution can be obtained with high probability. It has been proved that the PSO algorithm can effectively improve positioning accuracy in the problem of positioning of mobile robots [17]. Tdor [18] introduced the PSO algorithm into SLAM for the first time, while Havangi [19] used PSO to optimise the proposal distribution. PSO makes the particles aggregate at locations with high posterior probability before the weights are updated, and the impoverishment of particles can be prevent. Ye [20] proposed a random weight PSO strategy to improve the FastSLAM algorithm, to avoid particle degeneracy and maintain particle diversity. Seung [21] proposed a relational FastSLAM approach that integrated the PSO algorithm into the calculation of the weights and formation of the particles. Zuo [22] proposed a new FastSLAM method based on quantum-behaved PSO to improve the proposal distribution of particles and optimise the estimated particles. Due to the collaborative optimisation of PSO, the accuracy of SLAM was significantly improved. However, there still are some problems in with FastSLAM based on the PSO algorithm: The most typical of these is that the algorithm can easily fall into a local optimum, and another is the low calculation speed. To further enhance the algorithm, we propose an improved PSO algorithm FCPSO, in which the fractional differential is employed to speed up the evolution of the PSO algorithm and chaotic optimisation is utilised to address the problem of premature convergence.

After particle filtering process in FastSLAM is complete, the map and the robot’s path are stored in separate particles, each of which stores different information. The robot autonomously chooses one particle from the particle set to represent the results of FastSLAM. In most cases, the particle that has the highest importance weight is selected to represent the results [23]; however, the particle with the highest weight does not always produce the best result, and this cannot be guaranteed. Nosan [24] has proved that representation by means of particles provides a better result with a higher probability than when the particle with the highest weight is used alone. Hence, we define a virtual particle based on the mean of the particles, and the global optimisation value of the FCPSO algorithm is calculated using the position of this virtual particle. The improved FCPSO algorithm is then introduced into FastSLAM based on AGA-Resampling, which not only maintains the diversity of the particles but also alleviates the particle degeneracy problem in FastSLAM. We call this improved FastSLAM method IFastSLAM.

The remainder of this paper is organised as follows. In Section 2, we describe the standard FastSLAM algorithm, and the proposed improved algorithm is introduced. In Section 3, we discuss simulation experiments and field experiments. Finally, a summary and future prospects are given in Section 4.

2. Methodology

2.1. FastSLAM Algorithm Based on AGA-Resampling

SLAM is the process by which the robot draws the environment map and estimates its position [1]. A schematic diagram of this approach is shown in Figure 1. The mobile robot uses LIDAR to observe unknown landmarks by moving within the environment. The variables described below are defined at time t:

• $x_t$: state vector that represents the location and orientation of the robot
• $u_t$: The control input that drives the robot from state $x_{t-1}$ to state $x_t$
• $m_t$: A vector describing the position of the ith landmark
• $z_t$: The observation of the ith landmark from the robot

Besides, we define the following collections:

• $x_{1:t}=\left\{x_1,x_2,\dots ,x_t\right\}=\left\{x_{1:t-1},x_t\right\}$: The trajectory of the robot
• $u_{1:t}=\left\{u_1,u_2,\dots ,u_t\right\}=\left\{u_{1:t},u_t\right\}$: The control input of the robot
• $m=\left\{m_1,m_2,\dots ,m_n\right\}$: The collection of landmark
• $z_{1:t}=\left\{z_1,z_2,\dots ,z_t\right\}$: The collection of landmarks’ observation
• $n_{1:t}=\left\{n_1,n_2,\dots ,n_t\right\}$: The correspondence variable of landmarks

The core idea of the FastSLAM algorithm is to calculate the posterior probability of the robot trajectory and map by factorisation, as shown in

$$p(x_{1:t},m|z_{1:t},u_{1:t},n_{1:t})=p\left(x_{1:t}\right|z_{1:t},u_{1:t},n_{1:t})\prod _{n=1}^{N}p\left(m_n\right|x_{1:t},z_{1:t},n_{1:t}).$$

(1)

In FastSLAM, the Rao-Blackwellised particle filter is used to calculate the robot’s trajectory, which is represented by $p\left(x_{1:t}\right|z_{1:t},u_{1:t},n_{1:t})$. The possible trajectory of the robot is dictated by particles. The map is represented by a series of landmarks that follow a Gaussian distribution, which is updated using the observation model. Based on the premise that the position of the robot is known, the extended Kalman filter is used to estimate the landmarks of the map, which are represented by $p\left(m_n\right|x_{1:t},z_{1:t},n_{1:t})$.

2.1.1. Basic Flow of FastSLAM

Based on information from LIDAR and an odometer, the basic steps of the FastSLAM algorithm are as follows:

• Initialisation of the particles.
  We initialize N particles, each of which has an initial weight ${\omega }_0^{\left(i\right)}=0$.
• Retrieval.
  The initial step involves retrieval of a particle representing the posterior at time $t-1$ and the sampling of the robot’s position at time t. The position of the robot is predicted by sampling from a proposal distribution $\pi$, and is denoted as $x_t^{\left(i\right)}$ for the ith particle. Normally, the proposal distribution generally adopts the probabilistic motion model $p\left(x_t\right|x_{t-1},u_t)$.
• Importance weight.
  The importance weight for the ith particle ${\omega }_t^{\left(i\right)}$ represents the proportion between the target distribution $p\left(x_{1:t}^{\left(i\right)}\right|z_{1:t},u_{1:t-1},n_{1:t})$ and the proposal distribution $\pi$ mentioned above. It is defined as follows:

  $${\omega }_t^{\left(i\right)}=\frac{p\left(x_{1:t}^{\left(i\right)}\right|z_{1:t},u_{1:t-1},n_{1:t})}{\pi \left(x_{1:t}^{\left(i\right)}\right|z_{1:t},u_{1:t-1},n_{1:t})},i=1,\dots ,N$$

  (2)
• Resampling.
  We define the effective number of particles $N_{eff}$ as follows:

  $$N_{eff}=\frac{1}{{\sum }_{i=1}^N{\left({\omega }_t^{\left(i\right)}\right)}^2}.$$

  (3)
  By comparing $N_{eff}$ with the threshold set in advance, it can be determined whether to perform resampling. The threshold selected here is $N_{th}=N/2$, where N denotes the total number of particles. When $N_{eff}>N_{th}$, resampling is performed, in which higher weighted particles are copied and lower weighted particles are discarded.
• Measurement update.
  The posterior of the map feature estimation $p\left(m_n\right|x_{1:t},z_{1:t})$ is updated based on the robot trajectory $x_{1:t}$ and the observation $z_{1:t}$ of each particle.

The process of the standard FastSLAM is graphically depicted in Figure 2.

2.1.2. Particle Degeneracy Problem in FastSLAM

In the resampling phase, it is essential to remove those particles with large estimation errors. However, the resampling process leads to insufficient particle diversity: Only the higher weighted particles are replicated, while the lower weighted particles are rejected, and the diversity of the particles is therefore reduced.

The particle degeneracy problem can be effectively solved by adding those particles that can accurately estimate the robot’s position. However, since the position of the robot is unknown, it is difficult to maintain good particles. Only when the particle swarm is able to share the most likely position of the robot can a sufficient number of good particles be maintained.

2.1.3. Adaptive GA Resampling

In this section, we propose an adaptive strategy by introducing an improved resampling method based on the GA [25]. Before resampling is applied, higher weighted particles are used to correct for lower weighted particles through the crossover and mutation operations of the GA. The adaptive strategy is used to determine the crossover degree and mutation probability for the GA. Compared with traditional particle filtering, this resampling method based on an adaptive GA can effectively improve the diversity of the particle swarm, and this can to some extent solve the particle degradation problem of FastSLAM.

We assume that the particle sets are represented as $x_t^{\left(i\right)},{\omega }_t^{\left(i\right)}$ (i = 1, …, N), where ${\omega }_t^{\left(i\right)}$ represents the normalised particle weights. We sort particle weights ${\omega }_t^{\left(i\right)}$ (i = 1, …, N) in descending order and store them in W:

$$W=\left\{{\omega }_t^{\left(1\right)},{\omega }_t^{\left(2\right)},\dots ,{\omega }_t^{\left(N\right)}\right\}.$$

(4)

Then we define a threshold $W_T$ based on $N_{eff}$:

$$W_T=W\left(N_{eff}\right).$$

(5)

where $W\left(i\right)$ represents the ith weight in the set W.

The particles are divided into two groups by $W_T$ as follows:

$$x_t^{\left(i\right)}\in \begin{array}{ccc}{X}_L,& if& {\omega }_t^{\left(i\right)}\le W_T\\ X_H,& if& {\omega }_t^{\left(i\right)}>W_T\end{array}$$

(6)

where $X_L$ and $X_H$ are particle sets that contain the lower and higher weight particles, respectively. We assume that $x_{t,L}^{\left(i\right)}$ and $x_{t,H}^{\left(i\right)}$ represent the particles from $X_L$ and $X_H$, respectively. If $x_{t,c}^{\left(i\right)}$ represents the modified particles, the formulation of the crossover operator can be given by:

$$x_{t,c}^{\left(k\right)}=\mu x_{t,L}^{\left(i\right)}+(1-\mu )x_{t,H}^{\left(j\right)}$$

(7)

where $i=1,\dots N_L$, $j=1,\dots N_H$, and $k=i+j$. $N_L$ and $N_H$ denote the numbers of particles contained in $X_L$ and $X_H$, respectively. The parameter $\mu \in [0,1]$ represents the crossover degree of the particles. The greater the value of $\mu$, the more information will be transferred from $x_{t,L}^{\left(i\right)}$ to $x_{t,c}^{\left(i\right)}$ and the less information will be transferred from $x_{t,H}^{\left(j\right)}$ to $x_{t,c}^{\left(i\right)}$. Especially when $\mu$=1, $x_{t,c}^{\left(i\right)}=x_{t,L}^{\left(i\right)}$, no crossover occurs.

A mutation operator is applied to further increase the diversity of the particles. We assume that $x_{t,m}^{\left(i\right)}$ represents the mutated particles. It may happen on the modified particles $x_{t,c}^{\left(i\right)}$ according to:

$$x_{t,m}^{\left(i\right)}=\{\begin{array}{ccc}2x_{t,H}^{\left(j\right)}-x_{t,c}^{\left(i\right)},& if& r_l\le P_M\\ x_{t,c}^{\left(i\right)},& if& r_l>P_M\end{array}$$

(8)

where $r_l$ is a random variable drawn from a uniform distribution on [0,1]. $P_M$ denotes the mutation probability, which needs to be set in advance; if $P_M$ = 0, no mutation will occur.

An adaptive selection strategy is proposed here in order to determine the crossover degree $\mu$ and the variation probability $P_M$. It is well known that the smaller the variance in the particle weights, the better the approximation to the true posterior distribution. We assume that ${\sigma }^2$ represents the variance in the particle weights. Mathematical theorems have shown that for a sequence in the range [0,1], the variance is between zero and 1/4 [26], and the proof of this theorem is listed in Appendix A. We can therefore draw the conclusion that when the weights of the particles are within the range [0,1], the variance ${\sigma }^2$ is within the range [0,1/4]. The higher the value of ${\sigma }^2$, the more crossover needs to be performed. When ${\sigma }^2$ = 0, the quality of the particles is good enough that no crossover is necessary. The relationship between $\mu$ and ${\sigma }^2$ can therefore be represented as:

$$\mu =1-4{\sigma }^2$$

(9)

When ${\sigma }^2$ = 0, the parameter $\mu$ = 1, meaning that no crossover occurs. Conversely, when ${\sigma }^2$ = 1/4, the parameter $\mu$ = 0, meaning that a higher degree of crossover is needed to mitigate the degeneracy problem of the particles.

Mutation is applied to the particles $x_{t,c}^{\left(i\right)}$ which are obtained from crossover with a certain probability $P_M$. In general, $P_{m}in$ = 0.005 and $P_{m}ax$ = 0.01 [27].

When crossover is applied, the weights of the particle set also change. The variance in the new particle weights is expressed as ${\sigma }_{*}^2$. Similarly, the probability of mutation can also be determined based on ${\sigma }_{*}^2$; the greater the value of ${\sigma }_{*}^2$, the more mutation need to be performed, and the higher the probability of mutation $P_M$. In particular, when ${\sigma }_{*}^2$ = 1/4, the mutation probability $P_M=P_{max}$, and when ${\sigma }_{*}^2$ = 0, the mutation probability $P_M=P_{min}$. We therefore propose an adaptive selection strategy for $P_M$ as follows:

$$P_M=P_{min}+(P_{max}-P_{min})·4{\sigma }_{*}^2$$

(10)

In summary, we propose an improved resampling method based on the adaptive GA, which is referred to here as AGA-Resampling. This approach is used to replace the classical resampling of FastSLAM.

2.2. FastSLAM Optimised by FCPSO

2.2.1. PSO Improved by Fractional Differential

The PSO algorithm is derived from animal predation behavior. In this algorithm, the particle swarm is randomly initialised using the fitness function to evaluate the solution, and the optimal solution is found by iteration [28].

We assume that $X_i=(x_{i1},x_{i2},\dots ,x_{iN})$ represents the position of the ith particle, $P_i=(p_{i1},p_{i2},\dots ,p_{iN})$ represents the local optimal of the particles, and $P_g=(p_{g1},p_{g2},\dots ,p_{gN})$ represents the global optimisation of the particles. After t iterations, the velocities of the particles are updated by Equation (11) and the positions are updated using Equation (12):

$$v_{id}^t={\omega }_i{v}_{id}^{t-1}+c_1{r}_1(p_{id}-x_{id}^t)+c_2{r}_2(p_{gd}-x_{id}^t)$$

(11)

$$x_{id}^{t+1}=x_{id}^t+v_{id}^t.$$

(12)

In Equation (11), d denotes the dimension, where $d=1,\dots ,N$; $v_{id}$ is the d-dimensional component of the velocity of the ith particle; $x_{id}$ is the d-dimensional component of the position of the ith particle; $p_{id}$ is the d-dimensional component of $P_i$; $p_{gd}$ is the d-dimensional component of $P_g$; t is the number of iterations; $\omega$ is the inertia factor that determines the global and local optimisation ability; $c_1$ represents the individual learning factor of each particle; $c_2$ represents the collective learning factor for each particle; and $r_1$ and $r_2$ are random numbers between zero and one. In Equation (12), because the sample period between t and $(t+1)$ is 1, the moving distance in unit time is $v_{id}^t\times 1$, so the position $x_{id}^{t+1}$ can be calculated with the sum of a position $x_{id}^t$ plus a velocity $v_{id}^t$. The standard PSO algorithm has the problems of slow convergence and easy to fall into local convergence. So it is necessary to make improvements.

The convergence speed of PSO is limited by its algorithm structure. Since fractional-order model has superior memory characteristics, fractional differential theory is of great assistance to the dynamic evolution of particles in a particle swarm [29]. In this part, we introduce the fractional differential into the PSO algorithm to change the convergence speed.

The fractional differential equation [30] for the discrete-time domain defined by Grunwald–Letnikov is as follows:

$$D^{\alpha }\left[x\left(t\right)\right]=\frac{1}{T^{\alpha }}·\sum _{k=0}^r\frac{{(-1)}^k\mathsf{\Gamma }(\alpha +1)x(t-kT)}{\mathsf{\Gamma }(k+1)\mathsf{\Gamma }(\alpha -k+1)}$$

(13)

where $\mathsf{\Gamma }$ denotes the gamma function, T denotes the sampling period, and r denotes the cut-off order of the equation.

The speed update formula for the PSO algorithm is rearranged as follows:

$$v_{id}^t-{\omega }_i{v}_{id}^{t-1}=c_1{r}_1(p_{id}-x_{id}^t)+c_2{r}_2(p_{gd}-x_{id}^t).$$

(14)

Assuming $\omega$ = 1, this equation is a discrete form of differentiation. Assuming $T=1$ and following Pires et al. [30], we transform Equation ( 14 ) as follows:

$$D^{\alpha }\left[x\left(t\right)\right]=c_1{r}_1(p_{id}-x_{id}^t)+c_2{r}_2(p_{gd}-x_{id}^t).$$

(15)

Non-integer calculus cannot be directly processed by existing simulation tools, so it is necessary to use finite-dimensional ideas to approximate fractional calculus [31]. Only the first four items are considered, and Equation ( 15 ) can be expressed as:

$$v^t=\alpha v^{(t-1)}+1/2\alpha v^{t-2}+1/6\alpha (1-\alpha )v^{t-3}+1/24\alpha v(1-\alpha ){(2-\alpha )}^{t-4}+c_1{r}_1(p_{id}-x_{id}^t)+c_2{r}_2(p_{gd}-x_{id}^t).$$

(16)

By applying the idea of the fractional differential, the order of velocity differential $\alpha$ can be generalised to a fraction between zero and one, which will result in a more stable change and a longer memory effect. The value of $\alpha$ greatly affects the specific performance of the PSO algorithm: The dynamic performance of the PSO algorithm is weakened when $\alpha$ is small, while if the value of $\alpha$ is relatively large, the particle diversity will be increased and the global optimal value will be obtained easier. Based on previous experiments [32], the fractional order $\alpha$ is set to 0.6 where the algorithm converges at the fastest rate.

2.2.2. PSO Improved by Chaotic Optimisation

PSO is a relatively simple and fast-running algorithm. However, it cannot search for the global optimal solution when all particles move toward a local optimum, and this gives rise to the premature convergence phenomenon. The introduction of chaotic optimisation into the search algorithm can help particles escape from local optima by generating many neighboring solutions for the local optimum [33]. Hence, we use a chaotic search to update a certain particle whenever it is judged that PSO is showing premature convergence.

The judgment of premature convergence is based on the variance in the fitness of the particle swarm [34]. The variance in the fitness of the particle swarm is denoted by ${\sigma }_f^2$:

$${\sigma }_f^2=\sum _{i=1}^N\frac{f_i-f_{avg}}{f},$$

(17)

$$f=\{\begin{array}{cc}ma{x}_{1\le i\le m}\mid f_i-f_{avg}\mid ,& ma{x}_{1\le i\le m}\mid f_i-f_{avg}\mid >1\\ 1,& others\end{array}$$

(18)

where f is the normalization factor, $f_i$ the fitness of the ith particle, and $f_{avg}$ the average fitness of the particle swarm.

The fitness variance ${\sigma }_f^2$ reveals the aggregation degree of the particle swarm. The smaller the value of ${\sigma }_f^2$, the more aggregated the particle swarm. When ${\sigma }_f^2$ is less than a constant C, chaotic operation is performed.

The chaotic search space in this work is determined by the optimal position $p_g=(p_{g1},p_{g2},\dots ,p_{gn})$, which is found when the PSO algorithm falls into local convergence. The chaotic variables is generated by the Logistic function [35] as show in Equation (19).

$$y_{q^{\prime }}=4y_q(1-y_q),q=1,2,\dots ,n.$$

(19)

N chaotic variables $y_{q^{\prime }}(0<y_{q^{\prime }}<1)$ can be obtained by substituting n initial values $y_q(0<y_q<1)$ with small differences into Equation (19). A solution vector to the objective function is then generated according to Equation (20).

$$z_{q^{\prime }}=y_{q^{\prime }}-p_{gq},q=1,2,\dots ,n.$$

(20)

In summary, when the particles fall into a local optimum, the following steps are performed: (1) Use the Logistic function to generate the chaotic sequence with initial chaotic variable with values in the range [0,1]. (2) Calculate the fitness of each chaotic sequence and record the sequence with the best fitness value. (3) Replace the local optimal particle of the current particles with the chaotic sequence with the best fitness.

The standard PSO is improved by both fractional differential theory and chaotic optimisation. This improved algorithm is referred to here as fractional differential and chaotic particle swarm optimisation (FCPSO). The particle update speed is accelerated, and the hereditary nature of PSO is increased. The particle swarm is also updated when premature convergence occurs, which increases the diversity of the particle swarm.

2.2.3. The Proposal of the Virtual Particle

In general particle filter algorithm based on a standard PSO algorithm, the particle fitness in the PSO algorithm is calculated based on the weight of the particle, and the particle with the largest weight is taken as the global optimisation target [36]. However, it is proved that the mean value of the particle positions is closer to the real location than the particle with the maximum weight [24]. We therefore define a virtual particle according the mean value of the particle positions in this part. The virtual particle is not a real particle in PSO, and it is just used to determine a global optimum for PSO optimisation.

The position of the virtual particle is calculated as follows:

$$x_{vir}=\frac{1}{N}\sum _{i=1}^N{x}_i,$$

(21)

where $x_i$ represents the initial position of the ith particle.

We introduce FCPSO optimisation into the FastSLAM algorithm. In particular, the fitness function is calculated based on the position of the particle instead of the importance weight:

$$f_i=\Vert x_i-x_{vir}\Vert ,$$

(22)

For each particle, we calculate the local optimum as follows:

$$P_i=x_i.$$

(23)

We select the virtual particle as the global optimum target rather than the largest weighted particle as follows:

$$P_g=x_{vir}.$$

(24)

The main process of FCPSO is shown in Figure 3.

We use the FCPSO algorithm to optimise the FastSLAM algorithm. First, we use the particle filter based on AGA-Resampling to resample the possible robot position. We then define the virtual particle and use it as the optimisation target of FCPSO, which attracts particles to move to the actual position of the robot. The proposed method, which uses FastSLAM based on AGA-Resampling and is further optimised by FCPSO, is called IFastSLAM. This approach is different from the standard FastSLAM scheme because it uses an additional process to compensate for particle degeneracy. This compensation process serves as a key strategy. The overall scheme of the algorithm is shown in Figure 4, and pseudo-code is given in Algotithm 1.

IFastSLAM has several advantages: AGA-Resampling can effectively improve the diversity of particles, and the iteration speed of PSO with fractional differential optimisation is increased, further improving the speed of SLAM. In addition, PSO optimised by chaos can avoid local convergence and can effectively improve the accuracy of SLAM.

3. Experiments

In this section, we perform experiments with the simulation environment, the Victoria Park dataset, and field experiments, in order to study the performance of the improved algorithm. We performed simulation experiments using Matlab on Intel(R) Core (TM) i5-8250 [email protected] GHz RAM 8 GB PC. We used a robot equipped with LiDAR (SLAMTEC RPLIDAR) and an odometer for field experiments.

3.1. Simulation

3.1.1. Simulation Setup

The simulation was performed using MATLAB to verify the accuracy and stability of IFastSLAM. We developed the SLAM simulator based on the source code published by Bailey [37].

In this work, we assumed that the robot moved at a constant speed. The model for the motion is as follows:

$$\left[\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ {\theta }_{t+1}\end{array}\right]=\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]+\left[\begin{array}{c}v\mathsf{\Delta }tcos(G+{\theta }_t)\\ v\mathsf{\Delta }tsin(G+{\theta }_t)\\ v\mathsf{\Delta }tcos\left(G\right)/WB\end{array}\right]+W_t$$

(25)

where ${[x_t,y_t,{\theta }_t]}^T$ is the vector of the robot pose; v is the robot speed; G is the steering angle of the robot; $WB$ is the wheelbase of the robot; $\mathsf{\Delta }t$ is the sampling time interval; $W_t$ is the system noise, and $W_t\sim N(0,Q)$.

The observation model used in the simulation experiment is as follows:

$$z_t=\left[\begin{array}{c}{\rho }_i\\ {\beta }_i\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_x-x_t)}^2+{(m_y-y_t)}^2}\\ atan(m_y-y_t,m_x-x_t)-{\theta }_t\end{array}\right]+V_t$$

(26)

where ${[m_x,m_y]}^T$ is the coordinate of the ith landmark detected by the robot; ${\rho }_i$ and ${\beta }_i$ represent the distance and angle, respectively, of the ith landmark from the current robot position; $V_t$ is the observation noise; and $V_t\sim N(0,R)$.

The simulation environment is illustrated in Figure 5a. It is in effect a search area of size 250 × 200 m. In this simulation experiment, 17 waypoints and 35 landmarks were randomly set. The red trajectory represents the designated simulation path, the red ‘o’ is the waypoint, and the blue asterisk is the stationary landmark. The robot starts from (0,0) and moves counterclockwise along the red trajectory shown in Figure 5a to obtain the estimated trajectory and the estimated landmark. Figure 5b shows a screenshot of the process of simulation at a certain moment. In Figure 5b, the black triangle represents the robot, and the green circle represents the observation range of the robot, which is centered on the robot. The robot can observe landmarks within a certain measuring angle inside the green circle. The estimated trajectory is represented by a black line, and the estimated landmark is denoted by a red asterisk.

The relevant parameters in the simulation are as follows. The initial position of the robot is ${[0,0,0]}^T$; its speed is $v=3$ m/s; the maximum steering angle is ${20}^{\circ }$; The sampling time is $0.025$ s; error in the speed is ${\sigma }_v=0.3$ m/s; the sampling time of the laser radar is $0.2$ s; the maximum measuring distance is 30 m, the measurement error in the distance is ${\sigma }_{\rho }=0.1$ m; and the error in the angle is ${\sigma }_{\beta }$ = $1^{\circ }$; The systematic noise Q and the observation noise R are as follows:

$$Q=\left[\begin{array}{cc}{0.33}^2& 0\\ 0& {\left(\frac{3\pi }{180}\right)}^2\end{array}\right],R=\left[\begin{array}{cc}{0.1}^2& 0\\ 0& {\left(\frac{\pi }{180}\right)}^2\end{array}\right]$$

(27)

We use one hundred particles for simulation. Resampling is performed when $N_{eff}>50$.

3.1.2. Results and Analysis

We simulated FastSLAM, PSO-FastSLAM (FastSLAM based on PSO with particle weights), AGA-FastSLAM (FastSLAM based on AGA-Resampling), and IFastSLAM with the same simulation parameters in order to verify the superiority of the improved algorithm. The estimated robot path and landmark location are shown in Figure 6.

From the results, we can conclude that the estimated landmark location of IFastSLAM is essentially consistent with the actual landmark location, and the estimated path substantially coincides with the actual path. The proposed IFastSLAM algorithm is therefore more accurate than the other three algorithms.

In order to compare these four algorithms more intuitively, we compared the location estimation error of the robot in X axis and Y axis, as shown in Figure 7. The positioning error is calculated as follows:

$$error\left(t\right)=\sqrt{({\sum }_{i=1}^N{x}_i\left(t\right)/N-x_{true}{)}^T({\sum }_{i=1}^N{x}_i\left(t\right)/N-x_{true})}$$

(28)

The positioning error reflects the absolute error between the actual position of the robot and the average position of the particle swarm.

As shown in Figure 7a, the positioning error in X axis is less than 12 m in FastSLAM, less than 7 m in the standard PSO-FastSLAM algorithm optimised by particle weight, less than 4 m in AGA-FastSLAM, and within 3 m in IFastSLAM. The values of the estimation accuracy of the three improved FastSLAM algorithms are higher than that of the standard FastSLAM. In PSO-FastSLAM, the positioning accuracy is effectively improved due to the use of PSO optimisation. In AGA-FastSLAM, adaptive genetic resampling is used to handle the particle degeneracy problem, and the positioning accuracy is therefore also effectively improved. Furthermore, since the position of the virtual particle is used as the optimisation target, and the premature convergence phenomenon is solved in the FCPSO algorithm, the positioning error in IFastSLAM is smaller than in the other three algorithms. Similarly, in the IFastSLAM algorithm, the positioning error in Y axis is also smaller than for the other three algorithms, and the estimation accuracy is higher, as can be seen from Figure 7b.

As shown in

Table 1. Comparison of the RMSE and running time of SLAM algorithms.

Particle Number		20	50	100
FastSLAM	Time (s)	30.49	76.13	153.57
	RMSE (m)	8.43	7.90	6.84
PSO-FastSLAM	Time (s)	34.02	81.35	159.05
	RMSE (m)	6.31	5.10	4.79
AGA-FastSLAM	Time (s)	33.95	80.96	158.84
	RMSE (m)	5.12	3.78	3.25
IFastSLAM	Time (s)	31.17	78.05	156.98
	RMSE (m)	2.85	2.10	1.71

, the IFastSLAM algorithm requires a shorter calculation time than PSO-FastSLAM and IFastSLAM for the same number of particles; the reason for this is that the use of fractional differential theory improves the evolution speed of the particle swarm. In addition, the calculation time increases as the particle number increases.

As shown in Figure 8, the estimation accuracy of IFastSLAM is significantly higher than the other three algorithms for the same number of particles. We can also clearly see that FCPSO-FastSLAM maintains a low RMSE with fewer particles than the other three algorithms, since the use of the virtual particle in the IFastSLAM algorithm, it can maintain more effective particles than the FastSLAM algorithm and can therefore resolve the problem of particle degeneracy to some extent.

Compared with standard FastSLAM, IFastSLAM sacrifices a certain amount of calculation time, but its estimation accuracy is significantly improved, and the running time is lower than for PSO-FastSLAM and AGA-FastSLAM. In this sense, the IFastSLAM has better overall performance.

3.2. Verification Using the Victoria Park Dataset

In order to further compare the performance of IFastSLAM with PSO-FastSLAM and AGA-FastSLAM, we applied several algorithms to the Victoria Park dataset [38]. This dataset is provided by the Australian Center for Field Robotics (ACFR) and is widely applied by researchers to verify the effectiveness and evaluate the performance of the SLAM algorithm in the real environment.

The mobile test platform used by the ACFR is shown as Figure 9. It is equipped with LiDAR, an odometer, and GPS, and is driven a distance of 4 km over about half an hour. LiDAR is used to provide observation information on the features of the road (trees in the park). The odometer is used to provide information on the linear speeds and heading angles of vehicles. The GPS sensor is used to provide reference vehicle location information. In this dataset, the errors caused by the inaccuracy of GPS data is ignored, and the vehicle trajectory is approximately represented by GPS data [39].

The validation results of several algorithms on the Victoria Park dataset are shown in Figure 10a–c, where the blue line represents the real GPS track of the vehicle, the red line represents the vehicle track estimated by the algorithm, and the yellow point represents the position of the landmark feature point. From the white box in Figure 10, it can be seen that the IFastSLAM algorithm obtains an estimation result that is closer to the real GPS track.

The RMSE of the robot trajectory and CPU running times of three algorithms on the Victoria Park dataset are shown in

Table 2. Comparison of RMSE and running time with Voctoria Park dataset.

Algorithms	Running Time (s)	RMSE (m)
PSO-FastSLAM	1359.22	16.37
AGA-FastSLAM	1491.30	11.49
IFastSLAM	1406.57	9.15

. We can draw the conclusion that the RMSE of IFastSLAM is lower than the other two algorithms, and it also requires less running time. Overall, we have verified the superior performance of IFastSLAM on the Victoria Park dataset.

3.3. Field Experiment

We used the mobile robot shown in Figure 11a to perform field experiments to test the practicability of IFastSLAM. The robot was equipped with an internal odometer and a SLAMTEC RPLIDAR A2 LiDAR, and moved at a speed of 0.3 m/s to create a grid map in real-time using LiDAR and odometer data. The National Key Laboratory of Virtual Simulation of Chang’an University was selected as the experimental site, as shown in Figure 11b.

The robot was driven along the specified path within the experimental site to build an environmental map using radar scanning. Three algorithms were compared in this field experiment.

Figure 12a shows the map built by the PSO-FastSLAM algorithm, while Figure 12b,c show the maps built by AGA-FastSLAM and IFastSLAM in the same environment, respectively. There are obvious defects in the grid map built by PSO-FastSLAM as can be seen from Figure 12a, and a large deviation appears at the boundary of the map. Similarly, blurring occurs at the edge of the grid map built by the AGA-FastSLAM, as shown in Figure 12b. In contrast, the grid map built by the IFastSLAM is more precise, as shown in Figure 12c.

The results reveal that the map generated by IFastSLAM has fewer defects and higher precision, while the maps generated by the PSO-FastSLAM and AGA-FastSLAM algorithms are of low quality.

Figure 13 shows the estimated trajectory for the different algorithms. We can see that the trajectory estimated by the IFastSLAM algorithm correctly follows the ground truth, while the trajectory estimated by PSO-FastSLAM and AGA-FastSLAM obviously deviates from the ground truth of the robot in some places. IFastSLAM has better localisation accuracy than the other two algorithms. In addition, the running times of PSO-FastSLAM, AGA-FastSLAM, and IFastSLAM are 592.53 s, 647.09 s, and 629.74 s respectively, meaning that the computational efficiency of IFastSLAM is clearly higher.

Table 3. Comparison of RMSE for different algorithms in field experiment.

Algorithms	PSO-FastSLAM	AGA-FastSLAM	IFastSLAM
RMSE (m)	0.27	0.16	0.09

show the RMSE of different algorithms in field experiment. It can be seen that the RMSE of IFastSLAM algorithm is the smallest. It can be therefore concluded that IFastSLAM has better overall performance.

4. Conclusions

In the resampling process of the conventional FastSLAM process, the accuracy decreases with the number of iterations, due to particle degeneracy. We propose a framework named IFastSLAM that can enhance the performance of FastSLAM based on PSO. In this framework, an adaptive GA is introduced at the resampling stage, and a novel algorithm called FCPSO is proposed based on fractional differential and chaotic optimisation to improve PSO-FastSLAM. Compared with conventional frameworks, IFastSLAM has four significant benefits: (i) The diversity of particles is enriched by an adaptive GA; (ii) the evolutionary speed of the PSO algorithm is increase; (iii) the problem of premature convergence in PSO is alleviated; and (iv) the global optimisation accuracy is improved by using a virtual particle as the optimisation target, and the errors in the trajectory and landmark estimations are reduced.

We compare our proposed IFastSLAM approach with FastSLAM, PSO-FastSLAM, and AGA-FastSLAM on a series of SLAM issues. This comparison was made based on simulations, an experiment with the Victoria Park dataset provided by ACFR, and a field experiment with a mobile robot, showing promising results. In future work, we plan to integrate IFastSLAM into 3D maps and carry out further study on path planning and collision detection for mobile robots. In addition, the parameters used in FCPSO are taken directly from other studies, and we will readjust the parameters in the next study to make them more suitable for robot systems.