An Enhanced Particle Filtering Method Leveraging Particle Swarm Optimization for Simultaneous Localization and Mapping in Mobile Robots Navigating Unknown Environments

Abstract

With the rapid advancement of mobile robotics technology, Simultaneous Localization and Mapping (SLAM) has become indispensable for enabling robots to autonomously navigate and construct maps of unknown environments in real time. Traditional SLAM algorithms, such as the Extended Kalman Filter (EKF) and FastSLAM, have shown commendable performance in certain applications. However, they encounter significant limitations when dealing with nonlinear systems and non-Gaussian noise distributions, especially in dynamic and complex environments coupled with high computational complexity. To address these challenges, this study proposes an enhanced particle filtering method leveraging particle swarm optimization (PSO) to improve the accuracy of pose estimation and the efficacy of map construction in SLAM algorithms. We begin by elucidating the foundational principles of FastSLAM and its critical role in empowering robots with the ability to autonomously explore and map unknown territories. Subsequently, we delve into the innovative integration of PSO with FastSLAM, highlighting our novel approach of designing a bespoke fitness function tailored to enhance the distribution of particles. This innovation is pivotal in mitigating the degradation issues associated with particle filtering, thereby significantly improving the estimation accuracy and robustness of the SLAM solution in various operational scenarios. A series of simulation experiments and tests were conducted to substantiate the efficacy of the proposed method across diverse environments. The experimental outcomes demonstrate that, compared to the standard particle filtering algorithm, the PSO-enhanced particle filtering effectively mitigates the issue of particle degeneration, ensuring reliable and accurate SLAM performance even in challenging, unknown environments.

1. Introduction

Simultaneous Localization and Mapping (SLAM) technology [1] is a pivotal component in the realms of autonomous driving and mobile robotics, endowing robots with the capability for autonomous navigation in uncharted environments and forming the foundation for advanced robotic functionalities such as environmental comprehension, decision-making, and path planning. By fusing data from a variety of sensors, including LiDAR, cameras, and Inertial Measurement Units (IMUs) [2], SLAM technology enables the real-time construction of detailed maps of the surrounding environment while precisely localizing the robot itself. This capability is essential for the long-term autonomous operation of robots in dynamic environments. As technology continues to evolve, SLAM is becoming an indispensable part of intelligent infrastructure, propelling innovation and development across the automation industry [3]. Consequently, research and optimization of SLAM technology have profound implications for the future development of intelligent systems.

Despite the vast potential of SLAM technology, its implementation in dynamic, complex, and unknown environments poses significant technical challenges. Particularly in highly dynamic settings, the rapid changes in the environment demand greater adaptability and robustness from SLAM algorithms. Additionally, ensuring algorithmic precision while enhancing its responsiveness to environmental alterations is an issue that urgently needs addressing in current SLAM research [4]. Therefore, developing SLAM algorithms that maintain high accuracy and robustness across various environments is crucial for propelling the field forward.

FastSLAM [5], recognized as an efficient solution for the SLAM problem, adeptly manages nonlinear and non-Gaussian noise challenges by harnessing the combined power of particle filtering and the Extended Kalman Filter (EKF) [6]. Gaussian noise, also known as white noise, is a type of noise that follows a Gaussian distribution, which is characterized by a bell-shaped curve and is widely used in signal processing due to its simple and predictable properties. However, in many real-world applications, the noise encountered is non-Gaussian, meaning it does not adhere to a Gaussian distribution, thereby presenting more complex challenges for SLAM algorithms. This algorithm strategically divides the SLAM problem into two core components: the estimation of the pose of the robot and the estimation of environmental landmarks. Particle filtering is tasked with addressing the nonlinear aspects of pose estimation while the EKF independently processes each landmark, a decomposition that substantially reduces computational complexity and enables scalability in large-scale environments. However, FastSLAM encounters particle degradation issues under conditions of large-scale environments and high noise, which can diminish the diversity of particles and potentially hinder the ability to effectively explore the entire state space [7]. This degradation phenomenon, characterized by increasing insignificance of the weights of most particles over filtering iterations, with only a few particles retaining substantial weight, leads to a posterior probability that is predominantly represented by these few particles. Consequently, this not only results in a waste of computational resources but also poses a risk to the estimation accuracy, thereby limiting the broader applicability of FastSLAM. This contribution is significant for applications where traditional Gaussian-based models fall short of capturing the true nature of the underlying processes. Another method is the fractional α-stable filter [8], which introduces an adaptive filtering structure grounded in α-stable statistics and fractional-order calculus to address the state estimation problem in the presence of non-Gaussian noise, effectively enhancing the estimation accuracy of rapidly changing systems. Despite advancements in SLAM algorithm precision and robustness, addressing this degradation issue in particle filtering continues to be a subject of ongoing research and development [9,10,11].

