Robust outdoor loop closure detection method and system for robots

Abstract

This invention discloses a robust outdoor loop closure detection method and system for robots, relating to the field of intelligent robot technology, which can achieve robust loop closure detection for robots in outdoor road environments. The technical solution of this invention includes the following: First, a semantic node-edge graph is generated from a semantic point cloud, and nodes in two semantic graphs are matched; then, two pairs of nodes are selected using geometric consistency evaluation, and an aligned coordinate system is established by combining ground normal vectors, and the absolute spatial distribution similarity of the nodes in the semantic graph is evaluated to determine whether a loop closure frame is formed; finally, based on the constructed aligned coordinate system, fine matching of the point clouds is further performed to obtain the pose of the two registered point clouds. This invention can achieve robust loop closure detection for robots in outdoor road environments.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, specifically to a robust outdoor loop closure detection method and system for robots. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is one of the core technologies in the field of mobile robotics, providing robots with the ability to autonomously navigate, locate, and map in unknown environments. SLAM is not only the foundation of intelligent robot navigation but also plays a crucial role in tasks such as scene reconstruction and embodied interaction. When a robot explores autonomously, it needs to continuously perform self-localization and environmental mapping to perform tasks in complex and dynamic environments. However, due to various factors (such as sensor noise, environmental changes, and motion errors), odometry pose in SLAM often accumulates errors, leading to a decline in long-term navigation and localization accuracy. To overcome this problem, SLAM systems introduce loop closure detection technology. By identifying previously visited locations, it determines whether a loop has occurred, effectively correcting errors. The key to loop closure detection is enabling the robot to recognize locations it has previously visited, even if there are significant distances between different parts of the map. This allows the system to self-correct localization errors without relying on external positioning equipment.

[0003] Recently, several semantic graph matching-based methods have been proposed for outdoor loop closure detection. These graph-based methods are lightweight and can fully utilize high-level information in point clouds to better represent the topological relationships between instances. Specifically, a single-frame point cloud is transformed into a semantic node-edge graph, where nodes represent instances in the point cloud, and edges represent the relationships between these instances. Therefore, loop closure detection can be achieved by evaluating the similarity of the semantic graphs, typically by comparing node descriptors or global graph descriptors.

[0004] Generally, loop closure detection is performed first, followed by pose estimation to determine the correct loop closure. However, node descriptors or global graph descriptors cannot fully and accurately represent semantic graphs, thus limiting their robustness in semantic graph matching. Furthermore, performing graph matching before loop closure detection often overlooks the crucial role of already registered poses in finding the correct loop closure. Once the two semantic graphs are registered, the correct node correspondences can be easily obtained, providing valuable information for more effective loop closure detection, such as the spatial distribution of nodes.

[0005] Therefore, how to perform robust loop closure detection for robots in outdoor road environments is an urgent problem to be solved. Summary of the Invention

[0006] In view of this, the present invention provides a robust outdoor loop closure detection method and system for robots, which can realize robust loop closure detection of robots in outdoor road environments.

[0007] To achieve the above objectives, the technical solution of the present invention includes the following:

[0008] A robust outdoor loop closure detection method for robots includes the following steps:

[0009] First, a semantic node-edge graph is generated from the semantic point cloud, and nodes in the two semantic graphs are matched.

[0010] Then, two pairs of nodes are selected using geometric consistency evaluation, and an aligned coordinate system is established by combining the ground normal vector. The absolute spatial distribution similarity of nodes in the semantic graph is evaluated to determine whether a loop frame is formed.

[0011] Finally, based on the constructed aligned coordinate system, fine matching of the point clouds is further carried out to obtain the pose of the two point clouds after registration.

[0012] Furthermore, a semantic node-edge graph is generated from the semantic point cloud. Specifically, for the semantic point cloud P, point cloud information of specific semantic categories is first selected; then, a clustering method is used to cluster points of different semantic categories to obtain a point cloud containing multiple instances. Each instance is abstracted into an independent instance node, and is represented by its center position and semantic label. From the complete semantic point cloud P, a graph consisting of a series of semantic nodes {n1, n2, ..., n...} is obtained. i A node graph G composed of , ..., is used to calculate the upward normal vector N based on the ground points.

[0013] Furthermore, the nodes in the two semantic graphs are matched. Specifically, for the input point cloud pair P1 and P2, the two resulting node graphs are: and For each node in G1 and G2, a neighborhood semantic feature vector is generated as an additional identifier beyond the node's semantics; each node in G1 and G2 obtains a series of neighborhood semantic feature vectors. and

[0014] For any two nodes in G1 and G2 that have the same semantic label and Their similarity is if Exceeding the predefined threshold t nf ,but and These are considered matching node pairs; thus, a series of matching node pairs have been obtained, denoted as...

