Object point cloud-based grasp pose generation method and system

Abstract

The application belongs to the field of robot grasping, and particularly discloses a grasping pose generation method and system based on object point cloud i , i = 1, 2, …, N; for each rotation matrix R i : rotate the object coordinate system O0 according to the rotation matrix R i , to obtain a coordinate system O i , to obtain the coordinates of the point cloud under the coordinate system O i ; perform voxelization on the point cloud under the new coordinate system, to generate a voxel tensor; in the voxel tensor, slice layer by layer along a preset coordinate axis; and traverse along the axis, to find a clamping point group meeting a constraint condition on each layer of profile slice, and all the clamping point groups obtained form a set G i ; convert the coordinates of the clamping point groups in the set G i into coordinates in the coordinate system O0, and add them to a set G0; traverse all R i , and the set G0 is a candidate grasping pose; and further determine the grasping pose. The application can balance the accuracy and real-time performance of grasping pose generation.

Description

Technical Field

[0001] This invention belongs to the field of robot grasping, and more specifically, relates to a grasping pose generation method and system based on object point clouds. Background Technology

[0002] Industrial robots are important automated equipment in modern manufacturing. They are mainly responsible for material handling on the production line. Currently, most of the material handling industrial robots working on the production line are operated through teaching or pre-programming, and the gripping position needs to be set manually.

[0003] Currently, when determining the grasping pose based on object point clouds, one approach is to calculate the cumulative height features of the point cloud, transforming the complex point cloud into a voxelized mesh, and then training a classifier based on manual annotation. This method relies on manual annotation, and the network training results are uncertain. Another approach is to directly perform random sampling on the point cloud and then perform optimization search. The problem with this approach is that it does not consider the mechanical properties of grasping, and the quality of the results cannot be guaranteed. Another approach is based on the force closure theory, which takes into account mechanical properties, but the search time is usually long. Summary of the Invention

[0004] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a grasping pose generation method and system based on object point cloud, the purpose of which is to improve the accuracy and real-time performance of grasping pose generation.

[0005] To achieve the above objectives, according to a first aspect of the present invention, a method for generating grasping poses based on object point clouds is proposed, comprising the following steps:

[0006] Obtain the 3D point cloud of the object and generate N rotation matrices R. i , i = 1, 2, ..., N;

[0007] For each rotation matrix R i Perform the following steps:

[0008] (1) The three-dimensional point cloud of the object has an initial object coordinate system O0. The object coordinate system O0 is rotated according to the rotation matrix R. i Rotate to obtain a new coordinate system O. i The point cloud is obtained in the new coordinate system O. i The coordinates below;

[0009] (2) Perform voxelization on the point cloud in the new coordinate system to generate voxel tensors;

[0010] (3) In the voxel tensor, slice layer by layer along the preset coordinate axis; and traverse along the preset coordinate axis to find the clamping point group that meets the constraint conditions on each contour slice. The set G is composed of all the clamping point groups obtained.i ;

[0011] (4) Set G i The coordinates of the clamping point group are transformed into coordinates in the initial object coordinate system O0 and added to the set G0;

[0012] Traverse all rotation matrices R i Then, all elements in set G0 are the candidate grasp poses;

[0013] Based on the object's pose, a grasping pose is determined from the candidate grasping poses for grasping.

[0014] As a further preferred method, finding a set of clamping points that meet the constraints on each contour slice includes the following steps:

[0015] Multiple clamping point groups were initially determined on the contour slice using Hough transform;

[0016] Considering the stability of the contact between each clamping point group and the clamper, remove clamping point groups that do not meet the stability conditions;

[0017] For each set of gripping points, calculate the maximum reachable depth along the preset coordinate axis, modify the gripping point position according to the maximum reachable depth, and then add it to the set G. i middle.

[0018] As a further preferred method, multiple sets of clamping points are initially determined on the contour slice using Hough transform, including the following steps:

[0019] Based on the shape of the gripper, the Hough pattern of each pixel on the contour slice in the Hough space is determined, and then discretized into a grid in the Hough space at a certain resolution and superimposed; then the grid points that satisfy the superposition value are selected, that is, the intersection of the Hough patterns; each set of Hough patterns at each intersection point corresponds to a set of pixels on the contour slice, which is a gripping point group; thus, multiple gripping point groups are obtained.

[0020] As a further preferred option, the clamping point groups that do not meet the stability conditions are removed, specifically:

[0021] The stability condition is as follows: the pressure angle is determined based on the surface normal and the gripper normal at the clamping point, and the pressure angle must be less than a preset pressure angle threshold.

[0022] If any one of the clamping points in a set of clamping points does not satisfy the stability condition, then in set G... i Remove the clamping point group from the middle.

[0023] As a further preferred embodiment, the rotation matrix R i The method for determining it is as follows:

[0024] Take N points S uniformly distributed on a sphere of radius 1. i Rotate point [0,0,1] around the center of the sphere [0,0,0] to point S. i The rotation matrix is ​​R. i .

[0025] As a further preferred option, based on S i The rotation matrix R is calculated using the Rodriguez formula. i .

[0026] As a further preferred method, determining a grasping pose from the candidate grasping poses based on the object's pose includes the following steps:

[0027] Based on the robot's range of motion and interference from non-target objects, grasping postures are excluded from the candidate grasping posture set.

[0028] Based on the object's posture, the remaining grasping postures are evaluated: the coordinates of the center point of the gripping contact point are determined according to the object's posture and the grasping posture, and then the torque of the object's gravity about the center point is calculated; the torques corresponding to each grasping posture are compared, and the grasping posture with the smallest torque is selected as the final robot grasping posture.

[0029] According to a second aspect of the present invention, a grasping pose generation system based on object point clouds is provided, including a processor, the processor being used to execute the above-described grasping pose generation method based on object point clouds.

[0030] According to a third aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the above-described object point cloud-based grasping pose generation method.

[0031] In summary, compared with the prior art, the above-described technical solutions conceived by this invention mainly possess the following technical advantages:

[0032] 1. This invention is based on an object point cloud model. It pre-finds stable grasping postures of the object in various directions as candidate postures, and then quickly determines the final grasping posture from them, which can take into account both the accuracy and real-time performance of grasping posture generation.

[0033] 2. This invention proposes a method for processing uniformly rotating point cloud models, which decomposes the grasping problem in three-dimensional space into multiple planar grasping problems to facilitate calculation.

[0034] 3. This invention uses voxelized point cloud to solve for the gripping coordinates. Voxelization can effectively reduce the number of point cloud points and enable the point cloud to be aligned along the coordinate axes, reducing the overhead of traversing the entire set. At the same time, it can also make the gripping points naturally satisfy the stability condition of no lateral torque.

[0035] 4. This invention designs a method for searching layer by layer along a voxelized point cloud, and uses the Hough transform method for finding gripping point groups based on the shape of the gripper, which improves the efficiency of computation; and improves the reliability of gripping by using stability conditions and exploring the maximum depth. Attached Figure Description

[0036] Figure 1 In Figures (a) and (b), the coordinate system O0 and O2 are defined in the embodiments of the present invention. i Schematic diagram of a 3D point cloud model in a coordinate system;

[0037] Figure 2 This is a flowchart of the object point cloud-based grasping pose generation method according to an embodiment of the present invention;

[0038] Figure 3 Images (a)-(c) are visualization diagrams of rotational subdivision when N = 50, 500, and 5000 in the embodiments of the present invention.

[0039] Figure 4 This is a schematic diagram of a voxel grid according to an embodiment of the present invention;

[0040] Figure 5 Figures (a)-(d) are schematic diagrams of the layer-by-layer search process in an embodiment of the present invention;

