JPH0612490B2 - 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

The present invention relates to a control device for a robot, and in particular, can perform a so-called force-releasing control for bringing a movable part of the robot into a state in which it can be freely moved by an external force. The present invention relates to a robot control device.

[Conventional technology]

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

However, in order to assemble the work (articles) that continuously flow on the conveyor by arranging the robot on the continuous line that has been often used in the assembly work line, it is necessary to install the robot between the conveyor and the robot. Therefore, it is very difficult to simultaneously perform the control for the work of the robot itself and the control for the synchronization with the conveyor.

Therefore, if the robot is in a "force-relieved state" in which a movable part such as an arm can be freely moved by an external force while performing a certain work, it is possible to follow the movement of the work on the conveyor without particularly performing synchronous control. it can.

As a control device for a robot capable of such force release control, there is a control device disclosed in, for example, Japanese Patent Application Laid-Open No. 58-206389.

This device is configured so that an actuator for driving a previously designated movable part of a plurality of movable parts in a multi-axis robot, for example, a drive current to a motor is cut off so that the movable part can be freely moved by an external force. It is a thing.

However, in such a conventional robot control device capable of performing force release control, the drive current (power) of the motor for driving the robot is cut off, so that a contact for a large current is used as the cutoff means. In addition to having to use a large and expensive magnet switch with large capacity, it is also necessary to take measures to prevent inrush current that flows to the motor when the contact is turned on and off, and to frequently perform contact maintenance, by using an eccentricity switch. There were problems such as being there.

Therefore, control of movable parts such as arms in the robot is
Generally, the drive current of the motor that drives the movable 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 movable part. A controller for a robot that allows a movable part to move freely by an external force by setting a command value based on a deviation from a speed feedback value to zero regardless of the actual speed command value and the value of the speed feedback value. The present applicant has previously applied for a patent (Japanese Patent Application No. 59-265353).

By doing so, a relay switch or the like having a small contact capacity, a small size, and an inexpensive switch can be used as the switching control means for making the command value based on the deviation zero, and the rush current to the motor at the time of switching the contact is reduced. Since it does not flow, it is not necessary to take preventive measures for it, and the number of times of contact maintenance is small, so that the above-mentioned problems can be solved.

[Problems to be Solved by the Invention] However, even in such a robot control device, as a result, it is only necessary to set the drive current of the motor for driving each axis of the movable portion of the robot to zero to bring the motor into a power-relieved state. Therefore, if a follow-up operation is performed by an external force in that state, inertia, viscosity, static friction, and static friction on the movable part and the joint shafts (including the driving force transmitting part and the bearing part) against the displacement of the moving part of the robot due to the external force. Since various resistance forces due to dynamic friction etc. are generated, the follow-up force of the movable part cannot be zero or close to it, and smooth follow-up operation may not be possible. This follow-up force (force that opposes the above-mentioned resistance force ) Is large, the tool attached to the hand may come off the work.

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

[Means for solving problems]

Therefore, the robot control device according to the present invention controls the force of the movable part 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 at the same time, conveys the workpiece. According to the follow-up force detected by the follow-up force detection unit that detects the follow-up force received by the movable unit during the follow-up operation of engaging the movable unit with the conveyance of the work, A follow-up force compensating circuit that outputs a compensation command value for causing the motor that drives the movable portion to generate a torque larger than the follow-up force by which the movable portion pushes the workpiece in the follow-up direction is provided. , By controlling the drive current of the motor for driving the movable part by the compensation command value,
The movable part of the robot can be freely followed by an external force.

〔Example〕

Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings.

First, the construction and operation of the robot used in this embodiment will be described with reference to FIG.

In the figure, reference numeral 1 is a vertical articulated robot, which serves as a waist shaft 3 which is vertically erected on a base 2 which is fixed on a pedestal not shown, and a shoulder shaft which is fixed at right angles to the waist shaft. Motor 4
A first arm (upper arm) 5 connected to the second arm, a second arm (lower arm) 7 rotatably connected to the tip of the first arm by an elbow shaft 6, and a tip of the second arm 7. A hand 9 and the like which are rotatably connected to each other by a wrist shaft 8.

This is schematically shown in FIG.

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

The first arm 5 is moved by the motor 4 in the direction of arrow B to
The arm 7 is moved by the motor 11 in the direction of arrow C, and the hand 9
Is rotated in a vertical plane in the direction of the arrow D by a motor (not shown) built in the second arm 7, and the shoulder shaft, the elbow shaft 6 and the wrist shaft 8 connecting them are vertical joint shafts.

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

In addition, a tachogenerator for detecting the rotation speed and a potentiometer for detecting the rotation angle are attached to the output shafts of each of these motors.

The hand 9 has a piston rod fixed to a holder 12 connected to the wrist shaft 8 and having a rotation stopper of an air sillinder 13 attached thereto, and a force sensor 22 (details of which will be described later) is used to tighten a bolt at its tip. A nut runner 14 having a socket 14a is attached to the holder 12, and a pair of limit switches 15 and 16 is attached to the holder 12 so that the holder 13 is actuated by a dog 13a attached to a piston rod of the air cylinder 13. The upper limit position and the lower limit position of 14 can be detected.

On the other hand, 17 is a continuous conveyor (hereinafter simply referred to as "conveyor"), on which a work 18 such as an engine block or the like, which is positioned and fixed in a predetermined posture at a required position, is placed, and an arrow E is drawn in the working area of the robot 1. It is designed to be conveyed in a predetermined direction at a predetermined speed.

