模型预测控制滚动优化例子

function [sys,x0,str,ts] = Drcan1(t,x,u,flag) % Input: % t是采样时间, x是状态变量, u是输入(是做成simulink模块的输入,即CarSim的输出), % flag是仿真过程中的状态标志(以它来判断当前是初始化还是运行等) % Output: % sys输出根据flag的不同而不同(下面将结合flag来讲sys的含义), % x0是状态变量的初始值, % str是保留参数,设置为空 % ts是一个1×2的向量, ts(1)是采样周期, ts(2)是偏移量 switch flag case 0 [sys,x0,str,ts] = mdlInitializeSizes; % Initialization case 2 sys = mdlUpdates(t,x,u); % Update discrete states（更新离散状态） case 3 sys = mdlOutputs(t,x,u); % Calculate outputs（计算输出） % case 4 % sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time case {1,4,9} % Unused flags sys = []; otherwise error(['unhandled flag = ',num2str(flag)]); % Error handling end % End of dsfunc. %============================================================== % Initialization %============================================================== function [sys,x0,str,ts] = mdlInitializeSizes sizes = simsizes; %用于设置模块参数的结构体用simsizes来生成 sizes.NumContStates = 0; %模块连续状态变量的个数 sizes.NumDiscStates = 2; %模块离散状态变量的个数,实际上没有用到这个数值，只是用这个来表示离散模块 sizes.NumOutputs = 2; %S函数的输出，包括控制量和其它监测量 sizes.NumInputs = 2; %S函数模块输入变量的个数，即CarSim的输出量 sizes.DirFeedthrough = 1; % Matrix D is non-empty.模块是否存在直接贯通 sizes.NumSampleTimes = 1; %模块的采样次数，>=1 sys = simsizes(sizes); %设置完后赋给sys输出 x0 =[0.001;0.0001]; global U; U=0;%控制量初始化,这里面加了一个期望轨迹的输出，如果去掉，U为一维的 % global x; % x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1); % Initialize the discrete states. str = []; % Set str to an empty matrix. ts = [0.01 0]; % sample time: [period, offset] %End of mdlInitializeSizes %============================================================== % Update the discrete states %============================================================== function sys = mdlUpdates(t,x,u) sys = x; %End of mdlUpdate. %============================================================== % Calculate outputs %============================================================== function sys = mdlOutputs(t,~,u) %global a b; %global u_piao; global U; %global kesi; t_Start = tic; % 开始计算耗时 Nx=2;%状态量的个数 Nu=1;%控制量的个数 Ny=1;%输出量的个数 % Np =25;%预测步长 % Nc=5;%控制步长 Np =20;%预测步长 Nc=2;%控制步长 fprintf('Update start, t=%6.3f\n',t) %输入接口转换,x_dot后面加一个非常小的数，是防止出现分母为零的情况 % y_dot=u(1)/3.6-0.000000001*0.4786;%CarSim输出的是km/h，转换为m/s X1=u(1); X1_dot=u(2);%CarSim输出的是km/h，转换为m/s kesi=[X1;X1_dot]; A=[1 0.1; 0 2]; B=[0;0.5]; C=[1,0]; PHI_cell=cell(Np,1); %PHI=[CA CA^2 CA^3 ... CA^Np]' THETA_cell=cell(Np,Nc); %THETA for j=1:1:Np PHI_cell{j,1}=C*A^j; % demision:Ny* Nx for k=1:1:Nc if k<=j THETA_cell{j,k}=C*A^(j-k)*B; % demision:Ny*Nu else THETA_cell{j,k}=zeros(Ny,Nu); end end end PHI=cell2mat(PHI_cell); % size(PHI)=[(Ny*Np) * Nx] THETA=cell2mat(THETA_cell);% size(THETA)=[Ny*Np Nu*Nc] Q=50; R=10; Qq = kron(eye(Np),Q); % Q = [Np*Nx] * [Np*Nx] Rr = kron(eye(Nc),R); % R = [Nc*Nu] * [Nc*Nu] for i=1:1:Np xxxxxx(i,1)=1; end Yref = xxxxxx*1; E = PHI * kesi; %[(Nx*Np) * 1] H = THETA'*Qq*THETA + Rr; error=E' - Yref'; % Y=@(a,b)E+THETA*[a,b]'; f = error*Qq*THETA; % g = f'; % g = error*Qq*THETA; % 开始求解过程 % [X,fval,exitflag]= quadprog(H,f); %X为目标函数的最小值 %fval为目标函数的最优值； %exitflag为终止迭代的条件：若exitflag>0，表示函数收敛于解x； %exitflag=0,表示超过函数估值或迭代的最大次数； %exitflag<0表示函数不收敛于解x； %[X,fval,exitflag]= (H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);%H是目标函数 fprintf('exitflag=%d\n',exitflag); fprintf('H=%4.2f\n',H(1,1)); fprintf('f=%4.2f\n',f(1,1)); t_Elapsed = toc( t_Start ); %computation time 计算控制周期耗时 % 计算输出 %U(1)=ym(1);%得到控制增量 U=X; sys(1)= U(1); sys(2)= t_Elapsed; % %事件触发 % e = 0.1; % Y=X1; % for i=1:3001 % Yre(i,1)=1; % P=(Y(i)-Yre(i))^2; % if P>=e % % else % U=X; % sys(1)= U(j); % sys(2)= t_Elapsed; % end % end % end