JP5036661B2 - Interference check control apparatus and interference check control method - Google Patents

Description

  The present invention relates to an interference check control device and an interference check control method for a plurality of robots arranged close to each other and having overlapping operation areas.

  There is disclosed a technique for performing an interference check between robots in a robot system that operates simultaneously in a state where the operation areas of at least two robots overlap. In general, the robot is divided into several components, and the robot components are modeled with an approximate solid using, for example, a cylinder and a hemisphere so as to cover the robot components, and the distance between these approximate solids is calculated. A detection method (see Patent Document 1) is performed. For example, a robot is modeled by a simple line segment, a closest distance between the line segments is calculated, and an interference state is checked.

  As a technique for stopping the robots from interfering with each other using the interference check method as described above, for example, the size of the robot tool is larger than the size of the robot tool around the robot tool, which is a hand part to which an end effector is attached There is a method of providing a double interference determination region and determining whether to stop one or both according to the relationship between the interference state and the relative movement direction of each robot (see Patent Document 2). Also, calculate the posture at the time of stopping completion when starting the stopping operation to avoid interference, determine the presence or absence of interference between the robots for that posture, and each robot with respect to the posture at the time of stopping completion There is a prior art called a method (see Patent Document 3) in which it is determined that a stop is necessary if there is mutual interference and a stop is started.

JP 60-217410 A Japanese Patent No. 3351330 Japanese Patent No. 3907649

However, the prior art has the following problems.
When the robot's hand trajectory draws a straight line or an arc along the teaching trajectory, for example, each robot component may move in a complicated manner rather than a monotonous operation. This corresponds to a movement in which a certain robot component part and another robot component part relatively approach or separate from each other. When robots are arranged close to each other and their operation areas overlap, there is a big problem that there is a risk that the robot components will interfere with each other due to the complicated operation of the robot components. Interference check between robot components could not be done.

  For example, in Patent Document 2, the conventional method is limited to a robot tool and does not determine the distance between the component parts. There was a possibility of interference. In Patent Literature 3, when performing an interference check for each control calculation cycle for creating a robot control command, the stop position interval between the robot stop position calculated at a certain time and the stop position calculated at the next time by one control cycle. However, since it becomes large particularly during high-speed operation, it is not possible to determine the possibility that the robot components interfere with each other between adjacent stop positions where the interference check is performed.

  The present invention has been made to solve the above-described problems, and an interference check control apparatus and interference check control capable of determining with high accuracy the presence or absence of interference between components of a robot during a braking operation. The purpose is to obtain a method.

The interference check control device according to the present invention is an interference check control device for robots having overlapping portions in their respective operation areas, and stores the speed / position data of the robot calculated for each calculation cycle. Based on the speed / position data stored in the speed / position storage means, the threshold value storage means for storing in advance the threshold value of the interference determination area for performing the interference determination between the robots, Approach the braking time calculation means that calculates the time required to completely stop when braking is started from the present time, and the history of the closest distance between each component of each robot from the start of braking to the completion of braking inner product of the velocity of the nearest neighbor direction vector and each component of each robot to each other between the components of the respective robots based on the speed of each component to A robot distance estimating means for estimating with the upper limit value of the approach speed values being al calculated, and the closest distance history among the component parts which are estimated, pre-stored interference determination for the collision determination threshold value storage means Compared with the threshold value of the area, the interference determination means for determining the presence or absence of interference between the start of braking and the completion of braking, and the movement command value for each robot for each calculation cycle according to the interference determination result by the interference determination means And a command value generation means for storing the speed / position data of the robot in the current calculation cycle in the speed / position storage means.

