Robot positioning method and device, robot and storage medium

Abstract

The invention provides a robot positioning method and device, a robot and a storage medium. The method comprises the following steps: acquiring a current frame point cloud collected by a laser radar; for each to-be-matched point in the current frame point cloud, determining a matching fitting plane corresponding to the to-be-matched point according to the global static map and the real-time local map, and determining a point matching result of the to-be-matched point according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of a working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud; determining a point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud; and according to a point cloud matching result, determining the posture of the robot. The robot can be accurately positioned, and the positioning precision of the robot is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a robot positioning method, a robot positioning device, a robot and a storage medium. Background Art

[0002] With the development of science and technology, robots are gradually being widely used in many fields. When performing specific tasks, robots usually need to use map data to locate their own position and posture in order to achieve the navigation function. Therefore, it is very important for robots to accurately locate their own position and posture.

[0003] In the related technology, there are two main methods for robot positioning. One is a positioning method with 2D lidar sensor as the core, which mainly uses point-to-point or point-to-line matching on the basis of 2D static map to optimize the laser odometer for robot positioning; the other is a positioning method with 3D lidar sensor as the core, which mainly builds a three-dimensional static map of the robot's working area, uses the current frame point cloud and the static map to match points to surfaces, optimizes the laser odometer, and thus positions the robot.

[0004] However, both methods rely heavily on well-constructed static maps. If there are dynamic obstacles or the robot's working scene changes frequently, the robot's positioning will be lost and the single-point positioning accuracy will be low. Summary of the invention

[0005] The embodiments of the present invention provide a robot positioning method, a device, a robot and a storage medium to accurately position the robot and improve the positioning accuracy of the robot.

[0006] In a first aspect, an embodiment of the present invention provides a robot positioning method, comprising:

[0007] Get the current frame point cloud collected by the LiDAR;

[0008] For each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point is determined according to the global static map and the real-time local map, and a point matching result of the to-be-matched point is determined according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of the working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud;

[0009] Determine a point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud;

[0010] The position and posture of the robot are determined according to the point cloud matching result.

[0011] In a possible implementation, for each to-be-matched point in the current frame point cloud, determining a matching fitting plane corresponding to the to-be-matched point according to a global static map and a real-time local map includes:

[0012] For each to-be-matched point in the current frame point cloud, determining a second preset number of first search points closest to the to-be-matched point in the global static map;

[0013] Determine a first average distance between the point to be matched and the first search point; wherein the first average distance is an average value of the distances between the point to be matched and each first search point;

[0014] Determining whether the first average distance is greater than a first preset threshold;

[0015] If the first average distance is greater than the first preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

[0016] In a possible implementation manner, after determining whether the first average distance is greater than a first preset threshold, the method further includes:

[0017] If the first average distance is less than or equal to the first preset threshold, establishing a first fitting plane according to all first search points;

[0018] determining a first distance from each first search point to the first fitting plane;

[0019] Determine whether the first distances corresponding to all the first search points are less than or equal to a second preset threshold;

[0020] If the first distances corresponding to all the first search points are less than or equal to the second preset threshold, determining the first fitting plane as the matching fitting plane corresponding to the point to be matched;

[0021] If there is at least one first search point whose corresponding first distance is greater than the second preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

[0022] In a possible implementation, determining a matching fitting plane corresponding to the point to be matched according to the point to be matched and the real-time local map includes:

[0023] In the real-time local map, determining a third preset number of second search points closest to the point to be matched;

[0024] Determine a second average distance between the point to be matched and the second search point; wherein the second average distance is an average value of the distances between the point to be matched and each second search point;

[0025] Determining whether the second average distance is greater than the first preset threshold;

[0026] If the second average distance is less than or equal to the first preset threshold, establishing a second fitting plane according to all second search points;

[0027] determining a second distance from each second search point to the second fitting plane;

[0028] Determine whether the second distance corresponding to each second search point is greater than a second preset threshold;

[0029] If the second distances corresponding to all the second search points are less than or equal to the second preset threshold, the second fitting plane is determined as the matching fitting plane corresponding to the point to be matched.

[0030] In a possible implementation, the method further includes:

[0031] If the second average distance is greater than the first preset threshold, or there is a second search point whose corresponding second distance is greater than the second preset threshold, the point to be matched is determined as an interference point and is removed from the point cloud of the current frame.

[0032] In a possible implementation, determining the point matching result of the point to be matched according to the point to be matched, the matching fitting plane and a preset robust kernel function includes:

