A point cloud collision detection method and device for robot disorderly grasping

Abstract

A point cloud collision detection method and device for disordered robot grasping includes: randomly sampling an imported gripper model through patches to obtain point cloud data, scanning with a 3D camera to obtain point cloud data of stacked workpieces; calculating bounding boxes and performing spatially adaptive segmentation on the point cloud data to construct a hierarchical bounding box tree of the gripper model and the stacked workpieces; performing collision detection on the two hierarchical bounding box trees using a depth-first recursive search to obtain collision results under a single discrete posture; interpolating the robot's motion trajectory, dynamically updating the hierarchical bounding box tree of the gripper model based on each interpolated posture, and detecting collisions in the overall motion trajectory. The present invention performs spatially adaptive segmentation on point cloud data to adapt to the actual distribution of point clouds, improve space utilization, and enhance the balance of the tree structure; interpolating the robot's motion trajectory and dynamically updating the hierarchical bounding box tree avoids frequent reconstruction of the entire tree structure, thereby achieving efficient path-continuous collision detection.

Description

Technical Field

[0001] The present invention relates to the field of robot perception and to disordered grasping scenarios, and in particular to a point cloud collision detection method and device for disordered grasping by robots. Background Art

[0002] As intelligent robotic operations continue to evolve towards complex scenarios, autonomous perception, and human-machine interaction, higher requirements are placed on robots' perception and operational capabilities. Integrating 3D vision cameras into robots, giving them "visual perception" capabilities, can significantly improve their environmental adaptability and operational accuracy, making "robot + vision" play an increasingly important role in digital workshops and unmanned factories.

[0003] Robotic random grasping, a typical complex task, requires the robot to use 3D vision to obtain spatial information about the workpiece in an unknown, scattered, and stacked environment, calculate a feasible grasping pose, and autonomously complete the grasping operation. In practice, the robot's end effector (such as the gripper) is prone to collision with surrounding workpieces during approach or movement, resulting in grasp failure. Therefore, efficient and accurate collision detection technology has become an indispensable key component in robotic random grasping applications.

[0004] A wide variety of collision detection methods exist. Bounding volume (Bounding Volume) detection strategies are particularly popular for point cloud data generated by 3D visual scanning. These methods construct specific bounding volumes (such as spheres, axis-aligned bounding boxes (AABBs), and oriented bounding boxes (OBBs)) to simplify the object under test and determine collisions by performing intersection tests between the bounding volumes. Bounding volume-based methods are computationally simple and fast, making them particularly suitable for point cloud data, which is loosely structured and large in volume.

[0005] To further improve detection efficiency, existing technologies have developed a variety of bounding box organizational structures based on hierarchical spatial division, such as the Bounding Volume Hierarchy (BVH). Among these, one method uses a binary tree to gradually subdivide the point cloud space from top to bottom, generating hierarchical bounding boxes to accelerate detection. However, in complex three-dimensional scenes, simple binary tree division suffers from low tree construction efficiency. Another method, based on octree technology, recursively subdivides the three-dimensional space into eight sub-regions and constructs bounding boxes within each region to improve partition management efficiency. However, the uniformly divided octree structure is prone to hierarchical imbalance or redundant division when the point cloud is sparse or the workpiece shape is complex, affecting detection performance.

[0006] In the prior art, for example, Chinese patent CN112179602B discloses a method for collision detection of a robotic arm. This method performs surface rasterization processing on the virtual simulation scene model and the robotic arm model respectively to obtain corresponding point cloud data, uses an octree to segment the point cloud space, and constructs a multi-level bounding box tree structure through a mixed encapsulation method of spherical bounding boxes and directional 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 kinematic model of the robotic arm, collision detection on a continuous motion path is achieved based on a dynamic programming method. This technical solution has achieved good results in improving detection efficiency and continuous detection capabilities, and is particularly suitable for applications requiring static scenes and predictable motion paths.

