システム同定

1. 周波数特性とシステム同定
- 1.1. 周波数特性
- 1.2. Bode線図
- 1.3. システム同定の手順
- 1.4. 周波数特性の計測方法
  - 1.4.1. 市販の計測機器による計測
  - 1.4.2. 組み込みシステム内での実装による計測
2. 実システムの安定化
- 2.1. PD制御によるバネ・ダンパの付与
3. シミュレーション
4. 解析用Python関数
5. 実例
- 5.1. サーボモータ単体のシステム同定
  - 5.1.1. 加振中の様子
  - 5.1.2. 解析ソフト
- 5.2. 負荷ありサーボモータのシステム同定
  - 5.2.1. 2慣性系のモデル
  - 5.2.2. 加振中の様子
  - 5.2.3. 解析ソフト
  - 5.2.4. 2慣性系におけるシステム同定のコツ

1. 周波数特性とシステム同定

システム同定とは、実システムとモデルが同一（Identification）かどうか確かめる行為のことです。特に制御工学では周波数特性上でシステム同定することが大切です。
孫子の言葉に「彼を知り己を知れば百戦殆うからず」とある通り、制御対象の特徴と知ることは、それを制御する上でとても有利に働きます。そのため、システム同定は制御設計の中でも極めて重要な工程です。

1.1. 周波数特性

周波数特性とは、システムに単一正弦波を入力したときに出力される定常応答との、振幅および位相に関する相関関係を表したものです。

$\begin{aligned} In (t) & = A \sin (ω t + ω_{A}) \\ Out (t) & = B \sin (ω t + ω_{B}) \end{aligned}$

振幅： $\frac{B}{A} [倍] \to 20 \log_{10} \frac{B}{A} [dB]$
位相： $ω_{B} - ω_{A} [rad] \to \frac{180}{π} (ω_{B} - ω_{A}) [\deg]$

すでに伝達関数モデル $G (s)$ が得られている場合は、 $s$ に $j ω$ を代入して $j ω$ 解析を行うことで、振幅および位相の関係が得られます。

振幅： $| G (j ω) | [倍] \to 20 \log_{10} | G (j ω) | [dB]$
位相： $∠ G (j ω) [rad] \to \frac{180}{π} ∠ G (j ω) [\deg]$

周波数特性では、振幅は $dB$ 、位相は $\deg$ で表します。

覚えておくと便利な $dB$ 対応表

倍	$dB$
$1$	$0$
$2$	$6$
$3$	$10$
$10^{n}$	$20 n$

※ $1$ 倍と $10^{n}$ 倍以外は近似値
組み合わせれば色々なケースが分かるため、最低限、上表のものは覚えておくと便利です。例えば、下式のように $\sqrt{2}$ 倍、 $6$ 倍、 $5$ 倍の $dB$ 換算を簡単に導き出すことができます。

$\begin{aligned} \sqrt{2} [倍] & = 2^{1 / 2} [倍] \\ \to \frac{1}{2} 6 [dB] = 3 [dB] \\ 6 [倍] & = 2 [倍] \times 3 [倍] \\ \to 6 [dB] + 10 [dB] = 16 [dB] \\ 5 [倍] & = 10 [倍] \div 2 [倍] \\ \to 20 [dB] - 6 [dB] = 14 [dB] \end{aligned}$

定常応答と過渡応答
周波数特性は、あくまでシステムの定常応答の関係性について表したものです。定常応答とは、システムに信号を入力してから十分に時間が経ち、システムが定常状態になった後の出力応答のことです。従って、信号を入力した直後の過渡応答ついては、周波数特性からは直接読み取ることができません。一方で、制御工学の熟練者が、周波数特性を見ただけで過渡応答を含めた時間応答を瞬時に頭に思い描けるのは何故でしょうか？それは、周波数特性から伝達関数モデルを読み取り、（伝達関数モデルを逆ラプラス変換することで）過渡応答を含めた時間応答を導き出しているからです。

1.2. Bode線図

さきほどの単一正弦波の周波数を変化させていき、各周波数での振幅および位相の関係を、横軸を周波数、上段を振幅、下段を位相にとって周波数特性を描いたものをBode線図と言います。

Pythonでバネマスダンパ系のモデル $P (s)$ のBode線図を描いてみましょう。ここで、バネマスダンパ系とは、バネ $K$ 、マス（質量） $M$ 、ダンパ（粘性） $C$ 、加える力 $u (= f)$ 、位置 $y$ として、下図のようなメカ構成で表せます。

伝達関数モデルで表現すると、

$P (s) = \frac{y}{u} = \frac{1}{M s^{2} + C s + K}$

となります。ここでは並進系のモデルで説明していますが、回転系であっても、マスをイナーシャ、力をトルクと読み替えるだけで、同様の伝達関数モデルで表現できます。

Run_mck.py

