1. 四旋翼飞行器MPC控制概述
四旋翼飞行器作为一种典型的欠驱动系统，通过四个旋翼的转速调节实现六自由度运动控制。这种独特的结构使其在航拍、巡检、救援等领域展现出巨大应用价值，但同时也带来了显著的控制挑战。模型预测控制（MPC）凭借其显式处理约束、滚动优化和多变量协调控制的能力，成为解决四旋翼控制难题的有效方案。
在实际工程应用中，我们常遇到三个核心问题：1）姿态与位置的强耦合导致单环控制效果不佳；2）电机响应延迟和外部扰动影响跟踪精度；3）复杂的非线性特性使传统线性控制方法难以胜任。针对这些问题，MPC通过预测模型、滚动优化和反馈校正的三步策略，实现了优于传统PID的控制性能。
提示：四旋翼的欠驱动特性意味着我们需要用四个控制输入（电机转速）同时控制六个自由度（x,y,z位置和roll,pitch,yaw姿态），这是MPC算法需要解决的核心矛盾。
2. 系统建模与坐标系定义
2.1 机体与惯性坐标系转换
建立准确的数学模型是MPC控制的基础。我们采用右手坐标系定义：
- 惯性坐标系{E}：固定于地面，X_E指向正北，Y_E指向正东，Z_E垂直向下
- 机体坐标系{B}：原点在质心，X_B指向机头，Y_B指向右侧，Z_B垂直向下
两坐标系间的转换通过Z-Y-X欧拉角实现，旋转矩阵为：
matlab复制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复制m*dv/dt = R*[0;0;T] - [0;0;mg] - 0.5*ρ*Cd*A*v|v|
旋转动力学：
matlab复制I*dω/dt = τ - ω×(I*ω) - 0.5*ρ*Cm*A*ω|ω|
电机模型：
matlab复制τ_m = kt*(u - km*ω)/R
其中T为总升力，τ为力矩向量，τ_m为电机转矩。
3. MPC控制器设计
3.1 双环控制架构
采用分层控制策略：
- 外环位置控制器：生成期望姿态和总推力
- 内环姿态控制器：计算电机PWM指令
这种解耦设计有效降低了系统复杂度。在实际调试中，我们发现外环采样周期设为内环的2-3倍能取得最佳效果。
3.2 预测模型离散化
将连续模型离散化为：
matlab复制x(k+1) = A_k*x(k) + B_k*u(k)
y(k) = C_k*x(k)
使用零阶保持法离散化时，需要注意：
- 预测时域N_p选择10-20步
- 控制时域N_c通常取N_p的1/3
- 采样周期T_s建议10-50ms
3.3 目标函数设计
多目标加权函数：
matlab复制J = Σ(α||y-y_ref||² + β||Δu||² + γ||u-u_ref||²)
经验权重设置：
- 轨迹跟踪权重α取1.0
- 控制增量权重β取0.1
- 控制量权重γ取0.01
4. 约束处理技巧
4.1 物理约束建模
电机约束：
matlab复制u_min ≤ u ≤ u_max
Δu_min ≤ Δu ≤ Δu_max
姿态约束：
matlab复制|φ| ≤ 30°, |θ| ≤ 30°
速度约束：
matlab复制|v_xy| ≤ 5m/s, |v_z| ≤ 2m/s
4.2 约束软化技术
对关键约束添加松弛变量：
matlab复制J += ε^T*S*ε
s.t. g(x,u) ≤ ε
其中S为对角权重矩阵，ε≥0。这种方法能有效避免无解情况。
5. 实时优化实现
5.1 QP问题转化
将MPC转化为标准QP形式：
matlab复制min 0.5*z^T*H*z + f^T*z
s.t. A_ineq*z ≤ b_ineq
A_eq*z = b_eq
其中z=[Δu;ε]为优化变量。
5.2 求解器选择
实测性能对比（i7-1185G7@3.0GHz）：
| 求解器 | 平均求解时间(ms) | 最大迭代次数 |
|---|---|---|
| quadprog | 2.1 | 50 |
| qpOASES | 0.8 | 10 |
| OSQP | 1.2 | 100 |
对于实时性要求高的场景，推荐qpOASES。
6. MATLAB实现要点
6.1 仿真框架搭建
典型结构：
matlab复制% 初始化
model = initQuadrotorModel();
mpc = setupMPC(model);
% 主循环
for k = 1:N_steps
[u_opt, x_pred] = solveMPC(mpc, x_meas, ref);
applyControl(u_opt(1));
x_meas = updateState(x_meas, u_opt(1));
logData(k, x_meas, u_opt);
end
6.2 代码优化技巧
- 预计算不变矩阵
- 使用稀疏矩阵存储
- 并行计算雅可比矩阵
- 热启动优化器
7. 典型问题排查
7.1 发散问题处理
现象：状态预测严重偏离实际
解决方法：
- 检查模型参数准确性
- 减小预测时域
- 增加状态权重
7.2 震荡问题处理
现象：控制量高频振荡
解决方法：
- 增大控制增量权重β
- 添加低通滤波
- 检查采样同步性
8. 进阶应用方向
8.1 扰动观测器设计
扩展状态向量包含扰动估计：
matlab复制dx/dt = f(x,u) + d
dd/dt = 0
通过MPC同时估计和补偿扰动。
8.2 学习增强MPC
结合神经网络：
- 用NN学习模型误差
- 在线调整预测模型
- 自适应权重调整
在实际项目中，我们发现将MPC与简单的PID结合使用往往能取得更好的效果——MPC负责全局优化，PID处理局部扰动。这种混合策略在计算资源有限的平台上尤为实用。
最后需要强调的是，任何控制算法的成功应用都离不开仔细的参数调试。建议先从简化的线性模型开始验证算法框架，再逐步引入非线性因素和实际约束，这样可以有效降低调试难度。