A self-recovery monte carlo positioning algorithm

Abstract

The application relates to the technical field of navigation positioning algorithms, and particularly discloses a self-recovery Monte Carlo positioning algorithm, when the change of the moving distance or the moving direction of a robot exceeds a preset moving distance or moving direction change threshold, particles are updated according to a motion model to update the pose, and the weight of the particles is calculated according to the measurement probability of a laser radar likelihood domain model; the current positioning reliability gamma is calculated according to a reliability model; the relationship between gamma and a preset positioning reliability threshold eta is judged; when gamma is smaller than eta, it is considered that kidnapping has occurred, then the estimated pose reliability determines the particle distribution parameters and the particles are redistributed at the current position to obtain a current positioning result. The method proposes a positioning reliability model, performs reliability judgment on positioning, and further judges whether the robot kidnapping positioning problem occurs.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of navigation positioning algorithm. BACKGROUND

[0002] The traditional Monte Carlo localization (MCL) simulates the possible pose of the robot by throwing particles in space, and then matches the laser radar data with the environment map to evaluate the weight of each particle, and according to the weight, the importance sampling is carried out to make the particles concentrate around the real pose of the robot. However, MCL also has the following problems: 1, the number of particles sampled each time is constant, which may cause waste of computing resources; 2, when the particle group converges fully, too many particles will cause redundancy; 3, when the robot slips, the particle group cannot describe the jump out of the range of the real pose, resulting in positioning error. In order to solve the above problems, adaptive Monte Carlo localization (AMCL) introduces dynamic sampling (KLD), changes the number of sampling particles in each iteration, and alleviates the redundancy problem caused by the fixed number of MCL particles. At the same time, the repositioning function is realized by increasing random particles, avoiding the problem of losing positioning and being unable to reposition. Therefore, AMCL solves the problems of MCL particle redundancy and losing positioning and being unable to reposition.

[0003] AMCL solves the global positioning problem in the current form, but it has the problem of being unable to recover from robot kidnapping or global positioning failure. To some extent, the particles can only "survive" around a single pose. If this pose happens to be incorrect, the algorithm cannot recover. The random sampling function in AMCL solves the problem of MCL being unable to reposition to some extent, but the strategy of randomly sampling after losing positioning leads to slow recovery positioning speed, and the repositioning speed needs to be further improved. The AMCL algorithm reduces the influence of particle degeneration by continuously copying particles with large weights and deleting particles with small weights. However, as the time iteration increases, the variance of particle weights will increase, leading to the fact that the degeneration phenomenon cannot be completely avoided. After multiple consecutive iterations, in addition to a few particles, the weights of other particles become very small and can be ignored. Therefore, continuous iteration will lead to a large amount of computing resources being consumed on these insignificant particles, increasing the running time of the algorithm, wasting computing resources, and even possibly affecting the running result of the algorithm. The idea of the AMCL algorithm is to increase random particles to the particle set and introduce some random states in the motion model. However, this method raises two problems: one is how many particles should be added in each iteration of the algorithm, and the other is from which distribution these particles should be generated. Selecting a suitable proposal distribution can ensure the diversity and reasonableness of the samples, and thus ensure the final effect of the filter. Compared with simply increasing the number of particles, finding a suitable proposal distribution can not only reduce the running time of the algorithm, but also possibly improve the effect of the algorithm. SUMMARY

[0004] The application aims to provide an improved traditional adaptive Monte Carlo localization algorithm, proposes a localization reliability model, judges the localization reliability, and further judges whether a robot kidnapping localization problem occurs.

[0005] To achieve the above-mentioned purpose, the technical scheme of the application is as follows:

[0006] S1, the program starts running;

[0007] S2, the initial position of the robot is positioned as a given initial value, the particle position and direction are initialized according to the Gaussian distribution according to the given initial position mean and covariance, and the initial weight of the particle is averaged according to the total number of particles;

[0008] S3, when the movement distance or the change of the movement direction of the robot exceeds the preset movement distance or movement direction change threshold, the particle is updated according to the motion model, and the weight of the particle is calculated according to the measurement probability of the laser radar likelihood domain model;

[0009] S4, the current localization reliability γ is calculated according to the reliability model;

[0010] S5, the relationship between γ and the preset localization reliability threshold η is judged;

[0011] When γ is greater than η, it is considered that the reliability is high, then the particle is resampled according to the current weight, and the current localization result is calculated,

[0012] When γ is less than η, it is considered that the kidnapping phenomenon occurs, then the current pose is estimated according to the IMU motion model, the estimated pose is obtained, the localization reliability γ of the estimated pose is calculated according to the reliability model at the estimated pose, the localization reliability γ of the estimated pose is obtained, then the particle distribution parameters are determined according to the localization reliability γ of the estimated pose, and the particle is redistributed at the current position to obtain the current localization result.

[0013] The method for calculating the localization reliability γ through the reliability model is as follows:

[0014] Let represent the end point of the obstacle detected by the laser radar, let represent the pose of the robot in the global coordinate system at time t, let represent the position relationship between the laser radar local coordinate system and the robot coordinate system, let represent the angle of the radar beam relative to the coordinate system of the robot, and let represent the maximum distance detected by the laser radar, and the maximum distance detected by the laser radar is calculated according to the laser radar the output of the kth beam at time t The formula is: The end point position in the global coordinate system is obtained, when = It is considered that the laser radar does not monitor the obstacle, and the likelihood domain measurement model discards the measurement result of the maximum distance;

[0015] The Gaussian distribution is used to model the measurement noise in the measurement process, and in the two-dimensional x-y space, the distance between the coordinates of the obstacle measured by the laser radar and the nearest obstacle on the map m is represented by d k t, and the measurement probability of the laser radar is obtained by the Gaussian function For unestimable random noise in the measurement process, a uniform distribution is used to model: The measurement probability of the kth laser at time t is obtained as:

[0016] Assuming that the noise between the measurements of each beam of the laser radar is independent, the measurement probability at time t is obtained by the product of each The sum of the cubes of each is taken as the measurement probability of the particle;

[0017] The measurement probability is normalized as: , wherein represents the measurement noise parameter, represents the random noise parameter, represents the maximum measurement distance, represents the number of beams selected from all beams for calculating the measurement probability, and the highest value of the measurement probability in the particle set is taken as the positioning reliability gamma.

[0018] By adopting the above technical scheme, the beneficial effects of the present application are: the above method proposed by the present application is a self-recovery Monte Carlo localization algorithm (SR-MCL) fusing IMU, a positioning reliability model is constructed on the basis of the laser radar likelihood domain model, the positioning reliability is analyzed, and then the kidnapping problem is judged, in the case of the kidnapping problem of the robot, the position of the kidnapped robot is estimated by fusing IMU, in the case of the estimation result, the particles are redistributed at the estimated position, a particle redistribution model is constructed, and resampling is performed to obtain the actual position. The present application adopts the above method for the shortcomings of uncontrollable increase of particles in random position and increase of particle quantity of the traditional AMCL, the method has higher convergence efficiency compared with the global random distribution, so as to achieve the purpose of the present application.​​​ BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 is a flow chart of a self-recovery Monte Carlo localization algorithm involved in the present application. DETAILED DESCRIPTION

[0020] In order to further explain the technical solutions of the present application, the present application will be described in detail below through specific embodiments.

[0021] A self-recovery Monte Carlo localization algorithm, the algorithm flow of which is shown in Figure 1 , and the steps are as follows:

[0022] S1, the program starts running;

[0023] S2, the initial position of the robot is positioned as a given initial value, and the particle position and direction are initialized according to the given initial position mean and covariance in accordance with the Gaussian distribution, and the initial weight of the particle is evenly taken according to the total number of particles;

[0024] S3, when the change of the moving distance or the moving direction of the robot exceeds the preset threshold of the moving distance or the moving direction change, the particle is updated in the pose according to the motion model, and the weight of the particle is calculated according to the measurement probability of the laser radar likelihood domain model;

[0025] S4, the current positioning reliability γ is calculated according to the reliability model;

[0026] S5, the relationship between γ and the preset positioning reliability threshold η is judged;

[0027] When γ is greater than η, it is considered that the reliability is high, then the particle is resampled according to the current weight, and the current positioning result is calculated,

[0028] When γ is less than η, it is determined that the current position is unreliable, and it is considered that kidnapping has occurred, then the current pose of the kidnapped robot is estimated according to the IMU motion model, the estimated pose is obtained, the positioning reliability γ of the estimated pose is calculated according to the reliability model at the estimated pose, the particle distribution parameters are determined according to the positioning reliability γ of the estimated pose, and the particle is redistributed at the current position, the current positioning result is obtained, and the pose of the kidnapped robot is determined. The positioning reliability γ calculated by the reliability model is as follows:

[0029] The main idea of the likelihood domain model is to map the end point of the obstacle detected by the laser radar to the global coordinate space of the map, which is represented by , and is the pose of the robot in the global coordinate system at time t, and represents the position relationship between the local coordinate system of the laser radar and the coordinate system of the robot, and is represented by represents the angle of the radar beam relative to the coordinate system of the robot, and is represented by represents the maximum distance detected by the laser radar, and is calculated according to the laser radar represents the position of the kth beam at the tth moment represents the output of the kth beam at the tth moment is represented by the following formula: The end position of the robot can be obtained by the following formula: The position of the robot in the global coordinate system is calculated, and since the calculation is meaningful only when the radar detects an obstacle, the calculated coordinate is meaningless when Therefore, it is considered that the laser radar does not monitor the obstacle, and the likelihood domain measurement model simply discards the measurement result of the maximum distance.

[0030] Next, the measurement noise in the measurement process is modeled using a Gaussian distribution, which is as follows: in the two-dimensional x-y space, let represent the distance between the coordinate of the obstacle measured by the laser radar and the nearest obstacle on the map m, and the measurement probability of the laser radar can be obtained by a Gaussian function with 0 as the mean value, For the unestimable random noise in the measurement process, a uniform distribution is used for modeling: The measurement probability of the kth laser at the tth moment is obtained by:

[0031] Assuming that the noise between the measurements of the beams of the laser radar is independent of each other, the measurement probability at the tth moment is obtained by the product of the measurement probabilities of each However, when the range of is relatively large, the obtained probability value is particularly small, and therefore in the algorithm proposed in the present text, the sum of the cubes of each is taken as the measurement probability.

[0032] The reliability at the current moment is used to determine the measurement probability of the position of all particles in the current particle set, and since the particles are gathered around the correct position without kidnapping, all particles are still located at the position before kidnapping, and therefore the measurement probability of all particles becomes particularly low. In the method of the present text, the highest value of the measurement probability in the particle set is taken as the positioning reliability γ of the current positioning to determine kidnapping, and since the artificial setting of the parameters in the likelihood domain model will cause the measurement probability to have no definite range, the measurement probability is normalized as follows: wherein represents the measurement noise parameter, represents the random noise parameter,​​​ represents the maximum measurement distance, represents the number of beams selected from all beams for calculating the measurement probability. Since particles will gather to the particle position with high measurement probability, the positioning reliability take the maximum value of the measurement probability of all particle positions.

[0033] In summary, the positioning reliability model first calculates the measurement probability of the lidar according to the likelihood domain model of the lidar, obtains the particle weight, and obtains the weight of all particles, and then calculates the positioning reliability γ.

[0034] The above embodiments and drawings are not limited to the product form and style of the present application, and any appropriate changes or modifications made by any ordinary skilled person in the art shall be considered as not departing from the patent scope of the present application.

Claims

1. A self-recovering Monte Carlo localization algorithm, characterized by the following algorithm flow: S1. The program starts running; S2. The robot's initial position is determined by a given initial value. The particle position and orientation are initialized according to a Gaussian distribution based on the given initial position mean and covariance. The initial weight of the particles is the average of the total number of particles. S3. When the robot moves a distance or changes in direction that exceeds the preset threshold for the change in distance or direction, the particle updates its pose according to the motion model and calculates the particle's weight according to the measurement probability of the LiDAR's likelihood domain model. S4. Calculate the current positioning reliability γ based on the reliability model; S5. Determine the relationship between γ and the preset positioning reliability threshold η; When γ is greater than η, the reliability is considered high. Therefore, particle resampling is performed based on the current weights to calculate the current positioning result. When γ is less than η, it is considered that a kidnapping phenomenon has occurred. Then, the current pose is pre-estimated according to the IMU motion model to obtain the pre-estimated pose. At the pre-estimated pose, the positioning reliability γ is calculated according to the reliability model to obtain the positioning reliability γ of the pre-estimated pose. Then, the particle distribution parameters are determined according to the positioning reliability γ of the pre-estimated pose, and the particles are redistributed at the current position to obtain the current positioning result. The positioning reliability γ is calculated using the reliability model as follows: use The endpoint of the obstacle detected by the lidar is indicated by... express The robot's pose in the global coordinate system at any given time, using To represent the positional relationship between the lidar's local coordinate system and the robot's coordinate system, use... The angle of the radar beam relative to the robot's coordinate system is represented by... This indicates the maximum detection range of the lidar, based on the lidar... Time of the first Output of each beam Sutra: The endpoint can be obtained. At the position in global coordinates, when = When it is assumed that the lidar has not detected any obstacles, its likelihood domain measurement model discards the measurement results for the maximum distance. The measurement noise during the measurement process is modeled using a Gaussian distribution in a two-dimensional xy space. Let represent the distance between the coordinates of the obstacle measured by the lidar and the nearest obstacle on map m. The measurement probability of the lidar is given by a Gaussian function. Therefore, for unpredictable random noise during the measurement process, a uniform distribution is used for modeling: The measurement probability of the k-th laser beam at time t is obtained as follows: ; Assuming the noise between the various beams of the lidar is independent, then in At any given time, the measurement probability at each particle's position is determined by each... The product is obtained by multiplying each... The sum of the cubes is used as the measurement probability of the particle; Normalize the measurement probability: In the formula Indicates the measured noise parameter. Represents random noise parameters. Indicates the maximum measured distance. This represents the number of beams selected from all beams for calculating the measurement probability, with the highest value of the measurement probability in the particle set taken as the positioning reliability γ.

Images