非線形外乱オブザーバのMatlabサンプルプログラム

外乱オブザーバについて

以前、シンプルな伝達関数で表記した線形外乱オブザーバについてサンプルプログラムを紹介しました。
外乱オブザーバに関しては下記記事を読んでください。
fumikirinobouken.hatenablog.com

今回は非線形外乱オブザーバについての理論とサンプルプログラムを紹介したいと思います。
線形外乱オブザーバは割とお手頃に使えるけど、非線形外乱オブザーバなんてそんな使わんだろ…とか自分で研究に使ってて思ったりします。
理論も複雑だろうと思いきや、やってみると普段の方程式とそんなに変わらない感じでした。

使った教科書

Disturbance Observer-Based Control: Methods and Applications

作者: Shihua Li,Jun Yang,Wen-Hua Chen,Xisong Chen
出版社/メーカー: CRC Press
発売日: 2017/10/12
メディア: ペーパーバック
この商品を含むブログを見る

この教科書わかりやすいですね。
英語ですが、かなりとっつきやすいです。
この教科書の43ページからの内容になります。

理論

ではまず理論についてです。

外乱が存在するシステムを下記のように定義します。

$\dot{x}=f(x)+g_1(x)u+g_2(x)d$

それぞれ $u$ が既知の入力、そして $d$ が未知の入力、外乱です。
この外乱は一定外乱、すなわち $\dot{d}=0$ と仮定します。（実際に一定外乱でなくとも、遅れが発生しますが推定することができます。）
非線形なシステムなので、線形状態方程式のようなA行列、B行列とはきれいに書くことはできません。

このとき、推定外乱を $\hat{d}$ とし、非線形外乱オブザーバのフィードバックゲインを $l(x)$ とすると、推定外乱は下記のように表せます。

$\dot{\hat{d}} = l(x)[\dot{x}-f(x)-g_1(x)u-g_2(x)\hat{d}$ ]

外乱の推定誤差 $e_d$ は

$e_d = \hat{d}-d$

となり、この誤差の時間応答は

$\dot{e_d} =\dot{ \hat{d}}-\dot{d}$
$= l(x)[\dot{x}-f(x)-g_1(x)u-g_2(x)\hat{d}$ ]
$= -l(x)g_2(x)e_d$

となります。

このとき、上記の式が安定になるように $.(x)$ が選ばれれば外乱推定が可能となります。
ただし、この方法は状態変数 $\dot{x}$ が必要になるため、別のセンサーが必要になったり、微分のためのフィルタを用意しなければならず、またノイズも入りやすい特徴があります。

そこで、外乱オブザーバを下記のように表します。

$\dot{z}=-l(x)g_2(x)z-l(x)[\dot{x}-f(x)-g_1(x)u+g_2(x)p(x)$ ]
$\hat{d}=z+p(x)$

ここで、 $p(x)$ と $l(x)$ は以下の関係を持ちます。
$l(x)=\frac{\partial p(x)}{\partial x}$

行列の微分に関しては下記記事でわかりやすいサイトを紹介しています。
fumikirinobouken.hatenablog.com

このようにすると、誤差の時間応答の方程式は

$\dot{e_d} =\dot{ \hat{d}}-\dot{d}$
$= \dot{z}+\frac{\partial p(x)}{\partial x} \dot{x}$
$= -l(x)g_2(x)z-l(x)(g_2(x)p(x)+f(x)+g_1(x)u)+l(x)[f(x)+g_1(x)u+g_2(x)d$ ]]
$= -l(x)g_2(x)\hat{d}+l(x)g_2(x)d$
$= -l(x)g_2(x)e_d$

となり、 $\dot{x}$ を使わなくても先程と同じ式が導出され、推定が可能であることが示せました。

この非線形外乱オブザーバのブロック線図を下記に示します。

f:id:fumikirinobouken:20181108171738j:plain

例題とMatlabサンプルプログラム

例題としてテキストの定数と制御対象でシミュレーションしようとおもったのですが、いかんせんsinだの使われ非線形過ぎてシミュレーションがあってるんだか間違ってるんだか（テキストにも時間応答のってない）わからないので自前で例題をこしらえました。
制御対象は連続時間系で作ってあります。
非線形要素はtanh()で追加してあり、クーロン摩擦の近似とか思ってもらえるとなんとなく実機に近いイメージになるのかなって思います。いや定数まじで適当に決めただけなんで物理定数とか全然関係ないんですが。

制御対象
$\ddot{x}=-50x-4\dot{x}-5tanh(2\dot{x})+50u+50d$

 

clear
clc
close all

Ts=1e-3; % Sampling time 1ms
Tsim=20; % Simulation time 20s
t=0:Ts:Tsim-1*Ts;
N=Tsim/Ts;

% All model are continuous model

% Plant matrix without nonlinear element tanh()
A=[0 1
   -50 -4.0];
B=[0
   50];
C=[1 0];
D=0;

% Disturbance input matrix
Bd=[0
   50];

% Initialize of plant state variable
dotx(1:2,1:N)=0;
x(1:2,1:N)=0;

% Observer matrix without nonlinear element tanh()
Aob=[0 1
   -50 -4.0]
Bob=[0
   50]
Cob=[1 0]
g1=Bob;
g2=Bob;

% Initialize of observer state variable
dotz(1:1,1:N)=0;
z(1:1,1:N)=0;

% Feedback gain L
Lob=[0 5]*0.02

% Estimated disturbance
dhat(1:1,1:N)=0;

% Difinition of plant input at 5s 
u(1:N/4)   = 0;
u(N/4+1:N) = 1;

% Difinition of disturbance input at 10s 
d(1:N/2)   = 0;
d(N/2+1:N) = 1*1;

% Simulation
for i=1:N-1
    % Caluculation of observer and estimation of disturbance
    fx=A*x(:,i)+[0;-5*tanh(2*x(2,i))];
    px=Lob*x(:,i);
    dotz(1,i+1)=-Lob*g2*z(:,i)-Lob*(g2*px+fx+g1*u(i));
    z(:,i+1)=z(:,i)+dotz(:,i+1)*Ts;
    dhat(:,i) = z(:,i)+px;
    
    % Caluculation of plant
    dotx(:,i+1)=A*x(:,i)+[0;-5*tanh(2*x(2,i))]+B*u(i)+Bd*d(i);
    x(:,i+1)=x(:,i)+dotx(:,i+1)*Ts;
    
end


% Plot data
figure
hold on
grid on
plot(t,x(1,:)) % position
xlabel('Time [s])')
ylabel('position')
title('Position');

figure
hold on
grid on
plot(t,d) % Actual disturbance
plot(t,dhat) % Estimated disturbance
xlabel('Time [s])')
ylabel('input')
title('Disturbance estimation');
legend('Actual disturbance', 'Estimated disturbance')

実行結果

f:id:fumikirinobouken:20181108173427j:plain

こんな感じで推定できてますね。非線形外乱オブザーバのフィードバックゲインは小さめにしてあるので、ゲインを大きくすればもっと収束が早くなります。
おおよそのコードは正しいと思うけど、ただ、いつもシミュレーションをSimulinkで書いちゃうからMatlabオンリーで書くことに不安がある…添字とかミスありそうで不安…