The interference check control method according to the present invention is an interference check control method for robots having overlapping portions in their respective operation areas, and the robot is controlled based on the speed / position data stored in the first storage unit. Approach the braking time calculation step to calculate the time required to completely stop when braking is started from the present time, and the history of the closest distance between each component of each robot from the start of braking to the completion of braking Based on the velocity of each component to be estimated using the upper limit value of the approach speed value calculated from the inner product of the closest direction vector between the components of each robot and the velocity of each component of each robot Comparing the distance between the robots to be estimated, the history of the estimated closest distance between each component, and the threshold value of the interference determination area stored in advance in the second storage unit In accordance with the interference determination step for determining the presence or absence of interference between the start of braking and the completion of braking, and the movement determination value for each robot in each calculation cycle according to the interference determination result in the interference determination step, A command value generation step of storing the speed / position data of the robot in the first storage unit.

  According to the interference check control device and the interference check control method according to the present invention, the change in the closest distance between the constituent parts from the start of braking to the completion of braking is estimated based on the speed of the approaching constituent parts. Thus, it is possible to obtain an interference check control device and an interference check control method that can determine with high accuracy the presence or absence of interference between the components of the robot during the braking operation.

Hereinafter, preferred embodiments of an interference check control device and an interference check control method of the present invention will be described with reference to the drawings.
The present invention estimates the history of the closest distance between robot components from the start of braking to the completion of braking based on the speed of each approaching component, and whether there is interference between robots from the start of braking to the completion of braking. Is determined based on the estimation result of the change in the closest distance for each robot component. As a result, the cases where components interfere with each other during braking operation are not overlooked, and in the interference problem between robots that are arranged close to each other and the operation areas overlap, the interference state is discovered more than the conventional method, and the robot is damaged. There is an effect to prevent.

  In addition, braking in the following description refers to an operation in which a deceleration command is given to the drive source of each movable part in order to stop, and the vehicle stops after a time defined by the current speed and acceleration that can be output. That is.

Embodiment 1 FIG.
FIG. 1 is a configuration diagram of an interference check control apparatus according to Embodiment 1 of the present invention. 1 includes a robot speed / position storage unit 910, a braking time calculation unit 911, an inter-robot distance estimation unit 916, an interference determination unit 917, an interference determination threshold storage unit 918, and a command value generation unit 919. It has.

  Before describing each component, an interference check performed in the first embodiment will be described. First, a space that is formed by the movable range of the robot and through which the robot can pass is expressed as an operation area. When two or more robots intersect with each other in the operation area, there is a risk of collision between the robots. As an expression representing such a collision between robots, an expression of interference is used in a state where spaces occupied by robots intersect. In addition, the expression “interference” is also used in a state in which interference determination areas described later intersect. Also, checking whether such interference occurs is called an interference check. FIG. 2 is a diagram illustrating a configuration example of a robot system that performs an interference check in the first embodiment of the present invention. In the robot system as shown in FIG. 2, the interference check control devices 3A and 3B included in each robot control device of each robot perform the interference check between the robots every calculation cycle.

  The information is shared by communicating the position and speed information of each of the two robots by the interference check control devices 3A and 3B. This information is calculated by each of the interference check control devices 3A and 3B, and is communicated and shared by, for example, Ethernet (registered trademark) or RS232C.

  When checking the interference between the robots, it is necessary to calculate distance information between the robots as an index indicating the approaching state. Therefore, in the first embodiment, the distance between the robots is obtained by modeling with a line segment for each robot component and calculating the distance between the line segments.

  FIG. 3 is an exemplary diagram of a line segment model of the robot according to the first embodiment of the present invention. Each component is represented as a set of line segments 20. At this time, the subscripts i and j added to the robot component model are serial numbers when the robot component is counted as 1, 2,.

  In the first embodiment, the robot control device that controls the robot operation normally controls the robot based on the command generated by the command value generation means 919 for each calculation cycle. Here, the robot command value generated to control the robot motion is, for example, when a given start point and target point are given, the robot end effector draws a linear trajectory and starts from the target point. This refers to determining a target value at the current time of each drive source such as a motor provided in the robot so as to move relative to the point.

In the present invention, the interference determination is performed every time the command value calculation performed in the calculation cycle is performed. For this purpose, as shown in FIG. 1, the braking time calculation means 911 includes position information 901 including the current position and posture of the robot and speed information 902 including the current operation speed of the robot. Read from 910, and calculate the braking time t _stp of the robot when braking is started from the present time.

Next, the inter-robot distance estimation means 916 reads the braking time t _stp 903 from the braking time calculation means 911, and estimates and calculates the distances between the components of the robots from the start of braking to the completion of braking. More specifically, the inter-robot distance estimating means 916 calculates the distance L _ij between the robot component parts Link _i and the robot model of Link _j selected one by one for two robots from the start of braking to the completion of braking. , Estimate for all combinations.

Next, the interference determination unit 917 determines whether or not the inter-robot model distance L _ij estimated and calculated by the inter-robot distance estimation unit 916 falls within an allowable value for preventing interference from the start of braking to the completion of braking. Compare. Then, the command value generation unit 919 generates a command value reflecting the comparison determination result by the interference determination unit 917 and prevents interference between the robots.

  Here, the robot constituent part is a part constituting the robot defined for performing the interference determination when the robot is composed of a plurality of movable parts. For example, when viewed for each robot, a wrist unit, an arm unit, a base unit, and the like correspond to this robot component.

Next, each component in the first embodiment shown in FIG. 1 will be described in detail in order.
The robot speed / position storage unit 910 is a storage unit that stores information 900 about the position and speed of the robot generated by the command value generation unit at least until a calculation step one step before.

  The braking time calculation unit 911 acquires position information 901 and speed information 902 from the robot speed / position storage unit 910, and calculates a time required until the robot completely stops when braking starts.

  Here, the position information 901 is information related to the position of the robot. For example, the joint component, the position and orientation of the end effector, and the end points of the robot component necessary for calculating the distance between the robot component models constituting the robot It corresponds to position information. Further, the speed information 902 corresponds to information on the speed obtained by the difference between the previous calculation step and the current calculation step of the information on the position of the robot. Further, since the position and orientation of the end effector and the end point position information of the defined robot component can be calculated by kinematic calculation from the joint angle information of the robot, the position information 901 can be simply used as the joint angle information. In addition, information relating to the speed of the robot can also be calculated by calculating kinematics using the joint angular velocity information of the robot.

  Here, the calculation step means a set of a series of calculation processes for performing a command calculation for one control period for operating the robot, which is performed every predetermined control period. This corresponds to, for example, a process of processing a series of calculation groups as shown in FIG. 1 once.

For example, as a method for generating a command for the robot, the braking time calculation unit 911 is a movable part of each robot component for decelerating and stopping when a joint interpolation command for following each joint angle target value is being performed. The braking time t _stp is determined according to each acceleration. For example, when the subscript m is a joint number (m = 1, 2,...), The acceleration a _stpm having a different magnitude depending on each joint for stopping at the current joint angular velocity v _noum is a constant value for each joint. In some cases, the braking time t _stpm of each joint is _expressed by the following equation (1).
t _stpm = | v _now / a _stpm | (1)

Braking time calculation means 911, for each of the joints m can calculate the time _{t STPM} required before braking completion from the start of braking, further placing seeking the greatest _{t STPM} from the _{t STPM,} to as _{t stp} Can do. At this time, the braking time calculation means _911, a new acceleration _{a STPM} of each joint in accordance with _{t stp,} to simultaneously stop the braking time _{t stp} has _passed, as _t stpm ₌ t _stp, the above equation Recalculate using (1). When a plurality of robots are present, select the largest _{t stp} at each _{t stp} the overall _{t stp,} the braking time of each robot as _{t stpm = t stp,} using the above equation (1) Recalculate.

Next, when braking is started from the current time t _now, it is necessary to ensure that the robots do not interfere during the braking time t _stp . Therefore, an interference check method for ensuring that there is no interference between the robots during the braking time _tstp will be described next.

As preparation, first, robot modeling will be described.
For example, as shown in FIG. 3, the robot is modeled by the line segment 20, and the distance between the line segments representing the robot is calculated in order to check the interference between the robots.

