Livox laser radar-based dynamic object filtering method for three-dimensional point cloud map

Abstract

The present invention relates to a Livox laser radar-based dynamic object filtering method for a three-dimensional point cloud map. The method comprises: saving point clouds and corresponding poses of all frames of a mobile robot during mapping, setting the saved point cloud of each frame as a query frame, and converting point cloud data of the query frames and of a point cloud map into a fan-shaped grid cluster point cloud descriptor represented by local height extreme value difference information; on the basis of the fan-shaped grid cluster point cloud descriptor, by means of a region-adaptive point cloud ground extraction method, segmenting to obtain a ground point cloud estimation result; and obtaining a potential dynamic region by means of a descriptor comparison method, and on the basis of the ground point cloud estimation result, replacing a dynamic point cloud in an original point cloud map with a ground point cloud segmented in a two-dimensional grid in the query frames. The present invention can eliminate smears generated by dynamic objects in existing three-dimensional point cloud maps, and obtain a static point cloud map allowing for robot positioning and autonomous navigation.

Description

Dynamic object filtering method for 3D point cloud map based on Livox lidar Technical Field

[0001] The present invention relates to a method for filtering dynamic objects in a three-dimensional point cloud map based on a Livox laser radar, and belongs to the field of autonomous mobile robots. Background Art

[0002] With the advancement of science and technology and the increasing demands of society, robots have gradually become part of people's lives, with their applications expanding across a wider range and playing an increasingly important role in all aspects of human life. Currently, with the booming development of artificial intelligence, the mobile robotics industry is also experiencing rapid growth. An increasing number of mobile robot products are moving from experimental to practical applications, such as restaurant service robots and household sweeping robots in indoor scenarios. Furthermore, in outdoor scenarios, such as campus cleaning robots and warehouse logistics robots, people are placing higher demands on the autonomy and intelligence of mobile robots. In particular, robots offer irreplaceable advantages in specialized scenarios, such as post-disaster search and rescue, high-altitude operations, extreme environments like high and low temperatures and strong radiation, and deep space planetary exploration, assisting humans in completing tasks. The emergence of robots has effectively increased productivity, and continuously improving their intelligence and autonomy is a key development direction for the future of robotics.

[0003] For mobile robots to achieve unmanned and intelligent operation, reliable autonomous navigation is crucial. Autonomous navigation has been a hot research area, and Simultaneous Localization and Mapping (SLAM) technology is key to solving this problem. With precise and stable SLAM algorithms, a robot can determine its own position and perceive the surrounding structure for mapping. Combined with motion planning algorithms, it can search for a navigable path to its target, thereby completing autonomous navigation.

[0004] Using sensors like lidar and cameras, robots can reconstruct the spatial structure of their surroundings online. However, most existing SLAM methods assume that the environment they perceive is static and time-invariant, a significant limitation of SLAM systems' applicability in real-world scenarios. Dynamic objects can cause significant artifacts in the resulting map, negatively impacting subsequent map-based positioning and path planning. Therefore, filtering out dynamic objects from 3D point cloud maps is crucial for autonomous navigation of mobile robots.

[0005] Summary of the Invention

[0006] The present invention provides a method for filtering dynamic objects in a three-dimensional point cloud map based on Livox laser radar, aiming to solve at least one of the technical problems existing in the prior art.

[0007] The technical solution of the present invention relates to a method for filtering dynamic objects in a three-dimensional point cloud map. The method according to the present invention comprises the following steps:

[0008] S100, saving the point clouds and corresponding poses of all frames of the mobile robot during the mapping process, setting each saved point cloud frame as a query frame; converting the query frame and the point cloud map point cloud data into a fan-shaped grid cluster point cloud descriptor represented by local height extreme value difference information;

[0009] S200, obtaining a ground point cloud estimation result by segmenting the point cloud using a region-adaptive point cloud ground extraction method according to the sector-shaped grid cluster point cloud descriptor;

