ノッチフィルタ

信号処理
2022.12.04

はじめに

振動制御によく用いられるノッチフィルタを離散系でCで実装してみました。

環境

  • M1 Mac Monterey 12.4
  • gcc version 10.2
    • Cで実装します
  • Matlab(R2021a)
    *　ボード線図が見たいため
  • gnuplot(version 5.4)
    • 描画用

参考文献

ノッチフィルタを理解する

ナノスケールサーボ制御

ノッチフィルタ

  • ある周波数だけを落とすように作られたフィルタ

ノッチフィルタはバンドストップフィルタの一種で、これは特定範囲内の周波数を減衰する一方、他のすべての周波数は変化させることなく通過させるフィルタである。ノッチフィルタの場合、この周波数範囲が非常に狭い。 電気/電子用語の定義：NOTCH FILTER

連続時間伝達関数

  • 2次ノッチフィルタの連続時間伝達関数は$$G(s) = \frac{ s^2 + 2d\xi\omega_{N}s + \omega_{N}^2 }{s^2 + 2\xi\omega_{N}s + \omega_{N}^2}$$

となります。ここで\(s\)はラプラス演算子，\( \xi \)はノッチの幅，\(d\)はノッチの深さ,\(\omega\)はカットしたい周波数です。

とりあえずノッチフィルタのボード線図

  • Matlabでボード線図を書いてみます。ノッチフィルタは高域で用いられることが多いのでサンプリング周期10 kHzとした場合のナイキスト周波数5000[Hz]の0.1倍として除去したい周波数を決めました。
    *　\(\omega = 3.14*10^3\), \(\xi = 1\), \(d = 0.001\)とした場合
MATLAB
%ノッチフィルタ確認
Ts    = 100e-6               %サンプリング周期
f     = 1/Ts                 %サンプリング周波数
nyquistfreq = f/2            %ナイキスト周波数
omega = nyquistfreq*0.1*2*pi %ナイキストの0.1倍をカットオフ周波数とする
d     = 0.001                %ノッチ深さ
xi    = 1                    %ノッチ幅

num = [1 2*d*xi*omega omega*omega]
den = [1 2*xi*omega omega*omega]

%連続時間ノッチ
notch_s = tf(num, den)
%ボード線図
bode(notch_s)
legend("連続ノッチ")
fig1.png

\(3.14*10^3\) rad/sでゲインが落ちていることが確認できます。

離散時間伝達関数

  • z変換
    • z変換は連続時間を離散時間に変換するのに用いられます。
    • ラプラス領域とz領域の関係は$$z = e^{sT} \\ s = \frac{ln(z)}{T}$$
    • 詳細は以下URL参考
      フィルタの離散化について

双一次変換

  • 連続時間を離散時間に変換するには基本的には双一次変換を用います。
  • s領域の極が左反平面にあれば安定であるという現象が，z領域では単位円内にあれば安定であるというのに対応するからです。
  • 双一次変換は$$s = \frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}$$

これを連続時間の伝達関数sに代入してやることで離散系に直すことができます。

ノッチフィルタの離散系

  • 双一次変換はz変換の近似であるため(離散系でlogを表現することができない)，ノッチフィルタでは角周波数が離散系にするとずれてしまう（周波数歪み）恐れがあります。特にナイキスト周波数に近づくほど誤差が大きくなります。
  • ノッチの周波数を 500 Hzとした場合の離散系の結果は以下の通りとなります。
MATLAB
    %連続から離散変換
    Hd = c2d(notch_s,Ts)
    bode(notch_s,Hd)
    legend("連続ノッチ","プリワープ処理なし離散ノッチ")
fig2.png
  • 周波数ひずみによりノッチの周波数がずれていることがわかります。
  • そのため，あらかじめ$w$を補正しておく必要があります（プリワープ処理）$$ \omega_{修正} = \frac{2}{T}\tan{\left(\omega_{N} \frac{T}{2}\right)}$$
  • プリワープした周波数をノッチのカットオフ周波数と見なして双一次変換すれば良いです。
  • 双一次変換後のノッチフィルタの伝達関数
    • 地道に代入していきます$$ G[z] = \frac{s^2 + 2d\xi\omega_{修正}s + \omega_{修正}^2}{s^2 + 2 \xi \omega_{修正}s + \omega_{修正}^2 } $$
  • ここで以下とおく。$$ \omega_{修正} = \frac{2}{T}\tan{(\omega_N \frac{T}{2})} = \frac{2}{T} k\\ k = \tan{ \left(\omega_{N} \frac{T}{2} \right)}$$
  • 上記と双一次変換の式を代入して$$G[z]= \frac{s^2 + 2d\xi(\frac{2}{T} k)s + (\frac{2}{T} k)^2}{s^2 + 2 \xi (\frac{2}{T} k)s + (\frac{2}{T} k)^2 } \\ = \frac{（\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}）^2 + 2d\xi(\frac{2}{T} k)(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}) + (\frac{2}{T} k)^2}{(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}})^2 + 2 \xi (\frac{2}{T} k)(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}) + (\frac{2}{T} k)^2 } \\ = \frac{4(1-z^{-1})^2 + 8d\xi k(1-z^{-1})(1+z^{-1}) + 4k^2(1+z^{-1})^2}{4(1-z^{-1})^2 + 8\xi k(1-z^{-1})(1+z^{-1}) + 4k^2(1+z^{-1})^2}\\ =\frac{ (1 – 2d \xi k + k^2)z^{-2} + (2 k^2 -2)z^{-1} + (1 + 2d\xi k + k^2) }{(1 – 2\xi k + k^2)z^{-2} + (2k^2 -2)z^{-1} + (1 + 2\xi k + k^2)} \\$$
  • \(\omega_N\)はプリワープ処理する前のカットオフ周波数であることに注意

プリワープ処理後の離散ノッチの確認

  • 連続時間のノッチフィルタと計算したプリワープ処理後のノッチフィルタを比較します。
  • 実はこんなややこしい計算をしなくてもmatlabには便利な機能があるので，そちらとも比較します。
    • matlabのc2dオプションの’prewarp’と比較します。
    • 参考↓
    • 特に高次の伝達関数の場合は’matched’オプションを用いるのが良い(ナノスケールサーボ)
MATLAB
%%離散系ノッチフィルタ
%プリワープ処理 omega_n をomega_pとみなして計算
omega_p = 2.0/Ts*tan(omega*Ts/2.0) % wp = 2/T*tan(w*T/2) = 2/T*k

k = tan(omega*Ts/2.0)
%分子多項式
N2 = 1.0 - 2*d*xi*k + k*k;
N1 = 2.0*k*k - 2.0;
N0 = 1.0 + 2*d*xi*k + k*k;
%分母多項式
D2 = 1.0 - 2*xi*k + k*k;
D1 = 2.0*k*k - 2.0;
D0 = 1.0 + 2*xi*k + k*k;    

num_d = [N0 N1 N2]
den_d = [D0 D1 D2]
%プリワープ処理後の離散系ノッチ伝達関数
notch_d = tf(num_d,den_d,Ts)
% From matlab関数
Hdp = c2d(notch_s,Ts,'prewarp',omega);

%ボード線図比較
bode(notch_s,"-bo",notch_d,"-rx",Hdp,"-c^")
%xlim([3130, 3150])

legend("連続ノッチ","プリワープ処理後離散ノッチ","Matlab　プリワープノッチ")
3.png
4.png
  1. プリワープ処理した離散系ノッチのノッチ周波数が連続系ノッチのそれとほぼ一致している\(\omega_N = 3.14*10^3 \)
  2. 高域だと離散系ノッチの位相が連続系ノッチと少しずれる
  3. matlabに搭載のc2dの’prewarp’とほぼ同じ結果が得られたと思う。

ノッチフィルタ実装(c言語)

  • 上記のノッチフィルタの関数（信号，除去したい周波数，ノッチ深さ，ノッチ幅）
C
double Notch_Jisaku(double u, double omega, double d, double xi){
    double y;

    k = tan(omega*Ts/2.0);
    // 分子多項式
    N2 = 1 - 2*d*xi*k + k*k;
    N1 = 2.0*k*k - 2.0;
    N0 = 1.0 + 2*d*xi*k + k*k;
    // 分母多項式
    D2 = 1 - 2*xi*k + k*k;
    D1 = 2.0*k*k - 2.0;
    D0 = 1.0 + 2*xi*k + k*k;    

    y = ( N2*uZ2 + N1*uZ1 + N0*u - D2*yZ2 - D1*yZ1 )/D0;

    uZ2=uZ1;
    uZ1=u;
    yZ2=yZ1;
    yZ1=y;

    return y;
}
  • ノッチフィルタ動作確認
    • 周波数1 Hzの正弦波に除去したい周波数を入れてみて，ノッチフィルタを使うことで除去できるか確認してみます。
    • パラメータは上記と同様。\(d=0.001\)，\( \xi=1\), \(\omega = 3.14*10^3\) [rad/s]としてみました。 $$u(t) = sin(2\pi t) + sin(\omega_N t) $$
C
    // notch
    const double d         = 0.001;
    const double xi        = 1;
    double omega = 1/Ts/2*0.1*2*3.14159;

    notest_u = sin(2.0*3.14*t) + sin(omega*t);//除去したい周波数が入っている
    noreal_u = sin(2.0*3.14*t);//元信号
    // ノッチ自作
    notest_y = Notch_Jisaku(notest_u, omega, d, xi);
  • gnuplotでの描画結果
    notch.png
  • 初期の値はあまり除去されていませんが0.002秒(20サンプルほど)後ほどには真値の信号に近い値にフィルタリングしています。サンプリングタイムは100μsです。離散フィルタの遅れによるものだと考えます。
  • またノッチフィルタを使うと遅れることもわかります。

まとめ

ノッチフィルタを離散系をCで実装し，matlabでノッチフィルタのボード線図を連続・離散両方で確認しました。おおよそノッチによる周波数除去ができたと思います。

ノッチフィルタは高域では周波数ひずみの影響でノッチがずれるおそれがあるのでプリワープ処理をすることで改善することがわかりました。

ノッチフィルタは位相遅れを伴うので多数のノッチフィルタを組み合わせることが多いそうです。

2慣性系にノッチフィルタを入れることで振動が収まるかを今後は検証したいと思います。

コメント

タイトルとURLをコピーしました