Abstract

Simultaneous localization and map construction (SLAM) technology provides the foundation for indoor robots to realize autonomous path planning. The Rao-Blackwellized particle filtering (RBPF) algorithm is widely used to obtain information and perform map construction in unknown environments. This paper proposes a multisensor fusion algorithm that improves the RBPF-SLAM algorithm by addressing the issues of particle distribution errors and degradation. To achieve this, the extended Kalman filter (EKF) is first adopted to effectively fuse odometry and inertial navigation (inertial measurement unit (IMU)) data as the initial positional information. Then, a high-precision light detection and ranging (LIDAR) observation model is integrated to calculate the proposed distribution, and a threshold is introduced to determine the number of valid particles to simplify the resampling step. Finally, raster map construction experiments were carried out on both the Gazebo simulation environment and Robot Operating System (ROS)-based mobile robot Keepbot platform in different scenarios. The experimental results demonstrate that the optimized RBPF-SLAM algorithm generates a clearer and more complete map outline, which can effectively improve the accuracy of robot pose estimation, acquire a reliable 2D raster map with a reduced number of particles, and greatly reduces the computational effort.

1. Introduction

With the rapid development of robotics, mobile robots are increasingly used in various fields, including manufacturing, industry, and service sectors. Simultaneous localization and mapping (SLAM) is a key technology for the autonomous path planning of mobile robots. When mobile robots operate in unknown environments, they rely on sensors such as wheeled odometers, accelerometers, and gyroscopes to perceive changes in the robot’s attitude per unit of time. However, there is a challenge in integrating the observations from external sensors (e.g., light detection and ranging (LIDAR), depth cameras, etc.) to update the robot’s position and map information. Today, the mainstream solutions for improving the accuracy of robot pose prediction in map construction include filter-based SLAM methods and graph optimization-based SLAM methods. The former enables online map updates, with good real-time performance and high performance in small and medium scenes. However, as the environment scale increases, filter-based SLAM methods can cause error accumulation, resulting in inconsistencies in map construction. The latter realizes accurate mapping of large-scale environments through global optimization processing at the cost of consuming significant computational resources. In this paper, a SLAM algorithm based on the Rao-Blackwellized particle filtering (RBPF) is proposed for accurate localization and mapping of small and medium indoor scenes and application to autonomous mobile robots [1].

The particle filter (PF) is a recursive Bayesian filtering algorithm for nonlinear non-Gaussian systems based on the Monte Carlo principle. It approximates the posterior probability distribution by a series of state particles randomly sampled through the posterior probability distribution. This can be naturally applied to the study of SLAM [2]. Thrun and Montemerlo [3] introduced the PF algorithm to solve the SLAM problem of mobile robots. By assigning particle weights to obtain the posterior probability distribution of the state of the robot system, more accurate localization is achieved, but the complexity of constructing the map increases with the number of particles. To solve the problems of low efficiency and high computational complexity of constructing maps in PF algorithms, in a study by Doucet et al. [4], proposed to integrate the RBPF into the SLAM solution. RBPF-SLAM decomposes the process of robot map construction into robot pose estimation and pose-based map estimation, thereby greatly reducing the computational effort of the SLAM method, but the method still suffers from the particle depletion problem due to the dependence on the number of particles required for map construction; in a study by Wu et al. [5], an RBPF-SLAM algorithm is proposed under quantum particle swarm optimization (QPSO), which effectively prevents particle degradation and maintains particle diversity by introducing a quantum particle swarm optimization algorithm to update particle poses during resampling. Then, particle species are classified according to weights, and the resulting particle set is optimized and adjusted.

Mobile robots rely on sensors to position themselves accurately and reliably. However, the limitation of a single sensor can cause performance drift, especially when the robot moves quickly or steers at large angles, making it difficult to guarantee the reliability of the information provided at any given time. Therefore, multisensor fusion technology is proposed for SLAM to provide more comprehensive environmental information, thereby enhancing the mobile robot’s ability to sense its surroundings. Currently, four main methods are used for sensor data fusion: estimation theory, classification, inference, and artificial intelligence. In a study by Wang et al. [6], high-progress LIDAR data were used to correct the proposed distribution function based on odometer readings to reduce the number of particles required in the filtering process; in a study by Wang et al. [7], motion odometry was combined with LIDAR measurement data to adjust the particle weights and reestimate the particle weights by adaptive resampling to obtain accurate raster maps.

