Collision detection method and device for point cloud disorderly grabbed by robot

Abstract

The invention discloses a point cloud collision detection method and device for disordered grabbing of a robot, and the method comprises the steps: carrying out the random sampling of an imported clamping jaw model through a patch, obtaining point cloud data, and carrying out the scanning through a 3D camera, and obtaining the point cloud data of stacked workpieces; calculating a bounding box, performing spatial adaptive segmentation on the point cloud data, and constructing a clamping jaw model and a hierarchical bounding box tree of the stacked workpieces; performing collision detection on the two hierarchical bounding box trees by using depth-first recursive search to obtain a collision result under a single discrete pose; and interpolation is carried out on the motion trail of the robot, the hierarchical bounding box tree of the clamping jaw model is dynamically updated according to all poses generated through interpolation, and the collision condition of the whole motion trail is detected. According to the invention, spatial adaptive segmentation is carried out on the point cloud data to adapt to the actual distribution of the point cloud, so that the space utilization rate is improved and the balance of the tree structure is improved; and interpolation is carried out on the motion trail of the robot, the hierarchical bounding box tree is dynamically updated, frequent reconstruction of the whole tree structure is avoided, and efficient path continuous collision detection is realized.

Description

Technical Field

[0001] The present invention relates to the field of robot perception, and relates to an unordered grasping scenario, in particular to a point cloud collision detection method and device for unordered grasping of a robot. Background Technique

[0002] With the continuous evolution of intelligent operations of robots towards complex scenario applications, autonomous perception, human-machine interaction, etc., higher requirements are put forward for the perception and operation capabilities of robots. By integrating a 3D vision camera into the robot to endow it with the "visual perception" ability, its environmental adaptability and operation accuracy can be significantly improved, making "robot + vision" play an increasingly important role in digital workshops and unmanned factories.

[0003] As a typical complex operation scenario, unordered grasping of a robot requires the robot to obtain the spatial information of workpieces through 3D vision scanning in an unknown and scattered stacked workpiece environment, calculate feasible grasping poses, and autonomously complete the grasping operation. During the actual execution process, the end effector of the robot (such as a gripper) is extremely likely to collide with surrounding workpieces during the approaching or moving process of grasping, resulting in grasping failure. Therefore, efficient and accurate collision detection technology has become an indispensable key link in the application of unordered grasping of robots.

[0004] There are many existing collision detection methods. Especially for the point cloud data obtained by 3D vision scanning, a detection strategy based on a bounding volume is widely used. This method simplifies the representation of the detection object by constructing a specific type of bounding volume (such as a spherical bounding box Sphere, an axis-aligned bounding box Axis-Aligned Bounding Box, AABB, an oriented bounding box Oriented Bounding Box, OBB), and judges whether a collision occurs through the intersection test between the bounding boxes. The method based on the bounding box has the characteristics of simple calculation and fast speed, and is especially suitable for the case of point cloud data with loose structure and large quantity.

[0005] To further improve the detection efficiency, various bounding box organizational structures based on spatial hierarchical partitioning have been developed in the prior art, such as the Bounding Volume Hierarchy (BVH). Among them, there are methods that gradually subdivide the point cloud space from top to bottom based on a binary tree to generate hierarchical bounding boxes for accelerating detection. However, in complex three-dimensional scenes, simple binary tree partitioning has the problem of low tree construction efficiency. There are also methods based on Octree technology that recursively subdivide the three-dimensional space into eight sub-regions and construct bounding boxes within each region to improve the partition management efficiency. However, the Octree structure with uniform rule partitioning is prone to hierarchical imbalance or partitioning redundancy when the point cloud is sparse or the workpiece shape is complex, affecting the detection performance.

[0006] In the prior art, for example, Chinese Patent CN112179602B discloses a method for robotic arm collision detection. This method respectively performs surface rasterization processing on the virtual simulation scene model and the robotic arm model to obtain corresponding point cloud data, uses an Octree to partition the point cloud space, and constructs a multi-level bounding box tree structure through a hybrid encapsulation method of sphere bounding boxes and oriented bounding boxes. In the collision detection stage, based on a search strategy combining depth-first and breadth-first, the collision situation of a single configuration of the robotic arm is detected, and combined with the robotic arm kinematic model, collision detection on a continuous motion path is realized based on the dynamic programming method. This technical solution has achieved good results in improving the detection efficiency and continuous detection ability, and is particularly suitable for the application requirements of static scenes and predictable motion paths.

