Self-adaptive particle filtering method for laser SLAM (Simultaneous Localization and Mapping) task

Abstract

The invention provides a self-adaptive particle filtering method for a laser SLAM task. The problems that a traditional particle filtering algorithm is bottleneck in calculation efficiency, insufficient in dynamic environment adaptability and the like in a complex environment are solved. In the particle filtering prediction stage, particle swarm optimization (PSO) is introduced, particle distribution is optimized by using the global search capability of the PSO, and the inertia weight and the acceleration factor of the PSO are dynamically adjusted to enhance the adaptability of the algorithm in a complex dynamic environment, so that the algorithm is effectively prevented from falling into a local extreme value; in the resampling stage, KLD sampling and a selective resampling method are combined, a sampling strategy is dynamically switched according to the effective particle number (ESS) and the KL divergence, and the GPU is used for accelerating the resampling step to improve the calculation efficiency. The method has remarkable advantages in the aspects of improving the positioning precision and the mapping efficiency, and is suitable for robot navigation and autonomous movement tasks in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the SLAM technology, and particularly relates to an adaptive particle filtering method for laser SLAM tasks. This method aims to solve the bottleneck of the computational efficiency of traditional particle filtering algorithms and the problem of insufficient adaptability to dynamic environments, and improve the positioning and mapping performance of robots or unmanned systems in unknown environments. Background Art

[0002] In recent years, the robotics industry has been clearly listed as one of the ten important development fields, which indicates that robotics technology has become an important driving force for promoting the transformation and upgrading of the manufacturing industry. At the same time, with the rise of autonomous driving technology and the widespread application of intelligent systems in various fields, autonomous navigation ability has become one of the key indicators to measure the intelligence level of robots. As the core technology for achieving autonomous navigation, SLAM has naturally received extensive attention from the academic and industrial communities.

[0003] The goal of SLAM is to enable a robot or intelligent system to simultaneously complete its precise positioning and the real-time construction of an environmental map in a completely unknown environment. As an important branch of SLAM technology, laser SLAM has become a hot topic in current research and applications due to its high precision, strong stability, and good adaptability to complex environments. The laser SLAM technology combines the advantages of lidar sensors and SLAM algorithms, obtains two-dimensional or three-dimensional point cloud data of the surrounding environment through lidar, and combines the motion information of the robot to use advanced SLAM algorithms to estimate the position of the robot in real time and construct an environmental map.

[0004] In laser SLAM tasks, the particle filtering algorithm is widely used in robot pose estimation and map construction tasks due to its applicability to non-linear and non-Gaussian systems. However, the performance of traditional particle filtering algorithms still has many limitations in some complex environments. Traditional particle filtering relies on the motion model to predict the particle state, but in complex environments, this prediction is likely to cause particles to fall into local optimal solutions, resulting in a large deviation between the estimated distribution and the true distribution; during the iteration process, the particle weights may degenerate rapidly, leading to a reduction in the number of effective particles. This phenomenon not only reduces the accuracy of filtering, but may also cause the mapping effect to be distorted and the positioning accuracy to decrease; in high-dimensional state space or large-scale point cloud data processing scenarios, the computational complexity of traditional particle filtering algorithms increases significantly, limiting their performance in applications with high real-time requirements; in a dynamically changing environment, particle filtering requires more time to adjust the particle distribution to match the true state, which further affects the response speed of the system; traditional resampling methods tend to retain particles with larger weights and eliminate particles with smaller weights, but this strategy may lead to insufficient particle diversity, thus exacerbating the problem of sample impoverishment. Summary of the Invention

[0005] To solve the above problems, the present invention proposes an adaptive particle filtering method for laser SLAM tasks. This method comprehensively improves the performance of the particle filtering algorithm by introducing a combination strategy of PSO, KLD sampling and selective resampling, as well as GPU acceleration technology.

[0006] An adaptive particle filtering method for laser SLAM tasks according to the present invention includes the following steps:

[0007] S1. Use a lidar sensor to collect the original point cloud data in the environment;

