JP2533474B2 - Positioning control device - Google Patents

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a positioning control device suitable for improving the tracking characteristic of position control of a digitally controlled object such as an NC machine tool.

[Background of the Invention]

Conventionally, in order to eliminate an error in positioning control due to friction in a drive device to be controlled, there is one that changes a compensation coefficient for a drive signal to be controlled by a signal from a feedback system (JP-A-55-52109). reference).

However, even with such a control method, there is a problem in that when the controlled object vibrates with respect to the position command due to environmental changes such as temperature and humidity, it cannot be stabilized quickly and smoothly.

[Object of the Invention]

The present invention has been made to solve the above-described problems, and it is an object of the present invention to provide a positioning control device that can stabilize quickly and smoothly even when a controlled object vibrates in response to a position command. To aim.

[Outline of Invention]

The device of the present invention comprises a driving means for controlling a controlled object according to the movement command data calculated based on the position command data, a position detecting means for detecting the current position of the controlled object, and a current output from the position detecting means. In a positioning control device having position deviation calculation means for calculating a difference between position data and the position command data and outputting position deviation data, each history data of the movement command data and the current position data is sequentially updated. While storing the history data, the compensation coefficient setting and updating means for setting and updating the compensation coefficient by both the history data of the movement command data and the current position data stored in the history data storing means, and the compensation coefficient setting and updating means. Compensation coefficient storage means for storing the compensation coefficient set and updated by the compensation coefficient storage means and the compensation coefficient stored in the compensation coefficient storage means. By providing a compensation calculating means comprising using a digital filter for calculating the latest movement command data based on the position deviation data, it is to achieve the above object.

Example of Invention

Embodiments of the present invention will be described below with reference to the drawings. First
FIG. 1 is a block diagram showing an embodiment of a positioning control device according to the present invention. In the figure, 1 is a clock for outputting a pulse train of a constant cycle (sampling cycle), and 2 is position command data. The input position command data 2 is stored in the position command register 3
Is stored. The position deviation calculation unit 4 sends the difference between the contents of the position command register 3 and the current position data 18 of the load 15 described later to the position deviation register group 6 as position deviation data 5.
Each of the registers of the position deviation register group 6 sends its own data to the next register when the pulse from the clock 1 is input, and at the same time takes in and stores new data. This operation is performed by the movement command register group whose details will be described later.
The same applies to 10 and the current position register group 19.

The compensation calculation unit 7 takes in the contents of the position deviation register group 6, the compensation coefficient register 9, and the movement command register group 10, creates movement instruction data 8 by compensation calculation, and outputs it to the movement instruction register group 10 and the drive circuit 11. .

When the drive circuit 11 receives the movement command data 8, the motor 12
Voltage. The rotation of the motor 12 is transmitted to the ball screw 14 via the transmission 13 to move the load 15 that is the final control target in the direction of arrow a. The sensor 16 attached to the load 15 moves with the load 15 and slides on the current position display scale 17 to output the current position data 18. The current position data 18 is sent to the position deviation calculation unit 4 and at the same time stored in the current position register group 19.

Assuming that the position command data 2, position deviation data 5, movement command data 8 and current position data 18 one sampling time after the load 15 starts moving are u _i , x _i , y _I and z _I , respectively, compensation calculation is performed. The compensation calculation performed by the unit 7 is the calculation of the following equation (1) (see equation (17) in Nikkei Electronics 1977.8.8, page 105).

(However, when i−k ≦ 0, x _ik = y _ik = 0) A, a ₁ , a ₂ ……, a _l , b ₁ , b ₂ ……, b _l in this equation (1)
Is stored in the compensation coefficient register 9 and is called a compensation coefficient. In this case, a ₁ , a ₂ , ..., a _l are position commands U
(N) and the current position Z (n). It is a coefficient related to the difference. When the difference is large, the value of the movement command Y (n) is set to be large, and b ₁ , b ₂ , ..., b _l Is the past movement command Y (n)
Is a coefficient related to, and when Y (n) in the past increases,
Set so that the new value of Y (n) becomes smaller. A sets the direction in which the position deviation data 5 is amplified and the follow-up characteristic of the current position data 18 of the load 15 with respect to the position command data 2 is optimized. The value of l is the load on the movement command data 8.
It is determined by the delay order of the tracking characteristic of the current position data 18 of 15. That is, if the delay order is 1, l = 1, and if 2
= 2 and n, l = n.

Now, let us say that the data string of the position command data 2 from the start of inputting the position command data 2 to the n sampling time later is U (n) in the following expression (2).

