Initial mapping and map filtering method and equipment medium based on 3D laser radar

Abstract

The invention discloses an initial mapping and map filtering method based on a 3D laser radar and a device medium. The method comprises the following steps: preprocessing data; inputting the input data into a local speedometer module, a loopback detection and global optimization module and a point cloud segmentation and mapping module; in the local speedometer module, registration between the current frame point cloud and a local map is executed, and local speedometer information is obtained; in the loopback detection and global optimization module, key frames are selected to execute loopback detection, and the pose image is optimized and updated; in the point cloud segmentation and mapping module, carrying out point cloud segmentation on the input data; establishing association between poses and point clouds, and performing 2D projection on the associated point clouds to generate a 2.5 D grid map; and accumulating the associated poses and point clouds to generate an original 3D point cloud map, and filtering the original 3D point cloud map by using a 2.5 D grid map to obtain a new 3D point cloud map. According to the invention, dynamic objects can be rejected, and the environment can be accurately reflected.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to an initial mapping and map filtering method based on 3D laser radar and a device medium. Background Art

[0002] The prerequisite for the operation of an autonomous mobile robot is that it can stably and reliably obtain its own position and posture. However, in the actual working environment, dynamic and time-varying scenes often affect the positioning performance of the robot. At present, the mainstream solution for robot positioning and mapping is the Simultaneous Localization and Mapping (SLAM) technology, which uses sensors to perceive the surrounding environment and uses this information to locate itself and build an environmental map. Since the popularization of 3D lidar, various positioning and mapping algorithms have emerged in an endless stream. Depending on whether they integrate IMU information, they can be divided into pure lidar method and fusion method. In the case of violent movement of the robot, the pure lidar method is difficult to ensure the stability of positioning, and even the positioning may drift seriously. The fusion method integrates independent high-frequency IMU data, and its stability is properly guaranteed. However, none of the above methods make special treatments for dynamic environments, and it is difficult to ensure that the positioning and mapping links still maintain high accuracy, so they cannot meet the needs of accurate positioning and mapping.

[0003] In a dynamic environment, a large number of dynamic objects will pollute the established map and interfere with subsequent repositioning. To address the pollution problem of 3D point cloud maps, the usual process is to store the data of the mapping process and use post-processing to perform further filtering. For example, the Removert method, the ERASOR method, the RF-LIO method, etc. However, the above methods rely on the temporal order of the point cloud to perform dynamic clearing, and the clearing effect on dynamic objects is not good. Some methods that rely on range image algorithms have high requirements for the wiring harness of the 3D lidar. If the wiring harness cannot meet the requirements, when filtering the map, it is often unable to distinguish between dynamic and static objects due to low resolution, and methods that rely on occupancy grid algorithms often cannot meet real-time requirements. Therefore, the above methods are often difficult to meet design requirements in terms of filtering effect or real-time performance.

[0004] In order to solve the problem of low precision of traditional mapping methods without special treatment of dynamic objects, the present invention proposes a high-precision positioning and mapping system that can eliminate dynamic objects and correctly reflect the environment. Summary of the invention

[0005] In view of this, the purpose of the present invention is to propose an initial mapping and map filtering method based on 3D lidar, which can eliminate dynamic objects and accurately reflect the environment.

[0006] In order to achieve the above technical objectives, the technical solution adopted by the present invention is: The present invention provides an initial mapping and map filtering method based on 3D laser radar, comprising the following steps: Step 1: preprocess the lidar data and the inertial measurement unit data to obtain input data; Step 2: input the input data into the local odometer module, the loop detection and global optimization module, and the point cloud segmentation and mapping module respectively; Step 3: In the local odometer module, a local map is established, and the registration between the current frame point cloud and the local map is performed according to the input data and the local map, and then the local odometer information is obtained through an iterative error Kalman filter, and the pose of the frame point cloud is associated and accumulated in the local map; Step 4: In the loop detection and global optimization module, a key frame is selected according to the input data and the local odometer information to perform loop detection, and the pose graph is optimized, and the historical pose information is updated according to the local odometer information and the optimized pose graph; Step 5: In the point cloud segmentation and mapping module, the input data is segmented into point clouds; and the pose and point cloud are associated according to the updated historical pose information and the segmented point cloud, and the associated point cloud is projected in 2D to generate a 2.5D grid map; the associated pose and point cloud are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map.

[0007] Furthermore, the step 1 specifically includes: Step 11: aligning and downsampling the laser radar data and the inertial measurement unit data; Step 12: perform pose prediction based on the inertial measurement unit data, and estimate the lidar pose of each point in a frame of point cloud in the lidar data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated lidar pose, and remove the point cloud with motion distortion.

[0008] Furthermore, the step 4 specifically includes: Step 41, obtaining distance, angle and time information according to the local odometer information, performing multi-dimensional analysis according to the distance, angle and time information, and selecting key frames from the input data; Step 42, when a new key frame is added, according to the spatial posture of the new key frame, the nearest historical key frame information is searched within the set radius length; Step 43: If there is a historical key frame, the historical key frame information within a set range near the historical key frame is selected to form a historical loop subgraph; Step 44: Perform ICP registration on the historical loop subgraph and the new key frame, and determine whether it is a real loop based on the registration score; Step 45: If it is a true loop, the corresponding new key frame is added to the pose graph; Step 46, optimizing the pose graph by a pose graph optimizer; Step 47: Update the historical pose information according to the local odometer information and the optimized pose graph.

