Abstract
An artificial intelligent grey wolf optimizer (GWO)-assisted resampling scheme is applied to the Rao-Blackwellized particle filter (RBPF) in the simultaneous localization and mapping (SLAM). By doing this, we can make the diversity of the particles resampling and then obtain a better localization accuracy and fast convergence to realize indoor mobile robot SLAM. In addition, we propose an adaptive local data association (Range-SLAM) scheme to improve the computational efficiency for the algorithm of the nearest neighbor (NN) data association in the iteration of the RBPF prediction. Through the experiment and simulations, the proposed SLAM schemes have fast convergence, accuracy, and heuristics. Therefore, the improved RBPF and new data association schemes presented in this paper can provide a feasible method for the indoor mobile robot SLAM.
1. 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. Therefore, in recent years, the RBPF [6, 7] has been introduced as an effective method to solve the SLAM problem. The 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. The 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].
The 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. The 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. Therefore, the improved RBPF becomes more effective and flexible in the SLAM.
The 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.
2. Rao-Blackwellized Filter Review
According to Murphy [10], the key idea of solving the SLAM problem with a Rao-Blackwellized particle filter (RBPF) is to estimate a posterior and divide the state vector into two parts: one part is , which is estimated using the PF, and the other part is , where Kalman filters (KFs) are used. Denoting the measurements and states up to time with and , respectively, the joint probability density function (PDF) is given by Bayes’ rule as
If the first term is linear, it can be optimally estimated using a KF. To obtain the second term , 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 represents the pose of mobile robot. Then 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 bywhere is the control signal; and represent the Gaussian white noise with 0 mean value and their covariances are and .
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 is a sampling density function. Then, 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 bywhich is proportional to a measurement likelihood. Then, the obtained weights are normalized as follows:
For dividing the state vector 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
The measurement noise is assumed to be white and Gaussian distributed according to
Furthermore, is Gaussian:
The density of can be arbitrary, but it is assumed to be known. The Rao-Blackwellization for filtering algorithm is given in Algorithm 1. For the system equation (6), see (7)–(9) for system properties.
|
The 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 , , and , where is the particle accumulation and the initial value is zero. Detailed algorithm is given in Algorithm 2.
|
3. 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. This 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 , and the other one is the resampling; let us talk about it in detail.
Why resampling? The 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 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 iswhere N is the size after resampling, M is the size before resampling, , and Floor(·) gives the largest integer not exceeding the content. The 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). This 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.
4. Grey Wolf Optimization
GWO is a new kind of metaheuristic biological intelligence algorithm proposed by Mirjalili et al. [14]. The 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.
(a)
(b)
5. 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 is a constant, is the particle with highest weight, and is a known vector that comes from the observation. In the searching algorithm, parameters 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 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.
6. 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. There 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. The 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 . For n observations, we compute Mahalanobis distance between each and each stored landmark , in the map, and D is the minimum of :where is the expected observation of i-th landmark that existed in map. The innovation covariance matrix for the expected observation iswhere is the Jacobian matrix of the system of observation equations. The general assumption is . Confidence is given to determine the correlation threshold G1 and augmented threshold G2; if < G1, then and are a set of acceptable association; if there are sets of acceptable association with , then select the landmark with minimum norm distance (ND) to be a set of acceptable associations with , where ; if > G2, then is a new landmark to be stored; if G1 < < G2, then give up , and regard it as noises.
6.1. Efficiency Analysis
Assume there are M landmarks in the map right now; from the NN data association analysis, we can see the following:(1)If there is a new observed landmark by sensor and data association is made in the map, then it will need to compute 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). The 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).
(a)
(b)
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 is the robot real position in map. 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 > NUMmax, then reduce the radius R of circle S; otherwise, if NUM < NUMmin, then increase the radius under the premise of Rmin ≤ R ≤ Rmax.
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. This 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. The proposed method is easy but a valid one; we can finally determine mobile robot position and attitude accurately by RBPF above.
7. Experiment
In this paper, we take two wheels’ differential driving mobile robot as an experiment. Its motion model is shown in Figure 3. The state of mobile robot of world coordinate system is represented by the position and orientation vector , where are coordinates of mobile robot in k moment; is the angle between the forward direction of mobile robot and the positive direction of x-axis of the world coordinate system.
The linear velocity and rotational angular velocity of mobile robot in k moment arewhere 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 iswhere is the sampling period.
For the state observation model, in this paper, we set that there are totally N landmarks and the coordinates (i = 1, 2, …, N) do not change; then the observation model of mobile robot iswhere is the distance between the mobile robot and landmark i; 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°. The 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. The blue trajectory is the robot motion localization; light green small arcs are the ultrasonic scanning signs. The 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. The 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.
(a)
(b)
When applying the GWO algorithm to assist the resampling, randomly initialize artificial wolves in , its quantity , maximum iterations , searching wolves scaling factor , maximum number of searches , distance determinant factor , step length factor , updating scaling factor , , and . The 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. The 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. The most important result is that GWO resampling not only ensures the diversity of particles but also ensures the particles distribution approximate maximum weight after resampling.
This 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. The 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.
8. 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. The 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.
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 study.
Acknowledgments
The authors are grateful for the open-access datasets in GitHub. The research was funded by the 2021 High-level Talents Research Support Program of Shenyang Ligong University, China (No. 1010147001017), and also by the Basic Research Project of the University of Science and Technology Liaoning, China.