LEGO 部品を利用した回転型倒立振子のレシピを公開！（第 4 回：パラメータ同定 ー MATLAB / Simulink）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/19baa057-4c9d-ef41-f845-cd1b9ee9d3ed.png)

[YouTube 「かーたー」チャンネル](https://www.youtube.com/channel/UCQxZZypRXCsvALuvmWy68jw)

https://youtu.be/MfhpdYHZtJ0

https://youtu.be/ms35Zvcwrag

:::note info
<b><font color="#0070C0">LEGO 部品を利用した回転型倒立振子のレシピを公開！</font></b>
* [第 1 回：組み立て](https://qiita.com/Carter/items/6027bcb12dc7740e8dc3)
* [第 2 回：電子工作](https://qiita.com/Carter/items/ecad7ee7b8ea76c87c33)
* [第 3 回：運動方程式 ― MATLAB / Symbolic Math Toolbox](https://qiita.com/Carter/items/b4b77305f6a634a08468)
* [第 4 回：パラメータ同定 ― MATLAB / Simulink](https://qiita.com/Carter/items/80a1ed5d09f9dc420871)
* 第 5 回：現代制御 ― MATLAB / Simulink　…… <font color="#E84746">未完成</font>
* 第 6 回：アドバンスト制御 ― MATLAB / Simulink　…… <font color="#E84746">未完成</font>

<b>おまけ</b>
* [Simulink の分岐処理を利用して『回転型 LEGO 倒立振子』を振り上げ安定化！](https://qiita.com/Carter/items/db4bb11ead93ea3a1d42)


<b>補足</b>
*  [【補足説明】Simulink Support Package for Arduino Hardware と RASPLib をインストールして MinSegShield を動かしてみる](https://qiita.com/Carter/items/cc7faa5084aa4a69f4e7)
* [【補足説明】Simulink Support Package for Arduino Hardware で DC モータを動かしてみる](https://qiita.com/Carter/items/1c5f3ec49dbaa87bba79)
:::


＊ とりあえずアップしますね


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

　ラグランジュ法を利用した回転型倒立振子の運動方程式の導出については

* [第 3 回：運動方程式](https://qiita.com/Carter/items/b4b77305f6a634a08468)

で説明しました．今回は，運動方程式に含まれるパラメータを実験で定める（このことをパラメータ同定といいます）方法について説明します．



# <font color="#33CC33">2. 回転型倒立振子のモデル</font>

## <font color="#33CC33">2.1　ラグランジュ法により導出された運動方程式</font>

　 「[第 3 回：運動方程式編](https://qiita.com/Carter/items/b4b77305f6a634a08468)」で得られた運動方程式は

```math
\begin{align}
	&
	\bigl({J}_{1}
		+ {m}_{2}{L}_{1}^{2}
		+ {J}_{2}\sin^{2}{\theta}_{2}\bigr)\ddot{\theta}_{1}
	+ {m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot\ddot{\theta}_{2}
	\\
	&\qquad
	= {\tau}_{1} 
	- 2{J}_{2}\dot{\theta}_{1}\dot{\theta}_{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ {m}_{2}{L}_{1}{l}_{2}\dot{\theta}{}_{2}^{2}\sin{\theta}_{2}
		- {c}_{1}\dot{\theta}_{1}
	\tag{2.1}
	% --------
	\\
	&{m}_{2}{L}_{1}{l}_{2}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ {J}_{2}\ddot{\theta}_{2}
	\\
	&\qquad
	= {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{2.2}
\end{align}
```

です．$(2.1)$ 式はアームの運動方程式，$(2.2)$ 式は振子の運動方程式です．ただし，
```math
\begin{align}
	{J}_{1} = {J}_{\rm g1} + {m}_{1}{l}_{1}^{2},\ 
	{J}_{2} = {J}_{\rm g2} + {m}_{2}{l}_{2}^{2}
\end{align}
```
であり，パラメータは以下に示すとおりです．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/fd1b7a7b-67c2-170e-c9f1-64803637478e.png)|
|:-:|
|図 $2.1$　回転型倒立振子のパラメータ|

|記号　　　　|説明|
|---|---|
|$g$ $\rm[m/s^2]$|重力加速度|
|$m_1$ $\rm[kg]$ |アームの質量|
|$l_1$ $\rm[m]$  |アームの軸から重心までの長さ|
|$L_1$ $\rm[m]$  |アームの軸から振子の接合部までの長さ|
|$J_{\rm{g1}}$ $\rm[kg\cdot{m}^2]$|アームの重心まわりの慣性モーメント：$\theta_1$ 方向|
|$c_1$ $\rm[kg\cdot{m}^{2}/s]$ |アームの粘性摩擦係数|
|$m_2$ $\rm[kg]$ |振子の質量|
|$l_2$ $\rm[m]$  |振子の軸から重心までの長さ|
|$L_2$ $\rm[m]$  |振子の軸から先端までの長さ|
|$J_{\rm{g2}}$ $\rm[kg\cdot{m}^2]$|振子の重心まわりの慣性モーメント：$\theta_2$ 方向|
|$c_2$ $\rm[kg\cdot{m}^{2}/s]$ |振子の粘性摩擦係数|

## <font color="#33CC33">2.2　モータ駆動系のモデル</font>

　モータ駆動系（モータ $+$ モータドライバ）に定値の<font color="#0070C0">指令電圧 $v(t) = {v}_{\rm c}$ $(t \ge 0)$</font> を加えたときのふるまいを考えてみましょう．このとき，図 $2.2$ に示すように，定常状態では<font color="#0CA274">角度 $\theta_1(t)$</font> は一定の割合で増加していくことは容易に想像がつくことでしょう．これを時間微分したものが<font color="#E84746">角速度 $\omega_1(t) = \dot{\theta}_1(t)$</font> ですが，振動することなく，一定速度に収束します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/68891426-27c2-788e-0532-79357aaa6a12.png)|
|:-:|
|図 $2.2$　モータ駆動系のステップ応答|

つまり，入力を<font color="#0070C0">指令電圧 $v(t)$</font>，出力を <font color="#E84746">角速度 $\omega_1(t) = \dot{\theta}_1(t)$</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{myred}{{\omega}_{1}(s)}
	= \textcolor{mydarkyellow}{G(s)}
	\textcolor{myblue}{v(s)},\ 
	\textcolor{mydarkyellow}{G(s) = \dfrac{{\beta}_{1}}{s + {\alpha}_{1}}}
	\tag{2.3}
\end{align}
```
のように，<b><font color="#E48312">1 次遅れ系</font></b>となります．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/1a7fb3d4-50b4-263c-c7c8-cb35edc36c8f.png)|
|:-:|
|図 $2.3$　モータ駆動系のモデル|

したがって，入力を<font color="#0070C0">指令電圧 $v$</font>，出力を <font color="#0CA274">角度 ${\theta}_{1}$</font> としたモータ駆動系のモデルは，$(2.3)$ 式より
```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{mygreen}{y(s)}
	= P(s)
	\textcolor{myblue}{v(s)},\ 
	P(s) = G(s)\dfrac{1}{s}
	= \dfrac{{\beta}_{1}}{s(s + {\alpha}_{1})}
	\tag{2.4}
\end{align}
```
となり，$(2.4)$ 式を微分方程式で表すと，

```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}
	% =================================================
	&s(s + {\alpha}_{1}){\theta}_{1} = {\beta}_{1}v
	\\
	&\quad \Longrightarrow \quad 
	\ddot{\theta}_{1} + {\alpha}_{1}\dot{\theta}_{1} = {\beta}_{1}v
	\tag{2.5}
\end{align}
```

となります．モータ軸に水平面を回転するアームが負荷として取り付けられてている場合であっても，モデルは $(2.5)$ 式の形式です（パラメータ $\alpha_1$, $\beta_1$ の値は変わりますが）．

　それでは，つぎに，アームの運動方程式 $(2.1)$ 式について考えます．$(2.1)$ 式は入力がトルク ${\tau}_{1}$ となっていますが，アームと振子が取り付けられたモータ駆動系の入力は指令電圧 $v$ です．ここで，

* アームの先端に設置されている振子は LEGO 部品なので軽量である
* 使用したモータはギヤ付きである

というから，振子がアームに与える影響は十分小さいと考えることができます．したがって，アームと振子が取り付けられたモータ駆動系は，単純な

```math
\begin{align}
	\ddot{\theta}_{1} + {\alpha}_{1}\dot{\theta}_{1} = {\beta}_{1}v - d 
	\tag{2.6}
\end{align}
```

というモデルで近似できることがわかります．ただし，$\alpha_1$, $\beta_1$ は適当なパラメータです．また，$(2.5)$ 式と異なり，無視したダイナミクス（たとえば，静止摩擦，動摩擦や振子の振動）に起因した外乱 $d$ を考慮しています．外乱 $d$ を考慮したモデルは外乱オブザーバを構成する際に必要となります．


## <font color="#33CC33">2.3　未知パラメータ</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}
	% =================================================
	&\ddot{\theta}_{1} 
	+ \textcolor{myblue}{{\alpha}_{1}}\dot{\theta}_{1} 
	= \textcolor{myblue}{{\beta}_{1}}v 
	- d 
	\tag{2.6}
	% --------
	\\
	&\textcolor{myred}{{m}_{2}{L}_{1}{l}_{2}}
			\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ \textcolor{myblue}{{J}_{2}}\ddot{\theta}_{2}
	\\
	&\qquad
	= \textcolor{myblue}{{J}_{2}}
			\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ \textcolor{myred}{{m}_{2}g{l}_{2}}\sin{\theta}_{2}
		- \textcolor{myblue}{{c}_{2}}\dot{\theta}_{2}
	\tag{2.2}
\end{align}
```
となります．

　$(2.6),$ $(2.2)$ 式に含まれるパラメータを，値が既知あるいは直接的に測定可能なもの（<font color="#E84746">既知パラメータ</font>）と，そうでないもの（<font color="#0070C0">未知パラメータ</font>）に分類すると，以下のようになります．


|<b><font color="#E84746">既知パラメータ</font></b>|<b><font color="#0070C0">未知パラメータ</font></b>|
|---|---|
|$\bullet$ アーム：<font color="#E84746">$L_1$</font><br>$\bullet$ 振子：<font color="#E84746">$g$, $m_2$, $l_2$</font>|$\bullet$ アーム：<font color="#0070C0">$\alpha_1$, $\beta_1$</font><br>$\bullet$ 振子：<font color="#0070C0">$J_2$, $c_2$</font>|

　ここで，振子の<font color="#E84746">既知パラメータ</font>であるとした <font color="#E84746">$m_2$, $l_2$</font> は，振子を取り外すことで値を測定することができます．本実験装置は LEGO で製作していますので簡単に振子を取り外すことができますが，一般に，振子を取り外したくない場合も多いと思います．そこで，$(2.2)$ 式の両辺を <font color="#E84746">$m_2l_2$</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}
	% =================================================
	&\textcolor{myred}{{L}_{1}}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ \dfrac{\textcolor{myblue}{{J}_{2}}}
				{\textcolor{myred}{{m}_{2}{l}_{2}}}\ddot{\theta}_{2}
	\\
	&\qquad
	= \dfrac{\textcolor{myblue}{{J}_{2}}}
		{\textcolor{myred}{{m}_{2}{l}_{2}}}
				\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ \textcolor{myred}{g}\sin{\theta}_{2}
		- \dfrac{\textcolor{myblue}{{c}_{2}}}
				{\textcolor{myred}{{m}_{2}{l}_{2}}}\dot{\theta}_{2}
	\tag{2.7}
\end{align}
```
のように書き換えます．さらに，
```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}
	% =================================================
	\textcolor{myblue}{{\alpha}_{2}} 
	= \dfrac{\textcolor{myblue}{{J}_{2}}}
		{\textcolor{myred}{{m}_{2}{l}_{2}}},\ 
	\textcolor{myblue}{{\beta}_{2}} 
	= \dfrac{\textcolor{myblue}{{c}_{2}}}
		{\textcolor{myred}{{m}_{2}{l}_{2}}}
\end{align}
```
とおくと，<font color="#0070C0">$\alpha_1$, $\beta_1$</font> を<font color="#0070C0">未知パラメータ</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}
	% =================================================
	&\textcolor{myred}{{L}_{1}}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ \textcolor{myblue}{{\alpha}_{2}}\ddot{\theta}_{2}
	\\
	&\qquad
	= \textcolor{myblue}{{\alpha}_{2}}
\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ \textcolor{myred}{g}\sin{\theta}_{2}
		- \textcolor{myblue}{{\beta}_{2}}\dot{\theta}_{2}
	\tag{2.8}
\end{align}
```
が得られます．$(2.8)$ 式を用いると，振子の質量 $m_2$ や，軸から重心までの長さ $l_2$ を事前に知る必要はありません．


## <font color="#33CC33">2.4　モデルのまとめ</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}
	% =================================================
	&\ddot{\theta}_{1} 
	+ \textcolor{myblue}{{\alpha}_{1}}\dot{\theta}_{1} 
	= \textcolor{myblue}{{\beta}_{1}}v 
	- d 
	\tag{2.6}
	% --------
	\\
	&\textcolor{myred}{{L}_{1}}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ \textcolor{myblue}{{\alpha}_{2}}\ddot{\theta}_{2}
	\\
	&\qquad
	= \textcolor{myblue}{{\alpha}_{2}}
\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ \textcolor{myred}{g}\sin{\theta}_{2}
		- \textcolor{myblue}{{\beta}_{2}}\dot{\theta}_{2}
	\tag{2.8}
\end{align}
```
であり，<font color="#E84746">既知パラメータ</font>と<font color="#0070C0">未知パラメータ</font>はそれぞれ

|<b><font color="#E84746">既知パラメータ</font></b>|<b><font color="#0070C0">未知パラメータ</font></b>|
|---|---|
|$\bullet$ アーム：<font color="#E84746">$L_1$</font><br>$\bullet$ 振子：<font color="#E84746">$g$</font>|$\bullet$ アーム：<font color="#0070C0">$\alpha_1$, $\beta_1$</font><br>$\bullet$ 振子：<font color="#0070C0">$\alpha_2$, $\beta_2$</font>|

となります．







# <font color="#33CC33">3. アームのパラメータ同定</font>

## <font color="#33CC33">3.1　パラメータ同定の手順</font>

　ここでは，アーム角度の P 制御（比例制御）を行ったときの実験データを利用し，<b>2 次遅れ系</b>の特性に基づいて<font color="#0070C0">未知パラメータ</font>を定める（<b>パラメータ同定</b>を行う）方法を説明します．

　$(2.6)$ 式において，<font color="#E32D91">外乱 $d$</font> を無視する (<font color="#E32D91">$d = 0$</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}
	% =================================================
	&\ddot{\theta}_{1} 
	+ \textcolor{myblue}{{\alpha}_{1}}\dot{\theta}_{1} 
	= \textcolor{myblue}{{\beta}_{1}}v 
	- \textcolor{mydarkpink}{d} 
	\tag{2.6}
	\\
	& \quad \Longrightarrow \quad
	\ddot{\theta}_{1} 
	+ \textcolor{myblue}{{\alpha}_{1}}\dot{\theta}_{1} 
	= \textcolor{myblue}{{\beta}_{1}}v 
	\\
	& {\theta}_{1}(s) = P(s)v(s),\ 
	P(s) = \dfrac{\textcolor{myblue}{{\beta}_{1}}}
			{s(s + \textcolor{myblue}{{\alpha}_{1}})}
	\tag{2.9}
\end{align}
```
が得られます．いま，P コントローラ（比例コントローラ）
```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}
	% =================================================
	&u(t) = {k}_{\rm P}e(t),\ e(t) = {\theta}_{1}^{\rm ref}(t) - {\theta}_{1}(t)
	\tag{3.1a}
	\\
	& \quad \Longrightarrow \quad
	u(s) = {k}_{\rm P}e(s),\ e(s) = {\theta}_{1}^{\rm ref}(s) - {\theta}_{1}(s)
	\tag{3.1b}
\end{align}
```
によりアーム角度 $\theta_1$ を定値の目標値
```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}
	% =================================================
	{\theta}_{1}^{\rm ref}(t) = \left\{\begin{array}{ll}
			0 & (t < 0) \\
			{\theta}_{\rm 1c} & (t \ge 0) \\
		\end{array}\right.
\end{align}
```
に制御することを考えます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/79199200-931f-ccc2-6c23-303eab94b667.png)|
|:-:|
|図 $3.1$　アーム角度の P 制御（比例制御）|


ここで，$\theta_1^{\rm ref}$ から $\theta_1$ への伝達関数 $T(s)$ を求めると，
```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}
	% =================================================
	T(s)
	&= \dfrac{P(s){k}_{\rm P}}{1 + P(s){k}_{\rm P}}
	= \dfrac{\dfrac{\textcolor{myblue}{{\beta}_{1}}}
			{s(s + \textcolor{myblue}{{\alpha}_{1}})}{k}_{\rm P}}
			{1 + \dfrac{\textcolor{myblue}{{\beta}_{1}}}
					{s(s + \textcolor{myblue}{{\alpha}_{1}})}{k}_{\rm P}}
	\\
	&= \dfrac{\textcolor{myblue}{\beta_1}\textcolor{mydarkyellow}{{k}_{\rm P}}}
		{s^2 + \textcolor{myblue}{\alpha_1}s
			+ \textcolor{myblue}{\beta_1}\textcolor{mydarkyellow}{{k}_{\rm P}}}
	\tag{3.2}
\end{align}
```
となるので，<b>2 次遅れ要素の標準形</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}
	% =================================================
	T(s)
	&= \dfrac{{\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n1}}^{2}}
		{s^2 
			+ 2\textcolor{mygreen}{{\zeta}_{1}{\omega}_{\rm n1}}s 
			+ {\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n1}}^{2}}
	\quad (\textcolor{mygreen}{{\omega}_{\rm n1}} > 0)
	\tag{3.3}
\end{align}
```
で記述できることがわかります．ここで
* <font color="#0CA274">固有角周波数 ${\omega}_{\rm n1} > 0$</font>：速応性に関するパラメータ
* <font color="#0CA274">減衰係数 ${\zeta}_{1} > 0$</font>：安定度に関するパラメータ

です．$(3.2)$ 式と $(3.3)$ 式の係数を比較すると，
```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}
	% =================================================
	\left\{\begin{array}{rcl}
		2\textcolor{mygreen}{{\zeta}_{1}{\omega}_{\rm n1}}
			\hspace{-6pt}&=&\hspace{-6pt} 
			\textcolor{myblue}{\alpha_1}
		\\
		{\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n1}}^{2} 
			\hspace{-6pt}&=&\hspace{-6pt} 
				\textcolor{myblue}{\beta_1}
				\textcolor{mydarkyellow}{{k}_{\rm P}}
	\end{array}\right.
	\quad \Longrightarrow \quad 
	\left\{\begin{array}{rcl}
		\textcolor{mygreen}{{\omega}_{\rm n1}} 
			\hspace{-6pt}&=&\hspace{-6pt} 
			\sqrt{\textcolor{myblue}{\beta_1}
				\textcolor{mydarkyellow}{{k}_{\rm P}}}
		\\
		\textcolor{mygreen}{{\zeta}_{1}}
			\hspace{-6pt}&=&\hspace{-6pt} 
			\dfrac{\textcolor{myblue}{\alpha_1}}
				{2\textcolor{mygreen}{{\omega}_{\rm n1}}} 
		= \dfrac{\textcolor{myblue}{\alpha_1}}
				{2\sqrt{\textcolor{myblue}{\beta_1}
				\textcolor{mydarkyellow}{{k}_{\rm P}}}} 
	\end{array}\right.
	\tag{3.4}
\end{align}
```
が得られます．

https://youtu.be/-2p5xn7EOEA

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e00ac538-cdcd-857c-7b29-93f6d0786bbf.png)|
|:-:|
|図 $3.2$　アーム角の P 制御（比例制御）系のステップ応答|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/85d54811-bdc1-daa6-90b8-ff768cefe356.png)|
|:-:|
|図 $3.3$　アーム角の P 制御（比例制御）系のステップ応答の偏差 $e(t)$ の大きさ|

　2 次遅れ系のステップ応答が図 $3.2$ のように<font color="#E32D91">オーバーシュート ${A}_{\rm max}$</font> を生じるのは<font color="#0CA274">不足制動 $(0 < {\zeta}_{1} < 1)$</font> のときであり，<font color="#0CA274">減衰係数 ${\zeta}_{1}$ が $0$ に近づく</font>にしたがって，<font color="#E32D91">オーバーシュート ${A}_{\rm max}$ は大きくなる</font>という性質があります．したがって，$(3.4)$ 式より

* <font color="#E48312">「比例ゲイン $k_{\rm P} \rightarrow$ 大」 </font>
　$\Longrightarrow$ <font color="#0CA274">「減衰係数 $\zeta_{1} \rightarrow 0$」 </font>
　$\Longrightarrow$ <font color="#E32D91">「オーバーシュート $A_{\rm max} \rightarrow$ 大」 </font>

となります．ただし，指令電圧の大きさ $|v(t)|$ が上限値 ${v}_{\rm max}$ を超えないように
```math
\begin{align}
	0 < \max|v(t)| \le {v}_{\rm max}
	\tag{3.5}
\end{align}
```
を満足する必要があります．ここで，図 $3.3$ に示すように，偏差 $e(t) = \theta_{\rm 1c} - \theta_{1}(t)$ の大きさの最大値が
```math
\begin{align}
	\max|e(t)| = e(0) 
	= \textcolor{mydarkpink}{\theta_{\rm 1c}}
	\tag{3.6}
\end{align}
```
であることを考慮すると，
```math
\begin{align}
	\max|v(t)| = \textcolor{mydarkyellow}{{k}_{\rm P}}\max|e(t)|
	= \textcolor{mydarkyellow}{{k}_{\rm P}}
		\textcolor{mydarkpink}{\theta_{\rm 1c}}
	\tag{3.7}
\end{align}
```
なので，
```math
\begin{align}
	0 < \textcolor{mydarkyellow}{{k}_{\rm P}}
		\textcolor{mydarkpink}{\theta_{\rm 1c}} \le {v}_{\rm max}
	\tag{3.8}
\end{align}
```
を満足するように<font color="#E32D91">定値の目標値 $\theta_{\rm 1c} > 0$</font> と<font color="#E48312">比例ゲイン ${k}_{\rm P} > 0$</font> が選ばれている必要があります．

　なお，<font color="#0CA274"><b>不足制動</b> $(0 < \zeta_1 < 1)$</font> のとき，図 $3.2$ に示す<font color="#E32D91">行き過ぎ時間 $T_{\rm p}$</font> および<font color="#E32D91">オーバーシュート $A_{\rm max}$</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}
	% =================================================
	\textcolor{mydarkpink}{{T}_{\rm p}}
	&= \dfrac{\pi}{{\omega}_{\rm d1}},\quad
	{\omega}_{\rm d1} = \textcolor{mygreen}{{\omega}_{\rm n1}}
						\sqrt{1 - {\textcolor{mygreen}{\zeta}}_{\textcolor{mygreen}{1}}^2}
	\tag{3.9}
	\\
	\textcolor{mydarkpink}{{A}_{\rm max}}
	&= {\theta}_{\rm 1c}
	{e}^{-\textcolor{mygreen}{{\zeta}_{1}{\hskip0.5pt}{\omega}_{\rm n1}}
			\textcolor{mydarkpink}{{T}_{\rm p}}}
	\tag{3.10}
\end{align}
```
となります．

　アームのパラメータ同定の手順は以下のようになります．

#### アームのパラメータ同定の手順

<b>`ステップ 1`</b>
> $(3.8)$ 式を考慮したうえで，適当な<font color="#E32D91">定値の目標値 $\theta_{\rm 1c} > 0$</font> と<font color="#E48312">比例ゲイン $k_{\rm P} > 0$</font> を与えて実験を行い，図 $3.2$ のようにオーバーシュートを生じさせます．そして，実験データから<font color="#E32D91">行き過ぎ時間 $T_{\rm p}$</font> と<font color="#E32D91">オーバーシュート $A_{\rm max}$</font> を抽出します．

<b>`ステップ 2`</b>
> $(3.9)$, $(3.10)$ 式を逆算し，
> ```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}
> 	% =================================================
> 	\left\{\begin{array}{@{\hskip2pt}r@{\;}c@{\;}l}
> 		{\delta}_{1}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				\dfrac{1}{\textcolor{mydarkpink}{{T}_{\rm p}}}
> 					\log_{e}\dfrac{\textcolor{mydarkpink}{{\theta}_{\rm 1c}}}
> 								{\textcolor{mydarkpink}{{A}_{\rm max}}}
> 		\\
> 		\textcolor{mygreen}{{\omega}_{\rm n1}}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				\sqrt{\Bigl(\dfrac{\pi}
> 							{\textcolor{mydarkpink}{{T}_{\rm p}}}\Bigr)^{\!2} 
> 					+ {\delta}_{1}^{2}},\quad 
> 		\textcolor{mygreen}{{\zeta}_{1} }
> 			= \dfrac{{\delta}_{1}}{\textcolor{mygreen}{{\omega}_{\rm n1}}}
> 	\end{array}\right.
> 	\tag{3.11}
> \end{align}
> ```
> により <font color="#0CA274">減衰係数 $\zeta_1$</font> と<font color="#0CA274">固有角周波数 $\omega_{\rm n1}$</font> の値を定めます．

<b>`ステップ 3`</b>
> $(3.4)$ 式より
> ```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}
> 	% =================================================
> 	\left\{\begin{array}{rcl}
> 		\textcolor{myblue}{\alpha_1}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 			2\textcolor{mygreen}{{\zeta}_{1}{\omega}_{\rm n1}}
> 		\\
> 		 \textcolor{myblue}{\beta_1}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				\dfrac{{\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n1}}^{2}}
> 							{\textcolor{mydarkyellow}{{k}_{\rm P}}}
> 	\end{array}\right.
> 	\tag{3.12}
> \end{align}
> ```
> のように<font color="#0070C0">未知パラメータ $\alpha_1$, $\beta_1$</font> の値を定めます．




## <font color="#33CC33">3.2　同定実験</font>
### <font color="#33CC33">3.2.1　準備（極性の調整）</font>

　同定実験を行う前に，アームの回転方向が正しいかをチェックします．もし，回転方向が正しくない状態でフィードバック制御を行うと，不安定化して暴走してしまうので，必ずチェックする必要があります！

　まず，スイッチサイエンスさんから購入した外部電源

https://www.switch-science.com/catalog/1795/

で電力供給を行うので，マイコンシールド "MinSegShield M2V5" のスイッチを図 $3.4$ のように設定します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/032cd12f-7a1f-84f0-3a94-077602a54ab4.png)|
|:-:|
|図 $3.4$　マイコンシールド "MinSegShield M2V5" のスイッチの設定|