[0007] However, the existing collision detection technologies, including the above method, still have some common problems. First, it is difficult to adapt to regions with large changes in point cloud density and shape in complex scenes based on fixed rule partitioning (such as Octree), resulting in redundant point cloud partitioning and increasing the computational burden. Second, in the face of continuous changes in the posture during the movement of the robot, traditional methods usually need to frequently update or reconstruct the bounding box tree structure, with insufficient real-time performance, large update overhead, and affecting the detection response speed. In addition, most existing technologies rely on pre-built environment models and have limited adaptability to dynamically changing or real-time perceived operating environments. In scenarios with disordered stacking, complex and diverse shapes, and frequent dynamic changes in the environment, it is difficult to guarantee the stability and accuracy of collision detection. In summary, the prior art still has problems of poor balance between accuracy and efficiency and insufficient adaptability in dealing with real-time collision detection of continuous robot motion in dynamic and complex scenes, and there is an urgent need to propose a more flexible and efficient detection scheme for improvement. Summary of the Invention

[0008] The present invention aims to overcome the problem that in the scenario of unordered grasping by a robot, the end effector of the robot, i.e., the gripper, collides with the surrounding workpieces at the workpiece grasping position or collides with other workpieces during the movement of the gripper, and provides a point cloud collision detection method and device for unordered grasping by a robot. The present invention proposes a method for adaptive segmentation of point cloud data in space, efficiently constructs a hierarchical bounding box tree, interpolates the movement trajectory of the robot, and dynamically updates the hierarchical bounding box tree of the gripper model to achieve continuous path collision detection.

[0009] To achieve the above object, the first aspect of the present invention relates to a point cloud collision detection method for unordered grasping by a robot, including the following steps:

[0010] Step 1: Obtain point cloud data by randomly sampling the imported gripper model through patches, and obtain the point cloud data of stacked workpieces by scanning with a 3D camera;

[0011] Step 2: Calculate the bounding box and perform adaptive segmentation of the point cloud data in space to construct a hierarchical bounding box tree of the gripper model and the stacked workpieces;

[0012] Step 3: Perform collision detection on the two hierarchical bounding box trees by using depth-first recursive search to obtain the collision result at a single discrete pose;

[0013] Step 4: Interpolate the movement trajectory of the robot, and dynamically update the hierarchical bounding box tree of the gripper model according to each pose generated by the interpolation to detect the collision situation of the overall movement trajectory.

[0014] Preferably, the obtaining of point cloud data by randomly sampling the patches in Step 1 includes:

[0015] The gripper model data consists of a series of triangular patches. For each patch, the number of sampling points is determined by area calculation, and point cloud data is generated by using the method of random uniform sampling based on the determined number. Assume that the three vertex coordinates of any triangular patch are A(x1, y1, z1), B(x2, y2, z2), and C(x3, y3, z3) respectively, and the lengths of the three sides are calculated in turn as:

[0016] (1)

[0017] Use Heron's formula to calculate the area S of the triangular patch:

[0018] (2)

[0019] (3)

[0020] Set the sampling point density per unit area as n, and the total number of sampling points N is calculated as: N = round(n×S). Using the barycentric coordinate system of a triangle, sampling points are generated inside the triangle by the method of random interpolation. The position of any point P inside the triangle can be expressed as:

[0021] (4)

[0022] where α, β, and γ are the proportionality coefficients in the barycentric coordinates, describing the position relationship of point P inside the triangle relative to the three vertices, satisfying α + β + γ = 1 and α, β, γ ≥ 0.

[0023] The specific method for calculating α, β, and γ is: generate two random numbers r1 and r2, both of which are in the range of [0, 1], and calculate the weights through the following formula:

[0024] (5)

[0025] Point P is evenly distributed inside the triangle.

[0026] Preferably, the construction of the gripper model and the hierarchical bounding box tree for stacking workpieces described in step two includes:

[0027] Calculate the bounding box of all the input point cloud data, set it as the root node of the bounding box tree, and add it to the bounding box tree.

[0028] Starting from the root node, build the tree recursively downward; judge the shape of the point cloud through the bounding box of the current node, adaptively divide it into several subspaces according to the point cloud shape, then calculate the bounding boxes of the point cloud data in the subspaces, and add them to the bounding box tree;

[0029] When the node meets the condition for continued division, perform spatial division on the node until the node does not meet the division condition, and finally construct a hierarchical bounding box tree.

[0030] Preferably, the calculation of the bounding box described in step two includes:

[0031] The type of bounding box used for constructing the hierarchical bounding box tree is the axis-aligned bounding box AABB. Traverse all the point cloud data points to find the center C(x, y, z) of the bounding box, the three axial vectors u, v, w of the bounding box, and the corresponding half-axis lengths hu, hv, hw. The "axis-aligned" of the AABB bounding box means that each of its faces is parallel to the point cloud coordinate axes, u = (1, 0, 0), v = (0, 1, 0), w = (0, 0, 1).

[0032] Preferably, the spatial adaptive division of the point cloud data described in step two includes:

[0033] The point cloud data is predefined into three types according to its shape: strip-shaped point cloud, planar point cloud, and block-shaped point cloud. The semi-axis lengths in the axis-aligned bounding box (AABB) of the current point cloud are sorted from largest to smallest to obtain the first semi-axis length, the second semi-axis length, and the third semi-axis length, and the shape of the point cloud is judged according to the magnitudes of the three semi-axis lengths.

