A method for 2D laser mapping process loop identification

Abstract

The application provides a 2D laser mapping process loop recognition method, comprising the following steps: obtaining a mileage data sequence, generating a mileage loop feature sequence through a loop search method; performing bias processing on a subgraph corresponding to the mileage sequence i; obtaining an upper limit value of a theoretical registration point number by using a registration algorithm; registering the remaining subgraphs with the subgraph corresponding to the mileage sequence i by using the registration algorithm, and obtaining an optimal registration result as a loop pair. The application can greatly improve the success rate of loop search, the success rate of position during loop search, and the operability; in the mapping process, the operator performs specific mapping actions, so that the robot can perform loop judgment according to the state and time of the robot when the loop action occurs; thereby reducing the time for the robot to search for a loop and improving the efficiency of searching for a loop.

Description

Technical Field

[0001] This invention relates to the field of robot mapping technology, and in particular to a method for loop closure recognition in a 2D laser mapping process. Background Technology

[0002] When a robot is building a map using SLAM, it needs to identify which areas of the scene have already been scanned. These repeatedly scanned areas are called loop closures. Loop closures can correct the accumulated errors in the mapping process and play a crucial role in improving the quality of the map.

[0003] Existing technologies primarily identify loop closures by matching registration between sub-maps and by assessing the displacement deviation of odometers between sub-maps. However, this method heavily relies on environmental features, and when environmental features are similar and odometer displacements are small, it is prone to identifying incorrect loop closure pairs.

[0004] Secondly, existing technologies lack an interface for operator intervention in loop closure identification, meaning that operators cannot effectively improve the accuracy of loop closure identification during the mapping process. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a method for loop closure identification in 2D laser mapping. This invention improves the robustness of loop closure identification by applying manual intervention during the mapping process.

[0006] The technical solution of this invention is: a method for loop closure identification in a 2D laser mapping process, comprising the following steps:

[0007] S1) During the robot's propulsion process, the odometry data sequence odom_list(x) is recorded using an odometry system. N ,y N ,w N ); based on the robot's odometry data sequence odom_list(x N ,y N ,w N The subgraph information corresponding to the odometer data sequence is set, and the current odometer loopback search sequence id is set to i = 0; the loopback result result_loop is empty;

[0008] S2) During the mapping process, the robot's action of rotating more than 360 degrees in place is called a loop closure auxiliary feature action, and the odometry loop closure feature sequence array_loop is generated by loop closure search.

[0009] S3) Determine the length of the odometer loop search sequence id and the length of the odometer loop feature sequence array_loop. If the length of the odometer loop feature sequence array_loop is greater than that of the odometer sequence i, then proceed to step S4); if the length of the odometer loop feature sequence array_loop is less than that of the odometer sequence i, then output the loop result result_loop.

[0010] S4) The subgraph corresponding to the odometer sequence i is biased; and the registration algorithm is used to perform registration tests on the subgraphs corresponding to the odometer sequence i before and after the bias, to obtain the theoretical upper limit of the number of registration points true_s;

[0011] S5) Using the registration algorithm, register the subgraph j(h) corresponding to the remaining odometer loopback IDs with the subgraph corresponding to the odometer sequence i, and obtain the subgraph corresponding to the sequence number h with the highest number of registration points; and determine whether the number of registration points of the subgraph corresponding to the sequence number h with the highest number of registration points is greater than 0.8*true_s; if so, proceed to step S6).

[0012] S6) Extract a point cloud data with an area of ​​L*L from the sub-graph corresponding to the odometer sequence i, centered on the loop position, and perform offset processing. Then, use the registration algorithm to perform registration tests on the extracted parts of the sub-graph corresponding to the odometer sequence i before and after offset, and obtain the theoretical upper limit of the number of registration points true_s2.

[0013] S7) Extract a point cloud data p_test with an area of ​​L*L from the subgraph corresponding to the sequence number h with the highest number of registration points, centered at the loop closure position; and determine whether the number of registration points of p_test is greater than 0.9*true_s2; if so, record that h and i are a loop closure pair and add them to the loop closure result sequence result_loop.add{h,i}.

[0014] S8) Let the odometer sequence i = i + 1, and execute step S3).

[0015] Preferably, step S2) specifically includes the following steps:

[0016] S21), for the odometer data sequence odom_list(x) N ,y N ,w N Initialize the odometer sequence i = 0, and set the odometer loop sequence list array_loop to empty;