[0033] Determine a third distance between the to-be-matched point and the matching fitting plane, and a normal vector of the matching fitting plane;

[0034] Determining a weight coefficient according to the third distance and the robust kernel function;

[0035] According to the weight coefficient, the third distance and the normal vector are optimized to obtain a target point corresponding to the point to be matched;

[0036] Determine a noise item corresponding to the point to be matched according to the weight coefficient and a fourth distance between the point to be matched and the laser radar;

[0037] Based on the target point and the noise item, a point matching result of the point to be matched is determined.

[0038] In a possible implementation manner, determining a weight coefficient according to the third distance and the robust kernel function includes:

[0039] According to the expression: Determine the weight coefficient;

[0040] Wherein, s represents the weight coefficient, represents the robust kernel function, r represents the third distance, and pd represents the preset control coefficient.

[0041] In a second aspect, an embodiment of the present invention provides a robot positioning device, comprising:

[0042] The acquisition module is used to obtain the current frame point cloud collected by the laser radar;

[0043] A point matching module is used to determine, for each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point according to a global static map and a real-time local map, and to determine a point matching result of the to-be-matched point according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of a working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud;

[0044] A point cloud matching module, used to determine the point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud;

[0045] A positioning module is used to determine the position and posture of the robot according to the point cloud matching result.

[0046] In a third aspect, an embodiment of the present invention provides a robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the steps of the method described in the first aspect or any possible implementation method of the first aspect are implemented.

[0047] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method described in the first aspect or any possible implementation method of the first aspect are implemented.

[0048] Compared with the prior art, the embodiments of the present invention have the following beneficial effects:

[0049] The embodiment of the present invention matches each to-be-matched point in the acquired current frame point cloud in the global static map and the real-time local map, and can simultaneously maintain the global static map and the real-time local map, thereby improving the probability of successful matching of the to-be-matched point; by determining the matching fitting plane corresponding to the to-be-matched point, and determining the point matching result of the matching point according to the to-be-matched point, the matching fitting plane and the robust kernel function, the robust kernel function can be used for optimization to reduce the probability of matching errors, improve the accuracy of the point matching result, and thus improve the accuracy of the final positioning; according to the point matching results of all the to-be-matched points, the point cloud matching result of the current frame point cloud is determined, and the maintenance of the global static map and the real-time local map by the current frame point cloud can be achieved; finally, according to the point cloud matching result, the position and posture of the robot is determined, and accurate positioning of the robot can be achieved, thereby improving the accuracy of the robot positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0051] Figure 1A is an application scenario diagram of a robot positioning method provided by an embodiment of the present invention;

[0052] Figure 1B is an application scenario diagram of another robot positioning method provided by an embodiment of the present invention;

[0053] Figure 2 is a flow chart of an implementation of a robot positioning method provided by an embodiment of the present invention;

[0054] Figure 3 is a flowchart for determining a point cloud matching result provided by an embodiment of the present invention;

[0055] Figure 4 is a schematic diagram of a function image of a robust kernel function provided by an embodiment of the present invention;

[0056] Figure 5 is a schematic flow chart of a voxel filtering process provided by an embodiment of the present invention;

[0057] Figure 6 is a schematic structural diagram of a robot positioning device provided by an embodiment of the present invention;

[0058] Figure 7 Schematic diagram of a robot provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0059] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[0060] The inventors have found that in order to perform robot positioning, it is usually necessary to build a static map, obtain points or point clouds through sensors, and match them with the static map to achieve robot positioning. This is very dependent on the constructed static map. If there is interference from dynamic obstacles or the robot's working scene changes frequently, it will affect the robot's positioning, resulting in the loss of robot positioning or low positioning accuracy.

[0061] Furthermore, if the robot's working area changes, a new static map needs to be rebuilt and the static map needs to be updated to the robot's positioning module, which affects the convenience of the robot during actual deployment.

[0062] In order to realize the positioning of the robot quickly and accurately, in the implementation mode of the present invention, the historical frame point cloud is used to construct a real-time dynamic map, and each point in the acquired current frame point cloud is matched with the global static map and the real-time local map to improve the probability of successful matching of the points to be matched; and the robust kernel function is used for optimization to reduce the probability of matching errors and improve the accuracy of the point matching results, so as to maintain the global static map and the real-time local map, and then the robot is positioned to improve the accuracy of the final positioning.

[0063] In order to make the purpose, technical solutions and advantages of the present invention more clear, specific embodiments will be described below in conjunction with the accompanying drawings.