To overcome the limitations of existing algorithms, this paper proposes a novel FastSLAM method integrated with particle swarm optimization (PSO) [12]. Each particle in the PSO algorithm navigates through the solution space, updating its velocity and position by tracking both individual and group historical best positions. With its simplicity, efficiency, and ease of implementation, the PSO algorithm has achieved notable success in optimization problems across engineering, machine learning, and other domains [13,14]. Our primary contributions include the proposal of a new particle optimization strategy that enhances the performance of particle filtering by designing a fitness function set to optimize the PSO algorithm. This set of fitness functions considers not only the contribution of particles to the current estimation but also their diversity to prevent particle degradation. We have designed a series of experiments to validate the effectiveness of the proposed method and, through performance comparison with existing SLAM algorithms, demonstrate the advantages of our approach. A series of experiments have been meticulously designed to validate the efficacy of our proposed method. These experiments not only confirm the merits of our approach but also provide a comparative analysis against existing SLAM algorithms, demonstrating the superior performance of our PSO-enhanced FastSLAM method.

The structure of this paper is as follows: The first section introduces the background and research motivation of SLAM technology; the second section reviews the fundamental principles of relevant SLAM and PSO algorithms; the third section elaborates on the design and implementation of the proposed PSO-optimized FastSLAM algorithm in detail; the fourth section presents experimental results and performance evaluation; the final section summarizes the paper and proposes prospects for future research directions.

2. Related Works

In this section, we commence with an introduction to Particle Swarm Optimization and FastSLAM, followed by a discussion on the integration of these two methodologies and their combined applications in the realm of robotics and autonomous navigation.

2.1. PSO

PSO, introduced by Kennedy and Eberhart in 1995, is an optimization algorithm inspired by the collective behaviors of biological groups such as flocks of birds and schools of fish. The algorithm simulates the social interactions within these groups to optimize solutions to problems. In PSO, solutions are represented by a set of particles, and the entire swarm works cooperatively, sharing information between individual particles and the group to converge on the optimal solution [12].

Specifically, each particle in PSO has two key position attributes: the personal best position p_i, which represents the best solution found by that particle thus far, and the global best position g_i, which represents the best solution found by the entire swarm. The position update for each particle is governed by the following equations:

$$v_{i+1}=w{v}_i+c_1{r}_1(p_i-x_i)+c_2{r}_2(g_i-x_i)$$

(1)

$$x_{i+1}=x_i+v_{i+1},$$

(2)

where v_i and x_i represent the velocity and position of particle i, c₁ and c₂ are acceleration coefficients, r₁ and r₂ are random values within the range [0, 1], and w is the inertia weight that balances the exploration and exploitation capabilities of the algorithm. This mechanism allows the particles to balance global and local search, gradually moving towards the optimal solution.

The strengths of PSO lie in its simplicity, ease of implementation, and independence from the gradient information of the objective function, making it suitable for nonlinear and non-convex optimization problems. It can quickly converge in large-scale, high-dimensional search spaces, making it particularly effective for optimization tasks. Due to the nature of SLAM problems, which are not typically multimodal, this issue is less problematic. In SLAM, the pose of the robot and the positions of the landmarks often have a unique global optimal solution, and thus, the local convergence of PSO is not a major concern. This makes PSO well-suited for optimizing the particle distribution in SLAM, reducing particle degeneration, and improving both accuracy and robustness. These are the key reasons we chose PSO to enhance FastSLAM’s performance.

2.2. FastSLAM

FastSLAM is a probabilistic method that combines PF and the EKF to address the problem of SLAM in mobile robots navigating unknown environments. The key innovation of FastSLAM is the decomposition of the SLAM problem into two independent subproblems: the estimation of the pose of the robot and the landmark positions in the environment [5]. FastSLAM formulates the joint posterior distribution of the pose of robot s_t and the landmarks Θ = {μ_k, Σ_k} as

$$p(\Theta ,s_t|z_{1:t},u_{1:t})=p(s_t|z_{1:t},u_{1:t}){\displaystyle \prod _{k=1}^N}p({\mu }_k,{\Sigma }_k|z_{1:t},s_{1:t})$$

