WO2022110473A1

WO2022110473A1 - Robot mapping method and device, computer readable storage medium, and robot

Info

Publication number: WO2022110473A1
Application number: PCT/CN2020/140430
Authority: WO
Inventors: 何婉君; 刘志超
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2020-11-24
Filing date: 2020-12-28
Publication date: 2022-06-02
Also published as: CN112484738A; CN112484738B

Abstract

A robot mapping method and device, a computer readable storage medium, and a robot. The method comprises: obtaining a first point cloud and a first pose acquired by a robot at the current moment, and a second point cloud and a second pose acquired at a historical moment (S101); calculating a pose difference between the first pose and the second pose (S102); projecting the second point cloud to coordinates of the first point cloud according to the pose difference to obtain a third point cloud (S103); screening out a dynamic object point cloud in the first point cloud according to the third point cloud (S104); and removing the dynamic object point cloud from the first point cloud to obtain a fourth point cloud, and performing robot mapping by using the fourth point cloud (S105). By means of the method, the influence of the dynamic object point cloud can be eliminated from the current point cloud data according to historical point cloud data, so that the precision of mapping is greatly improved.

Description

Robot mapping method, device, computer-readable storage medium, and robot

This application claims the priority of the Chinese Patent Application No. 202011330699.X, filed with the Chinese Patent Office on November 24, 2020, the entire contents of which are incorporated herein by reference.

technical field

The present application belongs to the field of robotics, and in particular, relates to a method, device, computer-readable storage medium, and robot for mapping a robot.

Background technique

In the process of building a map, there are usually various dynamic objects in the surrounding environment, such as cars and pedestrians. If the point cloud collected by the lidar is used directly without processing, it is easy to construct the dynamic object point cloud into the map, so that the map matching effect during map building is poor, the robot pose estimation will have a large deviation, and the loopback quality and efficiency will be low. , the constructed map has lower accuracy and will affect the accuracy of subsequent positioning and navigation.

technical problem

In view of this, the embodiments of the present application provide a robot mapping method, device, computer-readable storage medium and robot, so as to solve the problem that the existing robot mapping method is easy to construct the point cloud of dynamic objects into the map, and the constructed map lower precision issues.

technical solutions

A first aspect of the embodiments of the present application provides a method for building a robot map, which may include:

Obtain the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and second pose collected at the historical moment;

calculating the pose difference between the first pose and the second pose;

Projecting the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud;

Filter out the dynamic object point cloud in the first point cloud according to the third point cloud;

The dynamic object point cloud is removed from the first point cloud to obtain a fourth point cloud, and the fourth point cloud is used for robot mapping.

Further, the filtering out the dynamic object point cloud in the first point cloud according to the third point cloud may include:

Screen out each candidate point from the first point cloud according to the third point cloud;

Clustering the selected candidate points to obtain each candidate point set;

Calculate the number of points in each candidate point set, the first variance of point coordinates in the main direction, and the second variance of point coordinates in the normal direction of the main direction;

According to the number of points, the first variance and the second variance, a preferred point set is selected from each candidate point set;

The dynamic object point cloud that meets the preset condition is screened out from the preferred point set.

Further, selecting each candidate point from the first point cloud according to the third point cloud may include:

Determine the corresponding point of the target point in the third point cloud, the target point is any point in the first point cloud, and the corresponding point is the point with the smallest distance from the target point;

Calculate the distance between the target point and the corresponding point, and calculate the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot;

If the distance between the target point and the corresponding point is greater than a preset first distance threshold, or the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot If the distance is greater than the preset second distance threshold, the target point is determined as a candidate point.

Further, after acquiring the first point cloud and the first pose collected by the robot at the current moment, it may also include:

De-distortion processing is performed on the first point cloud to obtain the first point cloud after the de-distortion;

Perform down-sampling processing on the dedistorted first point cloud to obtain a down-sampled first point cloud.

Further, the performing de-distortion processing on the first point cloud may include:

Obtain the first rotation angle and the second rotation angle of the lidar of the robot, the first rotation angle is the rotation angle when the first data point of the first point cloud is collected, and the second rotation angle is the rotation angle when the last data point of the first point cloud is collected;

