ArduinoとMATLAB/Simulinkを用いたDCモータのシステム同定

## モデルベース設計のはじめに

今回は, DCモータの数式によるモデリングと、実験データに基づいたシステム同定を行います。

ArduinoとMATLAB/Simulinkを連携させてDCモータを速度制御してみる
http://manao55.hatenablog.com/entry/2019/09/11/232544

の続きになります。(今回初めてQiitaに投稿してみました。お手柔らかにお願いします。)

制御を行うには, プラントの状態をよく把握し, プラントに合わせた制御器を設計することが大切です。

自転車に初めて乗るとき, 最初は怖がりながらもペダルをこいでゆき, 加速, ブレーキ, ハンドリングなどの感覚をつかんでゆき, 慣れていくと, 普通に乗ることができます。

人間は自転車に乗る過程で, 自転車の挙動を推定（？）してゆきます。今回は, 挙動を知る手段についてのテーマです。 


制御系設計では制御対象を数式で表現した（数式）モデルを作成し, それに基づいて設計を行うことが慣習的です。モデルベース開発と呼ばれていますが, そのもととなる, システム同定についてのお話です。

## DCモータの回路方程式, 運動方程式
では, DCモータの数式モデルを詳しく見ていきましょう。
下図のような, DCモータのプラントについて, 数式を追っていきます![Motorplant2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/bdb9979e-a998-421b-e82d-93973a7beb9a.jpeg)



Raは巻き線抵抗, Laは巻き線のインダクタンスであり, モータへ電圧uを加えると電機子には電流iが流れます。モータは回転すると, それによって逆起電力が発生します。（手回し発電機と同じ原理です）逆起電力はDCモータの回転速度に比例します。 

逆起電力定数をKe, 電機子の回転各速度をω(t)とすると, Ke*ω(t)と表すことができます。
抵抗での電圧降下Ra*i(t), コイルの電圧降下La*di(t)/dt, モータの逆起電力Ke*ω(t)をすべて加えたものがモータの印加電圧u(t)と釣り合うので, 電気回路の微分方程式は,

$$ R_{a}i(t) + L_{a}\frac{di(t)}{dt} + K_{e}\omega(t) = u(t)$$

となります。DCモータに電流が流れると, トルクが生じます。トルクは電流に比例するので, 発生トルクをΤ(t)とすると, 

$$ T(t) = K_{t}i(t) $$

となります。Ktはトルク定数と呼ばれます。

機械側の力学方程式に移りましょう。慣性モーメントがJ, 粘性摩擦係数がDの回転体にトルクがかかった出力側の回転運動方程式は下式のようになります。

$$ J \frac{d\omega(t)}{dt} + D\omega(t) = T(t) $$

モータの回転角度をθ(t)とおくと, 回転角の時間変化が回転角速度となるので, 

$$ \frac{d\theta(t)}{dt} = \omega(t) $$

となります。

## 伝達関数

ここから制御工学の理論を使って伝達関数を求めていきます。入力に対して何らかの因果を持って出力されるものをシステムと呼びます。その中で, 線形かつ時間特性が普遍なものを線形時不変システム(LTIシステム)と呼び, 伝達関数を求めることができます。

伝達関数は, 入力信号u(t)と, 出力信号y(t)をラプラス変換によって, 時間tの関数からs平面sの関数に変換したときの入力に対する出力の比です。

ラプラス変換は, 負期間において値が0となる時間関数f(t)に対して, 次のように定義されます。

$$ F(s) = \mathcal{L}[f(t)] = \int_0^∞ f(t)e^{-st}dt$$

sは複素数を値に持つ変数です。例えば, 

$$ \mathcal{L}[1] = \frac{1}{s}$$

微分公式は, 

$$ \mathcal{L}[\frac{df(t)}{dt}] = sF(s) - f(0) $$

f(0) は一般的に0と置くので, 微分関数のラプラス変換には, sが微分の作用をしているとも取れます。sは微分演算子と言われたりもします。では, DCモータの回路方程式

$$ R_{a}i(t) + L_{a}\frac{di(t)}{dt} + K_{e}\omega(t) = u(t)$$

