JPS62212812A - Control device for robot arm - Google Patents

Abstract

PURPOSE:To omit necessity for providing each servocontrol loop with an individual driving torque calculating auxiliary CPU by allowing respective servo CPUs to execute the operation of driving torque required for the control of respective servomotors. CONSTITUTION:Plural servo CPUs c1, c2 receiving a manipulator control command from a main CPU (a) and controlling a load to be driven around the respective shafts theta1, theta2 of the robot arm are connected so as to be mutually accessed. Respective servomotors e1, e2 execute prescribed position control and current control by respective servo CPUs c1, c2 and inverters d1, d2. Respective servo CPUs c1, c2 calculate driving torque corresponding to a position command signal based on the control command. Namely, information such as the rotational angles of other axes is inputted to the servo CPUs c1, c2 for executing the driving control of respective axes, on-line processing is executed to determine the torque command and the term of non-linear torque is feedforward compensated.

Description

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

(Prior Art) Recently, water movable joint type robots whose arms move two-dimensionally have been used for semiconductor device assembly and simple palletizing work.

FIG. 3 is a schematic external view of an example of such a horizontally articulated robot, as shown in FIG.
A post 1 that can be rotated by a drive motor is provided on a base 11 placed on the floor, and a first arm 2 is fixed horizontally to the tip of the post. A second arm 3 is also provided horizontally rotatably, and a wrist 4 at the distal end of the second arm 3 is provided. 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.

Reference numeral 6 indicates various cables such as a power cable that supplies power to this type of servo motor and a signal cable that transmits signals from various sensors, and 22 and 32 indicate a lid that covers the first arm 2 and the second arm 3. When performing maintenance or inspection of each arm drive system, remove the lid using the mounting screws.

By the way, such a robot arm 2.3 or ¥
-i”f The speed control system of the servo motor that drives 4 is:
As shown in FIG. 4, in response to the input of a predetermined position command, a feedback control system consisting of a position and speed control loop and a current loop is configured.

In the figure, a position command signal generated by a main CPU (not shown) is corrected to a predetermined position control signal by a servo control circuit, and sent to a servo motor together with an output signal of a current control loop composed of a minor loop. The servo motor controls the manipulator based on the applied 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 CF/V). Here, 1 indicates an integral gain, and K2 indicates a proportional gain. A servo motor is provided for each axis such as θl, 02, etc., and the output signal of the rotary encoder of each axis is input to the controller provided for each servo control circuit, and the output signal is input to the controller provided for each servo control circuit, and the output signal is input to the controller provided for each servo control circuit. The drive torque is calculated, and the nonlinear torque term is compensated by feedforward to the servo control circuit.

The torque command U for each of the above servo motors is as follows.

It is generally expressed by the following formula.

u=J(0)0+c(0,δ) +f(F) +g(θ
)=J(O)0+T(0,δ)...(
C) Here, JCO)0: Equivalent inertia of load, C(θ,
θ): Centrifugal force, Coriolis f(0): Frictional force, g(θ): Gravity, T(0, δ): Overall nonlinear torque, and this T(θ
, b) to the servo control circuit, feedforward compensation for the torque command is realized.

(Problems to be Solved by the Invention) Such a conventional servo motor control device can be easily represented as shown in FIG. 5 when looking at one servo motor. That is, a servo control circuit is provided for each servo motor for driving the load of each axis, which is connected to and controlled by the main CPU, and an auxiliary CPU is provided to determine the nonlinear torque that changes according to the displacement of the manipulator.
was provided, where calculations and control signals necessary for feedforward compensation were formed. In this way, the auxiliary CPU that calculates the drive torque is required separately from the calculation means for position commands provided for each servo control circuit, which increases the cost of the control device and makes it difficult to control data between each CPU. There were problems such as complication.

SUMMARY OF THE INVENTION An object of the present invention is to solve the problems of the prior art and provide a robot arm control device in which drive torque is calculated by a servo CPU that controls each servo motor.

