制御系CAD

制御を意識したモノつくりを目指して

ドローン

投稿日時: 2021年10月9日投稿者: cacsd

上図のようなドローンについて、次の文献をフォローしていきます。

Randal Beard: Quadrotor Dynamics and Control, Rev0.1, Brigham Young University, 2008

以下では、 $\sin(\cdot)$ 、 $\cos(\cdot)$ をそれぞれ $S_{(\cdot)}$ 、 $C_{(\cdot)}$ と略記します。

準備

●上図のように、ベクトル ${\bf p}$ を、ベクトル $\hat{n}$ のまわりに $\mu$ だけ回転して得られるベクトル ${\bf q}$ の表現式を求めます。

$\displaystyle{{\bf q}=\vec{ON}+\vec{NW}+\vec{WQ}}$
ただし
$\displaystyle{\vec{ON}=({\bf p}\cdot \hat{n}) \hat{n} }$
$\displaystyle{\vec{NW}=\frac{ {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} }{ || {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} ||}\overline{NQ}\,C_\mu =({\bf p}-({\bf p}\cdot \hat{n}) \hat{n})\, C_\mu \quad(\overline{NQ}=|| {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} ||)}$
$\displaystyle{\vec{WQ}=\frac{{\bf p}\times\hat{n}}{||{\bf p}||S_\phi}\overline{NQ}\,S_\mu=-(\hat{n}\times{\bf p})\,S_\mu \quad(\overline{NQ}=||{\bf p}||S_\phi)}$

から、次式を得ます。

$\displaystyle{{\bf q}=(1-C_\mu)({\bf p}\cdot \hat{n}) \hat{n}+C_\mu{\bf p} -S_\mu(\hat{n}\times{\bf p})}$

ここで

$\displaystyle{{\bf p}=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf q}=q_x^0\hat{i}^0+q_y^0\hat{j}^0+q_z^0\hat{k}^0}$
$\displaystyle{\hat{n}=\hat{n}_x \hat{i}^0+\hat{n}_y \hat{j}^0+\hat{n}_z \hat{k}^0}$

とおくと

$\displaystyle{{\bf p}\cdot \hat{n}=p_x^0\hat{n}_x+p_y^0\hat{n}_y+p_z^0\hat{n}_z }$
$\displaystyle{\hat{n}\times{\bf p}= {\rm det} \left[\begin{array}{ccc} \hat{i}^0 & \hat{j}^0 & \hat{k}^0\\ \hat{n}_x & \hat{n}_y & \hat{n}_z\\ p_x^0 & p_y^0 & p_z^0 \end{array}\right]}$
$\displaystyle{=(\hat{n}_yp_z^0-\hat{n}_zp_y^0)\hat{i}^0+(\hat{n}_zp_x^0-\hat{n}_xp_z^0)\hat{j}^0+(\hat{n}_xp_y^0-\hat{n}_yp_x^0)\hat{k}^0 }$

● $\hat{n}=\hat{i}$ の場合（left-handed rotation about z-axis）

$\displaystyle{{\bf q}=(1-C_\psi)({\bf p}\cdot \hat{n}) \hat{n}+C_\psi{\bf p} -S_\psi(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\psi) p_z^0 \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] }_{\hat{i}} \hat{n}+C_\psi \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\psi \left[\begin{array}{c} -p_y^0 \\ p_x^0 \\ 0 \end{array}\right] }$
$\displaystyle{ = \underbrace{ \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }_{R(\phi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

● $\hat{n}=\hat{j}$ の場合（left-handed rotation about y-axis)

$\displaystyle{{\bf q}=(1-C_\theta)({\bf p}\cdot \hat{n}) \hat{n}+C_\theta{\bf p} -S_\theta(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\theta) p_y^0 \underbrace{ \left[\begin{array}{c} 0 \\ 1 \\ 0 \end{array}\right] }_{\hat{j}} \hat{n}+C_\theta \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\theta \left[\begin{array}{c} p_z^0 \\ 0\\ -p_x^0 \\ \end{array}\right] }$
$\displaystyle{ =\underbrace{ \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }_{R(\theta)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

● $\hat{n}=\hat{k}$ の場合（left-handed rotation about x-axis）

$\displaystyle{{\bf q}=(1-C_\phi)({\bf p}\cdot \hat{n}) \hat{n}+C_\phi{\bf p} -S_\phi(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\phi) p_x^0 \underbrace{ \left[\begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right] }_{\hat{k}} \hat{n}+C_\phi \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\phi \left[\begin{array}{c} 0\\ -p_z^0 \\ p_y^0 \\ \end{array}\right] }$
$\displaystyle{ =\underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }_{R(\psi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

以上のようなベクトルの回転は、相対的に座標軸を回転させても、把握できます。

●right-handed rotation about z-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\psi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\psi)= \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$

●right-handed rotation about y-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\theta)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\theta)= \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }$

●right-handed rotation about x-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\phi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\phi)= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }$

座標系

●ドローンの運動の記述にあたっては、次のような地上座標系 ${\cal F}^i$ と機体固定座標系 ${\cal F}^b$ を用います。

${\cal F}^i$ における機体の位置は次の変数で表します。

$p_n$ : the inertial (north) position of the aircraft along $\hat{i}^i$ in ${\cal F}^i$
$p_e$ : the inertial (east) position of the aircraft along $\hat{j}^i$ in ${\cal F}^i$
$h$ : the altitude of the aircraft measured along $-\hat{k}^i$ in ${\cal F}^i$

また、 ${\cal F}^b$ は、 ${\cal F}^i$ から、次のオイラー角

$\phi$ : the roll angle defined with respect to ${\cal F}^{v2}$
$\theta$ : the pitch angle defined with respect to ${\cal F}^{v1}$
$\psi$ : the yaw angle defined with respect to ${\cal F}^{v}$

を用いて、次のように逐次回転して得られます。

$\displaystyle{{\cal F}^i\rightarrow{\cal F}^v\rightarrow{\cal F}^{v1}\rightarrow{\cal F}^{v2}\rightarrow{\cal F}^b$

ここで、 ${\cal F}^{v}$ は原点を機体重心にもつ回転前の座標系（ ${\cal F}^{i}$ に平行）を表しています。

● ${\cal F}^b$ における任意ベクトル ${\bf p}$ （後出の位置ベクトル、速度ベクトルなど）は、 ${\cal F}^{i}$ から、次のような変換を行って得られます。

$\displaystyle{ {\bf p}^b=R_v^b(\phi,\theta,\psi){\bf p}^v }$
$\displaystyle{ R_v^b(\phi,\theta,\psi)=R_{v2}^b(\phi) R_{v1}^{v2}(\theta) R_v^{v1}(\psi) }$
$\displaystyle{= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$
$\displaystyle{ = \left[\begin{array}{ccc} C_\theta C_\psi & C_\theta S_\psi & -S_\theta \\ S_\phi S_\theta C_\psi - C_\phi S_\psi & S_\phi S_\theta S_\psi + C_\phi C_\psi & S_\phi C_\theta \\ C_\phi S_\theta C_\psi + C_\phi S_\psi & C_\phi S_\theta S_\psi - S_\phi C_\psi & C_\phi C_\theta \end{array}\right] }$

● $\displaystyle{{\cal F}^v\rightarrow{\cal F}^{v1}$
$\displaystyle{ {\bf p}^{v1}=R_v^{v1}(\psi){\bf p}^v }$
$\displaystyle{ R_v^{v1}(\psi)= \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$

● $\displaystyle{{\cal F}^{v1}\rightarrow{\cal F}^{v2}$
$\displaystyle{ {\bf p}^{v2}=R_{v1}^{v2}(\theta){\bf p}^{v1} }$
$\displaystyle{ R_{v1}^{v2}(\theta)= \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }$

● $\displaystyle{{\cal F}^{v2}\rightarrow{\cal F}^b$
$\displaystyle{ {\bf p}^{b}=R_{v2}^b(\phi){\bf p}^{v2} }$
$\displaystyle{ R_{v2}^b(\phi)= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }$

● ${\cal F}^b$ における任意ベクトル ${\bf p}$ （後出の位置ベクトル、速度ベクトルなど）の時間変化がどのように表されるかを調べます。

これは、次式で表されます。

$\displaystyle{\frac{d}{dt}_i{\bf p}=\frac{d}{dt}_b{\bf p}+\omega_{b/i}\times{\bf p} }$

実際

$\displaystyle{{\bf p}+\delta{\bf p}=(1-C_{(-\delta \phi)})({\bf p}\cdot \hat{s}) \hat{s}+C_{(-\delta \phi)}{\bf p} -S_{(-\delta \phi)}(\hat{s}\times{\bf p}) }$
$\displaystyle{\delta{\bf p}\simeq \delta \phi (\hat{s}\times{\bf p}) \Rightarrow \frac{\delta{\bf p}}{\delta t}\simeq\frac{\delta \phi}{\delta t} (\hat{s}\times{\bf p}) }$
$\qquad\Downarrow$
$\displaystyle{\frac{d}{dt}_i{\bf p}\simeq \underbrace{\dot{\phi} \hat{s}}_{\omega_{b/i}}\times{\bf p} }$

運動方程式

${\cal F}^b$ における運動は次の変数を用いて表されます。

$u$ : the body frame velocity measured along $\hat{i}^b$ in ${\cal F}^b$
$v$ : the body frame velocity measured along $\hat{j}^b$ in ${\cal F}^b$
$w$ : the body frame velocity measured along $\hat{k}^b$ in ${\cal F}^b$
$p$ : the roll rate measured along $\hat{i}^b$ in ${\cal F}^b$
$q$ : the pitch rate measured along $\hat{j}^b$ in ${\cal F}^b$
$r$ : the yaw rate measured along $\hat{k}^b$ in ${\cal F}^b$

これらが ${\cal F}^i$ からどう表されるか、すなわち次の変数の微分とどう関係するか（キネマティックス）を調べます。

$p_n$ : the inertial (north) position of the quadrotor along $\hat{i}^i$ in ${\cal F}^i$
$p_e$ : the inertial (east) position of the quadrotor along $\hat{j}^i$ in ${\cal F}^i$
$h$ : the altitude of the aircraft measured along $-\hat{k}^i$ in ${\cal F}^i$
$\phi$ : the roll angle defined with respect to ${\cal F}^{v2}$
$\theta$ : the pitch angle defined with respect to ${\cal F}^{v1}$
$\psi$ : the yaw angle defined with respect to ${\cal F}^{v}$

● ${\cal F}^i$ と ${\cal F}^b$ における速度ベクトルについては

$\displaystyle{ \left[\begin{array}{c} u \\ v \\ w \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} C_\theta C_\psi & C_\theta S_\psi & -S_\theta \\ S_\phi S_\theta C_\psi - C_\phi S_\psi & S_\phi S_\theta S_\psi + C_\phi C_\psi & S_\phi C_\theta \\ C_\phi S_\theta C_\psi + C_\phi S_\psi & C_\phi S_\theta S_\psi - S_\phi C_\psi & C_\phi C_\theta \end{array}\right] }_{R_v^b} \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \end{array}\right] }$

の関係が成り立ちます。したがって、

$\displaystyle{ \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} C_\theta C_\psi &S_\phi S_\theta C_\psi - C_\phi S_\psi &C_\phi S_\theta C_\psi + C_\phi S_\psi \\ C_\theta S_\psi &S_\phi S_\theta S_\psi + C_\phi C_\psi &C_\phi S_\theta S_\psi - S_\phi C_\psi \\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta \end{array}\right] }_{R_b^v=(R_v^b)^T} \left[\begin{array}{c} u \\ v \\ w \end{array}\right] }$

● ${\cal F}^i$ と ${\cal F}^b$ における角速度ベクトルについては

$\displaystyle{ \left[\begin{array}{c} p \\ q \\ r \end{array}\right] = \underbrace{R_{v2}^b(\dot{\phi})}_{I} \left[\begin{array}{c} \dot{\phi} \\ 0 \\ 0 \end{array}\right] + R_{v2}^b(\phi)\underbrace{R_{v1}^{v2}(\dot{\theta})}_{I} \left[\begin{array}{c} 0 \\ \dot{\theta} \\ 0 \end{array}\right] + R_{v2}^b(\phi)R_{v1}^{v2}(\theta)\underbrace{R_{v}^{v1}(\dot{\psi})}_{I} \left[\begin{array}{c} 0 \\ 0 \\ \dot{\psi} \end{array}\right] }$
$\displaystyle{ = \left[\begin{array}{c} \dot{\phi} \\ 0 \\ 0 \end{array}\right] + \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{c} 0 \\ \dot{\theta} \\ 0 \end{array}\right] + \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] \left[\begin{array}{c} 0 \\ 0 \\ \dot{\psi} \end{array}\right] }$

の関係が成り立ちます。したがって、

$\displaystyle{ \left[\begin{array}{c} p \\ q \\ r \end{array}\right]= \left[\begin{array}{ccc} 1 & 0 & -S_\theta \\ 0 & C_\phi & S_\phi C_\theta \\ 0 & -S_\phi & C_\phi C_\theta \end{array}\right] \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] }$

すなわち、次の関係式を得ます。

●次に並進運動（ダイナミックス）は次式で表されます。

$\displaystyle{m\frac{d}{dt}_i{\bf v}=\frac{d}{dt}_b{\bf v}+\omega_{b/i}\times{\bf v}=\frac{1}{m}{\bf f} }$
$\displaystyle{ \frac{d}{dt}_b \left[\begin{array}{c} u\\ v\\ w \end{array}\right] + \left[\begin{array}{c} p\\ q\\ r \end{array}\right] \times \left[\begin{array}{c} u\\ v\\ w \end{array}\right] =\frac{1}{m} \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] }$

$\displaystyle{ \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] =- \underbrace{ \left[\begin{array}{ccc} 0 & -r & q\\ r & 0 &-p\\ -q & p & 0 \end{array}\right] \left[\begin{array}{c} u\\ v\\ w \end{array}\right] }_{\left[\begin{array}{c} qw-rv \\ ru-pw\\ pv-qu \end{array}\right]} +\frac{1}{m} \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] }$

●また回転運動（ダイナミックス）は次式で表されます。

$\displaystyle{\frac{d}{dt}_i{\bf h}=\frac{d}{dt}_b{\bf h}+\omega_{b/i}\times{\bf h}={\bf m} }$
$\displaystyle{{\bf h}=J\omega_{b/i} }$
$\displaystyle{J= \left[\begin{array}{ccc} \int(y^2+z^2)dm & -\int xydm & -\int xzdm \\ -\int yxdm & \int(x^2+z^2)dm & -\int yzdm \\ -\int zxdm & -\int zydm & \int(x^2+y^2)dm \end{array}\right] =\left[\begin{array}{ccc} J_x & 0 & 0 \\ 0 & J_y & 0 \\ 0 & 0 & J_z \end{array}\right] }$

$\displaystyle{ \frac{d}{dt}_bJ \left[\begin{array}{c} p\\ q\\ r \end{array}\right] + \left[\begin{array}{c} p\\ q\\ r \end{array}\right] \times J \left[\begin{array}{c} p\\ q\\ r \end{array}\right] = \left[\begin{array}{c} \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} J_x\dot{p}\\ J_y\dot{q}\\ J_z\dot{r} \end{array}\right] + \underbrace{ \left[\begin{array}{ccc} 0 & -r & q\\ r & 0 &-p\\ -q & p & 0 \end{array}\right] \left[\begin{array}{c} J_xp\\ J_yq\\ J_zr \end{array}\right] }_{\left[\begin{array}{c} (J_z-J_y)qr \\ (J_x-J_z)pr\\ (J_y-J_x)pq \end{array}\right]} = \left[\begin{array}{c} \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] }$

$\displaystyle{ \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] =- \underbrace{ \left[\begin{array}{ccc} 0 & 0 & \frac{J_z-J_y}{J_x}q\\ \frac{J_x-J_z}{J_y} r & 0 & 0\\ 0 & \frac{J_y-J_x}{J_z}p & 0 \end{array}\right] \left[\begin{array}{c} p\\ q\\ r \end{array}\right] }_{ \left[\begin{array}{c} \frac{J_z-J_y}{J_x}qr \\ \frac{J_x-J_z}{J_y}pr \\ \frac{J_y-J_x}{J_z}pq \end{array}\right] } + \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

外力

４つのモータへの回転指令を $\delta_f,\delta_r,\delta_b,\delta_\ell$ とします。このとき、各モータによる推力とトルクは次式で表されます。

$\displaystyle{F_f=k_1\delta_f}$
$\displaystyle{F_r=k_1\delta_r}$
$\displaystyle{F_b=k_1\delta_b}$
$\displaystyle{F_\ell=k_1\delta_\ell}$

$\displaystyle{\tau_f=k_2\delta_f}$
$\displaystyle{\tau_r=k_2\delta_r}$
$\displaystyle{\tau_b=k_2\delta_b}$
$\displaystyle{\tau_\ell=k_2\delta_\ell}$

また、４つのモータの合推力と各軸回りのトルクは次式で表されます。

$\displaystyle{F=F_f+F_r+F_b+F_\ell}$
$\displaystyle{\tau_\phi=\ell(F_\ell-F_r)}$
$\displaystyle{\tau_\theta=\ell(F_f-F_b)}$
$\displaystyle{\tau_\psi=\tau_r+\tau_\ell-\tau_f-\tau_b}$

すなわち

$\displaystyle{ \left[\begin{array}{c} F\\ \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -\ell k_1 & 0 & \ell k_1 \\ \ell k_1 & 0 & -\ell k_1 & 0 \\ -k_2 & k_2 & -k_2 & k_2 \end{array}\right] }_{{\cal M}} \left[\begin{array}{c} \delta_f\\ \delta_r\\ \delta_b\\ \delta_\ell \end{array}\right] }$

また、並進運動方程式の外力項は次式となります。

$\displaystyle{ \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] = R_v^b \left[\begin{array}{c} 0\\ 0\\ mg \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right] =mg \left[\begin{array}{ccc} -S_\theta \\ S_\phi C_\theta \\ C_\phi C_\theta \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right] }$

このときドローンの運動方程式に基づく状態方程式は次式となります。

$\displaystyle{ \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \\ \hline \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\ \hline \dot{u}\\ \dot{v}\\ \dot{w} \\ \hline \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] = \left[\begin{array}{ccc|ccc} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array}\right.}$
$\displaystyle{ \left.\begin{array}{ccc|ccc} C_\theta C_\psi &S_\phi S_\theta C_\psi - C_\phi S_\psi &C_\phi S_\theta C_\psi + C_\phi S_\psi & 0 & 0 & 0\\ C_\theta S_\psi &S_\phi S_\theta S_\psi + C_\phi C_\psi &C_\phi S_\theta S_\psi - S_\phi C_\psi & 0 & 0 & 0\\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta & 0 & 0 & 0\\ \hline 0 & 0 & 0 & 1 & S_\phi T_\theta & C_\phi T_\theta \\ 0 & 0 & 0 & 0 & C_\phi & -S_\phi \\ 0 & 0 & 0 & 0 & \frac{S_\phi}{C_\theta} & \frac{C_\phi}{C_\theta}\\ \hline 0 & r &- q & 0 & 0 & 0\\ r- & 0 &p & 0 & 0 & 0\\ q & -p & 0 & 0 & 0 & 0\\ \hline 0 & 0 & 0 & 0 & 0 & -\frac{J_z-J_y}{J_x}q\\ 0 & 0 & 0 & -\frac{J_x-J_z}{J_y} r & 0 & 0\\ 0 & 0 & 0 & 0 & -\frac{J_y-J_x}{J_z}p & 0 \end{array}\right] }$
$\displaystyle{ \times \left[\begin{array}{c} p_n \\ p_e \\ -h \\ \hline \phi \\ \theta \\ \psi \\ \hline u\\ v\\ w \\ \hline p\\ q\\ r \end{array}\right] + \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{1}{m} & 0 & 0 & 0 \\ \hline 0 & \frac{1}{J_x} & 0 & 0 \\ 0 & 0 & \frac{1}{J_y} & 0 \\ 0 & 0 & 0 & \frac{1}{J_z} \\ \end{array}\right] \left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -\ell k_1 & 0 & \ell k_1 \\ \ell k_1 & 0 & -\ell k_1 & 0 \\ -k_2 & k_2 & -k_2 & k_2 \end{array}\right]^{-1} \left[\begin{array}{c} \delta_f\\ \delta_r\\ \delta_b\\ \delta_\ell \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ \hline 0 \\ 0 \\ 0 \\ \hline -gS_\theta \\ gS_\phi C_\theta \\ gC_\phi C_\theta \\ \hline 0 \\ 0 \\ 0 \end{array}\right] }$

これを見ると、 $A$ 行列に状態変数が絡んでおり、複雑なダイナミクスをもっていることが分かります。

運動方程式の簡単化

次の仮定のもとで、運動方程式の簡単化を行ないます。

仮定１：　 $qr$ 、 $pr$ 、 $pq$ は小さく無視できる
仮定２：　 $\phi$ 、 $\theta$ は小さく無視できる

まず、仮定１のもとで、次の近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] =- \left[\begin{array}{c} \frac{J_z-J_y}{J_x}qr \\ \frac{J_x-J_z}{J_y}pr \\ \frac{J_y-J_x}{J_z}pq \end{array}\right] + \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] \simeq \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

このとき、仮定２をおけば次の近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] = \left[\begin{array}{ccc} 1 & S_\phi T_\theta & C_\phi T_\theta \\ 0 & C_\phi & -S_\phi \\ 0 & \frac{S_\phi}{C_\theta} & \frac{C_\phi}{C_\theta} \end{array}\right] \left[\begin{array}{c} p \\ q \\ r \end{array}\right] \simeq \left[\begin{array}{c} p \\ q \\ r \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \end{array}\right] \simeq \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] \simeq \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

これは、機体の傾きを独立して制御できることを示しています。

さらに次のような近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{p}_x \\ \dot{p}_y \\ \dot{p}_z \end{array}\right] = {R}_{v1}^b \left[\begin{array}{c} u \\ v \\ w \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{p}_x \\ \ddot{p}_y \\ \ddot{p}_z \end{array}\right] = \underbrace{\dot{R}_{v1}^b}_{\simeq0} \left[\begin{array}{c} {u}\\ {v}\\ {w} \end{array}\right] + {R}_{v1}^b \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] \simeq {R}_{v1}^b \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{p}_x \\ \ddot{p}_y \\ \ddot{p}_z \end{array}\right] \simeq \left[\begin{array}{ccc} C_\theta & S_\phi S_\theta & C_\phi S_\theta \\ 0 & C_\phi & -S_\phi \\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta \end{array}\right] (g \left[\begin{array}{c} -S_\theta \\ S_\phi C_\theta \\ C_\phi C_\theta \end{array}\right] +\frac{1}{m} \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right]) }$
$\displaystyle{ = \left[\begin{array}{c} 0 \\ 0 \\ g \end{array}\right] -\frac{1}{m}F \left[\begin{array}{ccc} C_\phi S_\theta \\ -S_\phi \\ C_\phi C_\theta \end{array}\right] \simeq \left[\begin{array}{c} 0 \\ 0 \\ g-\frac{1}{m}F \end{array}\right] }$

結局、ドローンの運動方程式は、次式のように簡単化されます。