(3)

where z_1:t represents all observations from time step 1 to t, and u_1:t represents the control inputs over the same period. μ_k and Σ_k denote the position and covariance matrix of landmark k, respectively. This decomposition shows that the estimation of the pose s_t is independent of the individual landmark estimates μ_k, significantly reducing computational complexity.

In FastSLAM, PF is employed to estimate the robot’s distribution of pose. PF approximates the posterior distribution using a set of weighted particles $s_t^{\left[m\right]}$, where each particle contains an estimate of the trajectory and the positions and covariance estimates of landmarks. At time step t, the pose $s_t^{\left[m\right]}$ of each particle is predicted using the motion model p (s_t|u_t, s_t−1) as

$$s_t^{[m]}~p(s_t|u_t,s_{t-1}^{[m]})$$

(4)

where $s_t^{\left[m\right]}$ represents the predicted pose of the m-th particle at time t. The motion model is typically represented as

$$s_t=f(s_{t-1},u_t)+{ϵ}_t$$

(5)

where f(·) is the motion model function, and ϵ_t is Gaussian noise representing system uncertainty.

Each particle then updates the estimated landmark positions using the Extended Kalman Filter. For the m-th particle, the position estimate ${\mu }_t^{\left[m\right]}$ of landmark k is updated as follows

$${\mu }_k^{[m]}={\mu }_k^{[m]}+K_t^{[m]}(z_t-h(s_t^{[m]},{\mu }_k^{[m]})),$$

(6)

where h($s_t^{\left[m\right]}$, ${\mu }_t^{\left[m\right]}$) is the observation model that relates the pose of robot $s_t^{\left[m\right]}$ to the landmark position ${\mu }_t^{\left[m\right]}$. The Kalman gain $K_t^{\left[m\right]}$ is calculated by

$$K_t^{[m]}={\Sigma }_k^{[m]}{H}_t^{\top }{(H_t{\Sigma }_k^{[m]}{H}_t^{\top }+R_t)}^{-1},$$

(7)

where H_t is the Jacobian of the observation model, and R_t is the observation noise covariance matrix. The weight $w_t^{\left[m\right]}$ of each particle is then computed based on the likelihood of the observations given the predicted pose and landmark estimates

$$w_t^{[m]}=p(z_t|s_t^{[m]},{\mu }_k^{[m]}),$$

(8)

where the weight represents the agreement between the particle and the actual observations.

To prevent particle degeneration, FastSLAM performs resampling based on the weights $w_t^{\left[m\right]}$, favoring particles with higher weights while replacing those with lower weights. This maintains the particle diversity and robustness of the algorithm. The EKF is used to update the position of landmarks within each particle. For particle $s_t^{\left[m\right]}$, the covariance matrix of the landmark positions is updated using the process model

$${\Sigma }_k^{[m]}=F_t{\Sigma }_{k,t-1}^{[m]}{F}_t^{\top }+Q_t,$$

(9)

where F_t is the Jacobian of the motion model, and Q_t is the process noise covariance matrix. The updated covariance matrix is then calculated as

$${\Sigma }_k^{[m]}=(I-K_t^{[m]}{H}_t){\Sigma }_k^{[m]}.$$

(10)

This update step adjusts the landmark position estimates based on the difference between the predicted and observed values, weighted by the Kalman gain. Compared to traditional EKF-SLAM, FastSLAM offers significant computational advantages. In EKF-SLAM, the complexity of jointly estimating both the robot pose and all landmark positions is O(N²), where N is the number of landmarks. FastSLAM, by decoupling the estimation of the robot pose and landmarks, reduces the complexity to approximately O (MN log N), where M is the number of particles. This reduction in complexity allows FastSLAM to efficiently handle environments with a large number of landmarks.

