CN114918913A - Collision detection method and device, computer equipment and storage medium - Google Patents
Collision detection method and device, computer equipment and storage medium Download PDFInfo
- Publication number
- CN114918913A CN114918913A CN202210343304.2A CN202210343304A CN114918913A CN 114918913 A CN114918913 A CN 114918913A CN 202210343304 A CN202210343304 A CN 202210343304A CN 114918913 A CN114918913 A CN 114918913A
- Authority
- CN
- China
- Prior art keywords
- bounding
- robot
- bounding box
- collision
- 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.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 25
- 238000012360 testing method Methods 0.000 claims abstract description 65
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 49
- 230000004888 barrier function Effects 0.000 claims abstract description 42
- 238000000034 method Methods 0.000 claims abstract description 24
- 238000004590 computer program Methods 0.000 claims description 13
- 238000010586 diagram Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 2
- 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
- 238000000605 extraction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
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; performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining a minimum collision bounding box pair related to the robot and the obstacles; and determining the collision point of the robot and the collision point of the barrier by a triangular surface patch intersection test algorithm according to the triangularization relation between the center point of the minimum collision bounding box of the robot and the barrier and the point. The collision detection method can accurately determine the position of the obstacle to which part of the industrial robot the obstacle collides.
Description
Technical Field
The application relates to the technical field of industrial robots, in particular to a collision detection method, a collision detection device, computer equipment and a storage medium.
Background
With the development of the technology of the industrial robot, it has become a trend that the industrial robot replaces manual labor, the industrial robot needs to make a path planning from an initial position to a target position in the working process, and collision detection is an important part of the path planning. Through real-time quick collision detection, the barrier information in the scene can be acquired more timely, and the requirement of quick movement of the robot is met.
At present, based on the contour data of the obstacles acquired by laser and images in collision detection, whether the motion track of the industrial robot intersects with the contour of the obstacle or not is determined, so that whether collision occurs or not is determined. However, the current collision detection method cannot accurately determine where the 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 where an obstacle collides with that part of an industrial robot, in view of the above technical problems.
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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining a minimum collision bounding box pair related to the robot and the obstacles;
and determining the collision point of the robot and the collision point of the barrier by a triangular surface patch intersection test algorithm according to the triangularization relation between the center point of the minimum collision bounding box of the robot and the barrier and the point.
In one embodiment, before determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm based on a triangulated relationship between the center point of a minimum collision bounding box of the robot and the obstacle and the point, the method further comprises: and respectively adopting a triangulation algorithm for the obstacle point cloud and the robot point cloud to obtain a triangulation relationship between points of the obstacle and the points and a triangulation relationship between the points of 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: and acquiring a scene point cloud of the obstacle and a scene point cloud of the robot, and extracting the target area to obtain the obstacle point cloud and the robot point cloud.
In one embodiment, accelerating the computation of the root bounding box of the obstacle and the root bounding box of the robot using the GPU further comprises: and/or, accelerating generation of a hierarchical bounding tree using a GPU; and/or determining the collision point of the robot and the collision point of the obstacle by using a GPU accelerated triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to a spatial order to generate a hierarchical bounding tree further includes: calculating the Morton code value of the 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 the 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 the hierarchical bounding tree by taking the root bounding box as a root node, taking the child bounding box as a child node and taking the grandchild bounding box as a leaf node.
In one embodiment, the determining a minimum collision bounding box pair about the robot and the obstacle by performing an intersection test on the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot by using a tree traversal algorithm according to a projection relationship between a point and a plane in the bounding box further includes: selecting a root bounding box corresponding to a root node in a hierarchical bounding tree of the robot, performing intersection test with the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier according to the projection relation of points and planes, and determining whether the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot; if the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot, acquiring a bounding box corresponding to a child node in the hierarchical bounding tree of the barrier, carrying out intersection test on the bounding box corresponding to the child node in the hierarchical bounding tree of the barrier and the bounding box corresponding to the child node in the hierarchical bounding tree of the robot according to the projection relationship between the point and the surface, and determining a collision bounding box pair corresponding to the child node; acquiring bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the robot according to the collision bounding box pairs corresponding to the child nodes; carrying out intersection test on bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to the projection relation between the point and the plane, 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 method determines the collision point of the robot and the collision point of the obstacle by a triangular patch intersection test algorithm based on a triangularization relationship between the center point of the minimum collision bounding box of the robot and the obstacle and further comprises the following steps: taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular 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 patch corresponding to the minimum collision bounding box of the obstacle through a triangular 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 patch corresponding to the minimum collision bounding box of the robot, and performing intersection test on all points in the minimum collision bounding box of the obstacle and the triangular patch corresponding to the minimum collision bounding box of the robot through a triangular patch intersection test algorithm to determine the collision point of the obstacle.
A collision detection apparatus, the apparatus comprising:
the robot point cloud computing module is used for computing a robot point cloud and a barrier point cloud;
the hierarchical bounding tree generating module is used for 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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
the minimum collision bounding box pair determining module is used for performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining the minimum collision bounding box pair related to the robot and the obstacles;
and the collision point determining module is used for determining the collision point of the robot and the collision point of the barrier through a triangular surface patch intersection testing algorithm according to the triangularization relation between the center point of the minimum collision bounding box of the robot and the barrier and the point.
A computer device comprising a memory and a processor, the memory storing a computer program, the processor implementing the following steps when executing the computer program:
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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining a minimum collision bounding box pair related to the robot and the obstacles;
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 center point and the point of the minimum collision bounding box of the robot and the obstacle.
A computer-readable storage medium, on which a computer program is stored which, when executed by a processor, carries out 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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining a minimum collision bounding box pair related to the robot and the obstacles;
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 center point and the point of the minimum collision bounding box of the robot and 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 encoded according to the spatial sequence to generate the hierarchical bounding tree, the hierarchical bounding box of the obstacle and the hierarchical bounding box of the robot are subjected to intersection test by using a tree traversal algorithm, the minimum collision bounding box pair of the robot and the obstacle is determined according to the projection relation between the point and the surface in the bounding boxes, the triangularization relation between the point and the minimum collision bounding box pair of the robot and the obstacle is determined, the collision point of the robot and the collision point of the obstacle are determined through a triangular patch intersection test algorithm, and the position of the obstacle and the position of the industrial robot are accurately determined by the collision detection method.
Drawings
FIG. 1 is a flow diagram of collision detection in one embodiment;
FIG. 2 is a schematic diagram of an embodiment of a collision detection apparatus;
FIG. 3 is a diagram of a Morton code value cut through a bounding box, in one embodiment;
FIG. 4 is a diagram illustrating an internal structure of a computer device according to an embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of and not restrictive on the broad application.
In one embodiment, as shown in fig. 1, there is provided a collision detection method including the steps of:
and 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.
A point cloud is a data set of points in a coordinate system. The root bounding box of the computed obstacle and the root bounding box of the robot may be of the common directional bounding box types such as: AABB, OBB type.
And S120, coding the root bounding box according to a spatial sequence to generate a hierarchical bounding tree.
The hierarchical bounding tree is an object partitioning method, and is essentially a tree structure, and nodes of the hierarchical bounding tree are bounding boxes. The root bounding box is used as a root node of the hierarchical bounding tree, and bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree.
S130, performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between the points and the surfaces in the bounding boxes, and determining the minimum collision bounding box pair related to the robot and the obstacles.
The tree traversal algorithm is a sequencing 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 according to the triangularization relation between the center point and the minimum collision bounding box of the robot and the obstacle.
The intersection test algorithm of the triangular surface plate detects the intersection of line segments and processes the complete coincidence condition when the same plane is in the condition, the intersection of the line segments in the space is processed without relation to the intersection of the line segments on the plane, the three coordinate surfaces are projected, and the cross product is utilized to judge or the affine transformation of coordinates is carried out to replace the projection of the coordinate surface to be processed, namely the rotation of the plane. When the two sides are separated, the intersection point of the straight line and the plane is adopted to judge whether the intersection point is on the line segment and in the triangle.
In the collision detection method, the high-precision scene object point cloud is triangulated, and the method is applied to the geometric intersection test of the triangular surface patches of the potential collision pairs, so that the accuracy of the collision detection result is ensured.
In one embodiment, before determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm based on a triangulated relationship between the center point of a minimum collision bounding box of the robot and the obstacle and the point, the method further comprises: respectively adopting a triangulation algorithm for the obstacle point cloud and the robot point cloud, such as: the rolling sphere method, Delaunay triangulation algorithm and Larsen algorithm obtain the triangulation relationship between points of the obstacle and the triangulation relationship between the 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: and acquiring a scene point cloud of the obstacle and a scene point cloud of the robot, extracting a target area, and acquiring the obstacle point cloud and the robot point cloud, wherein the reachable area of the robot mechanical arm planning path is used as the target area.
In one embodiment, a GPU is used to accelerate the computation of the root bounding box of the obstacle and the root bounding box of the robot; and/or, accelerating the generation of the hierarchical bounding tree using the GPU; and/or determining the collision point of the robot and the collision point of the obstacle by using a GPU accelerated triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to a spatial order to generate a hierarchical bounding tree includes: calculating the Morton code value of the 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 the 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 the hierarchical bounding tree by taking the root bounding box as a root node, taking the child bounding boxes as child nodes and taking the grandchild bounding box as a leaf node. The morton coding method may be iterative cross calculation, as shown in fig. 3, the whole cube is a root bounding box, wherein the sub-bounding boxes are a1, a2, A3.. An-1, An, and the grandchild bounding boxes are B1, B2, B3.. a.. Bn-1, Bn.
In one embodiment, the determining a minimum collision bounding box pair about the robot and the obstacle by using a tree traversal algorithm to perform intersection test on the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot according to the projection relation of points in the bounding boxes and a plane comprises: selecting a root bounding box corresponding to a root node in a hierarchical bounding tree of the robot, performing intersection test with the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier according to the projection relation between points and a plane, wherein the points are orthographically projected to observe whether the points are intersected with the plane, and determining whether the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot; if the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot, acquiring a bounding box corresponding to a child node in the hierarchical bounding tree of the barrier, performing intersection test on the bounding box corresponding to the child node in the hierarchical bounding tree of the barrier and the bounding box corresponding to the child node in the hierarchical bounding tree of the robot according to the projection relation of the point and the plane, and determining a collision bounding box pair corresponding to the child node; acquiring bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the robot according to the collision bounding box pairs corresponding to the child nodes; carrying out intersection test on bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to the projection relation of points and planes, 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, determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm by triangulating the relation between the center point and the minimum collision bounding box of the robot and the obstacle, comprises: taking the minimum collision bounding box of the obstacle as a reference, acquiring a triangular 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 patch corresponding to the minimum collision bounding box of the obstacle through a triangular 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, and performing intersection test on all points in the minimum collision bounding box of the obstacle and the triangular surface patch corresponding to the minimum collision bounding box of the robot through a triangular surface patch intersection test algorithm 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 order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least a portion of the steps in fig. 1 may include multiple steps or multiple stages, which are not necessarily performed at the same time, but may be performed at different times, which are not necessarily performed in sequence, but may be performed in turn or alternately with other steps or at least a portion of the other steps or stages.
In one embodiment, as shown in fig. 2, there is provided a collision detecting apparatus including:
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 bounding tree generating module 220, configured to encode the root bounding box according to a spatial order to generate a hierarchical bounding tree; the root bounding box is used as a root node of the hierarchical bounding tree, and bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
a minimum collision bounding box pair determining module 230, configured to perform intersection test on the hierarchical bounding trees of the obstacle and the robot by using a tree traversal algorithm according to a projection relationship between a point and a plane in the bounding box, and determine a minimum collision bounding box pair between the robot and the obstacle;
and the collision point determining module 240 is configured to determine the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm based on a triangularization relationship between the center point of the minimum collision bounding box of the robot and the obstacle and the point.
In one embodiment, a collision detection apparatus further includes: and the triangulation relation calculation module is used for respectively adopting a triangulation algorithm for the obstacle point cloud and the robot point cloud to obtain a triangulation relation between points of the obstacle and the points and a triangulation relation between the points of the robot.
In one embodiment, a collision detection apparatus further includes: and the target area extraction module is used for acquiring the scene point cloud of the obstacle and the scene point cloud of the robot, extracting the target area and acquiring the obstacle point cloud and the robot point cloud.
In one embodiment, a GPU is used to accelerate the computation of the root bounding box of the obstacle and the root bounding box of the robot; and/or, accelerating the generation of the hierarchical bounding tree using the GPU; and/or determining the collision point of the robot and the collision point of the obstacle by using a GPU accelerated triangular patch intersection test algorithm.
In one embodiment, the encoding the root bounding box according to a spatial order to generate a hierarchical bounding tree includes: calculating the Morton code value of the 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 the 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 the hierarchical bounding tree by taking the root bounding box as a root node, taking the child bounding boxes as child nodes and taking the grandchild bounding box as a leaf node.
In one embodiment, the minimum collision bounding box pair determining module 230 includes:
the first intersection testing unit is used for selecting a root bounding box corresponding to a root node in the robot hierarchical bounding tree and a root bounding box corresponding to the root node in the barrier hierarchical bounding tree, performing intersection testing according to the projection relation of points and surfaces, and determining whether the root bounding box corresponding to the root node in the barrier hierarchical bounding tree is intersected with the root bounding box corresponding to the root node in the robot hierarchical bounding tree or not;
the second intersection test unit is used for acquiring a bounding box corresponding to a child node in the hierarchical bounding tree of the barrier if the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot, performing intersection test on the bounding box corresponding to the child node in the hierarchical bounding tree of the barrier and the bounding box corresponding to the child node in the hierarchical bounding tree of the robot according to the projection relationship between the point and the surface, 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 the leaf nodes in the hierarchical bounding tree of the barrier and bounding boxes corresponding to the leaf nodes in the hierarchical bounding tree of the robot according to the collision bounding box pairs corresponding to the child nodes;
the third intersection testing unit is used for carrying out intersection testing on bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to the projection relation of points and 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 determining module 240 includes:
the robot collision point determining unit is used for acquiring a triangular patch corresponding to the minimum collision bounding box of the obstacle by taking the minimum collision bounding box of the obstacle as a reference, and performing intersection tests on all points in the minimum collision bounding box of the robot and the triangular patch corresponding to the minimum collision bounding box of the obstacle through a triangular patch intersection test algorithm to determine the collision point of the robot;
and the barrier collision point determining unit is used for acquiring a triangular patch corresponding to the minimum collision bounding box of the robot by taking the minimum collision bounding box of the robot as a reference, and performing intersection test on all points in the minimum collision bounding box of the barrier and the triangular patch corresponding to the minimum collision bounding box of the robot through a triangular patch intersection test algorithm to determine the collision point of the barrier.
In one embodiment, a computer device is provided, which may be a server, and the internal structure thereof 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 comprises a nonvolatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of an operating system and computer programs in the non-volatile storage medium. The database of the computer device is used for storing 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 those skilled in the art that the configuration shown in fig. 4 is a block diagram of only a portion of the configuration associated with the present application, and is not intended to limit the computing device to which the present application may be applied, and that a particular computing device may include more or fewer components than shown, or may combine certain components, or have a different arrangement of components.
In one embodiment, a computer device is provided, which comprises a memory and a processor, the memory storing a computer program, and is characterized in that in one embodiment, the computer device further comprises a memory and a processor, the memory storing the computer program, and the processor implementing the steps of the method embodiments described above when executing the computer program.
In an embodiment, a computer-readable storage medium is provided, on which a computer program is stored which, when being executed by a processor, carries out the steps of the above-mentioned method embodiments.
All possible combinations of the technical features in the above embodiments may not be described for the sake of brevity, but should be considered as being within the scope of the present disclosure as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent application shall be subject to the appended claims.
Claims (10)
1. A method of collision detection, 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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining a minimum collision bounding box pair related to the robot and the obstacles;
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 center point and the point of the minimum collision bounding box of the robot and the obstacle.
2. The method of claim 1, wherein before determining the collision point of the robot and the collision point of the obstacle through a triangular patch intersection test algorithm based on a triangulated relationship between the point and the minimum collision bounding box center of the robot and the obstacle, comprising:
and respectively adopting a triangulation algorithm to the obstacle point cloud and the robot point cloud to obtain a triangulation relationship between points of the obstacle and a triangulation relationship between the points of the robot.
3. The method of claim 1, wherein prior to 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:
and acquiring a scene point cloud of the obstacle and a scene point cloud of the robot, extracting the target area, and acquiring the obstacle point cloud and the robot point cloud.
4. The method of claim 1, wherein the computation of the root bounding box of the obstacle and the root bounding box of the robot is accelerated using a GPU; and/or, accelerating the generation of the hierarchical bounding tree using the GPU; and/or determining the collision point of the robot and the collision point of the obstacle by using a GPU (graphics processing unit) accelerated triangular patch intersection test algorithm.
5. The method of claim 1, wherein the encoding the root bounding box according to spatial order to generate a hierarchical bounding tree comprises:
calculating the Morton code value of the 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 the 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 the hierarchical bounding tree by taking the root bounding box as a root node, taking the child bounding box as a child node and taking the grandchild bounding box as a leaf node.
6. The method of claim 1, wherein determining a minimum collision bounding box pair for the robot and the obstacle using a tree traversal algorithm to intersect the hierarchical bounding tree of the obstacle and the hierarchical bounding tree of the robot according to a projection relationship of points in the bounding box and the plane comprises:
selecting a root bounding box corresponding to a root node in a hierarchical bounding tree of the robot, performing intersection test with the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier according to the projection relation between the point and the plane, and determining whether the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot;
if the root bounding box corresponding to the root node in the hierarchical bounding tree of the barrier is intersected with the root bounding box corresponding to the root node in the hierarchical bounding tree of the robot, acquiring a bounding box corresponding to a child node in the hierarchical bounding tree of the barrier, carrying out intersection test on the bounding box corresponding to the child node in the hierarchical bounding tree of the barrier and the bounding box corresponding to the child node in the hierarchical bounding tree of the robot according to the projection relationship between the point and the surface, and determining a collision bounding box pair corresponding to the child node;
acquiring bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in a hierarchical bounding tree of the robot according to the collision bounding box pair corresponding to the child nodes;
carrying out intersection test on bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the barrier and bounding boxes corresponding to leaf nodes in the hierarchical bounding tree of the robot according to the projection relation between the point and the plane, 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.
7. The method of claim 1, wherein determining the collision point of the robot and the collision point of the obstacle by a triangular patch intersection test algorithm based on a triangulated relationship between the center point of the minimum collision bounding box of the robot and the obstacle and the point 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 patch corresponding to the minimum collision bounding box of the robot, and performing intersection test on all points in the minimum collision bounding box of the obstacle and the triangular patch corresponding to the minimum collision bounding box of the robot through a triangular patch intersection test algorithm to determine the collision point of the obstacle.
8. A collision detecting apparatus, characterized in that the apparatus comprises:
the robot point cloud computing module is used for computing a robot point cloud and a barrier point cloud;
the hierarchical bounding tree generating module is used for 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 bounding boxes formed by cutting the root bounding box are used as child nodes or leaf nodes of the hierarchical bounding tree;
the minimum collision bounding box pair determining module is used for performing intersection test on the hierarchical bounding trees of the obstacles and the hierarchical bounding trees of the robot by using a tree traversal algorithm according to the projection relation between points and surfaces in the bounding boxes, and determining the minimum collision bounding box pair related to the robot and the obstacles;
and the collision point determining module is used for determining the collision point of the robot and the collision point of the barrier through a triangular patch intersection test algorithm according to the triangularization relation between the minimum collision bounding box centering point of the robot and the barrier and the point.
9. A computer device comprising a memory and a processor, the memory storing a computer program, wherein the processor implements the steps of the method of any one of claims 1 to 7 when executing the computer program.
10. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the method of any one of claims 1 to 7.
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 true CN114918913A (en) | 2022-08-19 |
| CN114918913B 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 (7)
| 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 |
| US20210308862A1 (en) * | 2020-04-03 | 2021-10-07 | Fanuc Corporation | Fast robot motion optimization with distance field |
| CN114012726A (en) * | 2021-11-08 | 2022-02-08 | 南京航空航天大学 | Aerospace mechanical arm collision detection method |
-
2022
- 2022-03-31 CN CN202210343304.2A patent/CN114918913B/en active Active
Patent Citations (7)
| 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 |
| US20210308862A1 (en) * | 2020-04-03 | 2021-10-07 | Fanuc Corporation | Fast robot motion optimization with distance field |
| 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 |
|---|---|
| CN114918913B (en) | 2024-04-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111429574B (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| CN111275816B (en) | Method for acquiring point cloud data and related equipment | |
| CN103678754A (en) | Information processing apparatus and information processing method | |
| WO2011115143A1 (en) | Geometric feature extracting device, geometric feature extracting method, storage medium, three-dimensional measurement apparatus, and object recognition apparatus | |
| CN107622530B (en) | Efficient and robust triangulation network cutting method | |
| CN110900602B (en) | Positioning recovery method and device, robot and storage medium | |
| CN110433467B (en) | Operation method and device of table tennis ball picking robot based on binocular vision and ant colony algorithm | |
| Huber | Automatic 3D modeling using range images obtained from unknown viewpoints | |
| CN108564604B (en) | Binocular vision stereo matching method and device based on plane constraint and triangulation | |
| CN113211495A (en) | Mechanical arm collision detection method and system, storage medium and mechanical arm | |
| 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 | |
| CN104573144A (en) | System and method for simulating offline point cloud of measuring equipment | |
| CN114918913B (en) | Collision detection method, device, computer equipment and storage medium | |
| CN112308917A (en) | Vision-based mobile robot positioning method | |
| Birr et al. | Oriented surface reachability maps for robot placement | |
| Neves et al. | An environment to support structural testing of autonomous vehicles | |
| CN112711012B (en) | Global position initialization method and system of laser radar positioning system | |
| JP2001250122A (en) | Method for determining position and posture of body and program recording medium for the same | |
| JP3548652B2 (en) | Apparatus and method for restoring object shape | |
| Denker et al. | On-line reconstruction of CAD geometry | |
| CN112344966A (en) | Positioning failure detection method and device, storage medium and electronic equipment | |
| JPH10124677A (en) | Method for estimating attitude of object and device therefor |
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 |