[0064] Figure 1A This is an application scenario diagram of a robot positioning method provided by an embodiment of the present invention. When the robot is positioned in the working area, it uses the laser radar set by itself to scan and collect the corresponding point cloud. The laser radar scans frame by frame at a preset angle. After scanning and detecting an object, it returns the corresponding point cloud to form the current frame point cloud. Figure 1A In the scanning angle setting of the laser radar, there are objects A and B. When the laser radar scans object A, it will return the corresponding point cloud (as shown by the dotted line between the laser radar and object A in Figure 1); similarly, when the laser radar scans object B, it will return the corresponding point cloud (as shown by the dotted line between the laser radar and object B in Figure 1). In addition, due to the different distances between the laser radar and objects A and B, the time to collect the point cloud will also be different.

[0065] In addition, assuming Figure 1A The scene shown is the robot's working area, based on which a global static map of the robot can be constructed, which contains objects A and B. Since there may be dynamic obstacles in the robot's working area, see Figure 1B The application scenario diagram of another robot positioning method shown in the figure also includes object C, which is a dynamic obstacle. At this time, when the laser radar scans and detects object C, it is difficult to match it with the static map, which will cause the robot to lose positioning or the positioning accuracy is not high. Therefore, the present invention also constructs a real-time local map, and the object C may be included in the implementation of the local map. By matching the current frame point cloud with the global static map and the real-time local map, the probability of successful matching of the points to be matched can be increased, thereby improving the positioning accuracy of the robot.

[0066] Accordingly, Figure 2 A flowchart of a robot positioning method provided by an embodiment of the present invention is described in detail as follows:

[0067] Step S201, obtaining the current frame point cloud collected by the laser radar.

[0068] In this embodiment, a laser radar installed on the robot is used to perform scanning and detection to collect a point cloud of the surrounding environment.

[0069] Step S202, for each to-be-matched point in the current frame point cloud, determine the matching fitting plane corresponding to the to-be-matched point according to the global static map and the real-time local map, and determine the point matching result of the to-be-matched point according to the to-be-matched point, the matching fitting plane and the preset robust kernel function; wherein the global static map is a static map of the working area corresponding to the robot, and the real-time local map is determined based on the first preset number of historical frame point clouds before the current frame point cloud.

[0070] In this embodiment, due to the difference in scanning time and return time, there are time differences in the points received in the point cloud, and the points in the current frame point cloud can be sorted and matched according to the time when the points were received. Among them, the point to be matched is matched with the global static map and the real-time local map, and a search can be performed in both the global static map and the real-time local map to accurately find the position corresponding to the point to be matched.

[0071] Here, considering that the original point cloud of the laser radar has certain errors, and the errors vary with the distance of the laser radar scanning, it is necessary to optimize the obtained results in the matching process, that is, to use a robust kernel function to determine the point matching result of the point to be matched to ensure the accuracy of the point matching result. Among them, the point matching result can be the point that the point to be matched matches the corresponding point in the global static map and / or the local real-time map.

[0072] In addition, the global static map can be constructed by scanning the working area of ​​the robot before the robot starts working. The local real-time map can be constructed locally by the robot according to the historical frame point cloud before the current frame point cloud during the working process. Among them, the first preset number of historical frame point clouds forming the local real-time map can be determined according to the size of the working area, for example, it can be 30 frames, 50 frames and 70 frames, etc., which is not limited here.

[0073] Optionally, when constructing the global static map, the initially obtained global static map may be subjected to uniformity and denoising processing to ensure the matching effect of the global static map.

[0074] Step S203, determining the point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud.

[0075] In this embodiment, the current frame point cloud includes multiple points to be matched. After obtaining the point matching result of each point to be matched, the point matching results of these points to be matched can be combined to form a local point cloud map corresponding to the current frame point cloud, that is, the point cloud matching result corresponding to the current frame point cloud can be obtained.

[0076] Step S204, determining the position and posture of the robot according to the point cloud matching result.

[0077] In this embodiment, through the point cloud matching results of the current frame point cloud obtained in the above steps, a matching range can be found from the global static map and / or the local real-time map, thereby determining the current position and posture of the robot, that is, obtaining the precise positioning of the robot.

[0078] Here, the iterative closest point algorithm can be used for point cloud registration to find matching points from the global static map and / or the local real-time map. For determining the position and posture of the robot, the Kalman filter can be used to achieve optimal estimation. Among them, this embodiment uses the iterative closest point algorithm, adopts an iterative method, finds matching points between the point cloud matching results and the corresponding map, and calculates the optimal transformation parameters between these points, so that the distance between the corresponding points is minimized.

