JPS61243514A - Robot controller - Google Patents

Abstract

PURPOSE:To make smooth following-up with an external force possible by detecting the following-up force of a moving part at the robot damping control time and controlling the riving current of a motor by a compensation command value outputted in accordance with this detected value. CONSTITUTION:When a limit switch 15 of a switching circuit 46 is turned on, a relay coil L1 of a command value control circuit 45 is excited, and movable contacts (c) of changeover switches SW1-SW11 are switched to contacts (a), and a speed command value and a speed feedback value inputted to speed control parts 26 and 36 are set to zero. Then, motors 4 and 10 or the like be come free to break the attitude of each arm or the like. This state is detected by a force sensor 22 to input a voltage corresponding to the state to a following-up force compensating circuit 60. The circuit 60 inputs a following-up compensation command value CS3 corresponding to the input from the sensor 22 to a current control part 37. Thus, though a current command value S2 outputted from the control part 36 is zero, the driving current corresponding to the command value CS3 is flowed to the motor 10 by the control part 37 to turn the arm or the like while following up the external force without resis tance.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a robot control device, and particularly to a robot control device.
The movable parts of can be moved freely by external force,
The present invention relates to a robot control device that can perform so-called stress relief control.

[Conventional technology]

In recent years, various industrial robots have come to be used on factory production lines, and assembly robots are also being put into practical use.

However, in order to place a robot on a continuous line, which has traditionally been widely used in assembly work lines, and have it perform the work of assembling articles (workpieces) that flow continuously on a conveyor, it is necessary to It is very difficult to control the work of the robot itself and synchronize the conveyor with the robot at the same time.

Therefore, if the robot is placed in a "relaxed state" in which movable parts such as arms can be moved freely by external force while performing a certain task, it can be made to follow the movement of the workpiece on the conveyor without any synchronous control. I can do it.

As a robot control device capable of such force-relaxation control, there is a conventional one such as that described in, for example, Japanese Unexamined Patent Publication No. 58-206389.

This device cuts off the drive current to an actuator, such as a motor, that drives a pre-designated movable part out of multiple movable parts in a multi-axis robot, so that the movable part can be moved freely by external force. It is something.

However, in conventional control devices for robots capable of stress relief control, the drive current (power) to the motor that drives the robot, for example, is cut off, and as a means of cutting off the power, a large current Not only is it necessary to use a large and expensive magnetic switch with a large contact capacity, but using it also makes it difficult to turn on and off the contact.
Problems include the need to take measures to prevent inrush current flowing into the motor when it is off, and the need to perform frequent maintenance on the contacts.

Therefore, the control of movable parts such as arms in robots is
Generally, the drive current of the motor that drives the moving part is controlled according to the command value based on the deviation between the speed command value and the speed feedback value from the speed detection system of the moving part. A robot whose movable = 3 part can be moved freely by external force by setting the command value based on the deviation from the speed feedback value to zero regardless of the actual speed command value and speed feedback value. The present applicant has previously filed a patent application for the control device (Japanese Patent Application No. 59-265353).

In this way, a compact and inexpensive relay switch with small contact capacity can be used as the switching control means for zeroing the command value based on the deviation, and an inrush current is generated in the motor when switching the contacts. Since there is no flow, there is no need to take preventive measures, and the number of times the contacts need to be maintained is reduced, so the above-mentioned problems can be solved.

[Problem that the invention seeks to solve]

However, even in such a robot control device, the result is that the drive current of the motor that drives each axis of the movable part of the robot is reduced to zero and the force is relaxed, so in that state, the follow-up operation by external force is not performed. When the robot's movable parts are displaced by external forces, the movable parts and joint axes (including the drive force transmission part and bearing part) have inertia,
viscosity.

Various resistance forces occur due to static friction, dynamic friction, etc.
It is not possible to make the following force of the moving part zero or close to it,
The problem is that smooth follow-up motion may not be possible, and if this follow-up force (a force that opposes the above-mentioned resistance force) is large, the tool attached to the hand may come off the workpiece. There was.

It is an object of the present invention to solve this problem and enable a smooth follow-up operation using an external force during force relief control of a robot.

[Means for solving problems]

Therefore, the robot control device according to the present invention controls the movable part to release force by setting the command value based on the deviation between the speed command value and the speed feedback value to zero regardless of the actual value, and also controls the movable part to release the force. A following force detection means detects a following force that the movable part receives during a follow-up operation due to an external force, and a torque that reduces the following force is applied to a motor that drives the movable part in accordance with the following force detected by the means. A follow-up force compensation circuit is provided that outputs a compensation command value to make the robot's movable part move by an external force. This allows for free follow-up operation.

〔Example〕

Embodiments of the present invention will be described below with reference to the accompanying drawings.

First, the configuration of the robot used in this embodiment and its work will be explained with reference to FIG.

In the figure, reference numeral 1 is a vertically articulated robot, which has a base 2 fixed on a pedestal (not shown), which serves as an axis 3 vertically erected, and an elbow axis fixed at right angles to this axis. motor 4
A first arm (upper arm) 5 connected to the first arm (upper arm) 5, a second arm (lower arm 1 [1a) 7] rotatably connected to the tip of the first arm by a shaft 6, and It consists of a hand 9 etc. which are rotatably connected to the tip by a wrist shaft 8.

This is schematically shown in FIG. 3.

The original axis 3 is a horizontal joint axis that is rotated by the motor 10 in the direction of arrow A within a horizontal plane.

The first arm 115 is moved in the direction of arrow B by the motor 4, the second arm 7 is moved in the direction of the arrow C by the motor 11, and the hand S is moved in the direction of the arrow by a motor (not shown) built in the second arm 7. The elbow shaft, the material shaft 6, and the wrist shaft 8, which rotate in a vertical plane and connect the hips, are vertical joint axes.

Incidentally, as the wrist shaft drive motor (not shown) for rotating the motors 4, 10, 11 and the hand 9, DC servo motors are used. As a speed reducer for transmitting the driving force of each of these motors, a speed reducer with relatively high reverse transmission efficiency (for example, one using a bevel gear mechanism) is used.

