制御系CAD

制御を意識したモノつくりを目指して

SCILABの基礎事項

投稿日時: 2022年8月17日投稿者: cacsd

[0] はじめに
SCILABは対話型数値計算プログラムで、次からフリーで入手できます。
http://www.scilab.org/products/scilab/download

[1] 行列データの入力
１）サイズ２×２の行列
$A=\left[\begin{array}{cc}1&2\\3&4\end{array}\right]$
を定義するために、次を入力してみます。
–> A=[1 2;3 4]
ここで、行列データは[ ]で囲み、要素の区切りはスペースまたはカンマ、行替えはセミコロン；を用いることに注意してください。次のように最後に；を置くと、出力が抑制されます。
–> A=[1 2;3 4];
すでに入力した行列データへのアクセスは、次のように行います。
–> A
また、特定の要素へのアクセスや修正は、次のように行います。
–> A(2,1)
–> A(2,1)=-5

２）２次の単位行列
$B=\left[\begin{array}{cc}1&0\\0&1\end{array}\right]$
は、次により入力できます。
–> B=eye(2,2)
便利なのは、次のように異なる行数と列数が許されることです。
–> B=eye(2,3)
これは、次のサイズ２×３の行列
$B=\left[\begin{array}{ccc}1&0&0\\0&1&0\end{array}\right]$
の定義を意味します。

３）サイズ３×２の零行列
$C=\left[\begin{array}{cc}0&0\\0&0\\0&0\end{array}\right]$
は、次により定義できます。
–> C=zeros(3,2)

４）３次の対角行列
$D=\left[\begin{array}{ccc}1&0&0\\0&2&0\\0&0&3\end{array}\right]$
は、次により定義できます。
–> D=diag([1 2 3])
すなわち、ベクトルにdiagを作用させているわけですが、行列にdiagを作用させ、対角成分を抽出することもできます。
–> d=diag(D)

５）行列A,B,C,Dを小行列としてもつブロック行列
$E=\left[\begin{array}{cc}A&B\\C&D\end{array}\right]$
を定義するためには、次のように入力します。
–> E=[A B;C D]

６）サイズ２×２の複素行列
$F=\left[\begin{array}{cc}1+i&2+3i\\3+2i&4+4i\end{array}\right]$
も、次により定義できます。
–> F=[1+%i, 2+3*%i;3+2*%i, 4+4*%i]
ここで、%iは虚数単位を意味します。

７）SCILABでは、空行列を定義できます。
–> G=[]

８）すべての要素が1である行列
$H=\left[\begin{array}{cc}1&1\\1&1\end{array}\right]$
は、次により定義できます。
–> H=ones(2,2)

[2] 行列データの管理
１）現時点でどのような行列を定義したかを調べるためには、次のように入力します。
–> who_user
また、たとえば入力した行列データdを消去するためには、次のように入力します。
–> clear d
すべてのデータを消去するときは、次のように入力します。
–> clear

２）行列データEのサイズを調べるには、次のように入力します。
–> [m,n]=size(E)

３）浮動小数点表示を行うためには、次のように入力します。
–> format(’e’)
これを固定小数点表示に戻すためには、次のように入力します。
–> format(’v’)

４）カレントディレクトリのチェックを行うためには、次のコマンドが使えます
–> pwd
–> ls
カレントディレクトリの変更のためには、次のコマンドを与えます。
–> cd(’dname’)

５）メモリ上のデータのセーブとロードのためには、次のコマンドを与えます。
–> save(’fname’)
–> load(’fname’)

[3] コロン記法
１）まず、次を入力してみてください。
–> h=1:3
これは、「form 1 to 3」を意味します。すなわちサイズ1×5の行列h=[1 2 3]が生成されます。コロン「:」はtoの意味で使われ、colon notationと呼ばれています。

２）それでは、次はどうでしょう。
–> h=0:0.5:3
これは、「form 1 to 3 by 0.5」を意味します。すなわち、２つのコロンで挟まれた0.5は増分を意味し、サイズ1×5の行列h=[1 1.5 2 2.5 3]が生成されます。ちなみに、h=1:3はh=1:1:3と同じと考えておきましょう。

３）それでは、次はどうでしょう。
–> h=0:-2
これは、h=1:1:-2の意味になりますが、1から-2まで、増分1では到達できないので、空行列となります。

[4] 小行列の抽出
１）コロン記法の利点は，行列の添え字部分に適用できることにあります。たとえば，行列Eの(2,3)要素には
–> E(2,3)
でアクセスできるのですが，この2と3は，スカラーである必要はなく，ベクトルが使えます。たとえば，次を確かめてください。
–> E([1 3],: )
これは第１行と弟２行からなるサイズ2×5の小行列が取り出されることに注意してください。列を指定する「：」はすべての列を意味しています。

２）次はどうでしょう。
–> E(:,2:4 )
ここで、2:4は[2 3 4]というベクトルでしたよね。第2～4列からなるサイズ5×3の小行列が取り出されることに注意してください。行を指定する「：」はすべての行を意味しています。

３）それでは，次はどうでしょう。
–> E([1 3],2:4 )

[5] 行列の分割
１）いろいろな応用では、ある行列を分割する必要が出てきます。これは小行列の抽出の観点から、たとえば次のようにできます。
–> [m,n]=size(E); i=2; j=2;
–> A=E(1:i,1:j);
–> B=E(1:i,j+1:n);
–> C=E(i+1:m,1:j);
–> D=E(i+1:m,j+1:n);
ここで、i=0のときは1:i=[]、i=mのときはi+1:m=[]などに注意してください。

[6] 行列演算
いま、行列Aが次のように定義されていることを確認してください。
$A=\left[\begin{array}{cc}1&2\\-5&4\end{array}\right]$
また、行列Bを新しく定義します。
–> B=eye(A)

１）行列A,Bの和、差、乗算は次のように計算できます。
–> X=A+B
–> X=A-B
–> X=A*B

２）行列Aが正則のとき、その逆行列は
–> X=inv(A)
で計算されます。しかし、これは逆行列の要素が欲しいとき以外は使わないようにしましょう。
多くの場合、逆行列は次の連立一次方程式
$AX=B$
または
$XA=B$
を解くときに用いますが、SCILABでは、次のように計算します。
–> X=A\B
–> X=B/A
それぞれ左からの割り算、右からの割り算と呼ばれます。
これは数値的に安定な数値計算アルゴリズムを得るためは直交行列だけの演算ですませることが必要だからです。

３）行列Aのべき乗と転置共役は次のように計算でされます。
–> X=A^2
–> X=A’

４）それでは、次の演算結果と比較してみましょう。
–> X=A.*B
–> X=A.\B
–> X=B./A
–> X=A.^2
–> X=A.’
すなわち、演算子の前に「ドット．」をおくと、要素ごとの演算やただの転置になります。

５）次に、組み込み関数としては、次が使えます。
sin,cos,tan
asin,acos,atan
sinh,cosh,tanh
asinh,acosh,atanh
sqrt
exp,log,log10
abs
real,imag

６）行列関数としては次が準備されています。
–> X=expm(A)
–> X=norm(A)
–> X=rank(A)
–> X=det(A)
ぞれぞれ、行列指数関数、ノルム、階数、行列式を計算します。

７）また、固有値問題を解くには次の関数を用います。
–> [V,R]=spec(A)
単に固有値だけを求めるのであれば
–> r=spec(A)
でＯＫです。

８）さらに、特異値問題を解くには次の関数を用います。
–> [U,S,V]=svd(A)
単に特異値だけを求めるのであれば
–> s=svd(A)
でＯＫです。

９）多項式 $x+1$ と $x+2$ の掛け算は、次のように行います。
–> r=convol([1 1],[1 2])

１０）いま、次のベクトルを定義します。
–> x=[3 1 5 4 2]
このとき、次はxの要素を大きい順番に並べ替えます。
–> [y,i]=gsort(x)
yがソーティングされた結果、iが元のどの位置にあったかを示すインデックスです。たとえば
–> x（i)
としてみるとyと同じ結果が出ます。

１１）次は、ベクトルの要素和、最大値、最小値を求める関数です。
–> y=sum(x)
–> y=max(x)
–> y=min(x)

[7] 関数のグラフ
１）次の関数のグラフを描いてみます。
$p(x)=\frac{1}{2\pi}e^{-\frac{x^2}{2}}\quad(-3\le x\le3)$
次のコマンドを与えてください。
–> x=-3:0.1:3;
–> y=1/sqrt(2*%pi)*exp(-x.*x/2)；
–> plot(x,y)
ここで，なぜx*xでなく，x.*xとしているか考えてみてください。

２）描画範囲（-3<=x<=3, 0<=ｙ<=0.4）を指定し，グリッドを描画するには，次のようなコマンドを追加します。
–> mtlb_grid
–> mtlb_axis([-3 3 0 0.5])
–> xtitle(’N(0,1)’)
ここで，「mtlb_」はMATLABのコマンド相当という意味です。

３）次の１次系の周波数応答を片対数グラフを描いてみましょう。
$|\hat{g}(j\omega)|=20\log_{10}\frac{1}{\sqrt{1+\omega^2}}\quad(0.1\le x\le10)$
次のコマンドを与えてください。
–> w=logspace(-1,1);
–> ga=20*log10(sqrt(1+w.^2).^(-1));
–> plot2d(w,ga,logflag=’ln’)
–> mtlb_grid

[8] プログラミング
これまでは対話型入力を行ってきましたが，一連のコマンドをエディタを用いてあらかじめ編集しておいて一括処理する，すなわちscilabでのプログラミングについて学びます。まず，白紙を示すアイコンをクリックして，エディタscipadを起動してください。

１） 1から9までの奇数の和を求める次のプログラムを入力し，ファイル名ex1.sceでセーブします。
//ex1.sce
//—–
s=0;
for i=1:2:9, s=s+i; end
disp(s,’sum=’)
//—–
//eof
scipadのexecuteコマンドをクリックして実行します。答えは25のはずです。

２）マシンイプシロン(machine epsilon)と呼ばれる特別な定数を求めてみましょう。次のプログラムをファイル名ex2.sceでセーブします。
//ex2.sce
//—–
s=1;
while 1+s>1, s=s/2; end
s=s*2;
disp(s,’eps=’),
//—–
//eof
scipadのexecuteコマンドをクリックして実行します。epsは次の%epsと等しくなります。
–-> %eps
次を実行してみてください。
–> 1+%eps/2==1
すなわち，
マシンイプシロン%eps=2^{-k}は，1+2^{-k-1}>1が成り立たなくなる最小のkを用いて定義されます。浮動小数点による数値計算では，1+%eps=1となる正数が存在することに注意してください。