$\displaystyle{m\ddot{p}_z=mg-(F_f+F_r+F_b+F_\ell)}$
$\displaystyle{J_x\ddot{\phi}=\ell(F_\ell-F_r)}$
$\displaystyle{J_y\ddot{\theta}=\ell(F_f-F_b)}$
$\displaystyle{J_z\ddot{\psi}=\tau_r+\tau_\ell-\tau_f-\tau_b}$

いま，重力補償のために $mg=4k_f\delta^*$ から決まる $\delta^*$ を各操作入力に前もって加えておくことにすると、次式が成り立ちます。

$\displaystyle{m\ddot{p}_z=4k_f\delta^*-(k_f\delta_f+k_f\delta_r+k_f\delta_b+k_f\delta_\ell)}$
$\displaystyle{=-k_f(\delta_f-\delta^*)-k_f(\delta_r-\delta^*)-k_f(\delta_b-\delta^*)-k_f(\delta_\ell-\delta^*))}$

$\displaystyle{J_x\ddot{\phi}=\ell(k_f\delta_\ell-k_f\delta_r) =\ell(k_f(\delta_\ell-\delta^*)-k_f(\delta_r-\delta^*))}$

$\displaystyle{J_y\ddot{\theta}=\ell(k_f\delta_f-k_f\delta_b)=\ell(k_f(\delta_f-\delta^*)-k_f(\delta_b-\delta^*))}$

$\displaystyle{J_z\ddot{\psi}=(k_\tau\delta_r+k_\tau\delta_\ell)-(k_\tau\delta_f+k_\tau\delta_b)}$
$\displaystyle{=(k_\tau(\delta_r-\delta^*)+k_\tau(\delta_\ell-\delta^*))-(k_\tau(\delta_f-\delta^*)+k_\tau(\delta_b-\delta^*))}$

このときドローンの線形状態方程式は次式となります。

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{p}_z \\ \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\\hline \ddot{z} \\ \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \\ \end{array}\right] }_{\dot{x}} = \underbrace{ \left[\begin{array}{cccc|cccc} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\\hline 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array}\right] }_{A} % \left[\begin{array}{c|c} % 0_{4\times4} & I_4 \\\hline % 0_{4\times4} & 0_{4\times4} % \end{array}\right] \underbrace{ \left[\begin{array}{c} {p_z} \\ {\phi} \\ {\theta} \\ {\psi} \\\hline \dot{z} \\ \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\ \end{array}\right] }_{x} }$
$\displaystyle{+ \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\\hline -\frac{k_f}{m} & -\frac{k_f}{m} & -\frac{k_f}{m} & -\frac{k_f}{m} \\ 0 & -\frac{\ell k_f}{J_x} & 0 & \frac{\ell k_f}{J_x} \\ \frac{\ell k_f}{J_y} & 0 & -\frac{\ell k_f}{J_y} & 0 \\ -\frac{k_\tau}{J_z} & \frac{k_\tau}{J_z} & -\frac{k_\tau}{J_z} & \frac{k_\tau}{J_z} \\ \end{array}\right] }_{B} % \left[\begin{array}{c} % 0_{4\times4} \\ % B_2 \\ % \end{array}\right] \underbrace{ \left[\begin{array}{c} \delta_f-\delta^* \\ \delta_r-\delta^* \\ \delta_b-\delta^* \\ \delta_\ell-\delta^* \\ \end{array}\right] }_{u} }$

倒立振子

投稿日時: 2021年10月9日投稿者: cacsd

倒立振子…Homework

倒立振子は制御なしには平衡状態を保てないので、制御技術の有効性を示すのに、よく研究されています。次図は様々な倒立振子を示しています。

図1 様々な倒立振子

これらの倒立振子について、まずは非線形シミュレータを開発する必要があります。以下に、要点だけを示しておきます。いずれも振子の角度は上向きの鉛直線から時計回りを正としています。

[1] PEND
$1^\circ$ 運動方程式の導出（数式処理の利用）
$\displaystyle{(1.1)\quad \ddot{\theta}=\frac{3g}{4\ell}\sin\theta }$
$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(1.2a)\quad \theta^*=0}$
$\displaystyle{(1.2b)\quad \theta^*=\pi}$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(1.3)\quad \frac{d}{dt} \left[\begin{array}{l} \theta \\ \dot{\theta} \end{array}\right] = \left[\begin{array}{l} \dot{\theta} \\ \frac{3g}{4\ell}\sin\theta \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
$\displaystyle{(1.5)\quad \frac{d}{dt}\left[\begin{array}{c} \theta-\theta^*\\ \dot{\theta} \end{array}\right] = \left[\begin{array}{cc} 0 & 1\\ \frac{3g\cos\theta^*}{4\ell} & 0 \end{array}\right] \left[\begin{array}{cc} \theta-\theta^*\\ \dot{\theta} \end{array}\right] }$
$6^\circ$ 線形シミュレータの開発

MAXIMA

/*pend.wxm*/
dth:'diff(th(t),t);
ddth:'diff(th(t),t,2);
x:ell*sin(th(t));
y:ell*cos(th(t));
J:(1/3)*m*ell^2;
T:(1/2)*m*(diff(x,t)^2+diff(y,t)^2)+(1/2)*J*dth^2;
U:m*g*y;
L:T-U;
LE:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce;
sol:solve(LE,ddth);
f:matrix([dth],[rhs(sol[1])]);
a21:diff(f[2,1],th(t)); a22:diff(f[2,1],dth); 
A:matrix([0,1],[a21,a22]);

Simulink

%sPEND.m
%-----
 function [sys,x0]=sPEND(t,state,input,flag)
%-----
 global g ell th0 
%-----
 if abs(flag)==1
  th=state(1);
  dth=state(2);
  ddth=3*g/(4*ell)*sin(th);
  sys=[dth;ddth];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[2;0;2;0;0;0];
  x0=[th0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lPEND.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 th0=0 %input('th(0) = <0,180> ')/180*pi;
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[th0;0];
 us=[];
 sim("nPEND_obj")
%----- 線形化
 [A0,B0,C0,D0]=linmod('ePEND_linmod',xs,us) 
%
 A=[0 1;0 0];
 A(2,1)=3*g/(4*ell)*cos(th0);
%----- 線形応答と非線形応答の比較
 th0=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 sim("nPEND_comp")  
%-----
%end

[2] CIP
$1^\circ$ 運動方程式の導出（数式処理の利用）
$(2.1)\quad \begin{array}{l} \displaystyle{\underbrace{ \left[\begin{array}{ccc} M+m & m\ell\cos\theta \\ m\ell\cos\theta & \frac{4}{3}m\ell^2 \end{array}\right] }_{M(\xi_1)} \underbrace{ \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta} \end{array}\right] }_{\dot{\xi}_2}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 & -m\ell\sin\theta\dot{\theta} \\ 0 & 0 \end{array}\right] }_{C(\xi_1,\xi_2)} \underbrace{ \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] }_{\dot{\xi}_1}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0\\ -m\ell g\sin\theta \end{array}\right] }_{G(\xi_1)} = \underbrace{ \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F}_{\tilde{F}}} \end{array}$
$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(2.2a)\quad \theta^*=0}$
$\displaystyle{(2.2b)\quad \theta^*=\pi}$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(2.3)\quad \frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\tilde{F}-C(\xi_1,\xi_2)\dot{\xi}_1}-G(\xi_1)) \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
平衡状態(2.2a)：
$\displaystyle{(2.5)\quad \frac{d}{dt}\left[\begin{array}{c} r\\ \theta-\theta^*\\ \dot{r}\\ \dot{\theta} \end{array}\right] = \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{3gm}{4M+m} & 0 & 0\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r\\ \theta-\theta^*\\ \dot{r}\\ \dot{\theta} \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\ \frac{4}{4M+m}\\ \frac{3}{(4M+m)\ell} \end{array}\right] F }$
$6^\circ$ 線形シミュレータの開発

MAXIMA

/*cip.wxm*/
dr:'diff(r(t),t);     ddr:'diff(r(t),t,2);
dth:'diff(th(t),t);   ddth:'diff(th(t),t,2);
x:r(t)+ell*sin(th(t));y:ell*cos(th(t));
J:(1/3)*m*ell^2;
T0:(1/2)*M*dr^2;
T1:(1/2)*m*(diff(x,t)^2+diff(y,t)^2)+(1/2)*J*dth^2;
V1:m*g*y;
L:T0+T1-V1;
LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce;
LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce;
sol:solve([LE1,LE2],[ddr,ddth]);
f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]);
a31:diff(f[3,1],r(t));a32:diff(f[3,1],th(t));
a33:diff(f[3,1],dr);a34:diff(f[3,1],dth);
a41:diff(f[4,1],r(t));a42:diff(f[4,1],th(t));
a43:diff(f[4,1],dr);a44:diff(f[4,1],dth);
b3:diff(f[3,1],F);b4:diff(f[4,1],F);
A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],[a41,a42,a43,a44]); 
A1:A,th(t)=0; 
A2:A,th(t)=%pi; 
B:matrix([0],[0],[b3],[b4]); 
B1:B,th(t)=0; 
B2:B,th(t)=%pi;

Simulink

%sCIP.m
%-----
 function [sys,x0]=sCIP(t,state,input,flag)
%-----
 global mc m ell g th0
%-----
 if abs(flag)==1
  F=input(1);
  r=state(1);  th=state(2);
  dr=state(3); dth=state(4);
  cth=cos(th); sth=sin(th);
  M=[mc+m m*ell*cth;
     m*ell*cth 4/3*m*ell^2];
  C=[0 -m*ell*dth*sth;
     0 0];
  G=[ 0;
     -m*ell*g*sth];
  W=M\(-C*[dr;dth]-G+[F;0]);
  ddr=W(1); ddth=W(2);
  sys=[dr;dth;ddr;ddth];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[4;0;4;1;0;0];
  x0=[0;th0;0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lCIP.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 th0=0 %input('th(0) = <0,180> ')/180*pi;
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[0;th0;0;0];
 us=0;
 sim("nCIP_obj")
%----- 線形化
 [A0,B0,C0,D0]=linmod('eCIP_linmod',xs,us) 
%
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%----- 線形応答と非線形応答の比較
 th0=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 sim("nCIP_comp")  
%-----
%end

[3] CIP2
$1^\circ$ 運動方程式の導出（数式処理の利用）
$(3.1)\quad \begin{array}{l} \displaystyle{\underbrace{ \left[\begin{array}{ccc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right] }_{M(\xi_1)} \underbrace{ \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta} \end{array}\right] }_{\dot{\xi}_2}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 & -m\ell\sin(\theta+\alpha)\dot{\theta} \\ 0 & 0 \end{array}\right] }_{C(\xi_1,\xi_2)} \underbrace{ \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] }_{\dot{\xi}_1}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} (M+m)g\sin\alpha\\ -m\ell g\sin\theta \end{array}\right] }_{G(\xi_1)} = \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F} \end{array}$

$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(3.2a)\quad \theta^*=0,\ F^*=(M+m)g\sin\alpha}$
$\displaystyle{(3.2b)\quad \theta^*=\pi,\ F^*=(M+m)g\sin\alpha}$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(3.3)\quad \frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\tilde{F}-C(\xi_1,\xi_2)\dot{\xi}_1}-G(\xi_1)) \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
平衡状態(3.2a)：
$(3.5)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r\\ \theta-\theta^*\\ \dot{r}\\ \dot{\theta} \end{array}\right] = \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{6\cos\alpha mg}{8M+(5-3\cos2\alpha)m} & 0 & 0\\ 0 & \frac{6(M+m)g}{(8M+(5-3\cos2\alpha)m)\ell} & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r\\ \theta-\theta^*\\ \dot{r}\\ \dot{\theta} \end{array}\right]}\\ \displaystyle{+ \left[\begin{array}{c} 0\\ 0\\ \frac{8}{8M+(5-3\cos2\alpha)m}\\ \frac{6\cos\alpha}{(8M+(5-3\cos2\alpha)m)\ell} \end{array}\right] (F-F^*)} \end{array}$
$6^\circ$ 線形シミュレータの開発

MAXIMA

/*cip2.wxm*/
dr:'diff(r(t),t); ddr:'diff(r(t),t,2);
dth:'diff(th(t),t); ddth:'diff(th(t),t,2);
x0:r(t)*cos(alpha); y0:r(t)*sin(alpha);
T0:(1/2)*M*(diff(x0,t)^2+diff(y0,t)^2); 
V0:M*g*y0;
x1:x0+ell*sin(th(t)); y1:y0+ell*cos(th(t));
J1:(1/3)*m*ell^2;
T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2;
V1:m*g*y1;
L:T0+T1-V0-V1;
LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp;
LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp;
sol:solve([LE1,LE2],[ddr,ddth]);
f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]);
a31:diff(f[3,1],r(t));a32:diff(f[3,1],th(t));
a33:diff(f[3,1],dr);a34:diff(f[3,1],dth);
a41:diff(f[4,1],r(t));a42:diff(f[4,1],th(t));
a43:diff(f[4,1],dr);a44:diff(f[4,1],dth);
b3:diff(f[3,1],F);b4:diff(f[4,1],F);
A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],
[a41,a42,a43,a44]); 
A1:A,th(t)=0,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; 
A2:A,th(t)=%pi,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; 
B:matrix([0],[0],[b3],[b4]); 
B1:B,th(t)=0,F=(M+m)*g*sin(alpha),trigreduce,ratsimp; 
B2:B,th(t)=%pi,F=(M+m)*g*sin(alpha),trigreduce,ratsimp;

Simulink

%sCIP2.m
%-----
 function [sys,x0]=sCIP2(t,state,input,flag)
%-----
 global mc m ell g alpha th0
%-----
 if abs(flag)==1
  F=input(1);
  r=state(1);  th=state(2);
  dr=state(3); dth=state(4);
  mc=1; m=0.1; ell=0.2; g=9.8;
  M=[mc+m m*ell*cos(th+alpha);
     m*ell*cos(th+alpha) 4/3*m*ell^2];
  C=[0 -m*ell*dth*sin(th+alpha);
     0 0];
  G=[(mc+m)*g*sin(alpha);
     -m*ell*g*sin(th)];
  W=M\(-C*[dr;dth]-G+[F;0]);
  ddr=W(1); ddth=W(2);
  sys=[dr;dth;ddr;ddth];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[4;0;4;1;0;0];
  x0=[0;th0;0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lCIP2.m
%-----
 clear all, close all
 global mc m ell g alpha th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 alpha=3/180*pi;
 th0=0 %input('ths   = <0,180> ')/180*pi;
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[0;th0;0;0];
 us=(mc+m)*g*sin(alpha);
 sim("nCIP2_obj")
%----- 線形化
 [A0,B0,C0,D0]=linmod('eCIP2_linmod',xs,us) 
%
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-6*cos(alpha)*m*g/(8*mc+(5-3*cos(2*alpha))*m); 
 A(4,1)=0; 
 A(4,2)=6*(m+mc)*g/((8*mc+(5-3*cos(2*alpha))*m)*ell);  
 B=zeros(4,1);
 B(3)=8/(8*mc+(5-3*cos(2*alpha))*m);
 B(4)=-6*cos(alpha)/((8*mc+(5-3*cos(2*alpha))*m)*ell); 
%----- 線形応答と非線形応答の比較
 th0=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 CM=[100      0 0 0; 
       0 180/pi 0 0]
 sim("nCIP2_comp")  
%-----
%end

[4] AIP
$1^\circ$ 運動方程式の導出（数式処理の利用）
$(4.1)\quad \begin{array}{l} \displaystyle{\underbrace{ \left[\begin{array}{ccc} (\frac{4}{3}m_1+4m_2)\ell_1^2 & 2m_2\ell_1\ell_2\cos(\theta_2-\theta_1)\\ 2m_2\ell_1\ell_2\cos(\theta_2-\theta_1) & \frac{4}{3}m_2\ell_2^2 \end{array}\right] }_{M(\xi_1)} \underbrace{ \left[\begin{array}{c} \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{array}\right] }_{\dot{\xi}_2}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 & -2m_2\ell_1\ell_2\sin(\theta_2-\theta_1) \dot{\theta}_2\\ 2m_2\ell_1\ell_2\sin(\theta_2-\theta_1) \dot{\theta}_1 & 0\\ \end{array}\right] }_{C(\xi_1,\xi_2)} \underbrace{ \left[\begin{array}{c} \dot{\theta}_1 \\ \dot{\theta}_2 \end{array}\right] }_{\dot{\xi}_1}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} -(m_1+2m_2)\ell_1g\sin\theta_1\\ -m_2\ell_2g\sin\theta_2 \end{array}\right] }_{G(\xi_1)} = \underbrace{ \left[\begin{array}{c} 1 \\ 0 \end{array}\right] \tau}_{\tilde{\tau}}} \end{array}$
$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(4.2a)\quad \theta_1^*\ne 0,\ \theta_2^*=0,\ \tau^*=-(m_1+2m_2)\ell_1g\sin\theta_1^*}$
$\displaystyle{(4.2b)\quad \theta_1^*\ne 0,\ \theta_2^*=\pi,\ \tau^*=-(m_1+2m_2)\ell_1g\sin\theta_1^*}$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(4.3)\quad \frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\tilde{\tau}-C(\xi_1,\xi_2)\dot{\xi}_1}-G(\xi_1)) \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
・ $\theta_1^*=0$ の場合の平衡状態(4.2a)：
$(4.5a)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right] =}\\ \displaystyle{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \frac{3(m_1+2m_2)g}{(4m_1+3m_2)\ell_1} & -\frac{9m_2g}{2(4m_1+3m_2)\ell_1} & 0 & 0\\ -\frac{9(m_1+2m_2)g}{2(4m_1+3m_2)\ell_1} & \frac{9m_2g}{(4m_1+3m_2)\ell_2} & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right]}\\ \displaystyle{+ \left[\begin{array}{c} 0\\ 0\\ \frac{6}{2(4m_1+3m_2)\ell_1^2}\\ -\frac{9}{2(4m_1+3m_2)\ell_1\ell_2} \end{array}\right] (\tau-\tau^*)} \end{array}$
・ $\theta_1^*=\alpha$ の場合の平衡状態(4.2a)：
$(4.5b)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right] =}\\ \displaystyle{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ a_{31}(\alpha) & a_{32}(\alpha) & 0 & 0\\ a_{41}(\alpha) & a_{42}(\alpha) & 0 & 0 \end{array}\right] \left[\begin{array}{c} \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right]}\\ \displaystyle{+ \left[\begin{array}{c} 0\\ 0\\ \frac{6}{2(4m_1+3(4-3\cos^2\alpha)m_2)\ell_1^2}\\ -\frac{6}{2(4m_1+3(4-3\cos^2\alpha)m_2)\ell_1\ell_2} \end{array}\right] (\tau-\tau^*)} \end{array}$

$6^\circ$ 線形シミュレータの開発

MAXIMA

/*aip.wxm*/
dth1:'diff(th1(t),t);     ddth1:'diff(th1(t),t,2);
dth2:'diff(th2(t),t);     ddth2:'diff(th2(t),t,2);
x1:ell1*sin(th1(t));      y1:ell1*cos(th1(t));
x2:2*x1+ell2*sin(th2(t)); y2:2*y1+ell2*cos(th2(t));
J1:(1/3)*m1*ell1^2;       J2:(1/3)*m2*ell2^2;
T1:(1/2)*m1*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth1^2;
T2:(1/2)*m2*(diff(x2,t)^2+diff(y2,t)^2)+(1/2)*J2*dth2^2;
V1:m1*g*y1; V2:m2*g*y2;
L:T1+T2-V1-V2;
LE1:diff(diff(L,dth1),t)-diff(L,th1(t))=tau,trigreduce,ratsimp;
LE2:diff(diff(L,dth2),t)-diff(L,th2(t))=0,trigreduce,ratsimp;
sol:solve([LE1,LE2],[ddth1,ddth2]);
f:matrix([dth1],[dth2],[rhs(sol[1][1])],[rhs(sol[1][2])]);
a31:diff(f[3,1],th1(t));  a32:diff(f[3,1],th2(t));
a33:diff(f[3,1],dth1);    a34:diff(f[3,1],dth2);
a41:diff(f[4,1],th1(t));  a42:diff(f[4,1],th2(t));
a43:diff(f[4,1],dth1);    a44:diff(f[4,1],dth2);
b3:diff(f[3,1],tau);      b4:diff(f[4,1],tau);
A:matrix([0,0,1,0],[0,0,0,1],
[a31,a32,a33,a34],[a41,a42,a43,a44]); 
A1:A,th1(t)=0,th2(t)=0,dth1=0,dth2=0,tau=(m1+2*m2)*ell1*g*sin(th1(t)),ratsimp; 
A2:A,th1(t)=%pi,th2(t)=0,dth1=0,dth2=0,tau=-(m1+2*m2)*ell1*g*sin(th1(t)),ratsimp;
A3:A,th1(t)=alpha,th2(t)=0,dth1=0,dth2=0,tau=(m1+2*m2)*ell1*g*sin(alpha),'diff(alpha,t)=0;ratsimp;
B:matrix([0],[0],[b3],[b4]); 
B1:B,th1(t)=0,th2(t)=0,dth1=0,dth2=0,tau=-(m1+2*m2)*ell1*g*sin(th1(t)),simplify; 
B2:B,th1(t)=%pi,th2(t)=0,dth1=0,dth2=0,tau=(m1+2*m2)*ell1*g*sin(th1(t)),simplify; 
B3:B,th1(t)=alpha,th2(t)=0,dth1=0,dth2=0,tau=-(m1+2*m2)*ell1*g*sin(alpha),simplify;

Simulink

%sAIP.m
%-----
 function [sys,x0]=sAIP(t,state,input,flag)
%-----
 global m1 m2 ell1 ell2 g th10 th20