FIG. 4 is a diagram geometrically showing the closest point of two line segments modeled in the first embodiment of the present invention. For example, if two line segments for measuring the distance between the robot models are model Link _i and Link _j , the distance between the models L _ij 33 (43) from the closest points 31, 32 (41, 42) of the two line segments. Is guided. As a method for calculating the nearest point, for example, a method as disclosed in Patent Document 1 can be used.

  Then, whether or not the robot interferes is determined using an interference determination area between the robot constituent parts composed of a cylinder and a hemisphere that sufficiently encloses the robot. FIG. 5 is an explanatory diagram of an interference determination area in the first embodiment of the present invention. As shown in FIG. 5, the robot is wrapped with a cylinder and a hemisphere at a distance r from the line segment model, and this is defined as a robot interference determination region. When the interference determination areas interfere with each other, it is determined that the robots interfere with each other.

For example, when the sizes of the interference determination areas are all r (that is, in the case of the interference determination area defined as the distance from the line segment 21 being r in FIG. 5), the braking time from the current time t _now It is assumed that there is no interference when the following equation (2) holds for the inter-model distance L _ij until t _stp elapses.
2r ≦ Lij (t) (t _now ≦ t ≦ t _now + t _stp ) (2)

  The model used for calculating the distance can use not only a line segment but also a polygon model or a detailed CAD model, but here, for simplicity, the line segment is handled. If control is performed so that such modeled robots do not interfere with each other, the actual robot does not interfere with each other.

Based on the interference checking method between the modeled robot components as described above, it is necessary to confirm whether the robot causes interference from the current time t _now to the braking completion time t _now + t _stp . In the prior art, as described in the problem to be solved, when braking is performed while the robot is operating, it may not be possible to check whether all robot components interfere with each other until braking is completed. there were.

  Therefore, in the present invention, when checking the interference between the robots, the interference check is performed for all combinations of the links of the respective robots. FIG. 6 is an explanatory diagram of an interference check method between two robots according to the first embodiment of the present invention. In the example of FIG. 6, the robot A that is the first robot and the robot B that is the second robot are each composed of four links.

  Then, when checking the interference between the robot A and the robot B, the interference check is performed for all combinations of the links A1 to 4A of the robot A and the links 1B to 4B of the robot B. However, the interference check can be omitted between the links having no risk of collision (for example, Link 1A and Link 1B in FIG. 6).

Further, an example in FIG. 6, the interference determination area size is performed and Link3A having a size of _{L 3ALim,} collision detection region size comparison for the closest distance between Link3B having a size of _{L 3BLim} . _Paying attention to a change in the distance L _ij (corresponding to 33 in FIG. 4) between the robot components having a risk of interference, whether or not this interferes is determined as follows.

Each code is defined as follows.
Link _i : Robot component model of robot A (i = 1, 2, 3, 4)
Link _j : Robot component model of robot B (j = 1, 2, 3, 4)
L _ij : Distance between Link _i and Link _j L _iLim , L _jLim : _Line segment model for each robot component model _{serving as an} interference determination area
Distance from Le this case, the Link _i of the robot A, the size _{L IjLim} interference determination region between the Link _j of the robot B is a following formula (3).
L _ijLim = L _iLim + L _jLim (3)

Therefore, by using L _ijLim defined by the above equation as a threshold value for interference determination, it can be determined that interference occurs when the inequality of the following equation (4) is not satisfied.
L _ijLim ≦ L _ij (4)

When braking is started from the current time, it is estimated whether or not the change curve L _ij (t) of the distance between the robot component models until the stop reaches L _ijLim even once, and the curve L _ij (t) is If L _ijLim is not reached, it can be determined that there is no interference. Then, next, the estimation method of distance _Lij between robot component part models is _demonstrated .

First, consider the approach between two robot component models. The inter-model distance is defined by the closest distance L _ij between the robot component models as described above. Therefore, if the history L _ij (t) of the closest distance L _ij between the robot component models can be traced, an accurate interference check can be performed. However, calculating the distance change from the start of braking to the stop over the entire time requires a large calculation load. For this reason, for example, when estimating interference in real time and considering a stop, it is not realistic from the viewpoint of calculation load to calculate over the entire time.

