Improved Global Localization and Resampling Techniques for Monte Carlo Localization Algorithm

Global indoor localization algorithms enable the robot to estimate its pose in pre-mapped environments using sensor measurements when its initial pose is unknown. The conventional Adaptive Monte Carlo Localization (AMCL) is a highly efficient localization algorithm that can successfully cope with global uncertainty. Since the global localization problem is paramount in mobile robots, we propose a novel approach that can significantly reduce the amount of time it takes for the algorithm to converge to true pose. Given the map and initial scan data, the proposed algorithm detects regions with high likelihood based on the observation model. As a result, the suggested sample distribution will expedite the process of localization. In this study, we also present an effective resampling strategy to deal with the kidnapped robot problem that enables the robot to recover quickly when the sample weights drop-down due to unmapped dynamic obstacles within the sensor’s field of view. The proposed approach distributes the random samples within a circular region centered around the robot’s pose by taking into account the prior knowledge about the most recent successful pose estimation. Since the samples are distributed over the region with high probabilities, it will take less time for the samples to converge to the actual pose. The percentage of improvement for the small sample set (500 samples) exceeded 90% over the large maps and played a big role in reducing computational resources. In general, the results demonstrate the localization efficacy of the proposed scheme, even with small sample sets. Consequently, the proposed scheme significantly increases the real-time performance of the algorithm by 85.12% on average in terms of decreasing the computational cost.


Introduction
Localization is the problem of estimating a robot's pose (position and orientation) in real-time relative to an external reference frame given a map of the environment and sensor data. In fact, the pose estimation problem is sensor noise compensation [1] where the mobile robot has to estimate its state from noisy and not directly observable information. The localization problem has received great attention in the robotic literature, as it was considered the key and first question to the Autonomous Guided Vehicle (AGV) navigation problem and one of the essential problems in mobile robots [2]. Localization can be classified into three cases [3]: knowing the initial robot's pose forces uncertainty to be local; thus, the pose estimation problem will be just a local position tracking problem. On the other hand, the global localization problem is raised when the initial pose is unknown; in this case, the created uncertainty will be global. In contrast, when the autonomous robot in operation mode is carried to an arbitrary location, the problem is extended to the kidnapped problem [4], and it is crucially more difficult than global localization. In the robotic literature, there is a wide range of approaches that solve indoor localization problems, including Extended Kalman Filter (EKF) [3], gridbased algorithm [5], multi hypothesis tracking [6], Monte Carlo Localization (MCL) [7], and so on. Indeed, MCL is considered as one of the subset approaches that can successfully deal with the created uncertainty. MCL is a probabilistic approach that can compute the instantaneous uncertainty of a robot, and it is convenient to local and global pose estimation problems. However, MCL also is known as Particle Filter, where it represents the uncertainty by a set of weighted particles (samples). Particle Filter was presented in 1993 [8] as an implementation for the nonlinear recursive Bayesian filters, where it represents the Probability Density Function (PDF) by a set of random samples. MCL uses particle filter over the EKF because the last one guarantees accuracy only for a linear system to which Gaussian noise is applied.
Unfortunately, conventional MCL still suffers from some drawbacks. As MCL represents the posterior by a set of weighted samples using particle filter, the number of samples required to solve global localization will be considerable, and this increases the complexity of MCL and computational resources. Therefore, minimizing the number of needed samples is one of the main defies to the MCL algorithm [9]. Another challenge that faces this family of localization approaches is the resampling process. When the probabilities drop-down due to localization failure or unnatural sensor noise, augmented MCL tries to add random samples over the entire map to overcome this problem [3]. However, as MCL is a stochastic algorithm, drawing random samples over the space might discard all poses near the real robot's pose. This negatively affects the real-time performance. In fact, MCL is not restricted to range sensors. For instance, an on-board color camera is used to detect indoor map features [10]. The Gist descriptors were used to calculate the likelihood in MCL based on the Principal Component Analysis (PCA) [11]. Another common approach for localizing mobile robots using vision sensors is RTAB-Map [12] and FAB-MAP [13] that assume an image retrieval scheme [14] to estimate a large PDF.
This paper proposes novel methodologies to reduce uncertainty in the global indoor localization problem. Section II introduces an optimized scheme at the initialization step that detects mapped regions with high probabilities based on the initial scan data. Section III presents an improved resampling strategy that allows quick recovery when the robot loses its pose in a learned map. Simulation results and discussions are presented in Section IV. Finally, the conclusion is provided in the last section.

