CN117288177A - Laser SLAM method for solving dynamic ghost - Google Patents

Abstract

Aiming at the problems that the laser SLAM is completed based on a static assumption, a large amount of dynamic ghost shadows are generated when the image is built in an environment containing dynamic obstacles, and the image building and positioning accuracy is reduced, the invention discloses a laser SLAM method for solving the dynamic ghost shadows. The method comprises the following steps: firstly, carrying out grid division on point cloud data acquired by a laser radar; then separating the ground points, removing noise by clustering non-ground points, judging an initial pose by using IMU information and point cloud information, and operating a laser odometer mode if the IMU initialization fails; and finally, introducing a growth height descriptor, acquiring a suspected dynamic residual image area, and eliminating the acquired dynamic residual image by utilizing space-time constraint. According to the invention, noise is removed through clustering, and the growth height descriptors and space-time constraint are introduced to remove dynamic residual shadows, so that the image construction and positioning precision are improved, and the influence of dynamic obstacles on the image construction is solved.

Description

A laser SLAM method to solve dynamic afterimages

Technical field

The invention belongs to the field of laser radar simultaneous positioning and mapping, and in particular relates to a laser SLAM method for solving dynamic afterimages.

Background technique

Simulation Location And Mapping (SLAM) technology uses sensors to build an environment map and simultaneously uses the environment map for autonomous positioning; laser SLAM uses lidar to complete simultaneous positioning and mapping, which can be used in poor light environments. It works well and can generate an environment map that is easy to navigate. However, since the point cloud registration of laser SLAM is based on static assumptions, there will be a large number of dynamic obstacles during actual mapping, resulting in dynamic residuals on the constructed map. These dynamic afterimages will cause the accuracy of mapping and positioning to decrease.

The document "The Peopleremover—Removing Dynamic Objects From 3-D Point CloudData by Traversing a Voxel Occupancy Grid" proposes a method to remove dynamic afterimages based on the light projection method. This method uses the judgment of whether the grid is hit or penetrated. Dynamic afterimage. However, this method needs to traverse all grids on each optical path, consumes a lot of computing resources, and has the problem of accidentally deleting static points.

The document "ERASOR: Egocentric Ratio ofPseudo Occupancy-based Dynamic ObjectRemoval for Static 3D Point CloudMap Building" proposes a method to remove dynamic afterimages based on differences in grid occupancy. This method can quickly find potential dynamic afterimage areas and simulate them through Eliminate dynamic afterimages by combining ground points within the potential dynamic afterimage area. However, this method has the following problems:

1. Only one height descriptor, that is, the maximum height difference, is used as the difference in grid occupancy status, which is easily affected by noise, and if there is a static point cloud above the dynamic afterimage, the dynamic afterimage in this area will be regarded as a static point cloud. , resulting in dynamic residual images that cannot be completely removed;

2. When dividing the grid into equidistant divisions, the point clouds in areas close to the lidar will be dense, and the point clouds in areas far away from the lidar will be sparse. Areas with sparse point clouds will cause inaccurate ground fitting;

3. Directly eliminating points other than ground points in the potential dynamic afterimage area will cause static points to be mistakenly deleted;

4. The fitting ground points are estimated independently in each grid, and there will be problems with inaccurate fitting of some ground points.

In response to the above-mentioned literature problems, this invention uses the radius difference of concentric circles formed by adjacent laser beams and the ground plane to divide the grid, so that the point cloud distribution is relatively uniform. At the same time, the growth height descriptor and spatiotemporal constraints are introduced to solve the problem that static point clouds cannot be eliminated. The dynamic afterimage problem below.

Contents of the invention