[0009] Furthermore, the step 47 is specifically as follows: After each pose graph optimization is completed, the pose graph is used to optimize the key frame pose set For the odometry pose set Update as shown below: (1) In the formula, I Represents the IMU coordinate system, IMU stands for inertial measurement unit, G represents the global coordinate system, k Indicates k frame, j Indicates j frame, The pose graph optimization stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Construct the updated pose set ; Represents the local odometer stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the local odometer stage j The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, represents the pose graph optimization stage The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Constructing pose graph to optimize keyframe pose set .

[0010] Furthermore, the step 5 performs point cloud segmentation on the input data, specifically including: Step 51: Dedistorted point cloud obtained at each moment , which contains Points, is described as a set as follows: (2) in, Indicates frame, Represents the laser radar coordinate system; For collections Each point contained in , which contains the information represented by the following formula: (3) point middle Represents the spatial coordinates corresponding to the point, Indicates the line index corresponding to the point and the acquisition time relative to the starting point; Step 52: Divide the 360° scanning range into regions and set The angle covered by each sector; for each frame point cloud ,get sectors, each of which The index of the corresponding sector is expressed as , obtained by the following formula: (4) If the current laser radar is for each point in the point cloud The sampling time is correct, that is, of The parameters are stable and reliable, and their value range is expressed as ,but The calculation is accelerated by: (5) Step 53: For mechanical laser radar, obtain the points of the point cloud With reliable parameter, The parameter indicates the harness in which the point cloud is currently located; Assume that a frame of point cloud has a total of Harness, define parameters For each loop The number of covered bundles, for each frame of the point cloud ,get loops, then each Corresponding loop line The index is represented as , calculated by the following formula: (6) Step 54: Each point in the point cloud Each has its corresponding sector and loop line The index of the definition collection is a set of points with the same index, obtained by the following formula: (7) in,s Indicate point Corresponding sectors , Indicate point The corresponding loop , ∧ indicates the relationship between and ; Assume that a set There are a total of Points, Representing a collection The number of points in ; then it is expressed by the following formula: (8) The points included Also contains the spatial coordinates, the bundle index and the acquisition time relative to the starting point This information, namely , through the following formula (9) Expressed as ,in express exist x - y The distance from the origin on the plane, express of z Axis height; (9) Step 55: Set each point set Corresponding to a space , described as , in each There are multiple representation spaces The parameters of the overall information, where the height information is expressed as , the distance information is expressed as , the height variance is expressed as , the above parameters are calculated according to the following formula: (10) Each space The information is collected by Determined by the average of all points in Step 56: Record each sector simultaneously during preprocessing For all spaces Loop Line The minimum value is , the maximum value is , for each sector Space in Perform sequential calculation and judgment, and realize point cloud segmentation based on the judgment results.

[0011] Furthermore, the step 56 implements point cloud segmentation according to the judgment result, which specifically includes: Step 561: and Make a judgment, if > , then skip the current sector where acquisition failed , enter the next sector Then return to step 561; if ≤ , then proceed to step 562; Step 562: From arrive In order as l Perform the calculation: 1) Calculate the height difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous line The height difference between Indicates the current sector and the current loop The corresponding height, Indicates the current sector And the previous line The corresponding height; 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous line The distance difference between Indicates the current sector and the current loop The corresponding distance, Indicates the current sector and the previous line The corresponding distance; 3) Calculate the slope: = ; Indicates the current sector and the current loop The corresponding slope; 4) Calculate the slope difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The slope difference between Indicates the current sector and the current loop The corresponding slope, Indicates the current sector And the previous line The corresponding slope; Step 563: Determine whether the conditions are met: , , and ,in, Indicates the ground height interval, represents the ground slope threshold, represents the ground slope difference threshold, Indicates the current sector and the current loop The corresponding height variance is, Represents the ground variance threshold; if yes, the corresponding point is put into the ground point cloud ; Otherwise, go to step 564; Step 564: Determine whether the conditions are met: , and ,in, Indicates the ceiling height range, represents the ceiling slope threshold, Indicates the ceiling slope difference threshold; if yes, the corresponding point is put into the ceiling point cloud ; Otherwise, go to step 565; Step 565: Determine whether the conditions are met: and ,in, represents the wall-like slope threshold, Represents the wall-like slope difference threshold; if so, the corresponding point is put into the wall-like point cloud ; Otherwise, skip the current space .

[0012] Furthermore, in step 5, the association between the pose and the point cloud is established according to the updated historical pose information and the segmented point cloud, and the associated point cloud is 2D projected to generate a 2.5D grid map; specifically, the steps include: Step 57: Dedistort the point cloud After segmentation, remove the ceiling point cloud And unclassified points, select the ground point cloud Point cloud of wall surface , and the pose set after global optimization The point cloud and pose are associated as shown in the following formula: (11) in, and Represents the global coordinate system The ground point cloud and wall-like point cloud under Indicates the pose graph optimization stage in The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the LiDAR pose in the IMU coordinate system, represents the distortion of the laser radar coordinate system Frame ground point cloud, represents the distortion of the laser radar coordinate system Frame wall point cloud; Step 58: In the global coordinate system Next, we create a resolution of 2.5D grid map ; Step 59: On the 2.5D grid map middle Indicates that one of the center points is located at And the diameter is The grid will and The points in the 2D projection are projected to In, assuming Included Ground points , and Represent the weights of the ground and wall-like surfaces respectively, and are calculated as follows Information included: (12) In the formula, The ground judgment information determines whether the attribute corresponding to this grid is ground or wall-like. is the grid height information; z Represents a three-dimensional coordinate system The value of the axis.

