JP2502312B2 - Command value calculation method in robot and object position control device - Google Patents

Description

TECHNICAL FIELD The present invention relates to a method for calculating a command value in a robot and an object position control device.

BACKGROUND ART When a robot is operated at high speed, the tracking error of the servo system becomes large and the locus accuracy deteriorates. This is because each joint of the robot is subjected to centrifugal force, Coriolis force, gravitational force, frictional force, etc., in addition to the mutual interference force of the links. A control method in which these forces are calculated by a mathematical model and feedforward compensation is generally called dynamic control. This control scheme can have a significant effect on improving the dynamic characteristics of the system if the mathematical model completely represents the system.

The prior art of the control method for such feedforward compensation will be described with reference to FIG. The position command value of the working end of the robot is stored in the memory, and the position command value stored in the memory is stored at every sampling time interval (that is, sampling period) ΔT that is predetermined by using a processing circuit such as a microcomputer. Read the value θa ^* . A part of the read command value θa is as shown in FIG. 10 (1). This value θa
The first-order differential value a, which is the difference between the two, is obtained as shown in FIG. Based on the first-order differential value a, the second-order differential value a is obtained as shown in FIG. 10 (3). In the same way, the third derivative Is calculated as shown in Fig. 10 (4). In the same manner, up to the h-th derivative can be obtained, but here, up to the third-derivative will be described. The read command value θa and the first to third differential values a to The time of the phase center of is indicated by reference marks t0 to t3. Two
The value of the differential value a at time t2 is zero.

