　はじめまして、東北大学のT-semiというサークルで学ロボに参加しているりょーつといいます。自分は高専からの編入生で、高専在籍中からずっと機械班でした。最近は人手不足の影響もあり、制御班としても活動しています。

1. はじめに

　夏休みに「なんか名前がカッコいい」という単純な理由でMPCについて勉強していたのですが、先日いい感じにモータを回すことができたので、メモを兼ねて記事を書きました。制御班歴がかなり浅いので、変なこと書いてる可能性は高いです。
　途中で変な改行が入ってしまうのでスマホよりもPCで読んでもらう方が読みやすいと思います。

2. モデル予測制御(MPC)とは

　モデル予測制御(MPC)は、制御周期ごとに未来の出力を予測し、その結果をもとに最適化を行う制御手法です。「未来の出力を予測する」というととても難しいことをしているように思えますが、実はそうでもありません。例えば下図に示すようなバネ係数$k$のバネがついた質量$M$の台車のモデルを考えましょう。この台車の運動方程式は変位を$x$とすると

\begin{eqnarray}
M\dfrac{d^2 x}{{dt}^2} + kx = 0
\tag{1}
\end{eqnarray}

となります。これは力学の授業でよく見る式だと思います。ではこの式を初期条件$x(0) = A$、$\dfrac{dx}{dt}(0) = 0$のもとで解くと

\begin{eqnarray}
x(t) = A \cos{\left(\sqrt{\dfrac{k}{M}}t\right)}
\tag{2}
\end{eqnarray}

の解が得られます。それはそうなのですが、これのすごいところは、時刻$t = 0$のときの状態(初期条件)が分かるだけであらゆる時刻$t$における台車の振る舞いが(2)式で表現されるということです。言い方を変えると、(2)式は「時刻$t = 0$の台車の状態から未来の台車の状態を推定する式」とも言えます。
　これらのことからモデル予測制御は動作をしながら何度も運動方程式を解いて、逐一入力を変化させるような制御と言えます。運動方程式ってマイコンで解けるの？と思われるかもしれませんが、ちゃんとコードを書いてやると解けます(しらんけど)。むしろコンピュータじゃないと解けないような運動方程式も世の中には存在しています(気になる方はナビエ・ストークス方程式でググろう)。そして上記の例を見て分かるように、モデル予測制御をするためには「①それなりに正しいモデル(運動方程式)」と「②初期条件」の2つが必要になります。

3. 数学的準備

　記事の後半でスカラーをベクトルで微分する変形が登場します。本章ではその時に使用する公式を証明しておきます。
　まず行列$A$とベクトル$\boldsymbol{ u }$を以下のように定義します。

\begin{eqnarray}
A = \left(
\begin{array}{cccc}
a_{ 11 } & a_{ 12 } & \ldots & a_{ 1n } \\
a_{ 21 } & a_{ 22 } & \ldots & a_{ 2n } \\
\vdots & \vdots & \ddots & \vdots \\
a_{ n1 } & a_{ n2 } & \ldots & a_{ nn }
\end{array}\tag{3}
\right)
\end{eqnarray}

\begin{eqnarray}
\boldsymbol{ u } = \left(
\begin{array}{c}
u_{ 1 }\\
u_{ 2 }\\
\vdots\\
u_{ n }
\end{array}\tag{4}
\right)
\end{eqnarray}

またスカラー$y_1$を以下の二次形式で定義します。

