モデル予測制御(MPC) step1

はじめに

MPC（モデル予測制御）について分かり易く解説しているサイトがいくつもあり、そこで勉強させていただいたので、簡単なモデルを作り、EXCELとDLL化したQPソルバーを使ってちょっと動かしてみます。

今回は基本的なロジックの確認が目的です。

大変勉強になった記事はこちらです。
モデル予測制御（MPC）による軌道追従制御
式の展開などなど多々参照させていただいています。

ダウンロード

このブログで説明しているEXCELファイルとQPソルバーのDLL及び、QPソルバーのＣソースファイルは、こちらからダウンロードできます → リンク

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

QpSolver.dll は32bit版EXCELのDLLとして作りました。（すみません。64bit版EXCELでは動作しません。）
ダウンロードしたEXCELとQPソルバーDLLは、シート 加減速 の MPC演算開始 ボタンを押すと演算を開始します。

MPC（モデル予測制御）の概要

現代制御では解析的なアプローチで制御入力を決定します。これに対して、MPC（モデル予測制御）では、未来のシステムの振る舞いを予測する予測ホライズン区間の制御応答を数理計画問題として扱い、数値演算的に最適制御入力を決定します。

数理計画は、様々な事象の最適化を対象とします（例えば → 発電プラントの最適運転）が、システムの制御入力の最適解まで計算してしまえ！というのは、なかなか大胆でナイスなアイデアです。

数理計画問題として扱う場合、システムの制約条件を設定できるのもメリットです。例えば、このブログのフィードフォワード制御の記事では、システムが許容する範囲を超えた制御入力をコントローラが要求してしまう例がありました。モデル予測制御(MPC)では、制約条件を設定できるため、システムが許容する範囲で制御性を改善できます。

今回は、状態方程式で表現したシステムに、MPC（モデル予測制御）を適用してみます。

システムの振る舞いを状態方程式から予測して数値演算的に最適制御入力を決定するため、制御対象のモデルそのものをコントローラに実装します。

このブログでは、精度の高い離散時間モデル（離散化方法 → リンク）を時間応答の確認のために作ってきましたが、その離散化モデルをコントローラに実装する前提で説明を進めていきます。

シミュレーションの構成

制御対象

トラックのプラトーニングというのをご存知でしょうか。

　参考 → リンク1　リンク2　リンク3

欧州ではトラックメーカーを中心に実証実験が実施されています。国内でもソフトバンクが積極的に技術開発を進めています。

トラックが車間距離10mほどで隊列走行をしますが、その状態で全車両が同期して加減速します。実際にどのようなシステム構成なのかは知りませんが、このブログでは、先頭車両から加減速指示が後続車両に車両間通信で配信（ブロードキャスト）されるものとします。

今回の step1 では、トラックのプラトーニングを想定し、加減速指示に従った正確な加減速制御を目標にします。

モデル化

今回は、基本的なロジックの確認が目的なので、これまでも扱ってきた簡易的な車両モデルである加速度の応答が一次遅れの車両モデルを使います。

車両のモデル化と離散化方法は以前の記事で説明しています。

　簡易的な車両モデル → リンク
　離散化方法 → リンク

簡易的な車両モデルでは、加速度、速度、走行距離の関係と加減速制御入力 Acmd を下記の状態方程式の形にモデル化しています。

$$\frac{d}{dt} \begin{bmatrix} L \\ V \\ A \\ \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & -\frac{1}{T} \\ \end{bmatrix} \begin{bmatrix} L \\ V \\ A \\ \end{bmatrix}+\begin{bmatrix} 0 \\ 0 \\ \frac{1}{T}\end{bmatrix} Acmd$$

この状態方程式をEXCELのシート 状態方程式 で下記のように離散化しています。

MPC（モデル予測制御）の演算

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

具体的な手順を以下の①～④で説明します。

① 予測ホライズン区間のシステムの振る舞いを表す状態方程式を作る

離散化した状態方程式より、１制御周期後の状態$x(1)$と出力方程式は $$x(1)=P x(0)+Q u(0)$$ $$y(1)=C x(1)$$
２制御周期後の状態$x(2)$は $$x(2)=P x(1)+Q u(1)$$ $$=P (P x(0)+Q u(0))+Q u(1)$$ $$=P^2x(0)+\begin{bmatrix} PQ & Q \end{bmatrix} \begin{bmatrix} u(0) \\ u(1) \\  \end{bmatrix}$$
３制御周期後の状態$x(3)$は $$x(3)=P^3x(0)+\begin{bmatrix} P^2Q & PQ & Q \end{bmatrix} \begin{bmatrix} u(0) \\ u(1) \\ u(2) \\  \end{bmatrix}$$
n制御周期後の状態$x(n)$は $$x(n)=P^nx(0)+\begin{bmatrix} P^{n-1}Q & P^{n-2}Q & \cdots & Q \end{bmatrix} \begin{bmatrix} u(0) \\ u(1) \\ \vdots \\ u(n-1) \\  \end{bmatrix}$$
予測ホライズン区間の状態方程式をまとめると $$\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)+GU$ とします。

EXCELのシート MPC で $F$ と $G$ を計算しています。

出力方程式は $$\begin{bmatrix} y(1) \\ y(2) \\ \vdots \\ y(n) \\  \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(n) \\  \end{bmatrix}$$ この式を $Y=CX$ とします。$Y$ はこのシステムの出力で車両の加減速度をまとめたベクトルになります。

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

制御工学的には評価関数。数理計画問題（ソルバー）的には目的関数になります。