Problems to be Solved by the Invention In such a prior art, the phase center is changed every time the difference is calculated once. Each one is delayed. Therefore, the read command value θa and the differential value a to each floor based on it When using the value at the same time of, for example, t3, an error occurs and a good response (that is, not responsiveness,
It is not possible to achieve the dynamic characteristics evaluated by overshift, residual vibration, etc.

It is an object of the present invention to read a command value stored in a memory and calculate the command value in a robot by eliminating an error due to a shift of a phase center and improving a dynamic characteristic evaluated by an overshift, a residual vibration and the like. A value calculation method and an object position control device are provided.

Means for Solving Problems The present invention is directed to a position command value θa stored in a memory.
Is read at a predetermined sampling time interval ΔT, the differential value a, a, ... Up to the h-th level (h is an integer of 1 or more) of the read value θa is calculated to obtain the read value θa, time Delay, and the differential value of the e floor (e is an integer of 1 or more and (h-1) or less) , And the delayed read value θa, the delayed differential values a, a, ... up to the h-1th floor, and the differential value of the hth floor are used for command value calculation. It is a command value calculation method in a featured robot.

The present invention also relates to the position command value stored in the memory. Is read at a predetermined sampling time interval ΔT, the differential value a, a, ... Up to the h-th level (h is an integer of 1 or more) of the read value θa is calculated, and the read value θa and Differential value a to the (h-1) th floor,
a, ... are interpolated in the sampling time interval ΔT, and these read θa are Delay only by e, the differential value of the e floor (e is an integer of 1 or more and (h-2) or less) The delayed read value θa, the delayed differential value up to the (h-2) th floor, the delayed differential value of the (h-1) th floor, and the differential value of the hth floor are used as command values. A method for calculating a command value in a robot characterized by being used for calculation.

The present invention also provides an arm 7 of an articulated robot having n degrees of freedom.
Is driven by the electric motor 1 via the speed reducer 5, and the position command value stored in the memory is At a predetermined sampling time interval ΔT, and the first and second differential values a, of the read value θa,
a is calculated, and the read value θa is delayed by the time ΔT, the first-order differential value a is delayed by the time ΔT / 2, and these delayed read value θa and the delayed first value Calculate the motor output torque command value (Nm) r using the differential value a and the differential value a of the second floor that has not been delayed, Where Jm is the moment of inertia of the motor, RG is the reduction ratio of the speed reducer 5, Dm is the coefficient of viscous friction on the motor side, and f (θ)
Is Where Jj is the component of the inertia matrix, a is the arm rotation angular acceleration, bjk is the coefficient of the centrifugal force / Coriolis force term, and a
Is an arm rotation angular velocity, Da is an arm-side viscous friction coefficient, fg is a gravitational term, m is a motor rotation angular velocity, and m is a motor rotation angular acceleration, a command value calculation method in a robot.

Further, according to the present invention, (a) the rotation speed detector 4 and the rotation angle detector 3 are coupled to the shaft of the electric motor 1 and the reduction gear 5 is coupled, and the output shaft of the reduction gear 5 includes the torque transmission mechanism 6. An industrial robot having a structure for driving the arm 7 via the arm, and (b) a command for generating an acceleration command value (a) r, a speed command value (a) r, and a position command value (θa) r for the arm 7. Value generating means 23, 25, (b1) Position command value of arm 7 Memory 23 that stores the value, and (b2) Store position command value stored in memory 23 With a sampling time interval ΔT set in advance, and (b3) the first and second floors of the value θa read from the reading means.
And (b4) the read value θa is delayed by the time ΔT in response to the outputs of the reading means and the calculating means, and the position command value (θ) is calculated.
a) r, the differential value a of the first floor is delayed by a time ΔT / 2 to be the speed command value (a) r, and the differential value a of the second floor which is not delayed is derived as the acceleration command value (a) r. Command value generating means 23, 25 having: and (c) computing section 15 from the acceleration (a) r, speed command value (a) r and position command value (θa) r from the generating means, Motor output torque command value (Nm) r
And a calculation unit 15 that calculates the sum of the command value (m) r of the motor rotation angular velocity and the command value (θm) r of the motor rotation speed, and (d) the command value (θm) of the rotation angle. a first subtractor 19 for subtracting the output of the motor rotational displacement θm detected by the rotation angle detector 3 from r, (e) a second subtractor 18 for deriving a motor current command value u, and (f) the motor Assuming that the shaft system composed of the rotor of No. 1, the reduction gear 5, the torque transmission mechanism 6 and the arm 7 is a system composed of a spring and a rigid body, the motor current command value u, the motor rotation displacement θm, and the motor rotation speed m. From the estimated value θs of the twist angle of the system composed of the spring and the rigid body,
A state observer 11 for obtaining an estimated value s of the time derivative of the twist angle, and (g) adders 16 and 17 for adding the estimated value θs of the twist angle and the estimated value s of the time derivative. (H) the second subtractor 18 outputs the output for the first subtractor 19,
A command value (Nm) r of the motor output torque from the calculation unit 15,
The sum of the motor rotation angular velocity command value (m) r is added, and the output m of the rotation speed detector 4 and the outputs of the adders 16 and 17 are subtracted from the added value to obtain the motor current command value u.
Is derived and given to the electric motor 1.

Operation According to the present invention, the command value θa ^{* of the} memory is read and the value θa is read. Delay, and based on the read value θa, the e-th derivative values from the first floor to the h-1th floor Since the calculation is performed after each delay, it is possible to improve the position accuracy of the arm by setting the error caused by the phase difference to zero.

According to the invention, the value read from the memory and h-1
The differential value up to the floor is interpolated in the sampling cycle ΔT, and the read value θa Delay, and the differential value up to the h-2 floor is However, the differential values of the (h-1) th floor and the hth floor are used for calculation without delay. As a result, the read value and the differential value of each floor can be used for the calculation without the phase being shifted, so that it is possible to improve the position accuracy of the arm without causing an error.

Further, according to the object position control device of the present invention, it is assumed that the system is composed of a spring and a rigid body, and in the calculation unit 15, the command value (Nm) r of the motor output torque and the command value of the motor rotation angular velocity (
m) r and the command value (θm) r of the motor rotation speed are calculated, and the sum is given to the second subtractor 18, thus performing feedforward control in reverse phase at the torque level. Since this is performed, the position control of the arm 7 can be performed without a time delay with respect to each of the command values.

Also in this object position control device, the position command value of the arm 7 stored in the memory 23 Based on the above, the differential values a and a of the first and second orders are calculated and obtained, and the centers of their phases are made to coincide with each other, so that good dynamic characteristics can be achieved.

