非線形モデル予測制御におけるニュートン法をpythonで実装する（強化学習との関係をそえて）

はじめに

非線形モデル予測制御におけるCGMRES法をpythoで実装するの記事の関連記事です。

入力制約付き非線形最適制御問題を解いてます。

ハミルトニアンの微分等はまだ必要ですが、C/GMRESのような計算手法を用いず、数値微分を用いたニュートン法を実装しているため前回の記事よりも簡易になっています。

また、すこーしだけ強化学習と最適制御について述べています。
強化学習好きな方にも興味を持っていただけることを勝手に期待しています。
ただ、双方にそれなりに知っている方からすると、当たり前のことを言うな！となるかと思いますので、すっと読み飛ばしていただけると嬉しいです。

本記事のコードは、
https://github.com/Shunichi09/linear_nonlinear_control
に上がっています。

おしながき

• 前提とすこーしだけ無駄話
• 問題の定式化とその具体例
• 予備知識
• 問題設定（前回記事の引用）
• オイラーラグランジュ方程式の導出（前回記事の引用）
• 結果
• 具体的なプログラムについて
• 最後に

前提とすこーしだけ無駄話

このパートは無駄話を含みます。NMPCにだけ興味があるんだ！という人は飛ばしてください。
以前から最適制御に興味があって、いろいろと勉強していると、多くの点で強化学習との共通点が存在することに気づき、今では、強化学習と最適制御の両輪で勉強しています。

実は最適制御系の研究が多い、計測と制御の2019年の3月号にもダイナミクスと機械学習の融合に挑むというタイトルで、特集が組まれています。この中の記事、制御工学者のための強化学習入門で、強化学習の手法の1つが適応最適制御の1つであることが述べられています。興味のある方はぜひ読んでみてください。
足立先生の制御工学の人工知能の近くて遠い関係の記事も面白いかと思います。なお、僕はこの記事に感銘を受けて、適応制御も少しずつですが、勉強中です。参考記事です。（僕の）

ここでは、それらの記事にinspireされたこともあり、すこーしだけ、強化学習と最適制御の関係について述べてみようと思います。

強化学習と最適制御の目標

そもそも強化学習と最適制御の目標って何なのでしょうか？

強化学習の目標は、

報酬の和の期待値を最大化する方策を求めることです

\text{maximize} \ J = E_{\tau \sim \pi_\theta(\tau)} \left[ \sum_t r(x_t, u_t) \right]

$r$はreward、$x$はstate、$u$はactionです（あえて、$x$と$u$を使ってます。制御側からの記事なので。）
さらに、$\pi$は方策（あるstate、$x$のもとでinput、$u$を吐き出すもの）で、$\theta$はパラメータになります。
なお、$\tau$は軌道です。stateとactionの時系列です。$\tau=(x_1, u_1, ... x_T, u_T)$となります。

では、有限（無限）時間最適制御の目的はなんでしょうか？（2次形式の一般的な形で述べています。）

時間ステップT間（無限時間）のコストを最小にする入力を求めることです

\text{minimize} \ J = \sum_t x[k]Qx[k] + u[k]Ru[k]

（正確に書くと）

\text{minimize} \ J = \sum_{k=0}^{N-1} \left [x[k]Qx[k] + u[k]Ru[k] \right ] + x[N] S_f x[N]

（Nは予測ステップ数）
（ですが、強化学習によせて書いてます）

ここで、$ - r(x_t, u_t) = x[k]Qx[k] + u[k]Ru[k]$にしてしまいましょう。
最大をとるか、最小をとるかは、報酬で考えるか、コストで考えるかだけの違いなので、強化学習には期待値を取るということが残ることを除けば、これらが同じように見えることは確かではあります。

ベルマン（最適）方程式

2つの分野が近いことをベルマン方程式からも感じることができます。
ベルマン（最適）方程式は、強化学習側からの人からすると、

V^{\pi}(x_t) = \max [E_{\pi} [r_{t+1} + \gamma V^{\pi}(x_{t+1})] ]

ここで、$r$はreward（即時報酬）、$V$は状態価値、$\gamma$は割引率、$\pi$は方策（あるstate、$x$のもとでinput、$u$を吐き出すもの）です。
となることは、多くの本や記事が述べてくれています。この式はモデルがあり、かつテーブル型つまり、stateとactionの数が有限であれば、もともと仮定している状態価値$V$を使って、反復的に解くことができることは知られているわけです。sergey levineはbootstrap updateと言っていますね。もちろん、モデルがない場合でもデータ・ドリブンに価値を推定していくQ学習等が提案されていますね。

では続いて最適制御です。一般的な最適制御は決定論的なモデルを持っている（あくまで一般的です。）と仮定しています。
モデルは以下の離散モデルを想定しましょう。

\boldsymbol{x}[k]= \boldsymbol{Ax} [k]+ \boldsymbol{Bu}[k]

ここで最適制御のベルマン方程式は、

V(x, k) = \min \left (L(x[k], u[k]) + x[N] S_f x[N] +\sum_{l=k+1}^{N-1} L(x(l), u(l)) \right ) \\
L(x[k], u[k]) = x[k]Qx[k] + u[k]Ru[k]

になります。そして、この式をとけばいいわけです！（解析的に解くことができるのは限定されたケースのみです。）

目標と同じように、最大最小を考慮し、期待値を取ることを除けば強化学習と同じですね。
そしてなんとこのベルマン方程式から簡単に下記の離散リカッチ方程式が導出することができます。

S(k) = Q + A^TS(k+1)A - A^TS(k+1)B(R+B^TS(K+1)B)^{-1}B^TS(k+1)A

詳しい証明をしてもいいのですが、ここでは、省略します。

興味がある人は、大塚先生の非線形最適制御入門（コロナ社）を見てみてください。（連続系での導出ももちろんできます。）
強化学習の勉強したうえでこの本を読むとそのつながりが鮮明に見えてきます。

長い前提のまとめ

さて、なんとなく2つのつながりが見えてきたところで。
強化学習は確率的な要素を含んだ、最適制御と言えそうだなーというのは分かっていただけたかと思います。
最適制御は、action（入力）は連続にする代わりに、評価関数を二次系（凸）にかつモデルを決定論的なものを用意しています。
強化学習は、モデルがない代わりにデータ・ドリブンにしたり、確率的な方策を扱うためにactionを有限にしたりしているわけですね！

なので、一般的な最適制御は強化学習の中の1つで、特に決定論的なモデルがあり、評価関数も入力に関して凸のケースであると言えそうです。　（もちろん確率的最適制御なども提案されていますが、そこら辺まで行くと同じになりそうなのであくまで一般的な話です。）

今述べたのは基本的な2つの分野の考え方であって、それぞれ拡張する手法はもちろん、昔から提案されているとは思いますが、実はここ最近？、強化学習の中でもモデルベース強化学習（モデル（報酬と状態遷移）を使用する（または獲得して使用する）強化学習のことです。）の内容の論文を多く見かけます。
この前開かれた、ICLR2019のトレンドでも、モデルベース強化学習の内容の発表が多かったことが知られています。
（参考：piqcyさんのブログや元の記事)

モデル使うならほぼ、最適制御じゃないか！と思われる方も多いかと思いますが
強化学習でいうモデルベースは

\boldsymbol{x}[k+1]= \boldsymbol{Ax}[k] + \boldsymbol{Bu}[k] \\
\boldsymbol{\dot{x}}= \boldsymbol{Ax} + \boldsymbol{Bu}

や

\boldsymbol{\dot{x}}= f(\boldsymbol{x}, \boldsymbol{u})

を特定で指しているわけではなく、今のstate（ここでいうs）とaction（ここでいうu）から、次の状態遷移と貰える報酬がわかりそれを使うことを指しているわけなので、いわゆる数式的に述べることのできる、この形だけではありません。モンテカルロツリーサーチはその例ですし、さらに言えば、Dynaのようなモデルを獲得しながら、制御する手法も多く提案されています。

2つの分野を結ぶ研究は結構hotであるというわけです！

さて、大まかな関係がわかったところで、通常のDQNなどの、データ・ドリブンの強化学習と比較して、モデルを使うことのメリットはどこにあるのでしょうか？

モデルベースにすることのメリットはサンプル効率が非常に良くなることであると言われています。（piqcyさんのpythonで学ぶ強化学習）

これは直感的にもわかりますね。
モデルによって次の状態が決まるので、データを集めずに（自分自身が実際に動かなくても）自分の方策を更新することができるからです。

さらに言うと、モデルがあることで、方策の勾配を解析的に算出、または、方策を算出できるケースがあります。最適制御はまさに後者の例の1つですね。
解析的に求まれば、なんとなく学習が安定しそうなことは自明かと思います。

かなり前置きが長くなってしまいましたが、モデルベースにするメリットが大まかにわかったところで、この記事の本題に入りましょう。

この記事は、その最適制御の中でも、非線形モデル予測制御（上記の有限時間最適制御問題を繰り返し解法する手法）のプログラムの解き方と書き方について述べています。
高校数学がある方であれば、そこまで難しくなく、理解ができると思います。
勝手ながら、少しでも多くの最適制御、または強化学習好きな方が、異なる分野に興味を持ってくれたならいいなという思いです。