Arduino Mega 2560 互換ボードによっては，

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d1a6efe8-2036-f8ab-a4ed-a9a3c60c83a9.png)

のように，ケーブルを別途，購入する必要がありますので，注意してください．

　USB ケーブルでマイコンボードとパソコンを接続してから，以下の Simulink モデル "check_arm.slx" をエクスターナル実行してください．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/ec14137f-bc5a-e114-9784-9b6c9c180cc1.png)|
|:-:|
|<b>Simulink モデル "check_arm.slx"</b>|


実行後，Simulink ブロック "Manual Switch" をダブルクリックして 0 から 1 に切り替えたとき，図 $3.5$ に示すように，

* <b>モータが時計回りに回転</b>
* <b>Simulink ブロック "Scope" の表示が正方向に増加</b>

するかどうかを確認してください．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/90053c63-5726-bd7e-31e9-56ba33abcfed.png)|
|:-:|
|図 $3.5$　極性の確認|

もし，これとは逆の動きをする場合は，以下のように設定してください．

* <b>モータが反時計回りに回転</b>した場合は，<b>Simulink ブロック "Gain (motor polarity)" の値を 1 から -1 に変更</b>してください．
* <b>Simulink ブロック “Scope” の表示が負方向に増加</b>した場合は，<b>Simulink ブロック "Gain (arm encoder polarity)" の値を 1 から -1 に変更</b>してください．



