Simulink の分岐処理を利用して『回転型  LEGO 倒立振子』を振り上げ安定化！

# <font color="#33CC33">0. 配布ファイル</font>

　本記事で利用した MATLAB 関連ファイルは github で公開していますので，ご自由にご利用ください！

:::note info
配布ファイル
* https://github.com/Prof-Carter/ip_swing_up 　$\cdots\cdots$ R2022b, R2022a, R2021b, R2021a, R2020b, R2020a, R2018a 用を提供（<b>実機実験</b>を行うファイル）

[おまけ](https://qiita.com/Carter/items/db4bb11ead93ea3a1d42#%E3%81%8A%E3%81%BE%E3%81%91)で説明する<b>非線形シミュレーション</b>を行うファイル
* https://github.com/Prof-Carter/ip_swing_up/blob/main/simulation.zip

も提供していますよ！
:::

ちなみに，本記事の<b>実機実験</b>は，[MinSeg 社](https://minseg.com/)の [MinSegShield M2V5 Dual Axis Balancing Kit](https://minseg.com/products/minsegshield-m2v5-dual-axis-balance-minseg-kit-new) を利用した<b>回転型 LEGO 倒立振子</b>により行っています．詳しくは

https://qiita.com/Carter/items/a3ddaad078bbebe42998

をご参照ください．

:::note warn
注意
今回，配布する実機実験用の Simulink モデルは，Manually で COM Port 番号を 3 に設定していますので，必要に応じて番号を変更してください．また，MinSegMega Shield キット付属の Arduino 互換ボードの種類によっては，Automatically と設定しても大丈夫です．
:::

とはいっても，多くの方は，この実機実験をする環境にはないと思いますが，非線形シミュレーションにより雰囲気だけでも感じ取ってください．

　なお，配布するファイル等の動作保証およびテクニカルサポート，発生した損害に対する責任は負いません．

# <font color="#33CC33">1. はじめに</font>

　何も考えずに

https://qiita.com/advent-calendar/2022/matlab

の最終日に登録してしまった…　だって，空席だったんで…　ただ単に，締切日を先延ばしにしたかっただけなんです．

　それはさておき，みなさん，<b>『倒立振子』</b>（「とうりつしんし」あるいは「とうりつふりこ」）って知ってますか？　<b>『倒立振子』とは『ほうきを建てる遊び』を，『自動制御』の技術によりこのように実現したもの</b>です．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f4d63c5d-5825-0052-7cce-5f8ff699e84f.png)

https://twitter.com/i/status/1476732122451034112

倒立振子の解説記事やスライドは

https://www.jstage.jst.go.jp/article/isciesci/56/4/56_KJ00008019364/_article/-char/ja/

https://youtu.be/MfhpdYHZtJ0

などをご参照ください．大御所の先生方の座談会の記事

https://www.jstage.jst.go.jp/article/sicejl1962/18/12/18_12_1029/_article/-char/ja/

なんかも面白いですよ．是非，ご一読ください．

---

　さて，本記事では，<b><font color="#E84746">LEGO 部品</font></b>を利用して製作された[<b><font color="#E84746">『回転型倒立振子 (Furuta Pendulum)』](https://en.wikipedia.org/wiki/Furuta_pendulum)

https://en.wikipedia.org/wiki/Furuta_pendulum

の振り上げ安定化制御</font></b>を，MATLAB/Simulink により実現する方法を説明します．Segway を模範とした<b>『車輪型倒立振子』</b>

https://youtu.be/l4cLZnz4gJw

とは異なり，『回転型倒立振子』は振子が<b><font color="#E84746">ソンビのよう繰り返し立ち上がらせる</font></b>ことができるので，展示会などで観客の皆さんを喜ばせるのに適しているのではないかと思います．<b>まずは，この[動画](https://www.youtube.com/watch?v=7YXKpn2O2ZE)をご覧ください．</b>

https://www.youtube.com/watch?v=7YXKpn2O2ZE

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8c7bb77f-bc94-8816-b01d-8fdd3dbd78a0.png)
また，『回転型倒立振子』は標準的な<b>『台車型倒立振子』</b>

https://youtu.be/vQ607X6Rid4

と比べてコンパクトな構造であるという利点もあります．

　ところで，マスワークス様から無償で提供されている [<b>Simulink Support Package for Arduino Hardware</b>](https://jp.mathworks.com/help/supportpkg/arduino/) を利用することで，Simulink によりお手軽に Arduino にコントローラを実装して動かしたり，リアルタイムで信号の計測を行うことができるようになりました．<b>なんとスバラシイことでしょう！</b> 　当然，~~情報工学科出身なのにプログラミング「よわよわ」な私は，~~ <b>Arduino を動かすのに</b>，スケッチ (sketch) ではなく，<b>Simulink を利用したくなりますよね</b>．

　多くの場合，『回転型倒立振子』の振り上げ安定化制御を実現するために，

* 振り上げコントローラ
* 安定化コントローラ

を適当な振子の角度で切り替えます．振子の角度だけで 2 つのコントローラを切り替えるだけであれば，[<b>"Switch"</b>](https://jp.mathworks.com/help/simulink/slref/switch.html) っていう Simulink ブロックを利用すれば良いでしょうが，振子の角度だけでなく角速度も切り替え条件に利用したり，無制御の領域を考えたりすると，<b><font color="#E84746">分岐処理</font></b>が複雑になり，なかなか大変そうです．

　そこで，本記事では，この複雑な<b><font color="#E84746">分岐処理</font></b>を Simulink で実装するために，Simulink ブロック [<b>"If"</b>](https://jp.mathworks.com/help/simulink/slref/if.html) および [<b>"If Action Subsystem"</b>](https://jp.mathworks.com/help/simulink/slref/ifactionsubsystem.html) を利用します．なお，[<b><font color="#E84746">MATLAB/Simulink Advent Calendar</font></b>](https://qiita.com/advent-calendar/2022/matlab) の記事ですので，<b>理論については[<font color="#33CC33">付録 A</font>](https://qiita.com/Carter/private/db4bb11ead93ea3a1d42#%E4%BB%98%E9%8C%B2-a%E6%8C%AF%E3%82%8A%E4%B8%8A%E3%81%92%E3%82%B3%E3%83%B3%E3%83%88%E3%83%AD%E3%83%BC%E3%83%A9) にまとめてます！</b>

　ちなみに，本記事のきっかけは [<b>toshi | Simulink の中の人</b>さん](https://twitter.com/toshi34780378)のつぶやきです．
　情報提供，ありがとうございました！

https://twitter.com/toshi34780378/status/1498432948097499137





# <font color="#33CC33">2. 振り上げ安定化制御を実現するコントローラの実装</font>

## <font color="#33CC33">2.1　回転型倒立振子の非線形モデル</font>

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/24c8aa3a-6f71-1255-09c2-e5e2fb4691f1.png)

　[<b>この記事</b>](https://qiita.com/Carter/items/80a1ed5d09f9dc420871)で説明したように，倒立振子の非線形モデルは
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	% =================================================
	\ddot{\theta}_{1} 
	&={}- {\alpha}_{1}\dot{\theta}_{1} 
	+ {\beta}_{1}v 
	\tag{1}
	% --------
	\\
	{L}_{1}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ {\alpha}_{2}\ddot{\theta}_{2}
	&= {\alpha}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ g\sin{\theta}_{2}
		- {\beta}_{2}\dot{\theta}_{2}
	\tag{2}
\end{align}
```
で与えられます．ただし，
```math
\begin{align}
    {\alpha}_{2} = \dfrac{{J}_{2}}{{m}_{2}{l}_{2}},\ 
    {\beta}_{2} = \dfrac{{c}_{2}}{{m}_{2}{l}_{2}}
\end{align}
```
です．パラメータの値は以下のとおりです．

|パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
|---|---|---|
|$g$       |$9.81 \times 10^{ 0}$ $[\rm{m/s^2}]$|g|
|$L_1$     |$7.00 \times 10^{-2}$ $[\rm{m}]$|L1|
|$\alpha_1$|$1.53 \times 10^{ 1}$|a1|
|$\beta_1$ |$7.06 \times 10^{ 1}$|b1|
|$\alpha_2$|$1.10 \times 10^{-1}$|a2|
|$\beta_2$ |$3.59 \times 10^{-2}$|b2|



## <font color="#33CC33">2.2　切り替え条件</font>

　振子の振り上げ安定化を行うために，本記事では，振子の角度や角速度に応じて

* <font color="#0070C0"><b>振り上げ制御</b></font>（導出は[<b><font color="#33CC33">付録 A</font></b>](https://qiita.com/Carter/private/db4bb11ead93ea3a1d42#%E4%BB%98%E9%8C%B2-a%E6%8C%AF%E3%82%8A%E4%B8%8A%E3%81%92%E3%82%B3%E3%83%B3%E3%83%88%E3%83%AD%E3%83%BC%E3%83%A9) を参照）
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	\definecolor{mydarkyellow}{rgb}{0.8941,0.5137,0.0706}
	% =================================================
    \textcolor{myblue}{v}\; &
    \textcolor{myblue}{= {f}_{\rm sw}(\dot{\theta}_{1},\,{\phi}_{2},\,\dot{\phi}_{2})}
    \\
    &\textcolor{myblue}{:= \dfrac{1}{{\beta}_{1}}\biggl(
            {\alpha}_{1}\dot{\theta}_{1}
            + {k}_{\rm sw}\dot{\phi}_{2}\cos{\phi}_{2}
            - \dfrac{{\alpha}_{2}}{{L}_{1}}
                \dot{\theta}{}_{1}^{2}\sin{\phi}_{2}
            + \dfrac{{\beta}_{2}}
                    {{L}_{1}}
              \dfrac{\dot{\phi}_{2}}
                    {\cos{\phi}_{2}}
        \biggr)}
    \tag{3}
\end{align}
```

* <font color="#0CA274"><b>安定化制御</b></font>
```math
\begin{align}
    \textcolor{mygreen}{v}\; &\textcolor{mygreen}{= Kx}
    \\ 
      &\textcolor{mygreen}{
       = {k}_{1}{\theta}_{1}
       + {k}_{2}\dot{\theta}_{1}
       + {k}_{3}{\theta}_{2}
       + {k}_{4}\dot{\theta}_{2}}
    \tag{4}
\end{align}
```

* <font color="#E48312"><b>無制御</b></font>
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	\definecolor{mydarkyellow}{rgb}{0.8941,0.5137,0.0706}
	% =================================================
    \textcolor{mydarkyellow}{v = 0}
    \tag{5}
\end{align}
```
を切り替えます．切り替え条件としては，以下の点を考慮しました．

* <font color="#0070C0"><b>振り上げ制御</b></font>を行うのは，振子が真下から水平の少し手前までの領域とします．
* <font color="#0CA274"><b>安定化制御</b></font>を行うのは，振子の倒立付近の領域とします．ただし，この領域に入っていたとしても，振子の角速度が大きいときは，制御入力を $0$ として<font color="#E48312"><b>無制御</b></font>とする（振子を惰性で回転させる）．
* 振子の角度が上記の領域に入っていないときは，制御入力を $0$ として<font color="#E48312"><b>無制御</b></font>とする（振子を惰性で回転させる）．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8ccac9ec-ef24-e92c-c2f3-bfb0ba563ef5.png)

<font color="#E48312"><b>無制御</b></font>の領域を比較的，大きく取りますが，これは，<b>倒立付近での振子の角速度を抑える</b>ためです．

　以上をまとめますと，<b><font color="#E84746">振り上げ安定化の分岐処理</font></b>は次式のようになります．
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	\definecolor{mydarkyellow}{rgb}{0.8941,0.5137,0.0706}
	% =================================================
    v = \left\{\begin{array}{ll}
           \textcolor{myblue}{
            {f}_{\rm sw}(\dot{\theta}_{1},\,{\phi}_{2},\,\dot{\phi}_{2})} &
            \textcolor{myblue}{(|{\theta}_{2}| \ge {\theta}_{\rm 2sw})}
           \\
           \textcolor{mygreen}{
             {k}_{1}{\theta}_{1}
           + {k}_{2}\dot{\theta}_{1}
           + {k}_{3}{\theta}_{2}
           + {k}_{4}\dot{\theta}_{2}} &
          \textcolor{mygreen}{ (|{\theta}_{2}| \le {\theta}_{\rm 2stb}}\ \\
            & \quad 
          \textcolor{mygreen}{
            {\rm and}\ |\dot{\theta}_{2}| \le {\omega}_{\rm 2stb})}
           \\
          \textcolor{mydarkyellow}{0} & 
            \textcolor{mydarkyellow}{({\rm otherwise})}
    \end{array}\right.
    \tag{6}
\end{align}
```


## <font color="#33CC33">2.3　Simulink ブロックでの実装</font>

### <font color="#33CC33">2.3.1　ステップ 1：ブロックの配置</font>

　$(6)$ 式の<b><font color="#E84746">分岐処理</font></b>を Simulink で実装するために，Simulink ブロック <b>"If"</b> や <b>"If Action Subsystem"</b> を使用します．これらの Simulink ブロックは，Simulink ライブラリ <b>"Ports & Subsystems"</b> に含まれています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/a869f5ca-574a-6619-91f7-16d737cdda6a.png)

　まず，空の Simulink モデルに <b>"Subsystem"</b> を配置します．そして，<b>"Subsystem"</b> のなかに以下のように Simulink ブロックを配置します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/454c7288-9ba8-8441-9dbf-596ea9add029.png)

### <font color="#33CC33">2.3.2　ステップ 2："If" の設定</font>

　<b>"If"</b> をダブルクリックし，

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/10b304b1-548c-dcf5-d91a-0c4e70339590.png)

のように設定します．なお，

* <b>"If"</b>
* <b>"If Action Subsystem"</b> の Action

では，後述の設定により

* u1：$|\theta_2|\ [{\rm rad}]$
* u2：$\theta_{\rm 2stb}\ [{\rm rad}]$
* u3：$\theta_{\rm 2sw}\ [{\rm rad}]$
* u4：$|\dot{\theta}_{2}|\ [{\rm rad/s}]$
* u5：$\omega_{\rm 2stb}\ [{\rm rad/s}]$

を意味していることに注意してください．つまり，<b>"If"</b> では，条件式を
```
if  u1 >= u3
  then 振り上げ制御
elseif u1 <= u2 and u4 <= u5
  then 安定化制御
else 無制御
```
と設定していることになります．



### <font color="#33CC33">2.3.3　ステップ 3："Gain", "Constant", "Demux", "Mux", "Merge" の設定</font>

　Simulink ブロック <b>"Gain"</b>, <b>"Constant"</b>, <b>"Demux"</b>, <b>"Mux"</b>, <b>"Merge"</b> は以下のように設定します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f1c1bcca-0f06-f9c5-b41e-2c7ee38ce6af.png)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d1a0ba5a-85ef-c85a-b783-430933331880.png)


### <font color="#33CC33">2.3.4　ステップ 4：ブロックの結線</font>

　Simulink ブロックの大きさや名前を適当に変更し，以下のように結線します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/684e1fa5-1ca9-5fef-c06d-d1390656a65a.png)



