function uk = MPC_NonCascade(r_pos, p, omega, iq)
% 非级联MPC控制器：直接控制"位置-速度-d/q轴电流"全状态，输出d/q轴电压指令
% 输入参数：
% r_pos : 位置参考值 (rad)
% p : 当前位置 (rad)
% omega : 当前速度 (rad/s)
% iq : 当前q轴电流 (A)
% 输出参数：
% uk : 最优电压指令 [ud; uq] (V)，2×1列向量
%% ========================= 1. 系统核心参数（根据电机实际调整）=========================
% 采样与MPC参数
T_s = 0.001; % 采样时间 (s)，非级联需小步长保证稳定性
Np = 15; % 预测时域步数
Nc = 8; % 控制时域步数（≤Np）
% 电机机械参数
J = 0.020; % 转动惯量 (kg·m²)
B = 0.008; % 粘性阻尼系数 (N·m/(rad/s))
Pn = 4; % 极对数
flux = 0.186; % 永磁磁链 (Wb)
% 电机电气参数
R_s = 0.4; % 定子电阻 (Ω)，避免与权重R冲突
Ld = 6e-3; % d轴电感 (H)
Lq = 6e-3; % q轴电感 (H)（隐极电机假设Ld=Lq）
% 约束参数
u_max = 310; % 最大电压 (V)（对应380V交流线电压）
i_max = 40; % 最大电流 (A)
% 权重矩阵（核心：平衡跟踪精度与控制平滑性）
Q_pos = 500; % 位置误差权重
R_ud = 0.1; % d轴电压权重
R_uq = 0.1; % q轴电压权重
%% ========================= 2. 定义连续状态空间模型 =========================
% 状态向量 x = [p; omega; id; iq] （4×1列向量）
% p:位置, omega:速度, id:d轴电流, iq:q轴电流
% 输入向量 u = [ud; uq] （2×1列向量）：d/q轴电压
% 状态方程：dx/dt = A_cont * x + B_cont * u
% 连续状态矩阵 A_cont（4×4）
A_cont = [
0, 1, 0, 0; % dp/dt = omega
0, -B/J, 1.5*Pn*(Ld-Lq)*iq/J, 1.5*Pn*flux/J; % d(omega)/dt = (电磁转矩-阻尼转矩)/J
0, 0, -R_s/Ld, omega*Lq/Ld; % did/dt = (ud - R_s*id + omega*Lq*iq)/Ld
0, 0, -omega*Ld/Lq, -R_s/Lq; % diq/dt = (uq - R_s*iq - omega*Ld*id - omega*flux)/Lq
];
% 连续输入矩阵 B_cont（4×2）
B_cont = [
0, 0; % 位置不受电压直接控制
0, 0; % 速度不受电压直接控制
1/Ld, 0; % d轴电压对id的影响
0, 1/Lq; % q轴电压对iq的影响
];
% 输出矩阵 C_cont（1×4）：仅关注位置输出（可扩展为[位置;速度;电流]）
C_cont = [1, 0, 0, 0];
%% ========================= 3. 连续模型离散化（矩阵指数法）=========================
% 构造增广矩阵用于离散化：[A_cont B_cont; 0 0]（6×6）
aug_cont = [A_cont, B_cont; zeros(size(B_cont,2), size(A_cont,1)+size(B_cont,2))];
% 矩阵指数离散化（精确离散）
aug_disc = expm(aug_cont * T_s);
% 提取离散状态矩阵Ad和输入矩阵Bd
Ad = aug_disc(1:4, 1:4); % 离散状态矩阵（4×4）
Bd = aug_disc(1:4, 5:6); % 离散输入矩阵（4×2）
%% ========================= 4. 构建增广状态空间（含前一时刻控制量）=========================
% 增广目的：将控制量的变化率纳入优化，避免控制量跳变
% 增广状态向量 x_aug = [x; u_prev] （6×1列向量）：x为原始状态，u_prev为前一时刻电压
% 增广状态方程：x_aug(k+1) = A_aug * x_aug(k) + B_aug * Δu(k)
% Δu(k)：控制增量（当前与前一时刻的电压差）
% 增广状态矩阵 A_aug（6×6）
A_aug = [
Ad, Bd; % 原始状态更新：x(k+1) = Ad*x(k) + Bd*u_prev(k)
zeros(2,4), eye(2); % 控制量更新：u(k) = u_prev(k) + Δu(k) → 此处u_prev(k+1)=u(k)
];
% 增广输入矩阵 B_aug（6×2）：仅控制增量影响状态
B_aug = [
Bd; % 原始状态受控制增量影响
eye(2); % 控制量受控制增量直接影响（u(k) = u_prev(k) + Δu(k)）
];
% 增广输出矩阵 C_aug（1×6）：输出仅与原始状态中的位置相关
C_aug = [C_cont, zeros(1,2)];
%% ========================= 5. 构建MPC预测矩阵（phi/F）=========================
% 预测输出 Y = phi * x_aug(k) + F * ΔU
% Y：未来Np步的位置预测（15×1）
% phi：状态预测矩阵（15×6）
% F：控制预测矩阵（15×16）→ Nc步控制增量，每步2维（Δud,Δuq）
% ΔU：未来Nc步控制增量序列（16×1）
% 定义维度参数
ny = size(C_aug,1); % 输出维度：1（仅位置）
nu = size(Bd,2); % 输入维度：2（ud,uq）
nx_aug = size(A_aug,1); % 增广状态维度：6
% 初始化预测矩阵
phi = zeros(Np*ny, nx_aug); % 15×6
F = zeros(Np*ny, Nc*nu); % 15×16
% 填充预测矩阵
for i = 1:Np % 遍历预测时域的每一步
% 状态预测矩阵：phi(i) = C_aug * A_aug^i
phi((i-1)*ny + 1:i*ny, :) = C_aug * (A_aug^i);
% 控制预测矩阵：F(i,j) = C_aug * A_aug^(i-j) * B_aug（j≤i，控制时域内）
for j = 1:min(i, Nc)
col_start = (j-1)*nu + 1;
col_end = j*nu;
F((i-1)*ny + 1:i*ny, col_start:col_end) = C_aug * (A_aug^(i-j)) * B_aug;
end
end
%% ========================= 6. 构建权重矩阵与参考轨迹 =========================
% 1. 输出权重矩阵 Q_bar（15×15）：对角矩阵，每步位置误差权重为Q_pos
Q_bar = kron(eye(Np), Q_pos);
% 2. 控制权重矩阵 R_bar（16×16）：每步控制增量权重为[R_ud,0;0,R_uq]
R_step = [R_ud, 0; 0, R_uq]; % 单步控制权重（2×2）
R_bar = kron(eye(Nc), R_step); % 扩展到Nc步（16×16）
% 3. 参考轨迹 r_seq（15×1）：未来Np步均跟踪位置参考值r_pos
r_seq = repmat(r_pos, Np, 1);
%% ========================= 7. 构造二次规划问题 =========================
% MPC目标函数：min_ΔU [0.5*ΔU'*H*ΔU + ΔU'*E]
% H = F'*Q_bar*F + R_bar（16×16）：二次项矩阵
% E = F'*Q_bar*(phi*x_aug - r_seq)（16×1）：线性项向量
% 初始化前一时刻控制量（首次运行设为0，实际应从历史值获取）
persistent u_prev; % 用persistent保留前一时刻控制量
if isempty(u_prev)
u_prev = [0; 0]; % 初始电压为0
end
% 构建当前增广状态向量 x_aug_current（6×1）
x_current = [p; omega; 0; iq]; % id参考为0，故当前id按0计
x_aug_current = [x_current; u_prev];
% 计算二次规划矩阵
H = F' * Q_bar * F + R_bar;
E = F' * Q_bar * (phi * x_aug_current - r_seq);
%% ========================= 8. 求解二次规划（带约束）=========================
% 计算最优控制序列
options = optimoptions('quadprog','Algorithm','active-set');
x0 = zeros(size(H,2),1);
U_k = zeros(Nc,1);
U_opt1 = quadprog(H,E,[],[],[],[],[],[],x0,options);
%% ========================= 9. 计算当前控制量并限幅 =========================
% 提取第一个控制增量（当前时刻的Δud,Δuq）
u_current1 = U_opt1(1:nu); % 2×1
% 计算当前电压指令：u_current = u_prev + Δu_current
u_current = u_prev + u_current1;
% 电压限幅（确保不超过最大电压）
u_current = max(min(u_current, u_max), -u_max);
% 电流限幅（间接约束：通过电压限幅实现，可扩展为显式电流约束）
% 此处省略显式电流约束，如需添加需在二次规划中增加不等式约束
%% ========================= 10. 更新状态与输出 =========================
u_prev = u_current; % 保存当前控制量为下一时刻的"前一时刻控制量"
uk = u_current; % 输出当前最优电压指令
% （可选）预测位置输出（用于调试）
% p_pred = phi(1:ny, :) * x_aug_current + F(1:ny, 1:nu) * Δu_current;
end输出为一个电压值修改代码
Flux是Yootheme为Joomla 1.5.x开发的新模板，采用Warp5框架，适用于游戏网站等。具备闪亮的菜单栏效果，并提供多种模块变化。
扫一扫
3355
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
