LoginSignup
7
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@PozihameBaystar(絶望M2生)

I&Iによる適応モデル予測制御の実装

Last updated at Posted at 2023-04-17

はじめに

　以前、C/GMRESとその亜種を実装する記事を書いたのですが、その延長線として、今回はC/GMRESをImmersion and Invariance(以下 I&I)という手法を用いて適応化した制御手法を再現したいと思います。もし誤り等ありましたら、お手数ですがコメント等で教えて頂けると助かります。

環境

Matlab R2023a

参考文献について

　自分が参考にした書籍、記事を先に説明します。これらを先に読んでいただけるとわかりやすいかと思います。

まずC/GMRESについて、以下の書籍、記事を参考にしております。

また、I&I法については以下の書籍、論文を参考にしました。

I&I法のC/GMRESへの適用については、以下の論文を参考にしました。

この記事では、この論文のPendulum Systemでの例を再現します。

Immersion and Invariance

　I&Iによる適応制御について軽く説明します。ここでは、システムの状態方程式が

\dot{x}=f_{0}(x)+f_{1}(x)\theta+g(x)u

の様に、外乱などの未知パラメータ$\theta$及び入力$u$についてaffineな形で表される場合を考えます。当然、最初は$\theta$の真の値はわからないので、制御しながらこれを真の値に収束させる事を考えます。

天下り的ではあるのですが、ここで$\theta$の推定値を$\hat{\theta}+\beta(x)$とします。$\beta(x)$は何かしらの$x$の関数です。ここで注意していただきたいのは、$\hat{\theta}$はあくまで推定値の一部であり、$\hat{\theta}$単体では推定値ではないという事です。この$\hat{\theta}+\beta(x)$をどうにかして真の値$\theta$に近づけて行かなくてはいけません。

これを数学的に表現するために、まず以下のような多様体を考えます。

z=\hat{\theta}-\theta+\beta(x)

多様体と言う表現がわかりづらければ、曲面のようなものだと思っておけば多分I&Iの最低限の理解においては大丈夫だと思います。さて、最初は真の値からズレている$\hat{\theta}+\beta(x)$をどうにかして真の値$\theta$に近づける、つまり最初は$0$ではない$z$が$0$に収束するような$\hat{\theta}$の更新法、及び上手くそうなるような関数$\beta(x)$を求めます。

ここで、以下のような拡大系を考えます。入力$u$が$u(x,\hat{\theta}+\beta(x))$となっているのは、入力が$x$及び推定値$\hat{\theta}+\beta(x)$を利用したフィードバックによって算出されている事を想定しているからです（何で読んだか忘れてしまったのですがC/GMRESも一応広義のフィードバックと言えるようです）。

\begin{align}
\dot{x}&=f_{0}(x)+f_{1}(x)\theta+g(x)u(x,\hat{\theta}+\beta(x))\\
\dot{\hat{\theta}}&=\omega(x,\hat{\theta})
\end{align}

この$\omega(x)$が求めるべき$\hat{\theta}$の更新法に当たります。この上で、$z$を時間微分してみましょう。次の様になります。尚、ここでは$\theta$は一定であるか、変化速度が十分に遅い場合を考えています。

\begin{align}
\dot{z}&=\dot{\hat{\theta}}+\frac{\partial\beta}{\partial x}\dot{x}\\
&=\omega(x,\hat{\theta})+\frac{\partial\beta}{\partial x}[f_{0}(x)+f_{1}(x)\theta+g(x)u(x,\hat{\theta}+\beta(x))]\\
&=\omega(x,\hat{\theta})+\frac{\partial\beta}{\partial x}[f_{0}(x)+f_{1}(x)(\hat{\theta}+\beta(x)-z)+g(x)u(x,\hat{\theta}+\beta(x))]
\end{align}

ここで、

\omega(x,\hat{\theta})=-\frac{\partial\beta}{\partial x}[f_{0}(x)+f_{1}(x)(\hat{\theta}+\beta(x))+g(x)u(x,\hat{\theta}+\beta(x))]

とすれば、

\dot{z}=-\frac{\partial\beta}{\partial x}f_{1}(x)z

となります。$f_{1}(x)$は状態方程式から既に決まっているので、この式の右辺の$z$の係数が$x$の値に関わらず全て負になるような$\beta(x)$を探せばいいことになります。