### <font color="#33CC33">3.2.2　アーム角の P 制御とパラメータ同定</font>

　アーム角の P 制御を行うための Simulink モデルを以下に示します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/c3de7943-21cd-2a53-f423-130b11a58a60.png)|
|:-:|
|<b>Simulink モデル "ex_p_cont.slx"</b>|

ここで，サブシステム "Arm (Vsupply = 9 [V])" のなかの Simulink ブロック "Gain (motor polarity)" や "Gain (arm encoder polarity)" は，極性を調整した <b>Simulink モデル "check_arm.slx" と同じ値</b>に設定しておいてください．

　つぎに，<font color="#E32D91">定値の目標値 $\theta_{\rm 1c} > 0$</font> と<font color="#E48312">比例ゲイン $k_{\rm P} > 0$</font> の値を設定します．

　$v_{\rm max} = 9\ {\rm [V]}$ なので，<font color="#E32D91">$\theta_{\rm 1c} = 90\ {\rm [deg]} = \pi/2\ {\rm [rad]}$</font> としたとき，$(3.8)$ 式より <font color="#E48312">比例ゲイン $k_{\rm P} > 0$</font> は
```math
\begin{align}
	0 < \textcolor{mydarkyellow}{{k}_{\rm P}}
	\le \dfrac{{v}_{\rm max}}{\textcolor{mydarkpink}{\theta_{\rm 1c}}}
	= \dfrac{9}{\textcolor{mydarkpink}{\pi/2}}
	\simeq 5.7296
	\tag{3.13}
\end{align}
```
を満足する必要があります．そこで，<font color="#E48312">$k_{\rm P} = 4$</font> と選びました．また，サンプリング周期は $h = 0.01\ {\rm [s]}$ ですので，これらの値を MATLAB のコマンドウィンドウで

```matlab:
>> ref = 90; h = 0.01; kP = 4;
```

のように設定します．そして，エクスターナル実行を行うと，

https://youtu.be/-2p5xn7EOEA

のように動作します．Simulink ブロック "Scope" での観測結果を図 $3.6$ に示します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/29e628da-ae8c-b15e-4197-348323958e4f.png)|
|:-:|
|図 $3.6$　Simulink ブロック "Scope" による観測結果|

振子の振動がアームにまったく影響を与えないわけではなく，若干，アームが振動しています．実験データを "ident_arm_data.mat" という名前の mat ファイルに保存するため，コマンドウィンドウで

```matlab:
>> save ident_arm_data t v theta1 ref h kP 
```

もしくは

```matlab:
>> save('ident_arm_data','t','v','theta1','ref','h','kP')
```

のように入力します．そして，以下の M ファイルを実行してください．

```matlab:M ファイル "ident_arm_2nd.m "
clear
format compact
% -----------------------
myred   = [232   71   70]/255;
mygreen = [ 12  162  116]/255;
myblue  = [  0  112  192]/255;
% -----------------------
load ident_arm_data
% -----------------------
theta1c = ref*pi/180;       % アーム角度の目標値
% -----------------------
t_data      = t;            % いったん，データを移動
theta1_data = theta1;
v_data      = v;
clear t theta1 v
% -----------------------
k = 0;
for i = 1:length(t_data)    % 2 秒までのデータを捨てる
   if t_data(i) >= 1;
       k = k + 1;
       t(k) = t_data(i) - 1;
       theta1(k) = theta1_data(i);
       v(k) = v_data(i);
   end
end

% ++++++++++ Step 1 ++++++++++
[theta1_max imax] = max(theta1);        % ステップ応答の最大値の抽出

imax_last = imax;
for i = imax:length(t)
    if theta1(i) == theta1_max
        imax_last = i;
    end
    if theta1(i) < theta1_max
        break
    end
end

Tp    = (t(imax) + t(imax_last))/2;     % 行き過ぎ時間
Amax  = theta1_max - theta1c;           % オーバーシュート

fprintf('Tp     = %3.2e;\n',Tp)
fprintf('Amax   = %3.2e;\n',Amax)

% ++++++++++ Step 2 ++++++++++
delta1 = - (1/Tp)*log(Amax/theta1c);
wn1    = sqrt((pi/Tp)^2 + delta1^2);    % 固有角周波数
zeta1  = delta1/wn1;                    % 減衰係数

fprintf('wn1    = %3.2e;\n',wn1)
fprintf('zeta1  = %3.2e;\n',zeta1)

% ++++++++++ Step 3 ++++++++++
alpha1 = 2*zeta1*wn1;                   % alpha1 の同定
beta1  = wn1^2/kP;                      % beta1 の同定

fprintf('alpha1 = %3.2e;\n',alpha1)
fprintf('beta1  = %3.2e;\n',beta1)

% -----------------------
t_sim = 0:0.001:1;                      % 同定されたパラメータを用いてシミュレーション
theta1_sim = theta1c*step(wn1^2,[1 2*zeta1*wn1 wn1^2],t_sim);

figure(1)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('north')

plot([Tp Tp],[0 theta1_max]*180/pi,'Color',myblue)
hold on
plot([ 0 Tp],[theta1_max theta1_max]*180/pi,'Color',myblue)
plot([ 0 1 ],[theta1c theta1c]*180/pi,'Color',myblue)

p1 = plot(t_sim,theta1_sim*180/pi,'Color',mygreen,'LineWidth',1.5);
p2 = stairs(t,theta1*180/pi,'Color',myred,'LineWidth',1.5);

plot(Tp,theta1_max*180/pi,'o','LineWidth',1.5,'MarkerSize',8,'Color',myblue)
hold off
xlim([0 0.8]);
ylim([0 120])

set(gca,'FontName','Arial','FontSize',16)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',18)
ylabel('$${\theta}_{1}(t)$$ [deg]', 'interpreter', 'latex','FontSize',18)

set(gca,'XTick',0:0.1:0.8)
set(gca,'YTick',0:30:120)

legend([p2 p1],{'Experiment','Simulation'},'Location','southeast')

grid on
```

