投稿者「cacsd」のアーカイブ

状態フィードバック

投稿日時: 投稿者:

状態フィードバック…Homework

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

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t)\quad(x(t)\in{\rm\bf R}^n,u(t)\in{\rm\bf R}^m) }

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

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

を考えます。ここで、mを入力変数の個数、nを状態変数の個数とするとき、Fはサイズm\times nのゲイン行列です。(2)を(1)に代入して、次式を得ます。

\displaystyle{(3)\quad \boxed{\dot{x}(t)=(A-BF)x(t)} }

以上の状況は次図のように図示されます。

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

これは、制御対象とコントローラ（状態フィードバック）が閉路を構成することから、閉ループ系と呼ばれます。ここで、興味深いのは、制御対象が不安定であっても（行列Aの固有値の一部が複素左半平面にあっても）、状態フィードバックを行って閉ループ系を構成すると、行列A-BFの固有値をすべて複素左半平面に移動させる可能性があることです。どのような条件の下で、どのようにゲイン行列Fを決定できるかを検討する問題を状態フィードバックによる安定化問題と言います。

●状態フィードバックゲイン行列Fを求めることは、適当な安定行列A_{F}を与えて、行列方程式

\displaystyle{(4)\quad A-BF=A_F\Leftrightarrow BF=A-A_F }

を解くことに他なりません。よく学生さんは次のように解きます。

\displaystyle{(5)\quad F=B^{-1}(A-A_F) }

でも一般にはBは正方行列ではないので、そもそも逆行列を考えることができません。A_Fを安定行列とする状態フィードバックが求まる条件の説明は次節で行いますが、ここではその準備として、状態フィードバックゲイン行列Fを求める公式を示します。

[2]　いま行列A_F=A-BFの特性多項式を次式で表します。

\displaystyle{(6)\quad {\rm det}(\lambda I_n-A_F)=\lambda^n+a_1'\lambda^{n-1}+\cdots+a_n' %=(\lambda-\lambda_1')\times\cdots\times(\lambda-\lambda_n') }

ケーリーハミルトンの定理から次式が成り立ちます。

\displaystyle{(7)\quad A_F^n+a_1'A_F^{n-1}+\cdots+a_{n-1}'A_F+a_n'I_n=0_{n\times n} }

これはn=2のとき、次のように書けます。

\displaystyle{(8)\quad \begin{array}{l} A_F^2+a_1'A_F+a_2'I_2=0\\ \quad\Downarrow\\ (A-BF)A_F+a_1'(A-BF)+a_2'I_2=0\\ \quad\Downarrow\\ AA_F-BFA_F+a_1'A-a_1'BF+a_2'I_2=0\\ \quad\Downarrow\\ A(A-BF)+a_1'A+a_2'I_2=BFA_F+a_1'BF\\ \quad\Downarrow\\ A^2+a_1'A+a_2'I_2=BF(A_F+a_1'I_2)+ABF\\ \quad\Downarrow\\ A^2+a_1'A+a_2'I_2=\left[\begin{array}{ccc} B &AB \end{array}\right] \left[\begin{array}{ccc} F(A_F+a_1'I_2)\\ F \end{array}\right] \end{array} }

したがって、１入力系で\left[\begin{array}{ccc} B &AB \end{array}\right]が正則ならば、次の公式を得ます。

\displaystyle{(9)\quad \boxed{F= \left[\begin{array}{ccc} 0 &1 \end{array}\right] \left[\begin{array}{ccc} B &AB \end{array}\right]^{-1} (A^2+a_1'A+a_2'I_2)} }

同様に、n=3のとき、次のように書けます。

\displaystyle{(10)\quad \begin{array}{l} A_F^3+a_1'A_F^2+a_2'A_F+a_3'I_3=0\\ \quad\Downarrow\\ (A-BF)(A_F^2+a_1'A_F+a_2'I_3)+a_3'I_3=0\\ \quad\Downarrow\\ A(A_F^2+a_1'A_F+a_2'I_3)+a_3'I_3\\=BF(A_F^2+a_1'A_F+a_2'I_3)\\ \quad\Downarrow\\ A((A-BF)A_F+a_1'(A-BF)+a_2'I_3)+a_3'I_3\\=BF(A_F^2+a_1'A_F+a_2'I_3)\\ \quad\Downarrow\\ A^2(A-BF)+a_1'A^2+a_2'A+a_3'I_3\\ =BF(A_F^2+a_1'A_F+a_2'I_3)+ABF(A_F+a_1'I_3)\\ \quad\Downarrow\\ A^3+a_1'A^2+a_2'A+a_3'I_3\\ =BF(A_F^2+a_1'A_F+a_2'I_3)+ABF(A_F+a_1'I_3)+A^2BF\\ \quad\Downarrow\\ A^3+a_1'A^2+a_2'A+a_3'I_3\\ =\left[\begin{array}{ccc} B &AB&A^2B \end{array}\right] \left[\begin{array}{ccc} F(A_F^2+a_1'A_F+a_2'I_3)\\ F(A_F+a_1'I_3)\\ F \end{array}\right] \end{array} }

したがって、１入力系で\left[\begin{array}{ccc} B &AB&A^2B \end{array}\right]が正則ならば、次の公式を得ます。

\displaystyle{(11)\quad F= \left[\begin{array}{ccc} 0 &0 &1 \end{array}\right] \left[\begin{array}{ccc} B &AB&A^2B \end{array}\right]^{-1} (A^3+a_1'A^2+a_2'A+a_3'I_3) }

n>3の場合も同様にして、A_F=A-BFの特性多項式(7)を

\displaystyle{(12)\quad %\begin{array}{l} A^n+a'_1A^{n-1}+\cdots+a'_nI_n =\left[\begin{array}{cccc} B & AB &\cdots & A^{n-1}B \end{array}\right] \left[\begin{array}{ccc} X_{n-1}\\ \vdots\\ X_1\\ F \end{array}\right] %\end{array} }

のように書き換えることができます。ここで、X_1,\cdots,X_{n-1}はサイズm\times nの適当な行列です。したがって、１入力系で\left[\begin{array}{cccc} B & AB &\cdots & A^{n-1}B \end{array}\right]が正則ならば、(12)の右辺の第２番目の行列の最下段にあるFを取りだすことにより、次の公式が得られます。

\displaystyle{(13)\quad \boxed{\begin{array}{l} F= \left[\begin{array}{cccc} 0 & \cdots & 0 & 1 \end{array}\right] \left[\begin{array}{cccc} B & AB &\cdots & A^{n-1}B \end{array}\right]^{-1}\\ \times (A^n+a'_1A^{n-1}+\cdots+a'_nI_n) \end{array}} }

このように、１入力n次系の場合は、Fのn個の要素が、A-BFの特性多項式のn個の係数（すなわちn個の固有値）を指定することにより決定されます。ところがm入力系の場合、A_Fの固有値だけでは決まらず、対応する固有ベクトルまで指定する必要があります。このことを以下で調べてみます。

[3]　いま、ジョルダン分解A_F=V\Lambda V^{-1}を仮定して

\displaystyle{(14)\quad A-BF=\underbrace{V\Lambda V^{-1}}_{A_F} \Rightarrow AV-V\Lambda=B\underbrace{FV}_{G} }

ここで、\Lambdaが対角行列の場合、A_Fの固有値\lambda_1',\cdots,\lambda_n'に対応する固有ベクトルをv_1,\cdots,v_nとして

\displaystyle{(15)\quad A \underbrace{ \left[\begin{array}{ccc} v_1&\cdots&v_n \end{array}\right] }_{V} - \underbrace{ \left[\begin{array}{ccc} v_1&\cdots&v_n \end{array}\right] }_{V} \underbrace{ {\rm diag}\{\lambda_1',\cdots,\lambda_m'\} }_{\Lambda}=}

\displaystyle{ \left[\begin{array}{ccc} (A-\lambda_1'I_n)v_1&\cdots&(A-\lambda_n'I_n)v_n \end{array}\right] =B \underbrace{ \left[\begin{array}{ccc} g_1&\cdots&g_n \end{array}\right] }_{G} }

これより、もし\lambda_1',\cdots,\lambda_n'がAの固有値と一致しないならば、固有ベクトルの表現式として次式を得ます。

\displaystyle{(16)\quad \underbrace{ \left[\begin{array}{ccc} v_1&\cdots&v_n \end{array}\right] }_{V}= \left[\begin{array}{ccc} (A-\lambda_1'I_n)^{-1}Bg_1&\cdots&(A-\lambda_n'I_n)^{-1}Bg_n \end{array}\right] }

したがって、状態フィードバックゲイン行列Fを求める次の手順が考えられます。

1) A_Fの固有値\lambda_1',\cdots,\lambda_n'をAの固有値とは異なるように指定する。

2) A_Fの固有ベクトルv_1,\cdots,v_nを決める自由度g_1,\cdots,g_nを、(15)のVが正則になるように与える。

3) 次式で Fを求める。

\displaystyle{(17)\quad \boxed{F=GV^{-1}} }

この手順は簡潔ですが、１）の制約があること、２）の具体的な手法が不明であることなどの難点があります。状態フィードバックを定める手続きは、ある最適化問題の解として得ることが多く、これについてはあと説明します。

演習 A31…Flipped Classroom

1^\circ 次のコードを実行し、A-BFの固有値を計算し、指定されているものになっていることを確かめよ。

MATLAB 
%a31_1.m
%-----
 clear all, close all
 A=[0 1;0 0]; B=[0;1]; p=[-1 -1];
 F=sf(A,B,p)
 lambda=eig(A-B*F)
%----- 
 A=[1 2;3 4]; B=[5;6]; p=[-1+i -1-i];
 F=sf(A,B,p)
 lambda=eig(A-B*F)
%----- 
 function F=sf(A,B,p)
   n=size(A,1); r=eig(A);
   a=[1 -r(1)]; for i=2:n, a=conv(a,[1 -r(i)]); end, a=a(n+1:-1:2);
   b=[1 -p(1)]; for i=2:n, b=conv(b,[1 -p(i)]); end, b=b(n+1:-1:2);
   X=zeros(n,n); for i=1:n, X(i,1:n-i+1)=[a(i+1:n) 1]; end
   Y=B; for i=2:n, Y=[B A*Y]; end
   F=real((b-a)/X/Y);
 end
%-----
%eof

2^\circ 次のコードを実行し、A-BFの固有値と固有ベクトルを計算し、指定されているものになっていることを確かめよ。

MATLAB 
%a31_2.m
%-----
 clear all, close all
 A=[0 0;0 -1]; B=[1 1;1 -1]; p=[-2 -3]; V=[1 0;0 1];
 [n,m]=size(B);
 G=[];
 for i=1:n, G=[G ((A-p(i)*eye(n,n))\B)\V(:,i)]; end
 F=G/V
 [V,R]=eig(A-B*F)
%-----
%eof

Note A31 不可制御の例
どのような制御対象に対しても，閉ループ系を安定化する状態フィードバックが求まるわけではありません。たとえば、皆さんは小さいころ棒立て遊びをやった記憶があると思います。不安定なモノを安定化できた楽しい思い出だったと思います。それでは、２本の棒を同時に立てることはできたと思いますか？この状況を次図のように表します。


図 ２本の棒立て制御