[0079] The embodiment of the present invention matches each to-be-matched point in the acquired current frame point cloud in the global static map and the real-time local map, and can simultaneously maintain the global static map and the real-time local map, thereby improving the probability of successful matching of the to-be-matched point; by determining the matching fitting plane corresponding to the to-be-matched point, and determining the point matching result of the matching point according to the to-be-matched point, the matching fitting plane and the robust kernel function, the robust kernel function can be used for optimization to reduce the probability of matching errors, improve the accuracy of the point matching result, and thus improve the accuracy of the final positioning; according to the point matching results of all the to-be-matched points, the point cloud matching result of the current frame point cloud is determined, and the maintenance of the global static map and the real-time local map by the current frame point cloud can be achieved; finally, according to the point cloud matching result, the position and posture of the robot is determined, and accurate positioning of the robot can be achieved, thereby improving the accuracy of the robot positioning.

[0080] In some embodiments, see Figure 3 The implementation flow chart for determining the point cloud matching result shown in the figure determines the matching fitting plane corresponding to each to-be-matched point in the current frame point cloud according to the global static map and the real-time local map. It can be: first, for each to-be-matched point in the current frame point cloud, in the global static map, determine the second preset number of first search points closest to the to-be-matched point; then determine the first average distance between the to-be-matched point and the first search points; wherein the first average distance is the average value of the distance between the to-be-matched point and each first search point; then, determine whether the first average distance is greater than a first preset threshold; if the first average distance is greater than the first preset threshold, determine the matching fitting plane corresponding to the to-be-matched point according to the to-be-matched point and the real-time local map.

[0081] In this embodiment, the global static map is first used for searching and matching, the first search point corresponding to the point to be matched is found, and the distance between each first search point and the point to be matched is obtained. Then, the first average distance between the point to be matched and the first search point is determined, and it is judged whether the first average distance is greater than a first preset threshold, so as to preliminarily determine whether the first search point matched in the global static map is a point that matches the point to be matched.

[0082] If the first average distance is greater than the first preset threshold, it means that the first search point does not match the point to be matched, and there is no matching point in the global static map. The point to be matched may correspond to a dynamic object in the working area or a changing environment. Therefore, the real-time local map can be used for matching to find the corresponding matching fitting plane.

[0083] Optionally, when determining the second preset number of first search points closest to the point to be matched, the point to be matched can be first transformed from the lidar coordinate system to the global coordinate system, and then in the global static map, with the point to be matched as the source, the second preset number of first search points closest to the point to be matched are determined.

[0084] Here, the second preset number of first search points may be 5 first search points, 6 first search points, 7 first search points, and so on.

[0085] The above describes the case where the first average distance is greater than the first preset threshold. The following specifically describes the case where the first average distance is not greater than the first preset threshold.

[0086] Optionally, after determining whether the first average distance is greater than a first preset threshold, this embodiment may further include: if the first average distance is less than or equal to the first preset threshold, establishing a first fitting plane based on all first search points; and determining the first distance from each first search point to the first fitting plane; and then determining whether the first distances corresponding to all first search points are less than or equal to a second preset threshold; if the first distances corresponding to all first search points are less than or equal to the second preset threshold, determining the first fitting plane as the matching fitting plane corresponding to the point to be matched; if there is at least one first search point whose first distance is greater than the second preset threshold, determining the matching fitting plane corresponding to the point to be matched based on the point to be matched and the real-time local map.

[0087] In this embodiment, the first average distance is less than or equal to the first preset threshold, indicating that the point to be matched may find a matching target point cloud in the global static map. Here, a first fitting plane is established based on all the first search points, and it is determined whether the first distances corresponding to all the first search points are less than or equal to the second preset threshold. The flatness of the first fitting plane can be detected to further determine whether the point to be matched matches the global static map.

[0088] Here, if the first distances corresponding to all the first search points are less than or equal to the second preset threshold, it means that the flatness of the first fitting plane meets the corresponding requirement, that is, the point to be matched matches the first fitting plane.

[0089] If there is at least one first search point whose corresponding first distance is greater than the second preset threshold, it means that the flatness of the first fitting plane does not meet the corresponding requirements, and the point to be matched does not match the first fitting plane, and can be matched again from the real-time dynamic map.

[0090] Here, the first preset threshold may be 0.05m, 0.1m, 0.15m, etc.

[0091] The above-mentioned embodiments mainly describe the process of matching the points to be matched with the global static map and determining the matching fitting plane. The following describes the process of matching the points to be matched with the real-time local map and determining the matching fitting plane.

