JP2906255B2 - Servo control device - Google Patents

Description

The present invention relates to a position, speed or force control device for a robot manipulator, a machine tool, an XY table or the like.

[Summary of the Invention]

The present invention relates to a control target driven by a machine having position detecting means such as an arm of a robot, a main shaft of a machine tool, a work table, or the like, in which a Coulomb friction, elasticity, viscosity, or a change in an acceleration force caused by a load change. The disturbance load on the controlled object consisting of external force from the outside world and the movement speed of the controlled object
It has a state estimation calculation unit (disturbance load / speed estimation observer) for simultaneously estimating using the driving force (or driving current) generated by the actuator and the detected current position data, and positively feedback the estimated disturbance load. The servo control device has a function of simultaneously performing a compensation for apparently eliminating a disturbance load and a speed feedback compensation using an estimated speed, thereby improving control performance. (See FIG. 1).

[Conventional technology]

A known or unknown disturbance torque is generated on the motor shaft of a mechanical drive system such as a robot or a machine tool, which interferes with mechanical drive, causes vibration, or causes an error following the target trajectory. There is a problem that causes deterioration of various controllability.

2. Description of the Related Art Conventionally, in the field of position / speed control, as shown in an example of FIG. 2, a control purpose is formed by forming a feedback control loop using a position signal 6 from a position detector or a feedback signal of a speed signal 14 to be controlled. Has been achieved. At this time, for the disturbance torque 7, the feedback loop uses the disturbance removal effect which the feedback loop naturally has, and the deviation is eliminated by setting the loop gain as large as possible. In addition, as can be seen from the transfer function block diagram of the conventional example shown in FIG. 5, the steady-state error is eliminated by inserting an integral compensation term 11 into the feedback loop for the error between the position command 5 and the position detection signal 6. The way has been taken. However, since the stability region of the control system is limited by the characteristics of the control target, the mere improvement of the gains 11, 12, and 13 is limited, and sufficient characteristics cannot be obtained.

Many control methods have been developed to deal with known disturbances by constructing a control system that considers changes in dynamic characteristics. However, it has been difficult to maintain the same control characteristics against unknown disturbances.

Therefore, as shown in FIG. 5, a disturbance torque observer 4 that treats the disturbance torque 7 as a part of the control target model 2 and estimates the disturbance torque 7 using the generated current or generated torque 8 of the actuator and the movement speed 14 is used. Methods to configure and apply to speed control systems have been developed. On the other hand, not only the speed control system but also a position control system is constructed, as shown in FIG.
As seen in FIG. 2, it is known that controllability can be improved by using a speed feedback loop together.

[Problems to be solved by the invention]

Many of robots and machine tools do not have a speed detecting means such as a tachogenerator and often have only a position detecting means for the purpose of reducing the size and weight. In this case, it is necessary to use the differential value of the position detection data as the speed detection value for the control position, but the differentiation by the analog circuit is not practical because the high frequency noise is easily picked up.
In addition, for a control target having a position detecting means of a type that generates a pulse representing a position, a speed detection circuit that generates a voltage proportional to the pulse frequency is widely used. There was a problem with accuracy, which also caused vibration. There is a problem that a method of inserting a low-pass filter in order to eliminate the ripple of the speed detection circuit is delayed, thereby deteriorating controllability. Also,
Digital servo controllers often employ a method of approximating the first-order difference between the current position and the position one sample before as velocity data, but this also tends to cause vibration and noise because the ripple increases at low speeds. .

As described above, for a position control system having only position detection means, a control device that outputs an estimated disturbance torque by an observer from a speed signal from a control target and a drive torque signal of the control system and uses the estimated disturbance torque in the position control system Then there was a problem in practice.

[Means for solving the problem]

In order to solve the above problem, the present invention uses an observer (state estimator) for calculating and processing both a position signal and a driving force signal of a control object, calculates an estimated disturbance load and an estimated speed, and determines that the estimated disturbance load is correct. The control device feedbacks and estimates the speed and performs servo control by performing speed feedback compensation. The control device has a position detection unit and determines a driving force (or a driving current) and a position detection value for a control target that does not have the speed detection unit. As an input, a state estimation calculation unit (disturbance torque / speed observer) for estimating disturbance and speed based on the observer theory is configured, and the estimated disturbance load is positively fed back, and (action effect) is used to estimate the speed using the estimated speed. This is a servo control device in which means for simultaneously performing feedback is performed by software (CPU device).

