ニュートン法に基づく非線形モデル予測制御：MATLABサンプルプログラム

はじめに

これまで，線形モデル，もしくは非線形モデルを線形化してモデル予測制御への適用について考えてきたわけですが．．．線形制御にはいくつか問題があります(TдT)

ramune6110.hatenablog.com

代表的な問題点として，実際の制御対象と物理モデルとの誤差が大きくなることが原因で制御がうまくできない場合が挙げられます．例えば，自動車の車線変更制御や障害物回避制御の場合，線形モデルだと実際の車両の運動と大きく異なるので，実世界での制御が難しいという問題が発生します．この課題の解決策の一つとして，非線形制御が挙げられます！

ここでは，非線形制御の内，非線形モデル予測制御についてまとめます．非線形モデル予測制御については，過去にも素晴らしい記事がいくつかあるので，是非参照してみてください(*´ｰ`)

myenigma.hatenablog.com

特に，大塚先生の「非線形モデル予測制御の研究動向」は是非目を通してみてくださいヽ(#ﾟДﾟ)ﾉ！

https://www.jstage.jst.go.jp/article/isciesci/61/2/61_42/_pdf

非線形モデル予測制御について

ここでは，大塚先生の「実時間最適化による機械システムのモデル予測制御」，「実時間最適化による制御の実応用」を下に，非線形モデル予測制御について簡単にまとめます！

http://www2.iee.or.jp/~dmec/committee/DMEC8005/presentations/MCHF-4_ohtsuka.pdf

www.coronasha.co.jp

まず，モデル予測制御とは，各時刻で有限時間未来までの最適制御問題を解いて制御入力を決定しているフィードバック制御です．これを数式ベースで説明すると，以下のように定式化されます．

$\begin{aligned} &\dot{x}(t)=f(x(t), u(t)) \\ &J=\varphi(x(t+T))+\int_{t}^{t+T} L(x(\tau), u(\tau)) d \tau \\ &C(x(t), u(t))=0 \end{aligned}$

上記の最適制御問題は，上から順に非線形状態方程式，評価関数，制約条件を意味します．モデル予測制御では，各時刻で求めた最適制御を全て使うのではなく，初期値のみ使用します！←ここ重要(#ﾟДﾟ)ｺﾞﾙｧ!!

さらに，求める最適制御と実際の制御入力の関係性を明らかにするために，現実の時刻 $t$ とは別に評価区間上の時間軸 $\tau$ を新たに導入し，時刻 $t$ をパラメータとみなして， $\tau$ 軸上で最適制御を求める手法がよくとられます．まとめると，時刻 $t$ で解くべき最適制御問題は，次のようになります！

$\begin{aligned} &\frac{d}{d \tau} x^{*}(\tau ; t)=f\left(x^{*}(\tau ; t), u^{*}(\tau ; t), p(t+\tau)\right), \quad x^{*}(0 ; t)=x(t) \\ &J=\varphi\left(x^{*}(T ; t), p(t+T)\right) \\ &\quad+\int_{0}^{T} L\left(x^{*}(\tau ; t), u^{*}(\tau ; t), p(t+\tau)\right) d \tau \\ &C\left(x^{*}(\tau ; t), u^{*}(\tau ; t), p(t+\tau)\right)=0 \end{aligned}$

このときの最適制御問題の停留条件は，以下のオイラー・ラグランジュ方程式によって与えられます．

$\begin{aligned} &\frac{d}{d \tau} x^{*}(\tau ; t)=f\left(x^{*}(\tau ; t), u^{*}(\tau ; t), p(t+\tau)\right) \\ &x^{*}(0 ; t)=x(t) \\ &\frac{d}{d \tau} \lambda^{*}(\tau ; t) \\ &\quad=-\left(\frac{\partial H}{\partial x}\right)^{\mathrm{T}}\left(x^{*}(\tau ; t), u^{*}(\tau ; t), \lambda^{*}(\tau ; t), \mu^{*}(\tau ; t), p(t+\tau)\right) \\ &\lambda^{*}(T ; t)=\left(\frac{\partial \varphi}{\partial x}\right)^{\mathrm{T}}\left(x^{*}(T ; t), p(t+T)\right) \\ &\frac{\partial H}{\partial u}\left(x^{*}(\tau ; t), u^{*}(\tau ; t), \lambda^{*}(\tau ; t), \mu^{*}(\tau ; t), p(t+\tau)\right)=0 \\ &C\left(x^{*}(\tau ; t), u^{*}(\tau ; t), p(t+\tau)\right)=0 \end{aligned}$

ここで，

$H(x, u, \lambda, \mu)=L(x, u)+\lambda^{\mathrm{T}} f(x, u)+\mu^{\mathrm{T}} C(x, u)$

はハミルトン関数と呼ばれるものです．さらに， $\lambda$ は随伴変数と呼ばれるベクトル， $\mu$ は制約条件 $C$ に対するラグランジュ乗数のベクトルです．

オイラー・ラグランジュ方程式(´・ω・｀) ？ナニソレ．．．となりませんか？←私はなりました(ノд｀)

一言で片づけてしまうと，オイラー・ラグランジュ方程式は一般的な非線形最適制御問題の停留条件(KKT条件)であるわけですが，導出過程及び詳細については以下の文献の第3章，第5章あたりを参照してください．←投げま～す┐(´～`；)┌ www.coronasha.co.jp

ここでは，オイラー・ラグランジュ方程式について以下のポイントを押さえて頂ければいいと思います！

オイラー・ラグランジュ方程式は最適制御問題の停留条件
オイラー・ラグランジュ方程式は二点境界値問題

