皆様こんにちは．18のhiraです．
近年，自動運転やロケットやドローン，戦闘機でも「モデル予測制御(MPC)」なる制御方法が注目されているようです．
なんでもこのMPCを使うと運動モデルさえあれば誰でも簡単に（注 “誰でも”等の範囲は要検討）複雑なロボットを好きな軌道で動かすことができるようです!
この魅力的なMPCは，なんとMATLABの「Model Predictive Control Toolbox」を使うことでほぼGUI上で手軽に作ることができます．しかも作ったコードはC/C++に変換でき，それをビルドすればRaspberry Piといったマイコン上で動かすこともできます．
今回は簡単なバネ台車モデルを使い，MATLAB上でのMPCモデル及び運動軌道の作成，さらにC/C++のコードの生成とビルド方法を紹介します．
モデル予測制御の特徴
モデル予測制御の概要としては，今後数ステップの理想軌道に対して，自身の運動特性(モデル)が出すであろう出力を予測し，最も妥当な出力を計算するという制御です．加えてその際に「ハンドル角度まで，推進力は何Nまで」といった制約を扱え，多くの入出力を簡単に加えることができます．
実際の計算としては理想軌道と実際軌道の誤差をコスト関数として，制約付き最適化問題を解くことを数値計算で毎ステップガンガン繰り返し解いていきます．これまでは計算機の能力が豊富な大型プラントなどでしか使われていませんでしたが，近年の計算機の発展により小型・高速応答の組み込み機器にも使われだしたようです．数値解を求めるということなので，方程式の唯一絶対の解とは必ずしも一致しません．しかしこれにより経路を方程式で記述する必要がなく，数値的な座標で与えてやればよいので，任意の経路を簡単に扱えるという利点があります．
そこで今回はこの特性を活かすべくMATLABのMPCを使って，
- ロボットのモデルを自作して使用する
- 任意の軌道に沿って移動させる
- 制約を与えて制御する
ことをやってみます．
1. Simulinkでモデルの雛形を作る
はじめにSimulink上でMPCを導入したモデルのブロック図を作ります．このブロック図の中身のモデルなどは次章以降で作成します．
まずSimulinkのBlank Model(最初から)を選択してまっさらなボードを表示します．これに左上のLibrary Browserから「MPC Controller」，「State-Space」，「From Workspace」，「Scope」を1つずつ追加し図のように接続します．
これらの役割としては，MPC Controllerは今回の中核となる軌道に対するモデルの振る舞いの予測と最適化後の実行値の出力を行います．次章以降でこのMPCが持つ内部モデル(状態方程式)の作成や各パラメータの調整を行います．なお初期時には「md」という端子がありますが，今回は使いません．残していても構いませんが，ダブルクリックしてmeasured disturbanceのチェックを外せば消すことができます．
State-Spaceが入力に対する実際のロボットの振る舞いを計算します．注意して頂くこととしては，ここには本来実際のロボットの入出力ブロックを接続します．今回は簡単のためMPCの内部モデルと同じ入出力(状態方程式)をコピペします（理想モデル予測に理想システムをつないでいるのだから当然システム出力も予測と一致する）が，本来は何かしらの方法で作成した実際のロボットの入出力値をつなぎます．例えばここは状態方程式ではなく本来は各種センサの値から推定した位置や速度だったり，外乱が入ったモデルだったりします．予測用の内部モデルと混同しないようにご注意ください．
From Workspaceが軌道の位置座標の配列で，この軌道に追従するようにロボットを動かします．名前の通り，この軌道を別に作成し，MATLABのワークスペースに保存しておき，ここで読み取ります．
Scopeはデバッグ用の出力監視用のグラフのプロットです．制御システム自体にはなんの寄与もしません．refとロボット出力の2本につながっているのは， 1 つのグラフ画面にrefとロボット出力の 2 つのグラフを一緒にプロットするためです．Scopeをコピーしてmvの線の出力を監視しても良いでしょう．
配線ができたらモデルを保存します．ファイル名は「mpc_tutorial.slx」とでもしておきます．このファイル名は後ほどモデルの呼び出しに使用するので使いやすい名前にしておくと良いでしょう．保存したら一旦閉じます．
2. コマンドラインでのMPC内部モデルの作成
続いてSimulinkのMPC Controllerブロックに与えるロボットの内部モデルを作成し，MPCに与えます．このモデルの定義をmファイルで行います．今回はバネでつながった車を引っ張り目標の位置（時間変化する）に移動させるシステムを考えます．
外力としてバネの力と床の摩擦抵抗があります．この車を力 F で引っ張って目標の位置 x に移動させます．今回制御入力としては F のみを作用させ，MPCはある時刻の目標位置 x(t) に追従するように最適な力 F を計算してシステムに送ります．
このシステムをMPCに落とし込むのに必要なのが状態方程式(state space)です．このバネモデルの場合は次のような式となります
\begin{aligned} \bold{x}&=\left(\begin{matrix}x\\v\end{matrix}\right)\\\frac{d}{dt}\left(\begin{matrix}x\\v\end{matrix}\right)&=\left(\begin{matrix}0&1\\-\frac{k}{m}&-\frac{c}{m}\end{matrix}\right)\left(\begin{matrix}x\\v\end{matrix}\right)+\left(\begin{matrix}0\\\frac{1}{m}\end{matrix}\right)F\\x&=\left(\begin{matrix}1&0\end{matrix}\right)\left(\begin{matrix}x\\v\end{matrix}\right)+(0)F\end{aligned}このように \bold{x} の微分と， \bold{x} から求まる新たな軌道値 x の 2 式で構成されるのが状態方程式です．たぶん．僕は制御の人ではないのでよく知りませんが．今回制御入力は F のみ，更新出力は x のみの両方共 1 要素だけですが，多要素のベクトルを入出力にすることも可能です．
この状態方程式をMPCの内部モデルに組み込みます．MPCの内部モデル設計用のmファイルとして，mpc_tutorial_params.m（名前は任意）を作成します．
ファイルに以下のように記述します．（ブログでのコードの上手い挿れ方が分からなかったためスクショにしています）
m ， c ， k にそれぞれ値を入れます．また今回のMPCではサンプリングレート T_s を 0.1s とします．図中の A ， B ， C ， D はssコマンドによる状態方程式の作成を意味し，
\begin{aligned} \dot{\bold{x}} &= \bold{A}\bold{x}+\bold{B}\bold{u}\\ \bold{y}&=\bold{C}\bold{x}+\bold{D}\bold{u} \end{aligned}に対応しています． \bold{u} は制御入力を意味し今回のバネモデルでは F (と定数のベクトル)に対応します． \bold{y} はシステムの出力で今回は位置 x です．ssコマンドによりこれら4要素が前述のバネモデルの状態空間として統一されます．これをc2d(continuous to discrete)コマンドで数値解析用の離散数学モデルに直します．この時サンプリングレート T_s で離散化します．
離散化したモデルplantdをMPCオブジェクトに与えます．これでSimulinkのMPC controllerに内部モデルが与えられました．open_systemで先程作成したSimulinkモデルを開きます．
3. 通らせたい軌道データの作成
ちょっと順番が前後するような気もするのですが，ここで軌道の作成を行います．今回のバネモデルではある時刻における車の位置 x の配列を定義します．
新しいmファイルを作成します．このmファイルで任意の位置座標 x の配列を作り，MPCが読み込む軌道データ配列にします．今回はテキトーな波っぽい軌道としてみました．また各xの要素に対応する時刻 t を配列tとしました．
図のグラフのように 0\mathrm{s} ~ 10.0\mathrm{s} までの時刻 t における車の位置 x を配列xとします．サンプリング周期はMPCモデルと同じ 0.1\mathrm{s} 周期とします．このためxおよびtは 101 個の要素の配列( 1\mathrm{x}101 の行ベクトル)となりました．
これをMPCが読み込む時系列位置データとするためには， t と組にして行列にする必要があります． 2\mathrm{x}101 配列refを作り，refの 1 行目をtに， 2 行目をxにします．ワークスペース中のこの行列refをMPCが読み込みます．
ワークスペースから消さないようにご注意ください．
4. SimulinkでMPCの実装
再びSimulinkモデルに戻ります．mpc_tutorial_params.mを実行すると 1 章で作成されたSimulinkモデルが開きます．開かれたモデルにはコマンドラインで定義した m などの各パラメータの値やMPCのモデルが組み込まれています．
ここで 2 ， 3 章に合わせ一部を変更します．まずFrom Workspaceをsiminから「ref」に変更します．これによりワークスペースにある行列refがMPCに読み込まれます．
また実際のロボットモデルであるState-Spaceの状態方程式にバネモデルの状態方程式を記述します．前述の通り，今回は外乱があったりセンサにより決まる実際のモデルではなく，簡単のため理想モデルとして状態方程式を用います．
State-Spaceブロックをダブルクリックすると状態方程式を記述するウィンドウが表示されます．ここに先程mファイルで記述したようにバネモデルの A ， B ， C ， D をそのまま書き写します．
refとState-Spaceの変更が完了したらいよいよMPCを形にします!MPC Controllerをダブルクリックするとウィンドウが表示されます．作成した「mpcobj」が読み込まれていると思います．この横の「Design」ボタンをクリックすると「MPC Designer」というウィンドウが開きます．ここでMPCの各パラメータを調整し，追従性や制約を調整します．
注: この後の図で「ref」であるべきブロックが「ref.mat」になっていますが「ref」が正しいです．refに読み替えてください．ちなみに最初はmatファイルのタイムシリーズオブジェクトとしてref.matを定義していたのですが，Cコード化の際にエラーが出たためワークスペース行列refに修正しました．
MPC Designerは次のような画面となっています．中央左のグラフはMPCの出力mv( =F )を表していて，その右のグラフは内部モデルのシステムの予測出力を 1 に収束させる場合の応答性を表しています．
左上から「Tuning」タブに遷移すると，各種チューニング用パラメータが表示されます．このツマミや数値をいじくって最適な応答性をもつパラメータに設定します．また制約もここで加えます．
まずは制約なしで応答性を速くしてみます．「Closed-Loop Performance」のツマミを「Aggressive」側にずらすとより急激に追従しようとします．
これによりMPCの出力 F も急激に増加します．今回はその最大値は約 38\mathrm{N} です．最小値は約 -13\mathrm{N} で，これは 13\mathrm{N} の力で「押し込んでいる」ということを意味します．
この力 F の上限，下限の制約を設定します．今回は最大 10\mathrm{N} ，最小 0\mathrm{N} とし，「押し込み」はしないと設定します．
「Constraints(制約)」ボタンをクリックすると制約を設定するウィンドウが表示されます．今回入力変数は u(1) の 1 つだけで，これが F に対応します．これの「Min」を 0\mathrm{N} に，「Max」を 10\mathrm{N} に設定します．OKを押すと 2 つのグラフが描き変わり，これまで 40\mathrm{N} だったりした出力 F が 0\mathrm{N}から10\mathrm{N} の範囲内に収まっているのが確認できます．このように制約やチューニング結果がリアルタイムに反映されるのは嬉しい点です．
グラフを見ながら他の制約や応答性，ホライズンも調整します．ちなみにホライズンとは何ステップ先まで予測するかを意味します．上のPrediction Horizonが何ステップ先までを予測するかの値で，サンプリング周期が 0.1s でPrediciton Horizonが 10 なら 1s 先まで予測しています．下のControl HoraizonがPrediction Horizonのうち最初の何ステップの間出力mvを変動させるかを意味します．Control Horizonの後はその最後の出力mvの値が残りのホライズンステップ間持続するとして，システム予測出力の計算に使用されます．
一般にホライズンが長い方が精度良く制御できますが，計算量が大きくなってしまうので良い塩梅に調整する必要があります．
さて，チューニングができたらこれを実機モデルに落とし込みます．作成したref.matの軌道に合わせロボットモデル（といっても理想モデルなのでほぼ同じですが）が上手く動くかをシミュレーションします．
右上の「Update and Simulate」を押し，「Update Block and Run Simulation」を押すと調整したMPCモデルを使ってSimulinkのモデルが実行されます．これが正しく軌道を追従していれば成功です．
ボタンを押したらSimulinkの画面に移ります．Scopeの画面をダブルクリックすると実行結果がプロットされています．今回，ref.matの配列である目標軌道は青色，MPCシステムによる実際の出力は黄色でプロットされます．
表示されたプロットのシステム出力（黃）が目標軌道（青）を追従していれば成功です！ちなみに応答性を遅くしてみると右上のようなだいぶ時間差のある反応を示します．またホライズンを増やすとより目標軌道に近い（重なろうとしている）軌道が出力されているのが分かります．
完成したMPCはファイルに保存しましょう．先程シミュレーションをしたUpdate and Simulateから「Generate Script」を選びます．新たなウィンドウが表示され，「scenario1」をチェックします．
これにより調整後のMPCの各パラメータを出力するmファイルが新たに作成されます．このファイルを実行してMPCモデルを開くと，調整後のMPCパラメータがMPCモデルに反映されます．
5. C/C++コードを生成してビルドする
せっかく作ったMPCでも，実際のロボットで使えなければ嬉しくありません．そのためにはC/C++のコードにして，ビルドできる必要があります．
そこでSimulinkモデルをコードに変換します．これを行うのが「Simulink Coder」及び「Embedded Coder」のツールボックスです．予めこれをMATALBにインストールしておきます．
注: 以下の解説では別のモデルを使用してしますがやることは同じです
MATLABからMPCのコード生成
CコードのCMAKEによるビルド
まずはじめに生成されたCコードの確認と，デバッグ用のprintfを追加します．ert_main.cがmain関数ファイルに相当し， 1 つだけやたら大きいファイルmpc_tutorial.cにMPC関数などが記述されています．これを開くと， 1676 行目以降に定義した行列refのtとxの配列があるのが分かります．
それでは実行用に少しコードを修正します．ert_main.cを開きます．このままだと延々とプログラムが実行され続けるので，適当なところで中断します．カウンタを設け，100ステップで終了します．
またこのままでは何も表示されないので各ステップごとに t と x の値を表示するようにします．mpc_tutorial.cの 1560 付近にprintf()を追加して各値を表示します．ここではmpc_tutorial_DW.last_mv_DSTATEが u に，mpc_tutorial_B.xk1[0]が x に相当します．
最後にCコードのあるフォルダ内にCMakeLists.txtを作成します．生成されたCコードをまとめれば十分です．またMATLABのインストール時に作成されるファイルも使用するためinclude先を追加するか，ファイルをそのままCコードのフォルダにコピーします．
buildディレクトリを作成して移動し，cmake .. ，make の順にコマンドを入力してビルドします．ビルドが成功したら ./実行ファイル で実行します．
実行すると以下のような数値の羅列が出てきます．左列が u(=F) ，右列が x です．この数値はSimulinkでプロットした各 t における F と x の値と同じです！Cコードのビルドでも自作したロボットモデルをMPCで動かすことができました！
おしまい
自作のロボットモデルと軌道を使いMPCを作り，Cで走らせることができました．Cコード面倒でヤダーという人も最先端のロボットが簡単に作れるかもしれません．
さて，そのMPCに必要な状態方程式はどのように立てましょう？ぼくはコンピュータ工学が専攻なので分かりません．唯一思いつくのは制御の人に土下座して頼むことくらいです．おまえべんきょうしろよ