A laser SLAM method for solving dynamic residual image

Abstract

In view to the problem that laser SLAM is completed based on static assumption, and a large number of dynamic residues are generated in the environment containing dynamic obstacles, resulting in the decline of mapping and positioning accuracy, the application discloses a laser SLAM method for solving dynamic residues.The method comprises the following steps: firstly, the point cloud data acquired by the laser radar is divided into grids; then, the ground points are separated, and the non-ground points are clustered to remove noise, the initial pose is judged by using the IMU information and the point cloud information, and if the IMU initialization fails, the laser odometry mode is run; finally, the growing height descriptor is introduced, the suspected dynamic residue area is acquired, and the acquired dynamic residues are removed by using the space-time constraint. The application removes the noise by clustering, introduces the growing height descriptor and the space-time constraint to remove the dynamic residues, improves the mapping and positioning accuracy, and solves the influence of dynamic obstacles on mapping.

Description

Technical Field

[0001] This invention belongs to the field of simultaneous localization and mapping of lidar, and particularly relates to a lidar SLAM method for solving dynamic afterimages. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology uses sensors to build an environmental map and simultaneously uses the environmental map for autonomous localization. LiDAR SLAM uses LiDAR to achieve simultaneous localization and mapping, enabling it to work in low-light environments and generate environmental maps that facilitate navigation. However, because the point cloud registration in LiDAR SLAM is based on static assumptions, there are many dynamic obstacles in actual mapping, resulting in dynamic ghosting on the constructed map. These dynamic ghostings lead to a decrease in the accuracy of mapping and localization.

[0003] The paper "The Peopleremover—Removing Dynamic Objects From 3-D Point Cloud Data by Traversing a Voxel Occupancy Grid" proposes a method for removing dynamic afterimages based on ray projection. This method uses the cases of grids being hit and penetrated to determine dynamic afterimages. However, this method requires traversing all grids along each light path, resulting in high computational resource consumption and the problem of accidentally deleting static points.

[0004] The paper "ERASOR: Egocentric Ratio of Pseudo Occupancy-based Dynamic Object Removal for Static 3D Point CloudMap Building" proposes a method for removing dynamic artifacts based on grid occupancy differences. This method can quickly find potential dynamic artifact regions and remove them by fitting ground points within these regions. However, this method has the following problems:

[0005] 1. Taking only one height descriptor, namely the maximum height difference, as the difference in grid occupancy status is easily affected by noise. Furthermore, if there is a static point cloud above the dynamic afterimage, the dynamic afterimage in that area will be treated as a static point cloud, resulting in the dynamic afterimage not being completely removed.

[0006] 2. When dividing the grid, equal intervals will result in dense point clouds in areas close to the lidar and sparse point clouds in areas far from the lidar. The sparse point clouds will cause inaccurate ground fitting.

[0007] 3. Directly removing points other than ground points from the potential dynamic ghosting area may result in the accidental deletion of static points;

[0008] 4. The fitted ground points are estimated independently within each grid cell, which may result in inaccurate fitting of some ground points.

[0009] To address the aforementioned issues in the literature, this invention utilizes the difference in radii of concentric circles formed by adjacent laser beams and the ground plane to divide the grid, resulting in a more uniform point cloud distribution. Simultaneously, by introducing a growth height descriptor and spatiotemporal constraints, it solves the problem of not being able to remove dynamic afterimages below static point clouds. Summary of the Invention

[0010] To address the shortcomings of existing methods, this invention provides a laser SLAM method for resolving dynamic ghosting, aiming to solve the problem of decreased mapping and positioning accuracy caused by dynamic ghosting during mapping in environments with dynamic obstacles. First, the difference in the radii of concentric circles formed by adjacent laser beams and the ground plane is used to divide the grid, ensuring a uniform distribution of the point cloud. Then, ground points are separated and noise is removed using Euclidean clustering, followed by IMU initialization. If initialization is successful, the system runs in laser inertial odometry mode; otherwise, it runs in laser odometry mode. Finally, potential dynamic ghosting regions are accurately identified by growing a height descriptor. Spatiotemporal constraints are introduced, and the point cloud information of each grid within the potential dynamic ghosting region is traversed using timestamps to remove dynamic ghosting, thus constructing a static map. A laser SLAM method for resolving dynamic ghosting:

[0011] S1: The acquired point cloud data is ordered by dividing it into grids using the difference in radii of concentric circles formed by adjacent laser beams and the ground plane, so that the point cloud is evenly distributed within the grids. The three-dimensional coordinate points are transformed into two-dimensional coordinate points through dimensionality reduction, which includes the following sub-steps:

[0012] S1.1: Transform the xy coordinates in the Cartesian coordinate system into a circle with an infinite radius. Set the radian parameter δθ, and divide the circle into M sectors according to the radian parameter δθ. The sector name is denoted as Sector:

[0013]

[0014] S1.2: Perform xy-plane ordering on each point:

[0015]

[0016] Where: Sector p(i) Represents point cloud i in Sector;

[0017] S1.3: Divide the point cloud, transforming the disordered point cloud into an ordered point cloud:

[0018] Ps ={p i ∈P|Sector p(i) =s} (3)

[0019] In the formula: P represents all point clouds; p(i) represents point cloud i; s represents sector s; P s Represents the set of point clouds in Sector s;

[0020] S1.4: Each sector is further divided into multiple sector bins, based on the difference in the radii of the concentric circles formed by adjacent laser beams and the ground plane.

[0021]

[0022] In the formula: Δr i,i+1 The difference in radii between the concentric circles formed by the laser beam and the ground plane; h represents the height of the lidar above the ground; a i a represents the angle between the i-th laser beam and the ground plane; i+1 This represents the angle between the (i+1)th laser beam and the ground plane;

[0023] S1.5: Perform dimensionality reduction processing on the point cloud data:

[0024] {x,y,z}={d,z}+α (5)

[0025] In the formula: α represents the angular information in the xy plane; {x,y,z} represents the coordinates of the laser point on the x-axis, y-axis, and z-axis;

[0026] S2: Fit ground points using a ground point fitting algorithm, and treat the fitted ground points as static points. This includes the following sub-steps:

[0027] S2.1: Set the fitted line:

[0028] z = kd + b (6)

[0029] In the formula: k is the slope of the line; b represents the constant term;

[0030] S2.2: Limit the absolute value of the slope of the line; a slope greater than the threshold will result in a vertical structure.

[0031]

[0032] In the formula: |k| represents the absolute value of the slope of the line; This indicates the set threshold.

[0033] S2.3: The ground is flat when the slope is less than the threshold, and flat when the slope is less than the minimum slope. When zb does not exceed a specific threshold T b :

[0034]

[0035] S2.4: The root mean square error of the fitted line does not exceed the set error threshold.

[0036] Rmse < E Rmse (9)

[0037] In the formula: Rmse represents the root mean square error of the fitted line; E Rmse Indicates the error threshold;

[0038] S2.5: Obtain the first point in the bin of the Sector, and then determine the distance of the point to the existing straight line. If the distance does not exceed the set threshold, the point can be fitted to the straight line. If the distance is greater than the threshold, a new straight line is started based on the point, and different lines are divided.

[0039] S2.6: The ground points are selected by fitting the straight line. Based on the selected straight line and the set parameters, the distance from the point to the straight line is calculated. Points within the threshold range are selected ground points.

[0040] S3: Use a clustering algorithm to cluster non-ground points, remove point clouds with a cluster size lower than 20 as noise, and perform feature extraction. This includes the following sub-steps:

[0041] S3.1: Based on Euclidean distance as the criterion, select a point P, and use KD-Tree to find the k points closest to point P. If the distance to point P is less than the set threshold, add it to the point set M. Continue until the number of elements in M ​​stops increasing, and the clustering ends. Otherwise, continue to search for points other than point P, and remove the clustered point cloud with fewer than 20 points.

[0042] S3.2: Calculate the curvature of each point in the laser scan frame, and then extract the edge features and planar features of the scan frame. The curvature calculation formula is as follows:

[0043]

[0044] In the formula: c represents the curvature of the point cloud; s represents the set of continuous points; p i and p j Let i and j represent the i-th point and the j-th point, respectively.

[0045] S3.3: Let p i For the target point, the edge features and planar features of the lidar scan frame are represented as follows: and Convert it to world coordinate system and Using the nearest neighbor search method, relevant feature points are located in and In the middle, calculate the distance from the target point to the relevant edge points:

[0046]

[0047] In the formula: Indicates the distance between the target point and the relevant edge points; Represent edge feature points; and They represent Two different points on the corresponding edge line;

[0048] S3.4: The formula for the distance from the target point to the associated plane is as follows:

[0049]

[0050] In the formula: Indicates the distance between the target point and the relevant plane; Represents planar feature points; and express Three different points on the corresponding plane;

[0051] S3.5: Estimate the pose of the current frame and the local map by solving an optimization problem:

[0052]

[0053] S4: Initialize the IMU. If initialization is successful, the system will run in laser inertial odometry mode; otherwise, it will run in laser odometry mode. This includes the following sub-steps:

[0054] S4.1: The raw IMU measurements include acceleration and angular velocity. All measurements are taken in IMU coordinate system B, which is the same as the robot coordinate system. The IMU measurement model is represented as follows:

[0055]

[0056]

[0057] In the formula: and ω represents the measured values ​​of angular velocity and acceleration at time t; t With a t Represents the true values ​​of angular velocity and acceleration; and This indicates that the gyroscope and accelerometer have zero bias. Represents the rotation matrix between the world coordinate system and the IMU coordinate system; and This represents the noise from the gyroscope and accelerometer; g represents gravitational acceleration.

[0058] S4.2: Use IMU measurements to estimate the robot's velocity v at time t+Δt. t+Δt Displacement p t+Δt and rotation R t+Δt The calculation formula is:

[0059]

[0060]

[0061]

[0062] In the formula: v t p represents the velocity at time t; t This represents the displacement at time t;

[0063] S4.3: If IMU initialization is successful, the system runs in laser inertial odometry mode; otherwise, it runs in laser odometry mode.

[0064] S5: Divide the point cloud data into multiple historical frames according to timestamps, then divide the occupancy descriptor range, use the growth height descriptor to compare the scan frames with the local map, obtain potential dynamic ghosting areas and exclude ground points, treat point clouds that are not always occupied in the bin as dynamic ghosting and remove them directly, treat point clouds that are always occupied in the bin as areas that contain both dynamic ghosting and static point clouds, specifically including the following sub-steps:

[0065] S5.1: Divide the point cloud data into multiple historical frames according to timestamps, and represent the point cloud at time t+1 and the local map corresponding to time t as follows: and Before dynamic ghosting detection, Transform to world coordinate system

[0066]

[0067] In the formula: This represents a laser scan frame in the radar coordinate system; This indicates the transformation relationship between the radar coordinate system and the world coordinate system; Represents a laser scan frame in the world coordinate system;

[0068] S5.2: Record the position of the point cloud in the world coordinate system, the expression is as follows:

[0069]

[0070] In the formula: This represents the position of the point cloud in the world coordinate system. This indicates the position of the point cloud along the x-axis in the world coordinate system; This indicates the position of the point cloud along the y-axis in the world coordinate system; This indicates the position of the point cloud along the z-axis in the world coordinate system;

[0071] S5.3: Divide the current frame and the local map into occupied descriptor ranges. Since the point cloud is sparser the farther away from the origin, select an area within a radius of 70 meters from the origin and a height of -1 meter to +3 meters.

[0072] S5.4: Obtain the point cloud within the bin. If there is no point cloud, ignore it. If there is only one point, it is the maximum height point. If there are multiple points, sort them according to their z-axis height values ​​and calculate the height difference between adjacent points. If the difference is always less than a threshold, take the last point as the maximum height point. If the difference is greater than the threshold, take the point with the smaller height value as the maximum height point. Exclude static point clouds above dynamic afterimages within the bin. This solves the problem that the height descriptor would consider all point clouds in the raster to be static point clouds when there are static point clouds above dynamic afterimages. Calculate the maximum height difference for each facet. and That is, growth height descriptors, and comparisons:

[0073]

[0074] S5.5: Regions that meet the conditions are potential dynamic ghosting regions.

[0075]

[0076] Where: M D Indicates the potential dynamic afterimage region; Represents a potential dynamic point set;

[0077] S5.6: Since ground points are static points, ground point information does not need to be considered when judging dynamic ghosting. Ground points are excluded, and then point clouds that are not always occupied in the bin are regarded as dynamic ghosting and removed. Bins that always exist in the point cloud are regarded as bins that contain both dynamic ghosting and static point cloud, and are denoted as Bins.

[0078] S6: Utilize spatiotemporal constraints to find points within the Bins that appeared in historical frames but do not exist in the current frame. Distinguish between static point clouds and dynamic afterimages within the Bins in detail, then remove dynamic afterimages and construct a static map.

[0079] The present invention has the following beneficial effects:

[0080] 1. By using dimensionality reduction and ground fitting algorithms, ground points are quickly separated, reducing the calculation time for judging dynamic afterimages and increasing computational efficiency.

[0081] 2. Due to the sparsity of point clouds, dividing the bins at equal intervals will result in denser point clouds in areas closer to the center and sparser point clouds in areas farther from the center, affecting the ground fitting effect. The bins are divided by the difference in the radii of the concentric circles formed by adjacent laser beams and the ground plane, so that the point cloud data within the bins is evenly distributed, making the fitted ground points more accurate.

[0082] 3. Noise is removed by Euclidean clustering algorithm, enabling operation in noisy environments. The system runs in laser odometry mode when IMU initialization fails, otherwise it runs in laser inertial odometry mode, which improves the stability of the system.

[0083] 4. A height descriptor growth method is proposed. The single height descriptor only takes the maximum height difference as the difference in the grid occupancy state. When there is a static point cloud above the dynamic ghost image, it will be considered that all point clouds in the grid are static point clouds. The height descriptor growth method can exclude the static point cloud above the dynamic ghost image, quickly and accurately find the potential dynamic ghost image region, add spatiotemporal constraints to divide the dynamic ghost image and static point cloud in detail, and accurately remove the dynamic ghost image. Attached Figure Description

[0084] 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.

[0085] Figure 1 A flowchart of a laser SLAM method for solving dynamic image retention;

[0086] Figure 2 A schematic diagram showing the radius difference of concentric circles formed by adjacent laser beams and the ground plane;

[0087] Figure 3 This is a schematic diagram of grid division;

[0088] Figure 4 This is a schematic diagram of the ground fitting process;

[0089] Figure 5 Flowchart of the growth height descriptor method;

[0090] Figure 6 Mapping for the LIO-SAM method;

[0091] Figure 7Graphing for the ERASOR method;

[0092] Figure 8 Mapping for a laser SLAM method to solve dynamic image retention. Detailed Implementation

[0093] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, a laser SLAM method for solving dynamic image retention is provided, such as... Figure 1 As shown, it includes the following steps:

[0094] S1: The acquired point cloud data is processed to create an ordered grid, which is then divided using the difference in the radii of concentric circles formed by adjacent laser beams and the ground plane. Figure 2 As shown; the method of dividing the grid at equal intervals has the problem that the point cloud is dense in the grid close to the lidar and sparse in the grid far from the lidar; this method can make the point cloud evenly distributed in the grid and accurately fit the ground points; by dimensionality reduction, the three-dimensional coordinate points are transformed into two-dimensional coordinate points, which specifically includes the following sub-steps:

[0095] S1.1: Transform the xy coordinates in the Cartesian coordinate system into a circle with an infinite radius. Set the radian parameter δθ, and divide the circle into M sectors according to the radian parameter δθ. The sector name is denoted as Sector:

[0096]

[0097] S1.2: Perform xy-plane ordering on each point:

[0098]

[0099] Where: Sector p(i) Let i represent the point cloud in the Sector.

[0100] S1.3: Divide the point cloud, transforming the disordered point cloud into an ordered point cloud:

[0101] P s ={p i ∈P|Sector p(i) =s} (3)

[0102] In the formula: P represents all point clouds; p(i) represents point cloud i; s represents sector s; P s This represents the set of point clouds in Sector s.

[0103] S1.4: Further divide each sector into ordered subdivisions, dividing each sector into multiple sector rings (bins), such as... Figure 3 As shown, the division is based on the difference in the radii of the concentric circles formed by adjacent laser beams and the ground plane:

[0104]

[0105] In the formula: Δr i,i+1 The difference in radii between the concentric circles formed by the laser beam and the ground plane; h represents the height of the lidar above the ground; a i a represents the angle between the i-th laser beam and the ground plane; i+1 This represents the angle between the (i+1)th laser beam and the ground plane.

[0106] S1.5: Perform dimensionality reduction processing on the point cloud data:

[0107] {x,y,z}={d,z}+α (5)

[0108] In the formula: α represents the angular information of the xy plane; {x,y,z} represents the coordinates of the laser point on the x-axis, y-axis, and z-axis.

[0109] S2: Fit ground points using a ground point fitting algorithm, and treat the fitted ground points as static points. This includes the following sub-steps:

[0110] S2.1: Set the fitted line:

[0111] z = kd + b (6)

[0112] In the formula: k is the slope of the line; b represents the constant term.

[0113] S2.2: Limit the absolute value of the slope of the line; a slope greater than the threshold will result in a vertical structure.

[0114]

[0115] In the formula: |k| represents the absolute value of the slope of the line; This indicates the set threshold.

[0116] S2.3: The ground is flat when the slope is less than the threshold, and flat when the slope is less than the minimum slope. When zb does not exceed a specific threshold T b :

[0117]

[0118] S2.4: The root mean square error of the fitted line does not exceed the set error threshold.

[0119] Rmse < E Rmse (9)

[0120] In the formula: Rmse represents the root mean square error of the fitted line; E Rmse This indicates the error threshold.

[0121] S2.5: Obtain the first point within the bin of the Sector, then determine the distance from that point to an existing line. If the distance does not exceed a set threshold, and is less than the threshold, the point can be fitted onto the line. If it exceeds the threshold, a new line is started based on that point, and the division of different lines is performed. Figure 4 As shown.

[0122] S2.6: The ground points are selected by using the fitted straight line. Based on the selected straight line and the set parameters, the distance from the point to the straight line is calculated. Points within the threshold range are selected ground points.

[0123] S3: Use a clustering algorithm to cluster non-ground points, remove point clouds with a cluster size lower than 20 as noise, and perform feature extraction. This includes the following sub-steps:

[0124] S3.1: Based on Euclidean distance as the criterion, select a point P, and use KD-Tree to find the k points closest to point P. If the distance to point P is less than the set threshold, add it to the point set M. Continue until the number of elements in M ​​stops increasing, and the clustering ends. Otherwise, continue to search for points other than point P, and remove the clustered point cloud with fewer than 20 points.

[0125] S3.2: Calculate the curvature of each point in the laser scan frame, and then extract the edge features and planar features of the scan frame. The curvature calculation formula is as follows:

[0126]

[0127] In the formula: c represents the curvature of the point cloud; s represents the set of continuous points; p i and p j Let i and j represent the i-th point and the j-th point, respectively.

[0128] S3.3: Let p i For the target point, the edge features and planar features of the lidar scan frame are represented as follows: and Convert it to world coordinate system and Using the nearest neighbor search method, relevant feature points are located in and In the middle, calculate the distance from the target point to the relevant edge points:

[0129]

[0130] In the formula: Indicates the distance between the target point and the relevant edge points; Represent edge feature points; and They represent The two points on the corresponding edge line are different.

[0131] S3.4: The formula for the distance from the target point to the associated plane is as follows:

[0132]