その結果，

```matlab:M ファイル "ident_arm_2nd.m " の実行結果
>> ident_arm_2nd
Tp     = 2.10e-01;
Amax   = 3.14e-01;
wn1    = 1.68e+01;
zeta1  = 4.56e-01;
alpha1 = 1.53e+01;
beta1  = 7.06e+01;
```

というパラメータ同定の結果が得られます．

|パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
|---|---|---|
|<font color="#E32D91">行き過ぎ時間 $T_{\rm p}$</font>|<font color="#E32D91">$2.10 \times 10^{-1}\ {\rm [s]}$</font>|<font color="#E32D91">Tp</font>|
|<font color="#E32D91">オーバーシュート $A_{\rm max}$</font>|<font color="#E32D91">$3.14 \times 10^{-1}\ {\rm [rad]}\ (= 18\ {\rm [deg]})$</font>|<font color="#E32D91">Amax</font>|
|<font color="#0CA274">固有角周波数 $\omega_{\rm n1}$</font>|<font color="#0CA274">$1.68 \times 10^{1}$</font>|<font color="#0CA274">wn1</font>|
|<font color="#0CA274">減衰係数 $\zeta_1$</font>|<font color="#0CA274">$4.56 \times 10^{-1}$</font>|<font color="#0CA274">zeta1</font>|
|<font color="#0070C0">$\alpha_1$</font>|<font color="#0070C0">$1.53 \times 10^{1}$</font>|<font color="#0070C0">alpha1</font>|
|<font color="#0070C0">$\beta_1$</font>|<font color="#0070C0">$7.06 \times 10^{1}$</font>|<font color="#0070C0">beta1</font>|

また，同定されたパラメータを用いたシミュレーション結果と実験結果を比較した図 $3.7$ のグラフが描画されます．これよりピーク値が一致していることはもちろんですが，定常状態で振子の振動の影響を受けていることを除けば，おおよそ一致していることが確認できます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/ae8ce68d-b5e5-d162-fc65-2c26970cbed8.png)|
|:-:|
|図 $3.7$　実験結果とシミュレーション結果の比較|

なお，振子を取り外して実験を行うと，図 $3.8$ のようになり，振子の振動による影響がないことがわかります．一方で，動摩擦や静止摩擦に起因した定常偏差が生じていることが確認できます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/8e676688-31f0-ccd0-c9ad-9bc3c74d1fb2.png)|
|:-:|
|図 $3.8$　実験結果とシミュレーション結果の比較<br>（振子を取り外して実験を行った場合）|

　振子の振動による反動トルクや動摩擦，静止摩擦は外乱 $d$ の一部であると考えることができますので，外乱オブザーバによりその推定値 $\hat{d}$ を計算し，フィードバックで補償することも考えられます．




# <font color="#33CC33">4. 振子のパラメータ同定</font>
## <font color="#33CC33">4.1　振子の自由振動</font>

　振子については，アームを固定したときの<b>振子の自由振動</b>の実験データを利用して，二種類の方法

* <b>線形 1 自由度振動系の特性に基づく方法</b>
* <b>最小 2 乗法に基づく方法</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}
	% =================================================
	&\textcolor{myred}{{L}_{1}}\cos{\theta}_{2}\cdot\ddot{\theta}_{1}
		+ \textcolor{myblue}{{\alpha}_{2}}\ddot{\theta}_{2}
	\\
	&\qquad
	= \textcolor{myblue}{{\alpha}_{2}}
\dot{\theta}{}_{1}^{2}\sin{\theta}_{2}\cos{\theta}_{2}
		+ \textcolor{myred}{g}\sin{\theta}_{2}
		- \textcolor{myblue}{{\beta}_{2}}\dot{\theta}_{2}
	\tag{2.8}
\end{align}
```
に含まれる<font color="#0070C0">未知パラメータ $\alpha_2$, $\beta_2$</font> の同定を行います．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/6cd6e1b2-9f44-7425-468a-4dd5f725280d.png)|
|:-:|
|図 $4.1$　振子角の基準|


　図 $4.1$ に示すように，振子角の基準を真上から真下に変更すると，
```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}
	% =================================================
	{\theta}_{2}(t) = {\phi}_{2}(t) - \pi
\end{align}
```
なので，$(2.8)$ 式は
```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}
	% =================================================
	&{}-\textcolor{myred}{{L}_{1}}\cos{\phi}_{2}\cdot\ddot{\theta}_{1}
		+ \textcolor{myblue}{{\alpha}_{2}}\ddot{\phi}_{2}
	\\
	&\qquad
	= \textcolor{myblue}{{\alpha}_{2}}
\dot{\theta}{}_{1}^{2}\sin{\phi}_{2}\cos{\phi}_{2}
		- \textcolor{myred}{g}\sin{\phi}_{2}
		- \textcolor{myblue}{{\beta}_{2}}\dot{\phi}_{2}
	\tag{4.1}
\end{align}
```
のように書き換えることができます．また，自由振動の実験を行う際に，アームが回転しないように固定したときを考えると，
```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}
	% =================================================
	\dot{\theta}_{1} = 0,\ 
	\ddot{\theta}_{1} = 0
\end{align}
```
なので，$(4.1)$ 式は
```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}
	% =================================================
	\textcolor{myblue}{{\alpha}_{2}}\ddot{\phi}_{2}
	+ \textcolor{myblue}{{\beta}_{2}}\dot{\phi}_{2}
	+ \textcolor{myred}{g}\sin{\phi}_{2}
	= 0
	\tag{4.2}
\end{align}
```
のように簡略化できます．$(4.2)$ 式がパラメータ同定を行う際に利用される微分方程式です．




## <font color="#33CC33">4.2　線形 1 自由度振動系の特性に基づく方法</font>
### <font color="#33CC33">4.2.1　パラメータ同定の手順</font>

　$(4.2)$ 式は非線形項を含みますので，このままでは解析が困難です．そこで，振子が真下近傍（$\phi_2 = 0$）で動作するとして，非線形項を
```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}
	% =================================================
	\sin{\phi}_{2} \simeq {\phi}_{2}
\end{align}
```
のように近似的に線形化します．その結果，$(4.2)$ 式は <b>2 階の線形微分方程式</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}
	% =================================================
	\textcolor{myblue}{{\alpha}_{2}}\ddot{\phi}_{2}
	+ \textcolor{myblue}{{\beta}_{2}}\dot{\phi}_{2}
	+ \textcolor{myred}{g}{\phi}_{2}
	= 0
	\tag{4.3}
\end{align}
```
で近似できることがわかります．$(4.3)$ 式は「振動工学」の分野でいう「<b>線形 1 自由度振動系</b>」のモデルに相当します．

　つぎに，$(4.3)$ 式を解析してみましょう．$(4.3)$ 式は<font color="#0CA274">減衰係数 $\zeta_2$</font> と<font color="#0CA274">固有角周波数 $\omega_{\rm n2}$</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}
	% =================================================
	\ddot{\phi}_{2}
	+ 2\textcolor{mygreen}{{\zeta}_{2}{\omega}_{\rm n2}}\dot{\phi}_{2}
	+ {\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n2}}^{2}{\phi}_{2}
	= 0
	\tag{4.4}
\end{align}
```
という形式に書き換えることができます．ただし，
```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}
	% =================================================
	\left\{\begin{array}{rcl}
		2\textcolor{mygreen}{{\zeta}_{2}{\omega}_{\rm n2}}
			\hspace{-6pt}&=&\hspace{-6pt} 
			\dfrac{\textcolor{myblue}{\beta_2}}
				{\textcolor{myblue}{\alpha_2}}
		\\
		{\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n2}}^{2} 
			\hspace{-6pt}&=&\hspace{-6pt} 
			\dfrac{\textcolor{myred}{g}}
				{\textcolor{myblue}{\alpha_2}}
	\end{array}\right.
	\tag{4.5}
\end{align}
```
という関係があります．振子の初期角度を $\phi_2(0) = \phi_{20}$，初期角速度を $\dot{\phi}_2(0) = 0$ としたとき，振子の自由振動は図 $4.2$ のように<b>「周期振動をする」</b>ので，<font color="#0CA274"><b>不足制動</b> $(0 < \zeta_2 < 1)$</font> となります．したがって，$(4.4)$ 式の解軌道は
```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}
	% =================================================
	&
	\phi_2(t) = {e}^{-\textcolor{mygreen}{{\zeta}_{2}
				{\hskip0.5pt}{\omega}_{\rm n2}}{\hskip0.5pt}t}\biggl(
				\cos{\omega}_{\rm d2}t 
			+ \dfrac{\textcolor{mygreen}{{\zeta}_{2}{\omega}_{\rm n2}}}{{\omega}_{\rm d2}}
			+ \sin{\omega}_{\rm d2}t
			\biggr)\phi_{20}
	\tag{4.6}\\
	&\qquad
	{\omega}_{\rm d2} = \textcolor{mygreen}{{\omega}_{\rm n2}}
\sqrt{1 - {\textcolor{mygreen}{\zeta}}_{\textcolor{mygreen}{2}}^{2}}
\end{align}
```
となり，<font color="#E32D91">振動周期 $T$</font> および<font color="#E32D91">減衰率 $\lambda$</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}
	% =================================================
	\textcolor{mydarkpink}{T}
	&= {T}_{i} 
	= \dfrac{2\pi}{{\omega}_{\rm d2}}
	\tag{4.7}
	\\
	\textcolor{mydarkpink}{\lambda}
	&= \dfrac{{A}_{i+1}}{{A}_{i}} 
	= {e}^{-\textcolor{mygreen}{{\zeta}_{2}{\hskip0.5pt}{\omega}_{\rm n2}}
			\textcolor{mydarkpink}{T}}
	\tag{4.8}
\end{align}
```
となり，それぞれが<b>一定値</b>であることがわかります．この性質を利用して振子のパラメータを同定します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e2ad1b6b-cc9a-514e-fd58-e5af0c20c04f.png)|
|:-:|
|図 $4.2$　振子の自由振動（$\phi_2 = 0$ の近傍）|

https://youtu.be/FKdadlxkLag


　振子のパラメータ同定の手順は以下のようになります．

#### 振子のパラメータ同定の手順

