JP6646466B2 - Position command controller and band removal filter - Google Patents

Description

  The present invention is a position command control device used for a servo control device that performs axis control of a machine tool, a robot arm, and the like.In particular, in order to suppress machine vibration of a plant to be controlled, a vibration component is calculated from a position command value. The present invention relates to a position command control device having a function of removing the position command.

  In general, a position command control device applied to axis control of a numerical control machine appropriately controls a control output τm of a drive servomotor to control a control target according to a position command value Xc commanded from a higher-level device. The position x of a certain target plant is controlled. Such a position command control device is required to have high command follow-up performance as well as system stability (including vibration suppression).

  FIG. 7 is a block diagram illustrating an example of a general position command control device. In this example, the torque control unit and the power amplifying unit are omitted, and the output τm of the position command control device 100 is used as it is as the output torque τm of the servo motor. The servomotor is connected to a ball screw or the like via a coupling or the like, and the rotational motion is converted into a linear motion of a drive table or the like to be controlled. , A mechanical member included in the target plant 200 in FIG. 7).

  Hereinafter, the position command control device 100 of FIG. 7 will be described. A position command value Xc commanded from a higher-level device (not shown) is input to the pre-filter 1. The pre-filter 1 is a band elimination filter (BRF) having a transfer function F (z) for eliminating a vibration component from the position command value Xc in order to reduce machine vibration generated in the target plant 200. is there. Note that, in many cases, the post-filter 1 performs an acceleration / deceleration process so that the position command value has an appropriate acceleration or jerk, but this is omitted here.

In this example, a feedforward configuration is provided in order to speed up the command response. Specifically, the position command value Xco, which is the output of the pre-filter 1, is time-differentiated by the differentiator 56 to become the speed feedforward amount Vco, and further time-differentiated by the differentiator 57 to obtain the acceleration command value A It becomes _F. Amplification factor A _TF in the amplifier 58, the object plant 200, is a constant for obtaining the acceleration and deceleration torque feed forward amount tau _F of the motor torque corresponding to generate the acceleration A _F.

  The feedback configuration is as follows. A position x detected by a position detector (not shown) installed on a control object or a servomotor is subtracted by a subtractor 50 from a position command value Xco output from the pre-filter 1. The position deviation output from the subtracter 50 is amplified by the position deviation amplifier 51 to a position loop gain Kp. The output is added to the speed feedforward amount Vco by the adder 52 to become a speed command value Vm.

The motor speed vm can be detected by time-differentiating a position detection value obtained from a position detector (not shown) provided in the servomotor. The subtractor 53 subtracts the motor speed vm from the speed command value Vm. The speed error output from the subtractor 53 is normally proportionally integrated and amplified by the speed error amplifier 54 at an amplification factor Gv. The output of the speed error amplifier 54 and the acceleration / deceleration torque feedforward amount τ _F are added by the adder 55 to obtain a torque command value τm of the servo motor.

  Next, the pre-filter 1 will be described. It is an essential requirement that the pre-filter 1 that processes the position command value Xc has a linear (linear) phase characteristic. For this reason, an FIR filter is generally mounted on the pre-filter 1, but since the machine vibration frequency generated in the target plant 200 is usually as low as several tens of Hz, this frequency band is removed. In order to design the drooping characteristics of the BRF, a frequency resolution of several Hz is required.

  For example, when considering the drooping characteristic with respect to a BRF having a removal center frequency of 20 Hz, it is necessary to set at least 20 Hz to the third point (three times the frequency resolution). Therefore, the frequency resolution needs to be set to 20/3 = 6.66 ··· Hz, and if the sampling operation cycle Ts = 1 ms, the filter order is 1000 / 6.66 ·· = 150. That is, in order to remove a low-frequency machine vibration frequency, it is necessary to mount an FIR filter having a high order exceeding 150.

  Here, a case where the reciprocating position command value Xc shown in the lower part of FIG. 8 is input to the position command control device 100 of FIG. 7 will be described. The host device (not shown) calculates the position command value Xc at a cycle of 1 ms and outputs it to the position command control device 100. The speed command value Vc on the upper side of FIG. 8 is a speed obtained from a change amount of the position command value Xc in each calculation cycle.

  For the pre-filter 1, an FIR filter designed with a filter order of 161 and a removal center frequency of 18.6 Hz is set as the BRF. Since (1000/161) · 3 ≒ 18.6, 18.6 Hz is set to three times the frequency resolution. FIG. 9 shows a position command value Xco, which is an output of the pre-filter 1, and a speed command value Vco, which is a time variation thereof. From the difference between the speed command value Vc in FIG. 8 and the speed command value Vco in FIG. 9, the effect of filtering by the pre-filter 1 can be confirmed.

  FIG. 10 is an enlarged view of the position command value Xco at the time of startup (per 100 ms) and at the end of the reciprocating operation (per 1050 ms) with respect to FIG. Here, the occurrence of the reverse run-out at start-up or the jump at stop-down includes the possibility that the target plant 200 collides with another structure, and is therefore an important issue to be avoided by the position command controller. However, from the enlarged display of FIG. 10, it can be confirmed that the reverse swing at the time of startup and the jump at the time of stoppage occur at about 60 μm.

  In a BRF for removing a low frequency band, a high droop characteristic is required in order to reduce a position command trajectory error occurring before and after the filter. However, an FIR filter designed according to a known frequency sampling method or the like in accordance with this requirement includes a negative sign in a filter coefficient sequence, and is capable of responding to general acceleration / deceleration operation commands as shown in FIG. In addition, the filter is liable to generate reverse run-out at start-up and jumping at stop.

