Improved Particle Filter Localization and Mapping Method Based on Harmony Search Algorithm

Abstract

The present invention discloses an improved particle filter localization and mapping method based on the harmony search algorithm, which includes initializing particles and particle weights; obtaining the particle set at the current moment according to the proposal distribution and calculating the particle weights; optimizing the particle pose using the harmony search algorithm; further optimizing using the niche algorithm; calculating and normalizing the optimized particle weights; calculating the number of effective particles, and performing adaptive optimization combination resampling based on the deviation-corrected EWA optimization algorithm; updating the map information of the particles based on the particle motion trajectory to establish an actual map. The particle filter method provided by the present invention can obtain more accurate filtering estimates while using fewer particles; the resampling improvement strategy solves the problem of particle impoverishment caused by traditional resampling while ensuring particle diversity. Improvements in multiple aspects improve the efficiency and accuracy of localization and mapping.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping, and particularly relates to an improved particle filter positioning and mapping method based on a harmony search algorithm. Background Art

[0002] Simultaneous Localization and Mapping of a mobile robot refers to the robot moving in an unknown environment, estimating its pose based on the observation information of sensors and the position information of an odometer, and then constructing a map according to its own position, so as to achieve the purpose of simultaneous localization and mapping. A real-time and high-precision SLAM algorithm is a key technology for the robot to achieve autonomous positioning, path planning, and autonomous navigation. The difficulty lies in how to use a filtering method to construct a map of an unknown environment and achieve precise positioning through the sensors carried by the robot itself.

[0003] Since lidar has the advantages of being less affected by lighting conditions and having a precise ranging range, it is currently the most widely used technology in the SLAM field. At the same time, the method based on particle filter has good real-time performance and can process positioning and mapping online. The traditional particle filter algorithm represents the posterior probability through a set of particles. However, as the number of algorithm iterations increases, the particle degradation problem will occur. During the particle filter resampling process, since a large number of low-weight particles are eliminated, the particle impoverishment phenomenon will be caused, reducing the diversity of particles. Summary of the Invention

[0004] In order to improve the performance and effect of the existing technical solutions, the present invention proposes an improved particle filter positioning and mapping method based on a harmony search algorithm, which uses the harmony search algorithm to optimize the particle set after sampling, and adds a niche algorithm to punish particles with low fitness for further optimization. Finally, an adaptive optimization combination resampling is performed using an EWA optimization algorithm based on deviation correction to alleviate the particle impoverishment problem caused by traditional resampling. Multiple improvements enhance the efficiency and accuracy of positioning and mapping. It includes initializing particles and particle weights; obtaining the particle set at the current moment according to the proposal distribution and calculating particle weights; optimizing the particle pose using the harmony search algorithm; further optimizing using the niche algorithm; calculating and normalizing the optimized particle weights; calculating the number of effective particles, and performing adaptive optimization combination resampling using an EWA optimization algorithm based on deviation correction; updating the map information of the particles based on the movement trajectory of the particles to establish an actual map. The particle filter method provided by the present invention can obtain a more accurate filtering estimate while using fewer particles; the resampling improvement strategy solves the particle impoverishment problem caused by traditional resampling while ensuring particle diversity. Multiple improvements enhance the efficiency and accuracy of positioning and mapping.

[0005] To achieve the above object, the present invention specifically adopts the following technical solutions:

[0006] An improved particle filter localization and mapping method based on the harmony search algorithm, characterized by including the following steps:

[0007] Step S1: At time t = 0, randomly select N particles, and denote the weight of the i-th particle among them as

[0008] Step S2: Use the odometer information as the proposal distribution, and estimate the particle set at the current moment according to the particle set and the proposal distribution at the previous moment, and calculate the weight of each particle according to the principle of importance sampling; and the proposal distribution for the particle set at the current moment is estimated, and the weight of each particle is calculated according to the principle of importance sampling;

[0009] Step S3: Use the harmony search algorithm to optimize the pose of the sampled particles. One particle in the particle set corresponds to a harmony in the harmony search algorithm, and the harmony is adjusted and optimized based on the fitness function to obtain a more accurate particle state;