予備知識

Newton法とは

後で使うのでここで補足しておきます。
ここでいう、Newton法は$[f_1(x, y), f_2(x, y)]^T = \boldsymbol{0}$といった連立方程式を近似的に解く手法のことを指します。もちろん高次でも使えます。
たくさん記事があるとは思いますので導出はそちらにお任せして、ここでは式の形だけ述べておきます。
参考は、この辺でしょうか？

Newton法はヤコビ行列を用いますので、

F = \begin{bmatrix} \frac{\partial f_1(x, y)}{\partial x} \  \frac{\partial f_1(x, y)}{\partial x} \\
           \frac{\partial f_1(x, y)}{\partial x} \   \frac{\partial f_1(x, y)}{\partial x} \end{bmatrix}

Newton法の更新式は、

\begin{pmatrix}
x_{k+1} \\
y_{k+1}
\end{pmatrix}
= 
\begin{pmatrix}
x_{k}\\
y_{k}
\end{pmatrix}
- F^{-1} 
\begin{pmatrix}
f_1(x_{k}, y_{k}) \\
f_2(x_{k}, y_{k})
\end{pmatrix}

で差分が小さくなるまで$(x, k)$を更新していけば、近似的に解を求められることになります。

オイラーラグランジュ方程式とベルマン方程式の関係

このあとで使う、最適制御問題を解法するのに必要なオイラーラグランジュ方程式ですが、
さっきのベルマン方程式から導出ができることは知られています。
詳しい証明は他の本にお任せしますが、ここで言いたいのは、今回実装するオイラーラグランジュ方程式を使った解法は、
ベルマン方程式を解いていることになるということです。

最適制御的な目線でいうなら、オイラーラグランジュ方程式の導出には変分法を用いるのに対して、ハミルトニアン・ヤコビ・ベルマン方程式（さっきのベルマン方程式）には、動的計画法を用います。そして、それらの2つはもちろん同じ最適制御問題を解いていますので繋がるというわけです。

問題設定

前回と同様に、教科書（コロナ社）と同じにしましょう。
前回の記事からの引用です。
評価関数の入力に対する係数を少しだけ変えていますが、結果が綺麗になるように調整しただけです。

では，問題の定式化を行います．
ここでは非線形最適制御入門における教科書（コロナ社）の例題と同様にするために，以下とします．
状態方程式は

\begin{bmatrix}
\dot{x_1} \\
\dot{x_2} \\
\end{bmatrix}
= 
\begin{bmatrix}
x_2 \\
(1-x_1^2-x_2^2)x_2-x_1+u \\
\end{bmatrix}, 

|u| \leq 0.5

とし，評価関数

J = \frac{1}{2}(x_1^2(t+T)+x_2^2(t+T))+\int_{t}^{t+T}\frac{1}{2}(x_1^2+x_2^2+0.5 u^2)d\tau

とします．
ここで，入力に対する不等式制約を考慮するために，ダミー入力$v$を用いて，

u^2+v^2-0.5^2=0

という等式制約を導入して，さらに評価関数を補正します．

J = \frac{1}{2}(x_1^2(t+T)+x_2^2(t+T))+\int_{t}^{t+T}\frac{1}{2}(x_1^2+x_2^2+0.5u^2)-0.01vd\tau

また，等式制約や状態方程式の制約を含めたハミルトニアン$H$は以下のように計算することができます。

H = \frac{1}{2}(x_1^2+x_2^2+0.5u^2)-0.01v+\lambda_1x_2+\lambda_2((1-x_1^2-x_2^2)x_2-x_1+u)+\rho(u^2+v^2-0.5^2)

オイラーラグランジュ方程式の導出

かなりサボりで申し訳ないのですが、ここも前回の記事からの引用です。
オイラーラグランジュ方程式は、最適化問題におけるKKT条件を示したものになります。
また、先ほども述べたようにこれはベルマン方程式から導出できます。

まず，必要になるのは，以下の3つです
すべてハミルトニアンを偏微分したものになります

\frac{\partial H}{\partial \boldsymbol{x}}, \frac{\partial \varphi}{\partial \boldsymbol{x}}, \frac{\partial H}{\partial \boldsymbol{u}}