[0133] In the formula: Indicates the distance between the target point and the relevant plane; Represents planar feature points; and express Three different points on the corresponding plane.

[0134] S3.5: Estimate the pose of the current frame and the local map by solving an optimization problem:

[0135]

[0136] S4: Initialize the IMU. If initialization is successful, the system will run in laser inertial odometry mode; otherwise, it will run in laser odometry mode. This includes the following sub-steps:

[0137] S4.1: The raw IMU measurements include acceleration and angular velocity. All measurements are taken in IMU coordinate system B, which is the same as the robot coordinate system. The IMU measurement model is represented as follows:

[0138]

[0139]

[0140] In the formula: and ω represents the measured values ​​of angular velocity and acceleration at time t; t With a t Represents the true values ​​of angular velocity and acceleration; and This indicates that the gyroscope and accelerometer have zero bias. Represents the rotation matrix between the world coordinate system and the IMU coordinate system; and The noise level is represented by the gyroscope and accelerometer; g represents the acceleration due to gravity.

[0141] S4.2: Use IMU measurements to estimate the robot's velocity v at time t+Δt. t+Δt Displacement p t+Δt and rotation R t+Δt The calculation formula is:

[0142]

[0143]

[0144]

[0145] In the formula: v t p represents the velocity at time t; t This represents the displacement at time t.

[0146] S4.3: If IMU initialization is successful, the system will run in laser inertial odometry mode; otherwise, it will run in laser odometry mode.

[0147] S5: Divide the point cloud data into multiple historical frames according to timestamps, then divide the occupancy descriptor range, and introduce the growth height descriptor. The flowchart of the growth height descriptor is as follows. Figure 5 As shown, the growth height descriptor is used to compare the scan frame with the local map to obtain potential dynamic ghosting regions and exclude ground points. Point clouds that are not always occupied within a bin are considered dynamic ghosting and are directly removed. Point clouds that are always occupied within a bin are considered regions that contain both dynamic ghosting and static point clouds. The specific steps include the following:

[0148] S5.1: Divide the point cloud data into multiple historical frames according to timestamps, and represent the point cloud at time t+1 and the local map corresponding to time t as follows: and Before dynamic ghosting detection, Transform to world coordinate system

[0149]

[0150] In the formula: This represents a laser scan frame in the radar coordinate system; This indicates the transformation relationship between the radar coordinate system and the world coordinate system; This represents a laser scan frame in the world coordinate system.

[0151] S5.2: Record the position of the point cloud in the world coordinate system, the expression is as follows:

[0152]

[0153] In the formula: This represents the position of the point cloud in the world coordinate system. This indicates the position of the point cloud along the x-axis in the world coordinate system; This indicates the position of the point cloud along the y-axis in the world coordinate system; This indicates the position of the point cloud along the z-axis in the world coordinate system.

[0154] S5.3: Divide the current frame and the local map into occupied descriptor ranges. Since the point cloud is sparser the farther away from the origin, select an area within a radius of 70 meters from the origin and a height of -1 meter to +3 meters.

[0155] S5.4: Obtain the point cloud within the bin. If there is no point cloud, ignore it. If there is only one point, it is the maximum height point. If there are multiple points, sort them according to their z-axis height values ​​and calculate the height difference between adjacent points. If the difference is always less than a threshold, take the last point as the maximum height point. If the difference is greater than the threshold, take the point with the smaller height value as the maximum height point. Exclude static point clouds above dynamic afterimages within the bin. This solves the problem that when there are static point clouds above dynamic afterimages, the height descriptor will consider all point clouds in the raster to be static point clouds. Calculate the maximum height difference for each facet. and That is, growth height descriptors, and comparisons:

[0156]

[0157] S5.5: The region that meets the conditions is the potential dynamic region.

[0158]

[0159] Where: M D Indicates the potential dynamic afterimage region; This represents a potential dynamic point set.

[0160] S5.6: Since ground points are static points, ground point information does not need to be considered when judging dynamic ghosting. Ground points are excluded. Then, point clouds that are not always occupied in the bin are regarded as dynamic ghosting and removed. Bins that always exist are regarded as bins that contain both dynamic ghosting and static point clouds, and are denoted as Bins.

