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

1 状態空間表現

投稿日時: 投稿者:
【本章のねらい】
・状態空間表現を導き，その構造をブロック線図で表す。
・状態空間表現に対する操作として座標変換と直列結合を行う。

1.1 状態空間表現の導出とブロック線図

古典制御では，主に１入力１出力をもつ線形ダイナミカルシステム（線形系）の入出力特性に注目し，これを伝達関数で表した。一方，現代制御では，多入出力をもつ線形系において，「まず入力が状態に影響を及ぼし，つぎに状態の一部が出力として現れる」と考え，前者を状態方程式で，後者を出力方程式で表す。すなわち，状態方程式と出力方程式をペアにした状態空間表現

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

を求めることが出発点となる。ここで，実ベクトルx(t),\ u(t),\ y(t)は時刻tにおけるそれぞれn個の状態変数，m個の入力変数，p個の出力変数を要素にもち，状態，入力，出力を表す。nを次数または次元数と呼ぶ。また，A,\ B,\ C,\ Dは，それぞれサイズn\times n,\ n\times m,\ p\times n,\ p\times mの実行列である（実は，状態空間表現の係数行列には定着した呼び名がない。以下では，行列A,B,C,Dを，それぞれA行列，B行列，C行列，D行列と呼ぶことがある。）。

以下では，(1.1)式の状態空間表現で表されるm個の入力変数，p個の出力変数，n個の状態変数をもつ線形系を，簡単にm入力p出力n次系と呼ぶ。これには，つぎのような特別な場合が含まれる。

ここで，(1)は直達項Du(t)がない場合である。(2)と(4)は入力を考えない自由系(unforced system)の場合で，n次自由系と呼ぶ。(3)と(4)は状態方程式だけを考える場合で，y=xとみなす。

ここでは，制御対象のダイナミクスは連立された線形常微分方程式で表されるとし，これから連立１階線形微分方程式の行列表現を導いて状態方程式を得る。
一方，状態変数の一部がどのように取り出されるかを出力方程式で表す。

例題1.1　つぎの運動方程式を考える。

\displaystyle{ M\ddot{r}(t)+D\dot{r}(t)+Kr(t)=f(t) }

ここで，r(t)は変位，f(t)は外力，Mは質量，Dは減衰係数，Kはバネ定数である。f(t)を入力変数とし，r(t)とv(t)=\dot{r}(t)を状態変数とする２次系の状態方程式を導出せよ。
解答　運動方程式から，つぎの連立１階微分方程式を得る。

\displaystyle{ \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}f(t) \end{array}\right. }

これを行列表現して，r(t)とv(t)を状態変数，f(t)を入力変数とする，つぎの状態方程式を得る。

\displaystyle{ \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} \underbrace{f(t)}_{u(t)} }

演習1.1　つぎの運動方程式から２次自由系の状態方程式を導出せよ。

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

演習1.2　つぎの連成した運動方程式から，u(t)を入力変数とする４次系の状態方程式を導出せよ。

\displaystyle{ \left\{\begin{array}{l} \ddot{x}_1(t)=-(x_1(t)-x_2(t))+u(t) \\ \ddot{x}_2(t)=-(x_2(t)-x_1(t)) \end{array}\right. }

例題1.2　つぎの運動方程式を考える。

\displaystyle{ J\ddot{\theta}(t)+D\dot{\theta}(t)=\tau(t) }

ここで，\theta(t)は回転角，\tau(t)はトルク，Jは慣性モーメント，Dは減衰係数である。\tau(t)を入力変数とし，\theta(t)と\omega(t)=\dot{\theta}(t)を状態変数とする２次系の状態方程式を導出せよ。また，次の場合の出力方程式を示せ。
(1) \theta(t)を計測できる場合
(2) \omega(t)を計測できる場合
解答　運動方程式から，つぎの連立１階微分方程式を得る。

\displaystyle{ \left\{\begin{array}{l} \dot{\theta}(t)=\omega(t) \\ \dot{\omega}(t)=-\frac{D}{J}\omega(t)+\frac{1}{J}\tau(t) \end{array}\right. }

これを行列表現すると，\theta(t)と\omega(t)を状態変数，\tau(t)を入力変数とする，つぎの状態方程式を得る。

\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & -\frac{D}{J} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ \frac{1}{J} \end{array}\right] }_{B} \underbrace{\tau(t)}_{u(t)} }

(1) \theta(t)を計測できる場合の出力方程式として，次式を得る。

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

(2) \dot{\theta}(t)を計測できる場合の出力方程式として，次式を得る。

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

演習1.3　例題1.2の(2)を考える。角速度を\omega(t)=\dot{\theta}(t)とおくと，運動方程式は\dot{\omega}(t)+{\omega}(t)=\tau(t)となる。いま一定のトルク\tau^*のもとで，一定の角速度\omega^*を得ているとする。このとき，x(t)=\omega(t)-\omega^*を状態変数，u(t)=\tau(t)-\tau^*を入力変数，y(t)=\omega(t)-\omega^*を出力変数とする状態空間表現を導出せよ（一般に，零状態x=0が平衡状態を表すように状態空間表現を導出する。）。

演習1.4　つぎの２階線形常微分方程式から，u(t)を入力変数，y(t)を出力変数とする状態空間表現を導出せよ。

\displaystyle{ \ddot{y}(t)+a_1\dot{y}(t)+a_2y(t)=u(t) }

MATLABを用いて状態空間表現 sys を定義するには，たとえば例題1.2の(1)で，J=1,D=-0.01の場合，つぎのコマンドを与えればよい。

%state_space.m
A=[0 1;0 -0.01]; B=[0;1]; C=[1 0]; D=0;
sys=ss(A,B,C,D)

ここで，A行列，B行列，C行列，D行列を参照するには，それぞれ

sys.a，sys.b，sys.c，sys.d

を用いる。たとえばA行列の(2,2)要素を-0.1に変更するには，つぎのコマンドを与えればよい。

sys.a(2,2)=-0.1

また，B行列，D行列が存在しない自由系 sys0 を定義するには，つぎのコマンドを与えればよい。

%state_space_cont.m
A=[0 1;0 -0.01]; B=[]; C=[1 0]; D=[];
sys0=ss(A,B,C,D)

もし状態方程式だけを指定する場合は，C=[]; D=[]; または C=eye(size(A)); D=0; とする。

SCILABを用いて状態空間表現 sys を定義するには，たとえば例題1.2の(1)で，J=1,D=-0.01の場合，つぎのコマンドを与えればよい。

//state_space.m
A=[0 1;0 -0.01]; B=[0;1]; C=[1 0]; D=0;
sys=syslin(A,B,C,D)

ここで，A行列，B行列，C行列，D行列を参照するには，それぞれ

sys.a，sys.b，sys.c，sys.d

を用いる。たとえばA行列の(2,2)要素を-0.1に変更するには，つぎのコマンドを与えればよい。

A(2,2)=-0.1; sys.a=A

また，B行列，D行列が存在しない自由系 sys0 を定義するには，つぎのコマンドを与えればよい。

//state_space_cont.m
A=[0 1;0 -0.01]; B=[]; C=[1 0]; D=[];
sys0=syslin(A,B,C,D)

もし状態方程式だけを指定する場合は，C=[]; D=[]; または C=eye(size(A)); D=zeros(size(A,1),size(B,2)); とする。

さて，状態空間表現のブロック線図を作成することにより，どのように入力変数から状態変数を経て出力変数につながるかを表すことができる。これは，Simulinkなどを用いた時間応答シミュレーションに役に立つ。

例題1.3　つぎの１次系の状態空間表現をブロック線図で表せ。

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

解答　積分器の出力をxとすると，その入力は\dot{x}であることに注意し，状態方程式の左辺\dot{x}と右辺-\frac{1}{T}x+\frac{K}{T}uを等しくなるよう描いて，つぎのブロック線図を得る（加え合せ点○で符号を表示しない場合は＋とみなす。また，●は引き出し点を表す。）。

演習1.5　つぎの１次系の状態空間表現をブロック線図で表せ。

\displaystyle{ \left\{\begin{array}{l} \dot{x}(t)=u(t) \\ y(t)=x(t)+u(t) \end{array}\right. }

例題1.4　つぎの２次系の状態空間表現をブロック線図で表せ。

\displaystyle{ \left\{\begin{array}{l} %\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 \\ \omega_n^2 \end{array}\right] %}_{B} u(t) \\ y(t) = %\underbrace{ \left[\begin{array}{cc} c_1 & 0 \end{array}\right] %}_{C} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] %}_{x} \end{array}\right. }

解答　\dot{x}_1(t)=x_2(t),\ \dot{x}_2(t)=-\omega_n^2x_1(t)-2\zeta\omega_nx_2(t)+\omega_n^2u(t),\ y(t)=x_1(t)より，つぎのブロック線図を得る。

演習1.6　つぎの２次系の状態空間表現をブロック線図で表せ。

\displaystyle{ \left\{\begin{array}{l} %\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 \\ 0 & -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 \\ \omega_n^2 \end{array}\right] %}_{B} u(t) \\ y(t) = %\underbrace{ \left[\begin{array}{cc} 0 & c_2 \end{array}\right] %}_{C} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] %}_{x} \end{array}\right. }

演習1.7　演習1.2 で得られた状態空間表現について，ブロック線図を描け。

1.2 状態空間表現の座標変換と直列結合

n次系の状態空間表現

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

に対して，座標変換

\displaystyle{(1.3)\quad x'(t)=Tx(t)\quad({\rm det}T\ne0) }

を行うと，つぎの状態空間表現を得る。

\displaystyle{(1.4)\quad \left\{\begin{array}{l} \dot{x}'(t)=A'x'(t)+B'u(t) \\ y(t)=C'x'(t) \end{array}\right. }

ただし，変換後の係数行列は次式で与えられる。

\displaystyle{(1.5)\quad \left\{\begin{array}{l} A'=TAT^{-1} \\ B'=TB \\ C'=CT^{-1} \\ \end{array}\right. }

例題1.5　２次系の状態空間表現

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] = \left[\begin{array}{cc} 0 & -a_2 \\ 1 & -a_1 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + \left[\begin{array}{c} 1 \\ 0 \end{array}\right] u(t) \nonumber \\ y(t)= \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] \end{array}\right. }

に対して，つぎの座標変換を行え。

\displaystyle{ \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ 1 & -a_1 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }

解答　状態空間表現に

\displaystyle{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ 1 & -a_1 \end{array}\right]^{-1} \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] = \left[\begin{array}{cc} a_1 & 1 \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] }

を代入して

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{cc} a_1 & 1 \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} \dot{x}_1'(t) \\ \dot{x}_2'(t) \end{array}\right] = \left[\begin{array}{cc} 0 & -a_2 \\ 1 & -a_1 \end{array}\right] \left[\begin{array}{cc} a_1 & 1 \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] + \left[\begin{array}{c} 1 \\ 0 \end{array}\right] u \nonumber \\ y(t)= \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} a_1 & 1 \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] \end{array}\right. }

この状態方程式の左から

\displaystyle{ \left[\begin{array}{cc} a_1 & 1 \\ 1 & 0 \end{array}\right]^{-1} = \left[\begin{array}{cc} 0 & 1 \\ 1 & -a_1 \end{array}\right] }

をかけて，つぎの状態空間表現を得る。

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1'(t) \\ \dot{x}_2'(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ -a_2 & -a_1 \end{array}\right] \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \end{array}\right] u(t) \nonumber \\ y(t)= \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] \end{array}\right. }

演習1.8　２次系の状態空間表現

\displaystyle{ \left\{\begin{array}{l} %\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 \\ \omega_n^2 \end{array}\right] %}_{B} u(t) \\ 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} \end{array}\right. \quad(\zeta<1) }

に対して，つぎの座標変換を行え。

\displaystyle{ \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] = \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }

演習1.9　２次系の状態空間表現

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] = \left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + \left[\begin{array}{c} b_{1} \\ b_{2} \end{array}\right] u(t) \nonumber \\ y(t) = \underbrace{ \left[\begin{array}{cc} c_1 & c_2 \end{array}\right] }_C \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] \quad(c_1\ne0) \end{array}\right. }

に対して，どのような座標変換

\displaystyle{ \left[\begin{array}{c} x_1'(t) \\ x_2'(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} t_{11} & t_{12} \\ t_{21} & t_{22} \end{array}\right] }_T \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }

を行なえば，出力方程式をつぎの形にできるか。

