自抗擾控制（ADRC）—— 擴展狀態觀測器

本文系韓京清老師1995年論文發表在控制與決策的論文《一類不確定對象的擴張狀態觀測器》的解讀與代碼實現。

---

原理

狀態觀測器的作用是通過系統的輸出估計系統的狀態。

線性系統理論中狀態觀測器的設計一般要求系統的內部動態（A,B,C）是已知的。但這不符合工程實際，很多時候我們並不瞭解控制對象的內部動態。

自抗擾控制的框架中，把系統的未知動態和外部擾動合併成總擾動，並且用一個擴展狀態來表示。

擴張狀態觀測器（Extended State Observer）的目的就是根據系統的輸出 $y$ 估計出這個擴張狀態。

在形如$(1)$式的一大類系統中，擴張狀態的估計可以視爲信號微分的估計：

舉個簡單的例子，根據空中移動的物體的軌跡把物體所受到的合外力（包含隨機擾動）估計出來。當已知了物體原本的合外力，就可以對物體施加控制（力）：控制量等於我們期望物體產生的運動所需要的力減去物體原來收到的合外力。這樣物體就會按照我們控制的軌跡移動。

其實原理很簡單，寥寥數行就講得清楚明白。

$g$ 的選取也很簡單，對$(5)$式做線性化處理：
$\left[\begin{array}{c} \delta \dot{x}_1 \\ \delta \dot{x}_2 \\ \vdots \\ \delta \dot{x}_n \\ \delta \dot{x}_{n+1} \\ \end{array}\right] = \left[\begin{array}{c} -g_1' & 1 & 0 & \cdots& 0\\ -g_2' & 0 & 1 & \cdots& 0\\ \vdots & \vdots & & &\vdots \\ -g_1' & 0 & \cdots & &1 \\ -g_{n+1}' & 0 &\cdots & &0\\ \end{array}\right] \left[\begin{array}{c} \delta x_1 \\ \delta x_2 \\ \vdots \\ \delta x_n \\ \delta x_{n+1} \\ \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ \vdots \\ 0 \\ -b(t) \\ \end{array}\right]$
上式中的方陣是個友矩陣，很容易對它做極點配置。

只要 $b$ 是有界的，就可以做到漸進跟蹤。

代碼

龍格庫塔

import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

def dxdt(F, X, t, h=1e-2):
    assert(len(F)==len(X))
    X = np.array(X)
    K1 = np.array([f(X, t) for f in F])
    dX = h*K1/2
    K2 = np.array([f(X+dX, t+h/2) for f in F]) 
    dX = h*K2/2
    K3 = np.array([f(X+dX, t+h/2) for f in F])
    dX = h*K3 
    K4 = np.array([f(X+dX, t+h) for f in F])

    dX = (K1 + 2*K2 + 2*K3 + K4)*h/6

    return dX, np.array([f(X, t) for f in F])

def trajectory(F, initial_point, num_points=1e4, h=1e-2):
    assert(len(F)==len(initial_point))
    
    n = int(num_points)
    dim = len(initial_point)
    X = np.zeros([n,dim])
    D = np.zeros([n,dim])
    
    X[0,:] = initial_point
    for k in range(1,n):
        dX, D[k-1,:] = dxdt(F,X[k-1,:],h*(k-1),h)
        X[k,:] = X[k-1,:] + dX
        
    return X.T, D.T

構造被觀測系統

論文中的實驗結果

代碼復現：

def f1(X, t):
    x, y = X[0], X[1]    
    return y

def f2(X, t):
    x, y = X[0], X[1]
    return -x*x*x - x -0.2*y + v(t)

def v(t):
    return 0.5 * np.sign(np.cos(t/2))

h = 1e-2
N = 2000
x,d = trajectory([f1,f2],(0,0),N,h)
fig = plt.figure()
plt.plot(x[0], color='red', label='original')
plt.plot(x[1], color='blue', label='1st order')
plt.plot(d[1], color='black', label='2nd order')
plt.legend()
plt.show()

下面我們將上述生成信號（紅線）作爲狀態觀測器的輸入，並期望ESO可以把黑線重構出來。

v0 = x[0]

def v(t):
    return v0[int(t/h)]

# 極點配置
p = np.poly1d([-15,-15,-15],True)
_, b1, b2, b3 = tuple(p.coef)


def g1(X, t):
    x1,x2,x3 = X[0], X[1], X[2]    
    return x2 - b1 * nle(x1 - v(t))

def g2(X, t):
    x1, x2, x3 = X[0], X[1], X[2]    
    return x3 - b2 * nle(x1 - v(t))

def g3(X, t):
    x1, x2, x3 = X[0], X[1], X[2]    
    return -b3 * nle(x1 - v(t))

# 論文中使用非線性誤差，這裏實驗用線性的
def nle(e):
    # nonlinear error
    # return  np.sign(e)*np.sqrt(np.abs(e))
    return e


N = 2000
z,_ = trajectory([g1,g2,g3],(0,0,0),N,h)
fig = plt.figure()
plt.plot(z[0], color='red', label='tracking')
plt.plot(z[1], color='blue', label='1st order')
plt.plot(z[2], color='black', label='2nd order')
plt.legend()
plt.show()

跟蹤結果以及二階導數

和原系統的擴張狀態對比

fig = plt.figure()
plt.plot(d[1], color='red', label='uncertain state')
plt.plot(z[2], color='black', label='tracking')
plt.legend()
plt.show()