ローパスフィルタで復習する制御工学

# この資料の目的

　この資料はローパスフィルタを使って制御工学を復習することを目的に作成しました。僕が授業で制御工学を勉強した際に問題は解けるようになったけれど何をやっているのかがよくわかりませんでした。そこでローパスフィルタという具体例を通して制御工学の意味を理解することを目指します。そのため計算の仕方などは省略しています。またローパスフィルタを理解するために勉強したので現代制御やPID制御など省略している内容も多々あります。

# 制御工学とは

<img width="400" alt="画像1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/aac87892-8499-b3a6-f6b3-5fcfcc5c9538.png">

　図のように入力と出力があるときに入力と出力の関係を調べてさらに出力を制御しようというのが制御工学です。制御工学は入力と出力の関係があれば電気工学や機械工学などに適用できます。

# ローパスフィルタの式

<img width="600" alt="画像2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/ed6125a3-6c8a-c83c-280c-a5382494b9fc.png">

　図のように入力電圧$Vin(t)$、出力電圧$Vout(t)$、電流$I(t)$、抵抗値$R$、コンデンサの静電容量$C$とおいて電圧の式を立てると
```math
Vin(t) = RI(t) + Vout(t)　（式1）
```

```math
\begin{aligned}
I(t) &= \frac{dQ(t)}{dt}　（Qは電荷量）\\ 
&= C\frac{dVout(t)}{dt}　（Q(t)=CVout(t),　Cは定数）　（式2）
\end{aligned}
```

式2を式1に代入すると
```math
Vin(t) = RC\frac{dVout(t)}{dt} + Vout(t)　（式3）
```

# 伝達関数

　式3を初期値0でラプラス変換します。入力電圧のラプラス変換を$L[Vin(t)] = U(s)$、出力電圧のラプラス変換を$L[Vin(t)] = U(s)$と置くと
```math
U(s) = RCsY(s) + Y(s)　（式4）
```
```math
G(s) = \frac{Y(s)}{U(s)} = \frac{1}{1 + RCs}　（式5）
```
このように
```math
伝達関数G(s) = \frac{出力のラプラス変換Y(s)}{入力のラプラス変換U(s)}　（式6）
```
と定義します。
<img width="300" alt="画像3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/a02d88f3-dc6b-f358-5028-a4752d741a30.png">

また図で表すと図のようになります。このような図をブロック線図といいます。

# 安定性

## インパルス入力

<img width="300" alt="画像4.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/ad906531-3a3c-bd5e-47b4-01c7ef8b4d9d.png">

　入力に図のようなインパルス関数を入力したときの出力を考えます。（$\varepsilon$は微小量）
```math
u(t) = \left\{
\begin{array}{ll}
\frac{1}{\varepsilon} & (0 < t < \varepsilon) \\
0 & (t > \varepsilon)
\end{array}
\right.　（式7）　インパルス関数
```

ラプラス変換をすると
```math
U(s) = L[u(t)] = 1
```
となります。
伝達関数の定義式より
```math
Y(s) = G(s)U(s)
```
ラプラス逆変換して
```math
y(t) = L^{-1}[G(s)U(s)]
```
なので
```math
y(t) = L^{-1}[G(s)]　(U(s) = 1)　（式8）
```
ローパスフィルタでは式5より
```math
y(t)= L^{-1}[\frac{1}{1 + RCs}] = \frac{1}{RC}e^{-\frac{1}{RC}t}　（式9）
```
<img width="500" alt="画像7.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/662c5b46-6bb6-acea-d263-6895d92b2786.png">

グラフは式9より図のようになります。図より0に収束しています。また$RC$を小さくするとt=0でのyが大きくなり、早く減少することが分かります。

## 単位ステップ入力

<img width="300" alt="画像6.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/e65f1b79-b7dc-7038-f654-f3989bbd6a13.png">

　次に入力に図のような単位ステップ関数を入力したときの出力を考えます。
```math
u(t) = 1 (t > 0)　単位ステップ関数　（式10）
```
ラプラス変換をすると
```math
U(s) = L[u(t)] = 1/s
```
となるので、
```math
y(t) = L^{-1}[G(s)\frac{1}{s}]　(U(s) = 1/s)　（式11）
```
ローパスフィルタでは式5より
```math
y(t)= L^{-1}[\frac{1}{1 + RCs}\frac{1}{s}] = 1 - e^{-\frac{1}{RC}t}　（式12）
```
<img width="500" alt="画像8.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/75657b49-1f78-081f-4000-1aad917fba20.png">

グラフは式12より図のようになります。図より1に収束しています。また$RC$を小さくすると早く減少することが分かります。

## 極と安定性の関係

　このように出力が∞に発散しないとき安定しているとします。伝達関数の分母$U(s) = 0$の解を極といます。全ての極の実部が負であればステップ入力の時に安定となります。実際式5の伝達関数では極は$s = -\frac{1}{RC}$となり$RC$は正なので極は負となり図の結果と一致します。（ステップ入力ではない場合にも目標値と制御値の差が発散するか収束するかが分かります。）(ラウスの安定判別法やフルビッツの安定判別法から複雑な式の極の正負を求めることができますが省略します。)