３）次は，if文を使った例です。次のプログラムをファイル名ex3.sceでセーブします。
//ex3.sce
//—–
x=input(’x= ‘);
if imag(x)~=0, disp(’complex number’)
elseif x>0, disp(’positive real number’)
elseif x<0, disp(’negative real number’)
else disp(’zero’)
end
//—–
//eof
scipadのexecuteコマンドをクリックして実行します。

４）関数の例として，行列の階数を計算するものを示しましょう。次のプログラムをファイル名rank2.sceでセーブします。
//rank2.sce
//—–
function r=rank2(A)
s=svd(A); tol=max(size(A))*s(1)*%eps; r=sum(s>tol);
endfunction
//—–
//eof
このとき，次を入力してください。
–> exec(‘rank2.sce’)
–> r=rank2([1 2;3 4])

５）次の微分方程式の解を求めることを考えます。
$\dot{x}(t)=-x(t)+\sin(10t)$
次のプログラムをファイル名ex4.sceでセーブします。
//ex4.sce
//—–
function dx=f(t,x), dx=-x+sin(10*t); endfunction
x0=1; t0=0; t=0:0.01:5;
x=ode(x0,t0,t,f);
plot(t,x), mtlb_grid
//—–
//eof
scipadのexecuteコマンドをクリックして実行します。

MATLABの基礎事項

投稿日時: 2022年8月17日投稿者: cacsd

[1] 行列データの入力 　
１）サイズ２×２の行列
$A=\left[\begin{array}{cc}1&2\\3&4\end{array}\right]$
を定義するために、次を入力してみます。
>> A=[1 2;3 4]
ここで、行列データは[ ]で囲み、要素の区切りはスペースまたはカンマ、行替えはセミコロン；を用いることに注意してください。次のように最後に；を置くと、出力が抑制されます。
>> A=[1 2;3 4];
すでに入力した行列データへのアクセスは、次のように行います。
>> A
また、特定の要素へのアクセスや修正は、次のように行います。
>> A(2,1)
>> A(2,1)=-5

２）２次の単位行列
$B=\left[\begin{array}{cc}1&0\\0&1\end{array}\right]$
は、次により入力できます。
>> B=eye(2,2)
便利なのは、次のように異なる行数と列数が許されることです。
>> B=eye(2,3)
これは、次のサイズ２×３の行列
$B=\left[\begin{array}{ccc}1&0&0\\0&1&0\end{array}\right]$
の定義を意味します。

３）サイズ３×２の零行列
$C=\left[\begin{array}{cc}0&0\\0&0\\0&0\end{array}\right]$
は、次により定義できます。
>> C=zeros(3,2)

４）３次の対角行列
$D=\left[\begin{array}{ccc}1&0&0\\0&2&0\\0&0&3\end{array}\right]$
は、次により定義できます。
>> D=diag([1 2 3])
すなわち、ベクトルにdiagを作用させているわけですが、行列にdiagを作用させ、対角成分を抽出することもできます。
>> d=diag(D)

５）行列A,B,C,Dを小行列としてもつブロック行列
$E=\left[\begin{array}{cc}A&B\\C&D\end{array}\right]$
を定義するためには、次のように入力します。
>> E=[A B;C D]

６）サイズ２×２の複素行列
$F=\left[\begin{array}{cc}1+i&2+3i\\3+2i&4+4i\end{array}\right]$
も、次により定義できます。
>> F=[1+i, 2+3*i;3+2*i, 4+4*i]
ここで、iは虚数単位として定義されています。もちろん再定義可能です。

７）MATLABでは、空行列を定義できます。
>> G=[]

８）すべての要素が1である行列
$H=\left[\begin{array}{cc}1&1\\1&1\end{array}\right]$
は、次により定義できます。
>> H=ones(2,2)

[2] 行列データの管理
１）現時点でどのような行列を定義したかを調べるためには、次のように入力します。
>> who
行列のザイズを合わせて表示するには、次のように入力します。
>> whos
また、たとえば入力した行列データdを消去するためには、次のように入力します。
>>clear d
すべてのデータを消去するときは、次のように入力します。
>>clear

２）行列データEのサイズを調べるには、次のように入力します。
>> [m,n]=size(E)

３）浮動小数点表示を行うためには、次のように入力します。
>> format short e
これを固定小数点表示に戻すためには、次のように入力します。
>> format

４）カレントディレクトリのチェックを行うためには、次のコマンドが使えます
>> pwd
>> ls
カレントディレクトリの変更のためには、次のコマンドを与えます。
>> cd dname

５）メモリ上のデータのセーブとロードのためには、次のコマンドを与えます。
>> save filename
>> load filename

[3] コロン記法
１）まず、次を入力してみてください。
>> h=1:3
これは、「form 1 to 3」を意味します。すなわちサイズ1×5の行列h=[1 2 3]が生成されます。コロン「:」はtoの意味で使われ、colon notationと呼ばれています。

２）それでは、次はどうでしょう。
>> h=0:0.5:3
これは、「form 1 to 3 by 0.5」を意味します。すなわち、２つのコロンで挟まれた0.5は増分を意味し、サイズ1×5の行列h=[1 1.5 2 2.5 3]が生成されます。ちなみに、h=1:3はh=1:1:3と同じと考えておきましょう。

３）それでは、次はどうでしょう。
>> h=0:-2
これは、h=1:1:-2の意味になりますが、1から-2まで、増分1では到達できないので、空行列となります。

[4] 小行列の抽出
１）コロン記法の利点は，行列の添え字部分に適用できることにあります。たとえば，行列Eの(2,3)要素には
>> E(2,3)
でアクセスできるのですが，この2と3は，スカラーである必要はなく，ベクトルが使えます。たとえば，次を確かめてください。
>> E([1 3],: )
これは第１行と弟２行からなるサイズ2×5の小行列が取り出されることに注意してください。列を指定する「：」はすべての列を意味しています。

２）次はどうでしょう。
>> E(:,2:4 )
ここで、2:4は[2 3 4]というベクトルでしたよね。第2～4列からなるサイズ5×3の小行列が取り出されることに注意してください。行を指定する「：」はすべての行を意味しています。

３)それでは，次はどうでしょう。
>> E([1 3],2:4 )

[5] 行列の分割
１）いろいろな応用では、ある行列を分割する必要が出てきます。これは小行列の抽出の観点から、たとえば次のようにできます。
>> [m,n]=size(E); i=2; j=2;
>> A=E(1:i,1:j);
>> B=E(1:i,j+1:n);
>> C=E(i+1:m,1:j);
>> D=E(i+1:m,j+1:n);
ここで、i=0のときは1:i=[]、i=mのときはi+1:m=[]などに注意してください。

[6] 行列演算
いま、行列Aが次のように定義されていることを確認してください。
$A=\left[\begin{array}{cc}1&2\\-5&4\end{array}\right]$
また、行列Bを新しく定義します。
>> B=eye(A)

１）行列A,Bの和、差、乗算は次のように計算できます。
>> X=A+B
>> X=A-B
>> X=A*B

２）行列Aが正則のとき、その逆行列は
>> X=inv(A)
で計算されます。しかし、これは逆行列の要素が欲しいとき以外は使わないようにしましょう。
多くの場合、逆行列は次の連立一次方程式
$AX=B$
または
$XA=B$
を解くときに用いますが、MATLABでは、次のように計算します。
>> X=A\B
>> X=B/A
それぞれ左からの割り算、右からの割り算と呼ばれます。
これは数値的に安定な数値計算アルゴリズムを得るためは直交行列だけの演算ですませることが必要だからです。

３）行列Aのべき乗と転置共役は次のように計算でされます。
>> X=A^2
>> X=A’

４）それでは、次の演算結果と比較してみましょう。
>> X=A.+B
>> X=A.-B
>> X=A.*B
>> X=A.\B
>> X=B./A
>> X=A.^2
>> X=A.’
すなわち、演算子の前に「ドット．」をおくと、要素ごとの演算やただの転置になります。

５）次に、組み込み関数としては、次が使えます。
sin,cos,tan,asin,acos,atan,sinh,cosh,tanh,asinh,acosh,atanh,sqrt,exp,log,log10,abs,real,imag

６）行列関数としては次が準備されています。
>> X=expm(A)
>> X=norm(A)
>> X=rank(A)
>> X=det(A)
ぞれぞれ、行列指数関数、ノルム、階数、行列式を計算します。

７）また、固有値問題を解くには次の関数を用います。
–>[V,R]=eig(A)
単に固有値だけを求めるのであれば
–>r=eig(A)
でＯＫです。

８）さらに、特異値問題を解くには次の関数を用います。
–>[U,S,V]=svd(A)
単に特異値だけを求めるのであれば
–>s=svd(A)
でＯＫです。

９）多項式 $x+1$ と $x+2$ の掛け算は、次のように行います。
–>r=convol([1 1],[1 2])

１０）いま、次のベクトルを定義します。
–>x=[3 1 5 4 2]
このとき、次はxの要素を小さい順番に並べ替えます。
–>[y,i]=sort(x)
yがソーティングされた結果、iが元のどの位置にあったかを示すインデックスです。たとえば
–>x（i)
としてみるとyと同じ結果が出ます。

１１）次は、ベクトルの要素和、最大値、最小値を求める関数です。
–>y=sum(x)
–>y=max(x)
–>y=min(x)

[7] 関数のグラフ
１）次の関数のグラフを描いてみます。
$p(x)=\frac{1}{2\pi}e^{-\frac{x^2}{2}}\quad(-3\le x\le3)$
次のコマンドを与えてください。
>> x=-3:0.1:3;
>> y=1/sqrt(2*pi)*exp(-x.*x/2)；
>> plot(x,y)
ここで，なぜx*xでなく，x.*xとしているか考えてみてください。またpiは $\pi$ として定義されています。

２）描画範囲（-3<=x<=3, 0<=ｙ<=0.4）を指定し，グリッドを描画するには，次のようなコマンドを追加します。
>> grid
>> axis([-3 3 0 0.5])
>> xtitle(’N(0,1)’)