1については先ほど説明した通りですが，2についてはオイラー・ラグランジュ方程式より明らかです．オイラー・ラグランジュ方程式をよく観察すると， $x(t)$ に対して初期条件を与え， $\lambda(t)$ に対して終端条件を与えていることが分かります！このように，初期条件と終端条件の両方で境界条件が与えられている方程式を二点境界値問題と呼びます．二点境界値問題は状態方程式などが非線形な場合，一般的に解析的に解くのが困難となります．そのため，勾配法やニュートン法などの反復法から数値解を得るといったアプローチがよくとられます．つまり，毎時刻以下に示す手順により近似解を取得することになるわけです．

1.システムの状態 $x(t)$ を計測する．
2. $x(t)$ を初期状態とした場合のオイラー・ラグランジュ方程式を勾配法やニュートン法で数値的に解いて，求めた最適制御の最初の要素を取得し，制御入力としてシステムに加える．

上記の処理を繰り返すことで，所望の目的を達成するための制御入力を求めます！

ニュートン法による非線形モデル予測制御の数値解法

ここでは，オイラー・ラグランジュ方程式を数値計算に適した問題で近似式に置き換えます．まず，評価区間 $0 \leq \tau \leq T$ をNステップに離散化し，時間刻みを $\Delta \tau = T / N$ とします．このとき，オイラー・ラグランジュ方程式の内，状態と随伴変数に関する微分方程式を差分近似することで，以下の離散時間二点境界値問題を得ることが出来ます！

$\begin{aligned} &x_{i+1}^{*}(t) =x_{i}^{*}(t)+f\left(x_{i}^{*}(t), u_{i}^{*}(t), p(t+i \Delta \tau)\right) \Delta \tau \\ &x_{0}^{*}(t) = x(t) \\ &\lambda_{i}^{*}(t)=\lambda_{i+1}^{*}(t) \\ &+\left(\frac{\partial H}{\partial x}\right)^{\mathrm{T}}\left(x_{i}^{*}(t), u_{i}^{*}(t), \lambda_{i+1}^{*}(t), \mu_{i}^{*}(t), p(t+i \Delta \tau)\right) \Delta \tau \\ &\lambda_{N}^{*}(t)=\left(\frac{\partial \varphi}{\partial x}\right)^{\mathrm{T}}\left(x_{N}^{*}(t), p(t+T)\right) \\ &\frac{\partial H}{\partial u}\left(x_{i}^{*}(t), u_{i}^{*}(t), \lambda_{i+1}^{*}(t), \mu_{i}^{*}(t), p(t+i \Delta \tau)\right)=0 \\ &C\left(x_{i}^{*}(t), u_{i}^{*}(t), p(t+i \Delta \tau)\right)=0 \end{aligned}$

これは，状態の系列，制御入力の系列，随伴変数の系列，ラグランジュ乗数の系列に対する連立代数方程式と見なすことが出来るわけですが・・・以下のテクニカルな処理により，本質的な未知量は制御入力の系列とラグランジュ乗数の系列だけであることが分かります！

まず，制御入力とラグランジュ乗数からなるベクトルを定義します．

$\begin{equation} U(t):=\left(\begin{array}{c} u_{0}^{*}(t) \\ \mu_{0}^{*}(t) \\ \vdots \\ u_{N-1}^{*}(t) \\ \mu_{N-1}^{*}(t) \end{array}\right) \end{equation}$

すると，オイラー・ラグランジュ方程式の状態方程式から状態の系列が求まり，それに付随して随伴変数の方程式からも随伴変数の系列が求まることが分かります．最後にオイラー・ラグランジュ方程式の内，残る条件式は以下の二つとなります．

$\begin{aligned} &\frac{\partial H}{\partial u}\left(x_{i}^{*}(t), u_{i}^{*}(t), \lambda_{i+1}^{*}(t), \mu_{i}^{*}(t), p(t+i \Delta \tau)\right)=0 \\ &C\left(x_{i}^{*}(t), u_{i}^{*}(t), p(t+i \Delta \tau)\right)=0 \end{aligned}$

このとき，これらの条件式は $U(t)$ ， $x(t)$ ， $t$ に依存しているため，離散時間二点境界値問題は

$\begin{equation} F(U(t), x(t), t)=\left(\begin{array}{c} \left(\frac{\partial H}{\partial u}\right)^{\mathrm{T}}\left(x_{0}^{*}(t), u_{0}^{*}(t), \lambda_{1}^{*}(t), \mu_{0}^{*}(t), p(t)\right) \\ C\left(x_{0}^{*}(t), u_{0}^{*}(t), p(t)\right) \\ \vdots \\ \left(\frac{\partial H}{\partial u}\right)^{\mathrm{T}}\left(x_{N-1}^{*}(t), u_{N-1}^{*}(t), \lambda_{N}^{*}(t), \mu_{N-1}^{*}(t), p(t+(N-1) \Delta \tau)\right) \\ C\left(x_{N-1}^{*}(t), u_{N-1}^{*}(t), p(t+(N-1) \Delta \tau)\right) \end{array}\right)=0 \end{equation}$