[0010] S300 , obtaining potential dynamic areas by comparing descriptors, and replacing the dynamic point cloud in the original point cloud map with the ground point cloud segmented within the two-dimensional grid in the query frame according to the ground point cloud estimation result.

[0011] Furthermore, the step S200 includes the following steps:

[0012] S110, using a two-dimensional grid model to represent the three-dimensional point cloud data according to a sector grid cluster division strategy;

[0013] S120: Set the storage information of each two-dimensional grid as the height extreme value difference within the grid.

[0014] Furthermore, in the sector grid cluster division strategy, the entire sector is divided into four areas, and the grid sizes near the center area and at distant locations are artificially set to be larger.

[0015] Furthermore, the height extreme value difference of the two-dimensional grid is obtained by the following calculation: Δh n =max{H n}-min{H n}

[0016] Where Δh n represents the height extreme value difference of the two-dimensional grid; Represents the set of height values ​​of all points in the two-dimensional grid, where represents the nth two-dimensional grid after division, p k Represents the kth point in the 3D point cloud data within the grid.

[0017] Furthermore, the step S200 includes the following steps:

[0018] S210, calculate and obtain the center point of a point set in a two-dimensional grid, subtract the center point from each point to obtain a point set A, and calculate its covariance matrix

[0019] S220, calculate and obtain the average value of the lowest height point of each two-dimensional grid, adjust it and use it as the initial ground estimation point, and then iteratively perform ground point cloud estimation. The calculation is expressed as follows:

[0020] Where, Indicates the lowest point z of all two-dimensional grids root The average value, z init represents a small fixed bias when making the initial ground estimate, represents the initial ground estimation point; represents the result of the lth ground estimation of the nth grid, express The center point of .

[0021] Furthermore, step S300 includes the following steps:

[0022] S310: The original point cloud map is used as the set of query frame point clouds, and is converted from the world coordinate system to the lidar coordinate system. The coordinate system conversion calculation is as follows:

[0023] Where, represents the original point cloud map, Indicates that Convert to In the laser radar coordinate system at the moment; represents the pose corresponding to the k-th query frame;

[0024] S320, based on the position of the pose corresponding to the current query frame in the original point cloud map, select a certain range in the original point cloud map as a local point cloud map;

[0025] S330, converting the local point cloud map and the query frame into a fan-shaped grid cluster point cloud descriptor, and obtaining a potential dynamic area by comparing the descriptors;

[0026] S340 , searching the potential dynamic area to obtain the dynamic point cloud of the two-dimensional grid and filtering it out.

[0027] Furthermore, in step S340, the retrieval of the dynamic point cloud includes at least the following steps:

[0028] S341. For the two-dimensional grid at the same position, calculate

[0029] S342, when It is determined that there is a dynamic point cloud at this position in the point cloud map.

[0030] Furthermore, in step S340, the original dynamic point cloud in the two-dimensional grid in the point cloud map is replaced based on the ground point cloud estimation result and the point cloud of the same two-dimensional grid in the query frame to filter out the dynamic point cloud, which is calculated as follows:

[0031] Where, represents a point cloud used to replace the dynamic point cloud, represents the ground point cloud estimation result, A point cloud representing the same two-dimensional grid in the query frame.

[0032] The technical solution of the present invention also relates to a computer-readable storage medium having program instructions stored thereon, and the above-mentioned method is implemented when the program instructions are executed by a processor.

[0033] The technical solution of the present invention also relates to a three-dimensional point cloud map dynamic object filtering system based on Livox laser radar, wherein the system includes a computer device containing the above-mentioned computer-readable storage medium.

[0034] The beneficial effects of the present invention are as follows:

[0035] The present invention's dynamic object filtering method for 3D point cloud maps based on Livox laser radar eliminates the smearing caused by dynamic objects in existing 3D point cloud maps, generating a static point cloud map suitable for robot positioning and autonomous navigation. This method solves the problem of dynamic object smearing in maps created by robots using Livox laser radar. The static map is generated through post-processing, facilitating subsequent positioning and motion planning for mobile robots based on existing point cloud maps. The method employs an improved sector-shaped grid clustering strategy, which not only addresses the spatial representation of distant and near point clouds but also significantly reduces the number of grids, thereby effectively improving the efficiency of subsequent algorithms. The method employs the height extreme difference within the grid to represent the stored information of each 2D grid. Compared to the traditional Scan Context method of encoding descriptors using maximum height, this method is more effective in describing the spatial distribution of dynamic objects and is less susceptible to noise and error. Subsequent comparisons using grid descriptors based on this height extreme difference information also make it easier to identify potential areas of the map containing dynamic objects. The method of the present invention uses an iterative ground estimation method based on PCA to perform the method on each grid, has good regional adaptability, and can correctly and completely estimate the ground. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] FIG1 is a basic flow chart of a method for filtering dynamic objects in a three-dimensional point cloud map according to an embodiment of the present invention.

[0037] FIG. 2 a is a schematic diagram of a conventional uniform grid division strategy according to an embodiment of the present invention.

[0038] FIG2 b is a schematic diagram of an improved sector grid cluster division strategy according to an embodiment of the present invention.

[0039] FIG3 is a schematic diagram of a point cloud mesh descriptor using local height extreme value difference information according to an embodiment of the present invention.

[0040] FIG4 is a diagram showing the effect of region-adaptive point cloud ground segmentation according to an embodiment of the present invention.

[0041] FIG5 a is a diagram showing the effect of a dynamic point cloud before filtering according to an embodiment of the present invention.

[0042] FIG5 b is a diagram showing the effect of dynamic point cloud filtering according to an embodiment of the present invention. DETAILED DESCRIPTION

[0043] The following will provide a clear and complete description of the concept, specific structure and technical effects of the present invention in conjunction with the embodiments and drawings to fully understand the purpose, scheme and effects of the present invention.

[0044] It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly fixed or connected to the other feature, or it may be indirectly fixed or connected to the other feature. The singular forms "a", "said" and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art. The terms used in this specification are only for describing specific embodiments and are not intended to limit the invention. The term "and / or" used herein includes any combination of one or more related listed items.

[0045] Should be understood that, although the present disclosure may adopt the term first, second, third etc. to describe various elements, these elements should not be limited to these terms.These terms are only used to distinguish the elements of the same type from each other.For example, without departing from the scope of the present disclosure, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element.The use of any and all examples or exemplary language ("for example", "such as" etc.) provided herein is only intended to better illustrate embodiments of the present invention, and unless otherwise required, will not impose limitations on the scope of the present invention.

[0046] 1 to 5 , in some embodiments, the method for filtering dynamic objects in a three-dimensional point cloud map based on a Livox laser radar according to the present invention is applied to a mobile robot and includes at least the following steps:

[0047] S100, saving the point clouds and corresponding poses of all frames of the mobile robot during the mapping process, setting each saved point cloud frame as a query frame; converting the query frame and the point cloud map point cloud data into a fan-shaped grid cluster point cloud descriptor represented by local height extreme value difference information;

[0048] S200, based on the fan-shaped grid cluster point cloud descriptor, segment the ground point cloud using a region-adaptive point cloud ground extraction method to obtain a ground point cloud estimation result;

[0049] S300 , obtaining potential dynamic areas by comparing descriptors, and replacing the dynamic point cloud in the original point cloud map with the ground point cloud segmented within the two-dimensional grid in the query frame according to the ground point cloud estimation result.