In view of the shortcomings of existing methods, the present invention provides a laser SLAM method to solve dynamic afterimages. The purpose is to solve the problem of dynamic afterimages generated when mapping in an environment containing dynamic obstacles, resulting in reduced mapping and positioning accuracy. . First, the radius difference between the concentric circles formed by adjacent laser beams and the ground plane is used to divide the grid so that the point cloud is evenly distributed; then the ground points are separated and the noise is eliminated through Euclidean clustering, and the IMU is initialized. If the initialization is successful, the system runs laser inertia Odometer mode, otherwise run laser odometry mode; finally, the potential dynamic afterimage area is accurately identified through the growth height descriptor, spatiotemporal constraints are introduced, and the point cloud information of each grid in the potential dynamic afterimage area is traversed through the timestamp to eliminate dynamic Afterimage, build static map. A laser SLAM method to solve dynamic afterimages:

S1: Orderly process the obtained point cloud data, use the radius difference of the concentric circles formed by the adjacent laser beams and the ground plane to divide the grid, so that the point cloud is evenly distributed within the grid, and reduce the three-dimensional coordinate points through dimensionality reduction. Transforming into two-dimensional coordinate points specifically includes the following sub-steps:

S1.1: Convert the x-y coordinates in the Cartesian coordinate system into a circular surface with infinite radius, set the radian parameter δθ, and divide the circular surface into M sectors according to the radian parameter δθ. The sector name is Sector:

S1.2: Order each point in the x-y plane:

In the formula: Sector _p(i) represents the point cloud i in Sector;

S1.3: Divide the point cloud and convert the disordered point cloud into an ordered point cloud:

P _s ={p _i ∈P|Sector _p(i) =s} (3)

In the formula: P represents all point clouds; p(i) represents point cloud i; s represents Sector s; P _s represents the point cloud collection of Sector s;

S1.4: Each sector sector is further divided in an orderly manner, and each sector is divided into multiple sector ring bins, which are divided by the radius difference of the concentric circles formed by the adjacent laser beams and the ground plane:

In the formula: Δr _i,i+1 represents the radius difference between the concentric circles formed by the laser beam and the ground plane; h represents the height of the lidar from the ground; a _i represents the angle between the i-th laser beam and the ground plane; a _i+1 Represents the angle between the i+1 laser beam and the ground plane;

S1.5: Dimensionality reduction processing of point cloud data:

{x,y,z}＝{d,z}+α (5)

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

S2: Fit the ground points through the ground point fitting algorithm, and treat the fitted ground points as static points, which specifically includes the following sub-steps:

S2.1: Set the fitting straight line:

z＝kd+b (6)

In the formula: k is the slope of the straight line; b represents the constant term;

S2.2: Limit the absolute value of the slope of the straight line. If the slope is greater than the threshold, a vertical structure will appear:

In the formula: |k| represents the absolute value of the slope of the straight line; Indicates the set threshold;

S2.3: The ground is flat when the slope is less than the threshold, and 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 straight line does not exceed the set error threshold:

Rmse＜E _Rmse (9)

In the formula: Rmse represents the root mean square error of the fitted straight line; E _Rmse represents the error threshold;

S2.5: Get the first point in the bin in the Sector, and then determine the distance between the point and the existing straight line. The distance does not exceed the set threshold. If it is less than the threshold, the point can be fitted to the straight line. If is greater than the threshold, use this point as the benchmark to start a new straight line and divide different straight lines;

S2.6: Screen the ground points through the fitted straight line. Calculate the distance from the point to the straight line based on the filtered straight line and the set parameters. Points within the threshold range are the selected ground points;

S3: Use the clustering algorithm to cluster non-ground points, remove point clouds with less than 20 points in the cluster as noise, and perform feature extraction, which specifically includes the following sub-steps:

S3.1: Based on the Euclidean distance as the judgment criterion, select a point P and use KD-Tree to find the k closest points to the point P. If the distance to the point P is less than the set threshold, put it into the point set M until it is in M If the elements no longer increase, the clustering ends. Otherwise, continue to search for points other than point P, and remove the obtained cluster point cloud with less than 20 points;