[0034] If the first semi-axis length is much longer than the second and third semi-axis lengths, it is regarded as a strip-shaped point cloud; if the first and second semi-axis lengths are similar and much larger than the third semi-axis length, it is regarded as a planar point cloud; if the first, second, and third semi-axis lengths are similar, it is regarded as a block-shaped point cloud.

[0035] The strip-shaped point cloud is divided into two pieces of point cloud data along the axis corresponding to the first semi-axis length; the planar point cloud is divided into four pieces of point cloud data along the axis corresponding to the first semi-axis length and the axis corresponding to the second semi-axis length; the block-shaped point cloud is divided into eight pieces of point cloud data along the axis corresponding to the first semi-axis length, the axis corresponding to the second semi-axis length, and the axis corresponding to the third semi-axis length.

[0036] Preferably, the collision detection between the two hierarchical bounding box trees described in step three includes:

[0037] Starting from the root nodes of the two trees, recursively search, select the bounding boxes of the two nodes and judge whether they overlap; if they do not overlap, end the judgment of the child nodes under the current node; if they overlap, further detection is performed;

[0038] For each pair of overlapping nodes, further detection is performed. If both nodes are non-leaf nodes: recursively detect the child nodes of these two nodes; if one node is a leaf node and the other is not: recursively detect the leaf node and the child nodes of the non-leaf node; if both nodes are leaf nodes: end the judgment of the child nodes under the current node;

[0039] When all relevant node pairs have been checked and there are no more child nodes to detect, end the collision detection process; if no overlapping nodes are found at the end, return a non-collision result, if a collision is found at the leaf nodes, return a collision result, and according to the early exit mechanism, as long as an overlap is found at two leaf nodes, directly end the detection process.

[0040] Furthermore, judging whether the bounding boxes overlap includes:

[0041] Use the separating axis theorem to judge whether two bounding boxes overlap; if there is an axis such that the projections of the two bounding boxes on this axis do not overlap, then they do not collide; otherwise, if the projections on all axes overlap, then they collide; the axes that need to be checked include: the three local coordinate axes of each bounding box, and the cross products of the local coordinate axes of the two bounding boxes, a total of 15 axes.

[0042] Preferably, the interpolation of the robot's motion trajectory described in step four includes:

[0043] During the process of the robot moving from the starting point A to the ending point B, a series of discrete poses of the gripper during the movement are generated through trajectory interpolation, and the pose includes position and orientation;

[0044] Among them, the position interpolation is carried out by linear interpolation. Specifically, let the three-dimensional coordinates of the starting point PA be (x1, y1, z1) and the three-dimensional coordinates of the ending point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the ending point PB:

[0045] (6)

[0046] Based on the set trajectory interpolation step size Δt and combined with the distance d between the two points, determine the required number of interpolation points n. Specifically: n = round(d / Δt). According to the calculated number of interpolation points n, along the connection direction between the starting point PA and the ending point PB, the intermediate position points are interpolated in a uniformly distributed manner. For each interpolation step size t (t ∈ [0,1], and the step size is 1 / n), the intermediate point position P(t) is calculated according to the following formula:

[0047] (7)

[0048] Among them, the orientation interpolation adopts the quaternion interpolation method. Specifically, let the starting orientation be the quaternion QA and the ending orientation be the quaternion QB, and calculate the angle θ between the quaternion QA and the quaternion QB:

[0049] (8)

[0050] And adopt the spherical linear interpolation algorithm to obtain the corresponding intermediate orientation Q(t) at each interpolation step size t:

[0051] (9)

[0052] Preferably, the dynamic update of the hierarchical bounding volume tree described in step four includes:

[0053] Based on the pose of the current robot movement, obtain the transformation matrix RT of the gripper model relative to the initial position; use the transformation matrix RT to update all axis-aligned bounding boxes AABB in the hierarchical bounding volume tree of the gripper model to oriented bounding boxes OBB. Specifically include:

[0054] Center point transformation. Taking the center point C of the AABB as a reference, apply the transformation matrix RT to obtain a new center point C′, where C′ = M·C, and M is the transformation matrix RT;

[0055] Axis - direction transformation: Rotate the standard coordinate axes (u, v, w) of the AABB in the local coordinate system through the rotation part R of the transformation matrix RT to obtain the new axis - directions of the OBB: u′ = R·u, v′ = R·v, w′ = R·w;

[0056] The half - axis lengths remain unchanged. During the update process from AABB to OBB, the half - axis lengths in each axis - direction remain unchanged.

[0057] The second aspect of the present invention relates to a point - cloud collision - detection device for disordered grasping of a robot, including a memory and one or more processors. Executable code is stored in the memory. When the one or more processors execute the executable code, it is used to implement the point - cloud collision - detection method for disordered grasping of the robot of the present invention.