\displaystyle{ y(t) = \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C'} \left[\begin{array}{c} x'_1(t) \\ x'_2(t) \end{array}\right] }

さて，つぎの２つの状態空間表現を考える。

\displaystyle{(1.6)\quad S_1: \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{(1.7)\quad S_2: \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. }

S_1の入力変数の数とS_2の出力変数の数が等しいとき，S_2の出力をS_1の入力に直接結合することを考える。その状態空間表現は次式で与えられる。

\displaystyle{(1.8)\quad \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}{c} 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. }

例題1.6　１次系の状態空間表現

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

の入力に，むだ時間を１次系として近似したときの状態空間表現

\displaystyle{ \left\{\begin{array}{l} \dot{x}_L(t)=-\frac{2}{L}x_L(t)+\frac{2}{L}u_L(t) \\ y_L(t)=2x_L(t)-u_L(t) \end{array}\right. }

の出力を結合して得られるシステムの状態空間表現を求めよ。
解答　まず，状態方程式は

\displaystyle{ \dot{x}(t)=-\frac{1}{T}x(t)+\frac{K}{T}u(t) }

にu(t)=y_L(t)=2x_L(t)-u_L(t)を代入し，

\displaystyle{ \dot{x}_L(t)=-\frac{2}{L}x_L(t)+\frac{2}{L}u_L(t) }

と合わせて

\displaystyle{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_L(t) \end{array}\right] = \left[\begin{array}{ccc} -\frac{1}{T} & 2\frac{K}{T} \\ 0 & -\frac{2}{L} \end{array}\right] \left[\begin{array}{c} x(t) \\ x_L(t) \end{array}\right] + \left[\begin{array}{c} -\frac{K}{T} \\ \frac{2}{L} \end{array}\right] u_L(t) }

のように得られる。また，出力方程式は

\displaystyle{ y(t)= \left[\begin{array}{ccc} 1 & 0 \end{array}\right] \left[\begin{array}{c} x(t) \\ x_L(t) \end{array}\right] }

のように得られる。

演習1.10 ２次系

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \end{array}\right] u(t) \nonumber \\ y(t) = \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] \end{array}\right. }

の入力に，むだ時間を２次系として近似したときの状態空間表現

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_{L1}(t) \\ \dot{x}_{L2}(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ -\frac{12}{L^2} & -\frac{6}{L} \end{array}\right] \left[\begin{array}{c} x_{L1}(t) \\ x_{L2}(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \end{array}\right] u_L(t) \nonumber \\ y_L(t) = \left[\begin{array}{cc} 0 & -\frac{12}{L} \end{array}\right] \left[\begin{array}{c} x_{L1}(t) \\ x_{L2}(t) \end{array}\right] +u_L(t) \end{array}\right. }

の出力を結合して得られるシステムの状態空間表現を求めよ。

MATLABを用いて座標変換を行うために，たとえば演習1.8で，\omega_n=1,\zeta=-0.01の場合，つぎのコマンドを与えればよい。

%coordinate_transformation.m
om=1; zeta=0.01;
A=[0 1;-om^2 -2*zeta*om]; B=[0;om^2]; C=[1 0]; D=0;
sys1=ss(A,B,C,D);
T=[1 0;-zeta*om om*sqrt(1-zeta^2)];
sys2=ss2ss(sys1,inv(T))

SCILABを用いて座標変換を行うために，たとえば演習1.8で，\omega_n=1,\zeta=-0.01の場合，つぎのコマンドを与えればよい。

//coordinate_transformation.m
om=1; zeta=0.01;
A=[0 1;-om^2 -2*zeta*om]; B=[0;om^2]; C=[1 0]; D=0;
sys1=syslin(‘c’,A,B,C,D);
T=[1 0;-zeta*om om*sqrt(1-zeta^2)];
sys2=ss2ss(sys1,T)

つぎに，MATLABを用いて直列結合を行うためには，たとえば演習1.10でL=1の場合，つぎのコマンドを与えればよい。

%serial_connection.m
A1=[0 1;0 0]; B1=[0;1]; C1=[1 0]; D1=0;
sys1=ss(A1,B1,C1,D1);
L=1;
A2=[0 1;-12/L^2 -6/L]; B2=[0;1]; C2=[0 -12/L]; D2=1;
sys2=ss(A2,B2,C2,D2);
sys=sys1*sys2

ここで， sys1 と sys2 の順番に注意する。

最後に，状態空間表現から伝達関数行列表現（8章の(8.2)式を参照。）を求めるには，コマンド tf を用いる。たとえば，上のむだ時間を２次系として近似したときの状態空間表現 sys2 の伝達関数を求めるには，コマンド tf(sys2) を与えればよい。

つぎに，SCILABを用いて直列結合を行うためには，たとえば演習1.10でL=1の場合，つぎのコマンドを与えればよい。

//serial_connection.m
A1=[0 1;0 0]; B1=[0;1]; C1=[1 0]; D1=0;
sys1=syslin(‘c’,A1,B1,C1,D1);
L=1;
A2=[0 1;-12/L^2 -6/L]; B2=[0;1]; C2=[0 -12/L]; D2=1;
sys2=syslin(‘c’,A2,B2,C2,D2);
sys=sys1*sys2

ここで， sys1 と sys2 の順番に注意する。

最後に，状態空間表現から伝達関数行列表現（8章の(8.2)式を参照。）を求めるには，コマンド ss2tf を用いる。たとえば，上のむだ時間を２次系として近似したときの状態空間表現 sys2 の伝達関数を求めるには，コマンド ss2tf(sys2) を与えればよい。

演習問題の解答

演習1.1　\omega=\dot{\theta}とおくと，\dot{\theta}=\omega, \dot{\omega}=-\frac{3g}{4\ell}\theta。したがって，つぎの状態方程式を得る。

\displaystyle{ \left[\begin{array}{c} \dot{\theta} \\ \dot{\omega} \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ -\frac{3g}{4\ell} & 0 \end{array}\right] \left[\begin{array}{c} \theta \\ \omega \end{array}\right] }

演習1.2　v_1=\dot{x}_1, v_2=\dot{x}_2とおくと，\dot{x}_1=v_1, \dot{v}_1=-(x_1-x_2)+u，\dot{x}_2=v_2, \dot{v}_2=-(x_2-x_1)。したがって，つぎの状態方程式を得る。

\displaystyle{ \left[\begin{array}{c} \dot{x}_1 \\ \dot{v}_1 \\ \dot{x}_2 \\ \dot{v}_2 \end{array}\right] = \left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ -1 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 1 & 0 & -1 & 0 \\ \end{array}\right] \left[\begin{array}{c} x_1 \\ v_1 \\ x_2 \\ v_2 \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \\ 0 \\ 0 \end{array}\right] u }

または

\displaystyle{ \left[\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \\ \dot{v}_1 \\ \dot{v}_2 \end{array}\right] = \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ -1 & 1 & 0 & 0 \\ 1 & -1 & 0 & 0 \\ \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\ v_1 \\ v_2 \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ 1 \\ 0 \end{array}\right] u }

演習1.3　一定のトルク\tau^*のもとで一定の角速度\omega^*を得ている。これらは運動方程式\dot{\omega}=-{\omega}+\tauを満足するので，0=-\omega^*+\tau^*。これを運動方程式から辺々引き算して，つぎの１次系としての状態空間表現を得る。

\displaystyle{ \left\{\begin{array}{l} \underbrace{\frac{d}{dt}(\omega-\omega^*)}_{\dot x} =\underbrace{-1}_{a}\underbrace{(\omega-\omega^*)}_{x} +\underbrace{1}_{b} \underbrace{(\tau-\tau^*)}_{u} \\ \underbrace{\omega-\omega^*}_{y} =\underbrace{1}_{c}\underbrace{\omega-\omega^*}_{x} \end{array}\right. }

演習1.4　x_1={y}, x_2=\dot{y}とおくと，\dot{x}_1=x_2, \dot{x}_2=-a_1x_2-a_2x_1+u。したがって，つぎの状態空間表現を得る。

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \\ \end{array}\right] = \left[\begin{array}{cccc} 0 & 1 \\ -a_2 & -a_1 \\ \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\ \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \\ \end{array}\right] u \\ y = \left[\begin{array}{cccc} 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \\ \end{array}\right] \end{array}\right. }

または

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_2 \\ \dot{x}_1 \\ \end{array}\right] = \left[\begin{array}{cccc} -a_1 & -a_2 \\ 1 & 0 \\ \end{array}\right] \left[\begin{array}{c} x_2 \\ x_1 \\ \end{array}\right] + \left[\begin{array}{c} 1 \\ 0 \\ \end{array}\right] u \\ y = \left[\begin{array}{cccc} 0 & 1 \end{array}\right] \left[\begin{array}{c} x_2 \\ x_1 \\ \end{array}\right] \end{array}\right. }

演習1.5　\dot{x}=u,\ y=x+uより，つぎのブロック線図を得る。

演習1.6　\dot{x}_1=x_2,\ \dot{x}_2=-2\zeta\omega_nx_2+\omega_n^2u,\ y=c_2x_2より，つぎのブロック線図を得る。

演習1.7　\dot{v}_1=-(x_1-x_2)+u,\ \dot{v}_2=-(x_2-x_1)より，つぎのブロック線図を得る。

演習1.8　状態空間表現に

\displaystyle{ \left[\begin{array}{c} x_1 \\ x_2 \end{array}\right] = \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right] \left[\begin{array}{c} x_1' \\ x_2' \end{array}\right] }

を代入して

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right] \left[\begin{array}{c} \dot{x}_1' \\ \dot{x}_2' \end{array}\right] \nonumber \\ = \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right] \left[\begin{array}{c} x_1' \\ x_2' \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \end{array}\right] u \nonumber \\ y= \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right] \left[\begin{array}{c} x_1' \\ x_2' \end{array}\right] \end{array}\right. }

この状態方程式の左から

\displaystyle{ \left[\begin{array}{cc} 1 & 0 \\ -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \end{array}\right]^{-1} = \frac{1}{\omega_n\sqrt{1-\zeta^2}} \left[\begin{array}{cc} \omega_n\sqrt{1-\zeta^2} & 0 \\ \zeta\omega_n & 1 \end{array}\right] }

をかけて，つぎの状態空間表現を得る。

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1' \\ \dot{x}_2' \end{array}\right] = \left[\begin{array}{cc} -\zeta\omega_n & \omega_n\sqrt{1-\zeta^2} \\ -\omega_n\sqrt{1-\zeta^2} & -\zeta\omega_n \end{array}\right] \left[\begin{array}{c} x_1' \\ x_2' \end{array}\right] + \left[\begin{array}{c} 0 \\ \frac{1}{\omega_n\sqrt{1-\zeta^2}} \end{array}\right] u \nonumber \\ y= \left[\begin{array}{cc} 1 & 0 \end{array}\right] \left[\begin{array}{c} x_1' \\ x_2' \end{array}\right] \end{array}\right. }

演習1.9

\displaystyle{ \underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] }_{C'} \underbrace{ \left[\begin{array}{cc} t_{11} & t_{12} \\ t_{21} & t_{22} \end{array}\right] }_T = \underbrace{ \left[\begin{array}{cc} c_1 & c_2 \end{array}\right] }_C \quad(Tは正則) }

を満足させればよいので，たとえばt_{11}=c_1(\ne0), t_{12}=c_2, t_{21}=0, t_{22}=1と選べばよい。

演習1.10

\displaystyle{ \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \\\hdashline \dot{x}_{L1}(t) \\ \dot{x}_{L2}(t) \end{array}\right] = \left[\begin{array}{cc:cc} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & -\frac{12}{L} \\\hdashline 0 & 0 & 0 & 1 \\ 0 & 0 & -\frac{12}{L^2} & -\frac{6}{L} \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \\ \hdashline x_{L1}(t) \\ x_{L2}(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \\ \hdashline 0 \\ 1 \end{array}\right] u_L(t) \\ y(t)= \left[\begin{array}{cc:cc} 1 & 0 & 0 & 0 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \\ \hdashline x_{L1}(t) \\ x_{L2}(t) \end{array}\right] \end{array}\right. }

UV Control

投稿日時: 投稿者:

●Drone becomes to be very popular tool to take a video from the sky. It is a surprise that we can control its motion with 6 degrees of freedom so smartly. What makes it possible?

In general, a drone has 4 propellers as shown. This means that 4 degrees of freedom can be controlled. They should be the heave z and the roll \phi, the pitch \theta, the yaw \psi. Assume that rotational speeds of 4 propellers can be set to the specified values, \omega_{1c}, \omega_{2c}, \omega_{3c}, \omega_{4c}.

Then we can produce 1 force F_z and 3 torques \tau_\phi, \tau_\theta, \tau_\psi to control the heave z and the roll \phi, the pitch \theta, the yaw \psi respectively by combining the 4 propellers’ thrusts as follows:

\left[\begin{array}{c} F_z \\ \tau_\phi \\ \tau_\theta \\ \tau_\psi \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] \left[\begin{array}{c} \omega_{1c} \\ \omega_{2c} \\ \omega_{3c} \\ \omega_{4c} \end{array}\right].

This means that the MIMO dynamics is decoupled into 4 SISO dynamics. We will employ a PID controller corresponding to each SISO dynamics. Therefore it is the static decoupling to make the control system design easier.

●Many underwater vehicles have been developed for various purposes. For example, we can find them as follows.

Note that 4 to 8 thrustors are equipped to realize the static decoupling. The number depends on motion necessary to achieve the mission of UVs. Most control systems will be designed in the same way as drone.

●Prof. Koterayama and Prof. Nakamura developed the following UV called as DELTA.

The mission of DELTA is for wide-area survey. It works in a towing mode for wade scanning, and in a self-propulsive mode for local investigation. The detail is discussed in the paper:

Masahiko NAKAMURA, Hiroyuki KAJIWARA, Wataru KOTERAYAMA:
Development of an ROV operated both as towed and self-propulsive vehicle
Ocean Engineering, vol.28, no.1, pp.1-43, 2000.

DELTA has only two thrustors. So only the heave z and the yaw \psi are controlled. However it is necessary to control the roll \phi and the pitch \theta to keep the equilibrium state. Such a control system can’t be realized by PID control. The following control system is designed by LQG approach.

For any UV, the collision avoidance is an important function. For example, a deep diving will be necessary to avoid an obstacle as follows.

In order to change the depth in DELTA, there is no way except decreasing the thrust. This means the large speed variation, by which hydrodynamic force will be varied largely. So we need a robust controller or a scheduling controller. Such a control scheme is depicted in the following.

Ship Control

投稿日時: 投稿者:

●For the directional control of a ship, consider the following NOMOTO model.

\displaystyle{ \dot{r}(t)=-\frac{1}{T(t)}r(t)+\frac{K(t)}{T(t)}\delta(t-L)+w(t)} }

