JPS6190207A - Robot controlling device - Google Patents

Abstract

PURPOSE:To take in most properly the change in each load inertia by reading out from a memory means the speed gain corresponding the present value detected of motion volume of an actuator and by calculating the output value of servo system based on the speed gain thus read out. CONSTITUTION:A mass is obtained from a mass table according to the work held by a robot RB. Next, according to the step of this mass, the value of a speed gain Kv of joint 3 is read out from a table in the memory, and written on a gain table. And joint angle theta3 detected and judged which range it belongs to. Next, based on the range to which the angle theta3 belongs and the step of the mass, the speed gain Kv is read out from the table in the memory, and written on the gain table. Next, a joint angle theta2 is detected and the range to which it belongs id judged, and based on the ranges to which theta2 and theta3 belong and the step of the mass, speed gain Kv of joint 1 is read out from the table in the memory and written on the gain table. These data are taken in a CPU where the output value of servo system is worked out and sent to driving unit.

Description

[Detailed Description of the Invention] <Technical Field of the Invention> The present invention relates to a robot control device, and in particular,
The present invention relates to a robot control device for controlling a robot having a rotational degree of freedom by incorporating the effects of fluctuations in inertia that change depending on the amount of operation of an actuator.

<Background of the Invention> In a robot having a rotational degree of freedom, such as an articulated robot or a polar coordinate robot, the load inertia with respect to the rotational degree of freedom varies depending on the amount of operation of an actuator. This kind of change in inertia. Since it affects the response in the servo system, it is necessary to incorporate it into the control system in some way.

However, in a servo system composed of analog circuits,
Due to its nature, it is impossible to incorporate such fluctuations. Furthermore, in multi-joint robots, joints near the base are greatly affected by variations in load inertia, so a reduction gear with a large reduction ratio must be used to reduce such effects. For this reason, this type of reduction gear increases costs, reduces efficiency, increases load fluctuations, increases play, and makes it difficult to perform minute feeds.
It had many shortcomings.

In addition, when configuring a position servo system using software, calculate the inertia around the axis of each joint from the joint angles of multiple joints, find the velocity gain corresponding to that inertia, and based on that, output in the position servo system. It is necessary to calculate the values, which requires a large amount of calculation time. For this reason, it is difficult to calculate the speed gain mentioned above in real time while the robot is operating. Conventionally, the speed gain is calculated off-line using the target angle of movement in advance, and the calculation result is It was stored in a memory, and during operation, the stored speed gain values were sequentially read out and used in the servo system. However, with this method, the above-mentioned offline processing is not possible during teaching operations, and in sensor feedback control using external sensors such as cameras, it is impossible to change and move the target point during operation. It has the disadvantage that it is possible.

<Purpose of the Invention> °This invention provides a speed gain value corresponding to the amount of movement of each actuator to the servo system in real time during robot operation, thereby controlling fluctuations in load inertia under various control conditions. The main purpose is to provide a robot control device that optimally incorporates Another object of the present invention is to provide a robot control device that can implement such control without reducing economic efficiency or deteriorating other performance.

<Structure and Effects of the Invention> In order to achieve the above-mentioned object, a robot control device according to the present invention controls the movement amount of the actuator of the robot and the speed gain in the position servo system.

and a reading means for reading out the corresponding speed gain value from the storage means in accordance with the current value of the operation amount of the actuator detected by the detection means, The output value of the servo system is calculated based on the speed gain value read out by the reading means.

According to this robot control device, it is possible to optimally set the response of the servo system in response to a change in the amount of operation of the actuator of the robot, and thus a change in inertia due to a change in the position or posture of the robot. In particular, the above effects can be achieved even when the target route or target position is changed during operation, such as in external sensor feedback. In addition, there is no deterioration in controllability such as oscillation or vibration due to inertia fluctuations, and it is possible to use a mechanism with a low reduction ratio, which reduces costs, improves efficiency, reduces load fluctuations, and reduces play. .

<Description of Embodiments> Hereinafter, embodiments of the present invention will be described by taking as an example the control device for an articulated robot shown in FIG. The multi-jointed robot (RB) in Figure 1 has 6 degrees of freedom, and has 6 joints.
- 6, each has a rotational degree of freedom of θ to θ6 (in this specification, the rotational degree of freedom in addition to the original rotation is also referred to as the "rotational degree of freedom"). These joints 1 to 6 are operated by servo motors M1 to M1 as drive mechanisms. M6 (not shown in FIG. 1), respectively.

