clc;
clear all;
close all;
%运行时间
time = 10;
%仿真步长
h = 0.01;
%时间定义
t =0.01 : h : time;
%跟踪信号
v0 = zeros( 1 , time/h);
for i = time/h/2+1 : time/h
v0(i) = 1;
end
rand_noise = 0.05 * randn(1 , time/h);
%加入随机噪声
vn = v0 + rand_noise;
%----------------------ADRC--------------------------%
%%
%--参数初始化--%
%跟踪微分器参数
r = 100;%r表示跟踪快慢
h0 = 5 * h;%h0代表信号平滑度（滤波效果）
v1_last = 0;
v2_last = 0;
v0_last = 0;
%扩张状态观测器参数
beta01 = 10;
beta02 = 200;
beta03 = 30;
alpha1 = 0.5;%文献里给了值，就别动了
alpha2 = 0.25;%文献里给了值，就别动了
delta = 0.0025;
b=1;
z1_last = 0;
z2_last = 0;
z3_last = 0;
%非线性误差反馈
nlsef_alpha1 = 0.7;
nlsef_alpha2 = 1;
%被控对象初始化
temp_y=[0.5 ; 0];
u_last= 0 ;
%%
%--ADRC正式开始--%
for k=1 : time/h
%第一轮迭代的处理
%两个参数分别为控制量和当前时间
parameter1 = u_last;
parameter2 = k * h;
tSpan=[0 h];
%利用龙格库塔法求解微分方程
[~,total_y] = ode45('plant',tSpan,temp_y,[],parameter1,parameter2);
%total_state里的元素都是龙格库塔一点点计算的结果，直接用最后一列，即计算结果即可
temp_y = total_y(length(total_y),:);
%记录下输出和输出的微分值
y(k) = temp_y(1);
dy(k) = temp_y(2);
%--跟踪微分器TD--%
v1(k) = v1_last + h * v2_last
v2(k) = v2_last + h * fst(v1_last - vn(k) , v2_last , r , h0);
% v2(k) = v2_last + deltaT * fst(v1_last - v0(k) , v2_last , r , h0);
x3(k) = -v1_last^2;
v1_last = v1(k);
v2_last = v2(k);
% v0_last = v0(k);
v0_last = vn(k);
%--扩张状态观测器ESO--%
e = z1_last - y(k);
z1(k) = z1_last + h * (z2_last - beta01 * e);
z2(k) = z2_last + h * (z3_last - beta02 * (fal(e , alpha1 , delta)) +b * u_last);
z3(k) = z3_last - h * beta03 * (fal(e , alpha2 , delta));
z1_last = z1(k);
z2_last = z2(k);
%--非线性误差反馈NLSEF--%
e1(k) = v1(k) - z1(k);
e2(k) = v2(k) - z2(k);
%使用非线性误差反馈，其实就是非线性的PID
u0(k) = beta01 * fal(e1(k) , nlsef_alpha1 , delta) + beta02 * fal(e2(k) , nlsef_alpha2 , delta);
u(k) = u0(k) - z3(k)/b;
u_last = u(k);
% %%
% %--可对比PD控制器--%
% u(k) = kp * e1(k) + kd * e2(k);
% u_last = u(k);
end
figure(1);
plot(t,u,'r');
figure(2);
subplot(311);
% plot(t,z1,'r',t,y,'k:',t,v0,'b','linewidth',2);
plot(t,z1,'r',t,y,'k:',t,vn,'b','linewidth',2);
xlabel('time(s)'),ylabel('z1,y');
legend('估计输出信号', '实际输出信号');
subplot(312);
plot(t,z2,'r',t,dy,'k:','linewidth',2);
xlabel('time(s)'),ylabel('z2,dy');
legend('估计输出微分信号', '实际输出微分信号');
subplot(313);
plot(t,z3,'r',t,x3,'k:','linewidth',2);
xlabel('time(s)'),ylabel('z3,x3');
legend('估计扰动', '实际扰动');
%%
%---------------------------函数部分-----------------------------%
%sat函数
function y=sat(a,d)
if abs(a)<=d
y=a/d;
else
y=sgn(a);
end
end
%符号函数
function y=sgn(x)
if x>0
y=1;
elseif x<0;
y=-1;
else
y=0;
end
end
%fst函数
function fn=fst(x1,x2,r,h)
d=h*r;
d0=h*d;
y=x1+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)*sgn(y);
end
fn=-r*sat(a,d);
end
%fal函数
function y=fal(e,alpha,delta)
if abs(e)>delta
y=abs(e)^alpha*sign(e);
else
y=e/(delta^(1-alpha));
end
end
% %-----------------------------------------------------------------%
%%
%系统方程----------------可以尝试多种龙格库塔的写法，把这个函数搞清楚，先照写
function dy = PlantModel(t,y,flag,p1,p2)
%第一个参数：控制量
u=p1;
%第二个参数：当前时间
time=p2;
%初始化2*1的向量
dy=zeros(2,1);
%方程
dy(1)=y(2);
dy(2)=-y(1)^2+u;
end
%--参数初始化--%
h = 0.01;
%跟踪微分器参数
r = 100;%r表示跟踪快慢
h0 = 5 * h;%h0代表信号平滑度（滤波效果）
v1_last = 0;
v2_last = 0;
v0_last = 0;
%扩张状态观测器参数
beta01 = 100;
beta02 = 200;
beta03 = 300;
alpha1 = 0.5;%文献里给了值，就别动了
alpha2 = 0.25;%文献里给了值，就别动了
delta = 0.0025;
b=1;
z1_last = 0;
z2_last = 0;
z3_last = 0;
%非线性误差反馈
nlsef_alpha1 = 0.7;
nlsef_alpha2 = 0.95;
%sat函数
function y=sat(a,d)
if abs(a)<=d
y=a/d;
else
y=sgn(a);
end
end
%符号函数
function y=sgn(x)
if x>0
y=1;
elseif x<0;
y=-1;
else
y=0;
end
end
In view of the fact that quite a few people asked me to ask for the simulink file of ADRC, I just lost a link to Baidu cloud disk, just click like it and go!
Link: https://pan.baidu.com/s/1d5x1jIekg-NVAIixeWeHIg
Extraction code: nfgg
copy this content and open the Baidu Netdisk mobile app, the operation is more convenient
Created on:
2020-11-7
9:29
Modified on:
2020-12-4
1:09
Modified content: Add ADRC's simulink download link.