LEGO 部品を利用した回転型倒立振子のレシピを公開！
- 第 1 回：組み立て
- 第 2 回：電子工作
- 第 3 回：運動方程式 ― MATLAB / Symbolic Math Toolbox
- 第 4 回：パラメータ同定 ― MATLAB / Simulink
- 第 5 回：現代制御 ― MATLAB / Simulink …… 未完成
- 第 6 回：アドバンスト制御 ― MATLAB / Simulink …… 未完成
おまけ
補足
＊ とりあえずアップしますね
1. はじめに
ラグランジュ法を利用した回転型倒立振子の運動方程式の導出については
で説明しました．今回は，運動方程式に含まれるパラメータを実験で定める（このことをパラメータ同定といいます）方法について説明します．
2. 回転型倒立振子のモデル
2.1 ラグランジュ法により導出された運動方程式
「第 3 回：運動方程式編」で得られた運動方程式は
\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)$ 式は振子の運動方程式です．ただし，
\begin{align}
{J}_{1} = {J}_{\rm g1} + {m}_{1}{l}_{1}^{2},\
{J}_{2} = {J}_{\rm g2} + {m}_{2}{l}_{2}^{2}
\end{align}
であり，パラメータは以下に示すとおりです．
 |
|---|
| 図 $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]$ | 振子の粘性摩擦係数 |
2.2 モータ駆動系のモデル
モータ駆動系（モータ $+$ モータドライバ）に定値の指令電圧 $v(t) = {v}_{\rm c}$ $(t \ge 0)$ を加えたときのふるまいを考えてみましょう．このとき，図 $2.2$ に示すように，定常状態では角度 $\theta_1(t)$ は一定の割合で増加していくことは容易に想像がつくことでしょう．これを時間微分したものが角速度 $\omega_1(t) = \dot{\theta}_1(t)$ ですが，振動することなく，一定速度に収束します．
 |
|---|
| 図 $2.2$ モータ駆動系のステップ応答 |
つまり，入力を指令電圧 $v(t)$，出力を 角速度 $\omega_1(t) = \dot{\theta}_1(t)$ としたシステムは
\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}
のように，1 次遅れ系となります．
 |
|---|
| 図 $2.3$ モータ駆動系のモデル |
したがって，入力を指令電圧 $v$，出力を 角度 ${\theta}_{1}$ としたモータ駆動系のモデルは，$(2.3)$ 式より
\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)$ 式を微分方程式で表すと，
\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 部品なので軽量である
- 使用したモータはギヤ付きである
というから，振子がアームに与える影響は十分小さいと考えることができます．したがって，アームと振子が取り付けられたモータ駆動系は，単純な
\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$ を考慮したモデルは外乱オブザーバを構成する際に必要となります．
2.3 未知パラメータ
モータ駆動を考慮した回転型倒立振子のモデルは
\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)$ 式に含まれるパラメータを，値が既知あるいは直接的に測定可能なもの（既知パラメータ）と，そうでないもの（未知パラメータ）に分類すると，以下のようになります．
| 既知パラメータ | 未知パラメータ |
|---|---|
| $\bullet$ アーム：$L_1$ $\bullet$ 振子：$g$, $m_2$, $l_2$ |
$\bullet$ アーム：$\alpha_1$, $\beta_1$ $\bullet$ 振子：$J_2$, $c_2$ |
ここで，振子の既知パラメータであるとした $m_2$, $l_2$ は，振子を取り外すことで値を測定することができます．本実験装置は LEGO で製作していますので簡単に振子を取り外すことができますが，一般に，振子を取り外したくない場合も多いと思います．そこで，$(2.2)$ 式の両辺を $m_2l_2$ で割り，
\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}
のように書き換えます．さらに，
\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}
とおくと，$\alpha_1$, $\beta_1$ を未知パラメータとした
\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$ を事前に知る必要はありません．
2.4 モデルのまとめ
モータ駆動を考慮した回転型倒立振子のモデルは
\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}
であり，既知パラメータと未知パラメータはそれぞれ
| 既知パラメータ | 未知パラメータ |
|---|---|
| $\bullet$ アーム：$L_1$ $\bullet$ 振子：$g$ |
$\bullet$ アーム：$\alpha_1$, $\beta_1$ $\bullet$ 振子：$\alpha_2$, $\beta_2$ |
となります．
3. アームのパラメータ同定
3.1 パラメータ同定の手順
ここでは，アーム角度の P 制御（比例制御）を行ったときの実験データを利用し，2 次遅れ系の特性に基づいて未知パラメータを定める（パラメータ同定を行う）方法を説明します．
$(2.6)$ 式において，外乱 $d$ を無視する ($d = 0$ とする) と，
\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 コントローラ（比例コントローラ）
\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$ を定値の目標値
\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}
に制御することを考えます．
 |