Further, the position of the closest point between the robot components changes from moment to moment. Therefore, from the history L _ij of the distance of closest approach finite _(t), to accurately estimate the L _{ij (t)} by considering the recent change in the position of contact over the entire time it is difficult.

Therefore, in the present _invention, the estimation of _{L ij,} taking into account the approach speed value L _{(·) ij} of _{L ij,} is performed as follows. However, (•) means a speed value with a mark at the top of the symbol representing the distance before (). With respect to the approach speed value L (·) _ij of L _ij , an upper limit value L (·) _{ijLim of the} approach speed value can be defined with respect to the approach of two lines so that no interference is overlooked due to the position change of the closest point. Therefore, it is possible in a way Komu added _L the integral value of the _{(·) ijLim} the _{L ij} far to estimate the approaching the upper limit of _L ij to brake completion from the start of braking (t).

Here, the definition of the upper limit L (·) _ijLim for the approach speed is shown below. FIG. 7 is an explanatory diagram showing that the speed at the link end is larger as the speed value than the speed at the closest position in the first embodiment of the present invention. As shown in FIG. 7, two robotic components of the line segment model Link _i and Link _j each recent speed near contact _{V icls, V jcls} the approaching speed value L _(·) corresponding to the differential value of _{L ij ij} relationship Is expressed by the following equation (5), where N _ijcls is the closest direction vector of the two line segment models.
L (·) _ij = N _ijcls (V _icls −V _jcls ) (5)

In addition, recent speed _{V icls} near the contact of Link _i is always slower than either of the endpoints speed of the end point velocity _{V iedg01} and _{V iedg02} of Link _i (reference numerals 61, 63, 64 in FIG. 7). For this reason, the higher end point speed with respect to the approach direction can be set to the upper limit value _Viedg . When the upper limit value V _iedg is used, since it is not necessary to calculate the closest point and the closest distance L _ij on the robot component model line segment for the purpose of knowing L (·) _ij , the calculation load is small. I'm sorry. When L _ij until the braking time t _stp elapses is estimated based on the upper limit value L (·) _ijd derived from the end point speed, no interference occurs.

Based on the above, when estimating the robot model distance L _ij until the braking time t _stp elapses, the following is performed. Plotting L (·) _ij from the start of braking to the completion of braking is a computational burden. Therefore, here, link end speed information (63, 64) of two finite line segment models for estimation is obtained as reference information, and L _ij is estimated based on the information.

FIG. 8 is an internal configuration diagram of the inter-robot distance estimation means 916 that estimates L _{ij according} to Embodiment 1 of the present invention. 8 includes a sampling time determining unit 912, a robot position calculating unit 913, an end point speed calculating unit 914, and an inter-robot distance estimated value calculating unit 915.

The sampling time determining means 912 determines the time for obtaining this reference information. Sampling time determination means 912 divides the braking time t _stp (903) from the start of braking to the stop by dividing it into n (n is a natural number), assuming that the robot proceeds along the trajectory and stops. For example, t _smpk (904) is determined as the following expression (6).
t _smpk = t _now + t _stp * k / n (k = 1, 2,... n−1) (6)

Here, the subscript k is a serial number of the sampling time. Next, at each time of sampling time t _smpk , the robot position calculation means 913 calculates the end point position (905) of the robot component. In addition, when the time taken for one calculation cycle is dt, the robot position calculation unit 913 similarly calculates the end point position (905) of the robot component at the time (t _smpk + dt) after dt.

As a method of calculating the link end speed information (see 63 and 64 in FIG. 7), here, for example, at the time t _smpk of two end points (in the following description, distinguished by edg01 and edg02) in the robot component model position _{p edg01 (t smpk)} and _{p edg02 (t smpk),} adopt a method of calculating the difference between _p Edg02 at time _{t smpk + dt (t smpk +} dt) and _{p edg02 (t smpk + dt)} .

As a result, the end point speed calculation means 914 causes the end point speeds V _iedg01 (t _smpk ) and V _iedg02 (t _smpk ) of each robot component at the sampling time t _smpk (k = 1, 2,..., N−1). ), V _jedg01 (t _smpk ), V _jedg02 (t _smpk ) (906).

Thereafter, the inter-robot distance estimated value calculation means 915 for calculating the estimated value of L _ij has either the end point velocity V _iedg01 or V _iedg02 of Link _i as the speed V _icls near the _closest point of Link _i , as described above. _Therefore , the larger end point speed with respect to the approaching direction can be set as the upper limit value _Viedg . Here, this upper limit value _Viedg is used as the speed upper limit value of the component.

The two end points velocity, _V IEDG in each _{t smpk (t smpk),} Knowing the _V jedg _(t smpk), the upper limit _{L (·) ijd (502)} has recently the tangent direction vector as _{N Ijcls,} It is obtained in the form of the following formula (7).
L (·) _ijd = N _ijcls · (V _iedg (t _smpk ) −V _jedg (t _smpk )) (7)

Of the two L (•) _ijd for each end point obtained here, the larger one in the approach direction is determined as the upper limit value L (•) _ijd . Also, here, for simplicity, sampling is performed with n = 3, and ΔL _ijd is calculated by approximating a trapezoid between each sampling. FIG. 9 is an explanatory diagram showing trapezoidal approximation for calculating the distance from the speed value when calculating the distance between the closest points using the end point speed of the robot component according to the first embodiment of the present invention.

By using the following equation (8) by trapezoidal approximation as shown in FIG. 9, an estimated value L _ijd at each sampling time is obtained.
L _ijd = L _ij (t _now ) + ΔL _ijd (8)

The interference determination means 917 is defined by the size of the interference determination area of the robot component model to be checked from the interference determination threshold storage means 918 that stores in advance the threshold value of the interference determination area for performing the interference determination. The interference determination threshold L _ijLim 908 is read out. Then, the interference determination unit 917 _compares the estimated value L _ijd 907 of the inter-robot distance obtained by the inter-robot distance estimation unit 916 using the above-described estimation method and the read interference determination threshold value L _ijLim 908. The following equation (9) is used to determine interference at every sampling time.
L _ijLim ≦ L _ijd (9)

  When the condition of the above equation (9) is satisfied, the interference determination unit 917 determines that no interference occurs and transmits the fact to the command value generation unit 919 (909). In this case, a movement command toward the target value that follows in the robot control device is generated as the movement command value. On the other hand, when the condition of the above equation (9) is not satisfied, the interference determination unit 917 determines that there is a risk of interference, and transmits a command change for starting a stop as a movement command value (909). To the command value generation means 919.

FIG. 10 is an explanatory diagram for starting deceleration when the interference determination unit 917 based on the estimation result determines that there is a risk of interference in the first embodiment of the present invention. As shown in FIG. 10, when it is determined that there is a risk of interference when the limit (601) determined to cause interference is reached based on the estimation result at the current time t _now 201 at which the determination is performed, The interference determination unit 917 transmits a command change (909) for starting deceleration stop to the command value generation unit 919.

  As described above, according to the first embodiment, the change in the closest distance is estimated based on the speed upper limit value for the change in the distance between the components from the start of braking to the completion of braking. Then, based on the estimation result of the change in distance for each robot component, it is determined whether there is no interference between the robots from the start of braking to the completion of braking. As a result, the case where the components interfere with each other during the braking operation is not overlooked, and in the interference problem between the robots arranged close to each other and overlapping the operation area, the effect of detecting the interference state without leakage compared to the conventional method There is.

Embodiment 2. FIG.
As shown in FIG. 8, the inter-robot distance estimation unit 916 in the first embodiment has estimated the inter-robot distance based on the end point speed obtained at each sampling time obtained by the end point speed calculation unit 914. . On the other hand, this Embodiment 2 demonstrates the case where more accurate estimation is performed when it has sufficient calculation capability for estimating the distance between robots.