[0041] Figure 6 (a)-(d) are schematic diagrams of the superposition, subtraction and results of the j-th layer and the 1st, 2nd, ..., j-1th layers when slicing layer by layer in the embodiments of the present invention;

[0042] Figure 7 This is a schematic diagram illustrating the definition of equilateral triangle parameters in an embodiment of the present invention;

[0043] Figure 8 This is a schematic diagram of a triangle with numerous fixed vertices in an embodiment of the present invention;

[0044] Figure 9 (a) and (b) are schematic diagrams of triangles that satisfy the voting conditions in an embodiment of the present invention;

[0045] Figure 10 Figures (a) and (b) are schematic diagrams illustrating the frictional stability conditions in an embodiment of the present invention.

[0046] Figure 11 This is a schematic diagram of the maximum clamping depth in an embodiment of the present invention. Detailed Implementation

[0047] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0048] This invention provides a method for generating grasping poses based on object point clouds. Using the object's 3D point cloud as input, and comprehensively considering the gripper shape and the stability of the gripping points, it generates multiple grasping coordinates for grasping in different directions. In this embodiment, a three-finger translational gripper is used as an example for illustration, making it easy to understand. The overall method is also applicable to other types of grippers.

[0049] Specifically, methods for generating grasping poses based on object point clouds, such as... Figure 2 As shown, it includes the following steps:

[0050] S1. Obtain the 3D point cloud of the object and generate a rotation matrix.

[0051] Obtain a 3D point cloud model of the object's surface (e.g., .stl, .ply files), containing the coordinates and normals of each point within the point cloud. The point cloud model is as follows: Figure 1 As shown, the red dots represent each point that makes up the point cloud model, and the purple lines represent the normal vectors of each point. The point cloud model has an initial object coordinate system, denoted as O0.

[0052] Specify a number of subdivision angles N, and generate N rotation matrices R. i The coordinate system is set with i = 1, 2, ..., N. The purpose of rotating the coordinate system is to examine the possibilities of gripping an object from different directions. The rotation matrix R is controlled by setting the subdivision number N. i The more numerous the items, the more comprehensively the possibilities of clamping are considered.

[0053] Rotation matrix R i The calculation method can be understood as distributing N points S roughly uniformly (in reality, it is impossible to achieve absolute uniformity; the only case of absolute uniformity is the vertices of the 5 convex regular polyhedra) on a sphere with a radius of 1. i (i = 1, 2, ..., N), each rotation matrix R i It means rotating point [0, 0, 1] around [0, 0, 0] to point S. i ,like Figure 3 As shown.

[0054] S i Generated by the following formula:

[0055]

[0056] Rotation matrix R i Calculated using the Rodriguez formula:

[0057] n i =S i ×[0,0,1]

[0058] α i =arccos(S i ·[0,0,1])

[0059]

[0060]

[0061] Where, n i,0 ,n i,1 ,n i,2 is n i The 1st, 2nd, and 3rd elements.

[0062] S2, Traverse all rotation matrices R i The candidate capture pose set G0 is obtained.

[0063] For each rotation matrix R i Perform the following steps:

[0064] S21, Coordinate system rotation

[0065] Rotate the object coordinate system O0 according to the rotation matrix R. i Rotate to obtain a new coordinate system O. i In this process, what is rotated is the coordinate system, resulting in the coordinate system O. i The point cloud coordinates and normal vectors of the internal object, such as Figure 1 As shown; and let the Z-axis of the clamp be aligned with coordinate system O. i The Z-axis is parallel (make the Z-axis of the clamp parallel to the coordinate system O). i (The X and Y axes can also be parallel), let O be... i The X, Y, and Z axes are X i ,Y i Z i .

[0066] S22, Voxelization