%-----
 if abs(flag)==1
  tau=input(1);
  th1=state(1);  th2=state(2);
  dth1=state(3); dth2=state(4);
  cth1=cos(th1); sth1=sin(th1);
  cth2=cos(th2); sth2=sin(th2);
  cth21=cos(th2-th1); sth21=sin(th2-th1);
  M=[4/3*m1*ell1^2+4*m2*ell1^2, 2*m2*ell1*ell2*cth21;
     2*m2*ell1*ell2*cth21 4/3*m2*ell2^2];
  C=[0 -2*m2*ell1*ell2*dth2*sth21;
     2*m2*ell1*ell2*dth1*sth21 0];
  G=[-(m1+2*m2)*ell1*g*sth1;
     -m2*ell2*g*sth2];
  W=M\(-C*[dth1;dth2]-G+[tau;0]);
  ddth1=W(1); ddth2=W(2);
  sys=[dth1;dth2;ddth1;ddth2];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[4;0;4;1;0;0];
  x0=[th10;th20;0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lAIP.m
%-----
 clear all, close all
 global m1 m2 ell1 ell2  g th10 th20
 m1=0.1; m2=0.2; ell1=0.1; ell2=0.2; g=9.8;
 th10=0 %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;  
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[th10;th20;0;0];
 us=-(m1+2*m2)*ell1*g*sin(th10)
 sim("nAIP_obj")  
%----- 線形化
 [A0,B0,C0,D0]=linmod('eAIP_linmod',xs,us) 
%
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=3*(2*m2+m1)*g/((3*m2+4*m1)*ell1); 
 A(3,2)=-9*m2*g/(2*(3*m2+4*m1)*ell1); 
 A(4,1)=-9*(2*m2+m1)*g/(2*(3*m2+4*m1)*ell2); 
 A(4,2)=3*(3*m2+m1)*g/((3*m2+4*m1)*ell2);  
 B=zeros(4,1);
 B(3)=3/((3*m2+4*m1)*ell1^2);
 B(4)=-9/(2*(3*m2+4*m1)*ell1*ell2);
%----- 線形応答と非線形応答の比較
 th20=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 CM=[180/pi      0 0 0; 
       0    180/pi 0 0]
 sim("nAIP_comp")  
%-----
%end

[5] PIP
$1^\circ$ 運動方程式の導出（数式処理の利用）
$(5.1)\quad \begin{array}{l} \displaystyle{\underbrace{ \left[\begin{array}{ccc} M+m_1+m_2 & (m_1+2m_2)\ell_1\cos\theta_1 & m_2\ell_2\cos\theta_2\\ (m_1+2m_2)\ell_1\cos\theta_1 & (\frac{4}{3}m_1+4m_2)\ell_1^2 & 2m_2\ell_1\ell_2\cos(\theta_2-\theta_1)\\ m_2\ell_2\cos\theta_2 & 2m_2\ell_1\ell_2\cos(\theta_2-\theta_1) & \frac{4}{3}m_2\ell_2^2 \end{array}\right] }_{M(\xi_1)} \underbrace{ \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{array}\right] }_{\dot{\xi}_2}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 & -(m_1+2m_2)\ell_2\sin\theta_2 \dot{\theta}_1 & -m_2\ell_2\sin\theta_2 \dot{\theta}_2\\ 0 & 0 & 2m_2\ell_1\ell_2\sin(\theta_2-\theta_1) \dot{\theta}_2\\ 0 & 2m_2\ell_1\ell_2\sin(\theta_2-\theta_1) \dot{\theta}_1 & 0\\ \end{array}\right] }_{C(\xi_1,\xi_2)} \underbrace{ \left[\begin{array}{c} \dot{r} \\ \dot{\theta}_1 \\ \dot{\theta}_2 \end{array}\right] }_{\dot{\xi}_1}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 \\ (m_1+3m_2)\ell_1g\sin\theta_1\\ -m_2\ell_2g\sin\theta_2 \end{array}\right] }_{G(\xi_1)} = \underbrace{ \left[\begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right] F}_{\tilde{F}} \end{array}$
$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(5.2)\quad \theta_1^*=0,\ \theta_2^*=0 }$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(5.3)\quad \frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\tilde{F}-C(\xi_1,\xi_2)\dot{\xi}_1}-G(\xi_1)) \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
平衡状態(5.2)：
$(5.5)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r\\ \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{r}\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right] =}\\ \displaystyle{\left[\begin{array}{cccccc} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & -\frac{3m_1g}{4M+m_1+m_2} & -\frac{3m_2g}{4M+m_1+m_2} & 0 & 0 & 0\\ 0 & \frac{3(4m+4m_1+m_2)g}{4(4M+m_1+m_2)\ell_1} & \frac{9m_2g}{4(4M+m_1+m_2)\ell_1} & 0 & 0& 0\\ 0 & \frac{9gm_1}{4(4M+m_1+m_2)\ell_2} & \frac{3(4m+m_1+4m_2)g}{4(4M+m_1+m_2)\ell_2} & 0 & 0& 0\\ \end{array}\right] \left[\begin{array}{c} r\\ \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{r}\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right]}\\ \displaystyle{+ \left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4}{4M+m_1+m_2}\\ -\frac{3}{(4M+m_1+m_2)\ell_1}\\ -\frac{3}{(4M+m_1+m_2)\ell_2} \end{array}\right] F \end{array}$
$6^\circ$ 線形シミュレータの開発

MAXIMA

/*pip.wxm*/
dr:'diff(r(t),t);         ddr:'diff(r(t),t,2);
dth1:'diff(th1(t),t);     ddth1:'diff(th1(t),t,2);
dth2:'diff(th2(t),t);     ddth2:'diff(th2(t),t,2);
x1:r(t)+ell1*sin(th1(t)); y1:ell1*cos(th1(t));
x2:r(t)+ell2*sin(th2(t)); y2:ell2*cos(th2(t));
J1:(1/3)*m1*ell1^2;       J2:(1/3)*m2*ell2^2;
T0:(1/2)*M*dr^2;
T1:(1/2)*m1*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth1^2;
T2:(1/2)*m2*(diff(x2,t)^2+diff(y2,t)^2)+(1/2)*J2*dth2^2;
V1:m1*g*y1; V2:m2*g*y2;
L:T0+T1+T2-V1-V2;
LE0:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce;
LE1:diff(diff(L,dth1),t)-diff(L,th1(t))=0,trigreduce;
LE2:diff(diff(L,dth2),t)-diff(L,th2(t))=0,trigreduce;
sol:solve([LE0,LE1,LE2],[ddr,ddth1,ddth2]);
f:matrix([dr],[dth1],[dth2],
[rhs(sol[1][1])],[rhs(sol[1][2])],[rhs(sol[1][3])]);
a41:diff(f[4,1],r(t));    a42:diff(f[4,1],th1(t));
a43:diff(f[4,1],th2(t));  a44:diff(f[4,1],dr);
a45:diff(f[4,1],dth1);    a46:diff(f[4,1],dth2);
a51:diff(f[5,1],r(t));    a52:diff(f[5,1],th1(t));
a53:diff(f[5,1],th2(t));  a54:diff(f[5,1],dr);
a55:diff(f[5,1],dth1);    a56:diff(f[5,1],dth2);
a61:diff(f[6,1],r(t));    a62:diff(f[6,1],th1(t));
a63:diff(f[6,1],th2(t));  a64:diff(f[6,1],dr);
a65:diff(f[6,1],dth1);    a66:diff(f[6,1],dth2);
b4:diff(f[4,1],F);b5:diff(f[5,1],F); b6:diff(f[6,1],F);
A:matrix([0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1],
[a41,a42,a43,a44,a45,a46],[a51,a52,a53,a54,a55,a56],
[a61,a62,a63,a64,a65,a66]); 
A1:A,r(t)=0,th1(t)=0,th2(t)=0,dr=0,dth1=0,dth2=0,F=0,ratsimp;
B:matrix([0],[0],[0],[b4],[b5],[b6]); 
B1:B,r(t)=0,th1(t)=0,th2(t)=0,dr=0,dth1=0,dth2=0,F=0,ratsimp;

Simulink

%sPIP.m
%-----
 function [sys,x0]=sPIP(t,state,input,flag)
%-----
 global mc m1 m2 ell1 ell2 g th10 th20
%-----
 if abs(flag)==1
  F=input(1);
  r=state(1);  th1=state(2); th2=state(3);  
  dr=state(4);  dth1=state(5); dth2=state(6);  
  cth1=cos(th1); sth1=sin(th1);
  cth2=cos(th2); sth2=sin(th2);
  M=[mc+m1+m2 m1*ell1*cth1 m2*ell2*cth2;
     m1*ell1*cth1 4/3*m1*ell1^2 0;
     m2*ell2*cth2 0 4/3*m2*ell2^2];
  C=[0 -m1*ell1*dth1*sth1 -m2*ell2*dth2*sth2;
     0 0 0;
     0 0 0];
  G=[ 0;
     -m1*ell1*g*sth1
     -m2*ell2*g*sth2];
  W=M\(-C*[dr;dth1;dth2]-G+[F;0;0]);
  ddr=W(1); ddth1=W(2); ddth2=W(3);
  sys=[dr;dth1;dth2;ddr;ddth1;ddth2];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[6;0;6;1;0;0];
  x0=[0;th10;th20;0;0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lPIP.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m1=0.1; m2=0.2; ell1=0.15; ell2=0.2; g=9.8;
 th10=0 %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;  
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[0;th10;th20;0;0;0];
 us=0;
 sim("nPIP_obj")  
%----- 線形化
 [A0,B0,C0,D0]=linmod('ePIP_linmod',xs,us) 
%
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-3*m1*g/(m1+m2+4*mc); 
 A(4,3)=-3*m2*g/(m1+m2+4*mc);  
 A(5,1)=0; 
 A(5,2)=(12*m1+3*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell1);  
 A(5,3)=9*m2*g/((4*m1+4*m2+16*mc)*ell1);   
 A(6,1)=0; 
 A(6,2)=9*m1*g/((4*m1+4*m2+16*mc)*ell2);  
 A(6,3)=(3*m1+12*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell2);   
 B=zeros(6,1);
 B(4)=4/(m1+m2+4*mc);
 B(5)=-3/((m1+m2+4*mc)*ell1);
 B(6)=-3/((m1+m2+4*mc)*ell2); 
%----- 線形応答と非線形応答の比較
 th10=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 th20=-1/180*pi %input('th(0) = <0,180> ')/180*pi;
 CM=[diag([100,180/pi,180/pi]),zeros(3,3)];
 sim("nPIP_comp")  
%-----
%end

[6] DIP
$1^\circ$ 運動方程式の導出（数式処理の利用）
$(6.1)\quad \begin{array}{l} \displaystyle{\underbrace{ \left[\begin{array}{ccc} M+m_1+m_2 & m_1\ell_1\cos\theta_1 & -m_2\ell_2\cos\theta_2\\ m_1\ell_1\cos\theta_1 & \frac{4}{3}m_1\ell_1^2 & 0\\ -m_2\ell_2\cos\theta_2 & 0 & \frac{4}{3}m_2\ell_2^2 \end{array}\right] }_{M(\xi_1)} \underbrace{ \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{array}\right] }_{\dot{\xi}_2}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 & -m_1\ell_2\sin\theta_2 \dot{\theta}_1 & -m_2\ell_2\sin\theta_2 \dot{\theta}_2\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{array}\right] }_{C(\xi_1,\xi_2)} \underbrace{ \left[\begin{array}{c} \dot{r} \\ \dot{\theta}_1 \\ \dot{\theta}_2 \end{array}\right] }_{\dot{\xi}_1}}\\ \displaystyle{ +\underbrace{ \left[\begin{array}{ccc} 0 \\ -m_1\ell_1g\sin\theta_1\\ -m_2\ell_2g\sin\theta_2 \end{array}\right] }_{G(\xi_1)} = \underbrace{ \left[\begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right] F}_{\tilde{F}} \end{array}$
$2^\circ$ 平衡状態の検討（fsolveの利用）
$\displaystyle{(6.2)\quad \theta_1^*=0,\ \theta_2^*=0 }$
$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
$\displaystyle{(6.3)\quad \frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\tilde{F}-C(\xi_1,\xi_2)\dot{\xi}_1}-G(\xi_1)) \end{array}\right] }$
$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
$5^\circ$ 線形化（linmodの利用）
平衡状態(6.2)：
$(6.5)\quad \begin{array}{l} \displaystyle{ \frac{d}{dt}\left[\begin{array}{c} r\\ \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{r}\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right] =}\\ \displaystyle{ \left[\begin{array}{cccccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & -\frac{3m_1g}{Mm_1+m_1^2+(3M+m1)m_2} & -\frac{3m_2g}{Mm_1+m_1^2+(3M+m1)m_2} \\ 0 & \frac{3(4Mm_1+4m_1^2+3m_2^2+3(18M+13m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} & \frac{9(2M+m_1)m_2g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} \\ 0 & \frac{9(2Mm_1+m_1^2+3(2M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} & \frac{3(4Mm_1+m_1^2+12(3M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \\ \end{array}\right.}\\ \displaystyle{\left.\begin{array}{cccccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r\\ \theta_1-\theta_1^*\\ \theta_2-\theta_2^*\\ \dot{r}\\ \dot{\theta}_1\\ \dot{\theta}_2 \end{array}\right]}\\ \displaystyle{+ \left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4m_1+3m_2}{4Mm_1+m_1^2+(3M+m1)m_2}\\ -\frac{3(2m_1+m2)}{2Mm_1+m_1^2+(3M+m1)m_2}\\ \frac{3m_1}{2(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \end{array}\right] F} \end{array}$

$6^\circ$ 線形シミュレータの開発

MAXIMA

/*dip.wxm*/
dr:'diff(r(t),t);         ddr:'diff(r(t),t,2);
dth1:'diff(th1(t),t);     ddth1:'diff(th1(t),t,2);
dth2:'diff(th2(t),t);     ddth2:'diff(th2(t),t,2);
x1:r(t)+ell1*sin(th1(t)); y1:ell1*cos(th1(t));
x2:r(t)+2*ell1*sin(th1(t))+ell2*sin(th2(t)); 
y2:2*y1+ell2*cos(th2(t));
J1:(1/3)*m1*ell1^2;       J2:(1/3)*m2*ell2^2;
T0:(1/2)*M*dr^2;
T1:(1/2)*m1*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth1^2;
T2:(1/2)*m2*(diff(x2,t)^2+diff(y2,t)^2)+(1/2)*J2*dth2^2;
V1:m1*g*y1; V2:m2*g*(y1+y2);
L:T0+T1+T2-V1-V2;
LE0:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce;
LE1:diff(diff(L,dth1),t)-diff(L,th1(t))=0,trigreduce;
LE2:diff(diff(L,dth2),t)-diff(L,th2(t))=0,trigreduce;
sol:solve([LE0,LE1,LE2],[ddr,ddth1,ddth2]);
f:matrix([dr],[dth1],[dth2],
[rhs(sol[1][1])],[rhs(sol[1][2])],[rhs(sol[1][3])]);
a41:diff(f[4,1],r(t));    a42:diff(f[4,1],th1(t));
a43:diff(f[4,1],th2(t));  a44:diff(f[4,1],dr);
a45:diff(f[4,1],dth1);    a46:diff(f[4,1],dth2);
a51:diff(f[5,1],r(t));    a52:diff(f[5,1],th1(t));
a53:diff(f[5,1],th2(t));  a54:diff(f[5,1],dr);
a55:diff(f[5,1],dth1);    a56:diff(f[5,1],dth2);
a61:diff(f[6,1],r(t));    a62:diff(f[6,1],th1(t));
a63:diff(f[6,1],th2(t));  a64:diff(f[6,1],dr);
a65:diff(f[6,1],dth1);    a66:diff(f[6,1],dth2);
b4:diff(f[4,1],F);b5:diff(f[5,1],F); b6:diff(f[6,1],F);
A:matrix([0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1],
[a41,a42,a43,a44,a45,a46],[a51,a52,a53,a54,a55,a56],
[a61,a62,a63,a64,a65,a66]); 
A1:A,r(t)=0,th1(t)=0,th2(t)=0,dr=0,dth1=0,dth2=0,F=0,
ell1=ell,ell2=ell,ratsimp;
B:matrix([0],[0],[0],[b4],[b5],[b6]); 
B1:B,r(t)=0,th1(t)=0,th2(t)=0,dr=0,dth1=0,dth2=0,F=0,
ell1=ell,ell2=ell,ratsimp;

Simulink

%sDIP.m
%-----
 function [sys,x0]=sDIP(t,state,input,flag)
%-----
 global mc m1 m2 ell1 ell2 g th10 th20
%-----
 if abs(flag)==1
  F=input(1);
  r=state(1);  th1=state(2); th2=state(3);  
  dr=state(4);  dth1=state(5); dth2=state(6);  
  cth1=cos(th1); sth1=sin(th1);
  cth2=cos(th2); sth2=sin(th2);
  cth21=cos(th2-th1); sth21=sin(th2-th1); 
  M=[mc+m1+m2 (m1+2*m2)*ell1*cth1 m2*ell2*cth2;
     (m1+2*m2)*ell1*cth1 (4/3*m1+4*m2)*ell1^2 2*m2*ell1*ell2*cth21;
     m2*ell2*cth2 2*m2*ell1*ell2*cth21 4/3*m2*ell2^2];
  C=[0 -(m1+2*m2)*ell1*sth1*dth1 -m2*ell2*sth2*dth2;
     0 0 2*m2*ell1*ell2*cth21*dth2;
     0 2*m2*ell1*ell2*cth21*dth1 0];
  G=[ 0;
     -(m1+3*m2)*ell1*g*sth1
     -m2*ell2*g*sth2];
  W=M\(-C*[dr;dth1;dth2]-G+[F;0;0]);
  ddr=W(1); ddth1=W(2); ddth2=W(3);
  sys=[dr;dth1;dth2;ddr;ddth1;ddth2];
%-----  
 elseif flag==3
  sys=state;
 elseif flag==0
  sys=[6;0;6;1;0;0];
  x0=[0;th10;th20;0;0;0];
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lDIP.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m=0.1; m1=m; m2=m; ell=0.15; ell1=ell; ell2=ell; g=9.8;
 th10=0 %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;  
%----- 平衡状態の計算
%----- 非線形シミュレータのチェック 
 xs=[0;th10;th20;0;0;0];
 us=0;
 sim("nDIP_obj")  
%----- 線形化
 [A0,B0,C0,D0]=linmod('eDIP_linmod',xs,us) 
%
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-18*m*g/(2*m+7*mc); 
 A(4,3)=3*m*g/(4*m+14*mc);  
 A(5,1)=0; 
 A(5,2)=(15*m+12*mc)*g/((2*m+7*mc)*ell);  
 A(5,3)=-(9*m+18*mc)*g/((8*m+28*mc)*ell);   
 A(6,1)=0; 
 A(6,2)=-(9*m+18*mc)*g/((2*m+7*mc)*ell);  
 A(6,3)=(15*m+48*mc)*g/((8*m+28*mc)*ell);   
 B=zeros(6,1);
 B(4)=7/(2*m+7*mc);
 B(5)=-9/((4*m+14*mc)*ell);
 B(6)=3/((4*m+14*mc)*ell);
%----- 線形応答と非線形応答の比較
 th10=1/180*pi %input('th(0) = <0,180> ')/180*pi;
 th20=-1/180*pi %input('th(0) = <0,180> ')/180*pi;
 CM=[diag([100,180/pi,180/pi]),zeros(3,3)];
 sim("nDIP_comp")  
%-----
%end

平衡状態回りの線形化

投稿日時: 2021年10月9日投稿者: cacsd

平衡状態回りの線形化…Homework

[0]　制御を考えるときは、まず対象の平衡状態(equilibrium state)を念頭におきます。そして何らかの要因でこの平衡状態が乱されたとき、元の平衡状態に戻すことが制御の基本的な役割と考えられます。制御の対象としては、力学系、電気系、化学系など様々なものがあり、その振舞いはサイエンスによってすでに十分検討されています。しかし、平衡状態を保持したり、遷移させたりという対象への働きかけは、エンジニアリングの範疇となります。人が楽に操れるドローンや１輪車というモノ作りは、制御という技術がなければ達成できなかったと言えます。

●制御系設計を確実に実施・達成するための方法として次図に示すHILS アプローチが知られています。
図1 HILS アプローチ

これから、制御系設計、仮想制御実験、制御実験とステップを踏むことが分かります。２台のPCを用いて実時間での閉ループシミュレーションを行う仮想制御実験の成否が決め手となるので、HILS:Hardware In the Loop Simulationがキーワードとなったと思われます。ただ、最初のステップである制御系設計を１台のPC上で十分検討することが大切であることは述べるまでもありません。制御系設計は①非線形シミュレータの開発と②コントローラの設計からなります。以下では、非線形シミュレータの開発について、
　 $1^\circ$ 運動方程式の導出（数式処理の利用）
　 $2^\circ$ 平衡状態の検討（fsolveの利用）
　 $3^\circ$ 非線形状態方程式の導出（S-functionの作成）
　 $4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
　 $5^\circ$ 線形化（linmodの利用）
　 $6^\circ$ 線形シミュレータの開発
の手順を具体的に説明しますが、その前に非線形系と線形系の振舞いは平衡状態周りではどう違うのかについて振り子を例にとって説明します。

[1]　高校の物理で出てきた次の単振り子を考えます。これは吊り荷を下げたクレーンの振止めの問題を考えるのに役立ちます。

図2 単振り子

左側に示すように鉛直線に沿って垂れ下がった状態が平衡状態です。右側に示すように重りを手で（ひもが緩まないように）持ち上げて離したときの運動方程式は

$\displaystyle{(1)\quad mL\ddot\theta(t)=-mg\sin\theta(t)\ \Rightarrow\ \boxed{\ddot\theta(t)=-\frac{g}{L}\sin\theta(t)} }$

で与えられます。ここで、 $L$ は振り子の長さ、 $m$ は重りの質量、 $g$ は重力加速度、 $\theta(t)$ は時刻 $t$ における振り子の鉛直線となす角度を表しています。初期時刻 $t=0$ では $\theta(0)\ne0, \dot{\theta}(0)=0$ とします。

これは簡単な微分方程式のように思えますが、そうではありません。実際、

$\displaystyle{(2)\quad \sin x=x-\frac{x^3}{3!}+\frac{x^5}{5!}-\frac{x^7}{7!}+\cdots }$

のように展開できますから、運動方程式は

$\displaystyle{(3)\quad \ddot\theta(t)=-\frac{g}{L}(\theta(t)-\frac{\theta^3(t)}{3!}+\frac{\theta^5(t)}{5!}-\frac{\theta^7(t)}{7!}+\cdots) }$

となるのですが、これは手計算では手の付けようもありません。しかしながら、 $\theta$ が十分小さく、高次項が無視できれば

$\displaystyle{(4)\quad \boxed{\ddot\theta(t)=-\frac{g}{L}\theta(t)}\qquad(\theta(0)\ne0, \dot{\theta}(0)=0) }$

となって、解は次式で表されます。

$\displaystyle{(5)\quad \theta(t)=\theta(0)\cos\sqrt{\frac{g}{L}}t }$

２つの運動方程式(1)と(4)の違いを数値シミュレーションで見てみると、図3のようになります。初期角度が10 $^\circ$ のときは（平衡状態近傍では）両者に差異は見られませんが、90 $^\circ$ のときは周期が一致しません。また初期角度が177 $^\circ$ のときのシミュレーションを行ってみると、両者の差異に愕然することでしょう。

図3 単振り子の振動シミュレーション（モデルが線形か非線形か、初期状態を平衡状態周辺にとるかによる相違）

演習A12-1…Flipped Classroom

次のコードを用いて、図2のシミュレーションを行え。

MATLAB

%a12_1.m
 clear all, close all
 t=[0,10];
 x0=[10/180*pi;0];
 [t1,y1]=ode45(@f1,t,x0);
 [t2,y2]=ode45(@f2,t,x0);
 plot(t1,180/pi*y1(:,1),t2,180/pi*y2(:,1)), hold on
 x0=[90/180*pi;0];
 [t1,y1]=ode45(@f1,t,x0);
 [t2,y2]=ode45(@f2,t,x0);
 plot(t1,180/pi*y1(:,1),t2,180/pi*y2(:,1))
 grid
 title(' ');
 xlabel('Time[s]');
 ylabel('x1 and x2');
 legend('x_1','x_2')
%-----
 function dx=f1(t,x), L=1; g=9.8; dx=[x(2);-g/L*sin(x(1))]; end
 function dx=f2(t,x), L=1; g=9.8; dx=[0 1;-g/L 0]*x;  end
%eof

SCILAB

coming soon

＜ヒント＞　(1)と(4)は次のような連立１階微分方程式として表せることに注意。

$\displaystyle{(6)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{l} \omega(t) \\ -\frac{g}{L}\sin\theta(t) \end{array}\right] }_{f(x(t))} }$

$\displaystyle{(7)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{g}{L}&0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{l} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} }$

[2]　制御対象として次図に示す水タンクを例にとって、非線形シミュレータの開発手順を説明します。

図4 水タンクTANK

$1^\circ$ 運動方程式の導出（数式処理の利用）
時刻 $t$ における水位、流入量、流出量をそれぞれ $h(t)$ 、 $q_i(t)$ 、 $q_o(t)$ とします。タンクの断面積を $S$ とすると、運動方程式は次式となります。

$\displaystyle{(8)\quad S\dot{h}(t)=-\underbrace{k\sqrt{h(t)}}_{q_o(t)}+q_i(t)}$

$2^\circ$ 平衡状態の検討（fsolveの利用）
(8)から平衡状態 $h^*$ と平衡入力 $q_i^*$ が次式のように求まります。

$\displaystyle{(9)\quad 0=-k\sqrt{h^*}+q_i^*\Rightarrow q_i^*=k\sqrt{h^*}}$

$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
(8)から次の非線形状態方程式を得ます。

$\displaystyle{(10)\quad \dot{h}(t)=f(h(t),q_i(t))=-\frac{k}{S}\sqrt{h(t)}+\frac{1}{S}q_i(t)}$

$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
(10)に基づいて制御対象TANKの平衡状態周りの挙動を模擬する非線形シミュレータを次図のように構成します。

図5 TANKの非線形シミュレータ

一般に、制御目的が平衡状態の安定化であれば、非線形シミュレータの入力側では平衡入力を加え、出力側では平衡状態を差し引いておきます。このとき、
「零入力 $u=0$ に対して零状態 $x=0$ が出力される」
はずですから、非線形シミュレータが正しく開発されているかのチェックに役立ちます。

$5^\circ$ 線形化（linmodの利用）

$\displaystyle{(11)\quad \begin{array}{l} \dot{h}(t)=f(h(t),q_i(t))=\\ f(h^*,q_i^*)+\frac{\partial f(h^*,q_i^*)}{\partial h}(h(t)-h^*)+\frac{\partial f(h^*,q_i^*)}{\partial q_i}(q_i(t)-q_i^*) \end{array} }$

から、次の線形状態方程式を得ます。

$\displaystyle{(12)\quad \underbrace{\frac{d}{dt}(h(t)-h^*)}_{\dot{x}(t)}=\underbrace{-\frac{k}{2S\sqrt{h^*}}}_{a}\underbrace{(h(t)-h^*)}_{x(t)}+\underbrace{\frac{1}{S}}_{b}\underbrace{(q_i(t)-q_i^*)}_{u(t)}}$

$6^\circ$ 線形シミュレータの開発
(12)に基づいて制御対象TANKの線形シミュレータを次図のように構成します。

図6 TANKの線形シミュレータ

● $k=0.1,S=1$ のとき、水位 $h^*=1$ の平衡状態にあるとします。今、バケツで水を入れたため、水位が $h(0)=1.2$ になったとします。このとき、平衡状態に戻る過程を非線形シミュレータと線形シミュレータで比較した結果を図7に示します。

図7 TANKの非線形応答と線形応答の比較

演習A12-2…Flipped Classroom

Simulinkを用いて図8を開発し、図7のグラフを描け。

図8 TANKの非線形シミュレータと線形シミュレータ

＜ヒント＞

Simulink

%sTANK.m
%-----
 function [sys,x0]=sTANK(t,state,input,flag)
%-----
 global h0
%-----
 if abs(flag)==1
  h=state(1);  
  qi=input(1);
  k=0.1; S=1;
  dh=-k/S*sqrt(h)+1/S*qi;
  sys=dh;
%-----  
 elseif flag==3
  sys=state(1);
 elseif flag==0
  sys=[1;0;1;1;0;0];
  x0=h0;
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lTANK.m
 clear all, close all
 global h0
 k=0.1; S=1;
 h0=1;
%----- 平衡状態の計算
 x0=[1;1];
 x=fsolve(@TANK,x0)
 xs=x(1), us=x(2)
%----- 非線形シミュレータのチェック
 xs=1; us=k*sqrt(xs)
 sim("nTANK_obj")
%----- 線形化
 [a,b,c,d]=linmod('eTANK_linmod',xs,us)
 x0=0.2;
 sim("eTANK_linsys")  
%----- 線形応答と非線形応答の比較
 h0=1.2
 sim("nTANK_comp")  
%------
 function err=TANK(x)
   h=x(1); u=x(2); k=0.1;
   err=-k*sqrt(h)+u;
 end
%eof

[3] 一般には、制御対象の運動方程式から、非線形状態方程式を求め、これを平衡状態と平衡入力まわりで線形化（１次近似）し、線形状態方程式を求めます。

いま、 $\xi_1,\cdots,\xi_n$ を状態変数、 $\zeta_1,\cdots,\zeta_m$ を入力変数として、運動方程式から、次のような連立１階微分方程式で表される非線形状態方程式を得ます。

$\displaystyle{(13)\quad \left\{\begin{array}{l} \dot{\xi}_1=f_1(\xi_1,\cdots,\xi_n,\zeta_1,\cdots,\zeta_m) \\ \quad\vdots \\ \dot{\xi}_n=f_n(\xi_1,\cdots,\xi_n,\zeta_1,\cdots,\zeta_m) \end{array}\right. }$

これを次のようにベクトル表示します。 $\xi$ が状態変数ベクトル、 $\zeta$ が入力変数ベクトルです。

$\displaystyle{(14)\quad \boxed{\dot{\xi}=f(\xi,\zeta)}\ \Leftrightarrow\ \underbrace{ \left[\begin{array}{l} \dot{\xi}_1 \\ \vdots \\ \dot{\xi}_n \end{array}\right] }_{\dot{\xi}} = \underbrace{ \left[\begin{array}{l} f_1(\xi,\zeta) \\ \quad\vdots \\ f_n(\xi,\zeta) \end{array}\right] }_{f(\xi,\zeta)} }$

平衡状態 $\xi^*$ と平衡入力 $\zeta^*$ は

$\displaystyle{(15)\quad \dot{\xi}^*=f(\xi^*,\zeta^*)=const.}$

を満足するベクトルとして規定されます。これらの周りで $f(\xi,\zeta)$ を１次近似すると次式を得ます。

$\displaystyle{(16)\quad \dot{\xi}=f(\xi,\zeta)\simeq \underbrace{f(\xi^*,\zeta^*)}_{\dot{\xi}^*} +\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}(\xi-\xi^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}(\zeta-\zeta^*) }$

ただし

$\displaystyle{(17)\quad A=\frac{\partial\,f(\xi^*,\zeta^*)}{\partial\,\xi}= \left[\begin{array}{ccc} \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \xi_1}&\cdots& \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \xi_n}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \xi_1}&\cdots& \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \xi_n} \end{array}\right] }$

$\displaystyle{(18)\quad B=\frac{\partial\,f(\xi^*,\zeta^*)}{\partial\,\zeta}= \left[\begin{array}{ccc} \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \zeta_1}&\cdots& \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \zeta_m}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \zeta_1}&\cdots& \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \zeta_m} \end{array}\right] }$