[0007] However, existing collision detection technologies, including the aforementioned methods, still suffer from some common problems. First, fixed-rule partitioning (such as octrees) makes it difficult to adapt to areas with large point cloud density and shape variations in complex scenes, resulting in redundant point cloud partitioning and increased computational burden. Second, in the face of continuous changes in the robot's posture during motion, traditional methods typically require frequent updates or reconstructions of the bounding box tree structure, which lacks real-time performance and carries high update overhead, impacting detection response speed. Furthermore, most existing technologies rely on pre-built environmental models and have limited adaptability to dynamically changing or real-time perceived working environments. In scenes with disordered stacking, complex and diverse shapes, and frequently changing environments, the stability and accuracy of collision detection are difficult to guarantee. In summary, existing technologies still suffer from poor balance between accuracy and efficiency and insufficient adaptability when dealing with real-time collision detection of continuous robot motion in dynamic and complex scenes. There is an urgent need to propose more flexible and efficient detection solutions to improve them. Summary of the Invention

[0008] This invention aims to overcome the problem of collisions between the robot's end effector, or gripper, and surrounding workpieces in the gripping position, or other workpieces during gripper motion, in robotic non-sequential grasping scenarios. It provides a point cloud collision detection method and device for robotic non-sequential grasping. The invention proposes a spatially adaptive segmentation method for point cloud data, efficiently constructing a hierarchical bounding box tree, interpolating the robot's motion trajectory, and dynamically updating the gripper model's hierarchical bounding box tree to achieve path-continuous collision detection.

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

[0010] Step 1: The imported gripper model is randomly sampled to obtain point cloud data, and the stacked workpiece point cloud data is obtained by scanning with a 3D camera;

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

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

[0013] Step 4: Interpolate the robot's motion trajectory, dynamically update the hierarchical bounding box tree of the gripper model according to the interpolated poses, and detect the collision of the entire motion trajectory.

[0014] Preferably, obtaining point cloud data by random sampling of patches in step 1 includes:

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

[0016] (1)

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

[0018] (2)

[0019] (3)

[0020] Assuming the sampling point density per unit area is n, the total number of sampling points N is calculated as: N = round(n×S). Using the triangle's barycentric coordinate system, random interpolation is used to generate sampling points inside the triangle. The position of any point P inside the triangle can be expressed as:

[0021] (4)

[0022] Where α, β, and γ are the proportional coefficients in the barycentric coordinates, which describe 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 to generate two random numbers r1 and r2, both of which are in the range [0, 1], and calculate the weights using the following formula:

[0024] (5)

[0025] Points P are evenly distributed inside the triangle.

[0026] Preferably, the step 2 of constructing the gripper model and the hierarchical bounding box tree of the stacked workpieces includes:

[0027] 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.

[0028] Starting from the root node, the tree is recursively constructed downwards; the point cloud shape is determined by the bounding box of the current node, and the point cloud shape is adaptively divided into several subspaces according to the point cloud shape. The bounding box of the subspace point cloud data is then calculated and added to the bounding box tree;

[0029] When a node meets the conditions for continued segmentation, the node is spatially segmented until the node no longer meets the segmentation conditions, and finally a hierarchical bounding box tree is constructed.

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

[0031] The bounding box type used to construct the hierarchical bounding box tree is an axis-aligned bounding box (AABB). All point cloud data points are traversed to determine the bounding box center C (x, y, z), the three axial vectors u, v, and w of the bounding box, and the corresponding semi-axis lengths hu, hv, and hw. The "axis alignment" of the AABB bounding box means that each of its faces is parallel to the point cloud coordinate axes, with u = (1, 0, 0), v = (0, 1, 0), and w = (0, 0, 1).

[0032] Preferably, the step 2 of performing spatial adaptive segmentation on the point cloud data includes:

[0033] Point cloud data is predefined into three types based on shape: strip point cloud, surface point cloud, and block point cloud. The semi-axis lengths within the axis-aligned bounding box (AABB) of the current point cloud are sorted from largest to smallest to obtain the first, second, and third semi-axis lengths. The point cloud shape is determined based on these three semi-axis lengths.

[0034] If the length of the first semi-axis is greater than that of the second and third semi-axis, it is considered a strip point cloud; if the lengths of the first and second semi-axis are similar and much greater than that of the third semi-axis, it is considered a surface point cloud; if the lengths of the first, second, and third semi-axis are similar, it is considered a block point cloud.

[0035] The strip point cloud is divided into two blocks of point cloud data by the axial direction corresponding to the first semi-axis length; the surface point cloud is divided into four blocks of point cloud data by the axial direction corresponding to the first semi-axis length and the axial direction corresponding to the second semi-axis length; the block point cloud is divided into eight blocks of point cloud data by the axial direction corresponding to the first semi-axis length, the axial direction corresponding to the second semi-axis length, and the axial direction corresponding to the third semi-axis length.

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

[0037] Start the recursive search from the root nodes of the two trees, select the bounding boxes of the two nodes to determine whether they overlap; if not, end the child node judgment under the current node; if they overlap, perform further detection;

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

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

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

[0041] 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, the cross product of the local coordinate axes of the two bounding boxes, a total of 15 axes.

[0042] Preferably, the interpolation of the robot motion trajectory in step 4 includes:

[0043] When the robot moves from the starting point A to the ending point B, a series of discrete postures of the gripper are generated by trajectory interpolation. The postures include position and attitude.

[0044] The position interpolation is done 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 end point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the end point PB:

[0045] (6)

[0046] Based on the set trajectory interpolation step length Δt and the distance d between the two points, the required number of interpolation points n is determined: n = round(d / Δt). Based on the calculated number of interpolation points n, intermediate points are interpolated uniformly along the line connecting the starting point PA and the end point PB. For each interpolation step length t (t ∈ [0,1], where the step length is 1 / n), the intermediate point position P(t) is calculated according to the following formula:

[0047] (7)

[0048] The posture interpolation adopts the four-element interpolation method, specifically: let the starting posture be the quaternion QA, the ending posture be the four-element QB, and calculate the angle θ between the quaternion QA and the quaternion QB:

[0049] (8)

[0050] And using the spherical linear interpolation algorithm, at each interpolation step t, the corresponding intermediate posture Q(t) is obtained:

[0051] (9)

[0052] Preferably, the step 4 of dynamically updating the hierarchical bounding box tree includes:

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

[0054] Center point transformation: Taking 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, where 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 by 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 length of the semi-axis remains unchanged. During the update process from AABB to OBB, the length of the semi-axis in each axis direction remains unchanged.

[0057] The second aspect of the present invention relates to a point cloud collision detection device for disordered grasping by a robot, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the point cloud collision detection method for disordered grasping by a robot of the present invention.

[0058] The present invention performs spatially adaptive segmentation on point cloud data, so that the bounding box tree construction can dynamically adapt to the actual distribution of the point cloud, improve space utilization and optimize the balance of the tree structure, laying the foundation for efficient traversal. In the collision detection stage, a depth-first recursive search is used to quickly screen potential collision areas and improve detection speed. For the robot's continuous motion path, discrete postures are generated through trajectory interpolation, and the bounding box tree is dynamically updated during the motion process. Combined with the rapid construction of AABB and the adaptability of OBB rotation, frequent reconstruction is avoided to ensure the real-time and high efficiency of path continuous collision detection. The synergistic effect of the above mechanisms enables the present invention to significantly improve the detection efficiency under continuous motion while ensuring detection accuracy.

[0059] This paper proposes a method for spatially adaptive segmentation of point cloud data and constructing a hierarchical bounding box tree. This method dynamically adjusts the spatial partitioning strategy based on the point cloud's shape characteristics to generate bounding boxes that better fit the actual distribution of the point cloud surface and a more balanced bounding box tree. This structure enables more effective collision avoidance determinations at the high-level node stage during collision detection, enabling early pruning, reducing deep recursion times, lowering overall traversal overhead, and significantly improving detection efficiency.