\begin{align}
y_1 &= \begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix} \begin{pmatrix}
a_{11} & a_{12} & \ldots & a_{1n} \\
a_{21} & a_{22} & \ldots & a_{2n} \\
\vdots & \vdots & \ddots & \vdots \\
a_{n1} & a_{n2} & \ldots & a_{nn}
\end{pmatrix} \begin{pmatrix}
u_1 \\
u_2 \\
\vdots \\
u_n
\end{pmatrix} &= \begin{pmatrix} 
\sum_{k = 1}^{n} u_k a_{k1} & \sum_{k = 1}^{n} u_2 a_{k2} & \ldots & \sum_{k = 1}^{n} u_n a_{kn}
\end{pmatrix}
\begin{pmatrix}
u_1 \\
u_2 \\
\vdots \\
u_n
\end{pmatrix} &= u_1\sum_{k = 1}^{n} u_k a_{k1} + u_2\sum_{k = 1}^{n} u_k a_{k2} + \ldots + u_n\sum_{k = 1}^{n} u_k a_{kn}\tag{5}
\end{align}

$y_1$を$u_1$で偏微分すると

\begin{align}
\dfrac{\partial y_1}{\partial u_1} 
&=
\sum_{k = 1}^{n} u_k a_{k1} + \sum_{k = 1}^{n} u_k a_{1k}
&=
\begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{11} \\
a_{21} \\
\vdots \\
a_{n1}
\end{pmatrix}
+
\begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{11} \\
a_{12} \\
\vdots \\
a_{1n}
\end{pmatrix}\tag{6}
\end{align}

になります。同様に$y$を$u_2$で偏微分すると

\begin{eqnarray}
\dfrac{\partial y_1}{\partial u_2} =
\sum_{k = 1}^{n} u_k a_{k2} + \sum_{k = 1}^{n} u_k a_{2k}&=
\begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{12} \\
a_{22} \\
\vdots \\
a_{n2}
\end{pmatrix} + \begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{21} \\
a_{22} \\
\vdots \\
a_{2n}
\end{pmatrix}\tag{7}
\end{eqnarray}

になります。規則性より$y$を$u_m$で偏微分すると

\begin{eqnarray}
\dfrac{\partial y_1}{\partial u_m} 
&= \sum_{k = 1}^{n} u_k a_{km} + \sum_{k = 1}^{n} u_k a_{mk}
&= \begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{1m} \\
a_{2m} \\
\vdots \\
a_{nm}
\end{pmatrix}
+
\begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{m1} \\
a_{m2} \\
\vdots \\
a_{mn}
\end{pmatrix}\tag{8}
\end{eqnarray}

になります。(6)～(8)式の関係をベクトル$\dfrac{dy_1}{d\boldsymbol{ u }}$を定義して1つにまとめると

\begin{eqnarray}
\left(\dfrac{dy_1}{d\boldsymbol{ u }}\right)^T = \begin{pmatrix} 
\dfrac{\partial y_1}{\partial u_1} & \dfrac{\partial y_1}{\partial u_2} & \ldots & \dfrac{\partial y_1}{\partial u_n}
\end{pmatrix} = \begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{11} & a_{21} & \ldots & a_{n1} \\
a_{12} & a_{22} & \ldots & a_{n2} \\
\vdots & \vdots & \ddots & \vdots \\
a_{1n} & a_{2n} & \ldots & a_{nn}
\end{pmatrix} + \begin{pmatrix} 
u_1 & u_2 & \ldots & u_n
\end{pmatrix}
\begin{pmatrix}
a_{11} & a_{12} & \ldots & a_{1n} \\
a_{21} & a_{22} & \ldots & a_{2n} \\
\vdots & \vdots & \ddots & \vdots \\
a_{n1} & a_{n2} & \ldots & a_{nn}
\end{pmatrix} = {\boldsymbol{ u }}^T ({A}^T + A)
\tag{9}
\end{eqnarray}

が得られます。複雑な計算が続きましたが、結局のところ

\begin{eqnarray}
\dfrac{d}{d\boldsymbol{ u }} ({\boldsymbol{ u }}^T A {\boldsymbol{ u }})= {\boldsymbol{ u }}^T ({A}^T + A)\tag{10}
\end{eqnarray}

になることが分かれば良いです。さらにベクトル$\boldsymbol{ p }$を

\begin{eqnarray}
\boldsymbol{ p } = \left(
\begin{array}{c}
p_{ 1 }\\
p_{ 2 }\\
\vdots\\
p_{ n }
\end{array}\tag{11}
\right)
\end{eqnarray}

と定義し、$\boldsymbol{ p }$を用いてスカラー$y_2$を

\begin{eqnarray}
y_2 = {\boldsymbol{ p }}^T \boldsymbol{ u } = 
\begin{pmatrix} 
p_1 & p_2 & \ldots & p_n
\end{pmatrix}
\begin{pmatrix}
u_{1} \\
u_{2} \\
\vdots \\
u_{n}
\end{pmatrix} = \sum_{k = 1}^{n} p_k u_k
\tag{12}
\end{eqnarray}

と定義します。$y_2$を$u_1$で偏微分すると

\begin{eqnarray}
\dfrac{\partial y_2}{\partial u_1} = p_1
\tag{13}
\end{eqnarray}

になります。同様に$y_2$を$u_2$で偏微分すると

\begin{eqnarray}
\dfrac{\partial y_2}{\partial u_2} = p_2
\tag{14}
\end{eqnarray}

になります。規則性より$y_2$を$u_m$で偏微分すると

\begin{eqnarray}
\dfrac{\partial y_2}{\partial u_m} = p_m
\tag{15}
\end{eqnarray}

になります。(13)～(15)式の関係をベクトル$\dfrac{dy_2}{d\boldsymbol{ u }}$を定義して1つにまとめると

\begin{eqnarray}
\left(\dfrac{dy_2}{d\boldsymbol{ u }}\right)^T = \begin{pmatrix} 
\dfrac{\partial y_2}{\partial u_1} & \dfrac{\partial y_2}{\partial u_2} & \ldots & \dfrac{\partial y_2}{\partial u_n}
\end{pmatrix} = \begin{pmatrix} 
p_1 & p_2 & \ldots & p_n
\end{pmatrix} = {\boldsymbol{ p }}^T
\tag{16}
\end{eqnarray}

のようにまとめられます。結局のところ

\begin{eqnarray}
\dfrac{d}{d\boldsymbol{ u }} ({\boldsymbol{ p }}^T\boldsymbol{ u })= {\boldsymbol{ p }}^T
\tag{17}
\end{eqnarray}

になることが分かれば良いです。

4. モータの運動方程式の離散化

　$s$領域において、モータに入力する電圧$u$と回転速度$\omega$は1次遅れ系の伝達関数を用いて

\begin{eqnarray}
\omega(s) = \dfrac{K}{1+Ts} u(s)
\tag{18}
\end{eqnarray}

の関係にあります。ただし$T$をモータの時定数、$K$をゲインとします。(18)式を

\begin{eqnarray}
\omega(s) + Ts\omega(s) = Ku(s)
\tag{19}
\end{eqnarray}

と変形し、初期値0として両辺を逆ラプラス変換するとモータに関する運動方程式

\begin{eqnarray}
\omega(t) + T \dfrac{d\omega(t)}{dt} = Ku(t)
\tag{20}
\end{eqnarray}

が得られます。ここで制御周期を$\Delta t$とおくと後退差分近似によって

\begin{eqnarray}
\dfrac{d\omega(t)}{dt} = \dfrac{\omega(t) - \omega(t - \Delta t)}{\Delta t}
\tag{21}
\end{eqnarray}

の関係が得られるため、(21)式を(20)式に代入すると離散化された運動方程式

\begin{eqnarray}
\omega(t) = \dfrac{T}{T + \Delta t}\omega(t-\Delta t) + \dfrac{K\Delta t}{T + \Delta t}u(t)
\tag{22}
\end{eqnarray}

が得られます。さらに見やすいように$\omega_n = \omega(t)$、$\omega_{n - 1} = \omega(t-\Delta t)$、$u_n = u(t)$とおくと

\begin{eqnarray}
\omega_n = \dfrac{T}{T + \Delta t}\omega_{n - 1} + \dfrac{K\Delta t}{T + \Delta t}u_n
\tag{23}
\end{eqnarray}