[0015] Furthermore, two pairs of nodes are selected using geometric consistency evaluation, and an aligned coordinate system is established by combining the ground normal vectors. The absolute spatial distribution similarity of the nodes in the semantic graph is then evaluated to determine whether a loopback frame is formed. Specifically:

[0016] For {pair1,pair2,…,pair b Any two matching pairs in} s and pair t Its first-stage consistency metric is d. st

[0017]

[0018] Set a threshold d1 and construct matrix F:

[0019] F[s][t]=F[t][s]=1,|d st | <d1

[0020] F[s][t]=F[t][s]=0,|d st |>d1

[0021] Where F[s][t] represents the value in the s-th row and t-th column of the matrix, and F[t][s] represents the value in the t-th row and s-th column of the matrix;

[0022] Based on F, evaluate the second-order consistency measure; for two first-order consistent pairs... s and pai t Its second-order consistency metric is defined as the consistency between pairs. s and pair t The number of pairs that are identical; construct a second-order matrix S to record all second-order consistency values, where the dimensions of both matrices F and S correspond to b, i.e., the number of matching pairs:

[0023] S[s][t]=0, F[s][t]=0

[0024]

[0025] S[s][t] represents the value in the s-th row and t-th column of matrix S;

[0026] For any matching node pair s and pair t ,when or When this happens, the value at the corresponding position in S will be set to 0, where d max It is a hyperparameter;

[0027]

[0028] Next, select the maximum value max(S) from S and extract the corresponding matching pair; where the selected matching pair is called pairs. max ={(pair) s1 pair t1 ),(pair s2 pair t2 ),…};In order to get from pairs max Select two suitable matching pairs and further examine the status of each matching pair in the first-stage consistency matrix. Consistency is measured by summing the elements of the corresponding rows in the first-stage matrix F. For (pairs)... s1 pair t1 The sum of its rows is defined as R_sum(s1,t1), as shown below:

[0029]

[0030] Similarly, pairs max Each element in the table is associated with a calculated row sum. By selecting the largest row sum, two matching node pairs are ultimately chosen.

[0031] The selected node matching pair is pair s1 and pair t1 ;

[0032] Let the ground normal vectors of G1 and G2 be N1 and N2, respectively, and let them be the z-axis Z1 and Z2. Select matching nodes. and O1 and O2 are respectively the origins; in addition, pair t1 Nodes in and Used to construct the x-axis;

[0033] Finally, the y-axis can be obtained by the cross product of the z-axis and x-axis. Complete spherical coordinate systems are then constructed in G1 and G2 respectively, for {pair1, pair2, ..., pair}. b For each pair of matching nodes in}, their spherical coordinates in their respective coordinate systems are calculated. If the difference between corresponding values ​​in the two coordinates is less than a certain threshold, the spherical coordinates are considered similar.

[0034] Furthermore, statistics are presented in {pair1,pair2,…,pair} b The number of matching pairs with similar spherical coordinates in G1 and G2 is defined as S_N. The similarity scores for G1 and G2 are:

[0035] similarity score=w3×max(S)+w4×S_N.

[0036] Where w3 is the weight of max(S) and w4 is the weight of S_N;

[0037] If the similarity score exceeds the set threshold, it is determined to be a loopback frame.

[0038] Furthermore, based on the constructed aligned coordinate system, fine matching of the point clouds is carried out to obtain the poses of the two point cloud frames after registration, specifically:

[0039] Establish and align spherical coordinate systems for G1 and G2. The relative pose transformation between G1 and G2 is as follows:

[0040]

[0041] in and Let G1 and G2 represent the spherical coordinate systems, respectively.

[0042] Based on the initial pose estimation The accuracy of relative pose estimation is further optimized by using the iterative nearest point (ICP) algorithm; finally, the pose after registration of two frame point clouds is obtained.

[0043] Another embodiment of the present invention provides a robust outdoor loop closure detection system for robots, comprising the following modules:

[0044] The graph generation module generates a semantic node-edge graph from the semantic point cloud and matches nodes in the two semantic graphs.

[0045] The similarity assessment module selects two pairs of nodes using geometric consistency assessment, establishes an aligned coordinate system by combining ground normal vectors, and assesses the absolute spatial distribution similarity of nodes in the semantic graph to determine whether a loop frame is formed.

[0046] The pose registration module further performs fine matching of point clouds based on the constructed alignment coordinate system to obtain the pose of the two point clouds after registration.

[0047] Furthermore, the graph generation module generates a semantic node-edge graph from the semantic point cloud in the following manner:

[0048] For a semantic point cloud P, point cloud information of a specific semantic category is first filtered out;

