モンテカルロMPCを実装してみた

この記事はMATLAB/Simulink Advent Calender 2021対応になります。

昨日は@HppyCtrlEngnrngさんの「このWebカメラを使ってな、このWebカメラを使ってな、ビジュアルフィードバックしようと思うたのじゃ」でした。MATLABと実機との連携は試作段階では非常に有用ですが、高価なIOボードが必要なこともあり、どうしても躊躇してしまいます。それをArduinoで代用できることは素晴らしいですね。通信プロトコル次第では、別のマイコンで自前実装するのも面白いのかも？

それはそうと、AIやプログラミング系と違い、制御系は技術の進歩が早くないので、Advent Calenderネタも尽きてしまいそうです。

はじめに

本記事は、モデル予測制御の一つであるモンテカルロMPC(以下、MCMPC)を実装します。
元となる論文[1]が大変わかりやすいため、実質的には元論文に対するサンプルコードを掲載した記事になります。

モンテカルロモデル予測制御とは？

MCMPCはモデル予測制御(以下、MPC)の一手法であり、
モンテカルロ法で操作入力列の候補を生成し、、
それぞれの評価値の大小から、制御入力を決定します。
なお、MPCの例に漏れず安定性は補償されないようです。

他のMPCに比べたメリット

• 評価関数の勾配が不要
• 物体間の衝突のような不連続な事象にも対応できる
• 並列化が容易なため、力押しでもリアルタイム制御を実現しやすい

デメリット

• モンテカルロ法に基づくため、結果に再現性がない
• 外乱に対して収束に時間がかかる傾向がある

理論

現時刻から一定時間先の未来までの操作入力列をランダムにたくさん生成し、
それぞれについてシミュレーションした結果を評価します。
その後、優れた評価値を示す上位の操作入力列から、操作入力列を決定します。

最もシンプルな実装は、次のstep0~4で実現できます。
一回の制御周期中にstep2,3を複数回繰り返すことで、
解の収束精度を改善できます。
しかし、今回の制御対象では、
効果がイマイチのわりに計算時が伸びたので省略します。

step0

制御開始時に各変数を初期化します。
以降、制御周期ごとにstep1~step4を繰り返すことで制御します。

step1

1回前の制御周期に計算した操作入力列を平均値として、
事前に設定した分散とから、正規分布に従って現時刻iから、i+(K-1)までの操作入力列を生成します。ただし、入力列の最終ステップi+(K-1)については、1回前の制御周期で求めた時点におけるK-1の値を平均値とします。

つまり、以下の式に基づいて現時刻iでの操作入力列の候補を生成します(式引用:参考文献[1])。

u_{\{i|k\}}= \begin{cases}
N(u_{\{i-1|k+1\}}^*,\sigma^2) (k=1,2,\dots,K-2) \\
N(u_{\{i-1|K-1\}}^*,\sigma^2) (k=K-1)
\end{cases}

step2

step1で予測した各操作入力列に対して予測シミュレーションし、
それぞれの評価値を算出します。
評価関数はstep2までの情報から計算できる関数であればよく、
微分できないような関数、例えばインパルス、ステップ等々にも対応できます。

この評価関数の自由度がMCMPCの大きなメリットであり、
数式ベースのC/GMRES等ですと数式をこねくり回す必要があるのに対して、
MCMPCは数値計算さえ出来れば不連続でもなんでも、
評価値さえ出すならばなんでもいいです。

step3

step2で算出した評価値が小さい(優れた結果)方からN'個を抽出し、
その加重平均を計算することで、最終的な制御入力とします。

加重平均の計算は参考文献に則り以下の式で計算します。