# Copyright (c) 2021 Koichi Sakata


from pylib_sakata import init as init
# uncomment the follows when the file is executed in a Python console.
# init.close_all()
# init.clear_all()

import os
import shutil
import numpy as np
from pylib_sakata import ctrl
from pylib_sakata import plot

print('Start simulation!')

# Common parameters
figurefolderName = 'figure_mck'
if os.path.exists(figurefolderName):
    shutil.rmtree(figurefolderName)
os.makedirs(figurefolderName)
dataNum = 10000
freqrange = [1, 1000]
freq = np.logspace(np.log10(freqrange[0]), np.log10(freqrange[1]), dataNum, base=10)
print('Common parameters were set.')

# Plant model
M = 2.0
C = 10.0
K = M * (2.0 * np.pi * 10.0)**2
Pns = ctrl.tf([1], [M, C, K])
Pns_frd = ctrl.sys2frd(Pns, freq)
print('Plant model was set.')

print('Plotting figures...')
# Plant
fig = plot.makefig()
ax_mag = fig.add_subplot(211)
ax_phase = fig.add_subplot(212)
plot.plot_tffrd(ax_mag, ax_phase, Pns_frd, '-', 'b', 1.5, 1.0, title='Frequency response of plant')
plot.savefig(figurefolderName+'/freq_P.svg')

print('Finished.')

ここで、Bode線図を描く際には、周波数の単位を $ω [rad / s] = 2 π f$ に変換し、横軸の周波数を $f [Hz]$ とすると工学的に分かりが良いです。

バネマスダンパ系の共振周波数（自然周波数） $f_{n} [Hz]$ は、

$f_{n} = \frac{1}{2 π} \sqrt{\frac{K}{M}}$

であり、上図は、ちょうど $f_{n} = 10 [Hz]$ となるようにパラメータ設定したときのバネマスダンパ系のBode線図となります。

また、一般的な2次系の伝達関数 $G_{2} (s)$ は、

$G_{2} (s) = A \frac{ω_{n}^{2}}{s^{2} + 2 ζ ω_{n} s + ω_{n}^{2}}$

$ω_{n} = 2 π f_{n}$

のようにゲイン $A$ 、共振角周波数 $ω_{n} [rad / s]$ （共振周波数 $f_{n} [Hz]$ ）、減衰係数 $ζ$ を用いて表すことができ、バネマスダンパ系のモデルとして表現できます。

1.3. システム同定の手順

自分が想定している制御対象モデル $P (s)$ の周波数特性と、実システム（実際の制御対象）の入出力応答から得られた周波数特性をBode線図上で比較していきます。

上述の通り、制御対象モデル $P (s)$ の周波数特性は、 $s$ に $j ω$ を代入して $j ω$ 解析を行うことで、下記のとおり得られます。

振幅： $| P (j ω) | [倍] \to 20 \log_{10} | P (j ω) | [dB]$
位相： $∠ P (j ω) [rad] \to \frac{180}{π} ∠ P (j ω) [\deg]$

一方、実システムの周波数特性は、実際に加振信号を入力し、そのときの入出力応答から解析します。（実システムにおける計測方法は次節にて説明します。）

$\begin{aligned} In (t) & = A \sin (ω t + ω_{A}) \\ Out (t) & = B \sin (ω t + ω_{B}) \end{aligned}$

振幅： $\frac{B}{A} [倍] \to 20 \log_{10} \frac{B}{A} [dB]$
位相： $ω_{B} - ω_{A} [rad] \to \frac{180}{π} (ω_{B} - ω_{A}) [\deg]$

モデルと実システムの各周波数特性が得られたら、下図のフローチャートに従ってシステム同定を行います。

1.4. 周波数特性の計測方法

ここでは、実システムの周波数特性の計測方法として、市販の計測機器によるものと組み込みシステム内での実装によるものの2つを紹介します。

1.4.1. 市販の計測機器による計測

FFTアナライザやサーボアナライザといった名称で計測機器が販売されています。
市販の計測機器は、大まかに加振用の信号生成器と信号出力のDAC、計測したいシステムの前後の信号を取り込むADC、信号解析部からなり、外部PCなどに解析結果を出力することができます。

メリット
- 標準で様々な加振信号が準備されている。
- 計測機器内で周波数特性まで解析してくれる。
デメリット
- DAC/ADCによるノイズ・遅れが信号に混入する。
- 高分解能なDAC/ADCを必要とするため高価：それなりのもので100万円～200万円/台

1.4.2. 組み込みシステム内での実装による計測

市販のモータドライバの制御では物足りない等の理由で、組み込みシステム上に、自前でリアルタイム制御を実装できる環境を用意することを考えてみましょう。組み込みシステムは、マイコン、DSP、FPGAなどのリアルタイム処理に優れたチップ上にディジタル制御を実装することが一般的です。近年では、PCベースでも安定した高速リアルタイム処理が可能になってきており、例えば、Beckhoff Automation社製のIPC (Industrial PC)を使うと、Visual Studioの開発環境上でC/C++ベースでリアルタイム制御を実装することができ、お勧めです。