|---|
| 図 $3.1$ アーム角度の P 制御（比例制御） |
ここで，$\theta_1^{\rm ref}$ から $\theta_1$ への伝達関数 $T(s)$ を求めると，
\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}
となるので，2 次遅れ要素の標準形
\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}
で記述できることがわかります．ここで
- 固有角周波数 ${\omega}_{\rm n1} > 0$：速応性に関するパラメータ
- 減衰係数 ${\zeta}_{1} > 0$：安定度に関するパラメータ
です．$(3.2)$ 式と $(3.3)$ 式の係数を比較すると，
\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}
が得られます．
 |
|---|
| 図 $3.2$ アーム角の P 制御（比例制御）系のステップ応答 |
 |
|---|
| 図 $3.3$ アーム角の P 制御（比例制御）系のステップ応答の偏差 $e(t)$ の大きさ |
2 次遅れ系のステップ応答が図 $3.2$ のようにオーバーシュート ${A}_{\rm max}$ を生じるのは不足制動 $(0 < {\zeta}_{1} < 1)$ のときであり，減衰係数 ${\zeta}_{1}$ が $0$ に近づくにしたがって，オーバーシュート ${A}_{\rm max}$ は大きくなるという性質があります．したがって，$(3.4)$ 式より
-
「比例ゲイン $k_{\rm P} \rightarrow$ 大」
$\Longrightarrow$ 「減衰係数 $\zeta_{1} \rightarrow 0$」
$\Longrightarrow$ 「オーバーシュート $A_{\rm max} \rightarrow$ 大」
となります．ただし，指令電圧の大きさ $|v(t)|$ が上限値 ${v}_{\rm max}$ を超えないように
\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)$ の大きさの最大値が
\begin{align}
\max|e(t)| = e(0)
= \textcolor{mydarkpink}{\theta_{\rm 1c}}
\tag{3.6}
\end{align}
であることを考慮すると，
\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}
なので，
\begin{align}
0 < \textcolor{mydarkyellow}{{k}_{\rm P}}
\textcolor{mydarkpink}{\theta_{\rm 1c}} \le {v}_{\rm max}
\tag{3.8}
\end{align}
を満足するように定値の目標値 $\theta_{\rm 1c} > 0$ と比例ゲイン ${k}_{\rm P} > 0$ が選ばれている必要があります．
なお，不足制動 $(0 < \zeta_1 < 1)$ のとき，図 $3.2$ に示す行き過ぎ時間 $T_{\rm p}$ およびオーバーシュート $A_{\rm max}$ は
\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}
となります．
アームのパラメータ同定の手順は以下のようになります．
アームのパラメータ同定の手順
ステップ 1
$(3.8)$ 式を考慮したうえで，適当な定値の目標値 $\theta_{\rm 1c} > 0$ と比例ゲイン $k_{\rm P} > 0$ を与えて実験を行い，図 $3.2$ のようにオーバーシュートを生じさせます．そして，実験データから行き過ぎ時間 $T_{\rm p}$ とオーバーシュート $A_{\rm max}$ を抽出します．
ステップ 2
$(3.9)$, $(3.10)$ 式を逆算し，
\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}により 減衰係数 $\zeta_1$ と固有角周波数 $\omega_{\rm n1}$ の値を定めます．
ステップ 3
$(3.4)$ 式より
\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}のように未知パラメータ $\alpha_1$, $\beta_1$ の値を定めます．
3.2 同定実験
3.2.1 準備（極性の調整）
同定実験を行う前に，アームの回転方向が正しいかをチェックします．もし，回転方向が正しくない状態でフィードバック制御を行うと，不安定化して暴走してしまうので，必ずチェックする必要があります！
まず，スイッチサイエンスさんから購入した外部電源
で電力供給を行うので，マイコンシールド "MinSegShield M2V5" のスイッチを図 $3.4$ のように設定します．
 |