[0058] The present invention makes the construction of the bounding - box tree dynamically adapt to the actual distribution of the point cloud by performing spatial adaptive segmentation on the point - cloud data, improving the space utilization rate and optimizing the balance of the tree structure, laying a foundation for efficient traversal. In the collision - detection stage, depth - first recursive search is used to quickly screen potential collision regions, improving the detection speed. For the continuous - motion path of the robot, discrete postures are generated through trajectory interpolation, and the bounding - box tree is dynamically updated during the motion process. Combining the fast construction of AABB and the rotational adaptability of OBB, frequent reconstruction is avoided, ensuring the real - time performance and efficiency of continuous - path collision detection. The above - mentioned mechanisms work together to greatly improve the detection efficiency under continuous motion while ensuring the detection accuracy.

[0059] The present invention proposes a method for spatially adaptively segmenting point - cloud data and constructing a hierarchical bounding - box tree, which dynamically adjusts the space - division strategy according to the shape characteristics of the point cloud to generate bounding boxes that better fit the actual distribution of the point - cloud surface and a bounding - box tree with a more balanced structure. This structure enables more effective determination of non - collision at the high - level node stage during the collision - detection process, thus achieving early pruning, reducing the number of deep - recursion times, lowering the overall traversal cost, and significantly improving the detection efficiency.

[0060] The advantages of the present invention are: performing spatial adaptive segmentation on the point - cloud data to better adapt to the actual distribution of the point cloud, improving the space utilization rate and enhancing the balance of the tree structure; interpolating the motion trajectory of the robot to dynamically update the hierarchical bounding - box tree, avoiding frequent reconstruction of the entire tree structure, and enabling more efficient continuous - path collision detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 is a schematic flowchart of a point - cloud collision - detection method for disordered grasping of a robot according to the present invention.

[0062] Figure 2Schematic diagram of the point cloud data obtained by randomly sampling the surface patches of the gripper model of the present invention.

[0063] Figure 3 Schematic diagram of the process of constructing a hierarchical bounding box tree of the present invention.

[0064] Figure 4a - Figure 4c Schematic diagram of the space adaptive segmentation of the point cloud data of the present invention, wherein Figure 4a Schematic diagram of dividing a strip-shaped point cloud into 2 pieces of point cloud; Figure 4b Schematic diagram of dividing a planar point cloud into 4 pieces of point cloud; Figure 4c Schematic diagram of dividing a block-shaped point cloud into 8 pieces of point cloud.

[0065] Figure 5 Schematic diagram of the process of collision detection of the hierarchical bounding box tree of the present invention.

[0066] Figure 6 Schematic diagram of the judgment of bounding box overlap of the present invention.

[0067] Figure 7 Schematic diagram of the process of path continuous collision detection of the present invention.

[0068] Figure 8 Schematic diagram of the device of the present invention. Detailed implementation manners

[0069] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0070] Embodiment 1

[0071] In the scenario of unordered grasping operation of a robot, when facing stacked workpieces, due to the mutual interlacing between the workpieces, the end effector of the robot, i.e., the gripper, collides with the surrounding workpieces at the workpiece grasping position, or collides with other workpieces during the movement of the gripper, resulting in grasping failure.

[0072] In order to overcome the above problems, the inventor provides the solution given in the following embodiments through research.

[0073] In this example, Figure 1 Schematic diagram of the process of a point cloud collision detection method for unordered grasping of a robot. The method includes the following steps:

[0074] Step 1: Obtain point cloud data by randomly sampling the surface patches of the imported gripper model, and use a 3D camera to scan to obtain the point cloud data of the stacked workpieces;

[0075] Step 2: Calculate the bounding box and perform space adaptive segmentation on the point cloud data to construct a hierarchical bounding box tree of the gripper model and the stacked workpieces;

[0076] Step 3: Use depth - first recursive search to perform collision detection on the two hierarchical bounding volume trees to obtain the collision results in a single discrete pose;

[0077] Step 4: Interpolate the robot motion trajectory, dynamically update the hierarchical bounding volume tree of the gripper model according to each pose generated by the interpolation, and detect the collision situation of the overall motion trajectory.

[0078] In this example, Figure 2 is a schematic diagram of the point cloud data obtained by randomly sampling the patches of the gripper model of the present invention. The method of obtaining point cloud data by randomly sampling the patches described in Step 1 includes:

[0079] The gripper model data consists of a series of triangular patches. For each patch, the number of sampling points is determined by area calculation, and the point cloud data is generated by the method of random uniform sampling based on the determined number. Assume that the three vertex coordinates of any triangular patch are A(x1, y1, z1), B(x2, y2, z2), C(x3, y3, z3), and the lengths of the three sides are calculated in turn as follows:

[0080] (1)

[0081] Use Heron's formula to calculate the area S of the triangular patch:

[0082] (2)

[0083] (3)