Then, the work 1 conveyed by this conveyor 17
A bolt 19 (for example, a bolt for fixing the head cover to the cylinder head) which is a work target is set on the robot 8. The robot 1 having the nut runner 14 attached to the hand 9 is required to tighten the bolt 19. It is something to try.

Further, 20 is a bolt passage detector fixed to the upper end portion of a stay 21 standing on the floor, and when the robot 1 stands by at a predetermined position as shown in the drawing, the conveyor 1
When the bolt 19 on the work 18 conveyed by 7 passes, it is detected.

As the bolt passage detector 20, for example, a reflection type photoelectric switch or the like is used.

Further, the positional relationship between the position where the bolt 19 of the bolt passage detector 20 detects and the nut runner 14 is that when the bolt passage detector 20 detects the passage of the bolt 19, the nut runner 14 is lowered to the lower limit. The vertical socket 14a has a relationship such that the bolt 19 is inserted.

Instead of the bolt passage detector 20, a dog is provided at a required position of the conveyor 17 corresponding to the bolt 19, and the dog is used to strike a limit switch attached to a fixed portion at a required position along the conveyor 17, The passage of the bolt 19 may be detected by turning on the limit switch.

In the force sensor 22, for example, as shown in FIG. 4, a large-diameter sensor holding cylinder 223 is fixedly mounted on a disc-shaped mounting plate 221 via a small-diameter stopper holding cylinder 222, and the sensor holding cylinder 22.
3. Secure each end of the cross-shaped sensor mounting plate 224 to 3,
A detection shaft 225 is vertically penetrated and fixed to the center of the sensor mounting plate 224.

Four sensor elements (for example, a strain sensor element such as a semiconductor strain cage) 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 detect the detection shaft 225. The inclination angle of 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 has the sensor mounting plate 2
Since 24 is distorted, each sensor element 226 is distorted according to the distortion amount.
The resistance value of changes.

Therefore, if the mounting plate 221 of the force sensor 22 is fixed to the tip of the piston of the air cylinder 13 shown in FIG. 2 and the detection shaft 225 is fixed to the base of the nut runner 14, the nut runner 14 receives an external force (conveyor in this example). When each movable part of the robot 1 moves following the moving force of the work 18 by the moving force of the work 18, the detection axis is inclined according to the amount of the following force received by the movable part. 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, reference numeral 23 is a central processing unit using a microcomputer or the like, and controls the overall control of the robot 1.

That is, a servo control unit for the motor 4 that drives the shoulder shaft that rotates the first arm 5 that is configured by the position command register 24, the position control unit 25, the speed control unit 26, the current control unit 27, and the like; Just like the servo control unit, the position command register 34, the position control unit 35, and the speed control unit 3
6, and a servo control unit for the motor 10 for driving the waist shaft 3 configured by the current control unit 37 and the like, and the second arm 7 configured similarly to these servo control units, though not shown. The servo control unit for the elbow shaft drive motor 11 and the servo control unit for the wrist shaft drive motor to be rotated are respectively controlled, and the air cylinder 13 and the nut runner 14 attached to the hand 9 shown in FIG. It also controls the nut runner 14 up and down by the air cylinder 13 and controls the rotation and stop of the socket 14a by its built-in motor.

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

The position control unit 25 uses 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 as A / A value converted into a digital value by the D converter 31, that is, a speed command value Sa based on a deviation from the current position value of the first arm 5 (corresponding to the angle θ in FIG. 3) is output, and the target position command value is also output. And the current position value match and each time positioning is completed, the central processing unit 23 is notified of this, and the central processing unit 23 measures the timing at which the next target position command value is output.

The speed control unit 26 receives a speed command value Sa from the position control unit 25, which is input via a command value control circuit 45 described later,
Tachogenerator 29 mounted on the output shaft of the motor 4
The current command value S ₁ based on the deviation from the speed feedback value from is output.

The current control unit 27 uses the current command value S from the speed control unit 26.
₁ is input via the adder circuit 32, and a drive current based on the deviation from the current feedback value from the current detector 28 that detects the drive current flowing through the motor 4 is passed through the motor 4.

Therefore, the position command register 24, the position control unit 25,
In the servo control unit for the motor 4 including the speed control unit 26 and the current control unit 27, a command value control circuit 45, which will be described later, uses a speed command value from the position control unit 25 and a tacho generator 2
As long as the speed feedback value from 9 is directly output to the speed control unit 26, the motor 4 is driven based on the target position command value from the central processing unit 23, and the first arm 5 is playback-controlled. (Positioning control) can be performed.

The respective units 34 to 42 forming the servo control unit for the motor 10 for rotationally driving the waist shaft 3 also function in exactly the same manner as the respective units 24 to 32 forming the servo control unit 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 unit 35 and the speed feedback value output from the tacho generator 39 attached to the output shaft of the motor 10 to the speed control unit 36 as they are. As long as this is done, it is possible to drive the motor 10 based on the target position command value from the central processing unit 23 and perform playback control (positioning control) on the orientation of the waist shaft 3 and the motor 4 integrated with it.

Further, each servo control unit for the motor 11 that drives the second arm 7 and each servo control unit for the wrist axis drive motor also functions in the same manner to drive each motor, respectively, and perform playback control of the second arm 7 and the hand 9. You can

The drive circuit for the air cylinder for controlling the air cylinder 13 in FIG. 2 is composed 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 air supply direction. Raise and lower. Socket 1
The rotation / stop of 4a is performed by controlling the power supply to the motor built in the nut runner 14 according to a command from the central processing unit 23.