<b>`ステップ 1`</b>
> アームを固定したときの振子の自由振動の実験データから，図 $4.2$ に示す $n$ 個の振動の頂点 $(\bar{t}_i,\ A_i)$ $(i = 1,\ 2,\ \ldots,\ n)$ を抽出します．そして，
> ```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}
> 	% =================================================
> 	\textcolor{mydarkpink}{{T}_{i}} 
> 	&\textcolor{mydarkpink}{{}= \bar{t}_{i+1} - \bar{t}_i}
> 	\tag{4.9}
> 	\\
> 	\textcolor{mydarkpink}{\lambda} 
> 	&\textcolor{mydarkpink}{{}= \dfrac{{A}_{i+1}}{{A}_{i}}}
> 	\tag{4.10}
> \end{align}
> ```
> を求めます．そして，$(4.9)$ 式および $(4.10)$ 式の平均値
> ```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}
> 	% =================================================
> 	\textcolor{mydarkpink}{T} 
> 	&\textcolor{mydarkpink}{{}= \dfrac{1}{n-1}\sum_{i=1}^{n}{T}_{i}}
> 	\tag{4.11}
> 	\\
> 	\textcolor{mydarkpink}{\lambda} 
> 	&\textcolor{mydarkpink}{{}= \dfrac{1}{n-1}\sum_{i=1}^{n}{\lambda}_{i}}
> 	\tag{4.12}
> \end{align}
> ```
> を計算し，これらを自由振動の<font color="#E32D91">振動周期 $T$</font> および<font color="#E32D91">減衰率 $\lambda$</font> とします．　

<b>`ステップ 2`</b>
> $(4.7)$, $(4.8)$ 式を逆算し，
> ```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}
> 	% =================================================
> 	\left\{\begin{array}{@{\hskip2pt}r@{\;}c@{\;}l}
> 		{\delta}_{2}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				\dfrac{1}{\textcolor{mydarkpink}{T}}
> 					\log_{e}\dfrac{1}
> 								{\textcolor{mydarkpink}{\lambda}}
> 		\\
> 		\textcolor{mygreen}{{\omega}_{\rm n2}}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				\sqrt{\Bigl(\dfrac{2\pi}
> 							{\textcolor{mydarkpink}{T}}\Bigr)^{\!2} 
> 					+ {\delta}_{2}^{2}},\quad 
> 		\textcolor{mygreen}{{\zeta}_{2} }
> 			= \dfrac{{\delta}_{2}}{\textcolor{mygreen}{{\omega}_{\rm n2}}}
> 	\end{array}\right.
> 	\tag{4.13}
> \end{align}
> ```
> により <font color="#0CA274">減衰係数 $\zeta_2$</font> と<font color="#0CA274">固有角周波数 $\omega_{\rm n2}$</font> の値を定めます．

<b>`ステップ 3`</b>
> $(4.5)$ 式より
> ```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}
> 	% =================================================
> 	\left\{\begin{array}{rcl}
> 		\textcolor{myblue}{\alpha_2}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 			\dfrac{\textcolor{myred}{g}}
> 			{{\textcolor{mygreen}{\omega}}_{\textcolor{mygreen}{\rm n2}}^{2}}
> 		\\
> 		 \textcolor{myblue}{\beta_2}
> 			\hspace{-6pt}&=&\hspace{-6pt} 
> 				2\textcolor{mygreen}{{\zeta}_{2}{\omega}_{\rm n2}}
> 				\textcolor{myblue}{\alpha_2}
> 	\end{array}\right.
> 	\tag{4.14}
> \end{align}
> ```
> のように<font color="#0070C0">未知パラメータ $\alpha_2$, $\beta_2$</font> の値を定めます．







### <font color="#33CC33">4.2.2　同定実験</font>
#### <font color="#33CC33">(a)　準備（極性の調整）</font>

　同定実験を行う前に，振子の回転方向が正しいかをチェックします．USB ケーブルでマイコンボードとパソコンを接続してから，以下の Simulink モデル "check_pend.slx" をエクスターナル実行してください．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/a283ad85-c29f-f4b8-ad2b-f946903ecd2a.png)|
|:-:|
|<b>Simulink モデル "check_pend.slx"</b>|


実行後，手動で内側から見て時計回りに振子を回転させたとき，図 $4.3$ に示すように，　

* <b>Simulink ブロック "Scope" の表示が正方向に増加</b>

するかどうかを確認してください．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/9cbcef08-0d5a-e6a1-c77f-02d52d885712.png)|
|:-:|
|図 $4.3$　極性の確認|

もし，これとは逆の動きをする場合は，以下のように設定してください．

* <b>Simulink ブロック “Scope” の表示が負方向に増加</b>した場合は，<b>Simulink ブロック "Gain (pendulum encoder polarity)" の値を 1 から -1 に変更</b>してください．


#### <font color="#33CC33">(b)　自由振動の実験</font>

　振子の自由振動を行うための Simulink モデルを以下に示します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/df3f0483-736d-252a-083a-0177c2357fba.png)|
|:-:|
|<b>Simulink モデル "ex_ident_pend.slx"</b>|

ここで，サブシステム "Pendulum" のなかの Simulink ブロック "Gain (pendulum encoder polarity)" は，極性を調整した Simulink モデル "check_pend.slx" と同じ値に設定しておいてください．

　自由振動の実験を行う前に，アームが回転しないように，LEGO 部品を利用するなどして，<b>アームを台座に固定</b>しておいてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/0a86f2d9-cdf1-8fbe-5316-a85922b01441.png)


　つぎに，MATLAB のコマンドウィンドウで

```matlab:
>> h = 0.01;
```

のようにサンプリング周期 $h = 0.01$ $\rm{[s]}$ を設定し，エクスターナル実行を行います．実行開始後，速やかに振子を手で水平付近まで持ち上げて，<font color="#E84746"><b>5 秒以内</b></font>に静かに振子を離します．実験の様子は以下の動画を参照してください．

https://youtu.be/FKdadlxkLag

実験が終了した後，実験データを "ident_pend_data.mat" という名前の mat ファイルに保存するため，コマンドウィンドウで

```matlab:
>> save ident_pend_data t phi2 h
```

もしくは

```matlab:
>> save('ident_pend_data','t','phi2','h')
```

のように入力します．



#### <font color="#33CC33">(c)　パラメータ同定</font>
　以下の M ファイルを実行してください．

```matlab:M ファイル "ident_pend_2nd.m"
clear
format compact
% -----------------------
myred   = [232   71   70]/255;
mygreen = [ 12  162  116]/255;
myblue  = [  0  112  192]/255;
% -----------------------
g = 9.81e+00;
% -----------------------
load ident_pend_data     % おもりなし
% load ident_pend_data2    % おもり 1 個
% load ident_pend_data3    % おもり 2 個
% -----------------------
tmin = 10;
tmax = 22;
% -----------------------
t_data    = t;
phi2_data = phi2;

% ===========================================================
figure(1)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('northwest')

area([0 tmin],[ 110  110],'FaceColor',myred,'FaceAlpha',0.2,'EdgeAlpha',0)
hold on
area([0 tmin],[-110 -110],'FaceColor',myred,'FaceAlpha',0.2,'EdgeAlpha',0)

area([tmax t(end)],[ 110  110],'FaceColor',myred,'FaceAlpha',0.2,'EdgeAlpha',0)
area([tmax t(end)],[-110 -110],'FaceColor',myred,'FaceAlpha',0.2,'EdgeAlpha',0)

hold on
plot(t,phi2*180/pi,'Color',myred,'LineWidth',1.5)
hold off

xlim([0 t(end)])
ylim([-110 110])

set(gca,'FontName','Arial','FontSize',16)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',18)
ylabel('$${\phi}_{2}(t)$$ [deg]', 'interpreter', 'latex','FontSize',18)

set(gca,'XTick',0:2:t(end))
set(gca,'YTick',-90:30:90)

grid on
% ===========================================================

clear t phi2
% -----------------------
k = 0;
for i = 1:length(t_data)
    if t_data(i) >= tmin & t_data(i) <= tmax
        k = k + 1;
        t(k)    = t_data(i) - tmin;
        phi2(k) = phi2_data(i);
    end
end

% ++++++++++ Step 1 ++++++++++
k = 0;
flag = 0;

for i = 1:length(t)-1
    if phi2(i) > 0 
        flag = 1;
    end
    if flag == 1 & phi2(i) < 0
        k = k + 1;
        num(k) = i;
        t0(k)  = t(i);
        A0(k)  = phi2(i);
        flag   = 0;
    end
end
% -----------------------
if phi2(1) < 0
    num = [1 num];
end
% -----------------------
for j = 1:length(num)-1   
    [A(j) imax(j)] = max(phi2(num(j):num(j+1)));
    imax(j) = imax(j) + num(j);

    imax_last(j) = imax(j);
    for i = imax(j):num(j+1)
        if phi2(i) == A(j)
            imax_last(j) = i;
        end
        if phi2(i) < A(j)
            break
        end
    end

    tb(j) = (t(imax(j)) + t(imax_last(j)))/2;
end

% ++++++++++ Step 2 ++++++++++
for i = 1:length(tb)-1
    T(i)      = tb(i+1) - tb(i);
    lambda(i) = A(i+1)/A(i);
end

T_average      = mean(T);
lambda_average = mean(lambda);

fprintf('T      = %3.2e\n',T_average)
fprintf('lambda = %3.2e\n',lambda_average)

% ++++++++++ Step 3 ++++++++++
xi2   = (1/T_average)*log(1/lambda_average);
wn2   = sqrt((2*pi/T_average)^2 + xi2^2);
zeta2 = xi2/wn2;

fprintf('wn2    = %3.2e\n',wn2)
fprintf('zeta2  = %3.2e\n',zeta2)

alpha2 = g/wn2^2;
beta2  = 2*zeta2*wn2*alpha2;

fprintf('alpha2 = %3.2e\n',alpha2)
fprintf('beta2  = %3.2e\n',beta2)

% -----------------------
figure(2)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('northeast')

plot(1:length(T),T,'o','Color',myblue,'LineWidth',1.5,'MarkerSize',8)
hold on
plot([1 length(T)],T_average*[1 1],'Color',myblue)
hold off
xlim([1 length(lambda)]);
ylim([0.6 0.85])
set(gca,'FontName','Arial','FontSize',16)
xlabel('$$i$$', 'interpreter', 'latex','FontSize',18)
ylabel('$${T}_{i}$$ and $$T$$ [s]', 'interpreter', 'latex','FontSize',18)

set(gca,'XTick',1:1:length(lambda))
set(gca,'YTick',0.6:0.05:0.85)

grid on

% -----------------------
figure(3)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('southeast')

plot(1:length(lambda),lambda,'o','Color',myblue,'LineWidth',1.5,'MarkerSize',8)
hold on
plot([1 length(lambda)],lambda_average*[1 1],'Color',myblue)
hold off
xlim([1 length(lambda)]);
ylim([0.75 1])
set(gca,'FontName','Arial','FontSize',16)
xlabel('$$i$$', 'interpreter', 'latex','FontSize',18)
ylabel('$${\lambda}_{i}$$ and $$\lambda$$', 'interpreter', 'latex','FontSize',18)

set(gca,'XTick',1:1:length(lambda))
set(gca,'YTick',0.75:0.05:1)

grid on

% -----------------------
wd2 = wn2*sqrt(1 - zeta2^2);
phi20 = A(1);

tsim = 0:0.001:(tmax-tmin)-tb(1);
phi2sim = exp(-zeta2*wn2*tsim).*(cos(wd2*tsim) + zeta2/sqrt(1 - zeta2^2)*sin(wd2*tsim))*phi20;

figure(4)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('southwest')

p1 = plot(tsim+tb(1),phi2sim*180/pi,'Color',mygreen,'LineWidth',1.5);
hold on
plot(tb,A*180/pi,'o','Color',myblue,'LineWidth',1.5,'MarkerSize',8);
p2 = stairs(t,phi2*180/pi,'Color',myred,'LineWidth',1.5);

hold off

xlim([0 tmax-tmin])
ylim([-90 90])

set(gca,'FontName','Arial','FontSize',16)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',18)
ylabel('$${\phi}_{2}(t)$$ [deg]', 'interpreter', 'latex','FontSize',18)

legend([p2 p1],{'Experiment','Linear Simulation'},'Location','southeast')

set(gca,'XTick',0:1:tmax-tmin)
set(gca,'YTick',-90:30:90)

grid on
```