FIG. 11 is an internal configuration diagram of the inter-robot distance estimating means 916a for estimating L _{ij according} to the second embodiment of the present invention. 11 includes a sampling time determining unit 912, a robot position calculating unit 913, a closest point / recent speed calculating unit 914a, and an inter-robot distance estimated value calculating unit 915.

11 calculates the closest distance L _ij and the approach speeds V _icls and V _{jcls at} the closest point for each of the n samples described in the first embodiment (914). ) And L (·) _ij is obtained by calculating the above equation (5).

  FIG. 12 is a diagram illustrating an example of a comparison between the approach speed change from the start of braking to the stop in Embodiment 2 of the present invention based on the closest distance and the end point speed of the robot component. The plots indicated by white circles indicate the estimation results by the inter-robot distance estimation means 916 in the first embodiment, and the plots indicated by black circles are the estimation results by the inter-robot distance estimation means 916a in the second embodiment. Is shown.

As shown in FIG. 12, instead of the upper limit value L (·) _ijd (502), the approach speed L (·) _ij (501) obtained by the time change of the closest distance L _ij between the actual robot component models. By using this, it is possible to predict the speed change (511) until the actual stop more accurately.

  As described above, according to the second embodiment, the speed change from the start of braking to the actual stop can be predicted with higher accuracy by calculating the closest distance and the approach speed at the closest point. .

Embodiment 3 FIG.
In the third embodiment, it is determined that interference occurs, and after the stop operation is started, the interference determination is continued, and if it is determined that there is no interference by the evaluation formula (equivalent to the above formula (4)), the operation is performed. The case where the process is continued again will be described.

13, in the third embodiment of the present invention, after the start of braking, explanatory view showing a continuous operation in the case where the estimated value L _IJD when subsequently subjected to interference check becomes larger than the threshold value L _IjLim interference determination area It is. When looking at the distance L _ij of a robot components with each other, the distance L _ij as estimated by the stop operation may become estimated increased (see 614 in FIG. 13).

On the other hand, the third embodiment performs deceleration when it is determined that there is a risk of interference in the interference determination at the current time (201). On the other hand, when it is determined that there is no risk of interference at time t ′ _now (202) after the start of deceleration, the deceleration is stopped and the operation is continued (see 615 in FIG. 13). In addition, if the vehicle has not been decelerated until then, a process of continuing the operation according to the trajectory is performed.

  As described above, according to the third embodiment, the stop operation is being performed at the position where the stop has been completed, particularly when it is necessary to generate a stop command dynamically calculated in real time as shown in Patent Document 3. In addition, the robot can continue to operate by determining whether or not to interfere, and stopping the deceleration as necessary. Since the estimation error also increases during high-speed operation, the determination at the start of braking is too large and the stop is started by determining whether interference occurs even during the stop operation. Even in this case, the continuous operation can be performed according to the subsequent situation.

  Thereby, it is possible to determine without overlooking the interference that has been overlooked by the conventional method, there is no interference between the robots, and there is an effect that the approach can be continued without interference to the optimum stop position.

  The present invention is not limited to the interference check in the case of robots. One is a robot and the other is also applicable to an external environment including a modeled peripheral device or jig. For such external environments, the shape and position data are registered in advance, and the same interference check is performed when information on the relative positional relationship with the robot and information on the relative relationship of motion can be shared by communication. Can be applied.

