matlab位置跟踪仿真

位置跟踪仿真

 

1、pid控制

PlantModel.m

function dx=PlantModel(t,x,flag,para)
global S A F c alfa beta kp kd
dx=zeros(2,1);

S=2;
if S==1
   rin=1.0;
   drin=0;
elseif S==2
   A=0.5;F=3;
   rin=A*sin(F*2*pi*t);
   drin=A*F*2*pi*cos(F*2*pi*t);
end

% c=30;
% alfa=500;
% beta=10;
kp=100;
kd=1.2;
e1=rin-x(1);
e2=drin-x(2);

% s=c*e1+e2;
% u=(alfa*abs(e1)+beta*abs(e2))*sign(s);

u=kp*e1+kd*e2;

dx(1)=x(2);
dx(2)=-(25+0*sin(3*2*pi*t))*x(2)+(133+0*sin(1*2*pi*t))*u;

pidtracking.m

clear all;
close all;
global S A F c alfa beta kp kd;

xk=[0,0];

ts=0.001;
T=1;
TimeSet=[0:ts:T];

[t,y]=ode45('PlantModel',TimeSet,xk,[],[]);
x1=y(:,1);
x2=y(:,2);

if S==1
   rin=1.0;
   drin=0;
elseif S==2
   rin=A*sin(F*2*pi*t);
   drin=A*F*2*pi*cos(F*2*pi*t);
end

e1=rin-x1;
e2=drin-x2;
% s=c*e1+e2;

for k=1:1:T/ts+1
%    u(k)=(alfa*abs(e1(k))+beta*abs(e2(k)))*sign(s(k));
   u(k)=kp*e1(k)+kd*e2(k);
end


set(gca,'fontsize',20)
% figure;
subplot(211)
plot(t,rin,'r--','linewidth',2);
hold on;
plot(t,y(:,1),'b','linewidth',2)
set(gca,'fontsize',20)
xlabel('time/s');ylabel('position');
%  legend('ideal position','actual position')
subplot(212)
plot(t,u,'b','linewidth',2);
set(gca,'fontsize',20)
xlabel('time/s');ylabel('u');

2 SVC

PlantModel.m

function dx=PlantModel(t,x,flag,para)
global S A F c alfa beta
dx=zeros(2,1);

S=2;
if S==1
   rin=1.0;
   drin=0;
elseif S==2
   A=0.5;F=3;
   rin=A*sin(F*2*pi*t);
   drin=A*F*2*pi*cos(F*2*pi*t);
end

c=30;
alfa=500;
beta=10;

e1=rin-x(1);
e2=drin-x(2);

s=c*e1+e2;


%  	delta=0.08;
% 	kk=1/delta;
% 	if s>delta
% 		sats=1;
% 	elseif abs(s)<=delta
% 		sats=kk*s;
% 	elseif s<-delta
% 		sats=-1;
%     end 

% u=(alfa*abs(e1)+beta*abs(e2))*sats;

 u=(alfa*abs(e1)+beta*abs(e2))*sign(s);
% u=(alfa*abs(x(1))+beta*abs(x(2)))*sign(s);
%  u=10*sign(s);

dx(1)=x(2);
dx(2)=-(25+5*sin(3*2*pi*t))*x(2)+(133+50*sin(1*2*pi*t))*u;

svctracking.m

clear all;
close all;
global S A F c alfa beta

xk=[0,0];

ts=0.001;
T=1;
TimeSet=[0:ts:T];

[t,y]=ode45('PlantModel',TimeSet,xk,[],[]);
x1=y(:,1);
x2=y(:,2);

if S==1
   rin=1.0;
   drin=0;
elseif S==2
   rin=A*sin(F*2*pi*t);
   drin=A*F*2*pi*cos(F*2*pi*t);
end

e1=rin-x1;
e2=drin-x2;
s=c*e1+e2;
   delta=0.08;
	kk=1/delta;

    
for k=1:1:T/ts+1
%    	if s(k)>delta
% 		sats(k)=1;
% 	elseif abs(s(k))<=delta
% 		sats(k)=kk*s(k);
% 	elseif s(k)<-delta
% 		sats(k)=-1;
%     end 
    
   u(k)=(alfa*abs(e1(k))+beta*abs(e2(k)))*sign(s(k));
end
set(gca,'fontsize',20)
figure(1);
plot(t,rin,'--',t,y(:,1),'b');
set(gca,'fontsize',20)
xlabel('time(s)');ylabel('Position tracking');
set(gca,'fontsize',20)
figure(2);
plot(t,u,'r');
xlabel('time(s)');ylabel('u');
set(gca,'fontsize',20)
figure(3);
plot(e1,e2,'r',e1,-c*e1,'b');
xlabel('time(s)');ylabel('Phase trajectory');
set(gca,'fontsize',20)
figure;
subplot(211)
plot(t,rin,'r--','linewidth',2);
hold on;
 plot(t,y(:,1),'b','linewidth',2)
 set(gca,'fontsize',20)
xlabel('time/s');ylabel('position');
%  legend('ideal position','actual position')
subplot(212)
plot(t,u,'b','linewidth',2);
set(gca,'fontsize',20)
xlabel('time/s');ylabel('u');
set(gca,'fontsize',20)
figure
plot(t,rin-y(:,1))