%PID Feedforward Controler clear all; close all; ts=0.001; sys=tf(133,[1,25,0]); dsys=c2d(sys,ts,'z'); [num,den]=tfdata(dsys,'v'); u_1=0;u_2=0; y_1=0;y_2=0; error_1=0;ei=0; for k=1:1:1000 time(k)=k*ts; A=0.5;F=3.0; yd(k)=A*sin(F*2*pi*k*ts); dyd(k)=A*F*2*pi*cos(F*2*pi*k*ts); ddyd(k)=-A*F*2*pi*F*2*pi*sin(F*2*pi*k*ts); %Linear model y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2; error(k)=yd(k)-y(k); ei=ei+error(k)*ts; up(k)=80*error(k)+20*ei+2.0*(error(k)-error_1)/ts; uf(k)=25/133*dyd(k)+1/133*ddyd(k); M=2; if M==1 %Only using PID u(k)=up(k); elseif M==2 %PID+Feedforward u(k)=up(k)+uf(k); end if u(k)>=10 u(k)=10; end if u(k)<=-10 u(k)=-10; end u_2=u_1;u_1=u(k); y_2=y_1;y_1=y(k); error_1=error(k); end figure(1); subplot(211); plot(time,yd,'r',time,y,'k:','linewidth',2); xlabel('time(s)');ylabel('yd,y'); legend('Ideal position signal','Position tracking'); subplot(212); plot(time,error,'r','linewidth',2); xlabel('time(s)');ylabel('error'); figure(2); plot(time,up,'k',time,uf,'b',time,u,'r','linewidth',2); xlabel('time(s)');ylabel('up,uf,u');注释这段代码

这段代码实现了一个基于 PID 控制器和前馈补偿的控制系统。具体来说，它包括以下几个部分：

1. 系统建模：首先定义一个连续系统模型 sys，其中传递函数为 133/(s^2 + 25s)，然后使用 c2d 函数将其离散化得到离散系统模型 dsys。

sys=tf(133,[1,25,0]); 
dsys=c2d(sys,ts,'z'); 
[num,den]=tfdata(dsys,'v');

2. 反馈控制器：利用 PID 控制器实现反馈控制。在每个时刻 k，通过测量系统输出 y(k) 与期望输出 yd(k) 之间的误差，计算出控制器输出 u(k)。

error(k)=yd(k)-y(k); 
ei=ei+error(k)*ts; 
up(k)=80*error(k)+20*ei+2.0*(error(k)-error_1)/ts; 

其中，ei 表示误差的积分项，error_1 表示上一个时刻的误差。这里的 PID 控制器系数是手动调整得到的，可以根据实际应用需要进行调整。
3. 前馈补偿：为了进一步提高系统的跟踪性能，引入了前馈补偿。在每个时刻 k，通过测量期望输出 yd(k) 的变化率 dyd(k) 和加速度 ddyd(k)，计算出前馈控制器输出 uf(k)。

uf(k)=25/133*dyd(k)+1/133*ddyd(k);

其中，系数 25/133 和 1/133 是根据系统模型得到的，可以根据实际应用需要进行调整。
4. 控制器输出求和：最终的控制器输出 u(k) 是反馈控制器输出 up(k) 和前馈控制器输出 uf(k) 的加权和，即

u(k)=up(k)+uf(k);

可以通过设置变量 M 来选择是否使用前馈补偿。当 M=1 时，只使用 PID 控制器；当 M=2 时，使用 PID 控制器和前馈补偿。
5. 限幅：为了保证控制信号 u(k) 的安全性，引入了限幅，将 u(k) 限制在 [-10, 10] 的范围内。

if u(k)>=10 
    u(k)=10; 
end 
if u(k)<=-10 
    u(k)=-10; 
end

6. 数据保存和显示：最后，将仿真结果保存并绘制图像，包括期望输出 yd(k)、实际输出 y(k)、误差 error(k)、控制器输出 up(k)、前馈控制器输出 uf(k) 和总输出信号 u(k)。

time(k)=k*ts;
A=0.5;F=3.0;
yd(k)=A*sin(F*2*pi*k*ts);
dyd(k)=A*F*2*pi*cos(F*2*pi*k*ts);
ddyd(k)=-A*F*2*pi*F*2*pi*sin(F*2*pi*k*ts);

y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2; 

figure(1); 
subplot(211); 
plot(time,yd,'r',time,y,'k:','linewidth',2); 
xlabel('time(s)');ylabel('yd,y'); 
legend('Ideal position signal','Position tracking'); 
subplot(212); 
plot(time,error,'r','linewidth',2); 
xlabel('time(s)');ylabel('error'); 

figure(2); 
plot(time,up,'k',time,uf,'b',time,u,'r','linewidth',2); 
xlabel('time(s)');ylabel('up,uf,u');

这里使用了 sin 函数生成了一个正弦波作为期望输出信号 yd(k)，然后通过离散系统模型计算出实际输出 y(k)。最后，使用 subplot 和 plot 函数绘制了两幅图，分别显示了期望输出 yd(k)、实际输出 y(k)、误差 error(k) 和控制器输出 up(k)、前馈控制器输出 uf(k) 和总输出信号 u(k)。