1. 四旋翼飞行器MPC控制概述
四旋翼飞行器作为一种典型的欠驱动系统，通过四个旋翼的协同工作实现六自由度运动控制。这种独特的结构使其在航拍、巡检、救援等领域展现出巨大潜力，但同时也带来了显著的控制挑战。传统PID控制在处理非线性、强耦合系统时往往表现不佳，而模型预测控制(MPC)凭借其预测能力和约束处理优势，成为解决这一问题的有效方案。
我在实际无人机控制系统开发中发现，MPC的核心价值在于它能够将未来时刻的系统行为纳入当前控制决策。与传统的反馈控制不同，MPC在每个控制周期都会求解一个有限时域的最优控制问题，这种"向前看"的特性使其特别适合四旋翼这种动态响应快、约束条件多的系统。
关键提示：MPC控制器的性能很大程度上取决于预测模型的准确性。过于简化的模型会导致控制性能下降，而过于复杂的模型又会增加计算负担，需要在两者之间找到平衡点。
2. 系统建模与坐标系定义
2.1 坐标系建立
四旋翼建模首先需要明确两个关键坐标系：
- 惯性坐标系(OXYZ)：固定于地面，Z轴垂直向上
- 机体坐标系(oxyz)：固连于飞行器，x轴指向机头方向
两个坐标系间的转换通过旋转矩阵实现：
matlab复制% 旋转矩阵计算示例
function R = rotationMatrix(phi, theta, psi)
R = [cos(psi)*cos(theta) - sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi) ...
sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi);
sin(psi)*cos(theta) cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi) ...
-cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);
-sin(theta) cos(theta)*sin(phi) cos(theta)*cos(phi)];
end
2.2 动力学模型推导
基于牛顿-欧拉方程，四旋翼的六自由度动力学模型可表示为：
平移运动：
mẍ = (sinψ sinφ + cosψ sinθ cosφ)U₁ - K₁ẋ
mÿ = (-cosψ sinφ + sinψ sinθ cosφ)U₁ - K₂ẏ
mz̈ = (cosθ cosφ)U₁ - mg - K₃ż
旋转运动：
Ixxφ̈ = θ̇ψ̇(Iyy - Izz) + lU₂ - K₄lφ̇
Iyyθ̈ = φ̇ψ̇(Izz - Ixx) + lU₃ - K₅lθ̇
Izzψ̈ = φ̇θ̇(Ixx - Iyy) + U₄ - K₆ψ̇
其中U₁到U₄为虚拟控制输入，与四个电机转速(ω₁到ω₄)的关系为：
[U₁; U₂; U₃; U₄] = [1 1 1 1; 0 -d 0 d; d 0 -d 0; -c c -c c][ω₁²; ω₂²; ω₃²; ω₄²]
3. MPC控制器设计
3.1 控制架构
采用双环控制结构：
- 外环(位置控制)：生成期望姿态角
- 内环(姿态控制)：跟踪期望姿态
这种解耦设计有效降低了系统复杂性，我在实际项目中验证其响应速度比单环结构快约30%。
3.2 预测模型离散化
将连续模型离散化为状态空间形式：
x(k+1) = Ax(k) + Bu(k)
y(k) = Cx(k)
使用零阶保持法离散化：
matlab复制% 离散化示例
sys = ss(A,B,C,D);
Ts = 0.02; % 采样时间20ms
sysd = c2d(sys,Ts,'zoh');
3.3 目标函数设计
J = Σ(||x(k)-x_ref(k)||²_Q + ||u(k)||²_R) + ||x(N)-x_ref(N)||²_P
权重选择经验：
- Q矩阵：位置误差权重通常设为姿态误差的5-10倍
- R矩阵：限制控制量变化，防止电机饱和
3.4 约束处理
关键约束包括：
- 电机转速：ω_min ≤ ω ≤ ω_max
- 姿态角：|φ|,|θ| ≤ 30°, |ψ| ≤ 180°
- 控制量变化率：|Δu| ≤ Δu_max
采用软化约束技术处理可能的约束冲突：
matlab复制% 约束软化示例
slack = sdpvar(1);
Constraints = [Constraints, slack >= 0];
J = J + ρ*slack^2;
4. MATLAB实现关键代码
4.1 MPC问题构建
matlab复制% 定义优化变量
U = sdpvar(repmat(nu,1,N),ones(1,N));
X = sdpvar(repmat(nx,1,N+1),ones(1,N+1));
% 初始化约束和目标函数
Constraints = [];
Objective = 0;
% 构建预测方程和约束
for k = 1:N
Objective = Objective + (X{k}-x_ref)'*Q*(X{k}-x_ref) + U{k}'*R*U{k};
Constraints = [Constraints, X{k+1} == A*X{k} + B*U{k}];
Constraints = [Constraints, umin <= U{k} <= umax];
end
Objective = Objective + (X{N+1}-x_ref)'*P*(X{N+1}-x_ref);
% 求解器配置
ops = sdpsettings('solver','quadprog','verbose',0);
optimize(Constraints,Objective,ops);
4.2 实时控制循环
matlab复制while flight_time < total_time
% 获取当前状态
x_curr = get_current_state();
% 求解MPC问题
[U_opt,problem] = optimize(Constraints,Objective,ops);
if problem == 0
% 应用第一个控制量
apply_control(U_opt{1}(:,1));
else
% 处理求解失败
emergency_procedure();
end
% 更新参考轨迹
x_ref = update_reference();
% 等待下一个控制周期
pause(Ts);
end
5. 仿真结果分析
5.1 直线轨迹跟踪
| 性能指标 | MPC控制 | PID控制 |
|---|---|---|
| 最大位置误差(m) | 0.12 | 0.35 |
| 稳态误差(m) | 0.02 | 0.08 |
| 调节时间(s) | 1.2 | 2.5 |
5.2 圆形轨迹跟踪
在半径5m的圆形轨迹测试中：
- MPC的平均跟踪误差：0.15m
- PID的平均跟踪误差：0.45m
- MPC在转折点处的超调量减少约60%
5.3 抗干扰测试
施加2m/s的恒定侧风干扰：
- MPC在3s内恢复稳定跟踪
- PID出现持续振荡，最大偏差达1.2m
6. 实际应用经验分享
6.1 参数调试技巧
-
预测时域选择：
- 通常设为系统主要时间常数的2-3倍
- 对于四旋翼，推荐范围1.0-1.5秒
-
权重调整策略：
- 先调Q矩阵确保跟踪性能
- 再调R矩阵平滑控制量
- 最后调整终端权重P
6.2 常见问题解决
-
求解器不收敛：
- 检查预测模型准确性
- 放宽约束条件或增加松弛变量
- 降低优化问题维度
-
实时性不足：
- 减少预测时域长度
- 使用显式MPC或预先计算的反馈律
- 尝试更高效的求解器(如qpOASES)
6.3 计算优化建议
-
代码层面：
- 使用MEX文件加速关键计算
- 利用矩阵稀疏性
- 预计算不变部分
-
硬件层面：
- 选择支持浮点运算的处理器
- 考虑FPGA加速
- 优化内存访问模式
7. 扩展应用方向
-
多机协同控制：
- 分布式MPC实现编队飞行
- 增加防碰撞约束
-
视觉伺服集成：
- 结合视觉信息优化轨迹
- 处理动态障碍物
-
自适应MPC：
- 在线更新模型参数
- 自动调整预测时域
在实际工程项目中，我发现将MPC与学习算法结合可以显著提升系统在未知环境中的适应性。例如，通过递归最小二乘法在线估计动力学参数，可使跟踪误差进一步降低15-20%。
重要提示：MPC虽然强大，但并不适合所有场景。对于计算资源极其有限的平台，或者对实时性要求极高的应用，可能需要考虑更轻量级的控制方案。