### <font color="#33CC33">2.3.5　ステップ 5："If Action Subsystem" の設定</font>

　最後に，3 つの <b>"If Action Subsystem"</b> の設定を行います．<font color="#0070C0"><b>"If Action Subsystem"</b></font>, <font color="#0CA274"><b>"If Action Subsystem1"</b></font>, <font color="#E48312"><b>"If Action Subsystem2"</b></font> の Action に <b>"If"</b> の分岐条件 (if, elseif, else) を結線していますので，それぞれに

* <font color="#0070C0"><b>"If Action Subsystem"</b></font>：<font color="#0070C0"><b>振り上げ制御</b></font>
* <font color="#0CA274"><b>"If Action Subsystem1"</b></font>：<font color="#0CA274"><b>安定化制御</b></font>
* <font color="#E48312"><b>"If Action Subsystem2"</b></font>：<font color="#E48312"><b>無制御</b></font>

の操作を表現します．

---
#### (a) <font color="#0070C0">"If Action Subsystem"</font> の設定

　<font color="#0070C0"><b>"If Action Subsystem"</b></font> では<font color="#0070C0"><b>振り上げ制御</b></font>のコントローラ $(3)$ 式を実装します．<font color="#0070C0"><b>"If Action Subsystem"</b></font> の入力端子 <b>"In1"</b> には <b>"Mux"</b> が結線されているので，<font color="#0070C0"><b>"If Action Subsystem"</b></font> のなかでは，