[0010] Step S4: Use the niche algorithm for optimization, and use the niche radius to punish the particles with low fitness within the radius to further optimize the particles;

[0011] Step S5: Calculate the weights of the optimized particles and normalize them;

[0012] Step S6: When the number of effective particles N eff is less than N / 2, perform adaptive optimization combination resampling based on the deviation-corrected EWA optimization algorithm;

[0013] Step S7: According to the optimized pose, update the maps of the respective particles with reference to the current lidar scan data, and use the map of the particle with the highest weight as the currently estimated map.

[0014] Furthermore, step S3 specifically includes:

[0015] Step S31: Initialize the harmony search algorithm, determine the values of various parameters, including: the size HMS of the harmony memory, the maximum value HMCR of the value-taking probability of the harmony memory max and the minimum value HMCR min 、the maximum value PAR of the pitch fine-tuning probability max and the minimum value PAR min 、the maximum value BW of the pitch fine-tuning bandwidth max and the minimum value BW min 、the number of creations T max ;

[0016] Step S32: Adjust the fixed harmony memory consideration rate (HMCR) so that it changes dynamically with the iteration number \(t_i\). The formula is: where;

[0017] Step S33: Adjust the fixed pitch adjustment rate (PAR) so that it changes dynamically with the iteration number \(t_i\). The formula is:

[0018] Step S34: Dynamically adjust the pitch adjustment bandwidth, which changes according to the iteration number \(t_i\). Taking \(T\) max / 2 as the threshold, the change of BW follows the following rules:

[0019] Step S35: In the generation of new harmonies, replace the harmony variable at a certain position in the optimal harmony in the harmony memory with the harmony variable at the same position in the new harmony, that is where, is the \(j\)-th harmony variable of the optimal harmony in the harmony memory.

[0020] Furthermore, Step S4 specifically includes:

[0021] Step S41: Introduce the latest observation value into the fitness function, and define the fitness formula as:

[0022] where, \(R\) k is the variance of the observation noise, \(Z\) k is the latest observation value, is the predicted observation value;

[0023] Step S42: Calculate the distance norm \(d\) i between particle \(x\) j and \(x\) ij : \(d\) ij = ||\(x\) i - \(x\) j ||, \(i, j = 1, 2, \ldots, N, i \neq j\);

[0024] Step S43: If \(d\) ij < \(\sigma\), where \(\sigma\) is the niche radius parameter, calculate the fitness \(f(x\) i and \(f(x\) j ) of particles \(x\) i ) and \(x\) j ). Compare the magnitudes of \(f(x\) i ) and \(f(x\) j ), and use the penalty function to reduce the fitness of the particle with the smaller value in \(f(x\) i ) and \(f(x\) j ), that is, the fitness of the particle with the lower fitness: where, For particles with lower fitness, p(k) is the penalty function;

[0025] Step S44: Use the Gaussian mutation operator to perform random perturbation: where μ is the perturbation control factor, G(0,1) is a Gaussian distribution random variable with a mean of 0 and a variance of 1, is the Gaussian perturbation term;

[0026] Step S45: Repeat Step S42 - Step S44 until the preset number of iterations is reached or the distance between particles is not less than the niche radius, then jump out of the niche algorithm.

[0027] Furthermore, Step S6 specifically includes:

[0028] Step S61: Calculate the effective particle number N eff to measure the degree of particle degradation, When the effective particle number N eff < N / 2, resampling is performed;

[0029] Step S62: Sort the particle weights in ascending order;

[0030] Step S63: Use the EWA optimization algorithm based on bias correction to calculate the average value distribution curve of the sorted particle set, and the expression is: where, are the average weights of the first i and the first i - 1 particles respectively, α is the tuning function, and is the bias correction term;

[0031] Step S64: Calculate the average value of the curve Taking as the benchmark, all particles are divided into high, medium, and low weight particles according to the particle weights, and two particle weight division thresholds are set as ω l and ω h , where, The high and low weight particles form a new unstable particle set S: or