のように漸化式の形で整理されます。ちょっとややこしい変形をしましたが、とりあえず(23)式が成り立つということだけ抑えてもらえるといいと思います。今後(23)式をさらに変形していくのでより見やすいように漸化式を1つシフトしておきます。また定数$a$と$b$を用いて

\begin{eqnarray}
\omega_{n+1} = a\omega_{n} + bu_{n+1}
\tag{24}
\end{eqnarray}

としておきます。ただし

\begin{eqnarray}
a = \dfrac{T}{T + \Delta t}
\tag{25}
\end{eqnarray}

\begin{eqnarray}
b = \dfrac{K\Delta t}{T + \Delta t}
\tag{26}
\end{eqnarray}

です。

5. モデル予測

　運動方程式が(24)式のように離散化されたのでこれを解いていきます。(24)式はある瞬間にモータの回転速度が$\omega_n$だった場合、次の制御で$u_{n+1}$の入力を与えることで回転速度が$\omega_{n+1}$に変化することを意味しています。したがって、これを繰り返すことで制御が$m$周したあとの回転速度$\omega_{n+m}$を予想することができます。では$\omega_{n+1}$は(24)式で予想されているため、$\omega_{n+2}$を予想してみます。(24)の漸化式に沿うように式を考えると

\begin{eqnarray}
\omega_{n+2} = a\omega_{n+1} + bu_{n+2}
\tag{27}
\end{eqnarray}

となります。ここで(24)式を(27)式に代入すると

\begin{eqnarray}
\omega_{n+2} = a^2 \omega_{n} + abu_{n+1} + bu_{n+2}
\tag{28}
\end{eqnarray}

が得られます。同様に$\omega_{n+3}$を求めると

\begin{eqnarray}
\omega_{n+3} = a^3 \omega_{n} + a^2 bu_{n+1} + abu_{n+2} + bu_{n+3}
\tag{29}
\end{eqnarray}

が得られます。なんとなく規則性が見えてきたでしょうか？漸化式の規則性より$m$周期後の回転数$\omega_{n+m}$は

\begin{eqnarray}
\omega_{n+m} = a^m \omega_{n} + b\sum_{k = 1}^{m} a^{m - k} u_{n + k}
\tag{30}
\end{eqnarray}

によって与えられます。簡潔に言うと(30)式は制御$m$周分後の未来の出力を予測する式となります。(28)～(30)式の内容をベクトルと行列を用いてまとめると

\begin{eqnarray}
\begin{pmatrix}
\omega_{n+1}\\
\omega_{n+2}\\
\vdots\\
\omega_{n+m}
\end{pmatrix}
=
\omega_n \begin{pmatrix}
a\\
a^2\\
\vdots\\
a^m
\end{pmatrix}
+
\begin{pmatrix}
b & 0 & \ldots & 0 \\
ab & b & \ldots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
a^{m-1}b & a^{m-2}b & \ldots & b
\end{pmatrix}
\begin{pmatrix}
u_{n+1}\\
u_{n+2}\\
\vdots\\
u_{n+m}
\end{pmatrix}
\tag{31}
\end{eqnarray}

となります。見やすくするためにベクトル$\boldsymbol{\omega}$と$\boldsymbol{u}$と$\boldsymbol{a}$および行列$B$を

\begin{eqnarray}
\boldsymbol{\omega} = \begin{pmatrix}
\omega_{n+1}\\
\omega_{n+2}\\
\vdots\\
\omega_{n+m}
\end{pmatrix}
\tag{32}
\end{eqnarray}

\begin{eqnarray}
\boldsymbol{u} = \begin{pmatrix}
u_{n+1}\\
u_{n+2}\\
\vdots\\
u_{n+m}
\end{pmatrix}
\tag{33}
\end{eqnarray}

