Voxel-based robot self-collision detection method

Abstract

This paper presents a voxel-based robot self-collision detection method, which relates to the fields of robot motion simulation and collision detection technology. It addresses the problems of high false negative rates and insufficient real-time performance of existing detection methods under extreme postures. The method achieves self-collision detection through steps such as establishing a robot kinematic model; constructing a voxelized virtual environment; dynamic voxelization of the robot rigid body; filtering invalid detection pairs; and parallel collision determination. This method employs a global voxelization strategy, covering the entire volume region of the robot rigid body, fundamentally eliminating detection blind spots caused by sparse surface sampling. Through direct voxel index collision determination and mask matrix pre-filtering, combined with a GPU parallel architecture, the response time under extreme postures is only 5.2ms~5.8ms. This method is applicable to complex operation scenarios such as aerospace component painting, automobile manufacturing, and precision assembly. This invention has the advantages of high detection reliability, strong real-time performance, and good adaptability.

Description

Technical Field

[0001] This invention relates to the field of robot motion simulation and collision detection technology, specifically to a robot self-collision detection method based on voxel spatial discretization and GPU parallel computing architecture in the safety monitoring method of industrial robots in high-end equipment manufacturing. This invention is applicable to the safety protection of multi-joint robots in complex operation scenarios such as aerospace component spraying, automobile manufacturing, and precision assembly. Background Technology

[0002] In the field of industrial automation, multi-joint robots have been widely used in complex work scenarios such as aerospace component painting and automobile manufacturing. When robots execute continuous trajectory movements, self-collisions are prone to occur between the links, wrists, and end effectors, leading to mechanical damage, work interruptions, and even safety accidents. According to industry statistics, self-collision accidents account for approximately 30% to 45% of industrial robot work interruptions; therefore, self-collision detection is a core technology for robot motion planning and simulation.

[0003] Existing robot self-collision detection methods mainly include geometric model methods, bounding box hierarchical tree methods, and continuous collision detection methods. Geometric model methods determine collisions by accurately calculating the penetration relationship of the rigid body surface mesh, but they have high computational complexity and insufficient real-time performance in multi-joint linkage scenarios. Bounding box hierarchical tree methods quickly eliminate non-intersecting objects by constructing a hierarchical bounding structure, but for slender robot parts, the bounding box redundancy space is large, resulting in a high false positive rate. Although continuous collision detection methods can solve the collision prediction problem in motion states, the traditional serial calculation mode is difficult to meet the stringent real-time requirements of scenarios such as aerospace painting.

[0004] Chinese invention patent CN114742944A discloses a conservative collision detection method (existing technology) for industrial robot path planning. This method uses a Merton code-based binary search algorithm to construct a linear hierarchical bounding volume (LBVH); it proposes a GPU-accelerated signed distance field (SDF) construction algorithm, but this SDF construction only focuses on the voxel mesh of the model's surface region, neglecting to calculate voxels inside the robot links; and it uses a particle swarm optimization-based envelope sphere generation algorithm for coarse screening. This method has the following inherent flaws:

[0005] (1) Detection blind zone caused by sparse surface sampling: When two robot parts come into close contact in extreme folding posture, the surface voxel mesh may produce a "detection blind zone" near the contact surface due to sparse sampling. That is, there are tiny gaps between the voxel meshes of the two parts on the surface, but the inside has been penetrated. At this time, the judgment based on the surface distance field may miss the detection.

[0006] (2) Limited fitting accuracy of the envelope ball: Using the envelope ball to replace the robot joint to perform collision detection has limited fitting accuracy on slender links or complex configuration parts, and the redundancy space can easily lead to an increase in the misjudgment rate.

[0007] (3) Configuration adaptation requires reconstruction: When the robot configuration changes, the SDF needs to be completely reconstructed, resulting in insufficient adaptability.