ここで、２本の棒の長さと質量をそれぞれ\ell_1,\ell_2とm_1,m_2とします。また鉛直線からの傾きを\theta_1,\theta_2とします。２本の棒は１つの軸に取り付けられており、自由に回転できるとします。この軸を左右に動かして回転トルクを与えるのですが、その結果として働くトルクをここではh_1\tau, h_2\tauで表します。このとき運動方程式は次式となります。ただし、J_1=\frac{4}{3}m_1\ell_1^2, J_2=\frac{4}{3}m_2\ell_2^2。

\displaystyle{(1)\quad \left\{\begin{array}{l} J_1\ddot\theta_1(t)=m_1g\ell_1\theta_1(t)+h_1\tau(t) \\ J_2\ddot\theta_2(t)=m_1g\ell_2\theta_2(t)+h_2\tau(t) \end{array}\right. }

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

\displaystyle{(2)\quad \begin{equation} \frac{d}{dt} \left[\begin{array}{c} \theta_1\\ \theta_2\\ \dot\theta_1\\ \dot\theta_2\\ \end{array}\right]= \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ a_1 & 0 & 0 & 0\\ 0 & a_2 & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} \theta_1\\ \theta_2\\ \dot\theta_1\\ \dot\theta_2\\ \end{array}\right]+ \left[\begin{array}{c} 0\\ 0\\ b_1\\ b_2\\ \end{array}\right]\tau }

ただし、a_1=\frac{3g}{4\ell_1}, a_2=\frac{3g}{4\ell_2}、b_1=\frac{h_1}{J_1}, b_2=\frac{h_2}{J_2}。

それでは、これを安定化する状態フィードバックゲインを求めてみます。上述の公式

\displaystyle{(3)\quad F= \left[\begin{array}{cccc} 0 & \cdots & 0 & 1 \end{array}\right] %\underbrace{ \left[\begin{array}{cccc} B & AB &\cdots & A^{n-1}B \end{array}\right]^{-1} %}_{可制御性行列} }

を参照すれば、次の行列が正則である必要があります。

\displaystyle{(4)\quad \begin{equation} \left[\begin{array}{cccc} B & AB & A^2B & A^3B \end{array}\right] = \left[\begin{array}{cccc} 0 &b_1 &0 &a_1b_1\\ 0 &b_2 &0 &a_2b_2\\ b_1&0 &a_1b_1 &0\\ b_2&0 &a_2b_2 &0\\ \end{array}\right] }

これは２つの棒の長さ違うときは正則となり、閉ループ系を安定化する状態フィードバックが求まります。ところが２つの棒が同じで、a_1=a_2, b_1=b_2となる場合は、第１行と第２行が同じとなり、第３行と第４行も同じとなり、正則性は失われます。これから全く同じ２本の棒の棒立ては成功しないと考えられます。これは「二兎を追うものは一兎も得ず」のことわざ通りですね。

２次系の周波数応答

投稿日時: 投稿者:

２次系の周波数応答…Homework

[1] 次のようにパラメタライズされた漸近安定な１入力１出力２次系の状態空間表現を考えます。

\displaystyle{(1a)\quad \underbrace{ \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{x}_2(t) \end{array}\right] }_{\dot{x}(t)} = \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(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ K\omega_n^2 \end{array}\right] }_{B} u(t) }

\displaystyle{(1b)\quad y(t)= \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} }

ここで、\zeta<1とします。また以下では簡単のためK=1とします。このときインパルス応答は、次式で与えられました。

\displaystyle{(2a)\quad G(t)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_I t}
\displaystyle{(2b)\quad \lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}}

この２次系に対して、正弦波入力

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

を考えます。２次振動系（\zeta<1）の場合、零状態応答は次式で与えられます（導出は Note A24-2を参照）。

\displaystyle{(4a)\quad y(t)=\frac{1}{\sqrt{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}}(\sin(\omega t+\theta)-e^{\lambda_Rt}\sin(\lambda_I t+\theta'))}
\displaystyle{(4b)\quad \theta=\tan^{-1}\frac{-2\zeta\frac{\omega}{\omega_n}}{1-(\frac{\omega}{\omega_n})^2}}
\displaystyle{(4c)\quad \theta'=\tan^{-1}\frac{-2\zeta\sqrt{1-\zeta^2}}{1-2\zeta^2-(\frac{\omega}{\omega_n})^2}}

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

\displaystyle{(5)\quad \boxed{y(t)\simeq\underbrace{\frac{1}{\sqrt{(1-\frac{\omega^2}{\omega_n^2})^2+4\zeta^2\frac{\omega^2}{\omega_n^2}}}}_{|\hat{G}(j\omega)|} \sin(\omega t+\underbrace{\tan^{-1}\frac{-2\zeta\frac{\omega}{\omega_n}}{1-\frac{\omega^2}{\omega_n^2}}}_{\angle\hat{G}(j\omega)})} }

ただし

\displaystyle{(6)\quad \hat{G}(s)=\frac{\omega_n^2}{s^2+2\zeta\omega_ns+\omega_n^2}}

これは、入力が正弦波のときは、時間が十分立てば、出力も正弦波となることを示しています。その振幅と位相はそれぞれ\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))です。

実際、２次系において\omega_n=1,\zeta=0.01,0.707,1のとき、ボード線図を描いてみると次のグラフが得られます

図1 ２次系の周波数応答の比較

周波数応答に基づく２次系の同定…Homework

[2] ２次振動系の場合、ゲイン線図

\displaystyle{(7)\quad |\hat{G}(j\omega)|=\frac{1}{\sqrt{(1-\frac{\omega^2}{\omega_n^2})^2+4\zeta^2\frac{\omega^2}{\omega_n^2}}} }

の頂点は次式で与えられます。

\displaystyle{(8)\quad \boxed{(\frac{\omega_p}{\omega_n},M_p)=\left(\sqrt{1-2\zeta^2},{1\over 2\zeta\sqrt{1-\zeta^2}}\right)\quad(\zeta\le{1\over\sqrt{2}})} }

ここで、頂点が現れる条件、\zeta\le{1\over\sqrt{2}}がついていることに注意してください。

実際、{d\over d\omega}|\hat G(j\omega_p)|=0を満足する\omega_pは

(9)\quad \begin{array}{l} \displaystyle{{d\over d\omega}|\hat G(j\omega)| =\left.-\frac{1}{2}\frac{2(1-(\frac{\omega}{\omega_n})^2)(-\frac{2\omega}{\omega_n^2})) +4\zeta^2\frac{2\omega}{\omega_n^2}}{((1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2)^{3/2}}\,\right|_{\omega=\omega_p}=0}\\ \displaystyle{\Rightarrow\ -(1-(\frac{\omega_p}{\omega_n})^2)+2\zeta^2=0}\\ \displaystyle{\Rightarrow\ (\frac{\omega_p}{\omega_n})^2=1-2\zeta^2} \end{array}

となって、次式のように表されます。

\displaystyle{(10)\quad \boxed{\frac{\omega_p}{\omega_n}=\sqrt{1-2\zeta^2}}}

また、このときの最大値は

(11)\quad \begin{array}{l} \displaystyle{M_p=|\hat G(j\omega_p)|=\frac{1}{\sqrt{(2\zeta^2)^2+4\zeta^2(1-2\zeta^2)}}}\\ \displaystyle{=\frac{1}{\sqrt{4\zeta^4+4\zeta^2-8\zeta^4}}=\frac{1}{\sqrt{4\zeta^2(1-\zeta^2)}}} \end{array}

となって、次式のように表されます。。

\displaystyle{(12)\quad \boxed{M_p=\frac{1}{2\zeta\sqrt{1-\zeta^2}}}}

(10)と(12)から、２次振動系はゲイン線図が頂点を持つ場合、その座標(\frac{\omega_p}{\omega_n},M_p)を求めて、減衰係数と固有角周波数(\zeta,\omega_n)が得られることを示しています。

●ちなみに、|\hat{G}(j\omega_b)|=\frac{|\hat{G}(j0)|}{\sqrt{2}}を満足する帯域幅\omega_bは次のように計算されます。

\displaystyle{(13)\quad \frac{1}{\sqrt{(1-(\frac{\omega_b}{\omega_n})^2)^2+4\zeta^2(\frac{\omega_b}{\omega_n})^2}} =\frac{1}{\sqrt{2}} }

において、\omega'=\frac{\omega_b}{\omega_n}とおくと

(14)\quad \begin{array}{l} \displaystyle{(1-\omega'\,^2)^2+4\zeta^2\omega'\,^2=1-2\omega'\,^2+\omega'\,^4+4\zeta^2\omega'\,^2=2}\\ \displaystyle{\Rightarrow\ \omega'^4-2(1-2\zeta^2)\omega'\,^2-1=0}\\ \displaystyle{\Rightarrow\ \omega'^2=1-2\zeta^2\pm\sqrt{4\zeta^4-4\zeta^2+2}} \end{array}

となって、次式のように表されます。

\displaystyle{(15)\quad \boxed{\frac{\omega_b}{\omega_n}=\sqrt{1-2\zeta^2\pm\sqrt{4\zeta^4-4\zeta^2+2}}１}}

演習A24…Flipped Classroom

図1のゲイン曲線#1を描け。

MATLAB 
%a24.m
%-----
 clear all, close all
 A1=[0 1;-1 -2*0.01]; 
 A2=[0 1;-1 -2/sqrt(2)]; 
 A3=[0 1;-1 -2*1]; 
 B=[0;1]; C=[1 0]; D=[];
 sys1=ss(A1,B,C,D);
 sys2=ss(A2,B,C,D);
 sys3=ss(A3,B,C,D);
 w=logspace(-1,1,1000);
 bode(sys1,sys2,sys3,w)
 grid
 title('Bode Diagrams of 2nd-order System')
 xlabel('Freq')
%ylabel('Gain')
 ylabel('Phase')
 legend('zeta=0.01','zeta=0.707','zeta=1')
%-----
%eof
SCILAB 
coming soon

Note A24-1　n次系の周波数応答

次の漸近安定な１入力１出力n次系の状態空間表現を考えます。

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

このとき、正弦波入力

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

に対する零状態応答を計算します。そのために

\displaystyle{(3)\quad u(t)=e^{j\omega t}=\cos(\omega t)+j\sin(\omega t)}

を、零状態応答の式

\displaystyle{(4)\quad y(t)=\int_0^tC\exp(A(t-\tau))Bu(\tau)d\tau}

に代入して

(5)\quad \begin{array}{l} \displaystyle{y(t)=\int_0^tC\exp(A(t-\tau))Be^{j\omega\tau}d\tau}\\ \displaystyle{=C\exp(At)\int_0^te^{j\omega\tau}\exp(-A\tau)Bd\tau}\\ \displaystyle{=C\exp(At)\int_0^t\exp(j\omega\tau I_n)\exp(-A\tau)Bd\tau}\\ \displaystyle{=C\exp(At)\int_0^t\exp((j\omega I_n-A)\tau)Bd\tau}\\ \displaystyle{=C\exp(At) \left[\frac{}{}\exp((j\omega I_n-A)\tau)\right]_0^t(j\omega I_n-A)^{-1}B}\\ \displaystyle{=C\exp(At) (\exp((j\omega I_n-A)t)-I_n)(j\omega I_n-A)^{-1}B}\\ \displaystyle{=-C\exp(At)(j\omega I_n-A)^{-1}B+C\exp(At)\exp(j\omega t I_n)\exp(-At)(j\omega I_n-A)^{-1}B}\\ \displaystyle{=-C\exp(At)(j\omega I_n-A)^{-1}B+C(j\omega I_n-A)^{-1}Be^{j\omega t}} \end{array}

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

(6)\quad \begin{array}{l} \displaystyle{y(t)=C\underbrace{\exp(At)}_{\rightarrow 0\ (t\rightarrow\infty)}(j\omega I_n-A)^{-1}B+\underbrace{C(j\omega I_n-A)^{-1}B}_{G(j\omega) =|G(j\omega)|e^{j\angle G(j\omega)}}e^{j\omega t}}\\ \displaystyle{\simeq |G(j\omega)|e^{j(\omega t+\angle G(j\omega))}}\\ \displaystyle{=|G(j\omega)|\cos(\omega t+\angle G(j\omega))+j|G(j\omega)|\sin(\omega t+\angle G(j\omega))} \end{array}

これから正弦波入力(3)に対する零状態応答は、t\rightarrow\inftyのとき次式で与えられます。

\displaystyle{(7)\quad y(t)\simeq|G(j\omega)|\sin(\omega t+\angle G(j\omega))}

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

Note A24-2　２次系の周波数応答

次の２次系を考えます。

\displaystyle{(1a)\quad \underbrace{ \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{x}_2(t) \end{array}\right] }_{\dot{x}(t)} = \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(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ \omega_n^2 \end{array}\right] }_{B} u(t) }

\displaystyle{(1b)\quad y(t)= \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} }

