A method, device, medium and robot for filtering dynamic objects in a point cloud map

Abstract

The present invention provides a method for filtering dynamic objects from a point cloud map. S1: Obtain a 3D point cloud map M constructed by a robot; S2: With the pose T of the robot at the current moment t as the center, in the point cloud map M, circle out a local map with a first radius and a height falling within a first height range; S3: Perform cubic grid sampling on the local map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference in the z direction of the local map; S4: Obtain the surrounding laser scan frame corresponding to the moment t, transform it from the pose of the robot at the corresponding moment to the point cloud map, perform cubic grid sampling on #imgabs0# and #imgabs1# respectively, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference in the z direction of the surrounding laser scan frame; S5: Filter dynamic objects according to the height difference of the grid point cloud of the local map and the height difference of the grid point cloud of the surrounding laser scan frame. t As the center, in the point cloud map M, circle out a local map with a first radius and a height falling within a first height range; S3: Perform cubic grid sampling on the local map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference in the z direction of the local map; S4: Obtain the surrounding laser scan frame corresponding to the moment t, transform it from the pose of the robot at the corresponding moment to the point cloud map, perform cubic grid sampling on #imgabs0# and #imgabs1# respectively, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference in the z direction of the surrounding laser scan frame; S5: Filter dynamic objects according to the height difference of the grid point cloud of the local map and the height difference of the grid point cloud of the surrounding laser scan frame.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and more specifically, to a method, device, medium and robot for filtering dynamic objects in a point cloud map. Background Art

[0002] Patrol robots use a pre-constructed 3D laser point cloud map to achieve map-based positioning and navigation. However, in most real-world scenarios, the environment will inevitably be interfered by dynamic objects such as pedestrians and vehicles during the map construction process of patrol robots; the "ghost images" formed by these dynamic objects in the point cloud map are not conducive to subsequent positioning or navigation tasks and may even cause the robot to lose its position. Therefore, it is necessary to automatically filter dynamic objects from the established 3D point cloud map.

[0003] The existing methods for filtering dynamic objects in the 3D point cloud map of mobile robots mainly include the following methods:

[0004] 1. Manually filter dynamic objects in the point cloud map; however, the method of manual filtering not only wastes manpower but also affects the automation level of the robot.

[0005] 2. Use multiple sensors, such as introducing a camera, and use image recognition to identify dynamic objects (people, vehicles) for online filtering of dynamic objects; the method of multi-sensor fusion will lead to complex algorithms and increased hardware costs.

[0006] The background description provided in this article is for the purpose of presenting the context of the present disclosure generally. Unless otherwise indicated herein, the materials described in this section are not prior art to the claims of this application and should not be admitted as prior art by including them in this section. Summary of the Invention

[0007] In view of the above technical problems in the related art, the present invention proposes a method for filtering dynamic objects in a point cloud map, which includes the following steps:

[0008] S1. Obtain the 3D point cloud map M constructed by the robot;

[0009] S2. With the pose T of the robot at the current moment t t as the center, in the point cloud map M, circle out a local map sub_map with a first radius and a height range that falls within the first height range;

[0010] S3. Perform cubic grid segmentation on the local map sub_map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference h in the z direction of the local map;

[0011] S4. Obtain the surrounding laser scan frame p corresponding to the moment t t∈[t-a,t+a], transformed from the pose T of the robot at the corresponding moment to the point cloud map, denoted as t∈[t-a,t+a] For Perform cubic grid quantization on respectively, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference in the z direction of the surrounding laser scan frame p t∈[t-a,t+a] [h1, h2, h3, …, h2a];

[0012] S5. According to the height difference of the grid point cloud of the local map sub_map and the height difference of the grid point cloud of the surrounding laser scan frame p t∈[t-a,t+a] to filter out dynamic objects.

