モデル予測制御(MPC) step2 〜Vertical Landing Control

はじめに

先日、映画「ドリーム（Hidden Figures）」のテレビ放送があり録画して見ました。NASAのロケット開発での黒人女性の活躍を描いています。以下ネタバレあり。

主人公の一人 キャサリン・ジョンソン が、打ち上げ → 周回 → 再突入までの軌道を解析的に手計算したり、解析的に計算できないところは、オイラー法（オイラー法かよ！）を使う提案するとか、もう一人の主人公の ドロシー・ヴォーン がFortranを独学で習得し、当時NASAに導入されたばかりのIBM System/360で数値計算したり、更に黒人女性の計算チーム全員をプログラマーへと転身させたり、どれも史実らしく大変面白かった。

ということで、今回は　モデル予測制御（MPC）STEP2として、ロケットの垂直着陸制御（Vertical Landing Control）について書いてみます。

とりあえず、こんな感じです。。。

ダウンロード

このブログで説明しているEXCELファイル（⑭MPC_step2.xlsm）とQPソルバー（QpSolver.dll ）は、こちら → リンク からダウンロードできます。 QPソルバーのソースコード（QpSolver.cpp）も参考として置いています。

QpSolver.dll は、32bit版EXCEL用と64bit版EXCEL用で別フォルダになっています。（最近のEXCELは、ほとんど64bit版なので、今回は64bit版も作りました。）

EXCELとQPソルバー（QpSolver.dll ）を利用するには、こちら → リンク を参考に、EXCELファイルのVBAの module1 の先頭に QpSolver.dll を置いたフォルダのフルパスを設定する必要があります。

 【注意：STEP1のDLLにはバグ（制約条件が無効になる）がありました！】
QPソルバーをDLL化する追加コード部分のバグで、EXCELの制約条件をDLLが正しく受け取れていなかったため制約条件が無効になっていました。（STEP1→リンク）
今回のSTEP2のDLL（32bit版と64bit版）では、そのコードを修正しています。

また、 STEP1でも説明したとおり、QPソルバーは、岩波書店出版の「FORTRAN 77 最適化プログラミング」に掲載のFORTRANのコードをCに変換したものです。ブログに掲載するにあたり、著者の福島雅夫先生と岩波書店様に御承諾をいただいています。

シミュレーションのシナリオ

ロケットが高度 \(3500[m]\) から垂直に \(200[m/s]\) の速度で降下しているところから始まります。グリッドフィンで減速しながら降下している状態です。

高度 \(2500[m]\) でロケットのエンジンを始動します。ここからが垂直着陸制御になります。

降下の初期位置は、目標位置に対し水平方向に \(400[m]\) ずれています。降下速度を落としながら、ロケットの機体を水平方向に \(400[m]\) 移動する必要があります。

ロケットに対して、各時刻での目標高度と目標位置をのみを指示します。
目標高度は、降下速度を \(8.0[m/s^2]\) で減速して、25秒で \(2500[m]\) を降下するように指示します。横方向には ± \(4.0[m/s^2]\) の加減速度で \(400[m]\) を横移動するように指示します。指示通りに機体を制御できれば、エンジンを始動から25秒後に着陸できるはずです。

ロケットは、指示された高度と水平位置を目標として、モデル予測制御（MPC）の制御ロジックを使ってエンジン推力とジンバル角の制御値を算出します。ロケットの姿勢制御は、機体の重心位置や慣性モーメントを考慮しながらエンジン推力とジンバル角を制御する複雑な制御対象ですが、ソルバーが数値演算的アプローチで適切な制御値を算出できるかがポイントです。

制御設計プロセス

今回は、ロケットの垂直着陸制御（Vertical Landing Control）を対象としていますが、モデル予測制御（MPC）の設計プロセス、制御ロジックは、基本的にSTEP1（→リンク）と同じです。

1. ロケットのモデルを作成し離散化
2. 予測ホライズン区間（今回も4制御周期）をまとめた状態方程式を作る
3. 制御システムの評価関数（目的関数）を定義
4. QPソルバーに設定し最適解（制御入力）を算出
5. 制御周期ごとに算出した最適解（制御入力）をシステムに入力して機体を制御

ロケットのモデル化

今回は、制御対象がロケットなので、最初にロケットのモデルについて説明します。ロケットの仕様及びシミュレーションのパラメータは以下の通りです。

モデル化の前提として、2次元、空気抵抗なし、ロケットの重心は固定の条件とします。
本来は、燃料消費による機体重心の移動や複雑な空気抵抗の変化が難しいところですが、STEP１と同様に、モデル予測制御の基本的な制御設計プロセスの確認が目的なので、簡略化したモデルにします。

