サイトマップ

Pythonによる制御工学入門 第2回

制御工学 Python 数値シミュレーション

Pythonによる制御工学入門 第2回

参考書の4.5「周波数応答」を自作の数値シミュレーションで再現する
専用モジュールを使えば一瞬だが、
今回は、各周波数の入力に対する応答を取得して、原始的に求める

実際の機器で求める場合も、同様の処理が必要なはず

周波数応答

まずは、controlモジュールを使って、周波数応答を確認する

1次遅れ系のボード線図

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import control.matlab as pyctrl

K = 1
T = [1, 0.5, 0.1]

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)

for i in range(len(T)):
    P = pyctrl.tf([0, K], [T[i], 1])
    gain, phase, w = pyctrl.bode(P, pyctrl.logspace(-2,2), plot=False)
    ax1.plot(w/(2*np.pi), 20*np.log10(gain), label='T=%.1f' % T[i])
    ax2.plot(w/(2*np.pi), phase*180/np.pi, label='T=%.1f' % T[i])

ax1.set_ylim(-45, 10); ax1.set_yticks([-40, -20, 0])
ax2.set_ylim(-95, 5); ax2.set_yticks([-90, -45, 0])
ax1.legend(); ax1.grid(); ax1.set_xscale('log')
ax1.set_ylabel('Gain [dB]')
ax2.legend(); ax2.grid(); ax2.set_xscale('log')
ax2.set_xlabel('[Hz]')
ax2.set_ylabel('Phase [deg]')
fig.tight_layout()

2次遅れ系のボード線図

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import control.matlab as pyctrl

zeta = [1, 0.7, 0.4]
omega_n = 1

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)

for i in range(len(zeta)):
    P = pyctrl.tf([0, omega_n**2], [1, 2*zeta[i]*omega_n, omega_n**2])
    gain, phase, w = pyctrl.bode(P, pyctrl.logspace(-2,2), plot=False)
    ax1.plot(w/(2*np.pi), 20*np.log10(gain), label='$\zeta$=%.1f' % zeta[i])
    ax2.plot(w/(2*np.pi), phase*180/np.pi, label='$\zeta$=%.1f' % zeta[i])

ax1.set_ylim(-90, 10); ax1.set_yticks([-80, -40, 0])
ax2.set_ylim(-190, 10); ax2.set_yticks([-180, -90, 0])
ax1.legend(); ax1.grid(); ax1.set_xscale('log')
ax1.set_ylabel('Gain [dB]')
ax2.legend(); ax2.grid(); ax2.set_xscale('log')
ax2.set_xlabel('[Hz]')
ax2.set_ylabel('Phase [deg]')
fig.tight_layout()

2次遅れ系のボード線図2

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import control.matlab as pyctrl

zeta = 0.7
omega_n = [1, 5, 10]

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)

for i in range(len(omega_n)):
    P = pyctrl.tf([0, omega_n[i]**2],[1, 2*zeta*omega_n[i], omega_n[i]**2])
    gain, phase, w = pyctrl.bode(P, pyctrl.logspace(-2,2), plot=False)
    ax1.plot(w/(2*np.pi), 20*np.log10(gain), label='$\omega_n$=%.1f' % omega_n[i])
    ax2.plot(w/(2*np.pi), phase*180/np.pi, label='$\omega_n$=%.1f' % omega_n[i])

ax1.set_ylim(-90, 10); ax1.set_yticks([-80, -40, 0])
ax2.set_ylim(-190, 10); ax2.set_yticks([-180, -90, 0])
ax1.legend(); ax1.grid(); ax1.set_xscale('log')
ax1.set_ylabel('Gain [dB]')
ax2.legend(); ax2.grid(); ax2.set_xscale('log')
ax2.set_xlabel('[Hz]')
ax2.set_ylabel('Phase [deg]')
fig.tight_layout()

数値演算による2次遅れ系のボード線図

今回の主題
各周波数の正弦波を入力としてモデルに入れて、
その際の出力からゲインの変化と位相の変化を求める

まずは、入れる入力の周波数を決める
0.01Hzから10Hzまでを等比で20分割するとすると、
0.01 \times r^{20} = 10 なので、対数を取ると、
-2 + 20 log_{10} r = 1 となり
r = 10^{3 / 20} となる

hz_s = []
hz = 0.01
while True:
    if hz > 10 * 10 ** (3 / 20):
        break
    hz_s.append(hz)
    hz = hz * 10 ** (3 / 20)

各周波数でシミュレーションを回す
時間分解能は0.1msで5sec回し、3~5secを保存する
モデルのパラメータは下記の設定

import numpy as np
import pandas as pd

(K, zeta, omega_n) = (1.0, 0.4, 5)

dt = 0.0001

df_list = []
for hz in hz_s:
    t_s = [0.0]
    x_s = [0.0]
    v_s = [0.0]
    a_s = [0.0]
    F_s = [0.0]

    for i in range(50000):
        if i < 100:
            F = 0.0
        else:
            F = 1.0 * np.sin(2.0 * np.pi * hz * t_s[-1])
        F_s += [F]
        a_s += [- 2.0 * zeta * omega_n * v_s[-1] - omega_n**2 * x_s[-1] + K * omega_n**2 * F]
        v_s += [v_s[-1] + a_s[-1] * dt]
        x_s += [x_s[-1] + v_s[-1] * dt]
        t_s += [t_s[-1] + dt]

    df = pd.DataFrame()
    df['in']  = F_s[30000:]
    df['out'] = x_s[30000:]
    df.index  = t_s[30000:]
    df_list += [df]