The command value control circuit 45 includes eleven changeover switches SW _{1 to} SW that are switched by the excitation / non-excitation of the relay coil L _{1.
It is} composed of an electromagnetic relay having ₁₁ (four changeover switches SW _{5 to} SW _{8 which} are inserted in the servo control unit for the motor 11 and the wrist shaft drive motor are not shown).

The movable contact pieces c of the changeover switches SW ₁ and SW ₂ of the command value control circuit 45 are connected to the input side of the speed control unit 26, respectively.
Each fixed contact a is connected to the ground, and each fixed contact b is connected to the output side of the position control unit 25 and the tacho generator 29.

The movable contact pieces c of the changeover switches SW ₃ and SW ₄ are respectively connected to the input side of the speed control section 36, each fixed contact a is grounded, and each fixed contact b is the output side of the position detection section 35 and the tacho generator. 39 are connected to each.

Not-shown changeover switches SW ₅ , SW ₆ and SW ₇ , SW ₈
In the servo control section for the motor 11 and the servo control section for the wrist axis drive motor, respectively, they are connected in exactly the same manner.

The changeover switches SW ₉ and SW ₁₀ are gravity compensation command values CS ₁ and C output from a gravity balance compensation circuit 50 described later.
It is used as an open / close switch by inserting S ₂ into a line that inputs to the adder circuit 32 and a similar adder circuit in a servo control unit (not shown) for the motor 11, respectively.

Further, the changeover switch SW ₁₁ is used as an open / close switch by being inserted in a line for inputting a follow-up force compensation command value CS ₃ output from a follow-up force compensating circuit 60 described later to the adding circuit 42.

A diode D ₁ connected to both ends of the relay coil L ₁
Is a flywheel diode.

In the command value control circuit 45, when the relay coil L ₁ is not excited, the movable contact pieces c of the changeover switches SW _{1 to} SW ₁₁ are switched to the fixed contact b side, and the actual speed command value and the speed feedback Although the value is passed as it is, the servo control unit for each motor is positioned as it is, but when the relay coil L ₁ is excited, the movable contact pieces c of the changeover switches SW _{1 to} SW ₁₁ are moved as shown in the drawing. Fixed contact a
, The speed command value and the speed feedback value input to the speed control parts 26, 36, ... in the servo control part for each motor are set to zero (earth value), and the current command value based on the deviation is set. S _1, S _2, regardless ... the deviation of the actual speed command value and the velocity feedback value, to zero regardless of the movements of such other words motor 4,10.

In this way, if the current command value is set to zero regardless of the actual speed command value and the speed feedback value, the position and speed feedback control becomes ineffective, so each motor 4,
10, 11, etc. are in a free state, so that the second
The waist axis 3, the first arm 5, the second arm 7, and the hand 9 of the illustrated robot 1 can be freely moved by an external force.

However, in this case, if the drive current of each motor is completely reduced to zero, the vertical joint axes are rotated by the weight of the first and second arms 5, 7 and the hand 9, and the postures of these movable parts are changed. Because it collapses and you can not work,
A gravity balance compensating circuit 50 is provided to maintain the gravity balance and prevent the posture from being collapsed. The details will be described later.

Further, a follow-up force compensating circuit 6 for performing a follow-up force compensation at the time of a follow-up operation, which is a feature of the present invention, with respect to the servo control unit of the motor 10 which drives the waist shaft 3 which is the horizontal joint axis.
Although 0 is provided, the details will be described later.

The changeover switches SW _{1 to} S ₁ of the command value control circuit 45
Even when the movable contact piece c of W ₁₁ is switched to the fixed contact b side, the deviation between the speed command value and the speed feedback value becomes zero when the positioning is completed, but at this time, the position and speed feedback control is effective. , An external force is applied to the waist shaft 3, the first and second arms 5, 7, etc.
When 1 etc. is rotated from the stop position, a rotational force that immediately returns it is generated.

Further, the command value control circuit 45 operates when it functions to make the current command values S ₁ , S ₂ , ... Zero (when the relay coil L ₁ is excited in this embodiment). The state.

Reference numeral 46 denotes a changeover circuit, which is composed of changeover switches 47 and 48 inserted in a power supply circuit for the power source Vcc and an ascending limit switch 15 attached to the hand 9 of the robot 1. The limit switch 15 is a normally closed type and is turned off when the nut runner 14 reaches the upper limit and is hit by the dog 14b.

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) when the movable contact piece i of the switching switch 48 is switched to the fixed contact g side as shown in the figure. The relay coil L ₁ of the circuit 45 is de-excited, and when the limit switch 15 is on, the relay coil L ₁ is excited.

When the movable contact piece i of the changeover switch 48 is changed over to the fixed contact h side, the changeover switch 47 causes the command value control circuit 4 to operate regardless of whether the limit switch 15 is on or off.
It is possible to switch between operation and non-operation of 5.

In this embodiment, the excitation of the relay coil L ₁ and the operation of the command value control circuit 45 correspond to each other, but if the fixed contacts a and b of the changeover switches SW _{1 to} SW ₁₁ are reversed. , De-excitation of relay coil L ₁ and command value control circuit 4
It is also possible to correspond to the operation of 5.

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

The gravity balance compensating circuit 50 includes a CPU (Central Processing Unit) 51, a memory 52 including a ROM as a program memory and a RAM as a data memory, and A / D converters 53, 54 and D / A for each pair. Converter 55, 56
It is composed of a microcomputer consisting of and.

