%% 使用增广模型[X(k+1);u(k)]=[Ad Bd;o I]*[X(K);u(k-1)]+[Bd;I]*Delata_u(k);
%% Yk=[Cd 0]*[X(K);u(k-1)]%%
function [uk,p_pred] = fcn(r,p,omega)
% 定义系统参数
Tms = 0.01; %定采样时间最适合的时间，超调小，响应快
B = 0.008; %电机粘性阻尼，N*m/(rad/s);
J = 0.020; %电机转动惯量，kg*m2;
Pn = 4;
flux =0.186;
iq_max = 40;
Kt = 1; %电机力矩常数，Nm/A_peak;
Np = 20;
Nc = 10;
Q = 4000;
R = 0.001;
% 定义离散状态空间矩阵
Am = [0 1; 0 -B]; %连续状态矩阵
Bm = [0; Kt/J]; %连续输入矩阵
Cm = [1 0];
Dm = [0];
M = expm([Am Bm; zeros(1,3)]*Tms); %使用矩阵指数精确离散化
Ad = M(1:2, 1:2); %定义离散状态矩阵
Bd = M(1:2,3:end); %定义离散输入矩阵
Cd = [1 0];
%定义增广状态矩阵
A = [Ad,Bd;zeros(size(Bd,2),size(Ad,1)),eye(size(Bd,2))];
B = [Bd;eye(size(Bd,2))];
C = [Cd,zeros(1,size(Bd,2))];
% X(k+1)=Az*X(K)+Bz*u(k) X(K)状态向量(n*1),u(k)输入向量(p*1)
% A系统状态矩阵(n+1*n+1), B系统输入矩阵(n+1*p)
% 参考【DR_CAN-MPC学习笔记】3&4.详细的MPC建模例子和matlab代码
% Xk = phi*xk + F*Uk Xk-(N+1)n*1 phi-(N+1)n*n xk-n*1 F-(N+1)n*Np Uk-Np*1
nx = size(A,1); %状态维度
nu = size(B,2); %输入维度
ny = size(C,1); %输出维度
% 构建预测矩阵
phi = zeros(Np*ny,nx); %状态预测矩阵Y=M*x(ki)+C*U(ki)
F = zeros(Np*ny, Nc*nu);
for i = 1:Np
phi((i-1)*ny + 1:i*ny,:) = C*A^i;
for j = 1:min(i,Nc)
F((i-1)*ny+1:i*ny, (j-1)*nu+1:j*nu) = C*A^(i-j)*B;
end
end
% 构建权重矩阵
Q_bar = kron(eye(Np), Q);
R_bar = kron(eye(Nc), R); %Nc*Nc矩阵
xk = [p; omega;0]; %Nx+1*1矩阵
rk = [repmat(r, Np-1, 1); r]; %Np*1矩阵
% 计算G,E,H
% 参考【DR_CAN-MPC学习笔记】3&4.详细的MPC建模例子和matlab代码
E = F'*Q_bar*(phi*xk-rk); %Nc*1矩阵
H = F'*Q_bar*F + R_bar; %Nc*Nc矩阵
% 计算最优控制序列
options = optimoptions('quadprog','Algorithm','active-set');
x0 = zeros(size(H,2),1);
U_k = zeros(Nc,1);
U_k = quadprog(H,E,[],[],[],[],[],[],x0,options);
% 提取第一个控制量
Delta_uk = U_k(1:nu,1);
x_pred = A*xk + B*U_k(1:nu,1);
uk = x_pred(3,:);
uk = max(min(uk,iq_max),-iq_max);
% 预测输出
p_pred = phi(1:ny, :) * xk + F(1:ny, 1:nu) * Delta_uk;
end
模仿这个写一个速度模型预测代码
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
1136