[0161] S6: Utilize spatiotemporal constraints to find points within the Bins that appeared in historical frames but do not exist in the current frame. Distinguish between static point clouds and dynamic afterimages within the Bins in detail, then remove dynamic afterimages and construct a static map.

[0162] This invention divides the grid by utilizing the difference in radii of concentric circles formed by adjacent laser beams and the ground plane, ensuring a uniform distribution of point clouds within each grid and accurately fitting ground points. Noise is eliminated through Euclidean clustering, and the system stability is improved by employing a switching mechanism between laser inertial odometry and laser odometry. A height descriptor growth method is proposed to address the problem of mistakenly identifying dynamic afterimages as static point clouds when they exist above them within the grid. Spatiotemporal constraints are introduced to remove dynamic afterimages, thus completing map construction.

[0163] The present invention is verified in conjunction with the experimental results below. Figure 6The map was constructed using the LIO-SAM method, but it contains a large number of dynamic ghosting images. Figure 7 When constructing a map using the ERASOR method, some dynamic ghosting images were not removed. Because there are static point clouds above some dynamic ghosting images in some grid cells, the height descriptor would mistakenly treat these dynamic ghosting images as static point clouds. Therefore, some dynamic ghosting images were retained. Figure 8 This paper presents a laser SLAM method for mapping to address dynamic ghosting. As shown in the figure, this method removes dynamic ghosting below the static point cloud. Compared with the ERASOR method, this method improves the removal rate of dynamic ghosting. This method introduces a growth height descriptor and spatiotemporal constraints to address the impact of dynamic obstacles on mapping.

[0164] The specific implementation schemes described above further illustrate the inventive purpose, technical solution, and beneficial effects of the present invention. The above embodiments are only used to illustrate the technical solution of the present invention and are not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that any modifications or equivalent substitutions made to the technical solution of the present invention are included within the scope of protection of the present invention.

Claims

