Multi-unmanned vehicle cooperative mapping method based on adaptive particle swarm optimization

Abstract

The invention discloses a multi-unmanned-vehicle cooperative mapping method based on an adaptive particle swarm algorithm, and aims to solve the problem that laser mapping fusion precision is reduced due to existence of dynamic obstacles during cooperative mapping of multiple unmanned vehicles in an indoor environment. The method comprises the following steps: firstly, carrying out feature matching through a self-adaptive local feature point matching region by adopting a joint compatible branch and bound algorithm, and calculating a joint mahalanobis distance optimization matching process of a feature matching hypothesis; secondly, by judging the change between the two frames, removing dynamic obstacles and updating a local map; secondly, optimizing a map fusion problem by adopting a particle swarm optimization algorithm, and searching a conversion matrix with the highest overlapping degree between maps; and finally, in order to prevent the particle swarm optimization from falling into local optimum, map fusion is realized in combination with a simulated annealing algorithm, and the map fusion precision is improved. According to the method, the map fusion problem is converted into the problem of solving the optimal solution by using the adaptive particle swarm algorithm fused with the simulated annealing algorithm, so that the map fusion precision is improved.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent perception and control equipment for unmanned systems, in particular to the simultaneous positioning and mapping of multiple unmanned vehicles, and more particularly to a collaborative mapping method for multiple unmanned vehicles based on an adaptive particle swarm algorithm. Background Art

[0002] Simulation Location And Mapping (SLAM) is widely used in the field of intelligent perception and control equipment represented by unmanned systems and robots, such as autonomous intelligent unmanned vehicles, surface unmanned ships, underwater salvage equipment, intelligent unmanned aerial vehicles, industrial robots, special operation robots, and service consumption robots. The simultaneous positioning and mapping of multiple unmanned vehicles mainly improves the efficiency of mapping through the collaborative work of multiple unmanned vehicles; through the collaborative work of multiple unmanned vehicles, the application functions of unmanned vehicles in complex scenarios such as intelligent warehousing, emergency rescue, and material handling can be increased. In terms of collaborative mapping of multiple unmanned vehicles, the accuracy of map construction depends not only on the accuracy of mapping of a single unmanned vehicle, but also on the accuracy of the mapping fusion algorithm. In addition, when mapping in an environment with dynamic obstacles, there are a large number of noise points in the laser data, which leads to a decrease in the accuracy of unmanned vehicle mapping and map fusion, and even map divergence.

[0003] The paper "A Robust, Multi-Hypothesis Approach to Matching Occupancy GridMaps" improves the grid map fusion algorithm. This improved method mainly calculates the transformation matrix by finding corresponding feature points in two maps, and divides all maps to be fused into two parts: feature maps and grid maps. Then, a set of relative transformation matrices is solved based on the feature correspondence between grid maps using a random consistency algorithm. However, the disadvantage of this method is that the accuracy of the calculated transformation matrix is ​​not high enough.

[0004] The paper "Multi-robot Map Construction with Improved Particle Swarm Optimization Algorithm" proposes a particle swarm optimization algorithm combined with a probability function for map registration. First, the particle swarm algorithm is used to search for the optimal transformation matrix between local maps to achieve map registration; then, based on the success rate of matching the overlapping areas of the local maps, an adaptive probability function is designed; finally, the registered local maps are fused into a global map. However, this method has the following problems:

[0005] 1. This method does not consider the impact of the accuracy of each local map on the map fusion effect, and is more dependent on the accuracy of the local map constructed by each unmanned vehicle.

[0006] 2. This method does not take into account the impact of dynamic obstacles on the mapping effect, and the mapping accuracy is low in an environment with dynamic obstacles.

[0007] 3. Although this method can prevent falling into the local optimal solution during map fusion to a certain extent, the ability to jump out of the local optimal solution is weakened when the map fusion probability is between 75% and 90%, and the map fusion effect is difficult to improve. Summary of the invention