わき道にそれますが、ここで一つ自分も疑問点がありあます。Nonlinear and Adaptive Control with Applicationsの3.4のCorollary 3.1を読む限り、リアプノフ関数の視点から見ると、

\frac{\partial\beta}{\partial x}=[f_{1}(x)]^{T}

と選んでしまえば、$z$は単調に$0$に近づくかは置いといて一応収束はするっぽいようなことが書かれてるのですが、正直なところ自信がありません。書籍の説明の中では$z$のリアプノフ関数単体ではなく何故か理想システムのリアプノフ関数とセットで出てきていたりと不明な点も残ります。ここら辺に関して何か正確な説明ご存じの方いらっしゃったら教えて頂けると助かります。

話を戻します。そして求まった$\beta(x)$と入力$u(x,\hat{\theta}+\beta(x))$から$\omega(x,\hat{\theta})$は数値計算出来るので、これを用いてオイラー積分する事で$\hat{\theta}$を更新して行きます。

実際の制御の流れをまとめると以下のような感じになります。

1）入力$u[k]$を印加すると同時にセンサーから$x[k]$の値を取得
2）$x[k]$と$\hat{\theta}+\beta(x[k])$の値を使用し、C/GMRESで次の制御ステップの入力$u[k+1]$を算出
3）$\beta(x[k])$と$u[k]$の値から$\omega(x[k],\hat{\theta})$を算出
4）オイラー積分$\hat{\theta}\leftarrow\hat{\theta}+\omega(x[k],\hat{\theta})\Delta t$により$\hat{\theta}$を更新
5）1）に戻る

$k$は制御ステップ、$\Delta t$はサンプリング時間です。

シミュレーション結果

　大塚先生の論文を見ても制御ホライズンの数値が見当たらなかったので勝手に1[sec]と設定しているのと、シミュレーション速度の関係上サンプリング時間を0.02[sec]に伸ばしています。そのせいか若干のズレは残ってしまいましたが、大方再現出来ているのではないかなと思います。$\theta$の真の値は10としています。
IandI_CGMRES.png

コード

close all; clear all; format compact; beep off;%#ok<CLALL> 

Ts = 0.02;  %サンプリング周期[sec]
t_sim_end = 20;   %シミュレーション時間
tsim = 0:Ts:t_sim_end;   %シミュレーションの基準となる時刻

%コントローラーのハイパーパラメーター
cgmres.ht = Ts;    %前進差分近似の時間幅[sec]
cgmres.zeta = 1/Ts; %操作量の安定化ゲインζ
cgmres.tf = 1.0;      %予測時間の最終長さ[sec]
cgmres.alpha = 0.5;  %予測時間の上昇ゲイン
cgmres.N = 50;  %予測区間の分割数

% 評価関数中の重み
cgmres.Q = [100 0; 0 1];
cgmres.R = 1;
cgmres.S = [100 0; 0 1];

%pendulumのパラメーター（これをI&I法で推定しながら最適制御する
pen.theta = 10;

% 初期条件
pen.x = [2;2];
cgmres.u = 0; %下で値を設定し直している

pen.len_x = length(pen.x);
cgmres.len_u = length(cgmres.u);

%コントローラー側が推測している値を格納する変数
cgmres.theta_hat = 0; %初期値は0としておく
cgmres.beta = Calcbeta(pen.x); %あくまで推測値はtheta_hat単体ではなくtheta_hat+betaである事に注意

cgmres.u_bar = (cgmres.theta_hat+cgmres.beta)*sin(pen.x(1)); %平衡状態での入力の予測値
cgmres.U = repmat(cgmres.u_bar,cgmres.len_u*cgmres.N,1);  
cgmres.dU = 0;
cgmres.u = cgmres.U(1); %コントローラに与える初期操作量


%目標地点
cgmres.x_ob = [1;0];


% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);


