Laser radar mapping method and system based on loopback detection and odometer

Abstract

The invention discloses a laser radar mapping method and system based on loopback detection and a speedometer. The method comprises the following steps: acquiring laser radar point cloud and speedometer information; extracting long sides of the laser radar point cloud information, judging whether the long sides are continuous long sides with the curvature greater than a threshold value, and selecting the continuous long sides with the curvature greater than the threshold value to establish a subgraph; loopback detection is carried out on the established sub-graph, loopback is optimized, and the pose of the robot is adjusted, so that a complete map is established. According to the method, the starting point and the ending point of loopback are selected to be performed on the long edge with the large curvature, the starting long edge with the large curvature and the ending long edge with the large curvature are closed, and therefore loopback of the whole map is achieved, and ghosting of the finally drawn global map is avoided.

Description

Technical Field

[0001] The present invention belongs to the technical field of map construction, and relates to a lidar mapping method and system based on loop detection and odometer. Background Art

[0002] There are a large number of large-sized irregular containers in the chemical, military, and civilian fields. In actual scenarios, tracked robots are usually used to clean the containers. The cleaning of these containers includes two cleaning requirements: cleaning the bottom of the container and cleaning the vertical surface of the container. Modeling of the container is required for cleaning the bottom of the container. Since the light conditions inside the container are uncontrollable, it may be completely dark or partially turbid, and the measurement range of vision-based solutions is limited. While lidar provides high-precision distance measurement, it lacks sufficient information for estimating the device's pose. To overcome these limitations, some systems have begun to combine odometry with lidar data to obtain more accurate and robust navigation results. The loop detection method improves the accuracy of long-term navigation by effectively correcting the accumulated positioning errors. However, correctly determining the starting point of the loop is crucial for the overall loop. The wrong selection of the starting point of the loop will cause ghosting in the finally drawn global map. Summary of the Invention

[0003] The purpose of the present invention is to provide a lidar mapping method and system based on loop detection and odometer. The starting point and ending point of the loop are selected on the long side with a larger curvature, and the starting long side with a larger curvature is closed with the ending long side with a larger curvature, so as to realize the loop of the whole map and avoid ghosting in the finally drawn global map.

[0004] The technical solution for realizing the purpose of the present invention is as follows:

[0005] A lidar mapping method based on loop detection and odometer includes the following steps:

[0006] S01: Obtain lidar point cloud and odometer information;

[0007] S02: Extract the long sides of the lidar point cloud information, determine whether they are continuous long sides with a curvature greater than a threshold, and select the continuous long sides with a curvature greater than the threshold to establish a sub-map;

[0008] S03: Perform loop detection on the established sub-map, optimize the loop, and adjust the robot pose to establish a complete map.

[0009] In the preferred technical solution, the method for determining whether the continuous long sides with a curvature greater than the threshold in step S02 includes:

[0010] Obtain the main direction and centroid of the lidar point cloud through principal component analysis;

[0011] Calculate the distance between the centroid of the lidar point cloud and the nearest point of the lidar point cloud, and compare it with a threshold. If it is less than the threshold, proceed to the next judgment;

[0012] Generate two parallel lines along the positive and negative directions of the main direction of the lidar point cloud respectively, and determine whether the lidar point cloud is between the two parallel lines. If it is between the parallel lines, it means the curvature is greater than the threshold; otherwise, the curvature is less than the threshold.

[0013] In the preferred technical solution, the method for generating two parallel lines includes:

[0014] Obtain the unit vector of the main direction of the lidar point cloud x main , y main

[0015] respectively represent the components of the unit vector of the main direction on the x and y axes. The second direction is perpendicular to the main direction, and the unit vector of the second direction is

[0016] The straight line l + generated along the positive direction of the main direction has a point P + =(x + , y + ) T is:

[0017]

[0018] where P g is the centroid of the lidar point cloud, and T offset is the translation amount of the centroid of the lidar point cloud along the main direction;

[0019] Combined with the unit vector of the second direction and P + to generate the straight line equation l + of the straight line l + (x, y) is:

[0020] l + (x, y): y main ×y - y main ×y + +x main ×(x - x + ) = 0

[0021] Similarly, the straight line l - generated along the negative direction of the main direction has a point P - =(x - , y - ) T is:

[0022]

[0023] Combine the unit vector in the second direction and P - Generate the straight line l - The straight line equation of (x, y) is:

[0024] l - (x, y): y main ×y - y main ×y - +x main ×(x - x - ) = 0.

[0025] In the preferred technical solution, the method for determining whether the lidar point cloud is between two parallel lines includes:

[0026] For the radar point cloud P at the current moment l(t) =(x l(t) , y l(t) ) T ∈S l(t) , if the following formula holds:

[0027] l + (x l(t) , y l(t) )×l - (x l(t) , y l(t) ) < 0

[0028] S l(t) is the set of the radar point cloud at the current moment;

[0029] Then the lidar point cloud is between the parallel lines, otherwise the lidar point cloud is outside the two parallel lines.

[0030] In the preferred technical solution, the 0th sub - map is established at the beginning in step S02. The origin of the 0th sub - map is P0=(x0, y0) T , the sub - map resolution is Res, and the two - dimensional array index value corresponding to each point of the lidar point cloud is calculated; during the map - building process, the odometer information is recorded. If the current odometer position exceeds the translation threshold compared with the previous nearest odometer position, the information of the odometer and the lidar is recorded. After the number of records accumulates to the threshold, the next sub - map is started to be established. The origin of the nth sub - map is P n =(x n , y n ) T , and the sub - map resolution is Res.

[0031] In the preferred technical solution, the loop detection in step S03 includes loop search, loop matching, and loop optimization. After the robot builds at least one sub-map, the loop search starts. The loop search distributes a series of loop checkpoints around the robot position t r =(x r , y r ). T The set S loop of loop checkpoints is as follows:

[0032] S loop ={p l =(x l , y l ) T |x l =x r +t offset , y l =y r +t offset},

[0033] where p l is the loop checkpoint, t offset represents the translation amount, t offset ∈[t min , t max , t min represents the minimum translation amount, and t max represents the maximum translation amount;

[0034] And it is judged whether these loop checkpoints are within the 0th sub-map and whether the two-dimensional array index value reaches the set value. If the number of loop checkpoints whose two-dimensional array index value reaches the set value exceeds the threshold, the loop search is considered successful;

[0035] After the loop search is successful, loop matching is performed. After the loop matching is completed, loop optimization is performed.

[0036] In the preferred technical solution, for loop matching, the point cloud of the lidar corresponding to the current odometry pose T r (t) and the set of points whose two-dimensional array index value reaches the set value detected by the loop checkpoints are used for ICP point cloud matching to obtain the optimized pose T r '(t). After the loop matching is completed, loop optimization is performed. The loop optimization adopts the sparse pose optimization method, and the pose with the stamped prior is adjusted to the pose with the stamped posterior in a non-linear optimization manner, so that the lidar point cloud realizes the establishment of the global map through the homogeneous transformation of the pose with the stamped posterior.

[0037] In the preferred technical solution, the method for establishing the global map includes:

[0038] By using the timestamp stamp lAlign with the timestamps of a series of post-stamped poses to obtain the stamp l The robot pose corresponding to the lidar data at a certain moment R r Represents the attitude, t r Represents the robot position;

[0039] Find all lidar data S l The set S of corresponding robot poses r , calculate the point cloud in the global coordinate system. The set S of point clouds p Is:

[0040] S p ={P p =(x p , y p ) T |P p =T r ×T l ×P l , T r ∈S r , P l ∈S l}} (2)

[0041] P l Represents an element in the lidar data set, T l Represents the pose of the lidar in the robot coordinate system, T r Represents the pose of the robot in the global coordinate system;

[0042] Calculate the element index values of the corresponding two-dimensional array for the elements in S p in sequence, and set them to the set value to realize the update of the global map.

[0043] The present invention also discloses a lidar mapping system based on loop detection and odometry, including a processor, and the processor is built-in with the above-mentioned lidar mapping method based on loop detection and odometry.

[0044] The present invention also discloses a computer storage medium, on which a computer program is stored, and when the computer program is executed, it realizes the above-mentioned lidar mapping method based on loop detection and odometry.

[0045] Compared with the prior art, the present invention has the following remarkable advantages:

[0046] In order to avoid ghosting in the finally drawn global map, the starting and ending points of the loop need to be selected on the long side with a larger curvature, and the starting long side with a larger curvature is closed with the ending long side, so as to realize the loop of the whole map. This enables the lidar to model the shape of the container under the condition of limited reflection intensity. Description of the Drawings

[0047] Figure 1 It is a flowchart of a lidar mapping method based on loop detection and odometer for a preferred embodiment;