上式にラプラス変換を適用すると,

$$ R_{a}I(s) + sL_{a}I(s) + K_{e}Ω(s) = U(s) $$
$$ I(s) = \frac{1}{sL_{a}+R_{a}}(U(s)-K_{e}Ω(s)) $$
となります。同様に, 回転運動方程式

$$ J \frac{d\omega(t)}{dt} + D\omega(t) = T(t) $$

に対してもラプラス変換を適用して, Ω(s)でまとめると、

$$ Ω(s) = \frac{1}{sJ+D}\tau(s) , \tau(s) = K_{t}I(s) $$

求まり, 角度と角速度に対しても,

$$ \theta(s) = \frac{1}{s}\omega(s) $$

と求めることができます。これらの関係式の入出力を関係づけることにより,下図のようなブロック図を得ることができます。
![blockdiagramdc2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/00a788c6-7495-8cd0-bffe-520cd644df17.jpeg)






一般的にDCモータは機械系の運動方程式の応答より, 電気系の回路方程式の応答速度が速いです。また, 今回のDCモータ（マブチ製 : FA-130RA）は, インダクタンスが小さいのでLa ≃ 0と近似します。（よくある仮定のようです）

U(s)からΩ(s)までの一巡伝達関数は, 
$$ G(s)=\frac{Ω(s)}{U(s)} = \frac{\frac{K_{t}}{R_a}\frac{1}{Js+D}}{1+\frac{K_tK_e}{R_a}\frac{1}{Js+d}}
$$
$$ = \frac{K_t}{R_aJs+R_aD+K_tK_e} $$
$$ = \frac{\frac{K_t}{R_aD+K_tK_e}}{\frac{R_aJ}{R_aD+K_tK_e}s+1}$$
$$ = \frac{K}{Ts+1} $$
$$ K = \frac{K_t}{R_aD+K_tK_e}, T = \frac{R_aJ}{R_aD+K_tK_e} $$

となります。ここでKはゲイン, Tは時定数といいます。 したがって, モータ入力電圧U(s)からΩ(s)までの伝達関数は,

$$ Ω(s)=\frac{K}{Ts+1}U(s)$$

となります。このシステムは, 1次遅れシステムと呼びます。今回はこれを制御対象のシステムとします。(タコメータによる回転速度変換がありますが, 速度計側は回転速度と電圧が比例するので, 定数倍の係数が掛けられるので結局1次遅れです。)

# ステップ応答によるパラメータ同定

ここまでで, DCモータの伝達関数が1次遅れ系であることが導出できたので, パラメータを取得する作業をしていきます。
1次遅れ系のパラメータであるKとTを求めていくにはステップ応答で求めていくのが良いでしょう。

単位ステップ入力 
$$
\text{u(t)=}
\begin{cases}
  \text{0 (t<0)} \\
  \text{1 (t>=0)}
\end{cases}
$$

のラプラス変換は, 
$$ U(s) = \frac{1}{s}$$
ですので, 出力のラプラス変換は,
$$ Y(s) = \frac{K}{Ts+1} \frac{1}{s} $$
逆ラプラス変換により, 出力応答は,
$$y(t) = \mathcal{L}^{-1}[\frac{K}{s(Ts+1)}] $$
$$= \mathcal{L}^{-1}[K(\frac{1}{s}-\frac{1}{s+\frac{1}{T}})]$$
$$= K(1-\mathrm{e}^{-\frac{t}{T}})$$

となります。この式の波形は, （MATLABにプロットしてもらいました。）

```MATLAB
%% first_step.m

K = 1;
T = 1
s = tf('s');
G = K / (T*s+1);
step(G);
```
![first.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/f4563511-827f-b92b-01ae-f7f110ffebc5.jpeg)

上の図のようになり, 定常値でKに, T秒後に最終値 K の 63.2 % に達します。
入力電圧から回転速度が1次遅れ系であるDCモータにステップ入力を加えて, その出力からシステムの同定を行っていきます。

![speedblock.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/225cb9b5-97ca-10c3-c7cc-9056462dd8eb.jpeg)