* u(1)：$x_1 = \theta_1\ {\rm [rad]}$（アームの角度）
* u(2)：$x_2 = \dot{\theta}_1\ {\rm [rad/s]}$（アームの角速度）
* u(3)：$\phi_2\ {\rm [rad]}$（真下を基準とした振子の角度）
* u(4)：$x_4 = \dot{\theta}_2 = \dot{\phi}_2\ {\rm [rad/s]}$（振子の角速度）
* u(5)：$k_{\rm sw}$：振り上げゲイン

が使用できます．

　<font color="#0070C0"><b>"If Action Subsystem"</b></font> には以下のようにブロックを配置し，<b>"Fcn"</b> を設定したうえで結線します．
|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/dda4b066-8983-d317-b2dd-516706a46c2e.png)|
|:-:|
|<font color="#0070C0"><b>"If Action Subsystem"</b></font> におけるブロックの配置，設定および結線|

<b>"Fcn"</b>（<b>R2020b 以降</b>の場合，[<b><font color="#33CC33">付録 B</font></b>](https://qiita.com/Carter/private/db4bb11ead93ea3a1d42#%E4%BB%98%E9%8C%B2-br2020b-%E4%BB%A5%E9%99%8D%E3%81%A7%E3%81%AE-simulink-%E3%83%96%E3%83%AD%E3%83%83%E3%82%AF-fcn-%E3%81%AE%E5%88%A9%E7%94%A8) を参照してください）では<font color="#0070C0"><b>振り上げコントローラ $(3)$ 式</b></font>の右辺
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	\definecolor{mydarkyellow}{rgb}{0.8941,0.5137,0.0706}
	% =================================================
    \textcolor{myblue}{\dfrac{1}{{\beta}_{1}}\biggl(
            {\alpha}_{1}\dot{\theta}_{1}
            + {k}_{\rm sw}\dot{\phi}_{2}\cos{\phi}_{2}
            - \dfrac{{\alpha}_{2}}{{L}_{1}}
                \dot{\theta}{}_{1}^{2}\sin{\phi}_{2}
            + \dfrac{{\beta}_{2}}
                    {{L}_{1}}
              \dfrac{\dot{\phi}_{2}}
                    {\cos{\phi}_{2}}
        \biggr)}
\end{align}
```
を以下のように記述しています．
```
(1/b1)*(a1*u(2) + u(5)*u(4)*cos(u(3)) - (a2/L1)*(u(2)^2)*sin(u(3)) + (b2/L1)*u(4)/cos(u(3)))
```

また，<font color="#0070C0"><b>振り上げコントローラ $(3)$ 式</b></font>で生成された制御入力 $v$ をそのまま使用すると，倒立点付近での振子の回転速度が過大になり，<font color="#0CA274"><b>安定化制御</b></font>の切り替えがうまくいかないことがあります．そこで，<b>"Saturation"</b> を利用して制御入力の大きさを $|v| \le {v}_{\rm sat}$ のように制限し，<font color="#0070C0"><b>振り上げコントローラ $(3)$ 式</b></font>を
```math
\begin{align}
    \textcolor{myblue}{
    v = \left\{\begin{array}{ll}
            -{v}_{\rm sat} & 
            (|\theta_2| \ge \theta_{\rm 2sw}\ {\rm and}\ v < -{v}_{\rm sat}) \\
            {f}_{\rm sw}(\dot{\theta}_{1},\,{\phi}_{2},\,\dot{\phi}_{2}) & 
            (|\theta_2| \ge \theta_{\rm 2sw}\ {\rm and}\ |v| \le {v}_{\rm sat}) \\
           {v}_{\rm sat} & 
            (|\theta_2| \ge \theta_{\rm 2sw}\ {\rm and}\ v > {v}_{\rm sat})
        \end{array}\right.
    }
    \tag{7}
\end{align}
```
のように修正します．

---
#### (b) <font color="#0CA274">"If Action Subsystem1"</font> の設定

　<font color="#0CA274"><b>"If Action Subsystem1"</b></font> では<font color="#0CA274"><b>安定化コントローラ $(4)$ 式</b></font>を実装します．<font color="#0CA274"><b>"If Action Subsystem1"</b></font> の入力端子 <b>"In1"</b> には状態変数
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	\definecolor{mydarkyellow}{rgb}{0.8941,0.5137,0.0706}
	% =================================================
    x = \left[\begin{array}{c}
            {x}_{1} \\
            {x}_{2} \\
            {x}_{3} \\
            {x}_{4} \\
        \end{array}\right]
    = \left[\begin{array}{c}
            {\theta}_{1} \\
            \dot{\theta}_{1} \\
            {\theta}_{2} \\
            \dot{\theta}_{2} \\
        \end{array}\right]
\end{align}
```
が入力されているので，以下のようにブロックを配置し，<b>"Gain"</b> を設定したうえで結線します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/f4089672-b4ff-4bbb-ca39-eb8880ff40ae.png)|
|:-:|
|<font color="#0CA274"><b>"If Action Subsystem1"</b></font> におけるブロックの配置，設定および結線|

---
#### (c) <font color="#E48312">"If Action Subsystem2"</font> の設定

　<font color="#E48312"><b>"If Action Subsystem2"</b></font> は<font color="#E48312"><b>無制御</b></font>とした $(5)$ 式を実装します．入力端子は不要なので，<b>"In1"</b> を削除したうえで，以下のようにブロックを配置し，<b>"Constant"</b> を設定したうえで結線します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0af73879-6689-745a-9dc4-893d7ad40929.png)|
|:-:|
|<font color="#E48312"><b>"If Action Subsystem2"</b></font> におけるブロックの配置，設定および結線|

