弹簧质量阻尼系统的MPC控制算法仿真m文件源码

clear;clc; delta_t = 0.01; %控制周期10ms sim_time = 5; %仿真时间5s %% 模型参数 m = 1; %滑块质量,增加了5%作为建模误差 k = 10; b = 10; pos = 0; vel = 0; A_ = [1 delta_t;-k*delta_t/m 1-b*delta_t/m]; %离散化预测模型参数A B_ = [0;delta_t/m]; %离散化预测模型参数B %% 控制参数 p = 60; %控制时域（预测时域） Q = 10*eye(2*p); %累计误差权重 W = 0.0001*eye(p); %控制输出权重 umax = 100; %控制量限制，即最大的力 lb=-umax*ones(p,1); %控制量下限 ub=umax*ones(p,1); %控制量上限 Rk = zeros(2*p,1); %参考值序列 %% 中间变量 xk = [pos;vel]; psi = zeros(2*p,2); %psi theta = zeros(2*p,p); %% 显示结果 t = 0:delta_t:sim_time; %时间序列 pos_ref = ones(length(t),1); %参考值序列 pos_out = zeros(length(t),1); %实际值序列 u_out = zeros(length(t),1); %控制量序列 tic %% 主循环 for k=1:length(t) Rk(1:2:end) = pos_ref(k); Rk(2:2:end) = vel; for i=1:1:p psi(i*2-1:i*2,1:2)=A_^i; end for i=1:1:p for j=1:1:i theta(i*2-1:i*2,j)=A_^(i-j)*B_; end end E = psi*xk-Rk; %E H = 2*(theta'*Q*theta+W); %H f = (2*E'*Q*theta)'; %f coder.extrinsic('quadprog'); Uk=quadprog(H,f,[],[],[],[],lb,ub); u=Uk(1); xk = A_*xk+B_*u; pos=xk(1); vel=xk(2); pos_out(k)=pos; u_out(k)=u; end toc figure(1);plot(t,pos_ref,'r--', t,pos_out); xlabel('Sampling Instant'); legend('Output'); figure(2);plot(t,u_out,'r'); xlabel('Sampling Instant'); legend('Control');