Grey Wolf Resampling-Based Rao-Blackwellized Particle Filter for Mobile Robot Simultaneous Localization and Mapping

,


Introduction
In a completely unknown environment, the mobile robots just relying on their carrying sensors to get environment information and successfully make simultaneous localization and mapping (SLAM) is a basic step to realize autonomous intelligent navigation [1].Meanwhile, it is a strategy that could be useful in a wider area of research, such as the motion control agents and microfluidics [2,3].Extended Kalman filter (EKF) is the most popular method used in the SLAM [4,5].However, the SLAM based on the EKF cannot create a large map and it also has a problem of the data association computation efficiency.erefore, in recent years, the RBPF [6,7] has been introduced as an effective method to solve the SLAM problem.e core idea of the RBPF technique is to use a particle filter (PF) and the Kalman filter (KF) to predict features in the map, where each particle represents a robot position and attitude.But there exists a new problem; that is, the particles have difficulty in the proposal distribution selection and resampling.In the SLAM problem, the experience tells us that we can select the robot motion model as the proposal distribution, but the method of PF resampling is still less efficient and accurate.Unfortunately, the resampling technique is more important, which can not only avoid particles degeneracy but also determine the robot localization accuracy.e traditional resampling method only pays attention to the identical distribution (ID) attribute [8] of the particle in the posterior probability prediction but ignores the particle diversity, which leads to poor robot positioning prediction accuracy [9].e contribution of this paper lies in the following: (1) introducing an artificial intelligent grey wolf optimization (GWO)-assisted resampling scheme for making the diversity of particles and (2) proposing an adaptive local data association (Range-SLAM) for NN's efficiency.e above two schemes solve the most intractable problem of conventional RBPF for SLAM.In our scheme of resampling proposed, it can not only guarantee the ID property but also not lose the diversity of particles.In addition, the proposed new data association method improves the computing efficiency in the RBPF.erefore, the improved RBPF becomes more effective and flexible in the SLAM.e organization of this paper is as follows: after reviewing the normal RBPF, we explain the Rao-Blackwellized resampling idea in Section 3. Sections 4 and 5 introduce the exploration techniques of GWO-assisted resampling.Section 6 describes a new data association method.Finally, Section 7 contains the experimental results carried out on real robots as well as in the simulations.Section 8 concludes this study.

Rao-Blackwellized Filter Review
According to Murphy [10], the key idea of solving the SLAM problem with a Rao- (1) ) is linear, it can be optimally estimated using a KF.To obtain the second term p(x p t |Y t ), it is necessary to apply nonlinear filtering techniques (the PF will be used).It is clear that the PF can be sampled in a lower dimensional state space (e.g., 3 dimensions), when x p t represents the pose of mobile robot.en the PF and KF can estimate obstacles in map with a known mobile robot pose.
For a mobile robot, the general movement and observation equations can be defined by where u t is the control signal; ε t and η t represent the Gaussian white noise with 0 mean value and their covariances are Q t and R t .
In the estimation of RBPF-SLAM algorithm, a probability ranging motion model is usually used as the recommended distribution: where the proposal distribution probability density function q is a sampling density function.en, the importance weight is calculated to compensate for the difference between the proposal distribution (Sequential Importance Sample, SIS) and the target distribution for the pose of the robot by which is proportional to a measurement likelihood.en, the obtained weights are normalized as follows: For dividing the state vector X t into two parts, the model of the state can be decomposed into linear/nonlinear Gaussian system [11,12] as follows: where the state noise is assumed to be white and Gaussian distributed with e measurement noise is assumed to be white and Gaussian distributed according to Furthermore, x p 0 is Gaussian: e density of x k 0 can be arbitrary, but it is assumed to be known.e Rao-Blackwellization for filtering algorithm is given in Algorithm 1.For the system equation (6), see (7)-(9) for system properties.
e resampling method (Sample Importance Resampling, SIR) is [9] to randomly get rid of unimportant particles and replace them with more likely ones.Generate a uniform distribution of random numbers.Let  μ ∼ U(0, N − 1 ), sw � 0, and j � 1, where sw is the particle accumulation and the initial value is zero.Detailed algorithm is given in Algorithm 2.