[0013] Furthermore, in step 5, the associated posture and point cloud are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map; specifically, the steps include: Step 510: and Point clouds are accumulated to obtain the original 3D point cloud map ; Step 511: Point on Traverse, if it corresponds to on If the threshold condition is met, it is judged as the ground, and there should be only ground points on it. To filter, is the point cloud map resolution, as shown in the following formula: (13) Filter out the dynamic points on the ground and get a new 3D point cloud map Only ground information and wall-like information are left, and dynamic obstacles are eliminated.

[0014] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the initial mapping and map filtering method based on 3D laser radar as described above is implemented.

[0015] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned initial mapping and map filtering method based on 3D laser radar.

[0016] By adopting the above technical solution, the present invention has the following beneficial effects compared with the prior art: The present invention includes a local odometer module, a loop detection and global optimization module, and a point cloud segmentation and mapping module. The local odometer module is used to calculate the local odometer, the loop detection and global optimization module is used for loop detection and global optimization, and the point cloud segmentation and mapping module is used for point cloud segmentation and mapping. Reliable loop detection and efficient pose graph optimization are achieved through the loop detection and global optimization modules, and the wall surface is clear and truly reflects the environment. The ground is robustly segmented through a continuity-based cloud segmentation algorithm, the cluttered point cloud is removed, and the ground segmentation error is minimized. Through grid map establishment and map filter filtering, only ground information and wall-like information are left in the new 3D point cloud map obtained by filtering, and dynamic obstacles are eliminated, thereby accurately reflecting the environment and solving the problem of residual dynamic objects in the point cloud map. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0018] Figure 1It is an execution flow chart of an initial mapping and map filtering method based on 3D laser radar provided in an embodiment of the present invention.

[0019] Figure 2 It is a schematic diagram of pose graph optimization provided by an embodiment of the present invention.

[0020] Figure 3 Schematic diagram of point cloud segmentation provided by an embodiment of the present invention.

[0021] Figure 4 It is a schematic diagram of a 2.5D grid map provided by an embodiment of the present invention.

[0022] Figure 5 It is a schematic diagram of grid calculation provided by an embodiment of the present invention.

[0023] Figure 6 It is a structural diagram of the experimental verification platform provided by an embodiment of the present invention.

[0024] Figure 7 It is a schematic diagram of the experimental verification environment provided by an embodiment of the present invention.

[0025] Figure 8 It is a top view and a z-axis view of the trajectory provided by an embodiment of the present invention.

[0026] Fig. 9 It is a ground height error comparison diagram provided by an embodiment of the present invention.

[0027] Fig.10 This is a comparison chart of mapping effects provided by an embodiment of the present invention.

[0028] Fig.11 It is a schematic diagram of the point cloud segmentation experiment results provided by an embodiment of the present invention.

[0029] Fig.12 This is a global effect diagram of a map filter provided by an embodiment of the present invention.

[0030] Fig.13 This is a local effect diagram of a map filter provided by an embodiment of the present invention.

[0031] Fig.14 It is a schematic diagram of an electronic device provided by an embodiment of the present invention.

[0032] Fig.15 It is a schematic diagram of a computer-readable storage medium provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0033] The present invention will be further described in detail below in conjunction with the accompanying drawings and examples. It is particularly noted that the following examples are only used to illustrate the present invention, but are not intended to limit the scope of the present invention. Similarly, the following examples are only partial embodiments of the present invention rather than all embodiments, and all other embodiments obtained by those of ordinary skill in the art without creative work are within the scope of protection of the present invention.

[0034] See also Figure 1 The present invention provides an initial mapping and map filtering method based on 3D laser radar, comprising the following steps: Step 1: Preprocess the lidar data and the inertial measurement unit (IMU) data to obtain input data; In this embodiment, the step 1 specifically includes: Step 11: aligning and downsampling the laser radar data and the inertial measurement unit data; Step 12: Perform pose prediction based on the inertial measurement unit data, and estimate the LiDAR pose of each point in a frame of LiDAR data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated LiDAR pose, and remove the point cloud with motion distortion. At the same time, the sampling order of each point is obtained by the precise sampling time of each individual point. Therefore, the points in the laser frame point cloud can be regarded as being sampled at the same time at the end of the frame, completing the point cloud motion distortion removal.

[0035] Step 2: input the input data into the local odometer module, the loop detection and global optimization module, and the point cloud segmentation and mapping module respectively; Step 3: In the local odometer module, a local map is established, and the registration between the current frame point cloud and the local map is performed according to the input data and the local map, and then the local odometer information is obtained through an iterative error Kalman filter, and the pose of the frame point cloud is associated and accumulated in the local map; In this embodiment, in step 3, the registration between the current frame point cloud and the local map is performed according to the input data and the local map, specifically: the point-surface residual between the current frame point cloud and the local map is calculated, and the registration is performed.

[0036] Step 4: In the loop detection and global optimization module, a key frame is selected according to the input data and the local odometer information to perform loop detection, and the pose graph is optimized, and the historical pose information is updated according to the local odometer information and the optimized pose graph; on the basis of the local odometer, loop detection and global optimization are further introduced to correct the pose graph, so that the odometer pose is updated synchronously, thereby improving the accuracy of mapping; In this embodiment, step 4 specifically includes: Step 41, obtaining distance, angle and time information according to the local odometer information, performing multi-dimensional analysis according to the distance, angle and time information, and selecting key frames from the input data; Step 42, when a new key frame is added, the nearest historical key frame information is searched within the set radius length according to the spatial posture of the new key frame; Step 43: If there is a historical key frame, the historical key frame information within a set range near the historical key frame is selected to form a historical loop subgraph; Step 44: Perform ICP registration on the historical loop subgraph and the new keyframe (ICP (Iterative Closest Point) registration is a commonly used point cloud registration technology, mainly used to accurately align two point cloud data), and determine whether it is a true loop through the registration score; Step 45: If it is a true loop, the corresponding new key frame is added to the pose graph; Step 46, optimizing the pose graph by a pose graph optimizer; Step 47: Update the historical pose information according to the local odometer information and the optimized pose graph.