開発環境さえ用意できれば、下図のように、リアルタイムFB（フィードバック）制御器に加えて、加振用の信号生成器をプログラム上で実装することができます。D/A, A/D変換を介することなく、ディジタル制御で扱うディジタル信号のまま入出力時間信号を保存でき、PythonやMATLAB等の解析ソフトを使って周波数応答特性を得ることができます。

メリット
- 市販の計測機器を必要としない。
- ディジタル信号のまま扱うことができるため、信号の劣化が起きない。
デメリット
- 制御工学の知識が必要になるため敷居が高い。
- 開発コストがかかる。

2. 実システムの安定化

システムに加振信号を入力して周波数特性を得るためには、そのシステムが安定である必要があります。さもなければ、出力信号が発散してしまうためです。

前出のバネマスダンパ系は、バネで繋がっており元の位置に戻ろうする力が働くため、安定系です。制御工学的にシステムの安定性（安定か不安定か）については、そのシステムの極（もしくは固有値）によって判断できるのですが、それについては別の章で詳しく解説します。

モータの位置決めを目的とした場合、モータのモデルはバネマスダンパ系のバネが無いモデル

$P (s) = \frac{y}{u} = \frac{1}{M s^{2} + C s}$

で表現できます。このシステムは不安定なため、モータの位置決めにはシステムを安定化させるためにFB制御器が必要不可欠になります。

ですが、そもそも適切なFB制御器をモデルベースで設計したいがためにシステム同定をしようとしていましたね。このように、不安定系に対してシステム同定をしたい場合、どうすれば良いのでしょうか？

まずは兎にも角にもシステムを安定化させる必要があるため、公称値（カタログ値）に基づいてFB制御器を設計する必要があります。モータのカタログ値やメカ設計値から、マス（回転系の場合はイナーシャ）の公称値は分かるので、それに基づいてFB制御器を設計します。

Tips

実は、モータ制御において、力（トルク） $u$ から速度 $\dot{y}$ までのモデル

$P (s) = \frac{\dot{y}}{u} = \frac{1}{M s + C}$

は、粘性 $C$ があることで安定なシステムであり、FB制御器を組まずともシステム同定は可能です。しかしながら、モータの位置センサ（エンコーダ）から実速度を得るためには、差分演算により求める必要があります。このとき、エンコーダ分解能による量子化誤差のせいで、特に低速域では実速度の値がノイジーなります。そのため、できればエンコーダ値をそのまま使いたく、緩いFB制御器でシステムを安定化させるといった手間を掛けてでも、力（トルク）から位置までのシステムに対して同定することを推奨します。

2.1. PD制御によるバネ・ダンパの付与

モータの力（トルク）から位置までの伝達関数の周波数特性を計測するために、PD(Proportional Differential: 比例微分)制御を使ってシステムを安定化してみましょう。

ここで繰り返しになりますが、もしシステムが下図のようなバネマスダンパ系であれば安定なので、FB制御を付加せずとも $d$ に加振信号を入力し、

$y = \frac{1}{M s^{2} + C s + K} d$

のまま入出力応答を計測できます。

次に、バネ $K$ の無いモータモデルに対して、下図のようなPD制御器によるフィードバック制御で安定化されたシステムを見てみましょう。

フィードバック制御が無い場合は、

$y = \frac{1}{M s^{2} + C s} d$

で不安定だったシステムが、PD制御器によって、

$y = \frac{1}{M s^{2} + (C + K_{D}) s + K_{P}} d$

と $d$ から $y$ までの特性がバネマスダンパ系と同じ形になり、安定化することがわかります。これはフィードバックシステム全体で見ると、「PD制御器の比例ゲイン $K_{P}$ と微分ゲイン $K_{D}$ というバネとダンパが付加された」と言うこともできます。

これにより、緩めのバネ特性と適切なダンパ特性をPD制御器によって付加することでシステムを安定化しておき、 $d$ に加振信号を入力している最中のモータモデルの入力信号 $u + d$ から出力信号 $y$ までの入出力応答を計測することができます。

$y = \frac{1}{M s^{2} + C s} (u + d)$

3. シミュレーション

先ほどの組み込みシステム

において、制御対象 $P (s)$ を、

$P (s) = \frac{1}{M s^{2} + C s}$

とし、制御器 $C (s)$ を、PD制御器

$C (s) = K_{P} + \frac{K_{D} s}{τ_{D} s + 1}$

とします。さらに、加振信号 $Sig (t)$ を、指数関数的に周波数が時間変化するChirp信号

$Sig (t) = A \sin (2 π f_{\min} \frac{k^{t} - 1}{In (k)})$

としてシミュレーションしてみましょう。ただし、 $A$ はChirp信号の振幅、 $f_{\min}$ は $(t = 0)$ 時点の開始周波数、ある時間 $t$ での周波数は $f_{\min} k^{t}$ となります。

また、PD制御器の微分器には、先ほどの式には無かった時定数 $τ_{D}$ が含まれていますが、これは純粋な微分器である $s$ は理論上実装することが不可能なので実際にはこのような形で実装されます。本章はシステム同定に関する章なので、これに関しては制御器設計の章で詳しく解説します。

Pythonでの実行モジュールは以下の通りです。PD制御器を実装し、モータモデルを安定化した状態で、 $50 [s]$ かけて $0.1 [Hz]$ から $2.0 [kHz]$ まで周波数が指数関数的に変化するChirp信号を加振した時の入出力応答から解析した周波数応答と、 $j ω$ 解析して得られるモータモデルの周波数応答を比較しています。PD制御器は極配置設計という手法で設計していますが、こちらについても制御器設計の章で詳しく解説します。

Run_sysid.py

# Copyright (c) 2021 Koichi Sakata


from pylib_sakata import init as init
# uncomment the follows when the file is executed in a Python console.
# init.close_all()
# init.clear_all()

import os
import shutil
import numpy as np
from scipy import signal
from control import matlab
from pylib_sakata import ctrl
from pylib_sakata import plot
from pylib_sakata import fft

print('Start simulation!')

# Common parameters
figurefolderName = 'figure_sysid'
if os.path.exists(figurefolderName):
    shutil.rmtree(figurefolderName)
os.makedirs(figurefolderName)
Ts = 1/8000
dataNum = 10000
freqrange = [1, 1000]
freq = np.logspace(np.log10(freqrange[0]), np.log10(freqrange[1]), dataNum, base=10)
s = ctrl.tf([1, 0], [1])
z = ctrl.tf([1, 0], [1], Ts)
print('Common parameters were set.')

# Plant model
M = 2.0
C = 10.0
K = 0.0
Pmechs = ctrl.tf([1.0], [M, C, K])
Dz = z**-3
Pnz = ctrl.c2d(Pmechs, Ts, method='zoh') * Dz
Pnz_frd = ctrl.sys2frd(Pnz, freq)
print('Plant model was set.')

# Design PD controller
freq1 = 10.0
freq2 = 10.0
zeta2 = 1.0
Cz = ctrl.pd(freq1, freq2, zeta2, M, C, K, Ts)
Cz_frd = ctrl.sys2frd(Cz, freq)
print('PID controller was designed.')

print('System identification simulation is running...')
Snz = ctrl.feedback(Pnz, Cz, sys='S')
SPnz = ctrl.feedback(Pnz, Cz, sys='SP')
t = np.linspace(0.0, 50, int(50/Ts))
chirp = signal.chirp(t, f0=0.1, f1=2000, t1=50, method='logarithmic', phi=-90)
u, tout, xout = matlab.lsim(ctrl.tf2ss(Snz), chirp)
y, tout, xout = matlab.lsim(ctrl.tf2ss(SPnz), chirp)

fft_axis, chirp_fft = fft.fft(chirp, Ts)
fft_axis, u_fft = fft.fft(u, Ts)
fft_axis, y_fft = fft.fft(y, Ts)

Pmeas_frd, coh = fft.tfestimate(u, y, freq, Ts)

print('Plotting figures...')
# Time response
fig = plot.makefig()
ax1 = fig.add_subplot(311)
ax2 = fig.add_subplot(312)
ax3 = fig.add_subplot(313)
plot.plot_xy(ax1, t, chirp, '-', 'm', 0.5, 1.0, [0, 50], ylabel='Sig [N]', title='Time response')
plot.plot_xy(ax2, tout, u, '-', 'b', 0.5, 1.0, [0, 50], ylabel='In [N]')
plot.plot_xy(ax3, tout, y*1.0e3, '-', 'b', 0.5, 1.0, [0, 50], xlabel='Time [s]', ylabel='Out [mm]')
plot.savefig(figurefolderName+'/time_inject.svg')

fig = plot.makefig()
ax1 = fig.add_subplot(311)
ax2 = fig.add_subplot(312)
ax3 = fig.add_subplot(313)
plot.plot_xy(ax1, fft_axis, chirp_fft, '-', 'm', 1.5, 1.0, freqrange, ylabel='Sig [N]', title='Power spectrum density', xscale='log')
plot.plot_xy(ax2, fft_axis, u_fft, '-', 'b', 1.5, 1.0, freqrange, ylabel='In [N]', xscale='log')
plot.plot_xy(ax3, fft_axis, y_fft*1.0e6, '-', 'b', 1.5, 1.0, freqrange, xlabel='Frequency [Hz]', ylabel='Out [um]', xscale='log')
plot.savefig(figurefolderName+'/fft_inject.svg')