|---|
| 図 $3.4$ マイコンシールド "MinSegShield M2V5" のスイッチの設定 |
Arduino Mega 2560 互換ボードによっては，
のように，ケーブルを別途，購入する必要がありますので，注意してください．
USB ケーブルでマイコンボードとパソコンを接続してから，以下の Simulink モデル "check_arm.slx" をエクスターナル実行してください．
 |
|---|
| Simulink モデル "check_arm.slx" |
実行後，Simulink ブロック "Manual Switch" をダブルクリックして 0 から 1 に切り替えたとき，図 $3.5$ に示すように，
- モータが時計回りに回転
- Simulink ブロック "Scope" の表示が正方向に増加
するかどうかを確認してください．
 |
|---|
| 図 $3.5$ 極性の確認 |
もし，これとは逆の動きをする場合は，以下のように設定してください．
- モータが反時計回りに回転した場合は，Simulink ブロック "Gain (motor polarity)" の値を 1 から -1 に変更してください．
- Simulink ブロック “Scope” の表示が負方向に増加した場合は，Simulink ブロック "Gain (arm encoder polarity)" の値を 1 から -1 に変更してください．
3.2.2 アーム角の P 制御とパラメータ同定
アーム角の P 制御を行うための Simulink モデルを以下に示します．
 |
|---|
| Simulink モデル "ex_p_cont.slx" |
ここで，サブシステム "Arm (Vsupply = 9 [V])" のなかの Simulink ブロック "Gain (motor polarity)" や "Gain (arm encoder polarity)" は，極性を調整した Simulink モデル "check_arm.slx" と同じ値に設定しておいてください．
つぎに，定値の目標値 $\theta_{\rm 1c} > 0$ と比例ゲイン $k_{\rm P} > 0$ の値を設定します．
$v_{\rm max} = 9\ {\rm [V]}$ なので，$\theta_{\rm 1c} = 90\ {\rm [deg]} = \pi/2\ {\rm [rad]}$ としたとき，$(3.8)$ 式より 比例ゲイン $k_{\rm P} > 0$ は
\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}
を満足する必要があります．そこで，$k_{\rm P} = 4$ と選びました．また，サンプリング周期は $h = 0.01\ {\rm [s]}$ ですので，これらの値を MATLAB のコマンドウィンドウで
>> ref = 90; h = 0.01; kP = 4;
のように設定します．そして，エクスターナル実行を行うと，
のように動作します．Simulink ブロック "Scope" での観測結果を図 $3.6$ に示します．
 |