[0017] S22) Determine if the odometer sequence i is less than N; if the odometer sequence i is less than N, proceed to step S23); if the odometer sequence i ≥ N, output the odometer loop feature sequence array_loop;

[0018] S23), Initial odometer value: base_x = x i base_y = y i base_w = w i And the half-circle marking angles half_w1 = base_w + 3.14; half_w2 = base_w - 3.14; and determine whether there is an odometer angle value in the odometer sequence from i to i+200 points that is within a neighborhood of half_w1 or half_w2; if so, execute step S24);

[0019] S24) Record the sequence ID k of the odometer angle value in a neighborhood of half_w1 and half_w2 at this time; and determine whether there is a neighborhood of the odometer angle value base_w in the odometer sequence k to k+200 points; if so, execute step S25);

[0020] S25) Record the sequence ID of the current odometer angle value within a neighborhood of base_w as m; and determine whether the x and y coordinates of all odometer sequences from i to m are within a neighborhood of the point (base_x, base_y); if so, store the current odometer sequence i into the odometer loop sequence table array_loop.

[0021] (S26) Then, the odometer sequence i = i + m is executed, and step S22 is performed.

[0022] Preferably, in steps S23), S24), and S25), if the determination result is negative, then the odometer sequence i = i + 50 is executed, and step S22 is executed.

[0023] Preferably, in steps S4) and S6), the offset refers to randomly shifting the sub-graph of the odometer loopback search ID by 0.5-1.5m and rotating it by 30-70°.

[0024] Preferably, in steps S5) and S7), if the determination result is negative, then step S8 is executed.

[0025] Preferably, in steps S4-S6), the registration algorithm specifically includes the following steps:

[0026] S41) Obtain two sub-images A and B that need to be registered, and obtain any point a(x) in sub-image A. a y a ), and any point b(x) in subgraph B. b y b );

[0027] S42), via map index map[i b ,j b The value of ] is used to search for each point in subgraph B, and the map index map[i] in subgraph B is obtained. b ,j b Collect all points whose value is 1 and store them in the set C(x). i y i In ), where map[i b ,j b Points with a value of 1 are used for mapping, map[i b ,j b Points with a value of 0 are blank points; b For row index, j b This is a column index; the index side length of the map is n.

[0028] S43) According to the search range {x,y,w}: x=[-2,2]; y=[-2,2]; w=[-3.14,3.14]; obtain the current search increments dx, dy, and dw according to the search step sizes of 0.1, 0.1, and 0.2 respectively;

[0029] According to the increments dx, dy, and dw, the set C(x) in subgraph B is... i y i Traverse all points in C(x); traverse all points C(x) i y i Map the data to subgraph A.

[0030] S44), the point (x) in subgraph B i y i The mapping relationship between the indices projected onto subgraph A is as follows:

[0031] i_new=n-[cos(dw)*(x i -x b )-sin(dw)*(y i -y b )+x a ] / l;

[0032] j_new=n-[sin(dw)*(x i -x b )-cos(dw)*(y i -y b )+y a ] / l;

[0033] Where, x i y idw represents the coordinates of the currently traversed point in subgraph B, l represents the rotation angle increment, and l represents the actual length of each grid cell.

[0034] Determine whether the value of map index map[i_new,j_new] in subgraph A is 1, and store all points with the value of map index map[i_new,i_new] as 1 into s_total;

[0035] S45) Determine whether the search range {x,y,w} has been completed. If so, output s_total, which has the most registration points, and its corresponding best position adjustment amount (best_x=dx, best_y=dy, best_w=dw).

[0036] If not, proceed to step S43).

[0037] Preferably, in steps S6 and S7), L is 1.5-2.5m.

[0038] The beneficial effects of this invention are as follows:

[0039] 1. This invention can significantly improve the success rate of loop closure search, as well as the success rate of position search during loop closure search and provide more user-friendly operation;

[0040] 2. In the mapping process, the present invention enables the robot to make loop judgments by having the operator perform specific mapping actions.

[0041] 3. This invention indirectly leaves specific action markers on the time sequence of map building by having the robot perform specific actions during the mapping process. The action markers indicate that the location may be a loop. Then, the robot is registered with the locations of looping actions to identify which loop locations belong to the same place, thereby reducing the time for the robot to search for loops and improving the efficiency of loop search. Attached Figure Description

