JP2576171B2 - Articulated arm control device - Google Patents

Description

The present invention relates to a multi-joint arm control device, and more particularly, to suppressing the vibration of the tip of a series of arms and simultaneously improving the positioning accuracy of the tip of the arm. is there.

[Prior Art] As a control system of an industrial robot having a working arm, for example, in a servo system for each joint of an arm, an output of an encoder for detecting a rotation angle of an output shaft such as an electric motor and a rotation speed are controlled. A system for feeding back an output of a tach generator to be detected is usually implemented. However, in a robot with a heavy load, the arm has a large moment of inertia, and the natural frequency related to the vibration of the arm is reduced by a reduction gear, a torque transmission mechanism, and a spring element such as the arm. There has been a problem that large vibrations are likely to occur and the positioning accuracy of the arm tip is reduced.

Regarding this issue, we have already designed the internal model of the system,
By correcting the input of the model so that the deviation between the estimated output value from this model and the actual output value becomes zero, and feeding back the corrected model output value and input value to the robot position command value, A proposal for preventing vibration has been made (JP-A-61-255415).

[Problems to be Solved by the Invention] However, in such a system, although a model is set independently for each of a plurality of arms, the arms actually interfere with each other. For this reason, each arm cannot be regarded as independent and the model becomes inappropriate. Therefore, it is difficult to say that truly accurate control is performed, and the prevention of vibration and the positioning accuracy are incomplete. However, if one model is designed with a series of arms, it becomes nonlinear, and the above method cannot be applied as it is.

SUMMARY OF THE INVENTION An object of the present invention is to solve the above-described problems, and by effectively treating a mathematical model of a ground that is originally nonlinear as linear for each angle of each arm, it is possible to effectively prevent arm vibration. And a multi-joint arm control device that realizes high-precision control.

[Means for Solving the Problems] That is, the gist of the present invention is, as illustrated in FIG. 1, a plurality of arms M1 arranged in series, and an actuator M2 for operating each of the arms M1. A position detecting means M3 for detecting the position of the actuator M2
And a control means M4 for controlling the actuator M2 to cause the arm M1 to perform a predetermined operation. An articulated arm control device comprising:
A mathematical model M5 that is set assuming that the system constituted by M1 is a system composed of a spring and a mass, using a nonlinear mathematical model M5 that estimates at least an actuator displacement or an arm displacement from an actuator drive command value. By treating the mathematical model M5 as linear for each displacement of the actuator M2, a feedback compensation amount is obtained in accordance with the actuator drive command value and the actual value of the actuator displacement, and the actuator drive command value is calculated based on the feedback compensation amount. A multi-joint arm control device characterized in that compensation is performed.

[Operation] Since the multi-joint arm control device of the present invention is multi-joint, the mathematical model M5 from which the actuator displacement or the arm displacement can be estimated is non-linear. Can be considered as a linear model M5. Therefore, it can be considered that the nonlinear mathematical model M5 also has a linear mathematical model corresponding to the displacement of the actuator M2 or the arm M1.

From this, the mathematical model M5 can be solved by modern control theory, and the control means M4 calculates the feedback compensation amount from the actuator drive command value and the actual value of the actuator displacement by using the mathematical model M5, and obtains the actuator drive command value. To compensate. This makes it possible to suppress a series of tip vibrations of the arm M1 when the arm M1 stops, and to control the tip of the arm M1 to an accurate position in consideration of the vibration.

Next, examples of the present invention will be described. The present invention is not limited to these, and includes various embodiments in a range not departing from the gist thereof.

Embodiment FIG. 2 and FIG. 3 show an embodiment of the articulated arm control device of the present invention. FIG. 2 is a schematic configuration diagram of the arm portion. The first arm 3, the second arm 5, and the third arm 3
The arm 7 is swingably connected via the swing shafts 9, 11, and 13. An output shaft of a servo motor described later is coupled to each of the shafts 9, 11, and 13 via a reduction mechanism composed of a combination of gears. The arms 3, 5, and 7 are connected to the servo motor by an electronic control circuit described later. Is configured to be able to be located at a desired rotational position when driven. An electrode 15 for welding is arranged at the tip of the third arm 7 so that electric welding can be performed at a desired position.

 FIG. 3 shows an overall block diagram of this embodiment.

