JPH06238581A - Modelling of objective, judgement of interference between objectives, and device therefor - Google Patents

Abstract

PURPOSE:To improve the judgement precision for the existence of the interference between objectives by modelling the outer shape of an objective article from a polyhedron, modeling the objective from a collected body consisting of a plurality of spheres, and judging if each spherical collected body belonging to each objective has a common region or not. CONSTITUTION:The outer shape of an objective is divided 1 to a plurality of drawing elements, and a polyhedron model approximate to the outer shape is formed 2, and a plurality of apexes belonging to the same drawing element are extracted 3 from the polyhedron model, and a sphere including all the extracted apexes is detected 4. Then, it is judged 5 if the error between the sphere and the outer shape of the objective is less than a prescribed value or not, and if the error is less than the prescribed value and the apex which is not processed exists 6, a plurality of apexes are extracted 8 from the residual apexes. Then, a composite spherical body is obtained by carrying out the joint calculation 9 so that the pertinent region of all the spheres detected for all drawing elements forms the sum set and/or the pruduct set, and a series of processings are completed.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of modeling objects, a method of discriminating interference between objects, and these devices. More specifically, the presence or absence of interference between objects, at least one of which is a moving object. The present invention relates to a modeling method and a device thereof suitable for determining, and a method and a device thereof for determining the presence or absence of interference between objects based on a modeling result by the modeling method or the device.

[0002]

2. Description of the Related Art Conventionally, the introduction and utilization of manipulators represented by industrial robots have been activated in various fields. Such a manipulator is not operated in an environment in which there are no obstacles around it, but a work object, a work tool for replacement, and the like are arranged in the operation area. Therefore, the operation of the manipulator must be controlled so as not to interfere with the work object, the replacement work tool, etc. from the beginning when the introduction of the manipulator is intended.
That is, it is necessary to determine whether or not there is interference between the manipulator and another object, and operate the manipulator so that the interference does not occur.

In order to meet such a demand, a method has conventionally been proposed for generating an operation path including obstacle avoidance off-line. This method is a method of simulating a target trajectory in advance off-line and checking whether or not the manipulator causes a collision in the motion along the trajectory. The collision is avoided by obtaining another trajectory without reaching the target position and causing the actual manipulator to perform the operation along the trajectory.