[0092] In some embodiments, Figure 3 As shown, according to the point to be matched and the real-time local map, the matching fitting plane corresponding to the point to be matched is determined. It can be that first, in the real-time local map, a third preset number of second search points closest to the point to be matched are determined; then the second average distance between the point to be matched and the second search points is determined; wherein the second average distance is the average of the distances between the point to be matched and each second search point; then, it is determined whether the second average distance is greater than a first preset threshold; if the second average distance is less than or equal to the first preset threshold, a second fitting plane is established according to all the second search points; then, the second distance from each second search point to the second fitting plane is determined; finally, it is determined whether the second distance corresponding to each second search point is greater than the second preset threshold; if the second distances corresponding to all the second search points are less than or equal to the second preset threshold, the second fitting plane is determined as the matching fitting plane corresponding to the point to be matched.

[0093] In this embodiment, the point to be matched can also be used as a source to search in the real-time local map to find the third preset number of second search points closest to the point to be matched. The second search points are used to gradually determine whether the point to be matched can be successfully matched with the real-time local map, including using the second average distance between the point to be matched and the second search point for preliminary judgment, and using the second distance from each second search point to the second fitting plane for secondary judgment. The above judgment process for the real-time local map is similar to the judgment process for the global static map, except that one uses a real-time local map and the other uses a global static map. You can refer to the description in the above embodiment, and will not repeat it here.

[0094] In addition, the third preset number may be the same as the second preset number, or may be different from the second preset number.

[0095] So far, the process of determining the matching fitting plane for each point to be matched has been introduced. The process of determining the point matching result for the point to be matched will be introduced below.

[0096] In some embodiments, the point matching result of the point to be matched is determined according to the point to be matched, the matching fitting plane and the preset robust kernel function. The method may be to first determine the third distance between the point to be matched and the matching fitting plane, and the normal vector of the matching fitting plane; then determine the weight coefficient according to the third distance and the robust kernel function; then, according to the weight coefficient, optimize the third distance and the normal vector to obtain the target point corresponding to the point to be matched; then, determine the noise item corresponding to the point to be matched according to the weight coefficient and the fourth distance between the point to be matched and the laser radar; finally, determine the point matching result of the point to be matched based on the target point and the noise item.

[0097] In this embodiment, when determining the point matching result, the third distance between the point to be matched and the matching fitting plane and the normal vector of the matching fitting plane are first determined, so as to obtain the preliminary matching situation between the point to be matched and the matching fitting plane.

[0098] Here, the plane equation of the matching fitting plane may be calculated first, and then the third distance from the to-be-matched point to the matching fitting plane may be calculated using the plane equation, and the normal vector of the matching fitting plane may be extracted from the plane equation.

[0099] Based on this, the preliminary matching point in the global static map or the real-time local map corresponding to the point to be matched can be found.

[0100] Considering the error in the original point cloud of the laser radar, and the error is related to the laser radar scanning distance, it is necessary to optimize the third distance and the corresponding normal vector to reduce the probability that the matched point does not conform to the actual situation. Among them, the weight coefficient and the residual term can be calculated using the robust kernel function, and the weight coefficient and the residual term can be used for optimization to find the final corresponding matching point of the point to be matched, that is, the point matching result.

[0101] Here, the third distance between the point to be matched and the matching fitting plane can be used as a residual. The larger the residual (third distance), the smaller the corresponding weight coefficient. In addition, the noise term can be calculated to add noise to the successfully matched points. Thus, the weight coefficient and the noise term can be used to optimize the successfully matched points, reducing the probability of optimizing the estimated pose in the wrong direction due to the mismatched points when optimizing the positioning of the robot in the future.

[0102] Optionally, this embodiment determines the weight coefficient according to the third distance and the robust kernel function, which may be based on the expression: Determine the weight coefficient; where s represents the weight coefficient, represents the robust kernel function, r represents the third distance, and pd represents the preset control coefficient.

[0103] In this embodiment, see Figure 4The diagram of the function graph of the robust kernel function is shown, wherein the larger the third distance (horizontal axis), the smaller the corresponding weight coefficient (vertical axis), in units of m.

[0104] In some embodiments, Figure 3 As shown, the robot positioning method provided by the present invention also includes: if the second average distance is greater than the first preset threshold, or there is a second distance corresponding to the second search point greater than the second preset threshold, the point to be matched is determined as an interference point, and the point to be matched is removed from the current frame point cloud.