その結果，

```matlab:M ファイル "ident_pend_2nd.m" の実行結果
>> ident_pend_2nd
T      = 6.87e-01
lambda = 8.78e-01
wn2    = 9.15e+00
zeta2  = 2.08e-02
alpha2 = 1.17e-01
beta2  = 4.46e-02
```

というパラメータ同定の結果が得られます．

|パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
|---|---|---|
|<font color="#E32D91">振動周期 $T$</font>|<font color="#E32D91">$6.87 \times 10^{-1}\ {\rm [s]}$</font>|<font color="#E32D91">T</font>|
|<font color="#E32D91">減衰率 $\lambda$</font>|<font color="#E32D91">$8.78 \times 10^{-1}$</font>|<font color="#E32D91">lambda</font>|
|<font color="#0CA274">固有角周波数 $\omega_{\rm n2}$</font>|<font color="#0CA274">$9.15 \times 10^{0}$</font>|<font color="#0CA274">wn2</font>|
|<font color="#0CA274">減衰係数 $\zeta_2$</font>|<font color="#0CA274">$2.08 \times 10^{-2}$</font>|<font color="#0CA274">zeta2</font>|
|<font color="#0070C0">$\alpha_2$</font>|<font color="#0070C0">$1.17 \times 10^{-1}$</font>|<font color="#0070C0">alpha2</font>|
|<font color="#0070C0">$\beta_2$</font>|<font color="#0070C0">$4.46 \times 10^{-2}$</font>|<font color="#0070C0">beta2</font>|

M ファイルでは，まず，図 $4.4$ の生データから実験開始後の<font color="#E84746"><b>10 秒間</b></font>と最後の 8 秒間のデータを削除しています ($t_{\rm min} = 10$, $t_{\rm max} = 22$)．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/e2ad2d63-cda1-897e-eb94-9e5ea8f2c509.png)|
|:-:|
|図 $4.4$　自由振動の実験結果 (生データ)|

そして，10 秒が 0 秒になるようにデータを変換した後，図 $4.5$ のように 12 秒間の各振動のピークとなる点を抽出します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/bf867c34-4c10-cd27-b6f9-98715b8edc6f.png)|
|:-:|
|図 $4.5$　自由振動における各振動のピークとなる点|

これらの点から各振動の振動周期 $T_i$ と減衰率 $\lambda_i$ を算出して，それらの平均も合わせて描画した結果が図 $4.6$, $4.7$ です．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/a2f2d613-1f0c-b135-a536-af143e69bea6.png)|
|:-:|
|図 $4.6$　各振動の振動周期 $T_i$ (○ 印) とその平均 $T$ (実線)|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/1d4dcb22-bf0f-18ef-65b1-361ac7492d95.png)|
|:-:|
|図 $4.7$　各振動の減衰率 $\lambda_i$ (○ 印) とその平均 $\lambda$ (実線)|

図 $4.6$ からわかるように，振動周期は振動が減衰していくにしたがって徐々に短くなり，一定値に漸近していることがわかります．一方，図 $4.7$ からわかるように，減衰率は振動が減衰していくにしたがって徐々に大きくなっていき，一定値に漸近していきますが，その後，動摩擦や静止摩擦の影響で減衰率が急に減少します．

　この同定の方法では，振子が真下近傍（$\phi_2 = 0$）で動作するとして，非線形項を
```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}
	% =================================================
	\sin{\phi}_{2} \simeq {\phi}_{2}
\end{align}
```
のように近似的に線形化した <b>2 階の線形微分方程式</b> $(4.3)$ 式に基づいています．したがって，振子角 $\phi_2$ が大きいときは振動周期や減衰率は一定ではなく，振子角 $\phi_2$ が小さくなると一定値に近づくということになります．つまり，<b>この方法を利用して同定するのであれば，初期角度 $\phi_2(0)$ が 30 [deg] 程度であるような実験データを利用した方が良いでしょう．ただし，振子角 $\phi_2$ が小さすぎると，無視した動摩擦や静止摩擦の影響を受ける可能性が高くなるので，注意が必要です．</b>

　最後に，同定されたパラメータの値を利用した線形シミュレーションの結果 ($(4.3)$ 式の解軌道である $(4.6)$ 式を描画) と実験結果を比較した結果を示します．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/6458a268-1aeb-3759-9a4e-de9015e39352.png)|
|:-:|
|図 $4.8$　実験結果と線形シミュレーション結果の比較|

両者を比べると，振動周期や減衰率が平均的に一致していることがわかります．



## <font color="#33CC33">4.3　最小 2 乗法に基づく方法</font>
### <font color="#33CC33">4.3.1　パラメータ同定の手順</font>
#### <font color="#33CC33">(a)　基本的な考え方</font>

　非線形モデル $(4.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}
	% =================================================
	\underbrace{\bigl[\begin{array}{cc}
		\ddot{\phi}_{2}(t) & \dot{\phi}_{2}(t)
	\end{array}\bigr]}_{\mbox{$\pmb{M}(t)$}}
	\underbrace{\left[\begin{array}{c}
		\textcolor{myblue}{{\alpha}_{2}} \\
		\textcolor{myblue}{{\beta}_{2}}
	\end{array}\right]}_{\mbox{$\textcolor{myblue}{\pmb{p}}$}}
	= \underbrace{{}- g\sin{\phi}_{2}(t)}_{\mbox{$N(t)$}}
	\tag{4.15}
\end{align}
```
が得られます．サンプリング周期を $h$ $\rm [s]$ としたとき，理想的には各サンプル時刻 $t = ih$ $(i = 0,\ 1,\ \ldots,\ k)$ で $(4.15)$ 式が成立します．したがって，$f[i] := f(ih)$ と記述すると，理想的には
```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}
	% =================================================
	\underbrace{\left[\begin{array}{cc}
		\ddot{\phi}_{2}[0] & \dot{\phi}_{2}[0] \\
		\ddot{\phi}_{2}[1] & \dot{\phi}_{2}[1] \\
		\vdots & \vdots \\
		\ddot{\phi}_{2}[k] & \dot{\phi}_{2}[k]
	\end{array}\right]}_{\mbox{$\pmb{M}_{\rm s}$}}
	\underbrace{\left[\begin{array}{c}
		\textcolor{myblue}{{\alpha}_{2}} \\
		\textcolor{myblue}{{\beta}_{2}}
	\end{array}\right]}_{\mbox{$\textcolor{myblue}{\pmb{p}}$}}
	= \underbrace{\left[\begin{array}{c}
		{}- g\sin{\phi}_{2}[0] \\
		{}- g\sin{\phi}_{2}[1] \\
		\vdots \\
		{}- g\sin{\phi}_{2}[k]
	\end{array}\right]}_{\mbox{$\pmb{N}_{\rm s}$}}
	\tag{4.16}
\end{align}
```
が成り立ちます．しかし，様々な要因で信号には不確かさが含まれるので，$(4.16)$ 式の $k + 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}
	% =================================================
	S &= \sum_{i=0}^{k}e[i]^{2},\ 
	e[i] = \pmb{M}[i]\textcolor{myblue}{\pmb{p}} - N[i]
	\\
	&= \|\pmb{M}_{\rm s}\textcolor{myblue}{\pmb{p}} - \pmb{N}_{\rm s}\|
	\\
	&= (\pmb{M}_{\rm s}\textcolor{myblue}{\pmb{p}} - \pmb{N}_{\rm s})^{\top}
		(\pmb{M}_{\rm s}\textcolor{myblue}{\pmb{p}} - \pmb{N}_{\rm s})
	\tag{4.17}
\end{align}
```
が最小となるように<font color="#0070C0">未知パラメータ $\pmb{p}$</font> の値を定めます．そのためには，$(4.16)$ 式を
```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}
	% =================================================
	\pmb{M}_{\rm s}\textcolor{myblue}{\pmb{p}} = \pmb{N}_{\rm s}
	\quad \Longrightarrow \quad
	\underbrace{\pmb{M}_{\rm s}^{\top}\pmb{M}_{\rm s}}_{2 \times 2}
	\overbrace{\textcolor{myblue}{\pmb{p}}}^{2 \times 1}
	= \underbrace{\pmb{M}_{\rm s}^{\top}\pmb{N}_{\rm s}}_{2 \times 1}
	\tag{4.18}
\end{align}
```
のように書き換え，<font color="#0070C0">未知パラメータ $\pmb{p}$</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}
	% =================================================
	\textcolor{myblue}{\pmb{p}} 
	= (\pmb{M}_{\rm s}^{\top}\pmb{M}_{\rm s})^{-1}
	\pmb{M}_{\rm s}^{\top}\pmb{N}_{\rm s}
	\tag{4.19}
\end{align}
```
により定めれば良いことが知られています．ここで，
```math
\begin{align}
	\pmb{M}^+ := (\pmb{M}_{\rm s}^\top\pmb{M}_{\rm s})^{-1}\pmb{M}_{\rm s}^\top
	\tag{4.20}
\end{align}
```
を $\pmb{M}$ の<b>擬似逆行列</b>と呼びます．

　一見，この方法でうまくいくように思うかもしれませんが，実は，問題があります．いま，ロータリエンコーダで振子の角度 $\phi_2$ を検出し，その実験データが図 $4.9$ であったとします． 

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/fa0afe7d-e6f5-b2d0-34c0-77fa32de41ab.png)|
|:-:|
|図 $4.9$　振子の角度 $\phi_2$|

ここで，エンコーダの分解能は $360\ \rm{[PPR]}$ であり，これを $4$ 逓倍でカウントしているので，角度データは $0.25\ \rm{[deg]}$ ごとの離散的な値でしか検出できないことに注意してください．実際の値とこの離散的な値との誤差を，<b>量子化誤差</b>といいますが，この影響で，角速度 $\dot{\phi}_2$ を後退差分近似
```math
\begin{align}
	\dot{\phi}_{2}[i] = \dfrac{{\phi}_{2}[i] - {\phi}_{2}[i-1]}{h}
	\tag{4.21}
\end{align}
```
で計算すると，図 $4.10$ のように高周波信号（<b>チャタリング</b>）が発生します．ただし，サンプリング周期は $h = 0.01\ {\rm[s]}$ です．　

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/017a3f00-c621-7903-7912-b6f9db3047d0.png)|
|:-:|
|図 $4.10$　振子の角速度 $\dot{\phi}_2$ (後退差分近似)|

さらに，角加速度 $\ddot{\phi}_2$ を後退差分近似
```math
\begin{align}
	\ddot{\phi}_{2}[i] = \dfrac{\dot{\phi}_{2}[i] - \dot{\phi}_{2}[i-1]}{h}
	\tag{4.22}