FastSLAM 2.0 improves the original FastSLAM algorithm by incorporating observation information into the particle proposal distribution, thereby improving the quality of particle updates and providing theoretical guarantees of convergence. However, even with these improvements, challenges remain in handling SLAM problems in noisy and complex environments. To further enhance particle filtering performance, PSO can be integrated into the FastSLAM framework. PSO is a swarm intelligence-based optimization technique that improves particle diversity by optimizing their distribution within the search space. This reduces particle degeneration and increases the robustness and accuracy of the algorithm, especially in challenging environments. Yasuda et al. [15] introduce an enhanced FastSLAM algorithm that improves the accuracy of self-localization for autonomous mobile robots within their environment by considering multiple particle candidates. Specifically, the authors propose an innovative approach where a set of particle candidates is retained during the self-localization process, and the final estimate is determined based on these candidates in subsequent steps. This addresses issues that may arise with traditional FastSLAM algorithms when dealing with observational noise and accumulated errors. Karaçam et al. [16] present an enhanced adaptive FastSLAM (AFastSLAM) algorithm, which integrates the estimation of time-varying noise statistics into the conventional FastSLAM framework. The refinement is achieved by employing maximum likelihood estimation and the expectation-maximization criterion, coupled with a one-step smoothing technique within the importance sampling process. Furthermore, to prevent the loss of positive definiteness in the process and measurement of noise covariance matrices, the paper utilizes an innovation in covariance estimation (ICE) methodology. Lingesh et al. [17] designed and evaluated an enhanced FastSLAM algorithm, which incorporates a module for tracking individuals using 2D LiDAR sensor data. The algorithm is capable of filtering out the unstable and dynamic noise generated by the movement of people, thereby providing more accurate localization and a clearer mapping of the environment.

2.3. Related Work of Integrating FastSLAM with Evolutionary Algorithm

In the domain of SLAM, the FastSLAM algorithm adeptly addresses the challenges of nonlinearity and non-Gaussian noise by synergistically merging the strengths of PF with the EKF. Despite its efficacy, FastSLAM encounters significant difficulties when tackling the degradation issues of particle filtering under conditions of large-scale environments and high noise levels. To surmount these obstacles, researchers have endeavored to integrate the PSO algorithm with FastSLAM, aiming to enhance the robustness and accuracy. This integration leverages the global search capabilities of PSO to refine the particle distribution, thereby mitigating particle degradation and augmenting the overall performance of the SLAM system.

Vahdat et al. [18] present an effective approach for the global localization of mobile robots using Differential Evolution and Particle Swarm Optimization. By integrating evolutionary algorithms with probabilistic motion and observation models, the proposed methods demonstrate superior performance in terms of convergence rate, accuracy, and computational efficiency, offering a robust solution for initial pose determination and pose tracking in mobile robotics. Moreno et al. [19] introduce the Evolutionary Localization Filter (ELF), which effectively addresses the challenges of global localization without relying on initial pose information by leveraging evolutionary computation concepts and focusing computational resources on the most promising areas. Zhang et al. [20] present an improved particle filter for mobile robot global localization based on particle swarm optimization. The proposed method, named particle swarm optimization filter, achieves robust and accurate positioning results in indoor environments with fewer particles compared to benchmark methods. This advancement could be integrated into a wide range of mobile robot systems to reduce computational costs and improve navigation efficiency. Zhang et al. [21] introduce an enhanced particle filter SLAM algorithm that employs PSO to bolster the accuracy and robustness of localization within similar environments. The efficacy is substantiated through comparative experiments with the Gmapping algorithm, demonstrating superior performance in terms of particle count, localization precision, and the topological correctness of robotic navigation. Moreno et al. [22] introduce a novel global localization filter that integrates Differential Evolution with Markov Chain Monte Carlo (MCMC) methods, effectively enhancing the accuracy of global localization for mobile robots in both 2D and 3D environments. Through testing in simulated and real-world scenarios, the approach has demonstrated significant advantages in reducing the requirement for particles and improving localization accuracy.

The collective research endeavors in the realm of SLAM have demonstrated that the amalgamation of evolutionary algorithms with the FastSLAM framework can significantly enhance the performance of SLAM algorithms, particularly when navigating dynamic environments and large-scale scenarios. These hybrid approaches have not only bolstered the precision of localization but also augmented the capacity to adapt to environmental fluctuations, thereby paving new avenues for the advancement of SLAM technologies. Despite the notable improvements in SLAM performance, challenges persist, such as the computational complexity of the algorithms and their adaptability to specific environmental conditions.

3. FastSLAM-PSO

FastSLAM relies on particle filtering to estimate the pose of the robot and employs the EKF to update landmark estimates. However, in complex and high-dimensional environments, the particle distribution may drift away from the optimal region over time, leading to a decline in estimation accuracy. By integrating the PSO algorithm into the particle filtering process of FastSLAM, particles are more effectively concentrated in regions of high posterior probability, thereby improving overall estimation accuracy.