The gravity balance compensating circuit 50 has a rotation angle θ ₁ (third position) from the horizontal position of the first arm 5 output from the potentiometer 30 attached to the output shaft of the motor 4.
The voltage signal corresponding to (see the figure) is converted into a digital value by the A / D converter 53, read into the CPU 51, and similarly, a potentiometer 49 attached to the output shaft of the motor 11 is installed.
The voltage signal corresponding to the rotation angle θ ₂ (see FIG. 3) of the second arm 7 with respect to the first arm 5 output from the A / D
It is converted into a digital value by the converter 54 and read into the CPU 51.

Then, in the memory 52, the length l ₁ and the total length l ₂ from the shoulder shaft 4a of the first arm 5 of the robot 1 to the center of gravity and the length l ₃ and the total length l _{4 of} the second arm 7 from the elbow shaft 6 to the center of gravity are stored in advance in the memory 52. , First
Each weight W ₁ , of the arm 5, the second arm 7, and the hand 9,
Tables of W ₂ , W ₃ (see FIG. 3), sin θ, cos θ are stored in advance, and the moments of gravity M (A), M (B) at the shoulder shaft 4a and the elbow shaft 6 which are vertical joint axes are stored. CPU51
Performs the following calculation to calculate the compensation command values CS ₁ and CS ₂ for generating a shaft torque that can withstand the D / A.
It is converted into an analog signal through the converters 55 and 56 and output.

M (A) = K ₁ cos θ ₁ + K ₂ sin (θ ₁ + θ ₂ −90 °) M (B) = K ₂ sin (θ ₁ + θ ₂ −90 °) where K ₁ = l ₁ W ₁ + l ₂ ( W ₂ + W ₃ ) K ₂ = l ₃ W ₂ + l ₄ W ₃ Since K ₁ and K ₂ are constants, it is advisable to store them in the memory 52 in advance.

In this way, the compensation command values CS ₁ and CS ₂ output from the gravity balance compensation circuit 50 are added to the addition circuit 32 and the motor 11 via the changeover switches SW ₉ and SW ₁₀ of the command value control circuit 45 during the force release control. Input to a similar adder circuit in a servo control unit (not shown) for speed control unit 26,
Even if the current command values S ₁ , ... Output from the ... Are zero, the current control section 27, ... Causes the gravity compensating drive current to flow through the motors 4 and 11, and the first and second arms 5, 7 and Since the axial torque capable of withstanding the own weight of the hand 9 is generated, the first and second arms 5 and 7 can maintain the posture while maintaining the gravity balance.

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

Further, the function of the gravity balance compensating circuit 50 may be provided in the central processing unit 23 in FIG. 1 so that the common CPU performs time-division processing.

Next, a specific example of the tracking force compensating circuit 60 in FIG.
This will be described with reference to FIGS.

Tracking force compensating circuit 60 in this embodiment shown in FIG.
Is a viscosity compensating circuit 61, an inertia compensating circuit 62, and a dynamic friction compensating circuit 63, and a volume VR ₁ for adjusting the level of a compensation command value output from each of these compensating circuits.
VR ₂ and VR ₃ , a follow-up force detection circuit 64 that detects a follow-up force by a signal from the force sensor 22, and a starting force compensation circuit 6
5. Force compensating circuit 66, compensation switching circuit 67, starting force compensating circuit 65 and volume VR for adjusting the levels of the compensation command values output from the force compensating circuit 66, respectively.
And _4, VR _5, is constituted by an adder 68 for adding the outputs of the volume VR ₁ to VR _5.

Viscosity compensation circuit 61 is a circuit for outputting a compensation command value for generating the axial torque to offset the viscous drag movable portion of the robot 1 is generated in the waist axis 3 when displaced motor 10, an operational amplifier OP ₁ The amplifier having an input resistance R ₁ and a feedback resistance R ₂ and having an amplification degree of less than 1.

Then, during the follow-up operation period T of the robot 1, the voltage signal Vtg, which is a speed feedback value generated by the tachogenerator 39 according to the rotational displacement speed of the waist shaft 3 as shown in FIG. Is input to output a voltage signal Va proportional to the input as shown in FIG.

The inertia compensating circuit 62 exerts an inertial force corresponding to the mass from the waist shaft 3 to the hand 9 when the waist shaft 3 of the robot 1 is rotationally displaced, and counteracts this when the rotation is started and stopped. The inertial force that acts on the motor 10 causes a compensation command value for causing the motor 10 to generate a shaft torque that cancels the inertial force.
It is a differentiator composed of an operational amplifier OP ₂ , an input resistance R ₃ , a capacitor C and a feedback resistance R ₄ .

When a voltage signal Vtg as shown in FIG. 8 (a) is input from the tacho generator 39, a pulsed voltage signal Vb as shown in FIG. 8 (c) as an inertia compensation command value (in acceleration direction during acceleration, When decelerating, it produces a torque that generates torque in the decelerating direction).

The dynamic friction compensation circuit 63 provides a compensation command value for causing the motor 10 to generate an axial torque that cancels a dynamic friction force generated between the waist shaft 3 of the robot 1 and the bearing or the driving force transmission mechanism when the waist shaft 3 is rotationally displaced. The output circuit is a comparator having hysteresis including an operational amplifier OP ₃ , resistors R ₅ and R _6, and a volume VR ₆ for generating a comparison voltage Vr ₁ .

Then, when the voltage signal Vtg as shown in FIG. 8 (a) is input from the tacho generator 39, the dynamic friction compensation command value from the time when the input speed is slightly increased to the time when the input speed becomes zero.
A rectangular wave voltage signal Vc (polarity for generating torque in the following direction) is output as shown in (d).