[0067] Let the point cloud after rotation be located at O. i The dimensions of the lower axis-aligned bounding box are Fill the rotated object point cloud to the specified voxel size v. s From the 3D mesh, three 3D meshes with shapes [W, H, D] are obtained: the object surface voxel mesh. Object entity voxel mesh Object corrosion voxel mesh Where W, H, and D are all integers, corresponding to X respectively. i ,Y i Z i Axis; within a voxel grid, the values ​​are only 0 and 1. 0 indicates that the position is not an object, and 1 indicates that the position is an object. Positions with a value of 1 are called grid points. They are mutually exclusive. The three elements satisfy the relationship:

[0068]

[0069] It can also be used as Figure 4 The relationship between the three can be understood by showing a two-dimensional slice. The left, middle, and right sides of the image represent...

[0070] voxel size v s The larger the value, the more V is generated. i The fewer the number of grid cells, the coarser the calculation results; conversely, the more numerous the cells, the finer the results, but the computation time and memory usage are greater. Therefore, the number of cells should be selected appropriately based on the size of the point cloud model. In this embodiment, v is selected. s The formula for floor is floor, which rounds down:

[0071]

[0072] In obtaining Next, we need to calculate the normal vector, which involves finding the distance. For each grid point, the normal vector of the nearest point cloud point is taken as the normal vector of the grid point.

[0073] S23, Searching layer by layer

[0074] In the voxel tensor, the set of gripping points is obtained by searching layer by layer, such as Figure 5 As shown, it includes the following steps:

[0075] S231, Layer-by-layer slicing

[0076] In voxel tensor In the process, slice layer by layer along the Z-axis (i.e., slice layer by layer along the axis parallel to the gripper's Z-axis; if the gripper's Z-axis is defined to be parallel to the X-axis, then slice layer by layer along the X-axis; for ease of explanation, we will consider slicing along the Z-axis from now on) until the bottom of the model is reached. The gripper's Z-axis is the axis of symmetry of the gripper's fingers. When the gripper is attached to the robot, the gripper's Z-axis and the robot's end effector's Z-axis are in the same direction.

[0077] During the slicing process, it is also necessary to consider whether all grid points in the j-th layer are reachable, i.e., whether they are occluded by grid points in the upper layer. The slicing calculation formula is as follows:

[0078]

[0079] Where [:,:,j] means selecting all elements with Z-axis index j; [:,:,0:j-1] means selecting all elements with Z-axis index j. i All elements with axis indices 1, 2, ..., j-1; The meaning is to verify Z. i The summation of axes represents the superposition of all layers above the j-th layer. The result is a binarized image, such as... Figure 6 As shown.

[0080] S232. Hough Transform for Finding Clamping Point Sets

[0081] To find contact points that conform to the closing shape on the contour of each layer, a naive method is to search for each point one by one, checking if any combination of three points forms an equilateral triangle. This is obviously too time-consuming. This invention uses the Hough transform to solve this problem. The Hough transform is a feature extraction method; the classic Hough transform is used to detect lines and circles. In this embodiment, considering a three-finger gripper with three fingers evenly distributed at 120° intervals, a Hough transform method for detecting equilateral triangles is designed.