[0037] In this embodiment, step 47 is specifically as follows: The data transmission between the local odometer and the global pose graph optimization is as follows Figure 2 As shown, Figure 2 T represents the set of odometer poses. Represents a point cloud map, represents the constraint factor (expressed by relative pose); represents the data in the local odometer stage, and Represents the data during the pose graph optimization phase.

[0038] After each pose graph optimization is completed, the pose graph is used to optimize the key frame pose set For the odometry pose set Update as shown below: (1) In the formula, I Represents the IMU coordinate system, IMU stands for inertial measurement unit, G represents the global coordinate system, k Indicates k frame, j Indicates j frame, The pose graph optimization stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Construct the updated pose set ; Represents the local odometer stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the local odometer stage j The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, represents the pose graph optimization stage The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Constructing pose graph to optimize keyframe pose set .

[0039] Step 5: In the point cloud segmentation and mapping module, the input data is segmented into point clouds; and the pose and point cloud are associated according to the updated historical pose information and the segmented point cloud, and the associated point cloud is projected in 2D to generate a 2.5D grid map; the associated pose and point cloud are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map.

[0040] In this embodiment, the point cloud segmentation of the input data in step 5 adopts a point cloud segmentation algorithm based on continuity, which specifically includes: Step 51: Dedistorted point cloud obtained at each moment , which contains Points, is described as a set as follows: (2) in, Indicates frame, Represents the laser radar coordinate system; For collections Each point contained in , which contains the information represented by the following formula: (3) point middle Represents the spatial coordinates corresponding to the point, Indicates the line bundle index corresponding to the point and the acquisition time relative to the starting point; the point cloud segmentation method designed by the present invention is optimized by combining the point cloud's own parameters, which can greatly improve the efficiency and accuracy of point cloud segmentation.

[0041] Step 52: The mechanical laser radar performs a 360° full-scale scan of the environment. The present invention also defines sectors. The 360° scanning range is divided into regions and set The angle covered by each sector; for each frame point cloud ,get sectors, each of which The index of the corresponding sector is expressed as , obtained by the following formula: (4) If the current laser radar is for each point in the point cloud The sampling time is correct, that is, of The parameters are stable and reliable, and their value range is expressed as ,but Speed ​​up the calculation by: (5) This point cloud segmentation method extends the 2D projection to 2.5D through the continuity relationship between 3D point cloud line bundles.

[0042] Step 53: For mechanical laser radar, obtain the points of the point cloud With reliable parameter, The parameter indicates the harness in which the point cloud is currently located; Assume that a frame of point cloud has a total of Harness, define parameters For each loop The number of covered bundles, for each frame of the point cloud ,get loops, then each Corresponding loop line The index is represented as , calculated by the following formula: (6) Step 54: Each point in the point cloud Each has its corresponding sector and loop line The index of the definition collection is a set of points with the same index, obtained by the following formula: (7) in, s Indicate point Corresponding sectors , Indicate point The corresponding loop , ∧ indicates the relationship between and ; Assume that a set There are a total of Points, Representing a collection The number of points in ; then it is expressed by the following formula: (8) The points included Also contains the spatial coordinates, the bundle index and the acquisition time relative to the starting point This information, namely , through the following formula (9) Expressed as ,in express exist x - y The distance from the origin on the plane, express of z Axis height; (9) Step 55: Set each point set Corresponding to a space , described as , in each There are multiple representation spaces The parameters of the overall information, where the height information is expressed as , the distance information is expressed as , the height variance is expressed as , the above parameters are calculated according to the following formula: (10) Each space The information is collected by It is determined by the average value of all points, which can avoid the limitation of the minimum value; Step 56: In actual environments, point cloud collection often has a range problem, so each sector is recorded simultaneously during the preprocessing process. For all spaces Loop Line The minimum value is , the maximum value is , for each sector Space in Perform sequential calculation and judgment, and realize point cloud segmentation based on the judgment results.

[0043] In this embodiment, the step 56 implements point cloud segmentation according to the judgment result, quickly segments the point cloud according to the point cloud characteristics, and identifies the ground point cloud and the wall-like point cloud; specifically, it includes: Step 561: and Make a judgment, if > , then skip the current sector where acquisition failed , enter the next sector Then return to step 561; if ≤ , then proceed to step 562; Step 562: From arrive In order as l Perform the calculation: 1) Calculate the height difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The height difference between Indicates the current sector and the current loop The corresponding height, Indicates the current sector and the previous line The corresponding height; 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The distance difference between Indicates the current sector and the current loop The corresponding distance, Indicates the current sector and the previous line The corresponding distance; 3) Calculate the slope: = ; Indicates the current sector and the current loop The corresponding slope; 4) Calculate the slope difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The slope difference between Indicates the current sector and the current loop The corresponding slope, Indicates the current sector and the previous line The corresponding slope; Step 563: Determine whether the conditions are met: , , and ,in, Indicates the ground height interval, represents the ground slope threshold, represents the ground slope difference threshold, Indicates the current sector and the current loop The corresponding height variance is, Represents the ground variance threshold; if yes, the corresponding point is put into the ground point cloud ; Otherwise, go to step 564; Step 564: Determine whether the conditions are met: , and ,in, Indicates the ceiling height range, represents the ceiling slope threshold, Indicates the ceiling slope difference threshold; if yes, the corresponding point is put into the ceiling point cloud ; Otherwise, go to step 565; Step 565: Determine whether the conditions are met: and ,in, represents the wall-like slope threshold, Represents the wall-like slope difference threshold; if so, the corresponding point is put into the wall-like point cloud ; Otherwise, skip the current space .