[0032] Step S65: Do not perform resampling operations on stable particles, perform resampling on the unstable particle set S, randomly select two particles from the particle set S, and the resampling formula is: x new = x a + KL(x b - x a ), where x new is the newly generated particle, x a is the particle with a larger weight among the randomly selected particles, xb is a particle with a smaller weight among randomly selected particles, K is the step coefficient, and L is the step size in the direction of (x b - x a ); The specific expression formula of L is where N s is the number of particles in the particle set S, β is the distribution probability of sampling particles in the neighborhood space of any sampling particle, and m is the dimension of the sampling space;

[0033] Step S66: When the sum of the newly generated particles and the stable particles reaches the total number of particles N, stop the resampling operation.

[0034] Furthermore, step S1 specifically includes: when t = 0, N particles are selected according to the prior probability p(x0) of the robot motion model, denoted as The weights of the sampled particles are evenly distributed as:

[0035] Furthermore, step S2 specifically includes: estimating the current particle set based on the particle set at the previous moment and the proposal distribution. The proposal distribution generally uses odometry information, and the sampled particles obey the importance density function: where x t is the sampling pose of the particle at time t, is the sampling pose of the particle at time t - 1, and u t is the odometry information at time t; Then, through the formula: the weight of each particle is calculated, where is the weight of the i-th particle at time t - 1, are the system observation, state model, and prior probability distribution function respectively.

[0036] Furthermore, in step S3:

[0037] Regarding each particle in the current particle set as a harmony, each pitch in the harmony is selected and adjusted to generate particles with a more accurate pose. Specifically:

[0038] Step A31: Initialize the harmony search algorithm and determine the values of various parameters, including: the size HMS of the harmony memory library, the maximum value HMCR of the harmony memory library value selection probability max and the minimum value HMCR min , the maximum value PAR of the pitch fine-tuning probability max and the minimum value PAR min , the maximum value BW of the pitch fine-tuning bandwidth maxand the minimum value BW min 、the number of creations T max ;

[0039] Step A32: Adjust the value-taking probability HMCR of the fixed harmony memory bank to make it change dynamically with the iteration number ti. The formula is:

[0040]

[0041] Step A33: Adjust the fixed pitch fine-tuning probability PAR to make it change dynamically with the iteration number ti. The formula is:

[0042]

[0043] Step A34: Dynamically adjust the pitch fine-tuning bandwidth, which changes according to the iteration number t. Using T max / 2 as the threshold, the change of BW follows the following rules:

[0044]

[0045] Step A35: Introduce the latest observation value into the fitness function. Define the fitness formula of the harmony search algorithm as where R k is the variance of the observation noise, Z k is the latest observation value, is the predicted observation value;

[0046] Step A36: Initialize the harmony memory bank. Each particle represents a harmony, and the pose parameters carried by the particle are each harmony variable; randomly generate HMS harmonies x1, x2,..., x HMS and put them into the harmony memory bank; the form of the memory bank is:

[0047]

[0048] Step A37: Generate a new harmony For each pitch of the new harmony Generate a random number rand1 between [0, 1]. If rand1 ≤ HMCR, randomly take out a harmony variable from the harmony memory bank as the new pitch. Otherwise, randomly generate a harmony variable from the solution space as the new pitch. The rule is:

[0049] where X is the solution space;

[0050] Step A38: If the harmony variable is obtained from the harmony memory bank, it needs to be fine-tuned; generate a random number rand2 between [0, 1]. If rand2 ≤ PAR, adjust the obtained harmony variable according to the fine-tuning bandwidth BW, otherwise, make no adjustment. The rule is:

[0051]

[0052] Step A39: After generating the new harmony, replace the harmony variable at a certain position in the new harmony with the harmony variable at the same position in the optimal harmony in the harmony memory bank, that is where, is the j-th harmony variable of the optimal harmony in the harmony memory bank;

[0053] Step A310: Update the harmony memory bank: Evaluate the new harmony xnew, that is, calculate its fitness. If the fitness is better than the worst value in the harmony memory bank, then use x new to replace the harmony x with the worst function value in the memory bank worst , otherwise, make no modification. The rule is: If then xworst = xnew:

[0054] Step A311: Repeat Step A37 - Step A310 until the number of iterations reaches Tmax.

