Semantic map matching method and system based on genetic algorithm

Abstract

The invention belongs to the field of large-scale semantic map matching, and particularly relates to a semantic map matching method and system based on a genetic algorithm. The method comprises the following steps: for received point cloud maps A and B, determining potential matching points based on geometric and semantic distribution; constructing the conversion matrix into an individual, and establishing a fitness function; and evaluating the advantages and disadvantages of individuals by virtue of a fitness function, and guiding population evolution by selecting crossover mutation operation to realize map matching. Feature points are reserved to the maximum extent while data noise is removed, and the calculation speed and precision are improved; fitness calculation between corresponding points is achieved through a semi-object-level geometric feature description method; the optimization problem modeling of the conversion matrix is a genetic algorithm, and a self-adaptive variable step size variation algorithm is put forward to jump out of local optimum, so that maximum global optimum is realized.

Description

Technical Field

[0001] The present invention belongs to the field of large-scale semantic map matching, and in particular relates to a semantic map matching method and system based on a genetic algorithm. Background Art

[0002] With the in-depth application of robots in firefighting and rescue, counter-terrorism and other fields, it is difficult for a single robot to perform tasks to meet the requirements of the tasks, and there is a phenomenon of using multiple robots to collaborate to complete complex tasks. Compared with a single robot, multi-robot collaboration can complete complex tasks more efficiently and robustly. However, the design of multi-robot collaborative systems is also more complicated, especially issues such as task planning, information transmission, and multi-source heterogeneous data fusion have greatly affected the stability and efficiency of multi-machine systems. The working scenes of multi-robot systems generally have large scales or diverse perspectives, and how to obtain a global map of the scene is a basic problem. Limited by network bandwidth and robot computing power, a single robot usually cannot have a global map of the scene. Therefore, it is necessary to match the maps of multiple robots and establish a global map.

[0003] Map matching finds the transformation matrix between two maps by matching similar features between maps. The maps used for fusion are generated by different robots, and the distance and perspective differences between robots will affect the similar features used for matching. The greater the distance between the two robots and the greater the perspective difference, the greater the difference in the geometric feature description of the same object, which increases the difficulty of matching. Therefore, the algorithms (ICP / NDT) that only rely on geometric features such as points, lines, normal vectors or curvature of point clouds for matching are highly dependent on the overlap rate and are too affected by noise. Some scholars have also proposed using semantic features for fusion and achieved good results. However, these algorithms require that the local maps have a large overlap rate (40% or even higher), a small rotation angle difference (30° or even lower), and a small translation error (10 meters or even lower). When the local maps do not meet these conditions, or in the local maps that meet these conditions, the algorithms still have the problem of extremely large errors. In addition, when the map scale increases, the number of point clouds will increase and the features will become more complicated, which will lead to reduced accuracy and increased time. How to balance efficiency and accuracy is also a problem that matching algorithms need to solve. Summary of the invention

[0004] The purpose of the present invention is to overcome the defects of the prior art and propose a semantic map matching method and system based on genetic algorithm.

[0005] In order to achieve the above object, the present invention proposes a semantic map matching method based on genetic algorithm, comprising:

[0006] Step 1) For the received point cloud maps A and B, determine potential matching points based on geometric and semantic distribution;

[0007] Step 2) construct the transformation matrix into individuals and establish a fitness function;

[0008] Step 3) Rely on the fitness function to evaluate the quality of individuals, use selection, crossover and mutation operations to guide population evolution and achieve map matching.

[0009] Preferably, the step 1) comprises:

[0010] Step 1-1) Filter the point cloud maps A and B according to the semantic labels and remove the points whose semantic labels are roads;

[0011] Step 1-2) For each point in the filtered point cloud map A, the response value is calculated respectively, and the point with the largest response value in the neighborhood is marked as a potential key point. The response value of the potential key point is compared with the threshold ξ, and the point greater than or equal to ξ is marked as a key point;

[0012] Step 1-3) Extract semantic features around key points;

[0013] Step 1-4) Determine potential matching points based on semantic features.

[0014] Preferably, the response value R in step 1-2) is:

[0015]

[0016] Among them, det(M) is the determinant of M, tr(M) represents the trace of M, and M is the point p i The covariance matrix of is:

[0017]

[0018] Among them, n j For point p i The local surface normal vector of point p i The number of points in the neighborhood with a radius of δ as the center, and the superscript T indicates transpose.

[0019] Preferably, the steps 1-3) include:

[0020] Calculate the 5 points closest to each key point and count the semantic label distribution of these 5 points

[0021] Calculate the 5 points closest to each point in point cloud map B And count the semantic label distribution of these 5 points.

[0022] Preferably, the steps 1-4) include:

[0023] For each key point in the point cloud map A Given a matrix T, the transformed coordinates if and and If the number of identical labels in is greater than 3, then it is considered and are potential matching points, where is the three-dimensional coordinate of the i-th point in the point cloud map B.

[0024] Preferably, in step 2), the conversion matrix is ​​constructed as individuals, including:

[0025] The transformation matrix is ​​divided into the rotation angle around the Z axis and the translation vector along the XY plane, and these rotation angles and translation vectors are combined into one entity.

[0026] Preferably, establishing a fitness function in step 2) includes:

[0027] For each potential matching point in the point cloud map A, calculate the W nearest points around it, and divide these W points into M groups according to their semantic labels. The semantic label set and coordinate set are represented as and in, m represents the mth group;

[0028] The fitness function F(T|A,B) is established as:

[0029]

[0030] Where T is the transformation matrix, and are potential matching points, For point and is the probability of the corresponding point, satisfying the following formula:

[0031]

[0032] in, express and The semantic feature similarity of and When they have the same semantic label, Right now:

[0033]

[0034] express and The geometric feature similarity.

[0035] Preferably, the step 3) comprises:

[0036] Step 3-1) Decompose the transformation matrix into two parts: the translation vector on the XY axis and the rotation angle around the Z axis. Encode each individual real number as the translation vector on the XY axis and the rotation angle around the Z axis. Generate several matrices uniformly within the upper and lower limits of the translation distance and the rotation angle. Select the X with the largest fitness to form the first generation population, and set the initial value of the elite individual.

[0037] Step 3-2) Obtain the individual with the highest fitness in the current population and compare it with the current elite individual. If the fitness of the current individual is greater than that of the elite individual, the current individual is the elite individual. Otherwise, the elite individual is not changed.

[0038] Step 3-3) If the fitness of the elite individual is greater than the threshold or reaches the maximum number of iterations, the population evolution is terminated, otherwise go to step 3-4);

[0039] Step 3-4) Perform roulette while retaining the best individuals in each generation;

[0040] Step 3-5) Based on the fitness of the elite individuals, the variable step length is adaptively adjusted, and the translation vector and rotation angle of each individual are crossover and mutation operations are performed respectively, and the number of iterations is increased by 1;

[0041] Step 3-6) Calculate the fitness of each individual and go to step 3-2).

[0042] Preferably, the steps 3-4) include:

[0043] Step a) setting the rotation angle step length R_init = 1, the translation vector step length T_init = 2, and the maximum fitness value Threshold = 1200;

[0044] Step b) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 3. If it is, the rotation angle is changed to step length R_step=R_init, T_step=T_init, and execute step f); otherwise, execute step c);

[0045] Step c) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 2.4. If it is, the rotation angle is changed to step length R_step=R_init / 2, T_step=T_init / 2, and execute step f); otherwise, execute step d);

[0046] Step d) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 2. If it is, the rotation angle is changed to step length R_step=R_init / 5, T_step=T_init / 5, and execute step f); otherwise, execute step e);

[0047] Step e) the rotation angle changes step length R_step=R_init / 10, T_step=T_init / 20, and execute step f);

[0048] Step f) The number of iterations gen is increased by 1.

[0049] On the other hand, the present invention proposes a semantic map matching system based on genetic algorithm, comprising:

[0050] A potential matching point extraction module is used to determine potential matching points for the received point cloud maps A and B based on geometric and semantic distribution;

[0051] A fitness calculation module based on semi-object-level features is used to construct the transformation matrix into individuals and establish a fitness function;

[0052] The population evolution module is used to evaluate the quality of individuals based on the fitness function, and use selection, crossover and mutation operations to guide population evolution and achieve map matching.