[0060] The advantages of the present invention are: spatially adaptive segmentation of point cloud data to better adapt to the actual distribution of point clouds, improve space utilization and enhance the balance of the tree structure; interpolate the robot's motion trajectory and dynamically update the hierarchical bounding box tree to avoid frequent reconstruction of the entire tree structure, thereby achieving more efficient path continuous collision detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a flow chart of a point cloud collision detection method for disordered grasping by a robot according to the present invention.

[0062] Figure 2This is a schematic diagram of point cloud data obtained by random sampling of facets in the gripper model of the present invention.

[0063] Figure 3 It is a schematic diagram of the process of constructing a hierarchical bounding box tree according to the present invention.

[0064] Figure 4a - Figure 4c Schematic diagram of the point cloud data space adaptive segmentation of the present invention, wherein, Figure 4a It is a schematic diagram of dividing a strip point cloud into two point clouds; Figure 4b It is a schematic diagram of dividing the surface point cloud into 4 point clouds; Figure 4c It is a schematic diagram of dividing a block point cloud into 8 point clouds.

[0065] Figure 5 It is a flowchart of the hierarchical bounding box tree collision detection of the present invention.

[0066] Figure 6 It is a schematic diagram of bounding box overlap judgment of the present invention.

[0067] Figure 7 It is a flow chart of the path continuous collision detection of the present invention.

[0068] Figure 8 It is a schematic diagram of the device of the present invention. DETAILED DESCRIPTION

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

[0070] Example 1

[0071] In the robot's disordered grasping operation scenario, when facing stacked workpieces, because the workpieces are intertwined with each other, the robot's end effector, that is, 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 inventors have provided solutions given in the following embodiments through research.

[0073] In this example, Figure 1 The present invention is a flowchart of a point cloud collision detection method for disordered grasping by a robot, and the method includes the following steps:

[0074] Step 1: The imported gripper model is randomly sampled to obtain point cloud data, and the stacked workpiece point cloud data is obtained by scanning with a 3D camera;

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

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

[0077] Step 4: Interpolate the robot's motion trajectory, dynamically update the hierarchical bounding box tree of the gripper model according to the interpolated poses, and detect the collision of the entire motion trajectory.

[0078] In this example, Figure 2 Schematic diagram of obtaining point cloud data by random sampling of a surface by a gripper model of the present invention. The step 1 of obtaining point cloud data by random sampling of a surface includes:

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

[0080] (1)

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

[0082] (2)

[0083] (3)

[0084] Assuming the sampling point density per unit area is n, the total number of sampling points N is calculated as: N = round(n×S). Using the triangle's barycentric coordinate system, random interpolation is used to generate sampling points inside the triangle. The position of any point P inside the triangle can be expressed as:

[0085] (4)

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

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

[0088] (5)

[0089] Points P are evenly distributed inside the triangle.

[0090] In this example, Figure 3 It is a flowchart of constructing a hierarchical bounding box tree, combined with Figure 3 The step 2 of constructing the gripper model and the hierarchical bounding box tree of the stacked workpieces 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, the tree is recursively constructed downwards; the point cloud shape is determined by the bounding box of the current node, and the point cloud shape is adaptively divided into several subspaces according to the point cloud shape. The bounding box of the subspace point cloud data is then calculated and added to the bounding box tree;

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

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

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

[0096] Point cloud data is predefined into three types based on shape: strip point cloud, surface point cloud, and block point cloud. The semi-axis lengths within the axis-aligned bounding box (AABB) of the current point cloud are sorted from largest to smallest to obtain the first, second, and third semi-axis lengths. The point cloud shape is determined based on these three semi-axis lengths.