[0008] In view of the shortcomings of the existing methods, the present invention provides a multi-unmanned vehicle collaborative mapping method based on an adaptive particle swarm algorithm, the purpose of which is to solve the problem of reduced mapping accuracy when mapping in an environment with dynamic obstacles. First, the positioning and mapping of the unmanned vehicle are divided into two parts: pose estimation and map estimation, and the joint compatible branch and bound algorithm is integrated to set an adaptive local feature point matching area, which can obtain a higher feature matching accuracy with less feature matching time; then, by judging the changes between the two frames, the dynamic obstacles are removed and the local map is updated; when the local maps constructed between the unmanned vehicles contain some of the same areas, the map fusion problem is converted into a problem of finding the optimal solution, and the particle swarm algorithm is used to find the transformation matrix that makes the two maps overlap the most; finally, in order to prevent the particle swarm algorithm from falling into the local optimum, the simulated annealing algorithm is integrated to achieve map fusion and improve the map fusion accuracy. The present invention uses an adaptive local feature matching area to process the matching of feature points, so that the accuracy of feature matching is higher, and by judging the changes between frames to remove dynamic obstacles, the accuracy of local maps is improved; the particle swarm algorithm is used to deal with the map fusion problem of multiple unmanned workshops, and a simulated annealing algorithm is introduced to prevent the particle swarm algorithm from falling into the local optimum. The current fitness value of each particle is compared with the previous fitness value. If the former is better than the latter, the individual optimal value is updated, otherwise the current solution is used as the new solution and the self-cognition function is adjusted. The fitness of the particle swarm is iteratively optimized, which improves the accuracy of map fusion. A collaborative mapping method for multiple unmanned vehicles based on an adaptive particle swarm algorithm:

[0009] S1: Split the state estimation of a single unmanned vehicle into two parts: pose estimation and state estimation. Perform pose initialization and pose estimation of a single unmanned vehicle, which specifically includes the following sub-steps:

[0010] S1.1: The state estimation of the unmanned vehicle is divided into two parts, one is the pose estimation and the other is the map estimation. The specific formula is as follows:

[0011] p(x 1:t ,m|z 1:t ,u 1:t-1 )=p(m|x 1:t ,z 1:t )·p(x 1:t |z1:t ,u 1:t-1 ) (1)

[0012] Where: m represents the constructed environment map; x 1:t =[x 1 ,x 2 ,···,x t ] T , represents the set of all positions of the unmanned vehicle from time 1 to time t; z 1:t =[z 1 ,z 2 ,···,z t ] T , represents the observation value of the laser radar from time 1 to time t; u 1:t-1 =[u 1 ,u 2 ,···,u t-1 ] T , represents the odometer input data from time 1 to time t-1.

[0013] S1.2: At the initial moment, the posture of the unmanned vehicle is initialized, and the prior probability distribution of the posture state of the unmanned vehicle is obtained according to the initial posture.

[0014] S1.3: Randomly select N particles from the probability distribution and set the weight of each particle to 1 / N. Each particle represents an estimate of the initial position of the unmanned vehicle.

[0015] S1.4: Estimate the position of the unmanned vehicle at time t based on the position estimation and odometer data of the unmanned vehicle at time t-1:

[0016]

[0017] Where: represents the i-th pose estimation of the unmanned vehicle at time t; Represents the i-th pose estimation composition from time 1 to time t-1.

[0018] S2: Perform feature matching by integrating the joint compatible branch and bound algorithm, set the adaptive local feature point matching area according to the density of feature points, and perform map estimation, which specifically includes the following sub-steps:

[0019] S2.1: According to the density of feature points, set the adaptive local feature matching area to ensure a high feature matching accuracy while reducing the size of the interpretation tree space that needs to be searched. The specific formula is as follows:

[0020] (x R,t -x m,t ) 2 +(y R,t -ym,t ) 2 ≤(r+d t ) 2 (3)

[0021] Where: (x R,t ,y R,t ) represents the estimated position of the unmanned vehicle in the world coordinate system at time t; (x m,t ,y m,t ) represents the position of a feature point in the local map in the world coordinate system; r represents the effective measurement distance of the laser radar; d t Represents the compensation distance at time t.

[0022] S2.2: Based on the environmental feature point m t and the laser radar observation z t Construct a feature matching hypothesis C t , the expression is:

[0023] C t =[c 1 ,c 2 ,···,c b ] T (4)

[0024] Where: c b Indicates the matching result between map features and observations. If c b A value of 0 means that no feature point in the constructed map at time t matches the observed value.

[0025] S2.3: Calculate the feature matching hypothesis C t The predicted observation values ​​of all feature points involved The specific formula is as follows:

[0026]

[0027] x t|t-1 =f(x t-1 ,u t-1 ) (6)

[0028] Where: h(·) represents the observation function of the unmanned vehicle; x t|t-1 represents the predicted position of the unmanned vehicle at time t; f(·) represents the motion function of the unmanned vehicle; x t-1 represents the estimated position of the unmanned vehicle at time t-1; u t-1 Represents the odometer information of the unmanned vehicle at time t-1.

[0029] S2.4: Calculate the observed value and verify the prediction error y t And the covariance matrix S of the prediction error t, the specific formula is as follows:

[0030]

[0031] Where: H s represents the Jacobian matrix of h(·) with respect to the position of the unmanned vehicle; P t|t-1 G represents the posterior prediction error covariance matrix of the unmanned vehicle posture at time t; t represents the covariance matrix of the lidar observation noise at time t.

[0032] S2.5: Calculate feature matching hypothesis C t The joint Mahalanobis distance

[0033]

[0034] S2.6: Define joint compatibility criteria:

[0035]

[0036] Where: The degrees of freedom are d = dim(y t ), chi-square distribution with confidence level α.

[0037] S2.7: Use the branch and bound search method to search the entire feature matching hypothesis interpretation tree, search incrementally from top to bottom to the bottom of the interpretation tree, and select the best one from all feature matches as the final feature matching result.

[0038] S3: Update particle weights, calculate particle degradation parameters, resample when the particle degradation parameters are greater than the threshold, select the optimal particle according to the weight value of each particle, obtain the motion trajectory of the unmanned vehicle, and complete the map update. It specifically includes the following sub-steps:

[0039] S3.1: Calculate the particle weight at the current moment according to the importance sampling formula, and assign a weight to each particle

[0040] S3.2: Calculate particle degradation parameter N eff :

[0041]

[0042] Where: represents the normalized weight.

[0043] S3.3: Calculate particle degradation parameter N eff :

[0044]

[0045] Where: represents the normalized weight,

[0046] S3.4: When the particle degradation degree exceeds the set threshold N th When , resample, otherwise continue sampling:

[0047] N eff >N th (15)

[0048] Where: N th =N / 2, N is the number of particles.

[0049] S3.5: Resample according to particle weights, delete some low-weight particles, and generate new high-weight particles.

[0050] S4: Setting a dynamic obstacle determination area and removing dynamic obstacles according to the changes between two frames, specifically including the following sub-steps:

[0051] S4.1: Set a dynamic obstacle determination area with the unmanned vehicle as the center, and determine whether there are obstacles in the area.

[0052] S4.2: If there is an obstacle, in the overlapping area between the previous frame and the current frame, which does not exist in the current frame but existed in the previous frame, it is determined to be a dynamic obstacle.

[0053] S4.3: Remove dynamic obstacles and update the local map.

[0054] S5: During the process of unmanned vehicle mapping, when the local maps constructed by two unmanned vehicles contain some of the same areas, the map fusion problem is transformed into the problem of finding the optimal solution. The particle swarm algorithm is used to find the transformation matrix that makes the two maps overlap the most. Specifically, it includes the following sub-steps:

[0055] S5.1: Discretize the grid map into a matrix of N rows and M columns:

[0056] m:[0,N]×[0,M]→R (16)

[0057] Where: m represents the confidence of the grid map, and the range of m is 0 to 1; the larger the value of m(x, y), the higher the probability that there is an obstacle in the grid.

[0058] S5.2: Map fusion is performed through the matrix transformation relationship between the coordinates of the unmanned vehicle. The transformation matrix is ​​expressed as:

[0059]

[0060] Where a is the translation parameter in the x direction; b is the translation parameter in the y direction; θ is the rotation parameter.

[0061] S5.3: When the unmanned vehicle builds a local map m 1 and m 2 After, m 1 Keep still, m 2 The translation and rotation are performed according to different transformation matrices until the overlapping area of ​​the two local maps is maximized. The overlapping area of ​​the two local maps is defined as:

[0062]

[0063] Where: Z(m 1 ,m 2 ) represents the similarity of the overlapping areas of two local maps; when m 1 [i,j]=m 2 [i,j] Eq(m 1 [i,j],m 2 [i,j])=1, otherwise Eq(m 1 [i,j],m 2 [i,j])=0.

[0064] S5.4: Select map similarity as the fitness function, then map m 1 and m 2 The similarity function can be expressed as:

[0065]

[0066] Where: dist(p 1 ,p 2 ) represents the grid p 1 With p 2 Manhattan distance; c represents the confidence of the grid map; num c (m 1 ) represents map m 1 The number of grids with parameter value c.

[0067] S5.5: Use particle swarm algorithm for map fusion, map m 1 and m 2 The convergence speed is Δ(m 1 ,m 2 ):

[0068]

[0069] Where: C is the set of grid map confidence.

[0070] S6: Integrate simulated annealing to prevent map fusion from falling into local optimality and complete map fusion:

[0071] S6.1: Set the initial temperature t_star, the final temperature t_end, and the current temperature t_cur. At the beginning of the iteration, t_cur = t_star, and the particle swarm algorithm iterates.