However, even if a trajectory capable of avoiding such a collision is generated offline, there is a risk that the manipulator may perform an unexpected operation due to an operator's operation error, unexpected disturbance, and the like. In such a case, it cannot be dealt with at all. In consideration of such inconvenience, as a method for avoiding collision of manipulators online, as an obstacle / singularity avoidance algorithm for a 6-DOF manipulator using a configuration graph, a polyhedron of links and obstacles constituting a manipulator are used. Model a polyhedron of objects as a collection of spheres with a constant radius, perform rough interference check in this state, perform movable joint angle calculation, and then check interference between polyhedrons to which a pair of spheres with a high possibility of interference belong.
Accurate calculation of movable joint angles {"Abstract / singularity avoidance algorithm for 6-DOF manipulator using configuration graph", Nobuhiko Kida, Hitoshi Yano, Hiroshi Noboi, 9th Annual Meeting of the Robotics Society of Japan ( (November 27, 28, 29, 1991)} and the like are proposed.

[0005]

In the above-mentioned "obstacle / singularity avoidance algorithm for a 6-DOF manipulator using a configuration graph", links,
The polyhedron that constitutes the obstacle is expressed as an aggregate of multiple spheres, but since the radii of all spheres are the same, the error between the aggregate of spheres and the polyhedron is reduced to improve the accuracy of interference check. If this is done, the radius of the sphere to be adopted must be reduced, and the number of spheres for expressing the polyhedron will be significantly increased. That is, since the number of spheres, which are the minimum unit of the interference check process, is significantly increased, the time required for the interference check between the link and the obstacle becomes significantly long. Therefore, it becomes almost impossible to carry out online interference check in real time.

On the contrary, if the radius of the sphere used is increased,
It is possible to reduce the number of spheres, which is the minimum unit for interference check processing,
It is possible to shorten the time required for the interference check between the link and the obstacle, but in this case, the error between the sphere assembly and the polyhedron becomes large, and it is not possible to achieve highly accurate interference check. Will end up. In particular, when applied to a die polishing device, the link is likely to come very close to the target work, so unless a highly accurate interference check is performed, most of the polishing operation will actually interfere with the work. Therefore, it becomes impossible to perform the scribing motion only at the very beginning.
Further, when operating two manipulators at the same time, if the accuracy of the interference check is low, the movable range of each manipulator is significantly restricted, which may cause a problem that work efficiency is reduced. There is.

[0007]

SUMMARY OF THE INVENTION The present invention has been made in view of the above problems, and it is an object of the present invention to provide a novel modeling method and apparatus capable of accurately modeling the outer shape of an object with a small number of spheres. A second object of the present invention is to provide an interference discrimination method and an apparatus therefor capable of performing highly accurate interference discrimination in a short time based on a compound sphere model obtained by the modeling method or the apparatus thereof. I am trying.

[0008]

In order to achieve the above first object, a method of modeling an object according to claim 1 is to divide an outer shape of the object into a plurality of surface elements, and Approximate with a polyhedron, and in response to the designation of some vertices of the polyhedron, set a sphere that includes all relevant vertices on the relevant side and has an error with respect to the outer shape of the object that is less than or equal to a predetermined value. ,
Repeat the setting of the sphere including each vertex of the polyhedron as many times as necessary,
This is a method of modeling a target object with a plurality of spheres by performing a connection operation between three-dimensional regions defined by all the set spheres.

According to the object modeling method of claim 2, in response to the fact that the sphere passing through all the specified vertices is not determined, the weighted 2 of the distance between each vertex and the corresponding point on the sphere is used.
This is a method of setting a sphere that minimizes the sum of multiplications. The object modeling apparatus according to claim 3 specifies an outer shape dividing means for dividing the outer shape of the object into a plurality of surface elements, a polyhedral approximation means for approximating each surface element with a polyhedron, and a part of vertices of the polyhedron. Vertex specifying means, a sphere setting means for setting all the specified vertices on the corresponding side and a sphere having an error with respect to the outer shape of the object of not more than a predetermined value, and a sphere including each vertex of the polyhedron It includes an iterative control means for iterating a required number of times, and a joint operation means for modeling an object with a plurality of spheres by performing a joint operation of three-dimensional regions defined by all the set spheres. .

In order to achieve the above-mentioned second object, a method for discriminating interference between objects according to claim 4 is one in which two objects are modeled by a plurality of spheres by the method according to claim 1 or claim 2. A method of determining the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object, and performing a logical operation based on the determination result to obtain a determination result indicating the presence or absence of interference.

According to a fifth aspect of the present invention, there is provided a method of discriminating interference between objects, which is less accurate than the modeling spheres obtained by the method of the first or second aspect, and has a smaller number of modeling spheres. Is modeled, the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object is determined, and only the portion corresponding to the modeling sphere determined to have the common area is invoiced. Based on the modeled sphere obtained by the method of claim 1 or claim 2, the presence or absence of a common area between the modeled sphere of one object and the modeled sphere of the other object is determined, and based on the determination result. This is a method of performing a logical operation to obtain a determination result indicating the presence or absence of interference.

According to a sixth aspect of the present invention, there is provided a method of discriminating interference between objects, in which at least one of the objects is a moving object, the position and orientation after a predetermined time has elapsed based on the moving speed and / or the moving acceleration of the moving object. Is calculated, and the processing of claim 4 or claim 5 is performed based on the calculated position and orientation. According to a seventh aspect of the present invention, there is provided an apparatus for discriminating interference between two objects, each of which is modeled as a plurality of spheres by the apparatus of the third aspect. Common area presence / absence determining means for determining the presence / absence of a common area with one modeled sphere, repetitive control means for performing determination by the common area presence / absence determining means for all modeled spheres of both objects, and common area presence / absence And a logical operation unit for performing a logical operation based on all the determination results obtained by the determination unit and outputting the result as a determination result indicating the presence or absence of interference.

According to another aspect of the present invention, there is provided a device for determining interference between objects, which models the outer shape of the object using a modeling sphere having a lower approximation accuracy and a smaller number than the modeling sphere obtained by the device according to claim 3. Sphere setting means for preliminary discrimination obtained as a modeled sphere,
Preliminary discriminating means for discriminating the presence or absence of a common area between the modeled sphere of one object and the modeled sphere of the other object, and the discrimination by the preliminary discriminating means for all modeled spheres of both objects Selection control means for performing the processing by the common area presence / absence determination means, the repetition control means, and the logical operation means only on the preliminary iteration control means and the portion corresponding to the modeling sphere determined to have the common area by the preliminary determination means. It also includes and.

According to a ninth aspect of the present invention, there is provided an apparatus for discriminating interference between objects, wherein, when at least one of the objects is a moving object, the position and orientation after a predetermined time has elapsed based on the moving speed and / or the moving acceleration of the moving object. Position / orientation calculation means for calculating
It further includes target state setting means for setting the calculated position and orientation as the position and orientation to be subjected to interference discrimination.

[0015]

According to the object modeling method of claim 1, all the vertices corresponding to the object are approximated by a polyhedron, and all the vertices corresponding to the vertices of the polyhedron are designated. A sphere which is included on the side of the polygon and whose error with respect to the outer shape of the object is equal to or less than a predetermined value is set, and the setting of the sphere including each vertex of the polyhedron is repeated as many times as necessary, and is defined by all the set spheres 3 Since the object is modeled with multiple spheres by performing a joint operation between dimensional areas, the radius of the sphere is set appropriately according to the partial outline of the object, and the inside or outside of the sphere is set. By the choice, a significantly higher accuracy of modeling can be achieved with a significantly smaller number of spheres compared to conventional methods. In particular, the outer shape of the object having the recessed portion can be modeled with high accuracy using a small number of spheres.

According to the object modeling method of claim 2, in response to the fact that the sphere passing through all the specified vertices is not determined, the weighting of the distance between each vertex and the corresponding point on the spherical surface is performed. Since the sphere that minimizes the sum of squares is set, it is possible to prevent the error from becoming too large locally and achieve high-accuracy modeling. In particular, it is possible to prevent the error between the sphere and the outer shape from increasing at a portion where the vertex intervals of the polyhedron are large.

According to the object modeling apparatus of claim 3, the outline of the object is approximated by a polyhedron by the polyhedron approximating means, part of the vertices of the approximated polyhedron is designated by the vertex designating means, and the sphere setting means. Thus, a sphere that includes all the specified vertices on the corresponding side and has an error with respect to the outer shape of the object that is equal to or less than a predetermined value is set. Then, the iterative control means repeats the setting of the sphere including the respective vertices of the polyhedron a required number of times, and performs the join operation between the three-dimensional regions defined by all the spheres set by the join operation means to obtain the object. Is modeled by multiple spheres. Therefore, by appropriately setting the radius of the sphere according to the partial outline of the object and selecting the inside of the sphere or the outside of the sphere, a model with a significantly high accuracy can be obtained with a significantly smaller number of spheres than the conventional method. Can be achieved. In particular, the outer shape of the object having the recessed portion can be modeled with high accuracy using a small number of spheres.

According to the method of discriminating interference between the objects of claim 4, two objects are modeled by a plurality of spheres by the method of claim 1 or 2, and one object is modeled as a sphere. Since the presence or absence of a common area with the modeling sphere of the other object is determined and a logical operation based on the determination result is performed to obtain a determination result indicating the presence or absence of interference, each object is modeled with high accuracy. A processing result indicating the presence or absence of a common area between a relatively small number of spheres can be easily obtained based on, for example, the center-to-center distance and the radius, and the logical operation based on the processing results for all spheres can be used to The presence or absence of interference between objects can be determined. Therefore, each object can be modeled with extremely high accuracy with a significantly smaller number of spheres compared to the conventional method, and after modeling, a detection process of whether or not a small number of spheres have a common area, all spheres Since it suffices to perform a logical operation based on the detection result of, the presence / absence of interference between objects can be easily determined as a whole with high accuracy.

According to the interference discrimination method between the objects of claim 5, the modeling accuracy is lower than that of the modeling sphere obtained by the method of claim 1 or 2, and the number of modeling spheres is small. The outer shape of the model is modeled, the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object is determined, and only the portion corresponding to the modeling sphere that is determined to have a common area exists Based on the modeling sphere obtained by the method according to claim 1 or 2, the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object is determined, and the determination result Therefore, the presence / absence of the common area is roughly determined in a short time on the basis of a small number of modeled spheres, because the determination result indicating the presence / absence of the interference is obtained by performing the logical operation based on. By this discrimination, it is possible to limit the region in which it is necessary to perform the highly accurate interference discrimination, and thereafter, the interference discrimination may be performed based on the modeling sphere that can approximate the limited region with high accuracy. As a result, the time required for the interference determination can be significantly reduced without lowering the accuracy of the interference determination.

According to the method of determining the interference between the objects of claim 6, when at least one of the objects is a moving object, the position after a predetermined time has elapsed based on the moving speed and / or the moving acceleration of the moving object. Since the posture is calculated and the processing of claim 4 or 5 is performed based on the calculated position and posture, it is actually determined whether the position and posture after a predetermined time causes interference. The position where the moving object corresponds to
It can be determined before the posture. As a result, it is possible to determine the presence or absence of interference before the moving object actually interferes with another target object, and it is possible to prevent a collision from occurring by performing a necessary avoidance operation based on the determination result.

In the case of the device for discriminating interference between objects according to claim 7, one of the objects is discriminated by the common area presence / absence discriminating means for two objects modeled by a plurality of spheres by the device according to claim 3. Existence of a common area between the one modeled sphere of No. 1 and the one modeled sphere of the other object, and the repetitive control means makes a judgment by the common area existence judgment means for all the modeled spheres of both objects. To perform. Then, based on all the determination results obtained by the common area presence / absence determining means, a logical operation is performed by the logical operation means and the result is output as a determination result indicating the presence or absence of interference. Therefore,
A processing result indicating the presence or absence of a common area of a relatively small number of spheres that model each object with high accuracy can be easily obtained based on, for example, the center-to-center distance and the radius, and processing for all spheres can be performed. By performing a logical operation based on the result, it is possible to determine the presence or absence of interference between the objects. As a result, each object can be modeled with extremely high accuracy with a significantly smaller number of spheres compared to the conventional method, and after modeling,
Since it is only necessary to perform a detection process as to whether or not a small number of spheres have a common area, and a logical operation process based on the detection results for all spheres, interference between the objects is easy and highly accurate as a whole. The presence or absence of can be determined.

In the case of the device for discriminating interference between objects according to claim 8, the preliminary discriminating sphere setting means makes modeling with a lower approximation accuracy and a smaller number than the modeled sphere obtained by the device according to claim 3. The sphere is modeled as the outer shape of the object, and the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object is determined by the preliminary determination means, and the preliminary iteration control means determines All the modeled spheres of the object are discriminated by the preliminary discrimination means. Then, the selection control means causes the common area presence / absence determining means, the iterative control means, and the logical operation means to perform processing only on the portion corresponding to the modeling sphere for which the preliminary determining means determines that the common area exists. Therefore, the presence or absence of the common area is roughly determined in a short time based on a small number of modeled spheres, and by this determination, the area for which it is necessary to perform the interference determination with high accuracy can be limited. As a result, it is sufficient to perform the interference discrimination based on the modeled sphere capable of approximating the limited region with high accuracy, and therefore the time required for the interference discrimination can be significantly shortened without lowering the accuracy of the interference discrimination.

According to the interference discrimination apparatus between the objects of claim 9, when at least one of the objects is a moving object, the position / orientation calculating means determines the moving speed and / or the moving acceleration of the moving object. Position and posture after a predetermined time has elapsed, and the calculated position by the target state setting means,
The posture can be set as a position and a posture which are targets of interference discrimination. Therefore, it is possible to determine whether or not the position / orientation after a predetermined time causes interference, until the moving object actually reaches the corresponding position / orientation. As a result, it is possible to determine the presence or absence of interference before the moving object actually interferes with another target object, and it is possible to prevent a collision from occurring by performing a necessary avoidance operation based on the determination result.

[0024]

Embodiments will be described in detail below with reference to the accompanying drawings showing embodiments. FIG. 1 is a flow chart for explaining an embodiment of the object modeling method according to the present invention.
In 1, the outline of the object is divided into a plurality of surface elements, a polyhedral model approximating the outline of the object is created in step SP2, and a plurality of vertices belonging to the same surface element are extracted from the polyhedral model in step SP3. In step SP4, a sphere that includes all the extracted vertices on the boundary surface or on the corresponding side with the boundary surface as a reference (hereinafter, simply referred to as "include") is detected, and in step SP5, it is detected. It is determined whether the error between the sphere and the outer shape of the object is less than or equal to a predetermined value, and if the error is not less than or equal to the predetermined value, the process of step SP4 is performed again to detect another sphere.

On the contrary, if it is determined in step SP5 that the error is less than or equal to the predetermined value, it is determined in step SP6 whether or not all the vertices of the polyhedron have been processed, and no processing is performed. When it is determined that the vertices are present, a plurality of vertices are extracted from the remaining vertices in step SP7 and the step S is performed again.
Perform the process of P4. When it is determined in step SP6 that all the vertices of the polyhedron have been processed, in step SP8, the corresponding regions of all the spheres detected for all the surface elements are unioned and / or producted with each other. The composite calculation is performed to obtain a composite spherical body, and the series of processing is terminated.

If a sphere whose error is less than a predetermined value cannot be extracted even after performing the above series of processing, the error is reduced to a predetermined value by, for example, increasing the number of vertices and recreating the polyhedral model. The following spheres can be extracted. In the processing of steps SP2 and SP3 described above, for example, when the object is a manipulator, it is possible to empirically recognize in advance whether the possibility of interference is large. Therefore, the size of the polyhedron approximated based on the recognition result, It suffices to set the number of extraction points to some extent, it is possible to obtain a compound spherical surface with a minimum number of spheres, and it is possible to maintain high accuracy of interference discrimination.

The process of step SP4 will be described in detail. However, in the following description, it is assumed that the inside of the sphere is the relevant region. If three arbitrary vertices (excluding the case where the three vertices are located on the same straight line), four vertices that are rectangular vertices, and n vertices that are regular n-gon vertices are extracted, all of these vertices It is possible to obtain a sphere that includes the on the boundary surface, but the sphere cannot be uniquely determined. Therefore, for example, a sphere having the maximum radius that can be handled by a computer or the like for calculation is provisionally determined. And
If the error between the sphere thus determined and the outer shape is larger than a predetermined value, the number of vertices is increased and a polyhedral model is created again. Specifically, in the case where the relationship between one plane forming a polyhedron and a sphere is given as shown in FIG. 2 (in the figure, r is a radius, e is an error, and l is the above-mentioned one plane. indicates the length of each), the possible range of values for the calculation are given in 3.4E-38~3.4E + 38, the radius of the maximum value R _max of 10 ¹⁰ m, the error e 10 ^{- 4} m
When set to (1/2) ² + (r−e) ² = r ²
Therefore, l = 2 {e (2r−e)} ^1/2 . By substituting r = 10 ¹⁰ and e = 10 ^-4 into this equation,
l = 2.828 × 10 ³ . That is, the radius is 10 ¹⁰ m
It can be seen that a plane with a length of 2.828 km can be modeled with an error of 0.1 mm by using one sphere of
Therefore, in most cases, the model sphere can be set so that the error is equal to or less than the predetermined value.

Further, when four arbitrary vertices, 2n vertices which are vertices of an n-prism, and a plurality of vertices which are vertices of a regular polyhedron are extracted, a sphere including all of these vertices on the boundary surface is unique. This ball is adopted because it is fixed. Furthermore, if a sphere that includes multiple arbitrary vertices on the boundary surface cannot be obtained, assume an arbitrary sphere that includes all vertices, and consider each vertex and the point on the sphere with respect to the assumed sphere. The center coordinate value and radius of the sphere that is assumed to minimize the weighted sum of squares of the distance are calculated. Specifically, the central coordinate value and the radius can be calculated by applying the Lagrange's inclination method or the like.

FIGS. 3A and 3B respectively show states in which a sphere having four vertices is assumed. Figure 3 (A)
At, there are vertices that are significantly distant from the points on the sphere, so the weighted sum of squares is quite large. On the other hand, in FIG. 3B, all the vertices are fairly close to the points on the spherical surface, and therefore the weighted sum of squares is minimized. That is, FIG.
It will be modeled based on the sphere of (B).

The processing of step SP8 will be described in detail. For example, in the case where the object has the outer shape shown in FIG. 4A and this outer shape is divided into four surface elements S ₁ , S ₂ , S ₃ , and S ₄ , the surface element S ₁ is If the spheres that can be modeled with high accuracy are S ₁₁ and S ₁₂ , and the spheres that can model other surface elements with high accuracy are S ₂₁ , S ₃₁ , and S ₄₁ , respectively, spheres S ₁₁ and S ₁₂ A complex sphere (a model in which one sphere is modeled by combining a plurality of spheres) that can approximate the surface element S ₁ with high precision can be obtained by obtaining the sum region of {see FIG. 4B}. Then, the object can be modeled with high accuracy by obtaining the intersection of this composite sphere and the other spheres S ₂₁ , S ₃₁ , and S ₄₁ (see FIG. 4C).

When it is specifically expressed by a mathematical expression, it is as shown in Equation 1.

[0032]

[Equation 1]

As is clear from the above description, by adopting this embodiment, it is possible to accurately model the outer shape of various objects with a relatively small number of spheres.
In particular, even if the outer shape of the object has a recessed portion or the like, it can be modeled with high accuracy without increasing the number of extra spheres.

[0034]

Concrete Example A case of modeling a gripper having the shape shown in FIG. 5 will be described. However, the position indicated by x in FIG. 5A is the origin, and the x-axis and the y-axis are set in the rightward and upward directions, respectively. In addition, the coordinate values of a plurality of points are shown in FIG.
It is set as shown in (A). Then, for the sake of simplicity, only the xy plane will be described.

In this case, a polyhedron including the outer shape shown in FIG. 5 (A) is created {see FIG. 5 (B)}, and a model is prepared so that the error is below a predetermined value for each plane forming the polyhedron. Specifies whether to set a fossilized sphere or a sphere that includes vertices on the boundary surface. Specifically, for example, for the plane marked with a circle in FIG. 5B, it is instructed that the modeling sphere should be set so that the error is equal to or less than a predetermined value (eg, 0.1 or less). It is instructed that a sphere including vertices on the boundary surface should be set for a plane with a triangle mark.

FIG. 5C shows the result of creating a modeled sphere based on the above-mentioned designation. The six spheres O1, O2 and
..., high-precision modeling has been achieved by O6. In this case, the center positions of the spheres O1, O2, ..., O6 are (0, 17.786001) (1
5, -94.95) (509.95, 20) (0, -4)
89.95) (-509.95, 20) (-15, -9)
4.95) and the radii are 500.05 and 2 respectively.
3.49639, 125.05, 500.05, 50
It was 0.05 and 500.05.