[0042] Figure 1 This is a flowchart of the method of the present invention;

[0043] Figure 2 This is a flowchart of the loop closure search of the present invention;

[0044] Figure 3 This is a flowchart of the registration algorithm of the present invention; Detailed Implementation

[0045] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings:

[0046] like Figure 1As shown, this embodiment provides a method for loop closure identification in a 2D laser mapping process, including the following steps:

[0047] S1) During the robot's propulsion process, the odometry data sequence odom_list(x) is recorded using an odometry system. N ,y N ,w N ); where N is the number of sequences, (x i ,y i ,w i ) represents the i-th odometer sequence;

[0048] Based on the robot's odometry data sequence odom_list(x N ,y N ,w N The subgraph information corresponding to the odometer data sequence is set, and the current odometer loopback search sequence id is set to i = 0; the loopback result result_loop is empty;

[0049] S2) During the mapping process, the robot's action of rotating more than 360 degrees in place is called a loop closure auxiliary feature action, and the odometry loop closure feature sequence array_loop is generated by loop closure search.

[0050] S3) Determine the length of the odometer loop search sequence id and the length of the odometer loop feature sequence array_loop. If the length of the odometer loop feature sequence array_loop is greater than that of the odometer sequence i, then proceed to step S4); if the length of the odometer loop feature sequence array_loop is less than that of the odometer sequence i, then output the loop result result_loop.

[0051] S4) Offset the sub-graph corresponding to odometer sequence i by 1m and rotate it by 50°; and use the registration algorithm to perform registration tests on the sub-graphs corresponding to odometer sequence i before and after the offset, and obtain the theoretical upper limit of the number of registration points true_s;

[0052] S5) Using the registration algorithm, register the subgraph j(h) corresponding to the remaining odometer loopback IDs with the subgraph corresponding to the odometer sequence i, and obtain the subgraph corresponding to the sequence number h with the highest number of registration points; and determine whether the number of registration points of the subgraph corresponding to the sequence number h with the highest number of registration points is greater than 0.8*true_s; if so, proceed to step S6).

[0053] S6) Extract a 2m*2m area of ​​point cloud data from the sub-image corresponding to odometer sequence i with the loop closure position as the center, and perform offset processing. Then, use the registration algorithm to perform registration tests on the extracted parts of the sub-image corresponding to odometer sequence i before and after offset, and obtain the theoretical upper limit of the number of registration points true_s2.

[0054] S7) Extract a 2m*2m area of ​​point cloud data p_test from the subgraph corresponding to the sequence number k with the highest number of registration points, centered at the loop closure position; and determine whether the number of registration points of p_test is greater than 0.9*true_s2; if so, record that h and i are a loop closure pair and add them to the loop closure result sequence result_loop.add{h,i}.

[0055] S8) Let the odometer sequence i = i + 1, and execute step S3).

[0056] As a preferred embodiment, such as Figure 2 As shown, in step S2), the loop closure search specifically includes the following steps:

[0057] S21), for the odometer data sequence odom_list(x) N ,y N ,w N Initialize the odometer sequence i = 0, and set the odometer loop sequence list array_loop to empty;

[0058] S22) Determine if the odometer sequence i is less than N; if the odometer sequence i is less than N, proceed to step S23); if the odometer sequence i ≥ N, output the odometer loop feature sequence array_loop;

[0059] S23), Initial odometer value: base_x = x i base_y = y i base_w = w i And the half-circle marking angles half_w1 = base_w + 3.14; half_w2 = base_w - 3.14; and determine whether there is an odometer angle value in the odometer sequence from i to i+200 points that is within a neighborhood of half_w1 or half_w2; if so, execute step S24);

[0060] S24) Record the sequence ID k of the odometer angle value in a neighborhood of half_w1 and half_w2 at this time; and determine whether there is a neighborhood of the odometer angle value base_w in the odometer sequence k to k+200 points; if so, execute step S25);

[0061] S25) Record the sequence ID of the current odometer angle value within a neighborhood of base_w as m; and determine whether the x and y coordinates of all odometer sequences from i to m are within a neighborhood of the point (base_x, base_y); if so, store the current odometer sequence i into the odometer loop sequence table array_loop.

[0062] (S26) Then, the odometer sequence i = i + m is executed, and step S22 is performed.

[0063] In a preferred embodiment, in steps S23), S24), and S25), if the determination result is negative, then the odometer sequence i = i + 50 is executed, and step S22 is executed.

