JP2000099105A - Control method of load machine - Google Patents

Abstract

(57) [Problem] Conventionally, an observer such as a robot is configured for each axis in consideration of a load on a CPU and a simplification of an arithmetic expression, and an external force is ignored. The present invention provides a method for controlling a load machine which overcomes all of these problems. SOLUTION: In a method of controlling and driving a load machine 40 by an electric motor 20, a motor 20 to be controlled and a load machine are controlled.
The state of 40 and the disturbance dis added to the control target are estimated, and the operation equation of the observer 50 is added to the operation equation of the known disturbance added to the control target to calculate.If the load machine is the robot 40, the calculation is added to the control target. Operation value of known disturbance di
In s cal, gravity, interference force, centrifugal force,
The operation of Coriolis force, frictional force, etc. is performed, and the state of disturbance dis added to the electric motor 20 or the load machine 40 is estimated by the observer 50.If the value exceeds a preset threshold value, the robot 40 is abnormal. A situation as a collision is detected.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

The present invention relates to a control method for driving a load machine via an electric motor such as a machine tool or an industrial robot.

[0002]

2. Description of the Related Art Conventionally, when a load machine is driven via an electric motor, such as a machine tool or an industrial robot, a spring element interposed between the two may cause a problem. For example, a robot has low rigidity due to a spring element of a speed reducer, and the robot arm vibrates at a natural frequency obtained from the spring element and the load-side inertia during acceleration / deceleration. This vibration causes a deterioration of the motion trajectory accuracy of the robot, or leads to a loss of tact-time if the transition to the next process is not performed smoothly due to residual vibration. Conventionally, in order to solve this problem, a method of estimating the vibration state of the robot arm by software using an observer and multiplying the obtained estimated value by a gain and feeding back the obtained value is used to suppress the arm vibration. . In addition, a disturbance observer that estimates not only the state of the controlled object but also the disturbance applied to the controlled object has been proposed [Research Institute of Electrical Engineers of Japan, Technical Committee of Industrial Electricity Application (1EA-89-10)]. In this disturbance observer,
There has also been proposed a method of detecting a collision of a load machine coupled to the electric motor based on the estimated disturbance torque and performing a retreat operation to reduce the damage of the collision. Furthermore, the method for detecting a collision based on an estimated disturbance disclosed in Japanese Patent No. 2749724 discloses a method of detecting a collision of a servomotor by an estimated disturbance, which detects that a movable portion driven by a servomotor collides with an obstacle by the estimated disturbance. Estimate the estimated disturbance torque applied to the movable part from the command and the speed, calculate the known disturbance torque including at least one of friction torque, gravity torque, and interference torque of another axis, and subtract the known disturbance torque from the estimated disturbance torque. Means for obtaining a disturbance torque and detecting a collision of the movable portion based on the differential disturbance torque. "

[0003]

However, for example, in an industrial robot, a conventional observer is provided for each axis in consideration of a load on a CPU (Central Processing Unit) and simplification of arithmetic expressions. That is, in a six-axis articulated robot, there are six observers closed for each axis. However, the robot is configured with an observer for each axis, despite the fact that the robot has a structure that is likely to be affected by external forces such as gravity that changes due to movement and interference and centrifugal forces that are received by high-speed movement of any axis. Since the external force is ignored, there is a defect that the calculation accuracy is deteriorated. On the other hand, if a magnified observer considering all axes is configured to improve the calculation accuracy, the amount of calculation becomes enormous, which is not practical. Further, when a disturbance observer for estimating an external force for each axis is configured to detect a collision of a robot, since the above-described gravity, the interference force, and the centrifugal force are included in the estimated disturbance, a true disturbance due to the collision can be separated. However, there is a disadvantage that the detection accuracy is lowered. Furthermore, Patent No. 2749724
Neither does it dispel the essential bottleneck in the prior art described above. That is, an error occurs in the [modeling error] estimated value due to the difference between the control target and the observer model, and furthermore, it cannot be denied that the calculation result of the differential disturbance torque contains an error. Here, the present invention provides a control method for a load machine that overcomes all the problems found in the conventional example based on adding an operation term of disturbance to an observer equation for estimating a state of a motor to be controlled and a load machine. The purpose is to provide.

[0004]

According to a first aspect of the present invention, there is provided a method for controlling the driving of a load machine by an electric motor, wherein the state of the electric motor to be controlled and the state of the load machine are determined. A load machine control method, characterized in that a calculation term of a known disturbance added to the control target is added to a calculation equation of an observer to be estimated to perform a calculation. As a result, even if gravity or an external force from another axis is received, it is possible to estimate with high accuracy.

According to a second aspect of the present invention, in the method for controlling the driving of a load machine by an electric motor, a control object is added to an operation equation of an observer for estimating a state of the electric motor to be controlled and a state of the load machine. A load machine control method characterized by performing a calculation by adding a known disturbance calculation term. As a result, not only can it be accurately estimated even when gravity or an external force from another axis is received, and a remarkable effect that the estimated disturbance does not fluctuate due to these effects is recognized.

According to a third aspect of the present invention, when the load machine is a robot, the known disturbance calculation term applied to the controlled object includes gravity, interference force, centrifugal force, Coriolis force, frictional force, etc. acting on the robot. The control method for a load machine according to claim 1 or 2, wherein a calculation of a physical force acting on the robot is performed. As a result, the influence of most external forces acting on the robot can be eliminated, and the estimation accuracy can be further improved.

According to a fourth aspect of the present invention, in the method for controlling the driving of a load machine by the electric motor, a state of a disturbance applied to the electric motor or the load machine is estimated by the observer, and a value of the disturbance exceeds a preset threshold value. The load machine control method according to claim 2 or 3, wherein in the case, an abnormality of the robot or a situation as a collision is detected. As a result, a highly sensitive collision detection function can be realized, which can contribute to the art.

[0008]

Embodiments of the present invention will be described below in detail with reference to the drawings. First Embodiment FIG. 1 is a control function block diagram for driving a load machine by an electric motor according to one embodiment of the present invention. Here, the load machine will be described as an industrial robot. In all the drawings, the same reference numerals represent the same or corresponding members. In FIG. 1, a portion surrounded by an alternate long and short dash line in the input stage in the figure is a motor position follow-up controller 10 for causing the position of the motor to follow the position command of the motor.
Outputs a torque command 18 to the motor. A portion surrounded by a chain line in the output stage is a speed reducer 30 described later.

First, when the motor position command θ _{m ref} is input to this control system, the subtractor 11 negatively feeds back the position output θ _m of the motor to the position command θ _{m ref} by the subtractor 11, and the motor obtained therefrom is obtained. with respect to the position deviation, multiplied by the position loop gain K _p by the position controller 12, the proportional control is performed. 14
The proportional integrator consists integrator 16 integral gain K ₁ of the speed controller and 16 with 15 speed loop gain K _v of the actual speed by differentiating the output position theta _m of the motor in the differentiator time 19 Is calculated and negatively fed back to the speed command from the position controller 12 in the subtractor 13, and the speed deviation is input to the proportional integrator 14. The output of the speed controller 15 and the output of the integrator 16 are added by an adder 17 and output as a torque command 18 to the next stage. This torque command 18 is subtracted by the reaction torque 36 from the load machine side from the reduction gear 30 to the electric motor side in the subtractor 21 at the input stage of the electric motor 20, and is introduced into the electric motor 20.