[0008] To address the aforementioned technical deficiencies, this invention proposes a robot self-collision detection method based on global voxelization and topology pre-filtering. This method directly determines collisions through voxel indexing, replacing SDF distance calculation and fundamentally eliminating detection blind spots. Summary of the Invention

[0009] The present invention aims to provide a voxel-based robot self-collision detection method to solve the problems of high false negative rate and insufficient real-time performance of existing detection methods under extreme postures.

[0010] A voxel-based robot self-collision detection method, which is implemented by the following steps:

[0011] Step 1: Establish the robot's kinematic model;

[0012] Step 2: Construct a voxelized virtual environment;

[0013] With the center of the robot base as the origin of the world coordinate system, a voxel mesh environment in a Cartesian coordinate system is constructed.

[0014] Step 3: Dynamic voxelization of the robot rigid body;

[0015] Perform global voxelization on each rigid body of the robot and assign a unique identifier;

[0016] Step 4: Invalid detection for filtering and parallel collision determination;

[0017] The mask matrix is ​​constructed based on kinematic topology to filter invalid detection pairs, and the global voxel state is updated through GPU atomic operations. Collisions are directly determined by voxel index conflicts.

[0018] Step 5: Repeat steps 3 and 4 to complete the robot's self-collision detection.

[0019] The beneficial effects of this invention are as follows: Compared with existing technologies, the detection method of this invention directly detects voxel occupancy conflicts, while existing technologies rely on approximate calculations of the surface distance field; existing technologies use spatial envelope spheres for coarse screening, while this invention uses kinematic topological mask matrix prior filtering. Specifically, it has the following advantages:

[0020] 1. Significantly improved detection reliability: By adopting a global voxelization strategy, the entire volume area of ​​the robot's rigid body is covered, and the false negative rate under extreme folding posture is reduced from 2.1% of the existing SDF method to 0.3%, fundamentally eliminating the detection blind zone caused by sparse surface sampling.

[0021] 2. High detection efficiency: Direct determination of voxel index conflict (O(1) complexity) and pre-filtering of mask matrix (reducing invalid computation by 46.4%), combined with GPU parallel architecture, the response time is only 5.2ms~5.8ms under extreme posture.

[0022] 3. High adaptability: No need to pre-calculate the distance field, no need to rebuild when the robot configuration changes, and supports standardized import of multiple robot models. Attached Figure Description

[0023] Figure 1 This is a flowchart of the voxel-based robot self-collision detection method described in this invention;

[0024] Figure 2 A schematic diagram illustrating the construction of a voxelized virtual environment;

[0025] Figure 3 A visualization of a 7×7 self-collision logic mask matrix;

[0026] Figure 4 A diagram showing the effect of self-collision detection in extreme robot postures;

[0027] Figure 5 This is a rendering showing the spatial distribution and contact details of the self-collision area. Detailed Implementation

[0028] Specific Implementation Method 1: Combination Figures 1 to 5 This embodiment describes a voxel-based robot self-collision detection method, which is implemented through the following steps:

[0029] Step S1. Robot kinematic modeling: Establish a robot kinematic model based on the DH parameter method;

[0030] Assume the robot includes One joint, A link, define the link. With connecting rod DH parameters between: link length Linkage offset Linkage torsion angle Joint angle (Variable); Link Relative to the link The homogeneous transformation matrix is:

[0031]

[0032] Pose matrix of the spray gun end effector relative to the base coordinate system We obtain this through matrix multiplication:

[0033]

[0034] Step S2. Constructing a voxelized virtual environment; such as Figure 2 As shown, a voxel mesh environment in a Cartesian coordinate system is constructed with the center of the robot base as the origin of the world coordinate system. The selection of voxel side length needs to take into account both detection accuracy and computational cost. Through experiments, it has been verified that a voxel side length of 2.0 mm can achieve the optimal balance between millimeter-level accuracy and real-time performance.