[0037]

[Embodiment 2] FIG. 6 is a block diagram showing an embodiment of the object modeling apparatus according to the present invention, in which a partition portion 1 for partitioning the outer shape of the object into a plurality of surface elements and the outer shape of the object are shown. A polyhedron model creation unit 2 that creates an approximate polyhedron model, a vertex extraction unit 3 that extracts a plurality of vertices belonging to the same surface element from the polyhedron model, and a sphere that detects a sphere that contains all the extracted vertices. The detection unit 4, the error determination unit 5 that determines whether the error between the detected sphere and the outer shape of the object is less than or equal to a predetermined value, and the determination of the error determination unit 5 that indicates that the error is not less than or equal to the predetermined value. In response to the determination result of the first iteration control unit 6 that repeatedly operates the sphere detection unit 4 to detect another sphere in response to the result, and the determination result of the error determination unit 5 that indicates that the error is less than or equal to a predetermined value, A process to determine whether all vertices of a polyhedron have been processed. In response to the discrimination result of the processed discriminating unit 7 and the processed discriminating unit 7 indicating that there are unprocessed vertices, the vertex extracting unit extracts a plurality of vertices from the remaining vertices. A vertex extraction control unit 8 for controlling 3;
In response to the determination result of the processed determination unit 7 indicating that all the vertices of the polyhedron have been processed, the corresponding regions of all the spheres detected for all the surface elements are unioned and / or multiplied with each other. And a combining operation unit 9 for performing a combination operation so as to obtain a composite spherical body.

Since the operation of each of the above-mentioned components is the same as the process of the corresponding step of the flowchart of FIG. 1,
Detailed description is omitted. Therefore, even when this embodiment is adopted, the outer shapes of various objects can be modeled with high accuracy using a relatively small number of spheres. In particular, even if the outer shape of the object has a recessed portion or the like, it can be modeled with high accuracy without increasing the number of extra spheres.

[0039]

[Third Embodiment] FIG. 7 is a flow chart for explaining an embodiment of the method for discriminating interference between objects according to the present invention. Two objects are processed by performing processing based on the flowchart of FIG. 1 or the block diagram of FIG. The processing after obtaining the composite spherical body that models each of the above with high accuracy is described.

In step SP1, one sphere (hereinafter, referred to as the first sphere) that constitutes the composite spherical body of one of the objects.
Is selected, in step SP2, one sphere (hereinafter, referred to as a second sphere) that constitutes the composite spherical body of the other object is selected, and in step SP3, the center coordinate value, the radius, and the first sphere of the first sphere are selected. It is determined whether or not the common area of the measuring sphere exists based on the center coordinate value and radius of the two spheres. For example, by determining the magnitude of the distance between the centers of both spheres and the sum of the radii of both spheres, it is possible to determine whether or not a common area exists. Specifically, this determination can be achieved by performing vector subtraction, vector inner product operation, scalar addition, scalar multiplication, and real number comparison once. Then, in step SP4 or step SP5, the discrimination result is represented by binary data. Therefore,
For example, "1" corresponds to the existence of the common area, and "0" corresponds to the absence of the common area.