[0050] The present invention addresses the problem of dynamic object smearing in point cloud maps created by robots using Livox laser radar when mapping in dynamic environments. Post-processing is used to remove the smearing and generate a static map, which facilitates the subsequent positioning and motion planning of the mobile robot based on the existing point cloud map. Specifically, the present invention employs a post-processing approach that requires saving the point clouds and corresponding poses of all frames of the mobile robot during the mapping process. Each saved point cloud frame is referred to as a query scan. The present invention utilizes a post-processing approach to filter dynamic objects from three-dimensional point cloud maps. The position of dynamic objects in point clouds varies from frame to frame, and smearing caused by a dynamic object at a certain location in a local point cloud map will not be present at the same location in all query frames. By traversing all query frames and comparing them with the point cloud map, dynamic objects can be filtered out. The present invention's dynamic object filtering method for three-dimensional point cloud maps includes at least three steps: converting point cloud data into a sector-shaped grid cluster descriptor, adaptive point cloud ground segmentation, and dynamic region screening based on descriptor comparison.

[0051] Furthermore, embodiments of the present invention utilize Livox LiDAR to collect 3D point cloud data. Specifically, existing dynamic object filtering algorithms all focus on mechanical LiDARs, which have fixed beams and angular resolution, allowing for easy conversion into range images. Dynamic objects are then filtered out by comparing these range images using methods such as viewpoint visibility. In recent years, solid-state LiDARs, with their advantages of low cost, long life, and automotive-grade performance, have become a growing trend in the LiDAR field. Livox's LiDAR products, such as AVIA and MID360, boast advantages such as small size, light weight, and low price. They have seen increasing adoption in robotics in recent years, becoming the preferred sensor for many LiDAR-equipped autonomous mobile robots. Embodiments of the present invention utilize Livox's AVIA hybrid solid-state LiDAR. This Livox hybrid solid-state LiDAR utilizes a rotating prism, resulting in a point cloud with non-repetitive scanning. This means that the laser beam's scan position within each frame of the point cloud is not fixed, and the resulting range image is uneven. Therefore, existing methods cannot be used to filter dynamic objects from point cloud maps.

[0052] In one embodiment, the method of the embodiment of the present invention converts point cloud data into a fan-shaped grid cluster descriptor, including at least the following steps:

[0053] S210, calculate and obtain the center point of a point set in a two-dimensional grid, subtract the center point from each point to obtain a point set A, and calculate its covariance matrix

[0054] S220: Calculate the average value of the lowest height point of each two-dimensional grid as the initial ground estimation point, and then iteratively perform ground point cloud estimation.

[0055] The grid clustering strategy adopted by the method in the embodiment of the present invention is an improved sector grid clustering strategy, which not only solves the spatial representation problem of point clouds at both far and near locations, but also significantly reduces the number of grids, thereby effectively improving the operational efficiency of subsequent algorithms. Referring to Figures 2a and 2b, the method in the embodiment of the present invention improves the grid clustering strategy based on Scan Context, dividing the entire sector into four regions along the radial direction. Based on the different grid sizes of each region, the grid sizes near the center and at distant locations are artificially set larger to solve the problem of uniform division.

[0056] In one application embodiment, the method of using a two-dimensional grid model to characterize three-dimensional point cloud data in an embodiment of the present invention is based on Scan Context. The three-dimensional point cloud descriptor of Scan Context is mainly used for loop detection and position recognition in laser SLAM. It divides a frame of three-dimensional point cloud data into angles and radii evenly in a polar coordinate system centered on the laser radar itself, uses the maximum value of the point cloud height in each grid as the characterization information of the grid, encodes a frame of point cloud into a two-dimensional matrix, and performs loop detection by comparing the similarity of grid matrices converted from different frames of point cloud. Among them, through the method of Scan Context that describes point clouds in grids, a large-scale point cloud is converted into a two-dimensional grid representation, and the maximum height is used as the encoding basis. The spatial distribution characteristics of the point cloud can be effectively retained without complicated calculations, while also having rotation invariance, and is suitable for the loop detection field of laser SLAM.

