自抗擾控制器中跟蹤微分器的設計

韓京清老師所寫的《自抗擾控制技術》中，介紹了跟蹤微分器的設計過程，下面就把數學原理和代碼來詳細記錄跟蹤微分器的設計過程。

跟蹤微分器的作用就是從被噪聲污染的信号中合理的提取微分信号。

（1）一般的跟蹤微分器

傳統的微分信号是用如下的微分環節來得到的：

v ˙ = w ( s ) v = s T s + 1 v = 1 T ( v − v T s + 1 ) \dot{v}=w(s)v=\frac{s}{Ts+1}v=\frac{1}{T}(v-\frac{v}{Ts+1}) v˙=w(s)v=Ts+1s​v=T1​(v−Ts+1v​)，其中 T T T是比較小的時間常數。其等價的模型如下圖所示的Simulink，其中 T = 0.01 T=0.01 T=0.01，輸入信号是 s i n ( t ) sin(t) sin(t)。

所得到的結果如下圖：

當輸入信号 v ( t ) v(t) v(t)變化比較緩慢而時間常數 T T T比較小時，就有近似關系 v ( t − T ) ≈ 1 T s + 1 v ( t ) v(t-T)\approx\frac{1}{Ts+1}v(t) v(t−T)≈Ts+11​v(t)，進而 v ( t ) ˙ ≈ 1 T ( v ( t ) − v ( t − T ) ) \dot{v(t)}\approx\frac{1}{T}(v(t)-v(t-T)) v(t)˙​≈T1​(v(t)−v(t−T))。證明過程有點長，有興趣的可以去看自抗擾控制這本書的第二章。

下面是用代碼來實作上面的過程，如果令 v ‾ = v T s + 1 \overline{v}=\frac{v}{Ts+1} v=Ts+1v​，則上面的微分環節公式就可以寫成 v ˙ = 1 T ( v − v ‾ ) \dot{v}=\frac{1}{T}(v-\overline{v}) v˙=T1​(v−v)，寫到這裡，關鍵就是如何求出 v ‾ \overline{v} v。可以利用 v v v和 v ‾ \overline{v} v之間的傳遞函數關系寫出 v ‾ ˙ = 1 T ( v − v ‾ ) \dot{\overline{v}}=\frac{1}{T}(v-\overline{v}) v˙=T1​(v−v)，就會得到下面的遞推公式：

{ v ( k ) ˙ = 1 T ( v ( k ) − v ‾ ( k ) ) v ‾ ( k + 1 ) = v ‾ ( k ) + h 1 T ( v ( k ) − v ‾ ( k ) ) \left\{ \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} \right. {v(k)˙​=T1​(v(k)−v(k))v(k+1)=v(k)+hT1​(v(k)−v(k))​

其中 h h 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）改進的跟蹤微分器

為了消除或減弱噪聲放大效應，把微分近似公式換成下面這一種。

v ( t ) ˙ ≈ v ( t − τ 1 ) − v ( t − τ 2 ) τ 1 − τ 2 , 0 &lt; τ 1 &lt; τ 2 \dot{v(t)}\approx\frac{v(t-\tau_1)-v(t-\tau_2)}{\tau_1-\tau_2}, 0&lt;\tau_1&lt;\tau_2 v(t)˙​≈τ1​−τ2​v(t−τ1​)−v(t−τ2​)​,0<τ1​<τ2​

延遲信号 v ( t − τ 1 ) v(t-\tau_1) v(t−τ1​)可以由慣性環節 1 τ 1 s + 1 \frac{1}{\tau_1s+1} τ1​s+11​來擷取，這個微分近似公式的傳遞函數為

v ( t ) ˙ = 1 τ 1 − τ 2 ( 1 τ 1 s + 1 − 1 τ 2 s + 1 ) v ( t ) = s τ 1 τ 2 s 2 + ( τ 1 + τ 2 ) s + 1 v ( t ) \dot{v(t)}=\frac{1}{\tau_1-\tau_2}(\frac{1}{\tau_1s+1}-\frac{1}{\tau_2s+1})v(t)=\frac{s}{\tau_1\tau_2s^2+(\tau_1+\tau_2)s+1}v(t) v(t)˙​=τ1​−τ2​1​(τ1​s+11​−τ2​s+11​)v(t)=τ1​τ2​s2+(τ1​+τ2​)s+1s​v(t)

其所對應的Simulink如下圖所示， τ 1 = 0.01 , τ 2 = 0.02 \tau_1=0.01,\tau_2=0.02 τ1​=0.01,τ2​=0.02。

其所對應的仿真圖如下所示。

根據上面的傳遞函數，可以寫出它的微分形式：

{ x 1 ˙ = x 2 x 2 ˙ = − 1 τ 1 τ 2 ( x 1 − v ( t ) ) − τ 1 + τ 2 τ 1 τ 2 x 2 v ˙ = x 2 \left\{ \begin{array}{lr} \dot{x_1}=x_2 &amp; \\ \dot{x_2} = -\frac{1}{\tau_1\tau_2}(x_1-v(t))-\frac{\tau_1+\tau_2}{\tau_1\tau_2}x_2 &amp; \\ \dot{v}=x_2 \end{array} \right. ⎩⎨⎧​x1​˙​=x2​x2​˙​=−τ1​τ2​1​(x1​−v(t))−τ1​τ2​τ1​+τ2​​x2​v˙=x2​​​

根據上面的微分方程，離散化成計算機容易處理的形式：

{ x 1 ( k + 1 ) = x 1 ( k ) + h x 2 ( k ) x 2 ( k + 1 ) = x 2 ( k ) − h ( 1 τ 1 τ 2 ( x 1 ( k ) − v ( k ) ) + τ 1 + τ 2 τ 1 τ 2 x 2 ( k ) ) v ( k ) ˙ = x 2 ( k ) \left\{ \begin{array}{lr} x_1(k+1) = x_1(k)+hx_2(k) &amp; \\ 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)) &amp; \\ \dot{v(k)}=x_2(k) \end{array} \right. ⎩⎨⎧​x1​(k+1)=x1​(k)+hx2​(k)x2​(k+1)=x2​(k)−h(τ1​τ2​1​(x1​(k)−v(k))+τ1​τ2​τ1​+τ2​​x2​(k))v(k)˙​=x2​(k)​​

下面是代碼：

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);
           

仿真圖如下：

而傳統的微分器的效果如下：

兩者的對比已經非常明顯了。（我也不清楚怎麼差别這麼大，是噪聲幅度太大了嗎）