[0048] Figure 2 It is a schematic diagram of the principal component analysis of lidar point cloud for a preferred embodiment;

[0049] Figure 3 It is a flowchart of the mapping algorithm for a preferred embodiment. Detailed Implementation Manner

[0050] The principle of the present invention is as follows: The starting and ending points of the loop of the present invention are selected on the long side with a larger curvature, and the starting long side with a larger curvature is closed with the ending long side, so as to realize the loop of the whole map and avoid ghosting in the finally drawn global map.

[0051] Embodiment 1:

[0052] As Figure 1 shown, a lidar mapping method based on loop detection and odometer includes the following steps:

[0053] S01: Obtain lidar point cloud and odometer information;

[0054] S02: Extract the long sides of the lidar point cloud information, determine whether they are continuous long sides with a curvature greater than the threshold, and select the continuous long sides with a curvature greater than the threshold to establish a sub-map;

[0055] S03: Perform loop detection on the established sub-map, optimize the loop, and adjust the pose of the robot to establish a complete map.

[0056] In one embodiment, the method for determining whether the continuous long sides have a curvature greater than the threshold in step S02 includes:

[0057] Obtain the principal direction and centroid of the lidar point cloud through principal component analysis;

[0058] Calculate the distance between the centroid of the lidar point cloud and the nearest point of the lidar point cloud, and compare it with the threshold. If it is less than the threshold, proceed to the next judgment;

[0059] Generate two parallel lines in the positive and negative directions along the main direction of the lidar point cloud, and determine whether the lidar point cloud is between the two parallel lines. If it is between the parallel lines, it means the curvature is greater than the threshold; otherwise, the curvature is less than the threshold.

[0060] In one embodiment, the method for generating two parallel lines includes:

[0061] Obtain the unit vector of the main direction of the lidar point cloud x main and y main respectively represent the components of the unit vector of the main direction on the x and y axes. The second direction is perpendicular to the main direction, and the unit vector of the second direction is

[0062] The straight line l + generated along the positive direction of the main direction has a point P + =(x + , y + ) T which is:

[0063]

[0064] where P g is the centroid of the lidar point cloud, and T offset is the translation amount of the centroid of the lidar point cloud along the main direction;

[0065] Combined with the unit vector of the second direction and P + to generate the straight line equation of the straight line l + (x, y) which is: + (x, y): y

[0066] l + (x, y): y main ×y - y main ×y + +x main ×(x - x + ) = 0

[0067] Similarly, the straight line l - generated along the negative direction of the main direction has a point P - =(x - , y - ) T which is:

[0068]

[0069] Combined with the unit vector of the second direction and P - to generate the straight line equation of the straight line l - (x, y) which is:

[0070] l - (x, y): y main ×y - y main ×y - +x main ×(x - x - ) = 0。

[0071] In one embodiment, the method for determining whether the lidar point cloud is between two parallel lines includes:

[0072] For the lidar point cloud P at the current moment l(t) =(x l(t) , y l(t) ) T ∈S l(t) , if the following formula holds:

[0073] l + (x l(t) , y l(t) )×l - (x l(t) , y l(t) ) < 0

[0074] S l(t) is the set of lidar point clouds at the current moment;

[0075] Then the lidar point cloud is between the parallel lines, otherwise the lidar point cloud is outside the two parallel lines.

[0076] In one embodiment, the 0th sub - map is established at the beginning in step S02. The origin of the 0th sub - map P0=(x0, y0) T , the sub - map resolution is Res, and the two - dimensional array index value corresponding to each point of the lidar point cloud in the sub - map is calculated; during the mapping process, the odometer information is recorded. If the current odometer position exceeds the translation threshold compared with the previous nearest odometer position, the information of the odometer and the lidar is recorded. After the recording times accumulate to the threshold, the next sub - map is started to be established. The origin of the nth sub - map P n =(x n , y n ) T , and the sub - map resolution is Res.

[0077] In one embodiment, the loop detection in step S03 includes loop search, loop matching, and loop optimization. After the robot has built at least one sub - map, the loop search starts. The loop search is carried out by distributing a series of loop check points around the robot position t r =(x r , y r ) T , and the loop check point set S loop is:

[0078] S loop ={p l =(x l , y l ) T |x l =x r +t offset , y l =y r +t offset} where p l is a loop closure checkpoint, t offset represents the translation amount, t offset ∈[t min , t max , t min represents the minimum translation amount, t max represents the maximum translation amount;

[0079] And determine whether these loop closure checkpoints are within the 0th sub - figure, and whether the two - dimensional array index value reaches the set value. If the number of loop closure checkpoints whose two - dimensional array index value reaches the set value exceeds the threshold, it is considered that the loop search is successful;

[0080] After the loop search is successful, loop matching is performed. After loop matching is completed, loop optimization is performed.

[0081] In one embodiment, for loop matching, directly use the point cloud corresponding to the current odometry pose T r (t) and the set of points whose two - dimensional array index value reaches the set value detected by the loop closure checkpoint for ICP point cloud matching to obtain the optimized pose T r ′(t). After loop matching is completed, loop optimization is performed. Loop optimization adopts a sparse pose optimization method, and uses non - linear optimization to adjust the pose with timestamp prior to the pose with timestamp posterior, so that the lidar point cloud realizes the establishment of the global map through the homogeneous transformation of the pose with timestamp posterior.

[0082] In one embodiment, the method for establishing the global map includes:

[0083] By aligning the timestamp stamp l of the lidar data with the timestamps of a series of poses with timestamp posterior, obtain the robot pose corresponding to the lidar data at the time of stamp l where R r represents the attitude, t r represents the robot position;

[0084] Find the set of robot poses S l corresponding to all lidar data S r , and calculate the point cloud in the global coordinate system. The set of point clouds Sp is:

[0085] S p ={P p =(x p , y p ) T |P p =T r ×T l ×P l , T r ∈S r , P l ∈S l}} (2)

[0086] P l represents an element in the lidar data set, and T l represents the pose of the lidar in the robot coordinate system, and T r represents the pose of the robot in the global coordinate system;

[0087] By calculating the elements in S p in sequence to obtain the element index values of the corresponding two-dimensional array and setting them to the set values, the global map is updated.

[0088] In another embodiment, a computer storage medium stores a computer program thereon, and when the computer program is executed, the above-mentioned lidar mapping method based on loop detection and odometer is implemented.

[0089] The lidar mapping method based on loop detection and odometer can adopt any of the above-mentioned lidar mapping methods based on loop detection and odometer, and the specific implementation will not be elaborated here.

[0090] In another embodiment, a lidar mapping system based on loop detection and odometer includes a processor, and the processor incorporates the above-mentioned lidar mapping method based on loop detection and odometer.

[0091] Specifically, taking a preferred embodiment as an example, the working process of a lidar mapping system based on loop detection and odometer is described as follows:

[0092] A lidar mapping system based on loop detection and odometer enables the lidar to model the shape of the container under the condition of limited reflection intensity, and of course, it is also applicable to the modeling of other scenarios. Such as Figure 3As shown in the figure, during the mapping process, the lidar and odometer information are first input. When creating the first submap, it is necessary to determine whether the starting scene has a large curvature. Select the scene with a large curvature to start creating the first map, add the lidar information and odometer information, and gradually update the submap through this information. If the number of updates is sufficient, a new submap is added. After adding, it is judged whether there is a loop. After a successful loop, the loop is optimized and the robot pose is adjusted to build a complete map.

[0093] The description method of the map consists of a two-dimensional array and the map origin P o =(x o , y o ) T and the map resolution Res. For the world coordinate P w =(x w , y w ) T , its corresponding two-dimensional array element <x index , y index > is:

[0094]

[0095] The floor(·) function represents rounding down. The two-dimensional array elements <x index , y index > are all 0 in the initial state.

[0096] Large irregular containers are composed of gentle edges and irregular edges. The applicable container bottom perimeter is less than 100 meters, and the odometer error at the container bottom is generally less than 3%. Therefore, the odometer data with timestamps is directly used for robot pose estimation, that is, the prior pose with a timestamp. The odometer data needs to obtain a series of optimized poses with timestamps through loop detection, that is, the posterior pose with a timestamp. The lidar data consists of timestamped point cloud information. By aligning the timestamp stamp l of the lidar data with the timestamps of a series of posterior poses with timestamps, the robot pose l corresponding to the lidar data at the moment of stamp (R r represents the attitude, t r represents the robot position) can be obtained and the global map is updated based on this.

[0097] Find all the robot pose sets S l corresponding to the lidar data S r , and calculate the point cloud in the global coordinate system. The set S p of the point cloud is:

[0098] S p ={P p=(x p , y p ) T |P p =T r ×T l ×P l , T r ∈S r , P l ∈S l}(2)

[0099] P l represents an element in the lidar data set, and T l represents the pose of the lidar in the robot coordinate system, and T r represents the pose of the robot in the global coordinate system;

[0100] The update of the global map is to obtain the element index values of the corresponding two-dimensional array for the elements in S p in sequence through formula (1), and set them to the set value, thus realizing the update of the global map. Generally, the set value is 100, which represents the darkest marking value in the grayscale map, improving the convenience of explaining the update process.

[0101] The stamped posterior pose needs to be optimized through loop detection. Loop detection includes several parts: creation of the local map (i.e., sub-map creation), loop search module, loop matching module, and loop optimization module.

[0102] The advantage of creating a sub-map is to establish the constraint between the stamped prior pose and the local map, and realize optimization by combining with the constraints of the local map, loop search module, and loop matching module. According to the characteristics of large irregular containers, there is only one loop in the container. Correctly determining the starting point of this loop is crucial for the overall loop. To avoid ghosting in the finally drawn global map, the starting point and ending point of the loop need to be selected on the long side with a larger curvature, and the starting long side with a larger curvature is closed with the ending long side, thus realizing the loop of the whole map.

[0103] The long side extraction analyzes the lidar data to determine whether the scene belongs to a continuous long side with a larger curvature. The set of lidar point clouds at the current moment is S l (t), and the number of elements in the set is n. The continuous long side with a larger curvature can be defined as that the lidar point cloud is unbroken and has a larger curvature. Whether the curvature is larger is judged by the following methods: a. Judge by the distance between the centroid of the point cloud and the nearest point of the point cloud being less than the threshold. b. Judge by comparing whether the lidar point cloud is within the range of two calculated parallel lines.

[0104] The centroid P of the lidar point cloud g =(x g , y g) is:

[0105]

[0106] The shortest distance d between the centroid of the point cloud and the lidar point cloud laser-min is:

[0107] d laser-min = argmin‖P g - P l(t) ‖, P l(t) ∈S l(t) (4)

[0108] If d laser-min is greater than the threshold, it means that the curvature is too large or discontinuous and cannot be used as the starting point for mapping.

[0109] Perform principal component analysis (PCA analysis) on the lidar point cloud. As Figure 2 shown, the principal direction and centroid of the lidar point cloud can be obtained. The principal direction is the direction in which the lidar point cloud is most concentrated. Generate two parallel lines along the positive and negative directions of the principal direction of the lidar point cloud respectively, and determine whether the lidar point cloud is between the two parallel lines. If it is between the parallel lines, it means that the curvature is greater than the threshold; otherwise, the curvature is less than the threshold.

[0110] The unit vector of the principal direction of the lidar point cloud The second direction is perpendicular to the principal direction, and the unit vector of the second direction is Therefore, there is a point P + on the line l + = (x + , y + ) T is:

[0111]

[0112] T offset is the translation amount of the centroid of the lidar point cloud along the principal direction.

[0113] Combined with the unit vector of the second direction and P + the straight-line equation of line l + can be generated as l + (x, y) is:

[0114] l + (x, y): y main ×y - y main ×y + + x main ×(x - x + ) = 0 (6)

[0115] Similarly, there is a point P on the straight line l generated along the negative direction of the main direction - =(x - ,y - ) - ) T is:

[0116]

[0117] Combined with the unit vector in the second direction and P - can generate the straight line equation of the straight line l - (x, y) as:

[0118] l - (x, y): y main ×y - y main ×y - +x main ×(x - x - ) = 0 (8)

[0119] If each point in the lidar point cloud is on the opposite side of l + and l - , it is considered that the current lidar point cloud is between the two parallel lines, otherwise it is considered that at least one point is outside the two parallel lines. For all lidar point clouds P l(t) =(x l(t) , y l(t) ) T ∈S l(t) , if equation (9) holds, the lidar point cloud is between the parallel lines, otherwise there is a lidar point outside the two parallel lines.

[0120] l + (x l(t) , y l(t) ) × l - (x l(t) , y l(t) ) < 0 (9)

[0121] If equation (9) holds, start building the 0th sub - map, the sub - map origin P0 = (x0, y0) T , and the sub - map resolution is Res. After each addition of lidar information, the map will be expanded accordingly to include all lidar point cloud information. The index value corresponding to each point in the point cloud can be obtained through (1), and the two - dimensional array index value corresponding to each point in the sub - map is found, and the corresponding index value of the two - dimensional array is set to 100.