---

　以上の設定を行うと，振り上げ安定化を実現するコントローラ $(6)$ 式は，Simulink により以下のように実装されます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/5cd96dc5-d88b-e35e-62a4-c65048518245.png)|
|:-:|
|振り上げ安定化コントローラ $(6)$ 式を Simulink により実装！！（Subsytem <b>"Swing-up and Stabilization"</b>）|




# <font color="#33CC33">3. 振り上げ安定化制御の実機実験</font>

　実機実験は LEGO 倒立振子で行っています．実験装置の組み立てや，Simulink モデルにより実機を動かす方法などについては，

* [「LEGO 部品を利用した回転型倒立振子のレシピを公開！」のまとめ](https://qiita.com/Carter/items/a3ddaad078bbebe42998)

をご参照ください．マイコン部は [<b>MinSegShield</b>](https://minseg.com/) のキットに含まれている Arduino 互換ボードと Shield を利用しています．


## <font color="#33CC33">3.1　作成した Simulink モデル</font>

　最終的に作成した Simulink モデル <b>"ex_sfbk_ip_swing_sat.slx"</b> を以下に示します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/de3f6fef-fecb-689c-0d8e-709480052948.png)|
|:-:|
|Simulink モデル <b>"ex_sfbk_ip_swing_sat.slx"</b>|

サンプリング周期は $h = 0.01\ {\rm [s]} = 10\ {\rm [ms]}$ とし，動的な部分は離散時間系で表現されています．

　なお，最初に振り上げ制御のトリガが必要ですので，最初の 0.1 秒間は <b>"Sine Wave"</b> と <b>"Gain"</b> により生成される正弦波（角周波数 15 [rad/s]，振幅 1 [V]）を 4.5 倍したものを制御入力 $v$ [V] として加えています．

　Simulink モデル <b>"ex_sfbk_ip_swing_sat.slx"</b> の詳しい設定は，実際に Simulink モデル <b>"ex_sfbk_ip_swing_sat.slx"</b> をダウンロードして確認していただければと思いますが，以下に，いくつか補足します．






|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/2981c07d-029e-e9b2-849b-6021da8c1a92.png)|
|:-:|
|Subsytem <b>"Inverted Pendulum"</b>|

