MATLABで学ぶ状態空間モデルの離散化 - MATLABパイセンが教える振動・騒音・音響・機械工学

はじめに
状態空間モデル（N自由度のバネ-マス-ダンパモデル）
離散化1：差分方程式からの離散化
離散化2：ZOH（Zero-Order Hold）
MATLABで解く状態空間モデル（N自由度モデル）
MATLABプログラム
1. 関連

はじめに

近年の海外大学のトレンドとして、純粋に振動工学や機械工学を深堀するような研究はされておらず、デジタルツインや状態監視、1DCAE、MBD、メタマテリアルといった研究やワードが目立つようになってきています。これらの分野は、振動工学＋制御工学＋IoT＋システムといったように、ひと昔前だと他分野と見なされた分野を横通しする必要があります。そのため、1人の教授が研究室運営をすることが多い日本の大学では先端の研究ができていないのが現状です。特に振動分野では上記研究テーマで優れた研究がなされていません。

今回紹介する「カルマンフィルタ」は状態変数を推定する手法で、状態変数＝振動とすると振動推定が可能になります。そのため、デジタルツインや状態監視などの分野で適用されています。

カルマンフィルタ自体は制御工学で非常に広く知られていて、制御工学では結構やりつくされた感がある理論です。ただし、制御工学では数自由度のモデルでの適用であり、振動工学で扱うような周波数帯域を扱えなかったのですが、海外大学では振動工学で扱うような周波数帯域への適用を目指しています。

上記のようなトレンドがありますが、振動工学を生業としている日本の方にはあまりなじみがないのではないでしょうか？そもそもトレンドを捉えていないのではないでしょうか？

日本の大学の文献はあまりいいのがないので、本記事では本リンクの論文を参考にしたいと思います。カルマンフィルタについては下記の著書を参考にしています。下記著書にはカルマンフィルタなどのMATLABプログラムが掲載されています。

なお、本記事ではカルマンフィルタを学習する前段階として、状態空間モデルの離散化について解説したいと思います。前回の記事を読んでいない方は下記も参考にしてください。

MATLABで学ぶ状態空間モデル（N自由度モデル）

N自由度のバネ-マス-ダンパモデルの状態空間モデル（状態方程式と出力方程式）について、MATLABのコードの示しつつ解説いたします。

状態空間モデル（N自由度のバネ-マス-ダンパモデル）

前回の記事と重複するので、簡単におさらい部分だけ記載します。図1のようなN自由度の運動方程式は式(1)でしたね。なお、細かい文字式についての説明は前回の記事を参照ください。

図1　バネマスモデル（N自由度）

$[M][\ddot{y} (t)] + [C][\dot{y} (t)] + [K][y(t)]=[u(t)] \tag{1}$

式(1)の状態方程式は式(3)になり、出力方程式は式(4)になりましたね。

$\left( \begin{array}{c} [ \dot{x_1}] \\ [ \dot{x_2}] \end{array} \right) = \left( \begin{array}{cc} [0] & [I] \\ -[M]^{-1}[K] & -[M]^{-1}[C] \end{array} \right) \left( \begin{array}{c} [x_1] \\ [x_2] [u] \end{array} \right) + \left( \begin{array}{c} [0] \\ [M]^{-1} \end{array} \right) \tag{3}$

$[y]= \left( \begin{array}{cc} [I] & [0] \end{array} \right) \left( \begin{array}{c} [x_1] \\ [x_2] \end{array} \right) \tag{4}$

一般化すると式(5)のようになりましたね。