[0013] Specifically, step S5 is as follows: the height difference of the point cloud in the i-th grid of the local map sub_map is h, and the height differences of the point cloud in the corresponding i-th grid in are [h1, h2, h3, …, h2a]. If there are any 4 or more hj in [h1, h2, h3, …, h2a] that are less than the height difference h, it is considered that the i-th grid in sub_map contains dynamic objects, and the dynamic objects are filtered out.

[0014] Specifically, in step S5, filtering out the dynamic objects specifically means:

[0015] S51. Select the grids with height differences less than h in [h1, h2, h3, …, h2a], and directly "stitch" all the point clouds in the grids together to form a larger point cloud a;

[0016] S52. Perform histogram statistics on the height distribution of the point cloud a, and record the height value where 80% of the points fall as h_true;

[0017] S53. Replace the height of the point cloud in the i-th grid of sub_map that is greater than h_true with h_true to complete the elimination of dynamic objects.

[0018] Specifically, the first radius is 60m, and the first height range is [-1, 1] meters.

[0019] Specifically, perform cubic grid quantization on the local map sub_map with 0.5 * 0.5 * 0.5 cubes, and perform cubic grid quantization on respectively with 0.5 * 0.5 * 0.5 cubes.

[0020] In a second aspect, another embodiment of the present invention discloses a device for filtering dynamic objects in a point cloud map, which includes the following units:

[0021] A point cloud map acquisition unit for acquiring the 3D point cloud map M constructed by the robot;

[0022] A local map acquisition unit, which is used to circle a local map sub_map in the point cloud map M with a first radius and a height falling within a first height range centered on the robot pose T at the current moment t; t For the center, in the point cloud map M, a local map sub_map is circled with a first radius while the height falls within the first height range;

[0023] A local map grid height difference acquisition unit, which is used to perform cubic gridification on the local map sub_map and calculate the height difference in the z direction of the point cloud in each grid to obtain the z-direction height difference h of the local map;

[0024] A current moment surrounding laser scan frame grid height difference acquisition unit, which is used to acquire the surrounding laser scan frame p corresponding to the moment t t∈[t-a,t+a] , which is transformed to the point cloud map by the robot pose T at the corresponding moment t∈[t-a,t+a] and is denoted as For perform cubic gridification respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively to obtain the z-direction height difference [h1, h2, h3,..., h2a] of the surrounding laser scan frame p t∈[t-a,t+a] ;

[0025] A dynamic object filtering unit, which is used to filter dynamic objects according to the grid point cloud height difference of the local map sub_map and the grid point cloud height difference of the surrounding laser scan frame p t∈[t-a,t+a] .

[0026] Specifically, the dynamic object filtering unit is: the height difference of the point cloud in the i-th grid in the map sub_map is h, and the height differences of the point cloud in the corresponding i-th grid in are [h1, h2, h3,..., h2a] respectively. If there are any 4 or more hj in [h1, h2, h3,..., h2a] that are less than the height difference h, it is considered that the i-th grid in sub_map contains dynamic objects, and the dynamic objects are filtered out.

[0027] Specifically, the filtering in the dynamic object filtering unit further includes the following units:

[0028] A first point cloud splicing unit, which is used to select the grids with height differences less than h in [h1, h2, h3,..., h2a], and directly "splice" all the point clouds in the grids together to form a larger point cloud a;

[0029] A first height value acquisition unit, which is used to perform a histogram statistics on the height distribution of the point cloud a, and record the height value into which 80% of the points fall as h_true;

[0030] A dynamic object elimination unit, which is used to replace the height of the point cloud in the i-th grid in sub_map that is greater than h_true with h_true to complete the elimination of dynamic objects.

[0031] In a third aspect, another embodiment of the present invention discloses a non-volatile memory, on which instructions are stored. When the instructions are executed by a processor, they are used to implement the above-mentioned method for filtering dynamic objects in a point cloud map.

[0032] In a fourth aspect, another embodiment of the present invention discloses a robot, which includes: a processing module, a chassis, a storage module, and a lidar. The storage module stores instructions, which are used to implement the above-mentioned method for filtering dynamic objects in a point cloud map when the instructions are executed.

[0033] For the method for filtering dynamic objects in a point cloud map of the present invention, a point cloud frame around the current moment is obtained and rasterized to obtain the height difference of the point cloud within the grid, and it is compared with the height difference of the rasterized point cloud of the local map at the current moment to obtain dynamic objects and filter out the dynamic objects. The method for filtering dynamic objects in a point cloud map of the present invention does not require any additional hardware cost; in addition, the method for filtering dynamic objects in a point cloud map of the present invention can filter out dynamic objects in the point cloud map after the patrol robot has built the map, that is, it can filter out dynamic objects in the point cloud map offline without real-time processing, with high flexibility. Description of the Drawings

[0034] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0035] Figure 1 It is a flowchart of a method for filtering dynamic objects in a point cloud map provided by an embodiment of the present invention;

[0036] Figure 2 It is a schematic diagram of a device for filtering dynamic objects in a point cloud map provided by an embodiment of the present invention;

[0037] Figure 3 It is a schematic diagram of a device for filtering dynamic objects in a point cloud map provided by an embodiment of the present invention. Detailed Embodiments

[0038] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art belong to the scope of protection of the present invention.

[0039] Embodiment 1

[0040] Reference Figure 1 , this embodiment provides a method for filtering dynamic objects from a point cloud map, which includes the following steps:

[0041] S1. Obtain the 3D point cloud map M constructed by the robot;

[0042] In this embodiment, the patrol robot can be controlled to walk to construct a 3D point cloud map of the working scene first. This map contains dynamic objects and is denoted as M.

[0043] Specifically, the working scene contains dynamic objects. The patrol robot can use SLAM technology to construct a 3D point cloud map. For example, the patrol robot is equipped with any one or more of a lidar, a millimeter-wave radar, and a vision sensor, and uses any one of these sensors to construct a 3D point cloud map.

[0044] This embodiment is introduced by taking the lidar as an example. Those skilled in the art know that a 3D point cloud map can be constructed using any other sensor, and this embodiment will not elaborate.

[0045] More generally, the 3D point cloud map is "stitched" together by frames of laser scan point cloud frames at different times. Assuming that the laser scan frame at time t is p t , and the robot pose is T t , then the map

[0046]

[0047] S2. With the robot pose T at the current time t t as the center, in the point cloud map M, circle out a local map sub_map with a first radius and the height falling within a first height range at the same time;

[0048] Specifically, the first radius is 60m.

[0049] The first height range is [-1, 1] meters.

[0050] Dynamic objects are generally on the ground and their height is about 2m. The radar installed on the robot is about 1m above the ground. Therefore, this embodiment selects the first height range [-1, 1]m to identify potential dynamic objects.

[0051] S3. Perform cubic grid quantization on the local map sub_map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference h in the z direction of the local map;

[0052] In this embodiment, the local map sub_map is processed by cubic grid quantization with 0.5 * 0.5 * 0.5, and the height difference in the z direction of the point cloud in each grid is calculated;

[0053] Specifically, in this embodiment, rasterization is performed using 0.5 * 0.5 * 0.5. On the one hand, the calculation speed is considered. On the other hand, some small interference points will be excluded. If the raster is divided too small, the calculation amount will be large, and at the same time, more interference points will be brought. However, if the raster is divided too large, the calculation amount will be small, but the corresponding accuracy will also be low.

[0054] S4. Obtain the surrounding laser scan frame p corresponding to time t t∈[t-a,t+a] , and convert it to the point cloud map according to the pose T of the robot at the corresponding time t∈[t-a,t+a] , denoted as For , perform cube rasterization respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively to obtain the height difference in the z direction of the surrounding laser scan frame p t∈[t-a,t+a] ; [h1, h2, h3,..., h2a]

