JPH02224004A - Interference check device for moving body - Google Patents

Abstract

PURPOSE:To decrease the number of spherical bodies and simplify hiearachic structure, to improve a modeling speed, and to speed up interference decision making operation by performing modeling by spherical bodies which cover the surfaces of respective bodies and excluding the insides of the bodies from the modeling. CONSTITUTION:A hiearachic spherical body model generating means B performs the modeling wherein the surfaces of the respective bodies are covered hiearachically with the spherical bodies which have plural kinds of radii according to image information showing the surfaces of the respective bodies. A spherical body interference arithmetic means E detects spherical bodies which interfere with each other according to the position of a modeled spherical body regarding two bodies at optional time which are stored in a spherical model storage means C and further detects interfering bodies among spherical bodies, which have small radii and belong to the mutual interfering spherical bodies as to only the spherical bodies which are judged to interfere with each other, hiearachically in order from the spherical body with the large diameter. Then when there is the mutually interfering spherical bodies which have the smallest radius eventually, it is judged that the two bodies interfere with each other, the interference position of the two bodies is found from the positions of the mutually interfering spherical bodies which have the smallest radii, and the arithmetic result is outputted. Consequently, the modeling speed is improved and the interference decision of the object is speeded up.

Description

[Detailed description of the invention] [Industrial application field]

The present invention relates to an interference checking device that judges interference between moving objects at high speed and with high precision, such as a device that checks the safety of the motion of a taught robot using a graphic simulator or the like, and a device that checks interference with objects during robot motion in real time. It can be applied to real-time collision detection devices for unmanned vehicles.

[Prior art]

Conventionally, for example, when determining robot collisions, the posture of the robot is displayed at each position of the motion path on the screen to teach the robot's continuous motion, or the programmed robot motion is displayed continuously on the screen. Graphic simulators that do this are known. With this device, the robot's continuous motion is displayed at its actual movement speed and the motion is simulated.
It is necessary to detect interference between moving robot arms or between a robot arm and a stationary object with high precision and high speed. Various methods are known for performing this interference determination. For example, as a first method, as seen in Japanese Patent Application Laid-Open No. 62-43706, each member of the robot and the work environment is covered with a polyhedron, and the presence or absence of interference is determined from the projected figure of the polyhedron; As a second method, JP-A-60-
As seen in Publication No. 99591, there is a method in which each member of the robot and the work environment is expressed as a line segment, and the distance between these line segments is calculated to determine the presence or absence of interference. As seen in the academic journal, Vol. 6, No. 1, 35,
A known method is to express the moving position of a robot or an environmental object as a time function, and use the function to determine that there is interference when a vertex collides with a surface or when edges collide. In addition, as a fourth method, as shown in the Journal of the Robotics Society of Japan, Vol. 5, No. 3, p. 2I, the robot and environmental objects are hierarchically approximated by cubes of different sizes, and each cube is included between two objects. Interference can be determined by examining the relationship,
As a fifth method, as shown in the proceedings of the 6th Annual Conference of the Robotics Society of Japan, page 105, the robot and environmental objects are hierarchically approximated by spheres with different radii, and the inclusion relationship between the spheres with different radii is calculated. A method is known in which interference between objects is determined by examining .

[Problems to be Solved by the Invention] The first method described above cannot perform a highly accurate interference check, for example, when the member is an open solid and it is difficult to approximate the shape with a simple cylinder or rectangular parallelepiped. And the second
With the method, high-speed determination is possible, but the modeling is too simple and the accuracy of interference determination is not good, and with the third method,
There is a problem in that as the number of surfaces increases, the total amount of W increases and the processing speed becomes extremely slow. In addition, the fourth method is a method using a so-called octree hierarchical structure, but because it uses cubic approximation, there is a problem that the coordinate transformation to represent a continuous moving object is complicated and takes time. . Furthermore, in the fifth method, in order to simplify this coordinate transformation, the robot and environmental objects are hierarchically approximated by spheres with different radii. Because we are finding a regular polyhedron that is inscribed in the sphere, circumscribed by each face that makes up the regular polyhedron, and whose center coincides with the center of gravity of the polyhedron, the generation mechanism of lower-level spheres becomes complicated and difficult to model. The problem is that it takes time. In addition, there may be many overlapping parts between spheres, a sphere of a lower hierarchy may contain the outside of a sphere of a higher hierarchy, or the same sphere may overlap two spheres of different upper hierarchy.
It may belong to one sphere, or it may belong to a sphere one level higher, but two
Since spheres that do not belong to spheres higher in the hierarchy or higher may be generated, the modeling efficiency is poor and modeling takes time. Furthermore, since spheres with a hierarchical structure are generated for the inside and outside of the object that are unnecessary to express the shape of the target object to be modeled, the number of spheres increases, which also reduces modeling efficiency. The problem is that it is not good. Further, the accuracy depends on the radius of the smallest sphere, and the calculation speed of collision determination depends on the number of collision determinations between spheres, that is, the hierarchical structure of the spheres. However, since the radius ratio of the spheres in each layer is fixedly determined by the number of faces of the polyhedron used, the radius ratio cannot be arbitrarily set based on the required interference determination accuracy. Therefore, it is difficult to arbitrarily change the hierarchical structure and the radius of the minimum sphere, making it difficult to adjust accuracy and calculation speed. Also, polyhedra belonging to a certain layer overlap, but when searching for spheres in the next lower layer, no consideration is given to the overlap of polyhedra, so spheres are generated unnecessarily and the search speed decreases. There is a problem. The present invention has been made to solve the above problems, and its purpose is to improve the modeling speed of objects and to simplify the hierarchical structure of spheres, thereby reducing the interference between spheres. The object of the present invention is to reduce the number of times of determination and to perform collision determination of objects at high speed and with high accuracy. [Means and operations for solving the problems] As shown in FIG. 1, the structure of the invention for solving the above problems is as follows: surface information storage means A that stores surface information representing the surface of each object;
a hierarchical sphere model generation means B that hierarchically covers and models the surface of each object with spheres each having a plurality of types of radii based on the surface information; and a sphere that stores the position of the generated modeling sphere. a model storage means C; a sphere position update means for updating the position of a modeling sphere with a plurality of radii related to a moving object according to the movement of the object and outputting it to the sphere model storage means C; Based on the memorized positions of modeling spheres regarding two objects at arbitrary times, spheres that interfere with each other are detected, and for only those spheres that are determined to interfere, a sphere with a smaller radius that belongs to the spheres that interfere with each other. The procedure of detecting spheres that interfere with each other is executed hierarchically from the sphere with the largest radius to the sphere with the smallest radius, and finally when there is a sphere with the smallest radius that interferes with each other, the two objects consists of a spherical interference calculation means E which determines the interference position of two objects from the position of the spheres with the minimum radius that interfere with each other, and outputs the results of these calculations. The surface information storage means A is a means for storing information regarding planes and curved surfaces forming the surface of an object, and is, for example, a device that stores functions of constituent planes and constituent white surfaces. Hierarchical sphere model generation means B is means for generating a sphere that covers the surface of the object determined based on the surface information, and for generating the position of the sphere. This method covers the surface of the object in a hierarchical manner and models the surface of the object using hierarchical spheres. The spherical model storage means C is a means for storing the positions of the generated spheres in each hierarchy, such as the center position and radius. The sphere position updating means is means for updating the position of the sphere in each layer according to the movement of the object and storing it in the sphere model storage means C. For example, the position and orientation of an object are expressed by parallel and rotational movements of a coordinate system fixed to the object, and these parallel and rotational movements can be expressed simultaneously by one matrix. Therefore, the center position of each sphere after movement as seen from the absolute coordinate system can be found by multiplying the center position as seen from the coordinate system fixed to each object by the above transformation matrix. The spherical interference calculation means E is a means for calculating the presence or absence of interference between objects and its position from the position of each sphere stored in the spherical model storage means C when the objects move. More specifically, first, it is determined whether or not a sphere with the largest radius, that is, a sphere in the highest hierarchy, interferes between two objects. This interference determination is made, for example, by determining whether the distance between the centers of two spheres is smaller than the sum of the radii of each sphere. If it is determined that there is no interference, interference determination between other objects is performed. On the other hand, if it is determined that there is interference, the determination of the presence or absence of interference is similarly executed for the sphere with the next smallest radius that belongs to that sphere, that is, the sphere in the lower hierarchy. When it is determined that there is interference, interference determination is sequentially executed for spheres in lower hierarchies that belong to the interfering sphere. On the other hand, if there are no interfering spheres in a certain hierarchy, go back to the next higher hierarchy and perform calculations to detect other interfering spheres in that hierarchy, and if there are interfering spheres, the above According to the above procedure, interference determination is performed for spheres in a lower hierarchy, and if there is no interference method, a calculation is executed to go back to the upper hierarchy and detect spheres that interfere in the same way. Finally, it is determined that the two objects will interfere when there is a sphere with the minimum radius that interferes with each other, and the interference position of the two objects is determined from the position of the sphere with the minimum radius that interferes with each other.

【Example】

The present invention will be described below based on specific examples. First Embodiment This first embodiment is used when inputting data on the shape and position of a robot and working environment, displaying the robot and working environment on a graphic display, and simulating the movements of the robot. This is a device that checks for interference between robots or between a robot and an object. As shown in Fig. 2, this device is composed of a computer device, including a CPU 1 that executes logical operations, etc., a ROM 2 that stores programs indicating its processing procedures, a RAM 3 that stores various data, and a robot and work environment. Data input device @ 4 for inputting position and shape data and various commands such as simulation start command, and graphic display 5 for displaying the robot's posture and work environment that change over time at every minute sampling time. and data input device 4
and an input/output interface 6.7 for connecting the graphic display 5 to the CI'U1. In addition, the RAM 3 includes a shape data memory 31 that stores information on surfaces, vertices, and ridge lines that express the shape of each component of the robot arm and work environment, and information on each component when the surface of each component is layered. A sphere model memory 32 that stores information regarding the center position, radius, and hierarchy of the sphere, and a joint angle memory 3 that stores the robot's current joint coordinates.
3, and a member position memory 34 for storing the current position of each of the five constituent members of the robot and work environment. Next, we will explain the operation of this device in the third section, which shows the processing procedure of the CPUI.
The explanation will be given according to the flowchart shown in the figure. In step 100, data for generating shapes for each component of the robot and work environment is first input from the data input device 4. Based on this input, data such as vertices, edges, and surfaces representing the shape of each component are calculated. These shape data are stored in the shape data memory 31. Next, in step 102, data on the constraint relationships and absolute coordinate positions of the robot and each component of the work environment are input from the data input device 4. As this input data is input, the robot and work environment are displayed on the graphic display 5, so that errors in input data can be easily detected and data input can be easily executed. The constraint relationships and position data of the robot and the work environment are stored in the member position memory 34. When the input of the position data in step 102 is completed, data specifying the bot to be interfered with and the work environment is input by the data input device 4 in step 04. Next, in step 10G, for each component of the robot or work environment specified in step 104, each surface of the component is hierarchically covered with spheres of three types of rice, large, medium, and small. , a hierarchical spherical modeling of the surface is performed. This spherical modeling of the surface is performed as follows. As shown in FIG. 4, a rectangular parallelepiped-shaped outer envelope 12 enclosing one component 11 is generated from surface information data referenced from the shape data memory 31 of the component 11. Furthermore, a sphere circumscribing the outer envelope 12 is considered, and this sphere is referred to as a large sphere 14. In this way, the top level big ball player 4
3 is generated as a sphere containing component II. Then, the center position and radius of the large sphere 14 viewed from the reference coordinates set for the component 11 are stored in the spherical model memory 32. Next, as shown in FIG. 5(a), the outer envelope 12 is attached to the inner sphere 1.
5 radius rI11 vs. l-, one side is 6m = ffrn+
is divided into a plurality of cells 17 of a cube. Outer envelope 12
Since the length of each side of the cell 17 is not necessarily a multiple of the length of the side of the cell 17, the cells 17 are arranged in an overlapping manner. Next, as shown in FIG. 5(b), each cell 17
A sphere 20 circumscribed by is generated, and among the spheres 20,
Extract only the seven intersecting spheres on the surface 13 of the component 11,
The extracted sphere is referred to as the medium sphere 15. Then, the center position of each medium sphere 15 as seen from the reference coordinates set on the component 11 is stored in the sphere model memory 32.Next, as shown in FIG. 6(a), the center position of each medium sphere 15 is Paying attention to the inscribed cells 17, one cell 17 is further divided into cubic cells 18 with the radius of the sphere being rs and the − side being βS=ffrs.At this time, each cell 18
are arranged so as to overlap in the same way as when the middle ball is generated. Next, each sphere 2 circumscribing each cell 18
1 is generated, only the spheres that intersect with the surface 13 of the component 110 are extracted, and the extracted spheres are defined as the small spheres 16. Then, the center position of each small sphere 16 viewed from the reference coordinates set for the component 11 is stored in the spherical model memory 32. In this way, as shown in FIG. 7, the component 11 has a large outer sphere 14 and a smaller medium 15 which includes the surface 13 of the component 11. The model is hierarchically modeled using three types of smaller spheres 16 having different radii. That is, according to FIG. 8, it can be seen that all the small spheres belong to only one medium sphere, and all the medium spheres belong to the large sphere, so that they are hierarchically arranged in a tree-like manner. Note that the radius rs of the small ball 16 is arbitrarily set in consideration of the accuracy of interference determination. Next, the radius "m" of the medium sphere 15 is automatically determined from the radius rl of the large sphere and the radius rs of the small sphere using the following formula. According to this formula, the small sphere, the medium sphere, and the medium sphere The volume ratio can be made equal to that of the large sphere.This provides an efficient hierarchical structure for searching when determining interference.Also, as an exception, since the outer dimensions of the component 11 are small, the diameter of the large sphere 14 is If the diameter is smaller than the diameter of the small sphere 16, the small sphere 16 is not generated and only the large sphere 14 is modeled.When the hierarchical sphere modeling of each component is completed as described above, the process proceeds to step The process moves to step 108, where a robot motion command is inputted from the data input device 4. Next, the process moves to step 110, in which the joint coordinates of the robot after the minute movement, i.e. , the joint angles are calculated, and the joint angle data is stored in the joint angle memory 33.Next, when the joint angle calculations are completed, the process moves to step 112, and the reference coordinate system taken for each component is A transformation matrix T that relates the and absolute coordinate system is calculated from the joint angles at that time. The position and orientation of each sphere are expressed as a 3x4 matrix.The vector representing the center position of each sphere viewed from the reference coordinates taken for each component is P.
Assuming that rel is the vector Pabs representing the center position of each sphere viewed from absolute coordinates, it can be found by the following equation. For objects such as robots whose constituent members move, it is necessary to calculate a transformation matrix T from each joint angle each time the object moves, and update the absolute coordinates of the center positions of all spheres that model the constituent members. However, in order to improve the efficiency of calculation, this update of the absolute coordinates may be performed only for the sphere that is the object of the interference check. In this way, the spherical model memory 32 stores the center position Ptel of each sphere as seen from the reference coordinates set for each component, and the center position Pabs as seen from the absolute coordinates of each sphere at the current time. There will be. Next, the process moves to step 114, and an interference check calculation is performed according to the following procedure. In step 114, interference checks between the spheres are performed hierarchically in the order of large balls, medium balls, and small balls, and in step 116,
If it is determined that the spheres (small spheres) in the lowest hierarchy are interfering with each other, the process proceeds to step 118, in which the occurrence of interference and the interference area are determined and displayed from the center positions of the small spheres that interfere with each other. , In step 120, the robot and work environment in the interference state are displayed, and the program ends. Next, the procedure of hierarchical checking using large balls, medium balls, and small balls in step 114 will be described below. 5 tepl, as shown in Figure 9(a), a large ball 14
Check between a and 14b, and if there is any interference, go to step 2. Execute. If not, the check ends. Therefore, first, until each component comes close to a certain extent,
The calculation time is extremely short since interference is determined between large balls. 5 step 2. As shown in FIG. 9 et al., one large ball 14a and medium balls 15b1 to 15b1 included in the opponent's large ball 14b.
Perform interference check with 15bn and find medium ball 15 that interferes.
bl. Extract 15b2.15b3. Also, conversely, large ball 14b
and medium balls 15aI to 15an included in the opponent's large balls 14a.
Check the interference with the medium ball 15a1.1 that interferes with the
Extract 5a2. If there is no interfering medium ball, 2
It is determined that the two components do not interfere, and the calculation is interrupted. 5step3. As shown in Figure 9ce, the extracted medium sphere 15bL 15b2.15h3.15a1.15
Check the interference between a2 and find the medium balls 1 that interfere with each other.
Extract 5bi, 15b2.15al and execute 3tep4. Furthermore, if there are no intermediate spheres that interfere with each other, it is determined that the two constituent members do not interfere, and the check calculation is interrupted. 5step4. As shown in FIG. 9(d), an interference check is performed between one interfering medium ball 15b2 and the small balls 16all to 16aly++ included in the opponent medium ball 15a1, and interfering small balls 16all and 16a12 are extracted. do. And, conversely, one medium ball 15a1 interfering with the small balls 16b21 to 16b2 included in the opponent medium ball 15b2.
Interfering globules are extracted from n*, and the next 5 steps
Execute step 5. Furthermore, when there is no interfering small ball, interference determination between the medium ball 15 b 1 and the small balls 16all to 16aln belonging to the opponent medium ball 15a1 is similarly executed. If no interfering small sphere is detected, the same process is performed for other interfering medium spheres, and when there are no mutually interfering medium spheres, it is determined that the two components do not interfere. 5step5. As shown in FIG. 9(e), the extracted globules 16all, 16a12.16b21.16b
22.16b23 are checked and the small balls 16a11.16b21 that interfere with each other are extracted. And in this case, the two components are the small balls 16a1.1.16
It is determined that there is interference at part b21. Furthermore, if there are no small spheres that interfere with each other, the process returns to step 5 to extract other medium spheres that interfere with each other. In the end, when interference between the globules in the lowest hierarchy is detected, it is determined that there is interference, and the position of the globules is determined to be the interference site. If there is no interference between the globules,
When processing is performed on other mutually interfering medium spheres in the upper hierarchy, and there are no mutually interfering medium spheres,
Finally, it is determined that there is no interference. Therefore, the tree structure shown in Fig. 8 is searched along the branch paths where there is a possibility of interference. Judgment time becomes shorter. In addition, when determining the interference of spheres, the radius of the two spheres is determined by rl.
+ r,, when the center position is p,, p, the following formula %
It is determined whether the formula % is satisfied or not. If the above formula is satisfied, the two spheres will interfere. By hierarchically checking large balls, medium balls, and small balls as described in (2) above, it is possible to efficiently narrow down the areas where interference occurs and perform a simple and high-speed interference check. Incidentally, if it is determined in step 116 that no interference occurs in the sphere in the lowest hierarchy, since no interference has occurred between the constituent members, the process moves to step 122.
It is determined whether or not the interference check calculation has been performed for all combinations of component members, and if the calculation has not been completed for all combinations of members, step 114
The interference check between the constituent members is similarly performed. If the interference check calculations have been completed for all the combinations of constituent members, the process moves to step 124, where it is determined whether the interference check calculations have been completed for all specified objects. In this case, interference check determination between other objects is similarly performed. If it is determined in step 124 that the interference check calculation has been completed for all specified combinations of objects, the process moves to step 126, and the robot calculates the joint angles stored in the joint angle memory 33 at that time. The attitude is acting W
The robot is displayed in that posture, and the working environment is also displayed. Next, the process moves to step 128, where it is determined whether all moving operations have been completed. If the movement is not completed, the process returns to step 1.10 and the above-mentioned interference determination process is repeatedly executed for the next moving point. Further, when it is determined that the movement is completed, this program ends. As explained below, the device according to the present invention can easily satisfy both the required conditions of accuracy and calculation speed, so it can be used, for example, to check high-speed interference during rough motion of a robot. This is particularly effective in cases where Furthermore, the apparatus of the present invention can model members of any shape with the same precision, and is a highly versatile apparatus that can be applied to both real robots and offline simulators. Second Example In this second example, the number of layers of the sphere model and the radius of the sphere in each layer are determined in order to optimize the search efficiency for areas where interference occurs, taking into account the size of the object, the radius of the minimum sphere, etc. is set automatically. Only the differences from the first embodiment will be explained below. In hierarchical sphere model generation means B, the depth of the hierarchy (hierarchy (number) n and the radius rl of the sphere at each level are calculated using the following equations (1) and (2), respectively. log ρr However, n is rounded off to an integer. However, i 1, 2...n Here, the process of deriving equation (1) is shown. If the radius of the sphere at each level is ri, then the lower sphere (radius r +
++) is expressed by equation (3). If ρ9 is constant at each level, the radius r1 of the maximum sphere and the radius r of the minimum sphere. The relationship with is expressed by equation (4). Here, if the radius ratio of the upper and lower spheres is ρ, then (3
) From the equation, ρ9=ρrs, so the equation (4) can be expressed as the equation (5). r, = r%, ρ,! (a-11, ,, (5)
By taking the logarithm of both sides of equation (5), equation (1) is obtained. Also, the above equation (3) is determined from equation (5) by r , @ = r
, @・p, 8 (@ -1), so it can be derived from both equations. In the above, the volume ratio pv of the upper and lower spheres is constant,
In other words, by defining the relationship that a certain number of lower spheres are included in the upper sphere, it is possible to equalize the number of lower spheres for each law in each hierarchy, so that the hierarchy from the maximum to the minimum sphere can be made uniform throughout the hierarchy. The number of balls and the number of balls in each layer can be set optimally in terms of search efficiency. Furthermore, even if the area ratio ρs = (r+/rt, )'' of the upper and lower spheres is constant, it goes without saying that the radius ratio ρ1 of the upper and lower spheres is similarly constant. Note that the radius ratio p is set depending on the size and shape of the object, and is set to approximately 2 in this embodiment.The number of layers n determined as described above and the radius r1 of the sphere at each layer A spherical model of the object is generated as follows using . Step 1: Generate a rectangular parallelepiped Rec 1 that contains the object and a sphere Sph that contains the rectangular parallelepiped Rac 1. (Hierarchical level: = l) Step 2 : Set the hierarchy level to +1, and set the rectangular parallelepiped Re
Divide c k-1 into cubic cells inscribed in a sphere of radius r + g (i.e., a cube with -side 72" r b). Here, each cubic cell is divided by each side of rectangular parallelepiped Rec k-1 and each cube Since the sides are arranged so that they match, the first
As shown by the hatched area in FIG. 0(a), in many cases each chamber cell overlaps with its adjacent cell. This overlapping portion is deleted as described later and reconfigured as a rectangular parallelepiped cell Rec,j, and all cells N=1.
2...)). Step 3: Check whether the sphere surrounding the rectangular parallelepiped cell Rack,j intersects with the surface of the object. If they intersect, add the sphere to the sphere model data, and if kin (i.e., hierarchy level k is not the lowest level), then
Step 2 is further executed for the rectangular parallelepiped Rec k+1. Here, deletion of the overlapping portion of cubic cells will be explained. As mentioned above, at each hierarchical level, overlap parts are added preferentially in the order in which lower order methods (i.e. lower cells) are generated, and cells that are later in the order are defined as rectangular parallelepiped cells excluding the overlap parts. It is something to do. That is, as shown in FIG. 10(b), the cubic cell Ree k
-1 as nine spheres Sph k with radius r1, 1 ~ Sph k
, 9, each rectangular parallelepiped cell Rec, 1 to Rec k,
When dividing into 9, determine the cube → = Le Rec, 1 so that it is inscribed in the first sphere 5phk, 1, and the next sphere 5ph
For k,2, the portion of the cube inscribed in Sph k,2 excluding the overlapping portion with the 16ck,1 is defined as a rectangular parallelepiped cell Rec,2. Below, 5phk,
3 to 5 phk, 9 are sequentially subjected to the same operation to obtain each rectangular parallelepiped cell Rec k, 1 without an overlapping portion, respectively.
=Reck,9 is defined. Next, the lower ball Spb
When generating k+l,i, each rectangular parallelepiped cell Rec
Since k, 1 to Rec k+ 9 are divided in the same way as above, the sphere 5phk+ generated in the overlapping part
1. Duplication of i is eliminated, and the number of sphere data can be reduced to the necessary minimum. In this way, when representing (modeling) the surface of an object, the number of spheres in each layer can be minimized, so that the search efficiency during interference checking does not decrease. Next, a recursive algorithm for hierarchical sphere pair model in which the number of layers of objects X and Y generated as shown below is ``WIrnV'' is described below. This algorithm can be applied even when the number of layers is unequal (nx≠m), but it is basically the same as the case of the first embodiment.In addition, in each step below, r true /fals
"return e" means to return true/f a to the previous step.
It means to return Ise. Step 1: Add a mark to the sphere data representing the possibility of interference (set 1 to the flag) for each sphere at the top level (-1 to the hierarchy level) of the two objects X and Y.Step 2
Execute. If the result of step 2 is true, it is assumed that objects X and Y are interfering with each other. If this is not the case (if a false result is obtained), it is assumed that the objects X and Y do not interfere with each other. Step 2: Step 3 is executed for all combinations of spheres to be checked that belong to objects X and Y, respectively. If step 3 is true, return true 5゜ Also, step 3 is true! :: Returns false if there is no such combination. Step 3 If both of the ball data belonging to the objects X and Y in the knee combination have marks indicating the possibility of interference, check for interference between these balls. Otherwise, returns false. As a result of the above check, if the balls interfere with each other, step 4 is executed for those two balls. If there is no interference i:j then false
did not return. Also, if the result of step 4 is true, tr
If ue is false, return false. Step 4: If both spheres are in the lowest hierarchy (mouth, ny), it is determined that objects X and Y are interfering with each other, and true is returned. If not, objects X, Y
If only the ball belonging to one of the balls is in the lowest hierarchy, step 5 is executed, and if both balls are not in the lowest hierarchy, step 6 is executed. If each result of step 5 and step 6 is true, set true, fa
If it is lse, returns false. Step 5: Check for interference between spheres for all combinations of balls in the lowest hierarchy belonging to one of objects . If there is interference, execute step 4. Returns false if there is no ball with interference. true if the result of step 4 is true, false
If e, return false. In addition, clear all the marks here (set the flag to 0)
By doing this, you can set it up so that you can immediately start from step 1 the next time you check for interference between objects. Step 6: For all combinations of one ball belonging to object If interference occurs, one of the spheres belonging to the object Y
Mark the sphere data in the lower hierarchy. Returns false if there are no marked balls. Similar processing is also performed for all combinations of one ball belonging to object Y and a plurality of balls one layer below the one ball belonging to object X. In this way, the hierarchy level is gradually lowered for each of the balls on the object X side and the balls on the object Y side, and balls that are likely to interfere with balls in higher hierarchy are selected. Then, if there is a ball with a mark, step 2 is executed for all combinations. The result of step 2 is tr
If it is UE, it returns true, and if it is false, it returns false. Note that, as in step 5, all marks are cleared (the flag is set to 0). Next, the processing procedure of CPt1l for determining interference according to the above algorithm will be explained based on FIGS. 12(a) and 12(b). (a) When the number of layers of the layered X sphere and Y sphere is the same. In step 200, the current search hierarchy i of the object X is initialized to 1, the current search hierarchy j of the object Y is initially set to the topmost hierarchy of 1, and the interference flag of the topmost sphere is turned on. By turning on the interference flag of the topmost sphere, a search for a pair of spheres in the lower hierarchy is started. Next, in step 202, it is determined whether or not the search for global pairs with both interference flags on has been completed in the current search hierarchy (i, j>).If the determination result is N, Step 204 and subsequent steps are executed, and if the determination result is YES, step 250
The following steps are performed. Since the search for all sphere pairs has not been completed during the first execution regarding the topmost sphere, the result of the determination in step 2020 is naturally NO. In step 204, the ball with the interference flag on,
One X sphere belonging to hierarchy i and one Y sphere belonging to hierarchy j
A sphere is selected, and one unsearched pair of spheres (iX, jY) is generated. In the next step 206, step 204
An interference calculation is performed for the pair of spheres (iL jY) generated in step 208, and it is determined in the next step 208 whether or not the two spheres iX and jY interfere. Note that, as shown in the first embodiment, the interference calculation is determined based on whether the distance between the centers of two spheres is smaller than the sum of the radii of the two spheres. Next, in step 210, it is determined whether the pair of spheres (iX, jY) both exist in the lowest hierarchy, and the determination result is N.
In the case of O, the process moves to step 212. Step 2
In step 12, it is determined whether one of the spheres in the pair of spheres (iX, jY) exists in the lowest hierarchy, and if the determination result is NO, the process moves to step 214. In step 214, interference determination is performed for the sphere iX and all of the lower spheres one hierarchy below that belong to the sphere jY. If the ball jY interferes, the interference flag of the ball below that ball jY is turned on. Next, the process moves to step 216, and as a result of the interference determination in step 214, it is determined whether at least one of the lower balls of ball "Y" is interfering with ball iX, and if the determination result is YES, Step 2
18 is executed. In step 218, similarly to step 214, interference determination is performed for the sphere jY and all of the lower spheres one hierarchy below that belong to the sphere iX. If the ball iX interferes, the interference flag of the ball below that ball iX is turned on. next,
Proceeding to step 220, as a result of the interference determination in step 218, at least one of the lower balls of ball iX is lower than ball j.
It is determined whether or not there is interference with Y, and the determination result is YES.
If so, step 222 is executed. Next, in step 222, the current search hierarchy (i, j)
The current search sphere pair (iX, jY) is stored in the area D(i, j) so that the next search sphere pair can be selected at . Further, other parameters necessary for continuation of processing after returning to the current search hierarchy (i, j) are also stacked. Next, in step 224, the current search hierarchy (i, j
) are each lowered one level. Note that the current search hierarchy (i, j) is the sphere pair (i, j) of interest.
X, jY) exists. In other words, the current search hierarchy (i, D is
It is defined as the hierarchy to which relative iX, jY belong. Further, the current search sphere pair (iX, jY) is defined as a sphere pair of the parent sphere iX and the parent sphere jY that are being noticed. In this way, in steps 222 and 224, when the stack processing for restoration and the update processing of the current search hierarchy are completed,
Returning to step 202, the above-described process is repeated with the current search hierarchy set to the next lower hierarchy. When steps 202 to 224 are repeated without branching, the search branch for searching for a pair of interference spheres is
Both the X sphere and the Y sphere extend to the lower hierarchy through the shortest route. Note that the above routine from step 202 to step 224 includes a step of branching the processing step depending on the judgment result, but by branching, the search branch returns to the upper layer and horizontally returns to the upper layer. It progresses by extending in the direction and then extending to the lower hierarchy. Among the above branching steps, in step 208, the sphere pair (
If it is determined that there is no interference between the spheres (i iX, jY) are generated. Then, in step 206 and subsequent steps, as described above, interference between the pair of spheres (i Then, a collision determination between the parent/sparse jY and the subordinate child sphere belonging to the parent/sparse jX is executed, and the interference flag of the interfering child sphere is turned on.Furthermore, in step 216, the parent/sparse 1x belonging to the parent/sparse jY is determined. If it is determined that there is no interfering child ball, or in step 220, the parent/distance j
If it is determined that there is no child sphere that interferes with parent/disparate iY belonging to Therefore, there is no need to extend the search branch. Therefore, in this case, the process returns to step 202, and in the subsequent step 204, another sphere pair (iX, jY) belonging to the current search hierarchy (i, j) is generated, and through the processes from step 206 onwards, that sphere pair ( A search branch is extended from iX, jY). If it is determined in step 202 that all sphere pairs with search flags on that belong to the current search hierarchy ci, j) have been searched, the process moves to step 250. In step 250, it is determined whether the current search hierarchy (i, j) is the highest floor Fl (1, 1>).If the determination result is NO, the process moves to step 252,
Since the global pair that is the search target belonging to the current search hierarchy (i, j) does not interfere, the Chisa flag of the spheres that make up the global pair is turned off. Then, the process moves to step 254, where the current search hierarchy (i,
j) is raised one level higher. This causes the search branch to return to the above hierarchy. When a search branch extends from a certain upper layer to a lower layer,
In step 222, data such as the current search sphere pair in the upper layer is stored, but in step 256, when returning from the lower layer to the higher layer, the data such as the current search sphere pair in the higher layer is restored. Therefore, when the process returns to step 202, the search for the interference area continues from the next unsearched pair of spheres in the upper layer. Meanwhile, in step 250, the current search hierarchy ci, j
) is determined to be the highest hierarchy, it means that the highest sphere does not interfere. Therefore, in this case, the process moves to step 258 and a conclusion is made that object X and object Y do not interfere. On the other hand, as the search for the interference region progresses, in step 210, the sphere IxL sphere jY of the current search sphere pair (iX, jY) determined to interfere in step 20g is determined.
If it is determined that both of them are in the lowest hierarchy, it can be determined that object Therefore, in this case, it is possible to proceed to step 226 and conclude that the two objects interfere. (b) When the number of layers of the layered X sphere and Y sphere is different. Steps 200 to 22 described above are performed until either one of the current pair of funicular spheres (iX, jY) reaches the lowest level.
4 and steps 250-258 are executed. In the above processing process, if it is determined in step 212 that one of the current search sphere pair (iX, jY) is in the lowest hierarchy, steps 280 and subsequent steps are executed. Among the pair of balls (iX, jY), when the ball at the lowest level is L1 and the opponent ball is M, the ball and the lower level one level below that belong to the opponent ball match, that is, one of the child balls.
One sphere kM (k is a variable that varies depending on the child sphere) is selected, and an unsearched sphere pair (L, kM) is generated. and,
In the next step 282, the interference calculation for the sphere pair (L, kM) is performed, and in the next step 284, the sphere pair (L, kM) is
, kM) are determined to interfere. If the determination result is YBS, the process moves to step 286, and when returning to the current search hierarchy (i, j), which is defined as a hierarchy in which estranged and estranged M exist, that hierarchy will be continued. The current search sphere pair (iX, j
Y) (=(L, M)) Area D(i, j)l: Memory is lost. In addition, other parameters necessary for restoration are stacked. Next, in step 288, the current search hierarchy (i, j) in which close relatives exist is lowered by one level. At this time, if the sphere iX exists at the lowest level, the level j of the Y sphere is lowered by one level. On the other hand, when the sphere iY exists in the lowest hierarchy, the hierarchy i of the X sphere is lowered by one hierarchy. Next, the process moves to step 290, where the current ball and child ball k are
M is considered as a new relative in the next interferometry search process. For this purpose, the current search sphere pair (iX, jY) defined as a sparse parent sphere pair is redefined as a sphere pair (L, kM). Then, the process returns to step 210 to proceed with the search for the next interferometry method. In this way, if one sphere belongs to the lowest hierarchy, only the other sphere is searched towards the lower hierarchy. If it is determined in step 284 that the pair of spheres (L, kM) does not interfere, the process proceeds to step 292, where it is determined whether the child sphere kM is the last sphere belonging to parent/disparate member M. If the determination result is NO, the process returns to step 280, and another child ball kM belonging to the parent/favorite group is selected. And step 28
2 or less, interference determination for the pair of spheres (L, kM) is performed in the same way as described above. In step 292, if it is determined that the child sphere kM is the last sphere belonging to the parent/disparate member M, this means that the parent/disapproval and the parent/disapprove do not interfere with each other, so the current search sphere (
iX, jY) must be changed. That is why step 294 exists. In step 294, it is determined whether or not the hierarchy i of the X sphere and the hierarchy j of the Y sphere are equal in the current search hierarchy (i, j). If the determination result is YES, it is sufficient to generate another pair of spheres belonging to the same hierarchy, so the process returns to step 202. If the determination result in step 294 is NO, the current search sphere pair (iX, jY) is returned to the sphere pair of the parent/disparate sphere and the parent/disparate sphere one layer above which the parent/disparate member M belongs, and the interferometry search is performed in that state. It needs to be continued from For this, the following step 2
At 96, the current search hierarchy (i, j) is raised by one hierarchy. In this case, if sphere iX exists in the lowest hierarchy,
The layer j of the Y sphere is raised by one layer. On the other hand, the sphere iY
exists in the lowest hierarchy, the hierarchy i of the X sphere is raised by one hierarchy. Also, in step 298, in the updated current search hierarchy (i, j), the last search sphere pair for which the interferometry search was performed is restored as the current search sphere pair (iX, jY). Note that the sphere pair is the value stored in area D(i,j) in step 286 previously executed. Then, return to step 210, and in step 212 select Y.
It is determined as S, and the process moves to step 280. Then, in step 280, the next new pair of unsearched spheres (L, kM) is generated with the current search sphere pair (iX, jY) as close and distant, and interference determination is performed for this pair of spheres. As described above, since only the balls that are likely to interfere are sequentially searched for in the lower hierarchy, the search speed is improved. Next, how the interferometry is determined by the above-mentioned interferometry search algorithm and the search processing procedure executed by cPul will be explained based on the example shown in FIG. 11. Figure 11 shows the topmost sphere (sphere X for object Y, sphere y for object Y, as in Figure 8 for the case where object ) to the lowest sphere (for object X, sphere X. 1 tl ”” I By the way, ■-■ in the figure indicates the procedure for checking interference between spheres between objects, and corresponds to the following description.Furthermore, referring to the following description, r(x
, y) means to perform an interference check between the ball X and the ball y. First, object X and object Y
When performing interference check between x, y
Mark each to indicate the possibility of interference (turn on the interference flag). ■ Due to the interference check of (x, y), x,
Let y be the presence of interference. ■C and y's lower layer glue (which was determined to have interference), yz. All combinations of y, (x, yl), (x, y
l).・By checking the interference of lx, y-, X, y, and y
, due to interference between yII! Mark I'2 respectively. ■Similarly, 'l+X2* "" in the lower hierarchy of y and
H) By checking the interference of all combinations with 1m, assign 7- to each of X 1 + X x that has interference. ■All combinations of marked balls (X+, yl). (xi, yzL (xz, Yi) + (X 2.Yl)
According to the interference check, there is interference between x and Yi, so X++ and Y+ are determined to have interference, respectively. That is, unmarked X1+ ・−, x− and ys+ ・
, y, is not checked for interference. ■ Similarly to ■, for X++Y+ which is considered to have interference,
l,! :Y+++ Yi2+ of the lower layer of yI...
, y3, and Yz and Yiz with interference are marked respectively. ■ Similar to ■, X++ was found to have interference! l”,
Xll+Xl7. of the lower hierarchy of y, and X. ”” +X
By checking the interference of all combinations with Im, each XlllXI2 with interference is marked. ■All combinations of marked balls (Xz, Yz). (Xll,yli)+ (X+2.yII)+ (XI
According to the interference check in ly12), there is interference between Xll and y, so Xz+yi is determined to have interference. In addition,
Since V++ is the lowest level sphere of object Y, the sphere to be checked for object Y will always be yII from now on. ■For XIIIYI+, which was found to have interference, yII and Xl
X111+X112 in the lower hierarchy of l. Interference checks are sequentially performed for all combinations of ""+Xl1m. !
+ + + L! Since there is no interference with + +, next (
Xiis, Yz) interference check. As a result,
Because interference occurs between 112 and yll, Xzm
is determined to have interference, and the check target is changed from XlI2 to a ball in the lower hierarchy of X1l11. ■Since there is no interference in the interference check between y and X and □ in the lower hierarchy of x1□, next an interference check is performed on (Xzz*, yII). As a result, since interference has occurred between Xl1112 and yl, x, 2 is determined to have interference. Since Xll□ is a sphere in the lowest hierarchy, the process ends here, and no interference check is performed for the next X11211, regardless of whether there is interference. In this way, object X and object Y are sphere X1l12 and sphere Y++
It is immediately determined that there is interference in a part where the However, if you want to know whether there is interference in all parts of the object, use Xll3+, which was excluded from the check in step
”r For all X11m1, it is sufficient to check the interference of Since the interference check is performed only on the attached balls, the number of combinations of balls to be checked can be extremely reduced, so the efficiency of checking for interference between objects can be greatly increased.

【Effect of the invention】

As described above, in the present invention, each object is modeled using a sphere that covers its surface, and the inside and outside of the object are excluded from modeling, so the number of spheres is small and the hierarchical structure is simplified. Improves modeling speed. Therefore, this has the effect of shortening the computation time for interference checking. Furthermore, since the model is made of a sphere, the movement of the sphere accompanying the movement of the object requires only the movement calculation of the center position of the sphere, and the updating calculation of the sphere accompanying the movement is simplified. Further, the accuracy of interference determination depends on the radius of the minimum sphere, and since the radius of the minimum sphere can be set arbitrarily, the accuracy of interference determination can be changed arbitrarily. Furthermore, since the model is hierarchical, the computation time can be shortened by increasing the depth of the hierarchy. Furthermore, collision detection of spheres is performed hierarchically, starting with the spheres in the upper hierarchy that are most likely to interfere, and then to the spheres in the lower hierarchy that belong to the interfering sphere, which has the effect of improving the speed of collision determination. . In other words, by eliminating portions where there is no possibility of interference in the process of moving from a rough check to a detailed check, it is possible to efficiently narrow down the portions where interference occurs.

[Brief explanation of the drawing]

Fig. 1 is a block diagram showing the overall structure of the present invention, Fig. 2 is a block diagram showing the structure of a device according to a specific embodiment of the present invention, and Fig. 3 is a block diagram showing the structure of a device according to a specific embodiment of the present invention. FIGS. 4, 5, and 6 are flowcharts showing the processing procedure of the CPU, respectively. is an explanatory diagram showing the hierarchical structure of the spherical model, Fig. 9 is an explanatory diagram showing the logic of collision detection, and Fig. 10 is a cubic cell with overlap, a rectangular parallelepiped cell with the overlap part removed, and the cells that contain them. FIG. 11 is a schematic diagram showing the relationship between spheres, FIG. 11 is an explanatory diagram showing interference checking between spherical models with different numbers of layers, and FIG. 12 is a flowchart showing the processing procedure of CPU for searching by interferometry. 1...CPU 3...RAM31...Shape data memory 32...Spherical model memory 33
...Joint angle memory 34...Part position memory Patent applicant Toyota Central Research Institute Co., Ltd.

Claims (1)

[Scope of Claim] A device for checking mutual interference of a plurality of objects including a moving object, comprising a surface information storage means storing surface information representing the surface of each object; hierarchical spherical model generation means for modeling the surface of each object by covering it hierarchically with spheres each having a plurality of different radii; spherical model storage means for storing the position of the generated modeling sphere; and a moving object. sphere position updating means for updating the position of the modeling sphere with a plurality of types of radii according to the movement of the object and outputting it to the sphere model storage means; and an arbitrary time point stored in the sphere model storage means. Based on the position of the modeling spheres with respect to the object, we detect spheres that interfere with each other, and for only those spheres that are determined to interfere, select spheres that further interfere with each other from among spheres with a smaller radius that belong to the spheres that interfere with each other. The detection procedure is executed hierarchically from the sphere with the largest radius to the sphere with the smallest radius, and finally, when there is a sphere with the smallest radius that interferes with each other, it is determined that the two objects will interfere, and the two objects are determined to interfere with each other. 1. An interference checking device for objects, comprising: a spherical interference calculation means for determining the interference position of two objects from the position of a sphere with a minimum radius of interference, and outputting the calculation results.