実践！スライディングモード微分器

# スライディングモード微分器とは？

漸近安定となるリアプノフ関数。スライディング面（あるいは線）と称される箇所にむかって、制御するという例の現在制御です。
現代制御のなかでは割とロバストで実用的な制御の１つですが、実は微分器にもなります。

スライディングモード制御そのものの解説は他の方に ~~丸投げ~~ お任せしようと思います。

良記事を紹介します。

https://qiita.com/HppyCtrlEngnrng/items/0b8d6a31e5123ac71e01

https://qiita.com/motorcontrolman/items/272f2514b933a493e95e

# 微分器とは？
読んで字のごとく微分を行う装置や計算モジュールです。
異なる時間に測定した複数の位置から速度を計算したりします。（いわいる時間微分）
産業用途では時間微分を計算することが多いかなという印象です。

しかしこの微分は、それ自体がノイズを増幅する性質があるうえ、PCなどのデジタル化されていると数値が量子化され、時間的にも連続的な微分ができずここでもノイズが発生するなど、意外と厄介者です。

たとえば普通に微分（差分）すると、↓の図の黄色線のように、パルス状のノイズが発生します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/173513/05ae9b07-28ee-408d-a044-9ee6b7a360fb.png)

対策としては一般的には平均や平滑化（ローパスフィルター）を行います。
しかし、位相の遅れが発生しますし、完全に元の波形を再現することは困難です。
↓ ローパスフィルターを行った例
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/173513/ac219cfd-6f9d-4cdb-81cf-bd29e4c69da8.png)

上記がよくある処理方法ですが、今回はスライディングモード制御を応用した微分器を紹介と簡単な解説します。

# スライディングモード微分器の一般式
ひとまずスライディングモード微分器の一般式を記載します。
~~~ math
\begin{cases}
\dot{x}_1 = x_2 - k_1 |x_1 - f(t)|^{1/2} \text{sgn}(x_1 - f(t)) \\
\dot{x}_2 = -k_2 \text{sgn}(x_1 - f(t))
\end{cases}
~~~

おおう...という感じですが、順に説明してみようと思います。

## 解説
解説というよりも解釈に近いのですが、簡単に説明してみます。

まず、元の式だと少し読みづらいので、$x_1 - f(t)$を誤差として下記のように直してみます。

~~~ math
\begin{flalign*}
&error = x_1 - f(t) \\
&\dot{x}_1 = x_2 - k_1 |error|^{1/2} \text{sgn}(error) \\
&\dot{x}_2 = -k_2 \text{sgn}(error)
\end{flalign*}
~~~
この$\{x}_1$と$\{x}_2$は一種の内部推定値であり、それぞれ以下の意味となります。
* $\{x}_1$ ＝ 測定値$f(t)$と一致する（したい）推定値
* $\{x}_2$ ＝ 推定値の一階微分値

よって、$error = x_1 - f(t)$は、内部推定値と測定値の誤差を表します。
つまりは測定値と内部推定値$\{x}_1$を一致（$error=0$)させるように$\{x}_1$・$\{x}_2$を符号関数sgnでスイッチングさせるイメージです。

なお、$\{x}_2$がスライディングモード微分器での計算結果（得られる微分値）です。

式の形を見ると分かる通り、ゲイン${k}_1$・${k}_2$の設定が非常に重要です。これを不適切に設定すると容易に発散します。
ただ、リアプノフ安定性条件もすでにわかっており、ゲインは$k_1 > \sqrt{k_2}$で設定すればokとのことです。
一般的には安全マージンをみて、$k_1 > 1.5\\sqrt{k_2}$が推奨されるようです。



:::note info
ちなみに、式の形としては、そのまんまスーパーツイスティング スライディングモード制御の応用です。
スーパーツイスティング スライディングモード制御は以下の式で、

~~~ math
\begin{flalign*}
&u(t) = -k_1 |s(t)|^{1/2} \text{sgn}(s(t)) + v(t) \\
&\dot{v}(t) = -k_2 \text{sgn}(s(t))
\end{flalign*}
~~~

この式の$s(t)$がリアプノフ関数を通して得られたスライディング面（線）からの距離を示します。
スーパーツイスティング スライディングモード制御は距離$s(t)$とその一階微分の速度をゼロとする制御ですが、ここにもリアプノフが隠れていて、このときのリアプノフ安定条件としては同じく $k_1 > \sqrt{k_2}$ となります。
:::


# 実装
ということでサクッとpythonで実装していきます。


## 概念コード
まずは以下の式を概念レベルでコーディングしてみます。
~~~ math
\begin{flalign*}
&error = x_1 - f(t) \\
&\dot{x}_1 = x_2 - k_1 |error|^{1/2} \text{sgn}(error) \\
&\dot{x}_2 = -k_2 \text{sgn}(error)
\end{flalign*}
~~~

~~~python
import math

def sgn(x):
    if x > 0:
        return 1.0
    elif x < 0.0:
        return -1.0
    else:
        return 0.0     

error = x1 - measurement
dx1 = x2 - k1 * math.sqrt(math.fabs(error)) *  sgn(error)
dx2 = -k2 * sgn(error)
~~~

