基于状态空间的PID控制

先记录下，有时间再整理

clc;
clear all;
ts=0.001;
tm=2;
distance=tm/ts;
A=[0 1 0 0 0 0;-9.3*10^6*0.1 -35.7*10^2 380.7*10^4*2 0 0 0;0 -30 -243 0 0 0;0 0 0 0 1 0;0 0 0 -0.12*10^2 -29.8*5*10^2 1.6*10^-2;0 0 1.9*10^8*0.12 0 -27.3*10^5*0.5 -0.0245*10^3*0.2];
B=[0 0;0 0;125 0;0 0;0 -1.49*10^1;0 0];
C=[0 0 0 0 1 0];
D=[0 0];
[num_u,den_u]=ss2tf(A,B,C,D,1);
[num_t,den_t]=ss2tf(A,B,C,D,2);

sys_u=tf(num_u,den_u);
sys_t=tf(num_t,den_t);

dsys_u=c2d(sys_u,ts)
dsys_t=c2d(sys_t,ts)

[dnum_u,dden_u]=tfdata(dsys_u,'v')
[dnum_t,dden_t]=tfdata(dsys_t,'v')

y1_u=0;y2_u=0;y3_u=0;y4_u=0;y5_u=0;y6_u=0;
u1=0;u2=0;u3=0;u4=0;u5=0;u6=0;

%u_t=200*ones(distance,1)
y1_t=0;y2_t=0;y3_t=0;y4_t=0;y5_t=0;y6_t=0;
u_t6=0;
u_t5=0;
u_t4=0;
u_t3=0;
u_t2=0;
u_t1=0

init_kp=135;    %初始pid参数
init_ki=0.001;
init_kd=1000;

error1=0;
error2=0;

x=[0,0,0];


rand('seed',0);
u_tr=200*ones(distance,1)+40*(rand(distance,1)-0.5*ones(distance,1));
u_tl=100*ones(distance,1)+40*(rand(distance,1)-0.5*ones(distance,1));
for k=1:distance
    time(k)=ts*k;
    yd_u(k)=8.0;
    du(k)=init_kp*x(1)+init_ki*x(2)+init_kd*x(3);
    u(k)=u1+du(k);
    if u(k)>=24
        u(k)=24;
    end
    if u(k)<=-24
        u(k)=-24;
    end
    y_u(k)=(-dden_u(2)*y1_u-dden_u(3)*y2_u-dden_u(4)*y3_u-dden_u(5)*y4_u-dden_u(6)*y5_u-dden_u(7)*y6_u+dnum_u(2)*u1+dnum_u(3)*u2+dnum_u(4)*u3+dnum_u(5)*u4+dnum_u(6)*u5+dnum_u(7)*u6);
    y_t(k)=(-dden_t(2)*y1_t-dden_t(3)*y2_t-dden_t(4)*y3_t-dden_t(5)*y4_t-dden_t(6)*y5_t-dden_t(7)*y6_t+dnum_t(2)*u_t1+dnum_t(3)*u_t2+dnum_t(4)*u_t3+dnum_t(5)*u_t4+dnum_t(6)*u_t5+dnum_t(7)*u_t6);
    error=yd_u(k)-(y_u(k)+y_t(k));
    u6=u5;
    u5=u4;
    u4=u3;
    u3=u2;
    u2=u1;
    u1=u(k);
    
    u_t6=u_t5;
    u_t5=u_t4;
    u_t4=u_t3;
    u_t3=u_t2;
    u_t2=u_t1;
    u_t1=u_tr(k);
    
    y6_u=y5_u;
    y5_u=y4_u;
    y4_u=y3_u;
    y3_u=y2_u;
    y2_u=y1_u;
    y1_u=y_u(k);
    
    y6_t=y5_t;
    y5_t=y4_t;
    y4_t=y3_t;
    y3_t=y2_t;
    y2_t=y1_t;
    y1_t=y_t(k);
    
    x(1)=error-error1;
    x(2)=error-2*error1+error2;
    x(3)=error;
    
    error2=error1;
    error1=error;
end

figure(1);
plot(time,yd_u,'r',time,y_u,'r','linewidth',2);
xlabel('时间（s）'),ylabel('yd,y');
grid on;
title('增量式PID跟踪信号响应曲线');
legend('理想位置信号','位置追踪');
hold on;
%figure(2);
plot(time,y_t,'b','linewidth',2);
xlabel('时间（s）'),ylabel('yd,y');
grid on;
title('只在T作用下');

%figure(3);

plot(time,y_t+y_u,'b','linewidth',2);
xlabel('时间（s）'),ylabel('yd,y');
grid on;
title('叠加');

figure(2)
plot(time,u_tr,'p-b',time,u_tl,'-.k','linewidth',2);
xlabel('时间（s）'),ylabel('yd,y');
grid on;
title('input_t');