JP6123327B2 - Digital filter circuit - Google Patents

Description

  The present invention relates to a digital filter circuit that removes noise contained in a position signal.

In order to remove noise contained in a signal or eliminate the influence of mechanical resonance, a digital filter circuit that realizes a low-pass filter or a notch filter by digital calculation is used. For example, for a robot that is a kind of production equipment, a rotary encoder is used to detect the position of a rotating shaft that constitutes an arm joint. However, since the position data output from the rotary encoder may contain noise, the noise may be removed by the digital filter circuit and input to the control circuit. For example, Patent Document 1 discloses an example thereof (however, a digital filter is applied to the two-phase signal of the encoder).
JP 2011-180073 A

Here, there are rotary encoders that detect the amount of rotation on the premise that there is a mechanical restriction on the rotation range, and those that are used for speed control in a state of infinite rotation. For example, when detecting the joint angle of an articulated robot, a so-called flange axis such as a horizontal 4-axis type fourth axis or a vertical 6-axis type sixth axis is capable of infinite rotation without mechanical limitations. It has become.
When positive and negative polarities are provided in the rotational direction of the shaft, 2's complement expression is generally used for the position data output from the rotary encoder. For example, in the case of 8-bit data, the positive value range is 1 to 127, and the negative value range is −1 to −128. Therefore, when the positive rotation position increases by one step from 7FH (= 127), it changes to 80H (= −128), and the position data becomes discontinuous.

  That is, as shown in FIG. 6, when the rotary shaft; rotary encoder continues to rotate in one direction, the data value increases in the positive direction and reaches the maximum value 127, then jumps to -128 and then increases in the positive direction. To do. At this time, for a while after the data value crosses the polarity, position data that does not make sense that the data value changes between 127 and -128 is output due to the action of the digital filter. Then, there is a problem that the position control cannot be normally performed due to these meaningless position data.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a digital filter circuit capable of filtering so as not to affect position control even when position data changes discontinuously.

  The digital filter circuit of the present invention is premised on that the response to the unit step input u (z) = z / (z−1) has a characteristic that converges to “1”. This means that the output value of the filter circuit finally converges to the same value as the input value. When the transfer function of the filter circuit is z-transformed and F (z), the function G (z) is {1-F (z)}. That is, the result of subtracting G (z) from the input value is equivalent to F (z).

Here, the above characteristic for the function F (z) is equivalent to the value of the function F (z) being “1” in the limit of z → 1 using the final value theorem. Therefore, if the limit of z → 1 is obtained for the function G (z), it becomes “0”. That is, since G (1) = 0, the function G (z) has a zero point at z = 1 by the factor theorem, and can be factorized as follows.
G (z) = {(z-1) / z} .H (z) (a)
Since the first term on the right side is (1-z ⁻¹ ), if the input is position data u, a function y having the following form with respect to u is equivalent to a function F (z).
y = u−u (1−z ⁻¹ ) · H (z) (b)
The term (1-z ⁻¹ ) is a filter that outputs the difference between the current input value u and the input value u · z ⁻¹ one (one sample) previous. Therefore, if the filter output is v, the output v is a speed. Although the position data is discontinuous in FIG. 6 earlier, the speed (position change amount per unit time) is continuous even if the position data is discontinuous. For example, even if the position data changes from 7EH (126) to 80H (-128) after elapse of unit time, the movement amount is 80H-7EH = 2H.
That is, even if the position data changes discontinuously, the speed change has continuity. Therefore, the digital filter circuit realized by the expression (b) can be filtered without affecting the position control without performing the behavior as shown in FIG.

The figure explaining the method which is 1st Embodiment and converts the structure of a digital filter Diagram showing the characteristics of the converted digital filter Functional block diagram showing the configuration of the robot controller FIG. 3 equivalent view showing the second embodiment FIG. 3 equivalent view showing the third embodiment The figure which shows the characteristic of the conventional digital filter

(First embodiment)
Hereinafter, the first embodiment will be described with reference to FIGS. 1 to 3. FIG. 3 is a functional block diagram illustrating a controller that controls a robot, which is an industrial device, with a focus on functional parts that control joints that constitute the arm of the robot. A rotary encoder 2 is disposed on a rotor portion (not shown) of the motor 1 forming the joint. The rotary encoder 2 counts the number of edge inputs of the two-phase pulse signal generated along with the rotation of the rotor by an up / down counter, and outputs a position signal as digital data. The position signal is input to the robot controller 3.

  The robot arm including the motor 1 is, for example, the horizontal 4-axis type fourth axis or the vertical 6-axis type sixth axis as described above, and is capable of infinite rotation without mechanical limitations. Structure. Positive and negative polarities are provided in the rotation direction of the joint axis, and 2's complement expression is used for the position data output from the rotary encoder 2.

  The digital filter 4 (digital filter circuit) built in the robot controller 3 is configured as a low-pass filter, removes noise included in the position signal output from the rotary encoder 2, and outputs it to the subtracter 5 and the differencer 6. . The subtracter 5 outputs a position deviation obtained by subtracting the encoder position signal from the position command input from the position command generation unit 7 to the position control unit 8. The position control unit 8 performs calculation based on the position deviation and generates and outputs a speed command.

  A subtracter 9 is disposed on the output side of the position controller 8. The difference unit 6 takes the difference between the current value and the previous value (one sample before) of the position data output from the digital filter 4 and outputs the difference to the subtracter 9. The subtractor 9 outputs a speed deviation obtained by subtracting the position data difference from the speed command to the speed control unit 10. When the speed control unit 10 calculates based on the speed deviation and generates a current command, the speed control unit 10 outputs the current command to the current control unit 11 in the next stage. The current control unit 11 includes a drive circuit such as an inverter circuit. When a PWM signal based on a current command is generated, the current control unit 11 generates an alternating current by switching a driving power source using the PWM signal, and fixes each phase of the motor 1. Output to the child winding.

Next, the configuration of the digital filter 4 in the present embodiment will be described with reference to FIGS. F (z) represents the frequency characteristic of the digital filter 4 expressed by z-transform. That is, as shown in FIG.
y = F (z) · u (1)
It becomes. here,
G (z) = 1-F (z) (2)
When the same filter as that shown in FIG. 1A is configured using the filter G (z) represented by the following formula, the result is as shown in FIG.

As for the characteristics of the filter F (z) of the present embodiment, if the unit step input u is expressed by z-transform, u (z)
u (z) = z / (z-1) (3)
It is assumed that the response to u (z) has a characteristic that converges to “1”. This means that if the input to the filter is a constant value a, the output of the filter will converge to the value a over time. In other words, when using a filter for the purpose of removing noise or removing the effects of mechanical resonance, the output value of the filter must match the input value in the absence of noise or vibration. .

ここで、変形したフィルタＧ（ｚ）についてｚ→１の極限を求めると、以下のようになる。

すなわち、Ｇ（１）＝０であるから、関数Ｇ（ｚ）はｚ＝１にゼロ点を有している。したがって、関数Ｇ（ｚ）は、以下のように因数分解することが可能である。

つまり（６）式は、図１（ｂ）に示すフィルタは、図１（ｃ）に示す構成のフィルタと等価であることを示している。 Using the final value theorem for the expression (3), it is expressed as follows.

Here, when the limit of z → 1 is obtained for the deformed filter G (z), it is as follows.

That is, since G (1) = 0, the function G (z) has a zero point at z = 1. Therefore, the function G (z) can be factored as follows.

That is, equation (6) indicates that the filter shown in FIG. 1B is equivalent to the filter having the configuration shown in FIG.

Moreover, the filter shown in FIG.1 (c) is represented by (7) Formula.
y = u−u (1−z ⁻¹ ) · H (z) (7)
Therefore, the filter shown in FIG. 1C is configured as shown in FIG. That is, in the digital filter 4, the input signal u is given to the subtractors 12 and 13 and the delay unit 14, and the subtracter 13 outputs a value obtained by subtracting the output of the delay unit 14 for one unit time from the input signal u. To do. The subtraction result is multiplied by a factor H (z) factorized in the multiplier 15 as a coefficient, and the subtracter 12 outputs a value obtained by subtracting the output of the multiplier 15 from the input signal u as an output signal y. To do.

Here, in the digital filter 4, (z-1) is not used as an arithmetic element,
(Z-1) / z = 1-z < ^-1 >
Is used as a computing element. That is, in the case of the calculation element (z−1), the difference between the input value one unit time ahead and the current input value is obtained, and it cannot be practically used. For this reason, the whole is delayed by one unit time to be (z−1) / z, thereby constituting an arithmetic element for obtaining the difference between the current input value and the input one unit time before.

  Further, since the input signal u is a position signal, the output signal v that has passed through the calculation element (z-1) / z has a speed. And even if the position is discontinuous, the speed is continuous. For example, in the above-described example, even if the position data 7EH (126) changes to 80H (−128) after the unit time has elapsed, the movement amount is 80H−7EH = 2H. Therefore, the filter H (z) to which the speed v is input functions as designed.

  That is, the digital filter 4 converts the filter F (z) into G (z) and configures the filter acting on the position u by factoring the differential term (z−1) / z. It can be said that the filter H (z) that acts on the velocity v is used without using an integral term that accumulates the error when a calculation error occurs. As a result, as shown in FIG. 2, even when the position signal becomes discontinuous, the behavior of the digital filter 4 is a value during the discontinuous change and does not output a transient value that cannot be used for control. .

（８）式を、サンプリング周波数Ｔｓで双一次変換して離散化すると、次式となる。

（９）式について、ｚ→１の極限を求めると、

となるから、単位ステップ応答が「１」に収束することが分かる。 <Specific examples of conversion>
Here, as an example, a case where the above-described method is applied to a primary low-pass filter _FLPF (s) having a cutoff frequency ωm given by the following equation will be described.

When the equation (8) is discretized by bilinear transformation at the sampling frequency Ts, the following equation is obtained.

When the limit of z → 1 is obtained for the equation (9),

Therefore, it can be seen that the unit step response converges to “1”.

＜本実施形態の手法で変換できない具体例＞
次の式で与えられる遮断周波数ωｎの１次ハイパスフィルタＦ_HPF（ｓ）について、上記の手法を適用してみる。

（１３）式を、サンプリング周波数Ｔｓで双一次変換して離散化すると、次式となる。

（１４）式について、ｚ→１の極限を求めると、

となるから、単位ステップ応答が「１」に収束しないことが分かる。 The filter G _LPF (z) obtained by _modifying the digital filter F _LPF (z) can be decomposed using the differential term (z−1) / z as a factor as follows.

<Specific examples that cannot be converted by the method of this embodiment>
The above method is applied to a first-order high-pass filter F _HPF (s) having a cutoff frequency ωn given by the following equation.

When the equation (13) is discretized by bilinear transformation at the sampling frequency Ts, the following equation is obtained.

For the equation (14), the limit of z → 1 is obtained.

Therefore, it can be seen that the unit step response does not converge to “1”.

Therefore, the filter G _HPF (z) obtained by _modifying the digital filter F _HPF (z) cannot be decomposed using the differential term (z−1) / z as a factor, as shown below.

As described above, according to the present embodiment, the position data output from the rotary encoder 2 is filtered for use in position control of the rotation axis of the robot arm, and the unit step input u (z) = z / (z For the digital filter 4 having a characteristic that the response to -1) converges to "1", the transfer function z-transformed is F (z).
A function represented by G (z) = 1−F (z) is expressed as G (z) = {(z−1) / z} · H (z).
Is expressed in a factorized form, and the output y with respect to the input u is configured in an operation form represented by equation (7). As a result, even if the position data u changes discontinuously, the change in the speed v is continuous, so the digital filter 4 realized by the equation (7) behaves as shown in FIG. Therefore, it is possible to perform the filtering so as not to affect the position control.

(Second Embodiment)
FIG. 4 is a view corresponding to FIG. 3 showing the second embodiment. The same parts as those of the first embodiment are denoted by the same reference numerals, description thereof is omitted, and different parts will be described. In the robot controller 21 shown in FIG. 4, a digital filter 22 (digital filter circuit) is inserted between the position command generator 7 and the subtracter 5. The digital filter 22 is configured by an arithmetic form in which a transfer function having a characteristic as a notch filter is decomposed by using a differential term (z−1) / z as a factor, like the digital filter 4.

  For example, when it is desired to suppress the vibration of the end effector of the robot arm, there is a method of preventing resonance by removing a vibration frequency lower than the operation band by arranging a notch filter on the output side of the position command generation unit 7. Be taken. Therefore, according to the second embodiment configured as described above, the digital filter 22 arranged on the output side of the position command generator 7 is also the same as that of the first embodiment for the purpose of preventing mechanical resonance. Similar effects can be obtained.

(Third embodiment)
FIG. 5 is a view corresponding to FIG. 3 showing the third embodiment, and only different portions from the second embodiment will be described. In the robot controller 31 shown in FIG. 5, the digital filter 4 arranged on the output side of the rotary encoder 2 is removed from the controller 21 of the second embodiment. That is, as described in the second embodiment, the configuration of the third embodiment is used only when the digital filter 22 is disposed on the output side of the position command generation unit 7 to prevent mechanical resonance. Should be adopted.

The present invention is not limited to the embodiments described above or shown in the drawings, and the following modifications or expansions are possible.
The present invention is not limited to a robot controller, and can be applied to any digital filter circuit that is used when position control is performed for other industrial equipment such as machine tools.

  2 is a rotary encoder, 3 is a robot controller, 4 is a digital filter (digital filter circuit), 21 is a robot controller, 22 is a digital filter (digital filter circuit), and 31 is a robot controller.

Claims (1)

This is a digital filter circuit that filters position data for use in position control of production equipment and has a characteristic that a response to a unit step input u (z) = z / (z−1) converges to “1”. And
If the transfer function of the filter circuit is z-transformed and F (z),
A function represented by G (z) = 1−F (z)
G (z) = {(z-1) / z} .H (z)
Is expressed in a factorized form, and the output y for the input u is
y = u−u (1−z ⁻¹ ) · H (z)
A digital filter circuit characterized in that it is configured in an arithmetic form.

Images