1. A laser SLAM method for solving dynamic image retention, characterized in that, Includes the following steps: S1: The acquired point cloud data is ordered by dividing it into grids using the difference in radii of concentric circles formed by adjacent laser beams and the ground plane, so that the point cloud is evenly distributed within the grids. The three-dimensional coordinate points are transformed into two-dimensional coordinate points through dimensionality reduction, which includes the following sub-steps: S1.1: Transform the xy coordinates in the Cartesian coordinate system into a circle with an infinite radius, set the radian parameter δθ, and divide the circle into M sectors according to the radian parameter δθ, denoted as Sector; S1.2: Perform xy-plane ordering on each point: Where: Sector p(i) Represents point cloud i in Sector; S1.3: Divide the point cloud into an ordered one; S1.4: Each sector is further divided into multiple sector bins, based on the difference in the radii of the concentric circles formed by adjacent laser beams and the ground plane. In the formula: Δr i,i+1 The difference in radii between the concentric circles formed by the laser beam and the ground plane; h represents the height of the lidar above the ground; a i a represents the angle between the i-th laser beam and the ground plane; i+1 This represents the angle between the (i+1)th laser beam and the ground plane; S1.5: Perform dimensionality reduction processing on the point cloud data: {x,y,z}={d,z}+α (3) In the formula: α represents the angular information in the xy plane; {x,y,z} represents the coordinates of the laser point on the x-axis, y-axis, and z-axis; S2: Fit ground points using a ground point fitting algorithm, and treat the fitted ground points as static points. This includes the following sub-steps: S2.1: Set the fitted line: z = kd + b (4) In the formula: k is the slope of the line; b represents the constant term; S2.2: Limit the absolute value of the slope of the line; if the slope is greater than the threshold, a vertical structure will be formed. S2.3: The ground is flat when the slope is less than the threshold, and flat when the slope is less than the minimum slope. When zb does not exceed a specific threshold T b ; S2.4: The root mean square error of the fitted line does not exceed the set error threshold E. Rmse ; S2.5: Obtain the first point in the bin of the Sector, and then determine the distance of the point to the existing straight line. If the distance does not exceed the set threshold, the point can be fitted to the straight line. If the distance exceeds the threshold, a new straight line is started based on the point, and different lines are divided. S2.6: Filter ground points using the fitted straight line. Based on the filtered straight line and the set parameters, calculate the distance from the point to the straight line. Points within the threshold range are selected ground points. S3: Use a clustering algorithm to cluster non-ground points, remove point clouds with a cluster size lower than 20 as noise, and perform feature extraction. This includes the following sub-steps: S3.1: Based on Euclidean distance as the criterion, select a point P, and use KD-Tree to find the k points closest to point P. If the distance to point P is less than the set threshold, add it to the point set M. Continue until the number of elements in M ​​stops increasing, and the clustering ends. Otherwise, continue to search for points other than point P, and remove the clustered point cloud with fewer than 20 points. S3.2: Calculate the curvature of each point in the laser scan frame, and then extract the edge features and planar features of the scan frame: In the formula: c represents the curvature of the point cloud; s represents the set of continuous points; p i and p j Let i and j represent the i-th point and the j-th point, respectively. S3.3: Let p i For the target point, the edge features and planar features of the lidar scan frame are represented as follows: and Convert it to world coordinate system and Using the nearest neighbor search method, relevant feature points are located in and In the process, the distance from the target point to the relevant edge points is calculated; S3.4: Calculate the distance from the target point to the associated plane; S3.5: Estimate the pose of the current frame and the local map by solving an optimization problem; S4: Initialize the IMU. If initialization is successful, the system will run in laser inertial odometry mode; otherwise, it will run in laser odometry mode. This includes the following sub-steps: S4.1: The raw IMU measurements include acceleration and angular velocity. All measurements are taken in the IMU coordinate system B, which is the same as the robot coordinate system. S4.2: Use IMU measurements to estimate the robot's pose at time t+Δt; S4.3: If IMU initialization is successful, the system runs in laser inertial odometry mode; otherwise, it runs in laser odometry mode. S5: Divide the point cloud data into multiple historical frames according to timestamps, then divide the occupancy descriptor range, use the growth height descriptor to compare the scan frames with the local map, obtain potential dynamic ghosting areas and exclude ground points, treat point clouds that are not always occupied in the bin as dynamic ghosting and remove them directly, treat point clouds that are always occupied in the bin as areas that contain both dynamic ghosting and static point clouds, specifically including the following sub-steps: S5.1: Divide the point cloud data into multiple historical frames according to timestamps, and represent the point cloud at time t+1 and the local map corresponding to time t as follows: and Before dynamic ghosting detection, Transform to world coordinate system S5.2: Record the position of the point cloud in the world coordinate system; S5.3: Divide the current frame and the local map into occupied descriptor ranges. Since the point cloud is sparser the farther away from the origin, select an area within a radius of 70 meters from the origin and a height of -1 meter to +3 meters. S5.4: Obtain the point cloud within the bin. If there is no point cloud, ignore it. If there is only one point, it is the maximum height point. If there are multiple points, sort them according to their z-axis height values ​​and calculate the height difference between adjacent points. If the difference is always less than a threshold, take the last point as the maximum height point. If the difference is greater than the threshold, take the point with the smaller height value as the maximum height point. Exclude the static point cloud above the dynamic afterimage within the bin. Calculate the maximum height difference for each surface element. and That is, growth height descriptors, and comparisons: S5.5: Regions that meet the conditions are potential dynamic ghosting regions; S5.6: Treat ground points as static points and do not consider them. Treat point clouds that are not always occupied in a bin as dynamic ghost images and remove them directly. Treat bins that always contain point clouds as bins that contain both dynamic ghost images and static point clouds, and denote them as Bins. S6: Utilize spatiotemporal constraints to find points within the Bins that appeared in historical frames but do not exist in the current frame. Distinguish between static point clouds and dynamic afterimages within the Bins in detail, then remove dynamic afterimages and construct a static map.

Images