\end{align}
```
で計算すると，図 $4.11$ のように激しいチャタリングを生じてしまいます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/6e6aefdd-aa6c-f84a-5830-e94d8c129cbf.png)|
|:-:|
|図 $4.11$　振子の角加速度 $\ddot{\phi}_2$ (後退差分近似)|

　このように，チャタリングの影響が無視できないほど大きい場合，最小 2 乗法を利用して同定しても，うまくいきません．


#### <font color="#33CC33">(b)　チャタリング対策</font>

　最小 2 乗法を利用する場合，チャタリングの影響を小さくするような信号処理が必要です．

　まず，差分近似の方法を改善してみましょう．さきほどの例では，<font color="#00B0F0"><b>後退差分近似</b></font>により角速度 $\dot{\phi}_2$ や角加速度 $\ddot{\phi}_2$ を算出しました．PID 制御を行っているときなどは，<b>オンライン（リアルタイム）</b>で微分信号を算出する必要があるので，現在と過去の角度データから角速度を算出する<font color="#00B0F0"><b>後退差分近似</b></font>がよく利用されます．

　しかし，パラメータ同定を行うのは<b>オフライン</b>の処理です．そのため，現在の角速度を算出する際に，過去の角度データだけでなく，未来の角度データも利用できます．このことを考慮して，<font color="#E48312"><b>中心差分近似</b></font>
```math
\begin{align}
	\dot{\phi}_{2}[i] = \dfrac{{\phi}_{2}[i+1] - {\phi}_{2}[i-1]}{2h}
	\tag{4.23}\\
	\ddot{\phi}_{2}[i] = \dfrac{\dot{\phi}_{2}[i+1] - \dot{\phi}_{2}[i-1]}{2h}
	\tag{4.24}
\end{align}
```
により角速度 $\dot{\phi}_2$ や角加速度 $\ddot{\phi}_2$ を算出した結果が図 $4.12$ および図 $4.13$ です．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/85091411-d043-125c-5900-f8d5084ceb20.png)|
|:-:|
|図 $4.12$　振子の角速度 $\dot{\phi}_2$（後退差分近似と中心差分近似の比較）|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/919fc414-4061-c93f-9e75-b3a27982c3d4.png)|
|:-:|
|図 $4.13$　振子の角加速度 $\ddot{\phi}_2$（後退差分近似と中心差分近似の比較）|

これらより，チャタリングが軽減されていることが確認できます．

　しかし，振子の角加速度 $\ddot{\phi}_2$ にはまだチャタリングが残っています．さらなるチャタリング除去のために，<b>ローパスフィルタ</b>を使用してみましょう．ここで注意することは，

* <font color="#E84746"><b>非線形モデル $(4.15)$ 式に含まれるすべての信号を，同じローパスフィルタに通過させる</b></font>

という点です．これは，ある信号をローパスフィルタ (LPF) に通過させると，高周波成分の振幅は小さくなりますが，<b>必要な低周波成分の位相が遅れてしまう</b>ことに対処するためです．とくに，強いローパスフィルタを使用する場合はこの位相の遅れが顕著に現れ，振幅も少し小さくなってしまいます．それに対して，<b>すべての信号 $\pmb{M}(t)$, $N(t)$ を同じローパスフィルタに通過</b>させると，

* <b>不必要な高周波成分は，除去される</b>
* <b>必要な低周波成分は，同じだけ振幅が小さくなり，同じだけ位相が遅れる</b>

ので，等式関係が成立し，位相のずれは問題となりません．

　具体的には，$(4.15)$ 式の両辺の信号を 3 次のローパスフィルタ
```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}
	% =================================================
	\textcolor{myred}{{G}_{\rm f}(s)} = \dfrac{1}{(1 + {T}_{\rm f}s)^3}
	\tag{4.25}
\end{align}
```
に通過させ，
```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}
	% =================================================
	\underbrace{\bigl[\begin{array}{cc}
		\textcolor{myred}{{G}_{\rm f}(s)}\ddot{\phi}_{2}(t) & 
		\textcolor{myred}{{G}_{\rm f}(s)}\dot{\phi}_{2}(t)
	\end{array}\bigr]}_{\mbox{$\pmb{M}_{\rm f}(t)$}}
	\underbrace{\left[\begin{array}{c}
		\textcolor{myblue}{{\alpha}_{2}} \\
		\textcolor{myblue}{{\beta}_{2}}
	\end{array}\right]}_{\mbox{$\textcolor{myblue}{\pmb{p}}$}}
	= \underbrace{{}- \textcolor{myred}{{G}_{\rm f}(s)}g\sin{\phi}_{2}(t)}_{\mbox{$N_{\rm f}(t)$}}
	\tag{4.26}
\end{align}
```
とします．ただし，$s = {\rm d}/{\rm d}t$ は微分演算子であり，
```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}
	% =================================================
	{M}_{\rm f1}(t) = \textcolor{myred}{{G}_{\rm f}(s)}\ddot{\phi}_{2}(t),\ 
	{M}_{\rm f2}(t) = \textcolor{myred}{{G}_{\rm f}(s)}\dot{\phi}_{2}(t)
\end{align}
```
です．

　3 次のローパスフィルタ $(4.25)$ 式の時定数を $T_{\rm f} = 0.05$ としてチャタリング除去の処理を行った結果を以下に示します．　

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/dbfe4a47-5f7b-a808-63eb-c90d99ff8000.png)|
|:-:|
|図 $4.14$　振子の角速度 $M_2=\dot\phi_2$ と LPF 通過後の $M_{\rm{f2}}$|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/d2cf912c-9817-93a4-884b-4bae3f623482.png)|
|:-:|
|図 $4.15$　振子の角加速度 $M_1=\ddot\phi_2$ と LPF 通過後の $M_{\rm{f1}}$|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/bf1cf7d2-b84f-5585-fcfd-fd8446245596.png)|
|:-:|
|図 $4.16$　振子の重力項 $N = -g\sin\phi_2$ と LPF 通過後の $N_{\rm{f}}$|

図 $4.14$ $\sim$ $4.16$ の結果から

* チャタリングが十分に除去されている
* 位相が同程度に遅れている
* 振幅が同程度に小さくなっている

ことが確認できます．

　最後に，パラメータ同定を行うための式を導出します．先と同様の議論により，理想的には
```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}
	% =================================================
	\underbrace{\left[\begin{array}{cc}
		{M}_{\rm f1}[0] & {M}_{\rm f2}[0] \\
		{M}_{\rm f1}[1] & {M}_{\rm f2}[1] \\
		\vdots & \vdots \\
		{M}_{\rm f1}[k] & {M}_{\rm f2}[k]
	\end{array}\right]}_{\mbox{$\pmb{M}_{\rm fs}$}}
	\underbrace{\left[\begin{array}{c}
		\textcolor{myblue}{{\alpha}_{2}} \\
		\textcolor{myblue}{{\beta}_{2}}
	\end{array}\right]}_{\mbox{$\textcolor{myblue}{\pmb{p}}$}}
	= \underbrace{\left[\begin{array}{c}
		{N}_{\rm f}[0] \\
		{N}_{\rm f}[1] \\
		\vdots \\
		{N}_{\rm f}[k]
	\end{array}\right]}_{\mbox{$\pmb{N}_{\rm fs}$}}
	\tag{4.27}
\end{align}
```
が成り立ちます．したがって，<font color="#0070C0">未知パラメータ $\pmb{p}$</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}
	% =================================================
	\textcolor{myblue}{\pmb{p}} 
	= (\pmb{M}_{\rm fs}^{\top}\pmb{M}_{\rm fs})^{-1}
	\pmb{M}_{\rm fs}^{\top}\pmb{N}_{\rm fs}
	\tag{4.28}
\end{align}
```
により定めれば良いことがわかります．



### <font color="#33CC33">4.3.2　同定実験</font>

　<font color="#33CC33"><b>4.2.2 (b)</b></font> で取得した自由振動の実験データを利用して，最小 2 乗法により振子のパラメータ同定を行う M ファイルを以下に示します．

```matlab:M ファイル "ident_pend_lsm.m"
clear
format compact
% -----------------------
myred   = [232   71   70]/255;
mygreen = [ 12  162  116]/255;
myblue  = [  0  112  192]/255;
% -----------------------
g   = 9.81e+00;
Tf2 = 0.05;
% -----------------------
load ident_pend_data     % おもりなし
% load ident_pend_data2    % おもり 1 個
% load ident_pend_data3    % おもり 2 個
% -----------------------
tmin = 10;
tmax = 22;

% =========================================
k = length(t);
% -----------------------
dphi2(1) = (- 3*phi2(1) + 4*phi2(2) - phi2(3))/(2*h);
for i = 2:k-1
    dphi2(i) = (phi2(i+1) - phi2(i-1))/(2*h);
end
dphi2(k) = (phi2(k-2) - 4*phi2(k-1) + 3*phi2(k))/(2*h);
% -----------------------
ddphi2(1) = (- 3*dphi2(1) + 4*dphi2(2) - dphi2(3))/(2*h);
for i = 2:k-1
  ddphi2(i) = (dphi2(i+1) - dphi2(i-1))/(2*h);
end
ddphi2(k) = (dphi2(k-2) - 4*dphi2(k-1) + 3*dphi2(k))/(2*h);

% =========================================
t_data = t;
phi2_data   = phi2;
dphi2_data  = dphi2;
ddphi2_data = ddphi2;

clear t phi2 dphi2 ddphi2
% -----------------------
k = 0;
for i = 1:length(t_data)
    if t_data(i) >= tmin & t_data(i) <= tmax
        k = k + 1;
        t(k,1)      = t_data(i) - tmin;
        phi2(k,1)   = phi2_data(i);
        dphi2(k,1)  = dphi2_data(i);
        ddphi2(k,1) = ddphi2_data(i);
    end
end

% =========================================
M1 = ddphi2;
M2 = dphi2;
N  = - g*sin(phi2);

sysGf2 = tf(1,[Tf2 1])^3;

Mf1 = lsim(sysGf2,M1,t);
Mf2 = lsim(sysGf2,M2,t);
Nf  = lsim(sysGf2,N ,t);

% -----------------------
figure(1)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('northwest')

p1 = plot(t,M1,'Color',myred,'LineWidth',1.5);
hold on
p2 = plot(t,Mf1,'Color',myblue,'LineWidth',1.5);
hold off

set(gca,'FontName','Arial','FontSize',20)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',22)
ylabel('$${M}_{1}(t)$$ and $${M}_{\rm f1}(t)$$', 'interpreter', 'latex','FontSize',22)

legend([p1 p2],{'$${M}_{1}(t)=\ddot{\phi}_{2}(t)$$\quad','$${M}_{\rm f1}(t)$$'},'Location','southeast','NumColumns',2)
set(legend, 'interpreter', 'latex','FontSize',20)

ylim([-150 150])
set(gca,'XTick',0:1:t(end))
set(gca,'YTick',-150:50:150)

grid on

% -----------------------
figure(2)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('northeast')

p1 = plot(t,M2,'Color',myred,'LineWidth',1.5);
hold on
p2 = plot(t,Mf2,'Color',myblue,'LineWidth',1.5);
hold off

set(gca,'FontName','Arial','FontSize',20)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',22)
ylabel('$${M}_{2}(t)$$ and $${M}_{\rm f2}(t)$$', 'interpreter', 'latex','FontSize',22)

legend([p1 p2],{'$${M}_{2}(t)=\dot{\phi}_{2}(t)$$\quad','$${M}_{\rm f2}(t)$$'},'Location','southeast','NumColumns',2)
set(legend, 'interpreter', 'latex','FontSize',20)

ylim([-15 15])
set(gca,'XTick',0:1:t(end))
set(gca,'YTick',-15:5:15)

grid on

% -----------------------
figure(3)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('southwest')

p1 = plot(t,N,'Color',myred,'LineWidth',1.5);
hold on
p2 = plot(t,Nf,'Color',myblue,'LineWidth',1.5);
hold off

set(gca,'FontName','Arial','FontSize',20)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',22)
ylabel('$$N(t)$$ and $${N}_{\rm f}(t)$$', 'interpreter', 'latex','FontSize',22)

legend([p1 p2],{'$$N(t)=-g\sin{\phi}_{2}(t)$$\quad','$${N}_{\rm f}(t)$$'},'Location','southeast','NumColumns',2)
set(legend, 'interpreter', 'latex','FontSize',20)

ylim([-15 15])
set(gca,'XTick',0:1:t(end))
set(gca,'YTick',-15:5:15)

grid on

% =========================================
Mf = [ Mf1  Mf2 ];

p2 = inv(Mf'*Mf)*Mf'*Nf;

alpha2 = p2(1);
beta2  = p2(2);

fprintf('alpha2 = %3.2e\n',alpha2)
fprintf('beta2  = %3.2e\n',beta2)

% -----------------------
[phi20 i] = max(phi2);
t0 = t(i);

sim('sim_nonlinear_pend')

figure(4)
set(gcf,'Color','white','PaperType','A3')
set(gcf,'Position',[100 100 800 400]) % [x0 y0 width height]
subplot('Position',[0.2 0.2 0.7 0.7]) % [left bottom width height]
movegui('southeast')

p1 = plot(tsim,phi2sim*180/pi,'Color',[ 12 162 116]/255,'LineWidth',1.5);
hold on
p2 = stairs(t,phi2*180/pi,'Color',myred,'LineWidth',1.5);
hold off

xlim([0 t(end)])
ylim([-90 90])

set(gca,'FontName','Arial','FontSize',20)
xlabel('$$t$$ [s]', 'interpreter', 'latex','FontSize',22)
ylabel('$${\phi}_{2}(t)$$ [deg]', 'interpreter', 'latex','FontSize',22)

legend([p2 p1],{'Experiment','Nonlinear Simulation'},'Location','southeast')

set(gca,'XTick',0:1:t(end))
set(gca,'YTick',-90:30:90)

grid on
```