Based on the above methods, this paper proposes a multisensor fusion algorithm based on improved RBPF-SLAM by investigating and analyzing the traditional RBPF-SLAM algorithm and addressing the problems of large particle distribution errors and particle degradation in the PF algorithm part. First, the extended Kalman filter (EKF) is used to fuse the wheeled odometer data with inertial measurement unit (IMU) data, and the resulting positional information is employed to construct the initial position estimation model for the mobile robot. Then, the observed scan information of the LIDAR sensor is introduced to optimize the proposed distribution of sampled particles. Next, the resampling step is improved by evaluating the effective particle number, which reduces the computational time and improves the algorithm’s efficiency. Finally, the improved RBPF-SLAM algorithm is validated through simulation experiments in the Gazebo simulation environment and real-world experiments using a hardware platform based on the Robot Operating System (ROS). The experimental results indicate that compared to the traditional algorithm, the proposed algorithm significantly enhances map-building accuracy and computational efficiency [815].

2. Materials and Methods

2.1. EKF-Based Multisensor Fusion Algorithm

The Kalman filter (KF) algorithm estimates the global optimum based on observed values and system states [16, 17], but it can be only applied to linear systems. However, the map-building environment of mobile robots usually lacks linearity, making it necessary to use the EKF method to fuse the information obtained from the wheeled odometer and IMU sensors [1820]. The multisensor fusion process using the EKF method is shown in Figure 1 [2123].

For the mobile robot Keepbot, to accurately describe the state of the robot at time , the pose of the robot at time is denoted as [24]:

As shown in Equation (1), denote the coordinates of the robot in the world coordinate system and its angle in the initial direction, denotes the movement speed of the robot in the -axis direction, denotes the movement speed of the robot in the -axis direction, and is the rotation angle of the robot. The corresponding system state transfer equation under EKF is derived, as shown in Equation (2):

Then, the observation equation for the robotic wheel odometer can be expressed, as shown in Equation (3):

As shown in Equation (3), denotes the observation matrix of the wheeled odometer, is a unit sixth order matrix, and is the covariance matrix of the prediction error and obeys Gaussian distribution. Since the mobile robot Keepbot only builds graphs in two dimensions, there is no need to consider the data in the IMU in the -axis. The observation equation of the IMU is shown in Equation (4) [25, 26]:

As shown in Equation (4), denotes the observation matrix of IMU, and is the covariance matrix of the observation error of IMU data and obeys Gaussian distribution [2729]. To sum up, the specific process of the EKF-based multisensor fusion algorithm is presented in Algorithm 1.

EKF-based sensor fusion algorithm for wheeled odometer and IMU
 Step 1 Acquire sensor data from the wheeled odometer and IMU.
 Step 2 Construct an EKF using a nonlinear model of a wheeled odometer.
 Step 3 Start status updates to the system and add system noise.
 Step 4 Update the system state quantities and the system covariance matrix by combining the state quantities of the previous moment and listening to the odometer information as the observed quantities and the observed covariance matrix.
 Step 5 Update the system state quantities and system covariance matrices obtained in Step 4 with the state by listening to IMU information as observation quantities and observation covariance matrices.
 Step 6 Use the fused system state quantities and covariance matrix as the initial bit poses of the SLAM algorithm.
 Step 7 End sensor information fusion at moment .
2.2. RBPF-SLAM Algorithm Based on Improved Particle Filtering
2.2.1. Optimize the Proposed Distribution of the Particle Filtering Part

The conventional RBPF-SLAM algorithm employs the motion model directly as the proposed distribution, making it easier to obtain the motion model for most robots, but the observation information from high-precision sensors such as LIDAR is discarded, resulting in low accuracy of the built map. In addition, the weight of particles with high posterior likelihood values in the motion model is too large, leading to a large weight difference between individual particles, and in this case, the diversity of particles will be reduced, and the particle degradation problem is serious. Therefore, the processing of the proposed distribution in the particle filtering part is enhanced by adding the observed model of the LIDAR scan to the above fused motion model as a hybrid proposed distribution to achieve more efficient sampling. Specifically, the sampling proposal distribution is expressed, as shown in Equation (5):

As shown in Equation (5), denotes the odometer measurement information at time denotes the known map information of the environment at time . The corresponding weights of the particles are calculated, as shown in Equation (6):

As shown in Figure 2, the variance of the high-accuracy LIDAR match is significantly lower than that of the odometer model. The laser odometry probabilistic model covers a large range smoothly, while the observation model has a small distribution, indicating a low probability of falling within the displayed range. Thus, sufficient particles are needed to fill the distribution of the observation model. If the observation recorded by LIDAR is considered to be corresponding to particle sampling, a large enough sample can be available to solve this problem.

Therefore, the observation update process can be divided into two cases. When the observation reliability is low, the default motion model is utilized to generate the new particle point set and the corresponding weights; when the observation reliability is high, it is sampled within the interval of the observation distribution, and the distribution of the set of sampled points is approximated as a Gaussian distribution. The parameters and of this Gaussian distribution can be calculated using the point set . Finally, the new particle point set and the corresponding weights are generated using sampling of this Gaussian distribution .

