←前章：離散時間の非線形MPC
前章では離散時間の非線形MPCについて学びましたが、今回はそれを連続時間に拡張してみましょう。
連続時間ダイナミクスの近似について
連続時間のモデル予測制御については基本的な考え方は離散時間と同じですが、何らかの方法で微分方程式の離散化を行う必要があります。この時に一番簡単なアイデアとしてはオイラー法で微分方程式を差分化してしまう方法です。
\dot{x_t} = f(x_t, u_t) ~~ \rightarrow ~~ x_{t+\Delta t} = x_t + f(x_t,u_t)\Delta t
このやり方は非常に楽ではありますが時間方向の積分を行う際にホライズン長が大きくなるにつれて急激に数値計算の誤差が大きくなってしまいます。これはルンゲクッタ法
\begin{align*}
& x_{t+\Delta t} = x_t + \frac{1}{6} (k_1 + 2 k_2 + 2 k_3 + k_4) \Delta t \\
& k_1 = f(x_t, u_t) \\
& k_2 = f\left( x_t + \frac{1}{2} k_1 \Delta t, u_t\right) \\
& k_3 = f\left( x_t + \frac{1}{2} k_2 \Delta t, u_t\right) \\
& k_4 = f(x_t + k_3 \Delta t, u_t) \\
\end{align*}
を使うことである程度抑え込むことが出来るのですが、それでもやはり誤差はそれなりに残りますし、何より補助変数k_1, k_2, k_3, k_4が増えるので計算量が増大し、ただでさえ計算量の大きなMPCに使うことはできれば避けたい手法ではあります。
直接コロケーション法（Direct Collocation method）
先述の問題点を解決する方法として、ガウス求積法を応用した直接コロケーション法というものがあります。これは基本的には微分方程式の解を多項式（3次多項式、ベジェ多項式、ルジャンドル多項式、etc.）で近似する方法です。近似の際に”上手く”選んだ複数の点（コロケーションポイント）では厳密に微分方程式を満たすよう多項式の係数を調整してやれば高い精度で方程式の解が得られるというものです。
エルミート・シンプソンコロケーション法による解法
MPCの問題設定（連続版）
簡単のために終端コストが無い以下のような最適化問題を考えます。
\mathrm{minimize} ~ J = \int_0^{t_f} L(x(t), u(t)) ~ dt \\
s.t.
\left\{
\begin{align*}
\dot{x} & = f(x_t, u_t) \\
x_{min} & \leq x_t \leq x_{max} \\
u_{min} & \leq u_t \leq u_{max}
\end{align*}
\right.
時間の離散化
t_0(=0)からt_fまでの時間を次のようにN区間に分割します。
t_0 = 0 \leq t_1 \leq t_2 \leq \dots \leq t_N = t_f
ここで、区間[t_k, t_{k+1}]の状態x_tを次のように3次多項式で近似します。
x_t = a_{k,0} + a_{k,1} t + a_{k,2} t^2 + a_{k,3} t^3
するとその時間微分は
\dot{x}_t = a_{k,1} + 2 a_{k,2} t + 3 a_{k,3} t^2
となります。
コロケーションポイント上での等式条件
コロケーションポイントは上記で定めた区間の真ん中の点になります。
t_{k,c} = \frac{t_k + t_{k+1}}{2}
微分方程式の右辺fは時間に陽に依存しないので、以下の議論は時間方向に平行移動しても不変になります。そこで、簡単のために適当な区間[0, h]（ただしh=t_{k+1} - t_k）を考えます。
すると以下の等式が成立します。
\begin{bmatrix}
x_0 \\
\dot{x}_0 \\
x_h \\
\dot{x}_h \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
1 & h & h^2 & h^3 \\
0 & 1 & 2h & 3h^2 \\
\end{bmatrix}
\begin{bmatrix}
a_{k,0} \\
a_{k,1}\\
a_{k,2} \\
a_{k,3} \\
\end{bmatrix}
逆行列を計算すると
\begin{bmatrix}
a_{k,0} \\
a_{k,1}\\
a_{k,2} \\
a_{k,3} \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
-\frac{3}{h^2} & -\frac{2}{h} & \frac{3}{h^2} & -\frac{1}{h} \\
\frac{2}{h^2} & \frac{1}{h^2} & -\frac{2}{h^3} & \frac{1}{h^2} \\
\end{bmatrix}
\begin{bmatrix}
x_0 \\
\dot{x}_0 \\
x_h \\
\dot{x}_h \\
\end{bmatrix}
となります。
したがって、コロケーションポイントにおける状態変数x_cの値とその時間微分\dot{x}_cは次のように計算されます。
x_c = x\left( \frac{h}{2} \right) = \frac{1}{2} (x_k + x_{k+1}) + \frac{h}{8} [f(x_k, u_k) - f(x_{k+1}, u_{k+1})]
\dot{x}_c = \dot{x} \left( \frac{h}{2} \right) = - \frac{3}{2h} (x_k - x_{k+1}) - \frac{h}{4} [f(x_k, u_k) + f(x_{k+1}, u_{k+1})]
ここで、状態変数と制御変数を適切に選んでやればコロケーションポイント上で厳密にダイナミクスを成立させることが出来ます。コロケーションポイント上での制御入力u_cを
u_c = \frac{u_k + u_{k+1}}{2}
で与えると、コロケーションポイント上での誤差\delta_kを次のように計算できます。
\begin{align*}
\delta_k & = \dot{x}_c - f(x_c, u_c) \\
& = - \frac{3}{2h} \left[ (x_k - x_{k+1}) + \frac{h}{6} [f(x_k,u_k) + 4f(x_c, u_c) + f(x_{k+1}, u_{k+1})] \right]
\end{align*}
したがって、ダイナミクスの制約条件として\delta_k=0としてやると、高い精度で最適化問題の離散化が出来ることになります。
コスト関数の離散化
ダイナミクスの近似精度が高ければコスト関数の計算はそこまで気を使う必要はなく、例えば以下のように台形公式を使えば十分な精度を出すことが可能です。
J = \frac{1}{2} \sum_{k=1}^{N-1} (L(x_{k+1}, u_{k+1}) + L(x_k, u_k))(t_{k+1} - t_k)
MPCの問題設定（離散化版）
今までの議論により直接コロケーション法を用いた離散化を行った最適化問題は次のようになります。
\mathrm{minimize} ~ J = \frac{1}{2} \sum_{k=1}^{N-1} (L(x_{k+1}, u_{k+1}) + L(x_k, u_k))(t_{k+1} - t_k) \\
s.t.
\left\{
\begin{align*}
\delta_k \propto (x_k - x_{k+1}) + & \frac{h}{6} [f(x_k,u_k) + 4f(x_c, u_c) + f(x_{k+1}, u_{k+1})] = 0\\
x_{min} & \leq x_k \leq x_{max} \\
u_{min} & \leq u_k \leq u_{max}
\end{align*}
\right.
ここまでくれば離散時間のダイナミクスの場合と全く同じ方法で解くことが可能となります。
ひとまず、モデル予測制御入門シリーズはここで終わりといたします。ご拝読いただきありがとうございました。
この記事で扱った手法の実例としては、別の記事でドローンの制御について紹介したいと思います。
Discussion