ここで、\zeta<1とします。このときインパルス応答は

\displaystyle{(2a)\quad G(t)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_I t}
\displaystyle{(2b)\quad \lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}}

でした。いま、正弦波入力

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

に対する時間応答を考えます。このとき、公式

\displaystyle{(4a)\quad \int e^{ax}\sin px \cos qx\,dx}
\displaystyle{=\frac{e^{ax}}{2} \left(\frac{a\sin(p+q)x-(p+q)\cos(p+q)x}{a^2+(p+q)^2}\right.+\left.\frac{a\sin(p-q)x-(p-q)\cos(p-q)x}{a^2+(p-q)^2}\right)}
\displaystyle{(4b)\quad \int e^{ax}\sin px \sin qx\,dx}
\displaystyle{=\frac{e^{ax}}{2} \left(-\frac{a\cos(p+q)x+(p+q)\sin(p+q)x}{a^2+(p+q)^2}\right.+\left.\frac{a\cos(p-q)x+(p-q)\sin(p-q)x}{a^2+(p-q)^2}\right)}

を用いて、周波数応答が次のように得られます。

(5)\quad \begin{array}{l} \displaystyle{y(t)=\int_0^t\frac{\omega_n^2}{\lambda_I}e^{\lambda_R(t-\tau)}\sin\lambda_I(t-\tau)\sin\omega\tau\,d\tau}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_It\int_0^te^{-\lambda_R\tau}\sin\omega\tau\cos\lambda_I\tau\,d\tau}\\ \displaystyle{-\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\cos\lambda_It\int_0^te^{-\lambda_R\tau}\sin\omega\tau\sin\lambda_I\tau\,d\tau}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times}\\ \displaystyle{(\left[\frac{e^{-\lambda_R\tau}(-\lambda_R\sin(\omega+\lambda_I)\tau-(\omega+\lambda_I)\cos(\omega+\lambda_I)\tau)}{2(\lambda_R^2+(\omega+\lambda_I)^2)}\right.}\\ \displaystyle{+\left.\frac{e^{-\lambda_R\tau}(-\lambda_R\sin(\omega-\lambda_I)\tau-(\omega-\lambda_I)\cos(\omega-\lambda_I)\tau)}{2(\lambda_R^2+(\omega-\lambda_I)^2)}\right]_0^t)}\\ \displaystyle{-\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times}\\ \displaystyle{(\left[-\frac{e^{-\lambda_R\tau}(-\lambda_R\cos(\omega+\lambda_I)\tau+(\omega+\lambda_I)\sin(\omega+\lambda_I)\tau)}{2(\lambda_R^2+(\omega+\lambda_I)^2)}\right.}\\ \displaystyle{+\left.\frac{e^{-\lambda_R\tau}(-\lambda_R\cos(\omega-\lambda_I)\tau+(\omega-\lambda_I)\sin(\omega-\lambda_I)\tau)}{2(\lambda_R^2+(\omega-\lambda_I)^2)^2}\right]_0^t)}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times}\\ \displaystyle{(\frac{e^{-\lambda_Rt}(-\lambda_R\sin(\omega+\lambda_I)t-(\omega+\lambda_I)\cos(\omega+\lambda_I)t)+\omega+\lambda_I}{2(\lambda_R^2+(\omega+\lambda_I)^2)}}\\ \displaystyle{+\frac{e^{-\lambda_Rt}(-\lambda_R\sin(\omega-\lambda_I)t-(\omega-\lambda_I)\cos(\omega-\lambda_I)t)+\omega-\lambda_I}{2(\lambda_R^2+(\omega-\lambda_I)^2)})}\\ \displaystyle{-\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times}\\ \displaystyle{(\frac{-e^{-\lambda_Rt}(-\lambda_R\cos(\omega+\lambda_I)t+(\omega+\lambda_I)\sin(\omega+\lambda_I)t)-\lambda_R}{2(\lambda_R^2+(\omega+\lambda_I)^2)}}\\ \displaystyle{+\frac{e^{-\lambda_Rt}(-\lambda_R\cos(\omega-\lambda_I)t+(\omega-\lambda_I)\sin(\omega-\lambda_I)t)+\lambda_R}{2(\lambda_R^2+(\omega-\lambda_I)^2)})} \end{array}

さらに変形すると

(6)\quad \begin{array}{l} \displaystyle{y(t)=\frac{\omega_n^2}{2\lambda_I}\sin\lambda_It\times\frac{-\lambda_R\sin(\omega+\lambda_I)t-(\omega+\lambda_I)\cos(\omega+\lambda_I)t}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{-\frac{\omega_n^2}{2\lambda_I}\cos\lambda_It\times\frac{-(-\lambda_R\cos(\omega+\lambda_I)t+(\omega+\lambda_I)\sin(\omega+\lambda_I)t)}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\sin\lambda_It\times\frac{-\lambda_R\sin(\omega-\lambda_I)t-(\omega-\lambda_I)\cos(\omega-\lambda_I)t}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{-\frac{\omega_n^2}{2\lambda_I}\cos\lambda_It\times\frac{-\lambda_R\cos(\omega-\lambda_I)t+(\omega-\lambda_I)\sin(\omega-\lambda_I)t}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{-\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times(\frac{-\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\sin(\omega+\lambda_I)t\sin\lambda_It-(\omega+\lambda_I)\cos(\omega+\lambda_I)t\sin\lambda_It}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\cos(\omega+\lambda_I)t\cos\lambda_It+(\omega+\lambda_I)\sin(\omega+\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\sin(\omega-\lambda_I)t\sin\lambda_It-(\omega-\lambda_I)\cos(\omega-\lambda_I)t\sin\lambda_It}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{+\lambda_R\cos(\omega-\lambda_I)t\cos\lambda_It-(\omega-\lambda_I)\sin(\omega-\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\sin(\omega+\lambda_I)t\sin\lambda_It-\lambda_R\cos(\omega+\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{-(\omega+\lambda_I)\cos(\omega+\lambda_I)t\sin\lambda_It+(\omega+\lambda_I)\sin(\omega+\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\sin(\omega-\lambda_I)t\sin\lambda_It+\lambda_R\cos(\omega-\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{-(\omega-\lambda_I)\cos(\omega-\lambda_I)t\sin\lambda_It-(\omega-\lambda_I)\sin(\omega-\lambda_I)t\cos\lambda_It}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})} \end{array}

これを、公式

\displaystyle{(7a)\quad \sin(\alpha\pm\beta)=\sin\alpha\cos\beta\pm\cos\alpha\sin\beta}
\displaystyle{(7b)\quad \cos(\alpha\pm\beta)=\cos\alpha\cos\beta\mp\sin\alpha\sin\beta}

を用いて、以下のように変形します。

(8)\quad \begin{array}{l} \displaystyle{y(t)=\frac{\omega_n^2}{2\lambda_I}\times\frac{-\lambda_R\cos\omega t}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega_n^2}{2\lambda_I}\times\frac{(\omega+\lambda_I)\sin\omega t}{\lambda_R^2+(\omega+\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}\times\frac{\lambda_R\cos\omega t}{\lambda_R^2+(\omega-\lambda_I)^2}+\frac{\omega_n^2}{2\lambda_I}\times\frac{-(\omega-\lambda_I)\sin\omega t}{\lambda_R^2+(\omega-\lambda_I)^2}}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\sin\lambda_It\times(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{+\frac{\omega_n^2}{2\lambda_I}e^{\lambda_Rt}\cos\lambda_It\times(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{(\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\sin\omega t\times\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{-\cos\omega t\times\frac{\omega_n^2}{2\lambda_I}(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{+e^{\lambda_Rt}\sin\lambda_It\times\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{+e^{\lambda_Rt}\cos\lambda_It\times\frac{\omega_n^2}{2\lambda_I}(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})} \end{array}

ここで

(9)\quad \begin{array}{l} \displaystyle{\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\omega_n^2+\omega^2+2\omega\lambda_I}-\frac{\omega-\lambda_I}{\omega_n^2+\omega^2-2\omega\lambda_I})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I} \frac{(\omega+\lambda_I)(\omega_n^2+\omega^2-2\omega\lambda_I)-(\omega-\lambda_I)(\omega_n^2+\omega^2+2\omega\lambda_I)} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I} \frac{-4\omega^2\lambda_I+2\lambda_I(\omega_n^2+\omega^2)} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{1} \frac{-2\omega^2+(\omega_n^2+\omega^2)} {(\omega_n^2+\omega^2)^2-4\omega^2\omega_n^2(1-\zeta^2)}}\\ \displaystyle{=\frac{\omega_n^2(\omega_n^2-\omega^2)} {(\omega_n^2-\omega^2)^2+4\omega^2\omega_n^2\zeta^2}}\\ \displaystyle{=\frac{1-(\frac{\omega}{\omega_n})^2}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}} \end{array}

(10)\quad \begin{array}{l} \displaystyle{\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\lambda_R^2+(\omega+\lambda_I)^2}+\frac{\omega-\lambda_I}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}(\frac{\omega+\lambda_I}{\omega_n^2+\omega^2+2\omega\lambda_I}+\frac{\omega-\lambda_I}{\omega_n^2+\omega^2-2\omega\lambda_I})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I} \frac{(\omega+\lambda_I)(\omega_n^2+\omega^2-2\omega\lambda_I)+(\omega-\lambda_I)(\omega_n^2+\omega^2+2\omega\lambda_I)} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I} \frac{2\omega(\omega_n^2+\omega^2)-4\omega\lambda_I^2} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I} \frac{\omega(\omega_n^2+\omega^2)-2\omega\omega_n^2(1-\zeta^2)} {(\omega_n^2+\omega^2)^2-4\omega^2\omega_n^2(1-\zeta^2)}}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I} \frac{\omega^3-\omega\omega_n^2(1-2\zeta^2)}{(\omega_n^2-\omega^2)^2+4\omega^2\omega_n^2\zeta^2}}\\ \displaystyle{=\frac{\omega}{\lambda_I}\frac{(\frac{\omega}{\omega_n})^2-(1-2\zeta^2)}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}} \end{array}

および

(11)\quad \begin{array}{l} \displaystyle{\frac{\omega_n^2}{2\lambda_I}(\frac{\lambda_R}{\lambda_R^2+(\omega+\lambda_I)^2}-\frac{\lambda_R}{\lambda_R^2+(\omega-\lambda_I)^2})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}\lambda_R(\frac{1}{\omega_n^2+\omega^2+2\omega\lambda_I}-\frac{1}{\omega_n^2+\omega^2-2\omega\lambda_I})}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}\lambda_R \frac{(\omega_n^2+\omega^2-2\omega\lambda_I)-(\omega_n^2+\omega^2+2\omega\lambda_I)} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{2\lambda_I}\lambda_R \frac{-4\omega\lambda_I} {(\omega_n^2+\omega^2)^2-4\omega^2\lambda_I^2}}\\ \displaystyle{=\frac{\omega_n^2}{1}(-\zeta\omega_n) \frac{-2\omega} {(\omega_n^2+\omega^2)^2-4\omega^2\omega_n^2(1-\zeta^2)}}\\ \displaystyle{=\frac{2\zeta\omega_n^3\omega} {(\omega_n^2-\omega^2)^2+4\omega^2\omega_n^2\zeta^2}}\\ \displaystyle{=\frac{2\zeta\frac{\omega}{\omega_n}}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}} \end{array}