[0049] Then, clustering methods are used to cluster points of different semantic categories, resulting in a point cloud containing multiple instances. Each instance is abstracted into an independent instance node, and represented by its center position and semantic label. From the complete semantic point cloud P, a point cloud consisting of a series of semantic nodes {n1, n2, ..., n...} is obtained. i A node graph G composed of , ..., is used to calculate the upward normal vector N based on the ground points.

[0050] Furthermore, the graph generation module matches nodes in the two semantic graphs in the following way:

[0051] Given the input point cloud pair P1 and P2, the two resulting node graphs are as follows: and

[0052] For each node in G1 and G2, a neighborhood semantic feature vector is generated as an additional identifier beyond the node's semantics; each node in G1 and G2 obtains a series of neighborhood semantic feature vectors. and

[0053] For any two nodes in G1 and G2 that have the same semantic label and Their similarity is if Exceeding the predefined threshold t nf ,but and The node pairs are considered to be matched;

[0054] At this point, a series of matching node pairs have been obtained, denoted as...

[0055] Furthermore, the similarity evaluation module performs the following steps:

[0056] For {pair1,pair2,…,pair b Any two matching pairs in} s and pair t Its first-stage consistency metric is d. st :

[0057]

[0058] Set a threshold d1 and construct matrix F:

[0059] F[s][t]=F[t][s]=1,|d st | <d1

[0060] F[s][t]=F[t][s]=0,|d st |>d1

[0061] Where F[s][t] represents the value in the s-th row and t-th column of matrix F, and F[t][s] represents the value in the t-th row and s-th column of matrix F;

[0062] Based on matrix F, evaluate the second-order consistency measure; for two first-order consistent matching pairs... s and pair t Its second-order consistency metric is defined as the consistency between pairs. s and pair t The number of pairs that are identical; construct a second-order matrix S to record all second-order consistency values, where the dimensions of both matrices F and S correspond to b, i.e., the number of matching pairs:

[0063] S[s][t]=0, F[s][t]=0

[0064]

[0065] S[s][t] represents the value in the s-th row and t-th column of matrix S;

[0066] For any matching node pair s and pair t ,when or When this happens, the value at the corresponding position in S will be set to 0, where d max It is a hyperparameter;

[0067]

[0068] Next, select the maximum value max(S) from S and extract the corresponding matching pair; where the selected matching pair is called pairs. max ={(pair) s1 pair t1 ),(pair s2 pair t2 ),…};In order to get from pairs max Select two suitable matching pairs and further examine the status of each matching pair in the first-stage consistency matrix. Consistency is measured by summing the elements of the corresponding rows in the first-stage matrix F. For (pairs)... s1 pair t1 The sum of its rows is defined as R_sum(s1,t1), as shown below:

[0069]

[0070] Therefore, pairs max Each element in the array is associated with a calculated row sum; by selecting the largest row sum, two matching node pairs are ultimately chosen.

[0071] The selected node matching pair is pair s1 and pair t1 ;

[0072] The ground normal vectors of G1 and G2 are N1 and N2, respectively, and N1 and N2 are used as z-axis Z1 and Z2, respectively, to select matching nodes. and O1 and O2 are respectively the origins; in addition, pair t1 Nodes in and Used to construct the x-axis;

[0073] Finally, the y-axis is obtained by the cross product of the z-axis and x-axis. Complete spherical coordinate systems are then constructed in G1 and G2 respectively, for {pair1, pair2, ..., pair}. b For each pair of matching nodes in}, their spherical coordinates in their respective coordinate systems are calculated. If the difference between corresponding values ​​in the two coordinates is less than a certain threshold, the spherical coordinates are considered similar.

[0074] Furthermore, statistics are presented in {pair1,pair2,…,pair} b The number of matching pairs with similar spherical coordinates in G1 and G2 is defined as S_N. The similarity scores for G1 and G2 are:

[0075] similarity score=w3×max(S)+w4×S_N

[0076] Where w3 is the weight of max(S) and w4 is the weight of S_N;

[0077] If the similarity score exceeds the set threshold, it is determined to be a loopback frame.

[0078] Furthermore, based on the constructed aligned coordinate system, fine matching of the point clouds is carried out to obtain the poses of the two point cloud frames after registration, specifically:

[0079] Establish and align spherical coordinate systems for G1 and G2. The relative pose transformation between G1 and G2 is obtained using the following formula: and Let G1 and G2 represent the spherical coordinate systems respectively:

[0080]

[0081] Based on the initial pose estimation The accuracy of relative pose estimation is further optimized by using the iterative nearest point (ICP) algorithm; finally, the pose after registration of two frame point clouds is obtained.

[0082] Beneficial effects:

[0083] 1. This invention provides a robust outdoor loop closure detection method and system for robots. It selects node pairs based on geometric consistency and constructs an aligned spherical coordinate system between two point cloud frames using ground normal vectors. Based on the constructed spherical coordinate system, the absolute spatial distribution of nodes is evaluated to determine whether the two point cloud frames constitute a loop closure frame. By constructing an aligned coordinate system using ground normal vectors and comparing the absolute spatial distribution of nodes in the two semantic maps, robust loop closure detection in outdoor environments is achieved. Attached Figure Description

[0084] Figure 1 This invention provides a framework diagram of an outdoor robot loop closure detection method.

[0085] Figure 2 This is a schematic diagram of the neighborhood descriptor for a semantic node. Detailed Implementation

[0086] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0087] Example 1: As Figure 1 The diagram shown is a framework diagram of a robust outdoor loop closure detection method for robots provided by an embodiment of the present invention. The method includes the following steps:

[0088] Step 1: Generate a semantic node-edge graph from the semantic point cloud and match nodes in the two semantic graphs. In this embodiment of the invention, considering the semantic LiDAR point cloud data P, the embodiment first filters out point cloud information of specific categories, such as ground and moving objects. These points typically contain less relevant information and may have different center locations. Next, the embodiment of the invention uses a clustering method to cluster points of different semantic categories, obtaining a point cloud containing multiple instances. Each instance is abstracted into an independent instance node and characterized by its center location and semantic label. Therefore, from the complete semantic point cloud P, this embodiment of the invention obtains a point cloud consisting of a series of semantic nodes {n1,n2,…,n...}. i The node graph G is composed of , ...}. Furthermore, considering that outdoor LiDAR point clouds typically contain stable ground information, this embodiment of the invention calculates an upward normal vector N based on the ground points, which will be used in subsequent loop closure detection.

[0089] Given a pair of input point clouds P1 and P2, the above process yields two node graphs, represented as follows: and

[0090] To construct node pairs, SLOOP incorporates checks on the neighborhood semantic features of nodes with similar semantic meanings, thereby obtaining higher-quality matching pairs. Specifically, in this embodiment of the invention, a neighborhood semantic feature vector is first generated for each node in G1 and G2, serving as an additional identifier beyond the node's semantics. For example, for nodes in G1... The embodiments of the present invention are based on Centered on a radius of d nf Search for other nodes within a spherical region, denoted as n1, n2, ..., n m Subsequently, embodiments of the present invention will assign semantic tags to these nodes and their corresponding... Spatial distance is organized into two vectors: semantic vector. and distance vector Before proceeding to the next step, this embodiment of the invention introduces a semantic library vector S = {s1, s2, ..., s...} l This vector contains all possible semantic labels in the semantic point cloud. Therefore, embodiments of the present invention are based on... The number of occurrences of each semantic tag in S and The corresponding average distance is used to generate a counting vector and an average distance vector, which are represented as follows: and Then, the embodiments of the present invention... and Perform normalization processing (still represented as) and ), and generate the fusion vector f i 1 The formula for generating the fusion vector is as follows. A schematic diagram of the neighborhood descriptor of a semantic node is shown below. Figure 2 As shown.

[0091]

[0092] f i 1 Describes the nodes The semantic features within the neighborhood are combined with information such as the semantic category, number, and distance of surrounding nodes. In this way, this embodiment of the invention obtains a series of neighborhood semantic feature vectors for each node in G1 and G2. and Subsequently, for any two nodes in G1 and G2 that have the same semantic label and Its similarity is defined as And it is calculated using the cosine similarity metric, as shown below. If f_s(f i 1 ,f j2 ) exceeding a predefined threshold t nf , then and are considered a matching pair of nodes.

[0093]

[0094] Therefore, a series of matching pairs of nodes are obtained in the embodiments of the present invention, denoted as

[0095] Step 2: Use geometric consistency evaluation to select two pairs of nodes, establish an aligned coordinate system in combination with the ground normal vector, and evaluate the similarity of the absolute spatial distribution of the nodes in the semantic map to determine whether a loop frame is formed.

[0096] In the embodiments of the present invention, when evaluating the similarity between G1 and G2, the embodiments of the present invention aim to find the aligned coordinate spaces of the two semantic maps to evaluate the spatial distribution of the nodes and estimate the pose transformation relationship of the point cloud. In an outdoor environment, the ground normal vector is usually stable and accurate, which helps the embodiments of the present invention determine the z-axis of the spherical coordinate system. The next task is to find the two most likely correct pairs of nodes, which will help the embodiments of the present invention construct the origin and x-axis of the coordinate system. By carefully examining the consistency metrics between the matching pairs, it helps to select the two pairs of nodes that are most likely to be correctly matched. For any two matching pairs pair b} in {pair1, pair2,..., pair s and pair t , their first-stage consistency metric is as follows. <00,00421>

[0098] In formula (5-3), the smaller |d st | indicates better first-order consistency between pair s and pair t . The embodiments of the present invention set a threshold d1. When |d st | < d1, pair s and pair t are considered to have first-order consistency; otherwise, they are considered to have inconsistent consistency. All first-order consistency cases will be recorded using the first-order matrix as follows. <00004,26>F[s][t] = F[t][s] = 1, |d st | < d1<u

[0100] F[s][t] = F[t][s] = 0, |d st | > d1<0u

[0101] Furthermore, based on F, a second-order consistency metric is evaluated. For two first-order consistent matching pairs... s and pair t The second-order consistency measure between them is defined as the number of pairs that are consistent with both of them. A second-order matrix S is used to record all second-order consistency values, as defined in equation (5-5). The dimensions of matrices F and S both correspond to b, i.e., the number of matching pairs.

[0102] S[s][t]=0, F[s][t]=0

[0103]

[0104] In S, larger values ​​indicate higher second-stage consistency between the corresponding two node pairs. Therefore, embodiments of the invention select matching pairs associated with the maximum value in S. However, directly searching for these values ​​is not advisable. When the corresponding two node pairs are too close, the calculated x-axis may produce significant errors due to unavoidable positional errors in node clustering.

[0105] Therefore, based on formula (5-5), the second-stage matrix S will be further processed as follows. For any matched node pair... s and pair t ,when or When this happens, the value at the corresponding position in S will be set to 0, where d max It is a hyperparameter.

[0106]

[0107] Next, select the maximum value max(S) from S (there may be multiple maximum values), and extract the corresponding matching pairs. The selected matching pairs are called pairs. max ={(pair) s1 pair t1 ),(pair s2 pair t2 ),...}. In order to get from Pairs max Two suitable matching pairs are selected, and each matching pair is further examined in the first-stage consistency matrix. Generally, for a pair of matching nodes, the matching node pair with a higher number of first-stage consistent matching pairs has higher correctness. Therefore, this embodiment of the invention measures consistency by summing the corresponding row elements in the first-stage matrix F (called the row summation value). For (pair) s1 pair t1 The sum of its rows is defined as R_sum(s1,t1), as shown below.

[0108]

[0109] Similarly, pairs max Each element in the table is associated with a calculated row sum. By selecting the largest row sum, two node matching pairs are ultimately chosen. For simplicity, let's assume the selected node matching pairs are 'pair'. s1 and pair t1 .

[0110] Let the ground normal vectors of G1 and G2 be N1 and N2, respectively, and let them be the z-axis Z1 and Z2. Select matching nodes. and These are designated as origins O1 and O2, respectively. Furthermore, pair... t1 Nodes in and It was used to construct the x-axis.

[0111] Finally, the y-axis can be obtained by the cross product of the z-axis and x-axis. Therefore, complete spherical coordinate systems are constructed in G1 and G2 respectively. For {pair1,pair2,…,pair…} b For each pair of matched nodes in}, their spherical coordinates in their respective coordinate systems can be calculated. Nodes in pair1 and The spherical coordinates are defined as and The same applies to other nodes. If the difference between corresponding values ​​in two coordinates is less than a certain threshold, they are considered similar.

[0112] Furthermore, statistics are presented in {pair1,pair2,…,pair} b The number of matching pairs with similar spherical coordinates in G1 and G2 is defined as S_N. Finally, S_N and max(S) are used to generate the similarity scores for G1 and G2, as shown in the following formula.

[0113] similarity score=w3×max(S)+w4×S_N

[0114] Step 3: Based on the constructed aligned coordinate system, further refine the point cloud matching to obtain the pose of the two point clouds after registration.

[0115] Given that spherical coordinate systems have been established for G1 and G2 in this embodiment of the invention, and these two coordinate systems are aligned, the relative pose transformation between G1 and G2 can be obtained by the following formula, wherein... and Let G1 and G2 represent the spherical coordinate systems, respectively.

[0116]

[0117] Furthermore, based on the initial pose estimation Similar methods, such as the Iterative Closest Point (ICP) algorithm, can be used to further optimize the accuracy of relative pose estimation.

[0118] The above completes the task of robust loop closure detection for robots in outdoor environments.

[0119] Example 2:

[0120] The robust outdoor loop closure detection system for robots provided in this embodiment of the invention consists of three modules, namely:

[0121] The graph generation module generates a semantic node-edge graph from the semantic point cloud and matches nodes in the two semantic graphs.

[0122] The similarity assessment module selects two pairs of nodes using geometric consistency assessment, establishes an aligned coordinate system by combining ground normal vectors, and assesses the absolute spatial distribution similarity of nodes in the semantic graph to determine whether a loop frame is formed.

[0123] The pose registration module further performs fine matching of point clouds based on the constructed alignment coordinate system to obtain the pose of the two point clouds after registration.

[0124] First, a semantic node-edge graph is generated from the semantic point cloud, and nodes in the two semantic graphs are matched. Second, two pairs of nodes are selected using geometric consistency evaluation, and an aligned coordinate system is established using ground normal vectors. The absolute spatial distribution similarity of nodes in the semantic graph is evaluated to determine whether a loopback frame is formed (if the similarity exceeds a threshold, it is determined to be a loopback frame). Finally, based on the constructed aligned coordinate system, fine-grained matching of the point clouds is further performed to obtain the poses of the two registered point cloud frames. If a loopback frame is determined, it is used for subsequent construction of the global point cloud map.

[0125] In this embodiment of the invention, the graph generation module generates a semantic node-edge graph from the semantic point cloud in the following manner:

[0126] For a semantic point cloud P, point cloud information of a specific semantic category is first filtered out;

[0127] Then, clustering methods are used to cluster points of different semantic categories, resulting in a point cloud containing multiple instances. Each instance is abstracted into an independent instance node, and represented by its center position and semantic label. From the complete semantic point cloud P, a point cloud consisting of a series of semantic nodes {n1, n2, ..., n...} is obtained. i A node graph G composed of , ..., is used to calculate the upward normal vector N based on the ground points.

[0128] In this embodiment of the invention, the graph generation module matches nodes in two semantic graphs in the following manner:

[0129] Given the input point cloud pair P1 and P2, the two resulting node graphs are as follows: and

[0130] For each node in G1 and G2, a neighborhood semantic feature vector is generated as an additional identifier beyond the node's semantics; each node in G1 and G2 obtains a series of neighborhood semantic feature vectors. and

[0131] For any two nodes in G1 and G2 that have the same semantic label and Its similarity is f_s(f i 1 ,f j 2 If f_s(f i 1 ,f j 2 Exceeding the predefined threshold t nf ,but and The node pairs are considered to be matched;

[0132] At this point, a series of matching node pairs have been obtained, denoted as...

[0133] In this embodiment of the invention, the similarity evaluation module specifically performs the following steps:

[0134] For {pair1,pair2,…,pair b Any two matching pairs in} s and pair t Its first-stage consistency metric is d. st :

[0135]

[0136] Set a threshold d1 and construct matrix F:

[0137] F[s][t]=F[t][s]=1,|d st | <d1

[0138] F[s][t]=F[t][s]=0,|d st |>d1

[0139] Where F[s][t] represents the value in the s-th row and t-th column of matrix F, and F[t][s] represents the value in the t-th row and s-th column of matrix F;

[0140] Based on matrix F, evaluate the second-order consistency measure; for two first-order consistent matching pairs... s and pair t Its second-order consistency metric is defined as the consistency between pairs. s and pair t The number of pairs that are identical; construct a second-order matrix S to record all second-order consistency values, where the dimensions of both matrices F and S correspond to b, i.e., the number of matching pairs:

[0141] S[s][t]=0, F[s][t]=0

[0142]

[0143] S[s][t] represents the value in the s-th row and t-th column of matrix S;

[0144] For any matching node pair s and pair t ,when or When this happens, the value at the corresponding position in S will be set to 0, where d max It is a hyperparameter;

[0145]

[0146] Next, select the maximum value max(S) from S and extract the corresponding matching pair; where the selected matching pair is called pairs. max ={(pair) s1 pair t1 ),(pair s2 pair t2 ),…};In order to get from pairs max Select two suitable matching pairs and further examine the status of each matching pair in the first-stage consistency matrix. Consistency is measured by summing the elements of the corresponding rows in the first-stage matrix F. For (pairs)... s1 pair t1 The sum of its rows is defined as R_sum(s1,t1), as shown below:

[0147]

[0148] Therefore, pairs max Each element in the array is associated with a calculated row sum; by selecting the largest row sum, two matching node pairs are ultimately chosen.

[0149] The selected node matching pair is pair s1 and pairt1 ;

[0150] The ground normal vectors of G1 and G2 are N1 and N2, respectively, and N1 and N2 are used as z-axis Z1 and Z2, respectively, to select matching nodes. and O1 and O2 are respectively the origins; in addition, pair t1 Nodes in and Used to construct the x-axis;

[0151] Finally, the y-axis is obtained by the cross product of the z-axis and x-axis. Complete spherical coordinate systems are then constructed in G1 and G2 respectively, for {pair1, pair2, ..., pair}. b For each pair of matching nodes in}, their spherical coordinates in their respective coordinate systems are calculated. If the difference between corresponding values ​​in the two coordinates is less than a certain threshold, the spherical coordinates are considered similar.

[0152] Furthermore, statistics are presented in {pair1,pair2,…,pair} b The number of matching pairs with similar spherical coordinates in} is defined as S_B, and the similarity scores for G1 and G2 are:

[0153] similarityscore=w3×max(S)+w4×S_N

[0154] Where w3 is the weight of max(S) and w4 is the weight of S_N;

[0155] If the similarity score exceeds the set threshold, it is determined to be a loopback frame.

[0156] In this embodiment of the invention, fine matching of point clouds is further performed based on the constructed aligned coordinate system to obtain the pose of the two point clouds after registration, specifically as follows:

[0157] Establish and align spherical coordinate systems for G1 and G2. The relative pose transformation between G1 and G2 is obtained using the following formula: and Let G1 and G2 represent the spherical coordinate systems respectively:

[0158]

[0159] Based on the initial pose estimation The accuracy of relative pose estimation is further optimized by using the iterative nearest point (ICP) algorithm; finally, the pose after registration of two frame point clouds is obtained.

[0160] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A robust outdoor loop closure detection method for robots, characterized in that, Includes the following steps: First, a semantic node-edge graph is generated from the semantic point cloud, and nodes in the two semantic graphs are matched. Then, two pairs of nodes are selected using geometric consistency evaluation, and an aligned coordinate system is established by combining the ground normal vectors. The absolute spatial distribution similarity of the nodes in the semantic graph is evaluated to determine whether a loopback frame is formed. Specifically: for Any two matching pairs in and Its first-phase consistency metric is Set a threshold Construct matrix F: in This represents the value in the s-th row and t-th column of the matrix. This represents the value in the t-th row and s-th column of the matrix; based on Evaluate the second-order consistency measure; for two first-order consistent matching pairs and Its second-order consistency metric is defined as... and The number of identical pairs; construct a second-order matrix. Used to record all second-order consistent values, matrix and The dimensions all correspond to That is, the number of matching pairs: This represents the value in the s-th row and t-th column of matrix S; For any matching node pair and ,when or hour, The value at the corresponding position will be set to ,in It is a hyperparameter; Next, from Select the maximum value And extract the corresponding matching pairs; where the selected matching pairs are In order to... Select two suitable matching pairs and further examine the status of each matching pair in the first-stage consistency matrix. This is done by analyzing the first-stage matrix. Consistency is measured by summing the elements of the corresponding rows. Its row summation value is defined as As shown below: Similarly, Each element in the array is associated with a calculated row sum; by selecting the largest row sum, two matching node pairs are ultimately chosen. The selected node matching pair is and ; set up and The ground normal vectors are respectively and They are respectively used as the z-axis and Select matching node and Each as the origin and ;also, Nodes in and Used to construct the x-axis; Finally, the y-axis can be obtained from the cross product of the z-axis and the x-axis, respectively. and Constructing a complete spherical coordinate system is crucial for... For each pair of matching nodes, their spherical coordinates in their respective coordinate systems are calculated. If the difference between corresponding values ​​in the two coordinate systems is less than a certain threshold, the spherical coordinates are considered similar. In addition, statistics in The number of matching pairs with similar spherical coordinates in the array, defined as... , and Similarity score for: in, for The weight, for The weights; If similarity score If the frame exceeds the set threshold, it is identified as a loopback frame. Finally, based on the constructed aligned coordinate system, fine matching of the point clouds is further carried out to obtain the pose of the two point clouds after registration.

2. The robust outdoor loop closure detection method for robots as described in claim 1, characterized in that, The process of generating a semantic node-edge graph from a semantic point cloud specifically involves: For semantic point clouds First, point cloud information with specific semantic categories is selected; Then, clustering methods are used to cluster points of different semantic categories, resulting in a point cloud containing multiple instances. Each instance is abstracted into an independent instance node, and represented by its center position and semantic label, thus forming a complete semantic point cloud. In the process, a sequence of semantic nodes was obtained. The node graph is composed of Calculate the upward normal vector based on the ground point. .

3. The robust outdoor loop closure detection method for robots as described in claim 1, characterized in that, The specific process for matching nodes in the two semantic graphs is as follows: For the input point cloud pair and The two node graphs obtained are respectively and ; for and Each node generates a neighborhood semantic feature vector, which serves as an additional identifier beyond the node's semantics. and Each node in the dataset obtained a series of neighborhood semantic feature vectors. and ; for and Any two nodes with the same semantic label and Their similarity is ,if Exceeding a predefined threshold ,but and The node pairs are considered to be matched; At this point, a series of matching node pairs have been obtained, denoted as... .