Resampling (SIR) Analysis
With the particle filter, if the described RBPF runs with exactly the steps described above, it will end up with all the particle weight in one single value.is will degrade estimation performance, because the normal resampling method above is just sure of ID for resampling (details below) and not sure of diversity of particles.Actually, there are two important factors that affect PF estimation performance: one is the proposal distribution which has been solved by motion model p(X t|t−1 |X t−1|t−1 ), and the other one is the resampling; let us talk about it in detail.
Why resampling? e motivation of resampling includes, but is not limited to, combating sample degeneracy (while avoiding impoverishment) and adjusting the number of particles online, essentially reducing the probability that the filter loses tracking and localization.However, notice 2 Journal of Robotics that resampling basic principle is abided by ID for resampling as well as the diversity of particles [8,9].Now a consistent ID metric and the sampling variance are introduced for assessment of the ID attribute of resampling.Discrepancies metric between the numbers of times that the particles are resampled is where N is the size after resampling, M is the size before resampling, N (m) t � floor(Nω (m) t ), and Floor(•) gives the largest integer not exceeding the content.e smaller the SV is, the better the ID quality of the resampling method is.If and only if the two distributions are exactly the same, the SV given in equation ( 10) is essentially a cost function that provides an efficient metric to measure the discrepancy between the discrete distributions of weighted particles before and after resampling.
From resampling analysis above, the problem of particle weight degeneracy and impoverishment is conventional PF associated and difficult to completely remove.Particle weights will be with serious degradation after using normal resampling (although with a perfect ID), and then particle impoverishment will also be more serious [8], so preventing and solving the dual problem are expected.
In order to overcome the problems caused by resampling, this paper designs a kind of intelligent resampling system; namely, in the sampling space, by means of searching optimal algorithm [13], we search particles with the approximate weight of the highest weight of the particle (but not the same as it), to replace the low weight of the particles in degeneracy.It is not like the normal resampling method above to just copy high weight particles (particles are the same).is will not only ensure the diversity of particles but also ensure that all of the weights of the particles have a high probability with SLAM iterations.Now let us firstly introduce a searching optimization algorithm, that is, GWO.
ALGORITHM 1: e Rao-Blackwellization for filtering algorithm. [ t num � 0 (the number of the i-th particle copied times and the initial value is zero.) Journal of Robotics

Grey Wolf Optimization
GWO is a new kind of metaheuristic biological intelligence algorithm proposed by Mirjalili et al. [14].e process of imitating the hierarchical leadership and hunting mechanism of wolves during natural wolf hunting is shown in Figure 1.For detailed algorithm, we refer the reader to Seyedali's work.

Grey Wolf Optimization-Assisted Resampling
When resampling, we must keep particles away from impoverishment and invariability.If the resampling has to be taken when particles get the degeneracy, we give up the least weight particles, instead of just mechanically copying the large weight particle and giving a 1/N weight to each one; we do this as follows.
Firstly, we give GWO a searching maximum objective function F as follows: where a is a constant, x p tHW is the particle with highest weight, and Y t is a known vector that comes from the observation.In the searching algorithm, parameters x p t to be optimized should obtain a clearly defined scope.In this paper, the wolf algorithm searching range is selecting the maximum weight of the particle to be the center and extending to two sides: where λ is an arbitrary constant, which can be used to adjust to enlarging and narrowing the range of variation of particles.Above all, we can find that GWO resampling can keep particles away from impoverishment and invariability, instead of just mechanically copying the large weight particle and giving a 1/N weight to each one.Of course, for the SLAM problem, we should know, in F function, that Y t that comes from the observation should be obtained from data association, and it is also an unavoidable SLAM problem.Let us talk about it in the next section.

Data Association and its Efficiency Analysis
SLAM refers to a mobile robot in an unknown environment gradually establishing a map of the surrounding environment and determining its own posture.In the SLAM process, the data association is a key issue for real-time map estimation and update based on observational information.
If the valid sensor observations correspond to landmarks presented in the map, they can all form a pair of associations to provide new information for SLAM update; if not, sensor observations will be new landmarks stored in the map.ere are a lot of data association methods, but the NN data association algorithm based on Mahalanobis distance has been widely used in SLAM [15,16].In this paper, we will still use NN data association but improve it.e NN algorithm is described as follows.Perform t + 1 time observation and the NN data association in detail.
Assume that the sensor returns n observations z � z 1 , z 2 , . . ., z n  .For n observations, we compute Mahalanobis distance D ni between each z n and each stored landmark LM i , i � 1, 2, . . ., M { } in the map, and D is the minimum of D ni : where Zp � h(X P t+1|t , X ) is the expected observation of ith landmark that existed in map.e innovation covariance matrix for the expected observation is where H t+1|t � (zZ p/zX t+1|t ) is the Jacobian matrix of the system of observation equations.e general assumption is (1) If there is a new observed landmark by sensor and data association is made in the map, then it will need to compute n * M times with NN to solve the SLAM problem.With the increment of the number of observed new landmarks, M will be expanded largely, and computation is increased.(2) For PF in RBPF, its dimension of the system state vector is 3, but, unfortunately, the dimension of the system state vector of KF is m, where m represents the existing landmarks in the map right now.In large and complex environment, with the increment of the number of observed new landmarks, the dimension of the system state vector of KF will also increase.
Computation of covariance matrix and other matrices will be dramatically increased.
To avoid reducing the operating efficiency and localization inaccuracy, we propose a SLAM algorithm with adaptive observation range (Range-SLAM).e principle of the algorithm is to use a circular local map to estimate the position and attitude of mobile robot in the world coordinate system, while updating the local map, as shown in Figure 2(a).

4
Journal of Robotics In Figure 2, S represents circular local map; S1 represents observation range; S2 represents constraint area; black points represent landmarks.First, we need to detect whether there are new landmarks in area S1 by constraint conditions: �������������������������� where Robot real−position � x real k y real k   T is the robot real position in map.m j � m x j m y j

􏽨 􏽩
T is the landmark in observation range; R is an adaptive radius scalar.If it is a new landmark, then add it to the system state vector matrix.At the same time, for the number of landmarks NUM in area S, if NUM > NUM max , then reduce the radius R of circle S; otherwise, if NUM < NUM min , then increase the radius under the premise of R min ≤ R ≤ R max .
For this purpose, we just do the data association in S space and not in the entire map space of the landmarks with one to one association; at the same time, the KF state vector is also limited to the dimension of the S space. is will greatly reduce the computational complexity of the data association, while a more important thing is to reduce the complexity of the KF in the process of computing.It is just like the memory of a computer; S space only works as the current operation of the domain, and the entire map is like the hard drive; the robot will store and refresh S space landmarks in the process of detecting new landmarks.e proposed method is easy but a valid one; we can finally determine mobile robot position and attitude accurately by RBPF above.

Experiment
In this paper, we take two wheels' differential driving mobile robot as an experiment.Its motion model is shown in Figure 3. e state of mobile robot of world coordinate system is represented by the position and orientation vector X k � [x k , y k , θ k ] T , where X k � [x k , y k ] T are coordinates of mobile robot in k moment; θ k is the angle between the forward direction of mobile robot and the positive direction of x-axis of the world coordinate system.e linear velocity υ k and rotational angular velocity ω k of mobile robot in k moment are where υ R,k , υ L,k are the linear velocities of the right and left wheels; L-axis is the spacing between two wheels.Assuming that the speed of the mobile robot is uniformly changed from k − 1 moment to k moment, the kinematic model of the mobile robot is where Δt is the sampling period.
For the state observation model, in this paper, we set that there are totally N landmarks and the coordinates [m x i , m y j ] T (i � 1, 2, . .., N) do not change; then the observation model of mobile robot is where r i k is the distance between the mobile robot and landmark i; ϕ i k is the angle between the forward direction and connection line of mobile robot.7.1.Sensor Features Detection.Sixteen Polaroid sonar sensors are individually with a beam width of 30 °. e robot explored the home environment and was manually controlled at an average speed of 0.2 m/s.During this exploration phase, the sonar ring acquired range data at a frequency of 1 Hz (i.e., 16 data sets per 1 s).
By testing the SLAM performance of the improved RBPF, we get a successful experiment of robot localization and mapping.In Figure 4(a) and Figure 4(b), the black boxes are obstacles located by ultrasonic sensors and they have been mapped by improved RBPF. e blue trajectory is the robot motion localization; light green small arcs are the ultrasonic scanning signs.e average localization error has been compared with other filtering algorithms; with the iterations increase, the proposed improved RBPF has a faster convergence than others, as shown in Figure 5.
e improved RBPF in the SLAM problem not only reduces the localization error but also has a faster convergence, because the GWO algorithm is used to assist in the resampling.
e before and after resampling particles change trend is shown in Figure 6.
In Figure 6, a large swarm of red particles in degeneracy needs to be resampled, and the resampling method is the GWO; namely, remove the amount of N particles with small weights before resampling and producing the same amount of N particles in the vicinity of the approximate maximum weight of particle to replace the original ones after resampling.e new generation particles are represented by the black dots in Figure 6; the distribution of black dots does not have an ideal distribution but not far away from the maximum weight, since the GWO encircling wolves have a small distance from the leader wolf.Such distribution is enough to satisfy the SV value in a very small range and the change of SV curve shown in Figure 7, and all the results prove that GWO resampling has a good ID quality.e most important result is that GWO resampling not only ensures the diversity   Journal of Robotics of particles but also ensures the particles distribution approximate maximum weight after resampling. is paper also proposes a new local data association method; experiments with the adjustment of domain of data association are made, as shown in Figure 8.We can see that the proposed method has a successful control of the length of state vector with KF prediction iteration, which simplifies the calculation of complexity and improves the efficiency.
e most important thing is that the proposed local data association method in this paper can have a successfully accurate SLAM with localization error controlled in a certain range and also avoid the complexity of normal data association and its endless growth with more new landmarks.

Conclusion
To solve the SLAM problem of mobile robot, this paper adopts an improved RBPF filtering scheme; the important SIR is combined with an artificial intelligent algorithm GWO, which makes the improved RBPF with fast convergence.e most important thing is that the improved RBPF is able to avoid the normal resampling only considering the particles weight ID quality but ignoring the diversity of particles having the significance of the localization of the robot by posterior probability distribution.In addition, this paper also improves the normal NN data association method and proposes a local data association-adaptive observation range (Range-SLAM) which not only reduces the complexity of the data association but also improves the efficiency of the NN algorithm as well as effectively avoiding the KF state vector from unlimited growth and reducing the computation complexity for RBPF-SLAM.
Blackwellized particle filter (RBPF) is to estimate a posterior p(X t |Y t ) and divide the state vector X t into two parts: one part is x PF.