TD微分跟踪器(原理解析和Matlab实现)
状态扩张观测器
学习韩老师的ADRC，把学习过程记录一下，方便日后复习。这篇文章针对模型这种微分跟踪器环节（TD）进行检验介绍，包括作用、原理和代码实现。
大致可分为如下几个作用：
(1)安排过渡过程
简单说就是让目标输入更加平滑没有突变。这个思想最开始是为了解决PID控制理论中超调和快速相互矛盾，
韩老师书里的原话是。根据控制目标和对象承受能力先安排合适的过渡过程。安排过渡过程的手法在控制工程实践中已常被采用。如，升温过程中的“升温曲线”，热处理过程中的“温度曲线”等。
(2)微分信号的提取
利用微分近似公式
y
=
v
(
t
)
−
v
(
t
−
τ
)
τ
y=\frac{v(t)-v(t-\tau)}{\tau}
y=τv(t)−v(t−τ) 来实现微分, 噪声放大效应很严重, 以至于无法利用。但如果用微分近似公式
y
=
v
(
t
−
τ
1
)
−
v
(
t
−
τ
2
)
τ
2
−
τ
1
y=\frac{v\left(t-\tau_{1}\right)-v\left(t-\tau_{2}\right)}{\tau_{2}-\tau_{1}}
y=τ2−τ1v(t−τ1)−v(t−τ2), 就可用传递函数
y
=
1
τ
2
−
τ
1
(
1
τ
1
s
+
1
−
1
τ
2
s
+
1
)
v
y=\frac{1}{\tau_{2}-\tau_{1}}\left(\frac{1}{\tau_{1} s+1}-\frac{1}{\tau_{2} s+1}\right) v
y=τ2−τ11(τ1s+11−τ2s+11)v
来实现近似微分，是两个惯性环节输出之差来实现微分功能,这样可以噪声放大效应,是二阶动态环节来实现微分功能的。
总结一下，这里
v
(
t
)
v(t)
v(t) 是控制目标,
v
1
(
t
)
v_{1}(t)
v1(t) 是对
v
(
t
)
v(t)
v(t) 安排的过渡过程,
v
2
v_{2}
v2
(
t
)
(t)
(t) 是过渡过程
v
1
(
t
)
v_{1}(t)
v1(t) 的微分信号。在这个框图中的
Σ
\Sigma
Σ 即TD跟踪器，可以是一个动态过程,也可以是一个函数发生器,是根据控制目标
v
(
t
)
v(t)
v(t) 和对象的能力安排的。
(3) 低通滤波器
TD微分跟踪器也可以当成一个低通滤波器。下图是最速跟踪滤波器的幅频曲线，可以看出可以很好的抑制高频分量。
(4) 配置系统零点
在线性系统理论中，只要系统能控，可以用状态反馈对系统任意配置所需极点，以此改善闭环系统的动态性能。但是若能实现配置系统零点的手法，有时更有效地改善闭环系统的动态性能然而零点配置是要用微分信号的，而传统的微分器对噪声很敏感，很难直接用于零点配置。由于这里提出的TD对噪声具有很好的滤波作用，它所给出的微分信号是可以用来实现零点配置的。
先说个题外话，这个话题我看到某乎上有一篇文章写的很有意思，通过频域分析来理解TD，分享一下。
TD的讨论
考虑一阶惯性系统，其传递函数为：
y
=
w
(
s
)
u
=
k
T
s
+
1
u
y=w(s) u=\frac{k}{T s+1} u
y=w(s)u=Ts+1ku
其中，k为系统放大系数，T为时间常数。
当T比较小时，可近似有
1
1
+
T
s
≈
e
−
T
s
\frac{1}{1+T s} \approx \mathrm{e}^{-Ts}
1+Ts1≈e−Ts
可以将输出看成对信号u的延迟，延迟时间为T。用公式表达为：
y
(
t
)
=
1
T
s
+
1
u
(
t
)
≈
e
−
T
s
u
(
t
)
=
u
(
t
−
T
)
y(t)=\frac{1}{T s+1} u(t) \approx \mathrm{e}^{-T_{s}} u(t)=u(t-T)
y(t)=Ts+11u(t)≈e−Tsu(t)=u(t−T)
经典微分器的传递函数为：
y
=
w
(
s
)
v
=
s
T
s
+
1
v
=
1
T
(
1
−
1
T
s
+
1
)
v
y=w(s) v=\frac{s}{T s+1} v=\frac{1}{T}\left(1-\frac{1}{T s+1}\right) v
y=w(s)v=Ts+1sv=T1(1−Ts+11)v
其时域的表达为：
y
(
t
)
≈
1
T
(
v
(
t
)
+
n
(
t
)
−
v
(
t
−
T
)
)
=
1
T
(
v
(
t
)
−
v
(
t
−
T
)
+
n
(
t
)
)
≈
v
˙
(
t
)
+
1
T
n
(
t
)
\begin{aligned} y(t) \approx & \frac{1}{T}(v(t)+n(t)-v(t-T))=\\ & \frac{1}{T}(v(t)-v(t-T)+n(t)) \approx \\ & \dot{v}(t)+\frac{1}{T} n(t) \end{aligned}
y(t)≈T1(v(t)+n(t)−v(t−T))=T1(v(t)−v(t−T)+n(t))≈v˙(t)+T1n(t)
其中，
n
(
t
)
n(t)
n(t)为随机噪声信号，可以看到，时间常数越小，噪声的干扰越明显，这就是经典微分器的噪声放大效应。
为了减弱噪声放大效应，考虑用两个积分环节相减来近似微分器，这也是微分跟踪器的雏形。
即
v
˙
(
t
)
≈
v
(
t
−
τ
1
)
−
v
(
t
−
τ
2
)
τ
2
−
τ
1
,
0
<
τ
1
<
τ
2
\dot{v}(t) \approx \frac{v\left(t-\tau_{1}\right)-v\left(t-\tau_{2}\right)}{\tau_{2}-\tau_{1}}, 0<\tau_{1}<\tau_{2}
v˙(t)≈τ2−τ1v(t−τ1)−v(t−τ2),0<τ1<τ2
其中
v
(
t
−
τ
1
)
,
v
(
t
−
τ
2
)
v\left(t-\tau_{1}\right),v\left(t-\tau_{2}\right)
v(t−τ1),v(t−τ2)分别用两个时间常数为
τ
1
,
τ
2
\tau_1,\tau_2
τ1,τ2的惯性环节模拟。所以其传递公式为：
y
=
1
τ
2
−
τ
1
(
1
τ
1
s
+
1
−
1
τ
2
s
+
1
)
v
=
s
τ
1
τ
2
s
2
+
(
τ
1
+
τ
2
)
s
+
1
v
\begin{aligned} y=& \frac{1}{\tau_{2}-\tau_{1}}\left(\frac{1}{\tau_{1} s+1}-\frac{1}{\tau_{2} s+1}\right) v= \frac{s}{\tau_{1} \tau_{2} s^{2}+\left(\tau_{1}+\tau_{2}\right) s+1} v \end{aligned}
y=τ2−τ11(τ1s+11−τ2s+11)v=τ1τ2s2+(τ1+τ2)s+1sv
其等价的状态变量实现为：
{
x
˙
1
=
x
2
x
˙
2
=
−
1
τ
1
τ
2
(
x
1
−
v
(
t
)
)
−
τ
1
+
τ
2
τ
1
τ
2
x
2
y
=
x
2
\left\{\begin{array}{l} \dot{x}_{1}=x_{2} \\ \dot{x}_{2}=-\frac{1}{\tau_{1} \tau_{2}}\left(x_{1}-v(t)\right)-\frac{\tau_{1}+\tau_{2}}{\tau_{1} \tau_{2}} x_{2} \\ y=x_{2} \end{array}\right.
⎩
⎨
⎧x˙1=x2x˙2=−τ1τ21(x1−v(t))−τ1τ2τ1+τ2x2y=x2
若时间常数
τ
1
,
τ
2
\tau_1,\tau_2
τ1,τ2很接近，则近似函数可以写为：
w
(
s
)
=
r
2
s
s
2
+
2
r
s
+
r
2
=
s
r
2
(
s
+
r
)
2
w(s)=\frac{r^{2} s}{s^{2}+2 r s+r^{2}}=s \frac{r^{2}}{(s+r)^{2}}
w(s)=s2+2rs+r2r2s=s(s+r)2r2
其中，
r
=
1
τ
r=\frac{1}{\tau}
r=τ1。可以看出这是一个临界阻尼状态，即可以最快地无超调地达到目标值。这个微分跟踪器被称为线性微分跟踪器。
考虑二阶积分器串联型系统
{
x
˙
1
=
x
2
x
˙
2
=
u
,
∣
u
∣
⩽
r
\left\{\begin{array}{l} \dot{x}_{1}=x_{2} \\ \dot{x}_{2}=u,|u| \leqslant r \end{array}\right.
{x˙1=x2x˙2=u,∣u∣⩽r
其以原点为终点的快速最优控制函数为
u
(
x
1
,
x
2
)
=
−
r
sign
(
x
1
+
x
2
∣
x
2
∣
2
r
)
u\left(x_{1}, x_{2}\right)=-r \operatorname{sign}\left(x_{1}+\frac{x_{2}\left|x_{2}\right|}{2 r}\right)
u(x1,x2)=−rsign(x1+2rx2∣x2∣)
若将
x
1
x_1
x1替换为
x
1
−
v
0
(
t
)
x_1-v_0(t)
x1−v0(t)，则系统
{
x
˙
1
=
x
2
x
˙
2
=
−
r
sign
(
x
1
−
v
0
(
t
)
+
x
2
∣
x
2
∣
2
r
)
\left\{\begin{array}{l} \dot{x}_{1}=x_{2} \\ \dot{x}_{2}=-r \operatorname{sign}\left(x_{1}-v_{0}(t)+\frac{x_{2}\left|x_{2}\right|}{2 r}\right) \end{array}\right.
{x˙1=x2x˙2=−rsign(x1−v0(t)+2rx2∣x2∣)
的解，再加速度限制(
∣
u
∣
⩽
r
|u| \leqslant r
∣u∣⩽r)下，可以最快的跟踪输入信号
v
0
(
t
)
v_0(t)
v0(t)，
x
2
x_2
x2可以当作
v
0
v_0
v0的微分信号，因此这个也是一个TD，被称为非线性微分跟踪器。
韩老师书上写 非线性TD效率比线性TD高。
设n阶系统
{
z
˙
1
=
z
2
⋮
z
˙
n
−
1
=
z
n
z
˙
n
=
f
(
z
1
,
z
2
,
⋯
,
z
n
)
\left\{\begin{array}{l} \dot{z}_{1}=z_{2} \\ \vdots \\ \dot{z}_{n-1}=z_{n} \\ \dot{z}_{n}=f\left(z_{1}, z_{2}, \cdots, z_{n}\right) \end{array}\right.
⎩
⎨
⎧z˙1=z2⋮z˙n−1=znz˙n=f(z1,z2,⋯,zn)
按菲利波夫意义有解，且其解满足
lim
t
→
∞
z
i
(
t
)
=
0
,
i
=
1
,
⋯
,
n
\lim _{t \rightarrow \infty} z_{i}(t)=0, i=1, \cdots, n
limt→∞zi(t)=0,i=1,⋯,n
那么对任意局部可积信号
v
(
t
)
,
t
∈
[
0
,
∞
)
v(t), t \in[0, \infty)
v(t),t∈[0,∞), 和任意
T
>
0
T>0
T>0, 微分方程
{
x
˙
1
=
x
2
⋮
x
˙
n
−
1
=
x
n
x
˙
n
=
r
n
f
(
x
1
−
v
(
t
)
,
x
2
r
,
⋯
,
x
n
r
n
−
1
)
\left\{\begin{array}{l} \dot{x}_{1}=x_{2} \\ \vdots \\ \dot{x}_{n-1}=x_{n} \\ \dot{x}_{n}=r^{n} f\left(x_{1}-v(t), \frac{x_{2}}{r}, \cdots, \frac{x_{n}}{r^{n-1}}\right) \end{array}\right.
⎩
⎨
⎧x˙1=x2⋮x˙n−1=xnx˙n=rnf(x1−v(t),rx2,⋯,rn−1xn)
的解第一个分量x1,将满足
lim
r
→
∞
∫
0
T
∣
x
1
(
r
,
t
)
−
v
(
t
)
∣
d
t
=
0
\lim _{r \rightarrow \infty} \int_{0}^{T}\left|x_{1}(r, t)-v(t)\right| \mathrm{d} t=0
r→∞lim∫0T∣x1(r,t)−v(t)∣dt=0
韩老师的书上有证明过程，感兴趣可以自行去看，如果感性的理解这个定理，可以把他看成是上面非线性TD的高阶推广。
如果考虑连续系统，上面提到的非线性TD有着非常好的效果，但是我们要处理的实际问题一定是离散的。考虑其离散形式：
{
f
=
−
r
sign
(
x
1
(
k
)
−
v
(
k
)
+
x
2
(
k
)
∣
x
2
(
k
)
∣
2
r
)
x
1
(
k
+
1
)
=
x
1
(
k
)
+
h
x
2
(
k
)
x
2
(
k
+
1
)
=
x
2
(
k
)
+
h
f
\left\{\begin{array}{l} f=-r \operatorname{sign}\left(x_{1}(k)-v(k)+\frac{x_{2}(k)\left|x_{2}(k)\right|}{2 r}\right) \\ x_{1}(k+1)=x_{1}(k)+h x_{2}(k) \\ x_{2}(k+1)=x_{2}(k)+h f \end{array}\right.
⎩
⎨
⎧f=−rsign(x1(k)−v(k)+2rx2(k)∣x2(k)∣)x1(k+1)=x1(k)+hx2(k)x2(k+1)=x2(k)+hf
这个系统的一个最大问题时，进入稳态后会有高频震颤，将sign函数改为线性饱和函数sat(x,d)也于事无补。因为上面的离散递推公式只是一种数值计算的近似解，并不是离散系统
{
x
1
(
k
+
1
)
=
x
1
(
k
)
+
h
x
2
(
k
)
x
2
(
k
+
1
)
=
x
2
(
k
)
+
h
u
,
∣
u
∣
⩽
r
\left\{\begin{array}{l} x_{1}(k+1)=x_{1}(k)+h x_{2}(k) \\ x_{2}(k+1)=x_{2}(k)+h u, \quad|u| \leqslant r \end{array}\right.
{x1(k+1)=x1(k)+hx2(k)x2(k+1)=x2(k)+hu,∣u∣⩽r
的最速控制综合函数。
因此韩老师推导了该离散系统的最速控制综合函数fhan，同样这里只给出结果。
{
d
=
r
h
d
0
=
d
h
a
0
=
h
x
2
y
=
x
1
+
a
0
a
1
=
d
(
d
+
8
∣
y
∣
)
a
2
=
a
0
+
sign
(
y
)
(
a
1
−
d
)
/
2
a
=
(
a
0
+
y
)
f
s
g
(
y
,
d
)
+
a
2
(
1
−
f
s
g
(
y
,
d
)
)
fhan
=
−
r
(
a
d
)
f
s
g
(
a
,
d
)
−
r
sign
(
a
)
(
1
−
f
s
g
(
a
,
d
)
)
\left\{\begin{array}{l} d=r h \\ d_0=dh \\ a_{0}=h x_{2} \\ y=x_{1}+a_{0} \\ a_{1}=\sqrt{d(d+8|y|)} \\ a_{2}=a_{0}+\operatorname{sign}(y)\left(a_{1}-d\right) / 2 \\ a=\left(a_{0}+y\right) \mathrm{fsg}(y, d)+a_{2}(1-\mathrm{fsg}(y, d)) \\ \text { fhan }=-r\left(\frac{a}{d}\right) \mathrm{fsg}(a, d)-r \operatorname{sign}(a)(1-\mathrm{fsg}(a, d)) \end{array}\right.
⎩
⎨
⎧d=rhd0=dha0=hx2y=x1+a0a1=d(d+8∣y∣)
a2=a0+sign(y)(a1−d)/2a=(a0+y)fsg(y,d)+a2(1−fsg(y,d)) fhan =−r(da)fsg(a,d)−rsign(a)(1−fsg(a,d))
其中，
fsg
(
x
,
d
)
=
(
sign
(
x
+
d
)
−
sign
(
x
−
d
)
)
/
2
\operatorname{fsg}(x, d)=(\operatorname{sign}(x+d)-\operatorname{sign}(x-d)) / 2
fsg(x,d)=(sign(x+d)−sign(x−d))/2。
即最速离散TD为
{
f
h
=
fhan
(
x
1
(
k
)
−
v
(
t
)
,
x
2
(
k
)
,
r
,
h
0
)
x
1
(
k
+
1
)
=
x
1
(
k
)
+
h
x
2
(
k
)
x
2
(
k
+
1
)
=
x
2
(
k
)
+
h
f
h
\left\{\begin{array}{l} \mathrm{fh}=\operatorname{fhan}\left(x_{1}(k)-v(t), x_{2}(k), r, h_{0}\right) \\ x_{1}(k+1)=x_{1}(k)+h x_{2}(k) \\ x_{2}(k+1)=x_{2}(k)+h \mathrm{fh} \end{array}\right.
⎩
⎨
⎧fh=fhan(x1(k)−v(t),x2(k),r,h0)x1(k+1)=x1(k)+hx2(k)x2(k+1)=x2(k)+hfh
h
0
h_0
h0为滤波因子，
h
0
h_0
h0越大滤波效果越好；
h
h
h为步长,
h
h
h越小，滤波效果越好；一般来说,
h
0
h_0
h0略大于步长
h
h
h；r为快速因子，r越大，跟踪越快。
function [x1,x2] = TD_2order(u)
T=0.01;
r=10;
h=0.1;
persistent x_1 x_2
if isempty(x_1)
x_1=0;
end
if isempty(x_2)
x_2=0;
end
x1k=x_1;
x2k=x_2;
x_1=x1k+T*x2k;
x_2=x2k+T*fhan(x1k,x2k,u,r,h);
x1=x_1;
x2=x_2;
end
function f=fhan(x1,x2,u,r,h)
d=r*h; %h为滤波因子 r为调节系数，r越大跟踪效果越好，但微分信号会增加高频噪声
d0=d*h; %反之，微分信号越平滑，会产生一定的滞后
y=x1-u+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)<=d0
a=x2+y/h;
else
a=x2+0.5*(a0-d)*sign(y);
end
if abs(a)<=d
f=-r*a/d;
else
f=-r*sign(a);
end
end
下图分别是跟踪直线信号和正弦信号的结果
可以看到如果追求快速响应，滤波效果会变差，怎么平衡和取舍需要遇到实际问题不断调试。
[1]张海丽, 张宏立. 微分跟踪器的研究与应用[J]. 化工自动化及仪表, 2013, 40(004):474-477.
[2]韩京清. 自抗扰控制技术[M]. 国防工业出版社, 2008.