最简单ADRC的matlab编程实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
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;%表示跟踪快慢
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('PlantModel',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);
    x3(k)=-v1_last^2;
    v1_last=v1(k);
    v2_last=v2(k);
    v0_last=vn(k);
    %----扩张状态观测器--%
    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);
    z3_last=z3(k);
    %---非线性误差反馈----%
    e1(k)=v1(k)-z1(k);
    e2(k)=v2(k)-z2(k);
    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);
end
figure(1);
plot(t,u,'r');
figure(2);
subplot(311);
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


    

1
2
3
4
5
6
7
8
9
%系统方程
function dy=PlantModel(t,y,flag,p1,p2)
u=p1;
time=p2;
dy=zeros(2,1);
dy(1)=y(2);
dy(2)=-y(1)^2+u;
end