After the determination in step SP4 or step SP5 is made, in step SP6 the first ball,
It is determined whether or not the above processing has been performed for all the combinations of the second balls, and if there is a combination for which the above processing has not been performed, the processing of step SP1 is performed again. On the contrary, if it is determined in step SP6 that the above processing has been performed for all combinations, logical operation is performed based on all binary data in step SP7, and the logical operation result is output as the interference determination result.
A series of processing ends.

A more detailed description will be given. For example, as shown in FIG. 8, a case will be described in which the outer shape of one object j is modeled by spheres j1, j2, j3, and the outer shape of the other object k is modeled by spheres k1, k2. Where sphere j1,
The space surrounded by j2 and j3 is S _j , each sphere is S _j1 ,
The space surrounded by S _{j 2} , S _j3 and the spheres k1, _k2 is represented by S _k , and each sphere is represented by S _k1 , S _k2 .

Under this premise, the equations 2 and 3 hold.

[0044]

[Equation 2]

[0045]

[Equation 3]

However, each sphere has a meaning on condition that the equation 4 is satisfied.

[0047]

[Equation 4]

The condition that the object j and the object k interfere with each other is that the common area of the closed areas given by the equations 2 and 3 exists, and is given by the equation 5.

[0049]

[Equation 5]

By substituting equations 2 and 3 into equation 5, equation 6 is obtained, and equation 6 is equivalent to equation 7 at the logic level.

[0051]

[Equation 6]

[0052]

[Equation 7]

In order for Equation 7 to hold, Equation 8 or Equation 9
It is necessary that

[0054]

[Equation 8]

[0055]

[Equation 9]

Here, considering Equation 4, Equation 8 and Equation 9 become Equation 10 and Equation 11, respectively.

[0057]

[Equation 10]

[0058]

[Equation 11]

However, since the latter half of the equation 11 is the same as the latter half of the equation 10, it can be omitted. Therefore, in order to determine the interference between the objects j and k, it is necessary to determine whether or not the spheres belonging to the objects j and k interfere with each other. Further, here, the relationship between the two closed regions Sa and Sb can be regarded as the presence or absence of the common region, and can be expressed by a simple binary logic.

Common area between both closed areas Sa and Sb, Equation 1
The event representing the existence of 2 is represented by x _{(a, b)} . And x
The value of _{(a, b)} is defined by Equation 13.

[0061]

[Equation 12]

[0062]

[Equation 13]

If the definition of equation 13 is used and the overlapping portion is omitted, equations 10 and 11 can be expressed by the following logical functions. x _{(j, k)} = x _{(j1, k1)} x _{(j1, k2)} x _{(j3, k1)} x _{(j3, k2)} + x _{(j2, k1)} x _{(j2, k2)} , K in a closed region is determined by the presence / absence of interference between spheres, which is the component unit of the closed region.

Further, to determine the interference between the spheres, the radii of the spheres j and k are set to Rj and Rk, and the position vector of the center coordinate is set to P.
j and Pk can be easily achieved by determining the magnitude of the sum of radii and the absolute value of the difference between center coordinates. Therefore, the interference discrimination can be expressed by Equation 14.

[0065]

[Equation 14]

Equation 14 has the same value as Equation 15.

[0067]

[Equation 15]

Therefore, if the vectors P _j and P _k are obtained, vector interference, vector inner product operation, scalar addition, scalar multiplication, and real number comparison are performed once to determine the collision between spheres. Can be achieved. Specifically, an Intel CPU (486SX), a coprocessor and an overdrive processor were operated at a clock frequency of 16 MHz, and the collision discrimination of 1000 pairs of balls, which is almost equivalent to a 6-axis dual-arm manipulator, was performed. The required time was about 6.6 msec.