[0044] As shown in Algorithm 1

[0045] Algorithm 1 first and Make a judgment and skip the sectors where acquisition failed , and then Start, such as Figure 3 As shown, Figure 3 (a) shows a portion of the points in a frame of point cloud information, and the point cloud with a z-axis height of -0.5m to 2.5m is dyed rainbow colors. (b) shows the point cloud information seen from the perspective of the lidar to illustrate the 3D points and space. The relationship between the two vertical dashed lines. All points between the two vertical dashed lines satisfy the condition ; Similarly, the points in the two horizontal dashed lines all meet the condition , the space that satisfies both conditions is , which is represented as a box in the figure. (c) will satisfy All spaces of conditions Projection arrived Coordinate system. Squares, circles, triangles and hexagons represent various spaces. , solid lines represent the slopes between spaces, different shapes indicate different judgment results, squares represent the ground, hexagonal stars represent the wall-like surface, triangles represent the ceiling, and circles represent the discarded space , because it does not meet any of the judgment conditions.

[0046] At the actual algorithm level, each space First and the previous space Perform difference calculation to obtain height difference Difference from distance , and use this to calculate the slope ,as well as and The slope difference between , indicating the flatness between the loops. Then, all the previous calculations are thresholded. For the ground and ceiling, the default posture runs in an environment without steep slopes. First, based on The height is judged, and the condition is met only within a specific range; then the absolute slope To make a judgment, the robot's current horizontal posture must be less than a certain threshold; finally, the relative slope difference To determine whether a plane is flat or not, a plane is one with a flatness less than a certain value. For the ground, in order to ensure its robustness, its internal height variance is also calculated. For wall-like points, only the absolute slope and relative slope are judged after the inversion.

[0047] In this embodiment, the step 5 establishes an association between the pose and the point cloud according to the updated historical pose information and the segmented point cloud, performs 2D projection on the associated point cloud, and generates a 2.5D grid map; specifically includes: Step 57: Dedistort the point cloud After segmentation, remove the ceiling point cloud And unclassified points, select the ground point cloud Point cloud of wall surface , and the pose set after global optimization The point cloud and pose are associated as shown in the following formula: (11) in, and Represents the global coordinate system The ground point cloud and wall-like point cloud under Indicates the pose graph optimization stage in The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the LiDAR pose in the IMU coordinate system, represents the dedistorted first Frame ground point cloud, represents the dedistorted first Frame wall point cloud; Step 58: In the global coordinate system Next, create a resolution of 2.5D grid map ;like Figure 4 As shown, its multi-dimensional and easily addressable characteristics reduce the complexity of the system.

[0048] Step 59: On the 2.5D grid map middle Indicates that one of the center points is located at And the diameter is The grid will and The points in the 2D projection are projected to In, assuming Included Ground points , and Represent the weights of the ground and wall-like surfaces respectively, and are calculated as follows Information included: (12) In the formula, The ground judgment information determines whether the attribute corresponding to this grid is ground or wall-like. is the grid height information; z Represents a three-dimensional coordinate system The value of the axis.

[0049] The grid map diagram is as follows Figure 5 As shown, Figure 5 (a) shows the segmented point cloud from a 3D perspective and The depth of the ground represents the 2.5D grid map middle (b) A portion of the ground in (a) is captured from a 2D bird’s-eye view. (c) A magnified grid enclosed by the box in (b) contains Ground points and Wall point , in the figure , .

[0050] In this embodiment, in step 5, the associated posture and point cloud are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map; specifically, the following steps are included: Step 510: and Point clouds are accumulated to obtain the original 3D point cloud map ; Step 511: Point on Traverse, if it corresponds to on If the threshold condition (<0) is met, it is judged as the ground, and there should be only ground points on it. Filter (using map filter to filter), is the point cloud map resolution, as shown in the following formula: (13) Filter out the dynamic points on the ground and get a new 3D point cloud map Only ground information and wall-like information are left in the point cloud map, and dynamic obstacles are removed, so as to accurately reflect the environment and solve the problem of residual dynamic objects in the point cloud map. Considering the size of the map storage, the resolution is finally right Perform voxel downsampling.

[0051] Embodiment 1: The present invention is based on the initial mapping and map filtering method of 3D laser radar, which is used for mobile robot navigation, positioning and mapping in dynamic environments. The specific implementation process is as follows: Step 1: Build the experimental platform and environment.

[0052] like Figure 6 As shown, based on the Scout-mini mobile robot platform, it is equipped with a multi-line laser radar 1 (Robosense RS-LiDAR-16 (20Hz)), an IMU inertial measurement unit 2 (Xsens MTi 30 (400Hz)), an edge computing box 3 (Nvidia Jetson AGX Xavier, ROS Melodic) as the computing platform, and a personal computer (AMD R7-4800H, ROS Noetic).