３）次の１次系の周波数応答を片対数グラフび描いてみましょう。
$|\hat{g}(j\omega)|=20\log_{10}\frac{1}{\sqrt{1+\omega^2}}\quad(0.1\le x\le10)$
次のコマンドを与えてください。
>> w=logspace(-1,1);
>> ga=20*log10(sqrt(1+w.^2).^(-1));
>> semilogx(w,ga)
>> grid

[8] プログラミング
これまでは対話型入力を行ってきましたが，一連のコマンドをエディタを用いてあらかじめ編集しておいて一括処理する，すなわちMATLABでのプログラミングについて学びます。まず，新規スクリプトを示すアイコンをクリックして，エディタを起動してください。

１） 1から9までの奇数の和を求める次のプログラムを入力し，ファイル名ex1.mでセーブします。
//ex1.m
//—–
s=0;
for i=1:2:9,
s=s+i;
end
disp(s,’sum=’)
//—–
//eof
エディタの実行ボタンをクリックします。答えは25のはずです。

２）マシンイプシロン(machine epsilon)と呼ばれる特別な定数を求めてみましょう。次のプログラムをファイル名ex2.mでセーブします。
//ex2.m
//—–
s=1;
while 1+s>1,
s=s/2;
end
s=s*2;
disp(s,’eps=’),
//—–
//eof
エディタの実行ボタンをクリックします。このsは次のepsと等しくなります。
>> eps
次を実行してみてください。
>> 1+eps/2==1
すなわち，
マシンイプシロンeps=a^{-k}は，1+a^{-k-1}>1が成り立たなくなる最小のkを用いて定義されます。浮動小数点による数値計算では，1+eps=1となる正数が存在することに注意してください。

３）次は，if文を使った例です。次のプログラムをファイル名ex3.mでセーブします。
//ex3.m
//—–
x=input(’x= ‘);
if imag(x)~=0,
disp(’complex number’)
elseif x>0,
disp(’positive real number’)
elseif x<0,
disp(’negative real number’)
else
disp(’zero’)
end
//—–
//eof
エディタの実行ボタンをクリックします。。

４）関数の例として，行列の階数を計算するものを示しましょう。次のプログラムをファイル名rank2.mでセーブします。
//rank2.m
//—–
function r=rank2(A)
s=svd(A);
tol=max(size(A))*s(1)*eps; r=sum(s>tol);
endfunction
//—–
//eof
このとき，次を入力してください。
>> r=rank([1 2;3 4])

５）次の微分方程式の解を求めることを考えます。
$\dot{x}(t)=-x(t)+\sin(10t)$
次のプログラムをファイル名ex4.mでセーブします。
//ex4.m
//—–
function dx=f(t,x),dx=-x+sin(10*t), endfunction
x0=1; t0=0;
t=0:0.01:5;
x=ode(x0,t0,t,f);
plot(t,x),
mtlb_grid
//—–
//eof
エディタの実行ボタンをクリックします。

SVD

投稿日時: 2022年8月14日投稿者: cacsd

制御系CADに関わる数値計算では実行列の特異値分解(Singular-Value Decomposition, SVD)を多用しますが、データ駆動分野の計算でも基盤技術となっています。

●いまデータは次の複素行列にまとめられているとします。

$\displaystyle{(1)\quad X=\left[\begin{array}{ccccc} x_1 & x_2 & \cdots & x_m \end{array}\right]\in{\bf C}^{n\times m} \quad(x_1,x_2,\cdots,x_n\in{\bf C}^n) }$

ここで列ベクトル $x_1,x_2,\cdots,x_n$ はn次元数ベクトルですが、データ駆動分野では、nの値は数個ではなく、数千、数万のオーダーになります。

● $X$ をサイズ $n\times m$ の複素行列（ ${\rm rank}\,X=k$ ）とします。このとき、ユニタリ行列 $U$ と $V$ が存在して

$\displaystyle{(2.1)\quad X=U\Sigma V^H\quad(U\in{\bf C}^{n\times n},\Sigma\in{\bf R}^{n\times m},V\in{\bf C}^{m\times m}) }$

が成り立ちます（ $V^H=V^{*T}$ ）。ここで、実行列 $\Sigma$ は次のように表されます。

$\displaystyle{(2.2)\quad \Sigma= \left[\begin{array}{cc} \widehat{\Sigma} & 0_{k\times m-k}\\ 0_{n-k\times k} & 0_{n-k\times m-k} \end{array}\right]\in{\bf R}^{n\times m} }$

$\displaystyle{(2.3)\quad \widehat{\Sigma} ={\rm diag}\{\sigma_1,\cdots,\sigma_k\}\quad(\sigma_1\ge\cdots\ge\sigma_k>0) }$

MATLABのコマンドでは、次のように求めます。

>> [U,S,V]=svd(X)

●いま $\boxed{n\ge m}$ とします。このとき(2)は次式となります。

$\displaystyle{(3.1)\quad X= \underbrace{\left[\begin{array}{cc} U_1 & U_2 \end{array}\right]}_{U} \underbrace{\left[\begin{array}{cc} \Sigma_1 \\ 0_{n-m\times m} \end{array}\right]}_{\Sigma}V^H =U_1\Sigma_1V^H }$

$\displaystyle{(3.2)\quad \Sigma_1={\rm diag}\{\sigma_1,\cdots,\sigma_k, 0,\cdots,0\}\in{\bf R}^{m\times m} }$

MATLABのコマンドでは、次のように求めます。

>> [U1,S1,V]=svd(X,’econ’)

(2)の場合は $U$ は大規模な行列となりますが、(3)の場合はスリムな行列となりますので、Economic SVDと呼ぶことがあります。

●いま実効階数を $r$ とします。すなわち微小な $\epsilon>0$ に対して

$\displaystyle{(4)\quad \sigma_1\ge\cdots\ge\sigma_r> \epsilon >\sigma_{r+1}\ge\cdots\ge\sigma_k>0 }$

とします。このとき(3)から次式のように近似できます。

$\displaystyle{(5.1)\quad X\approx \widetilde{X}= \underbrace{\left[\begin{array}{cc} \widetilde{U} & \cdot \end{array}\right]}_{U} \underbrace{\left[\begin{array}{cc} \widetilde{\Sigma} & 0_{r\times m-r} \\ 0_{n-r\times r} & 0_{n-r\times m-r} \end{array}\right]}_{\Sigma'} \underbrace{\left[\begin{array}{c} \widetilde{V}^H\\ \cdot \end{array}\right]}_{V^H} =\widetilde{U}\widetilde{\Sigma}\widetilde{V}^H }$

$\displaystyle{(5.2)\quad \widetilde{\Sigma} ={\rm diag}\{\sigma_1,\cdots,\sigma_r\}\quad(\sigma_1\ge\cdots\ge\sigma_r>\epsilon) }$

●(2.1)、(3.1)、(5.1)の関係はダイアディック形式を用いて次のように表されます。

$\displaystyle{(6)\quad \underbrace{\sum_{i=1}^{m}\sigma_iu_iv_i^H}_{U\Sigma V^H} =\underbrace{\sum_{i=1}^{k}\sigma_iu_iv_i^H}_{U_1\Sigma_1 V^H} \approx \underbrace{\sum_{i=1}^{r}\sigma_iu_iv_i^H}_{\widetilde{U}\widetilde{\Sigma}\widetilde{V}^H} }$

●相関行列 $XX^H$ または $X^HX$ は、(3.1)より、次式のように表されます。

$\displaystyle{(7.1)\quad \begin{array}{l} XX^H=U\left[\begin{array}{cc} \Sigma_1 \\ 0_{n-m\times m} \end{array}\right] \underbrace{V^HV}_{I_m}\left[\begin{array}{cc} \Sigma_1 & 0_{m\times n-m} \end{array}\right] U^H\\=U\left[\begin{array}{cc} \Sigma_1^2 & 0_{m\times n-m} \\ 0_{n-m\times m} & 0_{n-m\times n-m} \end{array}\right] U^H \end{array} }$

$\displaystyle{(7.2)\quad X^HX=V\left[\begin{array}{cc} \Sigma_1 & 0_{m\times n-m} \end{array}\right] \underbrace{U^HU}_{I_n}\left[\begin{array}{cc} \Sigma_1 \\ 0_{n-m\times m} \end{array}\right] V^H=V\Sigma_1^2 V^H }$

これらより

$\displaystyle{(8.1)\quad \begin{array}{l} XX^HU=U\left[\begin{array}{cc} \Sigma_1^2 & 0_{m\times n-m} \\ 0_{n-m\times m} & 0_{n-m\times n-m} \end{array}\right] \end{array} }$

$\displaystyle{(8.2)\quad X^HXV=V\Sigma_1^2 }$

すなわち、非零の特異値は相関行列 $XX^H$ または $X^HX$ の固有値の正の平方根です。もし $X=X^H$ の場合は $X$ の特異値は $X$ の固有値の絶対値となります。これはスカラの場合、次式を意味しています。

$\displaystyle{(9)\quad \sigma=\sqrt{xx^*}=\sqrt{(x_R+jx_I)(x_R-jx_I)}=\sqrt{x_R^2+x_I^2}=|x| }$

データ駆動分野では、nの値は数個ではなく、数千、数万のオーダーになります。一方、ｍの値はそのように大きな値とはなりません。したがって、(8.2)の小規模な固有値問題を解いて $\Sigma_1$ と $V$ を求め、(3.1)に基づいて、(8.1)の大規模な固有値問題の固有ベクトルの一部 $U_1$ を、次式により求めることが行われます。

$\displaystyle{(10)\quad U_1=XV\Sigma_1^{-1} }$

(5.1)に基づく場合は次式となります。

$\displaystyle{(11)\quad \widetilde{U}=X\widetilde{V}\widetilde{\Sigma}^{-1} }$

このような手法はスナップショットと呼ばれています。

●観測データを行ベクトルとしてもつ $X$ を考えます。

$\displaystyle{(12)\quad X=\left[\begin{array}{c} X_{11},\cdots,X_{1m}\\ \vdots\\ %X_{i1},\cdots,X_{im}\\ %\vdots\\ X_{n1},\cdots,X_{nm}\\ \end{array}\right] }$

ここで、 $n$ 回の観測が行われ、各観測では $m$ 個のデータを得るものとしています。これから

$\displaystyle{(13)\quad \bar{X}=\left[\begin{array}{c} 1\\ \vdots\\ 1 \end{array}\right] \left[\begin{array}{c} \displaystyle{\frac{1}{n}\sum_{i=1}^nX_{i1},\cdots,\frac{1}{n}\sum_{i=1}^nX_{im}} \end{array}\right] }$

を引いた行列 $B$ の特異値分解を

$\displaystyle{(14)\quad B=X-\bar{X}=U\Sigma V^H }$

とします。このとき、共分散行列は

$\displaystyle{(15)\quad C=\frac{1}{n-1}BB^H=\frac{1}{n-1}V\Sigma^2 V^H\Rightarrow D=\frac{1}{n-1}\Sigma^2 }$

のように表されます。すなわち共分散行列の主成分ベクトルと、その方向に沿う不変分散値が、行列 $B$ の特異値分解から得られることに留意します。

Marine Craft

投稿日時: 2022年2月10日投稿者: cacsd

Marine Craft

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{\eta}=J_\Theta(\eta)\nu\\ M\dot{\nu}+C(\nu)\nu+D(\nu)\nu+g(\eta)=\tau\\ \tau=Bu \end{array}\right. }$

$1^\circ$ $M=M^T$ , $\dot{M}=0$
$2^\circ$ $C(\nu)=-C^T(\nu)$
$3^\circ$ $D(\nu)>0$
$4^\circ$ $BB^T$ は正則
$5^\circ$ $J_\Theta(\eta)$ はオイラー角変換行列

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)\ddot{\eta}+C^*(\nu,\eta)\dot{\eta}+D^*(\nu,\eta)\dot{\eta}+g^*(\eta)=J^{-T}_\Theta(\eta)\tau\\ M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{\eta}=\eta-\eta_d,\ \tilde{\nu}=\nu-\nu_d}\quad\Rightarrow\quad\dot{\tilde{\eta}}=J_\Theta(\eta)\tilde{\nu} }$