:::note info
Subsytem <b>"Inverted Pendulum"</b>
* Subsytem <b>"Inverted Pendulum"</b> は上記のように記述されています．[MinSeg 社](https://minseg.com/)の Simulink ライブラリ [<b>Rensselaer Arduino Support Package Library (RASPLib)</b>](https://jp.mathworks.com/matlabcentral/fileexchange/62702-rensselaer-arduino-support-package-library-rasplib) で提供されている <b>"M1 Right Connector 2, 3"</b> によりアーム角度 $\theta_1$ を，<b>"M2 Left Connector 11, 12"</b> により振子角度を取り込んでいます．また，<b>"Left Motor Driver PWM6, PWM7"</b> によりモータを駆動しています（制限電圧 $|v| \le 9\ {\rm [V]}$）．

* <b>"Gain"</b>, <b>"Gain1"</b> はそれぞれカウント数（4 逓倍）から角度への変換係数です．また，<b>"Gain2"</b>, <b>"Gain3"</b>, <b>"Gain4"</b> は，正転・逆転の方向を決める係数です（想定の回転方向と逆の場合は -1 に設定します）．

* 暴走を防ぐため，Subsytem <b>"Inverted Pendulum"</b> において，アーム角度が 1 回転半を超える ($|\theta_1| > 3\pi\ {\rm [rad]}$) と，無制御 ($v = 0$) となるようにしています．
:::


:::note info
不完全微分器の離散化
角度から角速度への変換には，<b>不完全微分器（微分器と 1 次のローパスフィルタの直列結合）</b>
```math
\begin{align}
    G_{{\rm f}i}(s) = \dfrac{s}{1 + T_{{\rm f}i}s}\ 
    (i = 1,\,2)
    \tag{8}
\end{align}
```
を<b>双 1 次変換（台形近似）</b>により離散化した
```math
\begin{align}
    G_{{\rm f}i}(s)|_{s = \frac{2}{h}\frac{z - 1}{z + 1}} 
    = \dfrac{2z - 2}{(h + 2T_{{\rm f}i})z + h - 2T_{{\rm f}i}}\ (i = 1,\,2)
    \tag{9}
\end{align}
```
が使用されています．ただし，$z$ はシフトオペレータであり，1 サンプリングの進みを意味します．
:::



|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/df7fe7dd-9a4c-fab0-a256-a1615168c681.png)|
|:-:|
|Subsytem <b>"phi2 to theta2"</b>|

:::note info
Subsytem <b>"phi2 to theta2"</b>
Subsytem <b>"Inverted Pendulum"</b> に含まれる Subsytem <b>"phi2 to theta2"</b> は，振子角度を真下基準の $\phi_2$ から真上基準の $\theta_2$ に変換するために作成しました．
* この Subsytem では，<b>"Fcn (sin)"</b> により $\sin\phi_2$ を，<b>"Fcn1 (cos)"</b> により $\cos\phi_2$ を生成し，これらを <b>"Fcn2 (atan2)"</b> により逆正接の操作 $
    \tan^{-1}\dfrac{\sin\phi_2}{\cos\phi_2}