where r(t)=\dot{\psi}(t) and

\displaystyle{ T(t)=\frac{L}{U(t)}T',\ K(t)=\frac{U(t)}{L}K' \quad (U_1\le U(t)\le U_2) }

We assume the time lag L to take account of the delay to get the rudder effect. We use the same notation for the ship length L assuming no confusion. The time constant T and gain constant K are varied according to the variation of forward speed U. From the following equation, it is clear that the rudder effect is proportional to the square of the forward speed.

\displaystyle{ \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{L}\right)\frac{1}{T'}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{L}\right)^2\frac{K'}{T'}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }

Given a nominal forward speed U^*, the following equation holds.

\displaystyle{ \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{U^*}\right)\frac{1}{T^*}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{U^*}\right)^2\frac{K^*}{T^*}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }

where

\displaystyle{ T^*=\frac{L}{U^*}T',\ K^*=\frac{U^*}{L}K' \quad (U_1\le U^*\le U_2) }

●Zig-Zag Test:　Neglecting the time lag L and the forward speed variation, a simulation example of the Zig-Zag test is given as follows.

From this response, we used to obtain both time constant T and gain constant K. However, the response is not always the step response by which a linear system is completely characterized. As a ship has dynamics of an astatic (no-fixed-position) system, the Zig-Zag test is a compromise way for its identification.

We should notice that if we apply the unity feedback as shown in the above we can obtain the step response and identify (\zeta,\omega_n) from (T_p, 1+p_0) by the following formula.

\displaystyle{ (T_p, 1+p_0)\ \Rightarrow\ (\zeta,\omega_n)= \left( \sqrt{\frac{(\log p_0)^2}{(\log p_0)^2+\pi^2}}, \frac{\sqrt{(\log p_0)^2+\pi^2}}{T_p} \right) }

●PID control: Under the nominal forward speed U^*, neglecting the time lag L, consider the following NOMOTO model.

\displaystyle{ \dot{r}(t)=-\frac{1}{T^*}r(t)+\frac{K^*}{T^*}\delta(t)+w(t) }

Then PID control is presented as follows.

\displaystyle{ \delta(t)=K_P(\psi_c-\psi(t))+K_D\frac{d}{dt}(\psi_c-\psi(t))+K_I\int_0^t(\psi_c-\psi(\tau))\,d\tau }

Although we used to tune the P gain K_P, the D gain K_D, the I gain K_I on the site, here we want try to determine them based on the NOMOTO model. The closed-loop system by PID control is given by

Here assuming \dot{\psi}_c=0, we have

\displaystyle{ \delta(t)=-K_P\psi(t)-K_Dr(t)+K_P\psi_c+K_I\int_0^t(\psi_c-\psi(\tau))\,d\tau }

Therefore, we have another configuration of PID control system.

Letting \zeta=\frac{1}{2\sqrt{T^*K^*}},\omega_n=\sqrt{\frac{K^*}{T^*}}, the following state equation is obtained.

\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & -2\zeta\omega_n \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] }_{B} \underbrace{\delta(t)}_{u(t)} + \left[\begin{array}{c} 0 \\ w \end{array}\right] }

Then PID control is a state feed back with an integral action as follows.

\displaystyle{ \underbrace{\delta(t)}_{u(t)} =- \underbrace{ \left[\begin{array}{cc} K_P & K_D \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \end{array}\right] }_{x(t)} +\underbrace{K_P}_{G} \underbrace{\psi_c}_{v} +K_I \underbrace{\int_0^t(\psi_c-\psi(\tau))\,d\tau}_{x_I(t)} }

Therefore, after specifying the desired first overshoot (T_p',1+p_0'), we calculate the corresponding to (\zeta',\omega_n') and then determine PID gains by

\displaystyle{ K_P=\frac{\omega_n'^2}{\omega_n^2},\ K_D=\frac{2}{\omega_n^2}(\zeta'\omega_n'-\zeta\omega_n),\ K_I=\omega_I\frac{\omega_n'^2}{\omega_n^2} }

Here we try \omega_I=\frac{w_n'}{10} at first, and then tune it by observing the disturbance rejection.

The CLPS by PID control is presented by

\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_{CL}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\omega_n'^2 & -2\zeta'\omega_n' & \omega_I\omega_n'^2 \\ -1 & 0 & 0 \end{array}\right] }_{A_{CL}} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ x_I(t) \end{array}\right] }_{x_{CL}(t)} + \underbrace{ \left[\begin{array}{cc} 0 & 0\\ \omega_n'^2 & 1\\ 1 & 0 \end{array}\right] }_{B_{CL}} \left[\begin{array}{c} \psi_c \\ w \end{array}\right] }

Here, assuming \dot{w}=0, we differentiate it and obtain

\displaystyle{ \underbrace{ \left[\begin{array}{c} \ddot{\psi}(t) \\ \ddot{r}(t) \\ \ddot{x}_I(t) \end{array}\right] }_{\ddot{x}_{CL}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\omega_n'^2 & -2\zeta'\omega_n' & \omega_I\omega_n'^2 \\ -1 & 0 & 0 \end{array}\right] }_{A_{CL}} \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{x}_I(t) \end{array}\right] }_{\dot{x}_{CL}(t)} }

From the CLPS stability, we have

\displaystyle{ \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ x_I(t) \end{array}\right] }_{x_{CL}} \rightarrow \underbrace{ \left[\begin{array}{ccc} 0 & -1 & 0\\ \omega_n'^2 & 2\zeta'\omega_n' & -\omega_I\omega_n'^2 \\ 1 & 0 & 0 \end{array}\right]^{-1} }_{-A_{CL}^{-1}} \underbrace{ \left[\begin{array}{cc} 0 \\ \omega_n'^2\psi_c +w\\ \psi_c \end{array}\right] }_{B_{CL}} = \left[\begin{array}{cc} \psi_c\\ 0\\ \frac{w}{\omega_I\omega_n'^2} \end{array}\right] }

This means that the integral action is estimating the unknown disturbance.

The following simulation shows the CLPS responses by PD control. It is observed that the disturbance rejection is not sufficient because of the lack of integral action.

The following simulation shows the CLPS responses by PID control. Here \omega_I=2\frac{w_n'}{10}. It is observed that the disturbance rejection is satisfied but the transient response is fractured a little bit being off expectation.

●LQI control: We will try to determine the PID gains in the framework of LQI control. Consider the following error system:

\displaystyle{ \left[\begin{array}{c} \dot\psi(t)-\dot{\psi}_c \\ \dot r(t) \\ \dot{\delta}(t) \end{array}\right] = \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & -2\zeta\omega_n &\omega_n^2 \\ 0 & 0 & 0 \end{array}\right] \left[\begin{array}{c} \psi(t)-\psi_c \\ r(t) \\ \delta(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] \dot{\delta} (t) }

For the error system, we will minimize the following criterion function:

\displaystyle{ J=\int_0^\infty(\frac{1}{M_\psi^2}(\psi(t)-\psi_c)^2+\frac{T_\psi^2}{M_\psi^2}r^2(t) +\frac{1}{M_\delta^2}\delta^2(t)+\frac{T_\delta^2}{M_\delta^2}\dot{\delta}^2(t))\,dt }

Solving this problem, the PID gains are obtained as follows:

\displaystyle{ \dot{\delta}(t) =- \left[\begin{array}{ccc} f_\psi & f_r & f_\delta \end{array}\right] \left[\begin{array}{c} \psi(t)-\psi_c \\ r(t) \\ \delta(t) \end{array}\right] }
\displaystyle{ =- \underbrace{ \left[\begin{array}{ccc} f_\psi & f_r & f_\delta \end{array}\right] \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & -2\zeta\omega_n &\omega_n^2 \\ 1 & 0 & 0 \end{array}\right]^{-1} }_{ \left[\begin{array}{ccc} K_P & K_D & K_I \end{array}\right] } \left[\begin{array}{c} \dot\psi(t)-\dot{\psi}_c \\ \dot{r}(t) \\ \psi(t)-\psi_c \end{array}\right] }

In the case of M_{\psi}=1, T_{\psi}=0.5T, M_{\delta}=0.5, T_{\delta}=0.1T, we have the following simulation result, which seems to be very nice. Note that there is no feedforward term compared with PID control.

However, in the case of having the time lag L=9, the CLPS is unstable as follows:

●LPV control:

Assuming the following speed variation, the responses of CLPS by unity feedback vary largely.

Consider the following NOMOTO model based on a nominal forward speed U^*.

\displaystyle{ \left\{\begin{array}{l} \dot{\psi}(t)=r(t) \\ \displaystyle{\dot{r}(t)=-\left(\frac{U}{U^*}\right)\frac{1}{T^*}r(t) +\left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*}\delta(t)} \end{array}\right. }

Adding the rudder dynamics,

\displaystyle{ \dot{\delta}(t)=-\frac{1}{T_a}\delta(t)+\frac{K_a}{T_a}u(t) }

we have the following state-space representation.

\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{\delta}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ 0 & -\left(\frac{U}{U^*}\right)\frac{1}{T^*} & \left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*} \\ 0 & 0 & -\frac{1}{T_a} \end{array}\right] }_{A(U,U^2)} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ \frac{K_a}{T_a} \end{array}\right] }_{B} u(t) }
\displaystyle{ \underbrace{ \psi(t) }_{y(t)} = \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} }

Note that it is possible to have the following polytopic representation.

\displaystyle{ A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3 }

where

\displaystyle{A_1=A(U_1,U_1^2)}
\displaystyle{A_2=A(U_2,U_2^2)}
\displaystyle{A_3=A(\frac{U_1+U_2}{2},U_1U_2)}　

Defining U_3=\frac{U_1+U_2}{2},

\displaystyle{ p_1(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U-U_3 & U_2-U_3 \\ U^2-U_1U_2 & U_2^2-U_1U_2 \\ \end{array}\right]}
\displaystyle{ p_2(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_3 & U-U_3 \\ U_1^2-U_1U_2 & U^2-U_1U_2 \\ \end{array}\right]}
\displaystyle{ p_3(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_2 & U_2-U \\ U_1^2-U_2^2 & U_2^2-U^2 \\ \end{array}\right]}
\displaystyle{ p_0=\det \left[\begin{array}{cc} U_1-U_2 & U_2-U_3 \\ U_1^2-U_2^2 & U_2^2-U_1U_2 \\ \end{array}\right]}
\displaystyle{p_1(U,U^2)+p_2(U,U^2)+p_3(U,U^2)=1}

Based on this model, the framework of LPV control is proposed. LPV control can cover the variation included in the triangle region with the vertexes (U_1,U_1^2), (U_2,U_2^2) and temporal vertex (U_3,U_1U_2).

Constitute the following 2-port system.

\displaystyle{ P: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x} \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A(U,U^2)& 0 \\ -C & 0 \end{array}\right] }_{\cal{A}(U,U^2)} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_1} r + \underbrace{\left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_2} u\\ \underbrace{ \left[\begin{array}{c} y_{11} \\ y_{12} \end{array}\right] }_{y_1} = \underbrace{ \left[\begin{array}{cc} 0 &\omega_I\\ \omega_DCA(U,U^2) & 0 \end{array}\right] }_{C_1} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{11}} r + \underbrace{ \left[\begin{array}{c} 0 \\ \omega_DCB \end{array}\right] }_{D_{12}} u\\ \underbrace{ \left[\begin{array}{c} y \\ x_I \end{array}\right] }_{y_2} = \underbrace{ \left[\begin{array}{cc} C & 0\\ 0 & 1 \end{array}\right] }_{C_2} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{21}} r \end{array}\right.}

A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3

For this LPV model, design the following LPV controller.