これから次の線形状態方程式を得ます。

$\displaystyle{(19)\quad \boxed{\underbrace{\dot{\xi}-\dot{\xi}^*}_{\dot{x}}= \underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}}_A\underbrace{(\xi-\xi^*)}_x +\underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}}_B\underbrace{(\zeta-\zeta^*)}_u} }$

したがって、次に留意します。

線形状態方程式の「零状態 $x=0$ 」は平衡状態 $\xi=\xi^*$ にあることを意味し、これは平衡入力 $\zeta=\zeta^*$ によってもたらされるので「零入力 $u=0$ 」を前提とする。

●図9は非線形シミュレータ（非線形状態方程式）を用いて線形制御（状態フィードバック）の評価を行う場合の構成を、線形シミュレータ（線形状態方程式）を用いる場合と対比させています。

図9 非線形シミュレータと線形シミュレータの比較

演習A12-3…Flipped Classroom

非線形状態方程式(6)から線形状態方程式(7)を、次式を用いて示せ。

$\displaystyle{(20)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{cc} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{\dot{x}=\dot{\xi}-\dot{\xi}^*}= \underbrace{ \left[\begin{array}{cc} \frac{\partial f_1(\xi^*,\tau^*)}{\partial \xi_1} & \frac{\partial f_1(\xi^*,\tau^*)}{\partial \xi_2} \\ \frac{\partial f_2(\xi^*,\tau^*)}{\partial \xi_1} & \frac{\partial f_2(\xi^*,\tau^*)}{\partial \xi_2} \end{array}\right] }_{A=\frac{\partial f(\xi^*,\tau^*)}{\partial \xi}} \underbrace{ \left[\begin{array}{l} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{x=\xi-\xi^*} }$

●制御対象が $n$ 自由度をもつ力学系の場合、運動方程式の一般式は

$\displaystyle{(21)\quad M(\xi_1)\dot{\xi}_2+C(\xi_1,\xi_2)\xi_2+G(\xi_1)=\zeta\quad (\xi_2=\dot{\xi}_1) }$

で与えられます（ $\xi_1$ は $n$ 個の位置変数からなるベクトル、 $\xi_2$ は $n$ 個の速度変数からなるベクトル）。このとき非線形状態方程式は次式となります。

$\displaystyle{(22)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right]}_{\dot{\xi}} =\underbrace{ \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\zeta-C(\xi_1,\xi_2)\xi_2-G(\xi_1)) \end{array}\right]}_{f(\xi,\zeta)}} }$

また平衡状態 $\xi^*= \left[\begin{array}{l} \xi^*_1 \\ \xi^*_2 \end{array}\right]$ と平衡入力 $\zeta^*$ は非線形連立方程式

$\displaystyle{(23)\quad M(\xi_1)\underbrace{\dot{\xi}_2}_{0}+C(\xi_1^*,\xi_2^*)\xi^*_2+G(\xi_1^*)=\zeta^* }$

を解いて求めます（fsolveの利用）。さらに線形化を行うと

$\displaystyle{(24)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{cc} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cc} 0 & I_n \\ A_{21} & A_{22} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{l} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{x}+ \underbrace{ \left[\begin{array}{cc} 0 \\ B_2 \end{array}\right] }_{B} \underbrace{ (\zeta-\zeta^*) }_{u} }$

の形式となります。

Note A12　剛体振り子…Homework

一様な剛体の棒の一端を軸支した剛体振り子を考えます。この平衡状態はもちろん棒が鉛直線に沿っている状態ですが、回転軸が上端にある場合と下端にある場合とで２種類の平衡状態( $\alpha$ )と( $\beta$ )をもつところが単振り子と違います。（ $\beta$ )の方は、人が１輪車に乗った状況を模擬しているといえます。

図1 剛体振り子

いま回転軸回りの棒の運動を考えます。剛体の回転運動は「慣性モーメント×角加速度＝トルク」で表されますので、棒の回転角を $\theta$ とすると、運動方程式は次式となります。

$\displaystyle{(1)\quad J\ddot{\theta}(t)=-mg\ell\sin\theta(t) }$

ここで、 $2\ell$ は棒の長さ、 $m$ は棒の質量、 $g$ は重力加速度です。回転軸回りの慣性モーメント $J$ は、まず重心回りの慣性モーメント

$\displaystyle{(2)\quad J_0=\int_{-\ell}^{\ell}r^2\,dm=\int_{-\ell}^{\ell}r^2\,\frac{m}{2\ell}dr=\frac{m}{2\ell}\left[\frac{r^3}{3}\right]_{-\ell}^{\ell}=\frac{1}{3}m\ell^2 }$

を求めて、平行軸の定理を使って、次のように計算されます（積分範囲を $0$ から $2\ell$ としてもよい）。

$\displaystyle{(3)\quad J=J_0+m\ell^2=\frac{4}{3}m\ell^2 }$

結局、剛体振り子の運動を調べるためには、次の微分方程式を解くことになります。

$\displaystyle{(4)\quad \boxed{\ddot{\theta}(t)=-\frac{3g}{4\ell}\sin\theta(t) }}$

一般に平衡状態が何らかの原因で乱されたとき、元の平衡状態に戻すことが制御動作と考えられます。したがって、剛体振り子の平衡状態近傍の運動の理解が大切となります。単振り子の場合に調べたように、平衡状態回りでは右辺を１次近似しても、元の解の振舞いをよく表していました。でも剛体振り子の場合は２種類の平衡状態があるので、少し注意が必要です。 $\sin x$ の $x=a$ における展開式

$\displaystyle{(5)\quad \sin x=\sin a+\cos a\,(x-a)-\frac{\sin a}{2!}(x-a)^2+\cdots }$

を用いて、平衡状態( $\alpha$ )の近傍では（ $a=0$ ）、 $\sin\theta\simeq\theta$ として

$\displaystyle{(6)\quad \ddot\theta(t)=-\frac{3g}{4\ell}\theta(t) }$

また平衡状態( $\beta$ )の近傍では（ $a=\pi$ ）、 $\sin\theta\simeq\,\theta-\pi$ として

$\displaystyle{(7)\quad \ddot\theta(t)=\frac{3g}{4\ell}(\theta(t)-\pi) }$

のような１次近似を行います。これらに基づいてシミュレーションを行う場合は、それぞれ次式を用います。

$\displaystyle{(8)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{3g}{4\ell}&0 \end{array}\right] }_{A_\alpha} \underbrace{ \left[\begin{array}{l} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} }$

$\displaystyle{(9)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{l} \theta(t)-\pi \\ \omega(t)-0 \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}&0 \end{array}\right] }_{A_\beta} \underbrace{ \left[\begin{array}{l} \theta(t)-\pi \\ \omega(t)-0 \end{array}\right] }_{x(t)} }$

これらの線形状態方程式(8),(9)は、非線形状態方程式(4)を、線形化して得られます。

最後に、長さ $2\ell$ をもつ剛体振り子と同じ周期をもつ単振り子の長さ $L$ を求めておきます。これは撃力中心の位置を表しています。これは振れ止めの制御を行う場合、重要な役割を果たします。

$\displaystyle{(10)\quad \frac{g}{L}=\frac{3g}{4\ell}\Rightarrow \boxed{L=\frac{2}{3}\times 2\ell} }$

図2 剛体振り子と同じ周期をもつ単振り子の長さは？

線形系の状態空間表現

投稿日時: 2021年10月8日投稿者: cacsd

線形系の状態空間表現…Homework

[1] 　制御対象は一般には非線形系ですが、ここでは重ね合わせの原理の成り立つ線形系（linear system）を考えます。制御対象のモデルとして、次の状態空間表現が用いられます。

$\displaystyle{(1)\quad \boxed{\left\{\begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)&(x(t)\in{\bf R}^n,u(t)\in{\bf R}^m)\\ y(t)=Cx(t)&(y(t)\in{\bf R}^p) \end{array}\right.} }$

第１式は状態方程式とよばれ、 $x(t),u(t)$ はそれぞれ時刻 $t$ におけるｎ個の状態変数、ｍ個の入力変数（アクチュエータを操作する）からなるベクトルを表しています。また、第２式は出力方程式とよばれ、 $y(t)$ は時刻 $t$ におけるｐ個の出力変数（センサを用いて観測される）からなるベクトルを表しています。以下では状態空間表現(1)を簡単にn次系と参照します。

この状態空間表現は図1のようなブロック線図で表されます。

図1 状態空間表現(1)のブロック線図

[2]　具体例を示すために、次図に示すマス・バネ・ダンパからなる機械系を考えます。これは直動型の緩衝装置を付けたドアを手動で開ける場合をモデル化したものです。

図2 マス・バネ・ダンパからなる機械系

状態空間表現を得るための出発点は，ニュートンの運動第２法則

$\displaystyle{(2)\quad M\ddot{r}(t)=F(t) }$

です。ここで， $M$ はドアの質量， $r(t)$ は時刻 $t$ におけるドアの変位， $F(t)$ は時刻 $t$ においてドアに働く力であり

$\displaystyle{(3)\quad F(t)=-D\dot{r}(t)-Kr(t)+u(t) }$

のように表すことができます。ここで，ダンパが第1項の力を，ばねが第2項の力を与えます。 $u$ は人がドアに与える力とします。

$\displaystyle{(4)\quad M\ddot{r}(t)=-D\dot{r}(t)-Kr(t)+u(t) }$

これは2階の線形微分方程式ですが， $v(t)=\dot{r}(t)$ を定義すると

$\displaystyle{(5)\quad \left\{\begin{array}{l} \dot{r}(t)=v(t) \\ \dot{v}(t)=-\frac{D}{M}v(t)-\frac{K}{M}r(t)+\frac{1}{M}u(t) \end{array}\right. }$

のような1階の連立線形微分方程式で表されます。これらを行列表示すると

$\displaystyle{(6)\quad \underbrace{ \left[\begin{array}{c} \dot{r}(t) \\ \dot{v}(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{K}{M} & -\frac{D}{M} \end{array}\right] }_A \underbrace{ \left[\begin{array}{c} r(t) \\ v(t) \end{array}\right] }_{x(t)}+ \underbrace{ \left[\begin{array}{c} 0 \\ \frac{1}{M} \end{array}\right] }_B u(t) }$

のような状態方程式を得ます。ドアが閉まっている場合の状態と入力は、次のように零状態と零入力として与えられます。

$\displaystyle{(7)\quad x^*= \left[\begin{array}{c} 0 \\ 0 \end{array}\right],\ u^*=0 }$

一方、センサを用いて観測される変数を位置とするとき、これを状態の線形関数として表して、次の出力方程式を得ます。

$\displaystyle{(8)\quad \underbrace{r(t)}_{y(t)}= \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} r(t) \\ v(t) \end{array}\right] }_{x(t)} }$

演習A11…Flipped Classroom

次図のような連結台車を考えます。

図3 連結台車(ACCベンチマーク問題)

これは次の運動方程式で表されます。

$\displaystyle{(9)\quad \left\{\begin{array}{l} m_1\ddot{x}_1(t)=k(x_2(t)-x_1(t))+u(t)+w_1(t)\\ m_2\ddot{x}_2(t)=k(x_1(t)-x_2(t))+w_2(t) \end{array}\right. }$

このとき、次の状態方程式を求めよ。

$\displaystyle{(10)\quad \frac{d}{dt}\left[\begin{array}{c} x_1\\ x_2\\ \dot{x}_1\\ \dot{x}_2 \end{array}\right] = \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ a_{31} & a_{32} & a_{33} & a_{34} \\ a_{41} & a_{42} & a_{43} & a_{44} \end{array}\right] \left[\begin{array}{c} x_1\\ x_2\\ \dot{x}_1\\ \dot{x}_2 \end{array}\right] + \left[\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & 0\\ b_{31} & b_{32} & b_{33}\\ b_{41} & b_{42} & b_{43} \end{array}\right] \left[\begin{array}{c} u\\ w_1\\ w_2 \end{array}\right] }$

Note A11-1　代表的な状態方程式　

代表的な１次系の状態方程式は次式で表されます。

$\displaystyle{(1)\quad \boxed{\dot{x}(t)=\underbrace{-\frac{1}{T}}_{a}x(t)+\underbrace{\frac{K}{T}}_{b}u(t)}}$

代表的な２次系の状態方程式は次式で表されます。

$\displaystyle{(2)\quad \boxed{ \underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] }_{\dot x} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }_{x} + \underbrace{ \left[\begin{array}{c} 0 \\ K\omega_n^2 \end{array}\right] }_{B} u(t) }}$

これらをなぜこのようにパラメタライズするかはあとで述べます。

Note A11-2　直達項をもつ状態空間表現　

次のように直達項をもつ状態空間表現を考えることがあります。

$\displaystyle{(1)\quad \boxed{\left\{\begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)&(x(t)\in{\bf R}^n,u(t)\in{\bf R}^m)\\ y(t)=Cx(t)+Du(t)&(y(t)\in{\bf R}^p) \end{array}\right.} }$

直達項は制御対象のモデルに対して用いられることは稀ですが、あとで述べるコントローラのモデルに対して用いられます（コントローラも状態空間表現として表すこと自体が驚きかもしれませんが）。

Note A11-3　直列結合　

次の２つの状態空間表現

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot x_1(t)=A_1x_1(t)+B_1u_1(t) \\ y_1(t)=C_1x_1(t)+D_1u_1(t) \end{array}\right. }$

$\displaystyle{(2)\quad \left\{\begin{array}{l} \dot x_2(t)=A_2x_2(t)+B_2u_2(t) \\ y_2(t)=C_2x_2(t)+D_2u_2(t) \end{array}\right. }$

に対して直列結合 $u_1=y_2$ を行うと、次の状態空間表現を得ます。

$\displaystyle{(3)\quad \boxed{\left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right]= \left[\begin{array}{cc} A_1 & B_1C_2 \\ 0 & A_2 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + \left[\begin{array}{cc} B_1D_2 \\ B_2 \end{array}\right] u_2(t) \\ y_1(t)= \left[\begin{array}{cc} C_1 & D_1C_2 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] +D_1D_2u_2(t) \end{array}\right.} }$

Note A11-4　座標変換　

状態空間表現

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot x(t)=Ax(t)+Bu(t) \\ y(t)=Cx(t)+Du(t) \end{array}\right. }$

における状態 $x$ を新しい状態 $x'$ に、正則行列 $T$ を用いた

$\displaystyle{(2)\quad x'(t)=Tx(t)\ \Leftrightarrow\ x(t)=T^{-1}x'(t) }$

によって変換すると