Embodiment FIG. 1 is a block diagram of an industrial robot controller according to an embodiment of the present invention. The robot mechanical system 21 is controlled by feedback compensation based on the arm position command value stored in the memory 23.

FIG. 2 is a perspective view showing a simplified structure of the mechanical system 21 of the robot. The electric motor 1 is driven by the current command value u of the drive circuit 2, and its rotor or output shaft 1a is attached to the speed reducer 5. The output shaft of the speed reducer 5 is connected to the arm 7 of the robot through the torque transmission mechanism 6. Second
The configuration shown in the figure is hereinafter referred to as a controlled object.

FIG. 3 is a perspective view showing a mechanically equivalent structure of the structure of FIG. Focusing on the spring characteristics of the speed reducer 5, the torque transmission mechanism 6, and the arm 7, the control targets in FIG. 2 are the spring 9 coupled to the output shaft 1a of the motor 1 and the spring 9
It is equivalent to a configuration including a rigid arm 10 connected to the tip of the. When each axis of the robot is regarded as a spring-rigid system as shown in FIG. 2, the motion of the i-th arm of an articulated robot with n degrees of freedom is described by the following differential equation.

The meanings of the symbols in the above-mentioned first and second expressions are as follows. However, in the following description, the subscript i, which means the i-th arm, will be omitted.

θm; Motor rotation angle m; Motor rotation angular velocity m; Motor rotation angular acceleration θa; Arm rotation angle a; Arm rotation angular velocity a; Arm rotation angular acceleration Kc; Spring constant RG; Reduction ratio Dm; Motor side viscous friction coefficient Da; Arm Side viscous friction coefficient Jm; motor inertia moment Nm; motor output torque Jj; inertia matrix component bjk; coefficient of centrifugal force / Coriolis force term fg; gravity term To obtain the torque required for dynamic control, Motor output torque command value (Nm) from acceleration command value (a) r, speed command value (a) r, and position command value (θa) r
It suffices to calculate r. The subscript r represents a command value. If the right side of the first equation is f (θ), then from the first equation and the second equation, Is established. If the command value is given to each term of f (θ),
Since this is known by substituting this into θa, a, a, the rotation angle command value, rotation angular velocity command value, and rotation angular acceleration command value (θm) r, (m) r, (m) r of the electric motor 1 are then obtained. Need to ask. This is obtained by differentiating the first expression.

(Θm) r = (f (θ) / Kc + (θa) r) RG (4) (m) r = (f () / Kc + (a) r) RG (5) (m) r = (f () / Kc + (a) r) RG (6) The torque command value is obtained by substituting the fourth, fifth and sixth equations into the third equation.

This torque command value is multiplied by the rotational angular velocity command value of the electric motor and the speed feedback back gain, and then the rotational angle deviation of the electric motor and an appropriate proportional gain are added, and this is given as the electric current command value of the electric motor. Thus, the present invention can be realized.

Further, in another embodiment of the present invention, a state observer 11 is used as shown in FIG. Since the loop is configured, it is necessary to calculate the command value of the distortion and its derivative and feed it forward. The state observer 11 uses the current command value u and the speed measurement value m of the electric motor 1 to measure the distortion amount θs and the distortion amount θ.
It can be realized by using a minimum order observer that estimates the time change rate s of s. If the strain amount is θs, Therefore, by modifying the first equation, (θs) r = f (θ) / Kc (8) is obtained. Further, this is differentiated to obtain (s) r = (θ) / Kc (9). From the above, the torque command value required for dynamic control and
The total feedforward amount (NmF) r, which is the sum of the command values for each feedback amount of speed, strain and its derivative, is the first
It is given by the expression 0.

(Nmf) r = (Nm) r + Kv (m) r + Kps (θs) r + Kvs (s) r = f (θ) / RG + JmRG (a) r + (Kv + Dm) RG (a) r + [JmRG (θ) + (( Kv + Dm) RG + Kvs) (θ) + Kpsf (θ)] / Kc (10) where Kv, Kps, and Kvs are the feedback gains of the velocity, strain, and strain derivative in the coefficient circuits 12, 13, and 14, respectively. Is. In the calculation unit 15, (θa) r, (a) r, (
a) r, When, is given, the values obtained by substituting these into the equation 10 are added to the drive circuit input of the feedback control system using the state observer 11 to configure the control system of the present invention. You can