Further, a tacho generator for detecting the rotation speed and a potentiometer for detecting the rotation angle are attached to the output shaft of each of these motors.

When the hand is pressed, a force sensor 22 (details of which will be described later) is applied to the piston rod of the air cylinder 13 fixed to the holder 12 connected to the wrist shaft 8.
6 sockets with bolts at the tip and tighten them h 14 a
A nut runner 14 is installed and configured, and further,
A pair of limiters 1 to 15 and 15 and 16 are attached to the holder 12, and the upper and lower limit positions of the nut runner 14 are detected by actuating them by dogs 13 attached to the screws 1 and 15 and 16 of the air cylinder 13. I'm trying to make it possible.

On the other hand, reference numeral 17 denotes a continuous conveyor (hereinafter simply referred to as "conveyor"), on which a workpiece 18 such as an engine block, which is positioned and fixed at a predetermined position and in a predetermined posture, is placed, and the workpiece 18 is moved within the working area of the robot 1 by an arrow. It is designed to be transported at a predetermined speed in the E direction.

The workpiece 1 transported by this conveyor 17
A bolt 19 to be worked on (for example, a bolt for fixing a head cover to a cylinder head) is set in 8, and the work of tightening this bolt 19 is carried out by hand to the robot 1 with the nut runner 14 attached. It's something I'm trying to get done.

Furthermore, 20 is a bolt passage detector fixed to the upper end of the stay 21 installed on the floor, and when the robot 8 is waiting at a predetermined position as shown in the figure, it is detected by the conveyor 17. It is detected when the bolt 1S one day above the transported workpiece passes.

Note that as this bolt passage detector 20, for example, a reflective photoelectric switch or the like is used.

Further, the positional relationship between the position of the bolt passage detector 20 that detects the bolt 1 day and the nut runner 14 is such that the nut runner 14 is lowered to the lower limit when the bolt passage detector 20 detects the passage of the bolt 1 day. The relationship is such that the loading socket 14a impinges on the bolt 1S when the bolt 1S is pressed.

Instead of the bolt passage detector 20, a dog is provided at a predetermined position on the conveyor 17 corresponding to the bolt 1, and the dog detects a fixed part at a predetermined position along the conveyor 17 and hits a limit switch. The passing of the bolt in one day may be detected by turning on this limit switch.

The force sensor 22 is, for example, as shown in FIG.

A large-diameter sensor holding cylinder 223 is fixed to a disk-shaped mounting plate 221 via a small-diameter stopper holding cylinder 222, and each end of a cross-shaped sensor mounting plate 224 is fixed to the sensor holding cylinder 223. The detection shaft 22 is located in the center of the sensor mounting plate 224.
5 is fixed vertically.

Four sensor elements (for example, strain sensor elements such as semiconductor strain cages) 226 are attached to the sensor mounting plate 224, and a pair of stopper bolts 227 are screwed into the stopper holding cylinder 222 to connect the detection shaft 225. The angle of inclination is regulated.

When the detection shaft 225 is tilted with respect to the sensor holding cylinder 223 by an external force, the force sensor 22
24 is distorted, each sensor element 226 is distorted according to the amount of distortion.
resistance value changes.

Therefore, if the mounting plate 221 of the force sensor 22 is fixed to the tip of the screw 1 of the air cylinder 13 shown in FIG. (In this example, the moving force of the workpiece in one day by the conveyor 17) When each movable part of the robot 1 performs a follow-up operation, the detection axis is tilted according to the magnitude of the follow-up force that the movable part receives. The magnitude of the following force can be detected as a change in the resistance value of the sensor element 226.

Next, an embodiment of the control device for the robot 1 shown in FIG. 2 will be described with reference to FIG.

In FIG. 1, numeral 23 is a central processing unit using a microcomputer or the like, and is in charge of overall control of the robot 1.

That is, the position command register 249, the position control section 25, the speed control section 26. A servo control unit for the motor 4 that drives the circumferential shaft that rotates the first arm 5, which is configured by a current control unit 27 and the like, and a position command register 34° and a position control unit 35, just like this servo control unit. ．． Speed control section 36
．． and a servo control unit for the motor 10 that drives the original shaft 3, which is configured by a current control unit 37, etc., and a second arm 7 that rotates the second arm 7, which is also configured in the same manner as these servo control units, although not shown. In addition, the servo control unit for the twin shaft drive motor 11 and the servo control unit for the wrist shaft drive motor are respectively controlled to control the air cylinder 13 and nut runner 14 attached to the hand S shown in FIG. The cylinder 13 also controls the raising and lowering of the nut runner 14, and its built-in motor controls the rotation and stopping of the socket 14a.

Next, in the servo control section for the motor 4, the target position command value of the first arm 5 from the central processing section 23 is written into the position register 24 while being updated one after another.