[0035] In this embodiment, a hash table sparse storage strategy is adopted, which allocates memory only for the "occupied" voxels, reducing the video memory usage from the theoretical value of 3.5GB to 480MB.

[0036] Step S3. Dynamic voxelization of robot rigid bodies: Perform global voxelization (including internal voxels) on each rigid body and assign a unique identifier; the specific implementation process is as follows:

[0037] Step S31. Local Voxel Pre-generation: In the local coordinate system of each rigid body, the 3D model is voxelized to generate local voxel point sets. ,in, rigid body The total number of voxels, Let be the local voxel coordinates, where , , These are the x-axis, y-axis, and z-axis coordinates of the local voxel in the local coordinate system, respectively.

[0038] Step S32. World Coordinate Mapping: Based on the kinematic model in Step S1, the local voxels are mapped to the world coordinate system through homogeneous transformation:

[0039]

[0040] in, For the k-th trajectory time, , , They are respectively The coordinates of the local voxels in the world coordinate system at k trajectory time points; rigid body exist The homogeneous transformation matrix at time step 1.

[0041] Step S33. Identity Assignment: Assign a unique identifier to each rigid body. Forming voxel indexes and rigid bodies mapping pairs Among them, rigid body of Represented as: , The total number of rigid bodies, This is a voxel index used for subsequent collision and conflict determination.

[0042] Step S4. Invalid detection pair filtering and parallel collision determination: Based on the kinematic topology, a mask matrix is ​​constructed to filter invalid detection pairs. The global voxel state is updated through GPU atomic operations, and collisions are directly determined by voxel index conflicts.

[0043] In this embodiment, the mask matrix is ​​constructed based on the robot's kinematic topology, due to adjacent rigid bodies (such as links). With connecting rod Because kinematic constraints always result in adjacent collisions, these collisions are considered expected contact rather than self-collisions and are therefore filtered out. Non-adjacent rigid bodies (such as link 1 and link 4) may penetrate under extreme attitudes and are therefore retained for detection. The specific implementation process is as follows:

[0044] Step S41. Mask Matrix Construction: Construction Self-collision logic mask matrix Filter adjacent rigid body pairs and rigid body pairs of itself:

[0045]

[0046] in, Indicates the link With connecting rod For effective detection. For example... Figure 3 As shown, Figure 3 This is a visualization of a 7×7 self-collision logic mask matrix. In the diagram, 0 indicates no detection is needed (false collision), and 1 indicates a valid detection pair. For non-adjacent rigid bodies, a valid detection target is the rigid body itself (i.e., a link). =Link If the two are adjacent rigid bodies, then it is a logical collision, i.e.: If the collision is impossible, then it falls under the category of physically impossible collision. The mask matrix for a 7-rigid-body robot (base and 6 links) is:

[0047]

[0048] Step S42. Global Voxel State Update: Construct a global voxel state table S, using a hash table to store the voxel's occupancy status (Free / Occupied) and the occupying rigid body. Parallel updates of voxel states are achieved through GPU atomic operations. If multiple threads write to the same voxel simultaneously, only the result of the first thread that writes is retained, while the remaining threads record conflict information.

[0049] Step S43. Parallel Collision Detection: Start GPU thread blocks, each thread block corresponding to a set of valid detection pairs ( The thread block traverses the conflict buffer, filtering for rigid bodies as well. and conflict voxel set Calculate the collision confidence:

[0050]

[0051] in, , They are connecting rods , The total number of prime numbers. When ( When the confidence threshold is reached, it is determined to be a self-collision.

[0052] Step S5. Iterative optimization and collision warning;

[0053] Repeat steps S3-S4 to perform parallel collision detection for each trajectory point on the robot's motion trajectory until all trajectory points have been traversed. Set a maximum number of iterations. The iteration stops and the detection ends when any of the following conditions are met;

[0054] 1) Collision confidence This triggers a self-collision alarm;

[0055] 2) Trajectory point traversal complete, no collision risk;