$\displaystyle{(3)\quad \left\{\begin{array}{l} T^{-1}\dot{x}'(t)=AT^{-1}x'(t)+Bu(t) \\ y(t)=CT^{-1}x'(t)+Du(t) \end{array}\right. }$

すなわち

$\displaystyle{(4)\quad \boxed{\left\{\begin{array}{l} \dot{x}'(t)=\underbrace{TAT^{-1}}_{A'}x'(t)+\underbrace{TB}_{B'}u(t) \\ y(t)=\underbrace{CT^{-1}}_{C'}x'(t)+Du(t) \end{array}\right.} }$

を得ます。

１次系の追値制御

投稿日時: 2021年10月8日投稿者: cacsd

１次系の追値制御…Homework

[1] 定値外乱 $w$ をもつ１次系

$\displaystyle{(1)\quad \boxed{\dot{x}(t)=ax(t)+bu(t)+w}}$

を考えます。これに状態フィードバック

$\displaystyle{(2)\quad u(t)=-fx(t)}$

を行うと、閉ループ系は

$\displaystyle{(3)\quad \dot{x}(t)=(a-bf)x(t)+w}$

となります。この時間応答は

$(4)\quad \begin{array}{l} \displaystyle{x(t)=e^{(a-bf)t}x(0)+\int_0^t e^{(a-bf)(t-\tau)}w\,d\tau}\\ \displaystyle{=e^{(a-bf)t}x(0)+\left[\frac{1}{-(a-bf)}e^{(a-bf)(t-\tau)}w\right]_0^t}\\ \displaystyle{=e^{(a-bf)t}x(0)+\frac{w}{-(a-bf)}(1-e^{(a-bf)t})}\\ \displaystyle{\rightarrow \frac{w}{-(a-bf)}\ne 0 \quad (t\rightarrow\infty)} \end{array}$

となり、平衡状態への復帰はできないことがわかります。そこで、フィードフォワード項をもつ状態フィードバック

$\displaystyle{(5)\quad u(t)=-fx(t)+v(t)}$

を行うと、閉ループ系は

$\displaystyle{(6)\quad \dot{x}(t)=(a-bf)x(t)+bv(t)+w,\ a-bf<0}$

となり、この時間応答は

$\displaystyle{(7)\quad x(t)=e^{(a-bf)t}x(0)+\int_0^t e^{(a-bf)\tau} \underbrace{(bv(t)+w)}_{to\ be\ 0}\,dt}$

で表されます。ここで、フィードフォワード項を

$\displaystyle{(8)\quad v(t)=-\frac{1}{b}w}$

と定めれば、定値外乱の影響を除去できます。しかしながら、ここでは外乱は一定であることはわかっているが、その値までは分からないと仮定して話を進めていきます。したがって、外乱の値 $w$ を推定する仕組みを工夫する必要があります。

●いま、その仕組みとして、積分動作

$\displaystyle{(9)\quad \dot{x}_I(t)=x(t)\quad (x_I(0)=0)}$

を加えた状態フィードバック

$\displaystyle{(10)\quad {u(t)=-fx(t)\underbrace{-f_Ix_I(t)}_{+v(t)}=-fx(t)-f_I\int_0^tx(\tau)d\tau}}$

を考えます。これによる閉ループ系は

$\displaystyle{(11)\quad {\underbrace{ \left[\begin{array}{cc} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_E} = \underbrace{ \left[\begin{array}{cc} a-bf & -bf_I \\ 1 & 0 \end{array}\right] }_{A_{EF}} \underbrace{ \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{x_E} + \underbrace{ \left[\begin{array}{cc} w \\ 0 \end{array}\right] }_{w_E}} }$

となります。ここで、 $A_{EF}$ の特性方程式は

$\displaystyle{(12)\quad {\rm det}(\lambda I_2-A_{EF})=\lambda^2-\underbrace{(a-bf)}_{<0}\lambda+bf_I}=0$

ですが、その解が複素左半平面に含まれるように、 $f$ と $f_I$ を適切に選んでおきます。

＜このパートは最初は読み飛ばして結構です＞
このとき閉ループ系の時間応答は、 $A_{EF}$ が正則行列なので

$\displaystyle{(13)\quad \int\exp(A_{EF}t)\,dt=A_{EF}^{-1}\exp(A_{EF}t)=\exp(A_{EF}t)A_{EF}^{-1}}}$

に注意して、次のように計算できます。

$(14)\quad \begin{array}{l} \displaystyle{x_E(t)=\exp(A_{EF}t)x_E(0)+\int_0^t\exp(A_{EF}(t-\tau))w_E\,d\tau}\\ \displaystyle{=\exp(A_{EF}t)x_E(0)+\left[-\exp(A_{EF}(t-\tau))A_{EF}^{-1}w_E\right]_0^t}\\ \displaystyle{=\exp(A_{EF}t)x_E(0)-(I_2-\exp(A_{EF}t))A_{EF}^{-1}w_E}\\ \displaystyle{=\exp(A_{EF}t)(x_E(0)+A_{EF}^{-1}w_E)-A_{EF}^{-1}w_E}\\ \displaystyle{\rightarrow -A_{EF}^{-1}w_E \quad (t\rightarrow\infty)} \end{array}$

したがって、 $t\rightarrow\infty$ のとき、次式が成り立ちます。

$(15)\quad \begin{array}{l} \displaystyle{\left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] \rightarrow - \left[\begin{array}{cc} a-bf & -bf_I \\ 1 & 0 \end{array}\right]^{-1} \left[\begin{array}{cc} w \\ 0 \end{array}\right]}\\ \displaystyle{ = -\frac{1}{bf_I} \left[\begin{array}{cc} 0 & bf_I \\ -1 & a-bf \end{array}\right] \left[\begin{array}{cc} w \\ 0 \end{array}\right] = \left[\begin{array}{cc} 0 \\ \frac{1}{bf_I}w \end{array}\right]} \end{array}$

$\displaystyle{(16)\quad u(t)\rightarrow-fx(\infty)-f_Ix_I(\infty)=-\frac{1}{b}w}$

すなわち、積分動作をもつ状態フィードバック(10)により、定値外乱の影響を受けずに、平衡状態に復帰できていることが確かめられます。

●定値外乱 $w$ をもつ１次系(1)に対して、制御目的

$\displaystyle{(17)\quad \boxed{x(t)-r_c\rightarrow 0\quad(t\rightarrow\infty)}}$

を達成するために（ $r_c$ は目標値）、上述の積分動作を次のように変えてみます。

$\displaystyle{(18)\quad \boxed{\dot{x}_I(t)=x(t)-r_c\quad (x_I(0)=0)}}$

これを加えた状態フィードバック

$\displaystyle{(19)\quad \boxed{u(t)=-fx(t)\underbrace{-f_Ix_I(t)}_{+v(t)}=-fx(t)-f_I\int_0^t(x(\tau)-r_c)d\tau} }$

による閉ループ系は

$\displaystyle{(20)\quad \boxed{\underbrace{ \left[\begin{array}{cc} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_E} = \underbrace{ \left[\begin{array}{cc} a-bf & -bf_I \\ 1 & 0 \end{array}\right] }_{A_{EF}} \underbrace{ \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{x_E} + \underbrace{ \left[\begin{array}{cc} w \\ -r_c \end{array}\right] }_{w_E}} }$

となるので、 $t\rightarrow\infty$ のとき、次式が成り立ちます。

$\displaystyle{(21)\quad \begin{array}{l} \displaystyle{\left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] \rightarrow - \left[\begin{array}{cc} a-bf & -bf_I \\ 1 & 0 \end{array}\right]^{-1} \left[\begin{array}{cc} w \\ -r_c \end{array}\right]}\\ \displaystyle{= -\frac{1}{bf_I} \left[\begin{array}{cc} 0 & bf_I \\ -1 & a-bf \end{array}\right] \left[\begin{array}{cc} w \\ -r_c \end{array}\right] = \left[\begin{array}{cc} r_c \\ \frac{1}{bf_I}w+\frac{a-bf}{bf_I}r_c \end{array}\right]} \end{array} }$

$\displaystyle{(22)\quad u(t) \rightarrow -f\underbrace{r_c}_{x(\infty)}-f_I\underbrace{(\frac{1}{bf_I}w+\frac{a-bf}{bf_I}r_c)}_{x_I(\infty)} =-\frac{1}{b}(w+ar_c) }$

すなわち、積分動作をもつ状態フィードバック(19)により、定値外乱の影響を受けずに、制御目的(17)を達成できていることが確かめられます。

(21)から次式を得ます。

$\displaystyle{(23)\quad -f_Ix_I(t)\rightarrow -\frac{1}{b}w\underbrace{-\frac{a-bf}{b}}_{f_r}r_c}$

これから第２項 $f_rr_c$ を、図2のように、フィードフォワードしておくことが考えられます。

図1 積分動作を加えた状態フィードバックによる閉ループ系

● $a=-1$ 、 $b=1$ 、 $w=0$ 、 $r_c=1$ 、 $f=1$ 、 $f_I=1$ として、次のシミュレーション結果を得ます。

図2　図1のシミュレーション例

演習A04-1…Flipped Classroom

図2のグラフを描け。

MATLAB

%a04_1.m
 clear all, close all
 a=1; b=1; c=1; w=0; rc=1; 
 f=3; fI=1; fr=-(a-b*f)/b;
 AFE=[a-b*f -b*fI;
      1      0];
 wE=[w;-rc];
 sys=ss(AFE,wE,[eye(2);-f -fI],[]);
 wE2=[w-(a-b*f);-rc];
 sys2=ss(AFE,wE2,[eye(2);-f -fI],[]);
 x0=[0;0]; 
 t=0:0.1:5;
 u=ones(1,length(t));
 lsim(sys,sys2,u,t,x0)
 grid
 legend('without feed forward','with feed forward')
 title('Tracking Control of 1st-order System')
 xlabel('time')
 ylabel('u(t), xI(t) and x(t)')
%eof

SCILAB

coming soon

[2]　定値外乱 $w$ をもつ１次系

$\displaystyle{(24)\quad \left\{\begin{array}{lll} \dot{x}(t)=ax(t)+bu(t)+w,\ x(0)\ne0\\ y(t)=cx(t) \end{array}\right. }$

に対して、制御目的

$\displaystyle{(25)\quad y(t)-r_c\rightarrow 0\quad(t\rightarrow\infty)}$

を達成するために（ $r_c$ は目標値）、積分動作

$\displaystyle{(26)\quad \dot{x}_I(t)=y(t)-r_c\quad (x_I(0)=0)}$

を加えた状態フィードバック

$\displaystyle{(27)\quad u(t)=-fx(t)-f_I\int_0^t(y(\tau)-r_c)d\tau }$

を考えます。これは、実際には、状態オブザーバ

$\displaystyle{(28)\quad \dot{\hat{x}}(t)=(a-hc)\hat{x}(t)+bu(t)+hy(t),\ \hat{x}(0)=0 }$

によって推定された状態 $\hat{x}$ を用いて

$\displaystyle{(29)\quad u(t)=-f\hat{x}(t)-f_Ix_I(t) }$

を実施することになります。この積分動作を加えたオブザーバベースコントローラは、(29)を(28)に代入した

$\displaystyle{(30)\quad \dot{\hat{x}}(t)=(a-hc-bf)\hat{x}(t)-bf_Ix_I(t)+hy(t) }$

と、(26)を合わせて、つぎのように表されます。

$\displaystyle{(31)\quad \boxed{ \begin{array}{l} \left[\begin{array}{c} \dot{\hat{x}}(t) \\ \dot{x}_I(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} a-hc-bf & -bf_I \\ 0 & 0 \end{array}\right] }_{A_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right] + \underbrace{ \left[\begin{array}{cc} h & 0\\ 1 & -1 \end{array}\right] }_{B_K} \left[\begin{array}{c} y(t) \\ r_c \end{array}\right]\\ u(t)= \underbrace{- \left[\begin{array}{cc} f & f_I \end{array}\right] }_{C_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right] \end{array}} }$

これによる閉ループ系は、(27)を(24)に代入した

$\displaystyle{(32)\quad \dot{x}(t)=ax(t)-bf\hat{x}(t)-bf_Ix_I(t)+w(t) }$

と、(26)、(30)を合わせて

$\displaystyle{(33)\quad \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\ \dot{\hat{x}}(t) \end{array}\right] = \left[\begin{array}{ccc} a & -bf_I & -bf \\ c & 0 & 0 \\ hc & -bf_I & a-hc-bf \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r_c \\ 0 \end{array}\right] }$

のように表されます。そのブロック線図を図3に示します。

いま、閉ループ系(33)に、座標変換

$\displaystyle{(34)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\ e(t) \end{array}\right] = \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ -1 & 0 & 1 \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] }$

を行えば

$\displaystyle{(35)\quad \boxed{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\\hline \dot{e}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc|c} a-bf & -bf_I & -bf \\ c & 0 & 0 \\\hline 0 & 0 & a-hc \end{array}\right] }_{ A_{EF}'= \left[\begin{array}{c|c} A_{EF} & - \left[\begin{array}{cc} bf \\ 0 \end{array}\right] \\[5mm] \hline 0 & \widehat{a} \end{array}\right] } \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r_c \\\hline -w \end{array}\right]} }$

となります。オブザーバの誤差方程式が分離されており、 $f,f_I$ と $h$ を独立に設計してもよいことが分かります。

ここで、 $A_{EF}$ の特性方程式の解は左半平面にあるとすると、(35)より、 $t\rightarrow\infty$ のとき、次式が成り立ちます。

$\displaystyle{(36)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] \ \rightarrow\ \underbrace{ \left[\begin{array}{c|c} A_{EF}^{-1} & -A_{EF}^{-1} \left[\begin{array}{cc} bf \\ 0 \end{array}\right] \widehat{a}^{-1} \\[5mm]\hline 0 & \widehat{a}^{-1} \end{array}\right] }_{A_{EF}'\,^{-1}} \left[\begin{array}{c} -w \\ r_c \\\hline w \end{array}\right] \quad (t\rightarrow\infty) }$

すなわち

$\displaystyle{(37)\quad \begin{array}{l} \displaystyle{\left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] \rightarrow \left[\begin{array}{cc} a-bf & -bf_I \\ c & 0 \end{array}\right]^{-1} \left[\begin{array}{cc} -w-bf\widehat{a}^{-1}w\\ r_c \end{array}\right]}\\ \displaystyle{= \frac{1}{cbf_I} \left[\begin{array}{cc} 0 & bf_I \\ -c & a-bf \end{array}\right] \left[\begin{array}{cc} -w-bf\widehat{a}^{-1}w\\ r_c \end{array}\right]}\\ \displaystyle{= \left[\begin{array}{cc} \frac{1}{c}r_c \\ \frac{1}{bf_I}w+\frac{1}{f_I}f\widehat{a}^{-1}w+\frac{a-bf}{cbf_I}r_c \end{array}\right]} \end{array} }$

$\displaystyle{(38)\quad e(t) \rightarrow \widehat{a}^{-1}w }$

したがって

$\displaystyle{(39)\quad y(t)=cx(t) \rightarrow r_c }$
$\displaystyle{(40)\quad \begin{array}{l} u(t) \rightarrow -f\underbrace{\frac{1}{c}r_c}_{x(\infty)} -f_I\underbrace{(\frac{1}{bf_I}w+\frac{1}{f_I}f\widehat{a}^{-1}w+\frac{a-bf}{cbf_I}r_c)}_{x_I(\infty)}\\ =-\frac{1}{b}(w+\frac{a}{c}r_c)-f\widehat{a}^{-1}w \end{array} }$

を得ます。したがって、定値外乱が存在するときは状態オブザーバに関して定常偏差が残るにもかかわらず、制御目的(24)が成り立つことがわかります。

図3　積分動作を加えたオブザーバベースコントローラによる閉ループ系

１次系のLQI制御…Homework

[3] 定値外乱 $w$ をもつ１次系(23)に対して、制御目的(24)を達成するための制御則として、積分動作(25)をもつ安定化状態フィードバック(26)を考えます。これによる閉ループ系

$\displaystyle{(41)\quad \underbrace{ \left[\begin{array}{cc} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} a-bf & -bf_I \\ c & 0 \end{array}\right] }_{A_{EF}} \underbrace{ \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{x_E(t)} + \underbrace{ \left[\begin{array}{cc} w \\ -r_c \end{array}\right] }_{w_E} }$

に対して、評価関数

$\displaystyle{(42)\quad J=\int_0^\infty(q^2x^2(t)+r^2u^2(t))\,dt}$

を最小化するように $f$ と $f_I$ を決めるLQI制御問題を考えます。いま、出力方程式

$\displaystyle{(43)\quad \underbrace{ \left[\begin{array}{cc} qx(t)\\ ru(t) \end{array}\right] }_{y_{E}(t)} = \underbrace{ \left[\begin{array}{cc} q & 0 \\ -rf & -rf_I \end{array}\right] }_{C_{E}} \underbrace{ \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{x_{E}(t)} }$

を定義すると、閉ループ系の応答は

＜このパートは最初は読み飛ばして結構です＞
$\displaystyle{(44)\quad x_E(t)=\exp(A_{EF}t)(x_E(0)+A_{EF}^{-1}w_E)-A_{EF}^{-1}w_E }$

より

$\displaystyle{(45)\quad y_{E}(t)= C_{E}\exp(A_{EF}t) (x_E(0)+A_{EF}^{-1}w_E) - C_{E}A_{EF}^{-1}w_E }$

となります。ここで、第２項の定数項があるので、評価関数

$\displaystyle{(46)\quad J=\int_0^\infty\underbrace{(q^2y^2(t)+r^2u^2(t))}_{y^T_{E}(t)y_{E}(t)}\,dt }$

は発散してしまいます（そもそも外乱は未知としますので計算もできません）。

●そこで、定値外乱が抑制されているときは、平衡状態（ $x=0$ ）は $x_\infty=r_c$ に、平衡入力（ $u=0$ ）は $u_\infty=-\frac{1}{b}(w+ar_c)$ に変わっていることに注意し、これとの差を表す状態方程式を考えます。まず定値外乱抑制前の状態方程式は(1)と(2)を合わせて

$\displaystyle{(47)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} a & 0 \\ c & 0 \end{array}\right] }_{A_E} \underbrace{ \left[\begin{array}{cc} x(t) \\ x_I(t) \end{array}\right] }_{x_E(t)} + \underbrace{ \left[\begin{array}{cc} b \\ 0 \end{array}\right] }_{B_E} u(t) + \underbrace{ \left[\begin{array}{cc} w \\ -r_c \end{array}\right] }_{w_E} }$

となります。また、定値外乱抑制後には

$\displaystyle{(48)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{cc} x_\infty \\ x_{I\infty} \end{array}\right] }_{\dot{x}_{E\infty}=0} = \underbrace{ \left[\begin{array}{cc} a & 0 \\ c & 0 \end{array}\right] }_{A_E} \underbrace{ \left[\begin{array}{cc} x_\infty \\ x_{I\infty} \end{array}\right] }_{x_{E\infty}} + \underbrace{ \left[\begin{array}{cc} b \\ 0 \end{array}\right] }_{B_E} u_\infty + \underbrace{ \left[\begin{array}{cc} w \\ -r_c \end{array}\right] }_{w_E} }$

が成り立ちます。両者を辺々引き算して

$\displaystyle{(49)\quad \boxed{ \underbrace{\frac{d}{dt} \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{\dot{x}_{E1}(t)} = \underbrace{ \left[\begin{array}{cc} a & 0 \\ c & 0 \end{array}\right] }_{A_{E1}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{x_{E1}(t)} + \underbrace{ \left[\begin{array}{cc} b \\ 0 \end{array}\right] }_{B_{E1}} \underbrace{(u(t)-u_\infty)}_{u_{E1}(t)}} }$

となります。これを偏差系E1と呼ぶことにします。これに安定化状態フィードバック

$\displaystyle{(50)\quad \underbrace{u(t)-u_\infty}_{u_{E1}(t)} =- \underbrace{ \left[\begin{array}{cc} f & f_I \end{array}\right] }_{F_E} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{x_{E1}(t)} }$

を行った閉ループ系は

$\displaystyle{(51)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{\dot{x}_{E1}(t)} = \underbrace{ \left[\begin{array}{cc} a-bf & -bf_I \\ c & 0 \end{array}\right] }_{A_{EF}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{x_{E1}(t)} }$

となります。いま出力方程式

$\displaystyle{(52)\quad \underbrace{ \left[\begin{array}{cc} q(x(t)-x_\infty) \\ r(u(t)-u_\infty) \end{array}\right] }_{y_{E1}(t)} = \underbrace{ \left[\begin{array}{cc} q & 0 \\ -rf & -rf_I \end{array}\right] }_{C_{E1}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{x_{E1}(t)} }$

を定義して、評価関数

$\displaystyle{(53)\quad J=\int_0^\infty\underbrace{(q^2(x(t)-x_\infty)^2+r^2(u(t)-u_\infty)^2)}_{y^T_{E1}(t)y_{E1}(t)}\,dt }$

を最小化して、(50)のフィードバックゲイン $f$ と $f_I$ を決める制御方式を偏差系E1に基づくLQI制御と呼びます。

●さて、偏差系E1すなわち(49)の両辺を微分すると２つめの偏差系E2

$\displaystyle{(54)\quad \boxed{ \underbrace{\frac{d}{dt} \left[\begin{array}{cc} \dot{x}(t) \\ x(t)-r_c \end{array}\right] }_{\dot{x}_{E2}(t)} = \underbrace{ \left[\begin{array}{cc} a & 0 \\ c & 0 \end{array}\right] }_{A_{E2}} \underbrace{ \left[\begin{array}{cc} \dot{x}(t) \\ x(t)-r_c \end{array}\right] }_{x_{E2}(t)} + \underbrace{ \left[\begin{array}{cc} b \\ 0 \end{array}\right] }_{B_{E2}} \underbrace{\dot{u}(t)}_{u_{E2}(t)}} }$

を得ます。また(49)は次のように変形できることに注意します。

$\displaystyle{(55)\quad \left[\begin{array}{cc} \dot{x}(t) \\ x(t)-r_c \end{array}\right] = \underbrace{ \left[\begin{array}{cc} a & b \\ c & 0 \end{array}\right] }_{S} \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }$

これを(15)に代入すると

$(56)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{cc} a & b \\ c & 0 \end{array}\right] }_{S} \frac{d}{dt} \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]=\\ \underbrace{ \left[\begin{array}{cc} a & 0 \\ 1 & 0 \end{array}\right] }_{A_{E2}} \underbrace{ \left[\begin{array}{cc} a & b \\ c & 0 \end{array}\right] }_{S} \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] + \underbrace{ \left[\begin{array}{cc} b \\ 0 \end{array}\right] }_{B_{E2}} \underbrace{\dot{u}(t)}_{u_{E2}(t)} \end{array}$

この左から $S^{-1}$ をかけると３つめの偏差系E3

$\displaystyle{(57)\quad \boxed{\underbrace{ \frac{d}{dt} \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{\dot{x}_{E3}} = \underbrace{ \left[\begin{array}{cc} a & b \\ 0 & 0 \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}} + \underbrace{ \left[\begin{array}{cc} 0 \\ 1 \end{array}\right] }_{B_{E3}} \underbrace{\dot{u}(t)}_{u_{E3}(t)}} }$

を得ます。これに安定化状態フィードバック

$\displaystyle{(58)\quad \underbrace{\dot{u}(t)}_{u_{E3}(t)} =- \underbrace{ \left[\begin{array}{cc} k & k_I \end{array}\right] }_{K_E} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}(t)} }$

を行った閉ループ系は

$\displaystyle{(59)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{\dot{x}_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} a & b \\ -k & -k_I \end{array}\right] }_{A_{EK}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}(t)} }$

となります。いま出力方程式

$\displaystyle{(60)\quad \underbrace{ \left[\begin{array}{cc} q(x(t)-x_\infty) \\ r\dot{u}(t) \end{array}\right] }_{y_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} q & 0 \\ 0 & r\frac{d}{dt} \end{array}\right] }_{C_{E3}} \underbrace{ \left[\begin{array}{cc} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}(t)} }$

を定義して、評価関数

$\displaystyle{(61)\quad J=\int_0^\infty\underbrace{(q^2(x(t)-x_\infty)^2+r^2\dot{u}^2(t))}_{y^T_{E3}(t)y_{E3}(t)}\,dt }$

を最小化して、(59)のフィードバックゲイン $k$ と $k_I$ を決めます。(55)を(59)に代入して

$\displaystyle{(62)\quad \dot{u}(t) =- \underbrace{ \left[\begin{array}{cc} k & k_I \end{array}\right] }_{K_E} \underbrace{ \left[\begin{array}{cc} a & b \\ 1 & 0 \end{array}\right]^{-1} }_{S^{-1}} \left[\begin{array}{cc} \dot{x}(t) \\ x(t)-r_c \end{array}\right] =- \underbrace{ \left[\begin{array}{cc} f & f_I \end{array}\right] }_{F_E=K_ES^{-1}} \left[\begin{array}{cc} \dot{x}(t) \\ x(t)-r_c \end{array}\right] }$

これを積分して

$\displaystyle{(63)\quad u(t) =-fx(t)-f_I\int_0^t(x(\tau)-r_c)\,d\tau }$

を得ます。このようにして(3)のフィードバックゲイン $f$ と $f_I$ を決める制御方式を偏差系E3に基づくLQI制御と呼びます。

[4] 以下に、偏差系E3をLQG制御により安定化して、積分動作を加えたオブザーバベースコントローラを構成する手順を示します。