[0053] Compared with the prior art, the advantages of the present invention are:

[0054] 1. The present invention proposes a potential matching point calculation method based on geometric and semantic distribution, which can maximize the retention of feature points while removing data noise, thereby improving the calculation speed and accuracy;

[0055] 2. The present invention proposes a semi-object-level geometric feature description method, which uses the semantic distribution and geometric distribution of some points around the sampling point to describe the features of the point and realizes the fitness calculation between corresponding points;

[0056] 3. The present invention models the optimization problem of the conversion matrix as a genetic algorithm, and proposes an adaptive variable step size mutation algorithm to jump out of the local optimum and achieve the maximum global optimum. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is a flow chart of a semantic map matching method based on a genetic algorithm of the present invention;

[0058] Figure 2 is the algorithm registration result;

[0059] Figure 3 is the true value of the point cloud map;

[0060] Figure 4 It is the error distribution histogram of the method of the present invention and other algorithms. DETAILED DESCRIPTION

[0061] The purpose of the present invention is to solve the problem of large-scale semantic map registration, and a semantic map registration method based on genetic algorithm is proposed. By extracting potential matching points, solving the optimization matrix using genetic algorithm, and calculating fitness, semantic map matching under large viewing angle difference is achieved.

[0062] For local maps A and B composed of point clouds, where A = {X A ,L A},B={X B ,L B}. X A is the set of geometric coordinates of all points in the map, L A is the set of semantic labels for all points in the map, represents the three-dimensional coordinates of the kth point in map A, Represents the semantic label of the kth point in map A. The goal of map registration is to estimate the transformation matrix T between two local maps so that the error between the transformed map A and map B is minimized, that is, the error between the estimated transformation matrix and the true value matrix is ​​minimized, that is:

[0063] Error(T est ,T gt )=||log ((T gt ) -1 ·T est )|| (1)

[0064] For the convenience of description, the following definition is made: If point x in map A A and point x in map B B is the corresponding point, which needs to satisfy x A =T gt ·x B , that is, x A and x B Have the same coordinates in the global map.

[0065] The semantic map matching method based on genetic algorithm models the transformation matrix as an individual and constructs a fitness function, which guides the population evolution and completes the global optimization process. The semantic map matching method and system based on genetic algorithm include a potential matching point extraction module, a fitness calculation module based on semi-object level features and a population evolution module.

[0066] First, two points that may be corresponding points are extracted through the potential matching point extraction module; then the fitness of each individual is calculated through the fitness calculation module based on semi-object level features; finally, the direction of population evolution is controlled through the population evolution module to find the global optimal transformation matrix.

[0067] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0068] Example 1

[0069] Embodiment 1 of the present invention discloses a semantic map registration method based on a genetic algorithm.

[0070] System flow chart as follows Figure 1 shown.

[0071] 1. For the received point cloud maps A and B, determine potential matching points based on geometric and semantic distribution;

[0072] Potential matching point extraction refers to finding two points that are corresponding points in maps A and B, and calculating the fitness value of each individual based on the potential matching points. The purpose is to find and remove the wrong points that affect the matching and improve the matching speed. The specific steps are as follows:

[0073] 1.1. Remove the points with semantic labels as roads in the two local point cloud maps. This is because in this type of object, the local features of points are very similar, which makes it difficult to distinguish a point based on geometric features, resulting in incorrect matching.

[0074] 1.2. For each remaining point p in map A i , calculate the response value R of this point:

[0075]

[0076] Where det(M) is the determinant of M, and tr(M) is the trace of M. M is a point p i The covariance matrix of is:

[0077]

[0078] where n j is the local surface normal vector of the point. N is the point p i The number of points in the neighborhood with a radius of δ as the center.

[0079] Mark the point with the largest response value in the neighborhood as a potential key point, compare the response value of the potential key point with the threshold ξ, and mark the point greater than or equal to ξ as a key point. Here, ξ=0.01,δ=0.5m.

[0080] 1.3. Calculate the 5 points closest to each key point and count the semantic label distribution of these 5 points At the same time, calculate the 5 points closest to each point in the local map B And count the semantic label distribution of these 5 points.