$ を行っています．<b>"atan2"</b> を使用していますので，この操作により<b><font color="#E84746">振子が 1 回転以上の回転をしたときでも，常に $|\phi_2| \le \pi$ の振子角度 $\phi_2$ が得られます</font></b>．

* $|\phi_2| \le \pi$ の真下基準の振子角度 $\phi_2$ を，次式により真上基準の振子角度 $\theta_2$ に変換します（このとき，$|\theta_2| \le \pi$ となります）．
```math
\begin{align}
    \theta_2 = \left\{\begin{array}{ll}
			\phi_2 - \pi & (0 \le \phi_2 \le \pi) \\
			\phi_2 + \pi & (-\pi \le \phi_2 < 0) 
        \end{array}\right.
    \tag{10}
\end{align}
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e3ece400-0c48-cf27-58ce-0e1d551c4ea7.png)
:::






## <font color="#33CC33">3.2　M ファイル</font>

　実験を行う前に，

* サンプリング周期 $h$ の設定
* 不完全微分の時定数 $T_{{\rm f}1}$, $T_{{\rm f}2}$ の設定
* 分岐処理を行うための角度や角速度のしきい値の設定
* 振り上げゲイン $k_{\rm sw}$ の設定
* 最適レギュレータによる安定化コントローラのゲイン $K$ の設計

を行う必要があります．そのための M ファイルを以下に示します．

```matlab:M ファイル "ip_design_sat.m"
clear
format compact

h = 0.01;        % サンプリング周期

Tf1 = 0.08;      % ローパスフィルタの時定数
Tf2 = 0.08;      % ローパスフィルタの時定数

% =============================================
% If Action Subsystem (Swing-up Control) で
% Saturation を使用する場合（上限：1.75 [V]）

theta2stb =  25; % deg
omega2stb = 300; % deg/s
theta2sw  = 120; % deg

ksw   = 8;       % 振り上げゲイン
v_sat = 1.75;    % 振り上げ制御時の制御入力の上限値

% =============================================
g  = 9.81e+00;
L1 = 7.00e-02;

a1 = 1.53e+01;
b1 = 7.06e+01;

a2 = 1.10e-01;
b2 = 3.59e-02;

E = [ 1  0  0  0
      0  1  0  0
      0  0  1  0
      0  L1 0  a2 ];
F = [ 0  1  0  0
      0 -a1 0  0
      0  0  0  1
      0  0  g -b2 ];
G = [ 0
      b1
      0
      0 ];

disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')
disp(' システム行列 A, B')
disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')

A = inv(E)*F
B = inv(E)*G

disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')
disp(' 重み行列 Q, R')
disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')

q1 = 0.2;
q3 = 100;

Q = diag([q1 0 q3 0])
R = 1

disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')
disp(' 最適レギュレータによるコントローラゲイン K')
disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')

K = -lqr(A,B,Q,R)

disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')
disp(' A + B*K の固有値')
disp('+++++++++++++++++++++++++++++++++++++++++++++++++++++++')

eig(A + B*K)
```

M ファイル <b>"ip_design_sat.m"</b> を実行すると，以下の結果が得られます．

```matlab:M ファイル "ip_design_sat.m" の実行結果
>> ip_design
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
 システム行列 A, B
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
A =
         0    1.0000         0         0
         0  -15.3000         0         0
         0         0         0    1.0000
         0    9.7364   89.1818   -0.3264
B =
         0
   70.6000
         0
  -44.9273
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
 重み行列 Q, R
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
Q =
    0.2000         0         0         0
         0         0         0         0
         0         0  100.0000         0
         0         0         0         0
R =
     1
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
 最適レギュレータによるコントローラゲイン K
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
K =
    0.4472    0.6013   17.7962    1.5288
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
 A + B*K の固有値
+++++++++++++++++++++++++++++++++++++++++++++++++++++++
ans =
 -18.7279 +12.1869i
 -18.7279 -12.1869i
  -2.2029 + 0.8872i
  -2.2029 - 0.8872i