[0055] Specifically, in one implementation manner of this embodiment, a = 5; that is, obtain 10 surrounding laser scan frames at the current time t; and convert it to the point cloud map according to the pose T of the robot at the corresponding time t∈[t-5,t+5] , denoted as Then for , perform cube rasterization processing with 0.5 * 0.5 * 0.5 respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively.

[0056] Furthermore, part of the grids divided by p t∈[t-a,t+a] coincides with the grids divided by the local map sub_map. Due to the influence of the moving dynamic objects, the height difference between the grids of p t∈[t-a,t+a] and the grids of the local map sub_map at time t is inconsistent.

[0057] S5. Filter out dynamic objects according to the height difference of the grid point cloud of the local map sub_map and the height difference of the grid point cloud of the surrounding laser scan frame p t∈[t-a,t+a] .

[0058] Specifically, step S5 is: the height difference of the point cloud in the i-th grid in sub_map is h, and the height differences of the point cloud in the corresponding i-th grid in are [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10] respectively. If there are any 4 or more hj in [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10] that are less than the height difference h, it is considered that the i-th grid in sub_map contains dynamic objects, and the dynamic objects are filtered out.

[0059] Specifically, in step S5, filtering out the dynamic objects specifically means:

[0060] S51. Select the grids within [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10] with a height difference less than h, and directly "stitch" all the point clouds within the grids together to form a larger point cloud a;

[0061] S52. Conduct a histogram statistics on the height distribution of the point cloud a, and record the height value at which 80% of the points fall as h_true;

[0062] S53. For the point cloud within the i-th grid in the sub_map with a height greater than h_true, replace it with h_true to complete the removal of dynamic objects;

[0063] The method for filtering dynamic objects in the point cloud map of this embodiment obtains the surrounding point cloud frames at the current moment and rasterizes them to obtain the height difference of the point cloud within the grids, and compares it with the height difference of the rasterized point cloud of the local map at the current moment to obtain dynamic objects and filter out the dynamic objects. The method for filtering dynamic objects in the point cloud map of this embodiment does not require any additional hardware costs; in addition, the method for filtering dynamic objects in the point cloud map of this embodiment can filter out dynamic objects in the point cloud map after the patrol robot has built the map, that is, it can perform off-line non-real-time filtering processing of dynamic objects in the point cloud map, with high flexibility.

[0064] Embodiment 2

[0065] Reference Figure 2 , this embodiment discloses a device for filtering dynamic objects in a point cloud map, which includes the following units:

[0066] A point cloud map acquisition unit, used to acquire the 3D point cloud map M constructed by the robot;

[0067] In this embodiment, the patrol robot can be controlled to walk to construct a 3D point cloud map of the working scene first, and this map contains dynamic objects, denoted as M.

[0068] Specifically, the working scene contains dynamic objects, and the patrol robot can use SLAM technology to construct a 3D point cloud map. For example, the patrol robot is equipped with any one or more of a lidar, a millimeter-wave radar, and a vision sensor, and uses any one of these sensors to construct a 3D point cloud map.

[0069] This embodiment takes the lidar as an example for introduction. Those skilled in the art know that any other sensor can be used to construct a 3D point cloud map, and this embodiment will not elaborate.

[0070] More generally, the 3D point cloud map is "stitched" together by frames of laser scan point cloud frames at different times. Assume that the laser scan frame at time t is p t , and the robot pose is Tt , then the map

[0071]

[0072] A local map acquisition unit, which is used to take the robot pose T at the current moment t t as the center, in the point cloud map M, circle a local map sub_map with a first radius and a height falling within a first height range at the same time;

[0073] Specifically, the first radius is 60m.

[0074] The first height range is [-1, 1] meters.

[0075] Dynamic objects are generally on the ground and have a height of about 2m. The radar installed on the robot is about 1m above the ground. Therefore, in this embodiment, the first height range [-1, 1]m is selected to confirm potential dynamic objects.