だから

(12)\quad \begin{array}{l} \displaystyle{y(t)=\sin\omega t\times\frac{1-(\frac{\omega}{\omega_n})^2}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}}\\ \displaystyle{-\cos\omega t\times\frac{2\zeta\frac{\omega}{\omega_2}}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}}\\ \displaystyle{+e^{\lambda_Rt}\sin\lambda_It\times\frac{\omega}{\lambda_I}\frac{(\frac{\omega}{\omega_n})^2-(1-2\zeta^2)}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}}\\ \displaystyle{+e^{\lambda_Rt}\cos\lambda_It\times\frac{2\zeta\frac{\omega}{\omega_n}}{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}} \end{array}

すなわち、周波数応答は

\displaystyle{(13a)\quad y(t)=\frac{1}{\sqrt{(1-(\frac{\omega}{\omega_n})^2)^2+4\zeta^2(\frac{\omega}{\omega_n})^2}}(\sin(\omega t+\theta)-e^{\lambda_Rt}\sin(\lambda_I t+\theta'))}
\displaystyle{(13b)\quad \theta=\tan^{-1}\frac{-2\zeta\frac{\omega}{\omega_n}}{1-(\frac{\omega}{\omega_n})^2}}
\displaystyle{(13c)\quad \theta'=\tan^{-1}\frac{2\zeta\frac{\omega}{\omega_n}}{\frac{\omega}{\lambda_I} ((\frac{\omega}{\omega_n})^2-(1-2\zeta^2))}=\tan^{-1}\frac{-2\zeta\sqrt{1-\zeta^2}}{1-2\zeta^2-(\frac{\omega}{\omega_n})^2}}

のように表されます。

２次系のステップ応答

投稿日時: 投稿者:

２次系のステップ応答…Homework

[0]　次のm入力p出力n次系の状態空間表現を考えます。

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

いま行列Bの第j列をB_j、行列Cの第i列をC_iで表すとき、第j番目の入力u_jから第i番目の出力y_iまでの時間応答は

\displaystyle{(2)\quad y_i(t)=\underbrace{C_i\exp(At)x(0)}_{zero-input\ response} +\underbrace{\int_0^tG_{ij}(t-\tau)u_j(\tau)d\tau}_{zero-state\ response}}

のように表されます。ここで、G_{ij}(t)は(1)のインパルス応答行列の(i,j)要素で、次のように与えられます。

\displaystyle{(3)\quad G_{ij}(t)=C_i\exp(At)B_j}

このときステップ入力

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

に対する零状態応答はステップ応答と呼ばれ、インパルス応答を積分して

\displaystyle{(5)\quad y_i(t)=\int_0^tG_{ij}(\tau)d\tau}

のように計算されます。

[1] さて、次のようにパラメタライズされた漸近安定な１入力１出力２次系の状態空間表現を考えます。

\displaystyle{(6a)\quad \underbrace{ \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{x}_2(t) \end{array}\right] }_{\dot{x}(t)} = \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(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ K\omega_n^2 \end{array}\right] }_{B} u(t) }

\displaystyle{(6b)\quad y(t)= \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} }

以下では、簡単のためK=1とした２次系(7)に対するインパル応答を(6)から求めます。そのために、行列Aの固有値\lambda(A)=\{\lambda_1,\lambda_2\}を計算すると、\zeta>1、\zeta<1、\zeta=1に応じて、それぞれ次に示すようになります。

\displaystyle{(7)\quad \left\{\begin{array}{l} \lambda_1,\lambda_2=-\zeta\omega_n\pm\omega_n\sqrt{\zeta^2-1}\quad(\zeta>1)\\ \lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}\quad(\zeta<1)\\ \lambda_1=\lambda_2=\lambda=-\zeta\omega_n=-\omega_n\quad(\zeta=1) \end{array}\right. }

図1 A=\left[\begin{array}{cc} 0 & 1\\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right]の固有値分布

[3] ２次系のステップ応答(1)

\zeta>1のとき、\lambda_1,\lambda_2=-\zeta\omega_n\pm\omega_n\sqrt{\zeta^2-1}とおくと、インパルス応答とステップ応答はそれぞれ次式で与えられます。

\displaystyle{(8a)\quad G(t)=\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(e^{\lambda_2t}-e^{\lambda_1t})}
\displaystyle{(8b)\quad \boxed{S(t)=1+\frac{1}{\lambda_2-\lambda_1}(\lambda_1e^{\lambda_2t}-\lambda_2e^{\lambda_1t})}}

実際、インパルス応答を積分して、ステップ応答は次のように計算されます。

(9)\quad \begin{array}{l} \displaystyle{S(t)=\int_0^tG(\tau)d\tau=\int_0^t\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(e^{\lambda_2\tau}-e^{\lambda_1\tau})d\tau}\\ \displaystyle{= \frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}\left[\frac{e^{\lambda_2\tau}}{\lambda_2}-\frac{e^{\lambda_1\tau}}{\lambda_1}\right]_0^t}\\ \displaystyle{=\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(\frac{e^{\lambda_2t}}{\lambda_2}-\frac{e^{\lambda_1t}}{\lambda_1})-\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(\frac{1}{\lambda_2}-\frac{1}{\lambda_1})}\\ \displaystyle{=1+\frac{1}{\lambda_2-\lambda_1}(\lambda_1e^{\lambda_2t}-\lambda_2e^{\lambda_1t})} \end{array}

[4] ２次系のステップ応答(2)

\zeta<1のとき、\lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}とおくと、インパルス応答とステップ応答はそれぞれそれぞれ次式で与えられます。

\displaystyle{(10a)\quad G(t)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_I t}
\displaystyle{(10b)\quad \boxed{S(t)=1-\frac{\omega_n}{\lambda_I}e^{\lambda_Rt}\sin(\lambda_I t+\tan^{-1}\frac{\lambda_I}{-\lambda_R})}}

実際、インパルス応答を積分して、公式

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

を用いて、ステップ応答は次のように計算されます。

(12)\quad \begin{array}{l} \displaystyle{S(t)=\int_0^tG(\tau)d\tau=\int_0^t\frac{\omega_n^2}{\lambda_I}e^{\lambda_R\tau}\sin\lambda_I \tau d\tau}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I}\left[ \frac{e^{\lambda_R\tau}}{\lambda_R^2+\lambda_I^2}(\lambda_R\sin\lambda_I\tau-\lambda_I\cos\lambda_I\tau)\right]_0^t}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I}\frac{1}{\omega_n^2} (e^{\lambda_Rt}(\lambda_R\sin\lambda_It-\lambda_I\cos\lambda_It)+\lambda_I)}\\ \displaystyle{=1-\frac{\omega_n}{\lambda_I}e^{\lambda_Rt} (\sin\lambda_It\times\frac{-\lambda_R}{\omega_n}+\cos\lambda_It\times\frac{\lambda_I}{\omega_n})}\\ \displaystyle{=1-\frac{\omega_n}{\lambda_I}e^{\lambda_Rt} (\sin\lambda_It\cos\phi+\cos\lambda_It\sin\phi)}\\ \displaystyle{=1-\frac{\omega_n}{\lambda_I}e^{\lambda_Rt}\sin(\lambda_It+\phi) \quad(\phi=\tan^{-1}\frac{\lambda_I}{-\lambda_R})} \end{array}

[5] ２次系のステップ応答(3)

\zeta=1のとき、\lambda_1=\lambda_2=\lambda=-\zeta\omega_n=-\omega_nとおくと、インパルス応答とステップ応答はそれぞれそれぞれ次式で与えられます。

\displaystyle{(13a)\quad G(t)=\lambda^2te^{\lambda t}}
\displaystyle{(13b)\quad \boxed{S(t)=1+(\lambda t-1)e^{\lambda t}}}

実際、インパルス応答を積分して、ステップ応答は次のように計算されます。

(14)\quad \begin{array}{l} \displaystyle{S(t)=\int_0^tG(\tau)d\tau=\int_0^t\lambda^2\tau e^{\lambda\tau}d\tau}\\ \displaystyle{=\lambda^2\left[\tau \frac{1}{\lambda}e^{\lambda\tau}\right]_0^t -\lambda^2\int_0^t \frac{1}{\lambda}e^{\lambda\tau}d\tau}\\ \displaystyle{=\lambda te^{\lambda t}-\lambda\left[\frac{1}{\lambda}e^{\lambda\tau}\right]_0^t}\\ \displaystyle{=1+(\lambda t-1)e^{\lambda t}} \end{array}

２次振動系の同定…Homework

[6]　２次系(6)において、\zeta=0.01,1/\sqrt{2},1、\omega_n=1、K=1の場合、ステップ応答のシミュレーション例を示します。

図2 ２次系のステップ応答の比較

\zeta<1の場合の(7)で表される２次系を２次振動系と呼びます。そのステップ応答

\displaystyle{(15)\quad S(t)=1-\frac{\omega_n}{\lambda_I}e^{\lambda_Rt}\sin(\lambda_I t+\phi)\quad(\phi=\tan^{-1}\frac{\lambda_I}{-\lambda_R})}

の第１オーバーシュートについて、次が成り立ちます。

\displaystyle{(16)\quad \boxed{(T_p,1+p_0)=\left(\frac{\pi}{\omega_n\sqrt{1-\zeta^2}},1+\exp(-\frac{\zeta\pi}{\sqrt{1-\zeta^2}})\right)} }

実際、\dot{S}(t)=0を計算すると