Improved Global Localization Algorithm
This section proposes an efficient and reliable approach to achieve the best sample distribution in high likely areas at initialization instead of randomly distributing samples within a pose space. Samples can be initialized in high likely grid cells only based on the initial Lidar scan. We generate samples promptly in accordance with the observation model. In this case, the generated samples can be drawn directly into high probability regions. The suggested distribution as samples concentrated on a small portion of space will reduce uncertainty at start-up.
The pose of rigid mobile robots restricted to planer maps is given by three variables; one is the robot's heading direction and its two-position coordinates in the plane. Each free grid cell in the Occupancy Grid Map (OGM) depicts the robot's position (x, y) with different orientations θ, where (0 ≤ θ < 2π) as shown in Figure 1. To reduce the computational effort, we use a threshold angle with a 3degrees value and a threshold distance of 0.15 meters. Likelihood Field Range Finder model presented in Probabilistic Robotics textbook [3] used to calculate the measurement probability at each possible pose based on one Lidar scan z t = {z t 1 , z t 2 , … , z t n } collected at initialization.
We calculate the probability of initial scan z 0 given the expected initial robot's pose x 0 and the map, d k is the distance transform for ray k, σ is a standard deviation for normal distribution (measurement noise), z max denotes the maximum sensor range, w hit and w rand stand for expected and random measurement weights, respectively.
During this process, the robot should be in an immobile state. And as a result of these calculations, the proposed algorithm will nominate only the first N high probability grid cells. Where N indicates the number of samples used to track the robot's pose.
In this way, samples will be initialized in a way that guarantees reliable and very fast filter convergence once the robot moves. Besides, fewer samples ensure precise global localization. The pose with the highest probability can be treated as the estimated initial robot's pose.

Effective Augmented MCL
Augmented MCL provides a robust approach for the conventional MCL and solves global positioning problem and robot kidnapping [3]. while the random samples are added to the sample set based on the divergence between fast and slow average weights the proposed algorithm will infer their poses in accordance to the uniform normal distribution over a specified area. Our proposed algorithm takes place when the filter diverges after it was in convergence mode. In this case, the robot knows its last confident pose and can predict where it can reach based on its maximum velocity. Rather than drawing random poses over the pose configuration space, they will be confined to a circular region centered around the last filter convergence position, as shown in Figure 2. However, the radius of the circular area will be determined based on the robot's maximum velocity and the time passed since the last time the robot was confident about its pose estimate.  As the conventional augmented MCL evaluates the confidence level based on Importance weights and decides if new random samples are needed or not, the proposed approach also takes into account the measurement probability to adjust the area of the specified region. The area of the selected region gradually increases as the probabilities continue to the drop-down. If the localization performance keeps down, the samples will be totally randomized over the pose configuration space. Finally, the problem turns again into a global localization problem. Through this approach, fewer samples will be enough to re-estimate the accurate distribution of the robot's pose. This will increase the realtime performance of the algorithm in terms of decreasing the computational burden. Besides, since the samples are distributed over a specified region with high probabilities, it will take less time for the samples to converge to the true pose.