u_{\{i|k\}}^*=\frac{\Sigma^{N'}_{n=1}\{e^{-\frac{J_i^{(n)}}{\lambda}} 
u_{\{i|k\}}^{(n)}\}}
{\Sigma^{N'}_{n=1}e^{-\frac{J_i^{(n)}}{\lambda}}}\\
(k=0,1,\dots,K-1)

ただし、以下の式に基づいて$\lambda$を計算し、$J^{(n)}_i$(現時刻iにおける操作入力列の候補のうちn個目を表す)を正規化することで、計算精度の問題を回避しています。

\lambda = \Sigma^{N'}_{n=1}J^{(n)}_i\\

step4

求めた入力列のうち、最初の入力$u_{{i|0}}^*$を現在の制御周期における入力として出力します。

step1,2は各サンプルごとに独立に計算を進められるため、並列化が容易であり、
それによって速度を向上でます。

実装例

制御対象

制御対象は以下の二次遅れ系とします。
また、サンプリング時間は1secとして実装しました。
$$ G(s) = \frac{1}{s^2+0.5s+1} $$

モデルの予測計算関数

モデル予測(シミュレーション)する関数から実装します。

MATLABのシミレーションでよく利用する"lsim()"関数は、
伝達関数について、初期値ありのシミュレーションができません。
仮に、伝達関数ベースで"lsim()"により予測計算をする場合は、
制御開始時刻からの全ての操作入力を保持しておく必要があり、
現実的ではありません。

"lsim()"は、シミュレーション対象が状態方程式であれば、
初期値ありのシミュレーションができます。
よって、制御対象の伝達関数を状態方程式に変換して利用します。

本関数の入出力は次の通り設計しました

|||
|---|---|---|
|関数名|SimModel |
|入力|初期状態x_n,操作入力列 U,サンプリング時間 Ts[sec] |
|出力|出力 Y(=[x[0] x[1] ... x[n] ]) |
|備考|$x[n]=[x_1 x_2,\dots,x_m]^T$,$u[n]=[u_1, u_2 ,\dots, u_m]^T$,$U=[u_n u_n+1 , \dots , u_{n+(K-1)}]$|

実コード

function [Y] = SimModel(x_0,U,Ts)
%SimModel 初期状態と操作入力列に基づき応答をシミュレーションする
    %シミュレーション用の時系列
    [K,~] = size(U);
    t = 0:Ts:Ts*(K-1);

    %モデル定義
    model = ss(tf([1,1],[1,0.5,1]));
    
    %シミュレーション(モデル予測)
%     初期値ありはlsimの第一引数が状態空間モデルの場合のみ(公式doc参照)
    Y = lsim(model,U,t ,x_0);
end

制御入力列の生成関数

MATLABで指定の平均値と分散を持つ正規分布からの乱数生成は公式ドキュメントが参考になります。
https://jp.mathworks.com/help/matlab/math/random-numbers-with-specific-mean-and-variance.html

• 関数randn()は、平均0,分散1の正規分布からサンプルを返します。
• xの平均が$\mu_x$,分散$\sigma_x^2$のとき、
  $y=ax+b$で定義される確率変数は、平均$\mu_y=a\mu_x+b$、分散$\sigma_y^2 = a^2 \times \sigma_x^2$
• xが平均0,分散1のときを考えると、平均$\mu$、分散$\sigma^2$のサンプルは、
  $b=\mu, a=\sigma$とすることで得られる。

操作入力列を生成する関数は以下のように設計しました。

関数名	GenMonteCarloU
入力	元となる操作入力列 mu_U$=[u^{i-1}(0),u^{i-1}(1),\dots,u^{i-1}(K-1)]$ ,分散 mpc_sigma
出力	正規分布に従って生成された操作入力列U=$[u^i(0) u^i(1),\dots,u^i(K-1)]$ ただし、$u^(m)$は、時刻における操作入力列の時刻*からm番目の入力を示す。

実コード

function [U] = GenMonteCarloU(mu_U,mpc_Simga)
%GenMonteCarloU モンテカルロ法で操作入力列を生成する        
        [m,~] = size(mu_U);
        U = mpc_Simga .* randn([m,1]) + mu_U;
end

評価関数

評価関数としては、予測区間全体で、
指令値と出力との差がなるべく小さくなるように以下の関数を評価関数として
設定しました。

$
J(y) = \int_i^{i+(K-1)}(y(t)-指令値(t))^2dt
$
ただし、$y(t)$は制御対象とした伝達関数における出力に相当する。
※状態方程式の状態量ではないことに注意

関数名	EvalPredictiveY
入力	操作入力列 U、予測した状態の系列 Y、指令値の系列 Ref
出力	評価値

実コード

function [J] = EvalPredictiveY(U,Y,Ref)
    J = sum(( Ref' - Y).^2); 
end

コードの結合

これまでに作成した関数と組み合わせて
シミュレーション全体を作成します。

実コード

%% モンテカルロMPC
% 目的：任意のSISO伝達関数に対してMCMPCする
% 　　　入力制約と評価関数を与える
%       周波数ベースで評価関数を与えることを試す
clear all;
close all;
format compact;

%% シミュレーションのための初期設定
plant = tf([1,1],[1,0.5,1]);  %制御対象

log.u=[];   %操作入力のログ
T_end = 3;  %シミュレーション終了時刻
Ts = 1e-2;
t = 0:Ts:T_end; % シミュレーション時間
x0 = [0;0];       %シミュレーション初期値
x_n = x0;       %時刻nにおける状態変数値

% 指令値の生成
% ref = zeros(length(t),1);
% temp = linspace(0,1,200)'; %ここの200ってどこから？
% ref = [ref(1:end-length(temp))' temp'];
% ref = ref .*10;
% 矩形波状の指令値生成
ref = 100*sin(pi*t);
ref = -1*(gt(ref,1)-1);
ref(1) = 0;

%% step0.制御器の初期設定
mpc.Sigma = sqrt(1e0);      %標準偏差 = √分散 = √(σ^2)
mpc.Steps = 100;    %予測ステップ数 init100
mpc.N = 1000;         %サンプル数 初期値1000
mpc.N_dash = 10;    %エリート抽出する個数 初期値10
mpc.U_z = zeros(mpc.Steps,1);  %1制御周期前の操作入力列(単一入力限定)

for n = 2:length(t)-mpc.Steps
    disp(n)
    
    %1制御周期前の操作入力列から、今回の制御周期でベースとなる
    %操作入力列に更新
    mpc.U_z = [mpc.U_z(2:end);mpc.U_z(end)];
    
    %% step2.入力列の生成
    % 正規分布に従って制御入力のサンプルu_i(n)をN通り生成する。
    U_samples = [];   %操作入力列サンプル保存先　1サンプルが横ベクトル = [u(0) u(1) … u(Samples-1)]
    for i=1:mpc.N
        U_samples(:,i) = GenMonteCarloU(mpc.U_z,mpc.Sigma);
    end
    
    %% step3.予測シミュレーション
    % 操作入力列毎にシミュレーションし、評価値Jを算出する
    Jn = [];    %サンプル毎の評価値
    
    for i=1:mpc.N
        %ある操作入力列に対する予測シミュレーション
        Y = SimModel(x_n,U_samples(:,i),Ts);
        
        %評価関数の計算
        Jn(i) = EvalPredictiveY(U_samples(:,i),Y(:,1),ref(n:n+mpc.Steps-1));
    end
    
    %% step4.エリート抽出と荷重平均の計算
    % Jnの昇順でソートし、併せてU_iもソートする。
    % Jnの上からmpc.N_dash個だけエリートとして利用する。    
    
    %評価値に従って昇順に並べ替え
    [~, I] = sort(Jn);    
    Jn = Jn(I);    
    U_samples = U_samples(:,I);

    %重みづけ計算
    lambda = sum( Jn(1:mpc.N_dash) );%正規化用λ
    denom = sum(exp(- Jn(1:mpc.N_dash) ./ lambda ));    %加重平均の計算分母側
    nume = (sum( (exp(- Jn(1:mpc.N_dash) ./ lambda) .* U_samples(:,1:mpc.N_dash) )'))';
    mpc.U_z = nume ./ denom;
    
    %% シミュレーション(1ステップ)
    log.u(n) = mpc.U_z(1);        
    t_tmp = 0:Ts:Ts*(length(log.u)-1);    
    [y,~,x] = lsim(ss(plant),log.u,t_tmp,x0);
    
    x_n = x(end,:);    %ここendになっていたのはおかしくない？→ok、この時点ではyは1~nのシミュレーション結果        
    log.y(n) = y(end);
end

%% ----結果表示----
subplot(2,1,1);
plot(t(1:length(t)-mpc.Steps),log.u);
xlim([0,3]);
title('操作入力u');
subplot(2,1,2);
plot(t(1:length(t)-mpc.Steps),log.y);
hold on;
plot(t,ref);
title('出力y');
legend('y','ref');

実際のシミュレーション条件と結果は以下の通りです。
モンテカルロ法に基づくため、シミュレーション毎に結果は異なりますが、
そこそこ追従できていることがわかると思います。

入力制約を考慮した制御

入力制約を与えた場合の結果を示します。
ここでは、入力制約として$|u| <= 5$ としました。
つまり、step2で入力制約を満たさない操作入力列に対しては、
評価値を非常に大きく設定することで、入力制約としました。
(step1において、入力制約を満たす入力列のサンプル数が一定数になるまで生成し続ける方法もありますが、計算時間が一定ではなくなるので、実時間制御に利用するのは厳しいと思います)

評価関数はそのままとして、シミュレーション結果を以下に示します。
入力制約が考慮できています。

PID制御とことなり、指令値が変化するより以前から出力が変化していることから、
確かに予測して制御していることが確認できます。

あとがき

1~2年ほどC/GMRESをちょこちょこと取り組んでいたのですが、進んだり進まなかったりと
なかなか面白くならなかった日々。そんなところに、計測自動制御学会の論文集で参考文献[1]の論文を見つけ、読みやすく分かりやすかったので、さらっと実装してみました。計算時間を何も考慮していませんが、操作入力列の候補生成から評価値の算出までは独立に計算できますので、並列化しての実時間制御への応用もどうにかなりそうです。

業務ですと、結論先行で簡潔にまとめろといわれますが、たまにはだらだらと書くのも気持ちいいですね。

それにしても、MATLABは構造体の要素を宣言無しでどんどんと追加できるのが素晴らしい!!

参考文献

[1]加藤幹也・伊達　央・仲谷真太郎・大矢晃久
　　モンテカルロモデル予測制御による壁面との衝突を考慮したクアッドコプタの制御