Initial mapping and map filtering methods and equipment based on 3D LiDAR

Abstract

This invention discloses an initial mapping and map filtering method and device medium based on 3D LiDAR, comprising: preprocessing data; inputting input data into a local odometry module, a loop closure detection and global optimization module, and a point cloud segmentation and mapping module; in the local odometry module, performing registration between the current frame point cloud and the local map to obtain local odometry information; in the loop closure detection and global optimization module, selecting key frames to perform loop closure detection, optimizing the pose map and updating it; in the point cloud segmentation and mapping module, segmenting the input data into a point cloud; establishing the association between the pose and the point cloud, projecting the associated point cloud into 2D to generate a 2.5D raster map; accumulating the associated pose and point cloud to generate an original 3D point cloud map, and filtering the original 3D point cloud map using the 2.5D raster map to obtain a new 3D point cloud map. This invention can remove dynamic objects and accurately reflect the environment.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a method and device for initial mapping and map filtering based on 3D LiDAR. Background Technology

[0002] The prerequisite for autonomous mobile robots is their ability to reliably acquire their own pose. However, in real-world working environments, dynamic and time-varying scenarios often impact a robot's localization performance. Currently, the mainstream solution for robot localization and mapping is Simultaneous Localization and Mapping (SLAM), which uses sensors to perceive the surrounding environment and utilizes this information for self-localization and environmental map construction. Since the widespread adoption of 3D LiDAR, various localization and mapping algorithms have emerged. Based on whether they incorporate IMU information, they can be categorized into pure LiDAR methods and fusion methods. Pure LiDAR methods struggle to guarantee localization stability under conditions of vigorous robot movement, and may even experience severe drift. Fusion methods integrate independent high-frequency IMU data, thus ensuring a reasonable level of stability. However, none of these methods specifically address dynamic environments, making it difficult to maintain high accuracy in the localization and mapping process, and therefore failing to meet the requirements for accurate localization and mapping.

[0003] In dynamic environments, numerous moving objects can contaminate the constructed map, interfering with subsequent relocalization. To address the contamination problem of 3D point cloud maps, the typical process involves storing the mapping data and then performing further filtering using post-processing methods. Examples include the Removert, ERASOR, and RF-LIO methods. However, these methods rely on the temporal order of point cloud data for dynamic removal, which is ineffective for removing moving objects. Methods that partially rely on distance-image algorithms place high demands on the 3D LiDAR's beamwidth; if the beamwidth is insufficient, the low resolution often prevents the differentiation between dynamic and static objects during map filtering. Methods relying on occupancy grid algorithms often fail to meet real-time requirements. Therefore, the aforementioned methods often fall short of design requirements in terms of filtering effectiveness or real-time performance.

[0004] To address the low accuracy of traditional mapping methods that do not perform special processing on dynamic objects, this invention proposes a high-precision positioning and mapping system that can eliminate dynamic objects and accurately reflect the environment. Summary of the Invention

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

[0006] To achieve the above-mentioned technical objectives, the technical solution adopted by this invention is as follows:

[0007] This invention provides a method for initial mapping and map filtering based on 3D LiDAR, comprising the following steps:

[0008] Step 1: Preprocess the lidar data and inertial measurement unit data to obtain the input data;

[0009] Step 2: Input the input data into the local odometer module, the loop closure detection and global optimization module, and the point cloud segmentation and mapping module, respectively;

[0010] Step 3: In the local odometry module, a local map is established. Based on the input data and the local map, the registration between the current frame point cloud and the local map is performed. Then, the local odometry information is obtained through the iterative error Kalman filter. The pose of the frame point cloud is associated and accumulated into the local map.

[0011] Step 4: In the loop closure detection and global optimization module, key frames are selected based on the input data and local odometry information to perform loop closure detection, and the pose graph is optimized. Historical pose information is updated based on local odometry information and the optimized pose graph.

[0012] Step 5: In the point cloud segmentation and mapping module, the input data is segmented into a point cloud; and the relationship between pose and point cloud is established based on the updated historical pose information and the segmented point cloud. The associated point cloud is then projected in 2D to generate a 2.5D raster 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 raster map to obtain a new 3D point cloud map.

[0013] Furthermore, step 1 specifically includes:

[0014] Step 11: Align and downsample the lidar data with the inertial measurement unit data;

[0015] Step 12: Perform pose prediction based on inertial measurement unit data, estimate the lidar pose of each point in a frame of point cloud relative to the frame tail pose; determine the motion of the point cloud based on the calculated lidar pose, and remove point clouds with motion distortion.

[0016] Furthermore, step 4 specifically includes:

[0017] Step 41: Obtain distance, angle, and time information based on local odometer information; perform multi-dimensional analysis based on distance, angle, and time information; and select keyframes from the input data.

[0018] Step 42: When a new keyframe is added, search for the nearest historical keyframe information within its set radius length based on the spatial pose of the new keyframe.

[0019] Step 43: If a historical keyframe exists, select historical keyframe information within a set range around the historical keyframe to form a historical loop subgraph.

[0020] Step 44: Perform ICP registration between the historical loop closure sub-graph and the new keyframe, and determine whether it is a true loop closure based on the registration score;

[0021] Step 45: If it is a true loop, add the corresponding new keyframe to the pose graph;

[0022] Step 46: Optimize the pose graph using the pose graph optimizer;

[0023] Step 47: Update the historical pose information based on the local odometry information and the optimized pose graph.

[0024] Furthermore, step 47 specifically includes:

[0025] After each pose graph optimization, the keyframe pose set is optimized using the pose graph. odometry pose set The update is performed as shown in the following formula:

[0026] (1)

[0027] In the formula, I This indicates the IMU coordinate system, where IMU stands for Inertial Measurement Unit. G Represents the global coordinate system. k Indicates the first k frame, j Indicates the first j frame, For the pose graph optimization stage k The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Construct the updated pose set ; Indicates the local odometer stage. k The pose of the IMU in the global coordinate system at each frame point cloud moment. Indicates the local odometer stage. j The pose of the IMU in the global coordinate system at each frame point cloud moment. This indicates the first stage of pose graph optimization. The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Constructing a pose graph optimization keyframe pose set .