[0105] In this embodiment, if the second average distance is greater than the first preset threshold, or there is a second search point corresponding to a second distance greater than the second preset threshold, that is, the point to be matched cannot be successfully matched with the global static map, and cannot be successfully matched with the real-time local map, it means that the point to be matched is likely to be an interference point in the environment. Therefore, the point to be matched can be determined as an interference point, and the point to be matched will no longer be matched, and the next point to be matched in the current frame point cloud can be matched.

[0106] In addition, in some embodiments, after obtaining the current frame point cloud collected by the lidar, the current frame point cloud can also be voxel filtered to ensure that the number of lidar points in the current frame point cloud after voxel filtering is within the range of points with optimal matching effect in different spatial environments of the robot.

[0107] For example, Figure 5 The flow chart of the voxel filtering process shown in the figure shows that the voxel filtering of the current frame point cloud can be performed by first determining whether the number of points in the current frame point cloud is greater than a first preset number of points.

[0108] If the number of points in the current frame point cloud is greater than the first preset number of points, voxel filtering is performed on the current frame point cloud according to the preset filtering scale; then it is determined whether the number of points in the current frame point cloud after voxel filtering is greater than the second preset number of points. If the number of points in the current frame point cloud after voxel filtering is not greater than the second preset number of points, the filtering scale is updated, and voxel filtering is performed on the current frame point cloud according to the updated filtering scale until the number of points in the current frame point cloud is greater than the second preset number of points, so as to obtain the final current frame point cloud.

[0109] Here, updating the filter scale can be done using the expression: i+1 =r i ×0.5, update the filter scale, where r i+1 Indicates the filtering scale of the i+1th voxel filtering of the current frame point cloud, r i Indicates the filtering scale in the i-th voxel filtering of the current frame point cloud.

[0110] If the number of points in the current frame point cloud is not greater than the first preset number of points, the final current frame point cloud is directly obtained.

[0111] The embodiment of the present invention matches each to-be-matched point in the acquired current frame point cloud in the global static map and the real-time local map, and can simultaneously maintain the global static map and the real-time local map, thereby improving the probability of successful matching of the to-be-matched point; by determining the matching fitting plane corresponding to the to-be-matched point, and determining the point matching result of the matching point according to the to-be-matched point, the matching fitting plane and the robust kernel function, the robust kernel function can be used for optimization to reduce the probability of matching errors, improve the accuracy of the point matching result, and thus improve the accuracy of the final positioning; according to the point matching results of all the to-be-matched points, the point cloud matching result of the current frame point cloud is determined, and the maintenance of the global static map and the real-time local map by the current frame point cloud can be achieved; finally, according to the point cloud matching result, the position and posture of the robot is determined, and accurate positioning of the robot can be achieved, thereby improving the accuracy of the robot positioning. Among them, the global static map is used as the first priority for matching. When the matching is unsuccessful, it means that the environment has changed. Then matching with the real-time local map can comprehensively and accurately match the target point cloud, reduce the problem of positioning loss caused by changes in the robot's working environment, and improve the accuracy and precision of the robot's positioning; and, by matching the global static map and the local real-time map, when the working area changes, the real-time local map can be relied on for matching, which reduces the time of building a new static map and improves the convenience of deploying the robot.

[0112] It should be understood that the order of execution of the steps in the above embodiment does not necessarily mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.

[0113] The following is an embodiment of the device of the present invention. For details not described in detail therein, reference may be made to the corresponding method embodiment described above.

[0114] Figure 6 The schematic diagram of the structure of the robot positioning device provided by the embodiment of the present invention is shown. For the convenience of explanation, only the part related to the embodiment of the present invention is shown, which is described in detail as follows:

[0115] like Figure 6 As shown, the robot positioning device 60 includes:

[0116] An acquisition module 61 is used to acquire a point cloud of a current frame collected by a laser radar;

[0117] The point matching module 62 is used to determine, for each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point according to the global static map and the real-time local map, and determine the point matching result of the to-be-matched point according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of the working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud;

[0118] The point cloud matching module 63 is used to determine the point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud;

[0119] The positioning module 64 is used to determine the position and posture of the robot according to the point cloud matching result.

[0120] In a possible implementation, the point matching module 62 is specifically configured to:

[0121] For each to-be-matched point in the point cloud of the current frame, determining a second preset number of first search points closest to the to-be-matched point in the global static map;

[0122] Determine a first average distance between the point to be matched and the first search point; wherein the first average distance is an average value of the distances between the point to be matched and each first search point;

[0123] Determining whether the first average distance is greater than a first preset threshold;