[0072] S6.2: In the later stage of iteration, according to the simulated annealing principle, for each particle, the current fitness value is compared with the previous fitness value. If the former is better than the latter, the individual optimal value is updated. Otherwise, let ΔP represent the difference between the current fitness value and the previous fitness value.

[0073] S6.3: If exp(-ΔP / T)>r, where r represents a random number between 0 and 1, then accept the current solution as the new solution and adjust the self-cognition function c 1 :

[0074] c 1 =tc×exp(-t_cur / t_star)×num (21)

[0075] Where: c 1 represents the self-cognitive function; tc represents the initial value of the self-cognitive function; num represents a random number of 0 and 1.

[0076] S6.4: Execute annealing operation. If the stopping condition is met, the algorithm stops, otherwise it continues to iterate. The termination condition is met:

[0077]

[0078] S6.5: Finally, find the transformation matrix that maximizes the overlap between the two maps. This matrix is ​​the optimal transformation matrix.

[0079] S6.6: Implement map fusion based on the obtained optimal transformation matrix.

[0080] The present invention has the following beneficial effects:

[0081] 1. Use adaptive local feature matching areas to process feature point matching, making feature matching more accurate. By dynamically adjusting the search area according to the density of feature points, the amount of calculation is reduced while ensuring the accuracy of feature matching, effectively improving the speed and quality of map construction.

[0082] 2. The determination and removal of dynamic obstacles have been established, which can update local maps in real time and improve the mapping accuracy of unmanned vehicles in dynamic environments.

[0083] 3. The adaptive particle swarm algorithm integrated with the simulated annealing algorithm is used to deal with the map fusion problem of multiple unmanned workshops. The current fitness value of each particle is compared with the previous fitness value. If the former is better than the latter, the individual optimal value is updated. Otherwise, the current solution is used as the new solution and its own cognitive function is adjusted. The fitness of the particle swarm is iteratively optimized to prevent the algorithm from falling into the local optimum and improve the overall effect of map fusion. This process can effectively solve the problem of map information fusion when multiple unmanned vehicles work together and improve the accuracy of collaborative mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0085] Figure 1 This is a flow chart of a collaborative mapping method for multiple unmanned vehicles based on an adaptive particle swarm algorithm;

[0086] Figure 2 It is a schematic diagram of the adaptive local feature matching area;

[0087] Figure 3 Remove flow chart for dynamic obstacles;

[0088] Figure 4 Flowchart of particle swarm optimization algorithm fused with simulated annealing algorithm;

[0089] Figure 5 Local maps built for autonomous vehicles;

[0090] Figure 6 This is the map fusion effect of the feature point correspondence method;

[0091] Figure 7 To improve the map fusion effect of particle swarm optimization algorithm;

[0092] Figure 8 This paper presents a collaborative mapping method for multiple unmanned vehicles based on adaptive particle swarm algorithm. DETAILED DESCRIPTION

[0093] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and understandable, a multi-unmanned vehicle collaborative mapping method based on adaptive particle swarm algorithm is provided. Figure 1 As shown, the following steps are included:

[0094] S1: Split the state estimation of the unmanned vehicle into two parts: pose estimation and map estimation. Initialize the pose of the unmanned vehicle based on the lidar and odometer data, and perform pose initialization and pose estimation of the unmanned vehicle. Specifically, it includes the following sub-steps:

[0095] S1.1: The state estimation of the unmanned vehicle is divided into two parts, one is the pose estimation and the other is the map estimation. The specific formula is as follows:

[0096] p(x 1:t ,m|z 1:t ,u 1:t-1 )=p(m|x 1:t ,z 1:t )·p(x 1:t |z 1:t ,u 1:t-1 ) (twenty three)

[0097] Where: m represents the constructed environment map; x 1:t =[x 1 ,x 2 ,···,x t ] T , represents the set of all positions of the unmanned vehicle from time 1 to time t; z 1:t =[z 1 ,z 2 ,···,z t ] T , represents the observation value of the laser radar from time 1 to time t; u 1:t-1 =[u 1 ,u 2 ,···,u t-1 ] T , represents the odometer input data from time 1 to time t-1.

[0098] S1.2: At the initial moment, the posture of the unmanned vehicle is initialized, and the prior probability distribution of the posture state of the unmanned vehicle is obtained according to the initial posture.

[0099] S1.3: Randomly select N particles from the probability distribution and set the weight of each particle to 1 / N. Each particle represents an estimate of the initial position of the unmanned vehicle.