\displaystyle{ K_0: \left\{\begin{array}{l} \dot{x}_K=A_K(U,U^2)x_K+ \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & B_K^{(2)}(U,U^2) \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \\ u=C_K(U,U^2)x_K + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \end{array}\right.}

A_K(U,U^2)=p_1(U,U^2)A_{K1}+p_2(U,U^2)A_{K2}+p_3(U,U^2)A_{K3}
B_K(U,U^2)=p_1(U,U^2)B_{K1}+p_2(U,U^2)B_{K2}+p_3(U,U^2)B_{K3}
C_K(U,U^2)=p_1(U,U^2)C_{K1}+p_2(U,U^2)C_{K2}+p_3(U,U^2)C_{K3}
D_K(U,U^2)=p_1(U,U^2)D_{K1}+p_2(U,U^2)D_{K2}+p_3(U,U^2)D_{K3}

Form this, the following output feedback is obtained.

\displaystyle{ K: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_K \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A_K(U,U^2) & B_K^{(2)}(U,U^2) \\ 0 & 0 \end{array}\right] }_{A_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & 0\\ -1& 1 \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \\ u= \underbrace{ \left[\begin{array}{cc} C_K(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{C_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & 0 \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \end{array}\right. }

We can obtain H_\infty control by fixing as U=U^*. The following simulation shows the CLPS responses by H_\infty control. It is observed that the responses are slightly varied according to the three kinds of speed variation.


On the other hand, the following simulation shows the CLPS responses by the LPV control. It is observed that the responses are almost same in spite of the three kinds of speed variation.



LQG Control

投稿日時: 投稿者:

1. Optimal Control for MIMO system

\bullet Given an nth-order system (satisfying controllability and observability)

(1)   \begin{eqnarray*} &&\dot{x}(t)=Ax(t)+Bu(t),\ y(t)=Cx(t)\ \\ &&(x(t)\in{\rm\bf R}^n,\ u(t)\in{\rm\bf R}^m,\ y(t)\in{\rm\bf R}^p) \end{eqnarray*}

and the stabilizing state feedback

(2)   \begin{eqnarray*} u(t)=-Fx(t), \end{eqnarray*}

the closed-loop system is represented by

(3)   \begin{eqnarray*} &&\dot{x}(t)=\underbrace{(A-BF)}_{A_F}x(t),\ y(t)=Cx(t)\\ &&A_F=A-BF:\ stable\ matrix. \end{eqnarray*}

The behaviors of the state and the input are given by

(4)   \begin{eqnarray*} x(t)=\exp(A_Ft)x(0) \end{eqnarray*}

and

(5)   \begin{eqnarray*} u(t)=-F\exp(A_Ft)x(0). \end{eqnarray*}

respectively. Then consider a problem to determine F to minimize the criterion function

(6)   \begin{eqnarray*} J=\int_0^\infty(y^T(t)Qy(t)+u^T(t)Ru(t))\,dt. \end{eqnarray*}

\bullet The criterion function can be written as

(7)   \begin{eqnarray*} J&=&\int_0^\infty x^T(t)(C^TQC+F^TRF)x(t)\,dt\\ &=&x^T(0)\underbrace{\int_0^\infty \exp(A_F^Tt)(C^TQC+F^TRF)\exp(A_Ft)\,dt}_{\Pi}x(0)\\ &=&{\rm tr}\ \Pi x(0)x^T(0). \end{eqnarray*}

Note that we have the following constraint on \Pi:

(8)   \begin{eqnarray*} \Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0. \end{eqnarray*}

It is known that \Pi>0 holds because of the closed-loop stability. Taking account of all zero-input responses, instead of (7), we consider to minimize

(9)   \begin{eqnarray*} J={\rm tr}\ \Pi. \end{eqnarray*}

Therefore, we will minimize

(10)   \begin{eqnarray*} J'={\rm tr}\ \Pi+{\rm tr}\ ((\Pi A_F+A_F^T\Pi+C^TQC+F^TRF)\Gamma) \end{eqnarray*}

using undetermined multiplier \Gamma and the stability constraint (8). As the necessary conditions, we have

(11)   \begin{eqnarray*} \frac{\partial J'}{\partial\Pi}=I_n+\Gamma A_F^T+A_F\Gamma=0\Rightarrow \Gamma>0, \end{eqnarray*}

(12)   \begin{eqnarray*} \frac{\partial J'}{\partial F}=2(RF-B^T\Pi)\Gamma=0\Rightarrow F=R^{-1}B^T\Pi, \end{eqnarray*}

(13)   \begin{eqnarray*} \frac{\partial J'}{\partial\Gamma}=\Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0. \end{eqnarray*}

Substituting F=R^{-1}B^T\Pi into (13),

(14)   \begin{eqnarray*} \Pi (A-BR^{-1}B^T\Pi)+(A-BR^{-1}B^T\Pi)^T\Pi\\ +C^TQC+(R^{-1}B^T\Pi)^TRR^{-1}B^T\Pi=0 \end{eqnarray*}

That is, we have the Algebraic Riccati Equation (ARE) on \Pi:

(15)   \begin{eqnarray*} {\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0}. \end{eqnarray*}

By solving this, we can obtain \Pi>0 and then F by

(16)   \begin{eqnarray*} {F=R^{-1}B^T\Pi}. \end{eqnarray*}

The control methodology is called as LQ Control.

\bullet The sufficiency is discussed as follows. The following expression can be derived:

(17)   \begin{eqnarray*} y^TQy+u^TRu=(u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x) -\underbrace{\frac{d}{dt}x^T\Pi x}_{2x^T\Pi\dot{x}}. \end{eqnarray*}

In fact, substituting \dot{x}=Ax+Bu into the right hand side and using the Riccati equation,

(18)   \begin{eqnarray*} &&{\rm RHS}=u^TRu+2x^T\Pi Bu+x^T\Pi BR^{-1}B^T\Pi x-2x^T\Pi(Ax+Bu)\\ &&=u^TRu+x^T(\Pi A+A^T\Pi+C^TQC)x-2x^T\Pi Ax={\rm LHS}. \end{eqnarray*}

Integrating the both side,

(19)   \begin{eqnarray*} J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt-\left[x^T\Pi x\right]_0^\infty. \end{eqnarray*}

As x(t)=\exp(A_Ft)x(0)\rightarrow 0\ (t\rightarrow\infty),

(20)   \begin{eqnarray*} J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt+x^T(0)\Pi x(0). \end{eqnarray*}

Therefore, it is shown that u=-R^{-1}B^T\Pi x minimizes the criterion function.

Exercise 1
Given A=\left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right],\ B=\left[\begin{array}{cc} 0 \\ 1 \end{array}\right],\ C=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right], Q=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right],R=1, obtain the solution \Pi= \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \\ \end{array}\right]>0\Leftrightarrow \pi_1>0,\ \pi_1\pi_2-\pi_3^2>0 of the Riccati equation \Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0 and calculate F=R^{-1}B^T\Pi.

Expanding the Ricatti equation

(21)   \begin{eqnarray*} && \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 1 & 0 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \nonumber\\ &&-\left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ 1 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] + \left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \nonumber , \end{eqnarray*}

we have

(22)   \begin{eqnarray*} \left\{ \begin{array}{l} -\pi_3^2+1=0\\ \pi_1-\pi_2\pi_3=0\\ 2\pi_3-\pi_2^2+1=0 \end{array} \right. \nonumber \end{eqnarray*}

There are the two solutions on \pi_3 from -\pi_3^2+1=0, the two solutions on \pi_2 from 2\pi_3-\pi_2^2+1=0, the one solution \pi_1 from \pi_1-\pi_2\pi_3=0. Therefore, we have the four kinds of solution (\pi_1,\pi_2,\pi_3) as follows:

(23)   \begin{eqnarray*} \left\{\begin{array}{llll} \pi_3= 1 & \Rightarrow \left\{\begin{array}{l} \pi_2= \sqrt{3}\\ \pi_2=-\sqrt{3} \end{array}\right. & \left.\begin{array}{l} \Rightarrow\pi_1= \sqrt{3}\\ \Rightarrow\pi_1=-\sqrt{3} \end{array}\right. & \left.\begin{array}{ll} (1) &○\\ (2) &× \end{array}\right. \\ \pi_3=-1 & \Rightarrow \left\{\begin{array}{llll} \pi_2= j \\ \pi_2=-j \end{array}\right. & \hspace{-3mm} \left.\begin{array}{l} \Rightarrow\pi_1=-j\\ \Rightarrow\pi_1=j \end{array}\right. & \left.\begin{array}{ll} (3) &×\\ (4) &× \end{array}\right. \end{array}\right. . \end{eqnarray*}

The only (1) satisfies \pi_1,\pi_2>0,\ \pi_1\pi_2-\pi_3^2>0. Therefore we have

(24)   \begin{eqnarray*} F= \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} 1 & \sqrt{3} \end{array}\right] . \end{eqnarray*}

●How to solve ARE
Given the Riccati equation \Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0, consider the Hamilton matrix

(25)   \begin{eqnarray*} {M=\left[\begin{array}{cc} A & -BR^{-1}B^T \\ C^TQC & -A^T \end{array}\right]}. \end{eqnarray*}

The eigenvalues of the Hamilton matrix with size n\times n are distributed symmetrically to not only the real axis but also the imaginal axis. So there are n stable eigenvalues and n unstable eigenvalues. The eigenvectors corresponding to n stable eigenvalues \lambda_1,\cdots,\lambda_n are obtained as follows:

(26)   \begin{eqnarray*} \underbrace{ \left[\begin{array}{cc} A & -BR^{-1}B^T \\ -C^TQC & -A^T \end{array}\right]}_{M(2n\times 2n)} \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)} = \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)} \underbrace{ {\rm diag}\{\lambda_1,\cdots,\lambda_n\} }_{\Lambda^-(n\times n)}. \end{eqnarray*}

Based on this, we can obtain \Pi as

(27)   \begin{eqnarray*} \Pi=V_2V_1^{-1}. \end{eqnarray*}

A program by SCILAB to solve the Riccati equation is given as follows.

//opt.sce
function [F,p]=opt(A,B,C,Q,R)
W=R\B’; [V,R]=spec([A -B*W;-C’*Q*C -A’]);
p=diag(R); [dummy,index]=gsort(real(p));
n=size(A,1); j=index(n+1:n+n);
p=p(j); V1=V(1:n,j); V2=V(n+1:n+n,j);
X=real(V2/V1); F=W*X;
endfunction
//eof

Solve the Riccati equation in Exercise 1 by using the above program.

A=[0 1;0 0]; B=[0;1]; C=eye(2,2);
Q=diag([1 1]); R=1;
[F,p]=opt(A,B,C,Q,R)
poles=spec(A-B*F)

Exercise 2
Consider the following spring-connected carts as a control object.

The motion is governed by

(28)   \begin{eqnarray*} \left\{\begin{array}{l} m_1\ddot{x}_1(t)=-k(x_1(t)-x_2(t))+f_1(t) \\ m_2\ddot{x}_2(t)=-k(x_2(t)-x_1(t))+f_2(t) \end{array}\right. , \end{eqnarray*}

where k is a spring constant with the range 0.5<k<2. The state equation and output equation are given by

(29)   \begin{eqnarray*} \underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \\ \ddot{x}_1(t) \\ \ddot{x}_2(t) \\ \end{array}\right] }_{\dot{x}(t)} &=& \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ -\frac{k}{m_1} & -\frac{k}{m_1} & 0 & 0 \\ \frac{k}{m_2} & -\frac{k}{m_2}& 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}\\ &&+ \underbrace{ \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \\ \frac{k}{m_1} & 0 \\ 0 & \frac{k}{m_2} \end{array}\right] }_{B} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)}, \end{eqnarray*}

(30)   \begin{eqnarray*} \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }_{y(t)} &=&- \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}. \end{eqnarray*}

The control purpose is to regulate the zero-input response under the initial condition:

(31)   \begin{eqnarray*} \left[\begin{array}{c} {x}_1(0) \\ {x}_2(0) \\ \dot{x}_1(0) \\ \dot{x}_2(0) \\ \end{array}\right] = \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ \frac{k}{m_2} \end{array}\right] \end{eqnarray*}

by using the state feedback:

(32)   \begin{eqnarray*} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)} &=&- \underbrace{ \left[\begin{array}{cccc} g_{11} & g_{12} & g_{13} & g_{14} \\ g_{21} & g_{22} & g_{23} & g_{24} \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} {x}_1(t) \\ {x}_2(t) \\ \dot{x}_1(t) \\ \dot{x}_2(t) \\ \end{array}\right] }_{x(t)}. \end{eqnarray*}

In order to determine the state feedback gain F, for k fixed to an appropriate nominal value k^*, we will minimize the following criterion function:

(33)   \begin{eqnarray*} J=\int_0^\infty (q_1^2x_1^2(t)+q_2^2x_2^2(t)+r_1^2f_1^2(t)+r_2^2f_2^2(t))\,dt, \end{eqnarray*}

that is

(34)   \begin{eqnarray*} J&=&\int_0^\infty ( \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right]^T }_{y^T(t)} \underbrace{ \left[\begin{array}{cc} q_1^2 & 0 \\ 0 & q_2^2 \ \end{array}\right] }_{Q} \underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] }_{y(t)}\\ &&+\underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right]^T }_{u^T(t)} \underbrace{ \left[\begin{array}{cc} r_1^2 & 0 \\ 0 & r_2^2 \ \end{array}\right] }_{R} \underbrace{ \left[\begin{array}{c} f_1(t) \\ f_2(t) \end{array}\right] }_{u(t)} )\,dt. \end{eqnarray*}

For example, the closed-loop zero-input response is simulated under the following assumptions:

1) m_1=1, m_2=1, k^*=1,
2) q_1=\frac{1}{M_{x_1}}=1, q_2=\frac{1}{M_{x_2}}=1, r_1=\frac{\rho}{M_{f_1}}=1, r_2=\frac{\rho}{M_{f_2}}=1
(M_{x_1}=M_{x_2}=1, M_{f_1}=M_{f_2}=1,\ \rho=1).

Appendix 1

Check the following properties on matrix trace.

(35)   \begin{eqnarray*} {\rm tr}AB={\rm tr}BA, \end{eqnarray*}

(36)   \begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AXB=A^TB^T, \end{eqnarray*}

(37)   \begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AX^TB=BA, \end{eqnarray*}

(38)   \begin{eqnarray*} \frac{\partial}{\partial X}{\rm tr}AXBX^T=A^TXB^T+AXB \end{eqnarray*}

where \displaystyle{{\rm tr}A=\sum_{i}a_{ii}} for A=[a_{ij}]_{i,j=1,\cdots,n}. In fact,

(39)   \begin{eqnarray*} \sum_{i}[AB]_{ii}=\sum_{i}\sum_{j}a_{ij}b_{ji} =\sum_{j}\sum_{i}b_{ji}a_{ij}=\sum_{j}[BA]_{jj}, \end{eqnarray*}