[0055] Furthermore, Step S5 specifically includes: Update the weights of the particle set optimized in Step S3 and Step S4. Calculate the weight of each particle through the formula: where, is the weight of the i-th particle at time t - 1, are the system observation, state model, and prior probability distribution function respectively, and perform normalization:

[0056] Furthermore, Step S7 specifically includes: At time t, obtain the pose state of the robot according to the optimized particle information Combine the observation information of the sensor Update the map, where the map carried by the particle with the largest weight is the best estimated map at the current moment; For the map construction within time t, according to the information of the particles in the particle set Combine the observation values at each moment Calculate the environmental map data That is Perform the update of the map.

[0057] Compared with the prior art, the present invention and its preferred solutions utilize the harmony search algorithm to optimize the sampled particle set, incorporate the niche algorithm to penalize particles with low fitness for further optimization, and finally perform adaptive optimization combination resampling using the EWA optimization algorithm based on deviation correction to alleviate the particle depletion problem caused by traditional resampling, improving the efficiency and accuracy of positioning and mapping in multiple aspects. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a schematic diagram of the main step flow of the method of the present invention;

[0059] Figure 2 It is a schematic diagram of the process of optimizing the pose of sampled particles by the harmony search algorithm of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0060] To make the features and advantages of this patent more obvious and understandable, specific embodiments are given below for detailed description as follows:

[0061] It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those of ordinary skill in the technical field to which this application belongs.

[0062] In this embodiment, the harmony search optimization algorithm is combined with particle filtering to design a particle filtering algorithm optimized by harmony search. Its main idea is to use the harmony search algorithm for the optimization in the particle selection stage of particle filtering, improve the particles generated only using the odometer motion model as the proposal distribution, generate particles that meet certain performance index requirements, thereby making up for the unreliability of the ordinary particle filtering algorithm in the particle generation stage, shortening the convergence time of the particle filtering algorithm, and improving the estimation accuracy of the estimation algorithm. And an adaptive optimization combination resampling method based on the EWA optimization algorithm based on deviation correction is used to improve the traditional resampling method, alleviating the particle depletion problem caused in the traditional resampling process. And the improved algorithm is applied to robot SLAM, and the core is to improve the proficiency and estimation accuracy of the algorithm, and optimize the accuracy and stability of the robot's running trajectory and map construction.

[0063] Refer to Figure 1 and Figure 2 , the embodiment of the present invention provides an improved particle filtering positioning and mapping method based on the harmony search algorithm, including the following steps:

[0064] Step S1: At time t = 0, randomly select N particles and denote their weights as

[0065] Step S2: Use the odometer information as the proposal distribution, and according to the particle set at the previous moment And the proposed distribution estimates the particle set at the current moment and calculates the weight of each particle according to the principle of importance sampling;

[0066] Step S3: Use the harmony search algorithm to optimize the pose of the sampled particles. One particle in the particle set corresponds to one harmony in the harmony search algorithm, and the harmony is adjusted and optimized based on the fitness function to obtain a more accurate particle state;

[0067] Step S4: The niche algorithm is used for optimization, and particles with low fitness within the niche radius are punished to further optimize the particles;

[0068] Step S5: Calculate the weights of the optimized particles and normalize them;

[0069] Step S6: When the number of effective particles N eff is less than N / 2, an adaptive optimization combination resampling is performed based on the deviation-corrected EWA optimization algorithm;

[0070] Step S7: According to the optimized pose, the maps of each particle are updated with reference to the current lidar scan data, and the map of the particle with the highest weight is used as the currently estimated map.

[0071] Step S1 specifically includes:

[0072] When t = 0, N particles are selected according to the prior probability p(x0) of the robot motion model, denoted as The weights of the sampled particles are evenly distributed as:

[0073] Step S2 specifically includes:

[0074] Based on the particle set at the previous moment and the proposed distribution estimates the particle set at the current moment The proposed distribution generally uses odometry information, and the sampled particles obey the importance density function: where x t is the sampled pose of the particle at time t, is the sampled pose of the particle at time t-1, and u t is the odometry information at time t; then through the formula: the weight of each particle is calculated, where is the weight of the i-th particle at time t-1, are the system observation, state model, and prior probability distribution function respectively.

[0075] Step S3 specifically includes:

[0076] Treat each particle in the particle swarm at the current moment as a harmony, select and adjust each pitch in the harmony to generate a more beautiful harmony, that is, generate particles with more accurate poses;

[0077] Step S31: Initialize the harmony search algorithm and determine various parameter values: the size HMS of the harmony memory, the maximum value HMCR of the harmony memory selection probability max and the minimum value HMCR min 、the maximum value PAR of the pitch fine-tuning probability max and the minimum value PAR min 、the maximum value BW of the pitch fine-tuning bandwidth max and the minimum value BW min 、the number of creations T mmax ;

[0078] Step S32: Considering that a larger selection probability is beneficial to finding the local optimal solution in the initial stage of optimization, and a smaller selection probability is beneficial to increasing the diversity of solutions in the later stage. Therefore, adjust the fixed harmony memory selection probability HMCR so that it changes dynamically with the iteration number t. The formula is:

[0079] where t is the current iteration number;

[0080] Step S33: In the initial stage, a smaller fine-tuning probability is beneficial to finding the local optimal value, while in the later stage, a larger fine-tuning probability can increase the diversity of solutions. Therefore, adjust the fixed pitch fine-tuning probability PAR so that it changes dynamically with the iteration number t. The formula is:

[0081] where t is the current iteration number;

[0082] Step S34: Using a larger fine-tuning bandwidth in the initial stage of the search is beneficial for the algorithm to detect in a larger range. In the later stage of the search, a smaller fine-tuning bandwidth is beneficial for fine search in a small range. Therefore, dynamically adjust the pitch fine-tuning bandwidth, which changes according to the iteration number t. Taking T max / 2 as the threshold, the change of BW follows the following rules:

[0083]

[0084] Step S35: Set the optimization criterion (i.e., the fitness function). Introduce the latest observation value into the fitness function. Define the fitness formula of the harmony search algorithm as where, R k is the variance of the observation noise, Z k is the latest observation value, is the predicted observation value;

[0085] Step S36: Initialize the harmony memory library. Each particle represents a harmony, and the pose parameters carried by the particle are each harmony variable. Randomly generate HMS harmonies x1, x2, …, x HMS and put them into the harmony memory library. The form of the memory library is:

[0086]

[0087] Step S37: Generate a new harmony For each pitch of the new harmony Generate a random number rand1 between [0, 1]. If rand1 ≤ HMCR, randomly select a harmony variable from the harmony memory library as the new pitch; otherwise, randomly generate a harmony variable from the solution space as the new pitch. The rule is:

[0088] where X is the solution space;

[0089] Step S38: If the harmony variable is obtained from the harmony memory library, it needs to be fine-tuned. Generate a random number rand2 between [0, 1]. If rand2 ≤ PAR, adjust the obtained harmony variable according to the fine-tuning bandwidth BW; otherwise, make no adjustment. The rule is:

[0090]

[0091] Step S39: After the new harmony is generated, replace the harmony variable at a certain position in the optimal harmony in the harmony memory library with the harmony variable at the same position in the new harmony, that is where, is the j-th harmony variable of the optimal harmony in the harmony memory library;

[0092] Step S310: Update the harmony memory library. Evaluate the new harmony x new that is, calculate its fitness. If the fitness is better than the worst value in the harmony memory library, then replace x new with the harmony x worst with the worst function value in the memory library, otherwise, make no modification. The rule is: If then x worst = x new :

[0093] Step S311: Repeat steps S37, S38, S39, S310 until the number of iterations reaches Tmax.

[0094] Step S4 specifically includes:

[0095] Using the particle pose optimized by the harmony search algorithm, continue to judge the distance between each particle. If it is less than the niche radius, use the penalty function to further reduce the fitness value of the particle with a smaller fitness difference. Finally, use the Gaussian mutation operator to randomly perturb the particle fitness, so that the particle with a smaller fitness difference moves away from the particle with a better fitness;

[0096] Step S41: Introduce the latest observation value into the fitness function, and define the fitness formula as:

[0097] where R k is the variance of the observation noise, Z k is the latest observation value, is the predicted observation value;