PSO adjusts the velocity and position of particles in such a way that they follow the global best solution found by the swarm while maintaining their individual exploration space, ensuring sufficient diversity. This mechanism is particularly suitable for the nonlinear pose estimation problem in FastSLAM, and PSO can assist the particle swarm in quickly converging to a reasonable pose solution, even in the presence of complex environments or high levels of noise.

Within the particle filtering framework, each particle is characterized by its positional coordinates {x, y} and its orientation θ. In our FastSLAM-PSO, the population size parameter N of PSO is controlled by the number of particles in the FastSLAM algorithm. That is, after each time step, we optimize positional coordinates {x, y} and orientation θ of particles based on the current particle distribution, which allows the particles themselves to move directly rather than indirectly, which enhances the simplicity of the algorithm.

Our objective function is designed based on the measurement probability value associated with a given particle state. A higher measurement probability indicates a greater likelihood of the presence of a particle at that location, which is conducive to subsequent particle filtering operations. The PSO objective function is calculated as follows:

$$maxf(z)={\displaystyle \prod _{k=1}^{K}P}(z_{t,k}|s,{\Theta }_k)$$

(11)

where P (z_t,k|s, Θ_k) is the probability of observing the k-th landmark given the pose of the robot and the position of the k-th landmark.

The expression of PSO within FastSLAM can be combined with the state and importance weight of the particles to update the velocity and position of the particles. For the m-th particle, the velocity update formula is:

$$v_m^{(t+1)}=w\cdot v_m^{(t)}+c_1\cdot rand()\cdot (pbes{t}_m-x_m^{(t)})+c_2\cdot rand()\cdot (gbest-x_m^{(t)})$$

(12)

where $v_m^{(t)}$ is the velocity of the m-th particle at time t, $x_m^{(t)}$ is its position, pbest_m is the best position found by that particle to date, and gbest is the best position found by any particle in the swarm. The position update formula is:

$$x_m^{(t+1)}=x_m^{(t)}+v_m^{(t+1)}$$

(13)

This design allows particles to adjust their search direction and speed based on their historical best positions and the best positions, effectively exploring the potential optimal solution space while maintaining diversity. In this way, the FastSLAM-PSO can improve the performance of SLAM in complex environments, especially when dealing with high-noise data and large-scale environments. The complete algorithm is shown as Algorithm 1.

Algorithm 1. FastSLAM-PSO
1 Initialize world, robot, motion model, and measurement model
2 Initialize particles with initial pose and occupancy grid
3 Loop over each control step in the scene:
4    Move the robot according to the control input
5    Update the trajectory of the robot
6    Sense the environment to obtain measurements
7    Optimize the pose of all particles using PSO
8    For each particle:
9       Generate an initial guess from the motion model
10       Perform scan matching to refine the guess
11       Use the motion model to predict the pose
12       Calculate the weight of each particle based on the likelihood of the measurements
13       Normalize the weights
14    Select the best particle as the estimated robot pose
15    Perform adaptive resampling if the effective number of samples is low
16    Update the occupancy grid for each particle based on the true measurements
17 End loop

4. Experiments

In this section, a comprehensive suite of experiments has been meticulously designed to evaluate the performance of our proposed FastSLAM-PSO. The experimental design comprises three key components. First, a direct comparative analysis is undertaken between FastSLAM, FastSLAM-PSO and FastSLAM-DE, with the latter being an adaptation of FastSLAM-PSO where the optimization algorithm was switched from PSO to differential evolution (DE) [23]. Second, an in-depth examination of the PSO parameters is conducted. By adjusting the population parameters N of PSO, we discern their impact on the dynamics of particles and the overall convergence. Third, the experimental outcomes are showcased through visual representations of results, enabling a graphical comparison of performance across diverse experimental setups. This step is instrumental in highlighting disparities in performance under various configurations. Additionally, all figures presented in this manuscript were generated using Python.

4.1. Experiment Setting

We have designed three scenarios, as depicted in Figure 1. Figure 1a represents a small-scale scenario with distinct obstacles and landmarks, which poses a relatively straightforward challenge for SLAM algorithms. In contrast, Figure 1b illustrates a large-scale scenario that includes a lengthy corridor, presenting a more complex task for SLAM algorithms due to the increased difficulty in particle modeling. To further substantiate the universality and versatility of our approach, an additional square scenario has been incorporated as a medium-scale testing environment. This scenario simulates more generic environmental conditions, thereby bolstering our confidence in the broad applicability of the algorithm to real-world contexts. Through robustness testing in small-scale scenarios, assessment of complex environment handling capabilities in large-scale scenarios, and validation of universality and versatility in square scenarios, our experimental design comprehensively showcases the performance and advantages of the FastSLAM-PSO algorithm under various operational conditions.