水平方向(p)と垂直方向(z)の運動方程式

​推力 \( T\) の作用方向は鉛直軸に対して角度 \(\theta + \delta\) になります。
\[M \ddot{p} = T \sin(\theta + \delta)\] \[M \ddot{z} = T \cos(\theta + \delta) - Mg\]

回転方向（ピッチ方向）の運動方程式

​ジンバル角 \(\delta\) によって生じる重心まわりのモーメントは、距離 \(L_g\) とジンバル角 \(\delta\) に対する推力 \( T\) ​の垂直成分によって発生します。 \[I_r \ddot{\theta} = -T L_g \sin\delta\]

線形化

​運動方程式には \(\sin\) や \(\cos\) の非線形項が含まれているため、以下のように微小角の線形近似を行います。
\[\sin(\theta + \delta) \approx \theta + \delta\] \[\cos(\theta + \delta) \approx 1\] \[\sin\delta \approx \delta\]

線形近似を運動方程式に適用すると、次のようになります。
\[M \ddot{p} \approx T \theta + T \delta\] \[M \ddot{z} \approx T- Mg\] \[I_r \ddot{\theta} \approx -T L_g \delta\]

今回は、ジンバル角   \(\delta\)  と推力 \(T\) を制御入力 にします。
\[u = \begin{bmatrix} \delta \\ \Delta T \end{bmatrix} \]
ですが、状態方程式に \( T \delta \) の非線形項が含まれているため、更に線形化の対応が必要です。その対応としては、推力 \(T\) を基準となる平衡状態 \(T_0\) と変分 \( \Delta T\) として扱います。
\[T=T_0+\Delta T \hspace{1cm} (T_0=Mg)\]
運動方程式に適用。微小項の無視と、 \( T_0=Mg\) より
\[M \ddot{p} \approx (T_0 + \Delta T)(\theta + \delta) = T_0 \theta + T_0 \delta + \underbrace{\Delta T \theta + \Delta T \delta}_{\text{微小項}}\] \[\Rightarrow \ddot{p} \approx \frac{T_0}{M}\theta + \frac{T_0}{M}\delta = g\theta + g\delta \]
\[M \ddot{z} \approx (T_0 + \Delta T) - Mg = (Mg + \Delta T) - Mg = \Delta T\] \[\Rightarrow \ddot{z} \approx \frac{1}{M}\Delta T\]
\[I_y \ddot{\theta} \approx -(T_0 + \Delta T) L_g \delta \approx -T_0 L_g \delta = -Mg L_g \delta\] \[\Rightarrow \ddot{\theta} \approx -\frac{Mg L_g}{I_y}\delta\]

状態方程式にまとめる

状態ベクトルを \(x = \begin{bmatrix} p & \dot{p} & z & \dot{z} & \theta & \dot{\theta} \end{bmatrix}^T\) 、制御入力を \(u = \begin{bmatrix} \delta & \Delta T \end{bmatrix}^T\) とすると、次のような 2入力の状態方程式になります。 \[\begin{bmatrix} \dot{p} \\ \ddot{p} \\ \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & g & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} p \\ \dot{p} \\ z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ g & 0 \\ 0 & 0 \\ 0 & \frac{1}{M} \\ 0 & 0 \\ -\frac{Mg L_g}{I_y} & 0 \end{bmatrix} \begin{bmatrix} \delta \\ \Delta T \end{bmatrix}\]

状態方程式の離散化

この状態方程式をEXCELのシート【状態方程式】で、これまでと同じように離散化して逐次式の \(P\) と \(Q\) を求めます。離散化方法については、こちら →リンク で説明しています。

モデル予測制御（MPC）の予測ホライズン区間をまとめた状態方程式を作る

モデル予測制御（MPC）では、未来のシステムの振る舞いを予測する予測ホライズン区間を数理計画問題として扱い、数値演算的に最適制御入力を決定します。

STEP1（→リンク）でも説明していますが、予測ホライズン区間の逐次式をまとめると下記になります。

\[\begin{bmatrix} x(1) \\ x(2) \\ \vdots \\ x(n) \\  \end{bmatrix}=\begin{bmatrix} P \\ P^2 \\ \vdots \\ P^n \\  \end{bmatrix} x(0) +\begin{bmatrix} Q & 0 & \cdots & 0 \\ PQ & Q & 0 \\ \vdots & \vdots & \vdots \\P^{n-1}Q & P^{n-2}Q & \cdots & Q \end{bmatrix} \begin{bmatrix} u(0) \\ u(1) \\ \vdots \\ u(n-1) \\  \end{bmatrix} \]

