スライディングモード制御

# はじめに
スライディングモード制御について。

# スライディングモード制御の概要
　スライディングモード制御は、制御対象の状態変数を状態空間中の所定の(超)曲面上に拘束し、その曲面上を｢滑らせる｣ことで制御目標を達成する非線形制御則です。たとえば式(1)の入力アファインな連続時間非線形システムを考えます。

``` math
\dot{x}(t)=f(x(t))+g(x(t))u(t)\tag{1}\\
x\in\mathbb{R}^{n},u\in\mathbb{R}^{m}
```
スライディングモード制御器は、状態変数$x$が任意の初期状態から事前に設計された曲面

``` math
S(x)=0
```
に到達し、さらにその上を滑るように目標値へ収束するような制御入力$u$を決定します。このとき、曲面$S(x)=0$を滑り曲面といいます。滑り曲面上に拘束された状態変数は式(1)によらず式(2)のダイナミクスに従います。

``` math
S(x(t))=0\tag{2}
```
この状態をスライディングモードと呼びます。つまり、スライディングモード状態において滑り曲面をうまく設計することができれば、状態変数を所望のダイナミクスに従わせることが可能です。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/211913/02a5071c-2758-3732-27cf-fb7339640e39.png)

# 設計
　スライディングモード制御が有効に機能するためには、以下の条件が必要です。

- スライディングモードにおけるダイナミクス$S(x(t))=0$が理想的な特性を持つ
- 任意の初期状態$x_0$から有限時間で$S(x(t))=0$を達成する制御入力$u$が存在する

以下、この条件を順に説明します。

## 滑り曲面の設計
　滑り曲面はスライディングモード中のダイナミクスが理想的な特性を持つように設計する必要があります。たとえば状態変数が

``` math
x=\left[z,\dot{z},\ldots,z^{(n-1)}\right]^\mathsf{T}
```
のとき、滑り曲面を定数ベクトル$p$と$x$の内積により得られる超平面

``` math
S(x)=p^\mathsf{T}x
```
として定義すれば、スライディングモードにおけるダイナミクスは以下の線形微分方程式で記述されます。

``` math
\begin{aligned}
S(x(t))&=p^\mathsf{T}x(t)\\
&=p_{0}z(t)+p_{1}\dot{z}(t)+\cdots+p_{n-1}z^{(n-1)}(t)\\
&=0
\end{aligned}
```
従って、係数比較による極配置などで、原点が安定となるような$p$を決定することができます。また、一般には$S$は非線形関数とする場合もあります。

## 制御入力の設計
　状態変数を任意の初期状態から$S(x(t))=0$に到達させる制御入力について考えます。通常、この設計はLyapunovベースで行われます。たとえば、Lyapunov候補関数を

``` math
V(S)=\frac{1}{2}S^\mathsf{T}(x(t))S(x(t))
```
とおき、

``` math
\begin{aligned}
 \dot{V}(S)&=S^\mathsf{T}(x(t))\dot{S}(x(t))\\
&=S^\mathsf{T}(x(t))\frac{\partial S}{\partial x}\dot{x}(t)
\end{aligned}
```
が負定関数となるような$u(t)$を決定すればよいことになります。(1)の例について計算を進めると、

``` math
\begin{aligned}
 \dot{V}(S)&=S^\mathsf{T}(x(t))\frac{\partial S}{\partial x}\dot{x}(t)\\
&=S^\mathsf{T}(x(t))\frac{\partial S}{\partial x}\left(f(x(t))+g(x(t))u(t)\right)\\
&=S^\mathsf{T}(x(t))\left(\frac{\partial S}{\partial x}f(x(t))+\frac{\partial S}{\partial x}g(x(t))u(t)\right)
\end{aligned}
```
となることから、$(\partial S/\partial x)g(x)$が可逆との仮定のもと、次のような$u$を考えることができます。

``` math
\begin{aligned}
u(t)&=-\left(\frac{\partial S}{\partial x}g(x(t))\right)^{-1}\frac{\partial S}{\partial x}f(x(t))-\left(\frac{\partial S}{\partial x}g(x(t))\right)^{-1}K\mathrm{sgn}\left(S(x(t))\right)\\
&=:u_{eq}(t)+u_r(t)\\
u_{eq}(t)&:=-\left(\frac{\partial S}{\partial x}g(x(t))\right)^{-1}\frac{\partial S}{\partial x}f(x(t))\\
u_{r}(t)&:=-\left(\frac{\partial S}{\partial x}g(x(t))\right)^{-1}K\mathrm{sgn}\left(S(x(t))\right)
\end{aligned}
```
ここに、$K$は適当な定数、$\mathrm{sgn}$は要素ごとの符号関数です。$u_{eq}$が$\dot{V}$の括弧内を打ち消すことで$\dot{V}=0$を、$u_{r}$が$S(x)\neq0$のときに$\dot{V}<0$を達成するため、