If the following direction, that is, the rotation direction of the waist shaft 3 is reversed, the polarity of the voltage signal Vtg generated by the tacho generator 39 is reversed, so that each of these compensation circuits 61,
The polarities of the output signals of 62 and 63 are also inverted.

The tracking force detection circuit 64 is composed of a bridge circuit 641, a DC amplifier 642 and a lower filter 643, as shown in FIG. 7, for example.

The bridge circuit 641 includes a sensor element group 226 in which two of the four sensor elements 226 of the force sensor 22 shown in FIG.
With a and 226b as two sides and resistors Ra and Rb as the other two sides, a voltage is applied by a power source E between a and b, and a change in the resistance value of the sensor element groups 226a and 226b occurs between cd. Output the corresponding voltage.

The DC amplifier 642 has an operational amplifier OP ₈ and an input resistance R.
It is composed of c, Rd and a feedback volume VRf, and DC-amplifies the voltage output from the bridge circuit 641. The amplification degree is adjusted by the volume VRf.

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

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

The starting force compensation circuit 65 includes an operational amplifier OP ₄ and an input resistor R _7.
When the input signal Vd is generated, the amplifier having a large amplification degree, which is composed of the feedback resistor R ₈ and the feedback resistor R ₈ , immediately amplifies it to the saturation level and outputs a voltage signal Ve close to a rectangular wave as shown in FIG.
Is output.

However, when the voltage signal Vtg from the tacho generator 39 of FIG. 1 becomes large to some extent after the start of the follow-up operation, the relay Ry of the compensation switching circuit 67 for inputting it operates to open its normally closed contact Sy. the voltage signal Ve is no longer applied to the volume VR _4, the voltage signal Vf is Figure 9 as compensation command value applied to the volume VR ₄ (c)
As shown in, it becomes a signal only at startup.

The compensation switching circuit 67 includes an operational amplifier OP ₆ , an input resistor ₁₁ ,
A feedback resistor R ₁₂ and a comparator VR having a volume VR ₈ for generating a comparison voltage Vr ₂ and a relay Ry that operates when its output goes to a high level, and Vtg> Vr ₁ . At this time, the output of the operational amplifier OP ₆ changes from “L” to “H”, and thereafter, the hysteresis amount is adjusted by the resistor R ₁₂ so as to maintain the state until Vtg becomes “0”.

If the tracking direction can be reversed, the voltage signal Vtg from the tacho generator 39 may become negative, so that the absolute value of the compensation switching circuit 67 is set to a predetermined value regardless of whether the input signal Vtg is positive or negative. It may be configured by a window comparator that inverts an output exceeding the value and a relay Ry.

The force compensating circuit 66 is an amplifier composed of an operational amplifier OR ₅ , an input resistor R ₉ , a feedback resistor R _10, and a volume VR ₇ serving as an unbalance force setting device, and until the input signal Vtg increases to some extent and then disappears. A voltage signal Vg proportional to the input as shown in FIG. 9D is output as a compensation command value.

An offset is given by applying a positive or negative voltage to the inverting input terminal of the operational amplifier OP ₅ by the volume VR _7, thereby making the output signal Vg too large or too small,
Tracking force compensation can be unbalanced.

The output signals Va, Vb, Vc, Vf, and Vg of the compensation circuits 61, 62, 63, 65, and 66 are level-adjusted by the volumes VR _{1 to} VR ₅ , respectively, and then are analogized by the addition circuit 68. It is added and output as a tracking compensation command value CS ₃ .

The adder circuit 68 has five input resistors R for inputting the signals from the operational amplifier OP ₇ and the five volumes VR _{1 to} VR _5.
It is a non-inverting amplifier composed of _{13 to} R ₁₇ and a feedback resistor R ₁₈ .

By inputting the tracking compensation command value (voltage signal) CS ₃ output from the adder circuit 68 to the adder circuit 42 via the changeover switch SW _{11 of} FIG.
Even if the current command value S ₂ output from is zero, the current controller 37 causes the drive current corresponding to the follow-up compensation command value CS ₃ to flow through the motor 10 to generate viscous resistance in the waist shaft 3 during the follow-up operation. A shaft torque is generated to reduce the following force that opposes the force, inertial force, static friction force, dynamic friction force, etc., so that it can rotate following an external force with substantially no resistance.

In the following force compensating circuit 60 of FIG. 6, the force compensating circuit 66 is the minimum required in the present invention.

Here, a method of adjusting the tracking force compensating circuit 60 will be described.

First, the changeover switch 48 of the changeover circuit 46 of FIG. 1 is changed over to the contact h side (opposite to the figure), and the changeover switch 47 is changed over to the contact d side (the state shown in the drawing) to put the robot 1 into the force release state. , Adjust according to the following procedure.

At this time, the output of the volume VR ₇ of the force compensating circuit 66 is set to “0”, and the outputs of the volumes VR ₄ and VR ₅ are also set to “0”.
Leave.

(a) The axis to be compensated (the waist axis 3 in this embodiment) is moved at a constant speed, and the volumes VR ₃ and VR ₆ in FIG. ₆ are adjusted to adjust the dynamic friction compensation so that the following force is as small as possible. To do.

(b) The speed of moving the waist shaft 3 is set to medium speed and high speed, and there is no difference in the following force in each case.
Adjust the viscosity compensation by ₁ .

(c) The inertia compensation is adjusted by the volume VR ₂ so that the following force at the time of starting and stopping the movement of the waist shaft 3 is minimized.

(d) When the nut runner 14 shown in FIG. 2 is moved by an external force, the tracking force at the beginning of the movement becomes the smallest, and the volume VR ₄
Adjust the starting force compensation by.