(17)\quad \begin{array}{l} \displaystyle{\dot{S}(t)=-\frac{\omega_n}{\lambda_I}(\lambda_Re^{\lambda_Rt}\sin(\lambda_It+\phi)+e^{\lambda_Rt}\cos(\lambda_It+\phi)\times\lambda_I)=0}\\ \displaystyle{\Rightarrow \sin(\lambda_It+\phi)\times\frac{-\lambda_R}{\omega_n}-\cos(\lambda_It+\phi)\times\frac{\lambda_I}{\omega_n}=0}\\ \displaystyle{\Rightarrow \sin(\lambda_It+\phi)\cos\phi-\cos(\lambda_It+\phi)\sin\phi=0}\\ \displaystyle{\Rightarrow \sin(\lambda_It+\phi-\phi)=0}\\ \displaystyle{\Rightarrow \sin\lambda_It=0}\\ \displaystyle{\Rightarrow \lambda_IT_p=\pi} \end{array}

となって、次式を得ます。

\displaystyle{(18)\quad T_p=\frac{\pi}{\lambda_I}=\frac{\pi}{\omega_n\sqrt{1-\zeta^2}}}

この時刻T_pにおけるS(t)の値は

\displaystyle{(19)\quad S(T_p)=1-\frac{\omega_n}{\lambda_I}e^{\lambda_R\frac{\pi}{\lambda_I}}\sin(\lambda_I\frac{\pi}{\lambda_I}+\phi) =1-\underbrace{\frac{\omega_n}{\lambda_I}}_{\frac{1}{\sin\phi}}e^{\frac{\lambda_R}{\lambda_I}\pi}\underbrace{\sin(\pi+\phi)}_{-\sin\phi}}

となって、次式を得ます。

\displaystyle{(20)\quad S(T_p)=1+e^{\frac{\lambda_R}{\lambda_I}\pi}=1+\underbrace{\exp(-\frac{\zeta\pi}{\sqrt{1-\zeta^2}})}_{p_0} }

●したがって、次のように、(T_p,1+p_0)から(\zeta,\omega_n)を得ることができます。

(21)\quad \begin{array}{l} \displaystyle{\left\{\begin{array}{l} T_p=\frac{\pi}{\omega_n\sqrt{1-\zeta^2}}\\ p_0=\exp(-\frac{\zeta\pi}{\sqrt{1-\zeta^2}}) \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} T_p^2=\frac{\pi^2}{\omega_n^2(1-\zeta^2)}\\ \log p_0=-\frac{\zeta\pi}{\sqrt{1-\zeta^2}} \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} \omega_n^2=\frac{\pi^2}{T_p^2(1-\zeta^2)}\\ (\log p_0)^2=\frac{\zeta^2\pi^2}{1-\zeta^2} \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} \zeta^2=\frac{(\log p_0)^2}{(\log p_0)^2+\pi^2}\\ \omega_n^2=\frac{\pi^2}{T_p^2}\frac{(\log p_0)^2+\pi^2}{\pi^2} \end{array}\right.}\\ \displaystyle{\Leftrightarrow \boxed{\left\{\begin{array}{l} \zeta=\sqrt{\frac{(\log p_0)^2}{(\log p_0)^2+\pi^2}}\\ \omega_n=\frac{\sqrt{(\log p_0)^2+\pi^2}}{T_p} \end{array}\right.}} \end{array}

演習A23…Flipped Classroom

図2のステップ応答#1を描き、第１オーバーシュートの頂点の座標(T_p,1+p_0)をマウスをクリックして取得し、減衰係数と固有角周波数(\zeta,\omega_n)を求めるプログラムを作成せよ。

MATLAB 
%a23.m
%-----
 clear all, close all
 A1=[0 1;-1 -2*0.01]; 
 A2=[0 1;-1 -2/sqrt(2)]; 
 A3=[0 1;-1 -2*1]; 
 B=[0;1]; C=[1 0]; D=[];
 sys1=ss(A1,B,C,D);
 sys2=ss(A2,B,C,D);
 sys3=ss(A3,B,C,D);
 t=0:0.1:10;
 step(sys1,sys2,sys3,t) 
 grid
 title('Step Resp of 2nd-order System')
 xlabel('time')
 ylabel('y(t)')
 legend('zeta=0.01','zeta=0.707','zeta=1')
%-----
 [Tp,p0]=ginput(1);
 p0=p0-1;
 zeta=sqrt(log(p0)^2/(log(p0)^2+pi^2))
 wn=sqrt(log(p0)^2+pi^2)/Tp
 hold on
 plot(Tp,1+p0,'o')
 A1a=[0 1;-wn^2 -2*zeta]; 
 sys1a=ss(A1a,B,C,D);
 step(sys1a,t) 
%-----
%eof
SCILAB

ｎ次系の時間応答

投稿日時: 投稿者:

ｎ次系の時間応答…Homework

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

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

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

\displaystyle{(2)\quad y(t)=C\exp(At)x(0)+\int_0^tC\exp(A(t-\tau))Bu(\tau)d\tau}

を得ます。ここで、インパルス応答行列

\displaystyle{(3)\quad \boxed{G(t)=C\exp(At)B}}

を定義しますと（G(t)\in{\rm\bf R}^{p\times m}）、上式は

\displaystyle{(4)\quad \boxed{y(t)=\underbrace{C\exp(At)x(0)}_{zero-input\ response} +\underbrace{\int_0^tG(t-\tau)u(\tau)d\tau}_{zero-state\ response}}}

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

いま、(1)をラプラス変換して、x(0)=0とすると（零状態応答だけを考えます）

\displaystyle{(5a)\quad \hat{x}(s)-\underbrace{x(0)}_0=A\hat{x}(s)+B\hat{u}(s) \Rightarrow \hat{x}(s)=(sI_n-A)^{-1}B\hat{u}(s) }

\displaystyle{(5b)\quad \hat{y}(s)=C\hat{x}(s) \Rightarrow \hat{y}(s)=\underbrace{C(sI_n-A)^{-1}B}_{\hat{G}(s)}\hat{u}(s) }

ここで現れた

\displaystyle{(6)\quad \boxed{\hat{G}(s)=C(sI_n-A)^{-1}B}}

を伝達関数行列と呼びます。

●n次系の時間応答についての相互関係は次図のように表されます。多変数系の場合、i番目の入力からj番目の出力までのステップ応答と周波数応答を考えます。

図1 多変数系時間等応答の相互関係

２次系の時間応答…Homework

[2] さて、次のようにパラメタライズされた漸近安定な１入力１出力２次系の状態空間表現を考えます。\zeta>0、\omega_n、K はそれぞれ減衰係数、固有角周波数、定常ケインと呼ばれます。

\displaystyle{(7a)\quad \boxed{\underbrace{ \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{x}_2(t) \end{array}\right] }_{\dot{x}(t)} = \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(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ K\omega_n^2 \end{array}\right] }_{B} u(t)} }

\displaystyle{(7b)\quad y(t)= \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} }

以下では、簡単のためK=1とした２次系に対するインパルス応答(3)を求めます。そのために、行列Aの固有値\lambda(A)=\{\lambda_1,\lambda_2\}を計算すると、\zeta>1、\zeta<1、\zeta=1に応じて、それぞれ次に示すようになります。

\displaystyle{(8)\quad \boxed{\left\{\begin{array}{l} \lambda_1,\lambda_2=-\zeta\omega_n\pm\omega_n\sqrt{\zeta^2-1}\quad(\zeta>1)\\ \lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}\quad(\zeta<1)\\ \lambda_1=\lambda_2=\lambda=-\zeta\omega_n=-\omega_n\quad(\zeta=1) \end{array}\right.} }

図2 A=\left[\begin{array}{cc} 0 & 1\\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right]の固有値分布

[3] ２次系のインパルス応答(1)

\zeta>1のとき、\lambda_1,\lambda_2=-\zeta\omega_n\pm\omega_n\sqrt{\zeta^2-1}とおくと、インパルス応答は次式で与えられます。

\displaystyle{(9)\quad \boxed{G(t)=\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(e^{\lambda_2t}-e^{\lambda_1t})}}

実際

\displaystyle{(10a)\quad \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] }_{A} = \underbrace{ \left[\begin{array}{cc} 1 & 1\\ \lambda_1 & \lambda_2 \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{cc} \lambda_1& 0\\ 0 & \lambda_2 \end{array}\right] }_{\Lambda} \underbrace{ \left[\begin{array}{cc} 1 & 1\\ \lambda_1 & \lambda_2 \end{array}\right]^{-1} }_{V^{-1}} }

\displaystyle{(10b)\quad \exp(\Lambda t)= \left[\begin{array}{cc} e^{\lambda_1t}& 0\\ 0 & e^{\lambda_2t} \end{array}\right] }

となることから、インパルス応答は次のように計算されます。

(11)\quad \begin{array}{l} \displaystyle{G(t)=C\exp(At)B=CV\exp(\Lambda t)V^{-1}B}\\ \displaystyle{ =\left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{cc} 1 & 1\\ \lambda_1 & \lambda_2 \end{array}\right] \left[\begin{array}{cc} e^{\lambda_1t}& 0\\ 0 & e^{\lambda_2t} \end{array}\right] \frac{1}{\lambda_2-\lambda_1} \left[\begin{array}{cc} \lambda_2 & -1\\ -\lambda_1 & 1 \end{array}\right] \left[\begin{array}{cc} 0 \\ \omega_n^2 \end{array}\right]}\\ \displaystyle{ =\frac{\omega_n^2}{\lambda_2-\lambda_1} \left[\begin{array}{cc} e^{\lambda_1t}& e^{\lambda_2t} \end{array}\right] \left[\begin{array}{cc} -1\\ 1 \end{array}\right]}\\ \displaystyle{=\frac{\lambda_1 \lambda_2}{\lambda_2-\lambda_1}(e^{\lambda_2t}-e^{\lambda_1t})} \end{array}

[4] ２次系のインパルス応答(2)

\zeta<1のとき、\lambda_1,\lambda_2=\underbrace{-\zeta\omega_n}_{\lambda_R}\pm j\underbrace{\omega_n\sqrt{1-\zeta^2}}_{\lambda_I}とおくと、インパルス応答とステップ応答はそれぞれそれぞれ次式で与えられます。

\displaystyle{(12)\quad \boxed{G(t)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_I t}}

実際

\displaystyle{(13a)\quad \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] }_{A} = \underbrace{ \left[\begin{array}{cc} 1 & 0 \\ \lambda_R & \lambda_I \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right] }_{\Lambda} \underbrace{ \frac{1}{\lambda_I} \left[\begin{array}{cc} \lambda_I & 0 \\ -\lambda_R & 1 \end{array}\right] }_{V^{-1}}}
\displaystyle{(13b)\quad \exp(\Lambda t)= e^{\lambda_R t} \left[\begin{array}{cc} \cos\lambda_I t & \sin\lambda_I t\\ \sin\lambda_I t & \cos\lambda_I t \end{array}\right]}

となることから、インパルス応答は次のように計算されます。