The position control unit 25 converts the target position command value of the first arm 5 written in the position command register 24 and the position feedback signal (voltage) from the potentiometer 30 attached to the output shaft of the motor 4 into an A/D converter. It outputs a speed command value Sa based on the deviation from the value converted into a digital value by the converter 31, that is, the current position value of the first arm 5 (corresponding to the angle O in FIG. Each time the current position value matches and positioning is completed, this is notified to the central processing unit 23, and the central processing unit 23 uses this information to determine the next timing to output the target position command value.

The speed control unit 26 receives a speed command value Sa from the position control unit 25 that is input via a command value control circuit 45, which will be described later.
A current command value S1 is output based on the deviation from the speed feedback value from the tachogenerator attached to the output shaft of the motor 4.

The current control unit 27 receives the current command value S from the speed control unit 26.
1 is input through the adder circuit 32, and a drive current is applied to the motor 4 based on the deviation from the current feedback value from the current detector 28 which detects the drive current flowing through the motor 4.

Therefore, the position command register 242 position control
, □ Part 25. In the servo control section for the motor 4, which includes a speed control section 2, a current control section 27, etc., a command value control circuit 45, which will be described later, uses a speed command value from the position control section 25 and a speed feedback value from the tacho generator 29. As long as it is output as is to the speed control unit 26, the motor 4 is driven based on the eye shelf position command value from the central processing unit 23 to perform playback control (positioning control) of the first arm 5. I can do it.

The parts 64 to 42 that constitute the servo control part for the motor 10 that rotationally drives the original shaft 3 function in exactly the same way as the parts 24 to 32 that make up the servo control part for the motor 4 described above.
A command value control circuit 45, which will also be described later, outputs the speed command value sb output from the position control section 35 and the speed feedback value output from the tacho generator 3S attached to the output shaft of the motor 10 as they are to the speed control section 36. As long as this is done, the motor 10 can be driven based on the target position command value from the central processing section 23 to perform playback control (positioning control) of the direction of the original shaft 3 and the motor 4 integrated therewith.

Furthermore, the servo control units for the motor 11 that drives the second arm 7 and the wrist shaft drive motor function similarly to drive each motor, respectively, and perform playback control of the second arm 7 and hand 9. I can do it.

The air cylinder drive circuit that controls the air cylinder 13 shown in FIG. 2 consists of a known cylinder operation circuit, and its electromagnetic directional control valve is switched by a command from the central processing unit 23 to switch the direction of air supply. Raise and lower. socket 14
．． The rotation and stopping of the nut runner 14 is performed by controlling the power supply to the motor built in the nut runner 14 based on commands from the central processing section 23.

The command value control circuit 45 includes 11 changeover switches S W s that are switched by energizing and de-energizing the relay coil LL.
-SW o (Four changeover switches SW 5 to be inserted into the servo control section for the motor 11 and the wrist shaft drive motor)
SWa consists of an electromagnetic relay with a switch (not shown).

This command value control circuit 45 has a changeover switch SWt.

The movable contacts C of SW2 are each connected to the input side of the speed control section 26, each fixed contact a is connected to the ground, and each fixed contact A is connected to the output side of the position control section 25 and the tachogenerator 29, respectively. .

Further, the movable contacts C of the changeover switches 5w31 sw4 are respectively connected to the input side of the speed control unit 36, and each fixed contact a is connected to the ground, and each fixed contact is connected to the output side of the position detection unit 35 and the tachogenerator 3. are connected to each other.

Changeover switches SWs (not shown), SWs and SW?
+SWa is also connected in exactly the same way in the servo control section for the motor 11 and the servo control section for the wrist shaft drive motor, respectively.

The changeover switches S Ws and S W +o switch gravity compensation command values cs, cs2 output from a gravity balance compensation circuit 50, which will be described later, to the addition circuit 32 and the motor 1, respectively.
It is inserted into a line input to a similar adder circuit in a servo control section (not shown) for 1, and is used as an open/close switch.

Further, the changeover switch SW+ is inserted into a line for inputting a following force compensation command value C83 outputted from a following force compensation circuit 60, which will be described later, to the addition circuit 42, and is used as an opening/closing switch.

In addition, a diode D connected to both ends of the relay coil L1
1 is a flywheel diode.

=16- This command value control circuit 45 switches each changeover switch SW1 to SW, when the relay coil Ll is de-energized.

The movable contact piece C of each is switched to the fixed contact side, and the actual speed command value and speed feedback value are passed through as they are, and the servo control unit for each motor is operated as originally for positioning, but the relay coil L1 When energized, the movable contacts C of each of the changeover switches SW1 to SWo are switched to the fixed contact a side as shown in the figure, and the speed control units 2Ei, 36. Set both the speed command value and the speed feedback value input to 0 (earth value), and then set the current command value SL + S based on the deviation.
2 t... is set to zero regardless of the deviation between the actual speed command value and the speed feedback value, in other words, regardless of the movement of the motors 4, 10, etc.

In this way, if the current command value is set to zero regardless of the actual speed command value and speed feedback value, position and speed feedback control will no longer be effective, so each motor 4,
10.11 mag. becomes free, thereby making the second
The waist axis 3 of the robot 1 in the figure, the 17th arm 5. 27th-m7
, and hand 9 can be moved freely by external force.

However, in this case, if the drive current of each motor is completely reduced to zero, the first. The weight of the second arm 5°7 and the hand 9 rotates each vertical joint axis, causing the postures of these movable parts to collapse and making it impossible to work.
A gravity balance compensation circuit 50 is provided to maintain the gravity balance and prevent the posture from collapsing, the details of which will be described later.

Further, a following force compensation circuit 6 is provided for the servo control unit of the motor 10 that drives the side shaft 3, which is the horizontal joint axis, to perform the following force compensation during the following operation, which is a feature of the present invention.
0 is provided, the details of which will be described later.

In addition, each changeover switch S W + of the command value control circuit 45
~ Even when the movable contact piece C of S W + + is switched to the fixed contact side, the deviation between the speed command value and the speed feedback value becomes zero when positioning is completed, but at this time, the position and speed feedback! Because ll# is in effect,
Side axis 3. 1st. When an external force is applied to the second arm 5.7, etc. and the motor 10, 4, 11, etc. is rotated from the stopped position, a rotational force is immediately generated to return it to its original position.

In addition, in this command value control circuit 45, when the current command value S1+82+... is functioning to be zero (
In this embodiment, the state when the relay coil L is energized is referred to as the operating state.

Reference numeral 46 denotes a switching circuit, which is composed of changeover switches 47 and 48 inserted into the power supply circuit of the power supply VCC, and a two-way switch 15 attached to the hand 9 of the robot 1. Note that the limit switch 15 is of a normally closed type, and is turned off when the nut runner 14 reaches the limit and is struck by the dog 141).

This switching circuit 46 controls the command value when the limit switch 15 is off (when the nut runner 14 is in the raised position) by switching the movable contact i of the changeover switch 48 to the fixed contact g side as shown in the figure. The relay coil L of the circuit 45 is de-energized, and when the limit switch 15 is on, the relay coil L1 is energized.

Furthermore, if the movable contact i of the changeover switch 48 is switched to the fixed contact side, the command value control circuit 45 can be activated or deactivated by the changeover switch 47 regardless of whether the limit switch 15 is on or off. can.

In this embodiment, the excitation of the relay coil L corresponds to the operation of the command value control circuit 45, but if the fixed contacts a and b of the changeover switches SW1 to SW11 are reversed, the relay It is also possible to make the de-energization of the coils L, 1 correspond to the operation of the command value controller [45].

Next, a detailed example of the gravity balance compensation circuit 50 will be explained with reference to FIG.

The gravity balance compensation circuit 50 includes a CPU (central processing unit) 51, a memory 52 including R and OM as program memories and RAM as a data memory, and a pair of A/D converters 53, 54 and D. /A converter 55.
It is composed of a microcomputer consisting of 56.

The gravity balance compensation circuit 50 receives a voltage signal corresponding to the rotation angle θ1 (see FIG. 3) of the 17th arm 5 from the horizontal position, which is output from the potentiometer 3a attached to the output shaft of the motor 4. is converted into a digital value by the A/D converter 53 and read into the CPU 51, and the potentiometer 4 similarly attached to the output shaft of the motor 11
A voltage signal corresponding to the rotation angle θ2 (see Fig. 3) of the second arm 7 with respect to the 17th arm 5 outputted from the
/D converter 54 converts it into a digital value and converts it to cptr.
51.

Then, the length β1 from the circumferential axis 4a of the first arm 5 of the robot 1 to the center of gravity and the total length '2+27th
- Length β3 from opposite axis 6 of arm 7 to center of gravity and total length '4r
1st arm5. Each weight V of the second arm 7 and hand 9
J1. W2, W3 (see Figure 3), and sinθ,
A table of CoSθ is stored, and the moment of gravity M(
A), M(B) are calculated by the CP TJ 5 l by performing the following calculations, and the compensation command value cs, , C52 for generating shaft 1 herk that can withstand it is calculated by the D/A converter 55.
56, it is converted into an analog signal and output.

M(A)” Klcosol +25in(θ, +02
-90°) M(B) = K 2 sin (θ, 10f
)2-90') However, KH=j? 1 'JJ1+
12 (W2 +W3) 'to 2:R3W2
+ A' 4 W3. Since 2 is a constant for this Rlr, it is preferable to store this in the memory 52 in advance.

In this way, the compensation command values C8,, cs,, outputted from the gravity balance compensation circuit 50, are sent to the adder circuit 32 and the motor 11 via the changeover switches SWQ and SW+o of the command value control circuit 45 during the stress relief control. The speed control section 25. Current command value SI+... output from...
Even if the current controller 27. is zero, the current controller 27. . . causes a drive current for gravity compensation to flow through the motors 4 and 11, and the first . Since the shaft torque capable of resisting the weight of the second arm 5.7 and the hand 9 is generated, the first. The 27th arms 5 and 7 can maintain their postures while maintaining the gravity balance.

In this embodiment, the air cylinder 13 and nut runner 14 of the hand 9 are always held vertically (vertical) in a relaxed state, and the weight of the hand S applied to the wrist shaft 8 always acts in the vertical direction, so the moment is Since it does not occur,
Gravity compensation is not performed for the wrist axis alone.

Further, the function of the gravity balance compensation circuit 50 may be provided in the central processing unit 23 of FIG. 1, and time-sharing processing may be performed by a common CPU.

Next, a specific example of the tracking force compensation circuit 60 shown in FIG. 1 will be explained with reference to FIGS. 6 to 9.

The following force compensation circuit 60 in this embodiment shown in FIG. 6 includes a viscosity compensation circuit 61. The inertia compensation circuit 62°, the dynamic friction compensation circuit 63, and a menu volume A V Rlr for adjusting the level of the compensation command value output from each of these compensation circuits.
V R21V R3, a follow-up force detection circuit 64 that detects follow-up force based on a signal from the force sensor 22, and a starting force compensation circuit 65. Volumes VR4 and VR5 for adjusting the levels of the compensation command values outputted from the force compensation circuit 6, the compensation switching circuit 67, the starting force compensation circuit 65 and the force compensation circuit 66, respectively, and the volumes vRI to vR5. It is constituted by an adder circuit 68 that adds the outputs.

The viscosity compensation circuit 61 is a circuit that outputs a compensation command value for causing the motor 10 to generate a shaft torque that offsets the viscous resistance generated in the side shaft 3 when the movable part of the robot 1 is displaced. Resistor R1, feedback resistor R2
This is an amplifier with an amplification factor of less than 1.

A voltage signal, which is a speed feedback value, is generated by the tacho generator 3 during the follow-up operation period T of the robot 1 according to the speed of rotational displacement of the side shaft 3 as shown in FIG. (2) Input tg, and output a voltage signal Va proportional to the input as shown in the figure (b) as a viscosity compensation command value.

When the side shaft 3 of the robot 1 rotates, the inertia compensation circuit 62 acts to resist an inertia force corresponding to the mass from the side shaft 3 to the hand 9, especially when starting and stopping the rotation. This is a circuit that outputs a compensation command value to cause the motor 10 to generate shaft torque that offsets the inertial force that acts.
This is a differentiator consisting of an operational amplifier OP2, an input resistor R3, a capacitor C, and a feedback resistor R4.

Then, when a voltage signal Vtg as shown in FIG. 8(a) is input from the tacho generator 3rd, a pulse-like voltage signal Vb(as shown in FIG. 8(c)) is input as an inertia compensation command value.
It outputs a polarity that generates torque in the acceleration direction when accelerating and in the deceleration direction when decelerating.

The dynamic friction compensation circuit 63 issues a compensation command for causing the motor 10 to generate shaft torque that offsets the dynamic friction force generated between the bearing and the drive force transmission mechanism when the side shaft 3 of the robot l-1 rotates. This circuit outputs a value, and is a comparator with hysteresis consisting of an operational amplifier OP3, resistors R5 and R6, and a volume VRb for generating a comparison voltage vrI.

When a voltage signal Vtg as shown in Figure 8 (A) is input from the third day to the tacho generator, the dynamic friction compensation command value is changed from the time when the input speed increases slightly until it becomes zero as shown in Figure 8 (D). outputs a rectangular wave voltage signal VC (polarity that generates torque in the following direction).

Note that if the following direction, that is, the rotation direction of the side shaft 3 is reversed, the polarity of the voltage signal Vtg generated by the tough generator 3 will be reversed, so each of these compensation circuits 61
,62. The polarity of the output signal of Ei3 is also inverted.

For example, as shown in FIG. 7, the following force detection circuit 64 includes a bridge circuit 641, a DC amplifier 642, and a low-pass filter! 1643.

The bridge circuit 641 is a sensor element group 226 in which, for example, two sensor elements arranged at right angles are connected in series among the four sensor elements 226 of the force sensor 22 in FIG.
With a and 226b as two sides, and resistors Ra and Rh as the other two sides, a voltage is applied between a and b by a power source E, and a voltage is applied between c and d depending on the resistance value change of the sensor element groups 226a and 226b. Outputs the corresponding voltage.

The DC amplifier 642 has an operational amplifier OP8 and an input resistor Re.
, Rd, and a feedback volume VRf, and DC amplifies the voltage output from the bridge circuit 641. The degree of amplification is adjusted by the volume VRf.

The low-pass filter 643 includes a resistor R that constitutes an integrating circuit.
e, consists of a capacitor Ca and an operational amplifier OP9 that constitutes a buffer amplifier, and removes the noise component of the detection signal amplified by the DC amplifier 642.
A voltage signal Vd corresponding to the following force as shown in a) is output.