Encoders 17, 19, and 21 for detecting the swing angles of the arms 3, 5, and 7 are provided on the shafts 9, 11, and 13, and the signals are input to the electronic control circuit. In addition, each axis 9, 11, 13
The rotation angles of the servo motors 25, 27, 29 provided in the electronic control circuit 23 are controlled by the electronic control circuit 23. In the configuration of the electronic control circuit 23, the CPU 23a calculates and controls the drive amounts of the servo motors 25, 27, and 29 based on the input signals. The read only memory ROM23b has CPU2
A program used in the process of 3a, constants, data of a mathematical model described later, and the like are stored. The RAM 23c, which is a readable and writable memory, temporarily stores calculation results obtained by the CPU 23a, detection data from the encoders 17, 19, and 21.

The input unit 23d receives signals from the encoders 17, 19, 21 and the instruction input keyboard 31 and the like, and performs signal processing such as waveform shaping on these signals. Output section
23e includes a drive circuit for the servo motors 25, 27, 29, and the CPU 23
Based on the processing result executed in a, servo motors 25, 27, 2
9 or a signal for driving a high voltage circuit for welding (not shown) as required. Common bus 23f
Connects the CPU 23a, the ROM 23b, the RAM 23c, the input unit 23d, and the output unit 23e to mutually transmit data.

Here, a coordinate system is set as shown in FIG. 4 in order to set a mathematical model of the series of arms 3, 5, and 7. However, the first arm 3 and the second arm 5 are assumed to swing on the xy plane, and the third arm 7 is not swinging during the swing. l1, l2, l3 is the length of each arm 3,5,7, d1, d2, d3
Is the distance from axes 9,11,13 to the center of gravity of each arm 3,5,7, θA
1, θA2 represents the swing angle of the first and second arms 3,5. Furthermore,
M1, m2, m3 appearing thereafter are the mass of each arm 3,5,7, DA1,
DA2 and DA3 indicate the viscous resistance (for example, sliding friction) of each of the arms 3, 5, and 7.

Considering a two-dimensional vibration as the vibration at the tip of the third arm, the vibration is divided into a vibration component in the extension direction of the second arm 5 and a vibration component perpendicular thereto. Therefore, as shown in FIG. 5, a line q1 obtained by projecting the third arm 7 on a plane α formed by the extension line p1 of the second arm 5 and the z-axis, and an xy with respect to the second arm 5
Considering a perpendicular line p2 on the plane and a line q2 obtained by projecting the third arm 7 on a plane β formed by the z-axis, the angle of the third arm 7 is calculated by using the line
The angle θA31 between q1 and the z-axis and the angle θA32 between the line q2 and the z-axis are represented.

The equations of motion of the arms 3, 5, and 7 are derived by Lagrange's mechanics as follows.

Lagrangian L is defined as the difference between the total kinetic energy K and the total potential energy P of the system as follows:

L = K−P (1.1) The equation of motion is given by the following equation using coordinates representing kinetic / potential energy.

Where qi is coordinates representing kinetic / potential energy, i is its velocity, τi is the corresponding torque, and D is the dissipated energy. In order to apply the above equation of motion to the series of arms 3,5,7, Perform such processing.

(1) First, the position of the center of gravity of each of the arms 3, 5, 7 is obtained as follows.

Coordinates of the center of gravity of the first arm 3 x1 = d1 · COSθA1 y1 = d1 · SINθA1 z1 = 0 (1.3a) Coordinates of the center of gravity of the second arm 5 x2 = l1 · COSθA1 + d2 · COS (θA1 + θA2) y2 = l1 · SINθA1 + d2 · SIN ( θA1 + θA2) z2 = 0 (1.3b) Coordinate of the center of gravity of the third arm 7 x3 = l1 · COS θA1 + l2 · COS (θA1 + θA2) + d3 · SIN θA31 · COS (θA1 + θA2) -d3 · SIN θA32 · SIN (θA1 + θA1 · θ1 + θA1 · θ1 + θA1 + θA1) SIN (θA1 + θA2) + d3 · SINθA31 · SIN (θA1 + θA2) -d3 · SINθA32 · COS (θA1 + θA2) z3 = -d3 (1-SIN 2 θA31-SIN 2 θA32) 1/2 (2) of each arm 3, 5, 7 The velocity of the center of gravity is expressed as follows by differentiating equation (1.3).