[0057] In one application embodiment, the present invention utilizes a Scan Context descriptor approach to improve both the meshing strategy and mesh information representation for Livox solid-state LiDARs with small FoVs, enabling dynamic object filtering in 3D point cloud maps. Specifically, mechanical LiDARs typically have a 360° horizontal FoV, while the Livox AVIA solid-state LiDAR used in this invention has a horizontal FoV of 70.4°. Given this FoV constraint, the present invention utilizes sector-shaped rather than circular regions for meshing, using the Scan Context descriptor to divide the point cloud space into regularly spaced grids in both angular and radial directions.

[0058] In an application embodiment, the sector grid cluster division strategy of the embodiment of the present invention improves the grid division strategy based on Scan Context, divides the entire sector into four areas from the radial direction, and artificially sets the grid size near the center area and at a distance to be larger, thereby solving the problem that the farther away from the center of the lidar, the sparser the point cloud is and the number of point clouds in the distant grid is usually small, which is not conducive to subsequent ground estimation in the two-dimensional grid, and solves the problem that the grid size near the center is usually small and the number is too large, which is not conducive to characterizing the distribution of point clouds in space.

[0059] In one embodiment, the method of the embodiment of the present invention sets the storage information of each two-dimensional grid to the height extreme value difference within the two-dimensional grid. Specifically, according to the Scan Context, the information stored in each two-dimensional grid is the maximum height of all point clouds in the two-dimensional grid, so that the geometric feature distribution in the space can be approximately expressed, and position recognition and loop detection can be conveniently performed. In the three-dimensional point cloud map dynamic object filtering method of the embodiment of the present invention, the information stored in each two-dimensional grid is set to the height extreme value difference within the two-dimensional grid. Among them, the height extreme value difference Δh of the two-dimensional grid n Obtained by the following calculation: Δh n =max{H n}-min{H n}

[0060] Where Δh n represents the height extreme value difference of the two-dimensional grid; Represents the set of height values ​​of all points in the two-dimensional grid, where represents the nth two-dimensional grid after division, p k Represents the kth point in the 3D point cloud data within the grid. It can be understood that for a frame of point cloud Take one point p k , its coordinates p k ={x k ,y k , z k}, then z k Indicates the height of the point.

[0061] The method of the embodiment of the present invention uses the height extreme value difference within the grid to represent the storage information of each of the two-dimensional grids. Compared with the traditional method of using the maximum height to encode the descriptor in the Scan Context, the use of height extreme value difference information is more conducive to describing the distribution of dynamic objects in space, and is less affected by noise and errors. When the grid descriptor using the height extreme value difference information is subsequently compared, it is also easier to screen out potential areas where dynamic objects exist in the map.

[0062] Furthermore, see Figure 3 for the visualization result of using extreme height difference as a representation of grid information. In the fan-shaped grid cluster, different colors are used to represent the size of the extreme height difference within the grid: for example, blue indicates that the extreme height difference within the grid is very small, which can be considered as a potential ground area; green indicates that the extreme height difference within the grid is medium, which is a potential dynamic pedestrian area; red indicates that the extreme height difference within the grid is large, which may be a tree or a building.

[0063] In one embodiment, the present invention utilizes a principal component analysis (PCA)-based method to iteratively estimate the ground point cloud within each two-dimensional grid using the obtained fan-shaped grid cluster point cloud descriptor. This allows for adaptive point cloud ground segmentation based on the flatness of the region. This method, based on the fact that most dynamic objects in the real world inevitably come into contact with the ground, segments the ground from the point cloud. This facilitates subsequent screening of dynamic objects and helps detect traversable areas during robot navigation.

[0064] Specifically, first represent the point set P in a two-dimensional grid in the following matrix form:

[0065] And the center of the point set can be obtained by calculation The point set A obtained by subtracting the center from each point is expressed in matrix form as follows:

[0066] Then calculate the covariance matrix It is a 3×3 symmetric matrix with three eigenvalues. It can be understood that the eigenvector corresponding to the smallest eigenvalue is most likely to represent the normal vector of the ground contained in the grid.