(40)   \begin{eqnarray*} &&[\frac{\partial}{\partial X}{\rm tr}AXB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AXB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ij}b_{jk}\\ &&=\sum_{k}b_{jk}a_{ki}=[BA]_{ji}=[A^TB^T]_{ij}, \end{eqnarray*}

(41)   \begin{eqnarray*} &&[\frac{\partial}{\partial X}{\rm tr}AX^TB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AX^TB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ji}b_{jk}\\ &&=\sum_{k}b_{ik}a_{kj}=[BA]_{ij}, \end{eqnarray*}

(42)   \begin{eqnarray*} &&\frac{\partial}{\partial X}{\rm tr}AXBX^T =\frac{\partial}{\partial X}{\rm tr}(AXB)X^T +\frac{\partial}{\partial X}{\rm tr}AX(BX^T)\\ &&=AXB+A^TXB^T. \end{eqnarray*}

LQG Control

投稿日時: 投稿者:

1. Optimal Control for a SISO system

Given a 1st-order system with an input and an output:

(1)   \begin{eqnarray*} &&\dot{x}(t)=ax(t)+bu(t),\ y(t)=x(t)\ \\ &&(x(t)\in{\rm\bf R},\ u(t)\in{\rm\bf R},\ y(t)\in{\rm\bf R}) \end{eqnarray*}

and its stabilizing state feedback:

(2)   \begin{eqnarray*} u(t)=-fx(t), \end{eqnarray*}

the closed-loop system is represented by

(3)   \begin{eqnarray*} \dot{x}(t)=(a-bf)x(t),\ a-bf<0. \end{eqnarray*}

The state behavior x(t) and the input behavior u(t) are given by

(4)   \begin{eqnarray*} x(t)=e^{(a-bf)t}x(0) \end{eqnarray*}

and

(5)   \begin{eqnarray*} u(t)=-fe^{(a-bf)t}x(0). \end{eqnarray*}

respectively. Then consider a problem to determine f to minimize a criterion function

(6)   \begin{eqnarray*} J=\int_0^\infty(q^2x^2(t)+r^2u^2(t))\,dt. \end{eqnarray*}

The first term of the criterion function is calculated as

(7)   \begin{eqnarray*} J_x&=&\int_0^\infty q^2x^2(t)\,dt \nonumber\\ &=&\int_0^\infty q^2e^{2(a-bf)t}x^2(0)\,dt \nonumber\\ &=&q^2x^2(0)\left[\frac{1}{2(a-bf)}e^{2(a-bf)t}\right]_0^\infty \nonumber\\ &=&\frac{q^2x^2(0)}{2(a-bf)}\left[\underbrace{e^{2(a-bf)\infty}}_{0}-\underbrace{e^{2(a-bf)0}}_{1}\right] \nonumber\\ &=&-\frac{q^2}{2(a-bf)}x^2(0)>0\quad (a-bf<0), \end{eqnarray*}

and the second term of the criterion function is calculated as

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

Therefore, the criterion function can be written as

(9)   \begin{eqnarray*} J=\underbrace{\frac{-q^2}{2(a-bf)}x^2(0)}_{J_x}+\underbrace{\frac{-r^2f^2}{2(a-bf)}x^2(0)}_{J_u} =\underbrace{-\frac{q^2+r^2f^2}{2(a-bf)}}_{\Pi}x^2(0). \end{eqnarray*}

Minimizing J is equivalent to minimizing

(10)   \begin{eqnarray*} \Pi=-\frac{q^2+r^2f^2}{2(a-bf)}. \end{eqnarray*}

Differentiating by f brings

(11)   \begin{eqnarray*} \frac{d\Pi}{df} &=&-\frac{2r^2f}{2(a-bf)}-(-1)\frac{q^2+r^2f^2}{2(a-bf)^2}(-b)\nonumber\\ &=&-\frac{2r^2(a-bf)f+(q^2+r^2f^2)b}{2(a-bf)^2}\nonumber\\ &=&\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^2}. \end{eqnarray*}

Therefore

(12)   \begin{eqnarray*} \frac{d\Pi}{df}=0 & \Rightarrow & bf^2-2af-\left(\frac{q}{r}\right)^2b=0. \end{eqnarray*}

As f must satisfy a-bf<0, we have

(13)   \begin{eqnarray*} {f=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right)}\nonumber \end{eqnarray*}

This is uniquely determined because \Pi is downward convex as follows:

(14)   \begin{eqnarray*} \frac{d^2\Pi}{df^2}&=&\frac{2br^2f-2ar^2}{2(a-bf)^2} +(-2)\frac{br^2f^2-2ar^2f-q^2b}{2(a-bf)^3}(-b)\nonumber\\ &=&\frac{-r^2}{(a-bf)} +b\frac{-2r^2(a-bf)f-br^2f^2-q^2b}{(a-bf)^3}\nonumber\\ &=&\frac{-r^2(a-bf)^2-2r^2(a-bf)bf-r^2b^2f^2-q^2b^2}{(a-bf)^3}\nonumber\\ &=&\frac{-r^2(a-bf+bf)^2-q^2b^2}{2(a-bf)^3}\nonumber\\ &=&\frac{-r^2a^2-q^2b^2}{2(a-bf)^3}>0. \end{eqnarray*}

The closed-loop system by the f is given by

(15)   \begin{eqnarray*} \dot{x}(t)=(a-bf)x(t)=-\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}x(t). \end{eqnarray*}

In the case of a=\frac{1}{T},b=\frac{K}{T},

(16)   \begin{eqnarray*} \dot{x}(t)&=&-\frac{1}{T}\sqrt{1+\left(\frac{q}{r}\right)^2K^2}x(t)\nonumber\\ &=&-\frac{1}{\frac{T}{\sqrt{1+\left(\frac{q}{r}\right)^2K^2}}}x(t) \end{eqnarray*}

which means that the new time constant is shorter than the original time constant T.

Exercise 1
Letting a=-1, b=1, x(0)=1, consider the following cases:

Case#1: \displaystyle{\frac{q}{r}=2}
Case#2: \displaystyle{\frac{q}{r}=4}
Case#3: \displaystyle{\frac{q}{r}=8}

Then simulate the behaviors of x(t) and u(t) as follows.

Question:
Why do we use not only J_x but also J_u in the criterion function.
Answer:
Check that J_u is downward convex, and takes the minimum value at f=\frac{2b}{a} as follows:

(17)   \begin{eqnarray*} \frac{dJ_u}{df}&=&\frac{d}{df}\left(-\frac{r^2f^2}{2(a-bf)}x^2(0)\right)\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\left( \frac{2f}{(a-bf)}+(-1)\frac{f^2}{(a-bf)^2}(-b)\right)\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\frac{2(a-bf)f+f^2b}{(a-bf)^2}\nonumber\\ &=&\frac{-r^2x^2(0)}{2}\frac{(2a-bf)f}{(a-bf)^2}, \end{eqnarray*}

(18)   \begin{eqnarray*} \frac{d^2J_u}{df^2}&=&\frac{-r^2x^2(0)}{2}\left(\frac{2a-2bf}{(a-bf)^2} +(-2)\frac{(2a-bf)f}{2(a-bf)^3}(-b)\right)\nonumber\\ &=&-r^2x^2(0)\left(\frac{1}{a-bf}+b\frac{2(a-bf)f+bf^2}{(a-bf)^3}\right)\nonumber\\ &=&-r^2x^2(0)\frac{(a-bf)^2+2(a-bf)bf+b^2f^2}{(a-bf)^3}\nonumber\\ &=&-r^2x^2(0)\frac{(a-bf+bf)^2}{2(a-bf)^3}\nonumber\\ &=&-r^2x^2(0)\frac{a^2}{2(a-bf)^3}>0. \end{eqnarray*}

For example, letting b=1,q=1,r=1, for a=-1 and a=1, the overview of J_x, J_u, J=J_x+J_u are drown as follows.

Here the symbol “o” shows the minimum of J. Note that J_u is necessary to make J downward convex.

Appendix
In order to extend the above discussion to MIMO systems, we should be familiar with Lagrange’s method of undetermined multipliers. We will rewrite the above discussion by using this method as follows.

From (10), note that the constraint on \Pi is given by the following Lyapnov’s equation

(19)   \begin{eqnarray*} {2(a-bf)\Pi+q^2+r^2f^2=0} \end{eqnarray*}

Here \Pi>0 holds because of a-bf<0. Therefore, instead of minimizing \Pi, we will minimize

(20)   \begin{eqnarray*} J'=\Pi+\Gamma(2(a-bf)\Pi+q^2+r^2f^2). \end{eqnarray*}

using undetermined multiplier \Gamma and the stability constraint (19). As the necessary conditions, we have

