JPS62212806A - Control device for robot arm - Google Patents

Abstract

PURPOSE:To enable an operator to easily deal with the variation of a weight value of a work by changing and inputting the weight value of the work grasped by a robot arm to respective servo CPUs optionally at necessary timing to make the servo CPUs execute the operation of driving torque required for the control of servomotors. CONSTITUTION:After driving a main CPU (a) to rise a system, the operator inputs the weight of a robot and a work from a CRT/MDI (g) to a common RAM (b) through a main CPU (a). When positional commands are outputted from the main CPU (a) to respective servomotors e1, e2, the robot arm is started and the information of loads around respective axes is stored in the common RAM (b). Respective servo CPUs c1, c2 read out the information of the load around other axes from the common RAM (b) at the time when current loop control is not executed, and calculate the non-linear torque T (theta, dtheta/dt) through an on-line to execute feedforward compensation of a servocontrol system.

Description

[Detailed description of the invention] (Industrial application field) The present invention relates to a control device for a robot arm.

(Prior Art) Recently, in the assembly of semiconductor devices and simple retizing work, horizontal l5ilf, in which six arms move two-dimensionally,
FI type robots are being used.

FIG. 3 is a schematic external view of an example of such a horizontally open fM type robot. As shown in FIG. A first arm 2 is fixed horizontally to the tip of the post, and a second arm 3 is also provided to the tip of the first arm so as to be rotatable in the horizontal direction. It is provided at the wrist 4 at the tip of the arm 3. A servo motor for rotating the second arm 3 is housed in the first arm 2, and a servo motor for driving the wrist 4 is housed in the second arm 3. 6 is
These are various cables such as a power cable that supplies power to the servo motor and a signal cable that transmits signals from various sensors. 22.32 is a lid that covers the first arm and the second arm 3, and the lid that covers each arm drive system. When performing maintenance or inspection, remove the mounting screws.

By the way, the speed control system of the servo motor that drives the robot arm 2.3 or the wrist 4, as shown in FIG.
It constitutes a feedback control system consisting of a position and speed control loop and a current loop.

In the figure, the position command signal generated by the main CPU (not shown) is corrected to a predetermined position control signal by the servo control circuit, and sent to the servo motor together with the output signal of the current control loop consisting of a minor loop. A controller is connected to each servo control circuit, and each controller receives the IT value of the hand attached to the wrist 4 and the weight values of various workpieces gripped according to the interlocking of the manipulator from the main CPU. is input. The servo motor controls the manipulator based on the applied control signal. At this time, the rotational position of the load is detected by a rotary encoder and fed back to the servo control circuit through a frequency-voltage converter (F/V). . Here, K2 indicates a partial gain and K2 indicates a proportional gain. The servo motor is 0. ．． The output signal of the rotary encoder of each axis is input to the above controller, which calculates the drive torque required to control the servo motor, and converts the nonlinear torque term to the servo control circuit. compensate by feedforward.

The torque command U for each of the above-mentioned servo motors is generally expressed by the following formula.

u=J(θ)δ'+c(0,δ) +f((?) +g
(θ) = J (0) θ tenth (θ, θ)
...(C) Here, J(θ)0: Equivalent inertia of load.

C(θ, θ): centrifugal force, Coriolis, f(θ) two-frictional force, g(0): upward, T(0, δ): overall nonlinear torque, and this T(θ
, δ) to the servo control circuit, feedforward compensation for the torque command is realized.

(Problems to be Solved by the Invention) In such conventional servo motor control equipment, drive torque calculation is performed by a controller provided for each servo control circuit of each servo motor. The weight values of the hand and workpiece are set in the control program input to the main CPU, so when processing workpieces other than the specified fir, there were problems such as requiring a large amount of effort from the operator to change the program. .

The present invention solves the problems of the conventional technology by providing a servo CPU that controls each servo motor, and allowing an operator to appropriately input the weight value of the hand or workpiece to the bus line connecting these servo CPUs. The object of the present invention is to provide a control device for a robot arm that calculates a driving torque by inputting the following information.

(Means for Solving the Problems) The present invention includes a plurality of servo motors that drive a manipulator using a robot arm as an intermediary, and a plurality of calculation means that are provided in the control system of each servo motor and calculate the driving torque of each servo motor. , a bus line that connects these calculating means and makes it possible to mutually access the position information of the manipulator required for calculating the nonlinear torque to be feedforward compensated among the driving torque, and the above-mentioned The problems of the prior art are solved by providing a control device for a robot arm, characterized in that it is equipped with data storage means for appropriately inputting the weight value of the workpiece gripped by the robot arm into each calculation means. It is.

(Function) The control device of the present invention connects a bus line connecting each servo CPU to a manual data implant (MDI), for example.
) By providing a device such as a robot hand, the operator can input the information of the robot hand and the workpiece gripped by the hand as appropriate, and each servo CPU can calculate the nonlinear torque. System changes become easier.

(Example) Hereinafter, an example of the present invention will be described with reference to the drawings. Second
The figure shows a link L1 with a length of 11, a link L2 with a length of 2.
FIG. 2 is a mechanical diagram of a horizontally articulated mouth port 2 having a hand H having a length δ3. Link L1 rotates around the C1 axis, and link L2
is around the 02 car, and /\\nd H is configured as a rotating part around the 03 axis.

Here, the total weight of link LL is Ml (M1=mxt+
m+2+mt3), link L2 (7) total weight M2 (
M2=m21 +62+05), the weight of hand H and work W (7) is M5 (Mg = mh+MW), the center of gravity position of link L2 and hand H is w, respectively.
Assuming w2 and w5, the torques Tt, T2, and T5 of each axis of the manipulator are determined as follows.