[0053] The experimental verification environment is a large indoor office, which is similar to the working environment of an autonomous mobile robot. Figure 7 As shown, Figure 7 (a) shows the topographical view of an office, where the dot is the starting point for map construction. (b) shows the actual environment, corresponding to positions Ⅰ to VI in (a), including offices, long corridors, stairs, narrow passages and other complex environments.

[0054] Step 2: Implement “LiDAR / IMU local odometer” and “loop detection and pose graph optimization”.

[0055] The SLAM algorithm including loop detection and global optimization is implemented and compared with SOTA's FAST-LIO2 and LIO-SAM. The key to high-precision robot mapping lies in the trajectory (positioning accuracy) in the mapping stage. Figure 7 ), we start from the starting point and circle around the office, and finally return to the starting point. The trajectory diagrams and z-axis height diagrams obtained by different methods are shown in Figure 8 As shown in the figure, in the solid-line frame in the trajectory diagram, due to the lack of global optimization, loop constraints cannot be used, resulting in the failure of the FAST-LIO2 trajectory (line segment with a circle) to close at the starting point, with a large error; in the dotted-line frame in the trajectory diagram, the robot makes a sharp turn in front of a narrow passage, and the sparse point cloud causes the LIO-SAM (line segment with a square) factor graph optimization to fail, and its posture drifts. In contrast, from an aerial view, the trajectory obtained by the SLAM algorithm (line segment with a slash) proposed in the present invention is globally closed and has no drift throughout the entire process.

[0056] In addition to the drift problem, LIO-SAM also has a large ground height estimation error. This is because it extracts features from point clouds, and the feature-based laser odometer loses some constraints. Figure 8 From the z-axis diagram, the maximum height difference of LIO-SAM reaches 2.35 meters. In comparison, the height difference of the method proposed in the present invention is only 1.24 meters, which is about half of the LIO-SAM method.

[0057] In order to intuitively display the z-axis height error, the ground surface of the maps constructed by LIO-SAM and the method of the present invention is extracted using a progressive map filtering algorithm, as shown in Fig. 9 As shown, Fig. 9 (a) is the ground map generated by LIO-SAM, and (b) is the ground map generated by the method of the present invention. The Z-axis height represents the height from -1 meter to 2 meters. It can be seen that the ground height change of the method of the present invention is more uniform and the range is smaller.

[0058] According to the trajectories obtained by the three methods, the corresponding maps can be obtained by accumulating point clouds, such as Fig.10As shown in the figure, (a), (b) and (c) are respectively FAST-LIO2, LIO-SAM and the mapping method proposed in the present invention, and the right side of the figure are the local enlarged images of the same position of different methods. Due to the lack of global optimization, the map established by FAST-LIO2 has a large error and cannot reflect the real environment; LIO-SAM generally reflects the real environment, but due to trajectory drift, there are still deviations in details. As shown in the box diagram on the right, wall ghosting and multi-layer walls appear on the map. In contrast, the method proposed in the present invention benefits from reliable closed-loop detection and efficient pose graph optimization, and the wall is clear, which truly reflects the environment.

[0059] Step 3: Implement the “continuity-based point cloud segmentation algorithm”.

[0060] The point cloud segmentation algorithm of the present invention emphasizes real-time performance and robustness of ground segmentation. Therefore, a reference traditional fast segmentation algorithm is selected for comparison with the most advanced (State Of The Art, SOTA) ground segmentation algorithm Patchwork++ method.

[0061] According to Algorithm 1, the program is implemented and the result is as follows Fig.11 shown. Fig.11 The left side shows the segmentation results of the same frame of point cloud data using different algorithms. (a) and (b) are the fast segmentation algorithm and Patchwork++, respectively. One part is the unsegmented point cloud, and the other part is the segmented ground point cloud. (c) and (d) are the implementation results of the point cloud segmentation algorithm proposed in this invention, including ceiling point cloud, ground point cloud and wall-like point cloud. The difference is that (d) uses the acquisition time of the point cloud itself. information.

[0062] Fig.11 The right side is a local magnified comparison of the same position of different algorithms. It can be clearly seen that (a) and (b) both have the phenomenon of ground mis-segmentation. The lower edge of some walls and some clutter points are mistakenly segmented as ground points. The algorithm proposed in the present invention robustly segments the ground, removes the cluttered point cloud, and minimizes the ground segmentation error.

[0063] In terms of real-time performance, using a personal computer as the computing platform, the traditional fast segmentation algorithm takes 36.284ms, Patchwork++ takes 2.232ms, and the algorithm proposed in this invention takes 2.268ms, which utilizes the acquisition time of the point cloud itself. After the parameters are adjusted, the algorithm time consumption is reduced to 1.559ms. In addition, in actual tests, the resource usage of the Patchwork++ algorithm is several times that of the algorithm of the present invention, and the algorithm of this paper simultaneously segments multiple types of points, so it has an advantage in real-time performance.

[0064] Step 4: Implement “raster map creation and map filter”.