(21)   \begin{eqnarray*} \frac{\partial J'}{\partial \Pi}=1+2(a-bf)\Gamma=0\Rightarrow\Gamma>0, \end{eqnarray*}

(22)   \begin{eqnarray*} \frac{\partial J'}{\partial f}=(-2b\Pi+2r^2f)\Gamma=0\Rightarrow f=r^{-2}b\Pi, \end{eqnarray*}

(23)   \begin{eqnarray*} \frac{\partial J'}{\partial \Gamma}=2(a-bf)\Pi+q^2+r^2f^2=0. \end{eqnarray*}

Substituting f=r^{-2}b\Pi into (23),

(24)   \begin{eqnarray*} 2(a-br^{-2}b\Pi)\Pi+q^2+r^2r^{-4}b^2\Pi^2=0 \end{eqnarray*}

That is, we have the second-order equation on \Pi

(25)   \begin{eqnarray*} {2a\Pi-r^{-2}b^2\Pi^2+q^2=0} \end{eqnarray*}

which is called as Ricatti equation. By solving this, \Pi>0 is obtained by

(26)   \begin{eqnarray*} \Pi=\frac{a+\sqrt{a^2+r^{-2}q^2b^2}}{r^{-2}b^2}, \end{eqnarray*}

and f is given by

(27)   \begin{eqnarray*} f=r^{-2}b\Pi=\frac{1}{b}\left(a+\sqrt{a^2+\left(\frac{q}{r}\right)^2b^2}\right). \end{eqnarray*}

Lastly consider the following matrix:

(28)   \begin{eqnarray*} {M=\left[\begin{array}{cc} a & -r^{-2}b^2 \\ -q^2 & -a \end{array}\right]}. \end{eqnarray*}

which is called as Hamilton matrix. The stable eigenvalue is given by

(29)   \begin{eqnarray*} \lambda=-\sqrt{a^2+r^{-2}b^2q^2} \end{eqnarray*}

from

(30)   \begin{eqnarray*} {\rm det}(\lambda I_2-M)=\lambda^2-a^2-r^{-2}b^2q^2=0. \end{eqnarray*}

The corresponding eigenvector is obtained as

(31)   \begin{eqnarray*} \left[\begin{array}{cc} v_1 \\ v_2 \end{array}\right] =\left[\begin{array}{cc} 1 \\ \frac{-a-\sqrt{a^2+r^{-2}b^2q^2}}{-r^{-2}b^2} \end{array}\right] \end{eqnarray*}

Note that

(32)   \begin{eqnarray*} \Pi=v_2v_1^{-1}=\frac{a+\sqrt{a^2+r^{-2}q^2b^2}}{r^{-2}b^2} \end{eqnarray*}

and

(33)   \begin{eqnarray*} f=r^{-2}b\Pi=\frac{a+\sqrt{a^2+r^{-2}b^2q^2}}{b}. \end{eqnarray*}

PID Control

投稿日時: 投稿者:

1. Speed Control under Disturbance

Consider the above situation we we are driving a car under a unknown constant wind disturbance. Then assume that we are required to keep a specified speed v^*. In order to go forward against the wind disturbance, we will step down the acceleration pedal compared with the case of no wind disturbance. But usually we will not able to measure the disturbance force w^*. How do we manipulate the driving force in an automatic way?

Let m, v(t) and f(t) be the mass, the velocity and the driving force at time t of the car respectively. Its motion is governed by the following differential equation:

(1)   \begin{eqnarray*} m\dot{v}(t)=f(t)+w^*. \end{eqnarray*}

The usual speed control is given by

(2)   \begin{eqnarray*} f(t)=k_ve_v(t)\quad(e_v(t)=v^*-v(t)). \end{eqnarray*}

In addition to this proportional control, we need an extra term f'(t) to cancel the disturbance as follows:

(3)   \begin{eqnarray*} f(t)=k_ve_v(t)+f'(t)\quad(e_v(t)=v^*-v(t)). \end{eqnarray*}

Then substituting (2) into (1), the closed-loop system is described by

(4)   \begin{eqnarray*} m\dot{v}(t)=k_ve_v(t)+f'(t)+w^*. \end{eqnarray*}

Noting \dot{e}_v(t)=-\dot{v}(t), this can be rewritten as

(5)   \begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t)-\frac{1}{m}(f'(t)+w^*). \end{eqnarray*}

In general, the solution of \dot{x}(t)=ax(t)+bu(t) is derived as

(6)   \begin{eqnarray*} x(t)=e^{at}x(0)+\int_0^t e^{a(t-\tau)}bu(\tau)d\tau. \end{eqnarray*}

Therefore the solution of (4) is given by

(7)   \begin{eqnarray*} e_v(t)=e^{-\frac{k_v}{m}t}e_v(0)+\int_0^t e^{-\frac{k_v}{m}(t-\tau)}\frac{-1}{m}(f'(t)+w^*)d\tau. \end{eqnarray*}

Based on this expression, it is trivial that if f'(t)=-w^*, e_v(t)\rightarrow0 (t\rightarrow\infty). But this strategy can’t be used because of unknown w^*. We need some mechanism to estimate the the value of w^*.

Consider the following integral action:

(8)   \begin{eqnarray*} \dot{x}_{I}(t)=e_v(t)\Leftrightarrow x_{I}(t)=\int_0^te_v(\tau)d\tau. \end{eqnarray*}

Instead of (3), we will use the following proportional control with integral action (PI Control):

(9)   \begin{eqnarray*} \underline{f(t)=k_ve_v(t)+k_I\overbrace{\int_0^te_v(\tau)d\tau}^{x_{I}(t)}}. \end{eqnarray*}

Substituting (9) into (1) and using \dot{e}_v(t)=-\dot{v}(t), the closed-loop system is described by

(10)   \begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t)-\frac{k_I}{m}x_I(t)-\frac{1}{m}w^*. \end{eqnarray*}

Furthermore, combining with (8), we have

(11)   \begin{eqnarray*} \left[\begin{array}{c} \dot{e}_v(t) \\ \dot{x}_I(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 \\ \end{array}\right] }_{A} \left[\begin{array}{c} e_v(t) \\ x_I(t) \end{array}\right] + \left[\begin{array}{c} -\frac{1}{m}w^* \\ 0 \end{array}\right]. \end{eqnarray*}

Therefore if A is a stable matrix, when t\rightarrow\infty, the following holds:

(12)   \begin{eqnarray*} &&\left[\begin{array}{c} e_v(t) \\ x_I(t) \end{array}\right] \rightarrow -\left[\begin{array}{cc} -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 \\ \end{array}\right]^{-1} \left[\begin{array}{c} -\frac{1}{m}w^* \\ 0 \end{array}\right]\\ &&=\frac{m}{k_I} \left[\begin{array}{cc} 0 & \frac{k_I}{m}\\ -1 & -\frac{k_v}{m} \\ \end{array}\right] \left[\begin{array}{c} \frac{1}{m}w^* \\ 0 \end{array}\right] =\left[\begin{array}{c} 0\\ -\frac{1}{k_I}w^* \end{array}\right]. \end{eqnarray*}

Therefore

(13)   \begin{eqnarray*} \left\{\begin{array}{l} v(t)\rightarrow v^* \\ x_I(t)\rightarrow -\frac{1}{k_I}w^* \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}

This means that -k_Ix_I can estimate w^* which is unknown.

The above control strategy is depicted as follows:

Exercise 1
Execute the following program under SCILAB. Determine appropriate the D gain k_v (kv) and the I gain k_I (kI), observing the corresponding constant distance w^* estimated.
——————————————————————————————————————–
//cart3.sce
function dx=f(t,x),dx=A*x+w, endfunction
m=1; kv=3; kI=1; A=[-kv/m -kI/m;1 0];
ws=-1; w=[-1/m*ws;0]
v0=1; vs=0.5; x0=[vs-v0;0];
t0=0; t=0:0.1:20;
x=ode(x0,t0,t,f);
v=vs-x(1,:); d=-kI*x(2,:);
scf(1);
subplot(211), plot(t,v), mtlb_grid, title(‘v(t)’)
subplot(212), plot(t,d), mtlb_grid, title(‘w*’)
——————————————————————————————————————–

2. Position Control under Disturbance

Consider the above situation we we are driving a car under a unknown constant wind disturbance. Then assume that we are required to stop in front of the obstacle. How do we manipulate the driving force in an automatic way?

Let m, r(t), and f(t) be the mass, the position and the driving force at time t of the car respectively. The velocity is v(t)=\dot{r}(t). Its motion is governed by the following differential equation:

(14)   \begin{eqnarray*} m\ddot{r}(t)=m\dot{v}(t)=f(t)+w^*. \end{eqnarray*}

Consider the following integral action:

(15)   \begin{eqnarray*} \dot{x}_{I}(t)=e_r(t)\Leftrightarrow x_{I}(t)=\int_0^te_r(\tau)d\tau. \end{eqnarray*}

The position control under disturbance should be given by the following proportional and derivative control with integral action (PID Control, PD-I Control):

(16)   \begin{eqnarray*} \underline{f(t)=k_re_r(t)+k_v\overbrace{\frac{d}{dt}e_r(t)}^{e_v(t)}+k_I\overbrace{\int_0^te_r(\tau)d\tau}^{x_I(t)}}, \end{eqnarray*}

where e_r(t)=r^*-r(t), e_v(t)=-v(t). Substituting (16) into (14) and using \dot{e}_v(t)=-\dot{v}(t), the closed-loop system is described by

(17)   \begin{eqnarray*} \dot{e}_v(t)=-\frac{k_r}{m}e_r(t)-\frac{k_v}{m}e_v(t)-\frac{k_I}{m}x_I(t)-\frac{1}{m}w^*. \end{eqnarray*}

Combining with \dot{x}_I(t)=e_r(t), we have

(18)   \begin{eqnarray*} \left[\begin{array}{c} \dot{e}_r(t) \\ \dot{e}_v(t) \\ \dot{x}_I(t) \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -\frac{k_r}{m} & -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 & 0\\ \end{array}\right] }_{A} \left[\begin{array}{c} e_r(t) \\ e_v(t) \\ x_I(t) \end{array}\right] + \left[\begin{array}{c} 0\\ -\frac{1}{m}w^* \\ 0 \end{array}\right]. \end{eqnarray*}

Therefore if A is a stable matrix, when t\rightarrow\infty, the following holds:

(19)   \begin{eqnarray*} &&\left[\begin{array}{c} e_r(t) \\ e_v(t) \\ x_I(t) \end{array}\right] \rightarrow -\left[\begin{array}{ccc} 0 & 1 & 0\\ -\frac{k_r}{m} & -\frac{k_v}{m} & -\frac{k_I}{m}\\ 1 & 0 & 0\\ \end{array}\right]^{-1} \left[\begin{array}{c} 0 \\ -\frac{1}{m}w^* \\ 0 \end{array}\right]\\ &&= \left[\begin{array}{ccc} 0 & 0 & 1\\ 1 & 0 & 0\\ -\frac{k_v}{k_I} & -\frac{m}{k_I} & -\frac{k_r}{k_I} \end{array}\right] \left[\begin{array}{c} 0 \\ \frac{1}{m}w^* \\ 0 \end{array}\right] =\left[\begin{array}{c} 0\\ 0\\ -\frac{1}{k_I}w^* \end{array}\right]. \end{eqnarray*}

Therefore

(20)   \begin{eqnarray*} \left\{\begin{array}{l} r(t)\rightarrow r^* \\ v(t)\rightarrow 0 \\ x_I(t)\rightarrow -\frac{1}{k_I}w^* \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}

This means that -k_Ix_I can estimate w^* which is unknown.

The above control strategy is depicted as follows:

This is equivalent to the following structure:

Exercise 2
Execute the following program under SCILAB. Determine appropriate PID gains k_r,\ k_v,\ k_I (kr, kv, kI), observing the corresponding constant distance w^* (ws) estimated.
——————————————————————————————————————–
//cart4.sce
function dx=f(t,x),dx=A*x+w,endfunction
m=1; kr=5; kv=5; kI=1; A=[0 1 0;-kr/m -kv/m -kI/m;1 0 0];
ws=-1; w=[0;-1/m*ws;0]
r0=-1; rs=0; v0=0; vs=0; x0=[rs-r0;vs-v0;0];
t0=0; t=0:0.1:20;
x=ode(x0,t0,t,f);
r=rs-x(1,:); d=-kI*x(3,:);
scf(1);
subplot(211), plot(t,r), mtlb_grid, title(‘r(t)’)
subplot(212), plot(t,d), mtlb_grid, title(‘w*’)
——————————————————————————————————————–

PID Control

投稿日時: 投稿者:

1. Speed Control

We are driving a car with a constant speed. Consider the situation that we are necessary to reduce the speed to stop somewhere. How do we manipulate the driving force?

Let m, v(t) and f(t) be the mass, the velocity and the driving force at time t of the car respectively. Its motion is governed by the following differential equation:

(1)   \begin{eqnarray*} m\dot{v}(t)=f(t). \end{eqnarray*}

The problem is how to manipulate the driving force f(t). Consider the following proportional control (P Control):

(2)   \begin{eqnarray*} \underline{f(t)=-k_vv(t)}. \end{eqnarray*}

Substituting (2) into (1), we have the closed-loop system:

(3)   \begin{eqnarray*} \dot{v}(t)=-\frac{k_v}{m}v(t). \end{eqnarray*}

That is, the driving force is manipulated to be proportional to the velocity with negative sign. This means that for a high speed the large regulating force is produced in the reverse direction, and for a low speed the small regulating force is applied. This control strategy is called as feedback mechanism and depicted as follows:

In general, the solution of a differential equation \dot{x}(t)=ax(t) is given by

(4)   \begin{eqnarray*} x(t)=e^{at}x(0). \end{eqnarray*}

Therefore the solution of (3) is given by

(5)   \begin{eqnarray*} v(t)=e^{-\frac{k_v}{m}t}v(0). \end{eqnarray*}

This means

(6)   \begin{eqnarray*} v(t)\rightarrow 0\quad (t\rightarrow\infty). \end{eqnarray*}

That is, we can reduce the car speed gradually and stop it.

By the way, assume that we are required to change the current speed to the new speed v^*. How do we manipulate the driving force for the speed up or speed down?

Let the velocity error from the current velocity v(t) to the required constant speed v^* be

(7)   \begin{eqnarray*} e_v(t)=v^*-v(t). \end{eqnarray*}

Then consider the following proportional control:

(8)   \begin{eqnarray*} \underline{f(t)=k_ve_v(t)}, \end{eqnarray*}

i.e.

(9)   \begin{eqnarray*} f(t)=-k_vv(t)+k_vv^*. \end{eqnarray*}

Substituting (8) into (1), we have

(10)   \begin{eqnarray*} m\dot{v}(t)=k_ve_v(t). \end{eqnarray*}

Noting \dot{e}_v(t)=-\dot{v}(t), this can be rewritten as

(11)   \begin{eqnarray*} \dot{e}_v(t)=-\frac{k_v}{m}e_v(t). \end{eqnarray*}

Therefore, we have

(12)   \begin{eqnarray*} e_v(t)=e^{-\frac{k_v}{m}t}e_v(0)\ \rightarrow 0\quad (t\rightarrow\infty). \end{eqnarray*}

This means

(13)   \begin{eqnarray*} v(t)\rightarrow v^*\quad (t\rightarrow\infty). \end{eqnarray*}

The above control strategy is depicted as follows:

Exercise 1
Execute the following program under SCILAB. By changing the P gain k_v (kv) and the target speed v^* (vs), investigate how the velocity v(t) approaches to v^*.
——————————————————————————————————————–
//cart1.sce
function dx=f(t,x),dx=A*x,endfunction
m=1; kv=1; A=-kv/m;
v0=1; vs=0.5; x0=vs-v0;
t0=0; t=0:0.1:10;
x=ode(x0,t0,t,f);
v=vs-x;
scf(1);
plot(t,v), mtlb_grid, title(‘v(t)’)
——————————————————————————————————————–

2. Position Control
Under the speed control described in the above, we can’t specify the stop position. For example, in the case of encountering an obstacle at the distance r^* from the current position as shown in the following, we will be required to stop in front of the obstacle. How do we manipulate the driving force in order to stop within the running distance of r^*?

Let m, r(t), and f(t) be the mass, the position and the driving force at time t of the car respectively. The velocity is v(t)=\dot{r}(t). Its motion is governed by the following differential equation:

(14)   \begin{eqnarray*} m\ddot{r}(t)=m\dot{v}(t)=f(t). \end{eqnarray*}

Let the position error from the current position r(t) to the required target position r^* be

(15)   \begin{eqnarray*} e_r(t)=r^*-r(t). \end{eqnarray*}

Then consider the following proportional and derivative control (PD Control):

(16)   \begin{eqnarray*} \underline{f(t)=k_re_r(t)+k_v\overbrace{\frac{d}{dt}e_r(t)}^{e_v(t)}}, \end{eqnarray*}

i.e.

(17)   \begin{eqnarray*} f(t)=-k_rr(t)-k_vv(t)+k_rr^*. \end{eqnarray*}

Here e_v(t) is the speed error given by (7), in which v^* must be 0. Note e_v(t)=-v(t).

Substituting (16) into (1), we have

(18)   \begin{eqnarray*} m\dot{v}(t)=k_re_r(t)+k_ve_v(t). \end{eqnarray*}

Noting \dot{e}_v(t)=-\dot{v}(t), this can be rewritten as

(19)   \begin{eqnarray*} \dot{e}_v(t)=-\frac{k_r}{m}e_r(t)-\frac{k_v}{m}e_v(t). \end{eqnarray*}

Furthermore, taking account of \dot{e}_r(t)=e_v(t), we have

(20)   \begin{eqnarray*} \underbrace{ \left[\begin{array}{c} \dot{e}_r(t) \\ \dot{e}_v(t) \end{array}\right] }_{\dot{e}(t)} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{k_r}{m} & -\frac{k_v}{m} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} e_r(t) \\ e_v(t) \end{array}\right] }_{e(t)}. \end{eqnarray*}

