JPH03175508A - Position detection for fuel closed loop system - Google Patents

Abstract

PURPOSE:To detect a position with a precision higher than the resolution of a linear scale by estimating the speed by an observer and integrating the estimated speed to obtain an estimated position. CONSTITUTION:The observer including the speed of a servo motor is constituted to estimate the speed. This estimated speed is integrated to detect the estimated position. The speed is estimated in each prescribed period by the observer, and this estimated speed is integrated, that is, the estimated speed is multiplied by a prescribed period to estimate the moving position in this period. Consequently, the position between minimum units detected by a position detector is estimated. Thus, the position is detected with a resolution higher than the resolution of the position detector.

Description

DETAILED DESCRIPTION OF THE INVENTION Field of the Invention The present invention relates to a position detection system in a fully closed loop system.

Conventional technology Speed control is performed based on feedback pulses from a pulse coater attached to a servo motor, and position feedback signals from a position detector such as a linear scale attached to the movable part of the machine driven by the servo motor. Based on this, the control method for servo motors that performs position control is known as the full-closed loop method.

FIG. 4 is a block diagram of this fully closed loop method, and the position of the linear scale etc. attached to the mechanical movable part 16 is detected from the number of pulses P of the movement command outputted from the numerical control device etc. at the speed loop period T. The position feedback pulse Pf for each speed loop period T from the device (not shown) is subtracted, the difference is integrated (block 10), the position deviation is calculated, and the position deviation is multiplied by the position loop gain Kp (block 11) to obtain the speed command Ve. is calculated, and the speed feedback X1 from the pulse coater (not shown) attached to the servo motor 15 is subtracted from the speed command Ve, and the speed loop processing 12 is performed.
(kl is an integral gain, and 2 is a proportional gain) to calculate a torque command U (current command) and output it to the motor 15. Blocks 13 and 14 of the motor 15 show the motor transfer function, where Kt is the torque constant and Jm is the inertia. The servo motor 15 then drives the movable part 16 of the machine.

In the full-closed-loop servo motor control system described above, the position of the moving parts of the machine is detected by a position detector such as a linear scale, so the position detection accuracy is determined by the resolution of the position detector. , positioning accuracy is also determined by the resolution.

Problems to be Solved by the Invention As mentioned above, in the full-closed loop servo motor control system, the accuracy of detecting the position of the moving parts of the machine is determined by the resolution of the position detector, such as a linear scale, installed on the machine. Even if the resolution of the pulse coater attached to the servo motor is increased to detect the speed of the servo motor, the position detection and positioning accuracy do not improve.

SUMMARY OF THE INVENTION Therefore, an object of the present invention is to provide a fully closed loop position detection method that can detect a position with a higher resolution than that of a position detector.

Means for Solving the Problems The present invention uses a full-closed loop system to configure an observer that includes the speed of a servo motor, estimates the speed using the observer, integrates the estimated speed to obtain an estimated position, and calculates the position. By detecting the position between the minimum units detected by the detector, it is possible to detect a position with a resolution higher than that of the position detector.

By configuring an observer that includes the speed of the working servo motor, the speed can be estimated. The estimated position can be detected by integrating this estimated speed. Therefore, the speed is estimated by the observer every predetermined period, and the estimated speed is integrated, that is, the estimated speed is multiplied by the predetermined period to estimate the moving position during that period.

This makes it possible to estimate the position between the minimum units detected by the position detector.

Embodiment First, a conventional disturbance estimation observer in digital servo control in which servo control of a servo motor is performed by a processor will be explained.

FIG. 5 is a block diagram of a model for an observer in a servo motor, where 13 is a transfer function of the torque constant Kt of the servo motor shown in FIG. Transfer function 1 of Jm
4a and an integral term 14b. In FIG. 4, U represents a torque command as an input, and xl and x2 represent speed and disturbance torque as state variables.

In the model of FIG. 5, the state variable xl.

The state equation for X2 is as shown in equation (1) below.