[0076] A local map grid height difference acquisition unit, which is used to perform cubic gridification on the local map sub_map and calculate the height difference in the z direction of the point cloud in each grid to obtain the z direction height difference h of the local map;

[0077] In this embodiment, the local map sub_map is processed by cubic gridification with 0.5*0.5*0.5, and the height difference in the z direction of the point cloud in each grid is calculated;

[0078] Specifically, in this embodiment, 0.5*0.5*0.5 is used for gridification. On the one hand, the calculation speed is considered. On the other hand, some small interference points will be excluded. If the grid division is too small, the calculation amount will be large, and at the same time, more interference points will be brought. However, if the grid division is too large, the calculation amount will be small, but the corresponding accuracy will also be low.

[0079] A current moment surrounding laser scan frame grid height difference acquisition unit, which is used to obtain the surrounding laser scan frame p corresponding to the t moment t∈[t-a,t+a] , and is transformed to the point cloud map by the robot pose T at the corresponding moment t∈[t-a,t+a] and is denoted as For perform cubic gridification respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively to obtain the z direction height difference [h1, h2, h3,..., h2a] of the surrounding laser scan frame p t∈[t-a,t+a] ;

[0080] Specifically, in an implementation manner of this embodiment, a = 5; that is, 10 surrounding laser scan frames at the current moment t are obtained; and are transformed to the point cloud map by the robot pose T at the corresponding moment t∈[t-5,t+5] and are denoted as Then for Perform 0.5*0.5*0.5 cubic grid rasterization respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively.

[0081] Further, p t∈[t-a,t+a] Some of the rasterized grids are coincident with those of the local map sub_map. Due to the influence of moving dynamic objects, it will cause p t∈[t-a,t+a] The height difference between the grids of and the local map sub_map at time t is inconsistent.

[0082] Dynamic object filtering unit, used to filter dynamic objects according to the height difference of the grid point cloud of the local map sub_map and the height difference of the grid point cloud of the surrounding laser scan frame p t∈[t-a,t+a] Perform filtering of dynamic objects.

[0083] Specifically, the dynamic object filtering unit is: the height difference of the point cloud in the i-th grid in sub_map is h, The height differences of the point clouds in the corresponding i-th grids in are [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10] respectively. If there are any 4 or more hj less than the height difference h in [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10], it is considered that the i-th grid in sub_map contains dynamic objects, and the dynamic objects are filtered out.

[0084] Specifically, the filtering in the dynamic object filtering unit further includes the following units:

[0085] The first point cloud stitching unit, used to select the grids with height differences less than h in [h1, h2, h3, h4, h5, h6, h7, h8, h9, h10], and directly "stitch" all the point clouds in the grids together to form a larger point cloud a;

[0086] The first height value acquisition unit, used to perform histogram statistics on the height distribution of the point cloud a, and record the height value where 80% of the points fall as h_true;

[0087] The dynamic object removal unit, used to replace the height of the point cloud in the i-th grid in sub_map that is greater than h_true with h_true, and complete the removal of dynamic objects;

[0088] The method for filtering dynamic objects in a point cloud map according to this embodiment obtains the surrounding point cloud frames at the current moment and rasterizes them to obtain the height difference of the point cloud within the raster, and compares it with the height difference of the rasterized point cloud of the local map at the current moment to obtain dynamic objects and filter out the dynamic objects. The method for filtering dynamic objects in a point cloud map according to this embodiment does not require any additional hardware costs; in addition, the method for filtering dynamic objects in a point cloud map according to this embodiment can filter dynamic objects in the point cloud map after the patrol robot has built the map, that is, it can perform off-line dynamic object filtering processing on the point cloud map without real-time, with high flexibility.

[0089] Embodiment III

