Ramune6110の日記

技術的な事について記事にまとめていきたい

トップ > Model Predictive Control > 線形モデル予測制御入門：MATLABサンプルプログラム

線形モデル予測制御入門：MATLABサンプルプログラム

Model Predictive Control MATLAB

目次

はじめに

前々から，気になっていたモデル予測制御ですが，以下の記事を見てさらに学習意欲が湧いたので記事にします．

xtech.nikkei.com

上記の記事では，非線形モデル予測制御によりドリフト（横すべり）しながら障害物を回避する車両制御について書かれています．

トヨタは以前より，「うまい運転」を実現するために，モデル予測制御を取り入れていたようです．

news.mynavi.jp

記事の中では，こんな事が書かれています．

「制御においてMPCは可能性のある将来有望な手段と感じました。ただ、まだまだ小さな一歩でしかありません。これが製品になって実績ができてはじめて有効と判断できます。その意味でも、MPCを自動運転の制御に限らず、なんらかの形でアルゴリズムを世に出していければと考えています」（入江氏）

そういった意味では，今回のドリフト制御は大きな一歩を踏み出したと言えるのではないでしょうか！

さて，モデル予測制御については有識者の方々の素晴らしい記事が存在しています．

myenigma.hatenablog.com

qiita.com

いまさら，私が記事にするのもおこがましい話ですが，備忘録としてまとめたいと思います． 前置きが長くなりましたが，次から本題に入ります．

モデル予測制御ってなに？

モデル予測制御とは，「制御対象の数式モデルを基に，制御周期ごとに最適制御問題を解くことで，未来の予測値から現時刻で取るべき制御入力を求める方法」です． モデル予測制御の概念図は以下の通りです．

f:id:Ramune6110:20220205151825p:plain
モデル予測制御の概念図

モデル予測制御では図のように，制御周期(Sample Time)ごとに予測時間分（Prediction Horizon）の入力系列(Predicted Control Input)を計算します．ここでは，予測軌道(Predicted Output)が参照軌道(reference Trajectory)に追従するような入力系列を計算しています．実際に制御対象の入力として使われるのは，入力系列の最初の値になります．そして，時刻が遷移するたびに同様の流れで入力値を求めて制御していきます．やっていること自体は非常にシンプルなんです！

モデル予測制御における最適制御問題について

では，具体的に制御対象の数式モデルや最適制御問題はどのように表現されるのでしょうか？ ますは，制御対象の数式モデルを定義します．

{x}_{k+1} = A{x}_{k} + Bu_k {y}_{k} = C{x}_{k}

ここで x_k は状態量、 u_k は入力， y_k は出力です． 上記の方程式は，状態方程式と呼ばれるもので，現代制御理論で確立された表現です．

この制御モデルに基づき，最適制御問題は以下のように定式化されます．

\begin{aligned} \mathcal{J}_{k}^{*}=&\operatorname{minimize}_{u_{k+i}, x_{k+i}} \sum_{i=0}^{N-1} \ell\left(x_{k+i}, u_{k+i}\right)+\Psi\left(x_{k+N}\right) \\ \text { s.t. } &x_{k+i+1} =A x_{k+i}+B u_{k+i} \quad \forall i=0, \ldots, N-1 \\ &x_{k+i} \in \mathcal{X} \quad \forall i=0, \ldots, N-1 \\ &u_{k+i} \in \mathcal{U} \quad \forall i=0, \ldots, N-1 \\ &x_{k+N} \in \mathcal{T} \end{aligned}

ここで，\ellはステージコスト，\Psiは終端コストと呼ばれるものです．さらにNは考慮している予測時間の長さを表します．これらの具体的な数式は，制御対象や制御目的によって変動するため変数で置くことで一般化して表現しています．モデル予測制御では，毎時刻上記の最適制御問題を解くことで，現時刻で取るべき制御入力を算出しているのです！

数値シミュレーション

さて，数式だけ眺めていても具体的な作用や実装面でイメージがつかないため，ここでは簡単な例題を通じてモデル予測制御を体感してみましょう．

Case Study 1 : 倒立振子の姿勢制御

以下の方々の素晴らしい記事を参考に，モデル予測制御の数値シミュレーションをMATLABにより実装します．