```

## <font color="#33CC33">3.3　実機実験</font>

　振り上げ安定化制御の実機実験を「エクスターナルモード（監視と調整）」で行うと，以下のようになります．

https://twitter.com/7IJgWKwlox7sUhb/status/1600073209109282816

* さほど強くつついていないときは，倒れずに倒立を維持しています．
* 少し強めにつつくと，振子は倒れますが，速やかに倒立状態に復帰します．
* 強めにつつくと，振子は倒れた後，何回か回転しますが，徐々に回転速度が減少し，倒立状態に復帰します．

　一方で，振子の角度や角速度の制約が適切でないと，振子が荒ぶれます！　これが LEGO じゃなかったら危険です！ 

https://twitter.com/7IJgWKwlox7sUhb/status/1600073498218573824

っていうか，荒ぶっている方が再生回数が多いとは・・・


# <font color="#33CC33">4. おわりに</font>

　本記事では，回転型倒立振子の振り上げ安定化制御を，Simulink の分岐処理により実現する方法を説明しました．実機実験を行うために，結構，<b><font color="#E84746">ゴリゴリの作業</font></b>をしていることがわかります．この手の解説記事はあまり見当たらないと思います（~~査読付論文に投稿するような内容ではない~~）ので，皆さまの参考になれば幸いです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/092239c5-2223-b4a4-d89d-4f91e382d9be.png)


# <font color="#33CC33">参考文献</font>

1. https://en.wikipedia.org/wiki/Furuta_pendulum

2. [川田昌克（編著）・東　俊一・市原裕之・浦久保孝光・大塚敏之・甲斐健也・國松禎明・澤田賢治・永原正章・南　裕樹： 倒立振子で学ぶ制御工学， 森北出版 (2017.2.28)](https://www.amazon.co.jp/dp/4627792212/)（とくに，第 II 部 (発展編) 第 3 章 非線形制御 3.2.1 エネルギー法：浦久保先生のご担当分）　$\cdots$$\cdots$ [サポートページ](https://www.maizuru-ct.ac.jp/control/kawata/study/book_ip/book_ip_page.html)



# <font color="#33CC33">おまけ</font>

　ここまで読んで，「実機実験じゃなくてシミュレーションだけしたい！」と思った方，安心してください．ファイルを公開していています．詳細の説明をする余力はありませんが…

* https://github.com/Prof-Carter/ip_swing_up/blob/main/simulation.zip

https://twitter.com/7IJgWKwlox7sUhb/status/1600329624038572034



---
# <font color="#33CC33">付録 A　振り上げコントローラ</font>

　振り上げ制御を行うために，振子の力学的エネルギーが増大するように制御入力を加えることにします．

　倒立振子の非線形モデルは $(1)$, $(2)$ 式に示したように，
```math
\begin{align}
	\require{color}
	\definecolor{myred}{rgb}{0.9098,0.2784,0.2745}
	\definecolor{myblue}{rgb}{0,0.4392,0.7529}
	\definecolor{mygreen}{rgb}{0.0471,0.6353,0.4549}
	\definecolor{mypink}{rgb}{1,0.4,0.6}
	\definecolor{myskyblue}{rgb}{0,0.6902,0.9412}
	\definecolor{mydarkpink}{rgb}{0.8902,0.1765,0.5686}
	% =================================================
	\ddot{\theta}_{1}
    &=
	{}- {\alpha}_{1}\dot{\theta}_{1} 
	+ {\beta}_{1}v 
	\tag{A.1}
	% --------
	\\
	&\hspace{-2cm}
    {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ {J}_{2}\ddot{\theta}_{2}
	\\
	&
	= {J}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ {m}_{2}g{l}_{2}\sin{\theta}_{2}
		- {c}_{2}\dot{\theta}_{2}
	\tag{A.2}
\end{align}
```
で与えられます．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/1cf739f5-9fb6-2b9f-baa3-8d83e1b662ce.png)
　振子の力学的エネルギー $E$ は
```math
\begin{align}
    E = \dfrac{1}{2}{J}_{2}\dot{\theta}{}_{2}^{2} 
      + {m}_{2}gh > 0
    \tag{A.3}
\end{align}
```
となります．ここで，振子がぶら下がった状態を基準とした重心位置の高さ $h$ は
```math
\begin{align}
    h = {l}_{2} + {l}_{2}\cos{\theta}_{2}
      = {l}_{2}(1 + \cos{\theta}_{2})
\end{align}
```
ですので，$\rm (A.3)$ 式は
```math
\begin{align}
    E = \dfrac{1}{2}{J}_{2}\dot{\theta}{}_{2}^{2} 
      + {m}_{2}g{l}_{2}(1 + \cos{\theta}_{2}) > 0
    \tag{A.4}
\end{align}
```
となり，常に正です．この振子の力学的エネルギー $E$ を増大させるために，

* <b>力学的エネルギー $E$ を単調増加させる $(E > 0,\ \dot{E} > 0)$</b>
 
ことにします．

　力学的エネルギー $E$ の時間微分を求めると，$\rm (A.4)$ 式より
```math
\begin{align}
    \dot{E} = \dot{\theta}_{2}\bigl(
                {J}_{2}\ddot{\theta}_{2}
                - {m}_{2}g{l}_{2}\sin{\theta}_{2}
            \bigr)
    \tag{A.5}
\end{align}
```
となります．ここで，$\rm (A.2)$ 式および $\rm (A.1)$ 式より
```math
\begin{align}
	&{J}_{2}\ddot{\theta}_{2} - {m}_{2}g{l}_{2}\sin{\theta}_{2}
	\\
	&\qquad
	= {J}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		- {c}_{2}\dot{\theta}_{2}
		- {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot\ddot{\theta}_{1} 
    \\
	&\qquad
	= {J}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		- {c}_{2}\dot{\theta}_{2}
	\\
    &\qquad\qquad\qquad{}
		- {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot
    		\bigl(
                {}- {\alpha}_{1}\dot{\theta}_{1} + {\beta}_{1}v
            \bigr) 
    \tag{A.6}
\end{align}
```
となります．したがって，$\rm (A.6)$ 式を $\rm (A.5)$ 式に代入すると，
```math
\begin{align}
    \dot{E} = \dot{\theta}_{2}\bigl\{
                  & {J}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
                		- {c}_{2}\dot{\theta}_{2}
                	\\
                  &\hspace{1.3cm} {}
                	- {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot
                   	\bigl(
                        {}- {\alpha}_{1}\dot{\theta}_{1} + {\beta}_{1}v
                    \bigr) 
            \bigr\}
    \tag{A.7}
\end{align}
```
が得られます．

　つぎに，$\rm (A.7)$ 式に基づいて，<b>力学的エネルギー $\pmb{E}$ を単調増加させる $(E > 0,\ \dot{E} > 0)$</b> ような制御入力 $v$ を生成します．このような制御入力 $v$ のひとつとして，力学的エネルギーの時間微分 $\dot{E}$ が
```math
\begin{align}
    \dot{E} = {k}_{\rm sw}{m}_{2}{L}_{1}{l}_{2}
                (\dot{\theta}_{2}\cos{\theta}_{2})^{2}
    > 0
    \quad (\dot{\theta}_{2} \neq 0\ {\rm and}\ \cos{\theta}_{2} \neq 0)
    \tag{A.8}
\end{align}
```
となるように選ぶことにします．ただし，${k}_{\rm sw} > 0$ は設計者が与える振り上げゲインです．

$\rm (A.8)$ 式となるような制御入力 $v$ は，$\rm (A.7)$ 式と $\rm (A.8)$ 式が等しいという関係から，
```math
\begin{align}
    & \phantom{\;\Longrightarrow\quad}
    {J}_{2}\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
                		- {c}_{2}\dot{\theta}_{2}
    \\
    &\hspace{2cm}{}
    - {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot
        \bigl(
        {}- {\alpha}_{1}\dot{\theta}_{1} + {\beta}_{1}v
        \bigr)
    = {k}_{\rm sw}{m}_{2}{L}_{1}{l}_{2}\dot{\theta}_{2}\cos^{2}{\theta}_{2}
    \\
    & \Longrightarrow\quad
    \dfrac{{J}_{2}}{{m}_{2}{L}_{1}{l}_{2}}
            \dot{\theta}{}_{1}^{2}\sin{\theta}_{2}
    - \dfrac{{c}_{2}}
            {{m}_{2}{L}_{1}{l}_{2}}
      \dfrac{\dot{\theta}_{2}}
            {\cos{\theta}_{2}}
    + {\alpha}_{1}\dot{\theta}_{1} - {\beta}_{1}v
    = {k}_{\rm sw}\dot{\theta}_{2}\cos{\theta}_{2}
    \\
    & \Longrightarrow\quad
    \dfrac{{\alpha}_{2}}{{L}_{1}}
            \dot{\theta}{}_{1}^{2}\sin{\theta}_{2}
    - \dfrac{{\beta}_{2}}
            {{L}_{1}}
      \dfrac{\dot{\theta}_{2}}
            {\cos{\theta}_{2}}
   + {\alpha}_{1}\dot{\theta}_{1} - {\beta}_{1}v
    = {k}_{\rm sw}\dot{\theta}_{2}\cos{\theta}_{2}
    \\
    & \Longrightarrow\quad
    v = \dfrac{1}{{\beta}_{1}}\biggl(
            {\alpha}_{1}\dot{\theta}_{1}
            - {k}_{\rm sw}\dot{\theta}_{2}\cos{\theta}_{2}
            + \dfrac{{\alpha}_{2}}{{L}_{1}}
                \dot{\theta}{}_{1}^{2}\sin{\theta}_{2}
            - \dfrac{{\beta}_{2}}
                    {{L}_{1}}
              \dfrac{\dot{\theta}_{2}}
                    {\cos{\theta}_{2}}
        \biggr)
    \tag{A.9}
\end{align}
```
のように得られます．ただし，
```math
\begin{align}
    {\alpha}_{2} = \dfrac{{J}_{2}}{{m}_{2}{l}_{2}},\ 
    {\beta}_{2} = \dfrac{{c}_{2}}{{m}_{2}{l}_{2}}
\end{align}
```
であり，$\theta_2 \neq \pm \pi/2$ とします．

:::note warn
注意
$\cos{\theta}_{2} \neq 0$ である，すなわち，$\theta_2 \neq \pm \pi/2$（振子が左右水平ではない）としていますが，本記事で説明するように，振子が左右水平の付近にあるときは，無制御としますので，気にしないでください．
:::

　最後に，振り上げ制御では，振子が真下にある状態から開始しますので，振子の基準角を真下からの変位 $\phi_2$ に変換します．そのために，$\rm (A.9)$ 式において，$\theta_2 = \phi_2 + \pi$ とすると，振り上げコントローラ
```math
\begin{align}
    v = \dfrac{1}{{\beta}_{1}}\biggl(
            {\alpha}_{1}\dot{\theta}_{1}
            + {k}_{\rm sw}\dot{\phi}_{2}\cos{\phi}_{2}
            - \dfrac{{\alpha}_{2}}{{L}_{1}}
                \dot{\theta}{}_{1}^{2}\sin{\phi}_{2}
            + \dfrac{{\beta}_{2}}
                    {{L}_{1}}
              \dfrac{\dot{\phi}_{2}}
                    {\cos{\phi}_{2}}
        \biggr)
    \tag{A.10}
\end{align}
```
が得られます．ただし，$\phi_2 \neq \pm \pi/2$ です．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/2ad7f7b9-4e64-7f07-de43-c68866642bbf.png)


# <font color="#33CC33">付録 B　R2020b から R2022b の場合の Simulink ブロック "Fcn" の利用</font>

　<b>"Fcn"</b> は <b>R2020b から R2022b</b> では標準のライブラリから削除されていますので（<b>R2023a から復活しました！</b>），以下の手順で利用してください．
　MATLAB のコマンドウィンドウで
```
>> simulink3
```
と入力をして，以下の <b>Simulink Block Library 5.0</b> を起動してください．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/00c98f10-ffd6-66ae-5473-463fdf6ba967.png)
そして，ライブラリ <b>"Functions & Tables"</b> のなかから <b>"Fcn"</b> を <font color="#0070C0"><b>"If Action Subsystem"</b></font> にコピーしてください．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0be66e98-6192-e59c-3a25-55ba18510273.png)