[0084] Set the sampling point density per unit area as n, and the total number of sampling points N is calculated as: N = round(n×S). Use the barycentric coordinate system of the triangle and generate sampling points inside the triangle by the method of random interpolation. The position of any point P inside the triangle can be expressed as:

[0085] (4)

[0086] where α, β, γ are the proportionality coefficients in the barycentric coordinates, describing the position relationship of point P inside the triangle relative to the three vertices, satisfying α + β + γ = 1 and α, β, γ ≥ 0.

[0087] The specific method for calculating α, β, γ is: generate two random numbers r1 and r2, both of which are in the range of [0, 1], and calculate the weights through the following formula:

[0088] (5)

[0089] Point P is evenly distributed inside the triangle.

[0090] In this example,Figure 3 It is a schematic flow diagram of constructing a hierarchical bounding box tree, combined with Figure 3 , the construction of the hierarchical bounding box tree of the gripper model and the stacked workpieces described in step 2 includes:

[0091] Calculate the bounding box of all input point cloud data, set it as the root node of the bounding box tree, and add it to the bounding box tree.

[0092] Starting from the root node, build the tree recursively downward; judge the shape of the point cloud through the bounding box of the current node, adaptively divide it into several subspaces according to the shape of the point cloud, and then calculate the bounding box of the point cloud data of the subspace and add it to the bounding box tree;

[0093] In this example, the calculation of the bounding box described in step 2 includes:

[0094] The type of bounding box used to construct the hierarchical bounding box tree is the axis-aligned bounding box AABB. Traverse all point cloud data points to find the center C(x, y, z) of the bounding box, the three axial vectors u, v, w of the bounding box, and the corresponding half-axis lengths hu, hv, hw. The "axis-aligned" of the AABB bounding box means that each of its faces is parallel to the point cloud coordinate axes, u=(1, 0, 0), v=(0, 1, 0), w=(0, 0, 1).

[0095] In this example, Figure 4a - Figure 4c It is a schematic diagram of the spatial adaptive segmentation of point cloud data, combined with Figure 4a - Figure 4c , the spatial adaptive segmentation of the point cloud data described in step 2 includes:

[0096] Pre-define the point cloud data into three types according to the shape: strip-shaped point cloud, planar point cloud, and block-shaped point cloud. Sort the half-axis lengths in the axis-aligned bounding box AABB of the current point cloud from largest to smallest to obtain the first half-axis length, the second half-axis length, and the third half-axis length, and judge the shape of the point cloud according to the sizes of the three half-axis lengths.

[0097] If the first half-axis length is much longer than the second and third half-axis lengths, it is regarded as a strip-shaped point cloud; if the first and second half-axis lengths are similar and much larger than the third half-axis length, it is regarded as a planar point cloud; if the first, second, and third half-axis lengths are similar, it is regarded as a block-shaped point cloud.

[0098] The strip-shaped point cloud is divided into 2 pieces of point cloud data through the axis corresponding to the first half-axis length; the planar point cloud is divided into 4 pieces of point cloud data through the axis corresponding to the first half-axis length and the axis corresponding to the second half-axis length; the block-shaped point cloud is divided into 8 pieces of point cloud data through the axis corresponding to the first half-axis length, the axis corresponding to the second half-axis length, and the axis corresponding to the third half-axis length.

[0099] In this example, Figure 5It is a schematic flow chart of the collision detection of the hierarchical bounding box tree, combined with Figure 5 , the collision detection between the two hierarchical bounding box trees described in step three includes:

[0100] Recursively search starting from the root nodes of the two trees, select the bounding boxes of the two nodes and determine whether they overlap; if they do not overlap, end the judgment of the child nodes under the current node; if they overlap, perform further detection;

[0101] For each pair of overlapping nodes, perform further detection. If both nodes are non-leaf nodes: recursively detect the child nodes of these two nodes; if one node is a leaf node and the other is not: recursively detect the leaf node and the child nodes of the non-leaf node; if both nodes are leaf nodes: end the judgment of the child nodes under the current node;

[0102] When all relevant node pairs have been checked and there are no more child nodes to detect, end the collision detection process; if no overlapping nodes are found at the end, return a non-collision result, if a collision is found at the leaf nodes, return a collision result, and according to the early exit mechanism, as long as an overlap is found at two leaf nodes, directly end the detection process.

[0103] In this instance, Figure 6 It is a schematic diagram of the bounding box overlap judgment, combined with Figure 6 , determining whether the bounding boxes overlap includes:

[0104] Use the separating axis theorem to determine whether two bounding boxes overlap; if there is an axis such that the projections of the two bounding boxes on this axis do not overlap, then they do not collide; otherwise, if the projections on all axes overlap, then they collide; the axes that need to be checked include: the three local coordinate axes of each bounding box, and the cross products of the local coordinate axes of the two bounding boxes, a total of 15 axes.