[Action]

The position control calculation unit 3 of the servo controller 1 compares the data of the position command 5 with the position detection signal 6 of the control target, performs the calculation necessary for control using the data of the estimated speed 10, and generates a driving force command. Output. On the other hand, the disturbance load speed observer receives the driving force 8 and the position detection signal 6 and outputs the estimated disturbance load 9.
And the estimated speed 10 are output. The estimated disturbance load 9 is added to the output of the position control calculator 3 to obtain the driving force 8.

The controlled object 2 is driven and controlled by the difference between the driving force 8 and the disturbance load 7.

〔Example〕

An embodiment of the present invention will be described using an example in which a control target is a rotating system.

In FIG. 1, a position command signal 5 and a position detection signal 6 from a position detector of the control target 2 are output to a position control calculation unit 3 of the servo control device 1. On the other hand, a position detection signal 6 and a driving torque (driving force) signal 8 are input to the disturbance torque / speed observer 4, and the observer 4 performs an operation based on these two signals to obtain an estimated disturbance torque (load). ) Output the signal 9 and the estimated speed signal 10.

The estimated speed signal 10 is input to the position control calculator 3, and the estimated disturbance torque signal 9 is added to the output 10a of the position control calculator 3.

In the position control calculation unit 3, the position command 5 and the position detection signal 6
Is calculated using the estimated speed signal 10 to obtain a driving force for moving the control target to a predetermined position, and a driving torque command signal 10a is output. The driving torque signal 10a is added to the estimated disturbance torque signal 9 as described above to obtain the driving torque signal 8. The control target 2 is driven and controlled by a difference between the drive torque signal 8 and disturbance torque such as acceleration fluctuation and sliding friction of the control target.

The servo control device 1 including the position control calculation unit 3 and the disturbance torque speed observer 4 and the like is configured to perform calculation processing by software using a CPU device. However, the servo control device 1 may calculate and output by a position control calculation circuit and an observer circuit, respectively. It may be.

Next, the principle of disturbance torque estimation will be described. The first equation is the equation of motion for the controlled object, and the motor generated torque
T _m , moment of inertia J, angular position θ, disturbance torque T _L ,
Represents the time derivative.

In order to obtain a disturbance torque speed estimation observer, the control target is expressed by a state equation such as the second and third equations.
Equations (6) and (7) express the first equation in a state equation based on the definition of the fourth equation and the assumption of the fifth equation.

It is clear from the literature regarding the disturbance torque observer that the observer based on the expression (5) eventually follows the disturbance torque change to some extent.

Equation of motion: T _m = J + TL (Equation 1) Equation of state: = AX + BU (Equation 2) Y = CX (Equation 3) Definition: X ₁ = θ, u = T _m (Equation 4) X ₂ =, y = x ₁ = Θ X ₃ = TL By applying Gopinas' minimum dimension observer, which is a basic design method of modern control theory, to Equations 6 and 7, Expressions 10 and 11 expressed by Expressions 8 and 9 are obtained. . Each state quantity has the meaning shown in Equation 12.

L ₁ and L ₂ are parameter constants for obtaining the eigenvalues of the above system, and are determined so as to ensure system stability.

Next, discretization is performed to realize the disturbance torque speed estimation observer obtained by Expressions 10 and 11 on the control device by software. By performing the conversion defined by equation 13,
Expressions 16 and 17 expressed by Expressions 14 and 15 are obtained. The parameters in the equation are represented by equations 18 to 27.