Center-of-gravity velocity of the first arm 3 1 = −d1 · SINθA1 · A1 1 = d1 · COSθA1 / A1 1 = 0.・ (A1 + A2) 2 = l1 ・ COSθA1 ・ A1 + d2 ・ COS (θA1 + θA2) ・ (A1 + A2) 2 = 0… (1.4b) Velocity of the center of gravity of the third arm 3 = −l1 ・ SINθA1 ・ A1 −l2 ・ SIN (θA1 + θA2 ) ・ (A1 + A2) −d3 ・ SINθA31 ・ SIN (θA1 + θA2) ・ (A1 + A2) −d3 ・ SINθA32 ・ COS (θA1 + θA2) ・ (A1 + A2) + d3 ・ COSθA31 ・ COS (θA1 + θA2) ・ A31−A2 ・ A31−A2・ A32 3 = l1 ・ COSθA1 ・ A1 + l2 ・ COS (θA1 + θA2) ・ (A1 + A2) + d3 ・ SINθA31 ・ COS (θA1 + θA2) ・ (A1 + A2) −d3 ・ SINθA32 ・ SIN (θA1 + θA2) ・ (A1 + A2) θA1 + θA2) · A31 + d3 · COS θA32 · COS (θA1 + θA2) · A32 Here, g (θ) = SINθ · COSθ ·.

(3) From the above relationship, the kinetic energy K1,
K2 and K3 are represented as follows.

Kinetic energy of the first arm 3 K1 K1 = (1/2) · m1 (1 ² +1 ² +1 ² ) = (1/2) · m1 · d1 ² · A1 ² … (1.5a) kinetic energy K2 K2 = (1/2) · m2 · (2 2 +2 2 +2 2) = (1/2) · m2 · {l1 2 · A1 2 + d2 2 · (A1 + A2) 2 + 2l1 · d2 · COSθA2 · A1 · (A1 + A2)} ... (1.5b) kinetic energy K3 K3 of the third arm 7 = (1/2) · m3 · (3 2 +3 2 +3 2) = m3 · {l1 · l2 · COSθA2 + l1 · d3 · ( COSθA2 · SINθA31-SINθA2 · SINθA32) + l2 · d3 · SINθA31 + (1/2) · [l1 2 + l2 2 + d3 2 · (SIN 2 θA31 + SIN 2 θA32]} · A1 2 + m3 · l2 · d3 · SINθA31 + (m3 / 2)・ {L2 ² + d3 ²・ (SIN ² θA31 + SIN ² θA32)} ・ ・ A2 ² + (m3 / 2) ・ d3 ²・ COS ² θA31 ・ [1 + SIN ² θA31 / (1-SIN ² θA31−SIN ² θA32)] ・^{A31 2 + (m3 / 2)} · d3 2 · COS 2 θA32 · [1 + SIN 2 θA32 / (1-SIN 2 θA31-SIN 2 θA32)] · A32 2 + m3 · {l1 · l2 · COSθA2 + l1 · d3 · ( COSθA2 · SINθA31 + SINθA2 · SINθA32) + 2l2 · d3 · SIN 2 θA31 + l2 2 + d3 2 · (SIN 2 θA31 + SIN 2 θA32)} · A1 · A2 + m3 · d3 · COSθA31 · (l1 · SINθA2-d3 · SINθA32) · A1 · A31 + m3 · d3 · COSθA32 · (l1 · COSθA2 + l2 + d3 · SINθA31) · A1 · A32-m3 · d3 ² · COSθA31 · SINθA32 · A2 · A31 + m3 · d3 · COSθA3
2 · (l2 + d3 · SINθA31 ) · A2 · A32 + (m3 · d3 2 · SINθA31 · COSθA31 · SINθA32 · COSθA32 · A31 · A32) / (1-SIN 2 θA31-SIN 2 θA32) ... (1.5c) (4) From the above relationship, the potential energy P1,
P2 and P3 are represented as follows.

Potential energy of the first arm 3 P1 P1 = 0 (1.6a) Potential energy of the second arm 5 P2 P2 = 0 (1.6b) Potential energy of the third arm P3 P3 = −m3 · d3 · g · ( 1−SIN ² θA31−SIN ² θA32) ^1/2 (1.6c) (5) The total dissipated energy D is expressed as follows.

D = 1/2 · {DA1 · A1 ² + DA2 · A2 ² −DA3 · (A31 ² + A32 ² )} (1.7) The Lagrangian L is expressed by the following equation based on the equation (1.1).

L = K1 + K2 + K3-P1-P2-P3 (1.8) Using this equation, the torques at the arms 3, 5, and 7 are obtained from equation (1.2), and then simplified by the following operation. That is,
θA3i, A3i (i = 1,2) is considering a minute, can be approximated as ^{SINθA3i = θA3i COSθA3i = 1 θA3i 2} = 0 A3i 2 = 0 ... (1.9).

The torque thus simplified is represented by the following equation.

^{τ1 = {m1 · d1 2 +} m2 · l1 2 + m2 · d2 2 + 2m2 · l1 · d2 · CO
^{SθA2 + m3 · l1 2 + m3} · l2 2 + 2m3 · l1 · l2 · COSθA2 + 2m3 · d3 · (l2 + l1 · COSθA2) · θA31 -2m3 · l1 · d3 · θA32 · SINθA2} · A1 + {m2 · d2 2 + m2 · l1 · d2 · COSθA2 + m3 · l2 ² + m3 · l1 · l2 · COSθA2 + m3 · d3 · (2l2 + l1 · COSθA2) · θA31 -m3 · l1 · d3 · θA32 · SINθA2｝ · A2 + m3 · l1 · d3 · SINθA2 · l · A1 · A2 · l3・ A32 − {(m2 ・ l1 ・ d2 + m3 ・ l1 ・ l2) ・ SINθA2 + m3 ・ l1 ・ d3 ・ (θA31 ・ SINθA2 + θA32 ・ COSθA2)} ・ (A2 ² + 2A1 ・ A2) + 2m3 ・ d3 ・ (l1 ・ COSθA2 + l2) A1 · A31 + A2 · A3
1) -2m3 · l1 · d3 · SINθA2 · (A1 · A32 + A2 · A32) + DA1 · A1 ··· (1.10) τ2 = ｛m2 · d2 ² + m2 · l1 · d2 · COSθA2 + m3 · l1 ² · m3 · l1 · l2 · COSθA2 + m3 · d3 · (l1 · COSθA2 + 2l2) · θA31-m3 · l1 · d3 · θA32 · SINθA2} · A1 + (m2 · d2 2 + m3 · l2 2 + 2m3 · l2 · d3 · θA31) · A2 + m3 · l2 · d3 · A32 + {(m2 · l1 · d2 + m3 · l1 · l2) · SIN θA2 + m3 · l1 · d3 · (θA31 · SINθA2 + θA32 · COSθA2) · · A1 ² + 2m3 · l2 · d3 · (A1 · A31 + A2 · A2 · A2 · A2 · A2) 1.11) τ31 = (m3 · l1 · d3 · SIN θA2-m3 · d3 · θA32) · A1-m3 · d3 ² · θA32 · A2 + m3 · d3 ² · A31-m3 · d3 · (l2 + l1 · COSθA2 + d3 · θA31) · A1 ² -m3 · d3 · (l2 + d3 · θA31) · (A2 2 + 2A1 · A2) -2m3 · d3 2 · (A1 · A32 + A2 · A32) + DA3 · A31 + m3 · d3 · g · θA31 ... (1.12) τ32 = m3 · d3 · (L2 + l1 · COS θA2 + d3 · θA31) · A1
+ M3 · d3 · (l2 + d3 · θA31) · A2 + m3 · d3 ² · A32
＋ m3 ・ d3 ・ (l1 ・ SINθA2-d3 ・ θA32) ・ A1 ² −m3 ・ d
3 ² · θA32 · (A2 ² + 2 A1 · A2) + ² m3 · d3 ² · (
A1 · A31 + A2 · A31) + DA3 + A32 + m3 · d3 · g
・ ΘA32… (1.13) Here, based on the equation (1.10)-(1.13), After all, Here, C11 = a33 · (a22 · a44−a24 · a42) / V C12 = (− a12 · a33 · a44 + a13 · a32 · a44 + a14 · a33 · a42) / V C13 = a13 · (a24 · a42−a22 ·) a44) / V C14 = (a12 ・ a24 ・ a33−a14 ・ a22 ・ a33−a13 ・ a24 ・ a32) / V C21 = a33 ・ (a24 ・ a41−a21 ・ a44) / V C22 = (a11 ・ a33 ・ a44) −a13 ・ a31 ・ a44−a14 ・ a33 ・ a41) / V C23 = a13 ・ (a21 ・ a44−a24 ・ a41) / V C24 = (− a11 ・ a24 ・ a33 + a13 ・ a24 ・ a31 + a14 ・ a21 ・ a33) / VC31 = [a31 · (a24 · a42 – a22 · a44) + a32 · (a21 · a44 – a24 · a41)] / VC32 = [-a11 · a33 · a44 + a12 · a31 · a44 – a14 · (a31 · a42- a32 · a41)] / V C33 = [a11 · (a22 · a44 – a24 · a42) + a12 · (a24 · a41 – a21 · a44) + a14 · (a21 · a42 – a22 · a41)] / V C34 = [a11・ A24 ・ a32−a12 ・ a24 ・ a31 + a14 ・ (a22 ・ a31−a21 ・ a32)] / V C41 = a33 ・ (a21 ・ a42−a22 ・ a41) / V C42 = [− a11 ・ a33 ・ a42 + a12 ・ a33 ・a41 + a13 · (a31 · a42−a32 · a41)] / V C43 = a13 · (a22 · a41−a21 · a4 2) / V C44 = [a11 · a22 · a33 – a12 · a21 · a33 + a13 · (a21 · a32 – a22 · a31)] / V where V = a11 · a33 · (a22 · a44 – a24 · a42) · a12・ A33 ・ (a24 ・ a41−a21 ・ a44) + a13 ・ a31 ・ (a24 ・ a42 −a22 ・ a44) + a13 ・ a32 ・ (a21 ・ a44−a24 ・ a41) + a14 ・ a33 ・ (a21 ・ a42−a22 ・ a41 A drive unit including the servomotors 25, 27 of the first and second arms 3, 5 and a drive circuit in the output unit 23e is represented by a block diagram in FIG. In the figure, Ei is each servo motor 2
Voltage applied to 5,27, T1 to T3 are reaction force torques due to springs of arms 3,5,7, IMi is current flowing through each servomotor 25,27, θMi is rotation angle of each servomotor 25,27, ωMi Is the rotational angular velocity of each servo motor 25, 27, θri is the electronic control circuit 2.
The indicated angle (target angle) value from 3, θA1 to θA3 are arms
Actual angles of 3,5,7, RMi is the armature resistance of servomotors 25,27, G1, G2 is the gear ratio, JMi is the inertia of servomotors 25,27, DMi is the viscous resistance of servomotors 25,27, KS1 ~
KS3 is the spring constant of each arm 3,5,7, G2i, K1i, K2i, KEi, KA
i, KVi, KIi, KS1 to KS3, KTi, KPWMi represent predetermined coefficients. However, i = 1,2. The following equation is obtained from this block diagram.

Ei = ｛[G2i · K1i · (θri−θMi) −KVi · ωMi] • K2i−KIi · IMi｝ · KAi · KPWMi… (2.1) Ei−KEi · ωMi = RMi · IMi… (2.2) The following equations are derived from the above equations (2.1) and (2.2).

(RMi + KIi-KAi-KPWMi) -IMi + (KEi + KVi-K2i-KAi + KPWMi)-. Omega.Mi = G2i-K1i-K2i-KAi-KPWMi-(. Theta.ri-.theta.Mi) ...
(2.5) From the above equations (2.3) and (2.4), the following equation is derived.

From the above equations (2.5) and (2.6), the following equation is derived.

The above equation (2.7) is replaced as follows.

M1 = d21 · θM1 + d22 · ωM1 + d25 · θA1 + dr1 · θr1 M2 = d43 · θM2 + d44 · ωM2 + d47 · θA2 + dr2 · θr2 ... (3.
1) From the block diagram of FIG. 6, the torque of each arm 3, 5, 7 is given by τ1 = KS1 · (θM1 / G1-θA1) τ2 = KS2 · (θM2 / G2-θA2) τ31 = -KS3 · θA31 τ32 = −KS3 · θA32 (3.2) From Equations (1.15), (3.1), and (3.2), the nonlinear equation of motion is expressed as follows.

Where d61 = KS1 / G1 · C11 d63 = KS2 / G2 · C12 d65 = −KS1 · C11 d66 = −DA1 · C11 d67 = −KS2 · C12 d68 = −DA2 · C12 d69 = −KS31 · C13 d6A = − DA3 ・ C13 d6B = −KS32 ・ C14 d6C = −DA3 ・ C14 d81 = KS1 / G1 ・ C21 d83 = KS2 / G2 ・ C22 d85 = −KS1 ・ C21 d86 = −DA1 ・ C21 d87 = −KS2 ・ C22 d88 = − DA2 ・ C22 d89 = −KS31 ・ C23 d8A = −DA3 ・ C23 d8B = −KS32 ・ C24 d8C = −DA3 ・ C24 dA1 = KS1 / G1 ・ C31 dA3 = KS2 / G2 ・ C32 dA5 = −KS1 ・ C31 dA6 = − DA1, C31 dA7 =-KS2, C32 dA8 =-DA2, C32 dA9 =-KS31, C33 dAA =-DA3, C33 dAB =-KS32, C34 dAC =-DA3, C34 dC1 = KS1 / G1, C41 dC3 = KS2 / G2 / C42 dC5 = -KS1 / C41 dC6 = -DA1 / C41 dC7 = -KS2 / C42 dC8 = -DA2 / C42 dC9 = -KS31 / C43 dCA = -DA3 / C43 dCB = -KS32 / C44 dCC = -DA3 / C44.

here, The above equation (3.3) can be approximated as follows: = Aχ + Bu (3.4)

The elements d21 to dCC of the matrix A are KS1 to KS as described above.
S32, G1, G2, C11 to C44, DA1 to DA3, where KS1 to KS32 are spring constants, G1 is the gear ratio between the first arm 3 and its servomotor 25, and G2 is the gear ratio. The gear ratio between the two arms 5 and its servomotor 27, C1
1 to C44 are functions composed of a11 to a44 and having θA1 to θA32 as parameters as can be seen from the relationship between the expressions (1.10) to (1.13) and (1.14). Further, DA1 to DA3 are the arms 3,5. ,
7 viscous drag.

Here, if the vibration angles θA31 and θA32 are small and considered to be 0, A can be considered as a function of only θA1 and θA2 as in the following equation.

A = f (θA1, θA2) Therefore, the matrix A can be considered to be a constant value for each of the angle θA1 of the first arm 3 and the angle θA2 of the second arm 5, and the above equation of motion (= Aχ · Bu) It can be considered linear for each angle, and modern control theory can be applied for each angle. The values of θA1 and θA2 can be obtained by multiplying the output values of encoders 17 and 19 by the gear ratio, assuming that arms 3, 5, and 7 are in a stationary state.

In a general modern control theory, a control target as shown in FIG. 7 is represented by a linear equation as follows: = Aχ + Buy = Cχ. Here, the feedback gain F and the observer gain K are determined by the pole assignment method so that the desired control result is obtained, and the coefficients P, Q, and R of the following equation are obtained by discretization with hold. Observer 50 as shown in FIG.
Can be designed.

P = e ^{(A-KC) T} Here, T is a sampling time (for example, 1 msec), and τ is a variable for integration. Since the above equation (3.4) is of the same type, modern control theory can be applied and a similar observer can be designed.

However, And

When the process of the observer in FIG. 8 is represented by a calculation formula, it is as follows.

Z (n + 1) = P · Z (n) + Q · u (n) + R · y
(N) u (n) = u (n) + F · Z (n) This observer 50 is a state variable θA of a series of arms 3, 5, and 7.
1, θA2, θA31, θA32 can be estimated, and from these values, arms 3, 5,
The tip of the third arm 7 can be controlled to move to a desired position in consideration of the vibration of the seventh arm.

FIG. 9 shows a flowchart in which the processing of the observer 50 is realized by the electronic control circuit 23.

First, an initial value of Z (n) is given (step 10).
0). Next, the previous command value u (n) and y (n), that is, the output values of the encoders 17, 19, 21 are fetched (step 11).
0). Next, the matrix A is calculated (step 112). That is, A is calculated using the output values of the encoders 17 and 19 as θA1 and θA2 when the arms 3 and 5 are at rest, and using the above-described various constants and the like. Next, P, Q, and R are calculated from the above equations (step 114).