[0105] In this instance, Figure 7 It is a schematic flow chart of the path continuous collision detection of the present invention, combined with Figure 7 , the interpolation of the robot motion trajectory described in step 4 includes:

[0106] During the process of the robot moving from the starting point A to the ending point B, a series of discrete poses of the gripper during the movement are generated through trajectory interpolation, and the pose includes position and attitude;

[0107] Among them, the position interpolation is through linear interpolation. Specifically: Let the three-dimensional coordinates of the starting point PA be (x1, y1, z1) and the three-dimensional coordinates of the ending point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the ending point PB:

[0108] (6)

[0109] Based on the set trajectory interpolation step Δt and combined with the distance d between two points, the required number of interpolation points n is determined, specifically: n = round(d / Δt). According to the calculated number of interpolation points n, intermediate position points are interpolated in a uniformly distributed manner along the connection direction between the starting point PA and the ending point PB. For each interpolation step t (t ∈ [0,1], with a step size of 1 / n), the intermediate point position P(t) is calculated according to the following formula:

[0110] (7)

[0111] Among them, attitude interpolation adopts the quaternion interpolation method. Specifically: Let the starting attitude be the quaternion QA and the ending attitude be the quaternion QB, and calculate the angle θ between the quaternion QA and the quaternion QB:

[0112] (8)

[0113] And the spherical linear interpolation algorithm is adopted to obtain the corresponding intermediate attitude Q(t) at each interpolation step t:

[0114] (9)

[0115] In this example, the dynamically updated hierarchical bounding volume tree described in step 4 includes:

[0116] Based on the pose of the current robot movement, obtain the transformation matrix RT of the gripper model relative to the initial position; use the transformation matrix RT to update all axis-aligned bounding boxes AABB in the hierarchical bounding volume tree of the gripper model to oriented bounding boxes OBB, specifically including:

[0117] Center point transformation: Based on the center point C of the AABB, apply the transformation matrix RT to obtain a new center point C′, where C′ = M·C, and M is the transformation matrix RT;

[0118] Axis direction transformation: Rotate the standard coordinate axes (u, v, w) of the AABB in the local coordinate system through the rotation part R of the transformation matrix RT to obtain the new axis directions u′ = R·u, v′ = R·v, w′ = R·w of the OBB;

[0119] The half-axis lengths remain unchanged. During the update process from AABB to OBB, the half-axis lengths in each axis direction remain unchanged.

[0120] Embodiment 2

[0121] Refer to Figure 8, this embodiment relates to a point cloud collision detection device for robotic unordered grasping, which includes a memory and one or more processors. An executable code is stored in the memory. When the one or more processors execute the executable code, it is used for the point cloud collision detection method of robotic unordered grasping in Embodiment 1.

[0122] At the hardware level, the device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory. Of course, there may also be other hardware required for other services. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to implement the above Figure 1 method. Of course, in addition to the software implementation, the present invention does not exclude other implementation methods, such as logic devices or a combination of software and hardware, etc. That is to say, the execution subject of the following processing flow is not limited to each logic unit, and can also be hardware or logic devices.

[0123] The improvement of a technology can be clearly distinguished as either a hardware improvement (e.g., improvement of circuit structures such as diodes, transistors, switches, etc.) or a software improvement (improvement of method flows). However, with the development of technology, many improvements of method flows today can be regarded as direct improvements of hardware circuit structures. Almost all designers obtain the corresponding hardware circuit structure by programming the improved method flow into the hardware circuit. Therefore, it cannot be said that an improvement of a method flow cannot be implemented by a hardware entity module. For example, a programmable logic device (PLD) (such as a field programmable gate array (FPGA)) is such an integrated circuit whose logic function is determined by the user's programming of the device. The designer can program by himself to "integrate" a digital system on a PLD, without having to ask the chip manufacturer to design and fabricate a dedicated integrated circuit chip. Moreover, nowadays, instead of manually fabricating integrated circuit chips, this programming is mostly implemented using "logic compiler" software, which is similar to the software compiler used in program development and writing. The original code before compilation also has to be written in a specific programming language, which is called a hardware description language (HDL), and there is not only one kind of HDL, but many kinds, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, RHDL (Ruby Hardware Description Language), etc. Currently, the most commonly used are VHDL (Very-High-Speed Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art should also be clear that only by slightly logically programming the method flow with the above-mentioned several hardware description languages and programming it into the integrated circuit can the hardware circuit implementing the logical method flow be easily obtained.

[0124] The controller can be implemented in any suitable manner. For example, the controller can take the form of, for example, a microprocessor or a processor and a computer-readable medium storing computer-readable program code (such as software or firmware) executable by the (micro)processor, logic gates, switches, an application specific integrated circuit (ASIC), a programmable logic controller, and an embedded microcontroller. Examples of the controller include, but are not limited to, the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20, and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art also know that in addition to implementing the controller in the form of pure computer-readable program code, it is entirely possible to implement the same function by logically programming method steps so that the controller is in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, such a controller can be considered a hardware component, and the devices included therein for implementing various functions can also be regarded as the structures within the hardware component. Or even, the devices for implementing various functions can be regarded as either software modules for implementing the method or structures within the hardware component.