S3.2: Calculate the curvature of each point in the laser scanning frame, and then extract the edge features and plane features of the scanning frame. The curvature calculation formula is as follows:

In the formula: c represents the curvature of the point cloud; s represents the set of continuous points; p _i and p _j represent the i-th point and j-th point respectively;

S3.3: Let p _i be the target point, and the edge features and plane features of the lidar scanning frame are expressed as and Convert it to the world coordinate system as/> and/> Use the nearest neighbor search method to locate relevant feature points in and/> , calculate the distance from the target point to the relevant edge point:

In the formula: Represents the distance between the target point and the relevant edge point;/> Represents edge feature points;/> and/> Respectively expressed/> Corresponding to two different points on the edge line;

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

In the formula: Represents the distance between the target point and the relevant plane;/> Represents plane feature points;/> and/> Express/> Three different points on the corresponding plane;

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

S4: Initialize the IMU. If the initialization is successful, the system will run the laser inertial odometry mode. Otherwise, the system will run the laser odometry mode, which specifically includes the following sub-steps:

S4.1: The original IMU measurement includes acceleration and angular velocity. The measurement values are all in the IMU coordinate system B, which is the same as the robot coordinate system. The IMU measurement model is expressed as:

In the formula: with/> represents the measured values of angular velocity and acceleration at time t; ω _t and a _t represent the true values of angular velocity and acceleration;/> and/> Indicates the zero bias of the gyroscope and accelerometer;/> Represents the rotation matrix of the world coordinate system and the IMU coordinate system;/> and/> represents the noise of the gyroscope and accelerometer; g represents the acceleration of gravity;

S4.2: Use IMU measurement to estimate the speed v _t+Δt , displacement p _t+Δt and rotation R _t+Δt of the robot at time t+Δt. The calculation formula is:

In the formula: v _t represents the velocity at time t; p _t represents the displacement at time t;

S4.3: The IMU initialization is successful and the system runs the laser inertial odometry mode, otherwise it runs the laser odometry mode;

S5: Divide the point cloud data into multiple historical frames according to timestamps, then divide the occupation descriptor range, use the growth height descriptor to compare the scan frame with the local map, obtain the potential dynamic afterimage area and exclude ground points, and divide the bin into Point clouds that are not always occupied are regarded as dynamic afterimages and are eliminated directly. Point clouds that are always occupied in the bin are regarded as areas containing both dynamic afterimages and static point clouds. Specifically, the following sub-steps are included:

S5.1: Divide the point cloud data into multiple historical frames according to timestamps, and express the point cloud at time t+1 and the local map corresponding to time t as and/> Before dynamic afterimage detection, // Transform to world coordinate system/>

In the formula: Represents the laser scanning frame in the radar coordinate system;/> Represents the conversion relationship between the radar coordinate system and the world coordinate system;/> Represents the laser scanning frame in the world coordinate system;

S5.2: Record the position of the point cloud in the world coordinate system. The expression is as follows:

In the formula: is the position of the point cloud in the world coordinate system;/> Represents the position of the point cloud on the x-axis in the world coordinate system;/> Represents the position of the point cloud on the y-axis in the world coordinate system;/> Represents the position of the point cloud on the z-axis in the world coordinate system;

S5.3: Divide the current frame and the local map into occupancy descriptor ranges. Since the point cloud farther away from the origin is sparser, 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 in the bin. If there is no point cloud, it will not be considered. If there is one point, it is the maximum height point. If there are multiple points, sort them according to the z-axis height value, and calculate the height difference between two adjacent points in sequence. If It is always less than the threshold, and the last point is taken as the maximum height point. If it is greater than the threshold, the point with the smaller height value is taken as the maximum height point, and the static point cloud above the dynamic afterimage in the bin is excluded, which solves the problem of the existence of static point cloud above the dynamic afterimage. When the height descriptor considers the point clouds in the grid to be static point clouds, the maximum height difference of each bin is calculated. and/> That is, the growth height descriptor and comparison:

S5.5: The area that meets the conditions is the potential dynamic afterimage area:

In the formula: M _D represents the potential dynamic afterimage area; Represents a potential dynamic point set;

S5.6: Since the ground points are static points, there is no need to consider the ground point information when judging dynamic afterimages. The ground points are excluded, and then the point clouds that are not always occupied in the bin are regarded as dynamic afterimages and eliminated, and they will always exist. The bin of a point cloud is regarded as a bin that contains both dynamic afterimages and static point clouds, and is recorded as Bins;

S6: Use spatio-temporal constraints to find points in Bins that appear in historical frames but do not exist in the current frame to distinguish static point clouds and dynamic afterimages in Bins in detail, then eliminate dynamic afterimages and build a static map.

The invention has the following beneficial effects:

1. Through dimensionality reduction processing and ground fitting algorithm, ground points are quickly separated, reducing the calculation time for judging dynamic afterimages, and increasing calculation efficiency.

2. Due to the sparsity of point clouds, equally spaced bin division will result in denser point clouds in areas closer to the center of the circle, and sparse point clouds in areas farther from the center of the circle, affecting the ground fitting effect. Through adjacent laser beams and ground The difference in radius of the concentric circles formed by the plane divides the bins, making the point cloud data in the bins evenly distributed, making the fitted ground points more accurate.

3. The noise is eliminated through the Euclidean clustering algorithm, and it can operate in a noisy environment. When the IMU initialization fails, the system runs the laser odometry mode, otherwise it runs the laser inertial odometry mode, which improves the stability of the system.

4. Propose a growth height descriptor method. A single height descriptor only takes the maximum height difference as the difference in grid occupancy status. When there is a static point cloud above the dynamic afterimage, the point clouds in the grid will be considered to be static points. The cloud and growth height descriptor method can exclude static point clouds above dynamic afterimages, quickly and accurately find potential dynamic afterimage areas, add spatiotemporal constraints to divide dynamic afterimages and static point clouds in detail, and accurately eliminate dynamic afterimages.

Description of drawings

In order to explain the embodiments of the present invention or the technical solutions in the prior art more clearly, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description are only These are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without exerting creative efforts.

Figure 1 is a flow chart of a laser SLAM method to solve dynamic afterimages;

Figure 2 is a schematic diagram of the radius difference of the concentric circles formed by adjacent laser beams and the ground plane;

Figure 3 is a schematic diagram of grid division;

Figure 4 is a schematic diagram of the ground fitting process;

Figure 5 is a flow chart of the growth height descriptor method;

Figure 6 shows the mapping using the LIO-SAM method;

Figure 7 shows the mapping using the ERASOR method;

Figure 8 shows a laser SLAM method for mapping to solve dynamic afterimages.

Detailed ways

In order to make the above objects, features and advantages of the present invention more obvious and easy to understand, a laser SLAM method to solve dynamic afterimages, as shown in Figure 1, includes the following steps:

S1: Orderly process the obtained point cloud data, and divide the raster using the radius difference of the concentric circles formed by the adjacent laser beams and the ground plane, as shown in Figure 2; for the method of dividing the raster at equal intervals, there will be distance from the laser The point cloud in the grid close to the radar is dense, and the point cloud in the grid far away from the lidar is sparse; this method can make the point cloud evenly distributed in the grid and accurately fit the ground points; through dimensionality reduction, the three-dimensional The transformation of coordinate points into two-dimensional coordinate points specifically includes the following sub-steps:

S1.1: Convert the x-y coordinates in the Cartesian coordinate system into a circular surface with infinite radius, set the radian parameter δθ, and divide the circular surface into M sectors according to the radian parameter δθ. The sector name is Sector:

S1.2: Order each point in the x-y plane:

In the formula: Sector _p(i) represents the point cloud i in Sector.