This signal Vd is input to the starting compensation circuit 65 and force compensation circuit 66 in FIG.

The starting force compensation circuit 65 includes an operational amplifier oP4 and an input resistor R.
7 and a feedback resistor R8, which has a large amplification factor.
When the input signal Vd is generated, it is immediately amplified to the saturation level and a voltage signal Ve close to a rectangular wave as shown in FIG. 9(b) is output.

However, after the start of the follow-up operation, when the voltage signal Vtg from the tachogenerator 3 in FIG. Voltage signal Ve is no longer applied to volume VR4, and volume VR,
The voltage signal vf as the compensation command value applied to s is the ninth
As shown in Figure (c), this signal is only used at startup.

The compensation switching circuit 67 includes an operational amplifier OP6 and an input resistor +1
It consists of a comparator with hysteresis consisting of a + feedback resistor R12 and a volume V Ra for generating a comparison voltage Vr2, and a relay Ry that operates when its output becomes high level, and V t g>V r 1 The amount of hysteresis is adjusted by the resistor RI2 so that the output of the operational amplifier OP6 changes from L'' to H'' when the voltage Vtg reaches 0, and thereafter maintains this state until Vtg becomes 0.

Note that if the following direction can be reversed, the voltage signal V t g from the tachogenerator 3rd may become negative, so the compensation switching circuit 67 is connected to the compensation switching circuit 67 regardless of whether the input signal V t g is positive or negative. It is preferable to configure it by a window comparator and a relay Ry that inverts the output when the absolute value exceeds a certain value.