Obtain the angle difference between each point of the first point cloud and the horizontal positive direction of the lidar;

The first point cloud is de-distorted according to the first rotation angle, the second rotation angle and the angle difference, so as to obtain a de-distorted first point cloud.

Further, performing the de-distortion processing on the first point cloud according to the first rotation angle, the second rotation angle and the angle difference may include:

The first point cloud is dedistorted according to the following formula:

px′=cos((θ′-θ)*angleH/2π)*px+

sin((θ′-θ)*angleH/2π)*py

py′=-sin((θ′-θ)*angleeH/2π)*px+

cos((θ′-θ)*angleH/2π)*py

pz'=pz

Wherein, θ is the first rotation angle, θ′ is the second rotation angle, angleH is the angle difference, (px, py, pz) is the coordinate of any point in the first point cloud, ( px', py', pz') are the coordinates of the point after de-distortion processing.

Further, the projecting the second point cloud to the coordinates of the first point cloud according to the pose difference may include:

The second point cloud is projected onto the coordinates of the first point cloud according to the following formula:

px_new=cos(dθ)*px_old+sin(dθ)*py_old-dx

py_new=-sin(dθ)*px_old+cos(dθ)*py_old-dy

pz_new=pz_old

Among them, (dx, dy, dθ) is the pose difference, (px_old, py_old, pz_old) is the coordinate of any point in the second point cloud, (px_new, py_new, pz_new) is the projected point coordinate.

A second aspect of the embodiments of the present application provides a robotic mapping device, which may include:

The data acquisition module is used to acquire the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and the second pose collected at the historical moment;

a pose difference calculation module for calculating the pose difference between the first pose and the second pose;

a point cloud projection module, configured to project the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud;

a dynamic object point cloud screening module, configured to filter out the dynamic object point cloud in the first point cloud according to the third point cloud;

The mapping module is used to remove the dynamic object point cloud from the first point cloud, obtain a fourth point cloud, and use the fourth point cloud to perform robot mapping.

Further, the dynamic object point cloud screening module may include:

a candidate point screening submodule, configured to screen out each candidate point from the first point cloud according to the third point cloud;

The clustering sub-module is used to cluster the selected candidate points to obtain each candidate point set;

The calculation submodule is used to calculate the number of points in each candidate point set, the first variance of the point coordinates in the main direction, and the second variance of the point coordinates in the normal direction of the main direction;

The preferred point set screening submodule is configured to filter out the preferred point set from each candidate point set according to the number of points, the first variance and the second variance;

The dynamic object point cloud screening sub-module is used to screen out the dynamic object point cloud that meets the preset conditions from the preferred point set.

Further, the candidate point screening sub-module may include:

A corresponding point determination unit, used to determine the corresponding point of the target point in the third point cloud, the target point is any point in the first point cloud, and the corresponding point is the smallest distance from the target point point;

A distance calculation unit for calculating the distance between the target point and the corresponding point, and calculating the distance between the target point and the corresponding point in the normal direction of the connecting line between the target point and the robot;

The candidate point screening unit is used for if the distance between the target point and the corresponding point is greater than a preset first distance threshold, or the target point and the corresponding point are in the distance between the target point and the robot. If the distance in the normal direction of the connecting line is greater than the preset second distance threshold, the target point is determined as a candidate point.

Further, the robot mapping device may also include:

a de-distortion processing module, configured to perform de-distortion processing on the first point cloud to obtain the de-distorted first point cloud;

The downsampling processing module is configured to perform downsampling processing on the dedistorted first point cloud to obtain the downsampled first point cloud.

Further, the de-distortion processing module may include:

The rotation angle acquisition sub-module is used to acquire the first rotation angle and the second rotation angle of the lidar of the robot, and the first rotation angle is the rotation angle when the first data point of the first point cloud is collected , the second rotation angle is the rotation angle when the last data point of the first point cloud is collected;

an angle difference acquisition sub-module, configured to acquire the angle difference between each point of the first point cloud and the horizontal positive direction of the lidar;

A de-distortion processing sub-module, configured to perform de-distortion processing on the first point cloud according to the first rotation angle, the second rotation angle and the angle difference, to obtain a de-distorted first point cloud.

Further, the de-distortion processing sub-module is specifically configured to perform de-distortion processing on the first point cloud according to the following formula:

px′=cos((θ′-θ)*angleH/2π)*px+

sin((θ′-θ)*angleH/2π)*py

py′=-sin((θ′-θ)*angleeH/2π)*px+

cos((θ′-θ)*angleH/2π)*py

pz'=pz

Further, the point cloud projection module is specifically configured to project the second point cloud onto the coordinates of the first point cloud according to the following formula:

px_new=cos(dθ)*px_old+sin(dθ)*py_old-dx

py_new=-sin(dθ)*px_old+cos(dθ)*py_old-dy

pz_new=pz_old

A third aspect of the embodiments of the present application provides a computer-readable storage medium, where a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a processor, the steps of any one of the above robot mapping methods are implemented .

A fourth aspect of the embodiments of the present application provides a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the computer program when the processor executes the computer program. The steps of any one of the above robot mapping methods.

A fifth aspect of the embodiments of the present application provides a computer program product, which, when the computer program product runs on a robot, causes the robot to execute the steps of any one of the above robot mapping methods.

beneficial effect

The beneficial effects of the embodiments of the present application compared with the prior art are: the embodiments of the present application obtain the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and the second point cloud collected at the historical moment. pose; calculate the pose difference between the first pose and the second pose; project the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain the third point cloud; filter out the dynamic object point cloud in the first point cloud according to the third point cloud; remove the dynamic object point cloud from the first point cloud to obtain a fourth point cloud, And use the fourth point cloud for robot mapping. Through the embodiment of the present application, the influence of the point cloud of dynamic objects can be eliminated from the current point cloud data according to the historical point cloud data, thereby greatly improving the mapping accuracy.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present application more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only for the present application. In some embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.

FIG. 1 is a flowchart of an embodiment of a robot mapping method in an embodiment of the present application;

Fig. 2 is a schematic flowchart of filtering out the dynamic object point cloud in the first point cloud according to the third point cloud;

3 is a structural diagram of an embodiment of a robot mapping device in an embodiment of the application;

FIG. 4 is a schematic block diagram of a robot in an embodiment of the present application.

Embodiments of the present invention

In order to make the purpose, features and advantages of the invention of the present application more obvious and understandable, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the following The described embodiments are only some, but not all, embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

It is to be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described feature, integer, step, operation, element and/or component, but does not exclude one or more other features , whole, step, operation, element, component and/or the presence or addition of a collection thereof.

It should also be understood that the terminology used in the specification of the application herein is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural unless the context clearly dictates otherwise.

It should also be further understood that, as used in this specification and the appended claims, the term "and/or" refers to and including any and all possible combinations of one or more of the associated listed items .

As used in this specification and the appended claims, the term "if" may be contextually interpreted as "when" or "once" or "in response to determining" or "in response to detecting" . Similarly, the phrases "if it is determined" or "if the [described condition or event] is detected" may be interpreted, depending on the context, to mean "once it is determined" or "in response to the determination" or "once the [described condition or event] is detected. ]" or "in response to detection of the [described condition or event]".

In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.

Referring to FIG. 1, an embodiment of a robot mapping method in the embodiment of the present application may include:

Step S101 , acquiring a first point cloud and a first pose collected by the robot at the current moment, and a second point cloud and a second pose collected at a historical moment.

In the embodiment of the present application, the point cloud data can be collected through the lidar of the robot, and the pose data can be collected through the wheel odometer of the robot. The frequency of data collection can be set according to the actual situation. For example, the frequency of data collection can be set to 10Hz, that is, data collection is performed every 0.1 seconds. In order to facilitate the distinction, the point cloud data and pose data collected at the current moment are recorded as the first point cloud and the first pose.

Preferably, after the first point cloud is acquired, preprocessing processes such as de-distortion and downsampling may also be performed on the first point cloud, thereby further improving the accuracy of the data.

When performing the de-distortion processing, the first rotation angle and the second rotation angle of the lidar may be obtained first, wherein the first rotation angle is the rotation when the first data point of the first point cloud is collected The second rotation angle is the rotation angle when the last data point of the first point cloud is collected; then, the angles between each point of the first point cloud and the horizontal positive direction of the lidar are obtained respectively difference; finally, perform de-distortion processing on the first point cloud according to the first rotation angle, the second rotation angle and the angle difference, to obtain a first point cloud after de-distortion.

Specifically, the first point cloud can be dedistorted according to the following formula:

px′=cos((θ′-θ)*angleH/2π)*px+

sin((θ′-θ)*angleH/2π)*py

py′=-sin((θ′-θ)*angleeH/2π)*px+

cos((θ′-θ)*angleH/2π)*py

pz'=pz

Wherein, θ is the first rotation angle, θ′ is the second rotation angle, angleH is the angle difference, (px, py, pz) is the coordinate of any point in the first point cloud, ( px', py', pz') are the coordinates of the point after de-distortion processing. By traversing all points in the first point cloud according to the above formula, the first point cloud after de-distortion can be obtained.

When performing downsampling processing, the voxel grid downsampling method can be used to divide the dedistorted first point cloud into several cube grids of fixed size. The coordinate centroid of each point in the grid is used as the point coordinate after downsampling of the grid, namely:

Among them, (px'(n), py'(n), pz'(n)) is the coordinate of the nth point in the grid, 1≤n≤N, N is the total number of points in the grid, ( px", py", pz") are the point coordinates obtained after downsampling the grid, that is, each grid is downsampled as a point, and traversing all grids, the first point cloud after the downsampling can be obtained .

In the embodiment of the present application, the influence of the point cloud of dynamic objects is removed from the current point cloud data according to the historical point cloud data. Therefore, it is also necessary to obtain the point cloud data and pose data collected at the historical moment. It can be set according to the actual situation. In this embodiment of the present application, it is preferably set to a time 1 second before the current time. For the convenience of distinction, the point cloud data and pose data collected at historical moments are recorded as the second point cloud and the second pose here. It is easy to understand that after the second point cloud is acquired, preprocessing such as de-distortion and downsampling can also be performed on it. The specific process is similar to the preprocessing process for the first point cloud. Repeat. For the convenience of description, the first point cloud and the second point cloud mentioned in the following are the results obtained after preprocessing.

Step S102: Calculate the pose difference between the first pose and the second pose.

Here, the first pose is recorded as (x1, y1, θ1), and the second pose is recorded as (x2, y2, θ2), then the pose difference between the two can be calculated according to the following formula :

dx=x1-x2

dy=y1-x2

dθ=θ1-θ2

Then (dx, dy, dθ) is the pose difference.

Step S103: Project the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud.

Specifically, the second point cloud can be projected onto the coordinates of the first point cloud according to the following formula:

px_new=cos(dθ)*px_old+sin(dθ)*py_old-dx

py_new=-sin(dθ)*px_old+cos(dθ)*py_old-dy

pz_new=pz_old

Wherein, (px_old, py_old, pz_old) are the coordinates of any point in the second point cloud, and (px_new, py_new, pz_new) are the projected coordinates of the point. Traversing all the points in the second point cloud according to the above formula, the projected third point cloud can be obtained.

Step S104: Screen out the point cloud of dynamic objects in the first point cloud according to the third point cloud.

As shown in FIG. 2, step S104 may specifically include the following processes:

Step S1041: Screen out each candidate point from the first point cloud according to the third point cloud.

Taking any point in the first point cloud (denoted as the target point) as an example, first, the point with the smallest distance from the target point can be determined in the third point cloud, and it is used as the target The corresponding point of the point, the target point and the corresponding point constitute a point pair. In a specific implementation of the embodiment of the present application, a KD tree may be used to determine the point with the smallest distance from the target point.

Then, the distance between the target point and the corresponding point may be calculated, and the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot may be calculated. If the distance between the target point and the corresponding point is greater than a preset first distance threshold, or the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot If the distance is greater than the preset second distance threshold, the target point may be determined as a candidate point. The specific values of the first distance threshold and the second distance threshold may be set according to actual conditions, which are not specifically limited in this embodiment of the present application. Traversing all the points in the first point cloud according to the above method, each candidate point can be screened out.

Step S1042 , cluster each of the selected candidate points to obtain each candidate point set.

In the present application, any clustering method in the prior art may be selected according to the actual situation to perform clustering, which is not specifically limited in this embodiment of the present application.

Preferably, in a specific implementation of the embodiment of the present application, the Euclidean distance may be used for the segmentation metric. In the clustering process, if the distance between the current point and the previous point is within the preset threshold range, the current point is clustered into the category of the previous point; otherwise, the current point is set as a new clustering category, according to The distance judges whether the next point belongs to the same category as the point, and repeats the above process until all points are divided into different categories, and the points in each cluster category constitute a candidate point set.

Step S1043: Calculate the number of points in each candidate point set, the first variance of the point coordinates in the main direction, and the second variance of the point coordinates in the normal direction of the main direction.

Wherein, the main direction of any candidate point set is the direction corresponding to the mean angle, and the mean angle is the mean value of the rotation angles corresponding to each point in the candidate point set.

Step S1044: Screen out a preferred point set from each candidate point set according to the number of points, the first variance, and the second variance.

Taking any candidate point set as an example, if the number of points in the point set is greater than a preset number threshold, and the ratio of the first variance to the second variance is less than a preset ratio threshold, the point The set is determined as the preferred point set. The specific values of the number threshold and the ratio threshold may be set according to actual conditions, which are not specifically limited in this embodiment of the present application. By traversing each candidate point set according to the above method, the preferred point set can be filtered out.

Step S1045: Screen out the dynamic object point cloud that meets the preset condition from the preferred point set.

Specifically, the KD tree can be used to find points in the preferred point set whose distances are less than a preset third distance threshold between each point in the preferred point set and the point cloud space at the current moment, and mark these found points as dynamic object point clouds. The specific value of the third distance threshold may be set according to the actual situation, which is not specifically limited in this embodiment of the present application. In particular, all points in the preferred point set may also be marked as dynamic object point clouds.

Step S105 , remove the dynamic object point cloud from the first point cloud, obtain a fourth point cloud, and use the fourth point cloud to build a robot map.

To sum up, the embodiment of the present application obtains the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and the second pose collected at the historical moment; calculates the first pose and The pose difference between the second poses; the second point cloud is projected onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud; according to the third point The cloud filters out the dynamic object point cloud in the first point cloud; removes the dynamic object point cloud from the first point cloud to obtain a fourth point cloud, and uses the fourth point cloud for robot construction picture. Through the embodiment of the present application, the influence of the point cloud of dynamic objects can be eliminated from the current point cloud data according to the historical point cloud data, thereby greatly improving the mapping accuracy.

It should be understood that the size of the sequence numbers of the steps in the above embodiments does not mean the sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

Corresponding to the robot mapping method described in the above embodiment, FIG. 3 shows a structural diagram of an embodiment of a robot mapping apparatus provided by an embodiment of the present application.

In this embodiment, a robot mapping device may include:

The data acquisition module 301 is used to acquire the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and the second pose collected at the historical moment;

a pose difference calculation module 302, configured to calculate the pose difference between the first pose and the second pose;

A point cloud projection module 303, configured to project the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud;

A dynamic object point cloud screening module 304, configured to filter out the dynamic object point cloud in the first point cloud according to the third point cloud;

The mapping module 305 is configured to remove the dynamic object point cloud from the first point cloud, obtain a fourth point cloud, and use the fourth point cloud to construct a robot map.

Further, the dynamic object point cloud screening module may include:

Further, the candidate point screening sub-module may include:

a distance calculation unit, configured to calculate the distance between the target point and the corresponding point, and calculate the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot;

Further, the robot mapping device may also include:

Further, the de-distortion processing module may include:

px′=cos((θ′-θ)*angleH/2π)*px+

sin((θ′-θ)*angleH/2π)*py

py′=-sin((θ′-θ)*angleeH/2π)*px+

cos((θ′-θ)*angleH/2π)*py

pz'=pz

px_new=cos(dθ)*px_old+sin(dθ)*py_old-dx

py_new=-sin(dθ)*px_old+cos(dθ)*py_old-dy

pz_new=pz_old

Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described devices, modules and units can be referred to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

In the foregoing embodiments, the description of each embodiment has its own emphasis. For parts that are not described or described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

FIG. 4 shows a schematic block diagram of a robot provided by an embodiment of the present application. For convenience of description, only parts related to the embodiment of the present application are shown.

As shown in FIG. 4 , the robot 4 of this embodiment includes a processor 40 , a memory 41 , and a computer program 42 stored in the memory 41 and executable on the processor 40 . When the processor 40 executes the computer program 42 , the steps in each of the above embodiments of the robot mapping method are implemented, for example, steps S101 to S105 shown in FIG. 1 . Alternatively, when the processor 40 executes the computer program 42, the functions of the modules/units in each of the foregoing apparatus embodiments, such as the functions of the modules 301 to 305 shown in FIG. 3, are implemented.

Exemplarily, the computer program 42 may be divided into one or more modules/units, and the one or more modules/units are stored in the memory 41 and executed by the processor 40 to complete the this application. The one or more modules/units may 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 42 in the robot 4 .

Those skilled in the art can understand that FIG. 4 is only an example of the robot 4, and does not constitute a limitation to the robot 4. It may include more or less components than the one shown in the figure, or combine some components, or different components, such as The robot 4 may also include input and output devices, network access devices, buses, and the like.

The processor 40 may be a central processing unit (Central Processing Unit, CPU), or other general-purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), Field-Programmable Gate Array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The memory 41 may be an internal storage unit of the robot 4 , such as a hard disk or a memory of the robot 4 . The memory 41 can also be an external storage device of the robot 4, such as a plug-in hard disk equipped on the robot 4, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, Flash card (Flash Card) and so on. Further, the memory 41 may also include both an internal storage unit of the robot 4 and an external storage device. The memory 41 is used to store the computer program and other programs and data required by the robot 4 . The memory 41 can also be used to temporarily store data that has been output or will be output.

Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. Module completion, that is, dividing the internal structure of the device into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated in one processing unit, or each unit may exist physically alone, or two or more units may be integrated in one unit, and the above-mentioned integrated units may adopt hardware. It can also be realized in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing from each other, and are not used to limit the protection scope of the present application. For the specific working processes of the units and modules in the above-mentioned system, reference may be made to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of this application.

In the embodiments provided in this application, it should be understood that the disclosed apparatus/robot and method may be implemented in other ways. For example, the device/robot embodiments described above are only illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods, such as multiple units or Components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated modules/units, if implemented in the form of software functional units and sold or used as independent products, may be stored in a computer-readable storage medium. Based on this understanding, the present application can implement all or part of the processes in the methods of the above embodiments, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer When the program is executed by the processor, the steps of the foregoing method embodiments can be implemented. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form, and the like. The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM, Read-Only Memory) ), random access memory (RAM, Random Access Memory), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content contained in the computer-readable storage medium may 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, computer-readable Storage media exclude electrical carrier signals and telecommunications signals.