[0100] S1.4: Estimate the position of the unmanned vehicle at time t based on the position estimation and odometer data of the unmanned vehicle at time t-1:

[0101]

[0102] Where: represents the i-th pose estimation of the unmanned vehicle at time t; Represents the i-th pose estimation composition from time 1 to time t-1.

[0103] S2: Perform feature matching by integrating the joint compatible branch and bound algorithm, and set the adaptive local feature point matching area according to the density of feature points. It specifically includes the following sub-steps:

[0104] S2.1: According to the density of feature points, set the adaptive local feature matching area, such as Figure 2 As shown, while ensuring a high feature matching accuracy, the size of the interpretation tree space that needs to be searched is reduced. The specific formula is as follows:

[0105] (x R,t -x m,t ) 2 +(y R,t -y m,t ) 2 ≤(r+d t ) 2 (25)

[0106] d t =d min +αp t r (26)

[0107] Where: (x R,t ,y R,t ) represents the estimated position of the unmanned vehicle in the world coordinate system at time t; (x m,t ,y m,t ) represents the position of a feature point in the local map in the world coordinate system; r represents the effective measurement distance of the laser radar; d t represents the compensation distance at time t; d min represents the minimum compensation distance; α represents the proportional adjustment coefficient. The denser the feature points in the environment, the larger α is; p t It indicates the proportion of laser data greater than 3r / 4 at time t to all laser data at time t, p t The larger it is, the more data the lidar has to measure the edge, so the compensation distance at time t increases accordingly, and the feature matching area at time t also expands accordingly.

[0108] S2.2: Based on the environmental feature point m t and z t Construct a feature matching hypothesis C t , the expression is:

[0109] C t =[c 1 ,c 2 ,···,c b ] T (27)

[0110] Where: c bIndicates the matching result between map features and laser data. If c b A value of 0 means that no feature point in the map constructed at time t matches the laser data.

[0111] S2.3: Calculate the feature matching hypothesis C t The predicted observation values ​​of all feature points involved The specific formula is as follows:

[0112]

[0113] x t|t-1 =f(x t-1 ,u t-1 ) (29)

[0114] Where: h(·) represents the observation function of the unmanned vehicle; x t|t-1 represents the predicted position of the unmanned vehicle at time t; f(·) represents the motion function of the unmanned vehicle; x t-1 represents the estimated position of the unmanned vehicle at time t-1; u t-1 Represents the odometer information of the unmanned vehicle at time t-1.

[0115] S2.4: Calculate the observed value and verify the prediction error y t And the covariance matrix S of the prediction error t , the specific formula is as follows:

[0116]

[0117] Where: H s represents the Jacobian matrix of h(·) with respect to the position of the unmanned vehicle; P t|t-1 G represents the posterior prediction error covariance matrix of the unmanned vehicle posture at time t; t represents the covariance matrix of the lidar observation noise at time t.

[0118] S2.5: Calculate feature matching hypothesis C t The joint Mahalanobis distance

[0119]

[0120] S2.6: Define joint compatibility criteria:

[0121]

[0122] Where: The degrees of freedom are d = dim(y t ), chi-square distribution with confidence level α.

[0123] S2.7: Use the branch and bound search method to search the entire feature matching hypothesis interpretation tree, search incrementally from top to bottom to the bottom of the interpretation tree, and select the best one from all feature matches as the final feature matching result.

[0124] S3: Update particle weights, calculate particle degradation parameters, resample when the particle degradation parameters are greater than the threshold, select the optimal particle according to the weight value of each particle, obtain the motion trajectory of the unmanned vehicle, and complete the map update. It specifically includes the following sub-steps:

[0125] S3.1: Calculate the particle weight at the current moment according to the importance sampling formula, and assign a weight to each particle

[0126] S3.2: Calculate particle degradation parameter N eff :

[0127]

[0128] Where: represents the normalized weight.

[0129] S3.3: When the particle degradation degree exceeds the set threshold N th When , resampling is performed:

[0130] N eff >N th (37)

[0131] Where: N th =N / 2, N is the number of particles.

[0132] S3.4: Select the best particle according to the weight value of each particle, obtain the motion trajectory of the unmanned vehicle, and complete the map update:

[0133]

[0134] Where: represents the optimal particle estimate; z 1:t Represents the observed value.

[0135] S4: Set the dynamic obstacle determination area and remove dynamic obstacles according to the changes between two frames, such as Figure 3 As shown, it specifically includes the following sub-steps:

[0136] S4.1: Set a dynamic obstacle determination area with the unmanned vehicle as the center, and determine whether there are obstacles in the area.