(e) When the nut runner 14 is followed by an external force, the follower force at a substantially constant speed after starting becomes the smallest, and further, the volume VR is adjusted so that the nutrunner 14 does not move by itself.
Adjust force compensation by ₅ .

(f) When the nut runner 14 is engaged with the bolt 19, a slight pressing force is generated in the following direction, and the conveyor 17 (second
The unbalanced force is adjusted by the volume VR _{7 to the} extent that the engagement is not easily disengaged due to speed fluctuations and vibrations shown in the figure.

Next, the robot 1 shown in FIG. 2 is fixed on the conveyor 17 by using the control device shown in FIG. 1 which is adjusted as described above and can switch the playback control and the force release control as described above. The operation for tightening the bolt 19 of the work 18 moving at the speed will be described with reference to the flowchart of FIG.

It is assumed that the teaching work necessary for causing the robot 1 to perform the tightening work described below has been performed 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 description, the changeover switch 48 in the changeover circuit 46 of FIG. 1 is assumed to be changed over to the contact g side.

In this state, the switching circuit 46 causes the command value control circuit 45 to operate.
Since the power supply to the relay coil L ₁ is cut off, the movable contact pieces c of the changeover switches SW _{1 to} SW ₁₁ are all fixed contacts b.
Side, the waist shaft 3, the first and second arms 5,
7, motors 4, 10 and 11 for rotating the hand 9 respectively
Playback control such as is possible.

Therefore, in step 1 of FIG. 10, first, the motors 4, 10 and the like are playback-controlled to move the waist shaft 3, the first and second arms 5, 7 and the like to the original position (any retracted position may be used). Let

Next, the socket 14 a of the nut runner 14 attached to the hand 9 is conveyed by the conveyor 17 to the work 1
In step 2, the motors 4, 10 and the like are playback-controlled again so that the motors 4, 10 and the like are positioned so as to be located at the work point which is a predetermined standby position on the movement locus of the bolt 19 on the position 8. Put in a standby position.

In this state, waiting for the bolt passage detector 20 to detect the passage of the bolt 19 on the work 18, and when the bolt passage detector 20 detects the passage of the bolt 19, the detection signal is the central processing unit of FIG. 23, and the result of the determination in step 3 is YES, and in step 4, the air cylinder 13 is driven to lower the nut runner 14.

When the nut runner 14 starts descending, the ascending limit switch 15 is immediately turned on, so that the switching circuit 46 energizes the relay coil L ₁ of the command value control circuit 45,
Since all the movable contact pieces c of the changeover switches SW _{1 to} SW ₁₁ are changed over to the fixed contact a side as shown in FIG. 1, the drive motors 4, 10, 11 of each axis are all free,
The waist shaft 3 and the movable parts such as the first and second arms 5 and 7 are in a state of being free to move freely when an external force is applied, but the motors 4 and 11 are driven by the compensation command value from the gravity balance compensation circuit 50.
Since a drive current for generating an axial torque that resists the moment due to its own weight is applied to the device, balance is maintained and the postures of the first and second arms 5 and 7 are not disturbed.

Then, when the nut runner 14 is lowered to the lower limit, the socket 14a comes to engage the bolt 19 on the work 18 conveyed by the conveyor 17, and at the same time, the lower limit switch 16 is hit by the dog 14b to operate. Then, the judgment in step 5 is YES depending on the signal.
In step 6, the nut runner 14 is driven to rotate the socket 14a, and the bolt 19 is tightened.

In this way, the conveyor 17 moves the work 18 while the bolts 19 are held in the socket 14a of the nut runner 14 while the work 18 is being moved. Since the movable parts such as the arms 5 and 7 are in a state where they can be freely moved by an external force, the hand 9 follows the movement while tightening the bolt 19.
Can move horizontally.

At that time, each resistance force due to the viscosity, inertia, and dynamic friction acting on the waist shaft 3 is determined according to the sum of the respective command values of the above-described tracking force compensation circuit 60 for viscosity compensation, inertia compensation, and dynamic friction compensation. Although the driving currents flow in 10 to cancel each other out, static force and other resistance that cannot be completely compensated still cause the force sensor 22 to start the tracking operation.
The detection signal Vd appears from the following force detecting means by the following force detecting circuit 64, and the starting force compensation and the subsequent force compensation are performed according to the magnitude thereof, so that the nut runner 14 follows and moves with almost no resistance.

At that time, if the unbalance setting is made so that the compensation follow-up force is slightly larger than the resistance force by the volume VR ₇ of the force compensating circuit 66 of FIG.
Since a force that slightly presses the bolt 19 in the follow-up direction is generated, the engagement between the two is reliably maintained during the follow-up operation.

Then, by measuring the time from the start of tightening or the tightening torque, it is determined whether or not the tightening of the bolt 19 is completed in step 7, and when the tightening is completed, the driving of the nut runner 14 is stopped 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 ascending limit, the ascending limit switch 15 is turned off, so the command value control circuit 4
Since the release coil L ₁ of No. 5 is de-excited and each of the changeover switches SW _{1 to} SW ₁₁ is changed over to the contact b side, the drive motors _4, 10, ₁₁ of each axis are all in the playback control enabled state. Return to.

At the time of returning to a state where this playback control is possible,
Since the movable part of the robot 1 has moved from the state at the standby position, the position control part for each motor accumulates the position deviation due to the movement.

Therefore, when playback control is possible,
Each movable part immediately starts to return to the work point before the movement, but when the ascent limit switch 15 is turned off, the process returns from step 9 to step 1 to move to the first original position again. The movable parts such as the first and second arms 5 and 7 eventually return to their original positions while returning to the work point, and the above operation is repeated again.

In this way, while the nut runner 14 is tightening the bolts 19 of the work piece 18, each movable part of the robot 1 releases its force and follows the movement of the work piece 18, thereby requiring a very complicated control. The follow-up work that had to be done can be realized very easily.

Moreover, since the follow-up force compensation for reducing the follow-up force is performed corresponding to various resistance forces generated during the actual follow-up operation, the follow-up work can be performed extremely smoothly.

In addition, the conveyor 17 is stopped and the work 18 which is stationary
When tightening the upper bolt 19, the changeover switch 48 in the changeover circuit 46 is changed over to the contact point h side, and the changeover switch 47 is changed over to the contact point e side so that the stationary position of the bolt 19 coincides with the above-mentioned work point. If this is done, the stationary bolt 19 can be tightened by omitting the determination of bolt passage in step 3 of FIG.

Furthermore, if the changeover switch 48 is changed over to the contact h side,
By switching the changeover switch 47 to the contact point d side as necessary, the first and second arms 5, 7 and the like can be brought into a force releasing state in which they can be freely moved by external force.

In the above embodiment, an example in which the current command value is set to zero by switching both the actual speed command value and the speed feedback value to a zero value unrelated to the positioning control has been described. The current command value can also be made zero by switching the values to the same predetermined values that are irrelevant to the positioning control. Alternatively, the current command value may be directly switched to the zero value.

Further, the gravity balance compensating circuit 50 in the above-mentioned embodiment.
In the angle theta ₁ of FIG. 3 from the potentiometer 30 and 40, in accordance with the data corresponding to theta _2, had to calculate by calculating the gravity compensation value, pre beforehand the angle theta _1,
The optimum gravity compensation value for each vertical joint axis according to θ ₂ is stored as a table in the memory 52 of FIG.
The CPU 51 may read the gravity compensation value from the table according to the input angle data.

Further, the following force compensation is performed only for the waist axis, which has the greatest effect, but if it is also performed for each of the other axes such as the shoulder axis and the elbow axis, more complete compensation becomes possible. .

Next, another embodiment of the present invention provided with means for enhancing the safety during the force release control will be described with reference to FIGS. 11 to 13.

Although FIG. 11 shows only the safety means for the motor 4 that drives the first arm 5 of the robot 1, the same safety means is provided for the other motors 10, 11, etc. Is desirable. The first part is not shown.
This is the same as the previous embodiment shown in the figure.

In this embodiment, an electromagnetic brake 70 is attached to the drive shaft of the motor 4 (the shaft that rotates the first arm 5).

The electromagnetic brake 70 is an off-type that does not exert a braking force when energized by the brake power source 71 and exerts a braking force when the energization is cut off. Even if the robot 1 is broken, 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 that are turned off only when the relay coil L ₂ is excited are inserted in a connection line between the electromagnetic brake 70 and the brake power source 71. The diode D ₂ connected in parallel with the relay coil L ₂ is a flywheel diode.

Further, a current abnormality detection circuit 73 for inputting a setting signal from the current abnormality setting unit 72 and a current feedback signal from the current detector 28 via the switch SW ₁₂ to detect a current abnormality, and a setting by the speed abnormality setting unit 74. Signal and the speed feedback signal from the tacho generator 29 via the switch SW ₁₃ to detect a speed abnormality, a setting signal from the position abnormality setter 76, and a potentiometer 30 via the switch SW _14. And a position abnormality detection circuit 77 for detecting a position abnormality by inputting the position feedback signal from the.

The switches SW ₁₂ , SW ₁₃ , and SW ₁₄ are switches that are switched in conjunction with the switches SW _{1 to} SW ₁₁ by excitation of the relay coil L 1 in FIG. ₁ , and are normally movable contact pieces c as shown in the figure.
Are all switched to the contact a on the ground side, and are switched to the contact b during force release control to input the respective feedback signals to the respective abnormality detection circuits 73, 75, 77.

The output terminals of the abnormality detection circuits 73, 75, 77 are all connected to the other end of the relay coil L ₂ having one end connected to the power supply Vcc, and one of the output signals Ea, Eb, Ec But when a low level "L" by the abnormality detection, the normally closed contact SWa to energize the relay coil L _2,
Since SWb is opened, the electromagnetic brake 70 brakes and locks the rotating shaft of the motor 4, so that the rotation of the first arm 5 is stopped.

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

That is, the current abnormality setting device 72 is composed 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 -V and the ground.
As a voltage signal having a polarity corresponding to the direction of the current Im flowing in the motor 4 and proportional to its magnitude, the positive of the current feedback signal Vi generated from the current detector 28 of FIG. 11 as shown in FIG. The voltage Va corresponding to the normal limit and the voltage Vb corresponding to the negative normal limit are set and supplied to the current abnormality detection circuit 73.

The current abnormality detection circuit 73 includes resistors Rg to Rn, operational amplifiers OPa and OPb as comparators, and diodes Da and Db.
And a inverter INV that inverts its output.

Then, the current feedback signal Vi from the current detector 28 is respectively passed through the resistors Rg and Rj to the operational amplifier P.
Input to the non-inverting input terminal of Oa and the inverting input terminal of the operational amplifier OPb, and set the voltage Va set by the current abnormality setting device 72,
Vb is connected to the operational amplifier O via resistors Rh and Ri, respectively.
Input to the inverting input terminal of Pa and the non-inverting input terminal of the operational amplifier OPb.

Therefore, if Vb ≦ Vi ≦ Va, the operational amplifier O
Both outputs of Pa and OPb are "L", but Vi>
When Va or Vi <Vb, operational amplifiers OPa and OPa
The output EA of the window comparator obtained by adding any one of the outputs b to "H" by the diodes Da and Db is as shown in FIG. 13 (b).

The output signal Ea of the current abnormality detection circuit 73 obtained by inverting this output EA by the inverter INV is as shown in FIG. 13 (C), and becomes low level only when the motor current becomes abnormally large in any direction. To excite the relay coil L ₂ .

The speed abnormality setting unit 74 and the speed abnormality detecting circuit 75, and the position abnormality setting unit 76 and the position abnormality detecting circuit 77 have the same configuration as the current abnormality setting unit 72 and the current abnormality detecting circuit 73 shown in FIG. do it.

By providing such a safety means, the motor 4 is stopped and the first motor 5 is stopped regardless of any abnormality in the current value flowing through the motor 4 or any abnormality in the rotation speed or rotation position of the first arm 5. The rotation of the arm 5 can be stopped at that position. The power supply at that time may be cut off.

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

By the way, the present invention is not limited to the vertical articulated robot shown in FIG. 2, but can be applied to various robots such as a horizontal articulated robot and a vertical articulated robot having parallel link arms. When applied to a horizontal articulated robot, gravity balance compensation is unnecessary.

〔The invention's effect〕

As described above, according to the present invention, 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 movable part of the robot, the motor for driving the feasible part of the robot is controlled. In a control device for a robot configured to control a drive current, a moving part of a robot is set by setting a command value based on a deviation between a speed command value and a speed feedback value to zero regardless of an actual speed command value and a speed feedback value. Since it can be freely moved by external force, it is possible to use a compact and inexpensive electromagnetic relay etc. with a small contact capacity as command value control means, which not only reduces the number of contact maintenances, but also when switching contacts. Since the inrush current does not flow to the motor, it is not necessary to take preventive measures against it.

In addition, the actual follow-up force received by the movable part engaged with the conveyed work is detected, and the movable part is moved so as to generate a torque that is larger than the follow-up force and presses the work in the follow-up direction. Since the following force compensation for controlling the driving current of the driving motor is performed, the tool attached to the hand does not come off the work during the following operation of the robot.

[Brief description of drawings]

FIG. 1 is a block configuration diagram of a control device showing an embodiment of the present invention, and FIG. 2 is an external view of a robot around which a configuration of a vertical articulated robot to which the present invention is applied and work thereof are explained, Similarly, the figure is a schematic diagram for explaining the moment of gravity applied to the vertical joint axis of the robot, FIG. 4 is an enlarged perspective view showing the structure of the force sensor 22 in FIG. 2, and FIG. 5 is the gravity in FIG. FIG. 6 is a block circuit diagram showing a concrete example of the balance compensating circuit 50. FIG. 6 is a circuit diagram showing a concrete example of the following force compensating circuit 60 in FIG. 1. FIG. 7 is a configuration example of the following force detecting circuit 64 in FIG. 8 and 9 are signal waveform diagrams of respective portions for explaining the operation of the tracking force compensating circuit 60, and FIG. 10 is a flow chart showing an operation example of the central processing unit 23 in FIG. Fig. 11 shows safety measures FIG. 12 is a block diagram showing only a main part of another embodiment of the present invention, FIG. 12 is a circuit diagram showing a specific configuration of the current abnormality setting unit 72 and the current abnormality detection circuit 73, and FIG. 13 is a current abnormality detection. FIG. 9 is a signal waveform diagram of each part for explaining the operation of the circuit 73. 1 ... Vertical articulated robot, 3 ... Waist axis 4, 10, 11 ... DC servo motor 5 ... First arm, 6 ... Elbow axis, 7 ... Second arm 8 ... Wrist axis, 9 ...... Hand 13 ...... Air cylinder, 14 ...... Nutrunner 15,16 ...... Limit switch 17 ...... Continuous conveyor, 18 ...... Workpiece 19 ...... Bolt, 20 ...... Bolt passage detector 22 ...... Force sensor, 23 ... Central processing unit 29, 39 ... Tachometer 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 …… Following force detection circuit, 65 …… Starting force compensation circuit 66 …… Force compensation circuit, 67 …… Compensation switching circuit 68 …… Addition Road, 70 ...... electromagnetic brake

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Yasunori Horie 2 Takaracho, Kanagawa-ku, Yokohama, Kanagawa Nissan Motor Co., Ltd. 56) References JP-A-58-206389 (JP, A) JP-A-51-28959 (JP, A) JP-A-59-189416 (JP, A) JP-A-59-176805 (JP, A)

Claims (1)

[Claims] 1. A drive current of a motor for driving each movable part of the robot is controlled 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. In the controller of the robot described above, a command value control circuit for setting a command value based on the deviation between the speed command value and the speed feedback value to zero regardless of the actual speed command value and the speed feedback value, and this command value control A switching circuit for switching between operation and non-operation of the circuit; and a follow-up force detecting means for detecting a follow-up force received by the movable part during the follow-up operation for engaging the movable part with the conveyed work, According to the following force detected by the following force detecting means,
And a follow-up force compensating circuit for outputting a compensation command value for causing the motor for driving the movable part to generate a torque larger than the follow-up force by which the movable part pushes the work in the following direction. When the command value control circuit is operated to set the command value based on the deviation to zero, the drive current of the motor is controlled by the compensation command value output by the tracking force compensating circuit, and the movable part of the robot is controlled by an external force. A robot control device characterized in that the robot can freely follow the movement.