この式を \(X=Fx(0)+WU\) とします。今回も基本的な制御設計プロセスの確認が目的なので、予測ホライズンは4制御周期（n=4）としています。
\(X\) はロケットの状態変数 \[x(n)=\begin{bmatrix}p(n)& Vp(n)&z(n)&Vz(n)&\theta(n)&\omega(n)\end{bmatrix}^T \] を4制御周期分まとめたベクトル。
\(U\)  は制御入力 \[u(n)=\begin{bmatrix}\delta(n)&  \Delta T(n)\end{bmatrix} ^T\] \(\delta\) ：ジンバル角、 \( \Delta T \) ：推力 を４制御周期分まとめたベクトルです。

EXCELのシート【MPC】で \(F\) と \(W\) を計算しています。

出力方程式は
\[\begin{bmatrix} y(1) \\ y(2) \\ \vdots \\ y(4) \\  \end{bmatrix}=\begin{bmatrix} C & 0 & \cdots & 0 \\ 0 & C & 0 \\ \vdots & \vdots & \vdots \\ 0 & 0 & \cdots & C \end{bmatrix} \begin{bmatrix} x(1) \\ x(2) \\ \vdots \\ x(4) \\  \end{bmatrix} \] \[x(n)=\begin{bmatrix}p(n)& Vp(n)&z(n)&Vz(n)&\theta(n)&\omega(n)\end{bmatrix}^T \] \[y(n)=\begin{bmatrix}p(n) & z(n)\end{bmatrix}^T\] この式を \(Y=CX\) とします。 \( Y \) はこのシステムの出力で、ロケットの状態変数の中から、水平位置 \(p\) と高度 \(z\) を抽出したベクトルにしています。詳細は、EXCELのシート【MPC】で確認をお願いします。

システム制御の評価関数（目的関数）を定義

制御工学的には評価関数ですが、数理計画問題（ソルバー）的には目的関数です。
予測ホライズン区間の4制御周期において、下記①と②が制御性の評価の要素になります。

① ロケットに指示している目標水平位置と目標高度のベクトル \(Y_{ref}\) と、実際の高度と水平位置のベクトル  \( Y \)  の偏差

② 制御入力ベクトル \(U\) に含まれる ジンバル角  \(\delta\) と推力 \( \Delta T \) の大きさ

上記の各項に重み付け（ \(Q, R\) ）して2乗和にした評価関数 \(J\) は次の式で表されます。
\[J=(Y-Y_{ref})^T Q (Y-Y_{ref})+(U-U_{ref})^T R (U-U_{ref})\] \(Y_{ref}\) ：ロケットに指示している目標水平位置 \(p\) と目標高度 \(z\) のベクトル 
\(U_{ref}\) ：ジンバル角  \(\delta\) と推力 \( \Delta T \) の目標値ですが、最小制御入力が目標なので 0 
重み付け係数 \(Q, R\) ：異なる物理量 [rad] [kN] [m] を2乗和して評価関数にするための調整係数

この評価関数 \(J\) が最小であれば、目標高度と目標水平位置との偏差を最小にできる最小の制御入力 \(U\) （ジンバル角  \(\delta\) と推力 \( \Delta T \) ）となり、最適な制御入力の解が求まるはずです。

QPソルバーで最適解（制御入力）を算出

QPソルバーは2次計画問題の最適解を算出します。今回はそのQPソルバーをDLL化してEXCELから利用します。

2次計画問題

2次計画問題（QPソルバー）では

目的関数：　 \(c^Tx+\frac{1}{2}x^TGx\) 　→　最小
制約条件：　 \({a_i}^Tx=b_i\) 　 \(i=1,2,\cdots,m_e\)
　　　　　　 \({a_i}^Tx \geqq b_i\) 　 \(i=m_e+1,\cdots,m\)

を満たすベクトル  \(x\)  を求めます。
EXCEL VBAからQPソルバーを以下のように呼び出しています。

ソルバーのソースコードは、岩波書店出版の「FORTRAN 77 最適化プログラミング」に掲載のFORTRANのコードをCに変換したものです。ブログに掲載するにあたり、著者の福島雅夫先生と岩波書店様に御承諾をいただきました。2次計画問題(QP)ソルバーについては、「FORTRAN 77 最適化プログラミング」の4章に詳細な説明がありますので、そちらでご確認いただきたいです。大きな図書館には所蔵されていたりします。

制御問題の適用