When the above description is applied to a general manipulator, it is as follows. However, in the following description, S _ij indicates the j-th link of the i-th arm, and S _ij is expressed by a product set of Lmax (i, j) pairs of compound spherical surfaces that form each surface of the link. Sphere, number 16 is M
A closed region formed by max {i, j, Lmax (i, j)} spheres and formed by the m-th sphere is defined as Expression 17. Then, the composite spherical body belonging to each link is defined on the link coordinate system, and the matrix corresponding to the state of the link is multiplied to convert it into the base coordinate system. Of course, it is possible to define it on the base coordinate system from the beginning, but it is preferable to adopt the former method considering that the link operates.

[0070]

[Equation 16]

[0071]

[Equation 17]

Therefore, the closed region Sij can be expressed by the formula 18, and the formula 17 can be expressed by the formula 19.

[0073]

[Equation 18]

[0074]

[Formula 19]

From Equations 18 and 19, it can be seen that the closed region Sij can be expressed by Equation 20.

[0076]

[Equation 20]

When the j-th link of the i-th arm and the t-th link of the s-th arm collide with each other, the closed regions S _ij and S _st that include them by the complex spherical body have a common region. The number 21 is satisfied. Also, the composite spherical bodies S _ij , S
The phenomenon that the common area of _st exists can be expressed by Equation 22.

[0078]

[Equation 21]

[0079]

[Equation 22]

Therefore, considering Equation 19, Equation 22
Can be expressed as a logical product as shown in Equation 23.

[0081]

[Equation 23]

As is clear from the equation (19), since the equation (16) is the union of the spheres of the equation (24), which is its constituent element, the equation (25) is a logical sum form as shown in the equation (26). Can be written as.

[0083]

[Equation 24]

[0084]

[Equation 25]

[0085]

[Equation 26]

By substituting the equation 26 into the equation 23, the equation 27 is obtained.

[0087]

[Equation 27]

As is clear from the equation (27), the interference between the closed regions enclosed by the composite spherical body causes the phenomenon of whether or not there is a common region between the closed regions formed by the sphere which is the constituent unit. The binary variable shown is given as a logical function of Eq. Therefore, it is possible to determine the presence or absence of interference by performing a simple logical operation.

[0089]

[Equation 28]

In the above description, only the case of forming a closed region by the inner regions of two spheres has been described.
In addition, when the closed region is formed by the inner region of one sphere and the outer region of the other sphere, the closed region may be formed by the outer regions of two spheres. Introducing the logical function shown in Equation 29 into Equation 1, the Equation 14 and Equation 29 may be selectively adopted according to the constituent conditions of the closed region. However, in Equation 29, S _k is set outside the sphere and S _j is set inside the sphere. Further, in the case where the closed area is formed by the outer areas of the two spheres, the common area always exists, so the value of the logical function is unconditionally set to "1".

[0091]

[Equation 29]

As is clear from the above explanation,
By adopting the logical function of Equation 29, the object can be modeled by using the inner region or the outer region of the sphere, and the number of spheres required for modeling can be significantly reduced, and
A sufficiently high modeling accuracy can be achieved. Then, the presence or absence of a common area between the spheres forming each composite spherical body obtained by modeling a pair of objects can be set as the value of the above logical function, and the logic based on all the obtained logical function values can be set. By performing the calculation, it is possible to easily and accurately determine the presence or absence of interference between the objects.

In the above, the entire range of the outer shape of the pair of objects is modeled with high accuracy by the compound spherical body, and the presence or absence of the common area between the spheres forming the compound spherical body of each object is determined. The presence or absence of interference is determined based on all the determination results. However, we create a composite sphere to achieve relatively inaccurate modeling, predefine the regions where interference can occur based on this sphere, and then for only the limited region: It is preferable to accurately determine the presence / absence of interference based on the complex spherical body that can achieve highly accurate modeling.

[0094]

[Embodiment 4] FIG. 9 is a block diagram showing an embodiment of the apparatus for discriminating interference between objects of the present invention. By performing processing based on the flowchart of FIG. 1 or the block diagram of FIG.
The processing after the composite spherical body that models each of the two objects with high accuracy is described.

A first sphere selecting section 1 for selecting one sphere (hereinafter referred to as a first sphere) which constitutes a composite spherical body of one object.
1 and a second sphere selection unit 12 that selects one sphere (hereinafter, referred to as a second sphere) that constitutes the composite spherical body of the other object,
Center coordinate value of the first sphere, radius and center coordinate value of the second sphere,
A common area presence / absence determining unit 13 that determines whether or not a common area of both spheres exists based on a radius, a binary data assigning unit 14 that expresses the determination result as binary data, a first sphere,
An iterative control unit 15 that repeatedly operates the first sphere selection unit 11, the second sphere selection unit 12, the common area presence / absence determination unit 13, and the binary data allocation unit 14 until the above processing is performed for all combinations of the second spheres. , And performs a logical operation based on all binary data, and outputs a logical operation result as an interference determination result.

The operation of each component is the same as the processing of the corresponding steps of the flowchart of FIG. 7, so detailed description will be omitted. Therefore, by adopting the logical functions of equations (14) and (29), the object can be modeled by using the inner region or the outer region of the sphere, and the number of spheres required for modeling can be significantly reduced, and High modeling accuracy can be achieved. Then, the presence or absence of a common area between the spheres forming each composite spherical body obtained by modeling a pair of objects can be set as the value of the above logical function, and the logic based on all the obtained logical function values can be set. By performing the calculation, it is possible to easily and accurately determine the presence or absence of interference between the objects.

[0097]

[Embodiment 5] FIG. 10 is a block diagram showing another embodiment of the object-to-object interference discriminating apparatus of the present invention. The difference from the block diagram of FIG. 9 is the flowchart of FIG. 1 or the block diagram of FIG. The preliminary determination sphere setting unit 21 that obtains modeled spheres having a lower approximation accuracy and a smaller number than the modeled spheres obtained in 1.
And a preliminary discriminating unit 22 for discriminating whether or not there is a common area between the modeling sphere of one object and the modeling sphere of the other object,
Preliminary discriminator 2 for all modeled spheres of both objects
9, the preliminary repetition control unit 23 for performing the determination according to 2 and only the portion corresponding to the modeled sphere for which the common determination region 22 determines that the common region exists, the first sphere selection unit 11 in the block diagram of FIG. The only difference is that it further includes a two-ball selection unit 12, a common area presence / absence determination unit 13, a binary data allocation unit 14, an iterative control unit 15, and a selection control unit 24 for performing processing by the logical sum operation means 16. .