この M ファイル "ident_pend_lsm.m" を実行すると，

```matlab:M ファイル "ident_pend_lsm.m" の実行結果
>> ident_pend_lsm
alpha2 = 1.10e-01
beta2  = 3.59e-02
```
という結果が得られます．

|パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
|---|---|---|
|<font color="#0070C0">$\alpha_2$</font>|<font color="#0070C0">$1.10 \times 10^{-1}$</font>|<font color="#0070C0">alpha2</font>|
|<font color="#0070C0">$\beta_2$</font>|<font color="#0070C0">$3.59 \times 10^{-2}$</font>|<font color="#0070C0">beta2</font>|

そして，図 $4.14$ $\sim$ $4.16$ の結果が描画されます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/77264077-2c65-ccda-03bb-0cd974365c42.png)|
|:-:|
|図 $4.17$　振子の角加速度 $M_1=\ddot\phi_2$ と LPF 通過後の $M_{\rm{f1}}$|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/9e277953-31e8-971c-1ee3-b579c929f158.png)|
|:-:|
|図 $4.18$　振子の角速度 $M_2=\dot\phi_2$ と LPF 通過後の $M_{\rm{f2}}$|

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/bb18849f-1b8f-02cc-42f8-f9ba9f013340.png)|
|:-:|
|図 $4.19$　振子の重力項 $N = -g\sin\phi_2$ と LPF 通過後の $N_{\rm{f}}$|

さらに，M ファイル "ident_pend_lsm.m" のなかでは，同定されたパラメータを用いて

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/66b27051-80df-0301-a44b-a996c23bc878.png)|
|:-:|
|[Simulink ブロック "Fcn" についての補足](https://www.maizuru-ct.ac.jp/control/kawata/study/book4/errata/about_Fcn_2021_11_18.pdf)|
|<b>Simulink モデル "sim_nonlinear_pend.slx"</b>|

を実行した非線形シミュレーションを実行しており，実験結果と非線形シミュレーションの結果を比較した図 $4.20$ のグラフが描画されます．これより，両者がおおよそ一致していることが確認できます．

|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/4a783a78-c0be-4c88-d04f-6f4dbee4b08c.png)|
|:-:|
|図 $4.20$　実験結果と線形シミュレーションの結果|



# <font color="#33CC33">5. 振子が静止してしまう場合の対処方法について</font>

　振子を取り付けている[ロータリエンコーダ](https://www.autonics.com/model/A2500002016)としては，比較的，動摩擦や静止摩擦が小さいものを使用しています．しかし，振子が軽量であるために，エンコーダ軸の動摩擦や静止摩擦の影響で，振子を自由振動させたときに真下 ($\phi_2 = 0$) ですぐに静止してしまうことがあります (動摩擦や静止摩擦の影響の大きさはエンコーダの個体差に依存します)．このような場合，振子を手動で真上に持っていったときに，静止摩擦により簡単に倒立点で静止してしまい，倒立振子としてはおもしろくありません．

　この問題に対処したい場合は，振子の先端におもりとして [LEGO 部品 (ダブルべベルギア 12歯，デザイン ID：32270)](https://www.brickers.jp/?pid=24981433) を取り付けてください．

> <font color="#33CC33"><b>(1) おもりを取り付けていない場合</b></font>
> ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/950cfa5a-2fa2-94b7-3102-5e47b83f7eb6.png)
> |パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
> |---|---|---|
> |<font color="#E84746">$g$</font>|<font color="#E84746">$9.81 \times 10^{0}$ $[\rm{m/s^2}]$</font>|<font color="#E84746">g</font>|
> |<font color="#E84746">$L_1$</font>|<font color="#E84746">$7.00 \times 10^{-2}$ $[\rm{m}]$</font>|<font color="#E84746">L1</font>|
> |<font color="#0070C0">$\alpha_1$</font>|<font color="#0070C0">$1.53 \times 10^{1}$</font>|<font color="#0070C0">alpha1</font>|
> |<font color="#0070C0">$\beta_1$</font>|<font color="#0070C0">$7.06 \times 10^{1}$</font>|<font color="#0070C0">beta1</font>|
> |<font color="#0070C0">$\alpha_2$</font>|<font color="#0070C0">$1.10 \times 10^{-1}$</font>|<font color="#0070C0">alpha2</font>|
> |<font color="#0070C0">$\beta_2$</font>|<font color="#0070C0">$3.59 \times 10^{-2}$</font>|<font color="#0070C0">beta2</font>|

> <font color="#33CC33"><b>(2) おもりを 1 個取り付けた場合</b></font>
> ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/12fa65c1-b525-25df-074e-0a3532b50338.png)
> |パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
> |---|---|---|
> |<font color="#E84746">$g$</font>|<font color="#E84746">$9.81 \times 10^{0}$ $[\rm{m/s^2}]$</font>|<font color="#E84746">g</font>|
> |<font color="#E84746">$L_1$</font>|<font color="#E84746">$7.00 \times 10^{-2}$ $[\rm{m}]$</font>|<font color="#E84746">L1</font>|
> |<font color="#0070C0">$\alpha_1$</font>|<font color="#0070C0">$1.53 \times 10^{1}$</font>|<font color="#0070C0">alpha1</font>|
> |<font color="#0070C0">$\beta_1$</font>|<font color="#0070C0">$7.06 \times 10^{1}$</font>|<font color="#0070C0">beta1</font>|
> |<font color="#0070C0">$\alpha_2$</font>|<font color="#0070C0">$1.23 \times 10^{-1}$</font>|<font color="#0070C0">alpha2</font>|
> |<font color="#0070C0">$\beta_2$</font>|<font color="#0070C0">$3.04 \times 10^{-2}$</font>|<font color="#0070C0">beta2</font>|

> <font color="#33CC33"><b>(3) おもりを 2 個取り付けた場合</b></font>
> ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2552191/143afc83-c1d4-af9b-6aec-d32a926ae34e.png)
> |パラメータ|値　　　　　　　　　　　　　　|MATLAB 変数|
> |---|---|---|
> |<font color="#E84746">$g$</font>|<font color="#E84746">$9.81 \times 10^{0}$ $[\rm{m/s^2}]$</font>|<font color="#E84746">g</font>|
> |<font color="#E84746">$L_1$</font>|<font color="#E84746">$7.00 \times 10^{-2}$ $[\rm{m}]$</font>|<font color="#E84746">L1</font>|
> |<font color="#0070C0">$\alpha_1$</font>|<font color="#0070C0">$1.53 \times 10^{1}$</font>|<font color="#0070C0">alpha1</font>|
> |<font color="#0070C0">$\beta_1$</font>|<font color="#0070C0">$7.06 \times 10^{1}$</font>|<font color="#0070C0">beta1</font>|
> |<font color="#0070C0">$\alpha_2$</font>|<font color="#0070C0">$1.30 \times 10^{-1}$</font>|<font color="#0070C0">alpha2</font>|
> |<font color="#0070C0">$\beta_2$</font>|<font color="#0070C0">$2.70 \times 10^{-2}$</font>|<font color="#0070C0">beta2</font>|



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

　今回は，回転型倒立振子の未知パラメータを同定する手順について説明をしました．パラメータ同定の作業がうまくいけば，モデルに基づいた制御系設計を行うための見通しが良くなります．

　次回は，コントローラ設計について説明します．

（終わり）

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

1. 大須賀 公一：[メカニカルシステムの同定（センサの動特性を考慮した同定法とその検証実験）](https://www.jstage.jst.go.jp/article/sicejl1962/33/6/33_6_487/_pdf/-char/ja)，計測と制御，Vol.33, No.6, pp.487-493 (1994)
　$\cdots\cdots$ https://doi.org/10.11499/sicejl1962.33.6_487

1. 川田昌克（編著），東　俊一，市原裕之，浦久保孝光，大塚敏之，甲斐健也，國松禎明，澤田賢治，永原正章，南　裕樹： [倒立振子で学ぶ制御工学](https://www.amazon.co.jp/dp/4627787014)， 森北出版 (2017.2.28)
$\Longrightarrow$ サポートページは[こちら](https://www.maizuru-ct.ac.jp/control/kawata/study/book_ip/book_ip_page.html)

1. 計測自動制御学会編：[ロボット制御の実際](https://www.coronasha.co.jp/np/isbn/9784339083620/)　第 3 章「ロボットの同定」，コロナ社(1997)