[0137] S4.2: If there is an obstacle, in the overlapping area between the previous frame and the current frame, which does not exist in the current frame but existed in the previous frame, it is determined to be a dynamic obstacle.

[0138] S4.3: Remove dynamic obstacles and update the local map.

[0139] S5: During the process of unmanned vehicle mapping, when the local maps constructed by two unmanned vehicles contain some of the same areas, the map fusion problem is transformed into the problem of finding the optimal solution. The particle swarm algorithm is used to find the transformation matrix that makes the two maps overlap the most. Specifically, it includes the following sub-steps:

[0140] S5.1: Discretize the grid map into a matrix of N rows and M columns:

[0141] m:[0,N]×[0,M]→R (39)

[0142] Where: m represents the confidence model of the grid map, and the range of m is 0 to 1; the larger the value of m(x, y), the higher the probability that there is an obstacle in the grid.

[0143] S5.2: Map fusion is performed through the matrix transformation relationship between the coordinates of the unmanned vehicle. The transformation matrix is ​​expressed as:

[0144] TR a,b,θ (x,y):R 2 →R 2 (40)

[0145]

[0146] Where a is the translation parameter in the x direction; b is the translation parameter in the y direction; θ is the rotation parameter.

[0147] S5.3: When the unmanned vehicle builds a local map m 1 and m 2 After, m 1 Keep still, m 2 The translation and rotation are performed according to different transformation matrices until the overlapping area of ​​the two local maps is maximized. The overlapping area of ​​the two local maps is defined as:

[0148]

[0149] Where: Z(m 1 ,m 2 ) represents the similarity of the overlapping areas of two local maps; when m 1 [i,j]=m 2 [i,j] Eq(m 1 [i,j],m 2 [i,j])=1, otherwise Eq(m 1 [i,j],m 2 [i,j])=0.

[0150] S5.4: Select map similarity as the fitness function, then map m 1 and m 2 The similarity function can be expressed as:

[0151]

[0152] Where: dist(p 1 ,p 2 ) represents the grid p 1 With p 2 Manhattan distance; c represents the confidence of the grid map; num c (m 1 ) represents map m 1 The number of grids with parameter value c.

[0153] S5.5: Use particle swarm algorithm for map fusion, map m 1 and m 2 The convergence speed is Δ(m 1 ,m 2 ):

[0154]

[0155] Where: C is the set of grid map confidence.

[0156] S6: Fusion simulated annealing algorithm prevents map fusion from falling into local optimum, such as Figure 4 As shown, map fusion is completed:

[0157] S6.1: Set the initial temperature t_star, the final temperature t_end, and the current temperature t_cur. At the beginning of the iteration, t_cur = t_star, and the particle swarm algorithm iterates.

[0158] S6.2: In the later stage of iteration, according to the simulated annealing principle, for each particle, the current fitness value is compared with the previous fitness value. If the former is better than the latter, the individual optimal value is updated. Otherwise, let ΔP represent the difference between the current fitness value and the previous fitness value.

[0159] S6.3: If exp(-ΔP / T)>r, where r represents a random number between 0 and 1, then accept the current solution as the new solution and adjust the self-cognition function c 1 :

[0160] c 1 =tc×exp(-t_cur / t_star)×num (45)

[0161] Where: c 1represents the self-cognition function; tc represents the initial value of the self-cognition function; num represents a random number between 0 and 1.

[0162] S6.4: Execute annealing operation. If the stopping condition is met, the algorithm stops, otherwise it continues to iterate. The termination condition is met:

[0163]

[0164] S6.5: Finally, find the transformation matrix that maximizes the overlap between the two maps. This matrix is ​​the optimal transformation matrix.

[0165] S6.6: Implement map fusion based on the obtained optimal transformation matrix.

[0166] The invention firstly divides the state estimation of the unmanned vehicle into pose estimation and map estimation, and uses the laser radar and odometer data to initialize and estimate the pose of the unmanned vehicle; for feature matching, a joint compatible branch and bound algorithm is used to perform map estimation through adaptive local feature point matching area, and the joint Mahalanobis distance of the feature matching hypothesis is calculated to optimize the matching process; in the process of updating particle weights, the degradation degree of particles is calculated and resampled to ensure that particles with higher weights are selected, so as to obtain more accurate pose estimation and map update; then, by judging the changes between two frames, dynamic obstacles are removed and local maps are updated; when there are identical parts in the overlapping areas between local maps, a particle swarm algorithm is used to optimize the map fusion problem to find the transformation matrix with the highest overlap. In the fusion process, in order to prevent falling into the local optimum, the simulated annealing algorithm is combined to compare the current fitness value of each particle with the previous fitness value. If the former is better than the latter, the individual optimal value is updated, otherwise the current solution is used as the new solution and the self-cognition function is adjusted, and the fitness of the particle swarm is iteratively optimized, and finally the optimal transformation matrix is ​​found to achieve map fusion.

