JP2783321B2 - Control device for articulated robot - Google Patents

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an articulated robot having interference with each other, and more specifically, a force generated by one drive source is an acceleration force of an arm having one or more axes. The present invention relates to a control method for an articulated robot having such a configuration. 2. Description of the Related Art An articulated robot is generally driven by one drive source for one degree of freedom. At that time, the force (torque) generated by one drive source is considered as the movement of only one axis. Therefore, the feedback control system is designed by simply considering the influence from the other axis as a disturbance without considering the influence on the other axis. The situation is a feedback speed system independent of one axis as shown in FIG. For ease of explanation, a horizontal two-axis robot driven directly by torque sources T _e1 and T _e2 will be described. Write an equation of motion of the horizontal 2-shaft shown in FIG. _{7, T e1 = J 11 1 +} J 12 2 + C o + C e 1 ...... (1a) T e2 = J 12 1 + J 22 2 + C e 2 ...... (1b). _{However, J 11 = m 1 s 1} 2 + m 2 (l 1 2 + s 2 2 + 2l 1 s 2 cosθ 2) + I 1 + I 2 J 22 = m 2 s z 2 + I 2 J 12 = m 2 (s z 2 + l _{1 s 2 cosθ 2) + I} 2 C o: Coriolis force _{= -2m 2 l 1 s 2 (} sinθ 2) 1 2 C e 1: centrifugal force _{= -m 2 l 1 s 2 (} sinθ 2) 2 2 C e 2 : centrifugal force _{= m 2 l 1 s 2 (} sinθ 2) 1 2 I 1: arm 1 about the center of gravity inertia I _2: around the center of gravity inertia of the arm 2 also, theta ₁ is 1 shaft rotation angle, theta ₂ biaxial rotation Angle, l ₁ is one-axis arm length, l ₂ is two-axis arm length, s ₁ is one-axis center of gravity distance, s ₂ is 2
Axial distance to the center of gravity, m ₁ is 1 shaft mass, m ₂ is a two-axis mass. [Problems to be Solved by the Invention] Consider a case in which speed feedback proportional control is applied to each of the one and two axes to be controlled. In the feedback control, it is necessary to increase a loop gain in order to increase a high-speed response to a command and a deterrent to disturbance. However, in the control system having the interference represented by the equations (1a) and (1b), hunting is more likely to occur than when the axes are mechanically fixed and independently controlled, and it is difficult to stably control the axes. To explain the reason (1
The following equations (2a) and (2b) are derived from equations (a) and (1b). Here, the speed is low for both the one and two axes, and the nonlinear forces C _o , C _e 1, C
_e 2 shall be small and friction shall not be considered. _{1 = (J 22 Te1-J} 12 Te2) / D ...... (2a) 2 = (- J 12 Te1 + J 11 Te2) / D ...... (2b) _{However, ω 1 = 1, ω 2} = 2, D = J a ₁₁ · J ₂₂ -J ₁₂ ^2. In one-axis motion, that is, in the equation (2a), the component due to Te2 can be regarded as a disturbance torque. When the te2 = 0, 1 inertia shafting is _{J 22 / (J 11 · J} 22 -J 12 2) next, as compared with the case where mutual interference does not work, be considered as the inertia value becomes smaller Can be. Therefore, when the loop gain in each feedback control system is increased in the presence of interference, hunting occurs earlier than in the absence of interference, and the control system cannot be controlled stably. In addition, when the torque by Te2 is working, the control system becomes more unstable due to the disturbance force. This is exactly the same for a two-axis control system. As described above, a multi-axis control system having interference, particularly a multi-axis robot that directly drives by a driving source, has a disadvantage that the upper limit of the gain is low, and it is difficult to obtain a high-speed response and a high-rigidity servo. The present invention has been made in view of such conventional problems as described above. By calculating and synthesizing the torque commands of the respective shaft motors, the inertia term is made non-interfering, thereby improving the response speed in the control system and The purpose is to enable enhancement of rigidity. [Means for Solving the Problems] The present invention provides a speed command (ω _n ^* ) to a drive source of each axis of an N-axis articulated robot having an interference between axes, and a speed detection value (ω _n). ), The control device of the articulated robot having a speed control unit (101, 102) for each axis for speed control so as to eliminate the deviation amount from the angle data (θ-1) of the (N-1) axis other than the root first axis. ₂ ) means (107, 108, 207) for correcting the load inertia coefficient of each axis, and means (109, 109) for inputting the output of the means for correcting the load inertia coefficient of each axis and inputting the output of the means for correcting the gain of each axis. 20
8) and means (102, 202) for multiplying the output signal of the speed control unit of each axis by the corrected load inertia coefficient of each axis after the gain difference is compensated, and by setting different time constants And response delay compensation calculating means (103, 203) for equalizing the response delay of each axis of the signal after the multiplication. [Operation] FIG. 1 shows a decoupling control block 51 of the present invention, in which ω ₁ ^* , ω ₂ ^*, and ω ₃ ^* are velocity commands of one axis, two axes, and three axes, respectively, and ω ₁ , omega each one axis ₂ and omega _3, 2
Axis and three axis detection speeds, 11 and 12 are first and second speed controllers for performing an operation by a combination of proportional / integral / derivative from the speed command and the detected speed, respectively. I ₁₁ and I ₁₂ are speed control units. Output signals, 21 to 24 are inertial load data (non-interference data) memories or arithmetic units based on inertial load data, 31 is a multiplier, 32 is an adder, 41 and 42 are one-axis and two-axis force control compensators, I _21, I ₂₂ is the output of the non-interacting control block. FIG. 2 shows an overall speed control system including the control block shown in FIG. 1 as a control target. In FIG. 2, 301
Is a speed control unit, 302 is a non-interference control unit, 303 is a generated force control unit, 304 is a load unit, 305 is a detection unit, 306 is a speed command input, 3
07 represents the speed. Here, the equation of motion of the controlled object is described by equation (3). In the following description, τ and f are vectors relating to τ and f in equation (3), respectively. In the equation (3), f _{1 to} f _n are functions of ω ₁ to ω _{n. When} the values are small and can be ignored, the acceleration of the arm with respect to the generated force can be obtained by the following equation. However, from equation (4) The arm structure does not have the case. Also, in equation (5), It is. When a force as shown by the equation (5) is generated, the output can be apparently controlled one-to-one by τ ^* . For example, When the unit matrix is (However, τ _j ^* : output of each axis speed control unit, K _Ti / K _Ii : generation force control unit gain). (6) When writing down the formula, Therefore, each shaft torque (force) command generated by the non-interference control unit 302 is represented by the following equations (8-1) and (8-2). When the frequency characteristics of the generated force control units are different, the compensators 41, 42 and the like are provided so that the acting force works simultaneously, and the time delay is compensated. The nonlinear forces f _{1 to} f _n in the equation (3) are a centrifugal force, a Coriolis force, a friction force, an external force, and the like, and the compensation is performed by a feedback loop of each axis. EXAMPLES Hereinafter, the present invention will be specifically described based on examples shown in the drawings. FIG. 3 shows an embodiment in which the present invention is applied to the control of a horizontal two-axis robot. In the figure, reference numeral 301 denotes an operation block of the previously described robot motion equations (1a) and (1b);
Reference numeral 9 denotes inertia data for diagonalizing the inertia matrix. If this inertia data is known or θ
_When the change in the mass at the tip of the _two arms is small, it can be stored in a memory. The fact that decoupling in the inertia term can be achieved with the inertia data in FIG. It is clear that Incidentally, J ₁₁ ^* J ₁₁ is a representative value of J _11, by a change in the theta ₂
Is prevented from changing the loop gain in the one-axis speed loop. J ₂₂ is constant. Further, the gains K _n1 and K _n2 in the speed control units 101 and 201 are:
Inertia J ₁₁ ^*, is set to an appropriate value for J _22. The gain in the speed control units 101 and 201 can be set to a value larger than that at the time of interference because hunting can be prevented by decoupling. FIG. 4 shows the state of improvement by the present invention in comparison with the conventional one. That is, a graph (FIG. 4 (a), where T _n → ∞) showing the range of the stable gain setting when control is performed by the conventional control system shown in FIG. 6, and the present invention shown in FIG. Graph of stable gain range in control system (Fig. 4 (b))
It is clear from the comparison with that that the range has become wider. FIG. 5 shows a comparison of the speed step response at the time of interference and non-interference at the same loop gain. That is, FIGS. 5 (a1) and (a2) show the speed step response of one axis and two axes at the time of interference by the conventional control method, and FIGS. 5 (b1) and (b2) show the time step response at the time of non-interference by the control method of the present invention. 3 shows the speed step response of one axis and two axes in FIG. The interference force other than the inertia term, that is, the Coriolis force and the centrifugal force, can be compensated for by the speed loop rigidity because the speed change, that is, the force change is slower than the speed loop response speed. Reference numerals 104 and 204 denote current control units, and there is a difference in characteristics between the first and second axes including the torque constants 105 and 205. The gain difference is compensated for in the inertia data portions 109 and 208, and the delays are I ₁₂ ^* and I ₂₂
^It is necessary to perform correction at the stage of output of ^* . For example, K ₁₁ (s) = 1 / (1 + T ₁ s), K _T2 (s) = 1 /
If (1 + T ₂ s), then G ₁ (s) = 1 / (1 + T ₂ s), G
₂ Insert a compensation element such as (s) = 1 / (1 + T ₁ s), and
Equal feeds on the axis. If the difference between the delays at K ₁₁ (s) and K _T2 (s) can be ignored, the compensators 103 and 203 can be omitted. Incidentally, in the present embodiment has been described in example two horizontal-axis robot, although data to correct the inertia data was theta _2, if the horizontal three-axis robot theta _2,
It will be modified from θ _3. That is, angle data of an axis other than the root first axis is required. In that case, it is necessary to multiply the speed command signal of each axis by inertia data. However, from the example of the horizontal two-axis robot described in the present embodiment, the case of the n-axis robot can be easily implemented by those skilled in the art. Omitted. [Effects of the Invention] As described above, in the present invention, a compensation signal is generated based on the deviation between the speed command and the speed detection value for the drive source of each axis, and the compensation signal is used to control the inertia between the axes. To eliminate interference. Therefore, the loop gain of the feedback control system can be greatly increased by simply adding a storage function and an arithmetic unit to the conventional multi-axis speed control system and exchanging information between the axes. A multi-axis control system having high rigidity can be obtained.
In addition, when implementing the present invention in a digital speed control system having a CPU, only information exchange and addition of software are required.
The above functions can be realized.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram showing the configuration of a control system according to the present invention, FIG. 2 is a block diagram of a speed control system of the entire robot control system including the control system of the present invention, FIG. FIG. 4 is a block diagram of an embodiment of the present invention, FIG. 4 is a graph comparing a stable gain setting range between the conventional control system and the control system of the present invention, and FIG.
The figure is a graph comparing the speed step response between the conventional control system and the control system of the present invention. FIG. 6 is a block diagram showing an example of the conventional control system. FIG. 7 is the equation of motion of a horizontal two-axis robot. It is a schematic diagram for explaining. 11: 1-axis speed controller, 12: 2-axis speed controller 21: non-interference data, 122: non-interference data 23: non-interference data, 324: non-interference data 4 31: multiplier, 32: adder 41: 1-axis force control unit compensator 42: 2-axis force control unit compensator 51: decoupling control block

   ────────────────────────────────────────────────── ─── Continuation of front page                   (56) References JP-A-60-77210 (JP, A)                 JP-A-57-163090 (JP, A)                 JP-A-59-226903 (JP, A)                 JP-A-61-75401 (JP, A)                 JP-A-59-167706 (JP, A)                 JP-A-59-220806 (JP, A)

Claims (1)

(57) [Claims] Speed control for speed control so that there is no deviation between the speed command (ω _n ^* ) for the drive source of each axis of the N-axis articulated robot and the detected speed value (ω _n ) where there is interference between the axes. In the control device for an articulated robot having a unit (101, 102) for each axis, the angle data (θ ₂ ) of the (N-1) axis other than the root first axis
To correct the load inertia coefficient of each axis according to
7, 108, 207) and means for compensating for the difference in gain of each axis by inputting the output of the means for correcting the load inertia coefficient of each axis (109, 208).
Means for multiplying the output signal of the speed controller of each axis by the corrected load inertia coefficient of each axis after the gain difference has been compensated; and setting different time constants, A control device for an articulated robot, comprising: response delay compensation calculating means (103, 203) for equalizing the response delay of each axis of the multiplied signal.