CN108959753A - Collision checking method, system, readable storage medium storing program for executing and computer equipment - Google Patents
Collision checking method, system, readable storage medium storing program for executing and computer equipment Download PDFInfo
- Publication number
- CN108959753A CN108959753A CN201810670546.6A CN201810670546A CN108959753A CN 108959753 A CN108959753 A CN 108959753A CN 201810670546 A CN201810670546 A CN 201810670546A CN 108959753 A CN108959753 A CN 108959753A
- Authority
- CN
- China
- Prior art keywords
- model
- claw
- barrier
- sideline
- point
- 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
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
Abstract
The present invention provides a kind of collision checking method, system, readable storage medium storing program for executing and computer equipment, the described method includes: in a preset reference plane, barrier model and claw model are established respectively, the barrier model is the vertical view figure of the barrier, and the claw model is the vertical view figure of the claw；According to the parameter in each joint of mechanical arm, running track of the claw in the preset reference plane is calculated；When the claw model is in contact in any location point of the running track with the barrier model, determines that the claw and the barrier exist and collide.Collision checking method, system, readable storage medium storing program for executing and computer equipment in the present invention, pass through Rational Simplification object model and detection track, and finally determine whether to collide using simple pattern contact decision procedure, entire detection process is simple, simply, calculation amount is small, and the efficiency of collision detection is greatly improved.
Description
Technical field
The present invention relates to mechanical arm technical field, in particular to a kind of collision checking method, system, readable storage medium storing program for executing and
Computer equipment.
Background technique
With the continuous development of Machine Manufacturing Technology and automatic technology, it is born and miscellaneous mechanical arm (such as
Tetra- shaft mechanical arm of SCARA), mechanical arm usually by multiple joints and is arranged in the claw of end and forms, and each joint cooperates to
The pose for changing claw, to complete the task (such as transferring product) of feature.
Before the factory of mechanical arm or in R&D process, it usually needs carry out collision detection to mechanical arm, that is, need to sentence
Whether disconnected claw generates under a certain pose with the barrier in local environment is collided, to plan the motion path of mechanical arm.It touches
Hitting detection method is to guarantee that mechanical arm finds the key technology in a collisionless path from starting point to terminal, and superiority-inferiority is straight
Connect the effect for determining motion planning.
In the prior art, the collision checking method that mechanical arm uses at present is divided into two kinds, existing one is calling directly
Collision detection library, this library is integrated with too many content, influence whole collision detection algorithm efficiency, another method be with
Certain density is up-sampled in each axis of mechanical arm, judges whether each sampled point collides with other points and barrier,
This method is time-consuming and inaccurate.
Summary of the invention
Based on this, the object of the present invention is to provide a kind of collision checking method, system, readable storage medium storing program for executing and computers to set
It is standby, to improve the efficiency of mechanical arm collision detection.
A kind of collision checking method according to an embodiment of the present invention, comprising:
In a preset reference plane, barrier model and claw model are established respectively, the barrier model is described
The vertical view figure of barrier, the claw model are the vertical view figure of the claw；
According to the parameter in each joint of mechanical arm, running track of the claw in the preset reference plane is calculated；
When the claw model is in contact in any location point of the running track with the barrier model,
Determine that the claw and the barrier exist to collide.
In addition, a kind of collision checking method according to the above embodiment of the present invention, can also have following additional technology
Feature:
Further, the barrier model and the claw model are circular configuration, judge the barrier model
The step of being in contact with the claw model include:
The sum of calculate separately out the radius of the barrier model and the claw model, and calculate the two radius, with
To with reference to total radius；
Judge the center of the barrier model and the claw model away from whether being less than and described refer to total radius；
If so, determining the barrier model and the claw model is in contact.
Further, the claw model is circular configuration, and the barrier model is polygonized structure, judges the barrier
The step of hindering object model and the claw model to be in contact include:
The reference radius of the claw model is calculated, and calculates separately out the centre point of the claw model to the barrier
Hinder the point back gauge in each sideline of object model；
Judge whether there is the described back gauge less than the reference radius；
If so, determining the barrier model and the claw model is in contact.
Further, the step of calculating point back gauge of the centre point to the barrier model boundary include:
The coordinate of the centre point is obtained, and obtains the length and two endpoints in the barrier model either objective sideline
Coordinate；
Using the either objective endpoint in the target sideline as basic point, the target endpoint is established to the another of the target sideline
The primary vector of one endpoint, and establish the target endpoint to the centre point secondary vector；
The dot product of the primary vector and the secondary vector is calculated, and judges the dot product whether less than 0；
If being less than 0, calculate the target endpoint to the centre point distance, to obtain the centre point to described
The point back gauge in target sideline；
If not then judging whether the dot product is more than or equal to square of the length in the target sideline less than 0；
If so, calculating another endpoint in the target sideline to the distance of the centre point, to obtain the centre point
To the point back gauge in the target sideline；
If it is not, then calculating the mould of the primary vector and the secondary vector cross product, and divided by the length in the target sideline
Degree, with obtain the centre point to the target sideline point back gauge.
Further, the claw model is circular configuration, and the barrier model is polygonized structure, judges the barrier
The step of hindering object model and the claw model to be in contact include:
Respectively by the barrier model and the centre point of the claw model, to each sideline of the barrier model
It is projected on the straight line at place, to be projected out a line segment and a subpoint on every straight line；
Prolong preset length to extending out at the both ends of every line segment respectively, to obtain an extension on every straight line
Line segment, the preset length are consistent with the radius of the claw model；
Judge whether the subpoint on every straight line and the extension line segment are overlapped；
If so, determining the barrier model and the claw model is in contact.
Further, the claw model and the barrier model are polygonized structure, judge the barrier mould
The step of type and the claw model are in contact include:
Respectively by the barrier model and the claw model, to each of the barrier model and the claw model
It is projected on straight line where sideline, to be projected out two line segments on every straight line；
Judge whether two line segments on every straight line are overlapped；
If so, determining the barrier model and the claw model is in contact.
Further, described in a preset reference plane, the step of establishing barrier model and claw model respectively packet
It includes:
Establish the threedimensional model of barrier and claw；
The threedimensional model of the barrier and the claw is projected in the preset reference plane respectively, to obtain
State barrier model and the claw model.
A kind of collision detecting system according to an embodiment of the present invention, comprising:
Model building module, it is described for establishing barrier model and claw model respectively in a preset reference plane
Barrier model is the vertical view figure of the barrier, and the claw model is the vertical view figure of the claw；
Trajectory computation module calculates the claw in the preset reference for the parameter according to each joint of mechanical arm
Running track in plane；
Collision determination module, for when the claw model in any location point of the running track with the obstacle
When object model is in contact, determines that the claw and the barrier exist and collide.
The present invention also proposes a kind of computer readable storage medium, is stored thereon with computer program, and the program is processed
Device realizes above-mentioned collision checking method when executing.
The present invention also proposes a kind of computer equipment, including memory, processor and storage are on a memory and can be
The computer program run on processor, the processor realize above-mentioned collision checking method when executing described program.
Collision checking method, system, readable storage medium storing program for executing and computer equipment in the present invention, due to claw and obstacle
The height of object is unaffected to collision detection, therefore when progress collision model is established, directly ignore the height of claw and barrier
Feature, and in same preset reference plane, plane geometry model corresponding with the top view of barrier and claw is established respectively,
With the largest contours both reflected, since no matter claw is in planar movement or when moving up and down and barrier, bowing both at this time
View will necessarily have contact, therefore this collision checking method, and according to the parameter in each joint of mechanical arm, and it is vertical directly to ignore claw
To mobile track, the running track of claw planar is calculated, it then can be by analysis claw model in running track
Any location point on whether be in contact with barrier model, come judge claw and barrier with the presence or absence of collision, if it exists
Contact then determines that claw and barrier exist and collides.Therefore the collision checking method in the present invention, passes through Rational Simplification object
Model and detection track, and finally determine whether to collide using simple pattern contact decision procedure, entirely detected
Journey is simple, and simply, calculation amount is small, and the efficiency of collision detection is greatly improved.
Detailed description of the invention
Fig. 1 is the flow chart of the collision checking method in first embodiment of the invention；
Fig. 2 is the flow chart of the collision checking method in second embodiment of the invention；
Fig. 3 is the graphic rendition figure that point back gauge is calculated in second embodiment of the invention；
Fig. 4 is the graphic rendition figure of projecting method in second embodiment of the invention；
Fig. 5 is another graphic rendition figure of projecting method in second embodiment of the invention；
The structural schematic diagram of collision detecting system in Fig. 6 third embodiment of the invention.
Main element symbol description:
Following specific embodiments will further illustrate the present invention in conjunction with above-mentioned attached drawing.
Specific embodiment
To facilitate the understanding of the present invention, a more comprehensive description of the invention is given in the following sections with reference to the relevant attached drawings.In attached drawing
Give several embodiments of the invention.But the invention can be realized in many different forms, however it is not limited to this paper institute
The embodiment of description.On the contrary, purpose of providing these embodiments is make it is more thorough and comprehensive to the disclosure.
It should be noted that it can directly on the other element when element is referred to as " being fixedly arranged on " another element
Or there may also be elements placed in the middle.When an element is considered as " connection " another element, it, which can be, is directly connected to
To another element or it may be simultaneously present centering elements.Term as used herein " vertical ", " horizontal ", " left side ",
" right side " and similar statement are for illustrative purposes only.
Unless otherwise defined, all technical and scientific terms used herein and belong to technical field of the invention
The normally understood meaning of technical staff is identical.Term as used herein in the specification of the present invention is intended merely to description tool
The purpose of the embodiment of body, it is not intended that in the limitation present invention.Term " and or " used herein includes one or more phases
Any and all combinations of the listed item of pass.
Following embodiment can be used in the collision detection to tetra- shaft mechanical arm of SCARA, it is to be understood that, this
It does not represent and defines the application environment of the application collision detection, in other embodiments, others can also be used in
In the collision detection of mechanical arm.
Referring to Fig. 1, show the collision checking method in first embodiment of the invention, including step S01 is to step
S03。
Step S01 establishes barrier model and claw model, the barrier mould in a preset reference plane respectively
Type is the vertical view figure of the barrier, and the claw model is the vertical view figure of the claw.
It wherein, can be according to the vertical view figure of the barrier and the claw, to establish touching for object when specifically establishing
Detection model is hit, generates collision detection model further, it is also possible to project using the method for tripleplane.Meanwhile being established
The placement position of the barrier model and the claw model, should be according to barrier and claw in actual use process
Placement position set, to ensure the reliability of collision detection.
It should be pointed out that due to this collision checking method it is subsequent using image contact analysis method come disturbance in judgement object and
Both whether claw collides, it is clear that and the height of barrier and claw is unaffected to this collision checking method, therefore do not consider
Height, directly neglect herein.
It is in addition, selecting and overlooking figure as the purpose of barrier and claw model, obstacle is reflected in model
The largest contours of object and claw.It will be appreciated by those skilled in the art that, in other embodiments, barrier can also be selected
And claw looks up figure as corresponding collision detection model.
Step S02 calculates fortune of the claw in the preset reference plane according to the parameter in each joint of mechanical arm
Row track.
Wherein, joint parameter includes the relative rotation angle between joint length and each joint.
It should be pointed out that since no matter claw is in planar movement or when moving up and down and barrier, bowing both at this time
View will necessarily have a contact, system will judge the two to be contacted, therefore the running track pair that claw moves up and down
Entire collision detection does not influence, therefore does not consider the running track that claw moves up and down, directly neglects herein, and directly calculate
The running track of claw planar movement out.
Step S03, when the claw model is sent out in any location point of the running track with the barrier model
When raw contact, determines that the claw and the barrier exist and collide.
It should be understood that between the barrier model and the claw model point contact or the barrier model and
There is overlapping in the claw model, can take as and have sent contact for the barrier model and the claw model, and image
Between this contact can be judged using apart from calculation or image analysis technology.
To sum up, the collision checking method in the above embodiment of the present invention, collision checking method in the present invention are
System, readable storage medium storing program for executing and computer equipment, since the height of claw and barrier is unaffected to collision detection, therefore are carrying out
When collision model is established, directly ignore the altitude feature of claw and barrier, and in same preset reference plane, establishes respectively
Plane geometry model corresponding with the top view of barrier and claw, due to no matter claw planar movement or when moving up and down with
Barrier, the top view of the two will necessarily have contact, therefore this collision checking method in Position of collision point at this time, according to machinery
The parameter in each joint of arm, and directly ignore the track of claw longitudinal movement, the running track of claw planar is calculated, then
Whether can be in contact with barrier model in any location point of running track by analysis claw model, to judge card
Pawl and barrier contact if it exists with the presence or absence of collision, then determine that claw and barrier exist and collide.Therefore in the present invention
Collision checking method, by Rational Simplification object model and detection track, and using simple pattern contact decision procedure come most
Determine whether to collide eventually, entire detection process is simple, and simply, calculation amount is small, and the efficiency of collision detection is greatly improved.
Referring to Fig. 2, show the collision checking method in second embodiment of the invention, including step S11 is to step
S17。
Step S11 establishes the threedimensional model of barrier and claw.
Wherein, according to barrier and the design parameter (such as length) of claw, can establish out respectively barrier and
The threedimensional model of claw.The threedimensional model of barrier and claw is placement position simultaneously, should be according to barrier and claw in reality
Placement position in the use process of border is set, to ensure the reliability of collision detection.
Step S12 chooses a preset reference plane, and there is the two dimension pre-established to sit in the preset reference plane
Mark system.
Preferably, the preset reference plane selects the top view for reflecting the barrier and the claw threedimensional model
Plane.
Step S13 projects to the threedimensional model of the barrier and the claw in the preset reference plane, with
To barrier model and claw model.
Wherein, since the barrier model and the claw model are the image projected on a perspective plane, thus it is described
Barrier model and the claw model are plane geometric figure.Further, since the preset reference plane selects vertical view side
To plane, therefore the barrier model is the vertical view figure of the threedimensional model of the barrier, and the claw model is institute
State the vertical view figure of the threedimensional model of claw.
It should be understood that the preset reference plane can also select the reflection barrier in other embodiments
And the plane of the bottom view of the claw threedimensional model, but either top view or bottom view, it can reflect the obstacle
The peripheral largest contours of object model and the claw, such as the bowing of circular cone, bottom view is the circle for being relatively large in diameter one end and being projected out.
Step S14 calculates fortune of the claw in the preset reference plane according to the parameter in each joint of mechanical arm
Row track.
Wherein, joint parameter includes the relative rotation angle between joint length and each joint.Since no matter claw is flat
When face is mobile or moves up and down and barrier, top view both at this time will necessarily have contact, therefore calculate the claw
Running track when, the running track that claw moves up and down can be ignored, directly calculate the operation of the claw in the plane
Track.
As an example it is assumed that the joint length of the first axle of tetra- shaft mechanical arm of SCARA is a1, the joint length of the second axis is
a2, then the running track of catching can be calculated in we are as follows:
X=a1cos(θ1)+a2cos(θ1+θ2)
Y=a1sin(θ1)+a2sin(θ1+θ2)
Wherein, (x, y) indicates the coordinate of claw central point,Indicate the angle that claw is turned over around center point.?
In above-mentioned expression, there is not θ3, it is because of θ3What is influenced is height locating for claw, furthermore the third of tetra- shaft mechanical arm of SCARA
Axis is the axis for driving claw to move up and down, and the 4th axis is the axis for driving claw rotation, negligible herein.
In addition to this, in other embodiments, the step S14 can also follow the steps below specific implementation:
According to the relative rotation angle between the length and each joint in each joint of the mechanical arm, the claw is calculated
Travel path；
The travel path is projected in the preset reference plane, to obtain the running track.
It should be understood that present embodiment is the space travel path for first calculating the claw, by spatial row route
Diameter projects in preset reference plane, and to obtain the plane running track of the claw, therefore present embodiment compares above-mentioned reality
Mode is applied, more computationally intensive, therefore in the specific implementation, preferred above embodiment.
Step S15, judge the claw model in any location point of the running track whether with the barrier
Model is in contact.
Wherein, when determine the claw model in any location point of the running track can and the barrier mould
When type is in contact, S17 is thened follow the steps, it is equal in any location point of the running track when determining the claw model
When not being in contact with the barrier model, otherwise representing claw will not collide with barrier, and detection passes through.
Step S17 determines that the claw and the barrier exist and collides, and issues collision standby signal.
Wherein, the mode for issuing collision standby signal includes vibration, quarter-bell, alarm, buzzing, bullet frame prompt etc..
It is described in detail below in conjunction with concrete condition, judges what the barrier model and the claw model were in contact
Specific implementation step:
First, when the barrier model and the claw model are circular configuration, judge the barrier model and
The step of claw model is in contact include:
The sum of calculate separately out the radius of the barrier model and the claw model, and calculate the two radius, with
To with reference to total radius；
Judge the center of the barrier model and the claw model away from whether being less than and described refer to total radius；
If so, determining the barrier model and the claw model is in contact.
It should be understood that since barrier model and claw model are circular configuration, and judge between two circles whether
It is in contact, it is only necessary to both judge center away from whether both being greater than radius and be otherwise in contact if more than not contacting then.
Further, since the two-dimensional coordinate system that the preset reference plane pre-establishes, thus according to barrier model and
The centre point coordinate of claw model, and according to the distance between two o'clock calculation formula, the center that both can be calculated away from.
Second, the barrier model is polygonized structure when the claw model is circular configuration, the obstacle is judged
The step of object model and the claw model are in contact include:
The reference radius of the claw model is calculated, and calculates separately out the centre point of the claw model to the barrier
Hinder the point back gauge in each sideline of object model；
Judge whether there is the described back gauge less than the reference radius；
If so, determining the barrier model and the claw model is in contact.
It should be understood that claw model is circular configuration, and judges round and polygon since barrier model is polygon
Between whether be in contact, it is only necessary to judge whether the center of circle is all larger than round radius to the distance of polygon each edge, if then
It will not contact, otherwise can contact.
Specifically, referring to Fig. 3, the step of calculating point back gauge of the centre point to barrier model boundary packet
It includes:
The coordinate of the centre point is obtained, and obtains the length and two endpoints in the barrier model either objective sideline
Coordinate；
Using the either objective endpoint in the target sideline as basic point, the target endpoint is established to the another of the target sideline
The primary vector of one endpoint, and establish the target endpoint to the centre point secondary vector (as shown in Figure 3)；
The dot product (using the point pole calculation formula of two vectors) of the primary vector and the secondary vector is calculated, and
Judge the dot product whether less than 0；
If so, calculate the target endpoint to the centre point distance, to obtain the centre point to the target side
The point back gauge of line；
If it is not, then judging whether the dot product is more than or equal to square of the length in the target sideline；
If so, calculating another endpoint in the target sideline to the distance of the centre point, to obtain the centre point
To the point back gauge in the target sideline；
If it is not, then calculating the mould of the primary vector and the secondary vector cross product, and divided by the length in the target sideline
Degree, with obtain the centre point to the target sideline point back gauge.
Wherein, it is located at due to figure in the reference planes of built good coordinate system, therefore the point side in high number can be directlyed adopt
And the calculation formula (such as dot product, two o'clock range formula) of vector correlation is handled.
It should be understood that then represent the two vectors when the dot product of vector A1 and vector A2 are less than 0 and be mutually perpendicular to, this
Time point back gauge is distance (as shown in Figure 3) of the center of circle to basic point (one of endpoint), when the dot product of vector A1 and vector A2
More than or equal to side line length square when, also represent the two vectors and be mutually perpendicular to, this time point back gauge is that another is arrived in the center of circle
The distance (as shown in Figure 3) of endpoint in other situations, both calculates the cross product mould of vector and divided by side line length, can be obtained
Point back gauge.
Third, the barrier model is polygonized structure when the claw model is circular configuration, the obstacle is judged
The step of object model and the claw model are in contact can also include:
Respectively by the barrier model and the centre point of the claw model, to each sideline of the barrier model
It is projected on the straight line at place, to be projected out a line segment and a subpoint on every straight line；
Prolong preset length to extending out at the both ends of every line segment respectively, to obtain an extension on every straight line
Line segment, the preset length are consistent with the radius of the claw model；
Judge whether the subpoint on every straight line and the extension line segment are overlapped；
If so, determining the barrier model and the claw model is in contact.
It should be understood that claw model is circular configuration since barrier model is polygon (assuming that having n side), and
Judge whether be in contact between round and polygon, it can also be by the central point of barrier model and claw respectively to barrier n
Linear projection where side.One line segment and a point can be obtained to projection each time, this is projected into obtained line
Section extends the length of r to both sides, if this subpoint is exactly collisionless except the line segment of extension.If thrown each time
The point that shadow obtains is all within the extension line segment that projection obtains, then claw will bump against with barrier.
For example, Fig. 4 is please referred to Fig. 5, it is assumed that n apex coordinate of the side the n shape of barrier is respectively (x1,y1),…
(xn,yn), then calculating this polygon can lead to the line segment obtained after linear projection where the side Suo Lian of first and second vertex
Cross following manner calculating:
Vertex (xi,yi) point that obtains to this linear projection can be expressed as
Enable lminFor li, the smallest value in i=1 ..., n enables lmaxFor li, maximum value in i=1 ..., n, then polygon
The Projection Line Segment of shape just can be with several to (lmin,lmax) indicate.Extension line segment can use (lmin-r,lmax+ r) carry out table
Show.If the center of circle (x, y) point obtained to this linear projection is expressed as
If l < lmin- r or l > lmax+ r, then claw and barrier be exactly it is collisionless (as shown in Figure 5), otherwise, into
Row projects next time, i.e. the projection to Article 2 side.If having l after all projectionsmin-r≤l≤lmax+ r, then blocking
Pawl and barrier are exactly (as shown in Figure 6) collided.
In addition to this, it is also stated that, when claw model be polygonized structure, and the barrier model be circle
When structure, the mode of the judgement contact of this structure is circular configuration with claw model, and barrier model is polygonized structure
When it is similar, can equally take above-mentioned second or the analysis mode of third carry out contact analysis.
In addition, needing to obtain position and the claw of claw central point from positive kinematics when handling polygon claw
The angle that relative standard position turns over, then can be in the hope of the coordinate on all vertex of polygon claw and the expression on line segment side
Formula.For example, working asWhen, the coordinate on some vertex isSo after positive kinematics calculate,
The coordinate on this vertex is
Fourth, judging the barrier model when the claw model and the barrier model are polygonized structure
The step of being in contact with the claw model include:
Respectively by the barrier model and the claw model, to each of the barrier model and the claw model
It is projected on straight line where sideline, to be projected out two line segments on every straight line；
Judge whether two line segments on every straight line are overlapped；
If so, determining the barrier model and the claw model is in contact.
It should be understood that being directed to such case, the method based on projection (the third above-mentioned mode) can only be used.Assuming that
Hand is grabbed as the side m shape, and barrier is the side n shape, is projected then needing to carry out m+n time, is respectively corresponded m side for setting about grabbing and barrier
N side where straight line.Every once to be projected, two polygons can obtain two lines in this projection straight line
Section, if this two lines section is non-overlapping, can directly return to collisionless, otherwise be projected next time.If for institute
Some projections, two lines section all have overlapping, then it is exactly to have collision that hand, which is grabbed with barrier,.
In addition to this, it is also necessary to which supplementary explanation can reduce projection if polygon has certain symmetry
Number.For example, only need respectively to do primary projection to two adjacent sides if polygon is rectangle, and need not
It is all once projected to four edges, because rectangular two opposite side is parallel.
In addition, if claw or barrier instead of circle or polygon, other shapes, such as it is fan-shaped, then it can extend
A polygon or round is obtained, claw or barrier can be encased.Because importantly, algorithm energy in collision detection
It is enough to export the case where collision as collision, it is possible to which that claw or barrier progress are suitably expanded into what algorithm was easily processed
Shape.If the space of extension is excessive certainly, obtained collision detection result will be overly conservative.
Another aspect of the present invention also provides a kind of collision detecting system, please refers to Fig. 6, show third embodiment of the invention
In collision detecting system, comprising:
Model building module 11, for establishing barrier model and claw model, institute respectively in a preset reference plane
The vertical view figure that barrier model is the barrier is stated, the claw model is the vertical view figure of the claw；
Trajectory computation module 12 calculates the claw in the default ginseng for the parameter according to each joint of mechanical arm
Examine the running track in plane；
Collision determination module 13, for when the claw model in any location point of the running track with the barrier
When object model being hindered to be in contact, determines that the claw and the barrier exist and collide.
Further, the barrier model and the claw model are circular configuration, the collision determination module 13
Include:
First computing unit 131 for calculating separately out the radius of the barrier model and the claw model, and is counted
The sum of the two radius is calculated, to obtain with reference to total radius；
First judging unit 132, judge the center of the barrier model and the claw model away from whether be less than it is described
With reference to total radius；
When the center of the barrier model and the claw model is away from the total radius of the reference is less than, then the collision
Determination module 13 determines the barrier model and the claw model is in contact.
Further, the claw model is circular configuration, and the barrier model is polygonized structure, and the collision is sentenced
Cover half block 13 includes:
Second computing unit 133 for calculating the reference radius of the claw model, and calculates separately out the claw
Point back gauge of the centre point of model to each sideline of the barrier model；
Second judgment unit 134, for judging whether there is described back gauge for being less than the reference radius；
When determining in the presence of described back gauge for being less than the reference radius, then the collision determination module 13 determines
The barrier model and the claw model are in contact.
Further, second computing unit 133 includes:
Parameter obtains subelement 1331, and for obtaining the coordinate of the centre point, and it is any to obtain the barrier model
The length in target sideline and two extreme coordinates；
Vector establishes subelement 1332, for establishing the mesh using the either objective endpoint in the target sideline as basic point
Mark endpoint to the target sideline another endpoint primary vector, and establish the target endpoint to the of the centre point
Two vectors；
First dot product judgment sub-unit 1333, for calculating the dot product of the primary vector and the secondary vector,
And judge the dot product whether less than 0；
First point away from computation subunit 1334, for calculating the target when determining the dot product and being less than 0
Endpoint to the centre point distance, with obtain the centre point to the target sideline point back gauge；
Second dot product judgment sub-unit 1335, for when determining the dot product and not being less than 0, judge it is described to
Whether amount dot product is more than or equal to square of the length in the target sideline；
Second point is greater than for that ought determine the dot product equal to the target sideline away from computation subunit 1336
Length square when, calculate another endpoint in the target sideline to the distance of the centre point, to obtain the center of circle
Point arrives the point back gauge in the target sideline；
Mould computation subunit 1337 is pitched, is not greater than for the dot product ought to be determined equal to the target sideline
Length square when, calculate the mould of the primary vector and the secondary vector cross product, and divided by the length in the target sideline,
With obtain the centre point to the target sideline point back gauge.
Further, the claw model is circular configuration, and the barrier model is polygonized structure, and the collision is sentenced
Cover half block 13 includes:
First projecting cell 135, for respectively by the barrier model and the centre point of the claw model, to described
It is projected on straight line where each sideline of barrier model, to be projected out a line segment and a projection on every straight line
Point；
Unit 136 is prolonged in expansion, for prolonging preset length to extending out at the both ends of every line segment respectively, with described in every
An extension line segment is obtained on straight line, the preset length is consistent with the radius of the claw model；
First overlapping judging unit 137, for judging the subpoint and the extension line segment on every straight line
Whether it is overlapped；
When determining the subpoint on every straight line and the extension line segment can be overlapped, the collision determination
Module 13 determines the barrier model and the claw model is in contact.
Further, the claw model and the barrier model are polygonized structure, the collision determination module
13 include:
Second projecting cell 138, respectively by the barrier model and the claw model, to the barrier model and
It is projected on straight line where each sideline of the claw model, to be projected out two line segments on every straight line；
Second overlapping judging unit 139, judges whether two line segments on every straight line are overlapped；
When determining two line segments on every straight line can be overlapped, the collision determination module 13 is determined
The barrier model and the claw model are in contact.
Further, the model building module 11 includes:
Model foundation unit 111, for establishing the threedimensional model of barrier and claw；
Model projection unit 112 projects to the threedimensional model of the barrier and the claw for respectively described pre-
If in reference planes, to obtain the barrier model and the claw model.
Further, the collision detecting system further include:
Plane chooses module 14, for choosing a preset reference plane, has in the preset reference plane and builds in advance
The two-dimensional coordinate system stood.
Further, the collision detecting system further include:
Cue module 15 is collided, is used for when determining the claw and the barrier in the presence of colliding, and issue collision and mention
Show signal.
The present invention also proposes a kind of computer readable storage medium, is stored thereon with computer program, and the program is processed
Such as above-mentioned collision checking method is realized when device executes.
The present invention also proposes a kind of computer equipment, including memory, processor and storage are on a memory and can be
The computer program run on processor, the processor realize such as above-mentioned collision checking method when executing described program.
It will be understood by those skilled in the art that in flow charts indicate or logic described otherwise above herein and/or
Step may be embodied in and appoint for example, being considered the order list of the executable instruction for realizing logic function
In what computer-readable medium, for instruction execution system, device or equipment (such as computer based system including processor
System or other can be from instruction execution system, device or equipment instruction fetch and the system executed instruction) use, or combine this
A little instruction execution systems, device or equipment and use.For the purpose of this specification, " computer-readable medium " can be it is any can be with
Include, store, communicate, propagate, or transport program is for instruction execution system, device or equipment or in conjunction with these instruction execution systems
System, device or equipment and the device used.
The more specific example (non-exhaustive list) of computer-readable medium include the following: there are one or more wirings
Electrical connection section (electronic device), portable computer diskette box (magnetic device), random access memory (RAM), read-only memory
(ROM), erasable edit read-only storage (EPROM or flash memory), fiber device and portable optic disk is read-only deposits
Reservoir (CDROM).In addition, computer-readable medium can even is that the paper that can print described program on it or other are suitable
Medium, because can then be edited, be interpreted or when necessary with it for example by carrying out optical scanner to paper or other media
His suitable method is handled electronically to obtain described program, is then stored in computer storage.
It should be appreciated that each section of the invention can be realized with hardware, software, firmware or their combination.Above-mentioned
In embodiment, software that multiple steps or method can be executed in memory and by suitable instruction execution system with storage
Or firmware is realized.It, and in another embodiment, can be under well known in the art for example, if realized with hardware
Any one of column technology or their combination are realized: having a logic gates for realizing logic function to data-signal
Discrete logic, with suitable combinational logic gate circuit specific integrated circuit, programmable gate array (PGA), scene
Programmable gate array (FPGA) etc..
In the description of this specification, reference term " one embodiment ", " some embodiments ", " example ", " specifically show
The description of example " or " some examples " etc. means specific features, structure, material or spy described in conjunction with this embodiment or example
Point is included at least one embodiment or example of the invention.In the present specification, schematic expression of the above terms are not
Centainly refer to identical embodiment or example.Moreover, particular features, structures, materials, or characteristics described can be any
One or more embodiment or examples in can be combined in any suitable manner.
The embodiments described above only express several embodiments of the present invention, and the description thereof is more specific and detailed, but simultaneously
Limitations on the scope of the patent of the present invention therefore cannot be interpreted as.It should be pointed out that for those of ordinary skill in the art
For, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to guarantor of the invention
Protect range.Therefore, the scope of protection of the patent of the invention shall be subject to the appended claims.
Claims (10)
1. a kind of collision checking method characterized by comprising
In a preset reference plane, barrier model and claw model are established respectively, and the barrier model is the obstacle
The vertical view figure of object, the claw model are the vertical view figure of the claw；
According to the parameter in each joint of mechanical arm, running track of the claw in the preset reference plane is calculated；
When the claw model is in contact in any location point of the running track with the barrier model, determine
The claw and the barrier, which exist, to be collided.
2. collision checking method according to claim 1, which is characterized in that the barrier model and the claw model
It is circular configuration, judges that the step of barrier model and the claw model are in contact includes:
The sum of calculate separately out the radius of the barrier model and the claw model, and calculate the two radius, to be joined
Examine total radius；
Judge the center of the barrier model and the claw model away from whether being less than and described refer to total radius；
If so, determining the barrier model and the claw model is in contact.
3. collision checking method according to claim 1, which is characterized in that the claw model is circular configuration, described
Barrier model is polygonized structure, judges that the step of barrier model and the claw model are in contact includes:
The reference radius of the claw model is calculated, and calculates separately out the centre point of the claw model to the barrier
The point back gauge in each sideline of model；
Judge whether there is the described back gauge less than the reference radius；
If so, determining the barrier model and the claw model is in contact.
4. collision checking method according to claim 3, which is characterized in that calculate the centre point to the barrier mould
The step of point back gauge in type sideline includes:
The coordinate of the centre point is obtained, and obtains the length and two endpoints seats in the barrier model either objective sideline
Mark；
Using the either objective endpoint in the target sideline as basic point, establish the target endpoint to the target sideline another
The primary vector of endpoint, and establish the target endpoint to the centre point secondary vector；
The dot product of the primary vector and the secondary vector is calculated, and judges the dot product whether less than 0；
If being less than 0, calculate the target endpoint to the centre point distance, to obtain the centre point to the target
The point back gauge in sideline；
If not then judging whether the dot product is more than or equal to square of the length in the target sideline less than 0；
If so, calculating another endpoint in the target sideline to the distance of the centre point, to obtain the centre point to institute
State the point back gauge in target sideline；
If it is not, then calculate the mould of the primary vector and the secondary vector cross product, and divided by the length in the target sideline, with
Obtain the centre point to the target sideline point back gauge.
5. collision checking method according to claim 1, which is characterized in that the claw model is circular configuration, described
Barrier model is polygonized structure, judges that the step of barrier model and the claw model are in contact includes:
Respectively by the barrier model and the centre point of the claw model, to where each sideline of the barrier model
Straight line on project, to be projected out a line segment and a subpoint on every straight line；
Prolong preset length to extending out at the both ends of every line segment respectively, to obtain an extended line on every straight line
Section, the preset length are consistent with the radius of the claw model；
Judge whether the subpoint on every straight line and the extension line segment are overlapped；
If so, determining the barrier model and the claw model is in contact.
6. collision checking method according to claim 1, which is characterized in that the claw model and the barrier model
It is polygonized structure, judges that the step of barrier model and the claw model are in contact includes:
Respectively by the barrier model and the claw model, to each side of the barrier model and the claw model
It is projected on straight line where line, to be projected out two line segments on every straight line；
Judge whether two line segments on every straight line are overlapped；
If so, determining the barrier model and the claw model is in contact.
7. collision checking method according to claim 1, which is characterized in that it is described in a preset reference plane, respectively
The step of establishing barrier model and claw model include:
Establish the threedimensional model of barrier and claw；
The threedimensional model of the barrier and the claw is projected in the preset reference plane respectively, to obtain the barrier
Hinder object model and the claw model.
8. a kind of collision detecting system characterized by comprising
Model building module, for establishing barrier model and claw model, the obstacle respectively in a preset reference plane
Object model is the vertical view figure of the barrier, and the claw model is the vertical view figure of the claw；
Trajectory computation module calculates the claw in the preset reference plane for the parameter according to each joint of mechanical arm
Interior running track；
Collision determination module, for when the claw model in any location point of the running track with the barrier mould
When type is in contact, determines that the claw and the barrier exist and collide.
9. a kind of computer readable storage medium, is stored thereon with computer program, which is characterized in that the program is held by processor
Method as claimed in claim 1 is realized when row.
10. a kind of computer equipment, can run on a memory and on a processor including memory, processor and storage
Computer program, which is characterized in that the processor realizes side as claimed in claim 1 when executing described program
Method.
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 true CN108959753A (en) | 2018-12-07 |
| CN108959753B 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) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109741461A (en) * | 2018-12-18 | 2019-05-10 | 南京东巴电子科技有限公司 | The method of collision detection is realized during Three Dimensional Dynamic Simulation arrangement and method for construction |
| CN110941940A (en) * | 2019-12-04 | 2020-03-31 | 杭州广立微电子有限公司 | 3D winding method, storage device and system based on collision detection |
| CN113721618A (en) * | 2021-08-30 | 2021-11-30 | 中科新松有限公司 | Plane determination method, device, equipment and storage medium |
| CN115272379A (en) * | 2022-08-03 | 2022-11-01 | 杭州新迪数字工程系统有限公司 | Projection-based three-dimensional grid model outline extraction method and system |
| CN116541939A (en) * | 2023-05-25 | 2023-08-04 | 驻马店市博瑞建筑工程有限公司 | 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 (2)
| Title |
|---|
| 汪首坤等: "基于A~*改进算法的机械臂避障路径规划", 《北京理工大学学报》 * |
| 汪首坤等: "基于A~*改进算法的机械臂避障路径规划", 《北京理工大学学报》, vol. 31, no. 11, 15 November 2011 (2011-11-15), pages 1302 - 1306 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109741461A (en) * | 2018-12-18 | 2019-05-10 | 南京东巴电子科技有限公司 | The method of collision detection is realized during Three Dimensional Dynamic Simulation arrangement and method for construction |
| CN109741461B (en) * | 2018-12-18 | 2023-04-07 | 南京东巴电子科技有限公司 | Method for realizing collision detection in three-dimensional dynamic simulation construction scheme process |
| CN110941940A (en) * | 2019-12-04 | 2020-03-31 | 杭州广立微电子有限公司 | 3D winding method, storage device and system based on collision detection |
| CN110941940B (en) * | 2019-12-04 | 2023-06-13 | 杭州广立微电子股份有限公司 | 3D winding method, storage device and system based on collision detection |
| CN113721618A (en) * | 2021-08-30 | 2021-11-30 | 中科新松有限公司 | Plane determination method, device, equipment and storage medium |
| CN115272379A (en) * | 2022-08-03 | 2022-11-01 | 杭州新迪数字工程系统有限公司 | Projection-based three-dimensional grid model outline extraction method and system |
| CN115272379B (en) * | 2022-08-03 | 2023-11-28 | 上海新迪数字技术有限公司 | Projection-based three-dimensional grid model outline extraction method and system |
| CN116541939A (en) * | 2023-05-25 | 2023-08-04 | 驻马店市博瑞建筑工程有限公司 | Collision detection method and system in bridge design |
| CN116541939B (en) * | 2023-05-25 | 2024-01-30 | 驻马店市博瑞建筑工程有限公司 | Collision detection method and system in bridge design |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108959753B (en) | 2023-08-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108959753A (en) | Collision checking method, system, readable storage medium storing program for executing and computer equipment | |
| WO2021232669A1 (en) | Robot motion planning method, path planning method, grabbing method and devices thereof | |
| Danner et al. | Randomized planning for short inspection paths | |
| US9630320B1 (en) | Detection and reconstruction of an environment to facilitate robotic interaction with the environment | |
| US9327406B1 (en) | Object segmentation based on detected object-specific visual cues | |
| CN111707269B (en) | Unmanned aerial vehicle path planning method in three-dimensional environment | |
| CN110489202A (en) | The method quickly evaluated blocked for the perspectivity in the emulation of imaging sensor | |
| TWI680364B (en) | Coach apparatus and cooperative operation controlling method for coach-driven multi-robot cooperative operation system | |
| Janabi-Sharifi et al. | Automatic selection of image features for visual servoing | |
| Lim et al. | Mobile robot relocation from echolocation constraints | |
| Ge et al. | Boundary following and globally convergent path planning using instant goals | |
| CN110203325A (en) | The test method and system of the collision prevention function of ship autonomous navigation system | |
| JP6365207B2 (en) | Reflector position inspection method for automatic guided vehicle system and reflector position inspection system for automatic guided vehicle system | |
| CN109446886A (en) | Obstacle detection method, device, equipment and storage medium based on unmanned vehicle | |
| Chen et al. | An enhanced dynamic Delaunay triangulation-based path planning algorithm for autonomous mobile robot navigation | |
| CN113051730A (en) | Hybrid collision detection method based on convex hull and pre-judgment | |
| JP5885003B2 (en) | ROBOT TRAVEL RUNNING AREA CHECK METHOD, ROBOT, AND RECORDING MEDIUM | |
| CN113211495A (en) | Mechanical arm collision detection method and system, storage medium and mechanical arm | |
| Katsuki et al. | Design of an artificial mark to determine 3D pose by monocular vision | |
| Lane et al. | An efficient point in polyhedron algorithm | |
| CN109260709A (en) | A kind of generation method and device of dynamic route | |
| González-Banos et al. | Robot navigation for automatic model construction using safe regions | |
| CN110356862A (en) | A kind of stacker-reclaimer collision-proof method and system based on GPS positioning | |
| CN105719310B (en) | Collision detection method and device | |
| CN115908541A (en) | Curvature information-based vehicle-mounted laser radar point cloud clustering algorithm for indoor environment |
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 |