S1.3: Divide the point cloud and convert the disordered point cloud into an ordered point cloud:

P _s ={p _i ∈P|Sector _p(i) =s} (3)

In the formula: P represents all point clouds; p(i) represents point cloud i; s represents Sector s; P _s represents the point cloud collection of Sector s.

S1.4: Further divide each sector in an orderly manner, and divide each sector into multiple sector ring bins, as shown in Figure 3, divided by the radius difference of the concentric circles formed by the adjacent laser beams and the ground plane:

In the formula: Δr _i,i+1 represents the radius difference between the concentric circles formed by the laser beam and the ground plane; h represents the height of the lidar from the ground; a _i represents the angle between the i-th laser beam and the ground plane; a _i+1 Represents the angle between the i+1th laser beam and the ground plane.

S1.5: Dimensionality reduction processing of point cloud data:

{x,y,z}＝{d,z}+α (5)

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

S2: Fit the ground points through the ground point fitting algorithm, and treat the fitted ground points as static points, which specifically includes the following sub-steps:

S2.1: Set the fitting straight line:

z＝kd+b (6)

In the formula: k is the slope of the straight line; b represents the constant term.

S2.2: Limit the absolute value of the slope of the straight line. If the slope is greater than the threshold, a vertical structure will appear:

In the formula: |k| represents the absolute value of the slope of the straight line; Indicates the set threshold.

S2.3: The ground is flat when the slope is less than the threshold, and 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 straight line does not exceed the set error threshold:

Rmse＜E _Rmse (9)

In the formula: Rmse represents the root mean square error of the fitted straight line; E _Rmse represents the error threshold.

S2.5: Get the first point in the bin in the Sector, and then determine the distance between the point and the existing straight line. The distance does not exceed the set threshold. If it is less than the threshold, the point can be fitted to the straight line. If If it is greater than the threshold, then use this point as the basis to start a new straight line and divide different straight lines, as shown in Figure 4.

S2.6: Screen the ground points through the fitted straight line. Calculate the distance from the point to the straight line based on the filtered straight line and the set parameters. Points within the threshold range are the selected ground points.

S3: Use the clustering algorithm to cluster non-ground points, remove point clouds with less than 20 points in the cluster as noise, and perform feature extraction, which specifically includes the following sub-steps:

S3.1: Based on the Euclidean distance as the judgment criterion, select a point P and use KD-Tree to find the k closest points to the point P. If the distance to the point P is less than the set threshold, put it into the point set M until it is in M If the elements no longer increase, the clustering ends. Otherwise, continue to search for points other than point P, and remove the clustered point cloud obtained with less than 20 points.

S3.2: Calculate the curvature of each point in the laser scanning frame, and then extract the edge features and plane features of the scanning frame. The curvature calculation formula is as follows:

In the formula: c represents the curvature of the point cloud; s represents the set of continuous points; p _i and p _j represent the i-th point and j-th point respectively.

S3.3: Let p _i be the target point, and the edge features and plane features of the lidar scanning frame are expressed as and Convert it to the world coordinate system as/> and/> Use the nearest neighbor search method to locate relevant feature points in and/> , calculate the distance from the target point to the relevant edge point:

In the formula: Represents the distance between the target point and the relevant edge point;/> Represents edge feature points;/> and/> Respectively expressed/> correspond to two different points on the edge line.

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

In the formula: Represents the distance between the target point and the relevant plane;/> Represents plane feature points;/> and/> Express/> Three different points on the corresponding plane.

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

S4: Initialize the IMU. If the initialization is successful, the system will run the laser inertial odometry mode. Otherwise, the system will run the laser odometry mode, which specifically includes the following sub-steps:

S4.1: The original IMU measurement includes acceleration and angular velocity. The measurement values are all in the IMU coordinate system B, which is the same as the robot coordinate system. The IMU measurement model is expressed as:

In the formula: with/> represents the measured values of angular velocity and acceleration at time t; ω _t and a _t represent the true values of angular velocity and acceleration;/> and/> Indicates the zero bias of the gyroscope and accelerometer;/> Represents the rotation matrix of the world coordinate system and the IMU coordinate system;/> and/> represents the noise of the gyroscope and accelerometer; g represents the acceleration of gravity.

S4.2: Use IMU measurement to estimate the speed v _t+Δt , displacement p _t+Δt and rotation R _t+Δt of the robot at time t+Δt. The calculation formula is:

In the formula: v _t represents the velocity at time t; p _t represents the displacement at time t.

S4.3: The IMU initialization is successful and the system runs the laser inertial odometry mode, otherwise it runs the laser odometry mode.

S5: Divide the point cloud data into multiple historical frames according to timestamps, then divide the occupation descriptor range, and introduce the growth height descriptor. The growth height descriptor flow chart is shown in Figure 5. The growth height descriptor is used to scan the frame and Compare local maps to obtain potential dynamic afterimage areas and exclude ground points. Point clouds that are not always occupied in the bin are regarded as dynamic afterimages and are directly eliminated. Point clouds that are always occupied in the bin are regarded as containing both dynamic afterimages and The static point cloud area specifically includes the following sub-steps:

S5.1: Divide the point cloud data into multiple historical frames according to timestamps, and express the point cloud at time t+1 and the local map corresponding to time t as and/> Before dynamic afterimage detection, // Transform to world coordinate system/>

In the formula: Represents the laser scanning frame in the radar coordinate system;/> Represents the conversion relationship between the radar coordinate system and the world coordinate system;/> Represents the laser scanning frame in the world coordinate system.

S5.2: Record the position of the point cloud in the world coordinate system. The expression is as follows:

In the formula: is the position of the point cloud in the world coordinate system;/> Represents the position of the point cloud on the x-axis in the world coordinate system;/> Represents the position of the point cloud on the y-axis in the world coordinate system;/> Represents the position of the point cloud on the z-axis in the world coordinate system.

S5.3: Divide the current frame and the local map into occupancy descriptor ranges. Since the point cloud farther away from the origin is sparser, 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 in the bin. If there is no point cloud, it will not be considered. If there is one point, it is the maximum height point. If there are multiple points, sort them according to the z-axis height value, and calculate the height difference between two adjacent points in sequence. If It is always less than the threshold, and the last point is taken as the maximum height point. If it is greater than the threshold, the point with the smaller height value is taken as the maximum height point, and the static point cloud above the dynamic afterimage in the bin is excluded, which solves the problem of the existence of static point cloud above the dynamic afterimage. When , the height descriptor will think that the point clouds in the grid are all static point clouds, and calculate the maximum height difference of each bin. and/> That is, the growth height descriptor and comparison:

S5.5: The potential dynamic area is the one that meets the conditions:

In the formula: M _D represents the potential dynamic afterimage area; Represents a set of potentially dynamic points.

S5.6: Since the ground points are static points, there is no need to consider the ground point information when judging dynamic afterimages. The ground points are excluded, and then the point clouds that are not always occupied in the bin are regarded as dynamic afterimages and eliminated, and they will always exist. The bins of point clouds are regarded as bins that contain both dynamic afterimages and static point clouds, and are recorded as Bins.

S6: Use spatio-temporal constraints to find points in Bins that appear in historical frames but do not exist in the current frame to distinguish static point clouds and dynamic afterimages in Bins in detail, then eliminate dynamic afterimages and build a static map.

This invention divides grids by using the radius difference of concentric circles formed by adjacent laser beams and the ground plane, so that the point cloud in each grid is evenly distributed, accurately fits the ground points, and eliminates noise through the Euclidean clustering algorithm, using laser The switching mechanism between inertial odometry and laser odometry improves the stability of the system. The growth height descriptor method is proposed to solve the problem of mistakenly treating the dynamic afterimage as a static point cloud when there is a static point cloud above the dynamic afterimage in the grid, and introduces space and time. Constraints remove dynamic afterimages and complete map construction.