[0124] If the first average distance is greater than a first preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

[0125] In a possible implementation, the point matching module 62 is further configured to:

[0126] If the first average distance is less than or equal to a first preset threshold, establishing a first fitting plane according to all first search points;

[0127] determining a first distance from each first search point to the first fitting plane;

[0128] Determine whether the first distances corresponding to all the first search points are less than or equal to a second preset threshold;

[0129] If the first distances corresponding to all the first search points are less than or equal to the second preset threshold, the first fitting plane is determined as the matching fitting plane corresponding to the point to be matched;

[0130] If there is at least one first search point whose corresponding first distance is greater than the second preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

[0131] In a possible implementation, the point matching module 62 is specifically configured to:

[0132] In the real-time local map, determine a third preset number of second search points closest to the point to be matched; determine a second average distance between the point to be matched and the second search points; wherein the second average distance is the average of the distances between the point to be matched and each of the second search points;

[0133] Determining whether the second average distance is greater than a first preset threshold;

[0134] If the second average distance is less than or equal to the first preset threshold, establishing a second fitting plane according to all the second search points;

[0135] determining a second distance from each second search point to the second fitting plane;

[0136] Determine whether the second distance corresponding to each second search point is greater than a second preset threshold;

[0137] If the second distances corresponding to all the second search points are less than or equal to the second preset threshold, the second fitting plane is determined as the matching fitting plane corresponding to the point to be matched.

[0138] In a possible implementation, the point matching module 62 is further configured to:

[0139] If the second average distance is greater than the first preset threshold, or there is a second search point whose corresponding second distance is greater than the second preset threshold, the point to be matched is determined as an interference point and is removed from the point cloud of the current frame.

[0140] In a possible implementation, the point matching module 62 is specifically configured to:

[0141] Determine a third distance between the to-be-matched point and the matching fitting plane, and a normal vector of the matching fitting plane;

[0142] Determine a weight coefficient according to the third distance and the robust kernel function;

[0143] According to the weight coefficient, the third distance and the normal vector are optimized to obtain the target point corresponding to the point to be matched;

[0144] Determine a noise item corresponding to the point to be matched according to the weight coefficient and a fourth distance between the point to be matched and the laser radar;

[0145] Based on the target point and the noise item, a point matching result of the point to be matched is determined.

[0146] In a possible implementation, the point matching module 62 is specifically configured to:

[0147] According to the expression: Determine the weight coefficient;

[0148] In the formula, s represents the weight coefficient, represents the robust kernel function, r represents the third distance, and pd represents the preset control coefficient.

[0149] Figure 7 Schematic diagram of a robot provided by an embodiment of the present invention. Figure 7 As shown, the robot 70 of this embodiment includes: a processor 71, a memory 72, and a computer program 73 stored in the memory 72 and executable on the processor 71. When the processor 71 executes the computer program 73, the steps in the above-mentioned robot positioning method embodiments are implemented, for example Figure 2 Alternatively, when the processor 71 executes the computer program 73, the functions of each module in the above-mentioned device embodiments are realized, for example Figure 6 The functions of modules 61 to 64 are shown.

[0150] Exemplarily, the computer program 73 may be divided into one or more modules / units, one or more modules / units are stored in the memory 72 and executed by the processor 71 to implement the present invention. One or more modules / units may be a series of computer program instruction segments capable of implementing specific functions, and the instruction segments are used to describe the execution process of the computer program 73 in the robot 70. For example, the computer program 73 may be divided into Figure 6 Modules 61 to 64 are shown.

[0151] The robot 70 may include, but is not limited to, a processor 71 and a memory 72. Those skilled in the art will appreciate that Figure 7 This is only an example of the robot 70 and does not constitute a limitation of the robot 70. The robot 70 may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the robot may also include input and output devices, network access devices, buses, etc.

[0152] The processor 71 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (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 any conventional processor, etc.

[0153] The memory 72 may be an internal storage unit of the robot 70, such as a hard disk or memory of the robot 70. The memory 72 may also be an external storage device of the robot 70, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the robot 70. Further, the memory 72 may also include both an internal storage unit of the robot 70 and an external storage device. The memory 72 is used to store computer programs and other programs and data required by the robot. The memory 72 may also be used to temporarily store data that has been output or is to be output.

[0154] The technicians in the relevant field 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 for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or 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 each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0155] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0156] Those skilled in the art will appreciate 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. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0157] In the embodiments provided by the present invention, it should be understood that the disclosed devices / robots and methods can be implemented in other ways. For example, the device / robot embodiments described above are only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0158] 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 on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

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

