The Fool In The Valleyの雑記帳

-- 好奇心いっぱいのおじいちゃんが綴るよしなし事 --

トップ > コンピュータ-Mindstorms > Gyrod　【理論編　その２】

Gyrod　【理論編　その２】

コンピュータ-Mindstorms コンピュータ-Python コンピュータ

【理論編　その１】でGyrodの運動を定式化し、その系が可制御であることを述べました。次にその実現方法について話を進めます。

可制御であることが保証されている制御系を具体的に構築するために広く用いられているのが、図４のように制御入力を状態変数の重み付き加算値として与える状態フィードバックと呼ばれる方法です。

図4　状態フィードバック
Gyrodの場合、制御入力は支点を水平方向に動かす力だけで、\displaystyle{\boldsymbol{u}=u}　なので、

\begin{eqnarray}\displaystyle{
u=\boldsymbol{K} \boldsymbol{x}=\left(\begin{array}{llll}
k_1 & k_2 & k_3 & k_4
\end{array}\right)\left(\begin{array}{l}
x_1 \\
x_2 \\
x_3 \\
x_4
\end{array}\right)=k_1 x_1+k_2 x_2+k_3 x_3+k_4 x_4 　
}\tag{22}\end{eqnarray}

となる \displaystyle{u} が制御入力になります。 この状態フィードバックで構成される制御系の挙動は、状態方程式(18)に(22)を代入して得られる

\begin{eqnarray}\displaystyle{
\dot{\boldsymbol{x}}=(A+B K) \boldsymbol{x}
}\tag{23}\end{eqnarray}

で決定されます。ここで \displaystyle{A+B K} の固有値　\displaystyle{\lambda _ 1}、\displaystyle{\lambda _ 2}、\displaystyle{\lambda _ 3}、\displaystyle{\lambda _ 4}は状態フィードバックを適用した制御系の特性根と呼ばれ、(23)の解はこの特性根を使って一般に

\begin{eqnarray}\displaystyle{
x_i(t)=c_{i 1} e^{\lambda_1 t}+c_{i 2} e^{\lambda_2 t}+c_{i 3} e^{\lambda_3 t}+c_{i 4} e^{\lambda_4 t}
}\tag{24}\end{eqnarray}

の形で与えられます。そこで \displaystyle{\lambda _ 1}、\displaystyle{\lambda _ 2}、\displaystyle{\lambda _ 3}、\displaystyle{\lambda _ 4}の実部が負であれば(12)で定義した全ての状態変数

\begin{eqnarray*}\displaystyle{ \left(\begin{array}{l} x_1 \\ x_2 \\ x_3 \\ x_4 \end{array}\right)=\left(\begin{array}{l} \varphi \\ \dot{\varphi} \\ y \\ \dot{y} \end{array}\right) }\end{eqnarray*}

は時間とともに０に収束するのでこの制御系は安定することになります。

さて、制御系を設計するためには、\displaystyle{ K=\left(\begin{array}{llll} k _ 1 \ k _ 2 \ k _ 3 \ k _ 4 \end{array}\right) } を求めなければなりませんが、それは状態フィードバック系の特性根を指定して、その値になるように、\displaystyle{K} を決定する極配置法で求めることができます。その方法は以下のようになります。

\displaystyle{\boldsymbol{A}+\boldsymbol{B} \boldsymbol{K}}の固有値は、その特性方程式 \displaystyle{ |\mu \boldsymbol{I}-(\boldsymbol{A}+\boldsymbol{B} \boldsymbol{K})|=0} の根なので、

\begin{eqnarray}\displaystyle{
\left|\left(\begin{array}{cccc}
\mu & 0 & 0 & 0 \\
0 & \mu & 0 & 0 \\
0 & 0 & \mu & 0 \\
0 & 0 & 0 & \mu
\end{array}\right)-\left(\begin{array}{llll}
0 & 1 & 0 & 0 \\
\mathrm{a} & 0 & 0 & 0 \\
0 & 0 & 0 & 1 \\
b & 0 & 0 & 0
\end{array}\right)-\left(\begin{array}{l}
0 \\
\mathrm{c} \\
0 \\
\mathrm{~d}
\end{array}\right)\left(\begin{array}{llll}
\mathrm{k}_1 & \mathrm{k}_2 & \mathrm{k}_3 & \mathrm{k}_4
\end{array}\right)\right|=0
}\tag{25}\end{eqnarray}

で与えられます。この式を展開すると

\begin{eqnarray}\displaystyle{
\mu^4-\left(\mathrm{ck}_2+\mathrm{dk}_4\right) \mu^3-\left(\mathrm{a}+\mathrm{ck}_1+\mathrm{dk}_3\right) \mu^2+(\mathrm{ad}-\mathrm{bc}) \mathrm{k}_4 \mu+(\mathrm{ad}-\mathrm{bc}) \mathrm{k}_3=0
}\tag{26}\end{eqnarray}

であり、この根が \displaystyle{\boldsymbol{A}+\boldsymbol{B} \boldsymbol{K}} の固有値　\displaystyle{\lambda _ 1}、\displaystyle{\lambda _ 2}、\displaystyle{\lambda _ 3}、\displaystyle{\lambda _ 4} なので、根と係数の関係から

\begin{eqnarray}\displaystyle{
\begin{aligned}
& \left(c k_2+d k_4\right)=\lambda_1+\lambda_2+\lambda_3+\lambda_4 \\
& -\left(a+c k_1+d k_3\right)=\lambda_1 \lambda_2+\lambda_2 \lambda_3+\lambda_3 \lambda_4+\lambda_4 \lambda_1+\lambda_1 \lambda_3+\lambda_2 \lambda_4 \\
& -(a d-b c) k_4=\lambda_1 \lambda_2 \lambda_3+\lambda_2 \lambda_3 \lambda_4+\lambda_3 \lambda_4 \lambda_1+\lambda_4 \lambda_1 \lambda_2 \\
& (a d-b c) k_3=\lambda_1 \lambda_2 \lambda_3 \lambda_4
\end{aligned}
}\tag{27}\end{eqnarray}

となります。これを行列形式で書き直すと

\begin{eqnarray}\displaystyle{
\left(\begin{array}{cccc}
0 & c & 0 & d \\
-c & 0 & -d & 0 \\
0 & 0 & 0 & -a d+b c \\
0 & 0 & a d-b c & 0
\end{array}\right)\left(\begin{array}{l}
k_1 \\
k_2 \\
k_3 \\
k_4
\end{array}\right)=\left(\begin{array}{c}
\lambda_1+\lambda_2+\lambda_3+\lambda_4 \\
a+\lambda_1 \lambda_2+\lambda_2 \lambda_3+\lambda_3 \lambda_4+\lambda_4 \lambda_1+\lambda_1 \lambda_3+\lambda_2 \lambda_4 \\
\lambda_1 \lambda_2 \lambda_3+\lambda_2 \lambda_3 \lambda_4+\lambda_3 \lambda_4 \lambda_1+\lambda_4 \lambda_1 \lambda_2 \\
\lambda_1 \lambda_2 \lambda_3 \lambda_4
\end{array}\right)
}\tag{28}\end{eqnarray}

のように書き表されます。　そこで、実部が負である適当な \displaystyle{\lambda _ 1}、\displaystyle{\lambda _ 2}、\displaystyle{\lambda _ 3}、\displaystyle{\lambda _ 4}　を与え、行列演算で(28)を \displaystyle{k _ i(\mathrm{i}=1,2,3,4)} について解けば、系を安定させるための制御入力 \displaystyle{u} を

\begin{eqnarray}\displaystyle{
u=k_1 x_1+k_2 x_2+k_3 x_3+k_4 x_4
}\tag{29}\end{eqnarray}

として求めることができます。 Gyrodの具体的な値として

\displaystyle{ \begin{aligned} & \mathrm{M}: 0.90 \mathrm{~kg} \\ & \mathrm{L}: 0.11 \mathrm{~m} \\ & \mathrm{m}: 0.06 \mathrm{~kg} \\ & \mathrm{g}: 9.8 \mathrm{~m} / \mathrm{s}^2 \\ & \mathrm{I}=\frac{\mathrm{M}}{12}\left((2 L)^2+\left(\frac{2 L}{3}\right)^2\right): 0.00403 \end{aligned} }

を使うと

\begin{eqnarray}\displaystyle{
\begin{array}{ll}
a=\frac{(M+m) M L g}{(M+m) I+M m L^2}=167 & b=-\frac{M^2 L^2 g}{(M+m) I+M m L^2}=-16.1 \\
c=-\frac{M L}{(M+m) I+M m L^2}=-18.3 & d=\frac{I+M L^2}{(M+m) I+M m L^2}=2.85
\end{array}
}\tag{30}\end{eqnarray}

です。　系が安定であるためには4つの特性根の実部が負である必要があるので、例えば　-4±i、-6±2i　とすると、（28）は

\begin{eqnarray}\displaystyle{
\left(\begin{array}{cccc}
0 & -16.9 & 0 & 2.62 \\
16.9 & 0 & -2.62 & 0 \\
0 & 0 & 0 & -165 \\
0 & 0 & 1659 & 0
\end{array}\right)\left(\begin{array}{l}
k_1 \\
k_2 \\
k_3 \\
k_4
\end{array}\right)=\left(\begin{array}{c}
-20 \\
319 \\
-524 \\
680
\end{array}\right)
}\tag{31}\end{eqnarray}

となり、これを解くと

\begin{eqnarray}\displaystyle{
\left(\begin{array}{l}
k_1 \\
k_2 \\
k_3 \\
k_4
\end{array}\right)=\left(\begin{array}{l}
14.98 \\
1.27 \\
3.15 \\
2.43
\end{array}\right)
}\tag{32}\end{eqnarray}

が得られます。これを使うと(12)、(22)から

\begin{eqnarray}\displaystyle{
u=k_1 \varphi+k_2 \dot{\varphi}+k_3 y+k_4 \dot{y}=14.98 \varphi+1.27 \dot{\varphi}+3.15y+2.43 \dot{y}
}\tag{33}\end{eqnarray}

として制御入力が決まりますが、\displaystyle{\mathrm{r}=0.028}　なので、\displaystyle{\mathrm{y}=0.028 \xi}、\displaystyle{\dot{y}=0.068 \dot{\xi}}　という関係があり、それを代入すると

\begin{eqnarray}\displaystyle{
u=k_1 \varphi+k_2 \dot{\varphi}+k_3 y+k_4 \dot{y}=15 \varphi+1.3\dot{\varphi}+0.088 \xi+0.068 \dot{\xi}
}\tag{34}\end{eqnarray}

として、剛体の傾き、回転速度、車輪の角度、その角速度の線形結合の形で制御入力の大きさを決めることができます。そこでモーターの駆動電流を(34)に比例するように決めれば系が安定することになります。

その値を使った制御入力を与えた時の応答の様子を観察したのが下のビデオです。

youtu.be

傾きを補正するように制御力が働いていますが振動していて収束する様子がありません。解析モデルはかなり単純化したものなので、それで導出した係数が最適でないことは当然予想されることで、これを元にして係数をチューニングする必要があります。例えば、

\begin{eqnarray}\displaystyle{
u=k_1 \varphi+k_2 \dot{\varphi}+k_3 y+k_4 \dot{y}=20 \varphi+1.3 \dot{\varphi}+0.10 \xi+0.090 \dot{\xi}
}\tag{35}\end{eqnarray}

としたときの制御応答の様子を観察したのが下のビデオです。明らかに安定性が改善されているのが見て取れます。

youtu.be

←　Gyrod　【理論編　その１】

→　Gyrod　【ソフトウェア編】

→　Gyrod 　【ハードウェア編】