1. 四旋翼飞行器MPC控制的核心挑战
四旋翼飞行器的模型预测控制（MPC）本质上是一个多变量、强耦合的非线性控制问题。当我在2018年第一次尝试用MPC控制自制无人机时，最头疼的就是如何处理这几个关键矛盾：
-
实时性要求与计算复杂度：MPC需要在每个控制周期求解优化问题，而四旋翼动力学模型通常需要10ms以内的控制频率。用标准QP求解器跑完整优化，在我的i7笔记本上单次迭代就要30ms——完全不可用。
-
模型精度与鲁棒性：采用完整的6自由度非线性模型会导致优化问题非凸，而简化后的线性模型又难以应对大机动飞行。我曾在实验室亲眼见证过基于线性模型的MPC控制器在跟踪8字轨迹时直接失控撞墙。
-
状态约束的处理：电池电压限制导致的推力饱和、电机转速上下限等物理约束，在优化问题中表现为非线性不等式约束，常规QP根本无法处理。
matlab复制% 典型四旋翼推力约束示例
u_min = [0; 0; 0; 0]; % 电机最小转速
u_max = [950; 950; 950; 950]; % 电机最大转速(单位：RPM)
2. 控制模型的选择与简化技巧
2.1 分层控制架构设计
经过多次炸机教训后，我采用了分层控制策略：
- 外环位置控制：使用MPC处理x,y,z三轴位置跟踪
- 内环姿态控制：采用PID或LQR控制滚转/俯仰/偏航角
- 分配层：将虚拟控制量分配到四个电机
这种架构将6自由度问题分解为两个3自由度子系统，计算量直接降低60%。下面是外环MPC的简化模型：
$$
\begin{aligned}
\dot{x} &= v_x \
\dot{v_x} &= g\theta \
\dot{y} &= v_y \
\dot{v_y} &= -g\phi \
\dot{z} &= v_z \
\dot{v_z} &= \frac{T}{m} - g
\end{aligned}
$$
关键技巧：当俯仰/滚转角小于15°时，该简化模型误差<3%，却能将QP问题规模从24维降到12维
2.2 离散化方法的工程取舍
连续模型必须离散化才能用于MPC。对比几种常用方法：
| 方法 | 计算复杂度 | 精度 | 稳定性 |
|---|---|---|---|
| 欧拉法 | O(n) | 低 | 条件稳定 |
| 零阶保持(ZOH) | O(n²) | 中 | 较好 |
| 龙格-库塔4阶 | O(n⁴) | 高 | 优 |
实测发现，对于采样周期≤0.05s的情况，ZOH在精度和计算量间取得最佳平衡。Matlab实现示例：
matlab复制% 使用c2d进行ZOH离散化
Ts = 0.02; % 20ms控制周期
sys_d = c2d(sys_c, Ts, 'zoh');
3. 预测时域与控制时域的黄金比例
预测时域(Np)和控制时域(Nc)的选择直接影响性能：
- 过长的Np：会导致"过度预测"——无人机对突发障碍反应迟钝。在室内测试中，Np>1.5s时飞行器撞墙概率增加40%
- 过短的Np：相当于开环控制，轨迹跟踪会出现明显相位滞后
经过上百次飞行测试，总结出经验公式：
$$
N_p = \frac{2\pi}{\omega_n \cdot T_s}, \quad N_c = \lfloor N_p/3 \rfloor
$$
其中ωn是系统自然频率。对于典型四旋翼(ωn≈4rad/s)，Ts=0.02s时：
matlab复制w_n = 4; % 自然频率(rad/s)
N_p = ceil(2*pi/(w_n*Ts)); % 预测步长=79步(1.58s)
N_c = floor(N_p/3); % 控制步长=26步
4. 代价函数设计的艺术
4.1 权重矩阵的自动调节
传统固定权重方案在高速转弯时表现糟糕。我的解决方案是引入速度自适应权重：
$$
Q = \begin{cases}
diag([1,1,10,1,1,10]) & |v|<2m/s \
diag([5,5,2,5,5,2]) & |v|\geq2m/s
\end{cases}
$$
Matlab实现技巧：
matlab复制function Q = adaptive_Q(v)
if norm(v(1:3)) < 2
Q = diag([1,1,10,1,1,10]);
else
Q = diag([5,5,2,5,5,2]);
end
end
4.2 终端代价的凸优化处理
为避免"预测截断效应"，需要设计终端代价P。通过求解Lyapunov方程可获得最优P：
matlab复制[~,P,~] = dare(A,B,Q,R); % 离散代数Riccati方程
实测表明，这能使跟踪误差在最后10%预测时域内降低35%
5. 约束处理的工程hack
5.1 输入约束的软化技巧
硬约束可能导致QP无解。我的应对方案：
- 对电机转速约束添加松弛变量ε
- 在代价函数中增加ρε²项
- 设置ε的线性惩罚项
matlab复制rho = 1e5; % 松弛变量权重
H = blkdiag(H, rho*eye(4)); % 扩展H矩阵
f = [f; 1e3*ones(4,1)]; % 扩展f向量
5.2 状态约束的近似处理
对于禁飞区约束，可采用对数障碍函数近似：
$$
c(x) = -\log(h_{safe} - |p - p_{obs}|)
$$
Matlab快速实现：
matlab复制function cost = barrier_cost(p, p_obs, h_safe)
d = norm(p - p_obs);
if d >= h_safe
cost = 0;
else
cost = -log(h_safe - d);
end
end
6. QP求解的实时优化
6.1 热启动技巧
利用上一时刻的解warm-start当前优化：
matlab复制[U_opt,~,exitflag] = quadprog(H,f,A_ineq,b_ineq,[],[],[],[],U_prev);
if exitflag <= 0
% 使用备用PID控制器
U_opt = pid_control(x_ref, x);
end
6.2 代码生成加速
使用Matlab Coder生成mex文件可提升5-8倍速度：
matlab复制cfg = coder.config('lib');
cfg.DynamicMemoryAllocation = 'off'; % 禁用动态内存
codegen('mpc_quadrotor', '-config', cfg);
7. 实测效果与参数整定
在3DR Solo无人机平台上的测试数据：
| 指标 | PID | MPC(本方案) |
|---|---|---|
| 最大跟踪误差 | 0.82m | 0.15m |
| 能量消耗 | 100% | 83% |
| 抗风性(5m/s) | 失控 | 0.3m偏移 |
关键调试步骤：
- 先在地面站用0.5倍速运行
- 检查QP求解成功率(应>98%)
- 逐步提高速度直至出现震荡
- 调整Q矩阵中对应该状态的权重
8. 典型故障排查指南
8.1 QP频繁无解
可能原因：
- 预测时域过短
- 约束过于严格
- 模型失配
解决方案：
matlab复制options = optimoptions('quadprog',...
'ConstraintTolerance',1e-3,...
'OptimalityTolerance',1e-4);
8.2 高频震荡
处理流程：
- 检查离散化方法是否合适
- 降低预测时域Np
- 增加输入变化率权重ΔuᵀRΔu
9. 进阶优化方向
- 事件触发MPC：仅在误差超过阈值时重新计算
- 并行化计算：使用GPU加速QP求解
- 学习型MPC：在线更新模型参数
matlab复制% 简单的模型参数在线更新
if mod(k,10)==0
A_est = recursive_least_square(u_hist, x_hist);
end
这个方案已经成功应用于我们的农业植保无人机项目，在复杂果园环境中实现了厘米级轨迹跟踪。最关键的收获是：MPC不是越复杂越好，工程实现中80%的性能提升来自对20%关键参数的精心调节。