4. The robust outdoor loop closure detection method for robots as described in claim 1, characterized in that, The point cloud is further finely matched based on the constructed alignment coordinate system to obtain the pose of the two point cloud frames after registration, specifically: for and Establish and align a spherical coordinate system. and The relative pose transformation between them is : in and They represent and The spherical coordinate system in the middle; Based on the initial pose estimation The accuracy of relative pose estimation is further optimized by using the iterative nearest point (ICP) algorithm; finally, the pose after registration of two frame point clouds is obtained.

5. A robust outdoor loop closure detection system for robots, characterized in that, Includes the following modules: The graph generation module generates a semantic node-edge graph from the semantic point cloud and matches nodes in the two semantic graphs. The similarity evaluation module selects two pairs of nodes using geometric consistency evaluation, establishes an aligned coordinate system by combining ground normal vectors, and evaluates the absolute spatial distribution similarity of nodes in the semantic graph to determine whether a loopback frame is formed. The similarity evaluation module specifically performs the following steps: for Any two matching pairs in and Its first-phase consistency metric is : Set a threshold Construct matrix F: in This represents the value in the s-th row and t-th column of matrix F. This represents the value in the t-th row and s-th column of matrix F; Based on matrix Evaluate the second-order consistency measure; for two first-order consistent matching pairs and Its second-order consistency metric is defined as... and The number of identical pairs; construct a second-order matrix. Used to record all second-order consistent values, matrix and The dimensions all correspond to That is, the number of matching pairs: This represents the value in the s-th row and t-th column of matrix S; For any matching node pair and ,when or hour, The value at the corresponding position will be set to ,in It is a hyperparameter; Next, from Select the maximum value And extract the corresponding matching pairs; where the selected matching pairs are In order to... Select two suitable matching pairs and further examine the status of each matching pair in the first-stage consistency matrix. This is done by analyzing the first-stage matrix. Consistency is measured by summing the elements of the corresponding rows. Its row summation value is defined as As shown below: Therefore, Each element in the array is associated with a calculated row sum; by selecting the largest row sum, two matching node pairs are ultimately chosen. The selected node matching pair is and ; in and The ground normal vectors are respectively and , and respectively as z-axis and Select matching node and Each as the origin and ; also, Nodes in and Used to construct the x-axis; Finally, the y-axis is obtained by the cross product of the z-axis and the x-axis, respectively. and Constructing a complete spherical coordinate system is crucial for... For each pair of matching nodes, their spherical coordinates in their respective coordinate systems are calculated. If the difference between corresponding values ​​in the two coordinate systems is less than a certain threshold, the spherical coordinates are considered similar. In addition, statistics in The number of matching pairs with similar spherical coordinates in the array, defined as... , and Similarity score for: in, for The weight, for The weights; If similarity score If the frame exceeds the set threshold, it is identified as a loopback frame. The pose registration module further performs fine matching of point clouds based on the constructed alignment coordinate system to obtain the pose of the two point clouds after registration.

6. The robust outdoor loop closure detection system for robots as described in claim 5, characterized in that, The graph generation module generates a semantic node-edge graph from the semantic point cloud in the following manner: For semantic point clouds First, point cloud information with specific semantic categories is selected; Then, clustering methods are used to cluster points of different semantic categories, resulting in a point cloud containing multiple instances. Each instance is abstracted into an independent instance node, and represented by its center position and semantic label, thus forming a complete semantic point cloud. In the process, a sequence of semantic nodes was obtained. The node graph is composed of Calculate the upward normal vector based on the ground point. .

7. The robust outdoor loop closure detection system for robots as described in claim 6, characterized in that, The graph generation module matches nodes in two semantic graphs in the following manner: For the input point cloud pair and The two node graphs obtained are respectively and ; for and Each node generates a neighborhood semantic feature vector, which serves as an additional identifier beyond the node's semantics. and Each node in the dataset obtained a series of neighborhood semantic feature vectors. and ; for and Any two nodes with the same semantic label and Their similarity is ,if Exceeding a predefined threshold ,but and The node pairs are considered to be matched; At this point, a series of matching node pairs have been obtained, denoted as... .

8. The robust outdoor loop closure detection system for robots as described in claim 7, characterized in that, The point cloud is further finely matched based on the constructed alignment coordinate system to obtain the pose of the two point cloud frames after registration, specifically: for and Establish and align a spherical coordinate system, obtained using the following formula. and The relative pose transformation between them, where and They represent and spherical coordinate system in: Based on the initial pose estimation The accuracy of relative pose estimation is further optimized by using the iterative nearest point (ICP) algorithm; finally, the pose after registration of two frame point clouds is obtained.

Images