皆様こんにちは．18のhiraです．
自動車やドローン，ロケットや工場プラントなど様々な分野での応用が期待され，MATLABでも専用パッケージが提供されているのがモデル予測制御(MPC)です．従来は線形モデルが中心でしたが，最近開発が進む非線形のモデルを用いることでより多くのシステムに適用することができます．
今回はそんな非線形MPCを用いて，カッコよく自動車の経路追従をしてみます．
(動画)
この例のMATLABコードはこちらのgithubをご覧ください．
またMATLABによる(線形)モデル予測制御の作成とチューニング，シミュレーション，Cコード化の最初の一歩となるようなチュートリアルは，ぜひ過去の記事をご覧ください．
1. 非線形MPCとは
MPCはシステムの数理モデルを持っておき，ある入力 \bf u をシステムに与えた時の出力 \bf{y} を予測し，その予測結果が目標とする理想の出力軌道である「参照軌道」に近づくよう入力を最適化する制御です．この時用いられるシステムの数理モデルが非線形なのが今回の例です．
数理モデルが線形であるとは，例えば次のようなシステムです．
\begin{aligned} \bf{y}&=A\bf{x}+B\bf{u} \\ \begin{bmatrix}y_1\\y_2\end{bmatrix} &= \begin{bmatrix}a_{11} & a_{12}\\ 0 & a_{22}\end{bmatrix} \begin{bmatrix}x_1\\x_2\end{bmatrix} + \begin{bmatrix}0 & b_{12}\\ b_{21} & 0\end{bmatrix} \begin{bmatrix}u_1\\u_2\end{bmatrix}\\ &= \begin{bmatrix}a_{11}x_1+a_{12}x_2+b_{12}u_2\\ a_{22}x_2+b_{21}u_1\end{bmatrix}\end{aligned}
この時 a_{ij} ， b_{ij} はどれも定数とします．2行目のように定数行列 A ， B に状態 \bf{x} と入力 \bf{u} を単純に掛けているだけです．
これに対し非線形であるとは次のようなシステムです．
\begin{aligned} \begin{bmatrix}y_1\\y_2\end{bmatrix} &= \begin{bmatrix}x_1 u_1\\ x_1x_2+u_2\end{bmatrix}\end{aligned}
この非線形モデルは，線形モデルの2行目のような定数行列と状態/入力ベクトルの単純な積として書くことが出来ません．このモデルを(無理やり?)線形に近似する方法として，行列 A を定数に限らず x_1 などを含ませ，適宜 A の中の x_i をその時の値に更新していくという方法があります．実際MATLABでもこのような線形近似MPCが有りますが，できれば非線形のままで解きたいのが人情です．
今回はこの非線形モデルを用いて車モデルを記述します．この非線形モデルをMATLABの非線形MPCコントローラで作成します．
MATLABによる非線形MPCの作成
使用する非線形車両モデル
車の運動モデルには色々な種類があります．路面との摩擦を考慮したり，4駆かどうか，重心はどこかなど様々なバリエーションがありますが，今回は最も単純な3状態モデルを使用します．モデルは次の図の通りです．
この車モデルでは，後輪タイヤを中心とする位置2次元座標 x ， y ，と x 軸に対する車の角度 \theta を状態 \bf x とします．操縦者は速度 v とハンドル角度 \delta を入力 \bf u として与えます．そして今回の自動運転の目標は「与えられた位置 (x,y) を通る」こととします．このため使用者は各タイミング毎に通らせたい理想の位置(参照となる理想軌道:reference軌道)の (x,y) 座標を車システムに与え，車は実行結果の観測される出力としてセンサーを用いて現在の実際の位置 (x,y) を計測します．この出力として得られ，理想軌道との比較に用いられる要素を出力 \bf y とします． \mathrm{WB} は前輪-後輪間の長さ(wheel base)です．
そして車モデル(非線形)そのものは図右下の d{\bf{x}} /dt により与えられます．MPCコントローラはこの数理モデルを用いて入力に対する出力を計算して予測し，理想軌道に対して最適な軌道が出力されるようにします．以降ではこの数理モデルをどのようにMATLABのMPCで実装し，走らせるかを紹介します．
MATLABでの実装の概要
それでは実際にMATLABで非線形MPCを実装します．作成するのはこちらのgithubにあるmファイルです(matファイルはすべてmファイルの実行で生成されます)．各ファイルの役割を簡単に確認します．
何個もファイルがあって難しそうなことをやっていそうな感がありますが，実際はそんなことはありません．短いものなら数行で，MPCを行う上の5つなら合わせて100行も要りません．
-
nlmpc_test.m
MPCオブジェクトの作成とシミュレーションを行います．この時以下の別ファイルで作成する車モデルや(x,y)軌道を用います． -
carCT.m
車モデルを定義する関数です．引数として \bf x ， \bf u を与え，返り値として d{\bf x}/dt を得ます．連続時間(continuous time)のモデルです． -
carDT.m
carCT.mで作成した車モデルを数値計算用に離散時間化(discrete time化)します．と言ってもcarCTのモデルを短冊っぽく微小時間で区切って繰り返し実行するだけです．
-
carStateFcn.m
状態推定に使われる拡張カルマンフィルタ(EKF)のための車モデル式を与えます．と言ってもcarDTをスルーパスするくらいです． -
carMeasurementFcn.m
EKFでの状態推定に使われる観測値を指定します．と言っても番号を指定するくらいです． -
path.m
車が通る (x,y) 軌道の配列を作成します．あくまで事前に経路作成するだけで，これ自体はMPCの内部には関わっていません．
result_animation.m
実行結果をアニメーションにします．結果をキレイに見せるただの見栄え用です．
以降は各ファイルや関数の中身を見ていきます．
実装の詳細
それでは各ファイルの中身を見ていきます．例によってブログへのソースコードの良い挿れ方が分からないので画像にしています．コード自体はgithubに置いたものですので，そちらをご参照ください．
まずnlmpc_test.mです．前半ではMPCコントローラオブジェクトの作成を行います．
7行目のnlmpc()で非線形MPCオブジェクト本体nlobjを作成します．この時，引数として \bf x ， \bf y ， \bf u の各要素の個数を与えます．このnlobjに車モデルやMPCに関するパラメータを与えていきます．9，10行目でこの車MPCの実行インターバル間隔を設定します．今回は50ms間隔としました．
10，11行目の各「ホライズン」はMPCの重要な基本概念の1つで，予測フェーズにおいて何ステップ先まで出力を予測するかを意味します．大きいほど長期的/詳細な予測ができますが，リソース消費量や計算コストが増大するので注意が必要です．
14行目ではnlobjに今回使用する車モデルの数式(関数)を渡します．「carDT」を指定していますが，これはcarDT.mに書かれたfunctionを登録しています．車の非線形モデル本体はcarCT.mで，carDT.mはこれを離散時間化したものです．各モデル定義ファイルは次の通りです．
carCT.mでは引数として \bf x ， \bf u を与えます．それぞれ x ， y ， \theta と v ， \delta を要素に持ちます．この各要素を使って d{\bf x}/dt の関係式を作ります．この d{\bf x}/dt を返り値としてcarDT.mに渡し，単純に微分を10個( M 個)足し合わせて数値積分として \bf x を得ます．
nlmpc_test.mに戻って，17行目では出力 \bf y として用いる状態を指定します．今回は座標 (x,y) の2つを用います．そのためベクトル \bf x の該当する要素(1番目が x ，2番目が y )を選びます．20，21行目では \bf x ， \bf u への初期値を与えますが，原点での停止状態からのスタートとしました．22行目ではこれまでの値を用いてnlobjを有効化します．
24行目以降ではMPCに付随する機能を作成します．今回，3つの状態 \bf x ( x ， y ， \theta )に対して，目標として参照する要素および出力 \bf y として使用するのは x ， y の2要素です．そのため \theta は計算により推定する必要があります．この推定には25行目の拡張カルマンフィルタを用います．
拡張カルマンフィルタを作成するextexdedKalmanFilter()に推定に使う2つのモデルを登録します．このモデルがcarStateFcn.mおよびcarMeasurementFcn.mです．といってもモデル本体はcarDT.m(carCT.m)のものを流用するだけです．carStateFcn.mらでは，carDT.mとつなげるような番号の指定くらいしかしていません．
26~29行目では初期状態を登録してやります．29行目の「mv」は \bf u の別名です．制御変数 manipulated variableの略でMPCではよくこう表記されます．31行目のnlmpcmoveoptではMPCに与えるオプションのオブジェクトを作成します．このnlmpcmoveoptオブジェクトを使うことで，速度制限や位置制限といった，MPCの肝である「制約」を与えるといったことができます．ただし今回は制約はかけないので特に活用はしません．
34行目以降は実際のシミュレーション用にユーザの希望する (x,y) 軌道をloadしたり，結果の格納用の配列を作成します． (x,y) 軌道の作成は以下のpath.mで行います．
path.mでは通過地点xyを何個か与え，それをスプライン曲線でつないで車を辿らせる軌道を作成します．作成した軌道はxypath.matに保存し，nlmpc_test.mで使用します．
それでは最後にこれまでに作成したMPCを用いて自動運転のシミュレーションを実行します．nlmpc_test.mの最後を見てみます．
シミュレーションでは1~tLengthステップ目まで繰り返しステップ実行していきます．44行目ではEKFと現在の出力を用いて車の最新の状態 \bf x を更正します．46行目のyrefは作成した参照軌道 (x,y) を取り出します．47行目ではこれらの情報と作成したMPCオブジェクトを用いて，理想とする参照軌道を達成するための最適な入力mv( =\bf u )を計算します．ここがMPC計算の心臓部です．
49行目では得られた最適入力mvを用いてEKFの次の状態を推定します．51行目では最適入力を用いた状態 \bf x をcarDTにより計算します．実際の車では \bf y はGPSやらLidarで求まる実行結果ですが，ここではシミュレーションですので今の状態 \bf x にランダムなノイズがかかったものを擬似的に出力として使用します．この各ステップの実行結果をxHistoryに保存します．
シミュレーション実行で得られた実行結果xHistoryをresult_animation.mでアニメ実行すると，冒頭に載せたような車が軌道をたどる自動運転となります!
終わりに
なんだか敷居の高そうな非線形MPCによる自動運転ですが，MATLABの機能を使えば100行程度でできちゃいました．今後の発展としては，
- 速度制限のような「制約」を設ける
- もっと複雑なモデルを使う
- 障害物回避などの機能をつける
- 実際のロボットに適用する
といった応用が考えられます．ちなみにこれを極めたのがチューリッヒ工科大学の下の動画の例だそうです．
こういうのを見ると無力感に打ちのめされる心がウキウキしますね！