Simulinkモデルは, mファイルから呼び出される形で実行されます。

```MATLAB

% velo_id_tc.m

% velo_id_tc.m

% Initialize
clear all
close all

% Parameters
ts      = 1/50;
v_init   = 1.5; % initial input
v_const = 1.3; % offset voltage
v_step = 0.5; % step input voltage
s_time  = 10;  % step time
w_time  = 4;   % wait time

% Open simulink model
open_system('velo_id_tc_sl');
open_system('velo_id_tc_sl/Output');

% Start experiment
sim('velo_id_tc_sl');

% Parameter identification
y = yout.signals.values;
t = yout.time;

c1 = mean(y(w_time/ts:s_time/ts));
c2 = mean(y((s_time+w_time)/ts:end));

figure(1)
plot(t,y,...
    t,ones(size(t))*c1,'r--',...
    t,ones(size(t))*c2,'r--')
xlabel('Time [s]')
ylabel('Velocity [V]')

% Calculate Gain
K_id = (c2-c1)/v_step;
u_offset = (K_id*v_const -c1)/K_id;

% Calculate time constant
y2 = y(s_time/ts:end) - c1;
t2 = t(s_time/ts:end);
t2 = t2 - t2(1);

tc_idx  = min(find(y2 > (c2-c1)*0.632));
T_id = t2(tc_idx);

figure(2)
plot(t2,y2,'.',...
    T_id,(c2-c1)*0.632,'ro',...
    t2,ones(size(t2))*(c2-c1),'r--')
xlim([0 w_time])
xlabel('Time [s]'), ylabel('Velocity [V]')

% Display results
fprintf('== Results ==\n')
fprintf('K        = %f\n',K_id)
fprintf('T        = %f\n',T_id)
fprintf('u_offset = %f\n',u_offset)

% EOF 

```
ステップ応答を行う時, モータが静止状態からステップ応答を行うと, 静止摩擦の影響を受けてしまいます。v_initを加えて静止摩擦に打ち勝ってから回転を始め, v_constの電圧で一定速度で回転した後, 時刻s_timeにv_stepのステップ電圧を印加します。

![stepplot2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/ce7147cb-a1ce-a793-5246-68d32ec9efef.jpeg)
![stepplot3.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/ec1fd7f0-e2e0-28c7-5381-555173d00455.jpeg)
2番目の図は,ステップ応答開始した後のグラフです。以下は, 得られたパラメータの結果です。

```matlab
== Results ==
K        = 0.956056
T        = 0.640000
u_offset = 0.717069
```
今回では, ゲインが0.956, 時定数が0.64秒と求めることができました。
# システム同定の検証
同定実験で得たパラメータを使って, モデル（のシミュレーション）と実機がどの程度一致するかを見てみましょう。
![systemid.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/106152bf-d823-ba6b-feee-ca8fe61755a5.jpeg)

```matlab

% velo_id_verify.m

% Set identified parameters
K        = 0.956;
T        = 0.64;
u_offset = 0.717;

% Open simulink model
open_system('velo_id_verify_sl');
open_system('velo_id_verify_sl/Output');

% Start experiment
sim('velo_id_verify_sl')

% Save Parameters
save model_data K T u_offset

% EOF
```
simulinkファイルは, モータの実機と1次遅れのモデルの出力波形を比較する仕様で, mファイルはそれを呼び出す形式となっています。以下に検証した波形を示します。
![systemidentification.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/75ad176e-7f8e-1cd3-c22e-ce065191cc63.jpeg)
青色が1次遅れモデルのシミュレーション波形で, 黄色が実機の波形となります。
ノイズが混じってしまっていますが, 応答の傾向としてはほぼ一致しているとみて取れます。システム同定がうまくいきました。



# 参考文献
[1] 平田光男  ArduinoとMATLABで制御系設計をはじめよう！
Simulinkモデルとmファイルを参考にさせていただきました。感謝。
[2] 仮想制御研究室　理論と実践の架け橋　～システム同定からモデル予測制御(PFC)～
[3] 足立修一　システム同定の基礎