|---|
| 図 $3.6$ Simulink ブロック "Scope" による観測結果 |
振子の振動がアームにまったく影響を与えないわけではなく，若干，アームが振動しています．実験データを "ident_arm_data.mat" という名前の mat ファイルに保存するため，コマンドウィンドウで
>> save ident_arm_data t v theta1 ref h kP
もしくは
>> save('ident_arm_data','t','v','theta1','ref','h','kP')
のように入力します．そして，以下の M ファイルを実行してください．
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
その結果，
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 変数 |
|---|---|---|
| 行き過ぎ時間 $T_{\rm p}$ | $2.10 \times 10^{-1}\ {\rm [s]}$ | Tp |
| オーバーシュート $A_{\rm max}$ | $3.14 \times 10^{-1}\ {\rm [rad]}\ (= 18\ {\rm [deg]})$ | Amax |
| 固有角周波数 $\omega_{\rm n1}$ | $1.68 \times 10^{1}$ | wn1 |
| 減衰係数 $\zeta_1$ | $4.56 \times 10^{-1}$ | zeta1 |
| $\alpha_1$ | $1.53 \times 10^{1}$ | alpha1 |
| $\beta_1$ | $7.06 \times 10^{1}$ | beta1 |
また，同定されたパラメータを用いたシミュレーション結果と実験結果を比較した図 $3.7$ のグラフが描画されます．これよりピーク値が一致していることはもちろんですが，定常状態で振子の振動の影響を受けていることを除けば，おおよそ一致していることが確認できます．
 |
|---|
| 図 $3.7$ 実験結果とシミュレーション結果の比較 |
なお，振子を取り外して実験を行うと，図 $3.8$ のようになり，振子の振動による影響がないことがわかります．一方で，動摩擦や静止摩擦に起因した定常偏差が生じていることが確認できます．
 |
|---|
| 図 $3.8$ 実験結果とシミュレーション結果の比較 （振子を取り外して実験を行った場合） |
振子の振動による反動トルクや動摩擦，静止摩擦は外乱 $d$ の一部であると考えることができますので，外乱オブザーバによりその推定値 $\hat{d}$ を計算し，フィードバックで補償することも考えられます．
4. 振子のパラメータ同定
4.1 振子の自由振動
振子については，アームを固定したときの振子の自由振動の実験データを利用して，二種類の方法
- 線形 1 自由度振動系の特性に基づく方法
- 最小 2 乗法に基づく方法
により振子のモデル
\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}
に含まれる未知パラメータ $\alpha_2$, $\beta_2$ の同定を行います．
 |
|---|
| 図 $4.1$ 振子角の基準 |
図 $4.1$ に示すように，振子角の基準を真上から真下に変更すると，
\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)$ 式は
\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}
のように書き換えることができます．また，自由振動の実験を行う際に，アームが回転しないように固定したときを考えると，
\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)$ 式は
\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)$ 式がパラメータ同定を行う際に利用される微分方程式です．
4.2 線形 1 自由度振動系の特性に基づく方法
4.2.1 パラメータ同定の手順
$(4.2)$ 式は非線形項を含みますので，このままでは解析が困難です．そこで，振子が真下近傍（$\phi_2 = 0$）で動作するとして，非線形項を
\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)$ 式は 2 階の線形微分方程式
\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)$ 式は「振動工学」の分野でいう「線形 1 自由度振動系」のモデルに相当します．
つぎに，$(4.3)$ 式を解析してみましょう．$(4.3)$ 式は減衰係数 $\zeta_2$ と固有角周波数 $\omega_{\rm n2}$ により
\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}
という形式に書き換えることができます．ただし，
\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$ のように「周期振動をする」ので，不足制動 $(0 < \zeta_2 < 1)$ となります．したがって，$(4.4)$ 式の解軌道は
\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}
となり，振動周期 $T$ および減衰率 $\lambda$ は
\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}
となり，それぞれが一定値であることがわかります．この性質を利用して振子のパラメータを同定します．
 |