[0028] Furthermore, step 5 involves point cloud segmentation of the input data, specifically including:

[0029] Step 51, for in Real-time obtained distortion-free point cloud It contains One point, It is described as a set, as shown in the following formula:

[0030] (2)

[0031] in, Indicates the first frame, Indicates the coordinate system of the lidar;

[0032] For sets Each point included The information contained therein is represented by the following formula:

[0033] (3)

[0034] point middle This represents the spatial coordinates of the point. This indicates the harness index corresponding to the point and the acquisition time relative to the starting point;

[0035] Step 52: Divide the 360° scanning range into regions and set... The angle covered for each sector; for each frame of point cloud. ,get If there are 1 sector, then each of them The index of the corresponding sector is represented as It is obtained from the following formula:

[0036] (4)

[0037] If the current lidar has a view of each point in the point cloud The sampling time is correct, that is... of The parameters are stable and reliable, and their numerical range is expressed as follows: ,but Accelerate calculations using the following methods:

[0038] (5)

[0039] Step 53: For mechanical lidar, obtain the point cloud points. With reliable parameter, The parameter represents the current bundle in which the point cloud is located;

[0040] Assuming a point cloud frame has a total of One wire harness, define parameters For each loop The number of covered wire bundles for each frame of point cloud. ,get If there are several loops, then each of them... Corresponding loop The index is represented as Calculated by the following formula:

[0041] (6)

[0042] Step 54: Each point in the point cloud Each has its corresponding sector and loop Index, define set For the set of points with the same index, we have the following formula:

[0043] (7)

[0044] in, s Point corresponding sector , Point Corresponding loop , ∧ indicates a relationship with;

[0045] Suppose a certain set There are a total of One point, Represents a set The number of points existing in the matrix is ​​represented by the following formula:

[0046] (8)

[0047] Points included It also includes spatial coordinates, harness index, and acquisition time relative to the starting point.

[0048] This information, namely The following formula (9) is used to... Represented as ,in express exist x - y The distance from the origin on the plane. express of z Shaft height;

[0049] (9)

[0050] Step 55: Define the set of each point Corresponding to a space , described as In each Multiple representation spaces exist in the middle. The parameters of the overall information, where the height information is expressed as: Distance information is expressed as The height variance is expressed as The above parameters are calculated according to the following formula:

[0051] (10)

[0052] Each space All information is from a set It is determined by the average value of all points;

[0053] Step 56: Record each sector simultaneously during the preprocessing process. Corresponding to all spaces loop The minimum value is The maximum value is For each sector space Perform sequential calculations and judgments, and segment the point cloud based on the judgment results.

[0054] Furthermore, step 56, which involves segmenting the point cloud based on the judgment result, specifically includes:

[0055] Step 561, for and Make a judgment, if > If the current sector fails to acquire data, then skip that sector. Enter the next sector Then return to step 561; if ≤ Then proceed to step 562;

[0056] Step 562, from arrive In order of priority l Perform the calculation:

[0057] 1) Calculate the height difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The height difference between them Indicates the current sector and the current loop The corresponding height, Indicates the current sector And the previous ring road Corresponding height;

[0058] 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The distance difference between them Indicates the current sector and the current loop The corresponding distance, Indicates the current sector And the previous ring road The corresponding distance;

[0059] 3) Calculate the slope: = ; Indicates the current sector and the current loop The corresponding slope;

[0060] 4) Calculate the slope difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The difference in slope between them Indicates the current sector and the current loop The corresponding slope, Indicates the current sector And the previous ring road The corresponding slope;

[0061] Step 563: Determine if the conditions are met: , , and ,in, Indicates the range of ground elevation. Indicates the ground slope threshold. This represents the threshold for the difference in ground slope. Indicates the current sector and the current loop The corresponding height variance, This indicates the ground variance threshold; if so, the corresponding point is added to the ground point cloud. Otherwise, proceed to step 564;

[0062] Step 564: Determine if the conditions are met: , and ,in, Indicates the ceiling height range. Indicates the ceiling slope threshold. This represents the ceiling slope difference threshold; if so, the corresponding point is added to the ceiling point cloud. Otherwise, proceed to step 565;

[0063] Step 565: Determine if the conditions are met: and ,in, Indicates the slope threshold of the wall surface. This represents the threshold for the slope difference of the wall-like surfaces; if so, the corresponding point is plotted into the wall-like surface point cloud. Otherwise, skip the current space. .

[0064] Furthermore, in step 5, the association between pose and point cloud is established based on the updated historical pose information and the segmented point cloud. The associated point cloud is then 2D projected to generate a 2.5D raster map; specifically, this includes:

[0065] Step 57: In the distortion-removed point cloud After segmentation, the ceiling cloud pattern was removed. Among the unclassified points, select the ground point cloud. Cloud-like patterns on walls , and the pose set after global optimization The association between point cloud and pose is shown in the following equation:

[0066] (11)

[0067] in, and Representing the global coordinate system Ground-based point clouds and wall-like point clouds, This indicates that the pose graph optimization stage is in the first stage. The pose of the IMU in the global coordinate system at each frame point cloud moment. This represents the lidar pose in the IMU coordinate system. The first term represents the distortion of the lidar coordinate system. Frame ground point cloud, The first term represents the distortion of the lidar coordinate system. Frame-like wall point clouds;

[0068] Step 58: In the global coordinate system Let's establish a resolution of 2.5D raster map ;

[0069] Step 59: In the 2.5D raster map middle This indicates that one of the center points is located at And the diameter is The grid will and Projecting the points in the middle into 2D In the middle, assuming It includes ground points , and The weights of the ground and wall-like surfaces are respectively represented by the following formulas. Information included:

[0070] (12)

[0071] In the formula, The ground-based criteria determine whether the attribute corresponding to this grid cell is ground or wall-like. This refers to grid height information; z Representing a three-dimensional coordinate system The value of the axis.

[0072] Furthermore, in step 5, the associated pose and point cloud are accumulated to generate an original 3D point cloud map, and a new 3D point cloud map is obtained by filtering the original 3D point cloud map using a 2.5D raster map; specifically including:

[0073] Step 510, for all and Accumulate point clouds to obtain the original 3D point cloud map. ;

[0074] Step 511, for Points on Perform a traversal, if its corresponding On If the threshold condition is met, it is determined to be the ground, and there should only be ground points on it. This is the basis for further analysis. Perform filtering. The resolution of the point cloud map is shown in the following formula:

[0075] (13)

[0076] The new 3D point cloud map is obtained by filtering out dynamic points on the ground. Only ground information and wall-like information were retained, and dynamic obstacles were removed.

[0077] 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 the processor executes the program to implement the initial mapping and map filtering method based on 3D LiDAR as described above.

[0078] 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-described method for initial mapping and map filtering based on 3D LiDAR.

[0079] By adopting the above technical solution, the present invention has the following beneficial effects compared with the prior art:

[0080] This invention includes a local odometry module, a loop closure detection and global optimization module, and a point cloud segmentation and mapping module. The local odometry module is used to calculate and acquire local odometry. The loop closure detection and global optimization module is used for loop closure detection and global optimization. The point cloud segmentation and mapping module is used for point cloud segmentation and mapping. The loop closure detection and global optimization module achieves reliable loop closure detection and efficient pose map optimization, resulting in clear walls and a realistic representation of the environment. A continuous cloud segmentation algorithm robustly segments the ground, removing cluttered point clouds and minimizing ground segmentation errors. Through raster map creation and map filtering, the resulting 3D point cloud map retains only ground and wall-like information, eliminating dynamic obstacles and thus accurately reflecting the environment, solving the problem of residual dynamic objects in the point cloud map. Attached Figure Description

[0081] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0082] Figure 1 This is an execution flowchart of an initial mapping and map filtering method based on 3D LiDAR provided in an embodiment of the present invention.

[0083] Figure 2 This is a schematic diagram of pose graph optimization provided in an embodiment of the present invention.

[0084] Figure 3 This is a schematic diagram of point cloud segmentation provided in an embodiment of the present invention.

[0085] Figure 4 This is a schematic diagram of a 2.5D grid map provided in an embodiment of the present invention.

[0086] Figure 5 This is a schematic diagram of grid calculation provided in an embodiment of the present invention.

[0087] Figure 6 This is a structural diagram of the experimental verification platform provided in the embodiments of the present invention.

[0088] Figure 7 This is a schematic diagram of the experimental verification environment provided in the embodiments of the present invention.

[0089] Figure 8 These are the trajectory top view and z-axis diagram provided in the embodiments of the present invention.

[0090] Figure 9 This is a comparison chart of ground height errors provided in an embodiment of the present invention.

[0091] Figure 10 These are comparison images of the mapping effects provided in the embodiments of the present invention.

[0092] Figure 11 This is a schematic diagram of the point cloud segmentation experimental results provided in an embodiment of the present invention.

[0093] Figure 12 This is a global effect diagram of the map filter provided in an embodiment of the present invention.

[0094] Figure 13 This is a partial effect diagram of the map filter provided in an embodiment of the present invention.

[0095] Figure 14 This is a schematic diagram of an electronic device provided in an embodiment of the present invention.

[0096] Figure 15 This is a schematic diagram of a computer-readable storage medium provided in an embodiment of the present invention. Detailed Implementation

[0097] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be particularly noted that the following embodiments are for illustrative purposes only and do not limit the scope of the invention. Similarly, the following embodiments are only some, not all, embodiments of the present invention, and all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0098] Please see Figure 1 The present invention provides an initial mapping and map filtering method based on 3D LiDAR, comprising the following steps:

[0099] Step 1: Preprocess the lidar data and inertial measurement unit (IMU) data to obtain the input data;

[0100] In this embodiment, step 1 specifically includes:

[0101] Step 11: Align and downsample the lidar data with the inertial measurement unit data;

[0102] Step 12: Based on the inertial measurement unit (IMU) data, perform pose prediction to estimate the LiDAR pose of each point in a frame of point cloud data relative to the frame's end pose. Determine the point cloud motion based on the calculated LiDAR pose and remove motion distortion from the point cloud. Simultaneously, the sampling order of each point is obtained from its precise sampling time. Therefore, the points in the LiDAR frame point cloud can be considered as having been sampled simultaneously at the end of the frame, thus completing the point cloud motion distortion removal.

[0103] Step 2: Input the input data into the local odometer module, the loop closure detection and global optimization module, and the point cloud segmentation and mapping module, respectively;

[0104] Step 3: In the local odometry module, a local map is established. Based on the input data and the local map, the registration between the current frame point cloud and the local map is performed. Then, the local odometry information is obtained through the iterative error Kalman filter. The pose of the frame point cloud is associated and accumulated into the local map.

[0105] In this embodiment, step 3 involves performing registration between the current frame point cloud and the local map based on the input data and the local map. Specifically, this involves calculating the point-area residual between the current frame point cloud and the local map and performing registration.

[0106] Step 4: In the loop closure detection and global optimization module, key frames are selected based on the input data and local odometry information to perform loop closure detection and optimize the pose graph. Historical pose information is updated based on local odometry information and the optimized pose graph. Based on local odometry, loop closure detection and global optimization are further introduced to correct the pose graph, so that the odometry pose is updated synchronously and the accuracy of mapping is improved.

[0107] In this embodiment, step 4 specifically includes:

[0108] Step 41: Obtain distance, angle, and time information based on local odometer information; perform multi-dimensional analysis based on distance, angle, and time information; and select keyframes from the input data.