$\displaystyle{(3)\quad \dot{\eta}=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\dot{\tilde{\eta}}+\Lambda\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d+\Lambda\tilde{\eta}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=\dot{\eta}-s=\dot{\eta}_d-\Lambda\tilde{\eta}} }$

$\displaystyle{(6)\quad \dot{\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d=-\Lambda\tilde{\eta}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{\eta}^TK_p\tilde{\eta}}\quad(K_p>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{\eta}^TK_p\dot{\tilde{\eta}}=\tilde{\eta}^TK_p(-\Lambda\tilde{\eta}+s) \quad\Rightarrow\quad \boxed{\dot{V}_1=-\tilde{\eta}^TK_p\Lambda\tilde{\eta}+s^TK_p\tilde{\eta}} }$

●Step 2

$\displaystyle{(9)\quad s=\dot{\tilde{\eta}}+\Lambda\tilde{\eta}=\dot{\eta}-\dot{\eta}_d+\Lambda\tilde{\eta} \quad\Rightarrow\quad\dot{\eta}=s+\dot{\eta}_d-\Lambda\tilde{\eta} }$

$\displaystyle{(10)\quad \dot{s}=\ddot{\tilde{\eta}}+\Lambda\dot{\tilde{\eta}}=\ddot{\eta}-\ddot{\eta}_d+\Lambda\dot{\tilde{\eta}} \quad\Rightarrow\quad\ddot{\eta}=\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}} }$

$\displaystyle{(11)\quad \begin{array}{l} M^*(\eta)(\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}})\\ +C^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +D^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +g^*(\eta)=J^{-T}_\Theta(\eta)\tau \end{array} }$

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

$\displaystyle{(12)\quad \begin{array}{l} M^*(\eta)\dot{s}=-C^*(\nu,\eta)s-D^*(\nu,\eta)s+J^{-T}_\Theta(\eta)\tau\\ -M^*(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})-C^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-D^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-g^*(\eta)\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau -\underbrace{J^{T}_\Theta(\eta)M^*(\eta)J_\Theta(\eta)}_{M} \underbrace{J^{-1}_\Theta(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)C^*(\nu,\eta)J_\Theta(\eta)}_{C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)D^*(\nu,\eta)J_\Theta(\eta)}_{D(\nu)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)g^*(\eta)}_{g(\eta)})\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)) \end{array} }$

$\displaystyle{(13)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM^*(\eta)s} }$

$\displaystyle{(14)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM^*(\eta)\dot{s}+\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =-\tilde{\eta}^TK_p\Lambda\tilde{\eta}+s^TK_p\tilde{\eta}\\ +s^T(-C^*(\nu,\eta)s-D^*(\nu,\eta)s)\\ +s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}) -C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu}) -D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta))\\ +\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\+J^{T}_\Theta(\eta)K_p\tilde{\eta}) +\frac{1}{2}\underbrace{s^T(\dot{M}^*(\eta)-C^*(\nu,\eta))s}_{0}\\ -s^TD^*(\nu,\eta)s-\tilde{\eta}^TK_p\Lambda\tilde{\eta} \end{array} }$

$\displaystyle{(15)\quad \boxed{\begin{array}{l} \tau=M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})+C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\ -J^{T}_\Theta(\eta)K_p\tilde{\eta}-J^{T}_\Theta(\eta)K_ds \end{array}} }$

$\displaystyle{(14')\quad \boxed{\dot{V}_2=-s^T(D^*(\nu,\eta)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

MIMO Backstepping

投稿日時: 2022年2月1日投稿者: cacsd

MIMO Mass-Damper-Spring System

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{x}=x-x_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{x}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{x}}=v-v_d+\Lambda\tilde{x}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{x}} }$

$\displaystyle{(6)\quad \dot{\tilde{x}}=\tilde{v}=v-v_d=-\Lambda\tilde{x}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{x}^TK_p\tilde{x}}\quad(K_p>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{x}^TK_p\dot{\tilde{x}}=\tilde{x}^TK_p(-\Lambda\tilde{x}+s) \quad\Rightarrow\quad \boxed{\dot{V}_1=-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}} }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{\tilde{v}}+\Lambda\dot{\tilde{x}}=\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}} }$

$\displaystyle{(10)\quad \begin{array}{l} M\dot{s}+D(v)s=M(\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}})+D(v)(v-v_d+\Lambda\tilde{x})\\ =M\dot{v}+D(v)v-M(\dot{v}_d-\Lambda\dot{\tilde{x}})-D(v)(v_d-\Lambda\tilde{x})\\ =Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TMs} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM\dot{s}\\ =-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}\\ +s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s)\\ =s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(13)\quad \boxed{u=B^\dag(M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds)\quad(K_d>0) }$

$\displaystyle{(12')\quad \boxed{\dot{V}_2=-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

with Actuator Dynamics

$\displaystyle{(14)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu\\ T\dot{u}+u=u_c \end{array}\right. }$

●Step 3

$\displaystyle{(15)\quad Bu=z+\alpha_2\quad\Rightarrow\quad\dot{z}=B\dot{u}-\dot{\alpha}_2=BT^{-1}(u_c-u)-\dot{\alpha}_2 }$

$\displaystyle{(16)\quad \alpha_2=M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds }$

$\displaystyle{(12'')\quad \begin{array}{l} \dot{V}_2=s^T(z+\alpha_2-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(17)\quad \boxed{V_3=V_2+\frac{1}{2}z^Tz} }$

$\displaystyle{(18)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+z^T\dot{z}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x}+z^T(B\dot{u}-\dot{\alpha}_2)\\ =z^T(BT^{-1}(u_c-u)-\dot{\alpha}_2+s)-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(19)\quad \boxed{u_c=u+TB^\dag(\dot{\alpha}_2-s-K_zz)} }$

$\displaystyle{(18')\quad \boxed{\dot{V}_3=-z^TK_zz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

MIMO Backstepping of Robots

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{q}=v\\ M(q)\dot{v}+C(q,v)v+g(q)=\tau \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{q}=q-q_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{q}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{q}}=v-v_d+\Lambda\tilde{q}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{q}} }$

$\displaystyle{(6)\quad \dot{\tilde{q}}=\tilde{v}=v-v_d=-\Lambda\tilde{q}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{q}^TK_q\tilde{q}}\quad(K_q>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{q}^TK_q\dot{\tilde{q}}=\tilde{q}^TK_q(-\Lambda\tilde{q}+s) }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{v}-\dot{\alpha}_1 }$

$\displaystyle{(10)\quad \begin{array}{l} M(q)\dot{s}=M(q)\dot{v}-M(q)\dot{\alpha}_1\\ =\tau-C(q,v)v-g(q)-M(q)\dot{\alpha}_1\\ =\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM(q)s} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM(q)\dot{s}+\frac{1}{2}s^T\dot{M}(q)s\\ =\tilde{q}^TK_q(-\Lambda\tilde{q}+s)\\ +s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s)\\ +\frac{1}{2}s^T\dot{M}(q)s\\ =s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ +\underbrace{s^T(\frac{1}{2}\dot{M}(q)-C(q,v))s}_{0}-\tilde{q}^TK_q\Lambda\tilde{q}\\ \end{array} }$

$\displaystyle{(13)\quad \boxed{\tau=M(q)(\dot{v}-\dot{s})+C(q,v)(v-s)+g(q)-K_ds-K_q\tilde{q}} }$