\begin{eqnarray}
\boldsymbol{a} = \begin{pmatrix}
a\\
a^2\\
\vdots\\
a^m
\end{pmatrix}
\tag{34}
\end{eqnarray}

\begin{eqnarray}
B = \begin{pmatrix}
b & 0 & \ldots & 0 \\
ab & b & \ldots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
a^{m-1}b & a^{m-2}b & \ldots & b
\end{pmatrix}
\begin{pmatrix}
u_{n+1}\\
u_{n+2}\\
\vdots\\
u_{n+m}
\end{pmatrix}
\tag{35}
\end{eqnarray}

と定義して(31)式に代入すると

\begin{eqnarray}
\boldsymbol{\omega} = \omega_n \boldsymbol{a} + B \boldsymbol{u}
\tag{36}
\end{eqnarray}

のようにまとめられます。ちょっとめんどくさい計算が続きましたが、モデルの予測結果は(36)式の形でまとまるんやなぁと思ってもらえれば大丈夫です。

6. 評価関数

6.1 最適制御とは

　制御では位置や速度といったシステムの状態を変化させることが多いですが、その変化のさせ方にも注目する必要があります。
　例えばA君が宮城から京都まで移動させたいとします。これも宮城県にいるA君の位置を京都府に移動させる立派な制御の1つです。このときA君を移動させる手段はいろいろあります。最速で移動させるのであれば飛行機が良さそうです。しかしこれにはかなりのお金がかかってしまいます。新幹線はどうでしょうか。新幹線もそれなりに速いですがやはりまあまあなお金がかかってしまいます。お金をかけない移動手段として自転車が挙げられますが、これには多大な時間がかかってしまうため、制御としての追従性能は悪そうです。これらのことから、コスト抑えめでまあまあ速く移動できる手段として車が最適だと考えられます。
　このように状態を追従するだけではなく、動作に必要なコスト(モータ制御の場合は電圧)にも注目し、それらを両立できるような入力を計算して制御する手法を最適制御と言います。計算には状態の追従誤差$e$と入力の大きさ$u$、そしてそれらの重み(重要度)を決める定数$q$と$r$を使った評価関数というものを

\begin{eqnarray}
\int_{0}^{t} qe^2 + ru^2 dt
\tag{37}
\end{eqnarray}

のように定義してそれらが最小となるような$u$を求めます。(37)式はあくまでもイメージで、実際の計算には二次形式とかを使ったもっと複雑な式がバンバン出てきます..。

6.2 評価関数の計算

　まず評価関数のうち偏差に関連する項$J_e$を求めます。ある瞬間にモータの回転数が$\omega_n$だった場合に、制御1周期分あとの目標回転数を$\omega_{r_1}$、2周期分あとの目標回転数を$\omega_{r_2}$、$m$周期分あとの目標回転数を$\omega_{r_m}$、とします。制御$m$周期分あとのモデル予測の結果と目標値の差(偏差)を$e_m$とすると、制御1周期分あとについて

\begin{eqnarray}
e_1 = \omega_{r_1} - \omega_{n+1}
\tag{38}
\end{eqnarray}

が成り立ちます。もちろん制御1周期分あとについて

\begin{eqnarray}
e_2 = \omega_{r_2} - \omega_{n+2}
\tag{39}
\end{eqnarray}

が成り立ち、制御m周期分あとについては

\begin{eqnarray}
e_m = \omega_{r_m} - \omega_{n+m}
\tag{40}
\end{eqnarray}

が成り立ちます。(38)～(40)式で計算した偏差についても、ベクトル$\boldsymbol{e}$を

\begin{eqnarray}
\boldsymbol{e} = \begin{pmatrix}
e_1\\
e_2\\
\vdots\\
e_m
\end{pmatrix}
\tag{41}
\end{eqnarray}

のように定義し、(31)式を代入することで

\begin{eqnarray}
{\boldsymbol{e}} = \boldsymbol{\omega_r} - \boldsymbol{\omega} = \boldsymbol{\omega_r} - \omega_n \boldsymbol{a} - B \boldsymbol{u}
\tag{42}
\end{eqnarray}