However, mij displays the weight of the links L1*L2, the weight of the servo motor, etc. as a mass point system.

The torque T3 of the hand H around the 03 axis is T5 = +0g + f 1 C05) C2 + f2 (02, C3) 5! + fs (05) δ22 + f4 (02, Og)6+ 2 ”f5 (05)01 C2 = (AzM5 +J5 yy) f'3+
(A” M5 +AMg 12 cos05+J
5 Mari C2 + [AMg (A+12 cosθ5+ h
cos(θ 2 0g) ) +J5 t
tl OI+ (AMg 12 sinθ5)02
2+ (AMg (12sinθ3 + !l5in((+2005)))at”+
(2AMg 12 5in05) C102...
・ (1) Here, the coefficient A is A=+13 Sg, and the distance gl S g from the tip of the hand H to the center of gravity of the hand H including the workpiece is Ss = (13-1g,) mh / (mh
+Mw).・・・
・(2) Also, J! which expresses the inertia around the center of gravity of hand H in a coordinate system fixed to hand H! ! ! ! is J5 yy
=mh (15Is I -55) 2+MWSS...
-(3) However, if hand H is not gripping a workpiece, Mw in each of the above equations is set to zero.

The torque T2 of the link L2 around the 02 axis is T2=Kl
'θ5 +f1' (Og)C2 +f2'(C2, C3)C1 +fg'C03)02''+f4' (C2, C3)δ12 +f5'(C3)θ, δ2 +2θ301) (4).

The torque T1 of the link L1 around the 01 axis is.

T+ = f7 (02, Os)・(δ1+6
2+05)z +fs CO2, Os) ・ (θl +02)z +f9 (C2,05) ・ (C1+02+63) +f+o(C2, C3) ・ (01+02) +f, t (02, C3)・il+f+ 2
(C2, Os)' 0+ 2... (5) In these equations (1), (4), and (5), the term 51 is the equivalent inertia of the load, and the change in the rotation angle O1 of each axis This is a term that changes accordingly.

FIG. 1 is a block diagram showing an example of a control system for a robot arm that is controlled by applying the device of the present invention.

In the control device of the present invention, as shown in the figure, the main CPU a
In response to the control command of the manipulator commanded by
Multiple servo CPUs that control the loads driven around each axis of the robot arm 01, C2...
, CPU c 2- are connected to phase π accessing i'Tf@ via bus line f. Each servo motor e1, C2, servo CPU cI, c
2 and inverters d, d2, each servo CPU c1, C performs predetermined position control and flow control '11.
2... calculates the driving torque according to the position command signal based on the control command.

That is, the servo CPU c1.82 controls the drive of each axis.
Information such as the rotation angle of other axes is input to . This is compensated by forwards.

In addition, the shared (SHARED) RAMb stores information about each axis, and when the servo CPU is not controlling the current loop, it is used by each servo CPU c1, C2, C2, etc. in order to read out this information and perform torque @ calculation. It is set up so that it can be commonly accessed from...

This "(Right RAMb) is operated by the operator.

Also, the operation of the manipulator can be confirmed as image data.
[A capable CRT/MD Ig is connected and this CRT/
Make it possible to set the hand and workpiece tonnage using MD Ig.

Next, the operation of the device of the present invention will be explained.

After the main CPUa operates and boots the system,
The operator controls the robot hand and workpiece.
When inputting l from the CRT/MD Ig to the shared RAMb via the main cPUa, and outputting a position command to each servo motor el+e2... from the main CPUa, the robot arm is started and moves around each axis of the load. Next, each servo CPU c, c2... stores information about the other axes in the shared RAMb.
The nonlinear torque T (0, j) is read from the shared RAMb at a time when the current loop is not controlled, is calculated online using equations (L), (4), and (5), and is sent to the servo control system. On the other hand, feedforward compensation is performed.

Note that since the weight value of the hand itself does not normally change, only the value may be input to the work value.

Further, when the operator confirms on a CRT or the like that the robot hand has moved to a predetermined position, he or she can change and input the workpiece weight value as necessary.

(Effects of the Invention) As explained above, according to the robot arm control device of the present invention, the load rotated around the plurality of drive axes of the robot arm is controlled by the manipulator drive servo in accordance with the movement of the manipulator. When controlling the drive with a motor,
The workpiece ff1fk value held by the robot arm is changed and input to each servo CPU at the appropriate timing, and the drive torque required to control the servo motor is calculated, so the weight value of the workpiece can be calculated. The operator can easily deal with fluctuations in the process, and the system can be easily changed to process various workpieces.

[Brief explanation of drawings]

Fig. 1 is a block diagram showing an embodiment of the present invention, Fig. 2 is a mechanical diagram showing an example of a horizontally articulated robot, Fig. 3 is an external view of the robot, and Fig. 4 is a conventional device. It is a blow 2 diagram showing the composition of. l...post, 2...ml arm, 3...second
arm, 4...f neck, a...main CPU, b...
・・Shared RAM, OH, c2 ・・Servo CPU, dl
, d2... Inverter, e [ , C2... Servo motor, f... Bus line, g... CRT/MDI
.

Claims (1)

[Claims] A plurality of servo motors that drive the manipulator using the robot arm as an intermediary, a plurality of calculation means provided in the control system of each servo motor to calculate the drive torque, and a connection between these calculation means to calculate the amount of the drive torque. A bus line that allows mutual access of the position information of the manipulator required for calculating the nonlinear torque to be compensated for feedforward, and a predetermined calculation of the weight value of the workpiece gripped by the robot arm via this bus line. 1. A control device for a robot arm, comprising: data input means for inputting data to the means.