# Plant
fig = plot.makefig()
ax_mag = fig.add_subplot(311)
ax_phase = fig.add_subplot(312)
ax_coh = fig.add_subplot(313)
plot.plot_tffrd(ax_mag, ax_phase, Pmeas_frd, '-', 'm', 1.5, 1.0, ax_coh=ax_coh, coh=coh, title='Frequency response of plant')
plot.plot_tffrd(ax_mag, ax_phase, Pnz_frd, '--', 'b', 1.5, 1.0, freqrange, legend=['Measurement', 'Model'])
plot.savefig(figurefolderName+'/freq_P.svg')

print('Finished.')

モジュールを実行すると、以下2つの図が得られます。

上図は、加振信号 $Sig (t)$ 、入力信号 $In (t)$ 、出力信号 $Out (t)$ の時間応答です。

上図は、加振中の入出力時間応答から解析したシステムの周波数応答(Measurement)と、モデルの周波数応答(Model)の比較です。ほぼ一致しており、入出力時間応答から得られる周波数応答に基づいてシステム同定して差支えないことが分かります。図下段のCoherence（コヒーレンス）とは、入出力応答間の相関度合を示す指標で、 $0$ ～ $1$ の値を取り、 $1$ に近いほど相関が高いことを示しています。コヒーレンスの値が小さく相関が低いと、その領域の周波数応答の信頼性も低いと言えます。上図ではシミュレーションによる入出力応答を用いており、信号にノイズも入っていないため、コヒーレンスはほぼ $1$ に張り付いています。

4. 解析用Python関数

実際のシステム同定では、前節のようなシミュレーションではなく、同定したい実システムの制御系に対して加振信号 $Sig (t)$ を入力します。そのときの実システムの入力信号 $In (t)$ と出力信号 $Out (t)$ 、さらに加振中ということがわかるフラグ信号 $Flag (t)$ を保存して解析します。

Pythonパッケージpylib-sakataには、TwinCAT3 MeasurementのYTScope Projectで計測し保存したcsvデータから周波数応答データを得られる関数を用意しています。

pylib_sakata.meas.measdata2frd(filePath, inputName, outputName, flagName, freq, inputGain=1.0, outputGain=1.0, windivnum=4, overlap=0.5)

This function is for system identification from input and output time response data of measurement file.

Parameters:
filePath: file path of measurement file (.csv, .txt, and .mat are supported.)
inputName: input data name in the measurement file
outputName: output data name in the measurement file
flagName: flag data name in the measurement file
freq: 1-D array frequency data [Hz]
inputGain: inputdata gain (Optional), Default: 1.0, unit of input can be fixed by this parameter.
outputGain: outputdata gain (Optional), Default: 1.0, unit of output can be fixed by this parameter.
dt: sampling time of the time response data
windivnum: number of windows to divide the time response data
overlap: overlap ratio divided time response data (0 <= overlap < 1)
Returns:
freqresp: instance of FreqResp class
coh: 1-D array coherence data

class pylib_sakata.fft.FreqResp(freq, resp, dt=0)

Parameters:
freq: 1-D array frequency data [Hz]
resp: 1-D array frequency response data [complex data]
dt: sampling time (Optional), Default: 0, set the value >= 0. If dt = 0, the system is continuous time system.

Examples

import numpy as np
from pylib_sakata import meas

freq = np.logspace(np.log10(1.), np.log10(1000.), 10000, base=10)
freqresp, coh = meas.measdata2frd('data/001-inject.csv', 'ServoOut', 'PosErrUm', 'FlagNoise', freq, 1., 1.e-6)

5. 実例

前節で紹介したPython関数を用いて実際にシステム同定をした例を示します。

5.1. サーボモータ単体のシステム同定

BECKHOFFサーボモータAM8111-1F21単体のシステム同定の実例を示します。

5.1.1. 加振中の様子

TwinCAT3 Scope View (TE13xx)のYT chart機能を用いて、Chirp信号加振中のデータを取得している様子の動画です。

5.1.2. 解析ソフト

Chirp加振で得られた実データを用いてシステム同定するためのPython実行モジュールは以下の通りです。実データはコチラのfreq_resp.csvを用いています。

Run_sysid_exp.py

# Copyright (c) 2021 Koichi Sakata


from pylib_sakata import init as init
# uncomment the follows when the file is executed in a Python console.
# init.close_all()
# init.clear_all()

import os
import shutil
import numpy as np
from scipy import signal
from control import matlab
from pylib_sakata import ctrl
from pylib_sakata import plot
from pylib_sakata import fft
from pylib_sakata import meas

print('Start simulation!')

# Common parameters
figurefolderName = 'figure_sysid_exp'
if os.path.exists(figurefolderName):
    shutil.rmtree(figurefolderName)