(14)\quad \begin{array}{l} \displaystyle{G(t)=C\exp(At)B=CV\exp(\Lambda t)V^{-1}B}\\ \displaystyle{=\left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{cc} 1 & 0\\ \lambda_R & \lambda_I \end{array}\right] e^{\lambda_R t} \left[\begin{array}{cc} \cos\lambda_I t & \sin\lambda_I t\\ \sin\lambda_I t & \cos\lambda_I t \end{array}\right] \frac{1}{\lambda_I} \left[\begin{array}{cc} \lambda_I & 0\\ -\lambda_R & 1 \end{array}\right] \left[\begin{array}{cc} 0 \\ \omega_n^2 \end{array}\right]}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I} e^{\lambda_R t} \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{cc} \cos\lambda_I t & \sin\lambda_I t\\ \sin\lambda_I t & \cos\lambda_I t \end{array}\right] \left[\begin{array}{cc} 0\\ 1 \end{array}\right]}\\ \displaystyle{=\frac{\omega_n^2}{\lambda_I} e^{\lambda_R t}\sin\lambda_I t} \end{array}

[5] ２次系のインパルス応答(3)

\zeta=1のとき、\lambda_1=\lambda_2=\lambda=-\zeta\omega_n=-\omega_nとおくと、インパルス応答とステップ応答はそれぞれそれぞれ次式で与えられます。

\displaystyle{(15)\quad \boxed{G(t)=\lambda^2te^{\lambda t}}}

実際

\displaystyle{(16a)\quad \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] }_{A} = \underbrace{ \left[\begin{array}{cc} 1 & 1\\ \lambda & \lambda+1 \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{cc} \lambda & 1\\ 0 & \lambda \end{array}\right] }_{\Lambda} \underbrace{ \left[\begin{array}{cc} 1 & 1\\ \lambda & \lambda+1 \end{array}\right]^{-1} }_{V^{-1}}}
\displaystyle{(16b)\quad \exp(\Lambda t)= e^{\lambda t} \left[\begin{array}{cc} 1 & t\\ 0 & 1 \end{array}\right] }

となることから、インパルス応答は次のように計算されます。

(17)\quad \begin{array}{l} \displaystyle{G(t)=C\exp(At)B=CV\exp(\Lambda t)V^{-1}B}\\ \displaystyle{= \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{cc} 1 & 1\\ \lambda & \lambda+1 \end{array}\right] e^{\lambda t} \left[\begin{array}{cc} 1 & t\\ 0 & 1 \end{array}\right] \frac{1}{\lambda+1-\lambda} \left[\begin{array}{cc} \lambda+1 & -1\\ -\lambda & 1 \end{array}\right] \left[\begin{array}{cc} 0 \\ \omega_n^2 \end{array}\right]}\\ \displaystyle{= \omega_n^2 e^{\lambda t} \left[\begin{array}{cc} 1 & t+1 \end{array}\right] \left[\begin{array}{cc} -1\\ 1 \end{array}\right]}\\ \displaystyle{=\lambda^2te^{\lambda t}} \end{array}

２次振動系の同定…Homework

[7] ２次系(7)において、\zeta=0.01,1/\sqrt{2},1、\omega_n=1、K=1の場合、インパルス応答のシミュレーション例を示します。

図3 ２次系のインパルス応答の比較

\zeta<1の場合の(7)で表される２次系を２次振動系と呼びます。そのインパルス応答

\displaystyle{(18)\quad G(t)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_Rt}\sin\lambda_I t}

の第k+1番目(k=0,1,\cdots)の頂点の座標について、次が成り立ちます。

\displaystyle{(19)\quad \boxed{(t_k,G(t_k))=\left(\frac{k\pi+\phi}{\omega_n\sqrt{1-\zeta^2}},\omega_n\exp(-\frac{\zeta(k\pi+\phi)}{\sqrt{1-\zeta^2}})\right)} }

実際、\dot{G}(t)=0を計算すると

(20)\quad \begin{array}{l} \displaystyle{\dot{G}(t)=\frac{\omega_n^2}{\lambda_I}(\lambda_Re^{\lambda_Rt}\sin\lambda_It+e^{\lambda_Rt}\cos\lambda_It\times\lambda_I)=0}\\ \displaystyle{\Rightarrow \sin\lambda_It\times\frac{-\lambda_R}{\omega_n}-\cos\lambda_It\times\frac{\lambda_I}{\omega_n}=0}\\ \displaystyle{\Rightarrow \sin\lambda_It\cos\phi-\cos\lambda_It\sin\phi=0,\quad\phi=\tan^{-1}\frac{\sqrt{1-\zeta^2}}{\zeta}}\\ \displaystyle{\Rightarrow \sin(\lambda_It-\phi)=0}\\ \displaystyle{\Rightarrow \lambda_It_k-\phi=k\pi\quad(k=0,1,\cdots)} \end{array}

となって、次式を得ます。

\displaystyle{(21)\quad t_k=\frac{k\pi+\phi}{\lambda_I}=\frac{k\pi+\phi}{\omega_n\sqrt{1-\zeta^2}}

この時刻t_kにおけるG(t)の値は

\displaystyle{(22)\quad \begin{array}{l} G(t_k)=\frac{\omega_n^2}{\lambda_I}e^{\lambda_R\frac{k\pi+\phi}{\lambda_I}}\sin(\lambda_I\frac{\phi}{\lambda_I})\\ =\omega_n\underbrace{\frac{\omega_n}{\lambda_I}}_{\frac{1}{\sin\phi}}e^{\frac{\lambda_R}{\lambda_I}(k\pi+\phi)}\underbrace{\sin(k\pi+\phi)}_{(-1)^k\sin\phi}\\=(-1)^k\omega_ne^{\frac{\lambda_R}{\lambda_I}(k\pi+\phi)} \end{array} }

となって、次式を得ます。

\displaystyle{(23)\quad G(t_k)=(-1)^k\omega_ne^{\frac{\lambda_R}{\lambda_I}(k\pi+\phi)} =(-1)^k\omega_n\exp(-\frac{\zeta}{\sqrt{1-\zeta^2}}(k\pi+\phi)) }}

●さて、(19)からそれぞれ次の関係式を得ます。

\displaystyle{(24)\quad t_{k+2}-t_k=\frac{(k+2)\pi+\phi}{\omega_n\sqrt{1-\zeta^2}}-\frac{k\pi+\phi}{\omega_n\sqrt{1-\zeta^2}} =\frac{2\pi}{\omega_n\sqrt{1-\zeta^2}}=T}

\displaystyle{(25)\quad \frac{G(t_{k+2})}{G(t_k)} =\exp(-\frac{\zeta((k+2)\pi+\phi)}{\sqrt{1-\zeta^2}} +\frac{\zeta(k\pi+\phi)}{\sqrt{1-\zeta^2}}) =\exp(-\frac{2\pi\zeta}{\sqrt{1-\zeta^2}})=\Lambda}

(24)はインパルス応答の隣合う山と山の時間間隔（または谷と谷の時間間隔）Tは一定であることを示しています。(25)はインパルス応答の隣合う山と山の振幅比（または谷と谷の振幅比）\Lambdaは一定であることを示しています。

したがって、次のように、(T,\Lambda)から(\zeta,\omega_n)を得ることができます。

(26)\quad \begin{array}{l} \displaystyle{ \left\{\begin{array}{l} T=\frac{2\pi}{\omega_n\sqrt{1-\zeta^2}}\\ \Lambda=\exp(-\frac{2\pi\zeta}{\sqrt{1-\zeta^2}}) \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} T^2=\frac{4\pi^2}{\omega_n^2(1-\zeta^2)}\\ \log\Lambda=-\frac{2\pi\zeta}{\sqrt{1-\zeta^2}} \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} \omega_n^2=\frac{4\pi^2}{T^2(1-\zeta^2)}\\ (\log\Lambda)^2=\frac{4\pi^2\zeta^2}{1-\zeta^2} \end{array}\right.}\\ \displaystyle{\Leftrightarrow \left\{\begin{array}{l} \zeta^2=\frac{(\log\Lambda)^2}{(\log\Lambda)^2+4\pi^2}\\ \omega_n^2=\frac{4\pi^2}{T^2}\frac{(\log\Lambda)^2+4\pi^2}{4\pi^2} \end{array}\right.}\\ \displaystyle{ \Leftrightarrow \boxed{\left\{\begin{array}{l} \zeta=\sqrt{\frac{(\log\Lambda)^2}{(\log\Lambda)^2+4\pi^2}}\\ \omega_n=\frac{\sqrt{(\log p_0)^2+4\pi^2}}{T} \end{array}\right.}} \end{array}

演習A22…Flipped Classroom

図2の応答#1を描き、２つの山の頂点の座標をマウスをクリックして取得し、減衰係数と固有角周波数を求めるプログラムを作成せよ。

MATLAB 
%a22.m
%-----
 clear all, close all
 A1=[0 1;-1 -2*0.01]; 
 A2=[0 1;-1 -2/sqrt(2)]; 
 A3=[0 1;-1 -2*1]; 
 B=[0;1]; C=[1 0]; D=[];
 sys1=ss(A1,B,C,D);
 sys2=ss(A2,B,C,D);
 sys3=ss(A3,B,C,D);
 t=0:0.1:10;
 x0=B;
 initial(sys1,sys2,sys3,x0,t) 
 grid
 title('Impulse Resp of 2nd-order System')
 xlabel('time')
 ylabel('y(t)')
 legend('zeta=0.01','zeta=0.707','zeta=1')
%-----
 [Tp,p0]=ginput(2);
 T=Tp(2)-Tp(1);
 Lambda=p0(2)/p0(1),
 zeta=sqrt(log(Lambda)^2/(log(Lambda)^2+4*pi^2))
 wn=sqrt(log(Lambda)^2+4*pi^2)/T
 hold on
 plot(Tp(1),p0(1),'o',Tp(2),p0(2),'o')
 A1a=[0 1;-wn^2 -2*zeta]; 
 sys1a=ss(A1a,B,C,D);
 initial(sys1a,x0,t) 
%-----
%eof
SCILAB

Note A22-1　状態方程式の解

n次系の状態方程式

\displaystyle{(1)\quad\dot{x}(t)=Ax(t)+Bu(t)}

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

\displaystyle{(2)\quad\frac{d}{dt}(\exp(-At)x(t))=\exp(-At)Bu(t)}

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

\displaystyle{(3)\quad\left[\exp(-At)x(t)\right]_0^{t'}=\int_0^{t'}\exp(-At)Bu(t)dt}

すなわち

\displaystyle{(4)\quad\exp(-At')x(t')-x(0)=\int_0^{t'}\exp(-At)Bu(t)dt}

したがって

\displaystyle{(5)\quad x(t')=\exp(At')x(0)+\exp(At')\int_0^{t'}\exp(-At)Bu(t)dt}

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

\displaystyle{(6)\quad\boxed{x(t)=\exp(At)x(0)+\int_0^t\exp(A(t-\tau))Bu(\tau)d\tau}}

が得られます。

Note A22-2　重ね合わせの原理

次のm入力p出力n次系の状態空間表現を考えます。

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

２つの入力u_1とu_2に対するそれぞれの零状態応答は

\displaystyle{(2)\quad y_1(t)=\int_0^tC\exp(A(t-\tau))Bu_1(\tau)d\tau}

\displaystyle{(3)\quad y_2(t)=\int_0^tC\exp(A(t-\tau))Bu_2(\tau)d\tau}

となります。このとき入力

\displaystyle{(4)\quad \alpha u_1(t)+\beta u_2(t)}

に対する零状態応答は

\displaystyle{(5)\quad \alpha y_1(t)+\beta y_2(t)}

となります。実際、

(6)\quad \begin{array}{l} \displaystyle{\int_0^tC\exp(A(t-\tau))B(\alpha u_1(\tau)+\beta u_2(\tau))d\tau}\\ \displaystyle{=\alpha \int_0^tC\exp(A(t-\tau))Bu_1(\tau)d\tau +\beta \int_0^tC\exp(A(t-\tau))Bu_2(\tau))d\tau}\\ =\alpha y_1(t)+\beta y_2(t)} \end{array}

このとき、重ね合わせの原理が成り立つ、n次系(1)は線形系(Linear System)であると言います。重ね合わせの原理が成り立たないときは非線形系(Nonlinear System)であると言います。たとえば入力を２倍したときの時間応答が元の時間応答の２倍にならなければ非線形系であると判断できます。

漸近安定性

投稿日時: 投稿者:

漸近安定性…Homework

[0]　次図に示すように軸支された剛体振り子は２つの静止した平衡状態(\alpha)と(\beta)を持ちます。

図1 剛体振り子

いまこの平衡状態から棒を少し傾けて手を離すと、明らかに違った振舞いをします。実際には軸回りの摩擦や空気抵抗による抗力があるので、平衡状態(\alpha)の方は元に戻りますが、平衡状態(\beta)の方は元に戻ることなく平衡状態(\alpha)に落ち着くでしょう。前者を漸近安定、後者を不安定とよびます。このような違いは平衡状態回りでの振舞いを表す線形状態方程式にどのように反映されているのか気になるところです。

●この場合の運動方程式は次式となります。

\displaystyle{(1)\quad J\ddot{\theta}(t)=-mg\ell\sin\theta(t)-c\dot{\theta}(t)\quad(J=\frac{4}{3}m\ell^2) }

ここで、右辺の第２項が角速度に比例する抗力を表しています（c>0は定数）。これから平衡状態(\alpha)と(\beta)に応じて次の状態方程式を得ます。

\displaystyle{(2)\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}&-\frac{c}{J} \end{array}\right] }_{A=A_1} \underbrace{ \left[\begin{array}{l} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} }