次に、解析のための関数を定義する
主に、Fittingで求めた振幅と位相を調整する関数

from scipy.optimize import curve_fit

def sin_func(x, p0, p1, p2):
    return p0 * np.sin(2.0 * np.pi * p1 * x + p2)

def _convert_phase(phase):
    phase += 180
    phase %= 360
    if phase < 0:
        phase += 180
    else:
        phase -= 180
    return phase

def _convert_param(param):
    param = param.copy()
    param[2] = param[2] / np.pi * 180
    if param[0] < 0:
        param[0] = -param[0]
        param[2] += 180

    param[2] = _convert_phase(param[2])

    return param

def calc_f_func(param1, param2):
    param1 = _convert_param(param1)
    param2 = _convert_param(param2)

    freq = param1[1]
    gain = param2[0] / param1[0]
    phase = param2[2] - param1[2]
    phase = _convert_phase(phase)

    return param1, param2, freq, gain, phase

各周波数のデータに対して解析を行う
各データをFittingして、intとoutの振幅差からゲインを、位相差から位相変化を求める

data_s = []
for hz,df in zip(hz_s, df_list):
    p0 = [1.0, hz, 0.0]
    p1, conv = curve_fit(sin_func, np.array(df.index), np.array(df['in']), p0=p0)

    p0 = [1.0, hz, 0.0]
    p2, conv = curve_fit(sin_func, np.array(df.index), np.array(df['out']), p0=p0)

    param1, param2, freq, gain, phase = calc_f_func(p1, p2)

    data_s.append([freq, gain, phase])

df_res = pd.DataFrame(data_s)
df_res.columns = ['freq', 'gain', 'phase']

最後に、controlモジュールとの結果を比較して表示する

import matplotlib.pyplot as plt
import control.matlab as pyctrl

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)

ax1.plot(df_res['freq'], np.log10(df_res['gain'])*20.0, 'o-', label='sim')
ax2.plot(df_res['freq'], df_res['phase'], 'o-', label='sim')

zeta, omega_n = (0.4, 5)
P = pyctrl.tf([0, omega_n**2],[1, 2*zeta*omega_n, omega_n**2])
gain, phase, w = pyctrl.bode(P, pyctrl.logspace(-2,2), plot=False)
ax1.plot(w/(2*np.pi), 20*np.log10(gain), label='pyctrl')
ax2.plot(w/(2*np.pi), phase*180/np.pi, label='pyctrl')

ax1.set_ylim(-90, 10); ax1.set_yticks([-80, -40, 0])
ax2.set_ylim(-190, 10); ax2.set_yticks([-180, -90, 0])
ax1.legend(); ax1.grid(); ax1.set_xscale('log')
ax1.set_ylabel('Gain [dB]')
ax2.legend(); ax2.grid(); ax2.set_xscale('log')
ax2.set_xlabel('[Hz]')
ax2.set_ylabel('Phase [deg]')

fig.tight_layout()

きちんと求めることができている

おまけ：確認方法

実際の機器で解析を行うと、ノイズなどで、正確にFittingが行えない場合がある
その場合は、きちんと in / out のデータを確認する

i = 15
hz = hz_s[i]
df = df_list[i]
p0 = [1.0, hz, 0.0]
p1, conv = curve_fit(sin_func, np.array(df.index), np.array(df['in']), p0=p0)

p0 = [1.0, hz, 0.0]
p2, conv = curve_fit(sin_func, np.array(df.index), np.array(df['out']), p0=p0)

param1, param2, freq, gain, phase = calc_f_func(p1, p2)

fig = plt.figure(figsize=(6, 5))
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2, sharex=ax1, sharey=ax1)

ax1.plot(df.index, df['in'], label='raw')
ax1.plot(df.index, sin_func(df.index, param1[0], param1[1], param1[2] / 180 * np.pi), label='fit')

ax2.plot(df.index, df['out'], label='raw')
ax2.plot(df.index, sin_func(df.index, param2[0], param2[1], param2[2] / 180 * np.pi), label='fit')

ax1.set_xlabel('t'); ax1.set_ylabel('in'); ax1.grid(); ax1.legend()
ax1.set_title('%.2f Hz' % hz)
ax2.set_xlabel('t'); ax2.set_ylabel('out'); ax2.grid(); ax2.legend()

fig.tight_layout()

今回の場合は、生データとFitting結果がほぼ一致している

また、今回は、全条件（周波数）で同じ条件（サンプル0.1ms/2sec分）でFittingを行ったが、
入力する周波数に合わせて、サンプル周期や時間を変えた方が精度が良くなる

参考文献

この記事は以下の書籍を参考にしましたが、
私の拙い知識で書いておりますので、誤り等ありましたらご指摘ください

Pythonによる制御工学入門

Pythonによる制御工学入門

Amazon