Abstract
Adaptive Monte Carlo localization (AMCL) algorithm has a limited pose accuracy because of the nonconvexity of the laser sensor model, the complex and unstructured features of the working environment, the randomness of particle sampling, and the final pose selection problem. In this paper, an improved AMCL algorithm is proposed, aiming to build a laser radar-based robot localization system in a complex and unstructured environment, with a LIDAR point cloud scan-matching process after the particle score calculating process. The weighted mean pose of AMCL particle swarm is used as the initial pose of the scan matching process. The LIDAR point cloud is matched with the probability grid map from coarse to fine using the Gaussian-Newton method, which results in more accurate poses. Moreover, the scan-matching pose is added into the particle swarm as a high-weight particle. So the particle swarm after resampling will be more concentrated in the correct position. The particle filter and the scan-matching process form a closed loop, thus enhancing the localization accuracy of mobile robots. The experiment results demonstrate that the proposed improved AMCL algorithm is superior to the traditional AMCL algorithm in the complex and unstructured environment, by exploiting the high-accuracy characteristic of scan matching while inheriting the stability of AMCL.
1. Introduction
With the continuous expansion of robot applications, the working environment that robots confront with has become increasingly complex and unstructured. For intelligent mobile robots, the biggest technical challenge is to have the ability to sense the environment and acquire the perceptual information for autonomous localization, navigation, and driving control. In particular, it is more important for autonomous localization in unstructured complex environments. In a structured environment, the information perceived on the ground can be converted into a simple boundary structure to guide the formation of a perceptual behavior strategy. But in an unstructured environment with unevenness of the ground or different sizes of obstacles, it is difficult to describe the environment as a regular structure with the perceptual information of the environment, which increases the difficulty of autonomous localization of the robot. In addition, some complex terrains such as ravines and muddy ground in unstructured environments and bad weather such as rain, snow, fog, and dust can also affect localization accuracy and robustness.
So far, the problem of autonomous localization has been widely discussed in mobile robot systems. In the past decades, mobile robot localization technology can reliably complete positioning in some specific application scenarios. But achieving high-precision and high-reliability localization in a complex unstructured environment is still a challenging task.
In an industrial practical application scenario, the mobile robot’s operating environment is generally fixed at a period of time [1]. Probabilistic localization methods have been proven to be robust in this static environment. The probabilistic localization method is mainly based on extended Kalman filter [2], histogram filter, and particle filter, namely Monte Carlo localization (MCL) [3]. Lidar sensors are often used in robot localization and obstacle avoidance because they can accurately measure the distance of obstacles without complex image preprocessing like visual sensors. In particular, in industrial environments, most industrial robots are equipped with lidars to sense the environment, avoid obstacles, and apply the brakes. In the literature [4, 5], a standard Monte Carlo positioning method is used on the SICK Lidar with a localization error of about 0.05 m to 0.2 m. In the literature [6], a localization algorithm based on extended Kalman filter is used on the Pioneer2 robot with SICK Lidar; the pose error is approximately 0.25 m, 3.5°.
The Monte Carlo localization algorithm is a probabilistic localization algorithm applied to a two-dimensional occupation grid map [7], which uses the particle filter algorithm [8]. Particle swarm is used to describe and track the current possible pose of mobile robots in known maps [5]. It could estimate pose globally with a small amount of computation and a low memory footprint. At the same time, the estimated pose is very smooth during the locomotion of robots and suitable for navigation control of mobile robots [9]. However, it would always be affected by the strong nonconvexity of the laser radar sensor model and complex unstructured features of the environment. For example, the power substation inspection robot system shown in Figure 1 is under a strong magnetic field environment, affected by the sensor noise and the limitations of particle filter. In practical application, the accuracy and robustness of the Monte Carlo localization algorithm are limited.
However, in some complex and unstructured environments, extremely high localization accuracy is a key condition for robots to perform multiple tasks. A typical example is that a robot picks up an object from a fixed location (such as a conveyor belt or a workbench). To carry out global localization, the robot is generally equipped with a high-precision lidar sensor. Before picking up the object, robots need to move to the appropriate position. To improve the localization accuracy, some auxiliary localization devices such as magnetic stripe or QR codes are usually deployed near the corresponding position. However, installing auxiliary localization devices requires changing the production environment, which limits the flexibility of the robot system.
In order to solve the problem of accuracy and robustness of autonomous localization of robots in complex unstructured environments, this paper proposes an improved adaptive Monte Carlo localization algorithm based on laser scan matching, which combines the laser scan-matching algorithm [10] and Monte Carlo localization algorithm. It inherits the high precision of laser scan matching algorithm and the reliability of Monte Carlo localization algorithm. Using our proposed algorithm, the localization of mobile robot remains highly accurate and highly reliable in a complex unstructured environment without any auxiliary localization devices.
2. Monte Carlo Localization Algorithm
The Monte Carlo localization algorithm is basically divided into the following four steps as shown in Figure 2.
When a laser sensor is used to locate robot on a 2D grid map, if the robot pose is given, it is very easy to calculate the agreement between the laser beams and the occupied grid. Therefore, the MCL algorithm can be used, which represents the pose of the robot with many particles [11], as shown in Figure 3. Calculate the weight of the particle according to the agreement with the map; then, determine the estimated pose and locate the robot. However, there are some problems in the MCL algorithm: it cannot solve the robot kidnapping problem. Once the pose changes discontinuous, the localization will fail [12]. To improve the localization accuracy, many particles need to be added and result in slow localization convergence rate.
The AMCL algorithm is adapted from the MCL algorithm to solve above problems. The AMCL algorithm randomly adds free particles during resampling [13]. The number of free particles is calculated based on long-term estimated weights and short-term estimated weights :
In (1), is the average weight of all particles; the parameters and are, respectively, used to estimate the attenuation rate of the exponential filter with long-term and short-term weight () [14].
Kullback-Leibler divergence (KLD) algorithm is used to resample particle in the AMCL algorithm [15]. This method can adaptively calculate the required number of particles based on the distribution of particle weights. The upper bound of the number of particles is expressed as
In (2), parameters and are, respectively, the maximum error and the standard normal distribution quantile between the true distribution and the estimated distribution. is the number of nonempty in the state space of the particle. It can be seen that the upper bound of the number of particles has an approximately linear relationship with . During the initial global localization, the particles are more dispersed and is larger, so the upper bound of the number of particles are higher; when the global localization is completed; the problem became a trajectory tracking problem. At this time, the particles are more concentrated and converged. The number of nonempty state spaces is small; is small, and the upper bound of the number of particles decrease. In this way, the number of particles is effectively dynamically adjusted [16]. The total number of particles is reduced which results in better computational efficiency.
However, there is still a problem in the AMCL algorithm: when the robot is in a complex and unstructured environment with dynamic obstacles and unevenness of the ground, the optimal pose estimation given by the algorithm is the center of the particle swarm instead of the best matching pose between map and laser scanning result. Even after convergence, the localization accuracy and robustness cannot be guaranteed. In addition, once the error between the particle swarm and the actual pose is large, it will take some time to calibrate itself and even result in terrible localization deviation.
3. An Improved Adaptive Monte Carlo Localization in the Complex and Unstructured Environment
In order to solve the problem during the AMCL algorithm to estimate the pose of the robot as the weighted center deviation of the particle swarm in the complex unstructured environment, this paper adds a scan matching that matches the laser scanning with the map, as shown in Figure 4. The scan-matching algorithm gets the pose estimation from the AMCL algorithm as input. Then, the laser scans are aligned with the probability grid map using the Gaussian-Newton iteration method [17]. Finally, the optimized pose estimation solved by scan matching is added to the AMCL particle swarm.
3.1. Scan Matching Principle
For robots equipped with laser sensors, the localization problem in a probability grid map can be converted to a problem of matching optimization as shown in where is the occupation probability of the map at a given coordinate point , and is the coordinates of the endpoint of the laser scanning ray in the map coordinate system while the mobile robot is in position :
In (4), and represent the coordinates of the laser sensor, and is the orientation angle of the robot.
The goal of the scan matching is that given an initial estimate , which has a small deviation from the actual pose, find a deviation to align the laser scan results with the probability grid map [18]:
For the first-order Taylor expansion of , find the minimum value of expansion (solving partial derivative for and let it be 0), and finally, get where the Hessian matrix and partial derivative of to are
Since the map is a probability grid map, it is discrete and not continuous. In order to obtain the derivative of the map , bilinear interpolation is carried out in the and directions on the map [19], as shown in Figure 5.
According to Figure 5, the probability value of is
is the probability that grid is occupied. The partial derivative of is
The core of scan matching is to use the first-order Taylor series expansion of the Gaussian-Newton iteration method to approximately replace the nonlinear regression model. After iterations, the residual square sum (5) of the original model is minimized. Finally, is the optimized pose that the laser scan results are best aligned with the probability grid map.
3.2. Scan Matching Process in Complex Unstructured Environment
After global positioning in a complex and unstructured environment, the localization problem became a pose-tracking problem. Due to the complex and unstructured environment, robot pose estimated by the AMCL algorithm is the weighted mean of the particle swarm, and there is still a certain deviation from the actual pose. According to the scan-matching principle described above, the Gaussian-Newton iteration method is adopted which takes the pose from the AMCL algorithm as an initial value and optimizes it on the multiresolution map from coarse to fine. In each layer, the Gaussian-Newton method is used to solve the deviation , which results in a more accurate pose. The detailed process is shown in Figure 6.
If the particle swarm of the AMCL algorithm is concentered, namely, the solution of the pose estimation converges, the scan-matching algorithm is used to further optimize the pose. Firstly, the pose solved by the AMCL algorithm is used as the initial pose of the scan matching. Then, the Gaussian-Newton iterations are performed layer by layer from low resolution to high resolution on the multiresolution map. After iterating, it will get a more accurate pose. If the error approach to the solution of scan matching and the solution of the AMCL algorithm is smaller than threshold, the solution pose of scan matching will be inserted into the AMCL particle swarm as a high-weight particle; if the error is larger than threshold, it means that the solution of scan matching may be erroneous, so it should be abandoned.
Because the scan matching uses the solution of AMCL as the initial value, if the AMCL solution differs greatly from the actual pose, the scan matching will fail. In addition, the optimal particles obtained by scan matching must be inserted into the AMCL particle swarm to form a closed loop for reducing the difference between the solution of AMCL and scan matching.
It should be noted that the scan matching adopts the principle of layer-by-layer iteration on multiresolution maps. To make a map continuous and derivative, the map is a probability grid map. Low-resolution maps in multilayer maps are downsampled from the original high-resolution map.
4. Experiments
To analyze the effect of adding the scan matching to the original AMCL algorithm, experiments were performed on the mobile robot equipped with a single-line laser radar, respectively, using the original AMCL algorithm and proposed improved AMCL algorithm. The maximum linear velocity of the robot motion is set to 0.3 m/s, and the maximum angular velocity is set to 1 rad/s.
4.1. Original AMCL Algorithm Experiment
To verify the localization effect of the original AMCL algorithm, in the occupancy grid map built by a cartographer [20], a localization experiment was performed using the original AMCL algorithm. To speed up the convergence, the initial particle swarm is randomly generated near the actual initial pose. The small green arrow in Figure 7 represents a pose particle; the red arrow represents the estimated robot pose at each moment, and the red point cloud is laser scanning point cloud. The minimum figure of particles in the AMCL algorithm is 500 and the maximum is 5000. Figure 7(a) shows the initial state of the particle swarm. An approximate estimate of the robot’s initial pose is provided to speed up localization convergence. As can be seen from the figure, many particles are generated near the initial pose estimation. Figure 7(b) and Figure 7(c) show that the particles converge gradually when the robot moves. According to red point clouds in Figure 7(b) and Figure 7(c), the laser scanning points are not well aligned with the obstacle. This is due to the current environment being unstructured and complex; the weighted mean pose of particle swarm was used as the estimated pose in the original AMCL algorithm which results in a certain deviation between the estimated pose and the actual pose.
(a) Initial state
(b) Convergence process
(c) After convergence
4.2. Scan-Matching Experiment
To verify the pose optimization effect of the scan matching, we also chose to do experiment in a complex and unstructured environment under a noisy strong magnetic field complex environment (power substation). The laser point cloud corresponds to the weighted mean pose of the original AMCL algorithm, and the point cloud corresponds to the optimized pose after scan-matching optimization is drawn in Figure 8.
In Figure 8, the red point cloud is the laser point cloud scan result corresponding to the weighted mean pose of the original AMCL algorithm, and the blue point cloud is the laser point cloud scan result corresponding to the optimized pose after scan-matching optimization. Comparing the alignment effect of the laser point cloud with the black map obstacle, it shows that the pose corresponding to the red point cloud is not well aligned with the obstacle, which indicates that there is a deviation between the pose obtained by the original AMCL algorithm and the actual pose. After scan matching, the blue point cloud corresponding to the optimized pose is more aligned than the red point cloud indicating that the scan matching can effectively improve the pose accuracy when the robot is stationary.
4.3. Improved AMCL Algorithm Experiment
To compare the dynamic localization effect of the original AMCL algorithm with the proposed improved AMCL algorithm in the same complex unstructured experimental environment, we chose to insert or not the scan-matching solution back into the AMCL particle swarm to form a closed loop. An open-loop version and a closed-loop version of the improved AMCL algorithm are experimented.
The open-loop version of the improved AMCL algorithm uses weighted mean of original AMCL algorithm’s particle swarm as the initial value of scan matching but does not insert the scan-matching solution back into the particle swarm of the AMCL algorithm. The weighted mean of original AMCL algorithm’s particle swarm and scan matching solutions are recorded to verify the effect of scan matching on the pose accuracy.
The closed-loop version of the improved AMCL algorithm inserts the scan-matching optimization solution back into the particle swarm to form a closed loop with the AMCL algorithm if the scan matching is successful. The weighted mean of improved AMCL algorithm’s particle swarm and the scan matching solutions are recorded to analyze the difference between these two solutions.
The localization experiment was performed on a map with a size of 16.075 m × 9.9 m and a grid size of 0.025 m, as shown in Figure 9. Several straight line round-trip motion experiments and closed-loop motion experiments were conducted using open-loop and closed-loop versions of the improved AMCL algorithm, respectively.
4.3.1. Straight Line Round-Trip Motion Experiment Using the Open-Loop Version of Improved AMCL
The straight line round trip starts from the origin of the world coordinate system and moves along the positive direction of the X axis by more than 5 m (allowing small adjustment of angle to keep aligned). Then, controlling the robot back to original point and recording two kinds of pose estimation output as “endpoint 1.” Next, controlling the robot to move back more than 3 m along the negative direction of the X axis. Finally, controlling the robot to move back to the original point and recording two kinds of pose estimation output as “endpoint 2.” Repeating above actions for 6 times. The robot has run for more than 100 m and 10 minutes during this round-trip motion experiment.
The weighted mean of original AMCL algorithm’s particle swarm and the optimized solutions of scan matching are obtained. The deviations of these two pose estimations at origin are shown in Table 1. The trajectories from start point to endpoint 2 of these two methods are shown in Figure 10.
Table 1 shows that the maximum localization error of the original AMCL algorithm during the straight line round-trip motion experiment is about the size of the two grids (5 cm); the average localization error exceeds a grid size (2.5 cm). The maximum localization error after scan-matching optimization is less than one grid size (2.5 cm), and the average localization error is half a grid size (1.25 cm). It shows that the scan-matching algorithm can effectively improve the localization accuracy during robot moving.
4.3.2. Closed-Loop Motion Experiments Using the Open-Loop Version of Improved AMCL
For closed-loop motion experiments, the maximum speed of the robot is set to 0.3 m/s and the maximum angular velocity is 1 rad/s. The robot moves along a closed-loop path (heading angle changes more than 360°) and returns to the origin. Then, the robot fine-tune its pose to align with the origin as far as possible. The closed-loop experiments are repeated for 3 times. The localization error of the weighted mean of original AMCL algorithm’s particle swarm and the error of the corresponding scan matching solution are shown in Table 2. The trajectory of the robot in the first experiment is shown in Figure 11.
The maximum localization error of the original AMCL is 14.5 cm, 18.5 cm, and 0.057 rad, and the average localization error is 9.9 cm, 8.9 cm, and 0.035 rad. After completing scan-matching optimization based on the solution of the original AMCL, the maximum localization error reduces to 1.5 cm, 1.6 cm, and 0.012 rad, and the average localization error is 1.3 cm, 1.2 cm, and 0.010 rad. It shows that the accuracy of pose after scan matching is much higher than that of the original AMCL when the robot moves back to the origin.
Comparing Figure 10 with Figure 11, it shows that the scan-matching solution is better; the path is more continuous without large fluctuations if the robot moves along a straight line, but the scan-matching solution is fluctuant or even failed if the robot is spinning on a spot. The reason is that the laser-scanning frequency used in experiments is too low (5 Hz), which results in large deviation of laser scanning. So using the original AMCL solution as initial estimation for the scan matching has low accuracy.
Because of the above reasons, when the robot is turning as shown in the corners of Figure 11, the localization error of the scan-matching solution is larger than that of the original AMCL solution. However, after the robot moves back to the origin, the localization error of the scan-matching solution is much smaller than that of the original AMCL solution because AMCL relies too much on the motion model of the robot. When the robot is spinning on a spot, the sensor error becomes large (especially for odometer and laser), which results in bad localization accuracy of the AMCL algorithm during turning and affecting the subsequent localization.
4.3.3. Closed-Loop Motion Experiment Using the Closed-Loop Version of Improved AMCL
A closed-loop version of the improved AMCL algorithm is used below for closed-loop motion experiments, which inserts the scan-matching solution as a high-weighted particle back into the AMCL particle swarm to form a closed loop. Parameters of the experiment are similar to the above experiment. The experiment is repeated four times. The experiment result is shown in Table 3. The trajectory of the experiment is shown in Figure 12.
As shown in Table 3, with the closed-loop version of the improved AMCL algorithm, the scan-matching solution and the AMCL algorithm’s particle swarm that form a closed loop improve the accuracy of weighted mean of particle swarm greatly. Four times of loop motion experiments show that the maximum localization error of weighted mean for the particle swarm is 2.4 cm, 1.8 cm, and 0.017 rad, which is about the size of the grid, and the average localization error is 1.4 cm, 1.1 cm, and 0.010 rad, which is about half of the size of the grid. The maximum localization error of the scan-matching solution is 1.2 cm, 1.3 cm, and 0.032 rad, which is half the size of the grid, and the average localization error is 0.8 cm, 0.8 cm, and 0.019 rad, which is about one-third of the size of the grid.
The closed-loop version of the improved AMCL algorithm improves the accuracy of the weighted mean of AMCL particle swarm by inserting high-precision scan-matching solution, and the weighted mean of the particle swarm is used as the initial value of the next scan-matching process. Because the initial values of iteration are more accurate, the accuracy of the scan-matching solution is better. Under this mutual promotion, localization accuracy has been significantly improved.
As shown in Figure 12, because the accuracy of the initial value of iterations is higher, the amplitude of the scan-matching solution is significantly reduced with the closed-loop version of the improved AMCL algorithm, but there is still a small uncertainty when the robot is turning.
According to the above comparative analyses, in practical applications, especially in a complex and unstructured environment, the localization accuracy of the original AMCL algorithm is low and the maximum localization error exceeds 10 cm due to several reasons. The scan-matching process can significantly optimize the pose’s accuracy (error half reduced). The maximum static localization error is no more than one grid (2.5 cm), and the average localization error is only half a grid (1.25 cm). However, due to the limitation of laser sensor, the results of the laser scanning are not accurate enough during robot turning. It makes the scan matching uncertain and leads to a discontinuity in pose estimation.
Because the scan matching and AMCL form a closed loop, the accuracy of weighted mean pose of AMCL algorithm’s particle swarm has been greatly improved; the scan-matching solution is also smoother and more stable. The improved AMCL algorithm has reduced the maximum localization error to a grid (2.5 cm), and the average localization error has also been reduced to half a grid (1.25 cm); the maximum localization error of the scan-matching solution has been reduced to half grids (1.25 cm), and the average localization error has been reduced to less than 1 cm.
In the experiment, the scan-matching algorithm has strong adaptability to the laser radar noise and can correctly deal with the situation in a complex unstructured environment and has greater improvement in accuracy and reliability than the original AMCL algorithm. Therefore, the accuracy of the original AMCL algorithm can be improved by the scan-matching algorithm, and the robustness of the localization will not be reduced due to the increase of the scan-matching process.
In this paper, a scan matching is added into the original AMCL algorithm. It makes particle swarm converge to the correct pose more quickly, and the total number of particles converges to a lower level faster. Therefore, the computations of particle swarm are updated, and resampling are lower than the original AMCL algorithm.
5. Conclusion
Aiming at the problem that the traditional AMCL algorithm has a low localization accuracy in a complex and unstructured environment, this paper proposes an improved AMCL algorithm based on laser scan matching. Firstly, it adds a scan-matching process and applies the Gaussian-Newton iteration method to optimize the pose. Then, the scan-matching solution feeds back to the AMCL to form a closed loop, which improves the alignment of the laser radar scanning endpoints and map obstacles, leading to a higher localization accuracy. Combining the advantages of the high precision of the scan matching and stability of the AMCL pose, our proposed method improves the localization accuracy while maintaining its robustness. The experimental results demonstrate the effectiveness and feasibility of this method. The improved AMCL algorithm proposed in this paper has two kinds of pose outputs: a weighted mean of the particle swarm, which is more stable, and a scan-matching solution, which is more accurate. One of these poses can be chosen to meet the need for continuity and accuracy of the localization in practical applications. The performance of this method relies on the accuracy of the laser sensor. When the mobile robot turns, there are uncertainties in the pose estimation. The estimated pose will be improved with a higher scanning frequency of the laser sensor.
Data Availability
The experimental data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This paper was supported by the National Natural Science Foundation of China, no. 61672244.