1. 四旋翼飞行器MPC控制的核心挑战
四旋翼飞行器的模型预测控制（MPC）本质上是一个多变量、强耦合的非线性优化问题。与传统PID控制不同，MPC需要在每个控制周期求解一个开环优化问题，这对实时性提出了极高要求。我在实际项目中遇到过采样周期为50ms时，算法必须在30ms内完成所有计算的硬性约束。
飞行器的动力学模型通常采用牛顿-欧拉方程描述。以机体坐标系为例，平移运动方程为：
matlab复制% 平移动力学方程
m * ddot_p = [0; 0; -m*g] + R * [0; 0; T]
其中R是旋转矩阵，T为总推力。这个看似简单的方程在实际应用中会遇到三个主要问题：
- 姿态角在±90°时存在奇点（万向节锁问题）
- 电机推力非线性且存在延迟
- 气动效应在高速机动时不可忽略
2. 模型线性化与离散化技巧
2.1 工作点线性化方法
我推荐采用增量式线性化而非绝对线性化。具体步骤：
- 在当前状态x0处泰勒展开
- 保留一阶项得到雅可比矩阵
- 对控制输入u同样处理
matlab复制% 雅可比矩阵计算示例
A = jacobian(f, x);
B = jacobian(f, u);
C = eye(nx);
D = zeros(nx, nu);
sys_cont = ss(A,B,C,D);
2.2 离散化参数选择
采样时间Ts的选择需要权衡：
- 过大会导致预测精度下降
- 过小会增加计算负担
经验公式：
code复制Ts ≈ τ/10
其中τ是系统主导时间常数。对于典型四旋翼，Ts=0.02-0.05s较为合适。
3. 权重矩阵调参实战经验
3.1 对角权重矩阵设计
Q和R矩阵的配置直接影响控制性能。我的调参步骤：
- 先调姿态环再调位置环
- 从单位矩阵开始
- 按数量级调整
matlab复制Q = diag([10,10,10, 1,1,1, 5,5,5, 0.1,0.1,0.1]);
R = diag([0.1,0.1,0.1,0.1]);
3.2 动态权重策略
对于轨迹跟踪，我开发了动态权重方法：
matlab复制function Q = dynamic_Q(t)
if t < 2
Q = diag([1,1,10, 0.5,0.5,0.5, 2,2,2, 0.1,0.1,0.1]);
else
Q = diag([10,10,1, 1,1,1, 5,5,5, 0.1,0.1,0.1]);
end
end
4. 实时性优化技巧
4.1 热启动技术
利用上一时刻的解作为初始猜测，可减少30-50%优化时间：
matlab复制options = optimoptions('quadprog', 'InitialGuess', u_prev);
4.2 代码生成优化
将MATLAB代码转为C代码可提升5-10倍速度：
matlab复制cfg = coder.config('lib');
cfg.DynamicMemoryAllocation = 'off';
codegen('mpc_controller', '-config', cfg, '-args', {x0, ref})
5. 轨迹跟踪中的特殊处理
5.1 参考轨迹插值
对于离散参考点，我采用三次样条插值：
matlab复制ref_traj = spline(ref_time, ref_points, t);
5.2 前馈补偿设计
在MPC成本函数中加入前馈项：
matlab复制J = J + 0.1*norm(u - u_ff)^2;
6. 硬件在环测试要点
6.1 延时补偿方法
实测通信延迟约15-30ms，采用状态预测补偿：
matlab复制x_actual = A*x_measured + B*u_delayed;
6.2 电机混控处理
将虚拟控制量分配到四个电机：
matlab复制M = [1 1 1 1;
1 -1 1 -1;
-1 -1 1 1;
1 -1 -1 1];
u_motors = M \ [T; tau_x; tau_y; tau_z];
7. 常见问题排查指南
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 发散振荡 | Q矩阵太小 | 增大状态权重 |
| 响应迟缓 | R矩阵太大 | 减小控制权重 |
| 偏航漂移 | 未补偿陀螺偏差 | 增加积分项 |
| 计算超时 | 预测时域过长 | 减小Np或使用condensing |
8. 进阶优化方向
- 非线性MPC：采用CasADi框架
- 鲁棒MPC：考虑模型不确定性
- 学习型MPC：结合神经网络
- 分布式MPC：解耦姿态位置环
实际测试数据表明，优化后的MPC控制器在1m直径的圆形轨迹跟踪中，位置误差可控制在±5cm内，姿态误差小于2°，完全满足大多数应用需求。