FIG. 2 shows a schematic configuration of the control device 7 of the articulated robot 1-RB shown in FIG. The keyboard 8 is connected to the CPU (
Central Processing LJni
t) is for input/output to 9, and is for input/output to CPLJ
9 executes command analysis, command value calculation, arithmetic operations in the position control servo system, and the like. The output of the CPU 9 is given to a servo amplifier gain 80, and these servo amplifiers A1 to A6 amplify the respective output values from the CPU 9 and give them to the respective servo motors MtNM6.

The encoders E1 to L6 are photoelcoders attached to the servo motors M1 to M6, respectively, and detect the rotation angle of each motor and provide the detected rotation angle to the CPU 9.

FIG. 3 is a block diagram of a position servo system for one joint, and in this embodiment, there are a total of six similar servo systems. This servo system includes a velocity minor loop 11 within a position servo loop 10.

Command angle data 12 created based on teaching information stored in a memory (not shown) and commands input from the keyboard 8 in FIG. The feedback data 27 is subtracted to obtain the deviation signal 13.

This deviation signal 13 receives a position gain Kp in a block 14 to become a position output 15, and this position output 15 becomes a deviation signal 16 by subtracting the speed feedback data 29 fed back through the speed minor loop 11. Become. This deviation signal 16 is subjected to a servo amplifier gain KA in a servo amplifier 17 (one of the above A1 to A6), and its amplified output 18 is given to a motor M (one of the above M1 to M6).

In this motor M, the torque constant K in block 19 is
The generated torque becomes 20 depending on [, and this generated torque 20
is time-integrated in block 21 and converted into rotational speed 22 divided by inertia. In addition, in the figure, S
is the Laplace operator, and J is the inertia around the joint axis converted to around the motor axis. The rotation speed 22 is further time-integrated in block 23 and appears as the rotation angle 24 of the motor.

The rotation speed 22 of Tanabata is determined by the encoder E (the above E 1 to
E6), the encoder gain KE
is encoded while receiving it, resulting in an encoded output 25. However, in FIG. 2, encoders E1 to E6
Although it was assumed that the rotation angle is detected, in FIG. 3, for convenience of illustrating the concept, it is assumed that the rotation speed 22 is detected. After being time-integrated in block 26, encoder output 25 becomes position feedback data 27 which is returned to the previous stage of block 14, and becomes velocity feedback data 29 under the influence of velocity gain KV in block 28. Note that in these cases, viscous resistance is ignored for simplicity.

The transfer function G(s) in the servo system shown in FIG. 3 is expressed as the following equation (1).

However, ω. and ξ are the natural angular frequency and damping coefficient, respectively, and are given by. Further, the mechanical resonance angular frequency ωS for each joint is expressed as follows using the inertia J and the spring constant around the rotation axis. Here, the above-mentioned natural angular frequency ω. and the damping coefficient ξ is 1 (5) ω in order to prevent the operation of the robot 1-RB from becoming oscillatory. ≦i
It is set to approximately ωS ξ −1 (6).

Hereinafter, the handling of velocity gain will be explained using the joint 1 in FIG. 1 as an example. The causes of variation in the inertia J regarding the joint 1 include the joint angles of the joints 2 to 6 and the mass of a workpiece (not shown) held by the robot RB. In the specific example of robot RB shown in Fig. 1, the minimum value Jm of inertia J is
There is a relationship between in and the maximum value Jmax.

Jmin-6Jmax...(7
) By the way, as can be seen from equations (2) and (4), the natural angular frequency ω. is isomorphic to the resonant angular frequency ωS, that is, 1
/, /T and depends on inertia J.

Therefore, when the inertia J fluctuates due to the above-mentioned causes, ω at the same ratio. A change occurs in and ωS. Therefore, even if the inertia J changes, (5
) will always be satisfied. On the other hand, as for the damping coefficient ξ, when the value of the inertia J changes, the value changes according to equation (3) and no longer satisfies equation (6). For example, J"' 2 (Jmin + Jmax)
...If the value of speed gain Kv is set so that equation (6) is satisfied under the condition (8), then ξ=1.87 when J = Jml...
(9) At J=Jmax, ξ-0, 76...00
), so J < i (Jmin + Jmax)
-(In Ill, over-damped, J > i (Jmin + Jmax) ・
...at q force, there will be insufficient damping. Therefore, if the velocity gain Kv is configured to be changed according to the inertia J, then the damping coefficient ξ remains constant even if the inertia J changes, Stable control can be achieved.

By the way, among the causes of fluctuations in inertia J mentioned above, those caused by fluctuations in the joint angles of joints 4 to 6 are relatively small compared to other causes. The configuration is such that the value of the velocity gain Kv is changed according to the mass of -7. Moreover, these correspondence relationships also depend on the joint angle θ2. The operating range of θ3 is divided into a plurality of regions (16 in the example below), and one speed gain Ky is calculated in advance for each of the regions and stored in a memory such as a RAM. Furthermore, the mass of the workpiece is also divided into three stages.

This correspondence relationship regarding joints 1 to 3 is expressed as 11g 1
They are shown in Tables 3 to 3 respectively. Taking Table 1 as an example,
Joint angle θ2 of joint 2 is area (section) i (i=1 to 1
6), the joint angle θ3 of joint 3 is in the area j (j-1 to 16
), and the mass of the workpiece is the parameter k (k
-t, 2°3), then Kvi
, is the value of velocity gain Kv for joint 1.

Table 1 Also, the value of velocity gain Kv for joint 2 in Table 2 is:
Since the dependence on joint angle changes of joints 4 to 6 is ignored, the area i to which joint angle θ3 of joint 3 belongs and 7-
For the mass stage k to which 7 belongs, it is written as KVk. The value of velocity gain Kv for joint 3 in Table 3 similarly ignores the effect of rotation of joints 4 to 6, so 7-
Depending only on the mass step of 7, Kvk is shown.

Table 2 Table 3 These values of velocity gain Kv are obtained by calculating the inertia J for a predetermined value within each region of the joint angle θ2°θ3, for example, the rotation angle corresponding to the median value of that region. , above 1
3) This is the value obtained from the value of inertia J according to the relationship in equation 3. Regarding joints 4 to 6, in this example,
Such a correspondence relationship is not set, and a constant value is used as the value of the speed gain Kv.

