今回はバネマスダンパ系のゲイン線図に生じる極大値を除去するノッチフィルタを記述する.
周波数特性は状態空間表現からbode関数を使うと容易に求められるが、今回はそのようなメソッドやライブラリを一切使わずにfor文のみでシミュレーションする。そのほうが勉強になる。
システム
今回の制御対象は以下の図で示すバネマスダンパ系である.
このシステムの伝達関数、ゲイン特性、位相特性は以下のようになる。
\begin{gather}
G(s) = \frac{1}{ms^2+cs+k}\\
G_{log}(w)=20\log_{10}|\frac{Kw_{n}^2}{\sqrt{(w_{n}^2-w^2)^2+4 \zeta^2 w_{n}^2w^2}}| \rm{[dB]}\\\\
\angle G(w)=\tan^{-1}\frac{-2\zeta w_{n}w}{w_{n}^2-w^2}
\end{gather}
この導出は以下の記事で詳細に書いた。
そして時間応答を図２に、ボード線図を図3に示す.
($m = 5,~ k = 20, ~c = 1, ~f=1$としている)
ノッチフィルタとは
ノッチフィルタの伝達関数は以下のように表される.
\begin{gather}
G(s)=\frac{s^2+\zeta_n w_r s+w_r^2}{s^2+2\zeta_dw_r s+w_r^2} \tag{1}
\end{gather}
ここで$\zeta_n<\zeta_d$である.
状態空間表現にする(実現問題)
(1)式を逆ラプラス変換で時間領域に直す。
分母と分子で分けて考える。
\begin{gather}
X(s)=\frac{1}{s^2+2\zeta_d w_r s+ w_r^2}U(s) \tag{2}\\
Y(s) = (s^2+2\zeta_n w_r s +w_r^2)X(s)\tag{3}
\end{gather}
これらを逆ラプラス変換すると、
\begin{gather}
\ddot{x}(t)+2\zeta_d w_r \dot{x}(t)+w_r^2 x(t) = u(t) \tag{4}\\
y(t) = \ddot{x}+2\zeta_n w_r \dot{x}(t)+w_r^2x(t)\tag{5}
\end{gather}
ここで$x_1 = x, ~x_2 = \dot{x_1} = \dot{x}$とすると、
(4)式と(5)式は
\begin{gather}
\dot{x}_{2}+2\zeta_d w_r x_2 +w_r^2 x_1 =u\\
y = \dot{x}_2+2\zeta_n w_r x_2+w_r^2 x_1
\end{gather}
と表せる.ここで$\dot{x}_2 = u-2\zeta_d w_r x_2-w_r^2 x_1$であるので、状態空間表現にすると次のようになる。
\begin{gather}
\begin{bmatrix}
\dot{x}_1\\
\dot{x}_2\\
\end{bmatrix}
=
\begin{bmatrix}
0&1\\
-w_r^2 & -2\zeta_d w_r
\end{bmatrix}
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}
+
\begin{bmatrix}
0\\
1
\end{bmatrix}
u\\
y(t) = \begin{bmatrix}0 &2w_r(\zeta_n-\zeta_d)\end{bmatrix}
\begin{bmatrix}
x_1\\
x_2
\end{bmatrix}
+
\begin{bmatrix}0\\
1\end{bmatrix}u\\
\end{gather}
ノッチフィルタ単体のボード線図
(1)式のボード線図を書くために、フーリエ変換(s=jwを代入)する
\begin{align}
G(jw) &= \frac{(jw)^2+2\zeta_nw_r(jw)+w_r^2}{(jw)^2+2\zeta_dw_r(jw)+w_r^2}\\
&=\frac{w_r^2-w^2+2\zeta_nw_rwj}{w_r^2-w^2+2\zeta_dw_rwj}
\end{align}
ゲインは、
\begin{align}
|G(w)| &= \frac{\sqrt{(w_r^2-w^2)^2+(2w_rw\zeta_n)^2}}{\sqrt{(w_r^2-w^2)^2+(2w_rw\zeta_d)^2}}\\
&=\frac{\sqrt{w_r^4+w^4+2w_r^2w^2(2\zeta_n^2-1)}}{\sqrt{w_r^4+w^4+2w_r^2w^2(2\zeta_d^2-1)}}
\end{align}
位相は、
\begin{align}
\tan^{-1}\frac{2\zeta_nw_rw}{(w_r^2-w^2)}-\tan^{-1}\frac{2\zeta_dw_rw}{(w_r^2-w^2)}
\end{align}
ここで、
仮に$w_r$ = 2, $\zeta_d$ = 0.5, $\zeta_n$ = 0.0001とすると、ボード線図は図4のようになります。
matlab(.m)
ゲインは、伝達関数の分子と分母の割り算で求められ、移送は引き算で求められる.
これは指数法則に則っているためである。
clear
close all
Tsim = 20;
sampleT = 0.01;
omega_r = 2;
zeta_d = 0.5;
zeta_n = 0.0001;
%--- -- 時間応答 ------
A = [0 1; -omega_r^2 -2*zeta_d*omega_r];
b = [0; 1];
c = [0 -2*omega_r*zeta_d];
d = [0 1];
x0 = [0; 0];
Tn = round ( Tsim / sampleT );
f = ones (1 , Tn +1);
x_s = x0 ;
t_s = 0;
w = 0.1:0.01:100;
for i= 1:length(w)
G_1(i) = sqrt(omega_r^4+w(i)^4+2*omega_r^2*w(i)^2*(2*zeta_n^2-1));
G_2(i) = sqrt(omega_r^4+w(i)^4+2*omega_r^2*w(i)^2*(2*zeta_d^2-1));
G(i) = G_1(i)/G_2(i);
Gl(i) = 20*log(G(i));
Ph_1(i) = atan2 ( 2*zeta_n*omega_r*w(i) ,omega_r^2-w(i)^2);
Ph_2(i) = atan2 ( 2*zeta_d*omega_r*w(i) ,omega_r^2-w(i)^2);
ph(i) = Ph_1(i)-Ph_2(i);
ph(i) = ph(i)*180/pi;
end
%--- -- Figure --------
fss = 12;
fig = figure
subplot (211)
a11 = semilogx (w , Gl ); grid ;
set ( gca ,'linewidth',1 , 'fontsize' , fss )
set ( a11 ,'linewidth',2);
ylabel ('Gain[db]', 'fontsize', fss );
subplot (212)
a12 = semilogx (w ,ph ); grid ;
set ( gca ,'linewidth',1 , 'fontsize' , fss )
set ( a12 ,'linewidth',2);
ylabel ('Phase[deg]','fontsize', fss );
xlabel ( 'omega[rad/s]' ,'fontsize', fss );
Python(.py)
import numpy as np
import matplotlib.pyplot as plt
# Simulation parameters
Tsim = 20
sampleT = 0.01
omega_r = 2
zeta_d = 0.5
zeta_n = 0.0001
# Time response
A = np.array([[0, 1], [-omega_r**2, -2*zeta_d*omega_r]])
b = np.array([[0], [1]])
c = np.array([0, -2*omega_r*zeta_d])
d = np.array([0, 1])
x0 = np.array([0, 0])
Tn = round(Tsim / sampleT)
f = np.ones(Tn + 1)
x_s = x0
t_s = 0
# Frequency response
w = np.arange(0.1, 100, 0.01)
G_1 = np.sqrt(omega_r**4 + w**4 + 2*omega_r**2 * w**2 * (2*zeta_n**2 - 1))
G_2 = np.sqrt(omega_r**4 + w**4 + 2*omega_r**2 * w**2 * (2*zeta_d**2 - 1))
G = G_1 / G_2
Gl = 20 * np.log10(G)
Ph_1 = np.arctan2(2*zeta_n*omega_r*w, omega_r**2 - w**2)
Ph_2 = np.arctan2(2*zeta_d*omega_r*w, omega_r**2 - w**2)
ph = Ph_1 - Ph_2
ph = np.degrees(ph)
# Plotting
fss = 12
fig,axes = plt.subplots(2, 1, figsize=(10, 8))
axes.flatten()
axes[0].semilogx(w, Gl, linewidth=2)
axes[0].grid()
axes[0].set_ylabel('Gain [dB]', fontsize=fss)
axes[0].tick_params(axis='both', which='major', labelsize=fss)
axes[1].semilogx(w, ph, linewidth=2)
axes[1].grid()
axes[1].set_ylabel('Phase [deg]', fontsize=fss)
axes[1].set_xlabel('omega [rad/s]', fontsize=fss)
axes[1].tick_params(axis='both', which='major', labelsize=fss)
plt.tight_layout()
plt.show()
ノッチフィルタを加えたシステムの時間応答と周波数特性
上記の章で述べたノッチフィルタをバネマスダンパシステムに加えて時間応答と周波数応答を調べる。
どちらも、ノッチフィルタを加えることで、どう変化するのか確認する
時間応答
ノッチフィルタとバネマスダンパ系の伝達関数を掛け合わせて、
（上の実現問題のように）可制御系にすることもできる。
しかし、ここでは複雑な計算を少なくするために、ノッチフィルタの出力がバネマスダンバ系の入力となるためルンゲクッタ法を二つつなげる方法でプログラムを書く。
ちなみに実現問題として解くと次のようになる。
間違っているかもしれませんが。
時間応答を確認するために、バネマスダンパ系の伝達関数とノッチフィルタの伝達関数をかけて、
状態空間表現にする.(実現問題)
\begin{gather}
G(s)=\frac{s^2+\zeta_n w_r s+w_r^2}{s^2+2\zeta_dw_r s+w_r^2}\times\frac{1}{ms^2+cs+k}\\
=\frac{s^2+\zeta_n w_r s+w_r^2}{ms^4+cs^3+ks^2+2\zeta_dw_rms^3+2\zeta_dw_rcs^2+2\zeta_dw_rks+mw_r^2s^2+cw_r^2s+kw_r^2}\\
=\frac{s^2+\zeta_n w_r s+w_r^2}{ms^4+(c+2m\zeta_dw_r)s^3+(k+2c\zeta_dw_r+mw_r^2)s^2+(2\zeta_dkw_r+cw_r^2)s+kw_r^2}
\end{gather}
これを分母と分子で分けます。
\begin{gather}
X(s) = \frac{1}{ms^4+(c+2m\zeta_dw_r)s^3+(k+2c\zeta_dw_r+mw_r^2)s^2+(2\zeta_dkw_r+cw_r^2)s+kw_r^2}U(s)\\
Y(s) = (s^2+\zeta_n w_r s+w_r^2)X(s)
\end{gather}
これらを逆ラプラス変換すると
\begin{gather}
m\ddddot{x}(t)+C_1\dddot{x}(t)+C_2\ddot{x}(t)+C_3\dot{x}+kw_r^2x(t) = u(t) \tag{6}\\
y(t) = \ddot{x}+2\zeta_n w_r \dot{x}(t)+w_r^2x(t) \tag{7}
\end{gather}
ここで$C_1,C_2,C_3$はそれぞれ以下に示す通りである.
\begin{gather}
C_1=c+2m\zeta_dw_r\\
C_2 = k+2c\zeta_dw_r+mw_r^2\\
C_3 = 2\zeta_dw_rk+cw_r^2
\end{gather}
ここで、
\begin{align}
&x_{1} = x\\
&x_{2} = \dot{x}_1=\dot{x}\\
&x_{3} = \dot{x}_2 = \ddot{x}_1 = \ddot{x}\\
&x_{4} = \dot{x}_3 = \ddot{x}_{2} = \dddot{x}_{1} = \dddot{x}
\end{align}
とすると、(6)式は次のように示せる。
\begin{gather}
m\dot{x}_4+C_1x_4+C_2x_3+C_3x_2+kw_r^2x_1 = u(t)
\end{gather}
また,(7)式は次のように示せる.
\begin{gather}
y = x_3+2\zeta_nw_rx_2+w_r^2x_1
\end{gather}
したがって状態空間表現は
\begin{gather}
\begin{bmatrix}
\dot{x}_1\\
\dot{x}_2\\
\dot{x}_3\\
\dot{x}_4
\end{bmatrix}=
\begin{bmatrix}
0&1&0&0\\
0&0&1&0\\
0&0&0&1\\
-\frac{kw_r^2}{m}&-\frac{C_3}{m}&-\frac{C_2}{m}&-\frac{C_1}{m}
\end{bmatrix}
\begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4
\end{bmatrix}+
\begin{bmatrix}
0\\
0\\
0\\
1
\end{bmatrix}u
\end{gather}
\begin{gather}
y = \begin{bmatrix}
w_r^2&2\zeta_n w_r&1&0
\end{bmatrix}
\begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4
\end{bmatrix}
\end{gather}
時間応答
ボード線図はlogの性質上、掛け算を足し算に置き換えることができる。
これを利用したのが「重ね合わせの原理」である。
重ね合わせの原理を使わず、ノッチフィルタの周波数特性を求めたときと同様、以下のようにごりおすこともできる。
時間応答と周波数応答のプログラム(.m)
clear
close all
%時間応答はノッチフィルタの出力がバネマスダンパの入力となるようにする
%。周波数特性は重ね合わせの原理を使う。
Tsim = 20;
sampleT = 0.01;
Tn = round(Tsim/sampleT);
%パラメータ＝＝
omega_r = 2;%阻止周波数
zeta_d = 0.5;
zeta_n = 0.0001;
m = 5;
k = 20;
c =1;
omega = sqrt(k/m);%2
zeta = c/2/sqrt(m*k);
K = 1/k;
f = ones(1,Tn+1);
x0 = [0; 0];
x_s = x0 ;
t_s = 0;
%状態方程式
%ノッチフィルタ
A_N = [0 1; -omega_r^2 -2*zeta_d*omega_r];
B_N = [0; 1];
C_N = [0 -2*omega_r*zeta_d];
D_N = [0 1];
%バネマスダンパ系
A = [0 1; - omega ^2 -2* zeta * omega ];
B = [0; K * omega ^2];
C = [1 0];
f = ones (1 , Tn +1);
%ノッチフィルタ無しのときの距離ｘと力ｆの時間応答
for i = 1: Tn+1
t_s(i+1) = i*sampleT;
k1 = A*x_s(:,i)+B*f(i);
k2 = A*(x_s(:,i)+sampleT*k1/2)+B*f(i);
k3 = A*(x_s(:,i)+sampleT*k2/2)+B*f(i);
k4 = A*(x_s(:,i)+sampleT*k3)+B*f(i);
x_s(:,i+1) = x_s(:,i)+sampleT*(k1+2*k2+2*k3+k4)/6;
t_o(i) = t_s(i);
x_o(i) = C*x_s(:,i);
end
%バネマスダンパonlyノッチフィルタ無しの周波数特性
w =0.1:0.01:100;
for i = 1:length(w)
gain(i) = K*omega^2/sqrt(omega^4+w(i)^4+2*(2*zeta^2-1)*omega^2*w(i)^2);
gain_sys(i) = 20*log10(gain(i));
phase_sys(i) = atan2(-2*zeta*omega*w(i),omega^2-w(i)^2);
end
%ノッチフィルタありの時の距離と力の時間応答
%fpreからfを導く。ノッチフィルタの状態方程式を使って。
x_s_onlyNochi = x0;
for i = 1: Tn+1
k1 = A_N*x_s_onlyNochi(:,i)+B_N*f(i);
k2 = A_N*(x_s_onlyNochi(:,i)+sampleT*k1/2)+B_N*f(i);
k3 = A_N*(x_s_onlyNochi(:,i)+sampleT*k2/2)+B_N*f(i);
k4 = A_N*(x_s_onlyNochi(:,i)+sampleT*k3)+B_N*f(i);
x_s_onlyNochi(:,i+1) = x_s_onlyNochi(:,i)+sampleT*(k1+2*k2+2*k3+k4)/6;
x_o_onlyNoch(i) = C_N*x_s_onlyNochi(:,i)+f(i);
end
x0N = [0;0];
x_sn = x0N;
t_sN = 0;
for i = 1: Tn+1
t_sN(i+1) = i*sampleT;
k1 = A*x_sn(:,i)+B*x_o_onlyNoch(i);
k2 = A*(x_sn(:,i)+sampleT*k1/2)+B*x_o_onlyNoch(i);
k3 = A*(x_sn(:,i)+sampleT*k2/2)+B*x_o_onlyNoch(i);
k4 = A*(x_sn(:,i)+sampleT*k3)+B*x_o_onlyNoch(i);
x_sn(:,i+1) = x_sn(:,i)+sampleT*(k1+2*k2+2*k3+k4)/6;
t_o_N(i) = t_sN(i);
x_o_N(i) = C*x_sn(:,i);
end
x_o_N(Tn)
%ノッチフィルタ単体での周波数特性
for i= 1:length(w)
G_1(i) = sqrt(omega_r^4+w(i)^4+2*omega_r^2*w(i)^2*(2*zeta_n^2-1));
G_2(i) = sqrt(omega_r^4+w(i)^4+2*omega_r^2*w(i)^2*(2*zeta_d^2-1));
G(i) = G_1(i)/G_2(i);
Gl_noti(i) = 20*log(G(i));
Ph_1(i) = atan2 ( 2*zeta_n*omega_r*w(i) ,omega_r^2-w(i)^2);
Ph_2(i) = atan2 ( 2*zeta_d*omega_r*w(i) ,omega_r^2-w(i)^2);
ph(i) = Ph_1(i)-Ph_2(i);
ph_noti(i) = ph(i);
end
% 重ね合わせの原理(ノッチフィルタ + バネ・マス・ダンパ系)
ph_noti=ph_noti*180/pi;
phase_sys=phase_sys*180/pi;
gain_sum = Gl_noti + gain_sys;
phase_sum = (ph_noti + phase_sys);
fig = figure
%力
subplot(411);
plot(t_o,f,'linewidth',2,t_o,x_o_onlyNoch,'linewidth',2);
set(gca,'fontsize',20)
%plot(t_o,f,t_o,x_o,'linewidth',2);
grid on;
legend('fpre','f','Location','northeastoutside');
xlabel('time','fontsize',20);
ylabel('f[N]','fontsize',20);
%距離
subplot(412);
plot(t_o,x_o,'linewidth',2,t_o,x_o_N,'linewidth',2);
set(gca,'fontsize',20)
grid on;
legend('Nashi','Ari','Location','northeastoutside')
xlabel('time[s]','fontsize',20);
ylabel('x[m]','fontsize',20);
subplot(413);
semilogx(w,gain_sys,'linewidth',2,w,gain_sum,'linewidth',2);
set(gca,'fontsize',20)
grid on;
ylabel('Gain_Nashi[deg]','fontsize',20);
xlabel('omega[rad/s]','fontsize',20);
legend('Nashi','ari');
subplot(414);
semilogx(w,phase_sys,'linewidth',2,w,phase_sum,'linewidth',2);
set(gca,'fontsize',20)
grid on;
ylabel('Phase_Nashi[deg]','fontsize',20);
xlabel('omega[rad/s]','fontsize',20);
legend('nashi','ari');
%saveas(fig,'2.3.jpeg');
結果のグラフは上から、
力(青＝ノッチフィルタなし、オレンジ＝ノッチフィルタあり)
変位(青＝ノッチフィルタなし、オレンジ＝ノッチフィルタあり)
ゲイン線図(青＝ノッチフィルタなし、オレンジ＝ノッチフィルタあり)
位相線図(青＝ノッチフィルタなし、オレンジ＝ノッチフィルタあり)
である。
ノッチフィルタがない場合、ボード線図を見ると、共振周波数である2rad/s
(正確には$w_d = \sqrt{1-\zeta^2}w_n=\sqrt{1-0.05^2}\times2=1.997$rad/s)でゲインが高くなっているのがわかるが、ノッチフィルタがある場合はそれが抑えられている.
固有振動数をもった力を入力したとき
上では１Nという定数しか加えていないが、
固有振動数を加えた場合ちゃんと共振を抑制するのか確認する。
変更点は以下の通りである。
%f = ones (1 , Tn +1);
for j=1: Tn+1
T_s(j+1)=j*sampleT;
T_o(j)=T_s(j);
f(j)=sin(omega*T_o(j));
end
時間変化がわかりやすいように、100ｓまで確認する。
上から二つ目の変位の結果を見ると、青のノッチフィルタなしは共振してしまっているが、オレンジのノッチフィルタありの場合は抑えられていることが確認できる。
入力の振動数をずらすと(f(j)=sin((omega-1)*T_o(j));)
以下のように位相がずれてはいるが周期的外力がシステムに伝わっていることが確認できる。
コメント