The force compensation circuit 66 includes an operational amplifier OR5, an input resistor Rg,
This is an amplifier consisting of a feedback resistor R1 and a volume VR7 that serves as an unbalanced force setting device.
A voltage signal Vg proportional to the input as shown in FIG. S (d) is output as a compensation command value until the voltage increases to a certain extent and disappears.

In addition, the volume VR? By applying a positive or negative voltage to the inverting input terminal of the operational amplifier OP5, an offset can be applied to make the output signal Vg too large or too small, thereby making it possible to unbalance the tracking force compensation.

The output signals valvb, Vc, Vf+Vg of each of these compensation circuits 61, 62, 63, 65°66 are adjusted in level by respective volumes VR1 to VR5, and then added in an analog manner by an adding circuit 68. It is output as a follow-up compensation command value C83.

The adder circuit 68 is a non-inverting amplifier composed of an operational amplifier OP7, five input resistors R13 to R17, and a feedback resistor R18 to which signals from five volumes vR1 to vR5 are input.

By inputting the follow-up compensation command value (voltage signal) C83 output from the adder circuit 68 to the adder circuit 42 via the changeover switch 5WII shown in FIG.
Even if the current command value S2 output from C83 is zero, the current control unit 37 supplies a drive current to the motor 10 according to the follow-up compensation command value C83, thereby reducing the viscous resistance force generated in the side shaft 3 during the follow-up operation. , generates an axial torque that reduces the following force that opposes inertial force, static friction force, dynamic friction force, etc., and enables rotation following external force with almost no resistance.

In addition, in the following force compensation circuit 60 of FIG. 6, the minimum required force compensation circuit in this invention is 6 days.

Here, a method for adjusting the following force compensation circuit 60 will be explained.

First, the changeover switch 48 of the changeover circuit 46 in FIG.
After switching to the state shown in the figure) and putting the robot 1 in a relaxed state, make adjustments according to the following procedure.

At this time, the output of the volume vR7 of the force compensation circuit 66 is set to rOJ, and the output of the volume VR4°V Rs is also set to "0".

(a) Move the shaft to be compensated (in this embodiment, the side shaft 3) at a constant low speed, adjust the volume vR3°■R6 in FIG. 6, and adjust the dynamic friction compensation so that the following force is as light as possible.

(b) Set the speed at which the side shaft 3 is moved to medium speed and high speed, and adjust the volume VR so that there is no difference in the following force in each case.
Adjust the viscosity compensation by I.

(c) Adjust the inertia compensation using the volume vR2 so that the follow-up force when the side shaft 3 starts moving and stops is as small as possible.

