FastSLAM algorithm based on improved resampling method and particle selection

Abstract

The invention discloses a FastSLAM algorithm based on an improved resampling method and particle selection. The algorithm comprises the following steps: (1) a robot predicts robot poses according to input control data and updates the robot poses and road signs according to measuring data combined with a measuring model of an external sensor of the robot; (2) the robot poses are predicted by the calculation of a particle filter, and a particle resampling criterion is amended according to an effective particle number, particle weighting covariance and particle-measuring residual consistency; (3) new particles are generated by using an index grad method and a crossover operator; (4) the robot is positioned, and a map is created according to the generated new particles. The invention improves the particle resampling criterion and a new particle-generating method in the FastSLAM algorithm, thereby obviously improving the estimation consistency of the FastSLAM algorithm to the robot poses and simultaneously improving the precision of the robot positioning and map creation.

Description

technical field [0001] The present invention relates to the field of robot simultaneous positioning and map creation, specifically an improved algorithm to the traditional FastSLAM algorithm, especially the correction to the resampling standard judgment and the particle resampling method in the FastSLAM algorithm. Background technique [0002] Under the condition that its own position and attitude are uncertain, a mobile robot uses its own internal equipment and external measurement and sensing devices to create a map in a completely unknown environment, and at the same time uses the created map to perform autonomous positioning, which is commonly known as simultaneous positioning and map creation. question. Since Smith et al. proposed the simultaneous localization and map creation algorithm based on the extended Kalman filter, the extended Kalman algorithm has become the main method for studying the simultaneous localization and map creation of robots. However, with the in...

AI Technical Summary

Problems solved by technology

However, with the in-depth research on the problem of simultaneous localization and map creation of robots, it is found that the extended Kalman filter algorithm has obvious defects: first, its computational complexity is proportional to the square of the number of detected landmarks (N) in the environment (O (N 2 )), and even if only one landmark is detected at the same time, the entire state covariance matrix must be updated; the second is that the extended Kalman filter algorithm is not robust enough to deal with the data association problem, and it cannot automatically perform automatic detection of data association errors in a timely manner. recover

Although FastSLAM solved the complexity and data association problems of the extended Kalman filter algorithm, Bailey et al. found that the consistency of robot pose estimation using FastSLAM algorithm was poor, and the diversity of particles increased with the robot The movement of the robot decreases exponentially. Bailey also found that by increasing the number of particles, the consistency of robot pose estimation can only be slightly improved, but the resulting problem is that the real-time performance of the simultaneous positioning of the robot and the map creation process is severely damaged.

Images