アルゴリズム＜1次系のLQGI制御＞

入力パラメータ
$a,\,b,\,c,\,q>0,\,r>0,\,\sigma>0,\,\rho>0$
出力パラメータ
$A_K,\,B_K,\,C_K$

ステップ1: 偏差系の安定化

偏差系

$\displaystyle{(64)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} a & b \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E}} {\dot u}(t) }$

を、状態フィードバック

$\displaystyle{(65)\quad {\dot u}(t)=- \left[\begin{array}{cc} k & k_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }$

によるLQ制御で安定化します。その際、評価関数としては

$\displaystyle{(66)\quad \int_0^\infty \left( q_1^2(x(t)-x_\infty)^2+ q_2^2(u(t)-u_\infty)^2+ r^2{\dot u}^2(t)\right)\,dt }$

を用いる。さらに、 $f$ と $f_I$ を、次式から計算します。

$\displaystyle{(67)\quad \left[\begin{array}{cc} f & f_I \end{array}\right] = \left[\begin{array}{cc} k & k_I \end{array}\right] \left[\begin{array}{cc} a & b \\ c & 0 \end{array}\right]^{-1} }$

ステップ2: オブザーバゲイン $h$ の決定

$\displaystyle{(68)\quad {\rm\bf FARE:\ }\displaystyle{-\frac{1}{\rho^2}c^2\Gamma^2+2a\Gamma+\sigma^2=0} \label{eq7.2.28}}$

を解いて， $\Gamma>0$ を求め， $h$ をつぎのように定める。

$\displaystyle{(69)\quad h=\frac{1}{\rho^2}c\Gamma \label{eq7.2.29}}$

ステップ3: 積分動作を加えたオブザーバベースコントローラの構成

$f$ 、 $f_I$ 、 $h$ から、つぎを構成します。

$\displaystyle{(70)\quad \begin{array}{l} \dot{x}_K(t)=A_Kx_K(t)+B_K \left[\begin{array}{c} y(t) \\ r_c \end{array}\right]\\ u(t)=C_Kx_K(t) \end{array} }$

ただし

$\displaystyle{(71)\quad \begin{array}{l} A_K= \left[\begin{array}{cc} a-hc-bf & -bf_I \\ 0 & 0 \end{array}\right]\\ B_K= \left[\begin{array}{cc} h & 0\\ 1 & -1 \end{array}\right]\\ C_K=- \left[\begin{array}{cc} f & f_I \end{array}\right] \end{array} }$

この積分動作を加えたオブザーバベースコントローラによる制御方式をLQGI制御(LQG control with integral action)と呼びます。

１次系のLQ制御

投稿日時: 2021年10月8日投稿者: cacsd

１次系のLQ制御…Homework

[1] ある平衡状態周りの挙動が状態方程式

$\displaystyle{(1)\quad \dot{x}(t)=ax(t)+bu(t) }$

で表される制御対象に対して、状態フィードバック（state feedback）

$\displaystyle{(2)\quad \boxed{u(t)=-fx(t)} }$

を考えます。ここで、定数 $f$ はゲインと呼ばれ、 $a-bf<0$ を満たすものを選びます。(2)を(1)に代入して、次式を得ます。

$\displaystyle{(3)\quad \boxed{\dot{x}(t)=(a-bf)x(t)\quad (a-bf<0)} }$

これは制御対象とコントローラ（状態フィードバック）が閉路を構成することから（図1参照）、閉ループ系（closed-loop system）と呼ばれます。ここで、物理的に興味深いのは、制御対象が漸近安定でなくても（ $a\ge0$ ）、状態フィードバックを行って閉ループ系を漸近安定（ $a-bf<0$ ）にできることです。そのようなゲイン $f$ を決定する問題を状態フィードバックによる安定化問題と言います。

図1 状態フィードバックによる閉ループ系

[2] １次系(1)に対する安定化状態フィードバック(2)による閉ループ系に対して、評価関数

$\displaystyle{(4)\quad \boxed{J=\int_0^\infty(q^2x^2(t)+r^2u^2(t))\,dt} }$

を最小化するように $f$ を決めるLQ制御問題を考えます。

●閉ループ系における状態の振舞いは次式で与えられます。

$\displaystyle{(5)\quad x(t)=e^{(a-bf)t}x(0) }$

したがって、評価関数(4)の第１項は、次式のように計算されます。

$(6)\quad \begin{array}{lll} \displaystyle{J_x=\int_0^\infty q^2x^2(t)\,dt}\\ \displaystyle{=\int_0^\infty q^2e^{2(a-bf)t}x^2(0)\,dt}\\ \displaystyle{=q^2x^2(0)\left[\frac{1}{2(a-bf)}e^{2(a-bf)t}\right]_0^\infty}\\ \displaystyle{=\frac{q^2x^2(0)}{2(a-bf)}\left[\underbrace{e^{2(a-bf)\infty}}_{0}-\underbrace{e^{2(a-bf)0}}_{1}\right]}\\ \displaystyle{=-\frac{q^2}{2(a-bf)}x^2(0)>0\quad (a-bf<0)} \end{array}$

これから $f$ を大きくすれば、 $J_x$ はいくらでも小さくできることが分かります。

●閉ループ系における入力の振舞い $u=-fx$ は次式で与えられます。

$\displaystyle{(7)\quad u(t)=-fe^{(a-bf)t}x(0) }$

したがって、評価関数(4)の第２項は、次式のように計算されます。

$(8)\quad \begin{array}{lll} \displaystyle{J_u=\int_0^\infty r^2u^2(t)\,dt}\\ \displaystyle{=\int_0^\infty r^2f^2e^{2(a-bf)t}x^2(0)\,dt}\\ \displaystyle{=r^2f^2x^2(0)\left[\frac{1}{2(a-bf)}e^{2(a-bf)t}\right]_0^\infty}\\ \displaystyle{=-\frac{r^2f^2}{2(a-bf)}x^2(0)>0\quad (a-bf<0)} \end{array}$

$J_u$ が下に凸で、 $f=\frac{2b}{a}$ において最小値を取ることは、次式から分かります。

$(9)\quad \begin{array}{lll} \displaystyle{\frac{dJ_u}{df}=\frac{d}{df}\left(-\frac{r^2f^2}{2(a-bf)}x^2(0)\right)}\\ \displaystyle{=\frac{-r^2x^2(0)}{2}\left(\frac{2f}{(a-bf)}+(-1)\frac{f^2}{(a-bf)^2}(-b)\right)}\\ \displaystyle{=\frac{-r^2x^2(0)}{2}\frac{2(a-bf)f+f^2b}{(a-bf)^2}}\\ \displaystyle{=\frac{-r^2x^2(0)}{2}\frac{(2a-bf)f}{(a-bf)^2}} \end{array}$

$(10)\quad \begin{array}{lll} \displaystyle{\frac{d^2J_u}{df^2}=\frac{-r^2x^2(0)}{2}\left(\frac{2a-2bf}{(a-bf)^2}+(-2)\frac{(2a-bf)f}{2(a-bf)^3}(-b)\right)}\\ \displaystyle{=-r^2x^2(0)\left(\frac{1}{a-bf}+b\frac{2(a-bf)f+bf^2}{(a-bf)^3}\right)}\\ \displaystyle{=-r^2x^2(0)\frac{(a-bf)^2+2(a-bf)bf+b^2f^2}{(a-bf)^3}}r\\ \displaystyle{=-r^2x^2(0)\frac{(a-bf+bf)^2}{2(a-bf)^3}\}\\ \displaystyle{=-r^2x^2(0)\frac{a^2}{2(a-bf)^3}>0} \end{array}$

いま、 $b=1,q=1,r=1$ として、 $a=-1$ と $a=1$ の場合について、 $J_x$ 、 $J_u$ 、 $J=J_x+J_u$ の外形を描くと、次図のようになります。ここで、 $J$ には○で示す最小値が生まれていることに注意してください。

●それでは $J$ の最小値を与える $f$ を求めてみます。評価関数は

$(11)\quad \begin{array}{lll} \displaystyle{J=-\frac{q^2x^2(0)}{2(a-bf)}-\frac{r^2f^2x^2(0)}{2(a-bf)}}\\ \displaystyle{=\underbrace{-\frac{q^2+r^2f^2}{2(a-bf)}}_{\Pi}x^2(0)} \end{array} }$

のように書けますので、 $J$ の最小化は、次の $\Pi$ の最小化と等価です。

$\displaystyle{(12)\quad \boxed{\Pi=-\frac{q^2+r^2f^2}{2(a-bf)}} }$

図2 評価関数の概形

●この極値を求めるために $f$ で微分すると

$(13)\quad \begin{array}{lll} \displaystyle{\frac{d\Pi}{df}=-\frac{2r^2f}{2(a-bf)}-(-1)\frac{q^2+r^2f^2}{2(a-bf)^2}(-b)}\\ \displaystyle{=-\frac{2r^2(a-bf)f+(q^2+r^2f^2)b}{2(a-bf)^2}}\\ \displaystyle{=\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^2}} \end{array}$

これより

$\displaystyle{(14)\quad \frac{d\Pi}{df}=0 \ \Rightarrow \ bf^2-2af-\left(\frac{q}{r}\right)^2b=0 }$

ここで、 $f$ は $a-bf<0$ を満足させるものでなければならないので

$\displaystyle{(15)\quad \boxed{f=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right)} }$

となります。そしてこれが一意に定まることは、以下のように $\Pi$ が下に凸でであることから分かります。

$(16)\quad \begin{array}{lll} \displaystyle{\frac{d^2\Pi}{df^2}=\frac{2br^2f-2ar^2}{2(a-bf)^2}+(-2)\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^3}(-b)}\\ \displaystyle{=\frac{-r^2}{(a-bf)}+b\frac{-2r^2(a-bf)f-br^2f^2-q^2b}{(a-bf)^3}}\\ \displaystyle{=\frac{-r^2(a-bf)^2-2r^2(a-bf)bf-r^2b^2f^2-q^2b^2}{(a-bf)^3}}\\ \displaystyle{=\frac{-r^2(a-bf+bf)^2-q^2b^2}{2(a-bf)^3}}\\ \displaystyle{=\frac{-r^2a^2-q^2b^2}{2(a-bf)^3}>0} \end{array}$

[2] LQ制御問題の解は求まったので、今度は評価関数(4)の重み係数 $q$ と $r$ の与え方について考えます。まず被積分項 $q^2x^2(t)+r^2u^2(t)$ は物理量の２乗和になっていますので、予め代表時間と代表長さによる無次元化を行っておく必要があります。たとえば、状態変数と入力変数がとりうる範囲を想定して

$\displaystyle{(17)\quad |x(t)|<M_x, |u(t)|<M_u\Rightarrow q=\frac{1}{M_x}, r=\frac{1}{M_u} }$

とすると、重み係数は物理単位の無次元化の役割を果たすことになります。次に(15)による閉ループ系を考えます。

$\displaystyle{(18)\quad \dot{x}(t)=(a-bf)x(t)=-\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}x(t) }$

ここで、 $a=\frac{1}{T},b=\frac{K}{T}$ の場合を考えると

$\displaystyle{(19)\quad \dot{x}(t)=-\frac{1}{T}\sqrt{1+\left(\frac{q}{r}\right)^2K^2}x(t) =-\frac{1}{\frac{T}{\sqrt{1+\left(\frac{q}{r}\right)^2K^2}}}x(t) }$

となって、新しい時定数は元の時定数 $T$ より小さくなることが分かります。これを決めるパラメータは重み係数の比 $q/r$ で、これが大きいほど時定数 $T$ は小さくなります。

●パラメータ $\rho$ を導入して重み係数を次のように設定します。

$\displaystyle{(20)\quad q=\frac{1}{M_x},\ r=\frac{1}{\rho M_u}\ \Rightarrow\ \frac{q}{r}=\rho\frac{M_u}{M_x}}$

ここで、 $\frac{M_u}{M_x}$ は固定されますので、設計パラメータは $\rho$ のみになります。

$a=-1$ 、 $b=1$ 、 $x(0)=1$ 、 $M_x=1$ 、 $M_u=1$ として、設計パラメータを $\rho=2,4,8$ と変えてみると、次の応答を得ます。

図3 重み係数の比 $\rho=q/r$ による応答の変化

演習A03-1…Flipped Classroom

$1^\circ$ 図2のグラフを描け。またパラメータ $\rho=q/r$ の値による状態と入力の振る舞いを観察し、トレードオフの関係について述べよ。

MATLAB

%a03_1.m
clear all, close all
a=-1; b=1; c=1;
Mx=1; Mu=1; q=1/Mx; r0=1/Mu
x0=1; 
t=0:0.1:4;
%-----
rho=2; r=r0/rho;
f1=1/b*(a+sqrt(a^2+(q/r)^2*b^2));
sys1=ss(a-b*f1,b,[c;-f1],[]);
%-----
rho=4; r=r0/rho;
f2=1/b*(a+sqrt(a^2+(q/r)^2*b^2));
sys2=ss(a-b*f2,b,[c;-f2],[]);
%-----
rho=8; r=r0/rho;
f3=1/b*(a+sqrt(a^2+(q/r)^2*b^2));
sys3=ss(a-b*f3,b,[c;-f3],[]);
%-----
initial(sys1,sys2,sys3,x0,t)
grid
legend('rho=2','rho=4','rho=8'); 
title('LQ Control of 1st-order System')
xlabel('time')
ylabel('u(t) and x(t)')
%eof

SCILAB

coming soon

$2^\circ$ ハミルトン行列と呼ばれる

$\displaystyle{(21)\quad M=\left[\begin{array}{cc} a & -r^{-2}b^2 \\ -q^2 & -a \end{array}\right] }$

の安定な固有値

$\displaystyle{(22)\quad \lambda=-\sqrt{a^2+r^{-2}b^2q^2} }$

に対応する固有ベクトルは

$\displaystyle{(23)\quad \left[\begin{array}{cc} v_1 \\ v_2 \end{array}\right] =\left[\begin{array}{cc} 1 \\ \frac{-a-\sqrt{a^2+r^{-2}b^2q^2}}{-r^{-2}b^2} \end{array}\right] }$

となることを確かめ、次式を計算せよ。

$\displaystyle{(24)\quad f=r^{-2}bv_2v_1^{-1} }$

状態オブザーバ…Homework

[3] 状態フィードバックを使うには状態変数の変化をセンサで捉えてフィードバックする必要があるのですが、状態方程式が分かっているのですから、これを計算機内で解いてやれば良いのではないでしょうか？

いま制御対象（１次系）の状態空間表現を