The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the above-mentioned embodiments, those of ordinary skill in the art should understand that: it can still be used for the above-mentioned implementations. The technical solutions described in the examples are modified, or some technical features thereof are equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions in the embodiments of the application, and should be included in the within the scope of protection of this application.

Claims

A method for building a robot map, comprising:

Obtain the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and second pose collected at the historical moment;

calculating the pose difference between the first pose and the second pose;

Projecting the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud;

Filter out the dynamic object point cloud in the first point cloud according to the third point cloud;

The dynamic object point cloud is removed from the first point cloud to obtain a fourth point cloud, and the fourth point cloud is used for robot mapping.
The robot mapping method according to claim 1, wherein the filtering out the dynamic object point cloud in the first point cloud according to the third point cloud comprises:

Screen out each candidate point from the first point cloud according to the third point cloud;

Clustering the selected candidate points to obtain each candidate point set;

Calculate the number of points in each candidate point set, the first variance of point coordinates in the main direction, and the second variance of point coordinates in the normal direction of the main direction;

According to the number of points, the first variance and the second variance, a preferred point set is selected from each candidate point set;

The dynamic object point cloud that meets the preset condition is screened out from the preferred point set.
The robot mapping method according to claim 2, wherein the filtering out each candidate point from the first point cloud according to the third point cloud comprises:

Determine the corresponding point of the target point in the third point cloud, the target point is any point in the first point cloud, and the corresponding point is the point with the smallest distance from the target point;

Calculate the distance between the target point and the corresponding point, and calculate the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot;

If the distance between the target point and the corresponding point is greater than a preset first distance threshold, or the distance between the target point and the corresponding point in the normal direction of the line connecting the target point and the robot If the distance is greater than the preset second distance threshold, the target point is determined as a candidate point.
The method for building a robot map according to claim 1, wherein after acquiring the first point cloud and the first pose collected by the robot at the current moment, the method further comprises:

De-distortion processing is performed on the first point cloud to obtain the first point cloud after the de-distortion;

Perform down-sampling processing on the dedistorted first point cloud to obtain a down-sampled first point cloud.
The method for building a robot map according to claim 4, wherein the performing de-distortion processing on the first point cloud comprises:

Obtain the first rotation angle and the second rotation angle of the lidar of the robot, the first rotation angle is the rotation angle when the first data point of the first point cloud is collected, and the second rotation angle is the rotation angle when the last data point of the first point cloud is collected;

Obtain the angle difference between each point of the first point cloud and the horizontal positive direction of the lidar;

The first point cloud is de-distorted according to the first rotation angle, the second rotation angle and the angle difference, so as to obtain a de-distorted first point cloud.
The robot mapping method according to claim 5, wherein the performing a de-distortion process on the first point cloud according to the first rotation angle, the second rotation angle and the angle difference, comprising: :