As shown here, the solution is given by

(21)   \begin{eqnarray*} e(t)=\exp(At)e(0). \end{eqnarray*}

As A is a stable matrix, the following holds.

(22)   \begin{eqnarray*} \left\{\begin{array}{l} e_r(t)\rightarrow 0 \\ e_v(t)\rightarrow 0 \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}

This means

(23)   \begin{eqnarray*} \left\{\begin{array}{l} r(t)\rightarrow r^* \\ v(t)\rightarrow 0 \end{array}\right.\quad (t\rightarrow\infty). \end{eqnarray*}

The above control strategy is depicted as follows:

Based on (17), this is equivalent to the following structure:

Exercise 2
Execute the following program under SCILAB. Determine the D gain k_v (kv) for collision avoidance, such that no undershoot occurs in the position r(t).
——————————————————————————————————————–
//cart2.sce
function dx=f(t,x),dx=A*x,endfunction
m=1; kr=1; kv=2; A=[0 1;-kr/m -kv/m];
r0=-1; rs=0; v0=0; vs=0; x0=[rs-r0;vs-v0];
t0=0; t=0:0.1:10;
x=ode(x0,t0,t,f);
v=vs-x(2,:); r=rs-x(1,:);
scf(1);
subplot(211), plot(t,v), mtlb_grid, title(‘v(t)’)
subplot(212), plot(t,r), mtlb_grid, title(‘r(t)’)
——————————————————————————————————————–

Question:
Why the control law (2) is called as not D control but P control?
Answer:
In the situation of the speed control, we are assumed to measure the velocity, that is, we don’t have to differentiate the position. So we need to feed back the value proportional to the measured velocity. On the other hand, in the situation of the position control, we are assumed to measure the position, that is, we have to differentiate the position to get the velocity.

基礎事項

投稿日時: 投稿者:

[0] 表計算ソフトEXCELの基礎事項を、初歩的な統計計算を通して説明します。

[1] 次の集計表を作成しましょう。

各列の集計は、まず、セルA4に「=sum(A1:A3)」を入れ、これをセルB4,C4に、Copy & Paste （以下、C&P）します。このとき、セルB4,C4はそれぞれ、「=sum(B1:B3)」、「=sum(C1:C3)」
となっており、和をとる列が自動的に変更されていることに注意してください。同様に、各行の集計は、まず、セルD1に「=sum(A1:C1)」を入れ、これをセルD2,D3,D4にC&Pします。表計算の利点はデータ更新時の再計算にあります。適当に、データを変更して、再計算の便利さを確かめてみましょう。

[2] 次の成績データの偏差値を計算してください。

{(i,x_i)|i=1,…,10}={(1,89),(2,70),(3,63),(4,100),(5,81),(6,30),(7,56),(8,77),(9,90),(10,63)}

まず、留意すべきは、列Aの入力方法です。セルA2に初期値「1」を入力し、セルA3に「=A2+1」を入力し、これを領域A4:A11にC&Pします。

つぎに、和、個数、平均値、分散、標準偏差をそれぞれ次のように入力して求めておきます。

セルB12　⇒　=sum(B2:B11)
セルB13　⇒　=count(B2:B11)
セルB14　⇒　=average(B2:B11)
セルB15　⇒　=varp(B2:B11)
セルB16　⇒　=stdevp(B2:B11)

ここで、平均値と分散は、次式で与えられるのでした。

\displaystyle{m_x=\frac{1}{n}\sum_{i=1}^{n}x_i}
\displaystyle{\sigma_x^2=\frac{1}{n}\sum_{i=1}^{n}(x_i-m)^2}

偏差値とは，平均値50，標準偏差10となるように線形変換した値のことで、次式で計算されます。

\displaystyle{y_i=10\frac{x_i-m_x}{\sigma_x}+50}

列C,列D,列Eの作成法は、次の通りです。セルC2,D2,D2に、次のように入力します。

セルC2　⇒　=B2-$B$14
セルD2　⇒　=C2^2
セルE2　⇒　=10*C2/$B$16+50

ここで、セルB14とB16の参照は、それぞれ絶対アドレス$B$14と$B$16を用いることに留意してください。これらを下方にC&Pします。ちなみに、分布の形を変えないで60点以上が合格となるように、新しい平均値，標準偏差の値を調整して、ゲタをはかせることができます。

参考までに、平均値と分散の漸化式（オンライン計算式）を与えておきます。

\begin{array}{lll} m_n&=&\frac{1}{n}\sum_{i=1}^{n}x_i \nonumber\\ &=&\frac{n-1}{n}\left(\frac{1}{n-1}\sum_{i=1}^{n-1}x_i\right)+\frac{1}{n}x_n \nonumber\\ &=&\frac{n-1}{n}m_{n-1}+\frac{1}{n}x_n\nonumber \end{array}

\begin{array}{lll} \sigma_n^2&=&\frac{1}{n}\sum_{i=1}^{n}(x_i-m_n)^2\nonumber\\ &=&\frac{1}{n}\sum_{i=1}^{n}x_i^2-m_n^2\nonumber\\ &=&\frac{n-1}{n}\left(\frac{1}{n-1}\sum_{i=1}^{n-1}x_i^2\right)+\frac{1}{n}x_n^2 -\left(\frac{n-1}{n}m_{n-1}+\frac{1}{n}x_n\right)^2\nonumber\\ &=&\frac{n-1}{n}(\sigma_{n-1}^2+m_{n-1}^2)+\frac{1}{n}x_n^2- \left(\frac{(n-1)^2}{n^2}m_{n-1}^2+2\frac{n-1}{n^2}m_{n-1}x_n+\frac{1}{n^2}x_n^2\right)\nonumber\\ &=&\frac{n-1}{n}\sigma_{n-1}^2+\frac{n-1}{n^2}m_{n-1}^2 -2\frac{n-1}{n^2}m_{n-1}x_n+\frac{n-1}{n^2}x_n^2\nonumber\\ &=&\frac{n-1}{n}\sigma_{n-1}^2+\frac{n-1}{n^2}(x_n-m_{n-1})^2\nonumber \end{array}

それでは、与えられたデータの棒グラフを描きましょう。「挿入」、「グラフ」と進んで、「縦棒」、「2D-縦棒」を選びます。

[4] 次の計測データの回帰直線を計算してください。

{(x_i,y_i)|i=1,…,6}={(1,5.7),(3,10.4),(4,11.1),(6,19.5),(7,21.8),(10,26.2)}

ここで、行列（領域C10:D11）の逆行列を領域G10:H11に計算しています。そのためには、

セルG10　⇒　=MINVERSE(C10:D11)

を入力し、領域C10:D11を選択し、窓fxにカーソルを合わせ、”[Ctrl] + [Shift] + [Enter]” を入力します。

次に、この逆行列とベクトル（領域C12:C13）の掛け算を領域C15:C16に計算しています。そのためには、

セルC15　⇒　=MMULT(G10:H11,C12:C13)

を入力し、領域C15:C16を選択し、窓fxにカーソルを合わせ、”[Ctrl] + [Shift] + [Enter]” を入力します。

それでは、計算式の詳細を説明します。回帰直線ax+bは、次の最小化問題を解いて得られます。

\displaystyle{\min_{a,b} \frac{1}{n}\sum_{i=1}^{n}(y_i-ax_i-b)^2}

この解は、次式で与えられます。

\displaystyle{a=\frac{1}{d}(c_1q_2-c_2q_3)},\quad \displaystyle{b=\frac{1}{d}(-c_1q_3+c_2q_1)}

ただし、d=q_1q_2-q_3^2、また

\displaystyle{q_1=\frac{1}{n}\sum_{i=1}^{n}x_i^2},\quad \displaystyle{q_2=1},\quad \displaystyle{q_3=\frac{1}{n}\sum_{i=1}^{n}x_i}
\displaystyle{c_1=\frac{1}{n}\sum_{i=1}^{n}x_iy_i},\quad \displaystyle{c_2=\frac{1}{n}\sum_{i=1}^{n}y_i}

実際、目的関数を

\displaystyle{J(a,b)=\frac{1}{n}\sum_{i=1}^{n}(y_i-ax_i-b)^2}

とおくと、これが極値をとる条件は

\displaystyle{\left\{\begin{array}{l} \frac{\partial J}{\partial a}=\frac{2}{n}\sum_{i=1}^{n}(y_i-ax_i-b)(-x_i)=0\\ \frac{\partial J}{\partial b}=\frac{2}{n}\sum_{i=1}^{n}(y_i-ax_i-b)(-1)=0 \end{array}\right.}

となります。これは、上記の記号を用いて

\displaystyle{\left\{\begin{array}{l} -c_1+q_1a+q_3b=0\\ -c_2+q_3a+q_2b=0 \end{array}\right.}

のように書けます。これを行列表示すると

\left[\begin{array}{cc} q_1 & q_3 \\ q_3 & q_2 \end{array}\right] \left[\begin{array}{cc} a \\ b \end{array}\right] = \left[\begin{array}{cc} c_1 \\ c_2 \end{array}\right]

となって、aとbは、次式で与えられるからです。

\left[\begin{array}{cc} a \\ b \end{array}\right] = \frac{1}{d} \left[\begin{array}{cc} q_2 & -q_3 \\ -q_3 & q_1 \end{array}\right] \left[\begin{array}{cc} c_1 \\ c_2 \end{array}\right]

ちなみに、SCILABでは、次式で計算されます。

–> A=[[1 3 4 6 7 10]’ ones(6,1)];
–> b=[5.7 10.4 11.1 19.5 21.6 26.2]’;
–> A\b

それでは、与えられたデータの折れ線グラフを描きましょう。「挿入」、「グラフ」と進んで、そのグラフをと使えばよいでしょうか？実は「散布図」を選びます。グラフの背景はデフォールトが灰色の場合、白色に変更しましょう。

台車駆動型倒立振子の特性解析

投稿日時: 投稿者:

[1] 台車駆動型倒立振子CIP

上図のように、倒立振子を軸支した台車を、傾斜角 \alpha を持つレール上に載せた状況を考えます。このとき、まず運動方程式を導出し，これを平衡状態まわりで線形化し、状態方程式と出力方程式からなる状態空間表現を得ましょう。

以下では、倒立振子となる棒は長さ 2\ell、質量 m の一様な剛体で、台車の質量は M とします。棒の鉛直線からの傾きを \theta、台車のレールに沿う変位を r、台車を駆動する力 F とします。また，簡単のため、軸は、棒の端（棒の重心から -\ell の位置）に取り付けられ、台車の重心と一致し，駆動力の作用点でもあるとします。なお、この制御対象に対して、計測可能な物理変数は、r と \theta であるとします。

まず、軸の位置のx座標を x_0 、y座標を y_0 とすると

x_0=r\cos\alpha
y_0=r\sin\alpha

となり、台車の運動エネルギー T_0 と位置エネルギー V_0 は

T_0=\frac{1}{2}M(\dot{x}^2_0+\dot{y}^2_0)=\frac{1}{2}M\dot{r}^2_0
V_0=Mgy_0

のように表されます。また 、棒の重心位置のx座標を x_1 、y座標を y_1 とすると

x_1=x_0+\ell\sin\theta
y_1=y_0+\ell\cos\theta

となり、棒の運動エネルギー T_1 と位置エネルギー V_1 は

T_1=\frac{1}{2}m(\dot{x}^2_1+\dot{y}^2_1)+\frac{1}{2}J_1\dot{\theta}^2_0
V_1=mgy_1

のように表されます。ここで、J_1 は重心周りの慣性モーメントを表し

J_1=\frac{1}{3}m\ell^2

です。以上の準備の下で、ラグランジュの運動方程式は，ラグランジアンを

L=T_0+T_1-V_0-V_1

として

\frac{d}{dt}(\frac{\partial L}{\partial \dot{r}})-\frac{\partial L}{\partial r}=F
\frac{d}{dt}(\frac{\partial L}{\partial \dot{\theta}})-\frac{\partial L}{\partial \theta}=0

のように与えられます。

それでは、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);
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;

これを実行すると、LE1として、次の結果を得ます。

また、LE2として、次の結果を得ます。

これらを行列表示して、制御対象の運動方程式として

\displaystyle{ \left[\begin{array}{cc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right] \left[\begin{array}{c} \ddot{r} \\ \ddot{\theta} \end{array}\right] + \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] + \left[\begin{array}{c} (M+m)g\sin\alpha\\ -m\ell g\sin\theta \end{array}\right] = \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F }

を得ます。これを行列表示すると

M(\xi_1)\dot{\xi}_2+C(\xi_1){\xi}_2+G(\xi_1)=e_1\zeta

ただし

\xi_1= \left[\begin{array}{c} {r} \\ {\theta} \end{array}\right],\ \xi_2= \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right],\ \zeta=F

\displaystyle{ M(\xi_1)= \left[\begin{array}{cc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right],\ C(\xi_1)= \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right],\ e_1= \left[\begin{array}{c} 1 \\ 0 \end{array}\right] }

となります。これから、制御対象の非線形状態方程式

\left[\begin{array}{c} \dot{\xi}_1 \\ \dot{\xi}_2 \end{array}\right] = \left[\begin{array}{c} {\xi}_2 \\ M^{-1}(\xi_1)(e_1\zeta-C(\xi_1){\xi}_2-G(\xi_1)) \end{array}\right]

すなわち、状態変数ベクトルと入力変数を

\xi= \left[\begin{array}{c} \xi_1 \\ \xi_2 \end{array}\right] = \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right],\ \zeta=F