|---|
| 図 $4.2$ 振子の自由振動（$\phi_2 = 0$ の近傍） |
振子のパラメータ同定の手順は以下のようになります．
振子のパラメータ同定の手順
ステップ 1
アームを固定したときの振子の自由振動の実験データから，図 $4.2$ に示す $n$ 個の振動の頂点 $(\bar{t}_i,\ A_i)$ $(i = 1,\ 2,\ \ldots,\ n)$ を抽出します．そして，
\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)$ 式の平均値
\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}を計算し，これらを自由振動の振動周期 $T$ および減衰率 $\lambda$ とします．
ステップ 2
$(4.7)$, $(4.8)$ 式を逆算し，
\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}により 減衰係数 $\zeta_2$ と固有角周波数 $\omega_{\rm n2}$ の値を定めます．
ステップ 3
$(4.5)$ 式より
\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}のように未知パラメータ $\alpha_2$, $\beta_2$ の値を定めます．
4.2.2 同定実験
(a) 準備（極性の調整）
同定実験を行う前に，振子の回転方向が正しいかをチェックします．USB ケーブルでマイコンボードとパソコンを接続してから，以下の Simulink モデル "check_pend.slx" をエクスターナル実行してください．
 |
|---|
| Simulink モデル "check_pend.slx" |
実行後，手動で内側から見て時計回りに振子を回転させたとき，図 $4.3$ に示すように，
- Simulink ブロック "Scope" の表示が正方向に増加
するかどうかを確認してください．
 |
|---|
| 図 $4.3$ 極性の確認 |
もし，これとは逆の動きをする場合は，以下のように設定してください．
- Simulink ブロック “Scope” の表示が負方向に増加した場合は，Simulink ブロック "Gain (pendulum encoder polarity)" の値を 1 から -1 に変更してください．
(b) 自由振動の実験
振子の自由振動を行うための Simulink モデルを以下に示します．
 |
|---|
| Simulink モデル "ex_ident_pend.slx" |
ここで，サブシステム "Pendulum" のなかの Simulink ブロック "Gain (pendulum encoder polarity)" は，極性を調整した Simulink モデル "check_pend.slx" と同じ値に設定しておいてください．
自由振動の実験を行う前に，アームが回転しないように，LEGO 部品を利用するなどして，アームを台座に固定しておいてください．
つぎに，MATLAB のコマンドウィンドウで
>> h = 0.01;
のようにサンプリング周期 $h = 0.01$ $\rm{[s]}$ を設定し，エクスターナル実行を行います．実行開始後，速やかに振子を手で水平付近まで持ち上げて，5 秒以内に静かに振子を離します．実験の様子は以下の動画を参照してください．
実験が終了した後，実験データを "ident_pend_data.mat" という名前の mat ファイルに保存するため，コマンドウィンドウで
>> save ident_pend_data t phi2 h
もしくは
>> save('ident_pend_data','t','phi2','h')
のように入力します．
(c) パラメータ同定
以下の M ファイルを実行してください．
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
その結果，
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 変数 |
|---|---|---|
| 振動周期 $T$ | $6.87 \times 10^{-1}\ {\rm [s]}$ | T |
| 減衰率 $\lambda$ | $8.78 \times 10^{-1}$ | lambda |
| 固有角周波数 $\omega_{\rm n2}$ | $9.15 \times 10^{0}$ | wn2 |
| 減衰係数 $\zeta_2$ | $2.08 \times 10^{-2}$ | zeta2 |
| $\alpha_2$ | $1.17 \times 10^{-1}$ | alpha2 |
| $\beta_2$ | $4.46 \times 10^{-2}$ | beta2 |
M ファイルでは，まず，図 $4.4$ の生データから実験開始後の10 秒間と最後の 8 秒間のデータを削除しています ($t_{\rm min} = 10$, $t_{\rm max} = 22$)．
 |
|---|
| 図 $4.4$ 自由振動の実験結果 (生データ) |
そして，10 秒が 0 秒になるようにデータを変換した後，図 $4.5$ のように 12 秒間の各振動のピークとなる点を抽出します．
 |
|---|
| 図 $4.5$ 自由振動における各振動のピークとなる点 |
これらの点から各振動の振動周期 $T_i$ と減衰率 $\lambda_i$ を算出して，それらの平均も合わせて描画した結果が図 $4.6$, $4.7$ です．
 |