[0067] The method of the embodiment of the present invention first performs an initial ground estimation. For each two-dimensional grid, the point with the lowest height is most likely to be a ground point, which is recorded as z root Then, calculate the z of each grid root use represents the z of all grids root For the nth grid, the height is less than The point is retained as the initial ground estimate point, z init is a small fixed offset when performing initial ground estimation. Through the above initial ground estimation, points above a certain height can be excluded in the initial estimation stage. It represents the average value of the lowest points of all grids, which can reduce the impact of uneven ground. It is calculated as follows:

[0068] Where, Indicates the lowest point z of all two-dimensional grids root The average value, z init represents a small fixed bias when making the initial ground estimate, represents the initial ground estimation point; represents the result of the lth ground estimation of the nth grid, express The center point of .

[0069] After obtaining the initial ground estimation point, the ground estimation is performed iteratively. Specifically, let the result of the l-th ground estimation for the n-th grid be The normal vector can be calculated by the PCA algorithm and recorded as express The iteration of the method of the embodiment of the present invention includes at least the following steps: if the projection length of the vector formed by connecting a point with the center of the current ground estimation point cloud in the direction of the normal vector of the current ground estimation point cloud is less than the threshold It will be retained as the next ground estimation point cloud

[0070] It should be noted that the termination condition of the ground point cloud iterative estimation is That is, when the difference in the z direction of the center points of the two estimated ground point cloud results is less than 5 cm, the ground point cloud segmentation within the grid is considered to be completed and the iterative estimation operation is exited.

[0071] The point cloud ground extraction method of the embodiment of the present invention has good regional adaptability because it uses an iterative ground estimation method based on PCA for each two-dimensional grid. The commonly used point cloud ground segmentation method is to fit the ground plane using the RANSAC algorithm, which assumes that the ground in a frame of point cloud can be represented by the same plane model. This method performs poorly for scenes such as uneven roads and slopes, and can cause problems such as insufficient ground segmentation or segmentation errors. Referring to Figure 4, the ground segmentation algorithm of the embodiment of the present invention is compared with the RANSAC ground fitting algorithm. In the figure, the green points represent the segmented ground points, and the red points represent non-ground points. It can be seen from the figure that the RANSAC algorithm cannot completely segment the ground for slopes and uneven roads, and there is under-segmentation. For narrow paths for pedestrians, it cannot segment the road surface farther away, and it is easy to mistakenly segment the shoulder as the ground. However, the PCA-based ground point cloud segmentation algorithm of the present invention can correctly and completely estimate the ground in all these scenarios.

[0072] In one embodiment, the method of the embodiment of the present invention saves the point cloud and pose of all frames in the mapping process of a SLAM cycle as query frames, where the point cloud information contained in the kth query frame is recorded as It is expressed in the laser radar coordinate system, using represents the pose corresponding to the k-th query frame, Furthermore, the original point cloud map is recorded as The original point cloud map is taken as the collection of all query frame point clouds, which is expressed as follows:

[0073] It should be noted that the original point cloud map uses the world coordinate system as the reference system, while the point cloud of the query frame uses the lidar coordinate system as the reference system. Convert to The laser radar coordinate system at the time is converted to facilitate subsequent comparison. The coordinate system conversion is obtained by the following calculation:

[0074] Where, represents the original point cloud map, Indicates that Convert to In the laser radar coordinate system at the moment; represents the pose corresponding to the k-th query frame.

[0075] In one application embodiment, the method of the present invention requires preprocessing the query frame and original point cloud map before performing dynamic object filtering. This addresses the issue of the original point cloud map being large in scope and having a large number of point clouds, while the number of point clouds within each query frame is limited, thereby accelerating the post-processing algorithm. Specifically, based on the position of the pose corresponding to the current query frame in the original point cloud map, a local sub-map within a certain range of the original point cloud map is selected as the local point cloud map. Subsequently, only the local sub-map is compared with the query frame, thereby accelerating the computational speed of the entire post-processing process.