$\displaystyle{(25)\quad \left\{\begin{array}{lll} \dot{x}(t)=ax(t)+bu(t),\ x(0)\ne0\\ y(t)=cx(t) \end{array}\right. }$

とします。これを計算機で解きますが、その状態と出力を区別して

$\displaystyle{(26)\quad \left\{\begin{array}{lll} \dot{\hat{x}}(t)=a\hat{x}(t)+bu(t),\ \hat{x}(0)=0\\ \hat{y}(t)=c\hat{x}(t) \end{array}\right. }$

で表します。(26)から(25)の第１式どうしを辺々引くと

$\displaystyle{(27)\quad \frac{d}{dt}(\hat{x}(t)-x(t))=a(\hat{x}(t)-x(t)),\ \hat{x}(0)\ne0 }$

を得ます。ここで $a<0$ でない限り

$\displaystyle{(28)\quad \hat{x}(t)-x(t)\rightarrow 0\quad (t\rightarrow\infty) }$

とはなりません。そこで、図4に示すような工夫が行われました。

図4 状態オブザーバの考え方

● いま(2)に次のような補正項を加えます。

$\displaystyle{(29)\quad \dot{\hat{x}}(t)=a\hat{x}(t)+bu(t)-h(c\hat{x}(t)-y(t)),\ \hat{x}(0)=0 }$

すなわち

$\displaystyle{(30)\quad \dot{\hat{x}}(t)=(a-hc)\hat{x}(t)+bu(t)+hy(t),\ \hat{x}(0)=0 }$

ここで、 $h$ は適当な定数です。(30)から(26)の第１式どうしを辺々引くと

$\displaystyle{(31)\quad \frac{d}{dt}(\hat{x}(t)-x(t))=(a-hc)(\hat{x}(t)-x(t)),\ \hat{x}(0)\ne0 }$

を得ます。ここで $a-hc<0$ であるとすると

$\displaystyle{(32)\quad \hat{x}(t)-x(t)\rightarrow 0\quad (t\rightarrow\infty) }$

が成り立ちます。すなわち状態オブザーバ(30)の状態 $\hat{x}(t)$ は、制御対象の状態 $x(t)$ を漸近的に推定することができます。このとき $a-hc<0$ を満たす(30)を状態オブザーバ、 $h$ をオブザーバゲインと呼びます。

● １次系

$\displaystyle{(33)\quad \left\{\begin{array}{lll} \dot{x}(t)=ax(t)+bu(t),\ x(0)\ne0\\ y(t)=cx(t) \end{array}\right. }$

に対して、状態オブザーバ

$\displaystyle{(34)\quad \boxed{\dot{\hat{x}}(t)=(a-hc)\hat{x}(t)+bu(t)+hy(t),\ \hat{x}(0)=0\quad (a-hc<0)} }$

によって推定された状態 $\hat{x}$ を用いる状態フィードバック

$\displaystyle{(35)\quad u(t)=-f\hat{x}(t) }$

による閉ループ系の状態方程式は、次式となります。

$\displaystyle{(36)\quad \left[\begin{array}{cc} \dot{x}(t)\\ \dot{\hat{x}}(t) \end{array}\right]= \underbrace{ \left[\begin{array}{cc} a& -bf\\ hc& a-hc-bf \end{array}\right] }_{A_F} \left[\begin{array}{cc} x(t)\\ \hat{x}(t) \end{array}\right] }$

これから閉ループ系の安定性を判定したいのですが、このままでは見当がつきません。そこで、状態の推定誤差を $e(t)=\hat{x}(t)-x(t)$ とおくと、状態変数を変更した閉ループ系の状態方程式として、次式を得ます。

$\displaystyle{(37)\quad \boxed{ \left[\begin{array}{cc} \dot{x}(t)\\ \dot{e}(t) \end{array}\right]= \underbrace{ \left[\begin{array}{cc} a-bf & -bf\\ 0 & a-hc \end{array}\right] }_{A_F'} \left[\begin{array}{cc} x(t)\\ e(t) \end{array}\right]} }$

実際、状態の推定誤差の定義から、

$\displaystyle{(38)\quad \hat{x}(t)=x(t)+e(t) }$

に注意すると、状態フィードバック(35)は

$\displaystyle{(39)\quad u(t)=-fx(t)-fe(t) }$

これによる閉ループ系は、 $\dot{x}(t)=ax(t)+bu(t)$ に代入して

$\displaystyle{(40)\quad \dot{x}(t)=(a-bf)x(t)-bfe(t) }$

となります。これが(37)の第１式に他なりません。また、第２式は(31)そのもの、すなわち次式となります。

$\displaystyle{(41)\quad \dot{e}(t)=(a-hc)e(t) }$

閉ループ系の状態方程式は(40)と(41)に分離されており、 $a-bf$ と $a-hc$ は共に負となるように $f$ と $h$ を独立に設計してもよいことが分かります。この事実は分離定理と呼ばれています。

なお状態フィードバックを状態オブザーバの出力を用いて実施するコントローラ

$\displaystyle{(42)\quad \boxed{ \left\{\begin{array}{l} \dot{\hat{x}}(t)=(a-bf-hc)\hat{x}(t)+hy(t),\ \hat{x}(0)=0\\ u(t)=-f\hat{x}(t) \end{array}\right.} }$

は、オブザーバベースコントローラと呼ばれています。

演習A03-2…Flipped Classroom

$1^\circ$ １次系 $\dot{x}(t)=ax(t)+bu(t),y(t)=cx(t)$ に対する状態フィードバック $u(t)=-fx(t)$ と，状態オブザーバ $\dot{\hat{x}}(t)=(a-hc)\hat{x}(t)+hy(t)+bu(t)$ （ $a-hc<0$ ）を考える。状態オブザーバによって推定された状態を用いる状態フィードバック $u(t)=-f\hat{x}(t)$ による閉ループ系の状態方程式を求めよ。

$2^\circ$ 閉ループ系の $A$ 行列の固有値が
$\lambda_1=-\frac{1}{T_1}$ ， $\lambda_2=-\frac{1}{T_2}$ （ $T_2>T_1>0$ ）となるように， $f$ と $h$ を定めよ。
$f$ と $h$ に， $\lambda_1$ と $\lambda_2$ をどう対応させたか，およびその理由を述べよ。

１次系のLQG制御…Homework

[5] １次系(33)に対して、オブザーバベースコントローラ(42)による閉ループ系に対して、評価関数

$\displaystyle{(43)\quad J=\int_0^\infty(q^2y\,^2(t)+r^2u\,^2(t))\,dt \label{eq7.2.7}}$

を最小化するように、状態フィードバックゲイン $f$ と状態オブザーバゲイン $h$ を決定するLQG制御問題を考えます。

●評価関数は次のように計算できます。

状態方程式

$\displaystyle{(44)\quad \underbrace{ \left[\begin{array}{cc} \dot{x}(t)\\ \dot{e}(t) \end{array}\right] }_{\dot{x}_{CL}(t)} = \underbrace{ \left[\begin{array}{cc} a-bf & -bf\\ 0 & a-hc \end{array}\right] }_{A_{CL}} \underbrace{ \left[\begin{array}{cc} x(t) \\ e(t) \end{array}\right] }_{x_{CL}(t)} }$

と出力方程式

$\displaystyle{(45)\quad \underbrace{ \left[\begin{array}{cc} qx(t)\\ ru(t) \end{array}\right] }_{y_{CL}(t)} = \underbrace{ \left[\begin{array}{cc} q & 0 \\ -rf & -rf \end{array}\right] }_{C_{CL}} \underbrace{ \left[\begin{array}{cc} x(t) \\ e(t) \end{array}\right] }_{x_{CL}(t)} \label{eq7.2.5}}$

で表される閉ループ系の応答は

＜このパートは最初は読み飛ばして結構です＞
$\displaystyle{(46)\quad y_{CL}(t) = C_{CL}\exp(A_{CL}t) x_{CL}(0) \label{eq7.2.6}}$

このとき

$(47)\quad \begin{array}{lll} \displaystyle{J=\int_0^\infty y_{CL}^T(t)y_{CL}(t)\,dt}\\ \displaystyle{=\int_0^\infty x_{CL}^T(0)\exp(A_{CL}^Tt)C_{CL}^TC_{CL}\exp(A_{CL}t)x_{CL}(0)\,dt}\\ \displaystyle{=x_{CL}^T(0)\int_0^\infty \exp(A_{CL}^Tt)\underbrace{C_{CL}^TC_{CL}}_{Q_{CL}}\exp(A_{CL}t)\,dt\,x_{CL}(0)}\\ \displaystyle{=x_{CL}^T(0)\underbrace{\int_0^\infty \exp(A_{CL}^Tt)Q_{CL}\exp(A_{CL}t)\,dt}_{\Pi}\,x_{CL}(0)}\\ \displaystyle{=x_{CL}^T(0)\Pi x_{CL}(0)} \end{array}$

ここで、初期値

$\displaystyle{(48)\quad x_{CL}(0)= \left[\begin{array}{cc} x(0)\\ e(0) \end{array}\right]= \left[\begin{array}{cc} x(0)\\ \hat{x}(0)-x(0) \end{array}\right] \label{eq7.2.9}}$

をどう選ぶかですが、図4に示す閉ループ系を考えます。すなわち、(33)の代わりに、

$\displaystyle{(49)\quad \left\{\begin{array}{lll} \dot{x}(t)=ax(t)+bu(t)+\sigma n_x(t),\ x(0)\ne0\\ y(t)=cx(t)+\rho n_y(t) \end{array}\right. \label{eq7.2.10}}$

を考えます。ここで、新たに加えられた入力 $n_x(t)$ と $n_y(t)$ はそれぞれシステムノイズと観測ノイズとよばれ、定常正規確率過程として扱うのが普通です。特に、 $\sigma^2$ と $\rho^2$ は分散を表しています。ここでは確率的な議論を避けるため、 $n_x(t)$ と $n_y(t)$ は確定的なインパルス入力であると仮定します。

図5 オブザーバベース・コントローラによる閉ループ系の評価

このとき、閉ループ系の状態方程式は、次式となります。

$\displaystyle{(50)\quad &&\left[\begin{array}{cc} \dot{x}(t)\\ \dot{e}(t) \end{array}\right]= \underbrace{ \left[\begin{array}{cc} a-bf & -bf \\ 0 & a-hc \end{array}\right] }_{A_{CL}} \left[\begin{array}{cc} x(t)\\ e(t) \end{array}\right]\nonumber\\ &&+ \underbrace{ \left[\begin{array}{cc} \sigma & 0\\ -\sigma & h\rho \end{array}\right] }_{B_{CL}} \left[\begin{array}{cc} n_x(t)\\ n_y(t) \end{array}\right] \label{eq7.2.11}}$

出力方程式(45)の場合のインパルス応答は、インパルス入力を $n_x$ に与える場合と $x_y$ に与える場合の２通り

＜このパートは最初は読み飛ばして結構です＞
$\displaystyle{(51)\quad y^{(1)}_{CL}(t) = C_{CL}\exp(A_{CL}t) \underbrace{ \left[\begin{array}{cc} \sigma \\ -\sigma \end{array}\right] }_{x_{CL}(0)=B^{(1)}_{CL}} \label{eq7.2.12}}$

$\displaystyle{(52)\quad y^{(2)}_{CL}(t) = C_{CL}\exp(A_{CL}t) \underbrace{ \left[\begin{array}{cc} 0 \\ h\rho \end{array}\right] }_{x_{CL}(0)=B^{(2)}_{CL}} \label{eq7.2.13}}$

があるので、評価関数を改めて次式のように両者の２乗和とします。

$\displaystyle{(53)\quad J'=B_{CL}^{(1)T}\Pi B_{CL}^{(1)}+B_{CL}^{(2)T}\Pi B_{CL}^{(2)} \label{eq7.2.14}}$

これは、 $\Pi= \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]$ とおくと

$\displaystyle{(54)\quad J'=\pi_1\sigma^2+\pi_2(\sigma^2+h^2\rho^2)-2\pi_3\sigma^2 \label{eq7.2.15}}$

と計算できます。ここで、 $\pi_1,\pi_2,\pi_3$ を直接計算することは容易ではないのですが、

＜このパートは最初は読み飛ばして結構です＞
$A_{CL}$ が安定行列であることは， $\pi_1,\pi_2,\pi_3$ が，リャプノフ方程式

$\displaystyle{(55)\quad \begin{array}{lll} && \underbrace{ \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] }_{\Pi} \underbrace{ \left[\begin{array}{cc} a-bf & -bf \\ 0 & a-hc \end{array}\right] }_{A_{CL}}\nonumber\\ &&+ \underbrace{ \left[\begin{array}{cc} a-bf & 0 \\ -bf & a-hc \end{array}\right] }_{A_{CL}^T} \underbrace{ \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] }_{\Pi}\nonumber\\ &&+ \underbrace{ \left[\begin{array}{cc} q^2+r^2f^2 & r^2f^2 \\ r^2f^2 & r^2f^2 \end{array}\right] }_{C_{CL}^TC_{CL}>0} = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array} \label{eq7.2.16}}$

すなわち

$\displaystyle{(56)\quad 2(a-bf)\pi_1+q^2+r^2f^2=0 \label{eq7.2.17}}$
$\displaystyle{(57)\quad -bf\pi_1+(2a-bf-hc)\pi_3+r^2f^2=0 \label{eq7.2.18}}$
$\displaystyle{(58)\quad 2(a-hc)\pi_2-2bf\pi_3+r^2f^2=0 \label{eq7.2.19}}$

の解であることに等しいという結果を用います。

(56)より $\pi_1$ を求めれば，評価関数は

$\displaystyle{(59)\quad J'= \underbrace{ \frac{q^2+r^2f^2}{-2(a-bf)} }_{\pi_1} \sigma^2 +\pi_2(\sigma^2+h^2\rho^2)-2\pi_3\sigma^2 \label{eq7.2.20}}$

となります。ここで，第1項は，(11)で， $x(0)=\sigma$ としたものであることに注意します。いま， $\hat{x}(0)=x(0)$ かつ $w(t)=v(t)=0$ という場合を考えてみると，(50)の第2行目より

$\displaystyle{(60)\quad \dot{e}(t)=(a-hc)e(t),e(0)=0\ \Rightarrow\ e(t)=0\ \Rightarrow\ \hat{x}(t)=x(t) \label{eq7.2.21}}$

だから，(35)は状態フィードバック $u(t)=-fx(t)$ に相当します。この場合の議論はすでに行っており， $\pi_2,\pi_3$ は存在しないので，評価関数(59)は第1項だけとなり，これを最小化する $f$ は，リッカチ方程式

$\displaystyle{(61)\quad -r^{-2}b^2\pi_1^2+2a\pi_1+q^2=0 \label{eq7.2.22}}$

の解 $\pi_1>0$ を用いて次のように与えられます。

$\displaystyle{(62)\quad f=r^{-2}b\pi_1 \label{eq7.2.23}}$

そうすると，評価関数(59)の第2項と第3項は，状態オブザーバを用いるために生じると考えられます。このように，理想的な状態フィードバックに比べて評価関数の値が増えることを評価関数の劣化と呼びます。

つぎに，仮定 $f=r^{-2}b\pi_1$ ， $a-bf<0$ ， $a-hc<0$ のもとでは，(57)より $\pi_3=0$ を得ます。このとき，式(58)は

$\displaystyle{(63)\quad 2(a-hc)\pi_2+r^2f^2=0 \label{eq7.2.24}}$

となり，これから $\pi_2$ が求まります。したがって，評価関数(59)は

$\displaystyle{(64)\quad J'= \underbrace{ \frac{q^2+r^2f^2}{-2(a-bf)} }_{\#1} \sigma^2 + \underbrace{ \frac{\sigma^2+h^2\rho^2}{-2(a-hc)} }_{\#2} r^2f^2 \label{eq7.2.25}}$

と書けます。評価関数の劣化分を最小化するためには，#2を $h$ について最小化すればよいことになります。幸いなことに，#2は#1と同形であるので，これを最小化する $h$ の決定法は $f$ と同様に行ないます。

●以上の議論をまとめると、次のようになります。

アルゴリズム＜1次系のLQG制御＞

入力パラメータ
$a,\,b,\,c,\,q>0,\,r>0,\,\sigma>0,\,\rho>0$
出力パラメータ
$a_K,\,b_K,\,c_K$

ステップ1: 状態フィードバックゲイン $f$ の決定

$\displaystyle{(65)\quad \boxed{{\rm\bf CARE:\ }\displaystyle{-\frac{1}{r^2}b^2\Pi^2+2a\Pi+q^2=0}} }$

を解いて， $\Pi>0$ を求め， $f$ をつぎのように定める。

$\displaystyle{(66)\quad \boxed{f=\frac{1}{r^2}b\Pi} }$

ステップ2: オブザーバゲイン $h$ の決定

$\displaystyle{(67)\quad \boxed{{\rm\bf FARE:\ }\displaystyle{-\frac{1}{\rho^2}c^2\Gamma^2+2a\Gamma+\sigma^2=0}} }$

を解いて， $\Gamma>0$ を求め， $h$ をつぎのように定める。

$\displaystyle{(68)\quad \boxed{h=\frac{1}{\rho^2}c\Gamma} }$

ステップ3: オブザーバベースコントローラの構成

$\displaystyle{(69)\quad \left\{\begin{array}{l} \dot{x}_K(t)=a_Kx_K(t)+b_Ky(t)\\ u(t)=c_Kx_k(t) \end{array}\right. }$

ただし

$\displaystyle{(70)\quad a_K=a-hc-bf,\ b_K=h,\ c_K=-f }$

● $a=-1$ 、 $b=1$ 、 $c=1$ 、 $q=1$ 、 $r=1$ 、 $\sigma=1$ 、 $\rho=0.2$ 、 $x(0)=1$ 、 $\hat{x}(0)=0$ として、状態フィードバックだけとオブザーバベースコントローラを用いた場合とを比較してみると、次の応答を得ます。

図6 状態フィードバックとオブザーバベースコントローラを用いた場合の応答の変化

演習A03-3…Flipped Classroom

$1^\circ$ 図6のグラフを描け。

MATLAB

%a03_2.m
 clear all, close all
 a=-1; b=1; c=1; 
 q=1; r=1; 
 sig=1; rho=0.2;
 f=1/b*(a+sqrt(a^2+(q/r)^2*b^2));
 sys1=ss(a-b*f,[],[c;-f],[]);
 x0=1; 
 t=0:0.1:4;
 initial(sys1,x0,t), hold on
 h=1/c*(a+sqrt(a^2+(sig/rho)^2*c^2));
 AF=[a -b*f;
     h*c a-h*c-b*f];
 sys2=ss(AF,[],diag([c,-f]),[]);
 initial(sys2,[x0;0],t)  
 grid
 legend('sfc','obc')
 title('LQ OBC of 1st-order System')
 xlabel('time')
 ylabel('u(t) and x(t)')
%eof

SCILAB

coming soon

$2^\circ$ 上のシミュレーションにおいて、 $a-hc<a-bc$ となっていることを確かめよ。

Note A03　ラグランジュの未定定数法

１入力１出力１次系に対する最適制御（LQ制御）を考えましたが、一般の多入力多出力高次系の場合も同様な議論ができて、大変有用な制御手段として知られています。その説明はあとで行ないますが、そこではラグランジュの未定定数法によるアプローチが採用されます。

(11)の評価関数 $J$ を最小化する問題を、 $\Gamma$ をラグランジュの未定定数として

$\displaystyle{(1)\quad J=\Pi x^2(0)+\Gamma(2(a-bf)\Pi+q^2+r^2f^2) }$

を最小化する問題として考え直してみます。ここで、制約条件は、リャプノフ方程式と呼ばれる

$\displaystyle{(2)\quad \boxed{2(a-bf)\Pi+q^2+r^2f^2=0} }$

ですが、 $\Pi>0$ は $a-bf<0$ を意味することに注意します。そこで、必要条件として次を得ます。

$\displaystyle{(3)\quad \frac{\partial J}{\partial \Pi}&=&x^2(0)+2(a-bf)\Gamma=0\Rightarrow\Gamma>0 }$
$\displaystyle{(4)\quad \frac{\partial J}{\partial f}&=&(-2b\Pi+2r^2f)\Gamma=0\Rightarrow f=r^{-2}b\Pi }$
$\displaystyle{(5)\quad \frac{\partial J}{\partial \Gamma}&=&2(a-bf)\Pi+q^2+r^2f^2=0 }$

ここで、第２式から得られる $f=r^{-2}b\Pi$ を第３式に代入して

$\displaystyle{(6)\quad 2(a-br^{-2}b\Pi)\Pi+q^2+r^2r^{-4}b^2\Pi^2=0 }$

すなわち、リッカチ方程式と呼ばれる $\Pi$ の２次方程式

$\displaystyle{(7)\quad 2a\Pi-r^{-2}b^2\Pi^2+q^2=0 }$

を得ます。これから $\Pi>0$ を

$\displaystyle{(8)\quad \Pi=\frac{a+\sqrt{a^2+r^{-2}q^2b^2}}{r^{-2}b^2} }$

を求めて、 $F$ は

$\displaystyle{(9)\quad f=r^{-2}b\Pi=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right) }$

のように得られます。十分性の議論はここでは省きます。

１次系の時間応答

投稿日時: 2021年10月8日投稿者: cacsd

１次系の時間応答…Homework

[1] 次の１入力１出力１次系の状態空間表現を考えます。

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}(t)=ax(t)+bu(t)\\ y(t)=cx(t) \end{array}\right. }$

これから、１次系の時間応答の表現式

$\displaystyle{(2)\quad y(t)=ce^{at}x(0)+\int_0^tce^{a(t-\tau)}bu(\tau)d\tau}$

を得ます（Note A02-1参照）。ここで、インパルス応答と呼ばれる

$\displaystyle{(3)\quad \boxed{g(t)=ce^{at}b}}$

を定義しますと、上式は

$\displaystyle{(4)\quad \boxed{y(t)=\underbrace{ce^{at}x(0)}_{zero-input\ response} +\underbrace{\int_0^tg(t-\tau)u(\tau)d\tau}_{zero-state\ response}} }$

すなわち、１次系の時間応答は、零入力応答（第１項）と零状態応答（第２項）の和でとなります。

●実際、 $a=-1,b=1,c=1$ 、 $x(0)=1$ 、 $u(t)=\sin(10t)$ について $y(t)$ を求めてみると次のグラフが得られます。

図1 どれが零状態応答か？

[2] (1)への入力 $u$ として次図の時間関数を考えます。

図2 ステップ入力とインパルス入力( $T_s\rightarrow0$ )

左図において $T_s\rightarrow0$ としたものがステップ入力、

$\displaystyle{(5)\quad u(t)= \left\{\begin{array}{ll} 0 & (t\le0)\\ 1 & (t>0) \end{array}\right.}$

です。これを(2)に代入すると、

$\displaystyle{(6)\quad y(t)=\int_0^tg(t-\tau)\cdot 1 d\tau=\int_{t}^0g(t')(-dt')=\int_0^tg(t')dt'=\int_0^tg(\tau)d\tau}$

より、ステップ応答が次のように表されます。

$\displaystyle{(7)\quad \boxed{y(t)=\int_0^tg(\tau)d\tau}}$

すなわち、インパルス応答を積分してステップ応答が得られます。逆に、ステップ応答を微分してインパルス応答が得られることも分かります。

図2の右図は左図の微分 $\dot{u}$ を表していますが、 $T_s\rightarrow0$ としたものがインパルス入力（デルタ関数）です。実は(3)で定義したインパルス応答はこのインパルス入力に対する応答です。このことを理解するために、いま１次系の状態方程式を微分して

$\displaystyle{(8)\quad \ddot{x}(t)=a\dot{x}(t)+b\dot{u}(t)}$

を得ます。これは１次系に入力 $\dot{u}$ を与えた場合の応答は $\dot{x}(t)$ であることを示しています。図2の左図で $T_s\rightarrow0$ としたステップ入力に対する応答がステップ応答です。図2の右図で $T_s\rightarrow0$ としたインパルス入力はステップ入力の微分ですから、その応答はステップ応答の微分として得られ、インパルス応答となります。すなわち、ステップ応答を微分するとインパルス応答が得られることが分かります。

[3] 特別な３種類の入力（ステップ入力、インパルス入力、正弦波入力）に対する零状態応答（ステップ応答、インパルス応答、正弦波応答）を考え、これらの相互関係を調べます。

図3 １次系の時間応答の相互関係

一般に、「システムを知る」ということは「どんな入力を与えられても出力を推測できる」ことを意味します。したがって、平衡状態（ $x(0)=0$ ）にある場合、インパルス応答さえ分かれば、畳み込み積分を行って出力（零状態応答）が計算できます。この意味で、インパルス応答は重要なのです。また、インパルス応答自体の計算は、 $x(0)=b$ としたときの零入力応答を求めて行います。ステップ応答が分かれば、これを微分してインパルス応答が分かります。また、インパルス応答をラプラス変換したものが、正弦波応答と密接に関係しています。このことはあとで詳しく述べます。

[4] 次のようにパラメータライズされた漸近安定な１次系を考えます。 $T$ 、 $K$ はそれぞれ時定数、定常ゲインと呼ばれます。

$\displaystyle{(9)\quad \left\{\begin{array}{l} \dot{x}(t)=-\frac{1}{T}x(t)+\frac{K}{T}u(t)\\ y(t)=x(t) \end{array}\right. }$

このインパルス応答は次式で与えられます。

$\displaystyle{(10)\quad g(t)=\frac{K}{T}e^{-\frac{1}{T}t}}$

これを積分して

$\displaystyle{(11)\quad y(t)=\frac{K}{T}\int_0^te^{-\frac{1}{T}\tau}d\tau=\frac{K}{T}\left[\frac{e^{-\frac{1}{T}\tau}}{-\frac{1}{T}}\right]_0^t =-K(e^{-\frac{1}{T}t}-1)=K(1-e^{-\frac{1}{T}t}) }$

すなわちステップ応答は次式で表されます。

$\displaystyle{(12)\quad \boxed{y(t)=K(1-e^{-\frac{1}{T}t})}}$

ここで、 $t=T$ のときは

$\displaystyle{(13)\quad y(T)=K(1-\frac{1}{e})=0.632K}$

となります。また、 $t=0$ での接線は

$\displaystyle{(14)\quad y(t)=\dot{x}(0)t=\frac{K}{T}t}$

ですから、 $x(T)=K$ となります。この時定数 $T$ は、最終値 $K$ の63.2%まで到達する時間、または初期速度で到達した場合の時間（初期時刻における接線が $K$ と交わる時刻）として特徴付けられます。

●実際、 $T=0.5,1,2$ 、 $K=1$ について、ステップ応答 $x(t)$ を求めてみると次のグラフが得られます。最終値 $K=1$ に到達する時間は大体 $5T$ であることが分かります。

図4 １次系のステップ応答から時定数を求める補助線とは？

[5] 時定数は平衡状態に復帰する時間の目安にもなります。漸近安定な１次自由系を次のようにパラメータライズします。

$\displaystyle{(15)\quad \dot{x}(t)=-\frac{1}{T}x(t)}$

この解は

$\displaystyle{(16)\quad x(t)=e^{-\frac{1}{T}t}x(0)}$

ですから、 $t=T$ のときは

$\displaystyle{(17)\quad x(T)=\frac{1}{e}x(0)=0.367x(0)}$

となります。また、 $t=0$ での接線は

$\displaystyle{(18)\quad x(t)=\dot{x}(0)t+x(0)=-\frac{1}{T}x(0)t+x(0)}$

ですから、 $x(T)=0$ となります。この時定数 $T$ は、初期値の36.7%まで復帰する時間、または初期速度で復帰した場合の時間（初期時刻における接線が0と交わる時刻）として特徴付けられます。

●実際、時刻 $t=0$ において平衡状態 $x=0$ が乱され $x(0)=1$ になったとし、 $T=0.5,1,2$ について $x(t)$ を求めてみると次のグラフが得られます。平衡状態に復帰する時間は大体 $5T$ であることを表します。

<
図5 １次自由系において平衡状態に復帰する時間（時定数）を定める補助線とは？

演習A02-1…Flipped Classroom

$1^\circ$ 図1の時間応答を描くプログラムを作成せよ。

MATLAB


%a01_1.m
clear all, close all
a=-1; b=1; c=1;
sys=ss(a,b,c,[]);
t=0:0.01:10;
u=sin(10*t);
lsim(sys,u,t,1), hold on
lsim(sys,u,t,0)
lsim(sys,u*0,t,1)
grid
title('Time Response of 1st-order System')
xlabel('time')
ylabel('x(t)')
legend('total resp','zero-state resp','zero-input resp')
%eof

SCILAB

coming soon

$2^\circ$ 図4のステップ応答#1を描き、適当な補助線を引いて、交点の座標から時定数を求めるプログラムを作成せよ。

MATLAB

%a02_2.m
clear all, close all
T1=0.5; T2=1; T3=2; K=1;
a1=-1/T1; a2=-1/T2; a3=-1/T3;
b1=K/T1; b2=K/T2; b3=K/T3; 
c=1;
sys1=ss(a1,b1,c,[]);
sys2=ss(a2,b2,c,[]);
sys3=ss(a3,b3,c,[]);
t=0:0.01:5;
step(sys1,sys2,sys3,t)
grid
hold on
plot([0,5],(1-1/exp(1))*[1 1])
T=ginput(3)
title('Step Responses of 1st-order System')
xlabel('time')
ylabel('x(t)')
legend('T1=0.5','T2=1','T3=2','1-1/e')
%eof

SCILAB

coming soon

[6] １次系(1)をラプラス変換（Note A02-3参照）して、 $x(0)=0$ とすると（零状態応答を考えているので）

$\displaystyle{(19)\quad s\hat{x}(s)-\underbrace{x(0)}_0=a\hat{x}(s)+b\hat{u}(s) \Rightarrow \hat{x}(s)=(s-a)^{-1}b\hat{u}(s) }$

$\displaystyle{(20)\quad \hat{y}(s)=c\hat{x}(s) \Rightarrow \hat{y}(s)=\underbrace{c(s-a)^{-1}b}_{\hat{G}(s)}\hat{u}(s) }$

ここで現れた

$\displaystyle{(21)\quad \boxed{\hat{g}(s)=c(s-a)^{-1}b}}$

を伝達関数と呼びます。伝達関数 $G(s)$ に $s=j\omega$ を代入した $G(j\omega)$ を周波数伝達関数と呼び、正弦波入力に対する時間応答、すなわち周波数応答を特徴づけることができます。

●漸近安定な１次系(9)の伝達関数は次式で与えられます。

$\displaystyle{(22)\quad \hat{g}(s)=\frac{K}{Ts+1} }$

演習A02-2...Flipped Classroom

$1^\circ$ １次系のステップ応答を次式を逆ラプラス変換して求めなさい。

$\displaystyle{(23)\quad \hat{y}(s)=\underbrace{\frac{K}{Ts+1}}_{\hat{g}(s)}\frac{1}{s}=K(\frac{1}{s}-\frac{T}{Ts+1}) }$

$2^\circ$ 次のような無駄時間と呼ばれる特殊な入出力関係を考えます。

$\displaystyle{(24)\quad y(t)=u(t-L)\quad(L>0) }$

これをラプラス変換して無駄時間の伝達関数は次式をとなることを示しなさい。

$\displaystyle{(25)\quad \boxed{\hat{g}(s)=e^{-sL}} }$

ヒント：

$(26)\quad \begin{array}{l} \displaystyle{\hat{y}(s)=\int_0^\infty u(t-L)e^{-st}\,dt =\int_{-L}^\infty u(\tau)e^{-s(\tau+L)}\,d\tau}\\ \displaystyle{=\underbrace{e^{-sL}}_{\hat{g}(s)}\underbrace{\int_0^\infty u(\tau)e^{-s\tau}\,d\tau}_{\hat{u}(s)}} \end{array}$

[7] 漸近安定な１次系(9)を考えます。正弦波入力に対する零状態応答すなわち周波数応答は、Note A02-2を参照して

$\displaystyle{(27)\quad y(t)=\frac{K}{\sqrt{1+(\omega T)^2}}(\sin(\omega t-\tan^{-1}\omega T)+e^{-\frac{1}{T}t}\sin(\tan^{-1}\omega T))}$

ここで、 $t\rightarrow\infty$ とすると

$\displaystyle{(28)\quad \boxed{y(t)\simeq\underbrace{\frac{K}{\sqrt{1+(\omega T)^2}}}_{|\hat{g}(j\omega)|} \sin(\omega t\underbrace{-\tan^{-1}\omega T}_{\angle\hat{g}(j\omega)})} }$
ただし
$\displaystyle{(29)\quad \hat{g}(s)=\frac{K}{Ts+1} }$

これは、入力が正弦波のときは、時間が十分立てば、出力も正弦波となることを示しています。その振幅と位相はそれぞれ $\hat{g}(j\omega)$ の絶対値と偏角となっています。

$|\hat{g}(j\omega)|$ をゲイン、 $\angle\hat{g}(j\omega)$ を位相と呼びます。このゲイン線図と位相線図をペアにして片対数グラフにプロットしたものをボード線図と呼びます。ゲインはdb値( $20{\log_{10}|\hat{g}(j\omega)|$ )、位相はdeg値( $\frac{180}{\pi}\angle\hat{g}(j\omega)$ )です。

●実際、 $T=0.5,1,2$ について、ボード線図を描いてみると次のグラフが得られます

図6 １次系のボード線図から時定数を求める補助線とは？

ちなみに

$\displaystyle{(30)\quad |\hat{G}(j\omega_b)|=\frac{|\hat{G}(j0)|}{\sqrt{2}}}$

を満足する $\omega_b$ は

$\displaystyle{(31)\quad \omega_b=\frac{1}{T}}$

となり、帯域幅と呼びます。実際、上図で-3dBの水平線との交点が $1/T$ であることが確かめられます。

演習A02-3...Flipped Classroom

$1^\circ$ 図1のゲイン曲線#1を描き、3dB下がる点の座をマウスをクリックして取得し、時定数を求めるプログラムを作成せよ。

MATLAB

%a02_3.m
clear all, close all
T1=0.5; T2=1; T3=2; K=1;
a1=-1/T1; a2=-1/T2; a3=-1/T3;
b1=K/T1; b2=K/T2; b3=K/T3; 
c=1;
sys1=ss(a1,b1,c,[]);
sys2=ss(a2,b2,c,[]);
sys3=ss(a3,b3,c,[]);
sys0=ss([],[],[],1/sqrt(2));
w=logspace(-1,1);
bode(sys1,sys2,sys3,sys0,w)
grid
W=ginput(3)
title('Bode Diagrams of 1st-order System')
xlabel('Freq')
ylabel('Gain')
%ylabel('Phase')
legend('T1=0.5','T2=1','T3=2','-3dB')
%eof

SCILAB

coming soon

Note A02-1　状態方程式の解

●(１次自由系を表す)微分方程式

$\displaystyle{(1)\quad \dot{x}(t)=ax(t)}$

の解は

$\displaystyle{(2)\quad x(t)=e^{at}x(0)}$

と表されます。これが解であることは元の微分方程式に代入すればすぐに確かめられ、また次のようにして導くことができます。

元の微分方程式を

$\displaystyle{(3)\quad \dot{x}(t)-ax(t)=0}$

と書いて、左から積分因数と呼ばれる $e^{-at}$ をかけると

$\displaystyle{(4)\quad e^{-at}\dot{x}(t)-ae^{-at}x(t)=0}$

すなわち

$\displaystyle{(5)\quad \frac{d}{dt}(e^{-at}x(t))=0}$

したがって、 $c$ を定数として

$\displaystyle{(6)\quad e^{-at}x(t)=c\ \Rightarrow\ x(t)=e^{at}c}$

ここで、 $t=0$ とおくと $c$ は初期値 $x(0)$ に等しいので

$\displaystyle{(7)\quad \boxed{x(t)=e^{at}x(0)}}$

と表されます。

●(１次系の状態方程式を表す)微分方程式

$\displaystyle{(8)\quad \dot{x}(t)=ax(t)+bu(t)}$

の解を求めてみます。上と同様にして

$\displaystyle{(9)\quad \frac{d}{dt}(e^{-at}x(t))=e^{-at}bu(t)}$

までは問題ないと思います。これを $0$ から $t'$ まで積分して

$\displaystyle{(10)\quad \left[e^{-at}x(t)\right]_0^{t'}=\int_0^{t'}e^{-at}bu(t)dt}$

すなわち

$\displaystyle{(11)\quad e^{-at'}x(t')-x(0)=\int_0^{t'}e^{-at}bu(t)dt}$

したがって

$\displaystyle{(12)\quad x(t')=e^{at'}x(0)+e^{at'}\int_0^{t'}e^{-at}bu(t)dt}$

ここで、 $t$ を $\tau$ に、 $t'$ を $t$ と置き換えれば

$\displaystyle{(13)\quad \boxed{x(t)=e^{at}x(0)+\int_0^te^{a(t-\tau)}bu(\tau)d\tau}}$

が得られます。

Note A02-2　正弦波応答

(１次系の状態方程式を表す)微分方程式

$\displaystyle{(1)\quad \dot{x}(t)=ax(t)+bu(t) }$

に対して、正弦波入力

$\displaystyle{(2)\quad u(t)=\sin\omega t}$

に対する解を求めます。これは公式

$\displaystyle{(3)\quad \int e^{ax}\sin bx\,dx=\frac{e^{ax}}{a^2+b^2}(a\sin bx-b \cos bx)}$

を用いて、次のように得られます。

$(4)\quad \begin{array}{l} \displaystyle{x(t)=\int_0^te^{a(t-\tau)}b \sin\omega\tau\,d\tau=e^{at}b\int_0^te^{-a\tau} \sin\omega\tau\,d\tau}\\ \displaystyle{=e^{at}b\left[\frac{e^{-a\tau}}{a^2+\omega^2}(-a\sin\omega\tau-\omega\cos\omega\tau)\right]_0^t}\\ \displaystyle{=\frac{e^{at}b}{a^2+\omega^2}(e^{-at}(-a\sin\omega t-\omega\cos\omega t)+\omega)}\\ \displaystyle{=\frac{b}{\sqrt{a^2+\omega^2}}(\sin\omega t\frac{-a}{\sqrt{a^2+\omega^2}}-\cos\omega t\frac{\omega}{\sqrt{a^2+\omega^2}}+e^{+at}\frac{\omega}{\sqrt{a^2+\omega^2}})}\\ \displaystyle{=\frac{b}{\sqrt{a^2+\omega^2}}(\sin\omega t\cos\theta-\cos\omega t\sin\theta+e^{+at}\sin\theta)}\\ \displaystyle{=\frac{b}{\sqrt{a^2+\omega^2}}(\sin(\omega t-\theta)+e^{+at}\sin\theta)\quad (\theta=\tan^{-1}\frac{\omega}{-a})}\\ \displaystyle{=\frac{b}{\sqrt{a^2+\omega^2}}(\sin(\omega t+\phi)-e^{+at}\sin\phi)\quad (\phi=-\theta)} \end{array}$

Note A02-3　ラプラス変換

時間関数 $x(t)$ のラプラス変換は次式によって定義されます。

$\displaystyle{(1)\quad \boxed{\hat{x}(s)=\int_0^\infty x(t)e^{-st}\,dt}}$

Note A02-4　dB値 $20{\log_{10}(\cdot)$

１次系の状態空間表現

投稿日時: 2021年10月8日投稿者: cacsd

一般には制御対象は非線形系ですが、ここでは重ね合わせの原理の成り立つ線形系（linear system）を考えます。また１入力１出力１次系（SISO 1st-order system）に限って、制御理論の概要を漸近安定性、時間応答、安定化制御、追値制御の４つに分けて述べます。

１次系の状態空間表現…Homework

[1] 　制御対象が１入力１出力１次系の場合、そのモデルとして、次の状態空間表現が用いられます。

$\displaystyle{(1)\quad \boxed{\left\{\begin{array}{ll} \dot{x}(t)=ax(t)+bu(t)&(x(t),u(t)\in{\bf R})\\ y(t)=cx(t)&(y(t)\in{\bf R}) \end{array}\right.} }$

第１式は状態方程式とよばれ、 $x(t),u(t)$ はそれぞれ時刻 $t$ における状態変数、入力変数（アクチュエータを操作する）を表しています。また、第２式は出力方程式とよばれ、 $y(t)$ は時刻 $t$ における出力変数（センサを用いて観測される）を表しています。 ${\bf R}$ は実数の集合を表します。以下では状態空間表現(1)を簡単に１次系と参照します。

この状態空間表現は図3のようなブロック線図で表されます。

図1 １入力１出力１次系状態空間表現のブロック線図

●具体例を示すために、次図のような走行する車を考えます。

図2 走行する車

ニュートンの運動第２法則「質量×加速度＝外力」を適用するため、車は質点とみなし、直線運動をしているとします。車の質量を $m$ 、時刻 $t$ における車の速度を $v(t)$ 、車に働く外力（制動力）を $f(t)$ とすると、車の運動方程式は次の微分方程式で与えられます。

$\displaystyle{(2)\quad m\dot{v}(t)=f(t)\qquad(v(0)\ne0) }$

ここで、 $\dot{v}(t)=\frac{d}{dt}v(t)$ は車の加速度を表します。もし空気による抗力 $cv(t)$ を考慮する場合は、車の運動方程式は次式となります。

$\displaystyle{(3)\quad m\dot{v}(t)=-cv(t)+f(t) }$

いま車は一定速度 $v^*$ で等速運動をしているとします。このとき加速度は零となるので

$\displaystyle{(4)\quad 0=-cv^*+f^* }$

を満たす一定の制動力 $f^*=cv^*$ が定まります。実際には、この等速運動が何らかの原因により乱されたとき、これを速やかに元に戻すことが要求されます。そこで(3)から(4)を辺々引き算すると次式を得ます。

$\displaystyle{(5)\quad \underbrace{\frac{d}{dt}(v(t)-v^*)}_{\dot{x}(t)}=\underbrace{-\frac{c}{m}}_{a}\underbrace{(v(t)-v^*)}_{x(t)}+\underbrace{\frac{1}{m}}_{b}\underbrace{(f(t)-f^*)}_{u(t)} }$

これが入力１出力１次系の状態方程式の一例です。ここで、 $x=0$ と $u=0$ が、平衡状態 $v^*$ とこれを維持する平衡入力 $f^*$ を表していることに留意します。

１次系の漸近安定性…Homework

[2]　1次系(1)で表される制御対象は平衡状態にあるとします（ $x=0, u=0$ ）。いま平衡状態が乱され、時刻 $t=0$ において $x(0)\ne 0$ となったとします。特に制動力を変えないとすると $u=0$ です。このとき、元の平衡状態 $x=0$ に復帰できるならば、１次系(1)は漸近安定(asymptotically stable)であると言います。そこで

$\displaystyle{(6)\quad \boxed{\dot{x}(t)=ax(t)\qquad(x(0)\ne0)} }$

の解

$\displaystyle{(7)\quad x(t)=e^{at}x(0) }$

の振舞いを調べてみます。 $t\rightarrow\infty$ のとき

$\displaystyle{(8)\quad x(t) \rightarrow \left\{\begin{array}{ll} 0&(a<0)\\ x(0)&(a=0)\\ \infty&(a>0) \end{array}\right. }$

となるので、１次系(1)の漸近安定性の必要十分条件は

$\displaystyle{(9)\quad \boxed{a<0} }$

であることがわかります。

このように漸近安定性は、(1)において $u=0$ とした自由系(unforced system)(6)に基づいて判定されることに留意してください。

●上の等速運動をしている車の例を考えます。いま突風による急激な速度変化のため $v(0)\ne v^*$ となり、特に制動力を変えないとすると $u=0$ なので

$\displaystyle{(10)\quad \underbrace{\frac{d}{dt}(v(t)-v^*)}_{\dot{x}(t)}=\underbrace{-\frac{c}{m}}_{a}\underbrace{(v(t)-v^*)}_{x(t)} \qquad(x(0)=v(0)-v^*\ne0) }$

を解いて、元の速度 $v^*$ に復帰できるかを判断できます。ここで、 $a<0$ ですから、漸近安定であることが確かめられます。

以上の議論は数学的には明らかですが、物理的にはどのような力が働いて平衡状態に戻るのでしょうか？それは空気抵抗 $-cv(t)$ が速度の増減によって、逆方向に働くからといえます。速度に比例する抗力を減衰力、位置に比例する抗力を復元力といいます。モノつくりにおいて、これらを内在させることをパッシブ制御、コントローラ（補償器）を用いて補うことアクティブ制御といいます。

演習A01…Flipped Classroom

$1^\circ$ 状態方程式 $\dot{x}(t)=ax(t)+bu(t)$ が与えられるときの漸近安定性の判定は、なぜ $\dot{x}(t)=ax(t)$ に基づいて行うのか説明せよ。

$2^\circ$ MATLABまたはSCILABに次のコマンドを与えて、どのグラフが漸近安定であるか述べよ。

MATLAB

%a01.m
clear all, close all
a1=1; a2=0; a3=-5; 
b=1; c=1;
sys1=ss(a1,b,c,[]);
sys2=ss(a2,b,c,[]);
sys3=ss(a3,b,c,[]);
t=0:0.01:1;
x0=1;
initial(sys1,sys2,sys3,x0,t)
grid
title('Stability of 1st-order System')
xlabel('time')
ylabel('x(t)')
legend('a=1','a=0','a=-5')
%eof

SCILAB

coming soon

図3 どのグラフが漸近安定であるか？

制御技術セミナー

投稿日時: 2021年10月7日投稿者: cacsd

現代制御　制御対象はダイナミックスの支配原理（物理）に逆らっては動けません。その支配方程式は微分方程式で記述されるので、制御対象のモデルもここを出発点とするのが自然です。制御とは「平衡状態の安定化」と捉えることができます。これらに沿う制御理論が現代制御（状態空間法）です。現代制御の理論にはストーリ性があって、漸近安定性、状態フィードバックと可制御性、状態オブザーバと可観測性、最適制御など、極めて美しく体系化されています。最適制御では、なんと未知変数が行列の場合の２次方程式（リッカチ方程式）を解きますが、現在の計算機を用いればこれは難なくこなせます。また、多入力・多出力系や閉ループ系の扱いもスマートです。これまで制御を意識したモノつくり（対象を敢えて不安定にして制御で安定化）が多くのイノベーションを生み出してきました。本セミナーでは現代制御の基礎知識を説明します。

ロバスト制御１（ゲインスケジューリング制御）　現代制御では制御対象のモデルを準備しなければなりませんが、一般には正確なモデルの構築は困難です。そこで、モデルの不確かさをどう扱うかが議論され、ポストモダンとしてのロバスト制御理論が発展してきました。特に、古典制御でのループ整形の考え方をベースにした混合感度問題への定式化がよく知られています。これはモデルの不確かさを入出力伝達特性の摂動によって間接的に扱う場合ですが、支配方程式のパラメータの不確かさを直接的に扱う場合もあります。パラメータが時間変動しない場合はラウスフルビッツの安定判別法が有用であり、変動パラメータの場合はゲインスケジューリングを行うことが試みられてきました。近年は線形行列不等式を制約条件とする最適化問題への定式化によって、混合感度問題やゲインスケジューリング問題の求解計算が容易になってきました。本セミナーではゲインスケジューリング制御の基礎知識を説明します。

ロバスト制御２（スライディングモード制御）　ロバスト制御に対するもう一つのアプローチがスライディングモード制御です。これは可変構造制御の一つで、線形制御則とスイッチング制御則からなります。瞬時瞬時に制御則を切り替えることにより、切り替えない場合に比べて優位性を生み出すことができます。スライディングモード制御は「スイッチング直線の近傍」まで状態を持っていく動作と、「スイッチング直線内に閉じ込める」２つの動作からなり、これによりモデルの不確かさに対応できるロバスト制御の特徴を持たせることができます。スイッチング関数の選択には現代制御のLQ設計法や固有ベクトル配置法が用いられ、リヤプノフ関数を構成することにより閉ループ系の安定性を保証することができます。本セミナーではスライディングモード制御の基礎知識を説明します。

水中線状構造物：状態方程式

投稿日時: 2018年8月15日投稿者: cacsd

● $\displaystyle{\bf Derivation \#3a}$
$\displaystyle{\eta(\xi,\tau) =\displaystyle\sum_{i=1}^\infty q_i(\tau)\phi_i(\xi)\simeq q^T(\tau)\phi(\xi)}$
$\displaystyle{ q(\tau)= \left[\begin{array}{c} q_1(\tau) \\ \vdots\\ q_N(\tau) \\ \end{array}\right],\ \phi(\xi)= \left[\begin{array}{c} \phi_1(\xi) \\ \vdots\\ \phi_N(\xi) \\ \end{array}\right] }$

for $i=1,\cdots,N$
$\displaystyle{\sum_{j=1}^N {m^y_{ij}} \times\ddot{q}_j +\sum_{j=1}^N {c^y_{ij}} \times\dot{q}_j +\sum_{j=1}^N {k^y_{ij}} \times q_j }$
$\displaystyle{+\sum_{j,k,\ell=1}^N{B^y_{ijk\ell}} \times q_j q_k q_\ell+\sum_{j,k,\ell=1}^N{D^y_{ijk\ell}} \times q_j q_k \dot{q}_\ell {+\sum_{j,k,\ell=1}^N{E^y_{ijk\ell}} \times q_j \dot{q}_k \dot{q}_\ell+\sum_{j,k,\ell=1}^N F^y_{ijk\ell} \times q_j q_k \ddot{q}_\ell}$
$\displaystyle{+\sum_{j,k,\ell=1}^N{H^y_{ijk\ell}} \times q_j p_k p_\ell+\sum_{j,k,\ell=1}^N{L^y_{ijk\ell}} \times q_j p_k \dot{p}_\ell +\sum_{j,k,\ell=1}^N{M^y_{ijk\ell}} \times q_j \dot{p}_k \dot{p}_\ell+\sum_{j,k,\ell=1}^N N^y_{ijk\ell} \times q_j p_k \ddot{p}_\ell}$
$\displaystyle{=\int_0^1\phi(\xi)\frac{L^3}{EI} f_yd\xi}$

● $\displaystyle{\bf Derivation \#3b}$
$\displaystyle{\zeta(\xi,\tau)=\displaystyle\sum_{i=1}^\infty p_i(\tau)\psi_i(\xi)\simeq p^T(\tau)\psi(\xi)}$
$\displaystyle{ p(\tau)= \left[\begin{array}{c} p_1(\tau) \\ \vdots\\ p_N(\tau) \\ \end{array}\right],\ \psi(\xi)= \left[\begin{array}{c} \psi_1(\xi) \\ \vdots\\ \psi_N(\xi) \\ \end{array}\right] }$

for $i=1,\cdots,N$
$\displaystyle{\sum_{j=1}^N {m^z_{ij}} \times\ddot{p}_j +\sum_{j=1}^N {c^z_{ij}} \times\dot{p}_j +\sum_{j=1}^N {k^z_{ij}} \times p_j }$
$\displaystyle{+\sum_{j,k,\ell=1}^N{B^z_{ijk\ell}} \times p_j p_k p_\ell +\sum_{j,k,\ell=1}^N{D^z_{ijk\ell}} \times p_j p_k \dot{p}_\ell +\sum_{j,k,\ell=1}^N{E^z_{ijk\ell}} \times p_j \dot{p}_k \dot{p}_\ell +\sum_{j,k,\ell=1}^N F^z_{ijk\ell} \times p_j p_k \ddot{p}_\ell}$
$\displaystyle{+\sum_{j,k,\ell=1}^N{H^z_{ijk\ell}} \times p_j q_k q_\ell +\sum_{j,k,\ell=1}^N{L^z_{ijk\ell}} \times p_j q_k \dot{q}_\ell +\sum_{j,k,\ell=1}^N{M^z_{ijk\ell}} \times p_j \dot{q}_k \dot{q}_\ell +\sum_{j,k,\ell=1}^N N^z_{ijk\ell} \times p_j q_k \ddot{q}_\ell}$
$\displaystyle{=\int_0^1\psi(\xi)\frac{L^3}{EI} f_zd\xi}$

● $\displaystyle{\bf Derivation \#4}$

$\displaystyle{\left[\begin{array}{cc} M_{qq} & M_{qp} \\ M_{pq} & M_{pp} \\ \end{array}\right] \left[\begin{array}{c} \ddot{q} \\ \ddot{p} \end{array}\right] +\left[\begin{array}{cc} D_{qq} & D_{qp} \\ D_{pq} & D_{pp} \\ \end{array}\right] \left[\begin{array}{c} \dot{q} \\ \dot{p} \end{array}\right]}$
$\displaystyle{ +\left[\begin{array}{cc} K_{qq} & K_{qp} \\ K_{pq} & K_{pp} \\ \end{array}\right] \left[ \begin{array}{c} q \\ p \end{array} \right] = \left[\begin{array}{c} \int_0^1\phi(\xi)\frac{L^3}{EI} f_yd\xi \\ \int_0^1\psi(\xi)\frac{L^3}{EI} f_zd\xi \end{array}\right]}$

$\displaystyle{M_{qq}=[m^y_{ij} + \sum_{l,k=1}^NF^y_{ilkj}q_lq_k]_{i,j=1,\cdots,N}}$
$\displaystyle{M_{qp}=[\sum_{l,k=1}^N N^y_{ilkj} q_l p_k]_{i,j=1,\cdots,N}}$
$\displaystyle{M_{pq}=[\sum_{l,k=1}^N N^z_{ilkj} p_l q_k]_{i,j=1,\cdots,N}}$
$\displaystyle{M_{pp}=[m^z_{ij} + \sum_{l,k=1}^N F^z_{ilkj}p_l p_k]_{i,j=1,\cdots,N}}$
$\displaystyle{D_{qq}=[c^y_{ij} + \sum_{l,k=1}^ND^y_{ilkj}q_lq_k+ \sum_{l,k=1}^NE^y_{ilkj}q_l\dot{q}_k]_{i,j=1,\cdots,N}}$
$\displaystyle{D_{qp}=[\sum_{l,k=1}^NL^y_{ilkj}q_lp_k+\sum_{l,k=1}^NM^y_{ilkj}q_l\dot{p}_k]_{i,j=1,\cdots,N}}$
$\displaystyle{D_{pq}=[\displaystyle\sum_{l,k=1}^NL^z_{ilkj}p_lq_k+\displaystyle\sum_{l,k=1}^NM^z_{ilkj}p_l\dot{q}_k]_{i,j=1,\cdots,N} }$
$\displaystyle{D_{pp}=[c^z_{ij} + \displaystyle\sum_{l,k=1}^ND^z_{ilkj}p_lp_k+ \displaystyle\sum_{l,k=1}^NE^z_{ilkj}p_l\dot{p}_k]_{i,j=1,\cdots,N} }$
$\displaystyle{K_{qq}=[k^y_{ij} + \displaystyle\sum_{l,k=1}^NB^y_{ilkj}q_kq_l+ \displaystyle\sum_{l,k=1}^NH^y_{ilkj}p_kp_l]_{i,j=1,\cdots,N}}$
$\displaystyle{K_{pp}=[k^z_{ij} + \displaystyle\sum_{l,k=1}^NB^z_{ilkj}p_kp_l+ \displaystyle\sum_{l,k=1}^NH^z_{ilkj}q_kq_l]_{i,j=1,\cdots,N}}$