のように見やすくまとめることができます。あとは重み行列$Q$を

\begin{eqnarray}
Q = \begin{pmatrix}
q_1 & 0 & \ldots & 0 \\
0 & q_2 & \ldots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
0 & 0 & \ldots & q_m
\end{pmatrix}
\tag{43}
\end{eqnarray}

と定義してやると、評価関数のうち偏差に関する項$J_e$は

\begin{eqnarray}
J_e = 
{\boldsymbol{e}}^T Q {\boldsymbol{e}}
\tag{44}
\end{eqnarray}

という二次形式で表されます。同様に重み行列$R$を

\begin{eqnarray}
R = \begin{pmatrix}
r_1 & 0 & \ldots & 0 \\
0 & r_2 & \ldots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
0 & 0 & \ldots & r_m
\end{pmatrix}
\tag{45}
\end{eqnarray}

と定義することで評価関数のうち偏差に関する項$J_u$も

\begin{eqnarray}
J_u = 
{\boldsymbol{u}}^T R {\boldsymbol{u}}
\tag{46}
\end{eqnarray}

という二次形式で表されます。これらを足し合わせることで評価関数$J$が

\begin{eqnarray}
J = 
{\boldsymbol{e}}^T Q {\boldsymbol{e}} + {\boldsymbol{u}}^T R {\boldsymbol{u}}
\tag{47}
\end{eqnarray}

のように表現されます。あとでてきますが、重みづけ行列$Q$、$R$が対象行列であり、$Q^T = Q$、$R^T = R$を満たすというのがミソです。
　さらに(42)式を(47)式に代入して展開すると

\begin{eqnarray}
J&=
{\boldsymbol{u}}^T B^T Q B {\boldsymbol{u}} - 2({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB {\boldsymbol{u}}+ ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T Q ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})+{\boldsymbol{u}}^T R {\boldsymbol{u}}\\
&= {\boldsymbol{u}}^T (B^T Q B + R) {\boldsymbol{u}} - 2({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB {\boldsymbol{u}}+ ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T Q ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})
\tag{48}
\end{eqnarray}

のように入力$\boldsymbol{u}$を変数とする評価関数$J$が得られました。

7. 入力の決定

　最後に評価関数$J$を極小にするような入力$\boldsymbol{u}$を計算します。とてもややこしいですが、これは多変数関数の極値問題として解くことができます。高校3年生で学んだ2変数関数の極値問題を思い出してください。例えば$z = z(x,y)$の極値を探す場合、まず$\dfrac{\partial z}{\partial x}$と$\dfrac{\partial z}{\partial y}$を計算してこれらが共に0となる点(停留点)を求めましたよね。それとおんなじように評価関数$J$に対して$\dfrac{\partial J}{\partial u_1}$～$\dfrac{\partial J}{\partial u_m}$を計算し、それらすべてが0になる点を探せば良いです。数学的には2変数関数の極値問題で習ったようにヘッシアン$H$とかを使って停留点が極値を持つことを証明しないといけませんが、ここでは$\dfrac{\partial J}{\partial u_1}$～$\dfrac{\partial J}{\partial u_m}$すべてが0になる点では$J$が極小値を取るでしょうということにして極値を探すことにします。いちいち微分するのは面倒なので(9)、(17)の公式を使って計算します。$\dfrac{dJ}{d\boldsymbol{u}}$の各成分は$\dfrac{\partial J}{\partial u_1}$～$\dfrac{\partial J}{\partial u_m}$に対応しているため、

\begin{eqnarray}
\dfrac{dJ}{d\boldsymbol{u}} = \boldsymbol{0}
\tag{49}
\end{eqnarray}

を満たすような$\boldsymbol{u}$を計算すればよいことになります。(9)、(17)の公式に注意して(48)式の両辺を$\boldsymbol{u}$で微分すると