[0098] Step S42: Calculate the distance norm d i between particle x j and x ij : d ij = ||x i - x j ||, i, j = 1, 2,..., N, i ≠ j;

[0099] Step S43: If d ij < σ, where σ is the niche radius parameter, calculate the fitness f(x i ) and f(x j ) of particles x i and x j . Compare the magnitudes of f(x i ) and f(x j ), and use the penalty function to reduce the fitness of the particle with a smaller value in f(x i ) and f(x j ), that is, the particle with a lower fitness: where is the particle with a lower fitness, and p(k) is the penalty function;

[0100] Step S44: Use the Gaussian mutation operator to perform random perturbation on : where μ is the perturbation control factor, G(0, 1) is a Gaussian distribution random variable with a mean of 0 and a variance of 1, is the Gaussian perturbation term;

[0101] Step S45: Repeat steps S42, S43, and S44 until the preset number of iterations is reached or the distance between particles is not less than the niche radius, and then jump out of the niche algorithm.

[0102] Step S5 includes:

[0103] Update the weights of the optimized particle set in steps S3 and S4 through the formula: Calculate the weight of each particle, where, is the weight of the i-th particle at time t-1, are the system observation, state model, and prior probability distribution function respectively, and perform normalization:

[0104] Step S6 includes:

[0105] Step S61: Calculate the effective number of particles Neff to measure the degree of particle degradation, When the effective number of particles N eff < N / 2, resampling is performed;

[0106] Step S62: Sort the particle weights in ascending order;

[0107] Step S63: Use the EWA optimization algorithm based on bias correction to calculate the average value distribution curve of the sorted particle set, and the expression is: Where, is the average weight of the previous i and i-1 particles, α is the tuning hyperfunction, is the weight of the i-th particle, 1-α i is the bias correction term;

[0108] Step S64: Calculate The average value of the curve Taking as the benchmark, divide all particles into high, medium, and low weight particles according to the particle weights, and set the particle weight division threshold to ω l and ω h , where, The high and low weight particles form a new unstable particle set S: or

[0109] Step S65: Do not perform resampling operations on stable particles, perform resampling on the unstable particle set S, randomly select two particles from the particle set S, and the resampling formula is: x new = x a + KL(x b - x a ), where, x new is the newly generated particle, x a is the particle with a larger weight among the randomly selected particles, x b is the particle with a smaller weight among the randomly selected particles, K is the step coefficient, and L is the step size in the direction of (x b - x a ); The specific expression formula of L is where N s is the number of particles in the particle set S, β is the distribution probability of the sampled particles in the neighborhood space of any sampled particle, and m is the dimension of the sampling space;

[0110] Step S66: Stop the resampling operation when the sum of the newly generated particles and the stable particles reaches the total number of particles N.

[0111] Step S7 includes:

[0112] At time t, obtain the pose state of the robot according to the optimized particle information Combine the observation information of the sensor Update the map. Among them, the map carried by the particle with the largest weight is the best estimated map at the current moment; for the map construction within time t, according to the information of the particles in the particle set Combine the observation values at each moment Calculate the environmental map data That is Perform the update of the map.

[0113] The above is only a preferred embodiment of the present invention, and it is not a limitation of the present invention in other forms. Any person skilled in the art may use the disclosed technical content to make changes or modifications into equivalent embodiments with equivalent changes. However, any simple modification, equivalent change and modification made to the above embodiments based on the technical essence of the present invention without departing from the technical solution content of the present invention still belong to the protection scope of the technical solution of the present invention.

[0114] This patent is not limited to the above best implementation manner. Anyone can obtain various other forms of improved particle filter localization and mapping methods based on the harmony search algorithm under the inspiration of this patent. All equal changes and modifications made according to the scope of the patent application of the present invention shall fall within the scope covered by this patent.

Claims