(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. , and a bus line that connects these calculation means and makes it possible to mutually access the position information of the manipulator required for calculation of the nonlinear torque to be feedforward compensated among the drive torques. The problems of the prior art are overcome by providing a control device for a robot arm.

(Function) The control device of the present invention allows each servo CPU that controls the servo motors that drive loads around a plurality of axes to calculate the drive torque necessary for controlling the servo motors, thereby reducing the cost of the control device. can be reduced.

(Example) Hereinafter, one embodiment of the present invention will be described with reference to the drawings.

FIG. 2 shows a link L1. of length l. Length view 2 link L
2. It is a mechanical diagram of a horizontally articulated robot having /\\nd H with a length intersection of 3. The link L1 is rotatable around the 01 axis, the link L2 is rotatable around the 02 axis, and the /\nd H is rotatable around the θ3 axis.

Here, the total weight of link Ll is Ml (Mt = m1+
+mt 2 +mt 5), link L2 (7) total weight M 2 (M 2 = m 21 + m22
+ m23), t\7dH and the weight of the workpiece W as M
5 (M 5 = m h + yIW), and the center of gravity positions of links LL, L2, and /\nd H are respectively W (
, W 2 , W3, the torques TI , +2, +5 of each axis of the manipulator are determined as follows.

However, mij displays the weight of the link 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=, θ5 +f1 (Og)02 +f2 (θ2, θ3)dl + fg (θg)θ22 +f4 (θ2.θ5)θ12 +f5 (θ5)θ1θ2 = (A 2 M, +J, !ri θ 3+
(A” M3+AFw15墾2 cosθ6+J
5 yy) δ”2 + [AM5 (A+Q2 cos05+deaf1c
os(02+05)l+J5yy]
0 violet + (AM3If 2 51n05) 02 z
+ (AM5 (1!2 sinθ3+ l
15in(θ2 +05 ))) θ1z+ (2
AM5 Q2 5in(15) Ot 02...
・(1) Here, the coefficient A is A=1. −35, and the distance Ill S s from the tip of hand H to the center of gravity of hand H including the workpiece is S5 = (ms −
I31) It is calculated as mh/ (mh+Mw).
-(2) Also, Js expressing the inertia around the center of gravity of hand H in a coordinate system fixed to hand H
YY is J5yy=mh (85Qs t -3g)
z+MwS5 (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 +2 =
' eg +fl 'C05) θ2 +f2'('J2.θ3)51 +f5 '(θ3)θ2z " +4 " (θ2+ Os ) ('1z+f5'
(θ3)θ1θ2 +f days (θg)Cax”+2δ2δ3+2θ3θ1) (4).

The torque T1 of the link L1 around the 01 axis is TI=f7(
θ2. θ3) ・(θ1+02+03)2 +f days (θ2, θ3) Φ(0!+02)z +f9 (θ2.θ3) ・(θ1+02+05) +fl. (θ2. θ5) ・ (a+ +02) −t−f, t (02, θ3) ・ θ1”+
12 ((+2, Og) -612... (5)

In these equations (1), (4), and (5), the term θi is the equivalent inertia of the load, and is a term that changes according to changes in the rotation angle θi of each axis.

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
A plurality of servo CPUs that control loads driven around each axis of the robot arm 01.02... 1.
CPU c 2 . . . are connected to each other via a bus line f so as to be accessible to each other. Each servo motor etc.
+82 are the servo CPUc1. Each servo CPU c2, c2, . . . performs predetermined proximity control and current control using c2 and inverter dl+d2, and calculates a driving torque according to a displacement command signal based on the control command.

That is, servo cpuc1. which performs drive control of each axis. C2
...inputs information such as the rotation angle of other axes, performs online processing to determine the torque commands of equations (1), (4), and (5), and feeds the nonlinear torque term. This is compensated by forwards.

In addition, the SHARED RAMb stores information about each axis, and is commonly accessed by each servo CPU in order to read this information and perform torque calculations when the servo CPU is not controlling the current loop. It is set up so that you can.

(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,
Since each servo CPU calculates the drive torque required to control the servo motor, there is no need to provide a separate auxiliary CPU for drive torque calculation for each servo control loop, reducing control loop costs. can.

[Brief explanation of drawings]

:i'S Figure 1 is a block diagram showing an embodiment of the present invention, Figure 2 is a mechanism diagram showing an example of a horizontally articulated robot,
FIG. 3 is an external view of the robot, and FIGS. 4 and 5 are diagrams showing the configuration of the conventional device. l...post, 2...first arm, 3...second
arm, 4... wrist, a... main CPU, b.
・・Shared RAM, C1, c2 ・・Servo CPU, d+
, d2...inverter, el, C2...servo motor, f...bus line.

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. 1. A control device for a robot arm, comprising: a bus line that allows mutual access of position information of manipulators required for calculation of nonlinear torque subjected to feedforward compensation.