# フィードバック制御系

<img width="500" alt="画像9.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/4deb18f3-ec2d-0c18-844c-6b9d24ac45a7.png">

　図のようなフィードバック制御系はオープンループ制御系と比べて以下の優位点があります
- 外乱の影響を低減できる
- 制御系の変動に対して出力の影響が小さい（ロバストである）
- 目標値$r(t)$と制御値$y(t)$の差（定常偏差）（$\lim_{t \to \infty} (r(t) - y(t))$）を小さくできる
- 速応性を改善できる

証明はしませんが出力の結果によって制御を変化させるので上記の性質を満たすことはイメージできます。
　定数ゲイン$K$を変化させたときに極がどのように変化するかを示した図を根軌跡といいます。上述したように極の複素平面での位置から十分時間がたった時の応答（過渡応答）がわかります。（根軌跡の求め方については省略します。）

# 周波数応答

　先ほどインパルス入力やステップ入力での出力の応答を確認しました。今回は正弦波を入力した際の出力（周波数応答）を考えます。導出はしませんが入力が
```math
u(t) = \sin\omega t
```
の時出力は
```math
y(t) = |G(j\omega)|\sin(\omega t + \phi(\omega))
```
となります。
実部を$X$、虚部を$Y$として$G(j\omega) = X(\omega) + jY(\omega)$と書くと
ゲイン
```math
|G(j\omega)| = \sqrt{X^2 + Y^2}
```
位相
```math
\angle G(j\omega) = \arctan \frac{Y}{X}
```
となります。
ローパスフィルタのゲインと位相は式5より
```math
G(j\omega) =　\frac{1}{1 + j\omega RC}　（式13）
```
```math
|G(j\omega)| =　\frac{1}{|1 + j\omega RC|} = \frac{1}{\sqrt{1 + (\omega RC)^2}}　（式14）
```
```math
\angle G(j\omega) = \arctan \frac{-\omega RC}{1}　（式15）
```
となり$\omega RC = 0, 1, \infty$の時
```math
\omega RC = 0, |G| = 1, \angle G = 0 ^\circ
```
```math
\omega RC = 1, |G| = \frac{1}{\sqrt{2}}, \angle G = -45^\circ
```
```math
\omega RC = \infty, |G| = 0, \angle G = -90^\circ　（式16）
```
となります。

<img width="600" alt="画像14.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/637a502a-7855-bb24-959f-ac6c6be7d614.png">

$\omega T = 1$の時の入力と出力のグラフを図に示します。振幅が$|G| = \frac{1}{\sqrt{2}}$倍されて位相が$G = -45^\circ$ずれています。
　$\omega T$を変化させたときのゲインや位相の変化を表す方法が2通りあります。ベクトル軌跡とボード線図です。
　ベクトル軌跡は$G(j\omega)$の実数と虚数の変化を表します。

<img width="400" alt="画像11.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/d7baf4a5-d2a4-fb86-3eeb-072e0dde37fb.png">

式13より実部を$X$、虚部を$Y$として$\omega RT$を消去して計算したローパスフィルタのベクトル軌跡を図に示します。中心(0,0.5)半径0.5の円の下半分になります。式16の結果と一致しています。
　ボード線図はゲインと位相それぞれについて$\omega T$を変化させたときのグラフを書きます。その際にゲインはデシベルで表します。つまり$20\log|G(j\omega)|$を計算します。ゲインを対数表示することでボード線図でゲインと位相を足し算するだけで簡単に合成できます。

![画像13.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/a7137a2c-dbbd-21a2-8831-4eb787f589c0.png)

図に式14、式15より求めたローパスフィルタでのボード線図を示します。この図でゲインが0dBの時$|G| = 1$なので振幅はそのままでゲインが-40dBの時$|G| = 1/100$なので振幅はほぼ0になっています。つまりTを小さくとれば振動数が大きい（高周波）のみを除去できTを大きくするにつれて振動数が小さい（低周波）も除去されていくことが分かります。またTを大きくするにつれて位相のずれが負に大きくなっています。つまりTを大きくとると入力に対して出力の遅れが大きくなることが分かります。

# 現代制御

　古典制御では1入力1出力を扱ったが多入力多出力を扱えない、また2次以上の微分があると計算が面倒といった問題があるので現代制御では行列を用いて一回連立常微分方程式で表します（古典制御とやっていることは同じで行列になっただけ）。古典制御では入力と出力を一つの式として計算したが現代制御では
```math
\boldsymbol{\dot{x}}(t) = \boldsymbol{A}\boldsymbol{x}(t) + \boldsymbol{B}\boldsymbol{u}(t)　（式17）
```
```math
\boldsymbol{y}(t) = \boldsymbol{C}\boldsymbol{x}(t)　（式18）
```
上の入力と変数と変数の一回微分の関係式を状態方程式、下の出力と変数の関係式を出力方程式といい、一般化します。伝達関数は2つの式をラプラス変換してから$\boldsymbol{x}$を消去して
```math
G(s) = Y(s)/U(s) = C(s\boldsymbol{I} - \boldsymbol{A})^{-1}\boldsymbol{B}　（式19）
```
となります。