[0122] During the map building process, the odometry information of the robot is recorded. If the current odometry position exceeds the translation threshold compared to the position of the previous nearest odometry, the information of the odometry and the lidar is recorded. After the number of records accumulates to the threshold, the construction of the next submap starts. The origin P of the nth submap n =(x n , y n ), T and the submap resolution is Res.

[0123] The data contained in each submap is a pairing of <odometry, lidar point cloud>. After each data addition, the current odometry information is recorded. When subsequent odometry data is input, the distance between the current odometry and the last recorded odometry is judged. When the distance exceeds the threshold, the current <odometry, lidar point cloud> pairing is added. When the number of added information is sufficient, it is considered that the current submap is completed and the next submap is added.

[0124] After the robot builds at least one submap, loop search needs to start. The loop search is carried out by distributing a series of loop checkpoints around the robot position t r =(x r , y r ). T The significance of the loop checkpoints is to check whether the robot has returned to the map building starting point of the container loop. The judgment condition is: judge whether these loop checkpoints are within the 0th submap and whether the two-dimensional array index value reaches the set value (usually 100). If a large number of loop checkpoints are within the 0th submap and the two-dimensional index value is 100, it means that the current robot pose is near the starting point of the container loop, and loop judgment can be implemented. If the number of loop checkpoints with a two-dimensional array index value of 100 exceeds the threshold, the loop search is considered successful.

[0125] The set S of loop checkpoints loop is:[[]]

[0126] S loop ={p l =(x l , y l ) T |x l =x r +t offset , y l =y r +t offset} (10)