ここで，$\varphi$は終端状態$(t+T)$へのペナルティ関数，つまり，積分項の外，$\frac{1}{2}(x_1^2(t+T)+x_2^2(t+T))$になります．
実際計算してみると（ただの計算問題です）
なお，これからの入力はダミー入力を含んでいますので注意してください（ベクトル表記になっているのはそのためです）
算出する際もダミー入力は一生くっついてきます
ベクトルによる微分は検索していただくとたくさん説明がでてきますので，それにのっとると，

\frac{\partial H}{\partial \boldsymbol{x}} = 
\begin{bmatrix}
x_1 - 2 x_1 x_2 \lambda_2 - \lambda_2 \\ 
x_2 + \lambda_1 + (-3x_2^2-x_1^2+1)\lambda_2 \\
\end{bmatrix}\\
\frac{\partial \varphi}{\partial \boldsymbol{x}} =
\begin{bmatrix}
x_1 \\ 
x_2 \\
\end{bmatrix} \\
\frac{\partial H}{\partial \boldsymbol{u}} = 
\begin{bmatrix}
u + \lambda_2 + 2\rho u \\ 
-0.01+2\rho v \\
\end{bmatrix}
\\

これを計算したことで，最適性条件である以下の式たちを

\dot{\boldsymbol{x}} = f(\boldsymbol{x}, \boldsymbol{u}, t) , \boldsymbol{x}(t_0) = x_0 \\
\dot{\boldsymbol{\lambda}} = - \frac{\partial H}{\partial  \boldsymbol{u}}, \boldsymbol{\lambda}(t+T)=\frac{\partial \varphi}{\partial \boldsymbol{x}}(\boldsymbol{x}(t+T)) \\
\frac{\partial H}{\partial  \boldsymbol{u}} = 0 \\
C(\boldsymbol{x}, \boldsymbol{u}) = 0

すべて求めることができました．ここで$C$は等式拘束条件です．ここではダミー入力を含んだ，$u^2+v^2-0.5^2=0$の式になります．
また，上2つは，

状態方程式，随伴方程式

になります．
つまり，評価関数$J$を最小化する問題は，二点境界値問題に落とすことができたわけです．

しかし，これどのようにすれば解けるのでしょうか

解析的に解くのは難しいので，反復法で解きます．
基本的なアルゴリズムは以下の通りです．

1. まず，初期状態$\boldsymbol{x}$を取得する
2. 初期推定解$\boldsymbol{u}(t)$を適当に決定する
3. 1,推定解$\boldsymbol{u}(t)$を用いて，未来の状態と随伴状態を状態方程式，随伴方程式から求める
4. 最適性条件を満たすか確認
5. 推定解$\boldsymbol{u}(t)$を何らかの指針で改善
6. 3-を繰り返す

ここでいう最適性条件は，

\frac{\partial H}{\partial  \boldsymbol{u}} = 0 \\
C(\boldsymbol{x},  \boldsymbol{u}) = 0

の2つになります．
このやり方で，最適性条件を満たす推定解を得ることができれば，良いのですが．．．

何らかの指針

これが非常に難しいです．基本的な形であれば，評価関数の勾配を求めて更新する勾配法等で求めることができますが，ダミー入力，等式制約その他もろもろが入ってくるともはや単純な勾配法では困難です．

しかも，解析的に出すことははなからあきらめているのでここで，
一旦，制御量を離散化します

つまり，すべて差分方程式にするということです．状態方程式，随伴方程式，最適性条件等，すべてを離散化して差分方程式化します．
そうすることで，より簡便に近似解を得ることができるようになります．
例えば，10ステップとした場合は，10この入力（この場合，ダミー入力を入れると20個ですね）を求める問題になるわけです．

すべてを離散化した結果，こんな形で最適性行列を算出できます
もちろんこれらの変数はすべて，随伴方程式と状態方程式を満たした上での話です。

\boldsymbol{F} = 

\begin{bmatrix}
\frac{\partial H}{\partial u}(\boldsymbol{x}^*[0], \boldsymbol{u}^*[0], \boldsymbol{\lambda}^*[1]) \\
C(\boldsymbol{x}^*[0], \boldsymbol{u}^*[0], \boldsymbol{\lambda}^*[1]) \\
... \\
\frac{\partial H}{\partial u}(\boldsymbol{x}^*[N-1], \boldsymbol{u}^*[N-1], \boldsymbol{\lambda}^*[N]) \\
C(\boldsymbol{x}^*[N-1], \boldsymbol{u}^*[N-1], \boldsymbol{\lambda}^*[N]) 
\end{bmatrix}

さらに，$\boldsymbol{U}$を導入して，

\boldsymbol{U} = 