Z {(k + 1) T} =. Z (kT) +. U (kT)
(Equation 14) X (kT) = ・ Z (kT) + ・ U (kT) (Equation 15) Where z ₁ {(k + 1) T} is z _k11 , z ₂ {(k + 1)
T｝ is written as z _k21 , z ₁ (kT) is z _k1 , z ₂ (kT) is z _k2
Is written.

y (kT) is written as y _k and u (kT) is written as u _k .

The x ₁ X _k1, x ₂ a (kT) put out and x _k2, x ₃ and (kT) X _k3.

θ (kT) is represented by θ _k , (kT) is _represented by _k , and _TL (kT) is represented by _TLk .

(T is the sampling time, and J is the nominal value of the moment of inertia.) The sixth example shows that the disturbance torque speed estimation observer of the discretized form expressed by the equations (16) and (17) is incorporated in the control device. It is a flowchart shown in the figure, and the disturbance torque speed estimation is simply described as an observer. After initializing the data, the servo controller enters a repetitive loop, creates sequential position command data, waits for a sampling time, and performs position detection. The observer estimates the disturbance torque and speed based on the detected value and the previous input, and transfers them to the servo calculation routine. The servo outputs the calculation result as a driving torque, and updates the number of repetitions k.

 FIG. 7 shows a calculation routine of the observer.

.., Z _ki (i = 0, 1, 2,...) Are intermediate variables in the disturbance torque speed estimation observer, and the data value is updated every sampling time in the loop of the flowchart with the initial value being 0. Z _ii1 corresponds to Z {(k + 1)} in the fourteenth equation, is a variable after one sample of Z (k) corresponding to Z _ii , and is sequentially updated in this routine.

Equation 28 shows an equation for implementing control compensation using the estimated disturbance torque and the estimated speed. The gain is multiplied by the integral of the difference between the command position and the detected position, the gain is multiplied by the estimated speed and the detected position, and negative feedback is performed, and the estimated disturbance torque is positively fed back.

_k : Estimated speed T _Lk : Estimated disturbance torque Tm _k : Mechanical torque FIG. 8 shows a routine for realizing.

As a result, the transfer function block shown in FIG. 4 is realized.

[Effects of the Invention]

Taking a robot as an example, when the robot grips an object having an unknown weight, the dynamic characteristic parameter of the robot changes by an unknown amount. Further, not only robots but also parameters such as elasticity, viscosity, and friction of the mechanical drive system change depending on environmental temperature, humidity, and the like. The torque fluctuation generated at the time of driving due to these changes acts as a disturbance torque on the control system. The present control device can always maintain a constant control characteristic of the control system by apparently eliminating this disturbance torque.

Generally, the speed ripple of the position / speed control system is caused by disturbance noise, detection noise, drive torque ripple, and the like.In addition to the above-described disturbance torque elimination function, utilizing the low-pass filter characteristic of the disturbance torque speed estimation observer. By using the speed feedback using the estimated speed, detection noise at the time of speed detection can also be removed. By performing disturbance torque removal and estimated speed feedback at the same time, stable control can be realized even at a low speed where a speed ripple is likely to occur.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram of an embodiment of the present invention, showing a disturbance torque speed estimation observer and a control device using the observer. 2 and 3 are conceptual diagrams of a conventional control device. FIG. 4 is a block diagram showing a transfer function according to the embodiment of the present invention.
FIG. 5 is a block diagram showing a transfer function of the embodiment of FIG. FIG. 6 is a flowchart showing a control method according to the embodiment of the present invention, and FIGS. 7 and 8 are flowcharts of the observer and servo calculation routine of FIG. DESCRIPTION OF SYMBOLS 1 ... Servo control device 2 ... Control target 3 ... Position control calculation part 4 ... Disturbance load (torque) / speed estimation observer (state estimation calculation part) 5 ... Position command signal 6 ... Position signal 7 ... Disturbance load (torque) 8 Driving force (torque) signal 9 Estimated disturbance load (torque) signal 10 Estimated speed signal 11 Integral compensation gain 12 Speed feedback gain 13 Position feedback gain 14 …speed

Continuation of the front page (58) Field surveyed (Int.Cl. ⁶ , DB name) G05D 3/00-3/12 G05B 13/04

Claims (1)

(57) [Claims] 1. A servo control device for controlling a controlled object comprising a mechanical drive system having a position detecting means, wherein the servo load is calculated from a driving force signal for driving the controlled object and a position detection signal, and the disturbance load of the controlled object and A state estimation calculating means for simultaneously estimating the motion speed of the controlled object and outputting an estimated disturbance load and an estimated speed signal; and calculating a driving force command signal by calculating a position command signal, the position detection signal and the estimated speed signal. And a position control calculating means for outputting the estimated disturbance load signal, and the estimated speed signal is subjected to speed feedback compensation.