\displaystyle{(3)\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}&-\frac{c}{J} \end{array}\right] }_{A=A_2} \underbrace{ \left[\begin{array}{l} \theta(t)-\pi \\ \omega(t)-0 \end{array}\right] }_{x(t)} }

以下では、行列Aの固有値を調べて、漸近安定かどうかを判定できることを説明します。

[1]　制御対象が平衡状態にあることは線形状態方程式\dot{x}(t)=Ax(t)+Bu(t)において、x=0, u=0を意味します。そこで平衡状態が乱されてx\ne0となる時刻をt=0にとると、線形状態方程式は次式となります。

\displaystyle{(4)\quad \dot{x}(t)=Ax(t)\qquad(x(0)\ne0) }

漸近安定であることは平衡状態x=0に戻ることでしたから、次のように表すことができます。

\displaystyle{(5)\quad \boxed{x(t)\rightarrow 0\qquad(t\rightarrow\infty)} }

そのための条件を検討するため、微分方程式\dot{x}(t)=Ax(t)の解を求める必要があります。これは次式で表されます（Note A21-1参照）。

\displaystyle{(6)\quad \boxed{x(t)=\exp(At)x(0)} }

ここで、xはn次元ベクトル、Aはn次正方行列です。任意のn次正方行列Xに対して、行列指数関数\exp(X)は

\displaystyle{(7)\quad \boxed{\exp(X)=I_n+X+\frac{1}{2}X^2+\cdots+\frac{1}{k!}X^k+\cdots} }

で定義されます。そこで、解の表現式(6)をn=2の場合について詳しく説明します。

[2]　いま２次系に限定して、微分方程式\dot{x}(t)=Ax(t)を要素を明示して書くと

\displaystyle{(8)\quad \underbrace{ \left[\begin{array}{l} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \\ \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{l} x_1(t) \\ x_2(t) \end{array}\right] }_{x(t)} }

すなわち

\displaystyle{(9)\quad \left\{\begin{array}{l} \dot{x}_1(t)=a_{11}x_1(t)+a_{12}x_2(t) \\ \dot{x}_2(t)=a_{21}x_1(t)+a_{22}x_2(t) \end{array}\right. }

となります。もしAが対角行列で、a_{12}=a_{21}=0であれば

\displaystyle{(10)\quad \left\{\begin{array}{l} \dot{x}_1(t)=a_{11}x_1(t) \\ \dot{x}_2(t)=a_{22}x_2(t) \end{array}\right. }

となって、解は

\displaystyle{(11)\quad \left\{\begin{array}{l} x_1(t)=e^{a_{11}t}x_1(0) \\ x_2(t)=e^{a_{22}t}x_2(0) \\ \end{array}\right. }

すなわち

\displaystyle{(12)\quad \underbrace{ \left[\begin{array}{l} {x}_1(t) \\ {x}_2(t) \end{array}\right] }_{{x}(t)}= \underbrace{ \left[\begin{array}{cc} e^{a_{11}t} & 0 \\ 0 & e^{a_{22}t} \\ \end{array}\right] }_{\exp(At)} \underbrace{ \left[\begin{array}{l} x_1(0) \\ x_2(0) \end{array}\right] }_{x(0)} }

ですから、解の表現式(6)は納得できると思います。

[3]　それではAが対角行列でない場合はどうするかですが、一対一対応の変数変換

\displaystyle{(13)\quad \left\{\begin{array}{l} {y}_1(t)=t_{11}x_1(t)+t_{12}x_2(t) \\ {y}_2(t)=t_{21}x_1(t)+t_{22}x_2(t) \end{array}\right. }

すなわち

\displaystyle{(14)\quad \underbrace{ \left[\begin{array}{l} {y}_1(t) \\ {y}_2(t) \end{array}\right] }_{y(t)}= \underbrace{ \left[\begin{array}{cc} t_{11} & t_{12} \\ t_{21} & t_{22} \\ \end{array}\right] }_{T} \underbrace{ \left[\begin{array}{l} x_1(t) \\ x_2(t) \end{array}\right] }_{x(t)} }

を考えます。またこの逆変換を次式で表しておきます。

\displaystyle{(15)\quad \left\{\begin{array}{l} {x}_1(t)=v_{11}y_1(t)+v_{12}y_2(t) \\ {x}_2(t)=v_{21}y_1(t)+v_{22}y_2(t) \end{array}\right. }

すなわち

\displaystyle{(16)\quad \underbrace{ \left[\begin{array}{l} {x}_1(t) \\ {x}_2(t) \end{array}\right] }_{x(t)}= \underbrace{ \left[\begin{array}{cc} v_{11} & v_{12} \\ v_{21} & v_{22} \\ \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{l} y_1(t) \\ y_2(t) \end{array}\right] }_{y(t)} }

ここで、TV=VT=I_2が成り立つので、VはTの逆行列T^{-1}（V=T^{-1}）となっています。逆行列をもつ行列は正則行列と呼ばれます。

すなわち、微分方程式\dot{x}(t)=Ax(t)に対して、正則行列Tによる変数変換y(t)=Tx(t)を行ないます。x(t)=Vy(t)（V=T^{-1}）と両辺を微分した\dot{x}(t)=V\dot{y}(t)を代入して次を得ます。

\displaystyle{(17)\quad V\dot{y}(t)=AVy(t)\Rightarrow\dot{y}(t)=\underbrace{V^{-1}AV}_{\Lambda}y(t) }

この変換された行列\Lambdaをできるだけ対角化します。その議論は、線形代数において\Lambda=V^{-1}AVを標準形とする方法として学びました。もし\exp(\Lambda t)を求めることができると

\displaystyle{(18)\quad x(t)=Vy(t)=V\exp(\Lambda t)y(0)=V\exp(\Lambda t)V^{-1}x(0) }

となって、漸近安定性(5)の判定は、

\displaystyle{(19)\quad \boxed{\exp(\Lambda t)\rightarrow 0\qquad(t\rightarrow\infty)} }

の判定に帰着されます。ここで、収束先の0はn次の零行列です。

[4]　さて、２次行列の標準形は次の３つに分類されます。

\displaystyle{(20)\quad \Lambda_1= \left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right] \quad(\lambda(A)=\{\lambda_1,\lambda_2\}) }

\displaystyle{(21)\quad \Lambda_2= \left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right] \quad(\lambda(A)=\{\lambda_R\pm j\lambda_I\}) }

\displaystyle{(22)\quad \Lambda_3= \left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right] \quad(\lambda(A)=\{\lambda,\lambda\}) }

ここで、jは虚数単位\sqrt{-1}、\lambda_1,\lambda_2、\lambda_R,\lambda_I、\lambdaはすべて実数です。また任意のn次正方行列Xに対する行列式を{\rm det} Xで表すと

\displaystyle{(23)\quad \lambda(A)=\{\lambda:{\rm det}(\lambda I_n-A)=0\} }

すなわち、\lambda(A)は行列Aの固有値の集合です。{\rm det}(\lambda I_n-A)は特性多項式と呼ばれます。

上の３種類の\Lambdaについて\exp(\Lambda t)を具体的に計算してみます。その結果は次式となります。

\displaystyle{(24)\quad \exp(\Lambda_1 t)= \left[\begin{array}{cc} e^{\lambda_1t} & 0 \\ 0 & e^{\lambda_2 t} \end{array}\right] }

\displaystyle{(25)\quad \exp(\Lambda_2 t)=e^{\lambda_R t} \left[\begin{array}{cc} \cos(\lambda_It) & \sin(\lambda_It) \\ -\sin(\lambda_It) & \cos(\lambda_It) \end{array}\right] }

\displaystyle{(26)\quad \exp(\Lambda_3 t)=e^{\lambda t} \left[\begin{array}{cc} 1 & t \\ 0 & 1 \end{array}\right] }

したがって、漸近安定性(5)すなわち(19)が成り立つのは、(24)のとき\lambda_1<0かつ\lambda_2<0、(25)のとき\lambda_R<0、(26)のとき\lambda<0であることがわかると思います。したがって、

漸近安定性のための条件は行列Aのすべての固有値の実部が負であること

です。対偶をとれば、一つでも実部が負の固有値があれば不安定と判定します。実部が負の固有値を安定固有値、実部が非負の固有値を不安定固有値と呼びます。

●この漸近安定性の条件は、高次系についても同様に成り立ちます（Note A21-2を参照）。

●上の剛体振り子の場合の漸近安定性を判定するために行列Aの固有値を調べてみます。まず、平衡状態(\alpha)の場合、次式を得ます。

(27)\quad \begin{array}{l} \displaystyle{{\rm det}(\lambda I_2-\left[\begin{array}{cc} 0 & 1 \\ -\frac{3g}{4\ell}&-\frac{c}{J} \end{array}\right]) =\lambda^2+\frac{c}{J}\lambda+\frac{3g}{4\ell}=0}\\ \displaystyle{\Rightarrow \lambda=\frac{1}{2}(-\frac{c}{J}\pm\sqrt{\frac{c^2}{J^2}-4\frac{3g}{4\ell}})} \end{array} }

これから行列Aの固有値の実部は必ず負となります（根号内が負の場合は明らか、正の場合は根号を開いても\frac{c}{J}より小）。したがって、平衡状態(\alpha)は漸近安定と判定できます。

次に、平衡状態(\beta)の場合、次式を得ます。

(28)\quad \begin{array}{l} \displaystyle{{\rm det}(\lambda I_2-\left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}&-\frac{c}{J} \end{array}\right]) =\lambda^2+\frac{c}{J}\lambda-\frac{3g}{4\ell}=0}\\ \displaystyle{\Rightarrow \lambda=\frac{1}{2}(-\frac{c}{J}\pm\sqrt{\frac{c^2}{J^2}+4\frac{3g}{4\ell}})} \end{array} }

