ADRC-跟踪微分器TD的Maltab实现及参数整定

问题描述：

跟踪微分器TD基本概念：

Matlab及其实现：

跟踪效果：

例1：跟踪信号 sin(t) +0.5*rand(1,1)。

例2：跟踪部分时段为方波的信号，具体形式见代码get_command。

参数整定：

参考文献

问题描述：

在设计飞行器姿态控制器的过程中，参考指令为方波形式，致使信号不连续处的导数发生较大变化，严重影响了依赖于参考指令导数及其二阶导数的控制器（如SMC）的鲁棒性。因此，尝试借助自抗扰控制中的跟踪微分器实现信号及其对时间的导数的平滑处理。

跟踪微分器TD基本概念：

跟踪微分器(Tracking Differentiator, TD) 是“自抗扰控制器”（ADRC）[3] 的组成部分，最早由韩京清老师提出。提出的目的是为了解决在实际问题中，从不连续（如方波）或带随机噪声（如模型不确定性或外部干扰）的参考信号中，合理提取连续信号及微分信号的问题。简单来说，就是平滑化信号。在实际应用中，我们所得到的信号往往是带有噪声的,为了从这些含噪信号中提取或恢复原始信号，就需要设计滤波器，以得到原始信号的最佳逼近 [1]，因此TD可视为滤波器。

Matlab及其实现：

本文所实现的离散化TD参考了韩京清老师发表在《系统仿真学报》上的论文《跟踪微分器滤波性能研究》，参数设置较原文有所调整，复现结果可跟踪下文示例信号。

将下述代码放到同一目录下即可运行，Matlab版本为2022a，程序入口为main.m函数。

main.m

% 测试微分跟踪器.
clear,clc;

%% 初始化, 生成跟踪信号.
T = 0.01;  %积分步长.
End = 30;
tf = 0:T:End;    %跟踪时长.

%例程1
a = -1; b = 1;
v = sin(tf) + 0.5*(a + (b-a)*rand(1,1));

%例程2
%v = zeros(1, length(tf));
% for i = 1:length(tf)
%     v(i) = get_command(1 + i*T, End);
% end

x1 = zeros(1, length((v)));
x2 = zeros(1, length((v)));


%% 使用TD获取处理后的信号及其微分.
for i = 1:length(v)
    [x1(1, i), x2(1, i)] = TD(v(i), T);
end


%% 绘制信号.
close all;

figure
plot(tf, v, tf, x1, tf, x2);
xlabel('time'), ylabel('signal'), legend('原始输入 v', '跟踪输入x1', '跟踪输入导数x2' )

figure
subplot(3,1,1);
plot(tf,v);
legend('原始输入 v')
xlabel('time'), ylabel('signal');

subplot(3,1,2);
plot(tf,x1);
legend('跟踪输入x1')
xlabel('time'), ylabel('signal');

subplot(3,1,3);
plot(tf,x2)
legend('跟踪输入导数x2' )
xlabel('time'), ylabel('signal');


%% 参考文献
% [1]武利强,林浩,韩京清.跟踪微分器滤波性能研究[J].系统仿真学报, 2004, 16(4):3.DOI:10.3969/j.issn.1004-731X.2004.04.012.
% [2]https://blog.csdn.net/m0_37764065/article/details/108668033.  %复制链接得去掉最后的.

fst.m

function f=fst(v, x1_k, x2_k, r, h1, r2)
% fst = −r*sat(g(k),delta), 快速控制最优综合函数, 也称fhan函数
delta = h1*r;			%h1为滤波因子  r为调节系数，r越大跟踪效果越好，但微分信号会增加高频噪声
delta1 = delta*h1;	    %反之，微分信号越平滑，会产生一定的滞后
e_k = x1_k-v;
y_k = e_k + r2*h1*x2_k;

g_k = g(x2_k, y_k, delta, delta1, r, h1);
f = -r*sat(g_k, delta);


%% 局部函数
function g_k = g(x2_k, y_k, delta, delta1, r, h1)
a0=sqrt(delta^2 + 8*r*abs(y_k));

if abs(y_k) >= delta1
    g_k = x2_k + (a0-delta)*sign(y_k)/2;
else
    g_k = x2_k + y_k/h1; 
end


function f = sat(x, delta)
if abs(x)>=delta
    f = sign(x);
else
    f = x/delta;
end

TD.m

function [x1_, x2_] = TD(v, T)
% v 是原始期望指令, x2 = x1_dot, x1是滤波后的指令, T为积分步长.
% T = 0.01;  %积分步长, , 韩老师文章记作h.
r = 1000;    %决定跟踪快慢的参数.
r2 = 1;      %
h = 0.01;  %滤波性能与相位损失之间的权衡参数.
h1 = 8*h;

persistent x1 x2;   %仅函数内部可见的全局变量.

% 初始化变量.
if isempty(x1)
    x1 = v;
end
if isempty(x2)
    x2 = 0;
end
x1_k = x1;
x2_k = x2;

% TD 的离散化公式.
% x1(k+1) = x1(k) + h*x2(k)
% x2(k+1) = x2(k) + h*fst(x1(k)-v(k), x2(k), r, h1)
x1 = x1_k + T*x2_k;
x2 = x2_k + T*fst(v, x1_k, x2_k, r, h1, r2); %因为x1k是逐步算出的，故单独传入x1(k)与v(k)

% 输出
x1_ = x1;
x2_ = x2;
end

get_command.m

function X_d = get_command(t, End)
%alpha_c为攻角控制指令
%beta_c为侧滑角控制指令
%sigma_c为倾侧角控制指令

t_1 = 8;
t_2 = 20;

alpha_1 = 18;
alpha_2 = 15;
alpha_3 = 10;


sigma_1 = 20;
sigma_2 = -15;


% 
if t < t_2
alpha_c = alpha_1 * (t < t_1) + getdot(alpha_1, alpha_2, t_1, End, t) * (t >= t_1 && t < t_2); 
beta_c = 0;
sigma_c = sigma_1 * (t < t_1) + getdot(sigma_1, sigma_2, t_1, End, t) * (t >= t_1 && t < t_2);

% 函数指令
aero_angle_d            = [alpha_c beta_c sigma_c]';           %姿态角指令 弧度制 单位/rad
%aero_angle_d_dot        = [((alpha_1-alpha_2)/Simu.tf-t_1)*pi/180 0 ((sigma_1-sigma_2)/Simu.tf-t_1)*pi/180]';       %姿态角指令一阶导
aero_angle_d_dot        = [0 0 0]';       %姿态角指令一阶导
aero_angle_d_dot_dot    = [0 0 0]';     %姿态角指令二阶导

else
alpha_c = alpha_2; 
beta_c = 0;
sigma_c = 0;

% 函数指令
aero_angle_d            = [alpha_c beta_c sigma_c]';           %姿态角指令 弧度制 单位/rad
aero_angle_d_dot        = [0 0 0]';       %姿态角指令一阶导
aero_angle_d_dot_dot    = [0 0 0]';     %姿态角指令二阶导
end


% X_d                     = [aero_angle_d; aero_angle_d_dot; aero_angle_d_dot_dot]';
X_d = aero_angle_d(1);


function angle = getdot(angle1, angle2, t1, t2, t)
% 姿态角控制指令计算
%  angle为姿态角控制指令输出
%  angle1为初始时刻姿态角，angle2为结束时刻姿态角
%  t1为初始时刻，t2为结束时刻，t为当前时刻

angle = (angle2 - angle1)/(t2 - t1)*(t - t1) + angle1;