(d) When the nut runner 14 shown in Fig. 2 is moved by an external force, the following force at the beginning of movement is the smallest, and the volume vR4 is set so that it does not start moving by itself.
Adjust starting force compensation by

3l- (e) When the nut runner 14 is subjected to a following operation by an external force, the following force becomes the smallest at a substantially constant speed after starting,
Moreover, to prevent it from starting to move on its own, the volume
Adjust force compensation by R5.

(f) When the nut runner 14 is engaged with the bolt IS,
A slight pressing force is generated in the following direction, and the conveyor 17 (second
The unbalanced force is adjusted by the volume vR7 to such an extent that the engagement will not be easily disengaged due to speed fluctuations, vibrations, etc. (see Figure).

Next, by adjusting in this way and using the control device shown in FIG. 1 which can switch between playback control and stress relief control as described above, the robot 1 shown in FIG. The operation for tightening the bolt 19 of the workpiece 18 moving at a speed of 18 will be described with reference to the flowchart in FIG. 10.

It is assumed that the teaching work required to have the robot 1 perform the tightening work described below has been done in advance.

Further, the nut runner 14 attached to the hand 9 of the robot 1 is normally at the upper limit position, and the limit switch 15 is turned off by being hit by the dog 14b. In the following explanation, the switching circuit 4 in FIG.
It is assumed that the changeover switch 48 at 6 is switched to the contact g side.

In this state, the switching circuit 46 causes the command value control circuit 45 to
Since the power supply to the relay coil L1 is cut off, the movable contacts C of each changeover switch SW1 to SWo are all switched to the fixed contact side, and the side shaft 3, the first... second arm 5,
7. Each motor 4, 10.11 rotates the hand 9, respectively.
Playback control is possible.

Therefore, in step 1 of FIG. 10, first, playback control is performed on the motors 4 and 10, and the side shafts 3 and 3 are controlled. 1st ° 2nd arm 5
, 7, etc., to their original positions (any retracted position is fine).

Next, the socket 14a of the nut runner 14 attached to the hand S is connected to the workpiece 1 conveyed by the conveyor 17.
In step 2, the motors 4, 10, etc. are again controlled in playback so that the motors 4, 10, etc. are located at the work point, which is a predetermined standby position on the movement trajectory of the pole 1-IS on the 8, as shown in FIG. Take the prescribed standby position.

In this state, wait for the bolt passage detector 20 to detect the passage of the second bolt on the workpiece 18'', and when the bolt passage detector 20 detects the passage of the first bolt on the first This is input to the central processing unit 23 in the figure, and the determination in step 3 becomes YES, and in step 4, the air cylinder 13 is driven to lower the runner 1-runner 14.

When the nut runner 14 starts to descend, the switch 15 is immediately turned on, so the relay coil Ll of the command value control circuit 45 is energized by the switching circuit 46, and the movable contacts of each of the switching switches SW1 to SWu are energized by the switching circuit 46. C are all switched to the fixed contact a side as shown in FIG. When the movable parts such as the second arms 5 and 7 receive an external force, they become free to move freely, but the motors 4 and 1 are controlled by the compensation command value from the gravity balance compensation circuit 50.
1 is supplied with a drive current to generate shaft torque that resists the moment due to its own weight, so that the balance is maintained and the 1st. The posture of the second arm 5°7 does not collapse.

Then, when the nut runner 14 descends to its lower limit, the socket 14a begins to ram the second bolt of the workpiece 18 carried by the conveyor 17.
At the same time, the lower limit switch 1st is struck by dog 14b and activated, and the signal determines YES in step 5.
S, and in step 6 the nut runner 14 is driven to rotate the socket 14a and the bolt is tightened.

In this way, while the bolt 19 is being tightened with the socket 14a of the nut runner 14 pushed in, the conveyor 17 is moving the workpiece 18. Since each movable part such as the second arms 5, 7, etc. is in a state where it can be freely moved by external force, the hand plate can be moved in the horizontal direction by following the movement while tightening the bolt plate.

At this time, each resistance force due to viscosity, inertia, and dynamic friction acting on the side shaft 3 is applied to the motor according to the sum of each command value of viscosity compensation, damage compensation, and dynamic friction compensation from the following force compensation circuit 60 described above. 10, the driving current is approximately canceled out, but when the following movement actually starts due to static friction force and other resistance forces that cannot be compensated for, the following force is detected by the force sensor 22 and the following force detection circuit 64. A detection signal vd appears from the means, and starting force compensation and subsequent force compensation are performed according to the magnitude of the detection signal vd, so that the nut runner 14 follows and moves with almost no resistance.

At that time, the volume VR? of the force compensation circuit 66 in FIG.
If you set an unbalance so that the compensating follow-up force is slightly larger than the resistance force, NatsuTrunner 1
Since a force is generated that slightly presses the bolt 4 in the following direction against the bolt 1, engagement between the two is reliably maintained during the following operation.

Then, by measuring the time since the start of tightening or the tightening torque, it is determined in step 7 whether the bolt tightening for one day has been completed, and when the tightening is finished, the nut runner 14 is driven in step 8. At the same time, the air cylinder 13 is driven to raise the nut runner 14.

When the nut runner 14 rises to the upper limit, the upper limit switch 15 turns off, so the command value control circuit 4
Since the relay coil Ll of No. 5 is de-energized and the changeover switches SW1 to SW++ are all switched to the contact side, the drive motors 4°, 10, 11, etc. of each axis are all returned to a state in which playback control is possible.

When this playback control becomes possible again,
Since the movable part of the 6-bot 1 has moved from its standby position, the position control part for each motor accumulates a positional deviation corresponding to the movement.

Therefore, when playback control becomes possible,
Each movable part immediately begins to return to the working point before it moved, but when the lift limit switch 15 is turned off, the process returns from step 9 to step 1 and the process of moving it back to its original position is performed, so the side shaft 3 The movable parts, such as the second arms 5 and 7, return to their working points and eventually return to their original positions, and repeat the above operations again.

In this way, while the nut runner 14 is tightening the bolt 19 of the workpiece 18, each movable part of the robot 1 relaxes and follows the movement of the workpiece 18, which conventionally requires very complicated control. Follow-up work that was previously required can be accomplished very easily.

Furthermore, since following force compensation is performed to reduce the following force in response to various resistance forces generated during actual following operation, the following operation can be performed extremely smoothly.

In addition, the conveyor 17 is stopped and the stationary workpiece 18 is
When tightening 19 to upper port 1, switch the changeover switch 48 in the changeover circuit 46 to the contact ll side, and switch the changeover switch 47 to the contact C side,
If the resting position of the bolt on the 1st day matches the work point mentioned above, the determination of whether the bolt has passed in step 3 in Fig. 7 can be omitted, and the tightening of the stationary bolt on the 1st day can be performed. I can do it.

Furthermore, if the changeover switch 48 is switched to the contact side, the first and second arms 5, 7, etc. can be freely controlled by external force at any time by switching the changeover switch 47 to the contact d side as necessary. It is possible to put the body in a relaxed state where it can move freely.

In addition, in the second embodiment, an example was described in which the current command value was set to zero by switching both the actual speed command value and the speed feedback value to zero values unrelated to positioning control. The current command value can also be made zero by switching both values to mutually equal predetermined values unrelated to positioning control. Alternatively, the current command value may be directly switched to the zero value.

Moreover, the gravity balance compensation circuit 50 in the above-mentioned embodiment
In this case, the gravity compensation value was calculated according to the data corresponding to angles 01 and 02 in FIG. 3 from the potentiometers 30 and 40.
．． The optimum gravity compensation value for each vertical joint axis according to θ2 may be stored as a table in the memory 52 of FIG. 5, and the gravity compensation value may be read from the table by the CPU 5i according to the input angle data. good.

Furthermore, the following force compensation is applied to the waist 11i1 where the effect is greatest.
111; Although the compensation is performed only for the circumferential axis and the opposite axes, even more complete compensation can be achieved.

Next, another embodiment of the present invention, which is provided with means for increasing safety during strain relief control, will be described with reference to FIGS. 11 to 13.

Although FIG. 11 only shows safety measures 7 for the motor 4 that drives the first arm 5 of the robot 1, similar safety measures are also provided for other motors 10, 11, etc. It is desirable to provide one. Further, the parts not shown are the same as those of the above-described embodiment shown in FIG.

In this embodiment, an electromagnetic brake 70 is connected to the drive shaft of the motor 4 (the shaft for rotating the 40th-1st arm 5).
& installation.

This electromagnetic brake 70 is of an off type that does not apply a braking force when it is energized by a brake power source 71, but applies a braking force when the energization is cut off, and the power supply to the entire control device is Even if the robot 1 is cut off, the axis of the movable part is fixed so that the posture of the robot 1 can be maintained.

Normally closed contacts SWa and SWb are inserted into the connection line between the electromagnetic brake 70 and the brake power source 71, and are turned off only when the relay coil L2 is excited. Diode connected in parallel to relay coil L2! ” D2 is a flywheel diode.

Further, a current abnormality detection circuit 73 detects a current abnormality by inputting a setting signal from the current abnormality setting device 72 and a current feedback signal from the current detector 28 via the switch 5WI2, and a setting signal from the speed abnormality setting device 74. A speed abnormality detection circuit 75 detects speed abnormality by inputting the speed feedback signal from the tachogenerator 2 through the switch SW+a, and the potentiometer 30 through the setting signal from the position abnormality setting device 76 and the switch 5W14.
A positional abnormality detection circuit 77 is provided for detecting positional abnormality by inputting the positional feedback signal from the positional feedback signal.

Switches 5WI2, 5W13, and 5WI4 are activated by excitation of relay coil r-1 in FIG.
This is a switch that changes in conjunction with 5W11. Normally, all movable contacts C are switched to contact a on the ground side as shown in the figure, and during stress release control, they are switched to the contact side to send each feedback signal to each abnormality. It is input to the detection circuits 73, 75, and 77.

The output terminals of each of the abnormality detection circuits 73, 75, and 77 are all connected to one end of the relay coil I, 2 connected to the power supply VCC, and the other end of each output signal Ea.

When any one of Eb and Ec becomes a low level "L" due to abnormality detection, the relay coil L2 is energized and its normally closed contacts SWa and SWb are opened, so the electromagnetic brake 70 operates and the motor 4 Since the rotation axis of the first arm 5 is locked, the rotation of the first arm 5 is stopped.

The current abnormality setting device 72 and the current abnormality detection circuit 75 are configured as shown in FIG. 12, for example.

That is, the current abnormality setting device 72 consists of a volume VRa having both ends connected between the positive power supply V and the ground, and a volume VRb having both ends connected between the negative power supply -■ and the ground.
The positive current feedback signal Vi as shown in FIG. 13(A) generated from the current detector 28 in FIG. The voltage Va corresponding to the normal limit of
A voltage vb corresponding to the negative normal limit is set and supplied to the current abnormality detection circuit 73.

The current abnormality detection circuit 73 includes a resistor Rg-R,n, operational amplifiers OPa and OPh as comparators, and diodes Da and D.
It is composed of a window comparator consisting of a transistor B, and an inverter NV that inverts its output.

Then, the current feedback signal Vi from the current detector 28 is inputted to the non-inverting input terminal of the operational amplifier OPa and the inverting input terminal of the operational amplifier OPb via the resistors Rg + RJ, respectively, and the set voltage V by the current abnormality setting device 72 is inputted.
a and Vb are input to the inverting input terminal of operational amplifier OPa and the non-inverting input terminal of operational amplifier OPb via resistors Rh and Ri, respectively.

Therefore, if vb≦Vi≦Va, the outputs of operational amplifiers OPa and OPb are both L'', but Vi
>Va or Vi<Vb, the operational amplifier OPa
.

The output signal Ea of this current abnormality detection circuit 73, which is obtained by inverting this output EA by the inverter INV, is shown in FIG.
As shown in FIG. 2, only when the motor current becomes abnormally large in either direction, it becomes low level and excites the relay coil L2.

The speed abnormality setting device 74, the speed abnormality detection circuit 75, the position abnormality setting device 76, and the position abnormality detection port 77 are also exactly the same as the current abnormality setting device 72 and the current abnormality detection circuit 73 in FIG. 12 described above. Just configure it.

By providing such a safety measure, even if an abnormality occurs in the current value flowing through the motor 4 or in the rotational speed or rotational position of the first arm 5, the motor 4 will be stopped and the first arm 5 will be stopped. - Rotation of the arm 5 can be stopped at that position. At that time, the power supply may be cut off.

It goes without saying that it is desirable to provide such a safety device for all motors that drive each movable part of the robot 1.

By the way, the present invention is applicable not only to the vertically articulated robot shown in FIG. 2, but also to various robots such as horizontally articulated robots and vertically articulated robots having parallel link arms. Note that gravity balance compensation is not required when applied to a horizontal articulated robot.

C Effects of the Invention] As explained above, according to the present invention, the robot's movable parts are adjusted according to the command value based on the deviation between the speed command value and the speed feedback value from the speed detection system of the robot's movable part. To set a command value based on a deviation between a speed command value and a speed feedback value to zero regardless of the actual speed command value and speed feedback value, in a robot control device configured to control the drive current of a motor that drives a motor. Since the movable parts of the robot can be moved freely by external force, it is possible to use compact and inexpensive electromagnetic relays with small contact capacity as a command value control means, which not only reduces the number of times of contact maintenance. Since inrush current does not flow to the motor when switching the contacts, there is no need to take measures to prevent it.