%シミュレーションループ
for i = 1:length(tsim)

    %betaの更新
    cgmres.beta = Calcbeta(pen.x);

    log.u(:,i) = cgmres.u;
    log.x(:,i) = pen.x;
    log.estimate_theta(:,i) = cgmres.theta_hat + cgmres.beta;
    pen.x
    
    %シミュレーション計算（最後だけは計算しない）
    %この時は推測値ではなく真のthetaの値を使うのを忘れない事
    if i ~= length(tsim)
        [t,xi] = ode45( @(t,xi) pendulum(t,xi,cgmres.u,cgmres.x_ob,pen.theta),...
            [tsim(i) tsim(i+1)], pen.x, opts);
    else
        break
    end

    %1ステップあたりの時間間隔を計算
    Tl = cgmres.tf*(1-exp(-cgmres.alpha*Ts*i));

    %平衡状態で必要な入力を計算
    cgmres.u_bar = (cgmres.theta_hat+cgmres.beta)*sin(cgmres.x_ob(1));

    %C/GMRES法コントローラーを関数として実装
    [U, dU] = CGMRES_control(pen,cgmres,Tl);

    %theta_hatの更新
    w = Calcw(pen.x, cgmres.x_ob, cgmres.theta_hat, cgmres.beta, cgmres.u);
    cgmres.theta_hat = cgmres.theta_hat + w*cgmres.ht;

    cgmres.u = U(1);
    cgmres.U = U;
    cgmres.dU = dU;

    pen.x = xi(end,:)';

end


%グラフ化
figure();
subplot(4,1,1);
plot(tsim,log.u(1,:),'LineWidth',1); ylabel('u1'); xlabel('time[s]');
subplot(4,1,2);
plot(tsim,log.estimate_theta(:,:),'LineWidth',1); ylabel('θの推定値'); xlabel('time[s]');
subplot(4,1,3);
plot(tsim,log.x(1,:),'LineWidth',1); ylabel('x1'); xlabel('time[s]');
subplot(4,1,4);
plot(tsim,log.x(2,:),'LineWidth',1); ylabel('x2'); xlabel('time[s]');
hold off;



%C/GMRES法コントローラー
function [U, dU] = CGMRES_control(pen,cgmres,Tl)
    
    dt = Tl/cgmres.N; %制御ホライズン内での1ステップを計算

    %状態変数の時間微分を計算
    pen.dx = func(pen.x,cgmres.u,cgmres.x_ob,cgmres.theta_hat+cgmres.beta);

    %Fの各種を計算
    Fux = CalcF(cgmres.U+cgmres.ht*cgmres.dU, pen.x+cgmres.ht*pen.dx, cgmres, pen, dt);
    Fx = CalcF(cgmres.U, pen.x+cgmres.ht*pen.dx, cgmres, pen, dt);
    F = CalcF(cgmres.U, pen.x, cgmres, pen, dt);

    %クリロフ部分空間の最初の基底を求める
    left = (Fux - Fx)/cgmres.ht;
    right = -cgmres.zeta*F - ((Fx-F)/cgmres.ht);
    r0 = right - left;

    %1つ目の基底ベクトル
    Vm(:,1) = r0 ./ norm(r0);

     % Arnoldi process(Hm_を作成する)
    Hm = zeros(3,3);
    
    % GMRESの繰り返し回数(基底ベクトルの数)
    m = cgmres.len_u * cgmres.N; 
    
    %Arnoldi法
    for j=1:m
        Fux = CalcF(cgmres.U + ( Vm(:,j) * cgmres.ht ) , ...
                    pen.x + (pen.dx * cgmres.ht),cgmres,pen,dt);        
        Av = ( ( Fux - Fx ) / cgmres.ht );    %Fxtは関数Fの状態微分
        
        %Line 3
        for k=1:j
            Hm(k,j)=Av'*Vm(:,k);
        end    
        
        %Line 4
        temp_vec = 0;
        for k = 1:j
            temp_vec = temp_vec + Hm(k,j).*Vm(:,k);
        end
        
        v_hat = Av - temp_vec;
        
        %Line 5
        Hm(j+1,j) = norm(v_hat);
        
        Vm(:,j+1) = v_hat ./ Hm(j+1,j);
    end

    [UTMat,Omega] = ToUTMat(Hm);
    
    e = zeros( k + 1, 1 );
    e(1) = 1;   
    gm_ = norm(r0)*e;
    for k=1:length(Omega)
        gm_ = Omega{k}*gm_;
    end

    min_y = zeros(length(UTMat)-1,1);   %解の保存先

    for k=length(UTMat)-1 :-1:1
        min_y(k) = (gm_(k) - UTMat(k,:) * min_y)/UTMat(k,k);
    end

    du_new = cgmres.dU + Vm(:,1:m)*min_y;
    
    dU = du_new;
    U = cgmres.U + dU .* cgmres.ht;