評価関数としては、予測ホライズン区間の4制御周期分において、
自車の加減速度と加減速指令値の偏差
　　$自車の加減速度_k - 加減速指令値_k$　　$(k=1\cdots4)$　
は最小にしたい。

それを予測ホライズン区間の制御入力
　　$Acmd_k$　　$(k=1\cdots4)$
は最小で実現したい。

式で表現すると、評価関数値は $$\sum_{k=1}^4\big((自車の加減速度_k - 加減速指令値_k)\big)^2+\sum_{k=1}^4\big( Acmd_k\big)^2$$ となります。次に上記の式の各項の重み付けとベクトルでの表現を考えると、評価関数 $J$ は次の式で表されます。 $$J=(Y-Y_{ref})^TQ(Y-Y_{ref})+(U-U_{ref})^TR(U-U_{ref})$$ $Y$ はこのシステムの出力で予測ホライズン区間の車両の加減速度をまとめたベクトルです。
$Y_{ref}$ は予測ホライズン区間の加減速指令値をまとめたベクトルで、今回は4制御周期分の加減速指令値がプラトーニングの先頭車両から配信されると想定しています。
$U$は予測ホライズン区間の加減速制御入力 $Acmd$ をまとめたベクトル。
$U_{ref}$ は制御入力 $U$ の目標値ですが、今回はゼロにすることで制御入力 $U$はミニマムが条件になります。
$Q, R$ は重み付け係数の対角行列です。今回$Q$の対角値は 10000、$R$ の対角値は 1 にしています。

以上により、この評価関数 $J$ は、加減速指令値 $Y_{ref}$ と車両の加減速度 $Y$ の偏差が最小となる制御入力 $U$ を最小値で求めることになります。

③ 評価関数（目的関数）を目的変数ベクトルの2次式に変換して各係数行列を計算

評価関数 $J$ の式に
$$Y=CX=C(Fx(0)+GU)$$ を代入して整理すると $$J(U)=U^T(G^TC^TQCG+R)U+2((CFx(0)-Y_{ref})^TQCG-U_{ref}^TR)U+定数項$$ となり、評価関数 $J$ は $U$ の２次式になります。

予測ホライズン区間をまとめた状態方程式の係数行列 $F, G$ や、$U$ の２次の係数行列 $$(G^TC^TQCG+R)$$ と、$U$ の１次の係数行列 $$2((CFx(0)-Y_{ref})^TQCG-U_{ref}^TR)$$ は、EXCELのシート MPC で計算しています。

④ 制御演算周期ごとに評価関数の$U$の2次式を2次計画問題(QP)ソルバーに設定

制御演算周期ごとに評価関数である $U$ の2次式を目的関数として2次計画問題(QP)ソルバーに設定します。

$U$ の２次の係数行列 $$(G^TC^TQCG+R)$$ は定数行列であるため、１回計算すれば更新の必要はありません。
一方、$U$ の１次の係数行列 $$2((CFx(0)-Y_{ref})^TQCG-U_{ref}^TR)$$ は加減速指示値 $Y_{ref}$ を含むため、制御演算周期ごとに更新し、2次計画問題(QP)ソルバーに設定します。

ソルバーにはEXCELのシート MPC の下側で定義している制約条件式の行列 $A^T$ と $B$ も設定します。今回は、加減速を±3[m/s]以下としています。

簡易的な車両のモデル化、モデルの離散化、MPCの演算①～④については、EXCELとVBAで処理していますが、EXCELには、2次計画問題(QP)のソルバーは実装されていないため、2次計画問題(QP)のソルバーはCでコーディングし、DLL化してEXCEL VBAから呼び出しています。

2次計画問題(QP)のソルバーからは、予測ホライズン区間も含めた最適制御値 $U$（加減速制御入力値 $Acmd$ ）が返されます。

2次計画問題(QP)ソルバー

SQP（逐次２次計画法）でSQPのCソースを公開していますが、SQP（逐次２次計画法）は、2次計画問題(QP)ソルバーを内包しており、今回はそのQPソルバーを取り出してDLL化し、EXCELから呼び出せるようにしています。

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$を求めます。

記号が重複していて申し訳ないですが、目的関数の $G$ は評価関数の２次の係数行列 $$2(G^TC^TQCG+R)$$ $C$ は評価関数の１次の係数行列 $$2((CFx(0)-Y_{ref})^TQCG-U_{ref}^TR)$$ です。EXCEL VBAからQPソルバーを以下のように呼び出しています。

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

2次計画問題(QP)ソルバーについては、「FORTRAN 77 最適化プログラミング」の4章に詳細な説明がありますので、そちらでご確認いただきたいです。大きな図書館には所蔵されていたりします。

今回は、基本的なロジックの確認が目的なので、予測ホライズンは4制御周期としています。

結果

今回は step1 として、MPCのロジックで加減速指示値に応じた加減速制御入力が算出されるか確認しました。

まず最初の加速指示として、1.0[m/s2] のステップ入力の1次遅れ値（時定数1秒）を5秒間指示しています。その後、-1.0[m/s2] のステップ入力の1次遅れ値（時定数1秒）を5秒間指示しています。

この車両モデルで指示値通りに加減速するには、逆関数値となる1.0[m/s2]の加速と、-1.0[m/s2]の減速を制御入力値とする必要があります。

実際に動かしてみると、 加減速指示  に  加減速度  が追従するように、QPソルバーは逆関数である  制御入力値  を正確に算出しており、MPCのロジックは適切に動作しているようです。

車間距離は、レーダーなどのセンサで検知できるため、次の step2 では、車間距離(10m)もフィードバックする制御ロジックにして確認します。

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