|---|
| 図 $4.6$ 各振動の振動周期 $T_i$ (○ 印) とその平均 $T$ (実線) |
 |
|---|
| 図 $4.7$ 各振動の減衰率 $\lambda_i$ (○ 印) とその平均 $\lambda$ (実線) |
図 $4.6$ からわかるように，振動周期は振動が減衰していくにしたがって徐々に短くなり，一定値に漸近していることがわかります．一方，図 $4.7$ からわかるように，減衰率は振動が減衰していくにしたがって徐々に大きくなっていき，一定値に漸近していきますが，その後，動摩擦や静止摩擦の影響で減衰率が急に減少します．
この同定の方法では，振子が真下近傍（$\phi_2 = 0$）で動作するとして，非線形項を
\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}
のように近似的に線形化した 2 階の線形微分方程式 $(4.3)$ 式に基づいています．したがって，振子角 $\phi_2$ が大きいときは振動周期や減衰率は一定ではなく，振子角 $\phi_2$ が小さくなると一定値に近づくということになります．つまり，この方法を利用して同定するのであれば，初期角度 $\phi_2(0)$ が 30 [deg] 程度であるような実験データを利用した方が良いでしょう．ただし，振子角 $\phi_2$ が小さすぎると，無視した動摩擦や静止摩擦の影響を受ける可能性が高くなるので，注意が必要です．
最後に，同定されたパラメータの値を利用した線形シミュレーションの結果 ($(4.3)$ 式の解軌道である $(4.6)$ 式を描画) と実験結果を比較した結果を示します．
 |
|---|
| 図 $4.8$ 実験結果と線形シミュレーション結果の比較 |
両者を比べると，振動周期や減衰率が平均的に一致していることがわかります．
4.3 最小 2 乗法に基づく方法
4.3.1 パラメータ同定の手順
(a) 基本的な考え方
非線形モデル $(4.2)$ 式を書き換えると，
\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)$ と記述すると，理想的には
\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 乗誤差の総和
\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}
が最小となるように未知パラメータ $\pmb{p}$ の値を定めます．そのためには，$(4.16)$ 式を
\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}
のように書き換え，未知パラメータ $\pmb{p}$ の値を
\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}
により定めれば良いことが知られています．ここで，
\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}$ の擬似逆行列と呼びます．
一見，この方法でうまくいくように思うかもしれませんが，実は，問題があります．いま，ロータリエンコーダで振子の角度 $\phi_2$ を検出し，その実験データが図 $4.9$ であったとします．
 |
|---|
| 図 $4.9$ 振子の角度 $\phi_2$ |
ここで，エンコーダの分解能は $360\ \rm{[PPR]}$ であり，これを $4$ 逓倍でカウントしているので，角度データは $0.25\ \rm{[deg]}$ ごとの離散的な値でしか検出できないことに注意してください．実際の値とこの離散的な値との誤差を，量子化誤差といいますが，この影響で，角速度 $\dot{\phi}_2$ を後退差分近似
\begin{align}
\dot{\phi}_{2}[i] = \dfrac{{\phi}_{2}[i] - {\phi}_{2}[i-1]}{h}
\tag{4.21}
\end{align}
で計算すると，図 $4.10$ のように高周波信号（チャタリング）が発生します．ただし，サンプリング周期は $h = 0.01\ {\rm[s]}$ です．
 |
|---|
| 図 $4.10$ 振子の角速度 $\dot{\phi}_2$ (後退差分近似) |
さらに，角加速度 $\ddot{\phi}_2$ を後退差分近似
\begin{align}
\ddot{\phi}_{2}[i] = \dfrac{\dot{\phi}_{2}[i] - \dot{\phi}_{2}[i-1]}{h}
\tag{4.22}
\end{align}
で計算すると，図 $4.11$ のように激しいチャタリングを生じてしまいます．
 |
