一、系统概述
模型预测控制（MPC） 是一种先进控制策略，通过在线求解有限时域优化问题来生成控制输入。对于无人机系统，MPC能够有效处理：
-
多变量耦合：位置、姿态、速度的强耦合
-
状态/输入约束：电机转速限制、姿态角限制
-
轨迹跟踪：精确跟踪期望轨迹
-
抗干扰能力：应对风扰等外部干扰
离散系统优势：无人机控制器通常基于微处理器实现，离散MPC更适合数字实现，计算效率高。
二、无人机离散系统建模
2.1 四旋翼无人机连续时间模型
状态变量（12维）：
x=x,y,z,x˙,y˙,z˙,ϕ,θ,ψ,p,q,rTx=x,y,z,\\dot{x},\\dot{y},\\dot{z},ϕ,θ,ψ,p,q,r^Tx=x,y,z,x˙,y˙,z˙,ϕ,θ,ψ,p,q,rT
-
位置 (x,y,zx,y,zx,y,z)，速度 (x˙,y˙,z˙\dot{x},\dot{y},\dot{z}x˙,y˙,z˙)
-
欧拉角 (ϕ,θ,ψϕ,θ,ψϕ,θ,ψ)：滚转、俯仰、偏航
-
角速度 (p,q,rp,q,rp,q,r)：机体坐标系下的角速度
控制输入（4维）：
u=ω12,ω22,ω32,ω42Tu=ω_1\^2,ω_2\^2,ω_3\^2,ω_4\^2Tu=ω12,ω22,ω32,ω42T
- 四个电机的转速平方（与升力成正比）
非线性动力学方程：
matlab
% 四旋翼非线性模型（连续时间）
function dx = quadrotor_nonlinear(t, x, u, params)
% 参数
m = params.m; % 质量 (kg)
g = params.g; % 重力加速度 (m/s^2)
Ixx = params.Ixx; % 转动惯量
Iyy = params.Iyy;
Izz = params.Izz;
kf = params.kf; % 升力系数
km = params.km; % 力矩系数
L = params.L; % 臂长 (m)
% 状态提取
pos = x(1:3); % 位置
vel = x(4:6); % 速度
euler = x(7:9); % 欧拉角
omega = x(10:12); % 角速度
% 控制输入（电机转速平方）
w_sq = u;
% 总升力
F_total = kf * sum(w_sq);
% 机体坐标系下的力矩
tau = [
L*kf*(w_sq(2) - w_sq(4)); % 滚转力矩
L*kf*(w_sq(3) - w_sq(1)); % 俯仰力矩
km*(w_sq(1) - w_sq(2) + w_sq(3) - w_sq(4)) % 偏航力矩
];
% 旋转矩阵（从机体到惯性系）
phi = euler(1); theta = euler(2); psi = euler(3);
R = [
cos(theta)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
cos(theta)*sin(psi), sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
-sin(theta), sin(phi)*cos(theta), cos(phi)*cos(theta)
];
% 动力学方程
dpos = vel;
dvel = [0; 0; -g] + (1/m) * R * [0; 0; F_total];
% 欧拉角微分
T = [
1, sin(phi)*tan(theta), cos(phi)*tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)
];
deuler = T * omega;
% 角速度微分
domega = [
(tau(1) + (Iyy - Izz)*omega(2)*omega(3))/Ixx;
(tau(2) + (Izz - Ixx)*omega(3)*omega(1))/Iyy;
(tau(3) + (Ixx - Iyy)*omega(1)*omega(2))/Izz
];
dx = [dpos; dvel; deuler; domega];
end2.2 线性化与离散化
线性化点选择：悬停状态
线性化方法：
matlab
% 线性化无人机模型
function [A, B] = linearize_quadrotor(x0, u0, params, dt)
% 使用数值微分计算雅可比矩阵
n = length(x0); % 状态维度
m = length(u0); % 输入维度
% 计算f(x0, u0)
f0 = quadrotor_nonlinear(0, x0, u0, params);
% 计算A矩阵（对状态求导）
A = zeros(n, n);
epsilon = 1e-6;
for i = 1:n
x_pert = x0;
x_pert(i) = x_pert(i) + epsilon;
f_pert = quadrotor_nonlinear(0, x_pert, u0, params);
A(:, i) = (f_pert - f0) / epsilon;
end
% 计算B矩阵（对输入求导）
B = zeros(n, m);
for i = 1:m
u_pert = u0;
u_pert(i) = u_pert(i) + epsilon;
f_pert = quadrotor_nonlinear(0, x0, u_pert, params);
B(:, i) = (f_pert - f0) / epsilon;
end
% 离散化（零阶保持）
sysc = ss(A, B, eye(n), zeros(n, m));
sysd = c2d(sysc, dt, 'zoh');
A = sysd.A;
B = sysd.B;
end三、模型预测控制器设计
3.1 MPC问题 formulation
优化问题：
约束：
3.2 MPC求解器实现
matlab
classdef QuadrotorMPC < handle
% 无人机MPC控制器类
properties
% 系统参数
A; B; C; % 离散状态空间矩阵
nx; nu; ny; % 状态、输入、输出维度
% MPC参数
Np; % 预测时域
Nc; % 控制时域
Q; R; S; % 权重矩阵
% 约束
x_min; x_max; % 状态约束
u_min; u_max; % 输入约束
du_min; du_max; % 输入变化率约束
% 优化问题参数
H; f; % 二次规划参数
Aineq; bineq; % 不等式约束
Aeq; beq; % 等式约束
lb; ub; % 边界约束
end
methods
function obj = QuadrotorMPC(A, B, C, Np, Nc, Q, R, S)
% 构造函数
obj.A = A; obj.B = B; obj.C = C;
obj.nx = size(A, 1);
obj.nu = size(B, 2);
obj.ny = size(C, 1);
obj.Np = Np;
obj.Nc = Nc;
obj.Q = Q;
obj.R = R;
obj.S = S;
% 初始化约束（默认无约束）
obj.x_min = -inf(obj.nx, 1);
obj.x_max = inf(obj.nx, 1);
obj.u_min = zeros(obj.nu, 1);
obj.u_max = 1000 * ones(obj.nu, 1); % 电机最大转速平方
obj.du_min = -100 * ones(obj.nu, 1);
obj.du_max = 100 * ones(obj.nu, 1);
% 构建优化问题
obj.build_qp_matrices();
end
function build_qp_matrices(obj)
% 构建二次规划矩阵
% 扩展系统矩阵
Psi = zeros(obj.ny*obj.Np, obj.nx);
Gamma = zeros(obj.ny*obj.Np, obj.nu*obj.Nc);
Theta = zeros(obj.ny*obj.Np, obj.nu*obj.Nc);
% 构建Psi矩阵（状态预测）
for i = 1:obj.Np
Psi((i-1)*obj.ny+1:i*obj.ny, :) = obj.C * (obj.A^i);
end
% 构建Gamma和Theta矩阵（输入预测）
for i = 1:obj.Np
for j = 1:min(i, obj.Nc)
Gamma((i-1)*obj.ny+1:i*obj.ny, (j-1)*obj.nu+1:j*obj.nu) = ...
obj.C * (obj.A^(i-j)) * obj.B;
end
end
% 构建Theta矩阵（输入变化率）
Theta(1:obj.ny*obj.Nc, 1:obj.nu*obj.Nc) = kron(tril(ones(obj.Nc)), eye(obj.nu));
% 构建Hessian矩阵
obj.H = 2 * (Gamma' * kron(eye(obj.Np), obj.Q) * Gamma + ...
Theta' * kron(eye(obj.Nc), obj.R) * Theta + ...
kron(eye(obj.Nc), obj.S));
% 构建梯度向量（将在每次迭代中更新）
obj.f = [];
% 构建约束矩阵
obj.build_constraint_matrices();
end
function build_constraint_matrices(obj)
% 构建约束矩阵
% 输入约束
A_u = [kron(eye(obj.Nc), eye(obj.nu));
-kron(eye(obj.Nc), eye(obj.nu))];
b_u = [repmat(obj.u_max, obj.Nc, 1);
-repmat(obj.u_min, obj.Nc, 1)];
% 输入变化率约束
D = kron(diag(ones(obj.Nc,1)) + diag(-ones(obj.Nc-1,1), -1), eye(obj.nu));
A_du = [D; -D];
b_du = [repmat(obj.du_max, obj.Nc, 1);
-repmat(obj.du_min, obj.Nc, 1)];
% 状态约束（简化处理）
% 注意：完整的状态约束需要更复杂的处理
% 合并约束
obj.Aineq = [A_u; A_du];
obj.bineq = [b_u; b_du];
% 等式约束（无）
obj.Aeq = [];
obj.beq = [];
% 边界约束
obj.lb = repmat(obj.u_min, obj.Nc, 1);
obj.ub = repmat(obj.u_max, obj.Nc, 1);
end
function [u_opt, info] = solve_mpc(obj, x0, x_ref, u_prev)
% 求解MPC问题
% 输入：
% x0: 当前状态
% x_ref: 参考状态
% u_prev: 上一时刻控制输入
% 输出：
% u_opt: 最优控制序列（仅取第一个）
% info: 求解信息
% 构建梯度向量
Psi_x0 = zeros(obj.ny*obj.Np, 1);
for i = 1:obj.Np
Psi_x0((i-1)*obj.ny+1:i*obj.ny) = obj.C * (obj.A^i) * x0;
end
x_ref_vec = repmat(x_ref(1:obj.ny), obj.Np, 1);
obj.f = 2 * (Psi_x0 - x_ref_vec)' * kron(eye(obj.Np), obj.Q);
% 构建输入变化率约束的偏移量
u_prev_vec = [u_prev; zeros((obj.Nc-1)*obj.nu, 1)];
D = kron(diag(ones(obj.Nc,1)) + diag(-ones(obj.Nc-1,1), -1), eye(obj.nu));
b_du_offset = D(1:obj.nu, :) * u_prev_vec;
% 更新约束
b_du = obj.bineq(end-obj.Nc*obj.nu+1:end);
b_du(1:obj.nu) = b_du(1:obj.nu) - b_du_offset;
b_du(obj.nu+1:2*obj.nu) = b_du(obj.nu+1:2*obj.nu) + b_du_offset;
obj.bineq(end-obj.Nc*obj.nu+1:end) = b_du;
% 求解二次规划
options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex');
[U_opt, ~, exitflag] = quadprog(obj.H, obj.f', obj.Aineq, obj.bineq, ...
obj.Aeq, obj.beq, obj.lb, obj.ub, [], options);
if exitflag > 0
u_opt = U_opt(1:obj.nu);
info.exitflag = exitflag;
info.optimal = true;
else
warning('MPC求解失败，使用备用控制律');
u_opt = u_prev; % 保持上一时刻控制
info.exitflag = exitflag;
info.optimal = false;
end
end
end
end四、完整仿真示例
matlab
%% 无人机MPC控制仿真主程序
clear; close all; clc;
%% 1. 无人机参数设置
params.m = 1.2; % 质量 (kg)
params.g = 9.81; % 重力加速度 (m/s^2)
params.Ixx = 0.023; % 转动惯量 (kg·m^2)
params.Iyy = 0.023;
params.Izz = 0.046;
params.kf = 8.548e-6; % 升力系数
params.km = 1.6e-2; % 力矩系数
params.L = 0.225; % 臂长 (m)
%% 2. 线性化与离散化
% 悬停状态
x0 = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 高度1m悬停
u0 = params.m * params.g / (4 * params.kf) * ones(4, 1); % 悬停所需升力
% 采样时间
dt = 0.02; % 20ms采样周期
% 线性化并离散化
[A, B] = linearize_quadrotor(x0, u0, params, dt);
C = eye(12); % 输出所有状态
D = zeros(12, 4);
%% 3. MPC控制器设计
% MPC参数
Np = 20; % 预测时域
Nc = 10; % 控制时域
% 权重矩阵
Q = diag([10, 10, 20, ... % 位置权重
1, 1, 2, ... % 速度权重
5, 5, 5, ... % 姿态角权重
0.1, 0.1, 0.1]); % 角速度权重
R = 0.01 * eye(4); % 控制输入权重
S = 0.1 * eye(4); % 控制输入变化率权重
% 创建MPC控制器
mpc = QuadrotorMPC(A, B, C, Np, Nc, Q, R, S);
% 设置约束
mpc.u_min = zeros(4, 1); % 最小转速（平方）
mpc.u_max = 20000 * ones(4, 1); % 最大转速（平方）
mpc.du_min = -1000 * ones(4, 1); % 最小变化率
mpc.du_max = 1000 * ones(4, 1); % 最大变化率
%% 4. 轨迹跟踪仿真
% 仿真参数
T = 10; % 总仿真时间 (s)
N = floor(T / dt); % 仿真步数
% 初始化
x = x0; % 初始状态
u = u0; % 初始控制
x_history = zeros(12, N+1);
u_history = zeros(4, N);
t_history = zeros(1, N+1);
x_history(:, 1) = x;
t_history(1) = 0;
% 参考轨迹（圆形轨迹）
t_ref = linspace(0, T, N+1);
x_ref = zeros(12, N+1);
radius = 2; % 圆形半径
omega = 1; % 角速度
for k = 1:N+1
t = t_ref(k);
x_ref(1, k) = radius * sin(omega * t); % x位置
x_ref(2, k) = radius * (1 - cos(omega * t)); % y位置
x_ref(3, k) = 1; % 高度保持1m
x_ref(9, k) = atan2(cos(omega * t), 1 - sin(omega * t)); % 偏航角跟踪
end
%% 5. 主仿真循环
fprintf('开始MPC仿真...\n');
for k = 1:N
% 当前时间
t = (k-1) * dt;
% 获取当前参考状态
x_ref_k = x_ref(:, min(k, size(x_ref, 2)));
% 求解MPC问题
[u_opt, info] = mpc.solve_mpc(x, x_ref_k, u);
% 应用控制输入
u = u_opt;
% 记录
u_history(:, k) = u;
% 系统仿真（使用非线性模型）
[~, x_sim] = ode45(@(t, x) quadrotor_nonlinear(t, x, u, params), ...
[0, dt], x);
% 更新状态
x = x_sim(end, :)';
% 记录状态
x_history(:, k+1) = x;
t_history(k+1) = t + dt;
% 显示进度
if mod(k, 50) == 0
fprintf('时间: %.2f s, 位置误差: %.3f m\n', ...
t+dt, norm(x(1:3) - x_ref_k(1:3)));
end
end
fprintf('仿真完成！\n');
%% 6. 结果可视化
figure('Position', [100, 100, 1200, 800]);
% 子图1：三维轨迹
subplot(2, 3, 1);
plot3(x_history(1, :), x_history(2, :), x_history(3, :), 'b-', 'LineWidth', 2);
hold on;
plot3(x_ref(1, :), x_ref(2, :), x_ref(3, :), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('三维轨迹跟踪');
legend('实际轨迹', '参考轨迹', 'Location', 'best');
grid on; axis equal;
% 子图2：位置跟踪
subplot(2, 3, 2);
plot(t_history, x_history(1:3, :), 'LineWidth', 1.5);
hold on;
plot(t_ref, x_ref(1:3, :), '--', 'LineWidth', 1);
xlabel('时间 (s)'); ylabel('位置 (m)');
title('位置跟踪');
legend('x', 'y', 'z', 'x_{ref}', 'y_{ref}', 'z_{ref}', 'Location', 'best');
grid on;
% 子图3：姿态角
subplot(2, 3, 3);
plot(t_history, rad2deg(x_history(7:9, :)), 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('角度 (°)');
title('姿态角');
legend('\phi (滚转)', '\theta (俯仰)', '\psi (偏航)', 'Location', 'best');
grid on;
% 子图4：控制输入
subplot(2, 3, 4);
plot(t_history(1:end-1), u_history', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('控制输入 (ω²)');
title('电机转速平方');
legend('电机1', '电机2', '电机3', '电机4', 'Location', 'best');
grid on;
% 子图5：跟踪误差
subplot(2, 3, 5);
pos_error = vecnorm(x_history(1:3, :) - x_ref(:, 1:size(x_history, 2)), 2, 1);
plot(t_history, pos_error, 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('轨迹跟踪误差');
grid on;
% 子图6：相平面图
subplot(2, 3, 6);
plot(x_history(1, :), x_history(2, :), 'b-', 'LineWidth', 2);
hold on;
plot(x_ref(1, :), x_ref(2, :), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)');
title('XY平面轨迹');
legend('实际', '参考', 'Location', 'best');
grid on; axis equal;
%% 7. 性能指标计算
% 均方根误差
rmse_pos = sqrt(mean(pos_error.^2));
fprintf('\n性能指标:\n');
fprintf('位置RMSE: %.4f m\n', rmse_pos);
% 控制能量
control_energy = sum(sum(u_history.^2)) * dt;
fprintf('控制能量: %.4f\n', control_energy);
% 最大跟踪误差
max_error = max(pos_error);
fprintf('最大跟踪误差: %.4f m\n', max_error);
%% 8. 保存结果
save('quadrotor_mpc_results.mat', 'x_history', 'u_history', 't_history', ...
'x_ref', 't_ref', 'params', 'dt', 'Np', 'Nc');参考代码 无人机离散系统模型预测控制 www.youwenfan.com/contentcst/160596.html
五、关键问题与解决方案
5.1 计算复杂度优化
matlab
% 稀疏矩阵优化
function build_qp_matrices_sparse(obj)
% 使用稀疏矩阵减少内存占用
obj.H = sparse(obj.H);
obj.Aineq = sparse(obj.Aineq);
obj.Aeq = sparse(obj.Aeq);
end
% 热启动加速
function [u_opt, info] = solve_mpc_hotstart(obj, x0, x_ref, u_prev, U_guess)
% 使用上一时刻解作为初始猜测
options = optimoptions('quadprog', 'Display', 'off', ...
'Algorithm', 'interior-point-convex', ...
'InitBarrierParam', 0.01, ...
'InitTrustRegionRadius', 100);
[U_opt, ~, exitflag] = quadprog(obj.H, obj.f', obj.Aineq, obj.bineq, ...
obj.Aeq, obj.beq, obj.lb, obj.ub, ...
U_guess, options);
% ... 后续处理
end5.2 实时性保障
matlab
% 简化模型（降阶）
function [A_red, B_red] = reduce_model(A, B, n_red)
% 使用平衡截断降阶
sys = ss(A, B, eye(size(A)), zeros(size(B)), -1); % 离散系统
sys_red = balred(sys, n_red);
A_red = sys_red.A;
B_red = sys_red.B;
end
% 显式MPC（离线计算）
function mpc_explicit = design_explicit_mpc(A, B, Q, R, constraints)
% 使用MPT工具箱生成显式MPC控制器
model = LTISystem('A', A, 'B', B);
model.x.min = constraints.x_min;
model.x.max = constraints.x_max;
model.u.min = constraints.u_min;
model.u.max = constraints.u_max;
model.x.penalty = QuadFunction(Q);
model.u.penalty = QuadFunction(R);
mpc_explicit = MPCController(model, Np);
explicit_ctrl = mpc_explicit.toExplicit();
end六、扩展功能
6.1 抗干扰增强
matlab
% 扰动观测器
function [d_est] = disturbance_observer(x, u, x_prev, dt)
% 简化的扰动观测器
persistent d_hat;
if isempty(d_hat)
d_hat = zeros(size(x));
end
% 观测器增益
L = 0.1 * eye(length(x));
% 预测状态
x_pred = A * x_prev + B * u;
% 更新扰动估计
d_hat = d_hat + L * (x - x_pred);
d_est = d_hat;
end6.2 自适应MPC
matlab
classdef AdaptiveMPC < QuadrotorMPC
% 自适应MPC，在线更新模型
methods
function update_model(obj, x_data, u_data)
% 使用递归最小二乘法在线更新模型
% x_data: 状态数据序列
% u_data: 输入数据序列
% 简化的模型更新（实际应用需更严谨）
theta = (x_data(:, 2:end) - x_data(:, 1:end-1)) / dt;
phi = [x_data(:, 1:end-1); u_data];
% 最小二乘估计
AB_est = theta * pinv(phi);
% 更新模型（保持稳定性）
obj.A = 0.9 * obj.A + 0.1 * AB_est(1:obj.nx, 1:obj.nx);
obj.B = 0.9 * obj.B + 0.1 * AB_est(1:obj.nx, obj.nx+1:end);
% 重新构建QP矩阵
obj.build_qp_matrices();
end
end
end七、总结
本实现提供了完整的无人机离散系统模型预测控制MATLAB解决方案，包含：
-
精确建模：非线性动力学模型 + 线性化离散化
-
MPC核心：基于二次规划的优化求解，支持状态/输入约束
-
完整仿真：圆形轨迹跟踪示例，包含可视化与性能分析
-
工程优化：计算复杂度优化、实时性保障策略
-
扩展功能：抗干扰增强、自适应MPC等高级特性
关键优势：
-
直接处理多变量耦合与约束
-
良好的轨迹跟踪性能
-
适用于实时嵌入式实现
-
可扩展至更复杂场景（避障、编队等）
应用建议：
-
实际部署时考虑计算延迟，适当减小预测时域
-
添加积分项消除稳态误差
-
结合卡尔曼滤波进行状态估计
-
考虑输入饱和与执行器动力学