これから行列Aの固有値は実数となり、一つは正、他は負となります。したがって、平衡状態(\beta)は漸近安定ではないと判定できます。

実は２次系の場合特性多項式の係数がすべて正であることが、漸近安定であるための条件であることがラウスフルビッツの判定法として知られています。

演習A21-1…Flipped Classroom

1^\circ 行列指数関数について次の指数法則が成り立つことを示せ。

\displaystyle{(29)\quad XY=YX\Rightarrow\exp(X+Y)=\exp(X)\exp(Y) }

2^\circ 行列指数関数の定義から、(24)を示せ。

3^\circ 行列指数関数の定義から、次式を用いて、(25)を示せ。

\displaystyle{(30)\quad \left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right]= \underbrace{\lambda_RI_2}_{X}+ \underbrace{\lambda_I \left[\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right]}_{Y} }

4^\circ 行列指数関数の定義から、次式を用いて、(26)を示せ。

\displaystyle{(31)\quad \left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right]= \underbrace{\lambda I_2}_{X}+ \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right]}_{Y} }

演習A21-2…Flipped Classroom

与えられた行列Aに基づいて漸近安定性を調べるプログラムを次に示す。行列Pを実部が大きな順に表示されるように改良せよ。

MATLAB 
%a21.m
%-----
 clear all, close all
 A=rand(10);
 p=eig(A)
%-----
 sys=ss(A,[],[],[]);
 damp(sys)
%----- 
 P=[real(p) imag(p) -cos(angle(p)) abs(p) -1./real(p)]
%-----
%eof
SCILAB 
coming soon

Note A21-1　自由系の時間応答

n次自由系を表す微分方程式

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

の解は

\displaystyle{(2)\quad x(t)=\exp(At)x(0)}

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

元の微分方程式を

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

と書いて、左から積分因数と呼ばれる\exp(-At)をかけると

\displaystyle{(4)\quad\exp(-At)\dot{x}(t)-\exp(-At)Ax(t)=0}

すなわち

\displaystyle{(5)\quad\frac{d}{dt}(\exp(-At)x(t))=0}

したがって、cを定数ベクトルとして

\displaystyle{(6)\quad\exp(-At)x(t)=c\ \Rightarrow\ x(t)=\exp(At)c}

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

\displaystyle{(7)\quad x(t)=\exp(At)x(0)}

と表されます。

Note A21-2　高次系の漸近安定性

●行列指数関数を用いると、微分方程式

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

の解は次のように表されます。

\displaystyle{(2)\quad x(t)=\exp(At)x(0)=V\exp(\Lambda t)V^{-1}x(0)}

ここで

\displaystyle{(3)\quad \begin{array}{ll} &\exp(At)=V\exp(\Lambda t)V^{-1}={\displaystyle \sum_{i=1}^p\sum_{k=1}^{m_i}t^{k-1}}e^{\lambda_it}\,R_{ik} \\ &+{\displaystyle \sum_{i=1}^q\sum_{k=1}^{\ell_i}t^{k-1}}e^{\lambda_{Ri}t}\cos\lambda_{Ii}t\,C_{ik} +{\displaystyle \sum_{i=1}^q\sum_{k=1}^{\ell_i}t^{k-1}}e^{\lambda_{Ri}t}\sin\lambda_{Ii}t\,S_{ik} \end{array}

と書けることに注意します（R_{ik},C_{ik},S_{ik}は適当なn次実正方行列）。

したがって、任意のx(0)\ne0に対して、t\rightarrow\inftyのときx(t)\rightarrow 0となるための条件は

\displaystyle{(4)\quad \exp(\Lambda t)\rightarrow 0\quad(t\rightarrow\infty)}

となります。これはAのすべての固有値の実部が負を意味します。

●\dot{x}(t)=ax(t)（a=1,0,0.5）の解のグラフを見ると、a=0の場合は、漸近安定ではないが、発散はしないので、不安定とまではいえないのではないかと思うかもしれません。したがって零の固有値を不安定とみなすのか、安定とみなすか迷うところです。しかし、\dot{x}(t)=Ax(t)において、A=\left[\begin{array}{cc} 0& 1\\ 0& 0 \end{array}\right]の場合、解はx(t)=\left[\begin{array}{cc} 1& t\\ 0& 1 \end{array}\right]x(0)となって、x(0)の第２要素が零でない場合は発散します。

いま、\dot{x}(t)=Ax(t)において、Aのすべての固有値の実部は負または零の場合を考え、実部が零の固有値\lambdaの重複度をq\ge2とし、次が成り立つとします。

\displaystyle{(5)\quad n-{\rm rank}(A-\lambda I_n)=q}

左辺は幾何学的重複度、左辺は代数学的重複度と呼ばれています。すべての実部が零の固有値について、幾何学的重複度と代数学的重複度が等しいことが、解が発散しないために追加的に要請される条件であることが知られています。上の例の場合、幾何学的重複度は2-1=1、代数学的重複度は2ですから、この条件は成立していないことがわかります。

Note A21-3　自由系の時間応答は固有値と固有ベクトルからどのように構成されるか

●２次自由系

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

の解は次のように表されます。

\displaystyle{(2)\quad x(t)=\exp(At)x(0)}

２次行列Aの標準形は次の３つに分類されます。

\displaystyle{(3a)\quad \Lambda_1= \left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right] \quad(\lambda(A)=\{\lambda_1,\lambda_2\}) }

\displaystyle{(3b)\quad \Lambda_2= \left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right] \quad(\lambda(A)=\{\lambda_R\pm j\lambda_I\}) }

\displaystyle{(3c)\quad \Lambda_3= \left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right] \quad(\lambda(A)=\{\lambda,\lambda\}) }

ここで、jは虚数単位\sqrt{-1}、\lambda_1,\lambda_2、\lambda_R,\lambda_I、\lambdaはすべて実数です。

このとき２次行列は、対応する固有ベクトルを用いて、次のように表されます。

\displaystyle{(4a)\quad A=\underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]}_{V} \underbrace{\left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right]}_{\Lambda_1} \underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]^{-1}}_{V^{-1}} }

\displaystyle{(4b)\quad \begin{array}{l} A=\underbrace{\left[\begin{array}{cc} v_R+jv_I & v_R-jv_I \end{array}\right]}_{V} \left[\begin{array}{cc} \lambda_R+j\lambda_I & 0 \\ 0 & \lambda_R-j\lambda_I \end{array}\right]\\ \times\underbrace{\left[\begin{array}{cc} v_R+jv_I & v_R-jv_I \end{array}\right]^{-1}}_{V^{-1}}\\ =\underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]}_{V'} \underbrace{\left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right]}_{\Lambda_2} \underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]^{-1}}_{V'^{-1}} \end{array} }

\displaystyle{(4c)\quad A=\underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]}_{V} \underbrace{\left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right]}_{\Lambda_3} \underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]^{-1}}_{V^{-1}} }

ここで、v_1,v_2、v_R,v_I、v、v'はすべて２次元の実ベクトルです（v'は一般化固有ベクトル）。

これらの行列指数関数は次式となります。

\displaystyle{(5a)\quad \exp(A t)=\underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]}_{V} \underbrace{\left[\begin{array}{cc} e^{\lambda_1t} & 0 \\ 0 & e^{\lambda_2 t} \end{array}\right]}_{\exp(\Lambda_1 t)} \underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]^{-1}}_{V^{-1}} }

\displaystyle{(5b)\quad \exp(A t)=\underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]}_{V'} \underbrace{e^{\lambda_R t} \left[\begin{array}{cc} \cos(\lambda_It) & \sin(\lambda_It) \\ -\sin(\lambda_It) & \cos(\lambda_It) \end{array}\right]}_{\exp(\Lambda_2 t)} \underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]^{-1}}_{V'^{-1}} }

\displaystyle{(5c)\quad \exp(A t)=\underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]}_{V} \underbrace{e^{\lambda t} \left[\begin{array}{cc} 1 & t \\ 0 & 1 \end{array}\right]}_{\exp(\Lambda_3 t)} \underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]^{-1}}_{V^{-1}} }

これらを(2)に代入して

\displaystyle{(6a)\quad \begin{array}{l} x(t)=\underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]}_{V} \underbrace{\left[\begin{array}{cc} e^{\lambda_1t} & 0 \\ 0 & e^{\lambda_2 t} \end{array}\right]}_{\exp(\Lambda_1 t)} \underbrace{\left[\begin{array}{cc} v_1 & v_2 \end{array}\right]^{-1}x(0)}_{\left[\begin{array}{c} c_1\\c_2 \end{array}\right]}\\ =c_1e^{\lambda_1t}v_1+c_2e^{\lambda_2t}v_2 \end{array} }

\displaystyle{(6b)\quad \begin{array}{l} x(t)=\underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]}_{V'} \underbrace{e^{\lambda_R t} \left[\begin{array}{cc} \cos(\lambda_It) & \sin(\lambda_It) \\ -\sin(\lambda_It) & \cos(\lambda_It) \end{array}\right]}_{\exp(\Lambda_2 t)} \underbrace{\left[\begin{array}{cc} v_R & v_I \end{array}\right]^{-1}x(0)}_{\left[\begin{array}{c} c_1\\c_2 \end{array}\right]}\\ =c_1e^{\lambda_Rt}(\cos(\lambda_It)v_R-\sin(\lambda_It)v_I)+c_2e^{\lambda_Rt}(\sin(\lambda_It)v_R+\cos(\lambda_It)v_I) \end{array} }

\displaystyle{(6c)\quad \begin{array}{l} x(t)=\underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]}_{V} \underbrace{e^{\lambda t} \left[\begin{array}{cc} 1 & t \\ 0 & 1 \end{array}\right]}_{\exp(\Lambda_3 t)} \underbrace{\left[\begin{array}{cc} v & v' \end{array}\right]^{-1}x(0)}_{\left[\begin{array}{c} c_1\\c_2 \end{array}\right]}\\ =c_1e^{\lambda t}v+c_2e^{\lambda t}(tv+v') \end{array} }

●一般の自由系についても、同様の式が成り立ち、Aの固有値と固有ベクトルがどのように時間応答に寄与しているかの分析ができ、モード間の連成について考察することができます。

ドローン

投稿日時: 投稿者:

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

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{ \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] }

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

\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} }

倒立振子

投稿日時: 投稿者:

倒立振子…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

平衡状態回りの線形化

投稿日時: 投稿者:

平衡状態回りの線形化…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 剛体振り子と同じ周期をもつ単振り子の長さは？

線形系の状態空間表現

投稿日時: 投稿者:

線形系の状態空間表現…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.} }

を得ます。

１次系の追値制御

投稿日時: 投稿者:

１次系の追値制御…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)と呼びます。