[0097] If the length of the first semi-axis is greater than that of the second and third semi-axis, it is considered a strip point cloud; if the lengths of the first and second semi-axis are similar and much greater than that of the third semi-axis, it is considered a surface point cloud; if the lengths of the first, second, and third semi-axis are similar, it is considered a block point cloud.

[0098] The strip point cloud is divided into two blocks of point cloud data by the axial direction corresponding to the first semi-axis length; the surface point cloud is divided into four blocks of point cloud data by the axial direction corresponding to the first semi-axis length and the axial direction corresponding to the second semi-axis length; the block point cloud is divided into eight blocks of point cloud data by the axial direction corresponding to the first semi-axis length, the axial direction corresponding to the second semi-axis length, and the axial direction corresponding to the third semi-axis length.

[0099] In this example, Figure 5This is a flowchart of collision detection of a hierarchical bounding box tree, combined with Figure 5 The collision detection between the two hierarchical bounding box trees in step 3 includes:

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

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

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

[0103] In this example, Figure 6 This is a schematic diagram of bounding box overlap judgment, combined with Figure 6 , judging 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, the cross product of the local coordinate axes of the two bounding boxes, a total of 15 axes.

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

[0106] When the robot moves from the starting point A to the ending point B, a series of discrete postures of the gripper are generated by trajectory interpolation. The postures include position and attitude.

[0107] The position interpolation is done 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 end point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the end point PB:

[0108] (6)

[0109] Based on the set trajectory interpolation step length Δt and the distance d between the two points, the required number of interpolation points n is determined: n = round(d / Δt). Based on the calculated number of interpolation points n, intermediate points are interpolated uniformly along the line connecting the starting point PA and the end point PB. For each interpolation step length t (t ∈ [0,1], where the step length is 1 / n), the intermediate point position P(t) is calculated according to the following formula:

[0110] (7)

[0111] The posture interpolation adopts the four-element interpolation method, specifically: let the starting posture be the quaternion QA, the ending posture be the four-element QB, and calculate the angle θ between the quaternion QA and the quaternion QB:

[0112] (8)

[0113] And using the spherical linear interpolation algorithm, at each interpolation step t, the corresponding intermediate posture Q(t) is obtained:

[0114] (9)

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

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

[0117] Center point transformation: Taking 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, where 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 by 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;

[0119] The length of the semi-axis remains unchanged. During the update process from AABB to OBB, the length of the semi-axis in each axis direction remains unchanged.

[0120] Example 2

[0121] Reference Figure 8This embodiment relates to a point cloud collision detection device for disordered grasping by a robot, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, the point cloud collision detection method for disordered grasping by a robot of embodiment 1 is used.

[0122] At the hardware level, the device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory, and may also include other hardware required for the business. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 Of course, in addition to 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, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0123] Improvements to a technology can be clearly categorized as either hardware improvements (for example, improvements to circuit structures like diodes, transistors, and switches) or software improvements (improvements to process flows). However, with technological advancements, many process flow improvements can now be considered direct improvements to hardware circuit structures. Designers almost always create the corresponding hardware circuit structure by programming the improved process flow into the hardware circuit. Therefore, it cannot be said that a process flow improvement cannot be implemented using physical hardware modules. For example, a programmable logic device (PLD), such as a field programmable gate array (FPGA), is an integrated circuit whose logical function is determined by user programming. Designers can integrate a digital system onto a PLD by programming it themselves, without having to hire a chip manufacturer to design and manufacture a dedicated integrated circuit chip. Moreover, nowadays, instead of manually fabricating integrated circuit chips, this programming is mostly performed using software called a "logic compiler." This is similar to the software compilers used during program development. Before compilation, the original code must be written in a specific programming language, called a hardware description language (HDL). There are many types of HDL, including 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, and RHDL (Ruby Hardware Description Language). Currently, the most commonly used are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art will also understand that by simply programming the method flow in one of these hardware description languages ​​and then programming it into an integrated circuit, a hardware circuit that implements the logic method flow can be easily obtained.