The present invention is verified below based on the test results. Figure 6 shows the mapping using the LIO-SAM method. There are a large number of dynamic afterimages in the figure. Figure 7 shows the mapping using the ERASOR method. Some of the dynamic afterimages in the figure have not been removed. Due to some There are static point clouds above the dynamic afterimages in the grid. Using height descriptors will mistakenly regard these dynamic afterimages as static point clouds, so part of the dynamic afterimages are retained. Figure 8 shows a laser SLAM method to solve the dynamic afterimages. Figure, it can be seen from the figure that this method eliminates the dynamic afterimages under the static point cloud; compared with the ERASOR method for mapping, this method improves the elimination rate of dynamic afterimages; this method introduces the growth height descriptor and space-time Constraints solve the impact of dynamic obstacles on mapping.

The above-mentioned specific embodiments further illustrate the purpose, technical solutions and beneficial effects of the present invention. The above examples are only used to illustrate the technical solutions of the present invention, rather than limiting the creative protection scope of the present invention. Ordinary people in the art Skilled persons should understand that any modifications or equivalent substitutions made to the technical solutions of the present invention are included in the protection scope of the present invention.

Claims (1)

1. A laser SLAM method for solving dynamic ghost is characterized by comprising the following steps:

s1: the method comprises the following steps of sequentially processing the acquired point cloud data, dividing grids by utilizing concentric circle radius differences formed by adjacent laser beams and a ground plane, enabling the point cloud to be uniformly distributed in the grids, and converting three-dimensional coordinate points into two-dimensional coordinate points through dimension reduction, wherein the method specifically comprises the following sub-steps:

s1.1: converting an x-y coordinate in a Cartesian coordinate system into a circular surface with an infinite radius, setting an radian parameter delta theta, dividing the circular surface into M sectors according to the radian parameter delta theta, and marking the sectors as sectors;

s1.2: carrying out x-y plane ordering treatment on each point:

wherein: sector _p(i) Representing a point cloud i in a Sector;

s1.3: dividing point clouds, and converting unordered point clouds into ordered point clouds;

s1.4: each Sector is further divided in order, each Sector is divided into a plurality of Sector ring bins, and the Sector rings are divided by the concentric circle radius difference formed by adjacent laser beams and the ground plane:

wherein: Δr _i,i+1 Representing the difference of the radius of the concentric circles formed by the laser beam and the ground plane; h represents the height of the laser radar from the ground; a, a _i Representing the included angle between the ith laser beam and the ground plane; a, a _i+1 Representing the included angle between the (i+1) th laser beam and the ground plane;

s1.5: performing dimension reduction processing on the point cloud data:

{x,y,z}＝{d,z}+α (3)

wherein:alpha represents angle information of an x-y plane; { x, y, z } represents the coordinates of the laser spot in the x-axis, y-axis, and z-axis;

s2: fitting out ground points through a ground point fitting algorithm, and regarding the fitted ground points as static points, wherein the method specifically comprises the following substeps:

s2.1: setting a fitting straight line:

z＝kd+b (4)

wherein: k is the slope of a straight line; b represents a constant term;

s2.2: defining the absolute value of the slope of the straight line, wherein the slope is larger than a threshold value and can show a vertical structure;

s2.3: the ground being level when the slope is less than the threshold value, when the slope is less than the minimum slopeWhen z-b does not exceed a specific threshold T _b ；

S2.4: the root mean square error of the fitting straight line does not exceed the set error threshold E _Rmse ；

S2.5: acquiring a first point in a bin in a Sector, judging the distance from the point to an existing straight line, if the distance is smaller than a set threshold value, fitting the point to the straight line, and if the distance is larger than the threshold value, restarting a straight line by taking the point as a reference to divide different straight lines;