The first point cloud is dedistorted according to the following formula:

px′=cos((θ′-θ)*angleH/2π)*px+sin((θ′-θ)*angleH/2π)*py

py′=-sin((θ′-θ)*angleeH/2π)*px+cos((θ′-θ)*angleH/2π)*py

pz'=pz

Among them, θ is the first rotation angle, θ′ is the second rotation angle, angleH is the angle difference, (px, py, pz) is the coordinate of any point in the first point cloud, ( px', py', pz') are the coordinates of the point after de-distortion processing.
The robot mapping method according to any one of claims 1 to 6, wherein the projecting the second point cloud onto the coordinates of the first point cloud according to the pose difference includes the following steps: :

The second point cloud is projected onto the coordinates of the first point cloud according to the following formula:

px_new=cos(dθ)*px_old+sin(dθ)*py_old-dx

py_new=-sin(dθ)*px_old+cos(dθ)*py_old-dy

pz_new=pz_old

Among them, (dx, dy, dθ) is the pose difference, (px_old, py_old, pz_old) is the coordinate of any point in the second point cloud, (px_new, py_new, pz_new) is the projected point coordinate.
A robot mapping device, comprising:

The data acquisition module is used to acquire the first point cloud and the first pose collected by the robot at the current moment, and the second point cloud and the second pose collected at the historical moment;

a pose difference calculation module for calculating the pose difference between the first pose and the second pose;

a point cloud projection module, configured to project the second point cloud onto the coordinates of the first point cloud according to the pose difference to obtain a third point cloud;

a dynamic object point cloud screening module, configured to filter out the dynamic object point cloud in the first point cloud according to the third point cloud;

The mapping module is used to remove the dynamic object point cloud from the first point cloud, obtain a fourth point cloud, and use the fourth point cloud to perform robot mapping.
A computer-readable storage medium storing a computer program, characterized in that, when the computer program is executed by a processor, the robot mapping according to any one of claims 1 to 7 is realized steps of the method.
A robot, comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, when the processor executes the computer program, the implementation of claims 1 to 7 The steps of any one of the robot mapping methods.