|---|
| 図 $4.11$ 振子の角加速度 $\ddot{\phi}_2$ (後退差分近似) |
このように，チャタリングの影響が無視できないほど大きい場合，最小 2 乗法を利用して同定しても，うまくいきません．
(b) チャタリング対策
最小 2 乗法を利用する場合，チャタリングの影響を小さくするような信号処理が必要です．
まず，差分近似の方法を改善してみましょう．さきほどの例では，後退差分近似により角速度 $\dot{\phi}_2$ や角加速度 $\ddot{\phi}_2$ を算出しました．PID 制御を行っているときなどは，オンライン（リアルタイム）で微分信号を算出する必要があるので，現在と過去の角度データから角速度を算出する後退差分近似がよく利用されます．
しかし，パラメータ同定を行うのはオフラインの処理です．そのため，現在の角速度を算出する際に，過去の角度データだけでなく，未来の角度データも利用できます．このことを考慮して，中心差分近似
\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$ です．
 |
|---|
| 図 $4.12$ 振子の角速度 $\dot{\phi}_2$（後退差分近似と中心差分近似の比較） |
 |
|---|
| 図 $4.13$ 振子の角加速度 $\ddot{\phi}_2$（後退差分近似と中心差分近似の比較） |
これらより，チャタリングが軽減されていることが確認できます．
しかし，振子の角加速度 $\ddot{\phi}_2$ にはまだチャタリングが残っています．さらなるチャタリング除去のために，ローパスフィルタを使用してみましょう．ここで注意することは，
- 非線形モデル $(4.15)$ 式に含まれるすべての信号を，同じローパスフィルタに通過させる
という点です．これは，ある信号をローパスフィルタ (LPF) に通過させると，高周波成分の振幅は小さくなりますが，必要な低周波成分の位相が遅れてしまうことに対処するためです．とくに，強いローパスフィルタを使用する場合はこの位相の遅れが顕著に現れ，振幅も少し小さくなってしまいます．それに対して，すべての信号 $\pmb{M}(t)$, $N(t)$ を同じローパスフィルタに通過させると，
- 不必要な高周波成分は，除去される
- 必要な低周波成分は，同じだけ振幅が小さくなり，同じだけ位相が遅れる
ので，等式関係が成立し，位相のずれは問題となりません．
具体的には，$(4.15)$ 式の両辺の信号を 3 次のローパスフィルタ
\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}
に通過させ，
\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$ は微分演算子であり，
\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$ としてチャタリング除去の処理を行った結果を以下に示します．
 |
|---|
| 図 $4.14$ 振子の角速度 $M_2=\dot\phi_2$ と LPF 通過後の $M_{\rm{f2}}$ |
 |
|---|
| 図 $4.15$ 振子の角加速度 $M_1=\ddot\phi_2$ と LPF 通過後の $M_{\rm{f1}}$ |
 |
|---|
| 図 $4.16$ 振子の重力項 $N = -g\sin\phi_2$ と LPF 通過後の $N_{\rm{f}}$ |
図 $4.14$ $\sim$ $4.16$ の結果から
- チャタリングが十分に除去されている
- 位相が同程度に遅れている
- 振幅が同程度に小さくなっている
ことが確認できます．
最後に，パラメータ同定を行うための式を導出します．先と同様の議論により，理想的には
\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}
が成り立ちます．したがって，未知パラメータ $\pmb{p}$ の値を
\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}
により定めれば良いことがわかります．
4.3.2 同定実験
4.2.2 (b) で取得した自由振動の実験データを利用して，最小 2 乗法により振子のパラメータ同定を行う M ファイルを以下に示します．
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" を実行すると，
M ファイル "ident_pend_lsm.m" の実行結果
>> ident_pend_lsm
alpha2 = 1.10e-01
beta2 = 3.59e-02
という結果が得られます．
| パラメータ | 値 | MATLAB 変数 |
|---|---|---|
| $\alpha_2$ | $1.10 \times 10^{-1}$ | alpha2 |
| $\beta_2$ | $3.59 \times 10^{-2}$ | beta2 |
そして，図 $4.14$ $\sim$ $4.16$ の結果が描画されます．
 |