[0109] Step 42: When a new keyframe is added, search for the nearest historical keyframe information within its set radius length based on the spatial pose of the new keyframe.

[0110] Step 43: If a historical keyframe exists, select historical keyframe information within a set range around the historical keyframe to form a historical loop subgraph.

[0111] Step 44: Perform ICP registration between the historical loop closure sub-image and the new keyframe (ICP (Iterative ClosestPoint) registration is a commonly used point cloud registration technique, mainly used to accurately align two point cloud data sets), and determine whether it is a true loop closure based on the registration score;

[0112] Step 45: If it is a true loop, add the corresponding new keyframe to the pose graph;

[0113] Step 46: Optimize the pose graph using the pose graph optimizer;

[0114] Step 47: Update the historical pose information based on the local odometry information and the optimized pose graph.

[0115] In this embodiment, step 47 specifically includes:

[0116] Data transfer between local odometry and global pose graph optimization, such as Figure 2 As shown, Figure 2 In the tilde T, the odometry pose set is represented. Represents a point cloud map. Represents the constraint factor (indicated by relative pose); This represents data from the partial odometer phase, while This represents the data during the pose graph optimization phase.

[0117] After each pose graph optimization, the keyframe pose set is optimized using the pose graph. odometry pose set The update is performed as shown in the following formula:

[0118] (1)

[0119] In the formula, I This indicates the IMU coordinate system, where IMU stands for Inertial Measurement Unit. G Represents the global coordinate system. k Indicates the first k frame, j Indicates the first j frame, For the pose graph optimization stage k The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Construct the updated pose set ; Indicates the local odometer stage.k The pose of the IMU in the global coordinate system at each frame point cloud moment. Indicates the local odometer stage. j The pose of the IMU in the global coordinate system at each frame point cloud moment. This indicates the first stage of pose graph optimization. The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Constructing a pose graph optimization keyframe pose set .

[0120] Step 5: In the point cloud segmentation and mapping module, the input data is segmented into a point cloud; and the relationship between pose and point cloud is established based on the updated historical pose information and the segmented point cloud. The associated point cloud is then projected in 2D to generate a 2.5D raster 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 raster map to obtain a new 3D point cloud map.

[0121] In this embodiment, step 5, which involves point cloud segmentation of the input data, employs a continuous point cloud segmentation algorithm; specifically, it includes:

[0122] Step 51, for in Real-time obtained distortion-free point cloud It contains One point, It is described as a set, as shown in the following formula:

[0123] (2)

[0124] in, Indicates the first frame, Indicates the coordinate system of the lidar;

[0125] For sets Each point included The information contained therein is represented by the following formula:

[0126] (3)

[0127] point middle This represents the spatial coordinates of the point. The data represents the line bundle index corresponding to the point and the acquisition time relative to the starting point. The point cloud segmentation method designed in this invention, combined with the point cloud's own parameters for optimization, can significantly improve the efficiency and accuracy of point cloud segmentation.

[0128] Step 52: The mechanical lidar performs a 360° all-around scan of the environment. This invention also defines sectors. The concept involves dividing the 360° scanning range into regions and setting... The angle covered for each sector; for each frame of point cloud. ,get If there are 1 sector, then each of them The index of the corresponding sector is represented as It is obtained from the following formula:

[0129] (4)

[0130] If the current lidar has a view of each point in the point cloud The sampling time is correct, that is... of The parameters are stable and reliable, and their numerical range is expressed as follows: ,but Accelerate calculations using the following methods:

[0131] (5)

[0132] This point cloud segmentation method extends 2D projection to 2.5D by utilizing the continuity relationship between 3D point cloud bundles.

[0133] Step 53: For mechanical lidar, obtain the point cloud points. With reliable parameter, The parameter represents the current bundle in which the point cloud is located;

[0134] Assuming a point cloud frame has a total of One wire harness, define parameters For each loop The number of covered wire bundles for each frame of point cloud. ,get If there are several loops, then each of them... Corresponding loop The index is represented as Calculated by the following formula:

[0135] (6)

[0136] Step 54: Each point in the point cloud Each has its corresponding sector and loop Index, define set For the set of points with the same index, we have the following formula:

[0137] (7)

[0138] in, sPoint corresponding sector , Point Corresponding loop , ∧ indicates a relationship with;

[0139] Suppose a certain set There are a total of One point, Represents a set The number of points existing in the matrix is ​​represented by the following formula:

[0140] (8)

[0141] Points included It also includes spatial coordinates, harness index, and acquisition time relative to the starting point.

[0142] This information, namely The following formula (9) is used to... Represented as ,in express exist x - y The distance from the origin on the plane. express of z Shaft height;

[0143] (9)

[0144] Step 55: Define the set of each point Corresponding to a space , described as In each Multiple representation spaces exist in the middle. The parameters of the overall information, where the height information is expressed as: Distance information is expressed as The height variance is expressed as The above parameters are calculated according to the following formula:

[0145] (10)

[0146] Each space All information is from a set The average value of all points is used to determine the value, which avoids the limitation of the minimum value.

[0147] Step 56: In real-world environments, point cloud acquisition often involves a range issue; therefore, each sector is recorded simultaneously during preprocessing. Corresponding to all spaces loop The minimum value is The maximum value is For each sector space Perform sequential calculations and judgments, and segment the point cloud based on the judgment results.

[0148] In this embodiment, step 56 involves point cloud segmentation based on the judgment result, rapidly segmenting the point cloud according to its characteristics, and identifying ground point clouds and wall-like point clouds; specifically including:

[0149] Step 561, for and Make a judgment, if > If the current sector fails to acquire data, then skip that sector. Enter the next sector Then return to step 561; if ≤ Then proceed to step 562;

[0150] Step 562, from arrive In order of priority l Perform the calculation:

[0151] 1) Calculate the height difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The height difference between them Indicates the current sector and the current loop The corresponding height, Indicates the current sector And the previous ring road Corresponding height;

[0152] 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The distance difference between them Indicates the current sector and the current loop The corresponding distance, Indicates the current sector And the previous ring road The corresponding distance;

[0153] 3) Calculate the slope: = ; Indicates the current sector and the current loop The corresponding slope;

[0154] 4) Calculate the slope difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The difference in slope between them Indicates the current sector and the current loop The corresponding slope, Indicates the current sector And the previous ring road The corresponding slope;

[0155] Step 563: Determine if the conditions are met: , , and ,in, Indicates the range of ground elevation. Indicates the ground slope threshold. This represents the threshold for the difference in ground slope. Indicates the current sector and the current loop The corresponding height variance, This indicates the ground variance threshold; if so, the corresponding point is added to the ground point cloud. Otherwise, proceed to step 564;

[0156] Step 564: Determine if the conditions are met: , and ,in, Indicates the ceiling height range. Indicates the ceiling slope threshold. This represents the ceiling slope difference threshold; if so, the corresponding point is added to the ceiling point cloud. Otherwise, proceed to step 565;

[0157] Step 565: Determine if the conditions are met: and ,in, Indicates the slope threshold of the wall surface. This represents the threshold for the slope difference of the wall-like surfaces; if so, the corresponding point is plotted into the wall-like surface point cloud. Otherwise, skip the current space. .

[0158] As shown in Algorithm 1

[0159]

[0160] Algorithm 1 first performs... and Perform a judgment and skip the sectors that failed to be acquired. , and then from In the beginning, such as Figure 3 As shown, Figure 3 (a) shows a subset of points in a frame of point cloud information, with the points ranging from -0.5m to 2.5m along the z-axis colored in iridescent hues. (b) displays point cloud information as seen from a lidar viewpoint, illustrating 3D points and space. The relationship between them. Points between the two perpendicular dashed lines all satisfy the condition. Similarly, all points within the two horizontal dashed lines satisfy the condition. A space is defined by satisfying both conditions. In the diagram, this is represented by a box. In (c), the following conditions will be met: All spaces of conditions Projected to Coordinate systems. Squares, circles, triangles, and hexagons all represent different spatial dimensions. Solid lines represent the slope between different spaces. Different shapes indicate different judgment results: squares represent the ground, hexagons represent walls, triangles represent the ceiling, and circles represent spaces that are discarded. Because it does not meet any of the judgment conditions.

[0161] At the actual algorithm level, each space Firstly, in relation to the previous space The height difference is obtained by performing difference calculation. and distance difference And use this to calculate the slope ,as well as and The slope difference between This indicates the smoothness between the loops. Then, a threshold judgment is applied to all previous calculations. For the ground and ceiling, the default pose operation is in an environment without steep slopes, first based on... The height is determined, and the condition is only met within a specific range; then the absolute slope is determined. To make a judgment, the slope difference must be less than a certain threshold compared to the robot's current horizontal pose; finally, the relative slope difference is considered. To determine whether a surface is flat, a flatness level below a certain value is required. For the ground surface, to ensure robustness, its internal height variance was also assessed. The distinction is made by comparing the absolute slope and the relative slope of wall-type points after inverting the slope.

[0162] In this embodiment, step 5 establishes a relationship between pose and point cloud based on the updated historical pose information and the segmented point cloud, and then performs 2D projection on the associated point cloud to generate a 2.5D raster map; specifically, it includes:

[0163] Step 57: In the distortion-removed point cloud After segmentation, the ceiling cloud pattern was removed. Among the unclassified points, select the ground point cloud. Cloud-like patterns on walls , and the pose set after global optimization The association between point cloud and pose is shown in the following equation:

[0164] (11)

[0165] in, and Representing the global coordinate system Ground-based point clouds and wall-like point clouds, This indicates that the pose graph optimization stage is in the first stage. The pose of the IMU in the global coordinate system at each frame point cloud moment. This represents the lidar pose in the IMU coordinate system. The first term represents the distortion of the lidar coordinate system. Frame ground point cloud, The first term represents the distortion of the lidar coordinate system. Frame-like wall point clouds;

[0166] Step 58: In the global coordinate system Let's establish a resolution of 2.5D raster map ;like Figure 4 As shown, its multidimensional and easily addressable characteristics reduce the complexity of the system.

[0167] Step 59: In the 2.5D raster map middle This indicates that one of the center points is located at And the diameter is The grid will and Projecting the points in the middle into 2D In the middle, assuming It includes ground points , and The weights of the ground and wall-like surfaces are respectively represented by the following formulas. Information included:

[0168] (12)

[0169] In the formula, The ground-based criteria determine whether the attribute corresponding to this grid cell is ground or wall-like. This refers to grid height information; z Representing a three-dimensional coordinate system The value of the axis.

[0170] Raster map illustration as follows Figure 5 As shown, Figure 5 (a) shows the segmented point cloud from a 3D perspective. and The varying shades of the ground symbolize the 2.5D grid map. middle The numerical values ​​are high or low. (b) A portion of the ground in (a) is cropped from a 2D top-down view. (c) A magnified view of a grid enclosed by the box in (b), which contains... ground points and Types of wall spots In the picture , .

[0171] In this embodiment, step 5 involves accumulating the associated pose and point cloud to generate an original 3D point cloud map, and then filtering the original 3D point cloud map using a 2.5D raster map to obtain a new 3D point cloud map; specifically, this includes:

[0172] Step 510, for all and Accumulate point clouds to obtain the original 3D point cloud map. ;

[0173] Step 511, for Points on Perform a traversal, if its corresponding On If the threshold condition (<0) is met, it is determined to be the ground, and there should only be ground points on it. This is the basis for further analysis. Filtering is performed (using a map filter). The resolution of the point cloud map is shown in the following formula:

[0174] (13)

[0175] The new 3D point cloud map is obtained by filtering out dynamic points on the ground. Only ground and wall-like surface information is retained, eliminating dynamic obstacles, thus accurately reflecting the environment and resolving the issue of residual dynamic objects in point cloud maps. Considering map storage size, the final resolution is [not specified]. right Perform voxel downsampling.

[0176] Example 1:

[0177] This invention presents an initial mapping and map filtering method based on 3D LiDAR, used for navigation, localization, and mapping of mobile robots in dynamic environments. The specific implementation process is as follows:

[0178] Step 1: Set up the experimental platform and environment.

[0179] like Figure 6 As shown, based on the Scout-mini mobile robot platform, it is equipped with a multi-line LiDAR 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), and a personal computer (AMD R7-4800H, ROS Noetic).

[0180] The experimental verification environment was a large indoor office, similar to the working environment of an autonomous mobile robot. Figure 7 As shown, Figure 7 (a) shows the top view of the office floor plan, with the dot indicating the starting point for mapping. (b) shows the actual environment, corresponding to locations I to VI in (a), including complex environments such as offices, long corridors, staircases, and narrow passages.

[0181] Step 2: Implement "LiDAR / IMU local odometry" and "loop closure detection and pose graph optimization".

[0182] This paper implements a SLAM algorithm that includes loop closure detection and global optimization, and compares it with state-of-the-art (SOTA) algorithms FAST-LIO2 and LIO-SAM. The key to high accuracy in robot mapping lies in the trajectory (localization accuracy) during the mapping phase, which is crucial in real-world environments. Figure 7 In the example, starting from the starting point, the trajectory circle the office once and eventually returns to the starting point. The trajectory diagrams and z-axis height diagrams obtained by different methods are shown below. Figure 8As shown, within the solid-lined boxes in the trajectory diagram, the lack of global optimization prevents the use of loop closure constraints, causing the FAST-LIO2 trajectory (circular line segments) to fail to close at the starting point, resulting in significant errors. Within the dashed-lined boxes, the robot makes a sharp turn before a narrow passage; the sparse point cloud causes the LIO-SAM (square line segments) factor map optimization to fail, leading to pose drift. In contrast, from an aerial view, the trajectory obtained by the SLAM algorithm (slashed line segments) proposed in this invention is globally closed and exhibits no drift throughout.

[0183] Besides the drift problem, LIO-SAM also suffers from significant ground height estimation errors. This is because it extracts features from the point cloud, and feature-based laser odometry loses some constraints. Figure 8 Looking at the z-axis diagram, the maximum height difference of LIO-SAM reaches 2.35 meters, while the height difference of the method proposed in this invention is only 1.24 meters, which is about half that of the LIO-SAM method.

[0184] To visually display the z-axis height error, a progressive map filtering algorithm was used to extract the ground plane from both LIO-SAM and the map constructed using the method of this invention, as shown below. Figure 9 As shown, Figure 9 In the image (a), the ground surface of the map generated by LIO-SAM is shown, and in the image (b), the ground surface of the map generated by the method of the present invention is shown. The Z-axis height represents the height from -1 meter to 2 meters. It can be seen that the ground height variation of the method of the present invention is more uniform and the range is smaller.

[0185] Based on the trajectories obtained by the three methods, the corresponding maps can be obtained by accumulating point clouds, such as... Figure 10 As shown, (a), (b), and (c) represent the mapping methods of FAST-LIO2, LIO-SAM, and the proposed method, respectively. The right side of the figure shows magnified views of the same location using different methods. Due to the lack of global optimization, the map built by FAST-LIO2 has significant errors and cannot reflect the real environment. LIO-SAM generally reflects the real environment, but due to trajectory drift, there are still deviations in details, such as ghosting of walls and multiple layers of walls, as shown in the right-hand diagram. In contrast, the proposed method benefits from reliable loop closure detection and efficient pose graph optimization, resulting in clear walls and a true reflection of the environment.

[0186] Step 3: Implement the "continuous point cloud segmentation algorithm".

[0187] The point cloud segmentation algorithm of this invention emphasizes real-time performance and robustness of ground segmentation. Therefore, it compares the traditional fast segmentation algorithm with the state-of-the-art (SOTA) ground segmentation algorithm Patchwork++.

[0188] Implement the algorithm according to Algorithm 1, and the result is as follows: Figure 11 As shown. Figure 11 The left side shows the segmentation results of the same frame of point cloud data by 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 point cloud's built-in acquisition time. information.

[0189] Figure 11 The right side shows a magnified comparison of the same location using different algorithms. It can be clearly seen that both (a) and (b) exhibit ground missegmentation, with some lower edges of the walls and some noisy points being incorrectly segmented as ground points. In contrast, the algorithm proposed in this invention robustly segments the ground, removes cluttered point clouds, and minimizes ground segmentation error.

[0190] 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, utilizing the point cloud's own acquisition time. After parameter adjustments, the algorithm's execution time was reduced to 1.559ms. Furthermore, in actual testing, the Patchwork++ algorithm consumed several times more resources than the algorithm in this invention, and the algorithm in this paper simultaneously segments multiple types of points, thus exhibiting advantages in real-time performance.

[0191] Step 4: Implement "Raster Map Creation and Map Filtering".

[0192] Based on the point cloud segmentation results from Step 3, ceiling points and unclassified points are removed, and ground point clouds and wall-like point clouds are selected. These are then associated with the previously obtained globally optimized pose set (Equation (11)). A 2.5D raster map (e.g., ...) is then built in the global coordinate system. Figure 4 (As shown); project the points in the ground point cloud and wall-like point cloud into a 2.5D raster map using 2D projection, such as... Figure 5As shown, the information contained in each grid is calculated according to Equation (12). Based on Equation (12), it is determined whether the attribute corresponding to each grid is ground or wall-like, the grid height information is calculated, and finally the map filtering step is performed: First, all ground point clouds and wall-like point clouds are accumulated to obtain the original 3D point cloud map. 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 as ground, and only ground points should exist on it. Based on this, the filtering is performed as shown in Equation (13) below. The new 3D point cloud map obtained by filtering only retains ground information and wall-like information, and dynamic obstacles are removed. Considering the size of map storage, voxel downsampling is finally performed.

