A method and device for filtering dynamic objects from laser point clouds

Abstract

The present invention discloses a method and device for filtering dynamic objects from laser point clouds. Among them, the method includes: controlling a robot equipped with a laser sensor to travel around in a target environment, establishing an original map of the target environment based on the point clouds obtained by the laser sensor; controlling the robot to autonomously travel according to the original map, and obtaining the real-time point clouds scanned by the laser sensor; identifying and filtering the dynamic point clouds in the real-time point clouds, and determining the real-time map of the target environment according to the filtered real-time point clouds. The technical solution of this embodiment can, during the process of the robot using the laser sensor to build a map in real time, simultaneously judge the surrounding dynamic objects, filter them, and judge its own position, and this method improves the accuracy of navigation and positioning.

Description

Technical Field

[0001] Embodiments of the present invention relate to robot positioning and navigation technologies, and particularly to a method and device for filtering dynamic objects from laser point clouds. Background Art

[0002] During the driving process of a mobile robot, a laser sensor is used for navigation. The surrounding environment, such as walls, columns, etc., is scanned by the laser sensor to build a map.

[0003] However, when the laser sensor scans, many dynamic objects, such as other moving robots, walking staff, etc., are regarded as fixed objects to form a map. Therefore, there will be certain errors during the navigation process. Summary of the Invention

[0004] To solve the problems in the prior art, the present invention provides a method and device for filtering dynamic objects from laser point clouds to filter dynamic objects during the robot navigation process and improve the accuracy of navigation positioning.

[0005] In a first aspect, embodiments of the present invention provide a method for filtering dynamic objects from laser point clouds, including:

[0006] S1. Control a robot equipped with a laser sensor to drive around in a target environment, and establish an original map of the target environment according to the point cloud obtained by the laser sensor;

[0007] S2. Control the robot to autonomously drive according to the original map, and obtain the real-time point cloud scanned by the laser sensor;

[0008] S3. Identify and filter the dynamic point clouds in the real-time point cloud, and determine the real-time map of the target environment according to the filtered real-time point cloud.

[0009] Optionally, S2 includes:

[0010] Based on the predicted pose calculated by the odometer, convert the point cloud currently obtained by the laser sensor to the global coordinate system to obtain a predicted point cloud set.

[0011] Optionally, identifying and filtering the dynamic point cloud data in the real-time point cloud includes:

[0012] Filter out the point clouds within a certain distance threshold from the predicted point cloud set and the dynamic point set to obtain a filtered real-time point cloud set.

[0013] Optionally, determining the real-time map of the target environment according to the filtered real-time point cloud includes:

[0014] Match the filtered real-time point cloud with the original map to obtain the matching pose of the laser sensor;

[0015] The real-time point cloud obtained during the operation of the laser sensor is transformed based on the matching pose to obtain a real-time map in the global coordinate system.

[0016] Optionally, the method for determining the dynamic point set includes:

[0017] Calculate the blank area of the target environment based on the real-time map in the global coordinate system;

[0018] Add the points of the real-time map in the global coordinate system in the blank area to a temporary set, and replace the point cloud in the dynamic point set with the point cloud in the temporary set.

[0019] Optionally, calculating the blank area of the target environment based on the real-time map in the global coordinate system includes:

[0020] Add the area passed by the line connecting each point of the laser sensor and the real-time map in the global coordinate system to the blank area.

[0021] Optionally, the temporary set further includes: the points in the real-time map in the global coordinate system that are within a certain threshold distance from the points in the dynamic point set.

[0022] In a second aspect, an embodiment of the present invention further provides a device for filtering dynamic objects from laser point clouds, including:

[0023] An original map establishment module, configured to control a robot equipped with a laser sensor to drive around in a target environment, and establish an original map of the target environment according to the point cloud obtained by the laser sensor;

[0024] A real-time point cloud acquisition module, configured to control the robot to autonomously drive according to the original map, and acquire the real-time point cloud scanned by the laser sensor;

[0025] A real-time map establishment module, configured to identify and filter the dynamic point cloud in the real-time point cloud, and determine a real-time map of the target environment according to the filtered real-time point cloud.

[0026] The technical solution of this embodiment judges the surrounding dynamic objects and filters them while the robot is using the laser sensor to build a real-time map, and judges its own position, which improves the accuracy of navigation and positioning. Brief Description of the Drawings

[0027] Figure 1 It is a flowchart of a method for filtering dynamic objects from laser point clouds provided by Embodiment 1 of the present invention. Detailed Embodiment

[0028] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It can be understood that the specific embodiments described herein are only for explaining the present invention, rather than limiting the present invention. Additionally, it should be noted that for ease of description, only parts related to the present invention rather than all structures are shown in the accompanying drawings.

[0029] Embodiment 1

[0030] Figure 1 The following is a flowchart of a method for filtering dynamic objects from laser point clouds provided in Embodiment 1 of the present invention, which specifically includes the following steps:

[0031] S1. Control the robot equipped with a laser sensor to drive around in the target environment, and establish an original map of the target environment based on the point cloud obtained by the laser sensor.

