現代制御理論 1

現代制御理論です。

倒立振子

振子の長さ : l
重力 : g

運動方程式は、下記とします。
台車は平面を移動して、ある直線のみの軌道を描くとします。
移動量として\(x\)を使いたいのですが、後で\(\boldsymbol{x}\)を使いたいので\(r\)を使います。

\[
\begin{align*}
&(M +m)\ddot{r}+ml\ddot{\theta}\cos\theta-ml\dot{\theta}^2\sin\theta = F \\[0pt]
&\ddot{r}\cos\theta +l\ddot{\theta}-g\sin\theta = 0
\end{align*}
\]

\(\theta\)が小さいとして線形近似します。

\[
\begin{align*}
&(M +m)\ddot{r}+ml\ddot{\theta}-ml\dot{\theta}^2\theta = F \\[0pt]
&\ddot{r} +l\ddot{\theta}-g\theta = 0
\end{align*}
\]

\(\theta\)が小さいとして\(\dot{\theta}^2=0\)とします。

\[
\begin{align*}
&(M +m)\ddot{r}+ml\ddot{\theta} = F \\[0pt]
&\ddot{r} +l\ddot{\theta}-g\theta = 0
\end{align*}
\]

\(\ddot{r}\)についてまとめます。

\[
\begin{align*}
&(M+m)\ddot{r}+m(g\theta-\ddot{r}) = F \\[0pt]
&M\ddot{r} + mg\theta = F
\end{align*}
\]

\[
\ddot{r}=-\frac{mg\theta}{M}+\frac{F}{M}
\]

\(\ddot{\theta}\)についてまとめます。

\[
\begin{align*}
&(M+m)(g\theta-l\ddot{\theta})+ml\ddot{\theta} = F \\[0pt]
&Mg\theta-Ml\ddot{\theta}+mg\theta-ml\ddot{\theta}+ml\ddot{\theta} = F \\[0pt]
&Mg\theta-Ml\ddot{\theta}+mg\theta = F
\end{align*}
\]

\[
\ddot{\theta}=-\frac{(M+m)g\theta}{Ml}+\frac{F}{Ml}
\]

下記として整理します。
三変数としても整理できると思います。

\[
\begin{pmatrix}
\dot{r} \\
\ddot{r} \\
\dot{\theta} \\
\ddot{\theta}
\end{pmatrix}
=
\begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & 0 & \displaystyle \frac{-mg}{M} & 0 \\
0 & 0 & 0 & 1 \\
0 & 0 & \displaystyle \frac{-(M+m)g}{Ml} & 0
\end{pmatrix}
\begin{pmatrix}
r \\
\dot{r} \\
\theta \\
\dot{\theta}
\end{pmatrix}
+
\begin{pmatrix}
0 \\
\displaystyle \frac{1}{M} \\
0 \\
\displaystyle \frac{1}{Ml}
\end{pmatrix}
F
\]

まとめます。

\[
\dot{\boldsymbol{x}} = A\boldsymbol{x} + Bu
\]

\(\theta\)が次の瞬間(とは言っても1msとか)にゼロになるように制御入力\(u\)について考えます。

連立定数係数一階線形常微分方程式

それぞれ同じ変数の関数とします。
\(A\)と\(B\)は係数行列です。\(B\)はそれぞれの関数の方に閉じ込めてしまって、無くしてしまっても構いません。

\[
\begin{pmatrix}
f’ \\
g’ \\
h’ \\
i’ \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
f \\
g \\
h \\
i \\
\vdots
\end{pmatrix}
+
B
\begin{pmatrix}
s \\
t \\
u \\
v \\
\vdots
\end{pmatrix}
\]

また、下記としても良いです。
同じ代数を使っていますが、上の式とは一切関係が無いと考えてください。

\[
\begin{pmatrix}
f’ \\
g^{\prime\prime} \\
h’ \\
i^{\prime\prime\prime} \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
f \\
g’ \\
h \\
i^{\prime\prime} \\
\vdots
\end{pmatrix}
+
B
\begin{pmatrix}
s \\
t \\
u \\
v \\
\vdots
\end{pmatrix}
\]

よく見るのは下記。
ゼロから連番する文献もそこそこあります。

\[
\begin{pmatrix}
y_{1}^{\prime} \\
y_{2}^{\prime} \\
y_{3}^{\prime} \\
y_{4}^{\prime} \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
y_{1} \\
y_{2} \\
y_{3} \\
y_{4} \\
\vdots
\end{pmatrix}
+
B
\begin{pmatrix}
u_{1} \\
u_{2} \\
u_{3} \\
u_{4} \\
\vdots
\end{pmatrix}
\]

こちらもよく見ます。

\[
\begin{pmatrix}
x_{1}^{\prime} \\
x_{2}^{\prime} \\
x_{3}^{\prime} \\
x_{4}^{\prime} \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
x_{1} \\
x_{2} \\
x_{3} \\
x_{4} \\
\vdots
\end{pmatrix}
+
B
\begin{pmatrix}
u_{1} \\
u_{2} \\
u_{3} \\
u_{4} \\
\vdots
\end{pmatrix}
\]

こちらもよく見ます。係数行列\(B\)は\(f\)の方でそれぞれの係数としても構いません。

\[
\begin{pmatrix}
x_{1}^{\prime} \\
x_{2}^{\prime} \\
x_{3}^{\prime} \\
x_{4}^{\prime} \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
x_{1} \\
x_{2} \\
x_{3} \\
x_{4} \\
\vdots
\end{pmatrix}
+
\begin{pmatrix}
f_{1} \\
f_{2} \\
f_{3} \\
f_{4} \\
\vdots
\end{pmatrix}
\]

まとめます。

\[
\boldsymbol{x}^{\prime} = A\boldsymbol{x} + \boldsymbol{f}
\]

または、下記。

\[
\dot{\boldsymbol{x}} = A\boldsymbol{x} + B\boldsymbol{u}
\]

これは、下記のように解くことができます。変数は\(t\)とします。

\[
\boldsymbol{x}(t) = e^{At}\boldsymbol{x}(0) + \int_{0}^{t} e^{t-\tau}B\boldsymbol{u}(\tau)d\tau
\]

定数係数高階線形常微分方程式

教科書の多くは、定数係数高階線形常微分方程式を連立定数係数一階線形常微分方程式にする説明から入ります。

「古典制御理論と同じマスばねダンパーの話しじゃん。古典制御理論でいいじゃん。」と思ってしまっても仕方ないと思います。

定数係数高階線形常微分方程式は、連立定数係数一階線形常微分方程式にすることができます。
この操作は教科書を読めば理解できます。
いつか、書きます。

状態方程式

計算の結果、多変数も高階も扱う準備が整いました。状態方程式を定義しましょう。

これです。

\[
\begin{pmatrix}
x_{1}^{\prime} \\
x_{2}^{\prime} \\
x_{3}^{\prime} \\
x_{4}^{\prime} \\
\vdots
\end{pmatrix}
=
A
\begin{pmatrix}
x_{1} \\
x_{2} \\
x_{3} \\
x_{4} \\
\vdots
\end{pmatrix}
+
B
\begin{pmatrix}
u_{1} \\
u_{2} \\
u_{3} \\
u_{4} \\
\vdots
\end{pmatrix}
\]

入出力の数には厳密な方が良いと思います。
\(x\)と\(\dot{x}\)が\(n\)個、\(u\)が\(m\)個となります。

現場の話し

きちんと制御できるような状態方程式にするのが大変みたいです。
PIDで出来ることまでしかやらないという人もいます。

「なんとか推定」をやりたい人は、状態方程式からは逃げられません。

図にする

状態方程式を箱にしましょう。
後ろ向きに線が出ていますが、これはフィードバックではありません。現代制御理論のフィードバックはこの後すぐに説明します。

太い線としていますが、複数の線を描いた方がより正確な描写でしょう。

\(\boldsymbol{x}(t)\)と\(\boldsymbol{u}(t)\)を足して\(\dot{\boldsymbol{x}}(t)\)としているのはわかります。
何かしらの箱を通ると\(\dot{\boldsymbol{x}}(t)\)が\(\boldsymbol{x}(t)\)になっています。ここには微分の逆操作としてだと思いますが、積分器とする場合が多いです。

空の箱も気分の良いものではないと思いますが、空白でもいいのでは、と思います。

出力方程式

倒立振子に戻って、下記を得ることを考えます。

\[
\begin{pmatrix}
r \\
\dot{r} \\
\theta \\
\dot{\theta}
\end{pmatrix}
\]

移動量と倒れ量なわけですが、Wattの時代のようなイメージの機械的な方法でも取得できるでしょうし、ロータリーエンコーダーとIMUでも取得できるでしょう。
趣味に走ればステレオカメラでも取得できるかもしれません。単眼カメラでも。

下記として得られれば、取りたい情報を全て取れていることになります。

\[
\begin{pmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
r \\
\dot{r} \\
\theta \\
\dot{\theta}
\end{pmatrix}
\]

観測方程式とも言います。

入出力の数には厳密な方が良いと思います。
\(x\)が\(n\)個、\(y\)が\(l\)個となります。

\(C\)は、いつも上記のような綺麗な行列になるわけではありません。
抽象論のみで進めると「出力方程式って、なんやねん。」ということになってしまうので、具体例を出して説明しています。

どれかが取得できない場合があります。
そのときにはオブザーバーという方法で疑似的に補うということを教科書ではやります。

図にする

最もよく見る状態空間表現の図です。
状態方程式の図、としている文献も多いですが。
倒立振子について、この図で考えることができます。ただし、A次第で不安定となります。

古典制御理論のシステムと対応している部分は下記です。
制御対象は連立定数係数一階線形常微分方程式です。

こちらもよく見ます。

\(D\)も含めた議論というのは見たことがないので以降の説明では無視することにします。
入力を分岐させるだけなので、必要に迫られたら含めてください。

状態フィードバック

現代制御理論では、状態をフィードバックできます。
状態に係数をかけてフィードバックします。

こちらが古典制御理論のフィードバックの箱の集合と対応します。
図は似ていますが別物です。
状態をフィードバックできるとかそういうことではなく、補償器が一対一対応していないとかそういう次元で別物です。

今まで\(\boldsymbol{u}(t)\)としていたものを\(\boldsymbol{v}(t)\)とします。

\[
\boldsymbol{u}(t) = K\boldsymbol{x}(t) + \boldsymbol{v}(t)
\]

\[
\begin{align*}
\dot{\boldsymbol{x}}(t) &= A\boldsymbol{x}(t) + B(K\boldsymbol{x}(t) + \boldsymbol{v}(t)) \\[0pt]
&= (A+BK)\boldsymbol{x}(t) + B\boldsymbol{v}(t)
\end{align*}
\]

これで\(A+BK\)の固有値を操れるという議論をします。つまり、\(A\)が不安定を示していても\(BK\)により安定させるという方法をとります。また、反応を速くさせることもできます。

現代制御理論では出力フィードバックしないんかい

当然、こういう設計も考えることができて、実際にあります。大学院ならやるかもしれません。
積分器を1個だけおいているのは「こんな感じの設計もある。」という意図です。いい加減です。

入力は右からになっています。\(\boldsymbol{r}(t)\)は目標値です。
入力が右からの図にも慣れておきましょう。いきなり出てきます。

カルマンフィルター

1つの実装例です。
離散時間です。

最初の図は便宜的なものですが、間違いではありません。
時間表現を空間表現にした、とか言ってもいいです。

カルマンフィルターは直前の情報のみを使います。
推定値\(\widehat{\boldsymbol{x}}_{k-1}\)と共分散行列\(P_{k-1}\)はC言語などの変数で保持させます。

空白の箱は、教科書に書いてある数式をプログラムにしたものが入っているとしてください。
行列と確率が延々と続くやつです。
なので、この空白の箱にはカルマンゲインも入っています。
空白とすることに特別な意図はなくて、Dの次のEでもいいし、Zは使えないからXやYを使おうかとも考えましたが、しっくりきませんでした。

\(z^{-1}\)は空白でもいいかなと思っています。

フィードバックを追加します。
カルマンフィルターのみの説明としてはこれは不要です。

状態の推定値をフィードバックさせて固有値を操って安定させるわけですが、これは「なにフィードバック」なのでしょうか。
厳密には状態フィードバックではない気がします。

Kには共分散行列は行きません。

過去を少し消した図です。

過去を全て消した図です。

2段が更新されていくと考えても良いし、1段が更新されていくと考えても良いです。
プログラムの処理のことを考えると、1段が更新される、の方が事実に近いと思いますので最初の図のことを便宜的としています。

まとめ 1

結構いい加減に書きましたので、数式は間違っているかもしれません。
ただ、議論の流れはわかりやすく書いたつもりで、そちらが主眼なので、ご容赦ください。

カルマンフィルターも実際の実装を確認して書き直すつもりです。
もっと入出力を明確にしたいです。推定値\(\widehat{\boldsymbol{x}}_{k}\)は、外に出すと思いますので。

教科書では、状態フィードバックの説明のための準備が長すぎて、状態フィードバックの説明の前にはやる気は無くなっていると思います。

大枠を説明したので、後は教科書の数式を1つずつ理解すれば良いので、見通しが良くなっていると思います。

まとめ 2

倒立振子の運動方程式はオイラー・ラグランジュ運動方程式を使っています。

現代制御理論を抽象的な説明から始めないためにはオイラー・ラグランジュ運動方程式を先に理解しておく必要があると思います。

広告

IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト

« 古典制御理論 11     Windowsでplatformioを使う »