os.makedirs(figurefolderName)
Ts = 1/8000
dataNum = 10000
freqrange = [1, 1000]
freq = np.logspace(np.log10(freqrange[0]), np.log10(freqrange[1]), dataNum, base=10)
s = ctrl.tf([1, 0], [1])
z = ctrl.tf([1, 0], [1], Ts)
print('Common parameters were set.')

# Plant model
M = 0.027
C = 0.7
K = 0.0
Pmechs = ctrl.tf([1], [M, C, K])
numDelay, denDelay = matlab.pade(Ts*4, n=4)
Ds = ctrl.tf(numDelay, denDelay)
Dz = z**-3
Pns = Pmechs * Ds
Pnz = ctrl.c2d(Pmechs, Ts, method='zoh') * Dz
Pnz_frd = ctrl.sys2frd(Pnz, freq)

print('Getting measurement data...')
measfileName = 'data/freq_resp.csv'
# Frequency response
Pmeas_frd, coh = meas.measdata2frd(measfileName, 'ServoOutN[0]', 'ActPosUm[0]', 'FlagInject', freq, 1., 1.e-6, 8, 0.8)

# Time response
measdata = meas.getdata(measfileName)
time = measdata.time
NoiseOut = measdata.value[meas.getdataindex(measdata, 'NoiseOut[0]')]
ServoOutN = measdata.value[meas.getdataindex(measdata, 'ServoOutN[0]')]
ActPosUm = measdata.value[meas.getdataindex(measdata, 'ActPosUm[0]')]
FlagInject = measdata.value[meas.getdataindex(measdata, 'FlagInject')]

print('Plotting figures...')
# Time response
fig = plot.makefig()
ax1 = fig.add_subplot(411)
ax2 = fig.add_subplot(412)
ax3 = fig.add_subplot(413)
ax4 = fig.add_subplot(414)
plot.plot_xy(ax1, time, NoiseOut, '-', 'b', 0.5, 1.0, ylabel='[Nm]', legend=['Sig'], loc='upper right', title='Time response')
plot.plot_xy(ax2, time, ServoOutN, '-', 'b', 0.5, 1.0, ylabel='[Nm]', legend=['In'], loc='upper right')
plot.plot_xy(ax3, time, ActPosUm, '-', 'b', 0.5, 1.0, ylabel='[um]', legend=['Out'], loc='upper right')
plot.plot_xy(ax4, time, FlagInject, '-', 'b', 0.5, 1.0, xlabel='Time [s]', ylabel='[.]', legend=['Flag'], loc='upper right')
plot.savefig(figurefolderName+'/time_resp.svg')

# Plant
fig = plot.makefig()
ax_mag = fig.add_subplot(311)
ax_phase = fig.add_subplot(312)
ax_coh = fig.add_subplot(313)
plot.plot_tffrd(ax_mag, ax_phase, Pmeas_frd, '-', 'm', 1.5, 1.0, ax_coh=ax_coh, coh=coh, title='Frequency response of plant')
plot.plot_tffrd(ax_mag, ax_phase, Pnz_frd, '--', 'b', 1.5, 1.0, freqrange, legend=['Measurement', 'Model'])
plot.savefig(figurefolderName+'/freq_P.svg')

print('Finished.')

モジュールを実行すると、以下2つの図が得られます。

上図は、加振信号 $Sig (t)$ 、入力信号 $In (t)$ 、出力信号 $Out (t)$ 、フラグ信号（加振中: $1$ ）の時間応答です。前節にて紹介したPython関数を用いると、実機にて取得した入出力時間応答から、実際の周波数応答特性を得られます。モデルの周波数応答特性と比較したものを下図に示します。

モデルのマス（ここでは回転系なのでイナーシャ） $M$ とダンパ $C$ の値を調整することで、図のように実際の制御対象とモデルの周波数特性を一致させていきます。一致すればするほど、そのモデルとパラメータは実システムの特性をよく表していると言えます。満足のいくモデルが得られたら、そのモデルに基づいた制御設計に進みます。

5.2. 負荷ありサーボモータのシステム同定

サーボモータは何かメカ機構を駆動するためのアクチュエータですので、実際の現場においてはモータ単体で駆動することはほぼなく、基本的に何かしらメカ機構が繋がっています。

ここでは、簡易的に3DCADで作製した機構をモータ回転軸に取付け、負荷がある時のサーボモータのシステム同定をしてみます。3DCADモデルとgcodeはコチラになります。

この負荷機構は、軸取付部材と外周の部材とが4枚の板バネで繋がっている構造になっており、回転方向のみ剛性が低く、その他の方向は拘束されています。これにより、負荷機構を取り付けた状態は、2慣性系となります。

5.2.1. 2慣性系のモデル