QPソルバーで制御入力の最適解を求めたいわけです。
ロケット制御の評価関数 \(J\) ですが \[Y=CX=C(Fx(0)+WU)\] を代入して整理すると \[J(U)=U^T(W^TC^TQCW+R)U+2((CFx(0)-Y_{ref})^TQCW-U_{ref}^TR)U+定数項 \] となり、評価関数 \(J\) は \(U\) の2次式になります。 \(U\) は制御入力 \(u(n)=\begin{bmatrix}\delta(n)&  \Delta T(n)\end{bmatrix} ^T\) （ \(\delta\) はジンバル角、 \( \Delta T \) は推力）を４制御周期分をまとめたベクトルでした。

\(U\) の2次の係数行列 \[(W^TC^TQCW+R)\] と、 \(U\) の1次の係数行列 \[2((CFx(0)-Y_{ref})^TQCW-U_{ref}^TR)\] は、EXCELのシート MPC で計算しています。

目的関数の \(G\) は評価関数 \(J\)  の2次の係数行列（2次計画問題は2次の係数に 1/2 が付く前提のため2倍します） \[2(W^TC^TQCW+R)\] \(C\) は評価関数 \(J\)  の1次の係数行列 \[2((CFx(0)-Y_{ref})^TQCW-U_{ref}^TR)\] になります。

更に数理計画問題には制約条件式も設定します。制約条件式は目的変数（最適解の対象変数）について設定します。評価関数 \(J\) では、制御入力 \(U\) （ジンバル角  \(\delta\) と推力 \( \Delta T \) ）が目的変数になります。今回は、ジンバル角  \(\delta\) に対して角度制限 ±15.0 [deg] を設定しています。

制約条件は \(\geqq\) で記述するため

  \( -\delta[n] \geqq -15.0[deg] \) 　（  \( \delta[n] \leqq 15.0[deg]\)   ）
  \( \delta[n] \geqq -15.0[deg]\)

を制約条件式 \[AU=B\] にまとめます。

評価関数 \(J\) の2次の係数行列 \(G\) と1次の係数行列 \(C\) 、制約条件式 \(A, B\) を QPソルバー に制御サイクル毎に渡すことで、制御入力 \(U\) （ジンバル角  \(\delta\) と推力 \( \Delta T \) ）の最適解が求まります。

シミュレーション結果

ロケットが高度 \(2500[m]\) で垂直に \(200[m/s]\) の速度で降下しているところでロケットのエンジンを始動します。ここからが垂直着陸制御になります。

高度、降下速度、水平位置、水平移動速度の制御ですが、ともにOKそうです。
着地時の降下速度 -1.83[m/s] は減速不足か？とも思いましたが、SpaceXの着地速度は -5～6[m/s]のようで良しとしました。

機体の角度 θ や角速度 ω を抑えつつ、降下速度や水平移動速度を制御して、目標地点に着陸するのは相当に難易度高そうです。ジンバル角制御のノウハウなどの制御ロジックは一切組み込んでいないにもかかわらず、制御できてしまっています。すばらしい！

QPソルバーのバグ（QPソルバーをDLL化する追加コード部分のバグで、EXCELの制約条件をDLLが正しく受け取れていなかったため制約条件が無効になってしまう）は修正し、今回の制約条件である ジンバル角 ±15.0[deg] 内の条件で機体制御ができています。

数理最適化（QPソルバー）で本当に制御できてしまうのは何とも不思議です。制御ロジックの中にまで数値解析的な手法が導入されていることに、キャサリン・ジョンソン さんやドロシー・ヴォーンさんはどのように感じるでしょうか。

尚、EXCELシート【VLC】では、シミュレーション結果をアニメーションで確認できます。モデルのパラメータを変更する場合は、EXCELのシート【シミュレーション】の上部の表で変更してください。モデルのパラメータを変更して再計算する場合は【MPC演算開始】ボタンを押してください。

慣性モーメント \(I_r\) を 1.00E+04 [kg m2] に変更すると ジンバル角 ±5.0[deg]  でも着陸ができます。慣性モーメント \(I_r\) が 1.00E+05 [kg m2] のままでは機体制御ができず、目標地点に移動できないまま墜落してしまうのもアニメーションで確認できます。

簡単なアニメーションですが、同じモデルを使って人の操縦による着陸ゲームもできそうです。・・・が、おそらく人が操縦するには難易度が高すぎるのではないかと思います。LQRでの位置決め制御（→リンク）ですら手動操作は困難だったので。難しい制御は、制御器に任せた方が良さそうです。

最後までお読みいただきありがとうございました。