end


%Fの計算
function F = CalcF(U,x,cgmres,pen,dt)

    %Uを成形
    U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
    
    %xの予測計算
    X = Forward(x, U_temp, dt, pen, cgmres);
   
    %随伴変数の計算
    Lambda = Backward(X, U_temp, dt, pen, cgmres);

    %Fのサイズを定める
    F = zeros(cgmres.len_u*cgmres.N,1);

    %Fの算出
    for i=1:cgmres.N
        F(cgmres.len_u*(i-1)+1:cgmres.len_u*i) = CalcHu(U_temp(:,i),...
            cgmres.u_bar, Lambda(:,i), cgmres.R, [0;1]);
    end
end


%xの予測計算
function X = Forward(x,U,dt,pen,cgmres)
    
    X = zeros(pen.len_x,cgmres.N+1); %Xのサイズを定義

    X(:,1) = x; 

    for i = 2:cgmres.N+1
        dx = func(X(:,i-1),U(:,i-1),cgmres.x_ob,pen.theta);
        X(:,i) = X(:,i-1) + dt*dx;
    end
end


%随伴変数の計算
function Lambda = Backward(X, U, dt, pen, cgmres)

    Lambda = zeros(pen.len_x, cgmres.N);
    Lambda(:,cgmres.N) = cgmres.S*(X(:,cgmres.N+1)-cgmres.x_ob);

    for i = cgmres.N-1:-1:1
        Lambda(:,i) = Lambda(:,i+1)...
            + CalcHx(X(:,i+1),cgmres.x_ob,Lambda(:,i+1),cgmres.Q,...
            cgmres.theta_hat+cgmres.beta)*dt;
    end
end


%dH/du(の転置)
function dHdu = CalcHu(u,u_bar,lambda,R,B)
    dHdu = R*(u-u_bar) + B'*lambda;
end


%dH/dx(の転置)
function dHdx = CalcHx(x,x_ob,lambda,Q,theta)
    dfdx = Calcfx(x,theta);
    dHdx = Q*(x-x_ob) + dfdx'*lambda;
end


%df/dx(状態方程式の状態変数微分)
function dfdx = Calcfx(x,theta)
    dfdx = [0 1; -theta*cos(x(1))-1 -1];
end


function [UTMat,Omega] = ToUTMat(H)
%UNTITLED HをGivens回転でもって上三角行列に変換する
    m = length(H)-1;  %Givens回転する回数
    
    for i=1:m
        nu = sqrt(H(i,i)^2 + H(i+1,i)^2);
        
        c_i = H(i,i)/nu;
        s_i = H(i+1,i)/nu;
        
        Omega{i} = diag(ones(m+1,1));
        Omega{i}(i:i+1,i:i+1) = [c_i,s_i
                                -s_i,c_i];
                            
        H = Omega{i} * H;        
    end
    
    UTMat = H;
end


%pendulumのモデル(ode用)
function dxi = pendulum(t,xi,u,x_ob,theta)
    dxi = zeros(2,1); %dxiの型を定義
    dxi(1) = xi(2);
    dxi(2) = -theta*sin(xi(1)) - (xi(1)-x_ob(1)) - xi(2) + u;
end


%pendulumのモデル
function dx = func(xi,u,x_ob,theta)
    dx = zeros(2,1); %dxiの型を定義
    dx(1) = xi(2);
    dx(2) = -theta*sin(xi(1)) - (xi(1)-x_ob(1)) - xi(2) + u;
end


%theta_hatの更新用の関数
function w = Calcw(x, x_ob, theta_hat, beta, u)
    w = (x(2)^2) * cos(x(1)) + sin(x(1)) * (-(theta_hat+beta)*sin(x(1))...
        - (x(1) - x_ob(1)) - x(2) + u);
end

%beta計算の関数
function beta = Calcbeta(x)
    beta = -x(2)*sin(x(1));
end
7
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?