## 安定性

　$\boldsymbol{A}$の固有値を対角成分にもつ行列$\Lambda$、固有値ベクトルを並べたベクトル$\boldsymbol{V}$とすると$\boldsymbol{x}(t) = \boldsymbol{V}e^{\Lambda t}\boldsymbol{V}^{-1}\boldsymbol{x}(0)$と表せます。安定となるには$t\rightarrow \infty$で$x\rightarrow 0$となればよく固有値が負であれば安定となります。（$e^{(\alpha + j \beta)t} = e^{\alpha t}(\cos\beta t + j\sin\beta t)$は$\alpha < 0$で0に収束）

## 可制御性

　可制御性は名前の通り入力を制御することで出力を制御できること、つまり出力を任意の目標値に移せるかどうかです。式17がｎ行の行列とすると可制御となる必要十分条件はn行の行列である
```math
U_c = \begin{bmatrix}\boldsymbol{B} & \boldsymbol{AB} & \boldsymbol{A^2 B} & \cdots & \boldsymbol{A^{n-1} B}\end{bmatrix}
```
が$rank(U_c) = n$(フルランク)を満たすことです。

## 可観測性

　可観測性は名前の通り入力$\boldsymbol{u}(t)$と出力$\boldsymbol{y}(t)$から制御で用いる状態変数$\boldsymbol{x}(t)$が求まる（観測できるか）どうかです。式17、18がｎ行の行列とすると可観測となる必要十分条件はn列の行列である
```math
U_o = \begin{bmatrix}\boldsymbol{C} \\ \boldsymbol{CA} \\ \boldsymbol{C A^2} \\ \cdots \\ \boldsymbol{C A^{n-1}}\end{bmatrix}
```
が$rank(U_c) = n$(フルランク)を満たすことです。

# 離散時間制御

　これまでは連続値を考えてきました。しかしPCで制御を行う場合デジタル値つまり離散時間となります。
　離散時間制御ではラプラス変換の代わりにZ変換を用います。Z変換には$Z[f_{k+n}] = z^nZ [f_k] - z^{n-1}f_0 - z^{n-2}f_1 \cdots - zf_{n-1}$というラプラス変換とよく似た公式があります。
　また連続時間系の微分にあたるのは差分で$\frac{f_{k} - f_{k-1}}{T}$となります。差分を用いて式3を離散化すると($Vin = u、Vout = y$と置いた、Tはサンプリング周期)
```math
u_k = RC\frac{y_k-y_{k-1}}{T}+y_k
```
となり
```math
y_k = \frac{T}{T+RC}u_k+\frac{RC}{T+RC}y_{k-1}
```
となって
```math
\frac{RC}{T+RC}=a　（式20）
```
と置くと
```math
y_k = (1-a)u_k + ay_{k-1}　（式21）
```
となります。$R、C、T$は正なので$0<a<1$です。現在の出力を過去の出力と現在の入力から計算していることが分かります。

# Arduinoで実装
　MPU9250という9軸センサでX軸の角速度を調べてセンサの値とローパスフィルタをかけた値を比較します。式21よりArduinoのプログラムは以下のようになります。
```c
#include "MPU9250_NH.h"
MPU9250 mpu9250(AD0_LOW_);

float accel[3], gyro[3], mag[3];
float accel_offset[] = {0, 0, 0};
float gyro_offset[] = {0, 0, 0};
float mag_offset[] = {0, 0, 0};
float rc_constant;
float rc_gyrox;

void setup() {
  Serial.begin(9600);
  mpu9250.begin();
  mpu9250.set(a_4g_, g_2000dps_);
  mpu9250.offset(accel_offset, gyro_offset, mag_offset);
}

void loop() {
  if (mpu9250.available()) {
    mpu9250.get(accel, gyro, mag);
    Serial.print(gyro[0]);

    rc_constant = 0.6;
    rc_gyrox = rc_constant * rc_gyrox + (1 - rc_constant) * gyro[0];
    Serial.print(",");
    Serial.print(rc_gyrox);

    Serial.println();
  }

  else {
    Serial.println("error");
  }
}
```
rc_gyrox = rc_constant * rc_gyrox + (1 - rc_constant) * gyro[0];でローパスフィルタをかけています。

![画像15.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2603819/e2ad71b1-093e-2c1c-76c0-949b395afa90.png)

図に式21のaを0.2、0.5、0.8とした時のグラフを示します。青がローパスフィルタをかけていないデータ、赤がかけたデータです。aが大きいほど振動数が小さいところでもゲインの影響があり、位相の遅れも生じていることが確認できます。今回のサンプリング周期Tは0.019756sでした。式20からa=0.2の時RC=0.004939、、a=0.5の時RC=0.019756、a=0.8の時RC=0.079024です。ボード線図で確認したゲインと位相のRCに対する関係と一致していることが確認できます。

# 参考にした記事

http://akiracing.com/2018/01/23/arduino_rc_filter/
https://okasho-engineer.com/first-order-filter/
https://garchiving.com/lpf-by-program/