``` math
\begin{aligned}
 \dot{V}(S)&=S^\mathsf{T}(x(t))\left(\frac{\partial S}{\partial x}f(x(t))+\frac{\partial S}{\partial x}g(x(t))u(t)\right)\\
&=-KS^\mathsf{T}(x(t))\mathrm{sgn}\left(S(x(t))\right)
\end{aligned}
```
であり、$\dot{V}$は$K>0$に対し負定関数となります。また、$S(x(t))=0$においては$u(t)=u_{eq}(t)$であることもわかります。現実的には$u_{eq}$による打ち消しはモデル化誤差や外乱の影響で不完全ですが、$K$を十分大きく取ることでロバスト化を図ることができます。さらに$\lim_{S\to +0}u_{r}=K,\lim_{S\to -0}u_{r}=-K$であることから、状態変数が任意の初期状態から有限時間で滑り曲面上に到達することも保証されます。
　ただし、実際にこのような符号関数によるスイッチングを伴う入力はチャタリングを生じるため好ましくなく、通常は$\mathrm{sgn}$を適当な連続関数にて近似します。一般に飽和関数$\mathrm{sat}$や$\tanh$が用いられます。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/211913/d792aa5f-8a5f-8b38-880d-776a4050b64c.png)


# 設計例
## 倒立振子
倒立振子を対象に設計法を例示します。

``` math
\begin{aligned}
x&:=\left[
 \begin{array}{c}
  \theta(t)\\
  \dot{\theta}(t)
 \end{array}
\right]\\
\dot{x}(t)&=\left[
 \begin{array}{cc}
  \dot{\theta}(t)\\
  a\sin(\theta(t))
 \end{array}
\right]+\left[
 \begin{array}{c}
  0 \\ b
 \end{array}
\right]u(t)\\
&=:f(x(t))+Bu(t)
\end{aligned}
```
ここに$a,b$は適当な定数です。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/211913/2854d89b-6d90-d819-f1ec-5b57e9b185b4.png)
まずは滑り曲面を設計しましょう。ここでは

``` math
\begin{aligned}
S(x)&=p^\mathsf{T}x\\
&=\left[
\begin{array}{cc}
1&2
\end{array}
\right]
\left[
\begin{array}{c}
\theta\\
\dot{\theta}
\end{array}
\right]=0
\end{aligned}
```
とします。これにより、スライディングモード状態におけるダイナミクスは

``` math
\dot{\theta}(t)=-\frac{1}{2}\theta(t)
```
となり、指数関数的減衰を期待できます。次に対応する制御入力を決定します。例の通りLyapunov候補関数を

``` math
V(S)=\frac{1}{2}S^\mathsf{T}(x)S(x)
```
とおき、$\dot{V}$を負定関数とするために、以下の制御入力を考えます。ただし、符号関数を$\tanh$で近似します。

``` math
u(t)=-(p^\mathsf{T}B)^{-1}p^\mathsf{T}f(x(t))-(p^\mathsf{T}B)^{-1}K\mathrm{\tanh}\left(\alpha S(x(t))\right)
```