Reference numeral 20 denotes a motor, which comprises a second-order integral element relating to the motor inertia J _m and time, and the position θ _{m of the} motor is derived in accordance with the driving torque applied to the motor 20. Position theta _m of the motor output is applied to the [comprising a first means 31 to the third means 35] following the reduction gear 30, the motor position theta _m first means
At 31 the speed is reduced by the reduction ratio [1 / N], and the decelerated output from the subtractor 32 is used to calculate the position output θ _{L of the} robot arm 40.
Twist angle theta _s minus the can, the speed reducer spring constants K _c via the second means 33 for the coefficient, the load-side torque 34 is obtained. The reaction torque from the load machine to the motor
Reference numeral 36 denotes a third means 35 which receives the load-side torque 34 from the second means 33, and is formed by multiplying by a reduction ratio [1 / N].

At the input stage of the load machine, the external force received by the gravity acting on the robot's own axis and the operation of the other axis at the input stage of the load machine is a disturbance torque di applied to the robot arm 40.
s is added to the load-side torque 34 by the adder 41, and is introduced into the robot arm 40. Then, in the robot arm 40 composed of the robot arm inertia J _L and the time integral element of the second order, the speed reducer corresponding to the position θ _m of the motor output
The load side torque 34 from 30, according to the driving torque input determined according to the input obtained by adding the disturbance torque dis, the robot arm is displaced outputs a position theta _L. Further, the gravity acting on the own axis and the external force received by the operation of the other axis are added to the function control block diagram [FIG. 1 and FIG. 2 described later] as a disturbance torque dis added to the robot arm. Therefore,
The function control block diagram of FIG. 1 is represented by the following equation when expressed by a state equation.

[0012]

(Equation 1)

Conventionally, when an observer is formed for such a model, the model has been designed without considering the third term on the right side, which is a disturbance term (Equation 1). However, because of the above-described problem, in the embodiment of the present invention, the input to the motor and the position of the motor are used as inputs, and the observer equation for estimating the disturbance applied to the motor, the robot arm, and the robot arm includes:
It is designed as an all-dimensional observer of (Equation 3) to which the operation term dis cal of the known disturbance applied to the robot arm is added.

[Embodiment 2] FIG. 2 shows another embodiment of the present invention, and shows a control function block diagram when an observer is added. However, FIG. 2 shows a feedback path of the state on the load side estimated by the observer.

[0015]

(Equation 2)

In the observer of the equation (3), the observer gains L ₁ , L ₂ ,
When L ₃ , L ₄ , and L ₅ are selected to be stable, the state quantity estimated value X ₂ gradually approaches the state quantity X _{1 of the} actual machine. At this time, the disturbance applied to the robot arm is gravity, frictional force,
Interference from other axes, centrifugal force, Coriolis force [Coriolis for
ce], which are the motor position, speed, acceleration,
Alternatively, it can be represented by a twist angle and a twist angular velocity. Therefore, by substituting the operation value [51 introduced into the observer 50 in FIG. 2], which is the operation result, into the discal of the fourth term on the right side of (Equation 3), there is no modeling error between the controlled object and the observer equation. Estimated values including disturbance do not fluctuate due to these effects, and advanced control is possible. Note that the frictional force may be a form incorporated in the matrix A of Equation (3).

In another embodiment, the robot is provided with a collision detecting function for detecting whether or not a robot or a tip load has collided with a peripheral device by using an observer for estimating a disturbance applied to an object to be controlled. Can be. In other words, the estimated disturbance is compared with a set threshold value, and if the disturbance is equal to or greater than the threshold value, it is detected as a collision of the robot, and the robot is immediately stopped or flexibly stopped to prevent damage to equipment due to the collision and reduce damage [ Not shown]. Here, in the observer of the present invention, among various disturbances caused by the operation of the robot, a known component is added to the arithmetic expression of the observer, so that the estimated disturbance is not affected by the disturbance and the disturbance from the outside of the robot such as a collision. Only sensitive to collisions, it is possible to detect collisions with high sensitivity. Although the embodiment of the present invention has been described above, in the case of actual implementation by software servo, equation (3) is discretized and the calculation is performed as a sequential observer equation.

