%基于非线性干扰观测器的直升机滑模反演控制,北京理工大学学报,2009
clear
clc
Je=1.8145;
mh=0.43;
mb=3.43;
L1=0.88;
L2=0.35;
Kc=12;
ge=9.8;
n=1;
t=0;
Dt=0.001;
x1=2*pi/180;
x2=0;
b=15;
z=0;
u=0;
c1=1;
k1=5;
h=2;
gamma=10;
dx1=0.5;
delta=1.5;
for i=1:10000
%提供三种误差情形:1、阶跃误差(t=5s时,误差由0变为4)
%2、正弦误差项
%3、正态分布随机误差
% if t<5 %阶跃误差
% d=0;
% else
% d=10; %干扰项
% end
%d=4*sin(t); %正弦误差
d=10*rand; %正态分布随机误差
%x1的大小在-30°和30°之间
if mod(x1*180/pi,360)<-30
x1=-pi/6;
end
if mod(x1*180/pi,360)>30
x1=pi/6;
end
g=2*Kc/Je*cos(x1); %x1单位为弧度
f=-(mh*ge*L1-mb*ge*L2)/Je*cos(x1);
%非线性观测器
p=b*x2;
dz=-b*z+b*(-b*x2-f-g*u);
z=z+dz*Dt;
d_est=z+p;
ud=d_est/g;
%ud=0; %无观测器时,请取消注释此句
%反演滑模控制律
yd=20*pi/180; %输入指令 期望信号20°
dyd=0; %输入信号的一阶导数
ddyd=0; %输入信号的二阶导数
y=x1; %实际输出
dy=dx1; %实际输出的一阶导数
e1=y-yd;%
a1=-k1*e1+dyd;
de1=dy-dyd;
da1=-k1*de1+ddyd; %
e2=x2-a1;%
s=c1*e1+e2;%
ddelta=gamma*abs(s);
delta=delta+ddelta*Dt;%
u_BP=1/g*(c1*(k1*e1-e2)-f+da1-e1-h*s-delta*s/(abs(s)+0.005)); %滑模反演控制器输出
u=u_BP-ud;
Dx1=x2;
Dx2=g*u+f+d;
x1=x1+Dx1*Dt;
x2=x2+Dx2*Dt;
d_store(:,n)=[d;d_est];
y_store(:,n)=[yd;y];
u_store(n)=u_BP;
n=n+1;
t=t+Dt;
end
figure(1)
plot((1:n-1)*Dt,d_store(1,:),(1:n-1)*Dt,d_store(2,:))
title('干扰实际值和观测值')
xlabel('Time/s')
legend('实际值','估计值')
figure(2)
plot((1:n-1)*Dt,y_store(1,:)*180/pi,(1:n-1)*Dt,y_store(2,:)*180/pi)
title('俯仰角期望值和实际值')
xlabel('Time/s')
legend('期望值','实际值')
figure(3)
plot((1:n-1)*Dt,u_store)
title('控制器输出')
xlabel('Time/s')