In addition, when the movable part follows a motion caused by an external force, the actual following force that the movable part receives is detected, and the following force controls the drive current of the motor that drives the movable part so as to generate a torque that reduces the force. Since compensation is performed, the movable parts of the robot can be moved lightly with almost no resistance during the robot's follow-up operation, resulting in smooth follow-up action and the tool attached to the hand will not come off the workpiece.

[Brief explanation of drawings]

Fig. 1 is a block configuration diagram of a control device showing an embodiment of the present invention; Fig. 2 is an external view of the surroundings of the robot for explaining the configuration and work of a vertically articulated robot to which the invention is applied; Figure 4 is an enlarged perspective view showing the structure of the force sensor 22 in Figure 2, and Figure 5 is the gravity balance compensation in Figure 1. A block circuit diagram showing a specific example of the circuit 50, FIG. 6 is a circuit diagram showing a specific example of the tracking force compensation circuit 60 in FIG. 1, and FIG. 7 shows a configuration example of the tracking force detection circuit 64 in FIG. 6. 11 is a circuit diagram, FIGS. 8 and 9 are signal waveform diagrams of various parts for explaining the operation of the tracking force compensation circuit 60, FIG. 10 is a flow diagram showing an example of the operation of the central processing unit 23 in FIG. 12 is a block diagram showing only the essential parts of another embodiment of the present invention provided with safety means; FIG. FIG. 13 is a signal waveform diagram of each part for explaining the operation of the current abnormality detection circuit 73. 1. Vertical articulated robot 3. Eye axis 4.10.
11... DC servo motor 5... First arm 6... Opposite shaft 7 Second arm 8... Wrist shaft 9... Hand 13... Air cylinder 14... Nut runner 15.16... Limit switch 17・Continuous conveyor 18・Work 19
...Bolt 20...Bolt passage detector 22.
・Force sensor 23...Central processing unit 29.39
- Tacho generator 30, 40... Potentiometer 45... Command value control circuit 46... Switching circuit 50
... Gravity balance compensation circuit 60 ... Follow-up force compensation circuit 61 ... Viscosity compensation circuit 62 ... Inertia compensation circuit 63 ... Dynamic friction compensation circuit 64 ... Follow-up force detection circuit 65 - Starting force compensation circuit 66・・Force compensation circuit 67・Compensation switching circuit 68・
... Addition circuit 70 ... Electromagnetic brake m Figure 8 Follow-up period Figure 9 Follow-up period 1 (D Figure 13 Procedural amendment (white side September 5, 1985 Mr. Michibe Uga, Commissioner of the Patent Office) 1. Indication of the case Japanese Patent Application No. 60-84638 2. Name of the invention Robot control device 3. Person making the amendment Relationship to the case Patent applicant Nissan Motor Co., Ltd., 2-399 Takaracho, Kanayō-ku, Yokohama-shi, Kanagawa Prefecture Company 4, Agent 1-20-5 Higashiikebukuro, Toshima-ku, Tokyo “Correct it to J through it.

Claims (1)

[Scope of Claims] 1. Controlling the drive current of a motor that drives each movable part of the robot according to a command value based on a deviation between a speed command value and a speed feedback value from a speed detection system of a movable part of the robot. A control device for a robot configured to include a command value control circuit that makes a command value based on the deviation between the speed command value and the speed feedback value zero regardless of the actual speed command value and the speed feedback value; a switching circuit for switching between activation and non-operation of a control circuit; a following force detection means for detecting a following force received by the movable part during a follow-up operation due to an external force of the movable part; and a following force detected by the following force detecting means. Accordingly, a following force compensation circuit is provided that outputs a compensation command value for causing the motor driving the movable part to generate a torque that reduces the following force, and the switching circuit operates the command value control circuit. When the command value based on the deviation is set to zero, the drive current of the motor is controlled by the compensation command value output by the following force compensation circuit, so that the movable part of the robot can freely follow the motion by an external force. A robot control device characterized by: 2. Control of the robot according to claim 1, wherein the following force compensation circuit is a circuit that outputs a compensation command value for generating a torque that is not completely balanced with the following force detected by the following force detection means. Device.