U (n) = (u ₀ , u ₁ , u ₂ , ..., u _n ) ... (2) Similarly, the position deviation data 5, the movement command data 8 and the current position data 18 up to n samples later. each data sequence, X (n) = (x 0, x 1, x 2, ......, x n) ...... (3) Y (n) = {y 0, y 1, y 2, ......, y _n ) (4) Z (n) = (z ₀ , z ₁ , z ₂ , ..., z _n ) (5).

In the above, first, the system shown in FIG. 1 is in the initial state, the first compensation coefficient is not set, and the position deviation data 5 which is the output of the position deviation calculation unit 4 is directly inputted to the input data ( If the movement command data is 8),
That is, when Y (n) = X (n), the position command data 2, that is, U (n) is given by the following equation (8), Z (n) is given by the following equation (9) (see FIG. 3). ).

U (n) = (1,1,1, ..., 1) …… (8) Z (n) = (0,0.388,1,1.4,1.4,1.147,0.894, ……) …… (9) In such a system, in order to investigate the characteristics from the drive circuit 11 to the sensor 16, the drive circuit 11 is provided with the movement command data 8 shown in the following equation (6), and at this time, the current position data 18 of the load 15 is as follows ( It is assumed that the equation 7) (FIG. 2) is obtained.

Y (n) = (1,0,0, ……, 0) …… (6) Z (n) ＝ (0,0.368,0.767,0.914,0.969,0.988, ……) …… (7) First In setting the compensation coefficient, the method in the later-described compensation coefficient automatic setting is used from the data of the above equation (7), and the compensation coefficient shown in the following equation (10) is obtained by this.

l = 1, A = 1.582, a 1 = -0.368, b 1 = 0.418 ...... (10) when these perform the assignment to the compensation calculation on the equation (1),
Z (n) becomes as shown in equation (11) (Fig. 4), and 2
It reaches the target position after the sampling time and stabilizes.

Z (n) = (0,0.582,1,1,1, ...) (11) Describe the automatic setting of the compensation coefficient. When the system shown in FIG. 1 is in the initial state and the compensation coefficient is not stored in the compensation coefficient register 9, the compensation coefficient setting unit 20 is operated to obtain the compensation coefficients A, a ₁ and b ₁ . First,
It is assumed that the movement command data 8 of the equation (6) is given and the equation (7) is obtained as the follow-up of the load 15. This equation (7) is regarded as the response of the first-order lag system from FIG. 2, and l = 1 is set.

If the response characteristic of the load 15 is G (α), the following equations (17) and (18) are established without compensation.

Y (n) = X (n) (17) Z (n) = G (α) × Y (n) (18) If the load 15 is a first-order lag system, the characteristic g (s) of the load 15 )
Is However, K can be represented by gain s, and splice operator T ₁ can be represented by a response delay time constant of load 15. The characteristic h (s) of the correction calculation unit 7 is However, Ts can be represented by the sampling period. The product of the characteristic g (s) of the load 15 and the characteristic h (s) of the correction calculation unit 7, g (s) × h
(S) is Therefore, if the Z-transformed version of this is G (z), Becomes And E = e ^{-T ] -T1} Becomes Further expanding this equation, d ₁ = K × T ₁ (T / T ₁ + E-1) (18F) d ₂ = K × T ₁ (1-E-T / T ₁ E) (18G) ) E ₁ = -E ...... (18H) Becomes Here, α is a sampling operator.

When the transfer function of the compensation calculation unit 7 is D (α), the response characteristic of the load 15 is corrected to Z (n) = G (α) × D (α) × X (n) (20) It

Since X (n) = U (n) −Z (n) (21), when the compensation calculation unit 7 is used, the relationship between the position command data 2 and the current position data 18 is Z after correction. (N) = G _t (α) × U (n) (22) Therefore, the current position data G _t (α) is obtained by G _t (α) = Z (n) / U (n) (22A).