[0081] 1.4. For each key point in A, given a matrix T, the transformed coordinates if Proceed to the next step.

[0082] 1.5. If and If the number of identical labels in is greater than 3, then it is considered and are potential matching points.

[0083] 2. Construct the transformation matrix into individuals and establish the fitness function;

[0084] In the genetic algorithm, it is necessary to calculate the fitness of each individual to evaluate the quality of the individual. In this algorithm, the transformation matrix is ​​divided into the rotation angle around the Z axis and the translation vector along the XY axis, and the rotation angle and translation vector form an individual.

[0085] Through the extraction of potential matching points, a set of potential matching points has been obtained. This set of matching points will be used to calculate the fitness of the transformation matrix T. Specifically, the probability that two points are corresponding points under the transformation matrix T is calculated. The larger the probability value, the more likely the two points are corresponding points and the closer the transformation matrix T is to the true value matrix. The geometric and semantic distribution features of the surrounding N points are used to calculate the probability that two points are the same point.

[0086] For each point, the nearest W points around it are calculated, and these W points are divided into M groups according to their semantic labels. The semantic label set and coordinate set are represented as and in,

[0087] Specifically, given the matrix T, the point and The probability of being a corresponding point is expressed as:

[0088]

[0089] express and The semantic feature similarity of and When they have the same semantic label, Right now:

[0090]

[0091] express and The geometric feature similarity measures the geometric distance between two points.

[0092]

[0093] in express and The residual of for res k,i The covariance matrix of .

[0094]

[0095] R is the rotation matrix, For point The covariance matrix in the corresponding semi-object level represents the distance from map A to The geometric coordinate covariance of the point with semantic label M among the nearest W points. Since the point with semantic label M is part of the object of category M, not the whole object, it is called the semi-object level. It reflects the point Spatial distribution of nearby objects.

[0096] Therefore, for the transformation matrix T, its fitness is expressed as:

[0097]

[0098] 3. Rely on the fitness function to evaluate the quality of individuals, use selection, crossover and mutation operations to guide population evolution and achieve map matching.

[0099] Its function is to control the direction of population evolution, make the population evolve in the direction of smaller error and avoid falling into local optimum, and maintain population diversity. It includes steps such as population initialization, fitness calculation, selection, crossover and mutation, and termination condition judgment.

[0100] 3.1: Population initialization: Decompose the transformation matrix into two parts: the translation vector on the XY axis and the rotation angle around the Z axis. Encode each individual real number as the translation vector on the XY axis and the rotation angle around the Z axis. The upper and lower limits of the translation distance are -15 meters to 15 meters, and the upper and lower limits of the rotation angle are 0 to 180 degrees.

[0101] Since the genetic algorithm is sensitive to initial values, 2000 matrices were uniformly generated within the upper and lower limits of the translation distance and rotation angle, and the 100 matrices with the largest fitness were selected as the first generation population.

[0102] 3.2: Fitness calculation: Fitness calculation is performed on all individuals.

[0103] 3.3: Selection: Use the roulette algorithm for selection while retaining the best individuals in each generation.

[0104] 3.4: Crossover and mutation: Perform crossover and mutation operations on the translation vector and rotation angle of each individual. Perform real step crossover with a specific probability, and add random step lengths for the translation vector and rotation angle with a specific probability. Design an adaptive variable step length mutation algorithm. Based on the fitness of the elite individual, the variable step length is adaptively adjusted.

[0105] Specifically, when the fitness value of the elite individual is less than a certain threshold, the step length is changed. The specific steps are as follows:

[0106] a) Initialization: initial rotation angle step length R_init = 1, initial translation vector step length T_init = 2, maximum fitness value Threshold = 1200;

[0107] b) If the fitness of the elite individual of the previous generation is less than Threshold / 3, the rotation angle is changed to step length R_step=R_init, T_step=T_init; if it is greater than or equal to Threshold / 3, execute step c);

[0108] c) If the fitness of the elite individual of the previous generation is less than Threshold / 2.4, the rotation angle is changed into step length R_step=R_init / 2, T_step=T_init / 2; if it is greater than or equal to Threshold / 2.4, execute step d);

[0109] d) If the fitness of the elite individual of the previous generation is less than Threshold / 2, the rotation angle is changed to step length R_step=R_init / 5, T_step=T_init / 5; if it is greater than or equal to Threshold / 2, execute step e);

[0110] e) The rotation angle step length is R_step = R_init / 10, T_step = T_init / 20.

[0111] 3.5: Termination condition judgment: The evolution is terminated when the maximum number of iterations is reached or the fitness value of the elite individual reaches the threshold, otherwise repeat steps 2)-4).

[0112] Example 2

[0113] Embodiment 2 of the present invention discloses a semantic map matching system based on a genetic algorithm, which is implemented by the method of embodiment 1. The system includes:

[0114] A potential matching point extraction module is used to determine potential matching points for the received point cloud maps A and B based on geometric and semantic distribution;

[0115] A fitness calculation module based on semi-object-level features is used to construct the transformation matrix into individuals and establish a fitness function;

[0116] The population evolution module is used to evaluate the quality of individuals based on the fitness function, and use selection, crossover and mutation operations to guide population evolution and achieve map matching.

[0117] Verification experiment

[0118] In order to verify the performance of the proposed algorithm, experiments were conducted on the dataset, and some algorithms were selected for comparison. Experiments were conducted on the 05 sequence of the KITTI dataset. LIO-SLAM was used to build a point cloud map, and then the point cloud map was divided into two local sub-maps with an overlap rate of 30%. One of the local sub-maps was multiplied by a rotation matrix to form map A. The other local sub-map formed map B.

[0119] Take the example of a translation of -15 meters on both the X and Y axes and a rotation of 60° around the Z axis. That is, the coordinates of the corresponding points of local maps A and B differ by 15 meters in the X and Y axis directions and are rotated 60° around the Z axis. These two maps are sent to the matching algorithm of this paper. The matching results are as follows: Figure 2 shown.

[0120] Among them, red represents local map B, and blue represents the rotated local map A. It can be seen that the two sub-maps are well registered, and the transformation matrix error is 0.00010, which is very similar to the true value of the point cloud map.

[0121] Figure 3 is the true value of the point cloud map.

[0122] Furthermore, the fusion error was tested when the rotation angle was from 0 to 180°, the step length was 3°, and the translation was from -15 meters to 15 meters, the step length was 3 meters. The lcr and VPFBR algorithms were selected as comparison algorithms, and the evaluation index was the error between the rotation matrix output by the algorithm and the true rotation matrix. The error distribution histograms of the three algorithms are shown in Figure 1. Figure 4 As shown in Figure 2, it can be seen that the algorithm errors of the algorithms in this paper are mostly around 0, and when the error is 0, the number of algorithms proposed in this paper is the largest.

[0123] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention is described in detail with reference to the embodiments, it should be understood by those skilled in the art that any modification or equivalent replacement of the technical solutions of the present invention does not depart from the spirit and scope of the technical solutions of the present invention and should be included in the scope of the claims of the present invention.

Claims

1. A semantic map matching method based on genetic algorithm, comprising: Step 1) For the received point cloud maps A and B, determine potential matching points based on geometric and semantic distribution; Step 2) construct the transformation matrix into individuals and establish a fitness function; Step 3) Rely on the fitness function to evaluate the quality of individuals, use selection, crossover and mutation operations to guide population evolution and achieve map matching.

2. The semantic map matching method based on genetic algorithm according to claim 1, characterized in that: The step 1) comprises: Step 1-1) Filter the point cloud maps A and B according to the semantic labels and remove the points whose semantic labels are roads; Step 1-2) For each point in the filtered point cloud map A, the response value is calculated respectively, and the point with the largest response value in the neighborhood is marked as a potential key point. The response value of the potential key point is compared with the threshold ξ, and the point greater than or equal to ξ is marked as a key point; Step 1-3) Extract semantic features around key points; Step 1-4) Determine potential matching points based on semantic features.

3. The semantic map matching method based on genetic algorithm according to claim 2 is characterized in that: The response value R in step 1-2) is: Among them, det(M) is the determinant of M, tr(M) represents the trace of M, and M is the point p i The covariance matrix of is: Among them, n j For point p i The local surface normal vector of point p i The number of points in the neighborhood with a radius of δ as the center, and the superscript T indicates transpose.

4. The semantic map matching method based on genetic algorithm according to claim 2, characterized in that: The steps 1-3) include: Calculate the 5 points closest to each key point and count the semantic label distribution of these 5 points Calculate the 5 points closest to each point in point cloud map B And count the semantic label distribution of these 5 points.

5. The semantic map matching method based on genetic algorithm according to claim 2, characterized in that: The steps 1-4) include: For each key point in the point cloud map A Given a matrix T, the transformed coordinates if and and If the number of identical labels in is greater than 3, then it is considered and are potential matching points, where is the three-dimensional coordinate of the i-th point in the point cloud map B.

6. The semantic map matching method based on genetic algorithm according to claim 1, characterized in that: In the step 2), the conversion matrix is ​​constructed as an individual, including: The transformation matrix is ​​divided into the rotation angle around the Z axis and the translation vector along the XY plane, and these rotation angles and translation vectors are combined into one entity.

7. The semantic map matching method based on genetic algorithm according to claim 1, characterized in that: The step 2) establishes a fitness function, including: For each potential matching point in the point cloud map A, calculate the W nearest points around it, and divide these W points into M groups according to their semantic labels. The semantic label set and coordinate set are represented as and in, m represents the mth group; The fitness function F(T|A,B) is established as: Where T is the transformation matrix, and are potential matching points, For point and is the probability of the corresponding point, satisfying the following formula: in, express and The semantic feature similarity of and When they have the same semantic label, Right now: express and The geometric feature similarity.

8. The semantic map matching method based on genetic algorithm according to claim 1, characterized in that: The step 3) comprises: Step 3-1) Decompose the transformation matrix into two parts: the translation vector on the XY axis and the rotation angle around the Z axis. Encode each individual real number as the translation vector on the XY axis and the rotation angle around the Z axis. Generate several matrices uniformly within the upper and lower limits of the translation distance and the rotation angle. Select the X with the largest fitness to form the first generation population, and set the initial value of the elite individual. Step 3-2) Obtain the individual with the highest fitness in the current population and compare it with the current elite individual. If the fitness of the current individual is greater than that of the elite individual, the current individual is the elite individual. Otherwise, the elite individual is not changed. Step 3-3) If the fitness of the elite individual is greater than the threshold or reaches the maximum number of iterations, the population evolution is terminated, otherwise go to step 3-4); Step 3-4) Perform roulette while retaining the best individuals in each generation; Step 3-5) Based on the fitness of the elite individuals, the variable step length is adaptively adjusted, and the translation vector and rotation angle of each individual are crossover and mutation operations are performed respectively, and the number of iterations is increased by 1; Step 3-6) Calculate the fitness of each individual and go to step 3-2).

9. The semantic map matching method based on genetic algorithm according to claim 8, characterized in that: The steps 3-4) include: Step a) setting the rotation angle step length R_init = 1, the translation vector step length T_init = 2, and the maximum fitness value Threshold = 1200; Step b) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 3. If it is, the rotation angle is changed to step length R_step=R_init, T_step=T_init, and execute step f); otherwise, execute step c); Step c) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 2.

4. If it is, the rotation angle is changed to step length R_step=R_init / 2, T_step=T_init / 2, and execute step f); otherwise, execute step d); Step d) Determine whether the fitness of the elite individual of the previous generation is less than Threshold / 2. If it is, the rotation angle is changed to step length R_step=R_init / 5, T_step=T_init / 5, and execute step f); otherwise, execute step e); Step e) the rotation angle changes step length R_step=R_init / 10, T_step=T_init / 20, and execute step f); Step f) The number of iterations gen is increased by 1.

10. A semantic map matching system based on genetic algorithm, characterized in that: include: A potential matching point extraction module is used to determine potential matching points for the received point cloud maps A and B based on geometric and semantic distribution; A fitness calculation module based on semi-object-level features is used to construct the transformation matrix into individuals and establish a fitness function; and The population evolution module is used to evaluate the quality of individuals based on the fitness function, and use selection, crossover and mutation operations to guide population evolution and achieve map matching.

Images