[0032] In this embodiment, the laser sensor is installed on the robot. First, the robot is manually driven or remotely controlled to drive around in the target environment. During the driving process of the robot, the laser sensor scans the surrounding environment to form a point cloud map. Places with objects will be represented by point clouds, and places without objects will be blank.

[0033] S2. Control the robot to autonomously drive according to the original map, and obtain the real-time point cloud scanned by the laser sensor.

[0034] During the driving process of the robot according to the original map, it will scan the surrounding environment through the laser sensor to obtain the point cloud of the surrounding environment in real time. And based on the predicted pose calculated by the odometer, the point cloud currently obtained by the laser sensor is transformed into the global coordinate system to obtain a set of predicted point clouds.

[0035] S3. Identify and filter the dynamic point clouds in the real-time point cloud, and determine the real-time map of the target environment according to the filtered real-time point cloud.

[0036] After obtaining the set of predicted point clouds in the global coordinate system, it is necessary to first filter the dynamic point clouds in the predicted point clouds and then perform matching to obtain the real-time map in the global coordinate system to achieve the accurate positioning of the robot.

[0037] Specifically, filter the point clouds in the set of predicted point clouds and the dynamic point set that are within a certain distance threshold to obtain a set of filtered real-time point clouds.

[0038] After filtering, match the filtered real-time point cloud with the original map to obtain the matching pose of the laser sensor; the real-time point cloud obtained during the operation of the laser sensor is transformed based on the matching pose to obtain the real-time map in the global coordinate system.

[0039] In this embodiment, before step S2 starts, the dynamic point set Φ and the blank area set are both initialized as empty sets. The blank areas are determined according to the matched real-time map. If the point cloud data of an object is detected in a blank area, it means that the point cloud corresponds to the point cloud of a dynamic object and needs to be filtered out. Therefore, in this embodiment, the dynamic point set is determined according to the points in the real-time map that fall into the blank areas. The mapping in this embodiment is real-time, and the blank areas are also constantly changing. Therefore, a temporary set needs to be established, and the dynamic points detected in each round are first placed in the temporary set, and the data in the dynamic point set is continuously updated through the temporary set.

[0040] Specifically, the method for determining the dynamic point set includes: calculating the blank area of the target environment according to the real-time map in the global coordinate system; adding the points of the real-time map in the global coordinate system in the blank area to the temporary set, and replacing the point cloud in the dynamic point set with the point cloud in the temporary set.

[0041] Among them, calculating the blank area of the target environment according to the real-time map in the global coordinate system includes: adding the area passed by the line connecting each point in the laser sensor and the real-time map in the global coordinate system to the blank area.

[0042] Based on the above embodiment, the temporary set further includes: the points in the real-time map in the global coordinate system that are within a certain threshold distance from the points in the dynamic point set.

[0043] The technical solution of this embodiment compares with the original map during the process of the robot using the laser sensor to build a real-time map, simultaneously judges the surrounding dynamic objects, filters them out, and then judges its own position, increasing the accuracy of navigation and positioning.

[0044] Embodiment 2

[0045] Based on the above embodiment, this embodiment provides specific implementation steps for filtering dynamic objects from the laser point cloud, which are as follows:

[0046] 1. Establish an original map

[0047] Install the laser sensor on the robot, manually drive the robot or remotely control the robot to drive around in the environment. The laser sensor scans the surrounding environment to form a point cloud map. The places with objects are represented by point clouds, and the places without objects are blank.

[0048] 2. Establish a real-time map, filter out dynamic objects, and perform positioning.

[0049] The robot autonomously travels according to the original map, scans the surrounding environment at the same time, builds a real-time map, compares it with the original map, judges the surrounding dynamic objects, filters them out, and determines its own position.

[0050] The first step: Initialize the dynamic point set Φ as an empty set, and initialize the blank area set as an empty set;

[0051] The second step: Obtain the latest frame of scanned point cloud γ, and based on the predicted pose calculated by the odometer, convert it to the global coordinate system to obtain the predicted point cloud set γ pre ;

[0052] The third step: Judge the points in the predicted point cloud set γ pre and the points in the dynamic point set Φ within a certain threshold distance, filter them out from the original point cloud set γ pre to obtain the filtered point cloud set γ'; pre ;

[0053] The fourth step: Use the filtered point cloud γ' pre to match with the original map to obtain the matching pose P laser ;

[0054] The fifth step: Re-convert the point cloud set γ before conversion based on the matching pose to obtain the global point cloud γ matched ;

[0055] The sixth step: Calculate the points in γ matched and the points in Φ within a certain threshold distance, and add them to the temporary set Φ';

[0056] The seventh step: Calculate the points in γ matched within the blank area and add them to the temporary set Φ';

[0057] The eighth step: Φ = Φ';

[0058] The ninth step: For each point p in γ matched , add the area passed by the line connecting the vehicle laser P laser and p to the blank area p;

[0059] The tenth step: Repeat the second step.

[0060] Embodiment 3

[0061] This embodiment provides a device for filtering dynamic objects from laser point clouds, including:

[0062] An original map building module, configured to control a robot equipped with a laser sensor to travel around in a target environment, and build an original map of the target environment according to the point cloud obtained by the laser sensor;

[0063] A real-time point cloud acquisition module, which is used to control the robot to autonomously drive according to the original map and acquire the real-time point cloud scanned by the laser sensor;

[0064] A real-time map building module, which is used to identify and filter out the dynamic point clouds in the real-time point cloud and determine the real-time map of the target environment according to the filtered real-time point cloud.

[0065] Among them, the real-time point cloud acquisition module is specifically used for: based on the predicted pose calculated by the odometer, converting the point cloud currently acquired by the laser sensor into the global coordinate system to obtain a predicted point cloud set.

[0066] The real-time map building module is specifically used for:

[0067] Filtering out the point clouds in the predicted point cloud set and the dynamic point set whose distances are within a certain distance threshold to obtain a filtered real-time point cloud set.

[0068] Matching the filtered real-time point cloud with the original map to obtain the matching pose of the laser sensor;

[0069] The real-time point cloud acquired during the operation of the laser sensor is converted based on the matching pose to obtain a real-time map in the global coordinate system.

[0070] Among them, the method for determining the dynamic point set includes:

[0071] Calculating the blank area of the target environment according to the real-time map in the global coordinate system;

[0072] Adding the points of the real-time map in the global coordinate system in the blank area to a temporary set, and replacing the point clouds in the dynamic point set with the point clouds in the temporary set.

[0073] Specifically, calculating the blank area of the target environment according to the real-time map in the global coordinate system includes:

[0074] Adding the area passed by the connection line between each point of the laser sensor and the real-time map in the global coordinate system to the blank area.

[0075] Optionally, the temporary set further includes: the points in the real-time map in the global coordinate system whose distances from the points in the dynamic point set are within a certain threshold.

[0076] The device for filtering dynamic objects from laser point clouds provided by the embodiments of the present invention can execute the method for filtering dynamic objects from laser point clouds provided by any embodiment of the present invention, and has the corresponding functional modules and beneficial effects for executing the method.

[0077] Note that the above is only a preferred embodiment of the present invention and the technical principles applied. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein. Various obvious changes, re-adjustments, and substitutions can be made by those skilled in the art without departing from the protection scope of the present invention. Therefore, although the present invention has been described in more detail through the above embodiments, the present invention is not limited to the above embodiments. Without departing from the concept of the present invention, more other equivalent embodiments can be included, and the scope of the present invention is determined by the scope of the appended claims.

Claims

1. A method for filtering dynamic objects from laser point clouds, characterized in that, Including: S1. Control a robot equipped with a laser sensor to travel around in a target environment, and establish an original map of the target environment based on the point cloud obtained by the laser sensor; S2. Control the robot to autonomously travel according to the original map, and obtain the real-time point cloud scanned by the laser sensor; S3. Identify and filter out the dynamic point cloud in the real-time point cloud, and determine the real-time map of the target environment according to the filtered real-time point cloud; The S2 includes: Based on the predicted pose calculated by the odometer, convert the point cloud currently obtained by the laser sensor into the global coordinate system to obtain a predicted point cloud set; Identifying and filtering out the dynamic point cloud data in the real-time point cloud includes: Filter out the point cloud within a certain distance threshold between the predicted point cloud set and the dynamic point set to obtain a filtered real-time point cloud set; Determining the real-time map of the target environment according to the filtered real-time point cloud includes: Match the filtered real-time point cloud with the original map to obtain the matching pose of the laser sensor; The real-time point cloud obtained during the operation of the laser sensor is converted based on the matching pose to obtain a real-time map in the global coordinate system; The method for determining the dynamic point set includes: calculating the blank area of the target environment according to the real-time map in the global coordinate system; adding the points of the real-time map in the global coordinate system in the blank area to a temporary set, and replacing the point cloud in the dynamic point set with the point cloud in the temporary set; Among them, calculating the blank area of the target environment according to the real-time map in the global coordinate system includes: adding the area passed by the line connecting each point of the laser sensor and the real-time map in the global coordinate system to the blank area.

2. The method according to claim 1, wherein Calculating the blank area of the target environment according to the real-time map in the global coordinate system includes: Adding the area passed by the line connecting each point of the laser sensor and the real-time map in the global coordinate system to the blank area.

3. The method according to claim 1, wherein The temporary set further includes: the points in the real-time map in the global coordinate system that are within a certain threshold distance from the dynamic point set.

4. A device for filtering dynamic objects from laser point clouds, which is used to execute the method for filtering dynamic objects from laser point clouds according to any one of claims 1-3, characterized in that, Including: An original map establishment module, configured to control a robot equipped with a laser sensor to travel around in a target environment, and establish an original map of the target environment based on the point cloud obtained by the laser sensor; A real-time point cloud acquisition module, configured to control the robot to autonomously travel according to the original map, and obtain the real-time point cloud scanned by the laser sensor; A real-time map establishment module, configured to identify and filter out the dynamic point cloud in the real-time point cloud, and determine the real-time map of the target environment according to the filtered real-time point cloud.

Images