^{1}

^{2}

^{2}

^{1}

^{2}

^{1}

^{2}

^{1}

^{2}

^{3}

^{1}

^{2}

^{1}

^{2}

^{3}

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.

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 [

The Monte Carlo localization algorithm is a probabilistic localization algorithm applied to a two-dimensional occupation grid map [

Power substation experiment site.

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 [

The Monte Carlo localization algorithm is basically divided into the following four steps as shown in Figure

Basic steps of the Monte Carlo localization algorithm.

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 [

Matching particle swarms to grid maps.

The AMCL algorithm is adapted from the MCL algorithm to solve above problems. The AMCL algorithm randomly adds free particles during resampling [

In (

Kullback-Leibler divergence (KLD) algorithm is used to resample particle in the AMCL algorithm [

In (

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.

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

Flow chart of the proposed improved AMCL algorithm.

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

In (

The goal of the scan matching is that given an initial estimate

For the first-order Taylor expansion of

Since the map

Bilinear interpolation.

According to Figure

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 (

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

Flow chart of the scan-matching process.

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.

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.

To verify the localization effect of the original AMCL algorithm, in the occupancy grid map built by a cartographer [

Mobile robot global localization process based on AMCL.

Initial state

Convergence process

After convergence

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

Experiment result of laser point cloud alignment.

In Figure

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

Map of localization experiment.

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

Result of straight line round-trip motion experiments.

Robot |
Error of original AMCL | Error of open-loop version of improved AMCL | ||||
---|---|---|---|---|---|---|

Starting point | 0.014 | 0.011 | −0.004 | −0.019 | −0.014 | −0.009 |

Endpoint 1 | 0.053 | 0.032 | −0.024 | 0.006 | −0.006 | −0.014 |

Endpoint 2 | 0.052 | 0.014 | −0.004 | 0.002 | −0.008 | 0.003 |

Endpoint 3 | 0.051 | 0.009 | −0.027 | 0.009 | −0.014 | −0.011 |

Endpoint 4 | 0.042 | −0.010 | −0.018 | −0.018 | −0.011 | 0.001 |

Endpoint 5 | 0.055 | −0.028 | 0.006 | 0.004 | −0.017 | −0.007 |

Endpoint 6 | 0.049 | −0.005 | −0.016 | −0.019 | −0.010 | 0.000 |

Endpoint 7 | 0.043 | 0.030 | −0.007 | 0.016 | −0.010 | 0.008 |

Endpoint 8 | 0.027 | 0.014 | −0.003 | −0.020 | −0.008 | −0.007 |

Endpoint 9 | 0.018 | 0.000 | 0.016 | −0.020 | −0.013 | −0.011 |

Endpoint 10 | −0.015 | −0.018 | −0.002 | −0.009 | −0.008 | −0.004 |

Endpoint 11 | −0.026 | 0.001 | −0.008 | −0.007 | −0.007 | −0.021 |

Endpoint 12 | −0.038 | 0.000 | 0.013 | −0.010 | −0.004 | −0.020 |

|Maximum| | 0.055 | 0.032 | 0.027 | 0.020 | 0.017 | 0.021 |

|Minimum| | 0.015 | 0.000 | 0.002 | 0.002 | 0.004 | 0.000 |

|Mean| | 0.039 | 0.013 | 0.012 | 0.012 | 0.010 | 0.009 |

Trajectory of straight line round-trip motion experiment.

Table

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

Result of closed-loop motion experiments (open-loop version).

Robot |
Error of original AMCL | Corresponding scan matching solution error | ||||
---|---|---|---|---|---|---|

Experiment 1 | 0.145 | −0.017 | 0.057 | 0.009 | −0.007 | −0.012 |

Experiment 2 | −0.101 | −0.065 | 0.015 | −0.014 | −0.012 | −0.009 |

Experiment 3 | 0.052 | −0.185 | −0.032 | 0.015 | −0.016 | 0.008 |

|Maximum| | 0.145 | 0.185 | 0.057 | 0.015 | 0.016 | 0.012 |

|Minimum| | 0.052 | 0.017 | 0.015 | 0.009 | 0.007 | 0.008 |

|Mean| | 0.099 | 0.089 | 0.035 | 0.013 | 0.012 | 0.010 |

Trajectory of closed-loop motion experiment (open-loop version).

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

Because of the above reasons, when the robot is turning as shown in the corners of Figure

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

Result of closed-loop motion experiments (closed-loop version).

Robot |
Closed-loop version of improved AMCL algorithm | |||||
---|---|---|---|---|---|---|

Errors of weighted mean solution for particle swarm | Error of scan matching solution | |||||

Experiment 1 | −0.011 | −0.018 | −0.017 | −0.012 | −0.013 | −0.032 |

Experiment 2 | −0.024 | −0.002 | 0.001 | 0.004 | −0.006 | −0.008 |

Experiment 3 | −0.018 | −0.012 | 0.013 | 0.005 | 0.000 | 0.028 |

Experiment 4 | −0.001 | −0.012 | −0.008 | −0.011 | −0.012 | −0.006 |

|Maximum| | 0.024 | 0.018 | 0.017 | 0.012 | 0.013 | 0.032 |

|Minimum| | 0.001 | 0.002 | 0.001 | 0.004 | 0.000 | 0.006 |

|Mean| | 0.014 | 0.011 | 0.010 | 0.008 | 0.008 | 0.019 |

Result of closed-loop motion experiment (closed-loop version).

As shown in Table

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

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.

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.

The experimental data used to support the findings of this study are included within the article.

The authors declare that they have no conflicts of interest.

This paper was supported by the National Natural Science Foundation of China, no. 61672244.