|---|
| 図 $4.17$ 振子の角加速度 $M_1=\ddot\phi_2$ と LPF 通過後の $M_{\rm{f1}}$ |
 |
|---|
| 図 $4.18$ 振子の角速度 $M_2=\dot\phi_2$ と LPF 通過後の $M_{\rm{f2}}$ |
 |
|---|
| 図 $4.19$ 振子の重力項 $N = -g\sin\phi_2$ と LPF 通過後の $N_{\rm{f}}$ |
さらに，M ファイル "ident_pend_lsm.m" のなかでは，同定されたパラメータを用いて
 |
|---|
| Simulink ブロック "Fcn" についての補足 |
| Simulink モデル "sim_nonlinear_pend.slx" |
を実行した非線形シミュレーションを実行しており，実験結果と非線形シミュレーションの結果を比較した図 $4.20$ のグラフが描画されます．これより，両者がおおよそ一致していることが確認できます．
 |
|---|
| 図 $4.20$ 実験結果と線形シミュレーションの結果 |
5. 振子が静止してしまう場合の対処方法について
振子を取り付けているロータリエンコーダとしては，比較的，動摩擦や静止摩擦が小さいものを使用しています．しかし，振子が軽量であるために，エンコーダ軸の動摩擦や静止摩擦の影響で，振子を自由振動させたときに真下 ($\phi_2 = 0$) ですぐに静止してしまうことがあります (動摩擦や静止摩擦の影響の大きさはエンコーダの個体差に依存します)．このような場合，振子を手動で真上に持っていったときに，静止摩擦により簡単に倒立点で静止してしまい，倒立振子としてはおもしろくありません．
この問題に対処したい場合は，振子の先端におもりとして LEGO 部品 (ダブルべベルギア 12歯，デザイン ID：32270) を取り付けてください．
(1) おもりを取り付けていない場合
パラメータ 値 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}$ alpha1 $\beta_1$ $7.06 \times 10^{1}$ beta1 $\alpha_2$ $1.10 \times 10^{-1}$ alpha2 $\beta_2$ $3.59 \times 10^{-2}$ beta2
(2) おもりを 1 個取り付けた場合
パラメータ 値 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}$ alpha1 $\beta_1$ $7.06 \times 10^{1}$ beta1 $\alpha_2$ $1.23 \times 10^{-1}$ alpha2 $\beta_2$ $3.04 \times 10^{-2}$ beta2
(3) おもりを 2 個取り付けた場合
パラメータ 値 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}$ alpha1 $\beta_1$ $7.06 \times 10^{1}$ beta1 $\alpha_2$ $1.30 \times 10^{-1}$ alpha2 $\beta_2$ $2.70 \times 10^{-2}$ beta2
6. おわりに
今回は，回転型倒立振子の未知パラメータを同定する手順について説明をしました．パラメータ同定の作業がうまくいけば，モデルに基づいた制御系設計を行うための見通しが良くなります．
次回は，コントローラ設計について説明します．
（終わり）
参考文献
-
大須賀 公一：メカニカルシステムの同定（センサの動特性を考慮した同定法とその検証実験），計測と制御，Vol.33, No.6, pp.487-493 (1994)
$\cdots\cdots$ https://doi.org/10.11499/sicejl1962.33.6_487 -
川田昌克（編著），東 俊一，市原裕之，浦久保孝光，大塚敏之，甲斐健也，國松禎明，澤田賢治，永原正章，南 裕樹： 倒立振子で学ぶ制御工学， 森北出版 (2017.2.28)
$\Longrightarrow$ サポートページはこちら -
計測自動制御学会編：ロボット制御の実際 第 3 章「ロボットの同定」，コロナ社(1997)