駆動側のマス（質量） $M_{1}$ 、負荷側のマス（質量） $M_{2}$ 、2つの質点を繋ぐバネ $K_{reso}$ 、ダンパ $C_{reso}$ 、加える力 $u (= f)$ 、駆動側の位置 $y_{1}$ 、負荷側の位置 $y_{2}$ として、下図のようなメカ構成で表せます。ここでは並進系の機構で説明しており、2質点系といいます。回転系の場合は2慣性系といい、マスをイナーシャ、力をトルクと読み替えるだけで、同様に表現できます。

2慣性系のモデルは、次式のように剛体モデル $P_{rigid} (s)$ と共振モデル $P_{reso} (s)$ の線形和で表すことができます。

$\begin{aligned} P_{1} (s) & = \frac{y_{1}}{u} = P_{rigid} (s) + k_{1} P_{reso} (s) \\ P_{2} (s) & = \frac{y_{2}}{u} = P_{rigid} (s) + k_{2} P_{reso} (s) \end{aligned}$

ここで、 $P_{1} (s)$ は力（トルク） $u$ から駆動側の位置 $y_{1}$ までの伝達関数、 $P_{2} (s)$ は力（トルク） $u$ から負荷側の位置 $y_{2}$ までの伝達関数です。また、剛体モデル $P_{rigid} (s)$ と共振モデル $P_{reso} (s)$ は、次式で表されます。

$\begin{aligned} P_{rigid} (s) & = \frac{1}{(M_{1} + M_{2}) s^{2} + C s} \\ P_{reso} (s) & = \frac{1}{s^{2} + 2 ζ ω s + ω^{2}} \end{aligned}$

モード影響定数 $k_{1}$ , $k_{2}$ は、駆動側および負荷側のマス（イナーシャ）の関係で決まり、次式のように求まります。

$\begin{aligned} k_{1} & = \frac{M_{2}}{M_{1} (M_{1} + M_{2})} \\ k_{2} & = \frac{- 1}{M_{1} + M_{2}} \end{aligned}$

共振角周波数 $ω [rad / s]$ と減衰係数 $ζ$ は、次式のように求まります。

$\begin{aligned} ω & = \sqrt{\frac{K_{reso} (M_{1} + M_{2})}{M_{1} M_{2}}} \\ ζ & = \frac{C_{reso}}{2} \sqrt{\frac{M_{1} + M_{2}}{K_{reso} M_{1} M_{2}}} \end{aligned}$

5.2.2. 加振中の様子

モータ単体と同様に、負荷を取り付けた状態でTwinCAT3 Scope View (TE13xx)のYT chart機能を用いて、Chirp信号加振中のデータを取得している様子の動画です。

5.2.3. 解析ソフト

モータ単体と同様に、Chirp加振で得られた実データを用いてシステム同定するためのPython実行モジュールは以下の通りです。実データはコチラのfreq_resp_2mass.csvを用いています。

Run_sysid_2mass_exp.py

# Copyright (c) 2021 Koichi Sakata


from pylib_sakata import init as init
# uncomment the follows when the file is executed in a Python console.
# init.close_all()
# init.clear_all()

import os
import shutil
import numpy as np
from scipy import signal
from control import matlab
from pylib_sakata import ctrl
from pylib_sakata import plot
from pylib_sakata import fft
from pylib_sakata import meas

print('Start simulation!')

# Common parameters
figurefolderName = 'figure_sysid_2mass_exp'
if os.path.exists(figurefolderName):
    shutil.rmtree(figurefolderName)
os.makedirs(figurefolderName)
Ts = 1/8000
dataNum = 10000
freqrange = [1, 1000]
freq = np.logspace(np.log10(freqrange[0]), np.log10(freqrange[1]), dataNum, base=10)
s = ctrl.tf([1, 0], [1])
z = ctrl.tf([1, 0], [1], Ts)
print('Common parameters were set.')

# Plant model
M1 = 0.03
M2 = 0.08
M = M1 + M2
C = 2.4
K = 0.0
Creso = 3.5
Kreso = 68000.0
k1 = M2/(M1 * (M1 + M2))
k2 = -1.0/(M1 + M2)
omegaPreso = np.sqrt(Kreso * (M1 + M2)/(M1 * M2))
zetaPreso = 0.5 * Creso*np.sqrt((M1 + M2)/(Kreso * M1 * M2))
Pmechs1 = ctrl.tf([1], [M, C, K]) + k1 * ctrl.tf([1], [1, 2*zetaPreso*omegaPreso, omegaPreso**2])
Pmechs2 = ctrl.tf([1], [M, C, K]) + k2 * ctrl.tf([1], [1, 2*zetaPreso*omegaPreso, omegaPreso**2])
numDelay, denDelay = matlab.pade(Ts*4, n=4)
Ds = ctrl.tf(numDelay, denDelay)
Dz = z**-3
Pns1 = Pmechs1 * Ds
Pns2 = Pmechs2 * Ds
Pnz1 = ctrl.c2d(Pmechs1, Ts, method='zoh') * Dz
Pnz2 = ctrl.c2d(Pmechs2, Ts, method='zoh') * Dz
Pnz1_frd = ctrl.sys2frd(Pnz1, freq)
Pnz2_frd = ctrl.sys2frd(Pnz2, freq)
Pnz = Pnz1
Pnz_frd = Pnz1_frd
print('Plant model was set.')