In each embodiment, one axis of the robot has been described. However, the present invention can be applied to an extension to multiple axes or to a load machine other than the robot. Further, the disturbance applied to the controlled object of claim 2 may be a disturbance applied to the load machine or a disturbance applied to the electric motor. Obviously, an observer in which the disturbance estimation term described in claim 1 is omitted has an effect. The observer configuration consists of two parts, motor and load machine.
Although the model is modeled by approximating a form in which two inertia are combined, that is, a two-mass system, the load machine may be modeled as a rigid body that is firmly fixed to the electric motor.

[0019]

As described above, according to the present invention, it is possible to estimate a disturbance with high accuracy by adding a known disturbance operation term to the observer equation and performing a calculation to consider a dynamic force. This leads to a special effect that a precise control of the load machine can be easily performed. Also, when a collision is detected by an observer that estimates disturbance,
Since a true separation between the time of collision and the time of collision can be performed, a remarkable effect that a high-sensitivity collision detection function can be realized is also observed.

[Brief description of the drawings]

FIG. 1 is a control function block diagram when driving a robot arm according to a first embodiment of the present invention.

FIG. 2 is a control function block diagram including an observer when driving a robot arm according to a second embodiment of the present invention.

[Explanation of symbols]

10 Motor position tracking control unit 11 Subtractor 12 Position controller 13 Subtractor 14 Proportional integrator 15 Speed controller 16 Integrator 17 Adder 17a Computing unit 18 Torque command 19 Differentiator 20 Motor 21 Subtractor 30 Reducer 31 First Means 32 Subtractor 33 Second means 34 Load side torque 35 Third means 36 Reaction torque from load machine side to motor side 40 Robot arm 41 Adder 50 Observer 51 Known disturbance calculated value 52 Negative feedback path J _L Robot arm Inertia of J _m Motor inertia K _P Position loop gain K _V Speed loop gain K ₁ Integral gain K _C Reduction gear spring constant 1 / N Reduction gear reduction ratio θ _{m ref} Motor position command θ _m Motor position θ _S Torsion angle s ( D / dt with respect to time t) Differential symbol 1 / s Integral symbol T Transpose symbol dis Disturbance dis cal Operation value of known disturbance

(Equation 2)

(Equation 2)

 ────────────────────────────────────────────────── ─── Continued on the front page (72) Inventor Jun Hagiwara 2-1 Kurosaki Castle Stone, Yawatanishi-ku, Kitakyushu-shi, Fukuoka F-term (reference) 3F059 CA03 CA07 FB17 FC06 FC11 5H004 GA07 GA28 GB16 HA07 HB07 HB08 HB10 JA04 JB21 JB22 KA72 KB02 KB03 KB04 KB38 KB39 LA12 LA13 5H269 BB03 BB14 GG01 GG06 9A001 GG03 HH19 KK32

Claims (4)

[Claims] 1. A method for controlling driving of a load machine by an electric motor, wherein a calculation term of a known disturbance added to the control object is added to an operation equation of an observer for estimating a state of the electric motor to be controlled and a state of the load machine. And controlling the load machine. 2. A method for controlling the driving of a load machine by an electric motor, the method further comprising: adding the control object to an operation equation of an observer for estimating a state of the electric motor to be controlled and the load machine, and a disturbance applied to the control object. A method of controlling a load machine, wherein a calculation is performed by adding a calculation term of a known disturbance. 3. In the case where the load machine is a robot, the known disturbance calculation term applied to the control target acts on the robot such as gravity, interference force, centrifugal force, Coriolis force, frictional force, etc. acting on the robot. The control method for a load machine according to claim 1 or 2, wherein a calculation of a physical force is performed. 4. A method for controlling the driving of a load machine by the electric motor, wherein a state of a disturbance applied to the electric motor or the load machine is estimated by the observer, and when the value exceeds a preset threshold value, 4. The control method for a load machine according to claim 2, wherein an abnormality of the robot or a situation as a collision is detected.