\begin{bmatrix}
\boldsymbol{u}^*[0] \\
\rho^*[0] \\
... \\
\boldsymbol{u}^*[N-1] \\
\rho^*[N-1] \\
\end{bmatrix}

とします．ここで注意点は，繰り返しになりますが，入力はダミー入力を含んでいるということと，
等式拘束条件の数に応じて，$\rho$のみとしているものは数が増えるということです

例えば，仮に離散化したステップを10とした場合今回でいうと

• 入力が1つ
• ダミー入力が1つ
• 等式制約条件が1つ（それに対応するラグランジュ乗数は1つ）

になりますので，この行列は30×1になります

さて、ここからが今回は異なります。ここで求めたいのは、

\boldsymbol{F}(\boldsymbol{U}) = \boldsymbol{0}

この最適性行列の連立方程式を満たす入力の時系列$\boldsymbol{U}$です。前回は、この入力の時系列$\boldsymbol{U}$を算出するために、CGMRES法を用いて最適性行列の勾配$\frac{\partial \boldsymbol{F}}{\partial \boldsymbol{U}}$を計算することなく、追跡していましたが、今回はこれをNewton法で求めていきます。

なので、さっきのNewton法の式を使うと、

\boldsymbol{F}_n = \boldsymbol{F}(\boldsymbol{U}_{n}) \\
\boldsymbol{U_{n+1}} = \boldsymbol{U_{n}} - (\frac{\partial \boldsymbol{F}}{\partial \boldsymbol{U}}(\boldsymbol{U}_{n}))^{-1}\boldsymbol{F}_n

ここでの$n$は反復回数を示すものです。

ただ、ここで一点問題なのは$\frac{\partial \boldsymbol{F}}{\partial \boldsymbol{U}}$です。
解析的に計算して求めてもいいのですが、ここでは数値微分をしてしまいしょう。
$\boldsymbol{U}$を変化させて、後は、それを変化分で割ればよいですね。ここだと30個の変数があるので、30回数値微分をするので少し時間がかかります。詳しくはプログラムを見てください。

では最後にアルゴリズムをまとめておきましょう

1. まず，初期状態$\boldsymbol{x}$を取得する
2. 初期推定解$\boldsymbol{u}(t)$を適当に決定する
3. 1,推定解$\boldsymbol{u}(t)$を用いて，未来の状態と随伴状態を状態方程式，随伴方程式から求める
4. 最適性条件を満たすか確認
5. 推定解$\boldsymbol{u}(t)$（$\boldsymbol{U}$）をニュートン法を用いて更新、算出
6. 3-を繰り返す

結果

forkか、cloneしてもらって、

$ python main_example.py

を実行してもらうと

が出てきます。制約条件を考慮できていますね！

具体的なプログラムについて

詳しいコードは

にのっていますがプログラムの書き方の概要を少しだけ。

• SampleSystemClass

シミュレータ（環境）です。
入力を入れると、状態を更新してくれます。

• NMPCSimulatorSystemClass

これはNMPC内で保持しておくシミュレータです。
状態の予測と随伴方程式の計算をしてくれます。
通常、制御対象と、そのNMPCで予測を行うモデルは異なることが多いため、ここは分けています。
（このプログラム内では、モデル化誤差等はのせてないので式は同じになってます。）

• NMPCControllerWithNewtonClass

NMPCのシミュレータを使用し、最適入力を計算します。

最後に

制御工学の最適制御分野の中手法の1つ、非線形モデル予測制御について強化学習との関連をまとめながら、述べさせていただきました。
最適制御の欠点は、モデルがなければなにもできないことです。この安定解析などがしっかりと行えるこの美しい理論もモデルがなければ使えません。
強化学習の欠点は、確率的なモデルを扱ったり、そもそもモデルがないものを扱ったり、するので基本的にはデータ・ドリブンになります。そのため、学習が安定しなかったり、そもそも学習し終わっても、システムの安定性（絶対にゴールにたどり着くことや最適性など）が言える例は少ないです。

それぞれ改善する手法は多く提案されていますが、ここで言いたいのは、強化学習も最適制御の1つですし、最適制御もまた、強化学習の1つであるように僕は感じています。
そこで、互いの手法の欠点を指摘しあうことも重要ではあると思いますが、互いにその欠点を補いながら、新しい革新的な手法が生まれることを期待しています。
僕自身も最適制御と強化学習、双方のアプローチを深く理解して、その2つの研究の間を埋められるようになれればなと勝手に思っています。

最後まで読んでくださった方ありがとうございました。

一応内容については精査したつもりではありますが、一個人が校閲などなしで書いていますのでもし間違い等あれば、コメントをいただけると嬉しいです。