\begin{eqnarray}
\left(\dfrac{dJ}{d\boldsymbol{u}}\right)^T
= \boldsymbol{u}^T \left((B^T Q B + R) + (B^T Q B + R)^T \right) -2({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB
\tag{50}
\end{eqnarray}

となります。ここで$Q^T = Q$、$R^T = R$に注意すると

\begin{eqnarray}
(B^T Q B + R)^T = (B^T Q B)^T + R^T = B^T Q^T B + R^T = B^T Q B + R
\tag{51}
\end{eqnarray}

が成り立つため(50)式は

\begin{eqnarray}
\left(\dfrac{dJ}{d\boldsymbol{u}}\right)^T
= 2\boldsymbol{u}^T (B^T Q B + R) - 2({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB
\tag{52}
\end{eqnarray}

のように少しきれいになります。あとは(49)式を満たす$\boldsymbol{u}$を$\boldsymbol{u_r}$と置き、(52)式を(49)式に代入して整理すると

\begin{eqnarray}
\boldsymbol{u_r}^T (B^T Q B + R) - ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB = \boldsymbol{0}^T\\\\
\boldsymbol{u_r}^T (B^T Q B + R) = ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB\\\\
\therefore \boldsymbol{u_r}^T = ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB (B^T Q B + R)^{-1}
\tag{53}
\end{eqnarray}

によって$\boldsymbol{u_r}$が計算できることが分かります。ただし$\boldsymbol{u_r}$が計算できたところで、実際に次の制御入力としてマイコンに与える入力$u$は$\boldsymbol{u_r}$の第1成分のみなので

\begin{eqnarray}
u = ({\boldsymbol{\omega_r} - \omega_n \boldsymbol{a}})^T QB (B^T Q B + R)^{-1}
\begin{pmatrix}
1\\
\boldsymbol{0}
\end{pmatrix}
\tag{54}
\end{eqnarray}

を使って制御入力$u$を決定します。

8. シミュレータについて

　ブラウザ上のC言語環境で動作するシミュレータを作りました。以下のコードをコピペして実行してみてください。マクロ定義の部分の値をいろいろ変化させることで応答が変わってくると思います。シミュレータの応答を見ながら重みを変化させるといいと思います。ちなみに入力の重み$R$は単位行列$I$にすることが多いそうです。C言語なのでSTM32に移植してみるのも面白いかもしれません。

//一定の目標回転数"omega_ref"に追従させるシミュレータです。
#include <stdio.h>
#include <math.h>
/*以下のパラメータを決定してください------------------------------------------------------*/
/*モデルのパラメータ---------------------------------------------------------*/
#define n 5             //予測の数(<10)
#define T 0.05          //時定数 [s]
#define K 7             //ゲイン
#define dt 0.002        //制御周期 [s]
#define omega_start 100 //初期回転速度 [rpm]
#define omega_ref 2000  //目標回転速度 [rpm]

/*重みQの決定----------------------------------------------------------------*/
#define q_1 10
#define q_2 10
#define q_3 10
#define q_4 10
#define q_5 10
#define q_6 10
#define q_7 10
#define q_8 10
#define q_9 10

/*重みRの決定----------------------------------------------------------------*/
#define r_1 1
#define r_2 1
#define r_3 1
#define r_4 1
#define r_5 1
#define r_6 1
#define r_7 1
#define r_8 1
#define r_9 1
/*---------------------------------------------------------------------------------------*/
int main(void)
{   
    /*マイコン起動時に行う計算-------------------------------------------------*/
    /*モデルパラメータの宣言と計算------------------------------*/
    float a = T/(T + dt);
    float b = K*dt/(T + dt);
    float omega_zero = omega_start; //回転速度の制御周期ごとの初期値
    float pwm = 0;                  //制御入力
    float buff = 0;                 //途中計算に使用    

    /*行列の定義、初期化---------------------------------------*/
    float B[n][n];       //遷移行列
    float Q[9][9];       //重みづけ行列
    float R[9][9];       //重みづけ行列
    float BUFF[n][n];    //途中計算に使用

    for(int i = 0;i<n;i++){
        for(int j = 0;j<n;j++){
            Q[i][j] = 0;
            R[i][j] = 0;
            if(j>i){
                B[i][j]=0;
            }else{
                B[i][j]=b*pow(a,i-j);
            }
        }
    }
    /*Qにおもみの代入------------------------------------------*/
    Q[0][0] = q_1;
    Q[1][1] = q_2;
    Q[2][2] = q_3;
    Q[3][3] = q_4;
    Q[4][4] = q_5;
    Q[5][5] = q_6;
    Q[6][6] = q_7;
    Q[7][7] = q_8;
    Q[8][8] = q_9;

    /*Rにおもみの代入------------------------------------------*/
    R[0][0] = r_1;
    R[1][1] = r_2;
    R[2][2] = r_3;
    R[3][3] = r_4;
    R[4][4] = r_5;
    R[5][5] = r_6;
    R[6][6] = r_7;
    R[7][7] = r_8;
    R[8][8] = r_9;

    /*行列演算-------------------------------------------------*/
    /*QBをBUFFに代入-----------------------*/
    for(int i = 0;i<n;i++){
        for(int j = 0;j<n;j++){
            buff = 0;           
            for(int k = 0;k<n;k++){
                buff += Q[i][k]*B[k][j];
            }
            BUFF[i][j] = buff;           
        }
    }

    /*(BQB+R)をBに代入---------------------*/
    for(int i = 0;i<n;i++){
        for(int j = 0;j<n;j++){
            buff = 0;           
            for(int k = 0;k<n;k++){
                buff += BUFF[k][i]*B[k][j];
            }
            B[i][j] = buff + R[i][j];           
        }
    }

    /*(BQB+R)^-1をRに代入------------------*/
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            R[i][j]=(i==j)?1.0:0.0;
        }
    }    
    for(int i=0;i<n;i++){
        buff=1/B[i][i];
        for(int j=0;j<n;j++){
            B[i][j]*=buff;
            R[i][j]*=buff;
        }
        for(int j=0;j<n;j++){
            if(i!=j){
                buff=B[j][i];
                for(int k=0;k<n;k++){
                    B[j][k]-=B[i][k]*buff;
                    R[j][k]-=R[i][k]*buff;
                }
            }
        }
    }

    /*QB(BQB+R)^-1をBに代入------------------*/
    for(int i = 0;i<n;i++){
        for(int j = 0;j<n;j++){
            buff = 0;           
            for(int k = 0;k<n;k++){
                buff += BUFF[i][k]*R[k][j];
            }
            B[i][j] = buff;           
        }
    }
    /*-----------------------------------------------------------*/
    /*制御周期ごとに行う処理(50サイクル行う)-----------------------------------------------*/
    for(int k = 0;k<50;k++){       
        /*入力値(omega_ref - omega_zero*a)QB(BQB+R)^-1の計算------*/
        pwm = 0;
        for(int i = 0;i<n;i++){
            pwm += (omega_ref - omega_zero*pow(a,i+1))*B[i][0];
        }
        /*頭打ち用のコード----------------------------------------*/
        if(pwm > 1000){
            pwm = 1000;
        }else if(pwm < -1000){
            pwm = -1000;
        }
        /*モデル予測とシミュレーション結果の表示-------------------*/
        omega_zero = a*omega_zero + b*pwm;
        printf("%d:入力=%d,出力=%d\n",k+1,(int)pwm,(int)omega_zero);
    }
}

9. おわりに

　実験していて感じますが、モータの時定数$T$やゲイン$K$は印加する電圧によって結構バラつきます。つまりモデルの誤差が生じるのです(´・ω・｀)
　今後はこのモデル化する時の誤差とかをどうしようかなぁとか考えています。冬休みにちょっと本でも読もうかな。

目次