1. 四旋翼飞行器MPC控制概述
四旋翼飞行器作为典型的欠驱动系统，其轨迹跟踪控制一直是无人机领域的核心难题。传统PID控制在处理非线性、强耦合系统时表现乏力，而模型预测控制（MPC）凭借其预测能力和约束处理优势，成为解决这一问题的有效方案。
在实际工程应用中，我们常遇到三个典型痛点：一是飞行器在轨迹转折点容易出现超调震荡；二是抗风扰能力不足导致轨迹偏差；三是多约束条件下的控制输入优化困难。针对这些问题，MPC通过滚动优化和反馈校正机制，能够显著提升控制性能。
提示：MPC的核心价值在于将控制问题转化为在线优化问题，通过预测模型提前计算最优控制序列。这种"先看几步再走"的思路，特别适合四旋翼这类响应滞后的系统。
2. 系统建模关键要点
2.1 坐标系定义规范
建立准确的系统模型是MPC控制的基础。我们需要明确两个坐标系：
-
惯性坐标系（世界坐标系）OXYZ：
- X轴指向地理北向
- Y轴指向地理东向
- Z轴垂直地面向下
- 用于描述飞行器的绝对位置和姿态
-
机体坐标系oxyz：
- o点位于飞行器质心
- x轴指向机头方向
- y轴指向右侧机臂
- z轴垂直机身向下
- 用于描述飞行器的相对运动
坐标系转换通过旋转矩阵实现：
matlab复制% 旋转矩阵计算（Z-Y-X顺序）
R = [cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ;
cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ;
-sinθ, sinφ*cosθ, cosφ*cosθ];
2.2 动力学模型离散化
连续时间模型需要离散化以适应数字控制：
matlab复制% 状态空间模型离散化示例
Ts = 0.02; % 采样时间20ms
sys_d = c2d(sys_c, Ts, 'zoh');
% 或者手动实现欧拉离散化
x_k+1 = x_k + Ts*f(x_k,u_k)
注意：离散化方法影响模型精度，对于非线性强的系统，建议采用更精确的龙格-库塔法。
3. MPC控制器设计实践
3.1 双环控制架构实现
典型的双环控制结构如下：
code复制[位置指令] → [外环MPC] → [姿态指令] → [内环MPC] → [电机PWM]
↑ ↑ ↑
[位置反馈] [姿态反馈] [转速反馈]
外环MPC核心代码框架：
matlab复制function [att_cmd] = outer_MPC(pos_ref, pos_curr, params)
% 构建优化问题
opti = casadi.Opti();
% 定义决策变量
U = opti.variable(4, N); % 控制序列
% 构建预测模型
X_pred = [pos_curr];
for k = 1:N
X_pred = [X_pred, dyn_model(X_pred(:,k), U(:,k))];
end
% 目标函数
obj = 0;
for k = 1:N
obj = obj + (X_pred(:,k)-pos_ref(:,k))'*Q*(X_pred(:,k)-pos_ref(:,k));
obj = obj + U(:,k)'*R*U(:,k);
end
opti.minimize(obj);
% 求解
sol = opti.solve();
att_cmd = sol.value(U(:,1)); % 仅取第一个控制量
end
3.2 约束处理技巧
实际工程中约束处理直接影响飞行安全：
matlab复制% 电机转速约束
opti.subject_to(1000 <= U <= 2000); % RPM范围
% 姿态角约束
opti.subject_to(-30*pi/180 <= phi <= 30*pi/180);
opti.subject_to(-30*pi/180 <= theta <= 30*pi/180);
% 松弛变量处理（避免无解）
epsilon = opti.variable();
opti.subject_to(epsilon >= 0);
opti.minimize(obj + 1e6*epsilon^2);
4. 仿真验证关键步骤
4.1 典型轨迹生成
验证控制性能需要设计多种测试轨迹：
matlab复制% 直线轨迹
t = 0:0.02:10;
ref_line = [0.2*t; zeros(size(t)); -ones(size(t))];
% 圆形轨迹
ref_circle = [2*cos(0.2*t); 2*sin(0.2*t); -ones(size(t))];
% 阶跃响应测试
ref_step = [zeros(1,100), ones(1,100)*2; zeros(2,200)];
4.2 性能指标计算
定量评估控制效果：
matlab复制% 跟踪误差统计
pos_error = sqrt(sum((pos_act - pos_ref).^2, 1));
mean_error = mean(pos_error);
max_error = max(pos_error);
% 控制能耗统计
control_effort = sum(u.^2, 1);
total_energy = sum(control_effort);
% 稳定时间计算
settling_idx = find(abs(pos_error) < 0.05, 1);
settling_time = t(settling_idx);
5. 工程实现经验分享
5.1 实时性优化技巧
- 代码生成技术：
matlab复制% 使用MATLAB Coder生成C代码
cfg = coder.config('lib');
codegen('outer_MPC', '-config', cfg, '-args', {pos_ref, pos_curr, params});
- 热启动优化：
matlab复制% 重用上一周期的解作为初始猜测
opti.set_initial(U, prev_solution);
- 降阶模型：
matlab复制% 在保证精度的前提下简化模型
red_model = balred(full_model, 6); % 降为6阶
5.2 抗干扰实践方案
- 干扰观测器设计：
matlab复制function [d_est] = disturbance_observer(x, u, Ts)
persistent x_prev d_prev;
% 观测器参数
L = [0.5; 0.5; 0.5];
% 预测状态
x_pred = x_prev + Ts*(f(x_prev,u) + d_prev);
% 更新估计
d_est = d_prev + L*(x - x_pred);
% 存储状态
x_prev = x;
d_prev = d_est;
end
- 鲁棒MPC设计：
matlab复制% 考虑最坏情况干扰
for k = 1:N
X_pred(:,k+1) = f(X_pred(:,k), U(:,k)) + W(:,k);
opti.subject_to(norm(W(:,k)) <= w_max);
end
6. 常见问题排查指南
6.1 求解失败处理
- 不可行问题：
- 检查约束是否冲突
- 引入松弛变量
- 适当放宽约束范围
- 求解器发散：
- 调整预测时域长度
- 检查模型线性化是否正确
- 尝试不同求解器（ipopt、sqp等）
6.2 实际飞行调试
- 参数整定顺序：
code复制1. 内环姿态控制
2. 外环位置控制
3. 干扰补偿参数
4. 约束范围调整
- 现场问题诊断：
matlab复制% 记录关键数据用于分析
data = struct();
data.time = t;
data.pos_ref = pos_ref;
data.pos_act = pos_act;
data.u = u;
save('flight_log.mat', 'data');
在四旋翼MPC控制的实际应用中，我发现模型精度和计算效率的平衡是关键。经过多次飞行测试，采用二阶泰勒展开的近似模型配合5Hz的更新频率，能在大多数场景下取得良好效果。对于需要更高精度的场合，可以尝试基于神经网络的预测模型补偿，但要注意实时性保证。