Results and Discussions
This section outlines the simulation results and some analyses to evaluate the effectiveness of our proposed algorithms. Different areas with different indoor environments of OGMs were firstly drawn in AutoCAD software and then converted to OGM using MatLab software as shown in Figure 4. Simulated Lidar data was used to explore the environment. All tests were examined on a Laptop with 2.53 GHz (4 CPUs). To make the results more authentic, firstly, we create a standard robot path for each OGM by controlling the robot remotely, and then we did the tests using the created trajectory. Figure 4 shows our four indoor environments where we tested the proposed algorithms. As we see, they come in different areas. However, the resolution of all OGMs is 20 cells per meter. The mobile robot has been randomly placed over the pose space. While the robot is in an immobile state at start-up and does not have any prior information about its initial pose, the proposed algorithm starts to estimate the best sample distribution only based on the initial Lidar data using the observation model. When getting this estimation, the available samples will spread over the high likely areas with high measurement probabilities, starting from the highest probability until all samples are distributed.   By applying the observation model on each grid cell with different orientations, the robot gets the information about how the measurement probabilities carried on the pose configuration space with respect to its current pose as illustrated in Figure 5 above. While the robot randomly placed on spot A with 225 degrees heading direction, the algorithm shows that the region around spot A has the highest probabilities. However, it keeps also some far-away regions with high probabilities to prevent any localization failures due to unnatural sensor noise. Figure 6 and Figure 7 above represent the scattering of 10,000 and 500 samples according to the normal distribution over the pose space, respectively. As we see, even with a few numbers of samples, the proposed algorithm can do a good estimation.
Once the robot makes a simple movement, the filter convergence to the actual robot's pose will be very fast compared to the conventional AMCL, as shown in Figure 8 and Figure 9. The filter truly converges when the laser matching ratio is above the threshold (90%). In the proposed algorithm, there are many high probability samples near the true robot's pose, so, after one iteration of the resampling process, all samples with low probabilities will converge to the true pose. Figure 8 shows results for our proposed one, where the robot succeeded in recovering itself in the global environment for the two sample sets (10,000 & 500 samples) in record time. As the computational time for small sample sets is faster than the larger sets, they survive faster and maintains high performance during missions. But this is not applicable at all for the conventional AMCL, as at the global localization, samples are scattered randomly over the pose space, and they need time to catch the high likelihood region. The crux of our proposed algorithm is how the robot behaves when losing its convergence during tasks. As the robot locates itself reliably in the environment using our previous algorithm, the filter may lose the convergence when the robot encounters dynamic objects like humans or due to sensors noise or whatever that causes dropping in probabilities. Figure 10 illustrates how our proposed augmented MCL behaves when the mobile robot runs across dynamic objects. As the measurement probabilities drop-down, augmented MCL starts adding new random samples to the sample set in proportion to the amount of drop. The proposed algorithm restricts the region to add samples to a circular region around the last successful estimation based on the robot's maximum velocity. The area of the selected region where the robot has a chance to locate itself there increases over time if the robot still cannot recover itself. As a result of this technique, our robot has a great opportunity to estimate its pose promptly once the impact of the dynamic obstacles is gone.
The new approach guarantees to resample any high probability samples to a certain area around the robot's pose rather than publishing them randomly over the pose space. In this way, the probability of the robot recovering itself in record time is very high and the distance traveled by the robot when it loses itself will be very short compared to the classical approach as shown in Figure 11 below. Figure 11 compares the responses of the proposed and conventional MCL; the robot performs its mission at maximum velocity (0.5m/sec.). After 10 seconds, the robot encounters two dynamic objects that cause a drop in probabilities for three seconds. The proposed approach recovered the robot directly when the effect of the dynamic objects disappeared. In this case, the distance traveled by the robot around one meter. As we see, even a small sample set (500 samples) recovered the robot very efficiently.
At the same criteria, the conventional method recovered the robot after 4 meters in 8 seconds for 10,000 samples and around 7 meters in 14 seconds for 1,000 samples as illustrated in Figure 11. Also, we can notice that when the size of the sample set decreases the filter needs more time for recovering as the samples spread over the pose space, where it needs time to catch the high probability area. Regarding fast converging for the classical approach at initialization, instead of using global localization we used the local distribution method to publish all samples around the robot pose in order to speed up the convergence process at a start-up where we are not interested in this test.  Table 1 to Table 3 summarizes the results for our proposed approaches compared to the conventional AMCL. It shows the results for different environments by employing different sample sets. We noticed that our proposed approaches showed higher performance in terms of time and distance the robot traveled to recover itself, either at global localization or even when it loses itself during tasks. Also, small sample sets showed very efficient results and played a significant role in reducing computational resources. However, in the proposed effective augmented MCL approach, we see that the time and distance increase as the sample set decreases, this is because when the selected certain area increases the larger sample set can cover that area better than the small sample set, which means there are many samples will be around robot's pose. However, this is solved by AMCL, where it can add samples directly when the robot loses its pose.

Conclusions
In this work, we have proposed efficient approaches to improve mobile robot global positioning and resampling process in indoor environments, and this allows the mobile robot to infer its initial pose and recover itself in record time. Our approach takes into account only the initial scan measurements in order to detect the high likelihood regions based on the observation model, leading to the best sample distribution in high-probability areas instead of randomly scattering samples like previous methods. Furthermore, we devised an alternative scheme that allows AMCL to add the random samples around the robot's position when the probabilities drop-down by taking into account the prior knowledge about the most recent confident estimate pose. Several simulations were carried out to evaluate our localization approaches. The results demonstrate great superiority over conventional localization methods. Where it showed reliable and fast global localization. Moreover, it showed a smart resampling process that enables the robot to recover quickly when probabilities drop-down. This resulted in a noticeable increase in real-time performance in terms of decreasing the computational cost, and high efficiency of small sample sets to localize the mobile robots regardless of the size of indoor environments. However, there are limitations to the proposed approaches, where the dynamic map should not differ significantly, and the robot's environment should not be highly symmetrical. To further improve the computational time at global localization, global angle search will be considered in the next time. Besides, we will propose to actually detect dynamic obstacles like humans to prevent measurement probabilities from dropping.