(Considering the unit step input data U (n), U (n) = 1 / (1-α ^-1 ) ... (22B) is given, and therefore the output data Z (n) is Z (N) = G _t (α) / (1-α ^-1 ) ... (22C) Becomes Then, it becomes constant after the time n × T.

Where G _t (α) is Then, in order to match the output data Z (n) with the input data U (n), What should I do? In general, (n) should be larger than (delay time constant + 1) of the load 15. Therefore, considering the transfer function D (α) of the compensation calculation unit 7, (22A)
ceremony, Becomes And, if this equation (23) is transformed, become. Then, by substituting the equation (19) into G (α) of the equation (36), Becomes

Therefore, considering the case where the number of terms of G _t (α) is 1,
The number of transmissions D (α) is third-order in both the numerator and the denominator. In order to minimize the order of the transfer function D (α), considering ω (α) and φ (α) for canceling the singular point of G (α) and the zero point, 1-G _t (α) = ω (α) (1-α ⁻¹ ) …… (36B) G _t (α) = φ (α) (d ₁ × α ⁻¹ + d ₂ × α ⁻² …… (36C) satisfies ω (α ) And φ (α).

Substituting the expression (36B) into the expression (36A), 1-φ (α) (d ₁ × α ^-1 + d ₂ × α ^-2 ) = ω (α) (1-α ^-1 ) ... (36D ). Then, as ω (α) and φ (α), which are the simplest, by substituting ω (α) = 1 + Wa ^-1 (36D1) φ (α) = Φ (36D2) into the equation (36D), -Φ (d ₁ × α ^-1 + d ₂ × α ^-2 ) = (1 + Wa ^-1 ) (1-α ^-1 ) ... (36E). Here, according to the coefficient comparison method, Φ × d ₁ = W−1 (36F) Φ × d ₂ = −W (36G). Therefore, if W is deleted from the formulas (36F) and (36G), , Φ = 1 / (d ₁ + d ₂ ) ... (36H).

Substituting this Φ into equation (36C), Becomes Substituting equation (37) into equation (36), Is asked.

This formula (38) is the same as the transfer function of the digital filter, and the compensation calculation unit 7 can be regarded as a digital filter. When the output is obtained by the calculation process of the program, the calculation formula of the output of the digital filter is used. It can be calculated by a certain formula (1).

Since the expression (1) is nothing but the arithmetic expression of D (α),
When expressed by the coefficient of the equation (1), Becomes

Therefore, a ₁ = e ₁ (42) Becomes

From the above, the following equation (12) is given, and the compensation coefficient A = 1.582 is obtained from this equation (12). When a ₁ and b ₁ are obtained in the same manner, the values of the above equation (10) are obtained.

A = (z ₂ −z ₁ ) / (z ₂ ² −z ₁ · z ₃ ) ... (12) The compensation coefficient obtained by the automatic setting of the compensation coefficient is stored in the compensation coefficient register 9 and used for the compensation calculation. It

Next, the automatic correction of the compensation coefficient will be described. When the position command data 2 shown in the formula (8) is given and the compensation calculation is performed using the compensation coefficient of the formula (10) obtained by the automatic setting, the movement command data 8 is given. It is assumed that the change of the load 15 oscillates as shown in the following equation (13) (Fig. 5) due to the change.

Z (n) = (0,1.110,1.076,0.822,1.068,0.994, ...)
...... (13) In such a case, the compensation coefficient correction unit 21 is activated, and the new position of the new compensation coefficient A is calculated by using the current position data 18, the output movement command data 8 and the compensation coefficient obtained so far.
′, ▲ a ^′ ₁ ▼, ▲ a ^′ ₂ ▼ will be obtained.

That is, now, G (α) in the equation (19) changes, and G ′
If it becomes (α), Then, the following equation (45) is established at this time.

(1 + D (n) x G '(α)) x Z (n) = D (n) x G' (α) x U (n) (45) In the equation (45), G '(α) The following are known. Therefore, by substituting the equation (44) into the equation (45), G ′ (α) can be obtained by the coefficient comparison method.

 When the above equation (45) is expanded, F (α) × Z (n) = H (α) × U (n) (46)

However, F (α) = 1 + (A × d ′ ₁ + b ₁ + e ′ ₁ −1) × α ⁻¹ + (A × (d ′ ₂ + a ₁ × d ′ ₁ ) + b ₁ e ′
_{1 - b 1 -e' 1) ×} α -2 + (A × a 1 × d'2 + b 1 × e 1) × α - 3 H (α) = A × d'1 × α -1 + A × ( d' ₂ + a ₁ × d '
₁ ) × α ⁻² + A × a ₁ × d ′ ₂ × α ⁻³ .

U (n) in the above equation (8) is Is.

Substituting the equation (46) into the equation (45), F (α) × (1-α ⁻¹ ) × Z (n) = H (α) (4
8)

here, Therefore, if r ₀ ＝ z ₀ …… (50) r _k ＝ (z _k −z _{k -1} ) (k ＞ ＝ 1) …… (51), then r ₀ = 0, Becomes

Comparing the coefficients, r ₁ = A × d ′ ₁ (53) r ₂ + r ₁ × (A × d ′ ₁ + b ₁ + e ′ ₁ −1) = A × (d ′ ₂ + a ₁ × d ′ ₁ ) (54) r ₃ + r ₂ × (A × d ′ ₁ + b ₁ + e ′ ₁ −1) + r ₁ × (A × (d ′ ₂ + a ₁ × d ′ ₁ ) + b ₁ e ′ ₁ −b ₁ -E '
₁ ) = A x a ₁ x d' ₂ (55), e ′ ₁ = (− r ₁ ³ −r ₁ ² × (b ₁ −a ₁ −1) −r ₁ × (2r ₂ −a ₁ × (b ₁ −a ₁ −1)) − r ₃ −r ₂ × (b ₁ −a ₁ −1)) / (r ₂ + r ₁ × (r ₁ + b ₁ −a ₁ −1)) …… (57) d ′ ₂ = (− r ₁ ³ × (b ₁ −a ₁ −1) + r ₁ ² × (b ₁ −1) × (b ₁ −a ₁ −1) + r ₁ × (−r ₃ −r ₂ × (b ₁ −a ₁ −1)) + r ₂ ² ) / (A × (r ₂ + r ₁ × (r ₁ + b ₁ −a ₁ −1))) (58)

The above equations (56), (57), (58) are replaced by (41), (42),
By substituting it into the equation (43), a new D (α) can be obtained.

Also, from equation (41), Becomes

From the above, a new compensation coefficient A'is obtained by the following equation (14).

A'= (A · S · z 1 + P · A 2) / (z 1 · (S · z 1 + P · A) -A · (Q · z 1 + P · R)} = 0.877 ...... (14) When ▲ a ^' ₁ ▼ and ▲ b ^' ₁ ▼ are obtained in the same manner, (1
5) Get the value of expression.

▲ a ^′ ₁ ▼ = −0.240, ▲ b ^′ ₁ ▼ = 0.385 (15) When a new compensation coefficient is obtained in this way, the contents of the compensation coefficient register 9 up to that point are erased and the compensation coefficient is deleted. The new compensation coefficient obtained by the automatic correction is stored in the compensation coefficient register 9. When the compensation calculation is performed using the compensation coefficients of the equations (14) and (15), the follow-up of the load 15 is as shown in the following equation (16) (Fig. 6), which is the same as the equation (11). It reaches the target position after the sampling time and stabilizes.

Z (n) = (0,0.615,1,1,1, ...) (16) In the above embodiment, the case where the controlled object after the drive circuit 11 has a first-order lag element has been described. , It is applicable even when the delay element of the controlled object is second-order or higher. In this case, the number of registers of the register groups 6, 10 and 19 for storing data is increased as compared with the above embodiment. Further, the drive mechanism of the load 15 is not limited to the transmission 13 and the ball screw 14, and includes all the mechanisms capable of displacing the load 15. Therefore, not only linear movement but also rotational movement, for example, is included.
The displacement of the load 15 can also be detected from the amount of rotation of the motor other than the sensor 16 used in the above-described embodiment.

In the embodiment shown in FIG. 1, the position deviation calculation unit 4, the compensation calculation unit 7, the compensation coefficient setting unit 20 and the compensation coefficient correction unit 21 can be replaced with one microprocessor. FIG. 7 shows an example of a schematic configuration in this case. In FIG. 7, 1 is the first
It is a clock similar to that shown in the figure. Reference numeral 22 is a read only memory (ROM) in which the processing procedures of position deviation calculation, compensation calculation, compensation coefficient setting calculation and compensation coefficient correction calculation are stored in advance. Reference numeral 23 is a random access memory (RAM) for storing the compensation coefficient, which has the same function as the compensation coefficient register 9 in FIG. Reference numeral 24 is a RAM that stores the history of the position deviation data 5, the movement command data 8 and the current position data 18. The position deviation register group 6, the movement command register group 10 and the current position register group 19 in FIG.
Works the same as. Reference numeral 25 is a processor that performs processing according to the contents of the processing procedure storage ROM 22. Reference numeral 26 denotes collectively the controlled objects after the drive circuit 11 in FIG. 1, and when the movement command data 8 is received from the processor 25, the state is displaced, and the state after the displacement is changed to the current position data 18.
Is returned to the processor 25. FIG. 8 shows an example of a flow chart of the compensation coefficient automatic setting processing by the device of the present invention, and FIG. 9 shows an example of a flow chart of the compensation calculation processing after the compensation coefficient setting and the compensation coefficient automatic correction processing. It is a thing.

The processing operation when the positioning control apparatus according to the present invention is configured using the processor 25 will be described below with reference to FIGS. 7 to 9. When the system shown in FIG. 7 is in the initial state, that is, when the compensation coefficient is not determined, the compensation coefficient is set by the procedure shown in FIG. First, the compensation command setting movement command data 8 is output, and the current position data 18 corresponding to it is read and stored in the history data storage RAM 24. The IROHA procedure is repeated until the data necessary for the compensation coefficient setting calculation are available. Compensation coefficient setting calculation is performed if the data are complete, and the obtained compensation coefficient is stored in the compensation coefficient.
The data is stored in the RAM 23, and the compensation coefficient automatic setting process ends.

Next, the processing after setting the compensation coefficient will be described. Processor 25
When the pulse generated by the clock 1 is received, performs the processing shown in FIG. 9 within the sampling period. First, the position command data 2 and the current position data 18 corresponding to the position command data 2 are read, and it is determined whether the compensation coefficient needs to be corrected. If necessary, the contents of the compensation coefficient storage RAM 23 and the history data storage RAM 24
The content of is read and the compensation coefficient correction calculation is performed. The compensation coefficient newly obtained by the correction calculation is stored in the compensation coefficient storage RAM 23. Then, the difference between the position command data 2 and the current position data 18 obtained by the position deviation calculation, that is, the position deviation data 5, the contents of the compensation coefficient storage RAM 23 and the contents of the history data storage RAM 24 are read, and the compensation calculation is performed to move. The command data 8 is output to the controlled object 26. Finally, the current position data 18, the position deviation data 5, and the movement command data 8 obtained after the control are stored in the history data storage RAM 24, and the compensation calculation process and the compensation coefficient automatic correction process are completed.

As described above, the present invention can be implemented by one microprocessor, which simplifies the control system and facilitates generalization.

〔The invention's effect〕

As described above, according to the present invention, the drive means for controlling the controlled object according to the movement command data calculated based on the position command data, the position detection means for detecting the current position of the controlled object, and the position detection In a positioning control device having position deviation calculation means for calculating the difference between the current position data output from the means and the position command data and outputting the position deviation data, each history of the movement command data and the current position data History data storage means for sequentially updating and storing data, and compensation coefficient setting and updating means for setting and updating a compensation coefficient by both history data of movement command data and current position data stored in the history data storage means, Compensation coefficient storage means for storing the compensation coefficient set and updated by the compensation coefficient setting and updating means, and the compensation coefficient storage means for storing the compensation coefficient. Since the compensation calculation means including the digital filter for calculating the latest movement command data based on the compensation coefficient and the position deviation data is provided, the control target is controlled by the latest movement command data, and the control is performed. Even if the object vibrates in response to the position command, it is possible to stabilize the object quickly and smoothly.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram showing an embodiment of the device of the present invention, and FIG.
FIG. 6 to FIG. 6 are graphs showing the follow-up of load for explaining the operation of the device of the present invention, FIG. 7 is a block diagram showing the main part of another embodiment of the device of the present invention, FIG. 8 and FIG. 6 is a flowchart of correction coefficient automatic setting and correction processing in the example shown in FIG. 7. 1 ... Clock, 2 ... Position command data, 3 ... Position command register, 4 ... Position deviation calculation unit, 5 ... Position deviation data, 6 ... Position deviation register group, 7 ... Compensation calculation unit, 8 ...... Movement command data, 9 …… Compensation coefficient register, 10 …… Movement command register group, 11 …… Driving circuit,
12 …… motor, 13 …… transmission, 14 …… ball screw, 15 …… load, 16 …… sensor, 17 …… current position display scale, 18 …… current position data, 19 …… current position register group, 20 …… Compensation coefficient setting unit, 21 …… Compensation coefficient correction unit.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Toshiyuki Makino 2100 Kamiimaizumi, Ebina City, Hitachi Seiko Co., Ltd. (72) Inventor Takahiko Yamashita 2100 Kamiimazumi, Ebina City, Hitachi Seiko Co., Ltd. (56) References JP Sho 61-50756 (JP, A)

Claims (1)

(57) [Claims] 1. A drive means for controlling a control target according to movement command data calculated based on position command data, a position detection means for detecting a current position of the control target, and a current output from the position detection means. In a positioning control device having position deviation calculation means for calculating a difference between position data and the position command data and outputting position deviation data, each history data of the movement command data and the current position data is sequentially updated. While the history data storage means to be stored,
The compensation coefficient setting and updating means for setting and updating the compensation coefficient by both the history data of the movement command data and the current position data stored in the history data storing means, and the compensation coefficient set and updated by the compensation coefficient setting and updating means. Compensation coefficient storage means for storing and compensation calculation means using a digital filter for calculating the latest movement command data based on the compensation coefficient and the position deviation data stored in the compensation coefficient storage means. Characteristic positioning control device.