Note that in the above equation (1), if means acceleration and cross 2 means the degree of change in disturbance torque, but it is assumed that cross 2 = 0, assuming that there is no change in disturbance torque x2 in a short time.

If Equation (1) is a discrete value system with a sampling period (velocity loop period) T, then the following Equation (2) is used to configure the observer for Equation (2) above (xl(i) , K2(i) are estimated speed and estimated disturbance torque, Zl(i) and Z2(i) are variables during observer processing).

From equation (2)...(4) The bottom of the observer tank formed by equations (3) and (4) above is shown in a block diagram as shown in FIG. In addition, No. (
In equation 4), (Kl, 2)'' is a gain vector, and in FIG. 2, Z-1 represents a delay of one period T.

Multiply the input torque command u (i) by Kt-T/Jm (
By adding the estimated disturbance torque value x 2 (i) multiplied by T/Jm (block 2) to the value obtained in block 1), and further adding the estimated speed xi(i), the variable Z in equation (3) I (i+I) is calculated. By delaying this by one period (block 3), the variable Z,1(i) is found. Next, the actual speed x l (
Subtract the above variable Zl(i) from i) (xi(i)-Zl
(i)), multiply this value by the gain by 1 (block 4), and add the variable Z ] (i) to the estimated speed x
l(i) (Equation (4)) is found.

Furthermore, by adding the variable 22(i) to the value obtained by multiplying the gain by 2 (block 5) to the above (xl(1-Zl(i)), the estimated disturbance torque x2(i) can be found (Equation (4) ).

This estimated disturbance torque x2(i) is determined by the variable 2 from equation (3).
2 (i+1), and set the Kono variable 22(i+1) to 1
If the cycle is delayed (block 6), the variable 22 (i) will be found, and the calculations of equations (3) and (4) will be performed. This means that xl(i) is found for the estimated speed using this observer. As mentioned above, the estimated speed xi(i) can be obtained from the torque command u (i) and the actual speed xl(i) detected by the pulse coater by performing the observer processing shown in FIG. The characteristic of is the gain vector (K1゜K2)
” Determined by

Therefore, No. (3) formula.

In equation (4), when expressed using each and XI, the following equations (5) and (6)
The equations become...(5)...(6) In order to convert the above equations (5) and (6) into a continuous system, bilinear transformation z= (2+ST)/ Substituting (2-8T), the denominators of equations (5) and (6) become the following (7)
The formula becomes

(z-1) (2-1+KI) 2z=-L-(2S
T (2ST+Kl (2+ST) +2 (4-3
2T 2) 1 (2-5T) 2 When estimating the speed and position using an observer, the position feedback pulse from a position detector such as a linear scale is within the speed loop period or not. When moving at high speed, there is no need to estimate the position between position feedback pulses. Therefore, in order to detect accurate estimated speed and position at low frequencies, which is a problem, we set the cutoff frequency fc (Wc = 2rfc) of the observer, and set the pole arrangement to 1 and the damping constant so that it becomes a standard quadratic shape. P, 0.
5, the numerator of equation (7) should be as shown in equation (8) below.

4 to 2+4KITS+ (4-2KI- to 2) 52T
2. In order for the above equation (8) to hold true, the gains Kl and K2 may be set as shown in the following equation (9).

KI=(4/TPWcT)/(We2T'+2/T
PWcT+4) −(9)K2= (4Wc2 T2
)/ (Wc2 :r2 +2ffPWcT+4
)...(10) In this way, all the unknowns in the observer shown in Fig. 2 are obtained, so if the observer processing shown in Fig. 2 is performed every velocity loop processing by a processor that performs digital servo processing, the velocity x I ( i) can be estimated, and by integrating this, that is, multiplying the estimated speed x 1 (i) by the period T, the estimated movement amount for the period can be obtained.

FIGS. 3(a) and 3(b) are explanatory diagrams of position detection in this embodiment. FIG. 3(a) shows when the motor is rotating in the positive direction, and FIG. 3(b) shows when the motor is rotating in the negative direction. An example is shown when it is rotating.

In FIG. 3(a), the symbol Ps indicates the position detected by the linear scale, and the symbol Pse(n) indicates the estimated position estimated by the present invention. This example describes a case where positioning is to be performed at position 102.5. At sampling time T1, at least one grid of the linear scale has passed from the previous sampling time to the current sampling time TI, and the position Ps (1) detected by the linear scale is II 00J
Suppose it was. The estimated position Pse(1) at this time is
Starting point r100 of the position detected by the linear scale. O
Let it be J. In other words, if the position detected on the linear scale is different from the previous sampling time to the current sampling time (when it passes through at least one grid of the linear scale), the estimated position Pse(n) is detected on the linear scale. It is assumed that the position Ps(n) is equal to the position Ps(n).

At the next sampling time T2, if there is no change in the position detected by the linear scale, the estimated speed detected by the observer processing at the time of sampling (the estimated The estimated position Pse(2) is obtained by adding the value obtained by multiplying the sampling period T (the speed is assumed to have been converted to the moving speed of the machine) by the sampling period T (the amount of movement during period 7).

Pse (2)=Pse (2) +xl ・T=10
0+xl -T At the next sampling time T3, the linear scale crosses the grid and the detection time is rlolJ, so the estimated position Pse(3) becomes rlol, OJ. At sampling time T4, the detection position of the linear scale T is "101".
”, so the estimated position Pse (4) is Pse (4) = Pse (3) + xl ・T = 10
It becomes 1+x-T. The closer you get to the target position r102.5J, the smaller the position deviation becomes and the slower the speed becomes.
The amount of movement during period 7 also becomes smaller.

At sampling time T5, the linear scale crosses the grid and the detected position Pse(5) is rl O2J, so the estimated position Pse(5) is set to "102".

Then, at the time of subsequent sampling, T6. T7. At T8, the detected value of the linear scale is r102J and does not change, so the estimated positions Pse(6) to Pse(8) are as follows.

Pse (6)=Pse (5) +xl −TPse
(7)=Pse (6) +xl −TPse (8
)=Pse (7)+xl ・T Thus, at sampling time T8, the estimated position enters the in-position width of the target position "102.5J" and the positional deviation, which is the difference between the estimated position and the target position, becomes near "0". , in the above example where positioning is completed, the target position r102J within the unit of the linear scale is reached at sampling time T5, but the estimated position at this time is changed to the starting position r102. Since OJ is used, an error occurs between the exact position and the estimated position Pse within one unit of the linear scale. However, when the target position is approached, the positional deviation is small and the speed is slow, so the amount of movement per cycle is also small, and the above-mentioned error is slight.

FIG. 3(b) shows an example in which the motor rotates in the negative direction to perform positioning. In this case, if the linear scale crosses the grid from the previous sampling time to the current sampling time, the corresponding sampling position Pse (n) is the starting point of the scale position and "1" is added to the value of the detected position Ps(n) on the linear scale. be. For example, the value "102" detected by the linear scale at period T1
is different from the value Ps (1) detected at the previous sampling time, the estimated position Pse (1) is Pse (1) = P
s (1)+1=103. That is, when rotating in the negative direction, the value detected from the linear scale decreases and changes from rl 03J to r102J, and the starting point of the scale position of the detection position r102J is r103.
This is because ゜0''. And at sampling time T2
The detection position Ps(2) of the linear scale is r102J
If so, add the value obtained by multiplying the estimated speed X (negative value) detected by the observer by the period T, and calculate the specified sampling time T2
Let the estimated position Pse (2) be Pse (2) = Pse (1) + xl @T = 10
3+xl -T However, since the estimated speed X is negative, the estimated position Pse(2
) becomes 102.0<Pse (2)<103.0.

FIG. 1 is a flowchart of estimated position calculation processing performed by a processor that performs digital servo processing every velocity loop processing.

First, perform speed loop processing to obtain torque command U (
In step S1), a conventionally known observer process shown in FIG. 2 is performed using the torque command U and the actual motor speed xi detected from the pulse coater to obtain an estimated speed xi (step S2).

Next, subtract the previous cycle linear scale detection position stored in register R (Ps) from the position Ps detected from the linear scale, and if the value is positive (crossing the grid of the linear scale with positive rotation) ), estimated position Ps
The detected position Ps on the linear scale is stored in the register R (Pse) that stores e (steps S3 and S4).

For example, at sampling time T1 in FIG. 3(a). T3.
This is the case of T5.

Also, the position Ps of the period detected by the linear scale
If the value obtained by subtracting the detection position R (Ps) of the previous cycle from is negative (the grid of the linear scale is switched by rotating in the negative direction), the detection position Ps is set to "1" in the register R (Pse).
” The added value is stored (steps S3 and S6). For example, at the time of sampling in FIG. 3(b), Tl, T3. This is the case of T5.

In addition, when the detection position Ps on the linear scale at the time of previous sampling and the time of sampling are the same (Fig. 3 (a)
T1 and T2. T3 and T4. T5 and T6. T6 and T? ,
T7 and T8, TI and T2 in FIG. 3(b). T3 and T4
．． T5 and T6. T6 and T7. T7 and T8. T8 and T9)
, the value obtained by multiplying the estimated speed x1 calculated in step S2 by the speed loop period T is added to the value of the register R (Pse) that stores the estimated position Pse to obtain the estimated position Pse in the corresponding period. ) (step 33.S5).

Then, the detected position Ps on the linear scale is stored in the register R (Ps) (step S7), and the estimated position calculation process ends.

Based on the estimated position Pse thus obtained and stored in the register R(Pse), the processor performs position loop processing.

In the above embodiment, the process shown in Fig. 1 is performed for each speed loop process, but the estimated position is required when the motor is running at low speed, and the estimated position is not required at high speed. do not. Therefore, steps 82 to S7 may be performed only when the motor speed becomes equal to or less than a predetermined speed.

Effects of the Invention The present invention estimates the velocity using an observer, integrates the estimated velocity, and obtains the estimated position, thereby obtaining the position between the smallest units detected by a position detector such as a linear scale. Therefore, the position can be detected with higher precision than the resolution of the linear scale.

[Brief explanation of drawings]

Fig. 1 is a flowchart of an operation process of an embodiment of the present invention executed by a processor that performs digital servo control, Fig. 2 is a block diagram of a disturbance estimation observer, Fig. 3(a),
(b) is an explanatory diagram of position detection 1 positioning in one embodiment of the present invention, FIG. 4 is a block diagram of a fully closed loop method, and FIG. 5 is a block diagram of a model for an observer. 8xl... Estimated speed, Ps... Device composed of linear scale, Pse... Estimated position, U... Torque command, Xl... Actual motor speed, P/T... Movement command (number of pulses between speed loop periods), X2...disturbance torque. No. 0 (C1) 7

Claims (2)

[Claims] (1) In the full-closed loop system, the position of the mechanical moving part is detected by a position detector attached to the moving part of the machine driven by the servo motor, and the speed is detected using a pulse coater attached to the servo motor. Configure the observer as a state variable containing the speed of the motor,
A full-closed loop position detection method in which the observer estimates the speed, integrates the estimated speed, and detects the position between the smallest units detected by the position detector. (2) Detect the position detected by the position detector by sampling every predetermined period, and if there is a difference between the detected position at the previous sampling time and the current sampling time, start to the position detected at the current sampling time. If the position is the estimated position and the detected position at the previous sampling and the current sampling are the same, the estimated position is calculated by adding the value obtained by multiplying the estimated speed detected by the observer by the sampling period to the estimated position estimated at the previous sampling. The position detection method in a fully closed loop method according to claim 1.