[0090] This embodiment provides a robot, which includes: a processing module, a chassis, a storage module, and a lidar. The storage module stores instructions, which are used to implement a method for filtering dynamic objects in a point cloud map as described in Embodiment I when the instructions are executed.

[0091] In another embodiment, a robot includes: a processing module, a chassis, a storage module, a lidar, and also includes a device for filtering dynamic objects in a point cloud map as described in Embodiment II.

[0092] Embodiment IV

[0093] Reference Figure 3 , Figure 3 is a schematic structural diagram of a device for filtering dynamic objects in a point cloud map according to this embodiment. A device 20 for filtering dynamic objects in a point cloud map according to this embodiment includes a processor 21, a memory 22, and a computer program stored in the memory 22 and executable on the processor 21. When the processor 21 executes the computer program, the steps in the above method embodiment are implemented. Alternatively, when the processor 21 executes the computer program, the functions of each module / unit in the above device embodiments are implemented.

[0094] Exemplarily, the computer program can be divided into one or more modules / units. The one or more modules / units are stored in the memory 22 and executed by the processor 21 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program in the device 20 for filtering dynamic objects in a point cloud map. For example, the computer program can be divided into the respective modules in Embodiment II. For the specific functions of each module, please refer to the working process of the device described in the above embodiments, which will not be elaborated here.

[0095] The described point cloud map dynamic object filtering device 20 may include, but is not limited to, a processor 21 and a memory 22. Those skilled in the art can understand that the schematic diagram is only an example of the point cloud map dynamic object filtering device 20, and does not constitute a limitation on the point cloud map dynamic object filtering device 20. It may include more or fewer components than shown, or combine certain components, or different components. For example, the point cloud map dynamic object filtering device 20 may also include an input / output device, a network access device, a bus, etc.

[0096] The processor 21 may be a central processing unit (CPU), or may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The processor 21 is the control center of the point cloud map dynamic object filtering device 20, and connects various parts of the entire point cloud map dynamic object filtering device 20 through various interfaces and lines.

[0097] The memory 22 can be used to store the computer programs and / or modules. The processor 21 realizes various functions of the point cloud map dynamic object filtering device 20 by running or executing the computer programs and / or modules stored in the memory 22, and by calling the data stored in the memory 22. The memory 22 may mainly include a program storage area and a data storage area. Among them, the program storage area may store an operating system, application programs required for at least one function (such as a sound playback function, an image playback function, etc.); the data storage area may store data created according to the use of the mobile phone (such as audio data, phone book, etc.). In addition, the memory 22 may include high-speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, at least one magnetic disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0098] Among them, if the modules / units integrated in the point cloud map dynamic object filtering device 20 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, to implement all or part of the processes in the above-described embodiment methods of the present invention, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor 21, the steps of the above various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0099] It should be noted that the device embodiments described above are only illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. In addition, in the attached drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that they have a communication connection, which can be specifically implemented as one or more communication buses or signal lines. Those of ordinary skill in the art can understand and implement it without creative work.

[0100] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for filtering dynamic objects from a point cloud map, comprising the following steps: S1. Obtain the 3D point cloud map M constructed by the robot; S2, with the robot pose T at the current moment t t as the center, in the point cloud map M, circle out a local map sub_map with the first radius and the height falling within the first height range at the same time; S3. Perform cubic grid quantization on the local map sub_map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference h in the z direction of the local map; S4. Obtain the surrounding laser scan frame p corresponding to time t t∈[t-a,t+a] , and transform it to the point cloud map according to the pose T of the robot at the corresponding time t∈[t-a,t+a] , denoted as For , perform cubic grid sampling respectively, and calculate the height difference in the z direction of the point cloud in each grid respectively to obtain the height difference [h1, h2, h3, …, h2a] in the z direction of the surrounding laser scan frame p t∈[t-a,t+a] ; S5. Filter dynamic objects based on the height difference of the grid point cloud in the local map sub_map and the height difference of the grid point cloud in the surrounding laser scan frame p t∈[t-a,t+a] . Specifically, the height difference of the point cloud in the i-th grid in the local map sub_map is h, and the height differences of the point cloud in the corresponding i-th grid in are [h1, h2, h3, …, h2a]. If there are any 4 or more hj in [h1, h2, h3, …, h2a] that are less than the height difference h, it is considered that the i-th grid in sub_map contains dynamic objects, and the dynamic objects are filtered out.