[0056] 3) The number of iterations has reached... .

[0057] The method described in this embodiment performs complete voxelization of each rigid body of the robot, including its internal volume, using a global voxelization strategy, rather than just surface voxelization. This transforms the collision detection problem from calculating the surface distance field to detecting whether voxel indices conflict, allowing any volume overlap to be directly captured, fundamentally eliminating the detection blind zone caused by sparse surface sampling.

[0058] The method described in this embodiment employs kinematic topology pre-filtering: a mask matrix is ​​constructed based on the robot's kinematic topology to pre-filter adjacent rigid body pairs and its own rigid body pairs. This filtering does not rely on spatial position calculations and can be reused throughout the entire motion process after being constructed once. For a 7-rigid-body robot, the mask matrix effectively reduces the number of detected pairs from 28 to 15, a reduction of 46.4%.

[0059] The method described in this embodiment employs parallel determination of voxel index collisions: a hash table is used to sparsely store the global voxel state, and each voxel record occupies a rigid body. .

[0060] The method described in this embodiment achieves multi-threaded concurrent voxel state updates through GPU atomic operations: when multiple threads write to the same voxel at the same time, only the result of the first thread is retained, and the remaining threads record conflict information. Finally, the collision confidence is calculated based on the number of conflicting voxels.

[0061] like Figure 4 and Figure 5 As shown in this embodiment, Figure 4 This is a simulation diagram of robot self-collision. Figure 5 This is a detailed diagram of the robot's self-collision area. In the diagram, the white area represents the collision voxel, the left side shows the robot's base, and the right side shows the robot's spray gun end effector.

[0062] Specific Implementation Method Two: This implementation method is an example of the voxel-based robot self-collision detection method described in Specific Implementation Method One. This example uses the CMA GR-680 robot as an example.

[0063] Step 1. Robot parameter calibration;

[0064] The DH parameters of the CMA GR-680 robot were obtained by measuring with SolidWorks and calibrating with the manufacturer's technical manual, as shown in Table 1.

[0065] Table 1

[0066]

[0067] Step 2. Constructing a voxelized virtual environment;

[0068] A voxel mesh of 4000mm×6000mm×6000mm was constructed with the center of the robot base as the origin. The voxel side length L=2.0mm. Sparse storage using a hash table was adopted, with a video memory usage of 480MB and an environment build time of 11.5ms.

[0069] Step 3. Dynamic voxelization of the robot rigid body;

[0070] The robot is decomposed into 7 rigid bodies. A set of voxel points is pre-generated in the local coordinate system. World coordinate mapping is achieved through formula (3), and each rigid body is assigned a voxel point. .

[0071] Step 4. Parallel collision detection;

[0072] Construct the mask matrix shown in formula (5) and filter 15 valid detection pairs. Start the GPU thread block to perform parallel detection on 100 extreme poses, calculate the collision confidence using formula (6), and determine the self-collision state.

[0073] Step 5. Result Verification;

[0074] The test results show that the method of the present invention has a self-collision prediction response time of 5.2ms to 5.8ms within the joint movement speed range of 120° / s to 180° / s, and the extreme attitude missed rate is only 0.3%, which meets the real-time and safety requirements of aerospace component spraying operations.

[0075] Specific Implementation Method 3: This implementation method is a comparative verification example of the voxel-based robot self-collision detection method described in Specific Implementation Method 2 and existing methods:

[0076] On the same hardware platform (NVIDIA RTX 3060), this invention was compared with the prior art CN114742944A (surface SDF method). The test used a CMA GR-680 robot, and 50 sets of extreme folding postures (adjacent link angle ≤ 15°) were selected: as shown in Table 2.

[0077] Table 2

[0078]

[0079] Experimental results show that the present invention eliminates the detection blind zone caused by sparse surface sampling through global voxelization, avoids SDF approximation error by directly determining voxel index conflict, and reduces the false negative rate by 85.7%.