と定義するとき、制御対象の非線形状態方程式

\dot{\xi}=f(\xi,\zeta)

ただし

\displaystyle{ f(\xi,\zeta)= \left[\begin{array}{c} f_1(\xi,\zeta) \\ f_2(\xi,\zeta) \\ f_3(\xi,\zeta) \\ f_4(\xi,\zeta) \end{array}\right]= \left[\begin{array}{c} f_1(r,\theta,\dot{r},\dot{\theta},F) \\ f_2(r,\theta,\dot{r},\dot{\theta},F) \\ f_3(r,\theta,\dot{r},\dot{\theta},F) \\ f_4(r,\theta,\dot{r},\dot{\theta},F) \end{array}\right]= \left[\begin{array}{c} {\xi}_2 \\ M^{-1}(\xi_1)(e_1\zeta-C(\xi_1){\xi}_2-G(\xi_1)) \end{array}\right] }

を得ます。

さて、制御対象の平衡状態 \xi^* と、これを実現する平衡入力 \zeta^* を考えます。これらは、運動方程式において、加速度＝０（\dot{\xi}_2=0）とおいた、

C(\xi_1)\dot{\xi}_1+G(\xi_1)=e_1\zeta

すなわち

\displaystyle{ \left[\begin{array}{cc} 0 & -m\ell\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} \dot{r} \\ \dot{\theta} \end{array}\right] + \left[\begin{array}{c} (M+m)g\sin\alpha\\ -m\ell g\sin\theta \end{array}\right] = \left[\begin{array}{c} 1 \\ 0 \end{array}\right] F }

を解いて得られます。これより、平衡状態 \xi^* と、これを実現する平衡入力 \zeta^* として

\xi^*= \left[\begin{array}{c} \xi^*_1 \\ \xi^*_2 \end{array}\right]= \left[\begin{array}{c} r^* \\ \theta^* \\ \dot{r}^* \\ \dot{\theta}^* \end{array}\right]= \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \end{array}\right]\ or\ \left[\begin{array}{c} 0 \\ \pi \\ 0 \\ 0 \end{array}\right]
\zeta^*=F^*=(M+m)g\sin\alpha

を得ます。このとき、明らかに、次式が満足されています。

\dot{\xi}^*=f(\xi^*,\zeta^*)= \left[\begin{array}{c} {\xi}^*_2 \\ M^{-1}(\xi^*_1)(e_1\zeta^*-C(\xi^*_1){\xi^*}_2-G(\xi^*_1)) \end{array}\right]= \left[\begin{array}{c} 0 \\ 0 \end{array}\right]

この平衡状態（と平衡入力）まわりで、非線形状態方程式を、線形化（１次近似）します。

\dot{\xi}=f(\xi,\zeta)\simeq f(\xi^*,\zeta^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial\xi}(\xi-\xi^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial\zeta}(\zeta-\zeta^*)

ただし

\displaystyle{ \frac{\partial f(\xi^*,\zeta^*)}{\partial\xi} =\left.\left[\begin{array}{cccc} \frac{\partial f_1}{\partial r} & \frac{\partial f_1}{\partial\theta} &\frac{\partial f_1}{\partial\dot{r}} & \frac{\partial f_1}{\partial\dot{\theta}} \\ \frac{\partial f_2}{\partial r} & \frac{\partial f_2}{\partial\theta} &\frac{\partial f_2}{\partial\dot{r}} & \frac{\partial f_2}{\partial\dot{\theta}} \\ \frac{\partial f_3}{\partial r} & \frac{\partial f_3}{\partial\theta} &\frac{\partial f_3}{\partial\dot{r}} & \frac{\partial f_3}{\partial\dot{\theta}} \\ \frac{\partial f_4}{\partial r} & \frac{\partial f_4}{\partial\theta} &\frac{\partial f_4}{\partial\dot{r}} & \frac{\partial f_4}{\partial\dot{\theta}} \end{array}\right] \right|_{r=0,\theta=\theta^*,\dot{r}=0,\dot{\theta}=0,F=F^*}
\displaystyle{ \frac{\partial f(\xi^*,\zeta^*)}{\partial\zeta} =\left.\left[\begin{array}{cccc} \frac{\partial f_1}{\partial F} \\ \frac{\partial f_2}{\partial F} \\ \frac{\partial f_3}{\partial F} \\ \frac{\partial f_4}{\partial F} \end{array}\right] \right|_{r=0,\theta=\theta^*,\dot{r}=0,\dot{\theta}=0,F=F^*}

これから、f(\xi^*,\zeta^*)=0 に注意して、制御対象の線形状態方程式

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

すなわち

\underbrace{ \frac{d}{dt} \left[\begin{array}{c} r-r^* \\ \theta-\theta^* \\ \dot{r}-\dot{r}^* \\ \dot{\theta}-\dot{\theta}^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & a_{32} & 0 & 0\\ 0 & a_{42} & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r-r^* \\ \theta-\theta^* \\ \dot{r}-\dot{r}^* \\ \dot{\theta}-\dot{\theta}^* \end{array}\right] }_{x} +\underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ b_{32} \\ b_{42} \end{array}\right] }_{B} \underbrace{ (\dot{\zeta}-\dot{\zeta}^*) }_{u}

を得ます。

以上の計算をMAXIMAを用いて実行するには、次のプログラムを作成します。

/*cip.wxm*/
:
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);
Fs:(M+m)*g*sin(alpha);
A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],[a41,a42,a43,a44]);
A1:A,th(t)=0,F=Fs,trigreduce,ratsimp;
A2:A,th(t)=%pi,F=Fs,trigreduce,ratsimp;
B:matrix([0],[0],[b3],[b4]);
B1:B,th(t)=0,F=Fs,trigreduce,ratsimp;
B2:B,th(t)=%pi,F=Fs,trigreduce,ratsimp;

ここで、\theta^*=0 に対応する平衡状態周りの線形状態方程式のA行列がA1、B行列がB1で、それぞれ次のような結果となります。


また、\theta^*=\pi に対応するA行列がA2、B行列がB2で、それぞれ次のような結果となります。


計算工学演習第一レポート（１）

振り子の特性解析

投稿日時: 投稿者:

[0]　MAXIMAを用いて、倒立振子の運動方程式を導出し、非線形状態方程式を求め、これを平衡状態まわりで線形化（１次近似）し、線形状態方程式を求めます。

そのために必要となるのが、ベクトル値多変数関数

\displaystyle{y=f(x)\ (x\in{\rm\bf R}^n,y\in{\rm\bf R}^m)}

の x=a(\in{\rm\bf R}^n) まわりの１次近似式

\displaystyle{f(x)\simeq f(a)+\frac{\partial f(a)}{\partial x}(x-a)

です。これは、たとえば、 n=m=2 のとき、次のように計算されます。

\begin{array}{lll} \underbrace{ \left[\begin{array}{cc} f_1(x_1,x_2)\\ f_2(x_1,x_2) \end{array}\right] }_{f(x)}\nonumber\\ \simeq \left[\begin{array}{cc} f_1(a_1,a_2)+\frac{\partial\,f_1(a_1,a_2)}{\partial\,x_1}(x_1-a_1)+\frac{\partial\,f_1(a_1,a_2)}{\partial\,x_2}(x_2-a_2)\\ f_2(a_1,a_2)+\frac{\partial\,f_2(a_1,a_2)}{\partial\,x_1}(x_1-a_1)+\frac{\partial\,f_2(a_1,a_2)}{\partial\,x_2}(x_2-a_2) \end{array}\right]\nonumber\\ = \underbrace{ \left[\begin{array}{cc} f_1(a_1,a_2)\\ f_2(a_1,a_2) \end{array}\right] }_{f(a)} + \underbrace{ \left[\begin{array}{cc} \frac{\partial f_1(a_1,a_2)}{\partial x_1}&\frac{\partial f_1(a_1,a_2)}{\partial x_2}\\ \frac{\partial f_2(a_1,a_2)}{\partial x_1}&\frac{\partial f_2(a_1,a_2)}{\partial x_2} \end{array}\right] }_{\frac{\partial\,f(a)}{\partial\,x}} \underbrace{ \left[\begin{array}{cc} x_1-a_1\\ x_2-a_2\nonumber \end{array}\right] }_{x-a}\nonumber \end{array}

[1] 振り子PEN

上図の振り子PENを見てください。長さ、L=2\ell の振り子の重心周りの慣性モーメントは

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

のように計算できます。これから，振り子の軸まわりの慣性モーメントは、平行軸の定理を使って

\displaystyle{J=\frac{1}{3}m\ell^2+m\ell^2=\frac{4}{3}m\ell^2}

のように与えられます。また、軸まわりのトルクは

\displaystyle{\tau=mg\ell\sin\theta}

のように表わされます。したがって、振り子の運動方程式は、次式となります。

\displaystyle{J\ddot{\theta}=\tau\ \Rightarrow\ \ddot{\theta}=\frac{3g}{4\ell}\sin\theta}

それでは、この運動方程式を、ラグランジュの方法で導出してみます。その手順は次のようになります。

まず、振り子の重心のx、y座標と、軸周りの慣性モーメントは

x=\ell\sin\theta
y=\ell\cos\theta
J_0=\frac{1}{3}m\ell^2

ですから、運動エネルギーと位置エネルギーは

T=\frac{1}{2}m(\dot{x}^2+\dot{y}^2)+\frac{1}{2}J_0\dot{\theta}^2
V=mg\ell\cos\theta

となり、ラクランジアンを

L=T-V

として、ラグランジュの運動方程式は

\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right)-\frac{\partial L}{\partial\theta}=0

となります。これから振り子の運動方程式が得られます。

それでは、MAXIMAを用いて、振り子の運動方程式を導出してみましょう。

/*pen.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;
V:m*g*ell*cos(th(t));
L:T-V;
LE:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce;
sol:solve(LE,ddth);

次に、２階微分を含む運動方程式を１階微分だけの連立微分方程式に変換し、これをベクトル表示します。

\displaystyle{\ddot{\theta}=\frac{3g}{4\ell}\sin\theta \quad\Leftrightarrow \left\{\begin{array}{l} \dot{\theta}=\omega\\ \dot{\omega}=\frac{3g}{4\ell}\sin\theta \end{array}\right. \quad\Leftrightarrow \left[\begin{array}{c} \dot{\theta} \\ \dot{\omega} \end{array}\right] = \left[\begin{array}{c} \omega \\ \frac{3g}{4\ell}\sin\theta \end{array}\right]}

すなわち、状態変数ベクトル

\xi= \left[\begin{array}{c} {\theta} \\ {\omega} \end{array}\right]

を定義するとき、振り子の非線形状態方程式

\dot\xi=f(\xi)

ただし

f(\xi)= \left[\begin{array}{c} f_1(\xi) \\ f_2(\xi) \end{array}\right] = \left[\begin{array}{c} f_1(\theta,\omega) \\ f_2(\theta,\omega) \end{array}\right] = \left[\begin{array}{c} \omega \\ \frac{3g}{4\ell}\sin\theta \end{array}\right]

が得られました。ところで、振り子は２つの平衡状態（物理的な釣り合いの状態）を持ちます。

これらは次のように特徴付けられます。

\dot\xi^*=f(\xi^*)=0

すなわち

\underbrace{ \left[\begin{array}{c} \dot{\theta}^* \\ \dot{\omega}^* \end{array}\right] }_{\dot{\xi}^*} = \underbrace{ \left[\begin{array}{c} \omega^* \\ \frac{3g}{4\ell}\sin\theta^* \end{array}\right] }_{f(\xi^*)} = \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{0} \ \Rightarrow\ \xi^* = \left[\begin{array}{c} \theta^* \\ \omega^* \end{array}\right] = \left[\begin{array}{c} 0 \\ 0 \end{array}\right] \,or\, \left[\begin{array}{c} \pi \\ 0 \end{array}\right]

そこで、振り子の非線形状態方程式を、平衡状態まわりで線形化（１次近似）します。

\dot{\xi}=f(\xi)\simeq \underbrace{f(\xi^*)}_{\dot{\xi}^*=0} +\frac{\partial f(\xi^*)}{\partial\xi} (\xi-\xi^*)

ただし

\displaystyle{ \frac{\partial f(\xi^*)}{\partial\xi}=\left.\left[\begin{array}{cc} \frac{\partial f_1}{\partial\theta} & \frac{\partial f_1}{\partial\omega} \\ \frac{\partial f_2}{\partial\theta} & \frac{\partial f_2}{\partial\omega} \end{array}\right] \right|_{\theta=\theta^*,\omega=0}}

これから、振り子の線形状態方程式

\underbrace{\dot{\xi}-\dot{\xi}^*}_{\dot{x}}= \underbrace{\left.\frac{\partial f(\xi^*)}{\partial\xi}\right.}_A \underbrace{(\xi-\xi^*)}_{x}

すなわち

\underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta-\theta^* \\ \omega-\omega^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}\cos\theta^* & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta-\theta^* \\ \omega-\omega^* \end{array}\right] }_{x}

を得ます。以上の計算をMAXIMAを用いて実行するには、次のコマンドを与えます。

f:matrix([dth],[rhs(sol[1])]);
A:matrix([diff(f[1,1],th(t)),diff(f[1,1],dth)],[diff(f[2,1],th(t)),diff(f[2,1],dth)]);
A1:A,th(t)=0;
A2:A,th(t)=%pi;