[0065] Combined with the point cloud segmentation results of Step 3, the ceiling points and unclassified points are eliminated, and the ground point cloud and wall-like point cloud are selected. The point cloud and the pose set obtained previously after global optimization are associated (Equation (11)), and then a 2.5D grid map is established in the global coordinate system (e.g. Figure 4 The points in the ground point cloud and the wall-like point cloud are projected into a 2.5D grid map in 2D, as shown in Figure 5 As shown, the information contained in each grid is calculated according to formula (12). According to formula (12), the attribute corresponding to each grid is judged as ground or wall-like, the grid height information is calculated, and finally the map filter link is performed: first, all ground point clouds and wall-like point clouds are accumulated to obtain the original 3D point cloud map, and the points on the original 3D point cloud map are traversed. If the grid on the corresponding 2.5D grid map meets the threshold condition, it is judged to be the ground, and there should only be ground points on it. Based on this, filtering is performed, as shown in the following formula (13). Only ground information and wall-like information are left in the new 3D point cloud map obtained by filtering, and dynamic obstacles are eliminated. Considering the size of the map storage, voxel downsampling is finally performed.

[0066] After the above process is implemented, the global effect of the map filter is as follows Fig.12 As shown. After applying the map filter, the number of point clouds is greatly reduced at the same resolution, which saves storage space and further improves the real-time performance of the subsequent registration algorithm. On the established 2.5D map, all points on the 3D map are first traversed and filtered, and threshold judgment is performed. If it is judged to be the ground, it is considered that there should only be ground points on it. Based on this, filtering is performed to filter out dynamic points on the ground. Therefore, only ground information and wall-like information are left in the new 3D map. Finally, the system outputs a filtered point cloud map and a 2D accessibility grid map that can be used for planning, and saves the 2.5D grid map for subsequent global alignment and repositioning.

[0067] The local effect of the map filter is as follows Fig.13 As shown, Fig.13 (a) is the map obtained by LIO-SAM. Compared with the method (b) proposed in the present invention, it can be clearly seen that after the map filter, most of the dynamic objects in (b) are eliminated from the 3D point cloud map, which provides a basis for subsequent accurate repositioning and reduces the possibility of mismatching in subsequent frame-map alignment.

[0068] like Fig.14As shown, an embodiment of the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the above-mentioned initial mapping and map filtering method based on 3D lidar is implemented.

[0069] like Fig.15 As shown, an embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the above-mentioned initial mapping and map filtering method based on 3D laser radar is implemented.

[0070] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0071] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0072] The above descriptions are only some embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Any equivalent device or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for initial mapping and map filtering based on 3D laser radar, characterized in that: The steps include: Step 1: preprocess the lidar data and the inertial measurement unit data to obtain input data; Step 2: input the input data into the local odometer module, the loop detection and global optimization module, and the point cloud segmentation and mapping module respectively; Step 3: In the local odometer module, a local map is established, and the registration between the current frame point cloud and the local map is performed according to the input data and the local map, and then the local odometer information is obtained through an iterative error Kalman filter, and the pose of the frame point cloud is associated and accumulated in the local map; Step 4: In the loop detection and global optimization module, a key frame is selected according to the input data and the local odometer information to perform loop detection, and the pose graph is optimized, and the historical pose information is updated according to the local odometer information and the optimized pose graph; Step 5: In the point cloud segmentation and mapping module, perform point cloud segmentation on the input data; And establish the association between the pose and the point cloud based on the updated historical pose information and the segmented point cloud, perform 2D projection on the associated point cloud, and generate a 2.5D grid map; The associated poses and point clouds are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map.

2. The method for initial mapping and map filtering based on 3D laser radar according to claim 1, characterized in that: The step 1 specifically includes: Step 11: aligning and downsampling the laser radar data and the inertial measurement unit data; Step 12: perform pose prediction based on the inertial measurement unit data, and estimate the lidar pose of each point in a frame of point cloud in the lidar data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated lidar pose, and remove the point cloud with motion distortion.

3. The method for initial mapping and map filtering based on 3D laser radar according to claim 1, characterized in that: The step 4 specifically includes: Step 41, obtaining distance, angle and time information according to the local odometer information, performing multi-dimensional analysis according to the distance, angle and time information, and selecting key frames from the input data; Step 42, when a new key frame is added, according to the spatial posture of the new key frame, the nearest historical key frame information is searched within the set radius length; Step 43: If there is a historical key frame, the historical key frame information within a set range near the historical key frame is selected to form a historical loop subgraph; Step 44: Perform ICP registration on the historical loop subgraph and the new key frame, and determine whether it is a real loop based on the registration score; Step 45: If it is a true loop, the corresponding new key frame is added to the pose graph; Step 46, optimizing the pose graph by a pose graph optimizer; Step 47: Update the historical pose information according to the local odometer information and the optimized pose graph.

4. The method for initial mapping and map filtering based on 3D laser radar as claimed in claim 3, characterized in that: The step 47 is specifically as follows: After each pose graph optimization is completed, the pose graph is used to optimize the key frame pose set For the odometry pose set Update as shown below: （1） In the formula, I Represents the IMU coordinate system, IMU stands for inertial measurement unit, G represents the global coordinate system, k Indicates k frame, j Indicates j frame, The pose graph optimization stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Construct the updated pose set ; Represents the local odometer stage k The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the local odometer stage j The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, represents the pose graph optimization stage The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, multiple Constructing pose graph to optimize keyframe pose set .