s2.6: screening ground points through the fitted straight line, and calculating the distance from the points to the straight line according to the screened straight line and the set parameters, wherein the points in the threshold range are selected ground points;

s3: clustering non-ground points by using a clustering algorithm, and eliminating point clouds lower than 20 in the clustering as noise to extract features, wherein the method specifically comprises the following substeps:

s3.1: selecting a point P according to the Euclidean distance as a judgment criterion, searching k points closest to the point P through KD-Tree, if the distance from the point P is smaller than a set threshold value, putting the points into a point set M until elements in the points M are not increased any more, ending clustering, otherwise, continuously searching points except the point P, and removing the acquired cluster point cloud with less than 20 points;

s3.2: calculating the curvature of each point in the laser scanning frame, and then extracting edge features and plane features of the scanning frame:

wherein: c represents the curvature of the point cloud; s represents a set of consecutive points; p is p _i And p _j Respectively representing an ith point and a jth point;

s3.3: let p be _i For the target point, the edge and plane features of the lidar scanning frame are respectively represented asAnd->Converting it to +.>And->Positioning the relevant feature points at +.>Andcalculating the distance from the target point to the relevant edge point;

s3.4: calculating the distance from the target point to the associated plane;

s3.5: estimating the pose of the current frame and the local map by solving an optimization problem;

s4: initializing the IMU, wherein the successful system is initialized to operate the laser inertial odometer mode, otherwise, the laser inertial odometer mode is operated, and the method specifically comprises the following substeps:

s4.1: the original IMU measurement comprises acceleration and angular velocity, and the measurement values are the same as the robot coordinate system under the IMU coordinate system B;

s4.2: estimating the pose of the robot at the time t+delta t by using IMU measurement;

s4.3: the IMU is initialized to successfully operate the laser inertial odometer mode of the system, otherwise, the laser odometer mode is operated;

s5: dividing the point cloud data into a plurality of sections of historical frames according to time stamps, then dividing the occupied description sub-range, comparing a scanning frame with a local map by utilizing a growth height descriptor, acquiring a potential dynamic residual image region, excluding ground points, taking the point cloud which is not occupied all the time in the bin as a dynamic residual image, directly removing the point cloud, taking the point cloud which is occupied all the time in the bin as a region which contains both the dynamic residual image and the static point cloud, and specifically comprising the following substeps:

s5.1: dividing the point cloud data into a plurality of sections of historical frames according to time stamps, and respectively representing the point cloud at the time t+1 and a local map corresponding to the time t asAnd->Before dynamic ghost detection, will +.>Transition to world coordinate System +.>

S5.2: recording the position of the point cloud under a world coordinate system;

s5.3: dividing the occupied description sub-range of the current frame and the local map, and selecting an area with the height of-1 meter to +3 meters in a radius range of 70 meters from the original point because the point cloud which is farther from the original point is more sparse;

s5.4: acquiring a point cloud in a bin, if the point cloud is not considered, one point is the maximum height point, a plurality of points are present, the height difference between two adjacent points is calculated in sequence according to the size of the z-axis height value, and if the height difference is always smaller than a threshold value, the last point is taken as the maximum heightIf the height value of the point is larger than the threshold value, taking the point with a small height value as the maximum height point, eliminating the static point cloud above the dynamic ghost in the bin, and calculating the maximum height difference of each binAnd->I.e. growing a height descriptor and comparing:

s5.5: the potential dynamic ghost areas are the conditions met;

s5.6: taking the ground points as static points without consideration, taking the point cloud which is not occupied all the time in the bin as dynamic ghost, directly removing, taking the bin with the point cloud all the time as the bin containing both the dynamic ghost and the static point cloud, and recording as Bins;

s6: and searching points which appear in the historical frame but do not exist in the current frame in the Bins by utilizing space-time constraint, distinguishing static point cloud and dynamic ghost images in the Bins in detail, and then eliminating the dynamic ghost images to construct a static map.