2. According to the method described in claim 1, in step S5, filtering the dynamic objects specifically includes: S51. Select the grids with height differences less than h within [h1, h2, h3,..., h2a], and directly "stitch" all the point clouds in the grids together to form a larger point cloud a; S52. Perform a histogram statistics on the height distribution of the point cloud a, and record the height value where 80% of the points fall as h_true; S53. Replace the height of the point cloud in the i-th grid in sub_map that is greater than h_true with h_true to complete the removal of dynamic objects.

3. According to the method described in claim 2, the first radius is 60m, and the first height range is [-1, 1] meters.

4. According to the method described in claim 3, the local map sub_map is rasterized into 0.5*0.5*0.5 cubic grids, and is rasterized into 0.5*0.5*0.5 cubic grids respectively.

5. A device for filtering dynamic objects from a point cloud map, comprising the following units: A point cloud map acquisition unit, configured to obtain the 3D point cloud map M constructed by the robot; A local map acquisition unit, which is used to circle a local map sub_map in the point cloud map M with the pose T of the robot at the current moment t as the center, within a first radius and with the height falling within a first height range at the same time; t ​ A local map grid height difference acquisition unit, configured to perform cubic grid quantization on the local map sub_map, and calculate the height difference in the z direction of the point cloud in each grid to obtain the height difference h in the z direction of the local map; Current moment surrounding laser scan frame grid height difference acquisition unit, which is used to obtain the surrounding laser scan frame p corresponding to the t moment t∈[t-a,t+a] , which is transformed to the point cloud map by the pose T of the robot at the corresponding moment t∈[t-a,t+a] , denoted as For , cube gridization is respectively performed, and the height difference in the z direction of the point cloud in each grid is calculated respectively to obtain the height difference in the z direction of the surrounding laser scan frame p t∈[t-a,t+a] ; [h1, h2, h3,..., h2a] of the height difference in the z direction of The dynamic object filtering unit is used to filter dynamic objects according to the height difference of the grid point cloud in the local map sub_map and the height difference of the grid point cloud in the surrounding laser scan frame p t∈[t-a,t+a] ; specifically, for the height difference of the point cloud in the i-th grid in the map sub_map being h, and the height differences of the point cloud in the corresponding i-th grid in being [h1, h2, h3, …, h2a], if any 4 or more hj in [h1, h2, h3, …, h2a] are less than the height difference h, it is considered that the i-th grid in sub_map contains a dynamic object, and the dynamic object is filtered out.

6. According to the device described in claim 5, the filtering in the dynamic object filtering unit further includes the following units: A first point cloud stitching unit, configured to select the grids with height differences less than h within [h1, h2, h3,..., h2a], and directly "stitch" all the point clouds in the grids together to form a larger point cloud a; A first height value acquisition unit, configured to perform a histogram statistics on the height distribution of the point cloud a, and record the height value where 80% of the points fall as h_true; A dynamic object removal unit, configured to replace the height of the point cloud in the i-th grid in sub_map that is greater than h_true with h_true to complete the removal of dynamic objects.

7. A non-volatile memory, on which instructions are stored, and when the instructions are executed by a processor, they are used to implement a method for filtering dynamic objects from a point cloud map as described in any one of claims 1-4.

8. A robot, the robot comprising: A processing module, a chassis, a storage module, a lidar, the storage module stores instructions, and when the instructions are executed, they are used to implement a method for filtering dynamic objects from a point cloud map as described in any one of claims 1-4.

Images