It is a block diagram of the interference check control apparatus in Embodiment 1 of this invention. It is a figure showing the structural example of the robot system which performs an interference check in Embodiment 1 of this invention. It is an illustration figure of the line segment model of the robot in Embodiment 1 of this invention. It is the figure which geometrically represented the closest point of two line segments modeled in Embodiment 1 of the present invention. It is explanatory drawing of the interference determination area | region in Embodiment 1 of this invention. It is explanatory drawing of the interference check method between the two robots in Embodiment 1 of this invention. It is explanatory drawing showing that the speed in a link end is larger as a speed value than the speed in the nearest position in Embodiment 1 of this invention. It is an internal block diagram of the distance estimation means between robots which estimates _Lij in Embodiment 1 of this invention. It is explanatory drawing showing the trapezoid approximation for calculating distance from speed value, when calculating the distance between nearest contacts using the end point speed of the robot component in Embodiment 1 of this invention. In Embodiment 1 of this invention, it is explanatory drawing which starts a deceleration when it determines with the interference determination means 917 based on an estimation result having a risk of interference. It is an internal configuration diagram of a robot distance estimator 916a for estimating the L _ij of the second embodiment of the present invention. It is a figure showing an example of the comparison between the approach speed change from the start of braking to the stop in the second embodiment of the present invention based on the closest distance and the end point speed of the robot component. In Embodiment 3 of this invention, it is explanatory drawing which shows the continuation operation _{| movement} when the estimated value _Lijd becomes larger than the threshold value _LijLim of an interference determination area _| region when performing an interference check after the braking start.

Explanation of symbols

  910 Robot speed / position storing means, 911 Braking time calculating means, 912 Sampling time determining means, 913 Robot position calculating means, 914 End point speed calculating means, 914a Nearest contact / nearest speed calculating means, 915 Distance estimation value calculating means between robots, 916, 916a Inter-robot distance estimation means, 917 interference determination means, 918 interference determination threshold value storage means, 919 command value generation means.

Claims (4)

It is an interference check control device for robots having overlapping portions in their respective operation areas,
Speed / position storage means for storing robot speed / position data calculated at each calculation cycle;
An interference determination threshold storage means for storing in advance an interference determination area threshold for determining the interference between the robots;
Based on the speed / position data stored in the speed / position storage means, a braking time calculation means for calculating a time required until the robot completely stops when braking is started from the present time;
The history of the closest distance between each component part of each robot from the start of braking to the completion of braking is based on the speed of each approaching component part, the closest direction vector between each component part of each robot and each of the above An inter-robot distance estimating means for estimating using an upper limit value of the approach speed value calculated from the inner product of the speeds of the respective components between the robots;
The history of the closest distance between each estimated component is compared with the threshold value in the interference determination area stored in advance in the interference determination threshold value storage means, and interference between the start of braking and the completion of braking is compared. Interference determination means for determining presence or absence;
In response to the interference determination result by the interference determination unit, a command value is generated for generating a movement command value for each robot for each calculation cycle and storing the speed / position data of the robot in the current calculation cycle in the speed / position storage unit And an interference check control device. In the interference check control device according to claim 1,
The inter-robot distance estimation means estimates the history of the closest distance between the constituent parts of each robot from the start of braking to the completion of braking based on the upper limit speed value derived from the end point speeds of the approaching constituent parts. The interference check control apparatus characterized by the above-mentioned. In the interference check control device according to claim 1,
The inter-robot distance estimating means estimates the history of the closest distance between each component part of each robot from the start of braking to the completion of braking based on the approach speed at the closest point of each approaching component part. Interference check control device. An interference check control method between robots having overlapping portions in respective operation areas,
A braking time calculating step for calculating a time required until the robot completely stops when the robot starts braking from the current time based on the speed / position data stored in the first storage unit;
The history of the closest distance between each component part of each robot from the start of braking to the completion of braking is based on the speed of each approaching component part, the closest direction vector between each component part of each robot and each of the above An inter-robot distance estimation step for estimating using an upper limit value of the approach speed value calculated from the inner product of the speed of each component between the robots;
The history of the closest distance between each estimated component is compared with the threshold value of the interference determination area stored in advance in the second storage unit to determine the presence or absence of interference between the start of braking and the completion of braking. An interference determination step,
A command value generation for generating a movement command value for each robot in each calculation cycle according to the interference determination result in the interference determination step and storing the speed / position data of the robot in the current calculation cycle in the first storage unit An interference check control method comprising the steps of:

Images