[0064] In a preferred embodiment, if the determination result is negative in steps S5) and S7), then step S8 is executed.

[0065] As a preferred embodiment, such as Figure 3 As shown, in steps S4-S6), the registration algorithm specifically includes the following steps:

[0066] S41) Obtain two sub-images A and B that need to be registered, and obtain any point a(x) in sub-image A. a y a ), and any point b(x) in subgraph B. b y b );

[0067] S42), via map index map[i b ,j b The value of ] is used to search for each point in subgraph B, and the map index map[i] in subgraph B is obtained. b ,j b Collect all points whose value is 1 and store them in the set C(x). i y i In ), where map[i b ,j b Points with a value of 1 are used for mapping, map[i b ,j b Points with a value of 0 are blank points; b For row index, j b This is a column index; the index side length of the map is n.

[0068] S43) According to the search range {x,y,w}: x=[-2,2]; y=[-2,2]; w=[-3.14,3.14]; obtain the current search increments dx, dy, and dw according to the search step sizes of 0.1, 0.1, and 0.2 respectively;

[0069] According to the increments dx, dy, and dw, the set C(x) in subgraph B is... i y i Traverse all points in C(x); traverse all points C(x) i y i Map the data to subgraph A.

[0070] S44), the point (x) in subgraph B i y i The mapping relationship between the indices projected onto subgraph A is as follows:

[0071] i_new=n-[cos(dw)*(x i -x b )-sin(dw)*(y i -y b )+x a ] / l;

[0072] j_new=n-[sin(dw)*(x i -x b )-cos(dw)*(y i -y b )+y a ] / l;

[0073] Where, x i y i dw represents the coordinates of the currently traversed point in subgraph B, l represents the rotation angle increment, and l represents the actual length of each grid cell.

[0074] Determine whether the value of map index map[i_new,j_new] in subgraph A is 1, and store all points with the value of map index map[i_new,i_new] as 1 into s_total;

[0075] S45) Determine whether the search range {x,y,w} has been completed. If so, output s_total, which has the most registration points, and its corresponding best position adjustment amount (best_x=dx, best_y=dy, best_w=dw).

[0076] If not, proceed to step S43).

[0077] The embodiments and descriptions above are merely illustrative of the principles and preferred embodiments of the present invention. Various changes and modifications may be made to the present invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed.

Claims

1. A method for loop closure identification in a 2D laser mapping process, characterized in that, Includes the following steps: S1) During the robot's propulsion process, the odometry data sequence odom_list(x) is recorded using an odometry system. N ,y N ,w N ); based on the robot's odometry data sequence odom_list(x N ,y N ,w N The subgraph information corresponding to the odometer data sequence is set, and the current odometer loopback search sequence id is set to i = 0; the loopback result result_loop is empty; S2) During the mapping process, the robot's action of rotating more than 360 degrees in place is called a loop closure auxiliary feature action, and the odometry loop closure feature sequence array_loop is generated by loop closure search. S3) Determine the length of the odometer loop search sequence id and the length of the odometer loop feature sequence array_loop. If the length of the odometer loop feature sequence array_loop is greater than that of the odometer sequence i, then proceed to step S4); if the length of the odometer loop feature sequence array_loop is less than that of the odometer sequence i, then output the loop result result_loop. S4) Perform bias processing on the subgraph corresponding to the odometer sequence i; The registration algorithm is used to perform registration tests on the subgraphs corresponding to the odometry sequence i before and after the bias, and the theoretical upper limit of the number of registration points, true_s, is obtained. S5) Using the registration algorithm, register the subgraph j(h) corresponding to the remaining odometer loopback IDs with the subgraph corresponding to the odometer sequence i, and obtain the subgraph corresponding to the sequence number h with the highest number of registration points; and determine whether the number of registration points of the subgraph corresponding to the sequence number h with the highest number of registration points is greater than 0.8*true_s; if so, proceed to step S6). S6) Extract a point cloud data with an area of ​​L*L from the sub-graph corresponding to the odometer sequence i, centered on the loop position, and perform offset processing. Then, use the registration algorithm to perform registration tests on the extracted parts of the sub-graph corresponding to the odometer sequence i before and after offset, and obtain the theoretical upper limit of the number of registration points true_s2. S7) Extract a point cloud data p_test with an area of ​​L*L from the subgraph corresponding to the sequence number k with the highest number of registration points, centered at the loop closure position; and determine whether the number of registration points of p_test is greater than 0.9*true_s2; if so, record that h and i are a loop closure pair and add them to the loop closure result sequence result_loop.add{h,i}. S8) Let the odometer sequence i = i + 1, and execute step S3).