Is so small that it can be ignored. A block diagram of this control system is shown in FIG. 1 as described above. u is a drive circuit input, and Kp is a position loop gain. Reference marks 16,17,
Reference numerals 20 and 18 are arithmetic units, reference numeral 20 is a coefficient unit having a gain Kp, and a mechanism 21 is a machine including the electric motor 1, the speed reducer 5, the torque transmission mechanism 6 and the arm 7 shown in FIG. Shows the physical composition. In FIG. 3, the feedforward amount (Nmf) r is expressed by the eleventh equation.

(Nmf) r = (Nm) r + Kv (m) r + Kps (θs) r + Kvs (s) r (11) Note that in Expression 10, the value is small and has little effect on the whole.
It is also possible to ignore Dm etc. and simplify the calculation.

Further, here, respective command values (θm) r, (m) r, (m) r of the rotation angle, the rotation angular velocity and the rotation angular acceleration of the electric motor.
Was calculated, but θm, which can be measured by the detector,
By using the actual value of m, the torque command value required for dynamic control can be calculated online more accurately.

Referring to FIG. 4, position command value θa ^* of arm 10 is stored in memory 23. In the following description, the subscript r, which means a command value, is omitted to simplify the description. Command value θ stored in this memory 23
The a ^* is read by the processing circuit 22 at every sampling time interval ΔT as shown in FIG. The read value θa is calculated in the processing circuit 22, and the calculation result is temporarily stored in the memory 24. The processing circuit 22 and the memory 24 form a difference calculation circuit 25.

FIG. 5 is a flow chart for explaining the operation of the difference calculation circuit 25, and FIG. 6 is a waveform chart for explaining the calculation operation. With reference to these drawings, step
At v1, the command value θa ^* stored as described above is read from the memory 23. At step v2, the differential values a, a, ... Up to the h-th order (h is an integer of 1 or more, h ≧ 1) are calculated based on the read value θa. In this embodiment, the third derivative Are calculated and obtained. The position command value θa ^* stored in the memory 23 is shown in FIG. 6 (1), and the value to be read by the p-th sampling is indicated by θap. The value θa read from this memory 23 is the sixth
This is as shown in FIG. The first-order differential value a is
As shown in FIG. 6 (4), the second-order differential value a obtained based on this is as shown in FIG. 6 (6), and the third-order differential value. Is as shown in FIG. 6 (8). Reference marks t5 to t1
Each time indicated by 2 is a ΔT / 2 interval. Third derivative The phase center of is at time t8, and in step v3 this third derivative Is output to the calculation unit 15.

At step v4, the second-order differential value a obtained by the calculation shown in FIG. 6 (6) is delayed by the time ΔT / 2. So, in step v5, the second derivative a1 delayed by ΔT / 2
Is output as shown in FIG. 6 (7).

At step v6, the first-order differential value a shown in FIG. 6 (4) obtained by the calculation is further delayed by ΔT / 2. In other words, the first-order differential value a is the third-order differential value The value a1 at the time t6, which is earlier than the phase center p8 by the time ΔT, is output at step v7 as shown in FIG. 6 (5).

At step v8, the read command value θa is further
Delay T / 2. As a result, the value θa is greater than that at time t5. The value .theta.a1 at the time t8 delayed by only is output at step v9 as shown in FIG. 6 (3).

Thus, at time t8, the values θa, a and a
But the value At time t8 when the value of the phase center of
The operation is performed by substituting it into the expression. As a result, a calculation error due to the phase difference can be prevented and a good response can be achieved.

Generally speaking, after the differential value up to the hth order is obtained, the value θa read from the memory 23 is Just delay. In addition, each differential value from the 1st floor to the h-1th floor is Just delay. Here, l is an integer of 1 or more and h−1 or less, and 1 ≦ l ≦ h−1 (12). In this way, an error due to the phase difference can be prevented.