print('Getting measurement data...')
measfileName = 'data/freq_resp_2mass.csv'
# Frequency response
Pmeas_frd, coh = meas.measdata2frd(measfileName, 'ServoOutN[0]', 'ActPosUm[0]', 'FlagInject', freq, 1., 1.e-6, 8, 0.8)

# Time response
measdata = meas.getdata(measfileName)
time = measdata.time
NoiseOut = measdata.value[meas.getdataindex(measdata, 'NoiseOut[0]')]
ServoOutN = measdata.value[meas.getdataindex(measdata, 'ServoOutN[0]')]
ActPosUm = measdata.value[meas.getdataindex(measdata, 'ActPosUm[0]')]
FlagInject = measdata.value[meas.getdataindex(measdata, 'FlagInject')]

print('Plotting figures...')
# Time response
fig = plot.makefig()
ax1 = fig.add_subplot(411)
ax2 = fig.add_subplot(412)
ax3 = fig.add_subplot(413)
ax4 = fig.add_subplot(414)
plot.plot_xy(ax1, time, NoiseOut, '-', 'b', 0.5, 1.0, ylabel='[Nm]', legend=['Sig'], loc='upper right', title='Time response')
plot.plot_xy(ax2, time, ServoOutN, '-', 'b', 0.5, 1.0, ylabel='[Nm]', legend=['In'], loc='upper right')
plot.plot_xy(ax3, time, ActPosUm, '-', 'b', 0.5, 1.0, ylabel='[um]', legend=['Out'], loc='upper right')
plot.plot_xy(ax4, time, FlagInject, '-', 'b', 0.5, 1.0, xlabel='Time [s]', ylabel='[.]', legend=['Flag'], loc='upper right')
plot.savefig(figurefolderName+'/time_resp.svg')

# Plant
fig = plot.makefig()
ax_mag = fig.add_subplot(311)
ax_phase = fig.add_subplot(312)
ax_coh = fig.add_subplot(313)
plot.plot_tffrd(ax_mag, ax_phase, Pmeas_frd, '-', 'm', 1.5, 1.0, ax_coh=ax_coh, coh=coh, title='Frequency response of plant')
plot.plot_tffrd(ax_mag, ax_phase, Pnz_frd, '--', 'b', 1.5, 1.0, freqrange, legend=['Measurement', 'Model'])
plot.savefig(figurefolderName+'/freq_P.svg')

print('Finished.')

モジュールを実行すると、モータ単体と同様に、以下2つの図が得られます。

5.2.4. 2慣性系におけるシステム同定のコツ

モータ単体のシステム同定では、マス（イナーシャ） $M$ とダンパ $C$ の2つのパラメータをフィッテイングするだけなので容易にシステム同定できました。一方、2慣性系はフィッテイングしなければならないパラメータが5つもあり、どこから手をつけたら良いか分からないと思います。実は2慣性系におけるシステム同定のコツがあります。

はじめに、剛体モデル $P_{rigid} (s)$ のマスラインを合わせることで、 $M_{1} + M_{2}$ の値が定まります。次に、共振周波数 $f_{reso} [Hz]$ と反共振周波数 $f_{anti} [Hz]$ をグラフから読み取ります。

$\begin{aligned} f_{reso} & = \frac{1}{2 π} \sqrt{\frac{K_{reso} (M_{1} + M_{2})}{M_{1} M_{2}}} \\ f_{anti} & = \frac{1}{2 π} \sqrt{\frac{K_{reso}}{M_{2}}} \end{aligned}$

の式から、

$f_{reso} = f_{anti} \sqrt{\frac{(M_{1} + M_{2})}{M_{1}}}$

となり、はじめにフィッテイングしたマスライン $M_{1} + M_{2}$ およびグラフから読み取った共振周波数 $f_{reso}$ と反共振周波数 $f_{anti}$ の値を使ってそれぞれの駆動側のマス $M_{1}$ が定まります。

続いて共振周波数 $f_{reso}$ もしくは反共振周波数 $f_{anti}$ の式から負荷側のマス $M_{2}$ 、バネ $K_{reso}$ も定まります。

最後に共振のダンパ $C_{reso}$ を調整することで共振周波数 $f_{reso}$ のゲインの高さを調整します。剛体のダンパ $C$ の調整はモータ単体時と同様です。

以上より、2慣性系もきれいにシステム同定することができました。