2. The method for loop closure identification in a 2D laser mapping process according to claim 1, characterized in that: In step S2), the loop closure search specifically includes the following steps: S21), for the odometer data sequence odom_list(x) N ,y N ,w N Initialize the odometer sequence i = 0, and set the odometer loop sequence list array_loop to empty; S22) Determine if the odometer sequence i is less than N; if the odometer sequence i is less than N, proceed to step S23); if the odometer sequence i ≥ N, output the odometer loop feature sequence array_loop; S23), Initial odometer value: base_x = x i base_y = y i base_w = w i And the half-circle marking angles half_w1 = base_w + 3.14; half_w2 = base_w - 3.14; and determine whether there is an odometer angle value in the odometer sequence from i to i+200 points that is within a neighborhood of half_w1 or half_w2; if so, execute step S24); S24) Record the sequence ID k of the odometer angle value in a neighborhood of half_w1 and half_w2 at this time; and determine whether there is a neighborhood of the odometer angle value base_w in the odometer sequence k to k+200 points; if so, execute step S25); S25) Record the sequence ID of the current odometer angle value within a neighborhood of base_w as m; and determine whether the x and y coordinates of all odometer sequences from i to m are within a neighborhood of the point (base_x, base_y); if so, store the current odometer sequence i into the odometer loop sequence table array_loop. (S26) Then, the odometer sequence i = i + m is executed, and step S22 is performed.

3. The method for loop closure identification in a 2D laser mapping process according to claim 2, characterized in that: In steps S23), S24), and S25), if the determination result is negative, then the odometer sequence i = i + 50 is executed, and step S22 is executed.

4. The method for loop closure identification in a 2D laser mapping process according to claim 1, characterized in that: In steps S4) and S6), the offset refers to randomly shifting the subgraph of the odometer loopback search ID by 0.5-1.5m and rotating it by 30-70°.

5. The method for loop closure identification in a 2D laser mapping process according to claim 1, characterized in that: If the determination result is negative in steps S5) and S7), then step S8 is executed.

6. The method for loop closure identification in a 2D laser mapping process according to claim 1, characterized in that: In steps S4-S6), the registration algorithm specifically includes the following steps: S41) Obtain two sub-images A and B that need to be registered, and obtain any point a(x) in sub-image A. a y a ), and any point b(x) in subgraph B. b y b ); S42), via map index map[i b ,j b The value of ] is used to search for each point in subgraph B, and the map index map[i] in subgraph B is obtained. b ,j b Collect all points whose value is 1 and store them in the set C(x). i y i In ), where map[i b ,j b Points with a value of 1 are used for mapping, map[i b ,j b Points with a value of 0 are blank points; b For row index, j b This is a column index; the index side length of the map is n. S43) According to the search range {x,y,w}: x=[-2,2]; y=[-2,2]; w=[-3.14,3.14]; obtain the current search increments dx, dy, and dw according to the search step sizes of 0.1, 0.1, and 0.2 respectively; According to the increments dx, dy, and dw, the set C(x) in subgraph B is... i y i Traverse all points in C(x); traverse all points C(x) i y i Map the data to subgraph A. S44), the point (x) in subgraph B i y i The mapping relationship between the indices projected onto subgraph A is as follows: i_new=n-[cos(dw)*(x i -x b )-sin(dw)*(y i -and b )+x a ] / l; j_new=n-[sin(dw)*(x i -x b )-cos(dw)*(y i -and b )+and a ] / l; Where, x i y i dw represents the coordinates of the currently traversed point in subgraph B, l represents the rotation angle increment, and l represents the actual length of each grid cell. Determine whether the value of map index map[i_new,j_new] in subgraph A is 1, and store all points with the value of map index map[i_new,i_new] as 1 into s_total; S45) Determine whether the search range {x,y,w} has been completed. If so, output s_total, which has the most registration points, and its corresponding best position adjustment amount (best_x=dx, best_y=dy, best_w=dw). If not, proceed to step S43).

7. The method for loop closure identification in a 2D laser mapping process according to claim 1, characterized in that: In steps S6 and S7), L is 1.5-2.5m.

Images