5. The method for initial mapping and map filtering based on 3D laser radar as claimed in claim 4, characterized in that: In step 5, the input data is segmented into point clouds, which specifically includes: Step 51: Dedistorted point cloud obtained at each moment , which contains Points, is described as a set as follows: （2） in, Indicates frame, Represents the laser radar coordinate system; For collections Each point contained in , which contains the information represented by the following formula: （3） point middle Represents the spatial coordinates corresponding to the point, Indicates the line index corresponding to the point and the acquisition time relative to the starting point; Step 52: Divide the 360° scanning range into regions and set The angle covered by each sector; for each frame point cloud ,get sectors, each of which The index of the corresponding sector is expressed as , obtained by the following formula: （4） If the current laser radar is for each point in the point cloud The sampling time is correct, that is, of The parameters are stable and reliable, and their value range is expressed as ,but Speed ​​up the calculation by: （5） Step 53: For mechanical laser radar, obtain the points of the point cloud With reliable parameter, The parameter indicates the harness in which the point cloud is currently located; Assume that a frame of point cloud has a total of Harness, define parameters For each loop The number of covered bundles, for each frame of the point cloud ,get loops, then each Corresponding loop line The index is represented as , calculated by the following formula: （6） Step 54: Each point in the point cloud Each has its corresponding sector and loop line The index of the definition collection is a set of points with the same index, obtained by the following formula: （7） in, s Indicate point Corresponding sectors , Indicate point The corresponding loop , ∧ indicates the relationship between and ; Assume that a set There are a total of Points, Representing a collection The number of points in ; then it is expressed by the following formula: （8） The points included Also contains the spatial coordinates, the bundle index and the acquisition time relative to the starting point This information, namely , through the following formula (9) Expressed as ,in express exist x - y The distance from the origin on the plane, express of z Axis height; （9） Step 55: Set each point set Corresponding to a space , described as , in each There are multiple representation spaces The parameters of the overall information, where the height information is expressed as , the distance information is expressed as , the height variance is expressed as , the above parameters are calculated according to the following formula: （10） Each space The information is collected by Determined by the average of all points in Step 56: Record each sector simultaneously during preprocessing For all spaces Loop Line The minimum value is , the maximum value is , for each sector Space in Perform sequential calculation and judgment, and realize point cloud segmentation based on the judgment results.

6. The method for initial mapping and map filtering based on 3D laser radar as claimed in claim 5, characterized in that: In step 56, point cloud segmentation is implemented according to the judgment result, specifically including: Step 561: and Make a judgment, if > , then skip the current sector where acquisition failed , enter the next sector Then return to step 561; if ≤ , then proceed to step 562; Step 562: From arrive In order as l Perform the calculation: 1) Calculate the height difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous line The height difference between Indicates the current sector and the current loop The corresponding height, Indicates the current sector And the previous line The corresponding height; 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The distance difference between Indicates the current sector and the current loop The corresponding distance, Indicates the current sector and the previous line The corresponding distance; 3) Calculate the slope: = ; Indicates the current sector and the current loop The corresponding slope; 4) Calculate the slope difference: = ;in, Indicates the current sector and the current loop With the current sector and the previous line The slope difference between Indicates the current sector and the current loop The corresponding slope, Indicates the current sector and the previous line The corresponding slope; Step 563: Determine whether the conditions are met: , , and ,in, Indicates the ground height interval, represents the ground slope threshold, represents the ground slope difference threshold, Indicates the current sector and the current loop The corresponding height variance is, Represents the ground variance threshold; if yes, the corresponding point is put into the ground point cloud ; Otherwise, go to step 564; Step 564: Determine whether the conditions are met: , and ,in, Indicates the ceiling height range, represents the ceiling slope threshold, Indicates the ceiling slope difference threshold; if yes, the corresponding point is put into the ceiling point cloud ; Otherwise, go to step 565; Step 565: Determine whether the conditions are met: and ,in, represents the wall-like slope threshold, Represents the wall-like slope difference threshold; if so, the corresponding point is put into the wall-like point cloud ; Otherwise, skip the current space .

7. The method for initial mapping and map filtering based on 3D laser radar according to claim 6, characterized in that: In the step 5, the association between the pose and the point cloud is established according to the updated historical pose information and the segmented point cloud, and the associated point cloud is 2D projected to generate a 2.5D grid map; specifically, the following steps are included: Step 57: Dedistort the point cloud After segmentation, remove the ceiling point cloud And unclassified points, select the ground point cloud Wall-like point cloud , and the pose set after global optimization The point cloud and pose are associated as shown in the following formula: （11） in, and Represents the global coordinate system The ground point cloud and wall-like point cloud under Indicates the pose graph optimization stage in The position and posture of the IMU in the global coordinate system at the moment of the frame point cloud, Represents the LiDAR pose in the IMU coordinate system, represents the distortion of the laser radar coordinate system Frame ground point cloud, represents the distortion of the laser radar coordinate system Frame wall point cloud; Step 58: In the global coordinate system Next, create a resolution of 2.5D grid map ; Step 59: On the 2.5D grid map middle Indicates that one of the center points is located at And the diameter is The grid will and The points in the 2D projection are projected to In, assuming Included Ground points , and Represent the weights of the ground and wall-like surfaces respectively, and are calculated as follows Information included: （12） In the formula, The ground judgment information determines whether the attribute corresponding to this grid is ground or wall-like. is the grid height information; z Represents a three-dimensional coordinate system The value of the axis.

8. The method for initial mapping and map filtering based on 3D laser radar as claimed in claim 7, characterized in that: In the step 5, the associated posture and point cloud are accumulated to generate an original 3D point cloud map, and the original 3D point cloud map is filtered using the 2.5D grid map to obtain a new 3D point cloud map; specifically, the steps include: Step 510: and Point clouds are accumulated to obtain the original 3D point cloud map ; Step 511: Point on Traverse, if it corresponds to on If the threshold condition is met, it is judged as the ground, and there should be only ground points on it. To filter, is the point cloud map resolution, as shown in the following formula: （13） Filter out the dynamic points on the ground and get a new 3D point cloud map Only ground information and wall-like information are left, and dynamic obstacles are eliminated.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the initial mapping and map filtering method based on 3D lidar as described in any one of claims 1 to 8 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the initial mapping and map filtering method based on 3D laser radar as described in any one of claims 1 to 8 is implemented.

Images