Given that our study is anchored in a simulated environment, all sensor data and environmental feedback are generated through high-fidelity simulation software. This approach allows us to replicate the functionalities and characteristics of real LiDAR sensors without reliance on physical hardware. In our simulation experiments, the SLAM problem involving LiDAR is addressed by simulating laser line scanning strategies at various time points, ensuring that the algorithm can perform effective localization and map construction across diverse environmental conditions. Additionally, other parameter settings are as follows. The inertia weight w was meticulously calibrated to 0.5, striking a balance between the exploration of the search space and the exploitation of known promising areas. The cognitive constant c₁ and the social constant c₂ were both set to 1.5, which are instrumental in guiding particles toward their best-known positions and the best position, respectively. The initial velocities v_i of the particles were initialized to zero to avoid abrupt movements at the outset, ensuring a stable commencement of the optimization process. The velocity update equation is given by Equation (12). Furthermore, the entire algorithm is constrained in such a way that the collective particle distribution does not exceed the boundaries of the map.

4.2. Comparison between FastSLAM and FastSLAM-PSO

4.2.1. Small-Scale Scenario

In the small-scale scenario, the final mapping results are visually presented in Figure 2, which compares the mapping outcomes of FastSLAM, FastSLAM-PSO, and FastSLAM-DE. The Ground Truth represents the actual trajectory of the simulated environment, while the Estimated corresponds to the mapping results generated by the FastSLAM algorithm. The green line indicates the predicted trajectory by the SLAM algorithm. Within this confined setting, both the FastSLAM-PSO and FastSLAM-DE algorithms were tested against the conventional FastSLAM. As depicted in Figure 2, FastSLAM-PSO outperforms its counterpart in both mapping and trajectory tracking, aligning more closely with the actual trajectory. This enhancement is attributed to the optimization of particles by the PSO algorithm, which enhances the precision of particle filtering and, consequently, the fidelity of the estimated path to the real environment. The integration of PSO postulates that the performance is heightened, excelling in mapping and trajectory tracking and approximating the simulated scenario with greater accuracy. The FastSLAM-DE, while showing overall mapping accuracy, exhibits noticeable distortions, suggesting that while its global alignment is generally correct, the local adjustments require refinement. This statement indeed suggests that the integration of PSO has been more effective in enhancing the performance of the FastSLAM algorithm, particularly in terms of mapping accuracy and trajectory tracking, than DE. The success of PSO in this context can be attributed to its ability to navigate the solution space more effectively, optimize particle distribution, and enhance responsiveness to local environmental changes. The capacity of PSO to balance exploration and exploitation likely contributes to its superior performance in refining the estimates derived from sensor data.

To further scrutinize the disparity between FastSLAM and FastSLAM-PSO, Figure 3 juxtaposes their trajectories in a single frame, contrasting the estimated paths with the true trajectory along the X and Y axes. In Figure 3, “Difference” refers to the deviation between the estimated paths of the FastSLAM and FastSLAM-PSO algorithms and the true trajectory in both the X and Y axes. It is evident from Figure 3 that the FastSLAM algorithm, due to the random selection of particles without targeted optimization, yields suboptimal path results that deviate from the actual path. In contrast, the incorporation of PSO refines the particles prior to particle filtering, leading to outcomes that are more congruent with the true scenario. The FastSLAM-DE, despite good alignment with the true trajectory along the X-axis, exhibits larger deviations in the Y-axis, likely due to overall distortions in its mapping approach. This suggests that while the global positioning along the X-axis is well-maintained by FastSLAM-DE, the algorithm struggles with local accuracy along the Y-axis, potentially because of greater overall distortions not adequately compensated by its evolutionary strategy. Regarding the deviation in the X and Y axes, FastSLAM-PSO generally outperforms FastSLAM and FastSLAM-DE, with minor exceptions in the Y-axis, potentially due to alignment discrepancies in post-processing. Overall, FastSLAM-PSO exhibits smaller deviations in the majority of cases, indicating that the introduction of PSO significantly bolsters the accuracy and robustness.

4.2.2. Large-Scale Scenario

The mapping results for the large-scale scenario are visually depicted in Figure 4, showcasing the challenges faced by FastSLAM, FastSLAM-PSO, and FastSLAM-DE algorithms, particularly during the exploration of elongated corridors. The FastSLAM algorithm, lacking loop closure detection, accumulates significant deviations over time, leading to trajectory drift. In contrast, the FastSLAM-PSO algorithm, empowered by PSO optimization, mitigates these deviations, demonstrating superior path prediction and map construction capabilities. Similarly, FastSLAM-DE, which employs a differential evolution optimization framework, exhibits performance akin to FastSLAM-PSO, surpassing the traditional FastSLAM. Figure 5 compares the deviations of the three algorithms along the X and Y axes, revealing that the deviations in the FastSLAM algorithm are substantially greater than those of FastSLAM-PSO and FastSLAM-DE. Both FastSLAM-PSO and FastSLAM-DE provide more precise estimates in most instances, further validating the efficacy of optimization techniques in enhancing SLAM performance. The integration of advanced optimization algorithms like PSO and DE in SLAM methods significantly improves the estimation accuracy and robustness of the algorithms, especially in complex and large-scale environments.

4.2.3. Square Scenario

In this section, the performance variability of FastSLAM, FastSLAM-PSO, and FastSLAM-DE algorithms in a square-shaped environment was examined. As indicated in Figure 6 and Figure 7, FastSLAM-PSO exhibited stable performance, whereas FastSLAM-DE did not fare as well, suggesting that algorithm applicability, parameter settings, and environmental complexity significantly impact SLAM outcomes.

The mapping results for the square scenario visually presented in Figure 6 and Figure 7 highlight the distinct performances of the FastSLAM, FastSLAM-PSO, and FastSLAM-DE algorithms. FastSLAM-DE, while showing promise in some aspects, struggled with distortions and inaccuracies, particularly along the Y-axis, which may be attributed to its optimization framework’s difficulty in adapting to the regular patterns and repeated structures present in square environments. This challenge is further exacerbated by the lack of fine-tuned parameters that could enhance its adaptability to such settings. The comparative analysis of the three algorithms in Figure 6 and Figure 7 underscores the importance of selecting an optimization strategy that complements the specific characteristics of the SLAM problem at hand. FastSLAM-PSO’s superior performance is indicative of the effectiveness in enhancing SLAM algorithms, especially in challenging environments.

4.2.4. Time Analysis

As shown in

Table 1. The time analysis results.

	FastSLAM	FastSLAM-DE	FastSLAM-PSO
Scene_1	4101.973994	7867.448255	7140.355758
Scene_2	7334.884456	16145.49226	15157.19925
Scene_3	6159.537662	12651.76538	11456.88336

the experimental results indicate that while FastSLAM achieves the fastest mapping times across all scenarios, its performance is compromised in terms of accuracy and robustness. In contrast, FastSLAM-PSO and FastSLAM-DE, despite having longer mapping times, offer significantly better performance. Notably, FastSLAM-PSO outperforms FastSLAM-DE, suggesting that the PSO algorithm is more effective in optimizing the SLAM process, particularly in complex environments. The superior performance of FastSLAM-PSO can be attributed to its ability to refine particle distribution and mitigate degradation, leading to more accurate and reliable mapping outcomes.

These findings underscore the importance of selecting an appropriate optimization strategy for SLAM algorithms. FastSLAM-PSO emerges as a balanced choice, providing a commendable trade-off between mapping time and performance. Its enhanced capabilities make it a suitable option for applications where accurate and robust mapping is critical for safe and effective autonomous navigation.

In summary, the comprehensive experimental evaluation across varying scales and complexities has conclusively demonstrated the superior performance of the FastSLAM-PSO algorithm in comparison to the conventional FastSLAM and its DE counterpart. The FastSLAM-PSO algorithm, with its integration of Particle Swarm Optimization, has exhibited a heightened level of accuracy and robustness in mapping and trajectory tracking. This enhancement is attributed to the ability to effectively manage the optimization of particles, thereby mitigating the degradation typically associated with traditional SLAM approaches. The global search capabilities of PSO have been instrumental in refining the particle distribution, leading to more reliable and precise estimation of the robot pose and the surrounding environmental landmarks. The experimental outcomes, which include comparisons across small-scale, large-scale, and square scenarios, consistently show that FastSLAM-PSO maintains its superiority in terms of mapping quality and localization accuracy.

4.3. Analysis of the Population Parameters N