[0076] In one application embodiment, both the local point cloud map and the query frame are converted into fan-shaped grid descriptors obtained in the above steps, which are characterized by local height extreme value difference information. Based on the unified reference coordinate system, each two-dimensional grid descriptor is directly compared. Specifically, based on the characteristics of most dynamic objects in urban environments, such as pedestrians and vehicles, which inevitably come into contact with the ground, in the method of the embodiment of the present invention, there are three possible situations when comparing the local point cloud map and the query frame: first, there is an object on the ground at a certain location in the point cloud map, but there is no object on the ground at the same location in the query frame; second, there is an object on the ground at a certain location in the point cloud map and at the same location in the query frame; third, there is no object on the ground at a certain location in the point cloud map and at the same location in the query frame.

[0077] The method of the embodiment of the present invention regards the first case as a potential dynamic area in the point cloud map. This paper finds all potential dynamic areas in the point cloud map by descriptor comparison, and then filters out the dynamic points by retrieving the potential dynamic areas. Specifically, for the grid at the same position, use represents the height extreme value difference of the two-dimensional grid corresponding to the query frame, and Represents the height extreme value difference of the two-dimensional grid corresponding to the original point cloud map. First, calculate Since the above ratio should be close to 1 for static areas, if Much greater than Then there is a dynamic object at this position in the point cloud map. In some specific embodiments of the present invention, when setting Dynamic point cloud filtering is performed on the grid of the point cloud map.

[0078] In an application embodiment, in the dynamic point cloud filtering of the embodiment of the present invention, the above-mentioned point cloud ground segmentation method is used to perform iterative ground estimation based on principal component analysis on the grid. The ground point cloud estimation result is added to the point cloud of the same grid in the query frame to replace the original point cloud in the grid in the point cloud map, thereby achieving the filtering of the dynamic point cloud. The calculation is as follows:

[0079] Where, Represents the point cloud used to replace the dynamic point cloud, Represents the ground point cloud estimation result, 5a and 5b , the method of the embodiment of the present invention can effectively eliminate the smear caused by dynamic objects in the existing three-dimensional point cloud map.

[0080] It should be appreciated that the method steps in the embodiments of the present invention can be implemented or executed by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable memory. The method can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if desired, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, for this purpose, the program can be run on a programmed application-specific integrated circuit.

[0081] Furthermore, the operations of the processes described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by the context. The processes described herein (or variations and / or combinations thereof) may be performed under the control of one or more computer systems configured with executable instructions and may be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) that is executed collectively on one or more processors, by hardware, or a combination thereof. The computer program includes a plurality of instructions that can be executed by one or more processors.

[0082] Further, the method can be implemented in any type of computing platform that is operably connected to a suitable computer, including but not limited to a personal computer, a minicomputer, a mainframe, a workstation, a network or distributed computing environment, a separate or integrated computer platform, or in communication with a charged particle tool or other imaging device, etc. Various aspects of the present invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, an optical read and / or write storage medium, an RSM, a ROM, etc., so that it can be read by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the process described herein. In addition, the machine-readable code, or portions thereof, can be transmitted over a wired or wireless network. When such media includes instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor, the invention described herein includes these and other different types of non-transitory computer-readable storage media. When programmed according to the methods and techniques of the present invention, the present invention can also include the computer itself.

[0083] The computer program can be applied to input data to perform the functions described herein, thereby converting the input data to generate output data that is stored in a non-volatile memory. The output information can also be applied to one or more output devices such as a display. In a preferred embodiment of the present invention, the converted data represents a physical and tangible object, including a specific visual depiction of the physical and tangible object produced on the display.

[0084] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the aforementioned embodiments. As long as the technical effects of the present invention are achieved by the same means, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention. Within the scope of protection of the present invention, various modifications and variations of the technical solutions and / or implementation methods are possible.

Claims