[0008] S2. Combine PSO and particle filter prediction. On the basis of the traditional particle filter framework, introduce the PSO algorithm to improve the particle distribution state, predict the state of the particles according to the motion model of the robot, and optimize the search for the particle positions in combination with the PSO algorithm;

[0009] S3. Optimize the particle pose and adjust the particle state. Extract feature points from the environmental point cloud data and match them with the poses predicted by the particles to correct the positions and postures of the particles. In the iterative optimization process, gradually reduce the difference between the particle distribution and the true distribution;

[0010] S4. Dynamically adjust the PSO parameters. According to the current particle distribution state and environmental complexity, dynamically adjust the inertia weight and learning factor of the PSO algorithm;

[0011] S5. Calculate the ESS and KL divergence. Through the joint analysis of the ESS and KL divergence, judge whether the current particle set needs to be resampled to reduce the computational cost;

[0012] S6. According to the ESS and KL divergence calculated in step S5, adopt a dynamic resampling strategy. When the ESS is lower than the preset threshold or the KL divergence exceeds the specified range, trigger the resampling operation, remove the particles with smaller weights and regenerate new particles;

[0013] S7. Dynamically adjust the resampling frequency. In the case of rapid environmental changes or large fluctuations in particle distribution, increase the resampling frequency to quickly respond to environmental changes; conversely, reduce the resampling frequency when the environment is relatively stable to save computing resources;

[0014] S8. GPU-accelerate the resampling step. Divide the particle set into multiple subsets and allocate them to different GPU computing units for parallel execution, significantly shortening the resampling time.

[0015] Beneficial Effects

[0016] Compared with the prior art, the present invention can bring at least one of the following beneficial effects:

[0017] 1. By combining KLD sampling and selective resampling, the computational efficiency of the algorithm in processing large-scale point cloud data is improved.

[0018] 2. Dynamically switch the sampling strategy according to ESS and KL divergence, and at the same time adjust the resampling frequency based on the density and noise level of lidar point cloud data, alleviating the problem of sample impoverishment and improving the quality of particle distribution.

[0019] 3. Dynamically adjust the inertia weight and acceleration factor of PSO, enhancing the adaptability of the algorithm in complex dynamic environments.

[0020] 4. Specifically designed for laser SLAM tasks, it is more suitable for robot positioning and mapping in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] FIG. (1) is a flowchart of an adaptive particle filtering method for laser SLAM tasks implemented by the present invention;

[0022] FIG. (2) is a flowchart of PSO optimization adopted by the present invention;

[0023] FIG. (3) is a flowchart of dynamic resampling adopted by the present invention; DETAILED IMPLEMENTATION MANNER

[0024] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

[0025] As Figure 1 shown in the flowchart of the adaptive particle filtering method, it includes the following steps:

[0026] S1. Use a lidar sensor to collect the original point cloud data in the environment;

[0027] Specifically, use a lidar to scan the surrounding environment at a fixed frequency and return a series of three-dimensional coordinate points.

[0028] S2. Combine PSO and particle filter prediction;

[0029] Specifically, initialize a group of particles, and each particle represents a possible robot pose; the position of each particle is represented by its state vector, and its state vector usually includes the position x, y, and direction θ, that is, s i＝ [x i, y i, θ i , where s i represents the state of the i-th particle; define the fitness function based on the feature extraction result of the point cloud data, and the form is: f(x i ) = exp(-||z obs -z pred || 2 / (2σ2 ), where z obs is the actual observed value, and z pred is the observed value predicted according to the particle state. σ is the noise standard deviation, which is used to control the sensitivity of the fitness function.

[0030] S3. Optimize the particle pose and adjust the particle state;

[0031] Specifically, in each iteration, adjust the speed and position of the current particle according to its fitness value. The speed update formula of the particle is:

[0032] v i (t + 1) = ω·v i (t) + c1·rand()·(pbest i - x i (t)) + c2·rand()·(gbest - x i (t))

[0033] where v i (t) is the speed of the i-th particle at the t-th iteration, ω is the inertia weight, which controls the degree to which the particle maintains its original motion trend, c1 and c2 are acceleration factors, which respectively control the influence of individual experience and global experience on the particle motion, rand() is a random number in the interval [0, 1], pbest i is the best position of the i-th particle itself, and gbest is the best position of the entire population; adjust the position of the particle according to the updated speed as: x i (t + 1) = x i (t) + v i (t + 1).

[0034] S4. Dynamically adjust the PSO parameters;

[0035] Specifically, dynamically adjust the inertia weight and acceleration factors. The methods adopted are the linear decreasing strategy and the fixed ratio method. First, for the linear decreasing strategy of the inertia weight: where ω max is the initial inertia weight, set to 0.9, ω min is the minimum inertia weight, set to 0.4, t is the current iteration number, and T is the maximum iteration number; for the fixed ratio method of the acceleration factor: where C is the sum of the constants of the acceleration factors, f g and f l respectively represent the fitness values of the global optimum and the individual optimum. The entire process of steps S2 to S4 is as Figure 2 .

[0036] S5. Calculate ESS and KL divergence;

[0037] Specifically, the ESS and KL divergence are calculated as indicators to evaluate the quality of particle distribution. The calculation formula of ESS is: where ω i represents the weight of the i-th particle, and N is the total number of particles; the KL divergence formula is: where p(x) is the target distribution and q(x) is the current particle distribution.

[0038] S6. Adopt a dynamic resampling strategy based on the ESS and KL divergence calculated in step S5;

[0039] Specifically, when the calculated ESS value is lower than the set threshold N / 2, selective resampling is preferentially performed, and only the particles with lower weights are replaced, while the high-weight particles are retained; when the KL divergence exceeds the set threshold 0.5, KLD sampling is preferentially performed to ensure that the particle distribution is closer to the target distribution; if both conditions are met, selective resampling and KLD sampling are alternately performed to balance the calculation efficiency and the quality of particle distribution.

[0040] S7. Dynamically adjust the resampling frequency;

[0041] Specifically, it is adjusted according to the edge features in the current environmental observation data. When there are more edge features in the current observation data, that is, there are complex situations such as wall corners and obstacle boundaries, resampling needs to be performed more frequently. In this case, the threshold of ESS is set to N / 3, and the KL divergence threshold is set to 0.7 to ensure that the particle distribution can accurately capture these features; in a scene with few planar features in the observation data in an open field, the threshold of ESS is set to 2N / 3, and the KL divergence threshold is set to 0.3 to reduce the resampling frequency.

[0042] S8. GPU-accelerated resampling step;

[0043] Specifically, prepare the particle state and weight data on the host side and transfer them to the GPU video memory. Write a CUDA kernel function to implement the system resampling algorithm, set the thread block size to 256, the grid size to N / 256, compile the CUDA kernel function into executable code, and call it for resampling. Then transfer the particle state data after resampling from the GPU video memory back to the host memory.

Claims

1. An adaptive particle filter method for laser SLAM tasks, characterized in that Including: Obtain the original point cloud data; Introduce PSO in the particle filter prediction stage; Introduce the KLD sampling method in the particle filter resampling stage, and combine the KLD sampling method with the selective resampling method.

2. The adaptive particle filtering method according to claim 1, wherein The introduction of the particle swarm optimization algorithm in the prediction stage is as follows: Combine the particle swarm optimization algorithm with the prediction stage of the particle filter, and use the global search ability of PSO to optimize the position distribution of particles; Adjust the particle state by optimizing the particle pose to approximate the optimal trajectory and avoid local extreme value problems; Adjust the inertia weight and acceleration factor of PSO to enhance its adaptability in different scenarios.

3. The adaptive particle filtering method according to claim 1, wherein The introduction of the selective resampling and KLD sampling method in the resampling stage is to optimize the particle distribution according to the dynamic resampling strategy by calculating the effective particle number and KL divergence.

4. The adaptive particle filtering method according to claim 3, wherein The dynamic resampling strategy is as follows: Dynamically adjust the resampling frequency according to the density and noise level of the lidar point cloud data; Alternate selective resampling and KLD resampling; Based on the particle distribution state and statistical indicators, when the ESS is lower than the threshold, give priority to selective resampling; when the KL divergence exceeds the threshold, give priority to KLD sampling.

Images