以下、$a=1.0,b=1.0,\alpha=10.0$としてJuliaによるシミュレーションを行います(コードは最後に)。
シミュレーションの結果、状態変数と入力の時系列は下図のようになりました。目標通り、$\theta(t)\to0$を達成することができました。
![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/211913/4ccd763c-8fba-609d-e675-50e578ae929e.png)
また、位相平面を以下に示しますが、解軌道が滑り曲面$S(x)=\theta+2\dot{\theta}=0$上を滑っていることが確認できます。
![Figure_2.png](https://qiita-image-store.s3.amazonaws.com/0/211913/881790f3-f6e8-c96b-ab06-164bf5e308f9.png)
さらに、滑り曲面上での$\theta$の挙動を確認するため、$t=1.5$以降の応答をプロットしたのが下図ですが、$\theta$と$S(x(t))=0$の応答はよく一致していることが確認できます。
![Figure_3.png](https://qiita-image-store.s3.amazonaws.com/0/211913/f7183dfb-ad69-dc4b-d1ba-2f785226db8c.png)

## 二輪車両
続いて二輪車両モデル

``` math
\begin{aligned}
x&:=\left[
 \begin{array}{c}
  Y(t)\\
  \theta(t)
 \end{array}
\right]\\
\dot{x}(t)&=\left[
 \begin{array}{cc}
  V\sin({\theta}(t))\\
  0
 \end{array}
\right]+\left[
 \begin{array}{c}
  0 \\ 1
 \end{array}
\right]u(t)\\
&=:f(x(t))+Bu(t)\\
\end{aligned}
```
について、任意の初期位置$Y(0)$と初期角度$\theta(0)$から直線状レーン$Y=0$への遷移$\left(Y=\theta=0\right)$を考えてみましょう。ここに、$V>0$は速度です。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/211913/d192cdae-08a5-05e4-fded-c1424edcdf7a.png)


まずは倒立振子の例と同様に滑り曲面を設計します。ややトリッキーですが、以下の滑り曲面を設計しました。

``` math
\begin{aligned}
S(x)&=Y+\dot{Y}+\zeta\theta\\
&=Y+V\sin(\theta)+\zeta\theta,\ (\zeta> V)
\end{aligned}
```
また入力は符号関数を飽和関数で近似した以下のものを用います。

``` math
\begin{aligned}
u(t)&=-\left(\frac{\partial S}{\partial x}B\right)^{-1}\frac{\partial S}{\partial x}f(x(t))-\left(\frac{\partial S}{\partial x}B\right)^{-1}K\mathrm{sat}\left(\alpha S(x(t))\right)\\
&=-\frac{V\sin(\theta(t))}{\zeta+V\cos(\theta(t))}-\frac{K\mathrm{sat}\left(\alpha S(x(t))\right)}{\zeta+V\cos(\theta(t))}\\
&=:u_{eq}(t)+u_r(t)\\
u_{eq}(t)&:=-\frac{V\sin(\theta(t))}{\zeta+V\cos(\theta(t))}\\
u_{r}(t)&:=-\frac{K\mathrm{sat}\left(\alpha S(x(t))\right)}{\zeta+V\cos(\theta(t))}\\
\frac{\partial S(x)}{\partial x}&=\left[\begin{array}{cc}1&V\cos(\theta)+\zeta\end{array}\right]
\end{aligned}
```
この滑り曲面と制御入力により、$S(x(t))=0$上のダイナミクスは、$u(t)=u_{eq}(t)$に注意すると

``` math
\begin{aligned}
\dot{Y}(t)&=-Y(t)-\zeta\theta(t)\\
\dot{\theta}(t)&=-\frac{V\sin(\theta(t))}{\zeta+V\cos(\theta(t))}
\end{aligned}
```
となります。Lyapunovの安定定理等で$\theta\to0$が示されるため、$Y\to0$も成立します。

以下、$V=10.0, \zeta=10.5, K=15$の条件でシミュレーションしてみます。まず状態変数と入力の時系列および平面上の位置は下図の通りになり、$Y\to0,\theta\to0$が達成できました。
![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/211913/45ad97bb-a57f-d5a3-910d-72bdce3715e6.png)
![Figure_3.png](https://qiita-image-store.s3.amazonaws.com/0/211913/35139145-88ed-b555-da0c-99d5693699a2.png)
また解軌道は下図のようになり、$S$を非線形関数とした場合にもスライディングモード制御が機能していることが確認できます。
![Figure_2.png](https://qiita-image-store.s3.amazonaws.com/0/211913/89e301a4-c6bf-e064-092b-daef2be7fed3.png)

# ソースコード
## 倒立振子
``` julia:倒立振子
using PyPlot
using LinearAlgebra

# 4次Runge-Kutta法による状態遷移シミュレーション #
# 入力は零次ホールドを仮定 #
function rk4(x, u, dt, f)
    k1 = f(x, u);
    k2 = f(x+k1*dt/2, u);
    k3 = f(x+k2*dt/2, u);
    k4 = f(x+k3*dt, u);
    
    return x + dt/6 * (k1+2*k2+2*k3+k4);
end

function main_inv_pen()
    # サンプリング周期と時間ベクトル #
    dt = 0.01;
    t = 0:dt:10;
    
    # プラントダイナミクス #
    # dx(t)/dt = F(x(t), u(t)) = f(x(t))+g(x(t))u(t)
    a = 1.0; b = 1.0;
    f = x -> [
        x[2];
        a*sin(x[1])
    ];
    B = [
        0;
        b;
    ];
    F = (x,u) -> f(x) + B*u;
    
    # 初期状態 #
    x0 = [ 5.0; 0.0 ];
    
    # 切換平面のパラメータ #
    p = [ 1.0; 2.0 ];
    # 切換平面 #
    S = x -> p'*x;
    # 符号関数 #
    alpha = 10;
    sig = S -> tanh(alpha*S);
    # スライディングモード制御のゲイン #
    K = 2.0;
    # スライディングモード制御下の状態変数、入力 #

    xsmc = zeros(2, length(t));
    xsmc[:,1] .= x0;
    usmc = zeros(1, length(t));
    
    # シミュレーションループ #
    for k = 1:length(t)-1        
        # SMCの状態遷移 #
        usmc[k] = -(p'*B)\(p'*f(xsmc[:,k])) - K*sig(S(xsmc[:,k]));
        xsmc[:,k+1] = rk4(xsmc[:,k], usmc[k], dt, F);
    end
    usmc[end] = -(p'*B)\(p'*f(xsmc[:,end])) - K*sig(S(xsmc[:,end]));
    
    # プロット #
    close("all");
    figure();
    subplot(3,1,1);
    plot(t, xsmc[1,:]);
    ylabel("state x1");
    grid("on");
    title("time series");
    
    subplot(3,1,2);
    plot(t, xsmc[2,:]);
    ylabel("state x2");
    grid("on");
    
    subplot(3,1,3);
    plot(t, usmc[1,:]);
    xlabel("time");
    ylabel("input");
    grid("on");
    
    figure();
    plot(-1:5, -p[1]/p[2]*(-1:5), "k--", label = "sliding surface");
    plot(xsmc[1,:], xsmc[2,:], label = "state trajectory");
    xlabel("state x1");
    ylabel("state x2");
    legend();
    title("state space");
    grid("on");
end
```

## 二輪車両
``` julia:二輪車両

using PyPlot
using LinearAlgebra

# 4次Runge-Kutta法による状態遷移 #
# 入力は零次ホールドを仮定 #
function rk4(x, u, dt, F)
    k1 = F(x, u);
    k2 = F(x+k1*dt/2, u);
    k3 = F(x+k2*dt/2, u);
    k4 = F(x+k3*dt, u);
    
    return x + dt/6 * (k1 + 2*k2 + 2*k3 + k4);
end

# 飽和関数 #
function sat(x)
    return sign.(x) .* min(1, abs.(x));
end

function main_car()
    close("all");
    
    # サンプリング周期と時間ベクトル #
    dt = 0.01;
    t = 0:dt:25;
    
    # 速度 #
    V = 10.0;
    # システムの定義 #
    # dx(t)/dt = f(x(t))+Bu(t) #
    f = x-> [
        V*sin(x[2]);
        0
    ];
    B = [
        0;
        1
    ];
    
    # 進行方向(X)も含めた3次元モデル(シミュレーション用) #
    # dx(t)/dt = F(x(t),u(t)) #
    F = (x,u) -> [
        V*cos(x[3]);
        V*sin(x[3]);
        u
    ];
    
    # 状態変数ベクトル #
    x = zeros(3, length(t));
    # 初期状態(初期位置±50、初期角度±π) #
    x[:,1] = [
        2*(rand(2, 1).-0.5) * 50;
        2*(rand(1, 1).-0.5) * pi;
    ];
    # 入力 #
    u = zeros(1, length(t));
    
    # 滑り曲面の決定 #
    z = V + 0.5;
    S = x -> x[1] + V*sin(x[2]) + z*rem.(x[2], 2*pi, RoundNearest);
    # 符号関数の近似 #
    sig = S -> sat(5*S);
    # ゲイン #
    K = 15;
    
    # シミュレーションループ #
    for k in 1:length(t)-1
        # ∂S/∂x #
        Sx = [1 V*cos(x[3,k])+z];
        
        u[k] = -(Sx*B)\(Sx*f(x[2:3,k])+[K*sig(S(x[2:3,k]))]);
        x[:,k+1] = rk4(x[:,k], u[k], dt, F);
    end
    
    # プロット #
    figure();
    subplot(3,1,1);
    plot(t, x[2,:]);
    ylabel(L"Y");
    grid("on");
    title("time series");
    
    subplot(3,1,2);
    plot(t, x[3,:]);
    ylabel(L"\theta");
    grid("on");
    
    subplot(3,1,3);
    plot(t, u[1,:]);
    ylabel(L"u");
    
    grid("on");
    
    figure();
    theta = pi*(-1:0.01:1);
    plot(-V*sin.(theta)-z*theta, theta, "k--", label = "sliding surface");
    plot(x[2,:], rem.(x[3,:], 2*pi, RoundNearest), label = "state trajectory");
    grid("on");
    xlabel(L"Y");
    ylabel(L"\theta");
    legend();
    title("state space");
    
    figure();
    plot(x[1,:], x[2,:]);
    quiver(x[1,1:200:end], x[2,1:200:end], cos.(x[3,1:200:end]), sin.(x[3,1:200:end]), angles = "xy");
    xlabel(L"X");
    ylabel(L"Y");
    title("position");
    grid("on");
end
```