[0082] The position of the equilateral triangle on the plane is described using four parameters: [x c ,y c [r, θ] represent the x-coordinate of the center of the triangle, the y-coordinate of the center, the radius of the circumcircle, and the phase, respectively. Figure 7 As shown. Due to the central rotational symmetry of an equilateral triangle,

[0083] When one vertex of the fixed equilateral triangle is mark = (x m ,y m When ), an equilateral triangle is represented as Satisfy the following equation (1):

[0084]

[0085] This is equivalent to adding two constraints to an equilateral triangle, now tri(x) c ,y c |x m ,y m Even with only 2 degrees of freedom, there are still infinitely many such triangles, such as... Figure 8 As shown.

[0086] tri(x c ,y c |x m ,ym )exist In space (called Hough space), a hyperplane is formed, called a Hough figure. Each pixel on the contour, that is, V... i,j A grid point. Calculate the Hough pattern for each pixel on the contour (each pixel corresponds to one Hough pattern), and discretize it into a grid in Hough space at a certain resolution, then superimpose them; then select the Hough space grid points that satisfy the superposition value (determined by the number of contact points between the gripper and the object), that is, the intersection points of the Hough patterns that meet the conditions; each such intersection point corresponds to a set of Hough patterns, which are pixels on a set of contour slices. In this embodiment, the position with a superposition value of 3 is an equilateral triangle that meets the conditions, such as... Figure 9 As shown.

[0087] Get V i,j There are several equilateral triangles. The grid point where the vertex of each equilateral triangle is located is called a gripping point group, and each gripping point group corresponds to a gripping posture.

[0088] The above process is the method for finding the gripping point using a rotationally symmetric three-finger gripper used in this embodiment. This method can also be extended to other grippers. If a gripper of other shapes is used, the shape formed by the gripping point is determined to be a parametric equation in the form of equation (1) after a gripping point is fixed. Other processing ideas are the same.

[0089] S233, Stability Condition Determination

[0090] After initially obtaining the gripping point set through the Hough transform, the stability of the contact between the obtained gripping point set and the gripper needs to be considered. Many factors affect the stability of a gripping posture; two conditions are considered:

[0091] (i) Clamping force balance condition: The force applied to the surface of the object by the clamping point should be balanced, especially to prevent lateral torque from causing the object to roll. Since the clamping point is located in roughly the same plane, this condition is naturally satisfied.

[0092] (ii) Two key conditions for frictional stability are: to explore these conditions, it is necessary to know the normal vector of the object's surface and the normal vector of the gripper's fingers at the contact point. The angle between these two constitutes the pressure angle; the smaller the pressure angle, the less likely slippage will occur. This condition is decomposed into horizontal and vertical directions, and an appropriate pressure angle threshold α is selected. h ,α v (This is related to the material of the contact surface and is artificially specified) to determine whether it is stable, such as Figure 10As shown, this is because vertical friction is primarily relied upon to resist gravity, and calculating it separately is more consistent with reality. If any of the three contact points does not meet condition (ii), the results for that set will be discarded.

[0093] S234, Maximum clamping depth

[0094] To improve the reliability of the results, it is also necessary to explore the maximum achievable gripping depth for each clamping point group that meets condition (ii), and modify the position of the clamping points in the clamping point group according to this maximum gripping depth, such as... Figure 11 As shown, the clamping point group is then added to set G. i This is done to increase the potential contact area between the gripper and the object, ensuring that the object will not fall due to unforeseen disturbances during the gripping process. This is a further optimization of the gripping coordinates.

[0095] S24. Return to the initial coordinate system.

[0096] Set G i The coordinates of the clamping point group are transformed into coordinates within the initial object coordinate system O0 and added to the set G0.

[0097] S3. Based on the object's pose, determine the grasping pose from the candidate grasping pose set G0 and perform the grasping.

[0098] In S2, candidate grasping poses have been determined, and the robot's grasping posture can be determined by combining them with the object pose. However, in actual multi-object grasping scenarios, two factors need to be considered: the accessibility of the grasping position and the influence of the gripper on non-target objects. First, due to the robot's limited range of motion, most grasping poses are unreachable and are excluded. The remaining positions need to be re-verified to see if they interfere with the surrounding point cloud (determined based on other non-target objects in the scene); poses that interfere are also excluded. Finally, the grasping pose with the highest grasping score among the remaining poses is selected as the scene's grasping pose.

[0099] Specifically, the method for determining the crawling score is as follows:

[0100] Based on the 3D point cloud model of the object, the position of the object's center of gravity is pre-calculated; for each remaining grasping posture: the coordinates of the center point of the gripping contact point are determined and calculated according to the object's posture in the world coordinate system and the grasping posture; then, based on the position of the object's center of gravity, the torque of the object's gravity about the center point is calculated.

[0101] By comparing the torques corresponding to each grasping posture, the grasping posture with the smallest torque is selected as the final robot grasping posture.

[0102] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for generating grasping pose based on object point clouds, characterized in that, Includes the following steps: Obtain the 3D point cloud of the object and generate N rotation matrices R. i , i = 1, 2, ..., N; For each rotation matrix R i Perform the following steps: (1) The three-dimensional point cloud of the object has an initial object coordinate system O0. The object coordinate system O0 is rotated according to the rotation matrix R. i Rotate to obtain a new coordinate system O. i The point cloud is obtained in the new coordinate system O. i The coordinates below; (2) Perform voxelization on the point cloud in the new coordinate system to generate voxel tensors; (3) In the voxel tensor, slice layer by layer along the preset coordinate axes; Then, it traverses along the preset coordinate axis, searching for clamping point groups that meet the constraints on each contour slice, and the resulting set G comprises all clamping point groups. i ; (4) Set G i The coordinates of the clamping point group are transformed into coordinates in the initial object coordinate system O0 and added to the set G0; Traverse all rotation matrices R i Then, all elements in set G0 are the candidate grasp poses; Based on the object's pose, a grasping pose is determined from the candidate grasping poses for grasping.

2. The grasping pose generation method based on object point clouds as described in claim 1, characterized in that, Find the set of clamping points that meet the constraints on each contour slice, including the following steps: Multiple clamping point groups were initially determined on the contour slice using Hough transform; Considering the stability of the contact between each clamping point group and the clamper, remove clamping point groups that do not meet the stability conditions; For each set of gripping points, calculate the maximum reachable depth along the preset coordinate axis, modify the gripping point position according to the maximum reachable depth, and then add it to the set G. i middle.

3. The grasping pose generation method based on object point clouds as described in claim 2, characterized in that, Multiple clamping point groups are initially determined on the contour slice using Hough transform, including the following steps: Based on the shape of the gripper, the Hough pattern of each pixel on the contour slice in the Hough space is determined, and then discretized into a grid in the Hough space at a certain resolution and superimposed; then the grid points that satisfy the superposition value are selected, that is, the intersection of the Hough patterns; each set of Hough patterns at each intersection point corresponds to a set of pixels on the contour slice, which is a gripping point group; thus, multiple gripping point groups are obtained.

4. The grasping pose generation method based on object point clouds as described in claim 2, characterized in that, Remove the clamping point groups that do not meet the stability conditions, specifically: The stability condition is as follows: the pressure angle is determined based on the surface normal and the gripper normal at the clamping point, and the pressure angle must be less than a preset pressure angle threshold. If any one of the clamping points in a set of clamping points does not satisfy the stability condition, then in set G... i Remove the clamping point group from the middle.

5. The grasping pose generation method based on object point clouds as described in claim 1, characterized in that, The rotation matrix R i The method for determining it is as follows: Take N points S uniformly distributed on a sphere of radius 1. i Rotate point [0,0,1] around the center of the sphere [0,0,0] to point S. i The rotation matrix is ​​R. i .

6. The grasping pose generation method based on object point clouds as described in claim 1, characterized in that, Based on S i The rotation matrix R is calculated using the Rodriguez formula. i .

7. The grasping pose generation method based on object point clouds as described in any one of claims 1-6, characterized in that, Based on the object's pose, a grasping pose is determined from the candidate grasping poses for grasping, including the following steps: Based on the robot's range of motion and interference from non-target objects, grasping postures are excluded from the candidate grasping posture set. Based on the object's posture, the remaining grasping postures are evaluated: the coordinates of the center point of the gripping contact point are determined according to the object's posture and the grasping posture, and then the torque of the object's gravity about the center point is calculated; the torques corresponding to each grasping posture are compared, and the grasping posture with the smallest torque is selected as the final robot grasping posture.

8. A grasping pose generation system based on object point clouds, characterized in that, Includes a processor for executing the object point cloud-based grasping pose generation method as described in any one of claims 1-7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the object point cloud-based grasping pose generation method as described in any one of claims 1-7.

Images