[0160] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, 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 program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. Computer-readable media can include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal and software distribution medium, etc.

[0161] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention.

Claims

1. A robot positioning method, characterized in that: include: Get the current frame point cloud collected by the LiDAR; For each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point is determined according to the global static map and the real-time local map, and a point matching result of the to-be-matched point is determined according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of the working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud; Determine a point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud; The position and posture of the robot are determined according to the point cloud matching result.

2. The robot positioning method according to claim 1, characterized in that: For each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point is determined according to the global static map and the real-time local map, including: For each to-be-matched point in the current frame point cloud, determining a second preset number of first search points closest to the to-be-matched point in the global static map; Determine a first average distance between the point to be matched and the first search point; wherein the first average distance is an average value of the distances between the point to be matched and each first search point; Determining whether the first average distance is greater than a first preset threshold; If the first average distance is greater than the first preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

3. The robot positioning method according to claim 2, characterized in that: After determining whether the first average distance is greater than a first preset threshold, the method further includes: If the first average distance is less than or equal to the first preset threshold, establishing a first fitting plane according to all first search points; determining a first distance from each first search point to the first fitting plane; Determine whether the first distances corresponding to all the first search points are less than or equal to a second preset threshold; If the first distances corresponding to all the first search points are less than or equal to the second preset threshold, determining the first fitting plane as the matching fitting plane corresponding to the point to be matched; If there is at least one first search point whose corresponding first distance is greater than the second preset threshold, a matching fitting plane corresponding to the point to be matched is determined according to the point to be matched and the real-time local map.

4. The robot positioning method according to claim 2 or 3, characterized in that: Determining a matching fitting plane corresponding to the point to be matched according to the point to be matched and the real-time local map includes: In the real-time local map, determining a third preset number of second search points closest to the point to be matched; Determine a second average distance between the point to be matched and the second search point; wherein the second average distance is an average value of the distances between the point to be matched and each second search point; Determining whether the second average distance is greater than the first preset threshold; If the second average distance is less than or equal to the first preset threshold, establishing a second fitting plane according to all second search points; determining a second distance from each second search point to the second fitting plane; Determine whether the second distance corresponding to each second search point is greater than a second preset threshold; If the second distances corresponding to all the second search points are less than or equal to the second preset threshold, the second fitting plane is determined as the matching fitting plane corresponding to the point to be matched.

5. The robot positioning method according to claim 4, characterized in that: The method further comprises: If the second average distance is greater than the first preset threshold, or there is a second search point whose corresponding second distance is greater than the second preset threshold, the point to be matched is determined as an interference point and is removed from the point cloud of the current frame.

6. The robot positioning method according to any one of claims 1 to 3, characterized in that: Determining the point matching result of the point to be matched according to the point to be matched, the matching fitting plane and a preset robust kernel function includes: Determine a third distance between the to-be-matched point and the matching fitting plane, and a normal vector of the matching fitting plane; Determining a weight coefficient according to the third distance and the robust kernel function; According to the weight coefficient, the third distance and the normal vector are optimized to obtain a target point corresponding to the point to be matched; Determine a noise item corresponding to the point to be matched according to the weight coefficient and a fourth distance between the point to be matched and the laser radar; Based on the target point and the noise item, a point matching result of the point to be matched is determined.

7. The robot positioning method according to claim 6, characterized in that: Determining a weight coefficient according to the third distance and the robust kernel function includes: According to the expression: Determine the weight coefficient; Wherein, s represents the weight coefficient, represents the robust kernel function, r represents the third distance, and pd represents the preset control coefficient.

8. A robot positioning device, characterized in that: include: The acquisition module is used to obtain the current frame point cloud collected by the laser radar; A point matching module is used to determine, for each to-be-matched point in the current frame point cloud, a matching fitting plane corresponding to the to-be-matched point according to a global static map and a real-time local map, and to determine a point matching result of the to-be-matched point according to the to-be-matched point, the matching fitting plane and a preset robust kernel function; wherein the global static map is a static map of a working area corresponding to the robot, and the real-time local map is determined according to a first preset number of historical frame point clouds before the current frame point cloud; A point cloud matching module, used to determine the point cloud matching result of the current frame point cloud based on the point matching result of each to-be-matched point in the current frame point cloud; A positioning module is used to determine the position and posture of the robot according to the point cloud matching result.

9. A robot comprising a memory and a processor, wherein the memory is used to store a computer program, and the processor is used to call and run the computer program stored in the memory, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images