1. A method for filtering dynamic objects in a three-dimensional point cloud map, applied to a mobile robot, characterized in that: The method comprises the following steps: S100, saving the point clouds and corresponding poses of all frames of the mobile robot during the mapping process, and setting each saved point cloud frame as a query frame; converting the query frame and the point cloud map point cloud data into a fan-shaped grid cluster point cloud descriptor represented by local height extreme value difference information; S200, obtaining a ground point cloud estimation result by segmenting the point cloud using a region-adaptive ground extraction method according to the fan-shaped grid cluster point cloud descriptor; S300, obtaining a potential dynamic area by comparing descriptors, and replacing the dynamic point cloud in the original point cloud map with the ground point cloud segmented from the two-dimensional grid in the query frame according to the ground point cloud estimation result.

2. The method according to claim 1, characterized in that The step S200 includes the following steps: S110, using a two-dimensional grid model to characterize the three-dimensional point cloud data according to a fan-shaped grid cluster division strategy; S120: Set the storage information of each two-dimensional grid as the height extreme value difference within the grid.

3. The method according to claim 2, characterized in that in, In the sector grid cluster division strategy, the entire sector is divided into four areas, and the grid sizes near the central area and far away are artificially set larger.

4. The method according to claim 2, characterized in that: in, The height extreme value difference of the two-dimensional grid is obtained by the following calculation: Δh n =max{H n }-min{H n } In the formula, Δh n represents the height extreme value difference of the two-dimensional grid; Represents the set of height values ​​of all points in the two-dimensional grid, where represents the nth two-dimensional grid after division, p k Represents the kth point in the three-dimensional point cloud data within the two-dimensional grid.

5. The method according to claim 2, characterized in that: The step S200 includes the following steps: S210, calculate and obtain the center point of a point set in a two-dimensional grid, subtract the center point from each point to obtain a point set A, and calculate its covariance matrix S220, calculate and obtain the average value of the lowest point of each two-dimensional grid, adjust it and use it as the initial ground estimation point, and then iteratively perform ground point cloud estimation, and the calculation is expressed as follows: In the formula, Represents the lowest point z of all two-dimensional grids root The average value, z init Indicates when the initial ground estimate is made A small fixed offset of represents the initial ground estimation point; represents the result of the lth ground estimation of the nth grid, express The center point of .

6. The method according to claim 1, characterized in that The step S300 includes the following steps: S310, taking the original point cloud map as the set of query frame point clouds, and converting it from the world coordinate system to the laser radar coordinate system, the conversion calculation of the coordinate system is as follows: In the formula, represents the original point cloud map, Indicates that Convert to In the laser radar coordinate system at the moment; represents the pose corresponding to the kth query frame; S320, according to the position of the posture corresponding to the current query frame in the original point cloud map, taking a certain range near it in the original point cloud map as a local point cloud map; S330, converting the local point cloud map and the query frame into a fan-shaped grid cluster point cloud descriptor, and obtaining a potential dynamic area by comparing the descriptors; S340: Retrieve the potential dynamic area to obtain the dynamic point cloud of the two-dimensional grid and filter it out.

7. The method according to claim 6, characterized in that In step S340, the retrieval of the dynamic point cloud includes at least the following steps: S341. For the two-dimensional grid at the same position, calculate S342, when It is determined that there is a dynamic point cloud at the position in the point cloud map.

8. The method according to claim 6, characterized in that In step S340, the original dynamic point cloud in the two-dimensional grid in the point cloud map is replaced according to the ground point cloud estimation result and the point cloud of the same two-dimensional grid in the query frame to filter out the dynamic point cloud, which is calculated as follows: In the formula, represents a point cloud used to replace the dynamic point cloud, represents the ground point cloud estimation result, A point cloud representing the same two-dimensional grid in the query frame. 9 . A computer-readable storage medium having program instructions stored thereon, wherein the program instructions, when executed by a processor, implement the method according to any one of claims 1 to 8.

10. A dynamic object filtering system for 3D point cloud maps based on Livox laser radar, characterized in that: include: A computer device comprising a computer readable storage medium according to claim 9.

Images