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

[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;