[0127] where t offset represents the translation amount, t offset ∈[t min , t max , t minrepresents the minimum translation amount, t max represents the maximum translation amount.

[0128] After the loop search is successful, loop matching needs to be performed. The loop matching directly uses the current odometry pose T r(t) (with timestamp prior pose) to perform ICP point cloud matching on the point set with a two-dimensional array index value of 100 detected by the corresponding lidar point cloud and loop check points, and obtain the optimized pose T r ′(t) (with timestamp posterior pose). After the loop matching is completed, loop optimization is performed. The loop optimization adopts the sparse pose optimization method (SPA), and uses non-linear optimization to adjust the with timestamp prior pose to the with timestamp posterior pose, so that the lidar point cloud realizes the establishment of the global map through the homogeneous transformation of the with timestamp posterior pose.

[0129] The above embodiments are the preferred embodiments of the present invention, but the embodiments of the present invention are not limited by the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principle of the present invention shall be equivalent replacement methods and are all included in the protection scope of the present invention.

Claims

1. A lidar mapping method based on loop detection and odometer, characterized in that Including the following steps: S01: Obtain lidar point cloud and odometer information; S02: Extract the long sides of the lidar point cloud information, determine whether they are continuous long sides with a curvature greater than the threshold, and select the continuous long sides with a curvature greater than the threshold to establish a submap; S03: Perform loop detection on the established submap, optimize the loop, and adjust the robot pose to establish a complete map.

2. The lidar mapping method based on loop detection and odometer according to claim 1, wherein The method for determining whether the continuous long sides have a curvature greater than the threshold in step S02 includes: Obtain the main direction and centroid of the lidar point cloud through principal component analysis; Calculate the distance between the centroid of the lidar point cloud and the nearest point of the lidar point cloud, and compare it with the threshold. If it is less than the threshold, proceed to the next judgment; Generate two parallel lines respectively along the positive and negative directions of the main direction of the lidar point cloud, and determine whether the lidar point cloud is between the two parallel lines. If it is between the parallel lines, it means the curvature is greater than the threshold, otherwise the curvature is less than the threshold.

3. The method for lidar mapping based on loop detection and odometer according to claim 2, wherein The method for generating two parallel lines includes: Obtain the unit vector of the main direction of the lidar point cloud x main , y main respectively represent the components of the unit vector of the main direction on the x and y axes. The second direction is perpendicular to the main direction, and the unit vector of the second direction is The straight line l generated along the positive direction of the main direction + There is a point P on it + =(x + , y + ) T is as follows: Among them, P g is the centroid of the lidar point cloud, and T offset is the translation amount of the centroid of the lidar point cloud along the main direction; Combine the unit vector in the second direction and P + to generate the straight line l + The straight line equation of l + (x, y) is as follows: l + (x, y): y main ×y - y main ×y++x main ×(x - x + ) = 0 Similarly, there is a straight line \(l\) generated along the negative direction of the main direction - with a point \(P\) on it - =(x - , y - ) T as follows: Combine the unit vector in the second direction and P - Generate the straight line l - (x, y) The straight line equation is: l - (x, y): y main ×y - y main ×y - +x main ×(x - x - ) = 0。 4. The lidar mapping method based on loop detection and odometer according to claim 2, characterized in that, The method for determining whether the lidar point cloud is between the two parallel lines includes: For the radar point cloud P at the current moment l(t) =(x l(t) , y l(t) ) T ∈ S l(t) , if the following formula holds: l + (x l(t) ,y l(t) )×l - (x l(t) ,y l(t) )<0 S l(t) is the set of radar point clouds at the current moment; Then the lidar point cloud is between the parallel lines, otherwise the lidar point cloud is outside the two parallel lines.

5. The lidar mapping method based on loop detection and odometer according to claim 1, characterized in that In step S02, the 0th sub-map is initially established, and the origin of the 0th sub-map is P0 = (x0, y0). T , the resolution of the sub-map is Res, and the two-dimensional array index value corresponding to each point of the lidar point cloud is calculated; during the mapping process, the odometer information is recorded. If the current odometer position exceeds the translation threshold compared to the previous nearest odometer position, the information of the odometer and the lidar is recorded. After the recording count accumulates to the threshold, the next sub-map is started to be established. The origin of the nth sub-map is P n = (x n , y n ). T , and the resolution of the sub-map is Res.

6. The method for lidar mapping based on loop detection and odometer according to claim 1, characterized in that The loop detection in step S03 includes loop search, loop matching, and loop optimization. After the robot has built at least one submap, loop search starts. The loop search distributes a series of loop checkpoints around the robot's position t r =(x r , y r ) T and the set S loop of loop checkpoints is as follows: S loop = {p l = (x l , y l ) T | x l = x r + t offset , y l = y r + t offset} Among them, p l is a loopback checkpoint, and t offset represents the translation amount, where t offset ∈ [t min , t max , t min represents the minimum translation amount, and t max represents the maximum translation amount; And determine whether these loop check points are within the 0th submap and whether the two-dimensional array index value reaches the set value. If the number of loop check points whose two-dimensional array index value reaches the set value exceeds the threshold, it is considered that the loop search is successful; After the loop search is successful, perform loop matching, and after the loop matching is completed, perform loop optimization.

7. The method for lidar mapping based on loop detection and odometer according to claim 6, characterized in that, The loop closure matching directly uses the current odometry pose T r (t) corresponding lidar point cloud and loop closure check points check the point set where the two-dimensional array index value reaches the set value for ICP point cloud matching to obtain the optimized pose T′ r (t). After the loop closure matching is completed, loop closure optimization is performed. The loop closure optimization adopts a sparse pose optimization method, and the stamped prior pose is adjusted to the stamped posterior pose in a non-linear optimization manner, so that the lidar point cloud realizes the establishment of the global map through the homogeneous transformation of the stamped posterior pose.

8. The lidar mapping method based on loop detection and odometer according to claim 7, characterized in that, The method for establishing a global map includes: By aligning the timestamp stamp of the lidar data l with the timestamps of a series of stamped posterior poses, the stamp l is obtained, and the robot pose corresponding to the lidar data at the moment R r represents the attitude, and t r represents the robot position; Find all lidar data sets S l The pose set S of the corresponding robot in the global coordinate system r , and calculate the point cloud in the global coordinate system through pose transformation. The set S of the point cloud p is as follows: S p = {P p = (x p , y p ) T | P p = T r × T l × P l , T r ∈ S r , P l ∈ S l} P l represents an element within the lidar data set, T l represents the pose of the lidar relative to the robot, T r represents the pose of the robot in the global coordinate system; Calculate the element index values of the corresponding two-dimensional array in sequence for the elements in S p and set them to the set values, thus realizing the update of the global map.

9. A lidar mapping system based on loop detection and odometer, characterized in that, Including a processor, and the processor is built-in with the lidar mapping method based on loop detection and odometer according to any one of claims 1-8.

10. A computer storage medium, on which a computer program is stored, characterized in that, When the computer program is executed, it implements the lidar mapping method based on loop detection and odometer according to any one of claims 1-8.

Images