$\displaystyle{(12')\quad \begin{array}{l} \dot{V}_2=s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ -\tilde{q}^TK_q\Lambda\tilde{q}\\ =-s^TK_ds-\tilde{q}^TK_q\Lambda\tilde{q} \end{array} }$

SISO BackStepping

投稿日時: 2022年2月1日投稿者: cacsd

予備的考察

BackSteppingの邦訳は、読み方のバックステッピング以外には適切なものがないようですが、非線形系に対する一つの制御方式を表しています。そこで、以下ではBackSteppingをBS制御と呼ぶことにします。

BS制御の基本的なアイデアを、次の例を用いて説明します。

$\displaystyle{ \begin{array}{cll} (1.1) & \dot{x}_1=f(x_1)+x_2 & (x_1(0)\ne0)\\ (1.2) & \dot{x}_2=u & (x_2(0)=0)\\ (1.3) & y=x_1 \end{array} }$

ここで、(1.1)が非線形動作を、(1.2)がアクチュエータ動作を、(1.3)が観測式を表しています。したがって、制御対象は状態変数 $x_1,x_2$ をもつ２次元の非線形系です。

制御目的は状態変数 $x_1$ を速やかに零（平衡状態）に戻すこととします。

状態変数は $x_1,x_2$ の２つなので、BS制御系設計のために２回の変数変換を行います。

$\displaystyle{(*)\quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

以下では、変数変換後の状態変数 $z_1,z_2$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次を考えます。

$\displaystyle{(2)\quad \boxed{z_1=x_1} }$

また $x_2$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(3)\quad x_2=\alpha_1+z_2\quad\Rightarrow\quad\boxed{z_2=x_2-\alpha_1} }$

を考えます。このとき(1.1)は次式で表されます。

$\displaystyle{(4)\quad \dot{z}_1=f(z_1)+\alpha_1+z_2 }$

このフィードバック線形化を行うために

$\displaystyle{(5)\quad \boxed{\alpha_1=-f(z_1)-k_1z_1\quad(k_1>0)} }$

と定義すると、 $z_1$ の状態方程式として

$\displaystyle{(4')\quad \dot{z}_1=-k_1z_1+z_2 }$

を得ます。これに対するリャプノフ関数

$\displaystyle{(6)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(7)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(1.2)を用いて

$\displaystyle{(8)\quad \dot{z}_2=\dot{x}_2-\dot{\alpha}_1=u-\dot{\alpha}_1 }$

を得ます。これに対するリャプノフ関数を

$\displaystyle{(9)\quad \boxed{V_2=V_1+\frac{1}{2}z_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(10)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+z_2\dot{z}_2\\ =-k_1z_1^2+z_2(z_1+\dot{z}_2)\\ =-k_1z_1^2+z_2(z_1+u-\dot{\alpha}_1) \end{array} }$

ここで、操作入力を

$\displaystyle{(11)\quad \boxed{u=\dot{\alpha}_1-z_1-k_2z_2\quad(k_2>0)} }$

と選ぶと次式を得ます。

$\displaystyle{(10')\quad \boxed{\dot{V}_2=-k_1z_1^2-k_2z_2^2<0} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(12)\quad \left\{\begin{array}{l} z_1=x_1\\ z_2=x_2-\alpha_1\\ =x_2+f(x_1)+k_1x_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right.} }$

$\displaystyle{(13)\quad \left\{\begin{array}{l} x_1=z_1\\ x_2=z_2-f(z_1)-k_1z_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

変数変換後の閉ループ系は次式で表されます。

$\displaystyle{(14)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ \dot{z}_2=(\dot{\alpha}_1-z_1-k_2z_2)-\dot{\alpha}_1=-k_2z_2-z_1 \end{array}}\right. }$

すなわち

$\displaystyle{(15)\quad \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1 & 0\\ 0 & k_2 \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}+ \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} }$

このとき、２次安定性

$\displaystyle{(16)\quad V_2=\frac{1}{2}z^Tz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^T\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(17)\quad \boxed{||z(t)||\le ||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(K))} }$

が成り立ち、(13)から制御目的が達成されていることが分かります。

●フィードバック線形化(5)の実施においては、非線形関数 $f(z_1)= f(x_1)$ を正確にモデリングしておく必要がありますが、実際にはこれは一般には困難です。たとえば次の例を考えます。

$\displaystyle{(18)\quad f(x_1)=-a_0x_1-a_1x_1^2-a_2|x_1|x_1\quad(a_0>0, a_1>0, a_2>0) }$

ここで、 $a_0, a_1, a_2$ は未知とします。第１項と第３項は線形の減衰項とみなせますが、第２項は非線形の減衰力で打ち消す必要があります。たとえば、 $z_1=x_1$ に注意して

$\displaystyle{(19)\quad \alpha_1=\underbrace{-k_1z_1}_{linear\ damping}-\underbrace{\kappa_1 z_1^3}_{nonlinear\ damping}\quad(k_1>0, \kappa_1>0) }$

と選ぶとき、次式が成り立ちます。

$\displaystyle{(20)\quad \begin{array}{l} \dot{z}_1=f(z_1)+(\alpha_1+z_2)\\ =-a_0z_1-a_1z_1^2-a_2|z_1|z_1-(k_1+\kappa_1 z_1^2)z_1+z_2\\ =-(\underbrace{a_0+a_2|z_1|}_{good\ damping}+k_1)z_1\underbrace{-a_1z_1^2}_{bad\ damping}-\kappa_1 z_1^3+z_2 \end{array} }$

このとき、(6)の下で、次式を得ます。

$\displaystyle{(21)\quad \dot{V}_1=z_1\dot{z}_1=-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4+z_1z_2 }$

また、(9)の下で

$\displaystyle{(22)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4+z_1z_2+z_2(z_1+u-\dot{\alpha}_1) \end{array} }$

ここで、(11)のように $u=\dot{\alpha}_1-z_1-k_2z_2$ と選択すると

$\displaystyle{(23)\quad \begin{array}{l} \dot{V}_2 =-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4-k_2z_2^2 \end{array} }$

ここで、恒等式

$\displaystyle{(24)\quad \begin{array}{l} (\frac{1}{2\sqrt{\kappa_1}}x+\sqrt{\kappa_1}y)^2=\frac{1}{4\kappa_1}x^2+xy+\kappa_1y^2\\ \Lueftrightarrow -xy-\kappa_1y^2=-(\frac{1}{2\sqrt{\kappa_1}}x+\sqrt{\kappa_1}y)^2+\frac{1}{4\kappa_1}x^2 \end{array} }$

において、 $x=a_1z_1, y=z_1^2$ とおけば、次式を得ます。

$\displaystyle{(25)\quad \begin{array}{l} \dot{V}_2 =-(\frac{a_1}{2\sqrt{\kappa_1}}z_1+\sqrt{\kappa_1}z_1^2)^2+\frac{a_1^2}{4\kappa_1}z_1^2-(a_0+a_2|z_1|+k_1)z_1^2-k_2z_2^2\\ \le -(a_0+k_1-\frac{a_1^2}{4\kappa_1})z_1^2-k_2z_2^2 \end{array} }$

したがって、 $\kappa_1>0, k_1>\frac{a_1^2}{4\kappa_1}-a_0, k_2>0$ と選べば、 $\dot{V}_2<0$ を達成できます。すなわち、制御則(19)と(11)は、未知の $a_0, a_1, a_2$ を使わないので、BS制御はロバストであると言えます。

Mass-Damper-Spring System

次のMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{ \begin{array}{ll} (1.1) & \dot{x}=v\\ (1.2) & m\dot{v}+d(v)v+k(x)x=\tau\\ (1.3) & y=x \end{array} }$

ここで、 $x$ は位置を、 $v$ は速度を、 $m$ は質量を、 $d(v)$ は非線形の減衰係数を、 $k(x)$ は非線形のバネ係数を表しています。(1.1)と(1.2)が状態変数 $x,v$ をもつ非線形状態方程式、(1.3)が観測方程式です。

制御目的は、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau\\ y=x\\ e=y-y_d \end{array}\right. \Rightarrow \left\{\begin{array}{l} m\dot{v}+d(v)v+k(x)x=\tau\\ \dot{e}=v-\dot{y}_d \end{array}\right. }$

ここで、第１式が非線形動作を、第２式が追従誤差の振舞い(積分動作とは異なる)を表しています。

状態変数は $x_1=e$ 、 $x_2=\dot{e}$ の２つなので、BS制御系設計のために２回の変数変換を行います。

$\displaystyle{(*)\quad \boxed{ \left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e=y-y_d}\quad\Rightarrow\quad\dot{z}_1=v-\dot{y}_d }$

$v$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad v=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=v-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2-\dot{y}_d }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(7)\quad \boxed{\alpha_1=\dot{y}_d-(k_1+n_1(z_1))z_1}\quad(k_1>0, n_1(z_1)=\kappa_1|z_1|^{\nu_1}) }$

と定義すると、次式を得ます。

$\displaystyle{(6')\quad \dot{z}_1=-(k_1+n_1(z_1))z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-(k_1+n_1(z_1))z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(5)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{v}-\dot{\alpha}_1 }$

(3)を用いて次式を得ます。

$\displaystyle{(11)\quad m\dot{z}_2=m\dot{v}-m\dot{\alpha}_1=\tau-d(v)v-k(x)x-m\dot{\alpha}_1 }$

これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+mz_2\dot{z}_2\\ =-(k_1+n(z_1))z_1^2+z_1z_2+(\tau-d(v)v-k(x)x-m\dot{\alpha}_1)z_2 \end{array} }$

ここで、操作入力を

$\displaystyle{(14)\quad \begin{array}{l} \boxed{\tau=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2-n_2(z_2)z_2}\\ (k_2>0, n_2(z_1)=\kappa_2|z_2|^{\nu_2}) \end{array} }$

と選ぶと次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-(k_1+n_1(z_1))z_1^2-(k_2+n_2(z_2))z_2^2} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(15)\quad \left\{\begin{array}{l} z_1=e=x_1\\ z_2=v-\alpha_1\\ =v-(\dot{y}_d-(k_1+n_1(z_1))z_1)\\ =\dot{e}+(k_1+n_1(z_1))z_1\\ =x_2+(k_1+n_1(x_1))x_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right.} }$

$\displaystyle{(16)\quad \left\{\begin{array}{l} e=z_1\\ \dot{e}=z_2-(k_1+n_1(z_1))z_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

変数変換後の閉ループ系は次式で表されます。

$\displaystyle{(17)\quad \left\{\begin{array}{l} \dot{z}_1=-(k_1+n_1(z_1))z_1+z_2\\ m\dot{z}_2=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2-n_2(z_2)z_2\\ -d(v)v-k(x)x-m\dot{\alpha}_1\\ =-(k_2+n_2(z_2))z_2-z_1 \end{array}}\right. }$

すなわち

$\displaystyle{(18)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{cc} 1 & 0\\ 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1+n_1(z_1) & 0\\ 0 & k_2+n_2(z_2) \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}\\ + \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} \end{array} }$

このとき、２次安定性

$\displaystyle{(19)\quad V_2=\frac{1}{2}z^TMz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^TM\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(20)\quad \boxed{||z(t)||\le \beta||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(M^{-1}K), \beta=\sqrt{\frac{\sigma_1(M)}{\sigma_n(M)})} }$

が成り立ち、制御目的が達成されていることが分かります。

●平衡状態の安定化問題は、次のように目標軌道が一定の特別な場合と考えられます。

$\displaystyle{(21)\quad \left\{\begin{array}{l} y_d=0\\ \dot{y}_d=0 \end{array}\right. }$

簡単のため、 $n_1(z_1)=0$ 、 $n_1(z_1)=0$ とすると

$\displaystyle{(22)\quad \left\{\begin{array}{l} z_1=x\\ \alpha_1=-k_1z_1 \end{array}\right. }$

となり、次式を得ます。

$\displaystyle{(23)\quad \tau=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2 }$

これは、次のような非線形のPD制御とみなすことができます。

$\displaystyle{(24)\quad \tau=-K_P(x)x-K_D(v)v }$

実際、(4)より $z_1=x$ 、 $\dot{z}_1=v$ 、(5)より $z_2=v-\alpha_1=v+k_1z_1$ に注意して

$\displaystyle{(25)\quad \begin{array}{l} \tau=m(-k_1v)+d(v)v+k(x)x-z_1-k_2(v+k_1z_1)\\ =(d(v)-mk_1)v+(k(x)-1)x-k_2(v+k_1x)\\ =\underbrace{(d(v)-mk_1-k_2)}_{K_D(x)}v+\underbrace{(k(x)-1-k_1k_2)}_{K_P(x)}x \end{array} }$

以下では積分動作を導入するための２つのアプローチを示します。

Integrator Augmentation

外乱の影響を受ける次のMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{ \begin{array}{ll} (1.1) & \dot{x}=v\\ (1.2) & m\dot{v}+d(v)v+k(x)x=\tau+w\\ (1.3) & y=x \end{array} }$

ここで、 $w$ は外乱を表しています。

制御目的は、外乱に抗して、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。

積分動作を入れて状態方程式を拡大した基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\\ y=x\\ e=y-y_d \end{array}\right. \Rightarrow \left\{\begin{array}{l} m\dot{v}=\tau-d(v)v-k(x)x+w\\ \dot{e}=v-\dot{y}_d\\ \dot{e}_I=e \end{array}\right. }$

ここで、第１式が非線形動作を、第２式が追従誤差の振舞いを、第３式が積分動作を表しています。

状態変数は $x_1=e_I$ 、 $x_2={e}$ 、 $x_3=\dot{e}$ の３つなので、BS制御系設計のために３回の変数変換を行います。

$\displaystyle{(**)\quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3) \end{array}\right.} }$

以下では、変数変換後の状態変数 $z_1,z_2,z_3$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2,x3$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e_I}\quad\Rightarrow\quad\dot{z}_1=e }$

$e$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad e=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=e-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2 }$

を得ます。いま

$\displaystyle{(7)\quad \boxed{\alpha_1=-k_1z_1} }$

と定義すると

$\displaystyle{(6')\quad \dot{z}_1=-k_1z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(5)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{e}-\dot{\alpha}_1=v-\dot{y}_d-\dot{\alpha}_1 }$

$v$ に対する仮想的な操作変数 $\alpha_2$ を導入して、第３番目の変数変換

$\displaystyle{(11)\quad v=\alpha_2+z_3\quad\Rightarrow\quad \boxed{z_3=v-\alpha_2} }$

を考えます。このとき $z_2$ の状態方程式

$\displaystyle{(10')\quad \dot{z}_2=\alpha_2+z_3-\dot{y}_d-\dot{\alpha}_1 }$

を得ます。これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+(\alpha_2+z_3-\dot{y}_d-\dot{\alpha}_1)z_2 \end{array} }$

ここで

$\displaystyle{(14)\quad \boxed{\alpha_2=\dot{\alpha}_1+\dot{y}_d-z_1-k_2z_2} }$

と定義すると、次式を得ます。

$\displaystyle{(10'')\quad \dot{z}_2=-z_1-k_2z_2+z_3 }$

これから、次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-k_1z_1^2-k_2z_2^2+z_2z_3} }$

●Step 3

$z_3$ の状態方程式として、(11)から

$\displaystyle{(15)\quad \dot{z}_3=\dot{v}-\dot{\alpha}_2 }$

を得ます。このとき

$\displaystyle{(16)\quad \begin{array}{l} m\dot{z}_3=m\dot{v}-m\dot{\alpha}_2\\ =\tau+w-d(v)v-k(x)x-m\dot{\alpha}_2\\ =\tau-d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2\quad({\rm assuming}\ w=0) \end{array} }$

これに対するリャプノフ関数

$\displaystyle{(17)\quad \boxed{V_3=V_2+\frac{1}{2}mz_3^2} }$

に対して、次式を得ます。

$\displaystyle{(18)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+mz_3\dot{z}_3\\ =-k_1z_1^2-k_2z_2^2+(z_2+m\dot{z}_3)z_3\\ =-k_1z_1^2-k_2z_2^2+(z_2+\tau-d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2)z_3\\ \end{array} }$

ここで、操作入力を

$\displaystyle{(19)\quad \boxed{\tau=m\dot{\alpha}_2+d(v)\alpha_2+k(x)x-z_2-k_3z_3 }$

と選ぶと、次式を得ます。

$\displaystyle{(18')\quad \boxed{\dot{V}_3=-k_1z_1^2-k_2z_2^2-(d(v)+k_3)z_3^2} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(15)\quad \left\{\begin{array}{l} z_1=e_I=x_1\\ z_2=e-\alpha_1\\ =e+k_1e_I\\ =x_2+k_1x_1\\ z_3=v-\alpha_2\\ =v-(\dot{\alpha}_1+\dot{y}_d-z_1-k_2z_2)\\ =\dot{e}+k_1\dot{z}_1+z_1+k_2z_2\\ =x_3+k_1\dot{x}_1+x_1+k_2(x_2+k_1x_1)\\ \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3) \end{array}\right.} }$

$\displaystyle{(16)\quad \left\{\begin{array}{l} e_I=z_1\\ e=z_2-k_1z_1\\ \dot{e}=z_3-k_1\dot{z}_1-z_1-k_2z_2 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3) \end{array}\right.} }$

閉ループ系は次式で表されます。

$\displaystyle{(20)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ \dot{z}_2=-z_1-k_2z_2+z_3\\ m\dot{z}_2=m\dot{\alpha}_2+d(v)\alpha_2+k(x)x-z_2-k_3z_3\\ -d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2\\ =-(d(v)+k_3)z_3-z_2 \end{array}}\right. }$

すなわち

$\displaystyle{(21)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2\\ \dot{z}_3 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{ccc} k_1 & 0 & 0\\ 0 & k_2 & 0\\ 0 & 0 & d(v)+k_3 \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2\\ z_3 \end{array}\right]}_{z}\\+ \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -1 & 0 & 1\\ 0 & -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2\\ z_3 \end{array}\right]}_{z} \end{array} }$

このとき、２次安定性

$\displaystyle{(22)\quad V_3=\frac{1}{2}z^TMz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^TM\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(23)\quad \boxed{||z(t)||\le \beta||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(M^{-1}K), \beta=\sqrt{\frac{\sigma_1(M)}{\sigma_n(M)})} }$

が成り立ち、制御目的が達成されていることが分かります。

Integral Action by Constant Parameter Adaptation

次の定値外乱の影響を受けるMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ y=x \end{array}\right. }$

ここで、 $x$ は位置を、 $v$ は速度を、 $w$ は定値外乱を、 $m$ は質量を、 $d(v)$ は非線形の減衰係数を、 $k(x)$ は非線形のバネ係数を表しています。

制御目的は、定値外乱に抗して、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \begin{array}{l} \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ y=x\\ e=y-y_d=x-x_d \end{array}\right.\\ \quad\Downarrow\\ \left\{\begin{array}{l} m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ \dot{e}=v-\dot{y}_d=v-\dot{x}_d \end{array}\right. \end{array} }$

ここで、第１式が定値外乱下の非線形動作を、第２式が追従誤差の振舞いを表しています。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e=x-x_d}\quad\Rightarrow\quad\dot{z}_1=v-\dot{x}_d }$

$v$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad v=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=v-\alpha_1} }$

を考えます。このとき $z_2$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2-\dot{x}_d }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(7)\quad \boxed{\alpha_1=\dot{x}_d-k_1z_1} }$

と定義すると、次式を得ます。

$\displaystyle{(6')\quad \dot{z}_1=-k_1z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2+\frac{1}{2p}(\hat{w}-w)^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2+\frac{1}{p}(\hat{w}-w)\dot{\hat{w}}} }$

●Step 2

$z_2$ の状態方程式として、(5)と(7)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{v}-\dot{\alpha}_1=\dot{v}-(\ddot{x}_d-k_1\dot{z}_1)=\dot{v}-\ddot{x}_d+k_1(v-\dot{x}_d) }$

よって、(3)から次式を得ます。

$\displaystyle{(11)\quad \begin{array}{l} m\dot{z}_2=m\dot{v}-m\ddot{x}_d+mk_1(v-\dot{x}_d)\\ =\tau-d(v)v-k(x)x+w-m\ddot{x}_d+mk_1(v-\dot{x}_d) \end{array} }$

これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+mz_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+\frac{1}{p}(\hat{w}-w)\dot{\hat{w}}\\ +(\tau-d(v)v-k(x)x+w-m\ddot{x}_d+mk_1(v-\dot{x}_d))z_2 \end{array} }$

ここで、操作入力を

$\displaystyle{(14)\quad \boxed{\tau=d(v)(v-z_2)+k(x)x-\hat{w}+m\ddot{x}_d-mk_1(v-\dot{x}_d)-z_1-k_2z_2 }$

と選ぶと次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-k_1z_1^2-(k_2+d(v))z_2^2}+(\hat{w}-w)(\frac{1}{p}\dot{\hat{w}}-z_2) }$

ここで、外乱の予測式を

$\displaystyle{(15)\quad \boxed{\dot{\hat{w}}=pz_2} }$

と選べばよいことが分かります。

●閉ループ系は次式で表されます。

$\displaystyle{(16)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ m\dot{z}_2=d(v)(v-z_2)+k(x)x-\hat{w}+m\ddot{x}_d-k_1(v-\dot{x}_d)-z_1-k_2z_2\\ -d(v)v-k(x)x+w-m\ddot{x}_d+k_1(v-\dot{x}_d)\\ =-z_1-(k_2+d(v))z_2-(\hat{w}-w) \end{array}}\right. }$

すなわち