Therefore, in the case of this embodiment, a composite spherical body for achieving modeling with relatively low accuracy is created, and the region where interference may occur is limited in advance based on this spherical body. Then, with respect to only a limited area, the presence or absence of interference can be accurately determined based on the composite spherical body that can achieve highly accurate modeling. That is, it is possible to achieve both high accuracy of interference determination and reduction of the time required for interference determination.

[0099]

[Sixth Embodiment] FIG. 11 is a flow chart for explaining another embodiment of the method for discriminating interference between objects according to the present invention.
The processing after the processing based on the flowchart of FIG. 6 or the block diagram of FIG. 6 is performed to obtain a composite spherical body that models each of two objects with high accuracy will be described.
The object is a joint type manipulator.

In step SP1, the joint angle and the joint angular velocity of the joint type manipulator are detected, and in step SP2, the time required for performing the interference discrimination is determined based on the joint angle and the joint angular velocity of the joint type manipulator. By calculating the joint angle after a lapse of a predetermined time, which is not short, and obtaining and multiplying the coordinate conversion matrix based on the calculated joint angle in step SP3,
Convert from the link coordinate system to the base coordinate system. After that,
In step SP4, the same process as in the flowchart of FIG. 7 is performed to predict future collisions of the manipulator.

When it is predicted that a collision will occur,
Although not specifically shown in the flowchart of FIG. 11, a collision is avoided by performing a braking operation, a reversing operation, and the like. Therefore, according to this embodiment, it is possible to predict the future joint angle of the manipulator and determine whether or not there is interference at the predicted joint angle, and it is possible to perform online interference determination.

Although this embodiment describes a joint type manipulator, it is needless to say that it can be applied to a manipulator other than the joint type and a moving object other than the manipulator.

[0103]

[Embodiment 7] FIG. 12 is a block diagram showing still another embodiment of the object-to-object interference discriminating apparatus according to the present invention, in which processing based on the flowchart of FIG. 1 or the block diagram of FIG. The processing after the composite spherical body in which each of the objects is modeled with high accuracy is obtained is described. The object is a joint type manipulator.

It is not shorter than the time required to perform the interference determination based on the current state detection unit 31 which detects the joint angle and the joint angular velocity of the joint type manipulator and the joint angle and the joint angular velocity of the joint type manipulator. A future state calculation unit 32 that calculates the joint angle after a lapse of a predetermined time, and a coordinate conversion unit that converts the link coordinate system to the base coordinate system by obtaining and multiplying the coordinate conversion matrix based on the calculated joint angle. And 33. The composite spherical body whose coordinates have been converted by the coordinate conversion unit 33 is supplied to the apparatus of FIG. 9 to predict future collisions of the manipulator.

When it is predicted that a collision will occur,
Although not particularly shown in the block diagram of FIG. 12, the avoidance control unit causes the braking operation, the reversing operation, and the like to avoid the collision. Therefore, according to this embodiment, it is possible to predict the future joint angle of the manipulator and determine whether or not there is interference at the predicted joint angle, and it is possible to perform online interference determination.

Although this embodiment describes the articulated manipulator, it goes without saying that it can be applied to manipulators other than the articulated manipulator and moving objects other than the manipulator.

[0107]

As described above, according to the invention of claim 1, the radius of the sphere is appropriately set according to the partial outer shape of the object, and the inside of the sphere or the outside of the sphere is selected. The unique effect is that a significantly higher number of spheres in comparison can achieve a significantly higher accuracy of modeling.

According to the invention of claim 2, in addition to the effect of claim 1, the error is prevented from becoming too large locally.
It has the unique effect of achieving highly accurate modeling. According to the invention of claim 3, the radius of the sphere is appropriately set according to the partial outer shape of the object, and the inside of the sphere or the outside of the sphere is selected. It has the unique effect of being able to achieve significantly higher precision modeling.

According to the invention of claim 4, each object can be modeled with remarkably high accuracy with a remarkably small number of spheres as compared with the conventional method, and after the modeling, whether a small number of spheres have a common region or not. Detection processing and logical operation based on the detection results for all the spheres, it is a unique effect that it is possible to easily and quickly determine the presence / absence of interference between objects as a whole. Play.

According to the invention of claim 5, the presence or absence of a common area is roughly determined in a short time based on a small number of modeled spheres,
Since it is possible to limit the area where high-precision interference determination is necessary, it is only necessary to perform interference determination based on a modeled sphere that can approximate the limited area with high accuracy. Without lowering
It has a unique effect that the time required for interference discrimination can be significantly shortened.

According to the sixth aspect of the present invention, whether or not the position and orientation after a predetermined time cause interference can be determined before the moving object actually reaches the corresponding position and orientation. By performing the necessary avoidance operation based on the determination result, it is possible to prevent the occurrence of collision from occurring. According to the invention of claim 7, each object can be modeled with remarkably high accuracy by a remarkably small number of spheres as compared with the conventional method, and after modeling, it is detected whether or not a small number of spheres have a common area. Since it is only necessary to perform processing and logical operation based on the detection results for all spheres, there is a unique effect that the presence / absence of interference between objects can be determined easily as a whole with high accuracy and high accuracy.

According to the invention of claim 8, the presence or absence of a common area is roughly determined in a short time based on a small number of modeled spheres,
Since it is possible to limit the area where high-precision interference determination is necessary, it is only necessary to perform interference determination based on a modeled sphere that can approximate the limited area with high accuracy. Without lowering
It has a unique effect that the time required for interference discrimination can be significantly shortened.

According to the ninth aspect of the present invention, whether or not the position and orientation after a predetermined time cause interference can be determined before the moving object actually reaches the corresponding position and orientation. By performing the necessary avoidance operation based on the determination result, it is possible to prevent the occurrence of collision from occurring.

