A collision detection calculation method for intelligent painting robots
By mixing the enclosing body hierarchical tree and dynamic enclosing body hierarchical tree structure, combining LSS and OBB enclosing bodies, using OpenMP parallel calculation, the problem of poor speed and real-time performance of the traditional enclosing box collision detection algorithm is solved, and efficient collision detection is achieved, ensuring that The safety of robotic operations.
Patent Information
- Application Number
- CN202210646958.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-06-08
- Publication Date
- 2024-11-15
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
The traditional enclosing box collision detection algorithm cannot achieve ideal results, resulting in poor speed and real-time performance of collision detection, and cannot effectively avoid collisions in robots during operation, affecting the safety of workpieces and robots.
Adopt a hybrid enclosing body hierarchy tree structure, including building a hybrid enclosing body hierarchy tree and a dynamic enclosing body hierarchy tree of a single robot. By combining LSS and OBB enclosing bodies, hierarchy trees are built from bottom to top and top down, and using OpenMP parallelism The structure performs collision detection and calculation to realize parallel calculations among multiple robots.
It improves the accuracy and efficiency of collision detection, meets real-time requirements, avoids robot collisions, and ensures the safety of workpieces and robots.
Smart Images

Figure CN114936469B_ABST
Abstract
Description
Technical field:
[0001] The present invention relates to a robot collision detection algorithm, and in particular to a collision detection calculation method for an intelligent painting robot. Background technology:
[0002] With the rapid development of robot technology, the traditional online teaching method can no longer meet the working needs of robots, so the robot offline programming simulation system came into being. Because offline programming is different from online teaching, it cannot intuitively foresee the interference between the robot and the workpiece or tool. In order to avoid collisions between robots during operation, resulting in damage to the workpiece or robot or even threatening personal safety, collision detection is an indispensable part of robot simulation software. As an important topic in the research fields of computer graphics, system simulation, robotics, etc., collision detection has been studied by many scientific research institutions and scholars, and many mature algorithms have been obtained. With the continuous development of collision detection technology, some classic collision detection algorithms are also widely used, including the classic axial bounding box method AABB (Aligned AxisBounding Box), bounding sphere method (Spheres), oriented bounding box method OBB (Orient-ed Bounding Box), etc. These methods are suitable for objects with different geometric features, so the simplicity of detection is not very ideal, and the ideal effect cannot be obtained. The speed and real-time performance of collision detection are poor. Summary of the invention:
[0003] The purpose of the present invention is to propose a robot collision detection hybrid bounding volume algorithm according to the characteristics of robot spraying components, which solves the problems that the traditional bounding box collision detection algorithm cannot obtain the ideal effect and the speed and real-time performance of collision detection are poor.
[0004] The present invention is achieved by the following technical solutions:
[0005] A collision detection calculation method for an intelligent painting robot comprises the following steps:
[0006] 1. Construct a hybrid bounding volume hierarchy tree for a single robot
[0007] 1.1 Constructing the top-level multi-layer tree
[0008] The LSS robot link model is selected as the top layer of the hybrid bounding volume hierarchy tree and as the building primitive of the AABB bounding volume. The top layer multi-layer hierarchy tree is constructed in a bottom-up manner and a binary tree data structure. According to the above construction rules, a single robot hybrid bounding volume hierarchy tree based on the OpenGL development environment is formed.
[0009] 1.2 Constructing a multi-layer hierarchical tree in the middle layer
[0010] The connecting rod model of the painting robot is a rigid slender rod. The OBB bounding volume is used to establish the middle layer of the mixed bounding volume hierarchy tree. The number of OBB bounding volume hierarchy trees corresponds to the number of connecting rods. It is constructed in a top-down manner and a binary tree data structure. When the coordinates of the connecting rod change, the coordinates of the top OBB bounding volume in the middle layer hierarchy tree are updated, and its size and shape remain unchanged. There is no need to reconstruct the middle layer and the bottom layer of the middle layer hierarchy tree, and the middle layer hierarchy tree is quickly updated.
[0011] 1.3 Building the bottom layer
[0012] The bottom layer of the mixed bounding volume hierarchy tree is composed of the triangular facets enclosed by the OBB bounding volume that collides in the bottom layer of the middle layer hierarchy tree, wherein the coordinates of the triangular facets in the bottom layer of the middle layer hierarchy tree are in the base coordinate system of the robot, while the coordinates of the triangular facets in the bottom layer of the mixed bounding volume hierarchy tree are in the world coordinate system. After completing the collision detection of the middle layer, it needs to be established when and only when there is a collision in the bottom layer of the OBB bounding volume in the middle layer. When establishing, the vertex coordinates of the triangular facets enclosed by the OBB bounding volume that collides in the bottom layer of the middle layer hierarchy tree are converted to the world coordinate system respectively, and the triangular facets obtained after the conversion constitute the bottom layer of the mixed bounding volume hierarchy tree;
[0013] 2. Construct a dynamic bounding volume hierarchy tree between every two robots during collision detection;
[0014] The designed dynamic bounding volume hierarchy tree contains 3 layers of structure:
[0015] 2.1 Upper dynamic culling layer, first build the top-level hierarchical tree of each robot according to the top-level construction method of the mixed bounding volume hierarchical tree of a single robot, and then build it based on the link collision detection pair;
[0016] 2.2 Intermediate link layer: each link in the link collision detection pair that collides in the upper dynamic culling layer is re-surrounded by the OBB bounding volume. First, the OBB bounding volume hierarchy tree of each link is established according to the construction method of the intermediate layer hierarchy tree of the mixed bounding volume hierarchy tree of a single robot. Then, an intermediate link layer is constructed for each link collision detection pair, and its top layer is composed of the top OBB bounding volume of the OBB bounding volume hierarchy tree of the two links in the link collision detection pair. The intermediate link layer hierarchy tree is constructed downward according to the construction method of the upper dynamic culling layer.
[0017] 2.3 Bottom layer, first according to the single robot mixed bounding volume hierarchy tree, the bottom layer construction method will determine the triangle face pairs that need to be collided in the middle link layer for coordinate transformation, and then use these triangle face detection pairs to form the bottom layer of the dynamic hierarchy tree;
[0018] 3. Parallel implementation of collision detection
[0019] The scheme adopts three parallel structures of OpenMP, namely Omp Sections, Omp For and Omp Single, to design the dynamic bounding volume traversal tree process. By constructing a dynamic bounding volume hierarchical tree parallel structure, the parallel calculation of collision detection between multiple robots is realized. The specific workflow is as follows:
[0020] 1. Use Omp Sections to build a multi-robot dynamic bounding volume hierarchy tree;
[0021] 2. Use Omp For to perform dynamic culling layer AABB intersection detection;
[0022] 3. Use Omp Single to determine whether AABBs intersect;
[0023] If they intersect, the intersecting AABBs will generate the intermediate link layer OBB arrays;
[0024] If they do not intersect, the traversal ends;
[0025] 4. Use Omp For to detect the intersection of the intermediate connecting rod layer OBB to determine whether the OBB intersects;
[0026] If they do not intersect, the traversal ends;
[0027] If they intersect, determine whether the leaf node OBB intersects;
[0028] If they intersect, store them in the bottom triangle patch detection pair; if they do not intersect, index the array to generate the next layer of OBB intersection detection;
[0029] 5. Determine whether the number of OBB intersection detections in the next layer is 0;
[0030] If it is not 0, return to step 4 and use Omp For to perform the OBB intersection detection step of the middle connecting rod layer;
[0031] If the number of OBB intersection detections of the next layer is 0, check whether the number of triangle face pairs is 0; if the number of triangle face pairs is 0, the traversal ends; if the number of triangle face pairs is not 0, use Omp For to perform the bottom layer triangle face intersection detection;
[0032] 6. Determine whether the triangles intersect;
[0033] If the triangles intersect, store the intersecting triangles and the traversal ends;
[0034] If the triangles do not intersect, the variables for counting the number of intersections in the triangle detection and the calculation of converting the coordinates of the intersecting triangles into the world coordinate system are put into the Omp single structure for execution, and the thread that arrives first among the multiple threads executes it, and the traversal is completed.
[0035] Advantages of the present invention: In order to meet the accuracy requirements and improve the efficiency of collision detection, the present invention divides each robot model using a hybrid bounding volume hierarchy tree structure; in order to meet the real-time requirements, a dynamic bounding volume hierarchy tree is designed based on the hybrid bounding volume hierarchy tree structure, and on this basis, a parallel architecture is designed to realize parallel calculation of collision detection, thereby improving the efficiency of collision detection. Description of the drawings:
[0036] Figure 1 It is the top-level multi-layered hierarchical tree structure diagram of the present invention;
[0037] Figure 2 It is a top-level multi-layer AABB bounding volume hierarchy tree of a single robot of the present invention;
[0038] Figure 3 It is the middle layer multi-layer hierarchical tree structure diagram of the present invention, Figure 3 a is an even number of triangles, Figure 3 b is an odd number of triangles;
[0039] Figure 4 It is a flow chart of the parallel structure of the dynamic bounding volume hierarchical traversal tree of the present invention. Specific implementation method:
[0040] The present invention is a collision detection algorithm for intelligent painting robots. To meet the accuracy requirements and improve the efficiency of collision detection, each robot model is divided using a hybrid bounding volume hierarchy tree structure; to meet the real-time requirements, a dynamic bounding volume hierarchy tree is designed based on the hybrid bounding volume hierarchy tree structure, and on this basis, a parallel architecture is designed to realize parallel calculation of collision detection, so as to improve the efficiency of collision detection.
[0041] The specific steps are as follows:
[0042] 1. Construct a hybrid bounding volume hierarchy tree for a single robot
[0043] 1.1 Constructing the top-level multi-layer tree
[0044] Line swept sphere (LSS) has good compactness, low update cost, low AABB detection cost, and low AABB calculation based on LSS. Therefore, the top layer of the hybrid bounding volume hierarchy tree uses the LSS bounding robot link model and serves as the building element of the AABB bounding volume. The top-level multi-layer hierarchy tree is constructed in a bottom-up manner and a binary tree data structure. Figure 1 This is a top-level multi-layer tree structure diagram, which shows the structure of the top-level tree when the number of robot rigid bodies is 7, and it uses the AABB bounding volume as the top-level bounding volume.
[0045] according to Figure 1The construction rules of the single robot hybrid bounding volume hierarchy tree simulation effect diagram based on the OpenGL development environment is shown in the figure Figure 2 As shown, it is the top-level multi-layer AABB bounding volume hierarchy tree of a single robot. Figure 1 Each node Si-j is surrounded by an AABB with LSS as the primitive.
[0046] 1.2 Constructing a multi-layer hierarchical tree in the middle layer
[0047] Figure 3 It is a diagram of the structure of the middle-layer multi-layer hierarchical tree. The robot link model is a rigid and slender rod. At this time, it is more compact to enclose it with an OBB enclosing volume, so the OBB enclosing volume is used to establish the middle layer of the hybrid enclosing volume hierarchical tree. The number of OBB enclosing volume hierarchical trees corresponds to the number of links, which is constructed in a top-down manner and a binary tree data structure. When the coordinates of the link change, only the coordinates of the top-level OBB enclosing volume in the middle-layer hierarchical tree need to be updated, and its size and shape remain unchanged. There is no need to reconstruct the middle and bottom layers of the middle-layer hierarchical tree, so the middle-layer hierarchical tree can be quickly updated.
[0048] 1.3 Building the bottom layer
[0049] like Figure 3 As shown, the bottom layer of the hybrid bounding volume hierarchy tree is composed of the triangles enclosed by the OBB bounding volumes that collided in the bottom layer of the middle layer hierarchy tree, where the coordinates of the triangles in the bottom layer of the middle layer hierarchy tree are in the base coordinate system of the robot, while the coordinates of the triangles in the bottom layer of the hybrid bounding volume hierarchy tree are in the world coordinate system. Moreover, its construction process is quite special. It is established after the middle layer collision detection is completed, and only when there is a collision in the bottom layer of the OBB bounding volume in the middle layer. When establishing, the vertex coordinates of the triangles enclosed by the OBB bounding volume that collided in the bottom layer of the middle layer hierarchy tree are converted to the world coordinate system respectively, and the triangles obtained after the conversion constitute the bottom layer of the hybrid bounding volume hierarchy tree.
[0050] 2. Construct a dynamic bounding volume hierarchy tree between every two robots during collision detection
[0051] The designed dynamic bounding volume hierarchy tree contains 3 layers of structure:
[0052] 2.1 Construct the upper dynamic culling layer. First, construct the top-level hierarchical tree of each robot according to the construction method of the top level in the mixed bounding volume hierarchical tree of a single robot, and then construct the upper dynamic culling layer based on the link collision detection pair;
[0053] 2.2 Construct the intermediate link layer. Re-enclose each link in the link collision detection pair that collided in the upper dynamic culling layer with an OBB bounding volume (the OBB bounding volume is compact and easy to update). First, build the OBB bounding volume hierarchy tree for each link according to the construction method of the intermediate layer hierarchy tree of the mixed bounding volume hierarchy tree for a single robot; then construct an intermediate link layer for each link collision detection pair, the top layer of which is composed of the top OBB bounding volume of the OBB bounding volume hierarchy tree of the two links in the link collision detection pair, and then follow Figure 4 The construction method of the upper dynamic culling layer constructs the intermediate link layer hierarchy tree downward.
[0054] 2.3 Constructing the bottom layer. First, according to the bottom layer construction method of the mixed bounding volume hierarchy tree of a single robot, the coordinates of the triangular facet pairs that need to be detected in the middle link layer are transformed, and then these triangular facet detection pairs are used to form the bottom layer of the dynamic hierarchy tree.
[0055] 3. Parallel implementation of collision detection
[0056] The movement of multiple robots causes the positions of the robot links to change constantly, so the number of bounding volume intersection detections and the depth of the hierarchical tree traversal in the multi-robot collision detection process also change constantly. In order to improve the efficiency of collision detection, it is necessary to know the actual traversal process of the bounding volume hierarchical tree.
[0057] When establishing a bounding volume traversal hierarchical tree, the node traversal order in the hierarchical tree of the binary tree data structure type can be abstracted into a node bounding volume traversal order graph.
[0058] The scheme uses three parallel structures of OpenMP (OmpSections, OmpFor, OmpSingle) to design the dynamic bounding volume traversal tree process. By constructing a dynamic bounding volume hierarchical tree parallel structure, the parallel calculation of collision detection between multiple robots is realized, and the improvement of collision detection calculation efficiency is explored. The specific workflow is as follows Figure 4 As shown, Figure 4 The parallel structure flow chart of the dynamic bounding volume hierarchical traversal tree is as follows:
[0059] 1. Use Omp Sections to build a multi-level robot dynamic bounding volume hierarchy tree;
[0060] 2. Use Omp For to perform dynamic culling layer AABB intersection detection;
[0061] 3. Use Omp Single to determine whether AABBs intersect;
[0062] If they intersect, the intersecting AABBs will generate the intermediate link layer OBB arrays;
[0063] If they do not intersect, the traversal ends;
[0064] 4. Use Omp For to detect the intersection of the intermediate connecting rod layer OBB to determine whether the OBB intersects;
[0065] If they do not intersect, the traversal ends;
[0066] If they intersect, determine whether the leaf node OBB intersects;
[0067] If they intersect, store them in the bottom triangle patch detection pair; if they do not intersect, index the array to generate the next layer of OBB intersection detection;
[0068] 5. Determine whether the number of OBB intersection detections in the next layer is 0;
[0069] If it is not 0, return to step 4 and continue with the step of Omp For to perform the OBB intersection detection of the middle connecting rod layer;
[0070] If the number of OBB intersection detections of the next layer is 0, check whether the number of triangle face pairs is 0; if the number of triangle face pairs is 0, the traversal ends; if the number of triangle face pairs is not 0, use Omp For to perform the bottom layer triangle face intersection detection;
[0071] 6. Determine whether the triangles intersect;
[0072] If the triangles intersect, store the intersecting triangles and the traversal ends;
[0073] If the triangles do not intersect, the variables for counting the number of intersections in the triangle detection and the calculation of converting the coordinates of the intersecting triangles into the world coordinate system are put into the Omp single structure for execution, and the thread that arrives first among the multiple threads executes it, and the traversal is completed.
[0074] In the present invention, the upper dynamic culling layer mainly solves the AABB bounding volume update and AABB detection pair intersection detection; the calculation tasks in the middle link layer include converting the intersecting AABB bounding volumes in the upper dynamic culling layer into corresponding middle layer OBB bounding volume pairs, middle layer OBB bounding volume hierarchy tree node update calculation and OBB bounding volume pair intersection detection; the bottom layer calculation task is to perform triangle face detection pair intersection detection and store intersecting triangle facets.
[0075] In order to improve the thread granularity, the OBB bounding volume of the node in the intermediate link layer traversal tree is written into the array according to the level of expansion; the coordinate transformation in the OBB bounding volume coordinate system and the bounding volume intersection detection are all put into the parallel structure for execution; the thread task is allocated with the update, coordinate transformation and intersection detection of two child nodes as the allocation unit.
[0076] When calculating collision detection at the middle link layer, the triangle face detection pairs that need to be checked for intersection are stored in the bottom array; when the bottom layer is parallel, the for parallel structure is directly used to implement its parallel calculation. In order to avoid variable competition caused by multi-threaded operations on the same variable, the calculation of the intersection number variable in the triangle face detection pair and the conversion of the intersection triangle face coordinates to the world coordinate system are placed in the single structure for execution, and the first thread to arrive in the multi-threaded system will execute it.
Claims
1. A collision detection calculation method for an intelligent painting robot, characterized in that the calculation method comprises the following steps: 1) Construct a hybrid bounding volume hierarchy tree for a single robot 1.1) Construct the top-level multi-layer tree The LSS robot link model is selected as the top layer of the hybrid bounding volume hierarchy tree and as the building primitive of the AABB bounding volume. The top layer multi-layer hierarchy tree is constructed in a bottom-up manner and a binary tree data structure. According to the above construction rules, a single robot hybrid bounding volume hierarchy tree is formed. 1.2) Construct a multi-layer hierarchical tree in the middle layer The link model of the painting robot is a rigid slender rod. The OBB bounding volume is used to establish the middle layer of the hybrid bounding volume hierarchy tree. The number of OBB bounding volume hierarchy trees corresponds to the number of links. The OBB bounding volume hierarchy tree is constructed in a top-down manner and a binary tree data structure. 1.3) Build the bottom layer The vertex coordinates of the triangular facets enclosed by the OBB bounding volume that collides in the bottom layer of the intermediate layer hierarchy tree are converted to the world coordinate system respectively, and the triangular facets obtained after the conversion constitute the bottom layer of the mixed bounding volume hierarchy tree; 2) Construct a dynamic bounding volume hierarchy tree between every two robots during collision detection; 2.1) Construct the upper dynamic culling layer; First, build the top-level hierarchical tree of each robot according to the top-level construction method of the mixed bounding volume hierarchical tree of a single robot, and then build the upper dynamic culling layer based on the link collision detection pair; 2.2) Construct the middle connecting rod layer; Each link in the link collision detection pair that collides in the upper dynamic culling layer is resurrounded by an OBB bounding volume. First, the OBB bounding volume hierarchy tree of each link is established according to the construction method of the intermediate layer hierarchy tree of the mixed bounding volume hierarchy tree of a single robot. Then, an intermediate link layer is constructed for each link collision detection pair, and its top layer is composed of the top OBB bounding volume of the OBB bounding volume hierarchy tree of the two links in the link collision detection pair. The intermediate link layer hierarchy tree is constructed downward according to the construction method of the upper dynamic culling layer. 2.3) Build the bottom layer; First, according to the bottom layer construction method of the mixed bounding volume hierarchy tree of a single robot, the coordinates of the triangular facet pairs that need to be detected in the middle link layer are transformed, and then these triangular facet detection pairs are used to form the bottom layer of the dynamic hierarchy tree; 3) Parallel implementation of collision detection Three parallel structures of Open MP, Omp Sections, Omp For and Omp Single, are used to design the dynamic bounding volume hierarchy tree traversal process. By building a dynamic bounding volume hierarchy tree parallel structure, parallel computing of collision detection among multiple robots is realized.
2. The intelligent painting robot collision detection calculation method according to claim 1, characterized in that: In the step 1), when constructing the multi-layer hierarchical tree of the middle layer, when the connecting rod coordinates change, the coordinates of the top-level OBB bounding volume in the middle layer hierarchical tree are updated, and its size and shape remain unchanged. There is no need to reconstruct the middle layer and the bottom layer of the middle layer hierarchical tree, and the middle layer hierarchical tree is quickly updated.
3. The intelligent painting robot collision detection calculation method according to claim 1, characterized in that: The step 3) further comprises:
1. Use Omp Sections to build a multi-robot dynamic bounding volume hierarchy tree; 2. Use Omp For to perform dynamic culling layer AABB intersection detection; 3. Use Omp Single to determine whether AABBs intersect; If they intersect, the intersecting AABBs will generate the intermediate link layer OBB arrays; If they do not intersect, the traversal ends; 4. Use Omp For to detect the intersection of the intermediate connecting rod layer OBB to determine whether the OBB intersects; If they do not intersect, the traversal ends; If they intersect, determine whether the leaf node OBB intersects; If they intersect, store them in the bottom triangle patch detection pair; if they do not intersect, index the array to generate the next layer of OBB intersection detection; 5. Determine whether the number of OBB intersection detections in the next layer is 0; If it is not 0, return to step 4 and use Omp For to perform the OBB intersection detection step of the middle connecting rod layer; If the number of OBB intersection detections of the next layer is 0, check whether the number of triangle face pairs is 0; if the number of triangle face pairs is 0, the traversal ends; if the number of triangle face pairs is not 0, use Omp For to perform the bottom layer triangle face intersection detection; 6. Determine whether the triangles intersect; If the triangles intersect, store the intersecting triangles and the traversal ends; If the triangles do not intersect, the variables for counting the number of intersections in the triangle detection and the calculation of converting the coordinates of the intersecting triangles into the world coordinate system are put into the Omp single structure for execution, and the thread that arrives first among the multiple threads executes it, and the traversal is completed.