Based on such a correspondence relationship, control using the value of the speed gain Kv stored as a table in an appropriate area in the memory will be explained with reference to the flowchart of FIG. 4. In FIG. 4, in the first step 31, it is determined to which stage the mass of the workpiece belongs. In this step 31, the procedure differs depending on whether there is one type of work or multiple types of work.

That is, in the former case, first, by detecting the opening and closing of the band of robot RB, it is determined whether the hand is holding the workpiece, and the mass of the workpiece is calculated from a table in a pre-specified memory. Yomitan out. When there are multiple types of workpieces, the type of workpiece currently held by the robot (RB) is known from the operation program, and the corresponding mass is determined from the workpiece mass table.

In either case, it is determined to which stage the mass thus determined belongs. In the next step 32, the value of the velocity gain Ky for the joint 3 is read from a table in the memory corresponding to the contents of the g43 table, depending on the mass stage. Read speed gain K
The value of v is written to a gain table in memory.

In the next step 33, the joint angle θ3 is detected and it is determined to which region it belongs. This detection may be performed by reading the target angle of movement from the memory, or the actual joint angle may be detected by the encoder E3. In any case, the values of the joint angles while the robot RB is in motion are determined, and in this specification, the joint angles detected by these methods are referred to as "
"Current value".

In step 34, depending on the region to which the obtained joint angle θ3 belongs and the stage to which the workpiece mass belongs,
From a table in memory corresponding to Table 2, the value of velocity gain KV for joint 2 is read and written to the gain table. Similarly, in step 35, the joint angle θ2
is detected and it is determined which region it belongs to, and in the next step 36, the above-mentioned θ2. The value of the velocity gain Kv for the joint l corresponding to the region to which θ3 belongs and the stage to which the workpiece mass belongs is read from the table in the memory corresponding to Table 1 and written to the gain table.

In this way, the speed gain Kv at a specific time
Once the value of is specified for each joint, the value of this speed gain Kv is taken into the CPU Q along with the robot motion command value inputted by the input means, other constant data, feedback data, etc. Based on this, the output value in the servo system is calculated and given to the drive mechanism (step 37). In step 38, it is determined whether or not the operation command value is the final one, and the above steps are repeated until the final operation command value.

The above is the joint angle θ2 of joints 2 and 3. Velocity gain Kv of the servo system of joints 1 to 3 according to θ3 and the mass of the workpiece
However, if the influence of the joint angles θ4 to θ6 of joints 4 to 6 cannot be ignored mechanically, the influence of those joint angles 04 to θ6 may be incorporated. Bye. Conversely, in cases where the influence of the mass of the workpiece can be ignored, the mass of the workpiece can be removed from the parameters.

By the way, in a vertically articulated robot as shown in FIG. 1, the influence of gravitational torque on the joints 2 and 3 is relatively large and acts as a disturbance on the servo system, which adversely affects accuracy. Therefore, in the past, in a process called gravity compensation, a torque that compensates for the gravity torque is calculated using each joint angle and a mathematical model of the robot (mass, center of gravity position, arm length), and the torque is applied to the input side of the motor. I was giving it in addition. However, the cause of the variation in the gravitational torque for each joint is joints 2 to 2.
6, the joint angles 02 to θ6, the mass of the workpiece, etc., and are almost the same as the causes of the variation in inertia J described above. Therefore, if the velocity gain Kv and the gravity torque value are given to each region shown in Tables 1 to 3, and the table corresponding to these tables is stored in the memory, the above-mentioned results can be achieved. It is no longer necessary to calculate the influence of the gravitational torque in real time, and the tasks to be processed by the CPU 9 can be reduced.

In the above embodiment, an articulated robot is considered, and the motions are also expressed by joint angles. However, the present invention is not applicable only to articulated robots, but can also be applied to polar coordinate type robots shown in FIG. 5, and can be used for all robots having rotational degrees of freedom. In addition, in the robot shown in FIG. 5, the amount of extension of the linear actuator 40 affects the inertia around the rotation axis 41, so not only the joint angle but also the amount of movement of the various actuators in general, i.e. Joint angles, extension amounts, and other quantities that affect inertia J can be used as parameters.

[Brief explanation of the drawing]

FIG. 1 is a mechanical diagram showing an example of a robot controlled by an embodiment of this invention, FIG. 2 is an overall configuration diagram of an embodiment of this invention, and FIG. 3 is a servo system included in an embodiment of this invention. FIG. 4 is a flowchart showing an overview of control performed in the embodiment of the present invention, and FIG. 5 is a mechanical diagram showing another example of a robot controlled by the embodiment of the invention. 1-6...Joint, 7...Robot control device,
9...cpu io...speed minor loop, 11...position servo system, 40...
Linear actuator, Tateiro Sakino //0. ii. Written amendment to the proceedings of the deceased <to oneself, method> January 14, 1985 1. Indication of the case 1988 Patent Application No. 212315 2. Title of the invention Robot control device 3. Person making the amendment Case Relationship with Patent applicant address: 10 Hanazono Tsuchido-cho, Ukyo-ku, Kyoto-shi 616 Name (294
) Tateishi Electric Co., Ltd. Address: 1-21-22 Shimanouchi, Minami-ku, Osaka 542 Common pill 5, drawings to be amended and detailed description of the invention in the specification column 6, contents of the amendment (1) In the drawings, “Figure 3” has been corrected as shown in the attached sheet. (2) On page 4, line 12 of the specification, "variations in inertia are caused by the servo system" were corrected to "variations in inertia are caused by the servo system." (3) On page 4, line 13 of the specification, “to give” was amended to “to give.” (4) On page 10 of the specification box, lines 3 and 4, correct "E time integrated" to "r time integrated". (5) Line 9 on page 15 of the specification (6) Corrected “7-7” on page 15, line 9, the last line of page 15, and lines 3 and 4 on page 16 to “work”. (7) “Table 1” shown on page 15 of the specification has been amended as follows.

Claims (4)

[Claims] (1) A robot control device for controlling a robot having a position servo system including a minor velocity loop and having a rotational degree of freedom based on input information, which comprises: a movement amount of an actuator of the robot; a storage means for storing velocity gain values in the position servo system in correspondence with each other; a detection means for detecting the current value of the movement amount of the actuator during robot operation; reading means for reading out the value of the speed gain from the storage means; a robot operation command value and a constant value input by the input means; a value of the speed gain read by the reading means; A robot control device comprising: calculation means for calculating an output value in the servo system based on input information including feedback information obtained from the robot, and outputting the output value to a drive mechanism of the robot. (2) The storage means stores information about the inertia of the actuator at a predetermined amount of motion within the motion region, corresponding to each motion region formed by dividing the motion range of the actuator into a plurality of regions. A speed gain value is stored, and the reading means includes means for determining which of the regions the current value of the operation amount is included in and reading the speed gain value corresponding to the region. A robot control device according to claim 1. (3) The storage means stores the operation amount value and the speed gain value in correspondence with each other according to the type of workpiece held by the robot, and the robot control device The robot includes a workpiece specifying means for specifying the type of workpiece held by the robot, and the reading means reads a value of the speed gain corresponding to the type of workpiece specified by the workpiece specifying means and the current value of the operation amount. A robot control device according to claim 1 or 2. (4) The storage means stores a value of the speed gain and a value of gravitational torque related to the degree of freedom of rotation of the robot in correspondence with the amount of movement, and the reading means stores the value of the speed gain and the value of gravity torque regarding the rotational degree of freedom of the robot, in correspondence with the amount of movement. The value of the speed gain and the value of the gravitational torque corresponding to the current value are read out, and the calculation in the calculation means is performed on the value of the speed gain and the value of the gravitational torque read by the reading means. A robot control device according to claim 1 or 2, which includes: