干扰观测器笔记（1）

发布日期: 2025-04-13

Disturbance Observer Study

前言

近来笔者在完成本科毕设，用到一些与干扰估计相关的知识，现在主要用到ESO(Extended State Observer)状态扩张观测器和NDO(Nolinear Disturbance Observer)非线性干扰观测器。
笔者需要观测的是一个二阶拉格朗日动力学系统，其形式为： $$ M(\zeta)\ddot{\zeta}+C(\zeta)\dot{\zeta}+G(\zeta)=\tau+d\tag{1} $$

ESO(Extended State Observer)

观测器形式为： $$\left\{ \begin{array}{c} z_{est}=z+h(v-\beta_{1}fal_{1}\\ V_{est}=V+h(D-\beta_{2}fal_{2}-M^{-1}(u-CV-G))\\ D_{est}=D-h\beta_{3}fal_{3} \end{array} \tag{2a} \right.$$ 其中： $$ fal(e,\alpha,\delta)= \begin{cases} sign(e)·\vert{e}\vert^{\alpha},\quad \vert{e}\vert\geq \delta\\ \frac{e}{\delta^{1-\alpha}}, \quad \vert{e}\vert<\delta \end{cases} \tag{2b} $$ 贴上matlab的function代码：

function [Z_est, V_est, D_est] = Adaptive_MIMO_ESO(deta, beta1, beta2, beta3, k, u, y, h,M,C,G)
% 自适应MIMO扩张状态观测器 (10 维) 
% - 估计状态: 10 维 (位置, 姿态, 机械臂力矩) % - 估计速度: 10 维 (速度, 角速度, 力矩变化率)
% - 估计扰动: 10 维 (外力, 干扰) % 
% 输入: % deta - fal 平滑因子 
% beta1, beta2, beta3 - 观测增益 
% k - 自适应因子 % u - 10 维控制输入 (加速度, 角加速度, 机械臂力矩输入) 
% y - 10 维测量 (位置, 欧拉角, 机械臂力矩) 
% h - 离散步长 % % 输出: % Z_est - 10 维状态估计 (位置/角度/力矩)
% V_est - 10 维速度估计 (速度/角速度/力矩率) 
% D_est - 10 维扰动估计 (外力/外干扰)
% **状态维度**
n = 10;  % 状态变量数量

% **持久化变量（ESO 记忆前一时刻估计值）**
persistent Z_prev V_prev D_prev
if isempty(Z_prev),  Z_prev = zeros(n,1); end
if isempty(V_prev),  V_prev = zeros(n,1); end
if isempty(D_prev),  D_prev = zeros(n,1); end

% **误差计算**
e = Z_prev - y;  % 观测误差

% **自适应增益**
gain_limit = 10;
beta1_adapt = beta1 * min(gain_limit, (1 + k * norm(e)));
beta2_adapt = beta2 * min(gain_limit, (1 + k * norm(e)));
beta3_adapt = beta3 * min(gain_limit, (1 + k * norm(e)));

% **fal 非线性误差反馈**
fal_e1 = fal(e, 0.5, deta);
fal_e2 = fal(e, 0.5, deta);
fal_e3 = fal(e, 0.8, deta);

% **ESO 主方程**
Z_half = Z_prev + (h / 2) * (V_prev - beta1_adapt * fal_e1);
V_half = V_prev + (h / 2) * (inv(M)*D_prev - beta2_adapt * fal_e2 + inv(M)*(u-C*V_prev-G));
D_half = D_prev - (h / 2) * beta3_adapt * fal_e3;

% **梯形积分更新**
Z_est = Z_prev + h * (V_half - beta1_adapt * fal_e1);
V_est = V_prev + h * (inv(M)*D_half - beta2_adapt * fal_e2 + inv(M)*(u-C*V_prev-G));
D_est = D_prev - h * beta3_adapt * fal_e3;

% **更新持久化变量**
Z_prev = Z_est;
V_prev = V_est;
D_prev = D_est;
end

function fa = fal(e, alp, deta) % fal 函数 
    eps_val = 1e-6; % 避免数值溢出 
    fa = sign(e) .* max(eps_val, abs(e).^alp) .* (abs(e) > deta) + e ./ max(eps_val, deta.^(1 - alp)) .* (abs(e) <= deta); 
end

NDO(Nolinear Disturbance Observer)

系统表述为： $$ \left\{ \begin{array}{c} \dot{x}=f(x)+g_{1}(x)u+g_{2}(x)d \\ y=x \end{array} \right. \tag{3} $$ 则观测器形式为： $$ \left\{ \begin{array}{c} \dot{z}=-l(x)g_{2}(x)z-l(x)[g_{2}(x)p(x)+f(x)+g_{1}(x)] \\ \hat{d}=z+p(x) \end{array} \tag{4a} \right. $$ 在笔者需要观测的二阶系统中，f(x), g₁(x)和g₂(x)分别为:$ $$ \left\{ \begin{array}{c} f(x)=\left[\begin{matrix} 0_{10} & I_{10}\\ 0_{10} & -M^{-1}C \\ \end{matrix} \right] \\ \\ g_{1}(x)=g_{2}(x)=\left[\begin{matrix} 0_{10} \\ M^{-1} \\ \end{matrix} \right] \end{array} \tag{4b} \right. $$

贴上matlab的function代码:

function [dhat,dotz] = NDO(u,q,qdot,M,C,G,z)
    x=[q;qdot];
    A=[zeros(10,10) eye(10,10);
       zeros(10,10) -inv(M)*C];
    B=[zeros(10,10);
        inv(M)];
    C=[ones(1,10) zeros(1,10)];
    D=0;
    Bd=[zeros(10,10);
        inv(M)];
    Aob=[zeros(10,10) eye(10,10);
         zeros(10,10) -inv(M)*C];
    Bob=[zeros(10,10);
        inv(M)];
    Cob=[ones(1,10) zeros(1,10)];
    u=u-G;
    g1=Bob;
    g2=Bob;%20*10
    Lob_2=diag([0.2,0.2,0.2,0.4,0.4,0.4,0.8,0.8,0.8,0.8]);
    Lob=[2*zeros(10,10) Lob_2];%10*20

    px=Lob*x;%10*1
    fx=A*x;%20*1
    % persistent z
    % if isempty(z), z = zeros(10,1); end
    dotz=-Lob*g2*z-Lob*(g2*px+fx+g1*u);
    dhat=z+px;