Multisensor Fusion SLAM Research Based on Improved RBPF-SLAM Algorithm

,


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 realworld 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 [8][9][10][11][12][13][14][15].

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 [18][19][20].The multisensor fusion process using the EKF method is shown in Figure 1 [21][22][23].
For the mobile robot Keepbot, to accurately describe the state of the robot at time k, the pose of the robot at time k is denoted as x k [24]: As shown in Equation (1), X k ; Y k ; θ denote the coordinates of the robot in the world coordinate system and its angle in the initial direction, v x denotes the movement speed of the robot in the X-axis direction, v y denotes the movement speed of the robot in the Y-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), H ok denotes the observation matrix of the wheeled odometer, I 6 is a unit sixth order matrix, and w ok d ð Þ 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 Z-axis.The observation equation of the IMU is shown in Equation ( 4) [25,26]: As shown in Equation ( 4), H ik denotes the observation matrix of IMU, and w ik d ð Þ is the covariance matrix of the observation error of IMU data and obeys Gaussian distribution [27][28][29].To sum up, the specific process of the EKF-based multisensor fusion algorithm is presented in Algorithm 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 y k 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), u k−1 denotes the odometer measurement information at time k 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

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 k.
ALGORITHM 1: The specific flow of the EKF-based multisensor fusion algorithm.

Journal of Sensors 3
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 L i ð Þ of the observation distribution, and the distribution of the set of sampled points x t f g is approximated as a Gaussian distribution.The parameters k of this Gaussian distribution can be calculated using the point set x t f g.
Finally, the new particle point set and the corresponding weights are generated using sampling of this Gaussian 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 x 0 i ð Þ k of the particle point, and then the area near k is searched to calculate the match between the observation z k and the existing k 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 L i ð Þ of the observation distribution can be defined as k with the highest match is the area of peak probability of interval L i ð Þ .A fixed number of K points x t f g are randomly picked in the region centered at b k with a radius of Δ, where the sampling of each point is shown in Equation ( 8): By approximating the distribution of the sampled point set x t f g as a Gaussian distribution and considering both the motion model and the observation information, the parameters k of this Gaussian distribution can be calculated from the point set x t f g, as shown in Equation ( 9): As shown in Equation ( 9), 6) can be transformed into Equation (10): n o 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), e w i ð Þ k 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 N eff .
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 N=2, where N is the total number of particles.Only when N eff is smaller than N=2, 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.

4
Journal of Sensors 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.The simulation of the interior created in ROS through the Gazebo simulator is shown in Figure 5.

Results and Discussion
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 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 x and calculate the match of observation z k with the existing map m 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 b x i ð Þ k 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 k , and η i ð Þ of the Gaussian distribution at the moment k of the i − th particle are calculated, and then the Gaussian distribution and update the particle weights.
Step 7 After updating all the particles at moment k, the number of all valid particles is calculated to determine whether resampling is needed.
Step 8 Update the map for moment k and reacquire sensor information for the next moment of map construction.
Calculating the parameters of the Gaussian distribution μ k (i) , Σ k (i) , η (i)   Calculate and update the weights of the particles 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.

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).

Experimenting with Building Maps in Real-World
Scenarios.In real-world scenarios, robots encounter numerous uncertainties and complexities that are not present in  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 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.

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.

FIGURE 2 :
FIGURE 2: The likelihood function distribution of the observed model and the motion model.

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 Figure4.

ALGORITHM 2 :ʹ
The process of the improved RBPF-SLAM algorithm.Multisensor information fusion and construction of motion models by EKF methodAccording to the robot motion model, get the initial position x k improved proposal distribution x k

FIGURE 3 :
FIGURE 3: The flowchart of the improved RBPF-SLAM algorithm.

FIGURE 5 :FIGURE 6 :
FIGURE 5: The indoor simulation environment created in Gazebo.

FIGURE 7 :FIGURE 8 :
FIGURE 7: The block diagram of the mobile robot Keepbot architecture.

TABLE 1 :
The computation time of each step of the algorithm before and after improvement with different numbers of particles.