Next, the previously calculated Z (n) and the above u (n), y
(N) and Z (n
1) is obtained according to the above equation (step 120). Next, a new command value u (n) is obtained from the previously obtained Z (n) according to the above equation (step 130). Next, the new command value u (n) is output to the servo motors 25, 27, 29 (step 140).

Next, it is determined whether the processing termination condition is satisfied (step 150). The terminating condition refers to a state in which the command has been completed, for example, a state in which a series of operations such as welding has been completed.

If continued, a negative determination is made in step 150,
The processing of step 110 to step 150 is repeated again, and the third
The tip of the arm 7 is accurately moved to a predetermined position. This repetition is performed at a cycle of the sampling time T (for example, 1 msec), and the encoder 1 of the measured y (n) is
Assuming that A is obtained based on the formula A = f (θA1, θA2) as θA1, θA2 in the stationary state of the arms 3, 5 as described above, A Will be realized.

If an affirmative determination is made in step 150, the process proceeds to another process, or the process ends.

As described above, the articulated arm control device according to the present embodiment creates a mathematical model of the articulated arm in advance, obtains a linear equation of motion if the angle parameter is constant, and calculates various types of observers as observers. Designing arithmetic expressions. Therefore, in actual control, the modern control theory can be applied to each angle of the first and second arms 3, 5 to prevent the arms 3, 5, 7 from vibrating as a spring system, and to accurately set the third arm. 7 can be controlled to a predetermined position, and a robot or the like can perform highly accurate work.