[0080] The voxel-based robot self-collision detection method described in this invention can be integrated into robot offline programming software or real-time control systems, and is applicable to complex operation scenarios such as aerospace component painting, automobile manufacturing, and precision assembly. This invention has the advantages of high detection reliability, strong real-time performance, and good adaptability, and possesses significant industrial practical value.

[0081] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0082] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. A voxel-based robot self-collision detection method, characterized by: This method is implemented by the following steps: Step 1: Establish the robot's kinematic model; Step 2: Construct a voxelized virtual environment; With the center of the robot base as the origin of the world coordinate system, a voxel mesh environment in a Cartesian coordinate system is constructed. Step 3: Dynamic voxelization of the robot rigid body; Perform global voxelization on each rigid body of the robot and assign a unique identifier; specifically: Step 3: In the local coordinate systems of each rigid body of the robot, perform voxelization on the 3D model to generate local voxel point sets. ,in, rigid body The total number of voxels; Step 32: Based on the kinematic model described in Step 1, map the local voxels to the world coordinate system through homogeneous transformation; Step 33: Each rigid body is assigned a unique assignment. Forming voxel indexes and rigid bodies Mapping pairs; Step 4: Invalid detection for filtering and parallel collision determination; A mask matrix is ​​constructed based on kinematic topology to filter invalid detection pairs. The global voxel state is updated through GPU atomic operations, and collisions are directly determined based on voxel index conflicts. The specific process is as follows: Step 41: Construct the mask matrix. Self-collision logic mask matrix Filtering adjacent rigid body pairs and its own rigid body pairs can be expressed as follows: ； In the formula, Indicates the link With connecting rod For effective detection; Step 42: Global voxel state update; Construct a global voxel state table, and use a hash table to store the voxel occupancy status and occupancy rigid body. Parallel updates of voxel states are achieved through GPU atomic operations. If multiple threads write to the same voxel simultaneously, only the result of the first thread writing is retained, and the conflict information is recorded for the remaining threads. Step 43, Parallel Collision Detection: Start GPU thread blocks, each corresponding to a set of valid detection pairs; the thread blocks traverse the collision buffer, filtering for pairs that also include rigid bodies. and conflict voxel set And calculate the collision confidence, expressed as follows: ； In the formula, and They are connecting rods With connecting rod The total number of prime numbers; when When this occurs, it is determined to be a self-collision; The confidence threshold; Step 5: Repeat steps 3 and 4 to complete the robot's self-collision detection.

2. The voxel-based robot self-collision detection method according to claim 1, characterized in that: In step one, a kinematic model of the robot is established based on the DH parameter method, and the robot is defined as including... One joint, A link, define the link. With connecting rod DH parameters between: link length Linkage offset Linkage torsion angle and joint angle The connecting rod Relative to the link homogeneous transformation matrix for: ； Pose matrix of the spray gun end effector relative to the base coordinate system We obtain this through matrix multiplication: 。 3. The voxel-based robot self-collision detection method according to claim 1, characterized in that: In step two, a hash table sparse storage strategy is used to store the global voxel state and allocate memory for the voxels that occupy the voxels.

4. The voxel-based robot self-collision detection method according to claim 1, characterized in that: In step 3.2, the local voxel coordinates are... Mapping local voxels to the world coordinate system using homogeneous transformation: ； In the formula, For the k-th trajectory moment, , , These represent the x, y, and z coordinates of the local voxel in the world coordinate system at k trajectory times; rigid body exist The homogeneous transformation matrix at time step; , , These are the x-axis, y-axis, and z-axis coordinates of the local voxel in the local coordinate system.

5. The voxel-based robot self-collision detection method according to claim 1, characterized in that: In step five, the maximum number of iterations is set. The iteration stops when any of the following conditions are met; Collision confidence This triggers a self-collision alarm; Trajectory point traversal complete, no collision risk; The maximum number of iterations has been reached. .

Images