自抗扰控制器中跟踪微分器的设计

韩京清老师所写的《自抗扰控制技术》中，介绍了跟踪微分器的设计过程，下面就把数学原理和代码来详细记录跟踪微分器的设计过程。
跟踪微分器的作用就是从被噪声污染的信号中合理的提取微分信号。
（1）一般的跟踪微分器
传统的微分信号是用如下的微分环节来得到的：
$\dot{v}=w(s)v=\frac{s}{Ts+1}v=\frac{1}{T}(v-\frac{v}{Ts+1})$，其中$T$是比较小的时间常数。其等价的模型如下图所示的Simulink，其中$T=0.01$，输入信号是$sin(t)$。

所得到的结果如下图：

当输入信号$v(t)$变化比较缓慢而时间常数$T$比较小时，就有近似关系$v(t-T)\approx \frac{1}{Ts+1}v(t)$，从而$\dot{v(t)}\approx \frac{1}{T}(v(t)-v(t-T))$。证明过程有点长，有兴趣的可以去看自抗扰控制这本书的第二章。
下面是用代码来实现上面的过程，如果令$\overline{v}=\frac{v}{Ts+1}$，则上面的微分环节公式就可以写成$\dot{v}=\frac{1}{T}(v-\overline{v})$，写到这里，关键就是如何求出$\overline{v}$。可以利用$v$和$\overline{v}$之间的传递函数关系写出$\dot{\overline{v}}=\frac{1}{T}(v-\overline{v})$，就会得到下面的递推公式：

$\{\begin{array}{lr}\dot{v(k)}=\frac{1}{T}(v(k)-\overline{v}(k))& \\ \overline{v}(k+1)=\overline{v}(k)+h\frac{1}{T}(v(k)-\overline{v}(k))& \end{array}$
其中$h$是一个小的常数。
下面是代码的实现过程，对正弦信号求微分：

clear all;
h=0.001;
T=0.001;
time = 20;
N = time/T;
n=0:N-1;
x = sin(n*T);

for k=1:1:N
    xDelay(1)=0;
    
    y(k) = (x(k)-xDelay(k))/T;
    xDelay(k+1) = xDelay(k) + h*(x(k)-xDelay(k))/T;
end
plot(n*T,x,n*T,y);

得到的结果如下：

（2）改进的跟踪微分器
为了消除或减弱噪声放大效应，把微分近似公式换成下面这一种。
$\dot{v(t)}\approx \frac{v(t-{\tau }_1)-v(t-{\tau }_2)}{{\tau }_1-{\tau }_2},0<{\tau }_1<{\tau }_2$
延迟信号$v(t-{\tau }_1)$可以由惯性环节$\frac{1}{{\tau }_{1}s+1}$来获取，这个微分近似公式的传递函数为
$\dot{v(t)}=\frac{1}{{\tau }_1-{\tau }_2}(\frac{1}{{\tau }_{1}s+1}-\frac{1}{{\tau }_{2}s+1})v(t)=\frac{s}{{\tau }_1{\tau }_2{s}^2+({\tau }_1+{\tau }_2)s+1}v(t)$
其所对应的Simulink如下图所示，${\tau }_1=0.01,{\tau }_2=0.02$。

其所对应的仿真图如下所示。

根据上面的传递函数，可以写出它的微分形式：
$\{\begin{array}{lr}\dot{x_1}=x_2& \\ \dot{x_2}=-\frac{1}{{\tau }_1{\tau }_2}(x_1-v(t))-\frac{{\tau }_1+{\tau }_2}{{\tau }_1{\tau }_2}{x}_2& \\ \dot{v}=x_2& \end{array}$
根据上面的微分方程，离散化成计算机容易处理的形式：
$\{\begin{array}{lr}{x}_1(k+1)=x_1(k)+h{x}_2(k)& \\ x_2(k+1)=x_2(k)-h(\frac{1}{{\tau }_1{\tau }_2}(x_1(k)-v(k))+\frac{{\tau }_1+{\tau }_2}{{\tau }_1{\tau }_2}{x}_2(k))& \\ \dot{v(k)}=x_2(k)& \end{array}$
下面是代码：

clear all;
h=0.001;
T=0.001;
tau1=0.01;
tau2=0.02;
time = 20;
N = time/T;
n=0:N-1;
x = sin(n*T);

for k=1:1:N
    x1(1) = 0;
    x2(1) = 0;
    x1(k+1) = x1(k)+h*x2(k);
    x2(k+1) = x2(k)-h*((x1(k)-x(k))/tau1/tau2+(tau1+tau2)*x2(k)/tau1/tau2);
    y(k) = x2(k);
end
plot(n*T,x,n*T,y);

实现的结果如下图：

如果对这两个微分器分别加上噪声的话，再来比较谁的微分效果更好，代码如下：

clear all;
h=0.001;
T=0.001;
tau1=0.01;
tau2=0.02;
time = 20;
N = time/T;
n=0:N-1;
x = sin(n*T);
noise=-1+2*rand(1,N);% add noise[-1,1]
x = x+0.01*noise;% gain is 0.01

for k=1:1:N
    x1(1) = 0;
    x2(1) = 0;
    x1(k+1) = x1(k)+h*x2(k);
    x2(k+1) = x2(k)-h*((x1(k)-x(k))/tau1/tau2+(tau1+tau2)*x2(k)/tau1/tau2);
    y(k) = x2(k);
end
plot(n*T,x,n*T,y);

仿真图如下：

而传统的微分器的效果如下：

两者的对比已经非常明显了。（我也不清楚怎么差别这么大，是噪声幅度太大了吗）