[0193] After the above process is implemented in code, the global effect of the map filter is as follows: Figure 12 As shown, after applying the map filter, the number of point clouds is greatly reduced at the same resolution, saving storage space and further improving the real-time performance of subsequent registration algorithms. On the established 2.5D map, all points on the 3D map are first traversed and filtered, and a threshold judgment is performed. If a point is determined to be ground, it is assumed that only ground points exist on it, and filtering is performed accordingly to remove dynamic points on the ground. Therefore, the new 3D map only retains ground information and wall-like surface information. Finally, the system outputs a filtered point cloud map and a 2D accessibility raster map for planning purposes, while also saving the 2.5D raster map for subsequent global alignment and relocation.

[0194] A partial view of the map filter is shown below. Figure 13 As shown, with Figure 13 In the middle (a), the map obtained by LIO-SAM is compared with the method (b) proposed in this invention. It can be clearly seen that after the map filter, most of the dynamic objects in (b) are removed from the 3D point cloud map, which provides a basis for accurate relocalization and reduces the possibility of mismatch in subsequent frame-map registration.

[0195] like Figure 14 As shown, this embodiment of the invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the above-described initial mapping and map filtering method based on 3D LiDAR.

[0196] like Figure 15 As shown, this embodiment of the invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described initial mapping and map filtering method based on 3D LiDAR.

[0197] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0198] If the integrated unit is implemented as 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 this 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. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods of various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0199] The above description is only a part of the embodiments of the present invention and does not limit the scope of protection of the present invention. Any equivalent device or equivalent process transformation made based on the content of the present invention specification and drawings, or direct or indirect application in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A method for initial mapping and map filtering based on 3D LiDAR, characterized in that, Includes the following steps: Step 1: Preprocess the lidar data and inertial measurement unit data to obtain the input data; Step 2: Input the input data into the local odometer module, the loop closure detection and global optimization module, and the point cloud segmentation and mapping module, respectively; Step 3: In the local odometry module, a local map is established. Based on the input data and the local map, the registration between the current frame point cloud and the local map is performed. Then, the local odometry information is obtained through the iterative error Kalman filter. The pose of the frame point cloud is associated and accumulated into the local map. Step 4: In the loop closure detection and global optimization module, key frames are selected based on the input data and local odometry information to perform loop closure detection, and the pose graph is optimized. Historical pose information is updated based on local odometry information and the optimized pose graph. Step 5: In the point cloud segmentation and mapping module, the input data is segmented into a point cloud; Based on the updated historical pose information and the segmented point cloud, the pose and point cloud are associated, and the associated point cloud is projected in 2D to generate a 2.5D raster map. The associated poses and point clouds are accumulated to generate an original 3D point cloud map, and a new 3D point cloud map is obtained by filtering the original 3D point cloud map using a 2.5D raster map; wherein, the point cloud segmentation of the input data specifically includes: Step 51, for in Real-time obtained distortion-free point cloud It contains One point, It is described as a set; Step 52: Divide the 360° scanning range into regions and set... The angle covered for each sector; for each frame of point cloud. ,get If there are 1 sector, then each of them The index of the corresponding sector is represented as ; Step 53: For mechanical lidar, obtain the point cloud points. With reliable parameter, The parameter represents the current bundle in which the point cloud is located; Step 54: Each point in the point cloud Each has its corresponding sector and loop index, define set A set of points with the same index; Step 55: Define the set of each point Corresponding to a space , described as In each Multiple representation spaces exist in the middle. The parameters of the overall information, where the height information is expressed as: Distance information is expressed as The height variance is expressed as ; Step 56: Record each sector simultaneously during the preprocessing process. Corresponding to all spaces loop The minimum value is The maximum value is For each sector space Perform sequential calculations and judgments, and segment the point cloud based on the judgment results.

2. The initial mapping and map filtering method based on 3D LiDAR as described in claim 1, characterized in that, Step 1 specifically includes: Step 11: Align and downsample the lidar data with the inertial measurement unit data; Step 12: Perform pose prediction based on inertial measurement unit data, estimate the lidar pose of each point in a frame of point cloud relative to the frame tail pose; determine the motion of the point cloud based on the calculated lidar pose, and remove point clouds with motion distortion.

3. The initial mapping and map filtering method based on 3D LiDAR as described in claim 1, characterized in that, Step 4 specifically includes: Step 41: Obtain distance, angle, and time information based on local odometer information; perform multi-dimensional analysis based on distance, angle, and time information; and select keyframes from the input data. Step 42: When a new keyframe is added, search for the nearest historical keyframe information within its set radius length based on the spatial pose of the new keyframe. Step 43: If a historical keyframe exists, select historical keyframe information within a set range around the historical keyframe to form a historical loop subgraph. Step 44: Perform ICP registration between the historical loop closure sub-graph and the new keyframe, and determine whether it is a true loop closure based on the registration score; Step 45: If it is a true loop, add the corresponding new keyframe to the pose graph; Step 46: Optimize the pose graph using the pose graph optimizer; Step 47: Update the historical pose information based on the local odometry information and the optimized pose graph.

4. The initial mapping and map filtering method based on 3D LiDAR as described in claim 3, characterized in that, Step 47 specifically involves: After each pose graph optimization, the keyframe pose set is optimized using the pose graph. odometry pose set The update is performed as shown in the following formula: （1） In the formula, I This indicates the IMU coordinate system, where IMU stands for Inertial Measurement Unit. G Represents the global coordinate system. k Indicates the first k frame, j Indicates the first j frame, For the pose graph optimization stage k The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Construct the updated pose set ; Indicates the local odometer stage. k The pose of the IMU in the global coordinate system at each frame point cloud moment. Indicates the local odometer stage. j The pose of the IMU in the global coordinate system at each frame point cloud moment. This indicates the first stage of pose graph optimization. The pose of the IMU in the global coordinate system at each frame point cloud moment, multiple Constructing a pose graph optimization keyframe pose set .