[0125] The systems, devices, modules, or units illustrated in the above embodiments can be specifically implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer can be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or any combination of these devices.

[0126] For the convenience of description, when describing the above devices, they are described separately as various units according to their functions. Of course, when implementing the present invention, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0127] Embodiment 3

[0128] This embodiment relates to a computer-readable storage medium storing a program that, when executed by a processor, implements the point cloud collision detection method for unordered grasping of the robot in Embodiment 1.

[0129] Those skilled in the art will understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0130] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0131] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0132] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0133] In a typical configuration, a computing device includes one or more processors (CPUs), an input / output interface, a network interface, and memory.

[0134] The memory may include non-permanent memory in the form of computer-readable media, random access memory (RAM), and / or non-volatile memory, such as read-only memory (ROM) or flash memory (flash RAM). The memory is an example of computer-readable media.

[0135] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0136] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0137] It should be understood by those skilled in the art that the embodiments of the present invention may be provided as methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware aspects. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0138] The present invention may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present invention may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.

[0139] The embodiments of the present invention are all described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the system embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the corresponding descriptions in the method embodiments.

[0140] The above are only the embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, various modifications and changes can be made to the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the scope of the claims of the present invention.

Claims

1. A point cloud collision detection method for disordered grasping of a robot, characterized in that, The method includes the following steps: Step 1: Obtain point cloud data by randomly sampling the imported gripper model through triangular meshes, and obtain point cloud data of stacked workpieces by scanning with a 3D camera; Step 2: Calculate the bounding box and perform spatial adaptive segmentation on the point cloud data to construct a hierarchical bounding box tree for the gripper model and the stacked workpieces; Step 3: Use depth-first recursive search on the two hierarchical bounding box trees to perform collision detection to obtain the collision results at a single discrete pose; Step 4: Interpolate the robot motion trajectory, dynamically update the hierarchical bounding box tree of the gripper model according to each pose generated by the interpolation, and detect the collision situation of the overall motion trajectory.

2. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that The obtaining of point cloud data by randomly sampling through triangular meshes in Step 1 includes: The gripper model data consists of a series of triangular meshes. For each triangular mesh, determine the number of sampling points by area calculation, and generate point cloud data using the method of random uniform sampling based on the determined number; Assume the three vertex coordinates of any triangular mesh are A(x1, y1, z1), B(x2, y2, z2), C(x3, y3, z3) respectively, and the lengths of the three sides are calculated in sequence as: （1） Use Heron's formula to calculate the area S of the triangular mesh: （2） （3） Set the sampling point density per unit area as n, and the total number of sampling points N is calculated as: N = round(n×S); Use the barycentric coordinate system of the triangle to generate sampling points inside the triangle by the method of random interpolation; The position of any point P inside the triangle can be expressed as: （4） where α, β, γ are the proportionality coefficients in the barycentric coordinates, describing the position relationship of point P inside the triangle relative to the three vertices, satisfying α + β + γ = 1 and α, β, γ ≥ 0; The specific method for calculating α, β, γ is: Generate two random numbers r1 and r2, both of which are in the range of [0, 1], and calculate the weights through the following formula: （5） Point P is evenly distributed inside the triangle.

3. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that The construction of the hierarchical bounding box tree for the gripper model and the stacked workpieces in Step 2 includes: Calculate the bounding box of all the input point cloud data, set it as the root node of the bounding box tree, and add it to the bounding box tree; Starting from the root node, recursively build the tree downward; Judge the shape of the point cloud through the bounding box of the current node, adaptively segment it into several subspaces according to the point cloud shape, and then calculate the bounding box of the point cloud data in the subspace and add it to the bounding box tree; When the node meets the condition for continued segmentation, perform spatial segmentation on the node until the node does not meet the segmentation condition, and finally construct a hierarchical bounding box tree.

4. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that, The calculation of the bounding box in Step 2 includes: The type of bounding box used to construct the hierarchical bounding box tree is the axis-aligned bounding box AABB. Traverse all the points in the point cloud data to find the center C(x, y, z) of the bounding box, the three axial vectors u, v, w of the bounding box, and the corresponding semi-axis lengths hu, hv, hw; The "axis-aligned" of the AABB bounding box means that each of its faces is parallel to the point cloud coordinate axes, u=(1, 0, 0), v=(0,1, 0), w=(0, 0, 1).

5. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that, The spatial adaptive segmentation of the point cloud data in Step 2 includes: The point cloud data is predefined into three types according to its shape: strip-shaped point cloud, planar point cloud, and block-shaped point cloud; the semi-axis lengths in the axis-aligned bounding box (AABB) of the current point cloud are sorted from largest to smallest to obtain the first semi-axis length, the second semi-axis length, and the third semi-axis length, and the shape of the point cloud is judged according to the magnitudes of the three semi-axis lengths. If the first semi-axis length is much longer than the second and third semi-axis lengths, it is regarded as a strip-shaped point cloud; if the first and second semi-axis lengths are similar and much larger than the third semi-axis length, it is regarded as a planar point cloud; if the first, second, and third semi-axis lengths are similar, it is regarded as a block-shaped point cloud. The strip-shaped point cloud is divided into two pieces of point cloud data by the axis corresponding to the first semi-axis length; the planar point cloud is divided into four pieces of point cloud data by the axis corresponding to the first semi-axis length and the axis corresponding to the second semi-axis length; the block-shaped point cloud is divided into eight pieces of point cloud data by the axis corresponding to the first semi-axis length, the axis corresponding to the second semi-axis length, and the axis corresponding to the third semi-axis length.

6. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that, The collision detection between the two hierarchical bounding box trees described in Step 3 includes: Starting from the root nodes of the two trees, recursively search, select the bounding boxes of the two nodes and judge whether they overlap; if they do not overlap, end the judgment of the child nodes under the current node; if they overlap, further detection is carried out. For each pair of overlapping nodes, further detection is carried out. If both nodes are non-leaf nodes: recursively detect the child nodes of these two nodes; if one node is a leaf node and the other is not: recursively detect the leaf node and the child nodes of the non-leaf node; if both nodes are leaf nodes: end the judgment of the child nodes under the current node. When all relevant node pairs have been checked and there are no more child nodes to detect, end the collision detection process; if no overlapping nodes are found at the end, return the non-collision result, if a collision is found at the leaf nodes, return the collision result, and according to the early exit mechanism, as long as an overlap is found at the two leaf nodes, directly end the detection process.

7. A point cloud collision detection method for unordered grasping of a robot according to claim 6, characterized in that, Judging whether the bounding boxes overlap includes: Using the separating axis theorem to judge whether the two bounding boxes overlap; if there is an axis such that the projections of the two bounding boxes on this axis do not overlap, then they do not collide; otherwise, if the projections on all axes overlap, then they collide; the axes that need to be checked include: the three local coordinate axes of each bounding box, and the cross products of the local coordinate axes of the two bounding boxes, a total of 15 axes.

8. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that, The interpolation of the robot motion trajectory described in Step 4 includes: During the process of the robot moving from the starting point A to the ending point B, a series of discrete poses of the gripper during the movement are generated through trajectory interpolation, and the pose includes position and orientation. Among them, the position interpolation is carried out by linear interpolation. Specifically: Let the three-dimensional coordinates of the starting point PA be (x1, y1, z1) and the three-dimensional coordinates of the ending point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the ending point PB: （6） Based on the set trajectory interpolation step size Δt and combined with the distance d between two points, determine the required number of interpolation points n, specifically: n = round(d / Δt); According to the calculated number of interpolation points n, interpolate the intermediate position points in a uniformly distributed manner along the line connecting the starting point PA and the ending point PB; For each interpolation step size t (t ∈ [0,1], the step size is 1 / n), the intermediate point position P(t) is calculated according to the following formula: （7） Among them, attitude interpolation adopts the quaternion interpolation method, specifically: Let the starting attitude be the quaternion QA and the ending attitude be the quaternion QB, and calculate the angle θ between the quaternion QA and the quaternion QB: （8） And adopt the spherical linear interpolation algorithm to obtain the corresponding intermediate attitude Q(t) at each interpolation step size t: （9）。 9. A point cloud collision detection method for disordered grasping of a robot according to claim 1, characterized in that, The dynamically updated hierarchical bounding volume tree described in step four includes: Based on the pose of the current robot movement, obtain the transformation matrix RT of the gripper model relative to the initial position; Use the transformation matrix RT to update all axis-aligned bounding boxes AABB in the hierarchical bounding volume tree of the gripper model to oriented bounding boxes OBB, specifically including: Center point transformation, with the center point C of the AABB as the reference, apply the transformation matrix RT to obtain the new center point C′, where C′ = M·C, and M is the transformation matrix RT; Axis direction transformation, rotate the standard coordinate axes (u, v, w) of the AABB in the local coordinate system through the rotation part R of the transformation matrix RT to obtain the new axis directions u′ = R·u, v′ = R·v, w′ = R·w of the OBB; The half-axis lengths remain unchanged. During the update process from AABB to OBB, the half-axis lengths in each axis direction remain unchanged.

10. A point cloud collision detection device for disordered grasping of a robot, characterized in that, It includes a memory and one or more processors. Executable code is stored in the memory. When the one or more processors execute the executable code, it is used to implement the point cloud collision detection method for unordered grasping of the robot described in any one of claims 1-9.

Images