CN108959753B - Collision detection method, system, readable storage medium and computer device - Google Patents
Collision detection method, system, readable storage medium and computer device Download PDFInfo
- Publication number
- CN108959753B CN108959753B CN201810670546.6A CN201810670546A CN108959753B CN 108959753 B CN108959753 B CN 108959753B CN 201810670546 A CN201810670546 A CN 201810670546A CN 108959753 B CN108959753 B CN 108959753B
- Authority
- CN
- China
- Prior art keywords
- model
- obstacle
- claw
- point
- judging
- 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 63
- 210000000078 claw Anatomy 0.000 claims abstract description 169
- 238000000034 method Methods 0.000 claims abstract description 22
- 239000013598 vector Substances 0.000 claims description 51
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Numerical Control (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention provides a collision detection method, a system, a readable storage medium and computer equipment, wherein the method comprises the following steps: respectively establishing an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw; according to the parameters of each joint of the mechanical arm, calculating the running track of the claw in the preset reference plane; and when the claw model contacts with the obstacle model at any position point of the moving track, judging that the claw collides with the obstacle. According to the collision detection method, the system, the readable storage medium and the computer equipment, whether collision occurs or not is finally determined by reasonably simplifying the object model and the detection track and adopting a simple graph contact judgment mode, the whole detection process is simple and easy, the calculated amount is small, and the collision detection efficiency is greatly improved.
Description
Technical Field
The present invention relates to the field of mechanical arms, and in particular, to a collision detection method, a collision detection system, a readable storage medium, and a computer device.
Background
With the continuous development of mechanical manufacturing technology and automation technology, various mechanical arms (such as a SCARA four-axis mechanical arm) are produced, the mechanical arm generally consists of a plurality of joints and clamping jaws arranged at the tail end, and the joints are mutually matched to change the pose of the clamping jaws, so that the special tasks (such as transferring products) are completed.
Before the delivery of the mechanical arm or during the research and development process, the mechanical arm is generally required to be subjected to collision detection, that is, whether the claw collides with an obstacle in the environment under a certain pose needs to be judged, so as to plan the movement path of the mechanical arm. The collision detection method is a key technology for ensuring that the mechanical arm finds a collision-free path from a starting point to an end point, and the advantages and disadvantages of the collision detection method directly determine the effect of motion planning.
In the prior art, the collision detection methods adopted by the mechanical arm at present are divided into two methods, one method is to directly call the existing collision detection library, the library integrates too much content to influence the overall collision detection algorithm efficiency, the other method is to sample on each axis of the mechanical arm at a certain density and judge whether each sampling point collides with other points and obstacles, and the method is time-consuming and inaccurate.
Disclosure of Invention
Based on the foregoing, an object of the present invention is to provide a collision detection method, a system, a readable storage medium and a computer device, so as to improve the efficiency of collision detection of a mechanical arm.
According to an embodiment of the invention, a collision detection method includes:
respectively establishing an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw;
according to the parameters of each joint of the mechanical arm, calculating the running track of the claw in the preset reference plane;
and when the claw model contacts with the obstacle model at any position point of the moving track, judging that the claw collides with the obstacle.
In addition, a collision detection method according to the above embodiment of the present invention may further have the following additional technical features:
further, the obstacle model and the jaw model are both in a circular structure, and the step of judging that the obstacle model and the jaw model are in contact comprises the following steps:
respectively calculating the radiuses of the obstacle model and the claw model, and calculating the sum of the radiuses of the obstacle model and the claw model to obtain a reference total radius;
Judging whether the center distance between the obstacle model and the claw model is smaller than the reference total radius;
if yes, judging that the obstacle model and the claw model are in contact.
Further, the claw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
calculating the reference radius of the claw model, and respectively calculating the point edge distance from the center point of the claw model to each edge line of the obstacle model;
judging whether the point margin smaller than the reference radius exists or not;
if yes, judging that the obstacle model and the claw model are in contact.
Further, the step of calculating a point margin from the center point to the obstacle model margin includes:
acquiring coordinates of the center point, and acquiring the length of any target side line of the barrier model and coordinates of two end points;
taking any target endpoint of the target edge as a base point, establishing a first vector from the target endpoint to the other endpoint of the target edge, and establishing a second vector from the target endpoint to the center point;
Calculating a vector dot product of the first vector and the second vector, and judging whether the vector dot product is smaller than 0;
if the distance from the target endpoint to the center point is smaller than 0, calculating the distance from the center point to the target edge to obtain the point edge distance from the center point to the target edge;
if not, judging whether the vector dot product is larger than or equal to the square of the length of the target side line;
if yes, calculating the distance from the other end point of the target edge to the circle center point to obtain the point edge distance from the circle center point to the target edge;
if not, calculating a module of the cross product of the first vector and the second vector, and dividing the module by the length of the target side line to obtain the point edge distance from the center point to the target side line.
Further, the claw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
projecting center points of the obstacle model and the claw model onto straight lines where all side lines of the obstacle model are located respectively so as to project a line segment and a projection point on each straight line;
Respectively expanding the preset lengths outwards at the two ends of each line segment to obtain an extension line segment on each straight line, wherein the preset lengths are consistent with the radius of the claw model;
judging whether the projection point and the extension line segment on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact.
Further, the claw model and the obstacle model are both polygonal structures, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
projecting the obstacle model and the claw model onto straight lines where the side lines of the obstacle model and the claw model are located respectively so as to project two line segments on each straight line;
judging whether two line segments on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact.
Further, the step of respectively establishing the obstacle model and the claw model in a preset reference plane comprises the following steps:
establishing a three-dimensional model of the barrier and the claw;
and respectively projecting the three-dimensional models of the obstacle and the claw into the preset reference plane to obtain the obstacle model and the claw model.
A collision detection system according to an embodiment of the present invention includes:
the model building module is used for respectively building an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw;
the track calculation module is used for calculating the running track of the claw in the preset reference plane according to the parameters of each joint of the mechanical arm;
and the collision judging module is used for judging that the claw collides with the obstacle when the claw model contacts with the obstacle model at any position point of the moving track.
The present invention also proposes a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the collision detection method described above.
The invention also proposes a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, said processor implementing the collision detection method described above when executing said program.
The invention relates to a collision detection method, a system, a readable storage medium and computer equipment, wherein the height of a claw and an obstacle is not influenced on collision detection, so when a collision model is built, the height characteristics of the claw and the obstacle are directly ignored, planar geometric models corresponding to the top views of the obstacle and the claw are respectively built in the same preset reference plane to reflect the maximum outline of the two, and the top views of the claw and the obstacle are inevitably contacted when the claw moves in the plane or moves up and down, therefore, according to the parameters of each joint of a mechanical arm and directly ignoring the track of the longitudinal movement of the claw, the movement track of the claw in the plane is calculated, and then, whether the claw and the obstacle collide or not can be judged by analyzing whether the claw model contacts with the obstacle model at any position point of the movement track, and if the claw and the obstacle collide. Therefore, the collision detection method of the invention has the advantages of simple and simple whole detection process, small calculated amount and greatly improved collision detection efficiency by reasonably simplifying the object model and the detection track and adopting a simple graph contact judgment mode to finally determine whether the collision occurs.
Drawings
Fig. 1 is a flowchart of a collision detection method in a first embodiment of the present invention;
fig. 2 is a flowchart of a collision detection method in a second embodiment of the present invention;
FIG. 3 is a diagram illustrating the calculation of the margin of a point in a second embodiment of the present invention;
FIG. 4 is a diagram illustrating a projection method according to a second embodiment of the present invention;
FIG. 5 is another schematic explanatory diagram of the projection method in the second embodiment of the present invention;
fig. 6 is a schematic structural view of a collision detection system in a third embodiment of the present invention.
Description of main reference numerals:
the following detailed description will further illustrate the invention with reference to the above-described drawings.
Detailed Description
In order that the invention may be readily understood, a more complete description of the invention will be rendered by reference to the appended drawings. Several embodiments of the invention are presented in the figures. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
It will be understood that when an element is referred to as being "mounted" on another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements may also be present. The terms "vertical," "horizontal," "left," "right," and the like are used herein for illustrative purposes only.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and/or" as used herein includes any and all combinations of one or more of the associated listed items.
The following embodiments are applicable to collision detection for a SCARA four-axis mechanical arm, but it will be understood that the embodiments are not meant to limit the application environment of collision detection in the present application, and in other embodiments, may be applied to collision detection for other mechanical arms.
Referring to fig. 1, a collision detection method according to a first embodiment of the present invention includes steps S01 to S03.
Step S01, respectively establishing an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw.
When the method is specifically built, a collision detection model of an object can be built according to overlook patterns of the obstacle and the clamping jaw, and in addition, a three-dimensional projection method can be adopted to project and generate the collision detection model. Meanwhile, the established placement positions of the obstacle model and the jaw model should be set according to the placement positions of the obstacle and the jaw during actual use so as to ensure the reliability of collision detection.
It should be noted that, since the collision detection method subsequently adopts the image contact analysis method to determine whether the obstacle and the claw collide, it is obvious that the heights of the obstacle and the claw are not affected by the collision detection method, so that the heights of the obstacle and the claw are not considered, and the obstacle and the claw are directly ignored.
The object of selecting the top view pattern as the obstacle and jaw model is to reflect the maximum contour of the obstacle and jaw in the model. Those skilled in the art will appreciate that in other embodiments, the bottom view of the obstacle and the pawl may be selected as the corresponding collision detection model.
Step S02, calculating the running track of the claw in the preset reference plane according to the parameters of each joint of the mechanical arm.
The joint parameters comprise joint length and relative rotation angle among the joints.
It should be noted that, since the two top views will necessarily contact each other no matter the pawl moves in the plane or moves up and down, the system will determine that the two top views contact each other, so that the moving track of the pawl moving up and down does not affect the whole collision detection, the moving track of the pawl moving up and down is not considered, and is directly ignored here, and the moving track of the pawl moving in the plane is directly calculated.
And S03, judging that the claw collides with the obstacle when the claw model contacts with the obstacle model at any position point of the moving track.
It will be appreciated that point contact between the obstacle model and the jaw model, or the overlap of the obstacle model and the jaw model, is considered to be the point of contact sent by the obstacle model and the jaw model, and that such contact between images may be determined using distance calculation or image analysis techniques.
In summary, according to the collision detection method, the system, the readable storage medium and the computer device in the embodiment of the present invention, since the heights of the claw and the obstacle do not affect the collision detection, when the collision model is built, the height features of the claw and the obstacle are directly ignored, and planar geometric models corresponding to the top views of the obstacle and the claw are respectively built in the same preset reference plane, and no matter the claw moves in the plane or moves up and down with the obstacle, the top views of the claw and the obstacle are necessarily contacted at the position of the collision point, so that the method calculates the moving track of the claw in the plane according to the parameters of each joint of the mechanical arm and directly ignores the track of the longitudinal movement of the claw, and then can determine whether the claw collides with the obstacle by analyzing whether the claw model contacts with the obstacle model at any position point of the moving track, if so, the claw collides with the obstacle is determined. Therefore, the collision detection method of the invention has the advantages of simple and simple whole detection process, small calculated amount and greatly improved collision detection efficiency by reasonably simplifying the object model and the detection track and adopting a simple graph contact judgment mode to finally determine whether the collision occurs.
Referring to fig. 2, a collision detection method according to a second embodiment of the invention includes steps S11 to S17.
And S11, establishing a three-dimensional model of the obstacle and the claw.
Wherein, according to the design parameters (such as length, width, height, etc.) of the barrier and the claw, three-dimensional models of the barrier and the claw can be respectively built. Meanwhile, the three-dimensional model of the obstacle and the clamping jaw is a placement position, and the placement position of the obstacle and the clamping jaw in the actual use process is set according to the placement position, so that the collision detection reliability is ensured.
Step S12, selecting a preset reference plane, wherein the preset reference plane is provided with a pre-established two-dimensional coordinate system.
Preferably, the preset reference plane is a plane reflecting the top view of the three-dimensional model of the obstacle and the claw.
And S13, projecting the three-dimensional models of the obstacle and the clamping jaw into the preset reference plane to obtain an obstacle model and a clamping jaw model.
Wherein, because the obstacle model and the claw model are images projected onto a projection surface, the obstacle model and the claw model are plane geometric figures. In addition, since the preset reference plane is a plane in the overlooking direction, the obstacle model is a overlooking pattern of the three-dimensional model of the obstacle, and the jaw model is a overlooking pattern of the three-dimensional model of the jaw.
It will be appreciated that in other embodiments, the predetermined reference plane may be a plane reflecting a bottom view of the three-dimensional model of the obstacle and the claw, but may be a top view or a bottom view, both of which reflect the maximum peripheral contours of the obstacle model and the claw, for example, a circle projected from the larger diameter end of the cone.
Step S14, calculating the running track of the claw in the preset reference plane according to the parameters of each joint of the mechanical arm.
The joint parameters comprise joint length and relative rotation angle among the joints. The two top views of the claw are necessarily contacted when the claw moves on the plane or moves up and down, so that the moving track of the claw on the plane can be directly calculated by omitting the moving track of the claw moving up and down when the moving track of the claw is calculated.
For example, assume that the joint length of the first axis of the SCARA four-axis robotic arm is a 1 The joint length of the second shaft is a 2 Then we can calculate the movement track of the grabber as:
x＝a 1 cos(θ 1 )+a 2 cos(θ 1 +θ 2 )
y＝a 1 sin(θ 1 )+a 2 sin(θ 1 +θ 2 )
wherein (x, y) represents the coordinates of the center point of the jaws,indicating the angle through which the jaws turn about their own centre point. In the above expression, θ does not appear 3 Because of theta 3 The effect is the height of the claw, in addition, the third shaft of the SCARA four-shaft mechanical arm is a shaft driving the claw to move up and down, and the fourth shaft is a shaft driving the claw to rotate, which is negligible.
In addition, in other embodiments, the step S14 may be implemented as follows:
calculating the advancing path of the claw according to the length of each joint of the mechanical arm and the relative rotation angle between each joint;
and projecting the travelling path into the preset reference plane to obtain the running track.
It can be understood that, in this embodiment, the spatial travel path of the claw is calculated first, and the planar travel path of the claw is obtained by projecting the spatial travel path into the preset reference plane, so that the calculation amount of this embodiment is larger than that of the above embodiment, and therefore, in the specific implementation, the above embodiment is preferable.
And S15, judging whether the claw model is in contact with the obstacle model at any position point of the running track.
When judging that the claw model can be contacted with the obstacle model at any position point of the moving track, executing a step S17, and when judging that the claw model is not contacted with the obstacle model at any position point of the moving track, otherwise, representing that the claw cannot collide with the obstacle, and detecting the passing of the claw model.
And S17, judging that the claw collides with the obstacle, and sending out a collision prompt signal.
The mode of sending out collision prompt signals comprises vibration, alarm, buzzing, spring frame prompt and the like.
The following is a detailed description of specific situations, and the specific implementation steps for judging that the obstacle model and the claw model are in contact are as follows:
first, when the obstacle model and the claw model are both in a circular structure, the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
respectively calculating the radiuses of the obstacle model and the claw model, and calculating the sum of the radiuses of the obstacle model and the claw model to obtain a reference total radius;
judging whether the center distance between the obstacle model and the claw model is smaller than the reference total radius;
if yes, judging that the obstacle model and the claw model are in contact.
It can be understood that, because the obstacle model and the claw model are both circular structures, whether contact occurs between two circles is judged, and only whether the center distance between the two circles is larger than the radius of the two circles is judged, if so, the two circles are not contacted, otherwise, the two circles are contacted.
In addition, because the preset reference plane is a two-dimensional coordinate system established in advance, the center distance of the obstacle model and the claw model can be calculated according to the coordinates of the center points of the two points and a distance calculation formula between the two points.
Secondly, when the claw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
calculating the reference radius of the claw model, and respectively calculating the point edge distance from the center point of the claw model to each edge line of the obstacle model;
judging whether the point margin smaller than the reference radius exists or not;
if yes, judging that the obstacle model and the claw model are in contact.
It can be understood that, because the obstacle model is a polygon, the claw model is a circular structure, and whether the circle and the polygon are contacted is judged, only whether the distance from the center of the circle to each side of the polygon is larger than the radius of the circle is judged, if so, the circle is not contacted, otherwise, the circle is contacted.
Specifically, referring to fig. 3, the step of calculating a point margin from the center point to the obstacle model edge includes:
acquiring coordinates of the center point, and acquiring the length of any target side line of the barrier model and coordinates of two end points;
taking any target endpoint of the target edge as a base point, establishing a first vector from the target endpoint to the other endpoint of the target edge, and establishing a second vector from the target endpoint to the center point (as shown in fig. 3);
Calculating a vector dot product of the first vector and the second vector (using a dot pole calculation formula of the two vectors), and judging whether the vector dot product is smaller than 0;
if yes, calculating the distance from the target endpoint to the center point to obtain the point edge distance from the center point to the target edge line;
if not, judging whether the vector dot product is larger than or equal to the square of the length of the target side line;
if yes, calculating the distance from the other end point of the target edge to the circle center point to obtain the point edge distance from the circle center point to the target edge;
if not, calculating a module of the cross product of the first vector and the second vector, and dividing the module by the length of the target side line to obtain the point edge distance from the center point to the target side line.
Since the graph is located in the reference plane of the established coordinate system, the calculation formulas (such as vector dot product and two-dot distance formulas) related to the dot edges and the vectors in high numbers can be directly adopted for processing.
It will be understood that when the dot product of the vector A1 and the vector A2 is smaller than 0, the two vectors are perpendicular to each other, the dot margin is the distance from the center to the base point (one of the endpoints) (as shown in fig. 3), and when the dot product of the vector A1 and the vector A2 is greater than or equal to the square of the edge length, the two vectors are perpendicular to each other, the dot margin is the distance from the center to the other endpoint (as shown in fig. 3), otherwise, the dot margin is obtained by calculating the cross-product modulus of the two vectors and dividing the cross-product by the edge length.
Thirdly, when the jaw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the jaw model are in contact may further include:
projecting center points of the obstacle model and the claw model onto straight lines where all side lines of the obstacle model are located respectively so as to project a line segment and a projection point on each straight line;
respectively expanding the preset lengths outwards at the two ends of each line segment to obtain an extension line segment on each straight line, wherein the preset lengths are consistent with the radius of the claw model;
judging whether the projection point and the extension line segment on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact.
It can be understood that, since the obstacle model is polygonal (assuming that there are n sides), the jaw model is of a circular structure, and it is determined whether contact occurs between the circle and the polygon, and the center points of the obstacle model and the jaw may be projected to the straight line where the n sides of the obstacle are located, respectively. A line segment and a point can be obtained for each projection, the line segment obtained by the projection is extended by r lengths towards two sides, and if the projected point is outside the extended line segment, no collision exists. If the point of each projection is within the extension of the projection, the pawl will collide with the obstacle.
For example, please refer to fig. 4-5, assume that n vertex coordinates of n-sided shapes of the obstacle are (x 1 ,y 1 ),…(x n ,y n ) Then the line segment obtained after the projection of the polygon to the straight line where the edges connected by the first and second vertices are calculated can be calculated by the following way:
vertex (x) i ,y i ) The point projected onto this line can be expressed as
Let l min Is l i The smallest value in n, let l = 1 max Is l i The largest value in n, i=1..then the projected line segment of the polygon can be divided by a number of pairs (i min ,l max ) To represent. The extension line segment can be used (l) min -r,l max +r). If the point of the center (x, y) projected to this straight line is expressed as
If l<l min -r or l>l max +r, then the pawl is collision free with the obstacle (as shown in fig. 5), otherwise the next projection, i.e. the projection of the second side, is performed. If there is l after all projections min -r≤l≤l max +r, then the pawl collides with the obstacle (as shown in fig. 6).
In addition, it should be noted that, when the jaw model is of a polygonal structure and the obstacle model is of a circular structure, the contact analysis can be performed in the second or third analysis mode similarly to the case where the jaw model is of a circular structure and the obstacle model is of a polygonal structure.
In addition, when processing the polygonal jaw, the position of the center point of the jaw and the rotated angle of the jaw relative to the standard position need to be obtained from the positive kinematics, and then the coordinates of all the vertices of the polygonal jaw and the expression of the line segment sides can be obtained. For example, whenWhen the coordinate of a certain vertex is +.>Then after positive kinematic calculation, the coordinates of this vertex are
Fourth, when the jaw model and the obstacle model are both polygonal structures, the step of judging that the obstacle model and the jaw model are in contact includes:
projecting the obstacle model and the claw model onto straight lines where the side lines of the obstacle model and the claw model are located respectively so as to project two line segments on each straight line;
judging whether two line segments on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact.
It will be appreciated that for this case only projection-based methods (third way described above) can be used. Assuming that the grip is m-sided and the obstacle is n-sided, then m+n projections are required to be performed, corresponding to the straight lines where the m sides of the grip and the n sides of the obstacle are located, respectively. Each time a projection is performed, two line segments can be obtained by two polygons on the projection straight line, if the two line segments are not overlapped, the two line segments can be directly returned to be collision-free, otherwise, the next projection is performed. If for all projections the two line segments overlap, then the hand grip and the obstacle are bumped.
In addition, it should be noted that if the polygon has some symmetry, the number of projections can be reduced. For example, if the polygon is a rectangle, only one projection to each of the two adjacent sides is required, and one projection to all four sides is not necessary, since the two opposite sides of the rectangle are parallel.
Furthermore, if the jaws or obstacles are not circles or polygons, but other shapes, such as sectors, it is possible to expand a polygon or circle so that it can envelop the jaws or obstacles. Since it is important in collision detection that the algorithm can output the situation of a collision as a collision, the claw or the obstacle can be appropriately expanded into a shape that is easy to handle by the algorithm. Of course, if the expanded space is too large, the collision detection result obtained is too conservative.
In another aspect, referring to fig. 6, a collision detection system according to a third embodiment of the present invention includes:
the model building module 11 is configured to build an obstacle model and a jaw model in a preset reference plane, where the obstacle model is a top view pattern of the obstacle, and the jaw model is a top view pattern of the jaw;
The track calculation module 12 is used for calculating the running track of the claw in the preset reference plane according to the parameters of each joint of the mechanical arm;
and a collision determination module 13, configured to determine that the claw and the obstacle collide when the claw model contacts the obstacle model at any position point of the moving track.
Further, the obstacle model and the jaw model are both of a circular structure, and the collision determination module 13 includes:
a first calculation unit 131 for calculating radii of the obstacle model and the jaw model, respectively, and calculating a sum of the radii of the two to obtain a reference total radius;
a first judging unit 132 that judges whether or not a center distance between the obstacle model and the jaw model is smaller than the reference total radius;
when the center-to-center distance of the obstacle model and the jaw model is smaller than the reference total radius, the collision determination module 13 determines that the obstacle model and the jaw model are in contact.
Further, the claw model is a circular structure, the obstacle model is a polygonal structure, and the collision determination module 13 includes:
A second calculating unit 133, configured to calculate a reference radius of the jaw model, and calculate a point edge distance from a center point of the jaw model to each edge line of the obstacle model;
a second judging unit 134 for judging whether the dot margin smaller than the reference radius exists;
when it is determined that the point margin smaller than the reference radius exists, the collision determination module 13 determines that the obstacle model and the jaw model come into contact.
Further, the second calculating unit 133 includes:
a parameter obtaining subunit 1331, configured to obtain coordinates of the center point, and obtain a length of any target edge of the obstacle model and coordinates of two end points;
a vector establishing subunit 1332, configured to establish, with any target endpoint of the target edge as a base point, a first vector from the target endpoint to another endpoint of the target edge, and establish a second vector from the target endpoint to the center point;
a first dot product judging subunit 1333 for calculating a vector dot product of the first vector and the second vector, and judging whether the vector dot product is less than 0;
a first point distance calculating subunit 1334, configured to calculate, when it is determined that the vector dot product is less than 0, a distance from the target endpoint to the center point, so as to obtain a point edge distance from the center point to the target edge;
A second dot product judging subunit 1335 for judging whether the vector dot product is equal to or greater than the square of the length of the target edge when judging that the vector dot product is not less than 0;
a second point distance calculating subunit 1336, configured to calculate, when it is determined that the vector dot product is equal to or greater than the square of the length of the target edge, a distance from the other end point of the target edge to the center point, so as to obtain a point edge distance from the center point to the target edge;
and a cross module calculating subunit 1337 configured to calculate, when it is determined that the vector dot product is not equal to or greater than the square of the length of the target edge, a module of the first vector and the second vector cross product, and divide the length of the target edge to obtain a dot edge distance from the center point to the target edge.
Further, the claw model is a circular structure, the obstacle model is a polygonal structure, and the collision determination module 13 includes:
a first projection unit 135, configured to project center points of the obstacle model and the jaw model onto straight lines where each side line of the obstacle model is located, so as to project a line segment and a projection point on each straight line;
The extending unit 136 is configured to extend a preset length outwards at two ends of each line segment to obtain an extended line segment on each straight line, where the preset length is consistent with the radius of the jaw model;
a first overlap judging unit 137 for judging whether the projected point and the extension line segment on each of the straight lines overlap;
when it is determined that the projected point and the extension line segment on each of the straight lines overlap, the collision determination module 13 determines that the obstacle model and the jaw model come into contact.
Further, the claw model and the obstacle model are both polygonal structures, and the collision determination module 13 includes:
a second projection unit 138, configured to project the obstacle model and the jaw model onto straight lines where the respective edges of the obstacle model and the jaw model are located, so as to project two line segments on each of the straight lines;
a second overlap judging unit 139 that judges whether or not the two line segments on each of the straight lines overlap;
when it is determined that the two line segments on each of the straight lines overlap, the collision determination module 13 determines that the obstacle model and the jaw model come into contact.
Further, the model building module 11 includes:
a model building unit 111 for building a three-dimensional model of the obstacle and the claw;
and a model projection unit 112, configured to project the three-dimensional models of the obstacle and the jaw into the preset reference plane, so as to obtain the obstacle model and the jaw model.
Further, the collision detection system further includes:
the plane selection module 14 is configured to select a preset reference plane, where the preset reference plane has a two-dimensional coordinate system established in advance.
Further, the collision detection system further includes:
and the collision prompt module 15 is used for sending a collision prompt signal when the claw and the obstacle are judged to collide.
The present invention also proposes a computer-readable storage medium, on which a computer program is stored, which program, when being executed by a processor, implements a collision detection method as described above.
The invention also proposes a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, said processor implementing the collision detection method as described above when executing said program.
Those of skill in the art will appreciate that the logic and/or steps represented in the flow diagrams or otherwise described herein, e.g., a ordered listing of executable instructions for implementing logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device.
More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer readable medium may even be paper or other suitable medium on which the program is printed, as the program may be electronically captured, via, for instance, optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner, if necessary, and then stored in a computer memory.
It is to be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above-described embodiments, the various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, may be implemented using any one or combination of the following techniques, as is well known in the art: discrete logic circuits having logic gates for implementing logic functions on data signals, application specific integrated circuits having suitable combinational logic gates, programmable Gate Arrays (PGAs), field Programmable Gate Arrays (FPGAs), and the like.
In the description of the present specification, a description referring to terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
The foregoing examples illustrate only a few embodiments of the invention and are described in detail herein without thereby limiting the scope of the invention. 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 invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.
Claims (9)
1. A collision detection method, characterized by comprising:
respectively establishing an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw;
calculating the running track of the claw in the preset reference plane according to the joint parameters of the mechanical arm, wherein the joint parameters comprise joint length and relative rotation angles among the joints;
when the claw model contacts with the obstacle model at any position point of the moving track, judging that the claw collides with the obstacle;
the obstacle model and the claw model are any combination of a circular structure and a polygonal structure;
When the claw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
projecting center points of the obstacle model and the claw model onto straight lines where all side lines of the obstacle model are located respectively so as to project a line segment and a projection point on each straight line;
respectively expanding the preset lengths outwards at the two ends of each line segment to obtain an extension line segment on each straight line, wherein the preset lengths are consistent with the radius of the claw model;
judging whether the projection point and the extension line segment on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact;
the obstacle model is provided as a polygon with n sides, and n vertex coordinates of the obstacle model are respectively (x) 1 ，y 1 )......(x n ，y n ) The radius of the claw model is r, the center coordinates are (x, y), and the top of the obstacle model is based on the straight line connecting the first top point and the second top point of the obstacle modelPoint (x) i ，y i ) The point l projected to this line i Expressed as:
Let l min Is l i I=1..the smallest value in..n, let l max Is l i I=1..the largest value in..n, the projected line segments of the polygons corresponding to the obstacle model are expressed as pairs (l min ，l max ) The extension segment is denoted as (l) min -r，l max +r)；
The point l obtained by projecting the circle center (x, y) of the claw model to the straight line is expressed as:
if l < l min -r or l > l max +r, the pawl is collision-free with the obstacle;
otherwise, performing projection calculation on the next edge of the obstacle model, if l is arranged after projection on all edges of the obstacle model min -r≤l≤l max +r, the pawl collides with the obstacle.
2. The collision detection method according to claim 1, wherein the obstacle model and the jaw model are both of a circular structure, and the step of judging that the obstacle model and the jaw model are in contact includes:
respectively calculating the radiuses of the obstacle model and the claw model, and calculating the sum of the radiuses of the obstacle model and the claw model to obtain a reference total radius;
judging whether the center distance between the obstacle model and the claw model is smaller than the reference total radius;
if yes, judging that the obstacle model and the claw model are in contact.
3. The collision detection method according to claim 1, wherein the jaw model has a circular structure, the obstacle model has a polygonal structure, and the step of determining that the obstacle model and the jaw model are in contact further comprises:
calculating the reference radius of the claw model, and respectively calculating the point edge distance from the center point of the claw model to each edge line of the obstacle model;
judging whether the point margin smaller than the reference radius exists or not;
if yes, judging that the obstacle model and the claw model are in contact.
4. A collision detection method according to claim 3, wherein the step of calculating a point margin from the center point to the obstacle model margin comprises:
acquiring coordinates of the center point, and acquiring the length of any target side line of the barrier model and coordinates of two end points;
taking any target endpoint of the target edge as a base point, establishing a first vector from the target endpoint to the other endpoint of the target edge, and establishing a second vector from the target endpoint to the center point;
calculating a vector dot product of the first vector and the second vector, and judging whether the vector dot product is smaller than 0;
If the distance from the target endpoint to the center point is smaller than 0, calculating the distance from the center point to the target edge to obtain the point edge distance from the center point to the target edge;
if not, judging whether the vector dot product is larger than or equal to the square of the length of the target side line;
if yes, calculating the distance from the other end point of the target edge to the circle center point to obtain the point edge distance from the circle center point to the target edge;
if not, calculating a module of the cross product of the first vector and the second vector, and dividing the module by the length of the target side line to obtain the point edge distance from the center point to the target side line.
5. The collision detection method according to claim 1, wherein the jaw model and the obstacle model are each of a polygonal structure, and the step of determining that the obstacle model and the jaw model are in contact includes:
projecting the obstacle model and the claw model onto straight lines where the side lines of the obstacle model and the claw model are located respectively so as to project two line segments on each straight line;
judging whether two line segments on each straight line overlap or not;
If yes, judging that the obstacle model and the claw model are in contact.
6. The method of claim 1, wherein the step of establishing the obstacle model and the jaw model in a predetermined reference plane respectively comprises:
establishing a three-dimensional model of the barrier and the claw;
and respectively projecting the three-dimensional models of the obstacle and the claw into the preset reference plane to obtain the obstacle model and the claw model.
7. A collision detection system, comprising:
the model building module is used for respectively building an obstacle model and a claw model in a preset reference plane, wherein the obstacle model is a overlook pattern of the obstacle, and the claw model is a overlook pattern of the claw;
the track calculation module is used for calculating the running track of the claw in the preset reference plane according to the parameters of each joint of the mechanical arm; wherein the joint parameters comprise joint length and relative rotation angles among the joints;
the collision judging module is used for judging that the claw collides with the obstacle when the claw model contacts with the obstacle model at any position point of the moving track;
The obstacle model and the claw model are any combination of a circular structure and a polygonal structure;
when the claw model is of a circular structure, the obstacle model is of a polygonal structure, and the step of judging that the obstacle model and the claw model are in contact comprises the following steps:
projecting center points of the obstacle model and the claw model onto straight lines where all side lines of the obstacle model are located respectively so as to project a line segment and a projection point on each straight line;
respectively expanding the preset lengths outwards at the two ends of each line segment to obtain an extension line segment on each straight line, wherein the preset lengths are consistent with the radius of the claw model;
judging whether the projection point and the extension line segment on each straight line overlap or not;
if yes, judging that the obstacle model and the claw model are in contact;
the obstacle model is provided as a polygon with n sides, and n vertex coordinates of the obstacle model are respectively (x) 1 ，y 1 )......(x n ，y n ) The radius of the claw model is r, the center coordinates are (x, y), and the straight line connecting the first peak and the second peak of the barrier model is taken as a reference, then the peak (x i ，y i ) The point l projected to this line i Expressed as:
let l min Is l i I=1..the smallest value in..n, let l max Is l i I=1..the largest value in..n, the projected line segments of the polygons corresponding to the obstacle model are expressed as pairs (l min ，l max ) The extension segment is denoted as (l) min -r，l max +r)；
The point l obtained by projecting the circle center (x, y) of the claw model to the straight line is expressed as:
if l < l min -r or l > l max +r, the pawl is collision-free with the obstacle;
otherwise, performing projection calculation on the next edge of the obstacle model, if l is arranged after projection on all edges of the obstacle model min -r≤l≤l max +r, the pawl collides with the obstacle.
8. A computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, implements the method according to any of claims 1-6.
9. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the method according to any of claims 1-6 when executing the program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810670546.6A CN108959753B (en) | 2018-06-26 | 2018-06-26 | Collision detection method, system, readable storage medium and computer device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810670546.6A CN108959753B (en) | 2018-06-26 | 2018-06-26 | Collision detection method, system, readable storage medium and computer device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108959753A CN108959753A (en) | 2018-12-07 |
| CN108959753B true CN108959753B (en) | 2023-08-08 |
Family
ID=64486799
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810670546.6A Active CN108959753B (en) | 2018-06-26 | 2018-06-26 | Collision detection method, system, readable storage medium and computer device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108959753B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109741461B (en) * | 2018-12-18 | 2023-04-07 | 南京东巴电子科技有限公司 | Method for realizing collision detection in three-dimensional dynamic simulation construction scheme process |
| CN110941940B (en) * | 2019-12-04 | 2023-06-13 | 杭州广立微电子股份有限公司 | 3D winding method, storage device and system based on collision detection |
| CN113721618B (en) * | 2021-08-30 | 2024-05-24 | 中科新松有限公司 | Plane determination method, device, equipment and storage medium |
| CN115272379B (en) * | 2022-08-03 | 2023-11-28 | 上海新迪数字技术有限公司 | Projection-based three-dimensional grid model outline extraction method and system |
| CN116541939B (en) * | 2023-05-25 | 2024-01-30 | 驻马店市博瑞建筑工程有限公司 | Collision detection method and system in bridge design |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6429617B1 (en) * | 1999-10-22 | 2002-08-06 | Kawasaki Jukogyo Kabushiki Kaisha | Drive-controlling method and apparatus and robot having the apparatus |
| CN101925444A (en) * | 2008-01-22 | 2010-12-22 | 松下电器产业株式会社 | Robot arm |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN104392081A (en) * | 2014-06-30 | 2015-03-04 | 管小清 | Redundant mechanical arm moving obstacle avoiding algorithm |
| CN106003043A (en) * | 2016-06-20 | 2016-10-12 | 先驱智能机械（深圳）有限公司 | Obstacle avoidance method and obstacle avoidance system of mechanical arm |
-
2018
- 2018-06-26 CN CN201810670546.6A patent/CN108959753B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6429617B1 (en) * | 1999-10-22 | 2002-08-06 | Kawasaki Jukogyo Kabushiki Kaisha | Drive-controlling method and apparatus and robot having the apparatus |
| CN101925444A (en) * | 2008-01-22 | 2010-12-22 | 松下电器产业株式会社 | Robot arm |
| CN104029203A (en) * | 2014-06-18 | 2014-09-10 | 大连大学 | Path planning method for implementation of obstacle avoidance for space manipulators |
| CN104392081A (en) * | 2014-06-30 | 2015-03-04 | 管小清 | Redundant mechanical arm moving obstacle avoiding algorithm |
| CN106003043A (en) * | 2016-06-20 | 2016-10-12 | 先驱智能机械（深圳）有限公司 | Obstacle avoidance method and obstacle avoidance system of mechanical arm |
Non-Patent Citations (1)
| Title |
|---|
| 基于A~*改进算法的机械臂避障路径规划;汪首坤等;《北京理工大学学报》;20111115;第31卷(第11期);第1302-1306页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108959753A (en) | 2018-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108959753B (en) | Collision detection method, system, readable storage medium and computer device | |
| EP3410246B1 (en) | Robot obstacle avoidance control system and method, robot, and storage medium | |
| EP4155675A1 (en) | Robot motion planning method, path planning method, grabbing method and devices thereof | |
| US10562182B2 (en) | Robot, robot system, control device, and control method | |
| US10946521B2 (en) | Movement planning apparatus, moving robot, and movement planning program | |
| US9089971B2 (en) | Information processing apparatus, control method thereof and storage medium | |
| JP7314411B2 (en) | Obstacle information sensing method and device for mobile robot | |
| US20160158936A1 (en) | Collision avoidance method, control device, and program | |
| CN110320859B (en) | Robot control device, method for obtaining angle of robot joint, and storage medium | |
| JP5817611B2 (en) | Mobile robot | |
| CN103678754A (en) | Information processing apparatus and information processing method | |
| Karaszewski et al. | Assessment of next-best-view algorithms performance with various 3D scanners and manipulator | |
| CN112264991B (en) | Layered rapid on-orbit collision detection method suitable for space manipulator | |
| CN113269835A (en) | Industrial part pose identification method and device based on contour features and electronic equipment | |
| JP2018169660A (en) | Object attitude detection apparatus, control apparatus, robot and robot system | |
| EP2732935B1 (en) | Method for checking a travelable area of a plane of travel for a robot, robot and recording medium | |
| JP2019084649A (en) | Interference determination method, interference determination system, and computer program | |
| US6597967B2 (en) | System and method for planning a tool path along a contoured surface | |
| JP7500947B2 (en) | Interference assessment device, method, and program | |
| JP4389663B2 (en) | Image processing method and image processing apparatus | |
| CN111251335A (en) | High-precision mechanical arm collision detection method based on bounding box algorithm | |
| JP6848761B2 (en) | Distance evaluation method between objects and interference evaluation method between relatively moving objects | |
| JP2001250122A (en) | Method for determining position and posture of body and program recording medium for the same | |
| CN108858162B (en) | Position determination method and device for four-axis mechanical arm | |
| CN115797608B (en) | Building block building method and intelligent terminal |
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 |