Then, it is necessary to determine which method to use for the observation update process before sampling. First, the motion model is utilized to derive the new locus of the particle point, and then the area near is searched to calculate the match between the observation and the existing map . If the presence of in the search area makes the match highly reliable, the observation is considered highly reliable. The specific process is shown in Equation (7):

When the observation degree is relatively high, the range of the interval of the observation distribution can be defined as . The searched bit pose point with the highest match is the area of peak probability of interval . A fixed number of points are randomly picked in the region centered at with a radius of Δ, where the sampling of each point is shown in Equation (8):

By approximating the distribution of the sampled point set as a Gaussian distribution and considering both the motion model and the observation information, the parameters and of this Gaussian distribution can be calculated from the point set , as shown in Equation (9):

As shown in Equation (9), .

Then, Equation (6) can be transformed into Equation (10):

2.2.2. Improved Particle Resampling Strategy

After the new particle point set and the corresponding weights are generated, resampling can be performed. When every update of the particle point set is resampled using the weights, if particle point weights do not vary much in the update process or if some bad particle points have a larger weight than good points as a result of noise, resampling will cause the loss of good points [30]. Therefore, a reasonable implementation standard needs to be established to ensure the effectiveness of resampling. The improved resampling strategy measures the effectiveness through the parameters, as shown in Equation (11).

As shown in Equation (11), is the normalized weight of the particles. When the approximation between the proposed distribution and the target distribution is sufficiently good, the weights of the individual particle points are similar. However, when the approximation is poor, the weights of individual particle points vary more, indicating that a certain threshold can be used to determine the validity of parameter .

The basic idea of threshold determination is to organize the map as a balanced binary tree, so that only one path in all the trees representing the features in the map is modified during the particle update process, and then the map can be updated in logarithmic time, thus saving a lot of memory consumption and speeding up the map update process. So, this paper sets the threshold to , where is the total number of particles. Only when is smaller than , resampling is performed. Therefore, the number of calculations is reduced, which greatly simplifies the resampling process and avoids causing the loss of good particle points.

2.2.3. Improved RBPF-SLAM Algorithm Flow

The process of the improved RBPF-SLAM algorithm is shown in Algorithm 2, and the flowchart of the improved algorithm is shown in Figure 3.

Step 1 Receive sensor data information and fuse sensor data from the odometer and IMU by the EKF method.
Step 2 Construct a robot motion model based on the fused positional information and estimate the initial positional at moment .
Step 3 Generate the map at moment based on the initial position of the particle. Then, search in the area near and calculate the match of observation with the existing map .
Step 4 When the scan match is low, the particle weights are updated directly based on the motion model sampling and calculation.
Step 5 When the presence of in the search region makes the match highly reliable, the observation model obtained from the LIDAR by scan matching is fused with the robot motion model to obtain a new proposed distribution function, and the particles are sampled from the improved proposed distribution.
Step 6 The parameters , and of the Gaussian distribution at the moment of the particle are calculated, and then the Gaussian distribution sampling is used to generate a new particle point set and update the particle weights.
Step 7 After updating all the particles at moment , the number of all valid particles is calculated to determine whether resampling is needed.
Step 8 Update the map for moment and reacquire sensor information for the next moment of map construction.

3. Results and Discussion

3.1. Experiment and Analysis
3.1.1. Graph-Building Algorithm Simulation Experiment

To verify the effectiveness of the improved RBPF-SLAM algorithm, a comparative analysis of the traditional RBPF algorithm and the improved algorithm is simulated and tested. Simulation is performed on a computer equipped with Inter(R) Core(TM) i7-7700H CPU (2.80 GHz) and 16 GB RAM, running the Ubuntu 18.04 and ROS melodic system. The Keepbot robot model is a robotic URDF model created in ROS, as shown in Figure 4.

The simulation of the interior created in ROS through the Gazebo simulator is shown in Figure 5.

The keyboard control node is enabled to control the Keepbot simulation model to build maps in the indoor simulation environment using the traditional RBPF-SLAM algorithm and the improved RBPF-SLAM algorithm, respectively. In the simulated experimental tests, the minimum number of particles used in the traditional RBPF-SLAM algorithm was 40, with an average building time of 378 s, and the minimum number of particles used in the improved algorithm was 15, with an average building time of 306 s. The experimental results of the traditional RBPF-SLAM algorithm and the improved RBPF-SLAM algorithm for building maps in the simulation environment are shown in Figures 6(a) and 6(b), respectively.

The experimental results indicate that the traditional RBPF-SLAM algorithm has a long processing time and utilizes an excessive number of particles. The overuse of particles makes the proposed distribution too scattered, causing inaccurate displays of some obstacle edges. In contrast, the improved RBPF-SLAM algorithm incorporates sensor observation and robot motion models to suggest the distribution function for particle sampling. This reduces particle dissipation and enables more precise map building with fewer particles, indicating that the improved hybrid distribution model closely approximates the mobile robot’s real positional distribution model.