$\displaystyle{(17)\quad \underbrace{ \left[\begin{array}{cc} 1 & 0\\ 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1 & 1\\ -1 & -k_2-d(v) \end{array}\right]}_{A} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}+ \underbrace{ \left[\begin{array}{c} 0 \\ -1 \end{array}\right]}_{b} (\hat{w}-w) }$

$\displaystyle{(18)\quad \dot{\hat{w}}=-p \underbrace{ \left[\begin{array}{cc} 0 & -1 \end{array}\right]}_{b^T} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} }$

YMCPBのBS制御（暫定版）

投稿日時: 2022年2月1日投稿者: cacsd

Case１：３次元モデルに基づくBS制御

●制御対象YMCPBのピッチに関する運動方程式として次式を考えます。

$\displaystyle{(1)\quad \begin{array}{l} (I_y+J_y)\ddot{\theta}=D_b(H_{CG}-H_D)+T(H_T-H_{De}+H_e)\\ +D_eH_e+N_LL_L+N_eL_e+N_B(L_{CG}\cos\theta-L_B) -c_{\theta z}\dot{z}-c_{\theta\theta}\dot{\theta} \end{array} }$

これは次の形をとると考えられます。

$\displaystyle{(2)\quad J\ddot{\theta}(t)+D(\theta,\dot{\theta})\dot{\theta}+K(\theta)\theta(t)=B\theta_e(t)+w(t) }$

ここで、減衰係数 $D$ 、復元係数 $K$ は、 $\theta$ 、 $\dot{\theta}$ の非線形関数、 $w$ は外乱やモデル化誤差を表しています。また、 $\theta_e$ は船外機の取付角で、その動作は操作変数 $u_e$ を用いて、次式で表されます。

$\displaystyle{(3)\quad \dot{\theta}_e(t)=\Omega_e u_e(t)\quad(u_e(t)=0,\pm 1) }$

(2)と(3)を合わせた制御対象全体の非線形状態方程式を線形化すると次式となります。

$\displaystyle{(4)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ a_{63} & a_{66} & b_{62}\\ 0 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ 0 \\ \Omega_e \end{array}\right] }_{B} u_e(t)\\ +\underbrace{ \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ a_{62} & a_{64} & a_{65} & b_{61}\\ 0 & 0 & 0 & 0 \end{array}\right] \left[\begin{array}{c} z(t)-z^*\\ \dot{x}(t)-V^*\\ \dot{z}(t)\\ T(t)-T^* \end{array}\right] }_{w(t)} \end{array} }$

ここで、 $^*$ は平衡状態を表しています。

●以上から、制御対象は次式でモデル化されているとします。

$\displaystyle{(5)\quad \left\{\begin{array}{l} \dot{\theta}(t)=\omega(t)\\ \dot{\omega}(t)+a_{63}\theta(t)+a_{66}\omega(t)=b_{62}\theta_e(t)+w(t)\\ \dot{\theta}_e(t)=\Omega_e u_e(t) \end{array}\right. }$

制御目的は、まず船外機の取付角を $\theta_e^*$ に移動させること、その上で外乱に抗して船体のピッチ角の振動を抑え、平衡状態を保持することです。そのために、制御則には誤差

$\displaystyle{(6)\quad e(t)=\theta_e(t)-\theta_e^* }$

による積分動作

$\displaystyle{(7)\quad \dot{e}_I(t)=e(t) }$

を入れます。積分動作を入れて拡大した基礎式は次のようにまとめられます。

$\displaystyle{(8)\quad \left\{\begin{array}{l} \dot{\theta}(t)=\omega(t)\\ \dot{\omega}(t)+a_{66}\omega(t)+a_{63}\theta(t)=b_{62}\theta_e(t)+w(t)\\ \dot{\theta}_e(t)=\Omega_e u_e(t)\quad(\dot{e}(t)=\Omega_e u_e(t))\\ \dot{e}_I(t)=e(t) \end{array}\right. }$

ここで、第１式と第２式が非線形動作を、第３式がアクチュエータの動作を、第４式が積分動作を表しています。以下では、簡単のため $w=0$ とします。

状態変数は $x_1=\theta$ 、 $x_2=\omega$ 、 $x_3=\theta_e$ 、 $x_4=e_I$ の４つなので、BS制御系設計のために４回の変数変換を行います。

$\displaystyle{(9)\quad \boxed{ \begin{array}{l} z=\phi(x)\\ \left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3)\\ z_4=\phi_4(x_1,x_2,x_3,x_4)\\ \end{array}\right. \end{array} \Leftrightarrow \begin{array}{l} x=\phi^{-1}(z)\\ \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3)\\ x_4=\phi_4^{-1}(z_1,z_2,z_3,z_4)\\ \end{array}\right. \end{array}} }$

以下では、変数変換後の状態変数 $z_1,z_2,z_3,z_4$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2,x_3,x_4$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(10)\quad \boxed{z_1=\theta}\quad\Rightarrow\quad\dot{z}_1=\omega }$

また $\omega$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(11)\quad \omega=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=\omega-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式は

$\displaystyle{(12)\quad \dot{z}_1=\alpha_1+z_2 }$

となります。いま

$\displaystyle{(13)\quad \boxed{\alpha_1=-k_1z_1} }$

と定義すると