In this section, we designed an analysis of algorithm performance under different particle counts N in two scenarios. It can be observed that in the small-scale scenario scene_1 as Figure 8, larger particle counts demonstrate excellent performance with virtually no deviation. In contrast, smaller particle counts exhibit significant deviations. This may be attributed to the influence of particles, where a higher number of particles can more effectively model the surrounding environment. It was noted that the deviation increased progressively in the latter part of the scenario, possibly because, in the absence of loop closure detection, a greater number of particles can more accurately identify optimal positions, thereby enhancing the precision of the algorithm. Similar behavior is observed in scene_2, as depicted in Figure 9, where smaller particle counts result in greater deviations. The long corridor setup in scene_2 also introduces a higher potential for errors with increased particle counts, hence the superior performance of smaller particle counts in the initial phase. This observation has inspired us to consider whether an adaptive adjustment mechanism could be implemented to cater to different scenarios, thereby achieving more accurate localization results.

4.4. Analysis of Mapping Results

Figure 10 illustrates the final mapping outcomes of the FastSLAM-PSO algorithm in scenario 1, highlighting the impact of varying particle counts N on the mapping quality. It is observable that when N equals 10, the mapping performance is suboptimal, characterized by distorted paths and a skewed representation of the terrain, deviating from the actual topography. As the particle count increases, the mapping accuracy improves, demonstrating a trend towards stabilization. Notably, at N = 80, N = 90, and N = 100, the mapping performance appears to plateau, indicating that the ability to refine the map does not improve significantly with further increases in particle count. This stabilization likely occurs because the algorithm has sufficient particles to adequately explore and represent the environment, making the addition of more particles redundant. It is evident that the incorporation of PSO can partially compensate for the absence of loop closure detection, thereby enhancing the mapping results. However, at N = 100, the mapping quality is not as optimal as at N = 90, which may be attributed to the increased likelihood of errors arising from a higher number of particles.

To further elaborate, the mapping results are significantly influenced by the number of particles utilized in the FastSLAM-PSO algorithm. An inadequate particle count leads to a poor representation of the environment, with inaccuracies in both path tracing and terrain mapping. Conversely, an optimal particle count ensures a more accurate and reliable mapping outcome. The introduction of PSO serves to refine the particle distribution, reducing the degradation issues typically associated with particle filters and improving the ability to adapt to environmental changes. The decrement in performance at N = 100 suggests that there is an upper limit to the number of particles beyond which the benefits of increased diversity are offset by the potential for increased noise and error. This observation underscores the importance of calibrating the particle count to achieve a balance between exploration and exploitation, ensuring that the algorithm remains robust and efficient.

5. Conclusions

In this study, we have presented an innovative enhancement to the Simultaneous Localization and Mapping framework by integrating Particle Swarm Optimization with the FastSLAM algorithm. This integration was designed to address the critical issue of particle degradation, which commonly affects the accuracy and robustness of SLAM in complex and dynamic environments. Our approach leverages the global search capabilities of PSO to optimize the distribution of particles, thereby enhancing the performance of the FastSLAM algorithm. We introduced a novel fitness function tailored to the PSO algorithm, which was instrumental in guiding the particles toward regions of higher posterior probability. This strategic enhancement not only mitigated the degradation of particles but also improved the overall estimation accuracy and robustness of the SLAM solution across various operational scenarios. Through a series of meticulously designed experiments, we validated the effectiveness of our proposed method. The experimental results demonstrated that our PSO-enhanced FastSLAM algorithm significantly outperformed the standard particle filtering approach, particularly in terms of localization accuracy and robustness against particle degeneration.

The superiority of FastSLAM-PSO lies in its ability to maintain high accuracy and robustness in environments with high noise levels and large-scale spaces, which is a common challenge faced by traditional SLAM algorithms. Unlike methods that struggle with non-Gaussian noise, our PSO-enhanced FastSLAM algorithm uses the global search capability of PSO to effectively optimize particle distribution, leading to improved estimation accuracy and robustness. This advancement positions our method at the forefront of SLAM technology, offering a promising solution for real-world applications where traditional algorithms fall short. The insights gained from this research have highlighted the potential of swarm intelligence techniques in enhancing SLAM algorithms. The success of our approach suggests that by optimizing the particle distribution, we can achieve more precise and reliable SLAM performance, even in challenging environments characterized by high noise levels and large-scale spaces. This study has also inspired future work, which will focus on developing adaptive mechanisms that can dynamically adjust the particle count and other PSO parameters in real time based on the environmental conditions and the state of the algorithm.