[0167] The present invention is verified by combining the test results below: Figure 5 Local maps are constructed for unmanned vehicles, and map fusion is performed based on the local maps; Figure 6 This is the map fusion effect of the feature point correspondence method. It can be seen that dynamic obstacles remain on the map and the map fusion effect is not good; Figure 7 In order to improve the map fusion effect of particle swarm optimization algorithm, Figure 7 The following box shows that the algorithm has no ability to remove dynamic obstacles. Dynamic obstacles still remain on the map, and the algorithm fails to escape the local optimum. Figure 7 The local map in the frame above is not fully aligned, and the map fusion effect is not ideal; Figure 8This paper proposes a collaborative mapping method for multiple unmanned vehicles based on an adaptive particle swarm algorithm, which removes dynamic obstacles in the environment, improves the accuracy of local maps, and Figure 7 In comparison, the map registration effect is effectively improved, and the mapping accuracy is improved.

[0168] The specific implementation scheme described above further illustrates the purpose of the invention, technical scheme and beneficial effects of the present invention. The above embodiments are only used to illustrate the technical scheme of the present invention, rather than to limit the scope of protection of the present invention. Those skilled in the art should understand that any modification or equivalent substitution of the technical scheme of the present invention is included in the scope of protection of the present invention.

Claims

1. A multi-unmanned vehicle collaborative mapping method based on adaptive particle swarm algorithm, characterized in that: The following steps are involved: S1: Split the state estimation of the unmanned vehicle into two parts: pose estimation and map estimation. Initialize the pose of the unmanned vehicle based on the lidar and odometer data, and perform pose initialization and pose estimation of the unmanned vehicle. Specifically, it includes the following sub-steps: S1.1: The state estimation of the unmanned vehicle is divided into two parts, one is the pose estimation and the other is the map estimation. The specific formula is as follows: p(x 1:t ,m|z 1:t ,u 1:t-1 )=p(m|x 1:t ,z 1:t )·p(x 1:t |z 1:t ,u 1:t-1 ) (1) Where: m represents the constructed environment map; x 1:t =[x1,x2,···,x t ] T , represents the set of all positions of the unmanned vehicle from time 1 to time t; z 1:t =[z1,z2,···,z t ] T , represents the observation value of the laser radar from time 1 to time t; u 1:t-1 =[u1,u2,···,u t-1 ] T , represents the odometer input data from time 1 to time t-1; S1.2: At the initial moment, the posture of the unmanned vehicle is initialized, and the prior probability distribution of the posture state of the unmanned vehicle is obtained according to the initial posture; S1.3: Randomly select N particles from the probability distribution and set the weight of each particle to 1 / N. Each particle represents an estimate of the initial position of the unmanned vehicle. S1.4: Estimate the position of the unmanned vehicle at time t based on the position estimation and odometer data of the unmanned vehicle at time t-1: Where: represents the i-th pose estimation of the unmanned vehicle at time t; Represents the i-th pose estimation composition from time 1 to time t-1; S2: Perform feature matching by integrating the joint compatible branch and bound algorithm, using the density of feature points to set an adaptive local feature point matching area, and perform feature matching, which specifically includes the following sub-steps: S2.1: According to the density of feature points, set the adaptive local feature matching area to ensure a high feature matching accuracy while reducing the size of the interpretation tree space that needs to be searched. The specific formula is as follows: (x R,t -x m,t ) 2 +(y R,t -y m,t ) 2 ≤(r+d t ) 2 (3) Where: (x R,t ,y R,t ) represents the estimated position of the unmanned vehicle in the world coordinate system at time t; (x m,t ,y m,t ) represents the position of a feature point in the local map in the world coordinate system; r represents the effective measurement distance of the laser radar; d t represents the compensation distance at time t; S2.2: Based on the environmental feature point m t and the laser radar observation z t Construct a feature matching hypothesis C t , the expression is: C t =[c1,c2,···,c b ] T (4) Where: c b Indicates the matching result between map features and observations. If c b If the value of is 0, it means that no feature point in the constructed map at time t matches the observed value; S2.3: Calculate the feature matching hypothesis C t The predicted observation values ​​of all feature points involved S2.4: Calculate the observed value and verify the prediction error y t And the covariance matrix S of the prediction error t ; S2.5: Calculate feature matching hypothesis C t The joint Mahalanobis distance S2.6: Define joint compatibility criteria: Where: The degrees of freedom are d = dim(y t ), chi-square distribution with confidence level α; S2.7: Use the branch and bound search method to search the entire feature matching hypothesis interpretation tree, search incrementally from top to bottom to the bottom of the interpretation tree, and select the best one from all feature matches as the final feature matching result; S3: Update particle weights, calculate particle degradation parameters, resample when the particle degradation parameters are greater than the threshold, select the optimal particle according to the weight value of each particle, obtain the motion trajectory of the unmanned vehicle, and complete the map update. It specifically includes the following sub-steps: S3.1: Calculate the particle weight at the current moment according to the importance sampling formula, and assign a weight to each particle S3.2: Calculate particle degradation parameter N eff : Where: represents the normalized weight; S3.3: Calculate particle degradation parameter N eff : Where: represents the normalized weight, S3.4: When the particle degradation degree exceeds the set threshold N th When , resample, otherwise continue sampling; S3.5: Select the best particle according to the weight value of each particle, obtain the motion trajectory of the unmanned vehicle, and complete the map update: Where: represents the optimal particle estimate; z 1:t represents the observed value; S4: Setting a dynamic obstacle determination area and removing dynamic obstacles according to the changes between two frames, specifically including the following sub-steps: S4.1: Set a dynamic obstacle determination area with the unmanned vehicle as the center, and determine whether there are obstacles in the area; S4.2: If there is an obstacle, in the overlapping area between the previous frame and the current frame, which does not exist in the current frame but existed in the previous frame, it is determined to be a dynamic obstacle; S4.3: Remove dynamic obstacles and update the local map; S5: During the process of unmanned vehicle mapping, when the local maps constructed by two unmanned vehicles contain some of the same areas, the map fusion problem is transformed into the problem of finding the optimal solution. The particle swarm algorithm is used to find the transformation matrix that makes the two maps overlap the most. Specifically, it includes the following sub-steps: S5.1: Discretize the grid map into a matrix of N rows and M columns: m:[0,N]×[0,M]→R (12) Where: m represents the confidence of the grid map, and the range of m is 0 to 1; the larger the value of m(x, y), the higher the probability that there is an obstacle in the grid; S5.2: Map fusion through matrix transformation relationship between unmanned vehicle coordinates; S5.3: After the unmanned vehicle builds local maps m1 and m2, m1 remains stationary, and m2 is translated and rotated according to different transformation matrices until the overlapping area of ​​the two local maps is maximized. The overlapping area of ​​the two local maps is defined as: Where: Z(m1,m2) represents the similarity of the overlapping areas of two local maps; when m1[i,j]=m2[i,j], Eq(m1[i,j],m2[i,j])=1, otherwise Eq(m1[i,j],m2[i,j])=0; S5.4: Select map similarity as the fitness function, then the similarity function of maps m1 and m2 can be expressed as: Where: dist(p1,p2) represents the Manhattan distance between grids p1 and p2; c represents the confidence of the grid map; num c (m1) represents the number of grids with parameter value c in map m1; S5.5: Particle swarm algorithm is used for map fusion. The convergence speed of maps m1 and m2 is Δ(m1,m2): Where: C is the set of grid map confidence; S6: Integrate the simulated annealing algorithm to prevent map fusion from falling into local optimality and complete map fusion: S6.1: Set the initial temperature t_star, the final temperature t_end, and the current temperature t_cur. At the beginning of the iteration, t_cur = t_star, and the particle swarm algorithm iterates; S6.2: In the later stage of iteration, according to the simulated annealing principle, for each particle, the current fitness value is compared with the previous fitness value. If the former is better than the latter, the individual optimal value is updated. Otherwise, let ΔP represent the difference between the current fitness value and the previous fitness value. S6.3: If exp(-ΔP / T)>r, where r represents a random number between 0 and 1, then accept the current solution as the new solution and adjust the self-cognition function c1: c1=tc×exp(-t_cur / t_star)×num (16) Where: c1 represents the self-cognitive function; tc represents the initial value of the self-cognitive function; num represents a random number of 0 and 1; S6.4: Execute annealing operation. If the stopping condition is met, the algorithm stops, otherwise it continues to iterate; S6.5: Finally, find the transformation matrix that maximizes the overlap between the two maps. This matrix is ​​the optimal transformation matrix. S6.6: Implement map fusion based on the obtained optimal transformation matrix.

Images