[0124] The controller can be implemented in any suitable manner. For example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing computer-readable program code (e.g., 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 controllers 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 memory control logic. Those skilled in the art will also appreciate that, in addition to implementing the controller purely in computer-readable program code, the controller can also be implemented in the form of logic gates, switches, an application-specific integrated circuit, a programmable logic controller, an embedded microcontroller, etc. by logically programming the method steps. Therefore, such a controller can be considered a hardware component, and the means for implementing the various functions included therein can also be considered as structures within the hardware component. Alternatively, the means for implementing the various functions can be considered both a software module implementing the method and a structure within the hardware component.

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

[0126] For the convenience of description, the above device is described as being divided into 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] Example 3

[0128] This embodiment relates to a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, the point cloud collision detection method for disordered grasping by a robot of embodiment 1 is implemented.

[0129] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, 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 magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0130] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0131] These computer program instructions may 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, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0132] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

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

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

[0135] Computer-readable media includes permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. The 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 disc read-only memory (CD-ROM), digital versatile disc (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 transitory computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0136] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0137] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, 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 magnetic disk storage, CD-ROMs, optical storage, etc.) containing computer-usable program code.

[0138] The present invention may be described in the general context of computer-executable instructions, such as program modules, executed by a computer. Generally, program modules include routines, programs, objects, components, data structures, and the like 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 via a communications network. In a distributed computing environment, program modules may be located in both local and remote computer storage media, including storage devices.

[0139] The various embodiments of the present invention are described in a progressive manner. Similar portions between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiment is generally similar to the method embodiment, so its description is relatively simple. For relevant portions, refer to the description of the method embodiment.

[0140] The foregoing is merely an embodiment of the present invention and is not intended to limit the present invention. It will be apparent to those skilled in the art that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention are intended to be included within the scope of the claims of the present invention.

Claims

1. A point cloud collision detection method for robot disordered grasping, characterized in that: The method comprises the following steps: Step 1: The imported gripper model is randomly sampled to obtain point cloud data, and the stacked workpiece point cloud data is obtained by scanning with a 3D camera; Step 2: Calculate bounding boxes and perform spatial adaptive segmentation on the point cloud data to construct a gripper model and a hierarchical bounding box tree of the stacked workpieces; Step 3: Use depth-first recursive search to perform collision detection on the two hierarchical bounding box trees to obtain the collision result under a single discrete pose; Step 4: Interpolate the robot's motion trajectory, dynamically update the hierarchical bounding box tree of the gripper model based on the interpolated poses, and detect collisions on the entire motion trajectory; The point cloud data obtained by random sampling of patches in step 1 includes: The gripper model data consists of a series of triangular facets. For each facet, the number of sampling points is determined by area calculation, and point cloud data is generated using a random uniform sampling method based on the determined number. Assuming that the coordinates of the three vertices of any triangular facet are A(x1, y1, z1), B(x2, y2, z2), and C(x3, y3, z3), the lengths of the three sides are calculated as follows: （1） Use Heron's formula to calculate the area S of the triangle: （2） （3） Assume that the sampling point density per unit area is n, and the total number of sampling points N is calculated as: N = round(n×S); using the triangle's barycentric coordinate system, random interpolation is used to generate sampling points inside the triangle; the position of any point P inside the triangle can be expressed as: （4） Where α, β, and γ are the proportional coefficients in the barycentric coordinates, which describe the position relationship of point P inside the triangle relative to the three vertices, satisfying α+β+γ=1 and α, β, γ≥0; The specific method for calculating α, β, and γ is to generate two random numbers r1 and r2, both of which are in the range [0, 1], and calculate the weights using the following formula: （5） Points P are evenly distributed inside the triangle.

2. A point cloud collision detection method for disordered grasping by a robot according to claim 1, characterized in that: The construction of the gripper model and the hierarchical bounding box tree of the stacked workpieces described in step 2 includes: 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; Starting from the root node, the tree is recursively constructed downwards; the point cloud shape is determined by the bounding box of the current node, and the point cloud shape is adaptively divided into several subspaces according to the point cloud shape. The bounding box of the subspace point cloud data is then calculated and added to the bounding box tree; When a node meets the conditions for continued segmentation, the node is spatially segmented until the node no longer meets the segmentation conditions, and finally a hierarchical bounding box tree is constructed.

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

4. A point cloud collision detection method for robot disordered grasping according to claim 1, characterized in that: The spatial adaptive segmentation of point cloud data described in step 2 includes: The point cloud data is predefined into three types according to shape: strip point cloud, surface point cloud, and block point cloud. The semi-axis lengths in the axis-aligned bounding box AABB of the current point cloud are sorted from large to small to obtain the first semi-axis length, the second semi-axis length, and the third semi-axis length. The point cloud shape is determined based on the sizes of the three semi-axis lengths. If the length of the first semi-axis is greater than that of the second and third semi-axis, it is considered a strip point cloud; if the lengths of the first and second semi-axis are similar and much greater than that of the third semi-axis, it is considered a surface point cloud; if the lengths of the first, second, and third semi-axis are similar, it is considered a block point cloud. The strip point cloud is divided into two blocks of point cloud data by the axial direction corresponding to the first semi-axis length; the surface point cloud is divided into four blocks of point cloud data by the axial direction corresponding to the first semi-axis length and the axial direction corresponding to the second semi-axis length; the block point cloud is divided into eight blocks of point cloud data by the axial direction corresponding to the first semi-axis length, the axial direction corresponding to the second semi-axis length, and the axial direction corresponding to the third semi-axis length.

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

6. A point cloud collision detection method for robot disordered grasping according to claim 5, characterized in that: Determining whether bounding boxes overlap includes: 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, the cross product of the local coordinate axes of the two bounding boxes, a total of 15 axes.

7. A point cloud collision detection method for disordered grasping by a robot as claimed in claim 1, characterized in that: The interpolation of the robot motion trajectory described in step 4 includes: When the robot moves from the starting point A to the ending point B, a series of discrete postures of the gripper are generated by trajectory interpolation. The postures include position and attitude. The position interpolation is done 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 end point PB be (x2, y2, z2), and calculate the distance d between the starting point PA and the end point PB: （6） Based on the set trajectory interpolation step length Δt and the distance d between the two points, the required number of interpolation points n is determined: n = round(d / Δt). Based on the calculated number of interpolation points n, intermediate positions are interpolated in a uniformly distributed manner along the line connecting the starting point PA and the end point PB. For each interpolation step length t (t ∈ [0,1], the step length is 1 / n), the intermediate point position P(t) is calculated according to the following formula: （7） The posture interpolation adopts the four-element interpolation method, specifically: let the starting posture be the quaternion QA, the ending posture be the four-element QB, and calculate the angle θ between the quaternion QA and the quaternion QB: （8） And using the spherical linear interpolation algorithm, at each interpolation step t, the corresponding intermediate posture Q(t) is obtained: （9）。 8. A point cloud collision detection method for robot disordered grasping according to claim 1, characterized in that: The step 4 of dynamically updating the hierarchical bounding box tree includes: Based on the current robot motion pose, obtain the transformation matrix RT of the gripper model relative to the initial position; using the transformation matrix RT, update all axis-aligned bounding boxes AABB in the hierarchical bounding box tree of the gripper model to oriented bounding boxes OBB, specifically including: Center point transformation: Taking 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, where 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 by 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; The length of the semi-axis remains unchanged. During the update process from AABB to OBB, the length of the semi-axis in each axis direction remains unchanged.

9. A point cloud collision detection device for disordered grasping by a robot, characterized in that: The method comprises a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, the method is used to implement the point cloud collision detection method for disordered grasping of a robot according to any one of claims 1 to 8.

Images