Japanese Patent Publication No. 1-19772

  As described above, when an FIR filter designed by a known frequency sampling method or the like is applied as a pre-filter for removing a low-frequency machine vibration frequency component from the position command value Xc, the filter order is high. As a result, the number of multiplications of the filter operation increases, and the operation processing load increases. Further, by designing the filter characteristics steeply, there is a feature that the reverse swing at the time of starting and the jump at the time of stopping are easily generated. The problem to be solved by the present invention is a linear (linear) phase characteristic, in which the calculation processing load of the filter operation is small, and even if the filter characteristic is designed steeply, the reverse swing at the start and the jump at the stop are prevented. An object of the present invention is to provide a position command control device equipped with a pre-filter having a feature that can be performed.

  The present invention provides a multi-band notch filter, a gain compensator composed of cascaded SMA (moving average) filters for compensating for a 0 Hz band rejection band of the multi-band notch filter, and a linear (linear) phase. The above problem is solved by forming a pre-filter using a delay device for ensuring characteristics.

  The filter according to the present invention has the steep droop characteristic and the linear (linear) phase characteristic of the notch filter, and can be configured by arithmetic processing centered on delay and addition / subtraction operations, so that the arithmetic processing load of the filter operation is reduced. . For this reason, when mounted on the position command control device as a pre-filter, it is possible to reduce machine vibration and suppress the occurrence of position command trajectory errors before and after the pre-filter. Can be prevented from jumping.

FIG. 3 is a block diagram illustrating a configuration example of a filter according to the present invention. FIG. 2 is a block diagram illustrating a configuration of an MCIC in FIG. 1. 5 is a graph showing an example of gain characteristics of each part of the filter of the present invention. 6 is a graph showing an example of gain characteristics of the filter of the present invention and a conventional FIR filter. 9 is a graph showing a filter output of the present invention when the position command value Xc in FIG. 8 is input. FIG. 6 is an enlarged graph of a position command value Xco output from the filter of the present invention in FIG. 5. It is a block diagram showing an example of a general position command control device. 6 is a graph illustrating an example of a reciprocating position command value Xc input to the position command control device. 9 is a graph showing a conventional FIR filter output when the position command value Xc of FIG. 8 is input. 10 is an enlarged graph of a position command value Xco output from the conventional FIR filter of FIG. 9.

  Hereinafter, the best mode for carrying out the present invention will be described using an example (hereinafter, referred to as an example). FIG. 1 is a block diagram showing an example of a filter according to the present invention, and is assumed to be applied as a pre-filter 1 in a position command control device 100.

The multi-band notch filter 2 is, for example, a technique known in "Beginners Digital Filter" (by Shogo Nakamura, 1st edition, Tokyo Denki University Press, published May 20, 1994, pp. 164-169). It is. Hereinafter, the configuration and characteristics of the multiband notch filter 2 will be described. MCIC (11a, · · ·, 11b) is Modefai de CIC filter, where _the MCIC of C AS (= 2n) stages are cascaded.

  FIG. 2 is a block diagram illustrating the internal operation of the MCIC. The delay unit 20 outputs an input signal N samplings (cycles: N) before the input signal of the MCIC. The output of the delay unit 20 is subtracted from the input signal of the MCIC to obtain the output of the subtractor in the current cycle. The adder 22 adds the output of the subtractor in the current cycle and the output of the delay unit 23 to obtain the output of the adder in the current cycle. The delay unit 23 outputs an adder output before P sampling. The adder output of the current cycle is attenuated to 1 / Q by the amplifier 24, and becomes the current cycle output of the MCIC. The current cycle output of the MCIC (the output of the amplifier 24) becomes the input signal of the MCIC at the subsequent stage or the input signal of the subtractor 13 described later.

Here, N, P, and Q are positive integers that satisfy the relationship of N = PQ (where Q ≧ 2). With this configuration, the transfer function H (z) of the MCIC can be expressed by equation (1).

よって、図１の多重バンドノッチフィルタ２において、ＭＣＩＣをＣ_ＡＳ（＝２ｎ）段、カスケードに接続した部分の周波数特性Ｈｄ（ｊω）は、式（３）となる。

Further, the frequency characteristic H (jω) of the MCIC can be expressed by Expression (2), where the sampling operation cycle is Ts.

Therefore, in multi-band notch filter 2 in FIG. 1, MCIC a _C AS (= 2n) stage, part of the frequency characteristic Hd connected in a cascade (j [omega]) is a formula (3).

The output of the multiband notch filter 2 is the output value of the subtractor 13. The output of the multiband notch filter 2 is generated by the delay unit 12 by subtracting the output of the cascade-connected MCIC from the input of the multiband notch filter 2 delayed by n (NP) sampling. Therefore, the frequency characteristic Hn (jω) of the multi-band notch filter 2 is represented by Expression (4).

In the gain characteristic of the multi-band notch filter 2 shown in the equation (4), ω at which the gain = 0 is a value at which the gain of the cascaded MCIC portion becomes 1, and sin (ωPTs / 2) which is a denominator. Is zero, that is, a value such that sin (ωPTs / 2) = 0. That is, ω at which the gain = 0 is a value satisfying the following equation (5).
ω = ωp
ωp = (2π / P) Ts ⁻¹ k (k = 0, 1, 2,...) (5)

  When ω = ωp, the numerator sin (ωNTs / 2) is also zero, that is, sin (ωNTs / 2) = 0. Therefore, this ωp is the peak gain for each band in the MCIC portion. That is, ωp expressed by Expression (5) is the notch angular frequency of the multi-band notch filter 2.

  Here, it is considered that the filter of the present invention is applied to a position command control device as a pre-filter. First, in equation (5), P is selected so that the machine vibration frequency to be removed corresponds to ωp at k = 1. Then, since there is almost no frequency component of the position command value corresponding to the notch angular frequency at k = 2, 3,..., The notch characteristic of k ≧ 2 affects the output of the pre-filter. Absent.

Next, consider returning the gain characteristic to 1 for ωp (= 0) at k = 0, that is, for a notch region in the 0 Hz band. The SMA (6a,..., 6d) in the gain compensator 4 of FIG. 1 is a moving average filter. Here, the transfer function Hs (z) and the frequency characteristic Hs (jω) when the SMA with the moving average cycle number T is connected to the cascade of C _AS (= 2n) are expressed by the following equations (6) and (7). Become.

Now, comparing Equations (4) and (7), if the moving average cycle number T is selected as T = N, the notch region in the 0 Hz band of Hn (jω) is equivalent to the gain characteristic in Hs (jω). The amount can be compensated. Therefore, in order to match the phase characteristics, a delay unit 3 for providing a delay of γ cycle is introduced in series before the multi-band notch filter 2, and γ is determined by Expression (8).
γ = n {(T-1)-(NP)} (8)

Furthermore, when reducing the number of stages of SMA, by utilizing the fact that drooping characteristic of the gain becomes gradual, so as to satisfy the equation _{(9), C AS (=} 2n) the SMA stage, _{C S_1} stage and _{C S_2} stage In the SMA block of the _{CS_1} stage, a delay unit 8 for giving a delay of β cycles is introduced, and β is determined by the equation (10).
C _AS = C S — ₁ + C S _—2 = 2n (9)
β = {(T-1) / 2} · _{CS_2} (...) (10)

Next, using the weighting constants alpha and (0 ≦ α <1), the SMA block _C AS (= 2n) stages in the gain compensator 4, weighting the SMA block _{C S_1} stage. The amplifier 7 has an amplification factor of (1−α) times and is arranged after the SMA block of C _AS (= 2n) stages. On the other hand, the amplifier 9 has an amplification factor of α times and is arranged after the delay unit 8 of the SMA block of the _{CS_1} stage.

  The outputs of the amplifier 7 and the amplifier 9 are added by the adder 10, and the output of the adder 10 becomes the output of the gain compensator 4. Since the gain compensating unit 4 is a compensating unit that compensates the notch area in the 0 Hz band and approximates the low-frequency gain to 1, the output is added to the output of the multiplex band notch filter 2 by the adder 5. This is the output of the filter according to the invention.

FIG. 3 shows an example of the gain characteristic of the filter according to the present invention. The conditions are, as in the case of the above-described conventional FIR filter, a sampling operation cycle Ts = 1 ms and a removal center frequency of 18.6 Hz. From equation (5), P = 1 / (fp · Ts) = 1 / (18.6 · 1 · 10 ⁻³ ) = 53.76 ··· 53, and n = 2 to give an appropriate drooping characteristic , Q = 3. From the relationship between N = PQ and T = N, N = T = 159. Then, from Expression (8), γ = 2 {(159-1) − (159-53)} = 104. Here, in Expression (9), C _AS = 2n = 4 is divided into C _{S_1} = 3 and C _{S_2} = 1. Therefore, from equation (10), β = {(159-1) / 2} · 1 = 79. The weight constant α is adjusted and selected as α = 0.35 from the low-frequency gain characteristic.

  Hereinafter, FIG. 3 will be described in relation to the block diagram of FIG. MNFL in FIG. 3 shows a gain characteristic of a block including the delay unit 3 and the multi-band notch filter 2 in FIG. Similarly, SMA (4) in FIG. 3 shows the gain characteristics of the block composed of the four-stage SMA and amplifier 7 in FIG. 1, and SMA (3) in FIG. 3 shows the three-stage SMA, delay unit 8 and amplifier in FIG. 9 shows the gain characteristic of the block.

  Since the phase characteristics of these three blocks match, the gain characteristic of the filter of the present invention obtained by adding the three blocks arranged in parallel is LNFL in FIG. 3, and the notch in the 0 Hz band of the multi-band notch filter is provided. It can be seen that the gain characteristic of the region can be approximated to 1.

  FIG. 4 is a graph comparing the gain characteristics of the LNFL of FIG. 3, which is an example of the filter of the present invention, with the above-described conventional FIR filter. LNFL can be regarded as a BRF of a notch filter characteristic having a linear phase characteristic from the configuration. For this reason, a low-frequency elimination BRF having a high drooping characteristic can be configured with a smaller filter operation processing load than a conventional FIR filter.

  Here, an example of the filter of the present invention is mounted on the pre-filter 1 of the position command control device 100 of FIG. 7 and a response operation when the reciprocating position command value Xc shown in FIG. 8 is input will be described. FIG. 5 shows the output (response) waveform of the pre-filter 1. FIG. 6 is an enlarged view of the position command value Xco at the time of start-up and at the end of the reciprocating operation as compared with FIG. From the enlarged display, it can be confirmed that no reverse swing at the time of start-up and no jump at the time of stoppage have occurred.

The filter of the present invention applied here has the same removal center frequency: 18.6 Hz, C _AS = 4, but changes Q = 3 to Q = 2 to widen the removal band and enhance the machine vibration suppression effect. ing. With a preference for the Q, N = T = 159 is to become a N = T = 106, _{C S_2} from equation (10) must be an even _{value, C S_1 = 2, C S_2} = 2 Choose The weight constant α is adjusted and selected as α = 0.35 from the low-frequency gain characteristic.

  As described above, the filter according to the present invention has the steep droop characteristic and the linear (linear) phase characteristic of the notch filter, and can be configured by arithmetic processing centering on delay and addition / subtraction operations. The processing load is reduced. For this reason, when mounted on the position command control device as a pre-filter, it is possible to reduce machine vibration and suppress the occurrence of position command trajectory errors before and after the pre-filter. Can be prevented.

  Further, in the above embodiment, the filter according to the present invention is mounted on the input unit of the position command control device as a pre-filter for the position command value. You can also.

  1 Pre-filter, 2 Multi-band notch filter, 3, 8, 12, 20, 23 Delay device, 4 Gain compensator, 5, 10, 22, 52, 55 Adder, 6a, 6b, 6c, 6d SMA, 7 , 9, 24, 58 amplifier, 11 MCIC, 13, 21, 50, 53 subtractor, 51 position deviation amplifier, 54 speed error amplifier, 56, 57 differentiator, 100 position command controller, 200 target plant.

Claims (2)

A multi-notch filter block, a first moving average filter block, and a second moving average filter block are arranged in parallel to an input signal, and output sums of the three filter blocks are output as output signals. ,
The multi-notch filter block is configured by connecting a multi-band notch filter in series at a stage subsequent to the first delay unit to which the input signal is input ,
The multi-band notch filter includes a modified CIC filter in which a predetermined number of stages are cascaded, a third delay that delays an output value of the first delay by a predetermined sampling, and the cascade connection from the third delay. A subtractor that outputs a value obtained by subtracting the output of the modified CIC filter.
The first moving average filter block is configured by serially connecting a first amplifier at a stage subsequent to the moving average filter in which the predetermined number of stages is cascaded,
The second moving average filter block is configured by serially connecting a second delay unit and a second amplifier to a stage subsequent to the moving average filter in which the number of stages smaller than the predetermined number of stages is cascaded.
A band elimination filter characterized by the above-mentioned. In a position command control device that drives the target plant by a servomotor and controls the position of the target plant according to a position command value instructed by a higher-level device,
A position command control device comprising the band removal filter according to claim 1 as a pre-filter for removing a specific frequency component from the position command value.

Images