$\displaystyle{(12')\quad \dot{z}_1=-k_1z_1+z_2 }$

となります。これに対するリャプノフ関数

$\displaystyle{(14)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(15)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$\theta_e$ に対して仮想的な操作変数 $\alpha_2$ を導入して、第３番目の変数変換

$\displaystyle{(16)\quad \theta_e=\alpha_2+z_3\quad\Rightarrow\quad \boxed{z_3=\theta_e-\alpha_2} }$

を考えます。このとき $z_2$ の状態方程式として

$\displaystyle{(17)\quad \begin{array}{l} \dot{z}_2=\dot{\omega}-\dot{\alpha}_1\\ =b_{62}\theta_e-a_{63}\theta-a_{66}\omega-\dot{\alpha}_1\\ =b_{62}\alpha_2+b_{62}z_3-a_{63}z_1-a_{66}\alpha_1-a_{66}z_2-\dot{\alpha}_1\\ =b_{62}\alpha_2+b_{62}z_3-a_{63}z_1+a_{66}k_1z_1-a_{66}z_2+k_1\dot{z}_1\\ \end{array} }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(18)\quad \boxed{\alpha_2=\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)} }$

と定義すると、次式を得ます。

$\displaystyle{(17')\quad \dot{z}_2=-z_1-k_2z_2+b_{62}z_3 }$

これに対するリャプノフ関数を

$\displaystyle{(19)\quad \boxed{V_2=V_1+\frac{1}{2}z_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(20)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+(-z_1-k_2z_2+b_{62}z_3)z_2\\ =-k_1z_1^2-k_2z_2^2+b_{62}z_2z_3 \end{array} }$

●Step 3

$z_3$ の状態方程式として

$\displaystyle{(22)\quad \begin{array}{l} \dot{z}_3=\dot{\theta}_e-\dot{\alpha}_2 =\Omega_e u_e-\dot{\alpha}_2 \end{array} }$

このフィードバック線形化を行うために

$\displaystyle{(23)\quad \boxed{u_e=\frac{1}{\Omega_e}(\dot{\alpha}_2-b_{62}z_2-k_3z_3) }$

と定義すると、次式を得ます。

$\displaystyle{(22')\quad \begin{array}{l} \dot{z}_3 =\Omega_e\frac{1}{\Omega_e}(\dot{\alpha}_2-b_{62}z_2-k_3z_3)-\dot{\alpha}_2\\ =\dot{\alpha}_2-b_{62}z_2-k_3z_3-\dot{\alpha}_2\\ =-b_{62}z_2-k_3z_3 \end{array} }$

これに対するリャプノフ関数

$\displaystyle{(24)\quad \boxed{V_3=V_2+\frac{1}{2}z_3^2} }$

に対して、次式を得ます。

$\displaystyle{(25)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+z_3\dot{z}_3\\ =-k_1z_1^2-k_2z_2^2+b_{62}z_2z_3+(-b_{62}z_2-k_3z_3)z_3\\ =-k_1z_1^2-k_2z_2^2-k_3z_3^2 \end{array} }$

●Step 4

$e_I$ に対して仮想的な操作変数 $\alpha_3$ を導入して、第４番目の変数変換

$\displaystyle{(26)\quad e_I=\alpha_3+z_4\quad\Rightarrow\quad \boxed{z_4=e_I-\alpha_3} }$

を考えます。 $z_4$ の状態方程式は

$\displaystyle{(27)\quad \dot{z}_4=\dot{e}_I-\dot{\alpha}_3=e-\dot{\alpha}_3 }$

いま

$\displaystyle{(28)\quad \boxed{\dot{\alpha}_3=e+k_4z_4} }$

と定義すると

$\displaystyle{(27')\quad \dot{z}_4=e-(e+k_4z_4)=-k_4z_4 }$

これに対するリャプノフ関数

$\displaystyle{(29)\quad \boxed{V_4=V_3+\frac{1}{2}z_4^2} }$

に対して、次式を得ます。

$\displaystyle{(30)\quad \begin{array}{l} \dot{V}_4=\dot{V}_3+z_4\dot{z}_4\\ =-k_1z_1^2-k_2z_2^2-k_3z_3^2-k_4z_4^2 \end{array} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(31)\quad \left\{\begin{array}{l} z_1=\theta=x_1\\ z_2=\omega-\alpha_1=\omega+k_1z_1=x_2+k_1x_1\\ z_3=\theta_e-\alpha_2=x_3-\alpha_2\\ =x_3-\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)\\ z_4=e_I-\alpha_3=x_4-\int(e+k_4z_4)dt \end{array}}\right. %\quad\boxed{z=\phi(x)} }$

$\displaystyle{(32)\quad \left\{\begin{array}{l} x_1=z_1\\ x_2=z_2-k_1z_1\\ x_3=z_3+\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)\\ x_4=z_4+\int(e+k_4z_4)dt \end{array}}\right. %\quad\boxed{x=\phi^{-}(z)} }$

YMCPBの出力FB型SMI制御

投稿日時: 2022年2月1日投稿者: cacsd

YMCBのOF-SM制御

投稿日時: 2022年2月1日投稿者: cacsd

これまでのSM制御系の線形制御則は状態フィードバック型のものを設計してきました。そして、制御対象の３次元モデルを用いても十分な結果が得られることを確認しました。そこで、制御対象の５次元モデルに基づいて線形制御則として出力フィードバック型のものを設計してもうまく機能することが予想されます。これはヒーブを完全に無視するのではなく、間接的に考慮していることになることがメリットと考えられます。

Case１：５次元モデルに基づくOF-SM制御

MATLAB

%cYMCPB5_of_sm.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%=====OF-SM
 C=CM;
%Establish the number of inputs and outputs
 [nn,mm]=size(B);
 [pp,nn]=size(C);
%Change coordinates so the output distribution matrix is [0 I]
 nc=null(C);
 Tc=[nc'; C];
 Ac=Tc*A*inv(Tc);
 Bc=Tc*B;
 Cc=C*inv(Tc);
%Partition the input distribution matrix conformably
 Bc1=Bc(1:nn-pp,:);
 Bc2=Bc(nn-pp+1:nn,:);
%Find a transformation to bring about a special structure 
%in input and output distribution matrices
 [T,temp]=qr(Bc2);
 T=(flipud(T'))';
 Tb=[eye(nn-pp) -Bc1*inv(Bc2'*Bc2)*Bc2';
     zeros(pp,nn-pp) T'];
%In this new cordinate system, we have C=[0 T] and B=[0 B2']'
%A has no special structure yet
 Aa=Tb*Ac*inv(Tb);
 Ba=Tb*Bc;
 Ca=Cc*inv(Tb);
 A1111=Aa(1:nn-pp,1:nn-pp);
 A1121=Aa(nn-pp+1:nn-mm,1:nn-pp);
%The aim is to put (A1111,A1121) in the observability canonical form
 [Ab,Bb,Cb,Tobs,k]=obsvf(A1111,zeros(nn-pp,1),A1121,1000*eps);
%r is the dimension of the unobservable subspace 
%the number of invariant zeros of the system (A,B,C)
 r=nn-pp-sum(k);
 disp('Dimension of the unobservable subspace:'),r
 Ta=[Tobs zeros(nn-pp,pp);
     zeros(pp,nn-pp) eye(pp)];
 Af=Ta*Aa*inv(Ta);
 Bf=Ta*Ba;eig(Af)
 Cf=Ca*inv(Ta); 
%=====
 A11ti=Aa(1:nn-mm,1:nn-mm);
 B1ti=Aa(1:nn-mm,nn-mm+1:nn);
 C1ti=[zeros(pp-mm,nn-pp) eye(pp-mm,pp-mm)];
 [Kopt,pl]=opt(A11ti,B1ti,C1ti,diag([1 1]),1)
 K=Kopt/C1ti 
 A11bar=A11ti-B1ti*K*C1ti;
 lambda=eig(A11bar)
%---- 
 Tbar=[eye(nn-mm) zeros(nn-mm,mm);
       K*C1ti eye(mm)]; 
 Abar=Tbar*Aa*inv(Tbar);  
 A11bar2=Abar(1:nn-mm,1:nn-mm);
 lambda=eig(A11bar2) 
 A12bar=Abar(1:nn-mm,nn-mm+1:nn);
 A21bar=Abar(nn-mm+1:nn,1:nn-mm);
 A22bar=Abar(nn-mm+1:nn,nn-mm+1:nn); 
 Q1=eye(nn-mm); 
 P1=lyap(A11bar2',Q1)
% 
 P2=1
 B2=Ba(nn,mm)
 F2=B2'*P2 
 F=F2*[K eye(mm)]*T'
% 
 Q2=P1*A12bar+A21bar'*P2;
 Q3=P2*A22bar+A22bar'*P2; 
 W=inv(F2)'*(Q3+Q2'*inv(Q1)*Q2)*inv(F2);
 gamma0=0.5*norm(W) 
 gamma=gamma0*1.1
 rho=1
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth 
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2
%-----
%eof

図1 YMCPBに対する出力FB型SM制御のシミュレーション例

Case２：５次元モデルに基づくOF-SMI制御

●補償器を用いる方法
●ＳＭオブザーバを用いる方法（ｍ＝ｐ＝１の制約）

YMCPBのSMI制御(2)

投稿日時: 2022年2月1日投稿者: cacsd

ここでは、 LQI制御に倣って積分動作をもつスライディングモード制御（SMI2制御）系の設計について述べます。その評価は本来は非線形シミュレータ上で行うべきですが、ここでは６次元線形モデルに関して行います。

Case	設計モデル	状態FB	状態OB	積分動作	注意点
1	５次	５次	未使用	要	「絵に描いた餅」
2	５次	３次	不要	要	ヒーブのゲインを強制的に零
3	３次	３次	不要	要	ヒーブからの連成を抑制できるか
4	５次	５次	要	要	コントローラが微分方程式

Case１：５次元モデルに基づくSMI制御

●LQI制御における偏差系(7)を、改めて次のように書きます。

$\displaystyle{(1)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot x_2(t) \end{array}\right] }_{\dot{x}_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} + \underbrace{ \left[\begin{array}{c} 0\\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t)\\ (x_1(t)=x(t)-x_\infty, x_2(t)=u(t)-u_\infty) \end{array} }$

スイッチング関数として、次式を考えます。

$\displaystyle{(2)\quad s(t)= \underbrace{ \left[\begin{array}{cc} S_1 & S_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{S_2 \left[\begin{array}{cc} M & I \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} \ (M=S_2^{-1}S_1) }$

(1)に対して、座標変換

$\displaystyle{(3)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ S_1 & S_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ -S_2^{-1}S_1 & S_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} }$

を行って、次式を得ます。

$\displaystyle{(4)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] }_{\dot{x}'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right] }_{T_sA_{E3}T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ S_2 \end{array}\right] }_{T_sB_{E3}} {\dot u}(t) }$

$\displaystyle{(5)\quad \left\{\begin{array}{l} \bar{A}_{11}=A-BM\\ \bar{A}_{12}=BS_2^{-1}\\ \bar{A}_{21}=S_1(A-BM)\\ \bar{A}_{22}=S_1BS_2^{-1} \end{array}\right. }$

以下では、 $\bar{A}_{11}$ が安定行列となるようにスイッチング関数が選ばれていると仮定します。

●偏差系E3に対するSM制御は次式で与えられます。

$\displaystyle{(6)\quad {\dot u}(t)={\dot u}_\ell(t)+{\dot u}_n(t) }$

$\displaystyle{(7)\quad {\dot u}_\ell(t)=-\underbrace{(SB_{E3})^{-1}(SA_{E3}-\Phi S)}_{K_{E3}=\left[\begin{array}{cc} K_1 & K_2 \end{array}\right]}}x_{E3}(t) }$

$\displaystyle{(8)\quad {\dot u}_n(t)=-(SB_{E3})^{-1}\rho\frac{P_2s(t)}{||P_2s(t)||}}=-(SB_{E3})^{-1}\rho\,{\rm sgn}(P_2Sx_{E3}(t)) }$

ここで、 $\Phi$ は安定行列、 $P_2>0$ は $P_2\Phi+\Phi^TP_2=-I$ の解です。

これらを積分して、制御対象に対する積分動作をもつSM制御（SMI制御）を導出します。

$\displaystyle{(9)\quad {u}(t)={u}_\ell(t)+{u}_n(t) }$

まず(7)は次式のように書けます。

$\displaystyle{(10)\quad {\dot u}_\ell(t)=- \boxed{\underbrace{ (SB_{E3})^{-1}(SA_{E3}-\Phi S)S_E^{-1} }_{\left[\begin{array}{cc} F & F_I \end{array}\right]}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)} }$

これを積分して

$\displaystyle{(11)\quad \boxed{u_\ell(t)=-Fx(t)+F_I\int_0^t(r-y(\tau))d\tau} }$

次に(8)は次式のように書けます。

$\displaystyle{(12)\quad {\dot u}_n(t) =-(SB_{E3})^{-1}\rho\, {\rm sgn}( \boxed{\underbrace{ P_2SS_E^{-1} }_{\left[\begin{array}{cc} G & G_I \end{array}\right]}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)}) }$

これを積分すれば

$\displaystyle{(13)\quad \boxed{u_n(t)=(SB_{E3})^{-1}\rho\,\int_0^t\underbrace{{\rm sgn}(-G{\dot x}(\tau)+G_I(r-y(\tau)))}_{0,\pm 1}d\tau} }$

●制御対象YMCPBに対するSMI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB5_smi2.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%-----
 [n,m]=size(B);
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 SE=[A B;C 0];
 S=swflqr(AE,BE,QE)
%-----
 Phi=-5
 P2=0.5*inv(-Phi);
 P2S=P2*S/SE;
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 L=(Leq+LPhi)/SE; 
 rho=1
%----- 
 F=L(1:n)
 FI=L(n+m)
 G=P2S(1:n)
 GI=P2S(n+m)  
 Ln=inv(S*BE)*rho 
%F(1)=0; F(3)=0;
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
%-----
%eof
% F =
%    -1.4925   50.3338    0.2214  -11.8405   55.6410
% FI =
%    31.9622
% G =
%    -0.0113    0.1943   -0.0020    0.1934   -0.8772
% GI =
%    -0.6392
% Ln =
%     -1

図1　SMI2制御系シミュレーション

これを実行して次のシミュレーション結果を得ます。

図2　５次元モデルに基づくSMI2制御系と符号関数を通したSMI制御

Case1’：Case1でヒーブゲインを零とする場合

図3　ヒーブゲインを零とした場合のSMI2制御系と符号関数を通したSMI制御

Case２：３次元モデルに基づくSMI制御

●制御対象YMCPBに対するSMI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB3_smi2.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント id=128,ID=33
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys3(:,:,33));
%-----
 [n,m]=size(B);
 Mth=3/180*pi;
 Tth=0.5
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 SE=[A B;C 0];
 S=swflqr(AE,BE,QE)
%-----
 Phi=-5
 P2=0.5*inv(-Phi);
 P2S=P2*S/SE;
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 L=(Leq+LPhi)/SE;
 rho=1
%----- 
 F=L(1:n) 
 FI=L(n+m)  
 G=P2S(1:n)
 GI=P2S(n+m)  
 Ln=inv(S*BE)*rho 
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
%-----
%eof
% F =
%    48.8314  -14.5654   61.5153
% FI =
%    57.9634
% G =
%     0.3434    0.2175   -0.8772
% GI =
%    -1.1593
% Ln =
%     -1

図4　３次元モデルに基づくSMI2制御系と符号関数を通したSMI2制御