このままでは$\dot{x}_1$・$\dot{x}_2$つまり一階微分の値なので、積分して$\{x}_1$・$\{x}_2$にしてあげます。
~~~python
x1 += dx1 * dt
x2 += dx2 * dt
~~~

これで概念コードとしては完成です。
とてもシンプルですね！


## 実用的な実装
ということで、もう少し実用的な実装をしてみます。

~~~python
import math

class SlidingModeDifferentiator:

    def __init__(self, k1=1.5, k2=1.0, dt_sec=1.0):
        self.k1 = k1
        self.k2 = k2
        self.dt_sec = dt_sec

        self.x1 = 0.0
        self.x2 = 0.0
        
    #sgn関数
    def sgn(self, x):
        if x > 0:
            return 1.0
        elif x < 0.0:
            return -1.0
        else:
            return 0.0

    #処理
    def process(self,measurement):
        error = self.x1 - measurement
        dx1 = self.x2 - self.k1 * math.sqrt(math.fabs(error)) *  self.sgn(error)
        dx2 = -self.k2 * self.sgn(error)

        self.x1 += dx1 * self.dt_sec
        self.x2 += dx2 * self.dt_sec

        return self.x2

if __name__ == "__main__":
    smd = SlidingModeDifferentiator(k1=1.0, k2=0.10, dt_sec=1.0)
    for x in range(100):
        print(smd.process(x))
~~~

これでも良いですが、sgn関数を連続近似に変更することも可能です。
ここではtanhに変更してみます。

~~~python
import math

class SlidingModeDifferentiator:

    def __init__(self, k1=1.5, k2=1.0, dt_sec=1.0):
        self.k1 = k1
        self.k2 = k2
        self.dt_sec = dt_sec

        self.x1 = 0.0
        self.x2 = 0.0

    def process(self,measurement):
        error = self.x1 - measurement
        tanh_value = math.tanh(error*3.0)
        dx1 = self.x2 - self.k1 * math.sqrt(math.fabs(error)) *  tanh_value
        dx2 = -self.k2 * tanh_value

        self.x1 += dx1 * self.dt_sec
        self.x2 += dx2 * self.dt_sec

        return self.x2


if __name__ == "__main__":
    smd = SlidingModeDifferentiator(k1=1.0, k2=0.10, dt_sec=1.0)
    for x in range(100):
        print(smd.process(x))
~~~

:::note info
tanhのx3の係数はお好みで1～5程度で調整してください。
大きいほどsgn関数に近づきます。
:::

:::note info

$\dot{x}_1 = x_2 - k_1 |error|^{1/2} \text{sgn}(error)$
の解釈なのですが、本質的にやりたいことは、$\dot{x}_1 = x_2 - k_1 error^{1/2} $なのだと思います。
しかし、この計算だと$error^{1/2}$の解が複素数になる可能性があります。
（$error < 0$のとき）
$|error|^{1/2}$と絶対値にすることで複素数を回避して、消えた符号を取り戻すために、$\text{sgn}(error)$を追加しているイメージです。
基本コンセプト的にはスイッチングさせるのは$\dot{x}_2$だけっぽいですね。

となると、
~~~ math
\begin{flalign*}
&\dot{x}_1 = x_2 - k_1 |error|^{1/2} \text{sgn}(error) \\
&\dot{x}_2 = -k_2 \text{tanh}(error)
\end{flalign*}
~~~
のほうが良い？って話にもなりますが、実際には両方ともtanhのほうがいい感じに動いている気がします。
:::




## 実行結果
sin波形+ノイズでのテスト結果です。
緑が真値、青が今回手法であるスライディングモード微分器です。
比較用に差分+ローパスフィルターを黄色で示します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/173513/75e29e0c-2127-489a-829a-836e8b0fe4c8.png)


同程度の位相遅れであれば、スライディングモード微分器のほうが優秀そうですね。
ノイズを抑えつつピーク値が取れているのは*good*です。
ただゲイン調整が難しいです。
対象の周波数や振幅が変わるとゲインも調整する必要があります。
ある程度、動きが読めるものにしか適用が難しいかな、と思います。
反面ノイズ量については耐性が高い感じです。多少ノイズ量が変わっても特段の調整はいらない感じでした。

現物合わせでゲイン調整はかなり困難なので、使う時は実際の測定データを入手して机上でゲイン検討を推奨します。

:::note info
k1を固定しつつ、k2を調整というアプローチが良いようです。
私の場合、まずk1を調整して振幅をあわせつつ、k2を調整して好みの位相を探しました。
動きの少ない対象であれば、k2をかなり小さめ（0.1、0.01など）にする場合もあるようです。
:::

# おわりに
ということでスライディングモード微分器の解説？でした。
スライディングモード微分器はあまり日本語記事が少ないので参考になれば幸いです。
いいねなどでフィードバック頂けると励みになります。

そのうちスライディングモード制御の解説も書いてみたいですね。

他にも制御ライクな記事を書いていますので、興味があれば読んでみてください。
それではまた。