FIG. 7 shows a difference calculation circuit 25 in another embodiment of the present invention.
FIG. 8 is a flow chart for explaining the operation of FIG. 7, and FIG. 8 is a waveform chart for explaining the operation of the embodiment shown in FIG. At step w1, the position command value θa ^* of the arm 10 stored in the memory 23 is read. The command value θa ^* stored in this memory is as shown in FIG. 8 (1). Sampling time interval ΔT
Is equally divided into q with a cycle smaller than that, and the following calculation is performed. The value θa read from the memory 23 by the processing circuit 22 is as shown in FIG. 8 (2), and the value θa2 as shown in FIG. 8 (3) is calculated by interpolation.
Ask for. The sampling time interval ΔT is, for example, several tens of ms
ec, and the time interval divided into p equals 1 msec, for example.
It may be about. The interpolated value θa2 is as shown in Expression 13.

p represents the p-th sampling. u indicates the u-th interpolation calculation value in the time interval p equally divided in one sampling time interval ΔT.

Based on the value θa, the first-order differential value a is calculated as shown in FIG. 8 (5). Similarly, the second-order differential value a is calculated based on the first-order differential value a to obtain the value as shown in FIG. 8 (8). Third derivative Is calculated based on the second-order differential value a as shown in FIG. 8 (10). Times t15 to t21 indicate times shifted from each other by ΔT / 2.

By interpolating the first-order differential value a, a2 shown in FIG. 8 (6) is obtained. At times t15 to t17, the u-th interpolation value a2up is expressed by the fourteenth equation.

The u-th interpolation value a2 from time t17 to time t19
u (p + 1) is as shown in Expression 15.

Value a2up, a obtained by interpolating the second-order differential value a
2u (p + 1) and a2u (p + 2) are calculated as follows. First, the interpolation calculation value at times t15 to t17 shown in FIG.

In addition, the interpolation value of the second derivative from time t17 to t19 is the 17th
It is as in the formula.

The interpolated value of the second-order differential value a at times t19 to t21 is as shown in the 18th equation.

Thus, in step w2 in FIG. 7 described above, based on the value θa read from the memory 23, the first-order to third-order differential values a ... Is calculated. At step w3, at time t18, the eighth
Third-order differential value shown in Fig. (10) Is output.

At step w4, the interpolation calculation value a2 shown in FIG. 8 (9) of the second-order differential value θa is calculated and the value at time t18 is output.

In step w5, the interpolation calculation value a of the first-order differential value a shown in FIG. 8 (6) is delayed by ΔT / 2. The delayed second-order differential interpolation value a is as shown in FIG. 8 (7), and the value at time t18 is output at step w6.

At step w7, the interpolated value θa2 of the read value θa is further delayed by ΔT / 2 to obtain the value θa3 shown in FIG. 8 (4).

At step w8, the value θa3 shown in FIG.
The value of time t18 of is derived. Thus, at time t18, the read value θa and the differential values a and a of the first and second floors are interpolated, the interpolated value θa2 is delayed by the time ΔT, and the interpolated operation value a of the first differential is Delay by ΔT / 2, thus obtaining the values θa3, a3. As a result, time t18
At θt3, a3 together with the second-order differential interpolation value a2 and the third-order differential value a3, the first command value at time t18
The command value shown in Equation 0 is calculated.

Generally speaking, the command value θa ^* stored in the memory 23 is calculated by calculating the differential value of the read value θa up to the (h-1) th floor. h is an integer of 1 or more, and h ≧ 1. The read value θa and the differential value up to the (h−1) th floor are interpolated within the sampling period ΔT. Read value θ
Interpolation calculation value θa2 of a Just delay. The value a obtained by performing the interpolation calculation of the differential values of the first floor to (h-2) th floor is the time for each interpolated value of the first floor differential. Only delay each. l is an integer of 1 or more and (h-2) or less (1≤lh-2). In order to calculate the delayed interpolated value θa3 of the read value, the delayed interpolated value a3 up to the (h-2) th floor, the delayed interpolated value a2 of the (h-1) th floor, and the differential value a3 of the hth floor. Used for. In this way, it is possible to prevent an error due to a phase shift and achieve good responsiveness.

 You may make it interpolate the differential value of h-order.

Furthermore, if the parameters to be controlled are sufficiently accurately identified, as shown in FIG. 9 showing still another embodiment, even if the state observer 11 is not used as a servo compensator, the conventional one as shown in FIG. Only the feed back of θm, m is required. At this time, the feedback loop of the amount of strain due to the spring and its differential value θs, s is cut off, and these command values Kps (θ
s) r + Kvs (s) r may be set to zero, and the feedforward amount (Nmf) r may be set as in Expression 19.

(Nmf) r = (Nm) r + Kv (m) r (19) Effect As described above, according to the present invention, the command value stored in the memory is read and the differential value thereof is calculated to obtain the command value. When performing the calculation, it is possible to eliminate the error caused by the phase difference, and it is possible to improve the position accuracy of the arm.

That is, according to the present invention, since it is possible to match the phase centers of the command values and their differential values,
The dynamic characteristics evaluated by overshift, residual vibration, etc. can be improved.

Further, according to the object position control device of the present invention, it is possible to suppress vibration at the time of acceleration / deceleration due to a low rigidity part such as a power transmission part of an industrial robot, and yet realize a response with excellent followability. can do.

In particular, according to the present invention, the calculation unit 15 calculates the command value (N) of the motor output torque from the acceleration command value (a) r, the speed command value (a) r, and the position command value (θa) r of the arm 7.
m) r and the command value (m) r of the motor rotation angular velocity are calculated and given to the second subtractor 18 and added in anti-phase at the torque level to control the position of the arm 7 by the command value. It becomes possible to drive without time delay.

Furthermore, according to the present invention, the state observer 11 and the adder 1
Using 6,17, feedback control on speed is performed,
In this way, excellent control performance can be realized.

Moreover, it is also possible to simplify the configuration by estimating the state quantity, which cannot be directly measured by the sensor, such as the arm rotation displacement and the arm rotation speed, and obtaining the state quantity.

[Brief description of drawings]

FIG. 1 is a block diagram showing a controller for an industrial robot according to an embodiment of the present invention, and FIG. 2 is a perspective view showing a simplified configuration of a robot mechanical system 21 shown in FIG. 2 is a perspective view showing the structure shown in FIG. 2 as a model of a spring-inertia system, FIG. 4 is a view for explaining a sampling cycle .DELTA.T for reading from the memory 23, and FIG. Flowchart for explaining the operation of 25, No. 6
FIG. 7 is a waveform chart for explaining the operation of the difference calculation circuit 25, FIG. 7 is a flow chart for explaining the operation of the difference calculation circuit 25 of another embodiment of the present invention, and FIG. 8 is shown in FIG. FIG. 9 is a waveform diagram for explaining the operation in the shown embodiment, FIG. 9 is a block diagram of still another embodiment of the present invention, and FIG. 10 is a waveform diagram for explaining the arithmetic method of the prior art. 1 ... Electric motor, 2 ... Drive circuit, 5 ... Reduction gear, 6 ...
Torque transmission mechanism, 7 ... arm, 11 ... condition dryer, 1
2,13,14,20 …… Coefficient unit, 15 …… Calculator, 16,17,18,19…
… Calculator, 22 …… Processing circuit, 23,24 …… Memory, 25 ……
Difference calculation circuit

 ─────────────────────────────────────────────────── ─── Continuation of front page (72) Inventor Sadao Kubo 1-1 Kawasaki-cho, Akashi-shi Kawasaki Heavy Industries Ltd. Akashi Factory

Claims (4)

(57) [Claims] 1. A position command value stored in a memory Is read at a predetermined sampling time interval ΔT, the differential value a, a, ... Up to the h-th level (h is an integer of 1 or more) of the read value θa is calculated to obtain the read value θa, time Delay, and the differential value of the e floor (e is an integer of 1 or more and (h-1) or less) , And the delayed read value θa, the delayed differential values a, a, ... up to the h-1th floor, and the differential value of the hth floor are used for command value calculation. A command value calculation method for a characteristic robot. 2. A position command value stored in a memory Is read at a predetermined sampling time interval ΔT, the differential value a, a, ... Up to the h-th level (h is an integer of 1 or more) of the read value θa is calculated, and the read value θa and Derivatives up to the (h-1) th floor a, a,
... are interpolated in the sampling time interval ΔT, and these read θa are Delay only by e, the differential value of the e floor (e is an integer of 1 or more and (h-2) or less) The delayed read value θa, the delayed differential value up to the (h-2) th floor, the delayed differential value of the (h-1) th floor, and the differential value of the hth floor are used as command values. A method for calculating a command value in a robot, which is used for calculation. 3. An arm 7 of an articulated robot having n degrees of freedom,
The position command value stored in the memory is driven by the electric motor 1 via the speed reducer 5. Is read at a predetermined sampling time interval ΔT, and the first and second differential values a, a of the read value θa are read.
The read value θa is delayed by the time ΔT, the first-order differential value a is delayed by the time ΔT / 2, and the delayed read value θa and the delayed first-order differential are calculated. Value a and second-order differential value a without delay
And are used to calculate the motor output torque command value (Nm) r, Where Jm is the moment of inertia of the motor, RG is the reduction ratio of the speed reducer 5, Dm is the viscous friction coefficient on the motor side, and f (θ) is Where Jj is the component of the inertia matrix, a is the arm rotation angular acceleration, bjk is the coefficient of the centrifugal force / Coriolis force term, and a
Is an arm rotation angular velocity, Da is an arm-side viscous friction coefficient, fg is a gravitational term, m is a motor rotation angular velocity, and m is a motor rotation angular acceleration, a command value calculation method in a robot. 4. (a) The rotation speed detector 4 is attached to the shaft of the electric motor 1.
An industrial robot having a structure in which the rotation angle detector 3 and the reduction gear 5 are coupled, and the output shaft of the reduction gear 5 drives the arm 7 via the torque transmission mechanism 6; ) Command value generating means 23, 25 for generating an acceleration command value (a) r, a speed command value (a) r and a position command value (θa) r for the arm 7, and (b1) a position command value for the arm 7 Memory 23 that stores the value, and (b2) Store position command value stored in memory 23 With a sampling time interval ΔT set in advance, and (b3) the first and second floors of the value θa read from the reading means.
And (b4) the read value θa is delayed by the time ΔT in response to the outputs of the reading means and the calculating means, and the position command value (θ) is calculated.
a) r, a command having means for deriving the differential value a of the first floor as the speed command value (a) r by delaying it by the time ΔT / 2, and deriving the differential value a of the second floor which is not delayed as the acceleration command value. The value generating means 23, 25 and (c) the calculating section 15 calculate the motor output torque from the acceleration (a) r, the speed command value (a) r and the position command value (θa) r from the generating means. Command value (Nm) r
And a calculation unit 15 that calculates the sum of the command value (m) r of the motor rotation angular velocity and the command value (θm) r of the motor rotation speed, and (d) the command value (θm) of the rotation angle. a first subtractor 19 for subtracting the output of the motor rotational displacement θm detected by the rotation angle detector 3 from r, (e) a second subtractor 18 for deriving a motor current command value u, and (f) the motor Assuming that the shaft system composed of the rotor of No. 1, the reduction gear 5, the torque transmission mechanism 6 and the arm 7 is a system composed of a spring and a rigid body, the motor current command value u, the motor rotation displacement θm, and the motor rotation speed m. From the estimated value θs of the twist angle of the system composed of the spring and the rigid body,
A state observer 11 for obtaining an estimated value s of the time derivative of the twist angle, and (g) adders 16 and 17 for adding the estimated value θs of the twist angle and the estimated value s of the time derivative. (H) the second subtractor 18 outputs the output for the first subtractor 19,
A command value (Nm) r of the motor output torque from the calculation unit 15,
The sum of the motor rotation angular velocity command value (m) r is added, and the output m of the rotation speed detector 4 and the outputs of the adders 16 and 17 are subtracted from the added value to obtain the motor current command value u.
Is derived and applied to the electric motor 1.