JPS59136802A - Safety device of robot - Google Patents

Abstract

PURPOSE:To stop immediately the working of a robot when a fault arises by checking the present value of the mobile parts of the robot, the deviation between the present value and the target value and the variation of the deviation per unit time. CONSTITUTION:The present value PX of a robot is compared with the upper limit mobile position PU set at a the 1st setter 24 through a comparator 24. The comparator 24 delivers a fault signal em1 in the case of PX>=PU. The 2nd comparator 25 compares the value PX with the lower limit mobile position PD set at the 2nd setter 26 and delivers a fault signal em2 in the case of PX<=PD. The 3rd comparator 27 compares the absolute value ¦e¦ of the deviation between the value PX and the target value P with the maximum limit value ex of the deviation set at the 3rd setter 29 and delivers a fault signal em3 in the case of ¦e¦>=ex. The 4th comparator 30 compares the absolute value ¦e'¦ of the variation of the deviation (e) with the maximum limit value ey of the variation set at the 4th setter 31 and delivers a fault signal em4 in the case of ¦e'¦>=ey. Each of these fault signals actuates a relay 38 via an OR circuit 37 and then stops a servomotor 10 via a servo amplifier 21.

Description

[Detailed description of the invention] The present invention relates to a safety device for a robot.

First, an example of a conventional robot and its drive control device will be described with reference to FIGS. 1 and 2.

The articulated robot shown in Fig. 1 has a base 1 and a base 1.
A shoulder part 2 mounted on an upper surface so as to be rotatable in the direction of arrow A about an axis 1α perpendicular to the surface, an upper arm 3 connected to the shoulder part 2 so as to be rotatable in the direction of arrow B about an axis 2σ, and this upper arm. A middle arm 4 is connected to the tip of the middle arm 4 so as to be pivotable in the direction of the arrow C by a shaft 3αt, and a first lower arm 4 is coupled to the tip of the middle arm 4 so as to be pivotable in the direction of the arrow C by a shaft 4α. An arm 5 and a second shaft 5a provided at the distal end of the first lower arm 5 in a direction perpendicular to the shaft 4a↓ and thus connected rotatably in the direction of arrow E.
The lower arm 6 and this second lower arm 6↓; are connected to be rotatable in the direction of arrow F by a shaft 6a, and constitute six shafts including a mechanical hand 8 or a wrist 7 to which a welding gun or the like is attached.

A drive motor (not shown) is attached to each movable part from the shoulder 2 to the wrist 7, and these drive motors are controlled by a drive control device shown in FIG. The movable part is driven.

Next, the outline of the drive control device for the robot shown in FIG. 1 will be explained with reference to FIG. 2.

In addition, in the figure, only the block circuits related to the JR section 2 are specifically shown in each movable part from the shoulder part 2 to the wrist 7, but each circuit related to each movable part from the upper arm 3 to the wrist 7 is shown in detail. Since the configuration is exactly the same as 2, the explanation thereof will be omitted.

In the figure, a microcomputer 9 constituting a control section of the robot stores target value data P indicating the total amount of rotation of the motor 10 that drives the shoulder section 2 and positioning range data Δe (absolute value) which is a predetermined value. It also outputs target value data and positioning range data for each movable part from the upper arm 6 to the wrist 7, respectively.

The target value data P output from the microcomputer is written into the direct register 11.

In the current value register 12, current value data PX indicating the current value of the shoulder portion 2 is written from, for example, a photoelectric absolute encoder 1'5 attached to the output shaft 10α of the motor 10 via a reduction gear (not shown).

Note that the code disk in the absolute encoder 13 is
A speed reducer (not shown) attached to the output shaft 10 (2) of the motor 10 allows the shoulder portion 2 to rotate once within the maximum movable range, and the current value data PX written in the current value register 12 corresponds to the rotation of the motor 10. It will be updated accordingly.

The current value data px from the absolute encoder 16 is also input to the microcomputer 9 and is provided as data for displaying the current value of the shoulder 2, for example.

The subtracter 14 subtracts the current value data PX of the current value register 12 from the target value data P of the target value register 11 to obtain the deviation amount e between the two.

1st. The second function generators 15, 16 are each used to define a part of the position loop gain KV of this drive control device, and each of the second function generators 15, 16 are connected to the first function generator 1, as shown in FIG.
5 outputs a speed command υ of 01xe in accordance with the deviation amount e from the subtractor 14, and a second function generator 16 outputs a speed command υ of θ2×e in accordance with the deviation amount e.

The position loop gain KV is the total value of the amplification factor (gain) of the position feedback servo loop, and the position loop gain KV is the total value of the amplification factor (gain) of the position feedback servo loop.
It is defined as the value obtained by dividing the operating speed V (IIn / sec) by the deviation amount e (+nm).

Therefore, this position loop gain KV also includes the gain of the servo amplifier 21, which will be described later.

The comparator 17 calculates the absolute value 1el of the deviation amount e output from the subtracter 14 (obtained by an absolute value circuit not shown),
The positioning range data Δe of the positioning range register 18 written from the microcomputer 9 is compared with the positioning range data Δe, and only when e≦Δe, the positioning signal α is output to the data selector 1 and the microcomputer 9, respectively.

That is, the deviation amount e which is the output of the subtractor 14 is
Since the drive amount (rotation amount) becomes smaller as it approaches the target value P, it is determined whether the absolute value 1e1 of the deviation amount e has reached the positioning range data Δe, which is a predetermined value. It is possible to know whether the shoulder portion 2 has entered the positioning range (region).

Note that the reason why such a determination is made is to perform the following control.

That is, in order to specify the tip trajectory and robot posture while the tip of the robot (mechanical hand 8 in the case of FIG. 1) moves to the final target position, write to each target value register from shoulder 2 to wrist 7. Since the target value data has a plurality of relay points set between each origin and the final target position, it is necessary to create a timing for writing to each target value register. Taking an example of a drive control device for 2, the target value data indicating each passing relay point is set as P1 to pn-1+.
Assuming that the target value data indicating the final target position is Pn, the comparator 17 determines whether the shoulder 2 has entered the positioning range 1/IJ at P□ to Pn-1, respectively, and has entered the positioning range. At each point, the microcomputer 9
Pn is written into the target value register 11 in order.

-In addition, when doing this, the shoulder part 2 becomes P, ~ p, -.

It does not reach each passing relay point exactly, but since Δe is set to, for example, 4 to 5 and 11, there is no problem.

Another thing is to make the position loop gain KV as large as possible (Kv'=1/TM:
If TM is the response time constant of the motor 1o), the tracking accuracy will be better, but if this is done, hunting is likely to occur during positioning.

Therefore, it is determined whether each movable part from the shoulder part 2 to the wrist 7 has entered the positioning range at each final target position, and when the movable parts enter the positioning range, the position loop gain KV that had been increased until then is determined. Make smaller.

That is, taking the shoulder 2 as an example, before the data selector 1S receives the positioning signal a from the comparator 17 indicating that the shoulder 2 has entered the positioning range of the final target position,
Select the speed command υ of θ1e output from the first function generator 15, and when the above positioning signal a is input, the second
The speed command υ 92e output from the function generator 16 of
You can select '.

In addition, in this DEXE 1991, the positioning signal a indicating entering the positioning range of each passing relay point is ignored by the command from the microcomputer 9, but these positioning signals a are not ignored. Alternatively, switching between 01e and θ2e may be performed for each passing relay point.

If this is done, the deviation amount e will be 18 as shown in FIG.
When 1>Δe, the speed command V changes as shown by the straight line, and when IeI≦Δe, it changes as shown by the straight line ■, so the position loop gain KV in the positioning area shown with diagonal lines is It is small and hunting does not occur during positioning.

Returning to FIG. 2, the D/A converter 20 converts the speed command υ from the data selector 19 into a speed voltage signal S1 which is an analog value.

The servo amplifier 21 amplifies the deviation between the speed voltage signal SV from the D/A converter 20 and the speed feedback voltage signal Sv2 output from the tacho generator 22 directly connected to the output shaft 10a of the motor 10. voltage signal S
(2) Output signal 3 to the motor 10 to rotate the motor 10, thereby moving the shoulder portion 2 from the origin to the final target position.

By the way, in the above-mentioned robot drive control device, if a break in the position feedback servo loop, a failure in the absolute encoder, or an abnormality in the microcomputer occurs, the robot may run out of control. If the robot runs out of control, it could cause unexpected disasters such as injuring workers or colliding with other production equipment, destroying the robot itself and its production equipment, so it was necessary to take sufficient safety measures. .

Therefore, an object of the present invention is to provide safety yii for a robot having a fail-safe function that can constantly monitor the position feedback control system and immediately stop the robot if an abnormality as described above occurs.

Therefore, the robot safety device according to the present invention is capable of checking the current value of the movable part of the robot, checking the amount of deviation between the current value and the target value, and the unit of the deviation amount in the robot drive control device as described above. The amount of change (velocity) per hour is checked, and if any one of them becomes an abnormal value, the motors that drive the movable parts of the robot are immediately stopped.

Hereinafter, embodiments of the present invention will be described and explained with reference to FIG. 4 and subsequent figures of the drawings.

FIG. 4 is a block diagram showing an embodiment of the present invention.

In this figure, parts corresponding to those in FIG. 2 are denoted by the same reference numerals, and explanations of those parts will be omitted.

In the figure, the first comparator 23 receives the current value data px written in the current value register 12 and the first setter 2.
4, the first value indicating the upper limit movable position of the shoulder portion 2 is constantly compared with PU, and only when PX≧PU, the first abnormal signal e M 1 is outputted, and the second comparator 25 constantly compares the current value data px with a second peg PD that indicates the lower limit movable position of the shoulder portion 2, which is preset in the second setter 26.

The second abnormal signal ettt2 is output only when px≦PI).

Specifically, the maximum movable range of 111 part 2 is 0OOH to FF, which is the data range of Abflu 1 - encoder 13.
Assuming that it wraps around FH, the actual shoulder part 2 (1) i
'iJ motion range for example 150H-D8011 (150
11 is the origin of shoulder part 2), the first value pu is set to F80II, the second value 1)D is set to 080H, and the current value data px is set to I)X ≧F8011, P
≦08011 (7) Time d (71 mi, the first and second abnormal signals cm from the first and second comparators 23 and 25 respectively,
Make sure that ent2 is output.

The third comparator 27 outputs a value ICI obtained by converting the deviation amount C output from the subtracter 14 into an absolute value in an absolute value circuit 28, and a maximum limit value of the deviation amount preset on the third setter 2. The third abnormality signal ett3 is output only when 1e1≧ez.

The fourth comparator 30 outputs the absolute value l21 of the amount of change per unit time of the deviation amount e outputted from the absolute value circuit '56, which will be described later, and the amount of change preset in the fourth setter 61. A fourth value eg (for example, about 20 bits) indicating the maximum limit value of
The fourth abnormal signal etyt4 is output only when Iel≧ey.

The absolute value tel of the amount of change in the deviation amount e per unit time is:
I'm asking for it like this:

That is, when the oscillator '52 outputs the lock pulse GK as shown in FIG. 5(a), the timing circuit 33
The first . Second
timing pulse GK2.

Output CR2.

Then, the latch circuit 34 operates the subtracter 14 at the timing of the first timing pulse CK2 from the timing circuit 33.
The subtracter 35 latches the deviation amount e from the subtracter 14 at the timing of the second timing pulse CK3. Clock pulse GK from 55
The amount of change in the deviation amount e per unit time determined by the pulse width of is outputted, and the absolute value of the amount of change is determined by converting this amount of change into an absolute value in the absolute value circuit 36. .

Next, to explain the stopping means in detail, the first to fourth abnormal signals e from the first to fourth comparators 2, 25, 27, and 50 are
111 to e114 are inputted via an OR circuit 37 to the base of a switching transistor 6, which turns on and off a relay coil 38.

Note that 40 is a flywheel diode.

Three normally closed contacts 38 operated by a relay coil 38
A is inserted into the 3φ, 200V power supply line of the servo amplifier 21, and the first to fourth abnormal signals are sent via the OR circuit 67, and any one of 1 to eM4 is connected to the base of the switching transistor 3. By inputting the input, the switching transistor 3 is turned on and the relay coil 38 is activated, and the three normally closed contacts 38a are immediately opened and the power supply to the servo amplifier 21 is cut off. 10 stops.

Note that if the power supply to the servo amplifier 21 is cut off.

A normally closed contact (not shown) that was open during power supply is closed and a dynamic brake resistor is connected to the motor 10, so that the motor 10 is braked.

Next, the effects of the above embodiment will be explained.

(b) If the target value data P differs considerably from the normal value due to an abnormality in the microcomputer S itself, a teaching error, external noise, etc., the absolute value of the deviation amount e is 1.
Since el becomes Iel≧ex, the third abnormal signal el13 from the third comparator 27 turns on the switching transistor 3 and operates the relay coil 38, which opens the normally closed contact 38a and turns the motor 10 stops.

However, it is assumed that the difference between successive target value data in each target value data Pl-Ptt mentioned above is smaller than the third value ex.

(b) The wiring between the absolute encoder 13 and the current value register 12 in the position feed bank servo loop is disconnected, and the current value data PX becomes PX≧1” 80
II or px≦080H, the first or second comparator 23.25 outputs the first or second difference: (signal e
! II 1. Because em 2 outputs.

Otherwise, the remoter 10 stops.

This first or second abnormal signal eJl! l.

Another example where Cl1l 2 is output is absolute =
1- Disconnection failure inside the encoder N3, several faults in the absolute encoder 16 to the output shaft 10a of the motor 10 (when the origin position of the Ji part 2 and the predetermined origin position data are significantly different), etc. is possible.

(c) If the current value data PX changes discontinuously due to a break in the speed feedback loop or a reading error in the Abflue 1-encoder 13, the absolute value of the amount of change e per unit time of the deviation amount e is only 1. 1 is I
'e1≧ey, so the fourth
The motor 10 is stopped by the abnormality signal ex4.

(d) If the motor 10 continues to rotate at low speed due to some reason such as a break in the wiring of the command data system in the position feedback servo loop, the current value data px will change when the shoulder 2 exceeds the actual movable range. PX≧pu or P
Since X≦PD, similarly to the case (a) above, the first or second abnormal signal eIII, gg2 causes the motor 1
0 stops.

(e) Even if the shoulder portion 2 attempts to run out of control due to other factors than those listed in (a) to (d) above, check the upper and lower limits of the current value and check the amount of deviation to detect abnormalities caused by those factors.

The first to fourth comparators 23 respectively check the amount of change per unit time of the amount of deviation and the amount of deviation. - Since almost all of the angles of -25 and 27 degrees 30 can be captured, it is possible to prevent the shoulder portion 2 from running out of control.

Although the explanation is omitted, since the other movable parts of the robot from the upper arm 3 to the wrist 7 are constructed in the same manner as the shoulder part 2, these other movable parts will not run out of control.

However, the first to fourth values for each iiJ moving part need to be set according to the amount of momentum of each moving part.

In addition, 1. In a multi-jointed robot like the embodiment described in L, the movable part closer to the tip of the robot 1- has a smaller amount of momentum.
Although it is not necessarily necessary to provide the safety device according to the present invention in the drive control device for all movable parts, for example, for an orthogonal three-axis robot, etc., the safety device described above may be provided in the drive control device for all movable parts. It is necessary to provide

Furthermore, in the embodiment described above, when the first to fourth abnormal signals 1 to Cr4 are generated, which abnormal signal is generated?
It may also be possible to display the data and provide it as data for investigating the cause of the failure.

As described above, according to the present invention, various abnormalities in the position feedback loop control system can be reliably detected and the robot can be immediately stopped, thereby improving safety.

[Brief explanation of drawings]

1 is a schematic diagram showing an example of the configuration of a robot, FIG. 2 is a block diagram showing a conventional example of a drive control device for the robot shown in FIG. 1, and FIG. Diagram for explaining the second function generator. FIG. 4 is a block configuration diagram showing an embodiment of the present invention. 5(A) to 5(C) respectively show the latch circuit 34 of FIG.
3 is a timing chart for explaining the operation of the subtracter 35. FIG. 9...Microcomputer 10...Motor 1
1...Target value register 12...Current value register 1゛3...Absolute encoder 14.35...
・Subtractors 23.25, 27.30...first to fourth comparators 24
．． 26.29.31...First to fourth setting devices 32...
・Oscillator! 13...Timing circuit 34...
Launch circuit 37...OR circuit Applicant Nissan Motor Co., Ltd. Agent Patent attorney Takashi Osawa Figure 1

Claims (1)

[Claims] 1. In a robot drive control device that controls the drive of a motor that drives a movable part according to the amount of deviation between a current value and a target value of a movable part of the robot, the current value indicates a predetermined upper limit movable position. a first comparator that outputs a first abnormal signal only when the current value is equal to or greater than a first value indicating a predetermined lower limit movable position; a second comparator that outputs an abnormal signal; and a third comparator that outputs a third abnormal signal only when the deviation amount is greater than or equal to a third value indicating a predetermined maximum limit value. “a fourth comparator that outputs a fourth abnormal signal only when the amount of change in the deviation amount per unit time is equal to or greater than a first L value indicating a predetermined maximum limit value; A safety device for a robot, comprising: a stop means for stopping the motor only when any one of the first to fourth abnormality signals is output from a comparator.