5. The initial mapping and map filtering method based on 3D LiDAR as described in claim 1, characterized in that, The set in step 51 As shown in the following formula: （2） in, Indicates the first frame, Indicates the coordinate system of the lidar; For sets Each point included The information contained therein is represented by the following formula: （3） point middle This represents the spatial coordinates of the point. This indicates the harness index corresponding to the point and the acquisition time relative to the starting point; In step 52 Obtained from the following formula: （4） If the current lidar has a view of each point in the point cloud The sampling time is correct, that is... of The parameters are stable and reliable, and their numerical range is expressed as follows: ,but Accelerate calculations using the following methods: （5） Step 53 specifically includes: assuming that a point cloud frame has a total of One wire harness, define parameters For each loop The number of covered wire bundles for each frame of point cloud. ,get If there are several loops, then each of them... Corresponding loop The index is represented as Calculated by the following formula: （6） The set in step 54 From the following formula, we get: （7） in, s Point corresponding sector , Point Corresponding loop , ∧ indicates a relationship with; Suppose a certain set There are a total of One point, Represents a set The number of points existing in the matrix is ​​represented by the following formula: （8） Points included It also includes spatial coordinates, harness index, and acquisition time relative to the starting point. This information, namely The following formula (9) is used to... Represented as ,in express exist x - y The distance from the origin on the plane. express of z Shaft height; （9） In step 55, the height information is calculated according to the following formula. Distance information and height variance : （10） Each space All information is from a set It is determined by the average value of all points.

6. The initial mapping and map filtering method based on 3D LiDAR as described in claim 5, characterized in that, Step 56, which involves segmenting the point cloud based on the judgment result, specifically includes: Step 561, for and Make a judgment, if > If the current sector fails to acquire data, then skip that sector. Enter the next sector Then return to step 561; if ≤ Then proceed to step 562; Step 562, from arrive In order of priority 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 ring road The height difference between them Indicates the current sector and the current loop The corresponding height, Indicates the current sector And the previous ring road Corresponding height; 2) Calculate the distance difference: = ;in, Indicates the current sector and the current loop With the current sector And the previous ring road The distance difference between them Indicates the current sector and the current loop The corresponding distance, Indicates the current sector And the previous ring road 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 ring road The difference in slope between them Indicates the current sector and the current loop The corresponding slope, Indicates the current sector And the previous ring road The corresponding slope; Step 563: Determine if the conditions are met: , , and ,in, Indicates the range of ground elevation. Indicates the ground slope threshold. This represents the threshold for the difference in ground slope. Indicates the current sector and the current loop The corresponding height variance, This indicates the ground variance threshold; if so, the corresponding point is added to the ground point cloud. Otherwise, proceed to step 564; Step 564: Determine if the conditions are met: , and ,in, Indicates the ceiling height range. Indicates the ceiling slope threshold. This represents the ceiling slope difference threshold; if so, the corresponding point is added to the ceiling point cloud. Otherwise, proceed to step 565; Step 565: Determine if the conditions are met: and ,in, Indicates the slope threshold of the wall surface. This represents the threshold for the slope difference of the wall-like surfaces; if so, the corresponding point is plotted into the wall-like surface point cloud. Otherwise, skip the current space. .

7. The initial mapping and map filtering method based on 3D LiDAR as described in claim 6, characterized in that, Step 5 involves establishing a relationship between the pose and the point cloud based on the updated historical pose information and the segmented point cloud, and then projecting the associated point cloud into a 2D map to generate a 2.5D raster map. Specifically, this includes: Step 57: In the distortion-removed point cloud After segmentation, the ceiling cloud pattern was removed. Among the unclassified points, select the ground point cloud. Cloud-like patterns on walls , and the pose set after global optimization The association between point cloud and pose is shown in the following equation: （11） in, and Representing the global coordinate system Ground point clouds and wall-like point clouds below, This indicates that the pose graph optimization stage is in the first stage. The pose of the IMU in the global coordinate system at each frame point cloud moment. This represents the lidar pose in the IMU coordinate system. The first term represents the distortion of the lidar coordinate system. Frame ground point cloud, The first term represents the distortion of the lidar coordinate system. Frame-like wall point clouds; Step 58: In the global coordinate system Let's establish a resolution of 2.5D raster map ; Step 59: In the 2.5D raster map middle This indicates that one of the center points is located at And the diameter is The grid will and Projecting the points in the middle into 2D In the middle, assuming It includes ground points , and The weights of the ground and wall-like surfaces are respectively represented by the following formulas. Information included: （12） In the formula, The ground-based criteria determine whether the attribute corresponding to this grid cell is ground or wall-like. This refers to grid height information; z Representing a three-dimensional coordinate system The value of the axis.

8. The initial mapping and map filtering method based on 3D LiDAR as described in claim 7, characterized in that, In step 5, the associated pose and point cloud are accumulated to generate an original 3D point cloud map, and a new 3D point cloud map is obtained by filtering the original 3D point cloud map using a 2.5D raster map; specifically including: Step 510, for all and Accumulate point clouds to obtain the original 3D point cloud map. ; Step 511, for Points on Perform a traversal, if its corresponding On If the threshold condition is met, it is determined to be the ground, and there should only be ground points on it. This is the basis for further analysis. Perform filtering. The resolution of the point cloud map is shown in the following formula: （13） The new 3D point cloud map is obtained by filtering out dynamic points on the ground. Only ground information and wall-like information were retained, and dynamic obstacles were removed.

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, it implements the initial mapping and map filtering method based on 3D LiDAR as described in any one of claims 1 to 8.

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

Images