3.2. Real-World Environment-Building Experiments
3.2.1. Experimental Platform

The experimental platform used in this paper is the Keepbot mobile robot, which features a differential wheel drive model, and the robot’s hardware platform consists of two parts: an upper processing platform and a bottom control unit. The two parts communicate through a USB serial connection. The upper computer is responsible for real-time data collection and processing of LIDAR information and issuing control signals according to received instructions. The lower computer drives the motor to control the robot’s movement according to real-time command signals. The Keepbot robot is equipped with a hardware connection framework for each sensor (see Figure 7).

3.2.2. Experimenting with Building Maps in Real-World Scenarios

In real-world scenarios, robots encounter numerous uncertainties and complexities that are not present in simulation environments. To evaluate the performance of the traditional RBPF-SLAM algorithm and the proposed improved RBPF-SLAM algorithm, map reconstruction experiments are carried out in the same scenarios through the same trajectory. Specifically, the algorithms were tested in three distinct environments: a laboratory office scene, a long corridor outside the laboratory, and a corner of the fourth-floor corridor. The actual environment is shown in Figure 8.

The following shows the robot mapping results before and after applying the improved algorithm, respectively. Undetected areas of the map are marked in dark gray, while obstacles or other features are displayed in black. As shown in Figure 9, in the long corridor scenario with a single piece of environmental information, the effect of building the map does not differ significantly. However, with the conventional RBPF-SLAM algorithm (which only uses the odometer motion model as the proposed distribution for sampling), when the odometer error accumulates over time, the constructed map contains false walls and drifts in the red circle area. In contrast, the maps constructed using the improved RBPF-SLAM algorithm match the real environment with high accuracy. Figures 10 and 11 illustrate the significant differences between the effect of maps before and after improvement in the laboratory office scene with richer environmental information. Since the odometer provides only positional information, the map constructed by the traditional RBPF-SLAM algorithm exhibits position drift, and particle degradation occurs when the running time is too long, leading to insufficient particle diversity. Consequently, the created maps deviate from the real environment at several locations, resulting in a poor building effect. By contrast, the improved algorithm produces maps that closely resemble the real environment, thereby effectively addressing the issue of building accuracy degradation due to particle degradation.

In the real experiments, as shown in Figures 911, the average building time of the conventional RPBF-SLAM algorithm is 257, 346, and 266 s, and the average building time of the improved RPBF-SLAM algorithm is 189, 288, and 202 s, respectively. To further analyze the map-building effect of the algorithm proposed in this paper, multiple locations were selected in the established map, and measurements were analyzed for these locations. The selected measurement locations were labeled, as shown in Figure 12, and the errors between the measured and true values of each measurement location are shown in Figure 13.

To further investigate and analyze the computing time of the RPBF-SLAM algorithm before and after the improvement, the computing time required for different steps of the algorithm before and after the improvement with different numbers of particles is presented in Table 1. It can be seen from this data that the computation time of both algorithms is proportional to the number of particles used. However, for the same number of particles, the improved algorithm consumes much less time in updating particles and resampling than the traditional RBPF-SLAM algorithm. The combined effect of building the map shows that using the robot motion model superimposed with the sensor observation model as the proposed distribution can obtain more accurate maps with fewer particles. The hybrid proposal distribution is closer to the real positional information of the robot.

4. Conclusions

This paper proposes a multisensor fusion SLAM algorithm that addresses issues of large particle distribution errors and particle degradation in the particle filtering stage of the traditional RBPF-SLAM algorithm. First, the EKF method is used to fuse odometer and IMU data, constructing a motion model that serves as the algorithm’s initial input. Then, observed LIDAR scan information is introduced to optimize the distribution of sampled particles. Meanwhile, the resampling process of particles is improved by implementing a threshold to determine the number of valid particles, thereby mitigating the particle dissipation problem and enhancing the algorithm’s efficiency. Finally, to validate the effectiveness of our algorithm, it was tested in the Gazebo simulation environment in ROS, and experiments were also conducted on a Keepbot robot hardware platform based on the ROS system to construct raster maps in real scenarios. The results of these experiments indicate that our improved RBPF-SLAM algorithm can construct maps with higher accuracy and precision while consuming less computation time than the traditional algorithm.

Data Availability

The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest

The authors declare that there are no conflicts of interest regarding the publication of this paper.

Acknowledgments

This work was supported by the Natural Science Foundation of China under Grant No. 61673108, the Natural Science Foundation of Jiangsu Province under Grant No. BK20181050, the Industry-University-Research Cooperation Project of Jiangsu Province Grant Nos. BY2020335 and BY2020358, and the Yancheng Institute of Technology Postgraduate Practice Innovation Program Project under Grant No. SJCX22-XZ037.