という代数方程式として表すことが出来ます( -ω- `)ﾌｯ

非線形モデル予測制御では，毎時刻上記の代数方程式の解 $U(t)$ を求め，その最初の要素を実際の制御入力として用います！

この代数方程式を解く際に，ニュートン法などの反復法を使用して数値解を取得するわけです！

ニュートン法については，以下の記事を是非参照してみてください (・∀・)

myenigma.hatenablog.com

数値シミュレーション

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

Case Study 1 : セミアクティブダンパの制御

以下の素晴らしい論文を参考に，モデル予測制御の数値シミュレーションをMATLABにより実装します． www.semanticscholar.org

制御対象のセミアクティブダンパの図を示します．

f:id:Ramune6110:20220227112706p:plain — セミアクティブダンパ

また，セミアクティブダンパの数理モデルは以下で定義されます．

$\left(\begin{array}{c} \dot{x}_{1}(t) \\ \dot{x}_{2}(t) \end{array}\right)=\left(\begin{array}{c} x_{2}(t) \\ -\frac{k}{m} x_{1}(t)-\frac{1}{m} x_{2}(t) u(t) \end{array}\right)$

ここで，状態量 $x_k$ は2つあり，y方向の位置，y方向の速度，制御入力 $u(t)$ は減衰係数です．また，入力に対して以下の不等式制約を与えます．

$0 \leq u \leq u_{\max }$

このとき，ダミー入力 $v(t)$ を導入することで，上記の不等式制約を以下の等式制約に変換することが出来ます．

$C(x(t), u(t), v(t))=\left(u(t)-\frac{u_{\max }}{2}\right)^{2}+v^{2}(t)-\frac{u_{\max }^{2}}{4}=0$

さらに，評価関数を以下のように設定します．

$\begin{aligned} J=& \frac{1}{2} x^{T}(t+T) S_{f} x(t+T)+\int_{t}^{t+T}\left(\frac{1}{2}\left(x^{T}(\tau) Q x(\tau)+r_{1} u^{2}(\tau)\right)\right.\\ &\left.-r_{2} v(\tau)\right) \mathrm{d} \tau \end{aligned}$

ここで，評価関数の第一項目が終端コスト $\varphi$ ，第二項目がステージコスト $L$ です．今回の制御目的は，減衰係数を制御することで，全ての状態量を0に収束させることです．

下記がセミアクティブダンパの制御のMATLABコードになります．

clear;
close all;
clc;

%% Setup and Parameters
dt   = 0.01; % sample time
dimx = 2;    % Number of states
dimu = 3;    % Number of input(u1 = Attenuation coefficient, u2 = Dummy input, u3 = Lagrange multiplier)
diml = 2;    % Number of companion variable

sim_time = 20; % Simulation time [s]

%% Semi active dumper
system.m = 1; %[kg]
system.k = 1; %[N/m]
system.a = - system.k / system.m;
system.b = - 1 / system.m;

%% NMPC parameters
params_nmpc.tf = 1.0;             % Final value of prediction time [s]
params_nmpc.N = 5;                % Number of divisions of the prediction time [-]
params_nmpc.kmax = 20;            % Number of Newton's method
params_nmpc.hdir = 0.01;          % step in the Forward Difference Approximation

params_nmpc.x0 = [2;0];           % Initial state
params_nmpc.u0 = [0.01;0.9;0.03]; % Initial u

params_nmpc.sf = [ 1;10 ];        % Termination cost weight matrix
params_nmpc.q = [ 1;10 ];         % Weight matrix of state quantities
params_nmpc.r = [ 1;0.01 ];       % Weight matrix of input quantities

params_nmpc.umin = 0;             % lower input limit
params_nmpc.umax = 1;             % upper input limit

params_nmpc.dimx = dimx;          % Number of states
params_nmpc.dimu = dimu;          % Number of input(u1 = Attenuation coefficient, u2 = Dummy input, u3 = Lagrange multiplier)
params_nmpc.diml = diml;          % Number of companion variable
 
%% define systems
[xv, lmdv, uv, muv, fxu, Cxu] = defineSystem(system, params_nmpc);
[L, phi, qv, rv, sfv]         = evaluationFunction(xv, uv);
[obj, H, Hx, Hu, Huu, phix]   = generate_Euler_Lagrange_Equations(xv, lmdv, uv, muv, fxu, Cxu, L, phi, qv, rv, sfv);

%% Initial input value calculation using Newton's method
lmd0 = obj.phix(params_nmpc.x0, params_nmpc.sf);

for cnt = 1:params_nmpc.kmax
    params_nmpc.u0 = params_nmpc.u0 - obj.Huu( params_nmpc.u0, params_nmpc.r ) \ obj.Hu( params_nmpc.x0, params_nmpc.u0, lmd0, params_nmpc.r );
end

%% main loop
xTrue(:, 1) = params_nmpc.x0;
uk(:, 1) = params_nmpc.u0;
uk_horizon(:, 1) = repmat( params_nmpc.u0, params_nmpc.N, 1 );
current_step = 1;
sim_length = length(1:dt:sim_time);
normF = zeros(1, sim_length + 1);
solvetime = zeros(1, sim_length + 1);

for i = 1:dt:sim_time
    i
    % update time
    current_step = current_step + 1;
    
    % solve nmpc
    tic;
    [uk(:, current_step), uk_horizon(:, current_step), algebraic_equation] = NMPC( obj, xTrue(:, current_step - 1), uk_horizon(:, current_step - 1), system, params_nmpc);
    solvetime(1, current_step - 1) = toc;
    
    % optimality error norm F
    total = 0;
    for j = 1:params_nmpc.dimu * params_nmpc.N
        total = total + algebraic_equation(j, :)^2;
    end
    normF(1, current_step - 1) = sqrt(total);
    
    % update state
    T = dt*current_step:dt:dt*current_step+dt;
    [T, x] = ode45(@(t,x) nonlinear_model(x, uk(:, current_step), system), T, xTrue(:, current_step - 1));
    xTrue(:, current_step) = x(end,:);
end

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

plot_figure(xTrue, uk, normF, current_step);

%% define systems
function [xv, lmdv, uv, muv, fxu, Cxu] = defineSystem(system, nmpc)
    syms x1 x2 lmd1 lmd2 u1 u2 u3
    xv   = [x1; x2];     % state vector
    lmdv = [lmd1; lmd2]; % costate vector
    uv   = [u1; u2];     % control input(include dummy input)
    muv  = u3;           % Lagrange multiplier vectors for equality constraints
    
    % state equation
    fxu = [xv(2);
           system.a * xv(1) + system.b* xv(2) * uv(1)];
    % constraints
    Cxu = (uv(1) - nmpc.umax / 2)^2 + uv(2)^2 - (nmpc.umax / 2)^2; 
end

function [L, phi, qv, rv, sfv] = evaluationFunction(xv, uv)
    syms q1 q2 r1 r2 sf1 sf2
    qv  = [q1; q2];         % vector for diagonal matrix Q
    rv  = [r1; r2];         % weight vector
    sfv = [sf1; sf2];       % vector for diagonal matrix Sf
    Q   = diag([q1, q2]);   % square matrix
    R   = diag([r1, r2]);   % square matrix
    Sf  = diag([sf1, sf2]); % square matrix
    
    % stage cost
    L   = simplify( (xv' * Q * xv) / 2 + rv(1) * uv(1)^2 / 2 - rv(2) * uv(2) ); 
    % terminal cost
    phi = simplify( (xv' * Sf * xv) / 2 );                                      
end

function [obj, H, Hx, Hu, Huu, phix] = generate_Euler_Lagrange_Equations(xv, lmdv, uv, muv, fxu, Cxu, L, phi, qv, rv, sfv)
    H = simplify(L + lmdv' * fxu + muv' * Cxu); % H = Hamiltonian
    uv = [uv; muv];                             % extend input
    
    Hx   = simplify(gradient(H, xv));           % Hx = ∂H / ∂x
    Hu   = simplify(gradient(H, uv));           % Hu = ∂H / ∂u
    Huu  = simplify(jacobian(Hu, uv));          % Huu = ∂^2H / ∂u^2
    phix = simplify(gradient(phi, xv));         % Huu = ∂φ / ∂x
    
    obj.Hx   = matlabFunction(Hx, 'vars', {xv, uv, lmdv, qv});
    obj.Hu   = matlabFunction(Hu, 'vars', {xv, uv, lmdv, rv});
    obj.Huu  = matlabFunction(Huu, 'vars', {uv, rv});
    obj.phix = matlabFunction(phix, 'vars', {xv, sfv});
end

%% NMPC
function [uk, uk_horizon, algebraic_equation] = NMPC( obj, x_current, uk_horizon, system, nmpc )
    for cnt = 1:nmpc.kmax
        uk_horizon = uk_horizon - ( dFdu( obj, x_current, uk_horizon, system, nmpc ) \ F( obj, x_current, uk_horizon, system, nmpc ) );
    end
    
    algebraic_equation = F( obj, x_current, uk_horizon, system, nmpc );
    uk = uk_horizon(1:nmpc.dimu);
end

%% calculation dF = ∂F / ∂u
function dF = dFdu( obj, x_current, u, system, nmpc )
    dF = zeros( nmpc.dimu * nmpc.N, nmpc.dimu * nmpc.N );
    diff = nmpc.hdir;
    
    for cnt = 1:nmpc.N*nmpc.dimu
        u_buff_p = u;
        u_buff_n = u;

        u_buff_p(cnt) = u_buff_p(cnt) + diff;
        u_buff_n(cnt) = u_buff_n(cnt) - diff;
        
        % dF = ∂F / ∂u is the Forward Difference Approximation
        dF(:,cnt) = ( F( obj, x_current, u_buff_p, system, nmpc ) - F( obj, x_current, u_buff_n, system, nmpc ) ) / ( diff );
    end
end

%% calculation F = nonlinear algebraic equation
function ans_F = F( obj, x_current, u, system, nmpc )
    x   = Forward( x_current, u, system, nmpc );
    lmd = Backward( obj, x, u, nmpc );

    ans_F = zeros( nmpc.dimu * nmpc.N, 1 );

    for cnt = 1:nmpc.N
        ans_F((1:nmpc.dimu)+nmpc.dimu*(cnt-1)) = obj.Hu(x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)), ...
                                                        u((1:nmpc.dimu)+nmpc.dimu*(cnt-1)), ...
                                                        lmd((1:nmpc.diml)+nmpc.diml*(cnt-1)),...
                                                        nmpc.r );
    end
end

%% Prediction of the state from the current time to T seconds in the future (Euler approximation)
function x = Forward( x0, u, system, nmpc )
    dt = nmpc.tf / nmpc.N;
    
    x = zeros( nmpc.dimx * nmpc.N, 1 );
    x(1:nmpc.dimx) = x0;

    for cnt = 1 : nmpc.N-1
       x((1:nmpc.dimx)+nmpc.dimx*(cnt)) = x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)) ...
                                        + nonlinear_model( x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)), u((1:nmpc.dimu)+nmpc.dimu*(cnt-1)), system ) * dt; 
    end
end

%% Prediction of adjoint variables from current time to T seconds in the future (Euler approximation)
function lmd = Backward( obj, x, u, nmpc )
    dt = nmpc.tf / nmpc.N;
    
    lmd = zeros( nmpc.diml * nmpc.N, 1 );
    lmd((1:nmpc.diml)+nmpc.diml*(nmpc.N-1)) = obj.phix( x((1:nmpc.dimx)+nmpc.dimx*(nmpc.N-1)), nmpc.sf );
    
    for cnt = nmpc.N-1:-1:1                                                    
        lmd((1:nmpc.diml)+nmpc.diml*(cnt-1)) = lmd((1:nmpc.diml)+nmpc.diml*(cnt)) ...
                                             + obj.Hx( x((1:nmpc.dimx)+nmpc.dimx*(cnt)), u, lmd((1:nmpc.diml)+nmpc.diml*(cnt)), nmpc.q ) * dt;
    end
end

%% nonlinear state equation
function dx = nonlinear_model(xTrue, u, system)
    % dx = f( xTrue, u )
    dx = [xTrue(2);
          system.a * xTrue(1) + system.b * xTrue(2) * u(1)]; 
end

%% plot figure
function plot_figure(xTrue, uk, normF, current_step)
    % plot state
    figure(1)
    subplot(2, 1, 1)
    plot(0:current_step - 1, xTrue(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('y [m]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    subplot(2, 1, 2)
    plot(0:current_step - 1, xTrue(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$\dot{y}$ [m/s]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 1.0);
    
    % plot input
    figure(2);
    subplot(2, 1, 1)
    plot(0:current_step - 1, uk(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Attenuation coefficient [N/(m/s)]','interpreter','latex','FontSize',10);
    subplot(2, 1, 2)
    plot(0:current_step - 1, uk(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('dummy input $\upsilon$','interpreter','latex','FontSize',10);
    
    % plot Lagrange multiplier
    figure(3);
    plot(0:current_step - 1, uk(3,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Lagrange multiplier $\mu$','interpreter','latex','FontSize',10);
    
    % plot optimality error
    figure(4);
    plot(0:current_step - 1, normF(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('optimality error norm','interpreter','latex','FontSize',10);
end

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

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

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

f:id:Ramune6110:20220321200839p:plain — 制御入力とダミー入力の時系列データ

f:id:Ramune6110:20220321200902p:plain — ラグランジュ乗数の時系列データ

f:id:Ramune6110:20220321200915p:plain — 最適性誤差Fの時系列データ

結果より，制御目的はおおむね達成できていると言えそうです！また，制御入力に関しても制約条件内に収まっていることが確認できますね～．最適性誤差Fに関しては，初期誤差はかなり大きいですが，それ以外の時刻では，0に近い値を取っています(図だと分かりにくいです・・・(T_T)). また最適制御問題の平均計算時間は0.03[s]でした．しかし，ここではサンプリング周期0.01[s]と設定しているため，サンプリング周期内で制御入力の更新が出来ず，実機への適用が難しいことが分かります．このように，ニュートン法などの反復法は，解が収束するまでの計算時間を見積もることが難しく，一定のサンプリング周期でのフィードバック制御には向いていません(´；ω；｀)←これが本記事で一番言いたいこと！！！

Case Study 2 : ホバークラフトの位置制御

Case Study 1同様，以下の素晴らしい論文を参考に，モデル予測制御の数値シミュレーションをMATLABにより実装します． www.semanticscholar.org

制御対象のホバークラフトの図を示します．

f:id:Ramune6110:20220228142229p:plain — ホバークラフト

また，ホバークラフトの数理モデルは以下で定義されます．

$\left(\begin{array}{c} \dot{x_{1}} \\ \dot{x_{2}} \\ \dot{x_{3}} \\ \dot{x_{4}} \\ \dot{x_{5}} \\ \dot{x_{6}} \end{array}\right)=\left(\begin{array}{c} \dot{x_{4}} \\ \dot{x_{5}} \\ \dot{x_{6}} \\ \frac{u_{1} \cos x_{3}+u_{2} \cos x_{3}}{M} \\ \frac{u_{1} \sin x_{3}+u_{2} \sin x_{3}}{M} \\ \frac{u_{1} r+u_{2} r}{I} \end{array}\right)$

ここで，状態量 $x_k$ は6つあり，x方向の位置，y方向の位置，方位角，x方向の速度，y方向の速度，角速度です．また，制御入力 $u(t)$ は2つあり，それぞれ左右の推力です．さらに，入力に対して以下の不等式制約を与えます．

$u_{\min } \leq u_{i} \leq u_{\max }$

このとき，ダミー入力 $u_{di} (i = 1, 2)$ を導入することで，上記の不等式制約を以下の等式制約に変換することが出来ます．

$C_{i}=\left(u_{i}-\bar{u}\right)^{2}+u_{d i}^{2}-\left(u_{\max }-\bar{u}\right)^{2}$

ただし， $\bar{u}$ は以下の通り定義されます．

$\bar{u}=\frac{u_{\max }+u_{\min }}{2}$

さらに，評価関数を以下のように設定します．

$\begin{aligned} J=& \frac{1}{2}\left(x(t+T)-x_{f}\right)^{T} S_{f}\left(x(t+T)-x_{f}\right) \\ &+\frac{1}{2} \int_{t}^{t+T}\left(\left(x-x_{f}\right)^{T} Q\left(x-x_{f}\right)+u^{T} R u-k_{1} u_{d 1}-k_{2} u_{d 2}\right) \mathrm{d} \tau, \end{aligned}$

ここで，評価関数の第一項目が終端コスト $\varphi$ ，第二項目がステージコスト $L$ です．今回の制御目的は，左右の推力を制御することで，目標状態 $x_{f}$ へ全ての状態量を収束させることです．

下記がホバークラフトの位置制御のMATLABコードになります．

clear;
close all;
clc;

%% Setup and Parameters
dt   = 0.01; % sample time
dimx = 6;    % Number of states
dimu = 6;    % Number of input(u1 = thrust1, u2 = thrust1, u3 = dummy input, u4 = dummy input, u5 = Lagrange multiplier1, u6 = Lagrange multiplier2)
diml = 6;    % Number of companion variable

sim_time = 10; % Simulation time [s]
end_condition = 0.0005; % [m]

%% Hovercraft
system.M = 0.894;  % [kg]
system.I = 0.0125; % [kg・m^2]
system.r = 0.0485; % [m]

%% NMPC parameters
params_nmpc.tf = 1.0;                                         % Final value of prediction time [s]
params_nmpc.N = 10;                                           % Number of divisions of the prediction time [-]
params_nmpc.kmax = 20;                                        % Number of Newton's method
params_nmpc.hdir = 0.0001;                                    % step in the Forward Difference Approximation

params_nmpc.x0 = [ -0.25; 0.35; 0.0; 0.0; 0.0; 0.0 ];         % Initial state
params_nmpc.u0 = [ 0.2; 0.1; 0.2; 0.03; 0.01; 0.02 ];         % Initial u
params_nmpc.xf = [ 0.0; 0.0; 0.0; 0.0; 0.0; 0.0 ];            % target state

params_nmpc.sf = [ 10; 15; 0.1; 1; 1; 0.01 ];                 % Termination cost weight matrix
params_nmpc.q  = [ 10; 15; 0.1; 1; 1; 0.01 ];                 % Weight matrix of state quantities
params_nmpc.r  = [ 1; 1; 0.001; 0.001 ];                      % Weight matrix of input quantities
params_nmpc.ki = [ 0.28; 0.28 ];                              % Weight matrix of dummy input

params_nmpc.umin = -0.121;                                    % lower input limit [N]
params_nmpc.umax = 0.342;                                     % upper input limit [N]
params_nmpc.uavg = (params_nmpc.umax + params_nmpc.umin) / 2; % average input limit [N]

params_nmpc.dimx = dimx;                                      % Number of states
params_nmpc.dimu = dimu;                                      % Number of input(u1 = Attenuation coefficient, u2 = Dummy input, u3 = Lagrange multiplier)
params_nmpc.diml = diml;                                      % Number of companion variable
 
%% define systems
[xv, lmdv, uv, muv, fxu, Cxu] = defineSystem(system, params_nmpc);
[L, phi, qv, rv, kv, sfv]     = evaluationFunction(xv, uv, params_nmpc);
[obj, H, Hx, Hu, Huu, phix]   = generate_Euler_Lagrange_Equations(xv, lmdv, uv, muv, fxu, Cxu, L, phi, qv, rv, kv, sfv);

%% Initial input value calculation using Newton's method
lmd0 = obj.phix(params_nmpc.x0, params_nmpc.sf);

for cnt = 1:params_nmpc.kmax
    params_nmpc.u0 = params_nmpc.u0 - obj.Huu( params_nmpc.u0, params_nmpc.r ) \ obj.Hu( params_nmpc.x0, params_nmpc.u0, lmd0, params_nmpc.r, params_nmpc.ki );
end

%% main loop
xTrue(:, 1) = params_nmpc.x0;
uk(:, 1) = params_nmpc.u0;
uk_horizon(:, 1) = repmat( params_nmpc.u0, params_nmpc.N, 1 );
current_step = 1;
sim_length = length(1:dt:sim_time);
normF = zeros(1, sim_length + 1);
solvetime = zeros(1, sim_length + 1);

for i = 1:dt:sim_time
    i
    % update time
    current_step = current_step + 1;
    
    % solve nmpc
    tic;
    [uk(:, current_step), uk_horizon(:, current_step), algebraic_equation] = NMPC( obj, xTrue(:, current_step - 1), uk_horizon(:, current_step - 1), system, params_nmpc);
    solvetime(1, current_step - 1) = toc;
    
    % optimality error norm F
    total = 0;
    for j = 1:params_nmpc.dimu * params_nmpc.N
        total = total + algebraic_equation(j, :)^2;
    end
    normF(1, current_step - 1) = sqrt(total);
    
    % update state
    T = dt*current_step:dt:dt*current_step+dt;
    [T, x] = ode45(@(t,x) nonlinear_model(x, uk(:, current_step), system), T, xTrue(:, current_step - 1));
    xTrue(:, current_step) = x(end,:);
    
    % break condition
    x_target  = params_nmpc.xf;
    x_current = xTrue(:, current_step);
    if ( (x_current(1) - x_target(1))^2 + (x_current(2) - x_target(2))^2 ) <= end_condition
        break
    end
end

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

plot_figure(xTrue, uk, normF, params_nmpc, current_step);

%% define systems
function [xv, lmdv, uv, muv, fxu, Cxu] = defineSystem(system, nmpc)
    syms x1 x2 x3 x4 x5 x6 lmd1 lmd2 lmd3 lmd4 lmd5 lmd6 u1 u2 u3 u4 u5 u6
    xv   = [x1; x2; x3; x4; x5; x6];             % state vector
    lmdv = [lmd1; lmd2; lmd3; lmd4; lmd5; lmd6]; % costate vector
    uv   = [u1; u2; u3; u4];                     % control input(include dummy input)
    muv  = [u5; u6];                             % Lagrange multiplier vectors for equality constraints
    
    % state equation
    fxu = [xv(4);
           xv(5);
           xv(6);
           ( uv(1) * cos(xv(3)) + uv(2) * cos(xv(3)) ) / system.M;
           ( uv(1) * sin(xv(3)) + uv(2) * sin(xv(3)) ) / system.M;
           ( uv(1) * system.r + uv(2) * system.r ) / system.I];          
    % constraints
    Cxu = [(uv(1) - nmpc.uavg)^2 + uv(3)^2 - (nmpc.umax - nmpc.uavg)^2;
           (uv(2) - nmpc.uavg)^2 + uv(4)^2 - (nmpc.umax - nmpc.uavg)^2]; 
end

function [L, phi, qv, rv, kv, sfv] = evaluationFunction(xv, uv, nmpc)
    syms q1 q2 q3 q4 q5 q6 r1 r2 r3 r4 k1 k2 sf1 sf2 sf3 sf4 sf5 sf6
    qv  = [ q1; q2; q3; q4; q5; q6 ];           % vector for diagonal matrix Q
    rv  = [ r1; r2; r3; r4 ];                   % vector for diagonal matrix R
    kv  = [ k1; k2 ];                           % weight vector
    sfv = [ sf1; sf2; sf3; sf4; sf5; sf6 ];     % vector for diagonal matrix Sf
    Q   = diag([q1, q2, q3, q4, q5, q6]);       % square matrix
    R   = diag([r1, r2, r3, r4]);               % square matrix
    Sf  = diag([sf1, sf2, sf3, sf4, sf5, sf6]); % square matrix
    
    % stage cost
    L   = simplify( ((xv - nmpc.xf)' * Q * (xv - nmpc.xf)) / 2 + (uv' * R * uv) / 2 - kv(1) * uv(3) - kv(2) * uv(4) ); 
    % terminal cost
    phi = simplify( ((xv - nmpc.xf)' * Sf * (xv - nmpc.xf)) / 2 );                                      
end

function [obj, H, Hx, Hu, Huu, phix] = generate_Euler_Lagrange_Equations(xv, lmdv, uv, muv, fxu, Cxu, L, phi, qv, rv, kv, sfv)
    H = simplify(L + lmdv' * fxu + muv' * Cxu); % H = Hamiltonian
    uv = [uv; muv];                             % extend input
    
    Hx   = simplify(gradient(H, xv));           % Hx = ∂H / ∂x
    Hu   = simplify(gradient(H, uv));           % Hu = ∂H / ∂u
    Huu  = simplify(jacobian(Hu, uv));          % Huu = ∂^2H / ∂u^2
    phix = simplify(gradient(phi, xv));         % Huu = ∂φ / ∂x
    
    obj.Hx   = matlabFunction(Hx, 'vars', {xv, uv, lmdv, qv});
    obj.Hu   = matlabFunction(Hu, 'vars', {xv, uv, lmdv, rv, kv});
    obj.Huu  = matlabFunction(Huu, 'vars', {uv, rv});
    obj.phix = matlabFunction(phix, 'vars', {xv, sfv});
end

%% NMPC
function [uk, uk_horizon, algebraic_equation] = NMPC( obj, x_current, uk_horizon, system, nmpc )
    for cnt = 1:nmpc.kmax
        uk_horizon = uk_horizon - ( dFdu( obj, x_current, uk_horizon, system, nmpc ) \ F( obj, x_current, uk_horizon, system, nmpc ) );
    end
    
    algebraic_equation = F( obj, x_current, uk_horizon, system, nmpc );
    uk = uk_horizon(1:nmpc.dimu);
end

%% calculation dF = ∂F / ∂u
function dF = dFdu( obj, x_current, u, system, nmpc )
    dF = zeros( nmpc.dimu * nmpc.N, nmpc.dimu * nmpc.N );
    diff = nmpc.hdir;
    
    for cnt = 1:nmpc.N*nmpc.dimu
        u_buff_p = u;
        u_buff_n = u;

        u_buff_p(cnt) = u_buff_p(cnt) + diff;
        u_buff_n(cnt) = u_buff_n(cnt) - diff;
        
        % dF = ∂F / ∂u is the Forward Difference Approximation
        dF(:,cnt) = ( F( obj, x_current, u_buff_p, system, nmpc ) - F( obj, x_current, u_buff_n, system, nmpc ) ) / ( diff );
    end
end

%% calculation F = nonlinear algebraic equation
function ans_F = F( obj, x_current, u, system, nmpc )
    x   = Forward( x_current, u, system, nmpc );
    lmd = Backward( obj, x, u, nmpc );

    ans_F = zeros( nmpc.dimu * nmpc.N, 1 );

    for cnt = 1:nmpc.N
        ans_F((1:nmpc.dimu)+nmpc.dimu*(cnt-1)) = obj.Hu(x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)), ...
                                                        u((1:nmpc.dimu)+nmpc.dimu*(cnt-1)), ...
                                                        lmd((1:nmpc.diml)+nmpc.diml*(cnt-1)),...
                                                        nmpc.r,...
                                                        nmpc.ki);
    end
end

%% Prediction of the state from the current time to T seconds in the future (Euler approximation)
function x = Forward( x0, u, system, nmpc )
    dt = nmpc.tf / nmpc.N;
    
    x = zeros( nmpc.dimx * nmpc.N, 1 );
    x(1:nmpc.dimx) = x0;

    for cnt = 1 : nmpc.N-1
       x((1:nmpc.dimx)+nmpc.dimx*(cnt)) = x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)) ...
                                        + nonlinear_model( x((1:nmpc.dimx)+nmpc.dimx*(cnt-1)), u((1:nmpc.dimu)+nmpc.dimu*(cnt-1)), system ) * dt; 
    end
end

%% Prediction of adjoint variables from current time to T seconds in the future (Euler approximation)
function lmd = Backward( obj, x, u, nmpc )
    dt = nmpc.tf / nmpc.N;
    
    lmd = zeros( nmpc.diml * nmpc.N, 1 );
    lmd((1:nmpc.diml)+nmpc.diml*(nmpc.N-1)) = obj.phix( x((1:nmpc.dimx)+nmpc.dimx*(nmpc.N-1)), nmpc.sf );
    
    for cnt = nmpc.N-1:-1:1                                                    
        lmd((1:nmpc.diml)+nmpc.diml*(cnt-1)) = lmd((1:nmpc.diml)+nmpc.diml*(cnt)) ...
                                             + obj.Hx( x((1:nmpc.dimx)+nmpc.dimx*(cnt)), u, lmd((1:nmpc.diml)+nmpc.diml*(cnt)), nmpc.q ) * dt;
    end
end

%% nonlinear state equation
function dx = nonlinear_model(xTrue, u, system)
    % dx = f( xTrue, u )
    dx = [xTrue(4);
          xTrue(5);
          xTrue(6);
          ( u(1) * cos(xTrue(3)) + u(2) * cos(xTrue(3)) ) / system.M;
          ( u(1) * sin(xTrue(3)) + u(2) * sin(xTrue(3)) ) / system.M;
          ( u(1) * system.r + u(2) * system.r ) / system.I];
end

%% plot figure
function plot_figure(xTrue, uk, normF, nmpc, current_step)
    % plot state 
    figure(1)
    subplot(3, 1, 1)
    plot(0:current_step - 1, xTrue(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', 2.0);
    
    subplot(3, 1, 2)
    plot(0:current_step - 1, xTrue(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Y [m]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 2.0);
     
    subplot(3, 1, 3)
    plot(0:current_step - 1, xTrue(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', 2.0);
    
    % plot state 
    figure(2);
    subplot(3, 1, 1)
    plot(0:current_step - 1, xTrue(4,:), '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', 2.0);
    
    subplot(3, 1, 2)
    plot(0:current_step - 1, xTrue(5,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$\dot{Y}$ [m/s]','interpreter','latex','FontSize',10);
    yline(0.0, '--r', 'LineWidth', 2.0);
     
    subplot(3, 1, 3)
    plot(0:current_step - 1, xTrue(6,:), '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', 2.0);
    
    % plot input
    figure(3);
    subplot(2, 1, 1)
    plot(0:current_step - 1, uk(1,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$u_{1}$ [N]','interpreter','latex','FontSize',10);
    yline(nmpc.umin, '--b', 'LineWidth', 2.0);
    yline(nmpc.umax, '--b', 'LineWidth', 2.0);
    
    subplot(2, 1, 2)
    plot(0:current_step - 1, uk(2,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('$u_{2}$ [N]','interpreter','latex','FontSize',10);
    yline(nmpc.umin, '--b', 'LineWidth', 2.0);
    yline(nmpc.umax, '--b', 'LineWidth', 2.0);
    
    % plot dummy input
    figure(4);
    subplot(2, 1, 1)
    plot(0:current_step - 1, uk(3,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('dummy input $u_{d1}$','interpreter','latex','FontSize',10);
    subplot(2, 1, 2)
    plot(0:current_step - 1, uk(4,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('dummy input $u_{d2}$','interpreter','latex','FontSize',10);
    
    % plot Lagrange multiplier
    figure(5);
    subplot(2, 1, 1)
    plot(0:current_step - 1, uk(5,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Lagrange multiplier $\mu_{1}$','interpreter','latex','FontSize',10);
    subplot(2, 1, 2)
    plot(0:current_step - 1, uk(6,:), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('Lagrange multiplier $\mu_{2}$','interpreter','latex','FontSize',10);
    
    % plot optimality error
    figure(6);
    plot(0:current_step - 1, normF(1,1:current_step), 'ko-',...
        'LineWidth', 1.0, 'MarkerSize', 4);
    xlabel('Time Step','interpreter','latex','FontSize',10);
    ylabel('optimality error norm F','interpreter','latex','FontSize',10);
    
    % plot trajectory
    figure(7)
    plot(xTrue(1,:), xTrue(2,:), 'b',...
        'LineWidth', 2.0, 'MarkerSize', 4); hold on;
    grid on;
    % plot target position
    target = nmpc.xf;
    plot(target(1), target(2), 'dg', 'LineWidth', 2);
    xlabel('X [m]','interpreter','latex','FontSize',10);
    ylabel('Y [m]','interpreter','latex','FontSize',10);
    legend('Motion trajectory', 'Target position', 'Location','southeast',...
           'interpreter','latex','FontSize',10.0);
end

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

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

f:id:Ramune6110:20220321202313p:plain — ホバークラフトの位置に関する軌跡

f:id:Ramune6110:20220321202342p:plain — 制御入力の時系列データ

f:id:Ramune6110:20220321202400p:plain — ダミー入力の時系列データ

f:id:Ramune6110:20220321202413p:plain — ラグランジュ乗数の時系列データ

f:id:Ramune6110:20220321202426p:plain — 最適性誤差Fの時系列データ

結果より，初期位置から目的地までおおむね到達できていることが確認できます！また，制御入力に関しても制約条件内に収まっていることが確認できますね～．最適性誤差Fに関しても初期値含め，0に近い状態を保っていることが分かります. また最適制御問題の平均計算時間は0.65[s]と，サンプリング周期0.01[s]に比べて大幅に時間が掛かっていることが分かりました．Case Study 1に比べて制御入力が増加したこと，状態量が増加したこと，非線形性が増したことなど様々な理由により，処理負荷が増加したのだと思います．

まとめ

ここでは，一般的な非線形モデル予測制御の定式化及び最適性条件である代数方程式の簡単な流れと数値シミュレーション例についてまとめました．代数方程式の解を得るためにここではニュートン法を使用して数値解を取得したわけですが，案の定ニュートン法などの反復法では計算時間がかかり(正確にはサンプリング周期内では解けず)実機への適用は難しいことが確認できました(ˇ・ω・ˇ)

この代数方程式を高速で解くための研究というのはいくつかなされているそうですが，その中でも最も有名な手法がC/GMRES法だと思います．C/GMRES法に基づく非線形モデル予測制御についてもまとめたい＼(^o^)／

Ramune6110の日記

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