[Brief description of drawings]

FIG. 1 is a flowchart illustrating an embodiment of a method of modeling an object according to the present invention.

FIG. 2 is a diagram showing an example of a relationship between one plane forming a polyhedron and a sphere.

FIG. 3 is a schematic diagram illustrating selection of a sphere that minimizes a weighted sum of squares.

FIG. 4 is a diagram schematically illustrating an example of modeling an object.

FIG. 5 is a diagram illustrating a specific example of modeling a gripper.

FIG. 6 is a block diagram showing an embodiment of the object modeling apparatus of the present invention.

FIG. 7 is a flowchart illustrating an embodiment of the method for discriminating interference between objects of the present invention.

FIG. 8 is a diagram schematically illustrating an example of modeling a pair of objects.

FIG. 9 is a block diagram showing an embodiment of a device for discriminating interference between objects according to the present invention.

FIG. 10 is a block diagram showing another embodiment of the apparatus for discriminating interference between objects according to the present invention.

FIG. 11 is a flowchart illustrating another embodiment of the method for discriminating interference between objects according to the present invention.

FIG. 12 is a block diagram showing still another embodiment of the apparatus for discriminating interference between objects according to the present invention.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 division part 2 polyhedron model creation part 3 vertex extraction part 4 sphere detection part 6 1st iteration control part 9 combination calculation part 11 1st sphere selection part 12 2nd sphere selection part 13 common area | region presence / absence determination part 15 repetition control part 16 logic Calculation unit 21 Preliminary determination sphere setting unit 22 Preliminary determination unit 23 Preliminary repetition control unit 24 Selection control unit 31 Current state detection unit 32 Future state calculation unit 33 Coordinate conversion unit

Claims (9)

[Claims] 1. The outline of an object is divided into a plurality of surface elements,
Each surface element is approximated by a polyhedron, and in response to the designation of some vertices of the polyhedron, all relevant vertices are included on the relevant side, and the error with respect to the outer shape of the object is less than or equal to a predetermined value. Set the sphere, repeat the setting of the sphere including each vertex of the polyhedron for the required number of times, perform the join operation between the three-dimensional regions defined by all the set spheres, and model the object with multiple spheres. A method for modeling an object, characterized by: 2. A sphere that minimizes the weighted sum of squares of the distance between each vertex and the corresponding point on the spherical surface in response to the fact that the sphere that passes through all the specified vertices is not determined. Item 1. The method of modeling an object according to item 1. 3. A contour dividing means (1) for dividing the contour of an object into a plurality of surface elements, a polyhedron approximating means (2) for approximating each surface element with a polyhedron, and appointing a part of the polyhedron. Vertex designating means (3), sphere setting means (4) for setting all the designated vertices on the corresponding side and setting a sphere having an error with respect to the outer shape of the object of not more than a predetermined value, and each vertex of the polyhedron Iterative control means (6) that repeats the setting of the sphere including the number of times required, and 3 defined by all the set spheres.
A modeling device for an object, comprising: a combining operation means (9) for performing an operation of combining the dimensional regions to model the object with a plurality of spheres. 4. The method according to claim 1 or 2
Each object is modeled by multiple spheres, the existence of a common area between the modeling sphere of one object and the modeling sphere of the other object is discriminated, and a logical operation is performed based on the discrimination result to determine the interference. A method for discriminating interference between objects, which is characterized by obtaining a discrimination result indicating presence or absence. 5. The contour of an object is modeled by a modeling sphere having a smaller approximation accuracy and a smaller number than the modeling sphere obtained by the method according to claim 1 or 2, and modeling of one of the objects is performed. The presence or absence of a common area between the sphere and the modeling sphere of the other object is determined, and only the portion corresponding to the modeling sphere determined to have the common area is obtained by the method according to claim 1 or 2. Based on the modeled sphere, the presence or absence of a common area between the modeled sphere of one object and the modeled sphere of the other object is determined, and a logical operation is performed based on the determination result to determine the presence or absence of interference. The method for discriminating interference between objects according to claim 4, which obtains a result. 6. When at least one of the objects is a moving object, the position and orientation after a predetermined time has elapsed are calculated based on the moving speed and / or the moving acceleration of the moving object, and based on the calculated position and orientation. A method of discriminating interference between objects, wherein the method according to claim 4 or 5 is performed. 7. With respect to two objects each modeled by a plurality of spheres by the apparatus of claim 3, one modeling sphere of one object and one modeling sphere of the other object are common. Common area presence / absence determining means (1) for determining the presence or absence of an area
1) (12) (13) and common area presence / absence determining means (11) (12) for all modeled spheres of both objects
Iterative control means (15) for performing the discrimination according to (13)
And a logical operation means (16) for performing a logical operation based on all the determination results obtained by the common area presence / absence determining means (11) (12) (13) and outputting as a determination result indicating the presence or absence of interference. An interference determination device between objects, which is characterized in that 8. Preliminary discrimination sphere setting means for obtaining a modeling sphere having a lower approximation accuracy and a smaller number than the modeling sphere obtained by the apparatus according to claim 3 as a modeling sphere for modeling the outer shape of an object. (21), preliminary discrimination means (22) for discriminating the presence or absence of a common area between the modeling sphere of one object and the modeling sphere of the other object, and all modeling spheres of both objects Preliminary repetition control means (23) for performing the discrimination by the preliminary discrimination means (22) and the preliminary discrimination means (2
The common area presence / absence determining means (11), (12) and (13), the repetitive control means (15), and the logical operation means (16) only for the portion corresponding to the modeled sphere determined to have the common area by 2). 8. The apparatus for discriminating interference between objects according to claim 7, further comprising a selection control means (24) for performing processing according to. 9. A position / position for calculating a position and orientation after a predetermined time has elapsed, based on the moving speed and / or the moving acceleration of the moving object when at least one of the objects is a moving object.
9. The posture calculation means (31) (32) according to claim 7, further comprising: a target position setting means (33) for setting the calculated position and posture as a position and a posture which are targets of interference discrimination. Interference discrimination device between objects.