In the above embodiment, the servo motors 25, 27, 29 correspond to the actuator M2, the encoders 17, 19, 21 correspond to the position detecting means M3, and the electronic control circuit 23 corresponds to the control means M4.

The articulated arm control device of the present invention can apply the modern control theory by treating the non-linear mathematical model M5 as linear for each angle of the arm M1, and can apply the actuator drive command value in consideration of the vibration of the arm M1. Can be compensated for.

Therefore, even if the arm M1 attempts to vibrate as a spring system, the vibration can be prevented, the tip of the arm M1 can be accurately controlled to a predetermined position, and a robot or the like can perform highly accurate work.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating a basic configuration of the present invention, FIG. 2 is a schematic perspective view showing an arm portion of one embodiment of the present invention, FIG. 3 is a block diagram of an electronic control unit thereof, FIG. 5 and 6 are explanatory diagrams of the operation of each arm, FIG. 6 is a block diagram of a servo motor driving unit, FIG. 7 is a block diagram of a generalized control target, and FIG. 8 is a block diagram of the present embodiment. FIG. 9 is a flowchart of a process performed by the electronic control circuit. M1… Arm, M2… Actuator M3… Position detecting means, M4… Control means M5… Formula model 3,5,7… Arm 17,19,21… Encoder 23… Electronic control circuit 25, 27,29 …… Servo motor

 ──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-63-46521 (JP, A) JP-A-58-3001 (JP, A) JP-A-60-69712 (JP, A) JP-A-62 160502 (JP, A) JP-A-60-231205 (JP, A) JP-A-61-39110 (JP, A) JP-A-63-92903 (JP, U)

Claims (1)

(57) [Claims] 1. A plurality of arms arranged in series, an actuator for operating each arm, position detecting means for detecting a position of the actuator, and control means for controlling the actuator to cause the arm to perform a predetermined operation. Wherein the control means is a mathematical model set on the assumption that a system constituted by the actuator and the arm is a system composed of a spring and a mass, and wherein the actuator drive command By using a non-linear mathematical model for estimating at least the actuator displacement or arm displacement from the values and treating the mathematical model as linear for each displacement of the actuator, feedback compensation is performed according to the actuator drive command value and the actual value of the actuator displacement. Ask for the quantity and this feedback Articulated arm control apparatus characterized by compensating for the actuator drive command value at 償量.