1. An improved particle filter localization and mapping method based on the harmony search algorithm, characterized in that, It includes the following steps: Step S1: At time t = 0, randomly select N particles, and denote the weight of the i-th particle among them as Step S2: Use the odometer information as the proposal distribution, and estimate the particle set at the current moment based on the particle set at the previous moment and the proposal distribution, and calculate the weight of each particle according to the principle of importance sampling; ​ Step S3: Optimize the pose of the sampled particles using the harmony search algorithm. One particle in the particle set corresponds to one harmony in the harmony search algorithm. Adjust and optimize the harmony based on the fitness function to obtain a more accurate particle state; Step S4: Use the niche algorithm for optimization. Penalize the particles with low fitness within the niche radius using the niche radius to further optimize the particles; Step S5: Calculate the weights of the optimized particles and normalize them; Step S6: When the number of valid particles N eff is less than N / 2, perform adaptive optimization combined resampling based on the deviation-corrected EWA optimization algorithm; Step S7: According to the optimized pose, update the maps of each particle with reference to the current lidar scan data, and use the map of the particle with the highest weight as the currently estimated map; Step S3 specifically includes: Step S31: Initialize the harmony search algorithm and determine various parameter values, including: the size HMS of the harmony memory library, the maximum value HMCR of the harmony memory library value selection probability max and the minimum value HMCR min , the maximum value PAR of the pitch fine-tuning probability max and the minimum value PAR min , the maximum value BW of the pitch fine-tuning bandwidth max and the minimum value BW min , the number of creations T max ; Step S32: Adjust the fixed value probability HMCR of the harmony memory library so that it changes dynamically with the iteration number ti. The formula is: where; Step S33: Adjust the fixed pitch fine-tuning probability PAR so that it changes dynamically with the number of iterations ti. The formula is: Step S34: Dynamically adjust the tone fine-tuning bandwidth, which varies according to the iteration number ti, with T max / 2 as the threshold, and the BW change follows the following rules: Step S35: In the generation of the new harmony, replace the harmony variable at the same position in the new harmony with the harmony variable at a certain position of the optimal harmony in the harmony memory bank, that is i = j, where is the j-th harmony variable of the optimal harmony in the harmony memory bank; Step S4 specifically includes: Step S41: Introduce the latest observation value into the fitness function, and define the fitness formula as: Among them, R k is the variance of the observation noise, Z k is the latest observation value, is the predicted observation value; Step S42: Calculate particle x i and x j the distance norm d ij between: d ij = ||x i - x j ||, i, j = 1, 2, …, N, i ≠ j; Step S43: If d ij <σ, σ is the microhabitat radius parameter, calculate the particle x i With x j The fitness f(x i ) and f(x j ), compared with f(x i ) and f(x j ) size, and use the penalty function to reduce f(x i ) and f(x j ) has a smaller value, that is, the fitness of the particle with lower fitness: in, is a particle with low fitness, and p(k) is a penalty function; Step S44: Use the Gaussian mutation operator to perform random perturbation: where μ is the perturbation control factor, G(0,1) is a Gaussian distribution random variable with a mean of 0 and a variance of 1, is the Gaussian perturbation term; Step S45: Repeat Step S42 - Step S44 until the preset number of iterations is reached or the distance between particles is not less than the niche radius, then jump out of the niche algorithm.

2. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, characterized in that Step S6 specifically includes: Step S61: Calculate the number of effective particles N eff to measure the degree of particle degradation, When the number of effective particles N eff < N / 2, resampling is performed; Step S62: Sort the particle weights in ascending order; Step S63: Calculate the average value distribution curve of the sorted particle set by using the EWA optimization algorithm based on deviation correction, and the expression is: where are the average weights of the first i and the first i - 1 particles respectively, α is a tuning function and is a deviation correction term; Step S64: Calculate the average value of the curve Based on as the reference, all particles are divided into high, medium, and low weight particles according to the particle weight, and two particle weight division thresholds are set as ω l and ω h , where the high and low weight particles form a new unstable particle set S: or Step S65: No resampling operation is performed on stable particles. Resampling is performed on the unstable particle set S. Randomly select two particles from the particle set S. The resampling formula is: x new = x a + KL(x b - x a ), where x new is the newly generated particle, x a is the particle with a larger weight among the randomly selected particles, x b is the particle with a smaller weight among the randomly selected particles, K is the step coefficient, and L is the step length in the direction of (x b - x a ); The specific expression formula of L is where N s is the number of particles in the particle set S, β is the distribution probability of sampling particles in the neighborhood space of any sampling particle, and m is the dimension of the sampling space; Step S66: Stop the resampling operation when the sum of the newly generated particles and the stable particles reaches the total number of particles N.

3. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, wherein Step S1 specifically includes: when t = 0, N particles are selected according to the prior probability p(x0) of the robot motion model, denoted as The weights are evenly distributed to the sampled particles.

4. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, characterized in that Step S2 specifically includes: based on the particle set at the previous moment and the proposal distribution, estimate the particle set at the current moment The proposal distribution uses odometry information, and the sampled particles obey the importance density function: where x t is the sampled pose of the particle at time t, is the sampled pose of the particle at time t - 1, and u t is the odometry information at time t; then, through the formula: calculate the weight of each particle, where is the weight of the i-th particle at time t - 1, are the system observation, state model, and prior probability distribution function respectively.

5. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, characterized in that In Step S3: Regard each particle in the current particle set at the current moment as a harmony, select and adjust each pitch in the harmony to generate particles with a more accurate pose. Specifically: Step A31: Initialize the harmony search algorithm and determine various parameter values, including: the size HMS of the harmony memory, the maximum value HMCR of the harmony memory value probability max and the minimum value HMCR min , the maximum value PAR of the pitch adjustment probability max and the minimum value PAR min , the maximum value BW of the pitch adjustment bandwidth max and the minimum value BW min , the number of creations T max ; Step A32: Adjust the value-taking probability HMCR of the fixed harmony memory bank to make it change dynamically with the iteration number ti. The formula is: Step A33: Adjust the pitch fine-tuning probability PAR of the fixed value to make it change dynamically with the iteration number ti. The formula is: Step A34: Dynamically adjust the pitch fine-tuning bandwidth, which varies according to the iteration number t, with T max / 2 as the threshold, and the BW change follows the following rules: Step A35: The latest observation value is introduced into the fitness function, and the fitness formula of the harmony search algorithm is defined as where R k is the variance of the observation noise, Z k is the latest observation value, and is the predicted observation value; Step A36: Initialize the harmony memory bank. Each particle represents a harmony, and the pose parameters carried by the particle are each harmony variable; randomly generate HMS harmonies x1, x2, …, x HMS and put them into the harmony memory bank; the form of the memory bank is: Step A37: Generate a new harmony For each pitch of the new harmony Generate a random number rand1 between [0, 1]. If rand1 ≤ HMCR, randomly select a harmony variable from the harmony memory library as the new pitch; otherwise, randomly generate a harmony variable from the solution space as the new pitch. The rule is as follows: where X is the solution space; Step A38: If the harmony variable is obtained from the harmony memory bank, then it needs to be fine-tuned; generate a random number rand2 between [0,1]. If rand2 ≤ PAR, then adjust the obtained harmony variable according to the fine-tuning bandwidth BW, otherwise, do not make any adjustment. The rule is: Step A39: After the new harmony is generated, replace the harmony variable at a certain position in the optimal harmony in the harmony memory bank with the harmony variable at the same position in the new harmony, that is i = j, where is the j-th harmony variable of the optimal harmony in the harmony memory bank; Step A310: Update the harmony memory library: Evaluate the new harmony x new by calculating its fitness. If the fitness is better than the worst value in the harmony memory library, then replace x new with the harmony x worst with the worst function value in the memory library. Otherwise, make no modification. The rule is: If then x worst = x new ; Step A311: Repeat Step A37 - Step A310 until the iteration number reaches Tmax.

6. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, characterized in that Step S5 specifically includes: updating the weights of the optimized particle sets in Steps S3 and S4 through the formula: calculating the weight of each particle, where is the weight of the i-th particle at time t-1, are the system observation, state model, and prior probability distribution function respectively, and is normalized.

7. The improved particle filter localization and mapping method based on the harmony search algorithm according to claim 1, characterized in that Step S7 specifically includes: at time t, obtaining the pose state of the robot according to the optimized particle information Combined with the observation information of the sensor Update the map, where the map carried by the particle with the largest weight is the best estimated map at the current moment; for the map construction within time t, according to the information of the particles in the particle set Combined with the observation values at each moment Calculate the environmental map data That is Perform the update of the map

Images