$\begin{eqnarray} \left\{ \begin{array}{l} [\dot{x} (t)] = [A][x(t)]+[B][u(t)] \\ [y(t)] = [C] [x(t)] \end{array} \right. \tag{5} \end{eqnarray}$

状態空間モデル（状態方程式と出力方程式）は連続時間系なので、カルマンフィルタを適用するためには離散化する必要があります。

先に説明しておきますが、離散化した状態方程式と出力方程式は式(6)になります。

$\begin{eqnarray} \left\{ \begin{array}{l} [x(k+1)] = [A_d][x(k)]+[B_d][u(k)] \\ [y(k+1)] = [C_d] [x(k)] \end{array} \right. \tag{6} \end{eqnarray}$

離散化1：差分方程式からの離散化

式(5)の速度 $\dot{x}$ は、dx/dtとも表せますよね。さらに簡易的に表記すると式(7)のように表記できます。なお、kは任意の離散時間を意味します。

$\frac{[x(k+1)]-[x(k)]}{Δt} = [A][x(k)]+[B][u(k)] \tag{7}$

式(7)のx(k+1)について解くと、式(8)になります。

$[x(k+1)] = ([I]+[A]Δt)[x(k)]+[B]Δt[u(k)] \tag{8}$

式(8)と式(6)を対応させるとAdとBdが求まりますね。

少し余談ですが、式(8)を確認すると時刻kの変位x(k)とu(k)から、時刻k+1の変位x(k+1)が推定できることがわかります。
これはすごいことですよね。過去の情報から未来が推定できることになります。しかも、時刻kから時刻k+1が推定できるということは、時刻k+1から時刻k+2が推定でき…..というように、入力u(k)が変化しない自由振動であれば、初期値さえわかれば、遠い未来の振動も推定できることを意味します。

離散化2：ZOH（Zero-Order Hold）

0入力応答と0状態応答の和の形で状態方程式の解を得る方法です。ただし、このとき入力がΔt間で一定という仮定しています。なお、詳細な式展開はwikiを確認ください。

$e^ {\left( \begin{array}{cc} [A] & [B] \\ [0] & [0] \end{array} \right)Δt}= \left( \begin{array}{cc} [A_d] & [B_d] \\ [0] & [I] \end{array} \right) \tag{9}$

MATLABで解く状態空間モデル（N自由度モデル）

では、下記の例題を解いてみましょう。

自由度N=10
k=10^5
c=1
m=1
サンプリング周波数fs=1048576（dt=1/fs）
初期値のx：m1の変位を1、m2～m10の変位を0
求めたい状態変数：変位

全自由度の変位と速度が求まりますが、ここではm10の変位の結果だけを紹介します。

図2　解法の比較

MATLABの「ode45」が正解(真値)と仮定すると、”離散化1″と”離散化2″の両手法の結果が一致していますね。

ただし、これはサンプリング周波数が非常に大きい場合には両手法の結果が一致するのであって、サンプリング周波数を小さくしていくと誤差が大きくなります。では、サンプリング周波数を変化させて、真値との誤差がどのように推移していくか確認してみましょう。その検証結果が図3です。

図3　離散化手法の精度検証

図3から、離散化手法2のZOHが圧倒的に精度が高いことがわかりますね。

MATLABプログラム

実行ファイル

clear all;clc;close all
 
m_vec=ones(1,10);
k_vec=ones(1,10)*10^5;
[M]=eval_Mmatrix(m_vec); %質量行列
[K]=eval_Kmatrix(k_vec); %剛性行列
C=K*10^-5;
 
[V,D]=eig(K,M); %固有値D、固有ベクトルV
wn=sqrt(diag(D)); %固有角振動数
fn=wn/(2*pi); %固有振動数
 
 
% % 状態方程式（連続時間系）
% % dx/dt =Ac x(t) + Bc p(t)
Ac=[zeros(size(M)) eye(size(M))
    -inv(M)*K -inv(M)*C];
Bc=[zeros(size(M))
    -inv(M)];
 
Cc=[-inv(M)*K -inv(M)*C];
p=zeros(size(M,1),1);
 
x0 = zeros(size(M,1)*2,1);
x0(1) =1;
t0 = 0;
dt=1/400;
tf = 10;
time=t0:dt:tf;
 
% % % % % % % % % % % MATLABのode45を使った場合(ここでは理論値・真値とします)
[T,Y] = ode45(@state_equation, time, x0); 
figure
plot(T,Y(:,10),'k','linewidth',2)
hold on
xlabel('Time [s]')
ylabel('Displacement [m]')
 
% % % % % 状態方程式（離散時間系）
% % % % % % % 離散化1：差分方程式からの離散化
fs=2^20;
 
t0 = 0;
dt=1/fs;
tf = 4;
time=t0:dt:tf;
Adis=eye(size(Ac)) + Ac*dt; %離散化したA
Bdis=Bc*dt; %離散化したB
 
xdis=zeros(size(Adis,1),length(time));
xdis(:,1) = x0;
for ii1=1:length(time)-1
    xdis(:,ii1+1)=Adis*xdis(:,ii1)+Bdis*p;
end
 
plot(time,xdis(10,:),'b-','linewidth',1)
 
 
% % % % % % % 離散化2：ZOH（Zero-Order Hold）
temp=[Ac Bc
    zeros(size(Bc,2),size(Ac,2)) zeros(size(Bc,2),size(Bc,2))];
temp2=expm(temp*dt);
Adis2=temp2(1:size(Ac,1),1:size(Ac,2));
Bdis2=temp2(1:size(Ac,1),size(Ac,2)+1:end);
 
xdis2=zeros(size(Adis,1),length(time));
xdis2(:,1) = x0;
for ii1=1:length(time)-1
    xdis2(:,ii1+1)=Adis2*xdis2(:,ii1)+Bdis2*p;
end
 
plot(time,xdis2(10,:),'r--','linewidth',1)
xlim([time(1) time(end)])
legend('時刻歴積分 ode45 （真値と仮定）','離散化1：差分方程式からの離散化','離散化2：ZOH')

functionファイル

function [M]=eval_Mmatrix(m_vec)
 
M=diag(m_vec);
 
end

function [K]=eval_Kmatrix(k_vec)
 
K=zeros(length(k_vec));
for ii1=1:length(k_vec)
    if ii1==1
        K(ii1,ii1)=k_vec(ii1);
    else
        K(ii1-1:ii1,ii1-1:ii1)=K(ii1-1:ii1,ii1-1:ii1)+[1 -1;-1 1]*k_vec(ii1);
    end
end
 
end

function dx=state_equation(t,x)
 
 
m_vec=ones(1,10);
k_vec=ones(1,10)*10^5;
[M]=eval_Mmatrix(m_vec); %質量行列
[K]=eval_Kmatrix(k_vec); %剛性行列
C=K*10^-5;
 
% [V,D]=eig(K,M); %固有値D、固有ベクトルV
% wn=sqrt(diag(D)); %固有角振動数
% fn=wn/(2*pi); %固有振動数
 
 
% % 状態方程式
% % dx/dt =Ac x(t) + Bc p(t)
Ac=[zeros(size(M)) eye(size(M))
    -inv(M)*K -inv(M)*C];
Bc=[zeros(size(M))
    -inv(M)];
 
% Cc=[-Sa*inv(M)*K -Sa*inv(M)*C];
% Dc=Sa*inv(M);
p=zeros(size(M,1),1);
% size(p)
% size(Bc)
% size(Bc*p)
% 
% size(Ac)
% size(Ac*x)
dx=Ac*x+Bc*p;
end