qiita.com

myenigma.hatenablog.com

制御対象の倒立振子の図を示します．

f:id:Ramune6110:20220205163059p:plain
制御対象の倒立振子

また，今回のシミュレーションでは\thetaは0に近いという仮定を基に，線形近似した以下の数式モデルを制御対象とします．

\begin{gathered} A=\left(\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & 0 & \frac{m g}{M} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{(M+m) g}{l M} & 0 \end{array}\right) \Delta t+\left(\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \\ B=\left(\begin{array}{c} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{l M} \end{array}\right) \Delta t \end{gathered}

ここで，状態量 x_k は4つあり，x方向の位置，x方向の速度，倒立振子の傾き角，倒立振子の傾き角速度で構成されます．

さらに，ステージコストと終端コストを以下のように設定します．

\begin{gathered} \ell\left(x_{k}, u_{k}\right)=x_{k}^{T} Q x_{k}+u_{k}^{T} R u_{k} \\ \Psi\left(x_{k+N}\right)=x_{k+N}^{T} P x_{k+N} \end{gathered}

今回の制御目的は，全ての状態量が0に収束することとします．

下記が倒立振子制御のMATLABコードになります． また，ソルバーはYALMIPのIPOPTを使用しています．

yalmip.github.io

clear;
close all;
clc;

%% Ipopt solver path
dir = pwd;
addpath(fullfile(dir,'Ipopt'))

%% Setup and Parameters
x0 = [-0.02; 0.0; 0.1; 0.0]; % Initial state of the inverted pendulum

dt = 0.1; % sample time
nx = 4;   % Number of states
nu = 1;   % Number of input

P = 30 * eye(nx);               % Termination cost weight matrix
Q = diag([1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities
R = 0.01;                       % Weight matrix of input quantities
N = 10;                         % Predictive Horizon

% Range of states and inputs
xmin = [-5; -5; -5; -5];
xmax = [5; 5; 5; 5];
umin = -5;
umax = 5;

%% Linear Inverted Pendulum
M = 1.0;  % [kg]
m = 0.3;  % [kg]
g = 9.8;  % [m/s^2]
l = 2.0;  % [m]
system.dt = dt;
system.nx = nx;
system.nu = nu;
system.A = eye(nx) + [0.0, 1.0, 0.0, 0.0;
                      0.0, 0.0, m * g / M, 0.0;
                      0.0, 0.0, 0.0, 1.0;
                      0.0, 0.0, g * (M + m) / (l * M), 0.0] .* dt;
system.B = [0.0; 
            1.0 / M; 
            0.0; 
            1.0 / (l * M)] .* dt;
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% MPC parameters
params_mpc.Q = Q;
params_mpc.R = R;
params_mpc.P = P;
params_mpc.N = N;

%% main loop
xTrue(:, 1) = x0;
uk(:, 1) = 0;
time = 20.0;
time_curr = 0.0;
current_step = 1;
solvetime = zeros(1, time / dt);

while time_curr <= time
    % update time
    time_curr = time_curr + system.dt;
    current_step = current_step + 1;
    
    % solve mac
    tic;
    uk(current_step)= mpc(xTrue(:, current_step - 1), system, params_mpc);
    solvetime(1, current_step - 1) = toc;
    
    % update state
    xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(current_step);
end

%% solve average time
avg_time = sum(solvetime) / current_step;
disp(avg_time);

drow_figure(xTrue, uk, current_step);

%% model predictive control
function uopt = mpc(xTrue, system, params_mpc)
    % Solve MPC
    [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc);
    if ~feas
        uopt = [];
        return
    else
        uopt = u(:,1);
    end
end
      
function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params)
    % extract variables
    N = params.N;
    % define variables and cost
    x = sdpvar(system.nx, N+1);
    u = sdpvar(system.nu, N);
    constraints = [];
    cost = 0;
    
    % initial constraint
    constraints = [constraints; x(:,1) == xTrue];
    % add constraints and costs
    for i = 1:N
        constraints = [constraints;
            system.xl <= x(:,i) <= system.xu;
            system.ul <= u(:,i) <= system.uu
            x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)];
        cost = cost + x(:,i)' * params.Q * x(:,i) + u(:,i)' * params.R * u(:,i);
    end
    % add terminal cost
    cost = cost + x(:,N+1)' * params.P * x(:,N+1);
    ops = sdpsettings('solver','ipopt','verbose',0);
    % solve optimization
    diagnostics = optimize(constraints, cost, ops);
    if diagnostics.problem == 0
        feas = true;
        xopt = value(x);
        uopt = value(u);
        Jopt = value(cost);
    else
        feas = false;
        xopt = [];
        uopt = [];
        Jopt = [];
    end
end
        
function drow_figure(xlog, ulog, current_step)
    % Plot simulation
    figure(1)
    subplot(2,2,1)
    plot(0:current_step - 1, xlog(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('X[m]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    subplot(2,2,2)
    plot(0:current_step - 1, xlog(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$\dot{X}$[m/s]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    subplot(2,2,3)
    plot(0:current_step - 1, xlog(3,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$\theta$[rad]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    subplot(2,2,4)
    plot(0:current_step - 1, xlog(4,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$\dot{\theta}$[rad/s]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    figure(2)
    plot(0:current_step - 1, ulog(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Input F[N]','interpreter','latex','FontSize',10);
    yline(5.0, '--b', 'LineWidth', 2.0);
    yline(-5.0, '--b', 'LineWidth', 2.0);
end

下記のGitHubリポジトリでも公開しています．

https://github.com/Ramune6110/MATLAB_Robotics_Engineering/blob/main/Model_Predictive_Control/Linear_Model_Predictive_Control/Inverted_Pendulum.mgithub.com

上記コードの実行結果を以下に示します．

f:id:Ramune6110:20220205185759p:plain
状態量の時系列データ

f:id:Ramune6110:20220205185843p:plain
入力値の時系列データ

数値シミュレーション結果より，制御目的が達成されていることが確認できますね！ ただし，予測ホライズンの長さやコストの重み行列の設定値によっては，きれいに収束しない場合もあります．そのため，モデル予測制御では，ある程度のパラメータ調整は必須と言えますね～．

また，最適制御問題の平均処理時間は約0.29sでした．平均処理時間についても，予測ホライズンの長さ，制御周期，最適化ソルバーによって変わるので一概に言えませんが，今回の結果に関して言えばリアルタイム性があるかどうかは微妙なところですかね～．倒立振子は割と不安定系なので，もう少し早く処理できるのが望ましいです．

Case Study 2 : 自動車の車線変更の制御

以下の資料を参考に，モデル予測制御の数値シミュレーションをMATLABにより実装します．

https://web.stanford.edu/class/archive/ee/ee392m/ee392m.1056/Lecture14_MPC.pdf

上記資料では，自動車の車両制御をモデル予測制御により実現しています．問題設定は以下の図の通りです．

f:id:Ramune6110:20220205224710p:plain
自動車の車線変更

ここでは，自動車は一定速度で走行しながらステアリング角度のみ制御することで，車線変更を実現するものとします．ステアリング角度の変動が微小だと仮定すると，自動車の縦方向の数理モデルは以下のように近似されます．

\begin{gathered} A=\left(\begin{array}{cc} 1 & 0 \\ V\Delta t & 1 \end{array}\right) \\ B=\left(\begin{array}{c} \Delta t \\ \frac{1}{2}V\Delta t^{2} \\ \end{array}\right) \end{gathered}

ここで，状態量 x_k は2つあり，縦方向のステアリング角速度，y方向の偏差で構成されます．

さらに，ステージコストと終端コストを以下のように設定します．

\begin{gathered} \ell\left(x_{k}, u_{k}\right)=(x_{k} - x_{target})^{T} Q (x_{k} - x_{target})+u_{k}^{T} R u_{k} \\ \Psi\left(x_{k+N}\right)=(x_{k+N} - x_{target})^{T} P (x_{k+N} - x_{target}) \end{gathered}

今回の制御目的は， x_{target} へ追従することとします．

下記が車線変更の制御のMATLABコードになります．

clear;
close all;
clc;

%% Ipopt solver path
dir = pwd;
addpath(fullfile(dir,'Ipopt'))

%% Setup and Parameters
x0 = [0.0; 2.0]; % Initial state of the inverted pendulum

dt = 0.1;  % sample time
v  = 30.0; % driving speed
nx = 2;    % Number of states
nu = 1;    % Number of input

P = 30 * eye(nx);     % Termination cost weight matrix
Q = diag([1.0, 1.0]); % Weight matrix of state quantities
R = 0.01;             % Weight matrix of input quantities
N = 20;               % Predictive Horizon

% Range of states and inputs
xmin = [-5; -5];
xmax = [5; 5];
umin = -1;
umax = 1;

x_target = [0.0; 1.0]; % target state value

%% Linear Inverted Pendulum
system.dt = dt;
system.nx = nx;
system.nu = nu;
system.A = [1.0, 0.0;
            v * dt, 1.0];
system.B = [dt;
            0.5 * v * dt^2];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

system.target = x_target;

%% MPC parameters
params_mpc.Q = Q;
params_mpc.R = R;
params_mpc.P = P;
params_mpc.N = N;

%% main loop
xTrue(:, 1) = x0;
uk(:, 1) = 0;
time = 1.0;
time_curr = 0.0;
current_step = 1;
solvetime = zeros(1, time / dt);

while time_curr <= time
    % update time
    time_curr = time_curr + system.dt;
    current_step = current_step + 1;
    
    % solve mac
    tic;
    uk(current_step) = mpc(xTrue(:, current_step - 1), system, params_mpc);
    solvetime(1, current_step - 1) = toc;
    
    % update state
    xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(current_step);
end

%% solve average time
avg_time = sum(solvetime) / current_step;
disp(avg_time);

drow_figure(xTrue, uk, current_step);

%% model predictive control
function uopt = mpc(xTrue, system, params_mpc)
    % Solve MPC
    [feas, ~, u, ~] = solve_mpc(xTrue, system, params_mpc);
    if ~feas
        uopt = [];
        return
    else
        uopt = u(:,1);
    end
end
      
function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, params)
    % extract variables
    N = params.N;
    % define variables and cost
    x = sdpvar(system.nx, N+1);
    u = sdpvar(system.nu, N);
    constraints = [];
    cost = 0;
    
    % initial constraint
    constraints = [constraints; x(:,1) == xTrue];
    % add constraints and costs
    for i = 1:N
        constraints = [constraints;
            system.xl <= x(:,i) <= system.xu;
            system.ul <= u(:,i) <= system.uu
            x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)];
        cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i);
    end
    % add terminal cost
    cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target);
    ops = sdpsettings('solver','ipopt','verbose',0);
    % solve optimization
    diagnostics = optimize(constraints, cost, ops);
    if diagnostics.problem == 0
        feas = true;
        xopt = value(x);
        uopt = value(u);
        Jopt = value(cost);
    else
        feas = false;
        xopt = [];
        uopt = [];
        Jopt = [];
    end
end
        
function drow_figure(xlog, ulog, current_step)
    % Plot simulation
    figure(1)
    subplot(2,1,1)
    plot(0:current_step - 1, xlog(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlim([0, 11]);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Heading angle[deg]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    subplot(2,1,2); 
    plot(0:current_step - 1, xlog(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlim([0, 11]);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Lateral error[m]','interpreter','latex','FontSize',10);
    yline(1.0, '--r', 'LineWidth', 1.0);
    yline(1.5, '--k', 'LineWidth', 2.0);
    yline(2.5, 'k', 'LineWidth', 4.0);
    yline(0.5, 'k', 'LineWidth', 4.0);
    
    figure(2)
    plot(0:current_step - 1, ulog(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlim([0, 11]);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Steering input[deg/s]','interpreter','latex','FontSize',10);
    yline(1.0, '--b', 'LineWidth', 2.0);
    yline(-1.0, '--b', 'LineWidth', 2.0);
end

下記のGitHubリポジトリでも公開しています．

https://github.com/Ramune6110/MATLAB_Robotics_Engineering/blob/main/Model_Predictive_Control/Linear_Model_Predictive_Control/Lateral_Control_of_Car.mgithub.com

実は，倒立振子制御のコードとほとんど変わりません．変更箇所は，制御対象の数理モデル，ステージコストと終端コストぐらいです．

さらに，上記コードの実行結果を以下に示します．

f:id:Ramune6110:20220205230801p:plain
状態量の時系列データ

f:id:Ramune6110:20220205230908p:plain
入力値の時系列データ

数値シミュレーション結果より，きれいに車線変更できていることが確認できました！また，最適制御問題の平均処理時間は約0.38sでした．今回，予測ホライズンを20，制御周期を0.1sと設定したことに起因すると思います．が，自動車のようにリアルタイム性を求められる対象の制御としては，まだまだ改善する必要がありそうです．．．

Case Study 3 : 自動車の障害物回避

以下の論文の自動車モデルを参考に，自動車障害物回避のためのモデル予測制御の数値シミュレーションをMATLABにより実装します．

https://www.jstage.jst.go.jp/article/jacc/49/0/49_0_71/_pdf/-char/en

問題設定は以下の図の通りです．

f:id:Ramune6110:20220213142200p:plain
自動車の障害物回避[3]

ここでは，自動車の加速度，ステアリング角度を制御することで，静的障害物の回避制御をします． 今回の数値シミュレーションにおける最適制御問題を以下のように定義します．

\begin{aligned} \mathcal{J}_{k}^{*}=&\operatorname{minimize}_{u_{k+i}, x_{k+i}, \delta_{i}} \sum_{i=0}^{N-1} \ell\left(x_{k+i}, u_{k+i}\right) + p \delta_{i} +\Psi\left(x_{k+N}\right) \\ \text { s.t. } &x_{k+i+1} =A x_{k+i}+B u_{k+i} \quad \forall i=0, \ldots, N-1 \\ &(x_{k+i} - x_{obstacle})^{T} (x_{k+i} - x_{obstacle}) > \delta_{i} \quad \forall i=0, \ldots, N-1 \\ &x_{k+i} \in \mathcal{X} \quad \forall i=0, \ldots, N-1 \\ &u_{k+i} \in \mathcal{U} \quad \forall i=0, \ldots, N-1 \\ &x_{k+N} \in \mathcal{T} \end{aligned}

さらに，ステージコストと終端コストを以下のように設定します．

\begin{gathered} \ell\left(x_{k}, u_{k}\right)=(x_{k} - x_{target})^{T} Q (x_{k} - x_{target})+u_{k}^{T} R u_{k} \\ \Psi\left(x_{k+N}\right)=(x_{k+N} - x_{target})^{T} P (x_{k+N} - x_{target}) \end{gathered}

注目すべきは，自車と障害物との距離が0以下にならないような制約を入れていることです．ただし，この制約はそのまま考慮するとハード制約となるため，今回はスラック変数 \delta_{i} を導出することで，制約条件を緩和(ソフト制約化)しています．ここで，スラック変数とは，ある程度の制約違反を許容する際に用いられる概念です．スラック変数の詳細については，式の記事が参考になります．

myenigma.hatenablog.com

今回の制御目的は，障害物回避をしながら目的地へ到達することとします．

下記が自動車の障害物回避のMATLABコードになります．

clear;
close all;
clc;

%% Ipopt solver path
dir = pwd;
addpath(fullfile(dir,'Ipopt'))

%% Setup and Parameters
x0 = [0.0; 0.0; 0.0; 0.0; 0.0; 0.0]; % Initial state of the inverted pendulum

dt = 0.1; % sample time
nx = 6;   % Number of states
nu = 2;   % Number of input

P = 200 * eye(nx);                        % Termination cost weight matrix
Q = diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]); % Weight matrix of state quantities
R = diag([0.1, 0.1]);                     % Weight matrix of input quantities
N = 10;                                   % Predictive Horizon
p = 200;                                  % salck weight value

% Range of states and inputs
xmin = [-5; -5; -5; -5; -5; -5];
xmax = [6; 6; 5; 5; 5; 5];
umin = [-1; -5];
umax = [1; 5];

% target state value
x_target = [6.0; 1.0; 0.0; 0.0; 0.0; 0.0]; 

% Obstacle
obs.pos = [3.0; 0.0];
obs.own_vehicle_diameter = 0.5; % [m]
obs.obstacle_diameter    = 0.5; % [m]
obs.r = obs.own_vehicle_diameter + obs.obstacle_diameter;

%% Linear Car models
M = 1500;     % [kg]
I = 2500;     % [kgm^2]
lf = 1.1;     % [m]
lr = 1.6;     % [m]
l = lf + lr;  % [m]
Kf = 55000;   % [N/rad]
Kr = 60000;   % [N/rad]
V = 20;       % [m/s]

system.dt = dt;
system.nx = nx;
system.nu = nu;

A53 = 2 * (Kf * lf + Kr * lr) / I;
A55 = -2 * (Kf * lf + Kr * lr) / (I * V);
A56 = -2 * (Kf * lf^2 + Kr * lr^2) / I;
A63 = 2 * (Kf + Kr) / M;
A65 = -2 * (Kf + Kr) / (M * V);
A66 = -2 * (Kf * lf + Kr * lr) / (M * V);
system.A = eye(nx) + [0.0, 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.0, 0.0, 0.0, 0.0, 1.0;
                      0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
                      0.0, 0.0, A53, 0.0, A55, A56;
                      0.0, 0.0, A63, 0.0, A65, A66] .* dt;

B51 = 2 * Kf / M;
B61 = 2 * Kf * lf / I;
system.B = [0.0, 0.0; 
            0.0, 0.0;
            0.0, 0.0;
            0.0, 1.0;
            B51, 0.0; 
            B61, 0.0] .* dt;
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

system.target = x_target;

%% MPC parameters
params_mpc.Q = Q;
params_mpc.R = R;
params_mpc.P = P;
params_mpc.N = N;
params_mpc.p = p;

%% main loop
xTrue(:, 1) = x0;
uk(:, 1) = [0; 0];
time = 3.0;
time_curr = 0.0;
current_step = 1;
solvetime = zeros(1, time / dt);

while time_curr <= time
    % update time
    time_curr = time_curr + system.dt;
    current_step = current_step + 1;
    
    % solve mac
    tic;
    uk(:, current_step) = mpc(xTrue(:, current_step - 1), system, obs, params_mpc);
    solvetime(1, current_step - 1) = toc;
    
    % update state
    xTrue(:, current_step) = system.A * xTrue(:, current_step - 1) + system.B * uk(:, current_step);
end

%% solve average time
avg_time = sum(solvetime) / current_step;
disp(avg_time);

drow_figure(xTrue, uk, obs, x_target, current_step);

%% model predictive control
function uopt = mpc(xTrue, system, obs, params_mpc)
    % Solve MPC
    [feas, ~, u, ~] = solve_mpc(xTrue, system, obs, params_mpc);
    if ~feas
        uopt = [];
        return
    else
        uopt = u(:, 1);
    end
end
      
function [feas, xopt, uopt, Jopt] = solve_mpc(xTrue, system, obs, params)
    % extract variables
    N = params.N;
    % define variables and cost
    x = sdpvar(system.nx, N+1);
    u = sdpvar(system.nu, N);
    delta = sdpvar(1, N);
    constraints = [];
    cost = 0;
    
    % initial constraint
    constraints = [constraints; x(:,1) == xTrue];
    % add constraints and costs
    for i = 1:N
        constraints = [constraints;
            system.xl <= x(:,i) <= system.xu;
            system.ul <= u(:,i) <= system.uu
            x(:,i+1) == system.A * x(:,i) + system.B * u(:,i)];
        cost = cost + (x(:,i) - system.target)' * params.Q * (x(:,i) - system.target) + u(:,i)' * params.R * u(:,i);
    end
    % add collition avoidance constraints
    for i = 1:N
        pos = obs.pos;
        r = obs.r;
        distance = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
        constraints = [constraints; distance >= delta(1, i)];
        cost = cost + delta(1, i)' * params.p * delta(1, i);
    end
    % add terminal cost
    cost = cost + (x(:,N+1) - system.target)' * params.P * (x(:,N+1) - system.target);
    ops = sdpsettings('solver','ipopt','verbose',0);
    % solve optimization
    diagnostics = optimize(constraints, cost, ops);
    if diagnostics.problem == 0
        feas = true;
        xopt = value(x);
        uopt = value(u);
        Jopt = value(cost);
    else
        feas = false;
        xopt = [];
        uopt = [];
        Jopt = [];
    end
end
        
function drow_figure(xlog, ulog, obs, x_target, current_step)
    % Plot simulation
    figure(1)
    subplot(2, 1, 1)
    plot(0:current_step - 1, ulog(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('delta [rad]','interpreter','latex','FontSize',10);
    yline(1.0, '--b', 'LineWidth', 2.0);
    yline(-1.0, '--b', 'LineWidth', 2.0);
    
    subplot(2, 1, 2);
    plot(0:current_step - 1, ulog(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Acceleration [$m/s^2$]','interpreter','latex','FontSize',10);
    yline(5.0, '--b', 'LineWidth', 2.0);
    yline(-5.0, '--b', 'LineWidth', 2.0);
    
    figure(2)
    % plot trajectory
    plot(xlog(1,:), xlog(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    hold on;
    grid on;
    axis equal;
    % plot initial position
    plot(xlog(1, 1), xlog(1, 2), 'dm', 'LineWidth', 2);
    % plot target position
    plot(x_target(1), x_target(2), 'dg', 'LineWidth', 2);
    xlabel('X[m]','interpreter','latex','FontSize',10);
    ylabel('Y[m]','interpreter','latex','FontSize',10);
    % plot obstacle
    pos = obs.pos;
    r = obs.obstacle_diameter;
    th = linspace(0,2*pi*100);
    x = cos(th); 
    y = sin(th);
    plot(pos(1) + r*x, pos(2) + r*y, 'r', 'LineWidth', 2);
    plot(pos(1), pos(2), 'r', 'MarkerSize', 5, 'LineWidth', 2);
    % plot vegicle
    for i = 1:current_step
        r_vegicle = obs.own_vehicle_diameter;
        th = linspace(0,2*pi*100);
        x = cos(th); 
        y = sin(th);
        X = xlog(1,i);
        Y = xlog(2,i);
        plot(X + r_vegicle * x, Y + r_vegicle * y, 'b','LineWidth', 2);
        hold on;
    end
    
    legend('Motion trajectory','Initial position', 'Target position','Obstacle', 'Location','southeast',...
           'interpreter','latex','FontSize',10.0);
end

下記のGitHubリポジトリでも公開しています．

https://github.com/Ramune6110/MATLAB_Robotics_Engineering/blob/main/Model_Predictive_Control/Linear_Model_Predictive_Control/Collision_Avoidance_of_Car.mgithub.com

倒立振子制御，自動車の車線変更制御のコードとの違いは，制御対象の数理モデル，制約条件，ステージコスト，終端コストです．

さらに，上記コードの実行結果を以下に示します．

f:id:Ramune6110:20220213154058p:plain
自動車の障害物回避制御

f:id:Ramune6110:20220213151332p:plain
入力値の時系列データ

数値シミュレーション結果より，きれいに障害物回避できていることが確認できました！しかし，最適制御問題の平均処理時間は約4.32sとかなり遅い結果となりました．予測ホライズンを10，制御周期0.1sに加え，スラック変数を導入したことで決定変数が増加したことが原因だと思います．車線変更制御同様，リアルタイム性の改善がまだまだ必要ですね＞＜

おわりに

今回は，モデル予測制御の中でも最も基本の線形モデル予測制御について，数値例を示しながらまとめました．やはり，モデル予測制御の欠点である計算負荷が重いという問題が顕著に表れました．また，今回の数値例は比較的，線形モデルに近似しやすいものを扱いましたが，制御対象によってはそれが難しいことがあります．モデル予測制御は奥が深い～ (^-^)/.

参考文献

1.http://liu.diva-portal.org/smash/get/diva2:690771/FULLTEXT01.pdf

2.myenigma.hatenablog.com

3.https://www.researchgate.net/publication/332491944_A_new_strategy_for_rear-end_collision_avoidance_via_autonomous_steering_and_differential_braking_in_highway_driving/figures?lo=1&utm_source=google&utm_medium=organic