CN114918913B - Collision detection method, device, computer equipment and storage medium - Google Patents
Collision detection method, device, computer equipment and storage medium Download PDFInfo
- Publication number
- CN114918913B CN114918913B CN202210343304.2A CN202210343304A CN114918913B CN 114918913 B CN114918913 B CN 114918913B CN 202210343304 A CN202210343304 A CN 202210343304A CN 114918913 B CN114918913 B CN 114918913B
- Authority
- CN
- China
- Prior art keywords
- bounding box
- obstacle
- robot
- bounding
- root
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 25
- 238000012360 testing method Methods 0.000 claims abstract description 57
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 47
- 238000000034 method Methods 0.000 claims abstract description 26
- 238000004590 computer program Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The application relates to a collision detection method, a collision detection device, a computer device and a storage medium. The method comprises the following steps: calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud; coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree; using a tree traversal algorithm to make intersection tests on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle; and determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle. By adopting the collision detection method, the position of the obstacle and the position of the industrial robot can be accurately determined.
Description
Technical Field
The present application relates to the technical field of industrial robots, and in particular, to a collision detection method, a collision detection device, a computer device, and a storage medium.
Background
With the development of industrial robot technology, industrial robots have become a trend to replace manual labor, and the industrial robots need to make a path planning from an initial position to a target position during operation, and collision detection is an important part of the path planning. Through real-time quick collision detection, obstacle information in a scene can be acquired more timely, and the requirement of quick movement of a robot is met.
Currently, based on collision detection, whether an industrial robot motion track intersects with the outline of an obstacle or not is determined through laser and image acquisition of outline data of the obstacle, so that whether collision occurs or not is determined. However, the current collision detection method cannot accurately determine where an obstacle collides with which part of the industrial robot.
Disclosure of Invention
In view of the above, it is necessary to provide a collision detection method capable of accurately determining at which position an obstacle collides with that part of an industrial robot.
A collision detection method, the method comprising:
Calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
Using a tree traversal algorithm to make intersection tests on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle;
And determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle.
In one embodiment, before the triangle relation between the midpoint and the point of the minimum collision bounding box of the robot and the obstacle is determined by the triangle patch intersection test algorithm, the method further comprises: and respectively adopting a triangulation algorithm to the obstacle point cloud and the robot point cloud to obtain the triangulation relation between points of the obstacle and the robot.
In one embodiment, before calculating the root bounding box of the obstacle and the root bounding box of the robot according to the obstacle point cloud and the robot point cloud, the method further comprises: the method comprises the steps of obtaining scene point clouds of obstacles and scene point clouds of robots, extracting target areas, and obtaining the obstacle point clouds and the robot point clouds.
In one embodiment, the root bounding box of the obstacle and the root bounding box of the robot are calculated using GPU acceleration, further comprising: and/or, generating a hierarchical surrounding tree using GPU acceleration; and/or determining collision points of the robot and the obstacle by using a GPU acceleration triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to the spatial order generates a hierarchical bounding tree, and further includes: calculating a Morton code value of coordinates of each point in the root bounding box in a Morton coding mode, and dividing the root bounding box into a plurality of sub bounding boxes according to the Morton code value; acquiring a Morton code value of each point in the child bounding box, and dividing the child bounding box into a plurality of grandchild bounding boxes according to the Morton code value; and generating a hierarchical bounding tree by taking the root bounding box as a root node, the child bounding box as a child node and the grandchild bounding box as a leaf node.
In one embodiment, the intersecting test is performed on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between the points and the surfaces in the surrounding boxes by using a tree traversal algorithm, and the determining the minimum collision surrounding box pair about the robot and the obstacle further includes: selecting a root bounding box corresponding to a root node in a robot hierarchical bounding tree, performing intersection test on the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree according to the projection relation of the point and the surface, and determining whether the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree is intersected with the root bounding box corresponding to the root node in the robot hierarchical bounding tree or not; if the root bounding box corresponding to the root node in the obstacle hierarchy bounding tree intersects with the root bounding box corresponding to the root node in the robot hierarchy bounding tree, acquiring a bounding box corresponding to a child node in the obstacle hierarchy bounding tree, intersecting the bounding box corresponding to the child node in the obstacle hierarchy bounding tree with the bounding box corresponding to the child node in the robot hierarchy bounding tree, and determining a collision bounding box pair corresponding to the child node according to the projection relation of the point and the plane; acquiring bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the obstacle and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to collision bounding box pairs corresponding to the child nodes; intersecting the bounding box corresponding to the leaf node in the hierarchical bounding tree of the obstacle with the bounding box corresponding to the leaf node in the hierarchical bounding tree of the robot, and determining a collision bounding box pair corresponding to the leaf node according to the projection relation of the point and the plane; the collision bounding box pair corresponding to the leaf node is the smallest collision bounding box pair related to the robot and the obstacle.
In one embodiment, the method includes the steps of triangulating the relationship between the midpoint and the point of the minimum collision bounding box of the robot and the obstacle, determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm, and further comprising: taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the obstacle, and performing intersection test on all points in the minimum collision bounding box of the robot and the triangular surface patch corresponding to the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm to determine collision points of the robot; and taking the minimum collision bounding box of the robot as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the robot, passing all points in the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm, and performing intersection test on the triangular surface patch corresponding to the minimum collision bounding box of the robot to determine the collision point of the obstacle.
A collision detection apparatus, the apparatus comprising:
the root bounding box calculation module is used for calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
the hierarchical surrounding tree generating module is used for encoding the root surrounding boxes according to the space sequence to generate a hierarchical surrounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
The minimum collision bounding box pair determining module is used for using a tree traversal algorithm to enable the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot to carry out intersection tests according to the projection relation between points and planes in the bounding box, and determining the minimum collision bounding box pair related to the robot and the obstacle;
And the collision point determining module is used for determining the collision point of the robot and the collision point of the obstacle through a triangular surface patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle.
A computer device comprising a memory storing a computer program and a processor which when executing the computer program performs the steps of:
Calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
Using a tree traversal algorithm to make intersection tests on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle;
And determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle.
A computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps of:
Calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
Using a tree traversal algorithm to make intersection tests on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle;
And determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle.
According to the collision detection method, the device, the computer equipment and the storage medium, the root bounding box of the obstacle and the root bounding box of the robot are calculated according to the obstacle point cloud and the robot point cloud, the root bounding boxes are coded according to the spatial sequence, the hierarchical bounding tree is generated, the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot are subjected to intersection tests according to the projection relation between points and surfaces in the bounding box, the minimum collision bounding box pair related to the robot and the obstacle is determined, the triangulated relation between the point and the point of the minimum collision bounding box pair of the robot and the obstacle is determined through a triangular patch intersection test algorithm, the collision point of the robot and the collision point of the obstacle are determined, and the position of the obstacle, which is collided with the part of the industrial robot, is precisely determined by the collision detection method.
Drawings
FIG. 1 is a flow chart of collision detection in one embodiment;
FIG. 2 is a schematic view of a collision detecting apparatus in one embodiment;
FIG. 3 is a schematic diagram of a Morton code value versus cut bounding box in one embodiment;
Fig. 4 is an internal structural diagram of a computer device in one embodiment.
Detailed Description
The present application will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present application more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.
In one embodiment, as shown in fig. 1, there is provided a collision detection method including the steps of:
s110, calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud.
Wherein the point cloud is a data set of points in a certain coordinate system. The root bounding box of the obstacle and the root bounding box of the robot can be calculated as common direction bounding box types such as: AABB, OBB type.
S120, encoding the root bounding box according to the spatial sequence to generate a hierarchical bounding tree.
Wherein, the hierarchical bounding tree is an object partition method, essentially a tree-shaped structure, and the nodes are bounding boxes. The root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree.
S130, using a tree traversal algorithm to enable the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot to be subjected to intersection tests according to projection relations of points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle.
The tree traversal algorithm is an order traversal queue structure, and the root node is firstly performed, and then the child node or the leaf node is circularly calculated.
And S140, determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm by triangulating the relationship between the minimum collision bounding box pair midpoint and the point of the robot and the obstacle.
The triangular surface patch intersection test algorithm is used for detecting line segment intersection and processing of complete superposition when the same plane is adopted, the line segment intersection processing in space is irrelevant to line segment intersection processing on the plane, three coordinate plane projections are made, and cross product is used for judging or affine transformation of coordinates is used for replacing projection of the coordinate plane to be made, namely plane rotation. When the two sides are separated, the intersection point of the straight line and the plane is adopted, and whether the intersection point is on the line segment or in the triangle is judged.
In the collision detection method, the accuracy of the collision detection result is ensured by triangulating the object point cloud of the high-precision scene and applying the method to the geometric intersection test of the triangular patches of the potential collision pair.
In one embodiment, before the triangle relation between the midpoint and the point of the minimum collision bounding box of the robot and the obstacle is determined by the triangle patch intersection test algorithm, the method further comprises: and respectively adopting a triangulation algorithm to the obstacle point cloud and the robot point cloud, such as: the method comprises the steps of a rolling ball method, a delusian triangulation algorithm and a Lawsen algorithm, and obtaining the triangulation relation between points of the obstacle and the triangulation relation between points of the robot.
In one embodiment, before computing the root bounding box of the obstacle and the root bounding box of the robot from the obstacle point cloud and the robot point cloud: the method comprises the steps of obtaining scene point clouds of obstacles and scene point clouds of robots, extracting target areas, and obtaining the obstacle point clouds and the robot point clouds, wherein a robot arm planning path reachable area serves as the target area.
In one embodiment, the root bounding box of the obstacle and the root bounding box of the robot are computed using GPU acceleration; and/or, generating a hierarchical surrounding tree using GPU acceleration; and/or determining collision points of the robot and the obstacle by using a GPU acceleration triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to a spatial order generates a hierarchical bounding tree, including: calculating a Morton code value of coordinates of each point in the root bounding box in a Morton coding mode, and dividing the root bounding box into a plurality of sub bounding boxes according to the Morton code value; acquiring a Morton code value of each point in the child bounding box, and dividing the child bounding box into a plurality of grandchild bounding boxes according to the Morton code value; and generating a hierarchical bounding tree by taking the root bounding box as a root node, the child bounding box as a child node and the grandchild bounding box as a leaf node. The Morton encoding mode may be iterative cross computation, as shown in FIG. 3, the whole cube is A root bounding box, wherein child bounding boxes are A1, A2, A3 … An-1, an, and grandchild bounding boxes are B1, B2, B3 … … Bn-1, bn.
In one embodiment, the intersecting test of the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation of the points and the planes in the surrounding boxes is performed by using a tree traversal algorithm, and the determining of the minimum collision surrounding box pair about the robot and the obstacle includes: selecting a root bounding box corresponding to a root node in a robot hierarchical bounding tree, performing intersection test on the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree according to the projection relation between a point and a plane, performing orthographic projection on the point to observe whether the point intersects the plane, and determining whether the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree intersects the root bounding box corresponding to the root node in the robot hierarchical bounding tree; if the root bounding box corresponding to the root node in the obstacle hierarchy bounding tree intersects with the root bounding box corresponding to the root node in the robot hierarchy bounding tree, acquiring a bounding box corresponding to a child node in the obstacle hierarchy bounding tree, intersecting the bounding box corresponding to the child node in the obstacle hierarchy bounding tree with the bounding box corresponding to the child node in the robot hierarchy bounding tree, and determining a collision bounding box pair corresponding to the child node according to the projection relation of the point and the plane; acquiring bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the obstacle and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to collision bounding box pairs corresponding to the child nodes; intersecting the bounding box corresponding to the leaf node in the hierarchical bounding tree of the obstacle with the bounding box corresponding to the leaf node in the hierarchical bounding tree of the robot, and determining a collision bounding box pair corresponding to the leaf node according to the projection relation of the point and the plane; the collision bounding box pair corresponding to the leaf node is the smallest collision bounding box pair related to the robot and the obstacle.
In one embodiment, the method for determining the collision point of the robot and the collision point of the obstacle by the triangular patch intersection test algorithm based on the triangularization relation between the points and the minimum collision bounding box of the robot and the obstacle comprises the following steps: taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the obstacle, and performing intersection test on all points in the minimum collision bounding box of the robot and the triangular surface patch corresponding to the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm to determine collision points of the robot; and taking the minimum collision bounding box of the robot as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the robot, passing all points in the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm, and performing intersection test on the triangular surface patch corresponding to the minimum collision bounding box of the robot to determine the collision point of the obstacle.
It should be understood that, although the steps in the flowchart of fig. 1 are shown in sequence as indicated by the arrows, the steps are not necessarily performed in sequence as indicated by the arrows. The steps are not strictly limited to the order of execution unless explicitly recited herein, and the steps may be executed in other orders. Moreover, at least a portion of the steps in fig. 1 may include a plurality of steps or stages, which are not necessarily performed at the same time, but may be performed at different times, and the order of the steps or stages is not necessarily sequential, but may be performed in rotation or alternatively with at least a portion of the steps or stages in other steps or other steps.
In one embodiment, as shown in fig. 2, there is provided a collision detecting apparatus, the apparatus comprising:
A root bounding box calculation module 210, configured to calculate a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
A hierarchical surrounding tree generating module 220, configured to encode the root bounding box according to a spatial order, and generate a hierarchical surrounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
A minimum collision bounding box pair determining module 230, configured to use a tree traversal algorithm to intersect the hierarchical bounding tree of the obstacle with the hierarchical bounding tree of the robot, and determine a minimum collision bounding box pair related to the robot and the obstacle according to a projection relationship between points and surfaces in the bounding box;
The collision point determining module 240 is configured to determine a collision point of the robot and a collision point of the obstacle by using a triangle patch intersection test algorithm based on a triangle relationship between a point and a midpoint of a minimum collision bounding box of the robot and the obstacle.
In one embodiment, a collision detection apparatus further includes: and the triangulation relation calculation module is used for respectively adopting a triangulation algorithm to the obstacle point cloud and the robot point cloud to obtain the triangulation relation among points related to the obstacle and the robot point-to-point triangulation relation.
In one embodiment, a collision detection apparatus further includes: the target area extraction module is used for acquiring scene point clouds of the obstacle and scene point clouds of the robot, extracting the target area and acquiring the obstacle point clouds and the robot point clouds.
In one embodiment, the root bounding box of the obstacle and the root bounding box of the robot are computed using GPU acceleration; and/or, generating a hierarchical surrounding tree using GPU acceleration; and/or determining collision points of the robot and the obstacle by using a GPU acceleration triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to a spatial order generates a hierarchical bounding tree, including: calculating a Morton code value of coordinates of each point in the root bounding box in a Morton coding mode, and dividing the root bounding box into a plurality of sub bounding boxes according to the Morton code value; acquiring a Morton code value of each point in the child bounding box, and dividing the child bounding box into a plurality of grandchild bounding boxes according to the Morton code value; and generating a hierarchical bounding tree by taking the root bounding box as a root node, the child bounding box as a child node and the grandchild bounding box as a leaf node.
In one embodiment, the minimum collision bounding box pair determining module 230 includes:
The first intersection test unit is used for selecting a root bounding box corresponding to a root node in the robot hierarchical bounding tree, performing intersection test according to the projection relation of the root and the surface, and determining whether the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree is intersected with the root bounding box corresponding to the root node in the robot hierarchical bounding tree;
The second intersection test unit is used for acquiring the bounding box corresponding to the child node in the obstacle layer bounding tree and the bounding box corresponding to the child node in the robot layer bounding tree if the root bounding box corresponding to the root node in the obstacle layer bounding tree is intersected with the root bounding box corresponding to the root node in the robot layer bounding tree, performing intersection test on the bounding box corresponding to the child node in the obstacle layer bounding tree and the bounding box corresponding to the child node in the robot layer bounding tree according to the projection relation of the point and the plane, and determining a collision bounding box pair corresponding to the child node;
the leaf node bounding box acquisition unit is used for acquiring bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the obstacle and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to collision bounding box pairs corresponding to the child nodes;
the third intersection test unit is used for performing intersection test on the bounding boxes corresponding to the leaf nodes in the hierarchical bounding tree of the obstacle and the bounding boxes corresponding to the leaf nodes in the hierarchical bounding tree of the robot according to the projection relation of the points and the surfaces, and determining collision bounding box pairs corresponding to the leaf nodes; the collision bounding box pair corresponding to the leaf node is the smallest collision bounding box pair related to the robot and the obstacle.
In one embodiment, the collision point determination module 240 includes:
The robot collision point determining unit is used for taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the obstacle, intersecting all points in the minimum collision bounding box of the robot with the triangular surface patch corresponding to the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm, and determining the collision point of the robot;
the obstacle collision point determining unit is used for taking the minimum collision bounding box of the robot as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the robot, enabling all points in the minimum collision bounding box of the obstacle to pass through a triangular surface patch intersection test algorithm, and performing intersection test on the triangular surface patch corresponding to the minimum collision bounding box of the robot to determine the collision point of the obstacle.
In one embodiment, a computer device is provided, which may be a server, the internal structure of which may be as shown in fig. 4. The computer device includes a processor, a memory, and a network interface connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The database of the computer device is used to store the collision results. The network interface of the computer device is used for communicating with an external terminal through a network connection. The computer program is executed by a processor to implement a collision detection method.
It will be appreciated by persons skilled in the art that the architecture shown in fig. 4 is merely a block diagram of some of the architecture relevant to the present inventive arrangements and is not limiting as to the computer device to which the present inventive arrangements are applicable, and that a particular computer device may include more or fewer components than shown, or may combine some of the components, or have a different arrangement of components.
In an embodiment a computer device is provided comprising a memory and a processor, said memory storing a computer program, characterized in that in an embodiment a computer device is provided comprising a memory and a processor, the memory storing a computer program, the processor implementing the steps of the method embodiments described above when executing the computer program.
In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored which, when executed by a processor, carries out the steps of the method embodiments described above.
The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples illustrate only a few embodiments of the application, which are described in detail and are not to be construed as limiting the scope of the application. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the application, which are all within the scope of the application. Accordingly, the scope of protection of the present application is to be determined by the appended claims.
Claims (9)
1. A collision detection method, the method comprising:
Calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
Using a tree traversal algorithm to make intersection tests on the hierarchical surrounding tree of the obstacle and the hierarchical surrounding tree of the robot according to the projection relation between points and planes in the surrounding boxes, and determining the minimum collision surrounding box pair related to the robot and the obstacle;
the method comprises the steps of determining the collision points of a robot and an obstacle through a triangular relation between the center point and the point of a minimum collision bounding box of the robot and the obstacle and a triangular patch intersection test algorithm;
the method for determining the minimum collision bounding box pair of the robot and the obstacle comprises the following steps of:
Selecting a root bounding box corresponding to a root node in a robot hierarchical bounding tree, performing intersection test on the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree according to the projection relation of the point and the surface, and determining whether the root bounding box corresponding to the root node in the obstacle hierarchical bounding tree is intersected with the root bounding box corresponding to the root node in the robot hierarchical bounding tree or not;
If the root bounding box corresponding to the root node in the obstacle hierarchy bounding tree intersects with the root bounding box corresponding to the root node in the robot hierarchy bounding tree, acquiring a bounding box corresponding to a child node in the obstacle hierarchy bounding tree, intersecting the bounding box corresponding to the child node in the obstacle hierarchy bounding tree with the bounding box corresponding to the child node in the robot hierarchy bounding tree, and determining a collision bounding box pair corresponding to the child node according to the projection relation of the point and the plane;
Acquiring bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the obstacle and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to collision bounding box pairs corresponding to the child nodes;
Intersecting the bounding box corresponding to the leaf node in the hierarchical bounding tree of the obstacle with the bounding box corresponding to the leaf node in the hierarchical bounding tree of the robot, and determining a collision bounding box pair corresponding to the leaf node according to the projection relation of the point and the plane; the collision bounding box pair corresponding to the leaf node is the smallest collision bounding box pair related to the robot and the obstacle.
2. The method of claim 1, wherein prior to triangulating the robot to the minimum collision bounding box with the obstacle to the midpoint and point, determining the collision point of the robot and the collision point of the obstacle by a triangular patch intersection test algorithm, comprising:
and respectively adopting a triangulation algorithm to the obstacle point cloud and the robot point cloud to obtain the triangulation relation between points of the obstacle and the robot.
3. The method of claim 1, wherein before computing a root bounding box of the obstacle and a root bounding box of the robot from the obstacle point cloud and the robot point cloud:
The method comprises the steps of obtaining scene point clouds of obstacles and scene point clouds of robots, extracting target areas, and obtaining the obstacle point clouds and the robot point clouds.
4. The method of claim 1, wherein the computing of the root bounding box of the obstacle and the root bounding box of the robot is accelerated using a GPU; and/or, generating a hierarchical surrounding tree using GPU acceleration; and/or determining collision points of the robot and the obstacle by using a GPU acceleration triangular patch intersection test algorithm.
5. The method of claim 1, wherein said encoding the root bounding box according to a spatial order generates a hierarchical bounding tree, comprising:
calculating a Morton code value of coordinates of each point in the root bounding box in a Morton coding mode, and dividing the root bounding box into a plurality of sub bounding boxes according to the Morton code value;
Acquiring a Morton code value of each point in the child bounding box, and dividing the child bounding box into a plurality of grandchild bounding boxes according to the Morton code value;
And generating a hierarchical bounding tree by taking the root bounding box as a root node, the child bounding box as a child node and the grandchild bounding box as a leaf node.
6. The method of claim 1, wherein triangulating the point-to-point relationship between the minimum collision bounding box of the robot and the obstacle, determining the collision point of the robot and the collision point of the obstacle by a triangular patch intersection test algorithm, comprises:
Taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the obstacle, and performing intersection test on all points in the minimum collision bounding box of the robot and the triangular surface patch corresponding to the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm to determine collision points of the robot;
And taking the minimum collision bounding box of the robot as a reference, acquiring a triangular surface patch corresponding to the minimum collision bounding box of the robot, passing all points in the minimum collision bounding box of the obstacle through a triangular surface patch intersection test algorithm, and performing intersection test on the triangular surface patch corresponding to the minimum collision bounding box of the robot to determine the collision point of the obstacle.
7. A collision detection apparatus, characterized in that the apparatus comprises:
the root bounding box calculation module is used for calculating a root bounding box of the obstacle and a root bounding box of the robot according to the obstacle point cloud and the robot point cloud;
the hierarchical surrounding tree generating module is used for encoding the root surrounding boxes according to the space sequence to generate a hierarchical surrounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and a bounding box cut by the root bounding box is used as a child node or a leaf node of the hierarchical bounding tree;
The minimum collision bounding box pair determining module is used for using a tree traversal algorithm to enable the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot to carry out intersection tests according to the projection relation between points and planes in the bounding box, and determining the minimum collision bounding box pair related to the robot and the obstacle;
And the collision point determining module is used for determining the collision point of the robot and the collision point of the obstacle through a triangular surface patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point and the point of the robot and the minimum collision bounding box of the obstacle.
8. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor implements the steps of the method of any of claims 1 to 6 when the computer program is executed.
9. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the steps of the method of any of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210343304.2A CN114918913B (en) | 2022-03-31 | 2022-03-31 | Collision detection method, device, computer equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210343304.2A CN114918913B (en) | 2022-03-31 | 2022-03-31 | Collision detection method, device, computer equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114918913A CN114918913A (en) | 2022-08-19 |
| CN114918913B true CN114918913B (en) | 2024-04-19 |
Family
ID=82805661
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210343304.2A Active CN114918913B (en) | 2022-03-31 | 2022-03-31 | Collision detection method, device, computer equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114918913B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101852609A (en) * | 2010-06-02 | 2010-10-06 | 北京理工大学 | Ground obstacle detection method based on binocular stereo vision of robot |
| CN101866386A (en) * | 2010-06-25 | 2010-10-20 | 杭州维肖软件科技有限公司 | Flexible body bump processing method based on energy balance |
| CN110515358A (en) * | 2019-08-26 | 2019-11-29 | 杭州芯控智能科技有限公司 | Automatic production line concurrent control system and control method based on integrated controller |
| CN111652908A (en) * | 2020-04-17 | 2020-09-11 | 国网山西省电力公司晋中供电公司 | Operation collision detection method for virtual reality scene |
| CN113211495A (en) * | 2021-04-12 | 2021-08-06 | 北京航天飞行控制中心 | Mechanical arm collision detection method and system, storage medium and mechanical arm |
| CN114012726A (en) * | 2021-11-08 | 2022-02-08 | 南京航空航天大学 | Aerospace mechanical arm collision detection method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11724387B2 (en) * | 2020-04-03 | 2023-08-15 | Fanuc Corporation | Fast robot motion optimization with distance field |
-
2022
- 2022-03-31 CN CN202210343304.2A patent/CN114918913B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101852609A (en) * | 2010-06-02 | 2010-10-06 | 北京理工大学 | Ground obstacle detection method based on binocular stereo vision of robot |
| CN101866386A (en) * | 2010-06-25 | 2010-10-20 | 杭州维肖软件科技有限公司 | Flexible body bump processing method based on energy balance |
| CN110515358A (en) * | 2019-08-26 | 2019-11-29 | 杭州芯控智能科技有限公司 | Automatic production line concurrent control system and control method based on integrated controller |
| CN111652908A (en) * | 2020-04-17 | 2020-09-11 | 国网山西省电力公司晋中供电公司 | Operation collision detection method for virtual reality scene |
| CN113211495A (en) * | 2021-04-12 | 2021-08-06 | 北京航天飞行控制中心 | Mechanical arm collision detection method and system, storage medium and mechanical arm |
| CN114012726A (en) * | 2021-11-08 | 2022-02-08 | 南京航空航天大学 | Aerospace mechanical arm collision detection method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114918913A (en) | 2022-08-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109509210B (en) | Obstacle tracking method and device | |
| US11300964B2 (en) | Method and system for updating occupancy map for a robotic system | |
| CN110501017A (en) | A kind of Mobile Robotics Navigation based on ORB_SLAM2 ground drawing generating method | |
| JP7300550B2 (en) | METHOD AND APPARATUS FOR CONSTRUCTING SIGNS MAP BASED ON VISUAL SIGNS | |
| CN112651944B (en) | 3C component high-precision six-dimensional pose estimation method and system based on CAD model | |
| CN111415420B (en) | Spatial information determining method and device and electronic equipment | |
| Kallmann | Navigation queries from triangular meshes | |
| CN115793647A (en) | Robot obstacle avoidance path planning method, system and medium | |
| Wu et al. | A survey on monocular 3D object detection algorithms based on deep learning | |
| CN113160401B (en) | Object-oriented visual SLAM lightweight semantic map creation method | |
| Dietrich et al. | Probabilistic multi-sensor fusion based on signed distance functions | |
| CN114918913B (en) | Collision detection method, device, computer equipment and storage medium | |
| Birr et al. | Oriented surface reachability maps for robot placement | |
| Neves et al. | An environment to support structural testing of autonomous vehicles | |
| Wiemann et al. | An evaluation of open source surface reconstruction software for robotic applications | |
| Oliveira et al. | Incremental scenario representations for autonomous driving using geometric polygonal primitives | |
| JP2001250122A (en) | Method for determining position and posture of body and program recording medium for the same | |
| CN114398455A (en) | Heterogeneous multi-robot cooperative SLAM map fusion method | |
| Berseth et al. | ACCLMesh: Curvature-based navigation mesh generation | |
| JP3548652B2 (en) | Apparatus and method for restoring object shape | |
| JP3739209B2 (en) | Automatic polygon generation system from point cloud | |
| Zhang et al. | Haptic interaction with a polygon mesh reconstructed from images | |
| Wietrzykowski | On the representation of planes for efficient graph-based SLAM with high-level features | |
| An et al. | Tracking an RGB-D camera on mobile devices using an improved frame-to-frame pose estimation method | |
| Steffen | Visual SLAM from image sequences acquired by unmanned aerial vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |