1. 四旋翼飞行器MPC控制的核心挑战
四旋翼飞行器的模型预测控制（MPC）轨迹跟踪问题本质上是一个多变量、强耦合的非线性控制难题。与传统的PID控制相比，MPC最大的优势在于能够显式处理系统约束（如电机转速限制、姿态角范围）并预测未来多步的系统行为。
在实际飞行测试中，我发现四旋翼的动力学特性会带来三个主要挑战：
- 欠驱动特性：仅通过4个电机的转速差控制6个自由度（x,y,z位置和roll,pitch,yaw姿态）
- 实时性要求：通常需要在10-50ms内完成优化计算
- 模型不确定性：空气阻力、电池电压波动等未建模动态
2. 动力学建模与线性化处理
2.1 非线性动力学方程
采用牛顿-欧拉法建立机体坐标系下的动力学模型：
matlab复制% 位置动力学
dxdt = v;
dvdt = [0;0;-g] + R*[0;0;T]/m;
% 姿态动力学
dphidt = inv(I)*(M - cross(omega, I*omega));
其中R是旋转矩阵，T为总推力，M为力矩向量。这个模型需要考虑电机动力学延迟（实测约30-50ms），我在代码中增加了二阶电机模型：
matlab复制% 电机动态模型
tau = 0.035; % 时间常数实测值
domega_dt = (omega_cmd - omega)/tau - kf*omega^2;
2.2 工作点线性化
在悬停点附近进行线性化时，我发现两个关键技巧：
- 姿态角线性化范围应限制在±15°内（超过20°时线性模型误差显著增大）
- 需要补偿重力项：
u0 = m*g/(4*kf)（kf为升力系数）
线性化后的状态空间模型：
matlab复制A = [zeros(3) eye(3) zeros(3,4);
zeros(3,6) [0 g 0;-g 0 0;0 0 0] zeros(3,3);
zeros(4,6) diag([-1/tau -1/tau -1/tau -1/tau])];
3. MPC控制器设计细节
3.1 预测时域与采样周期选择
通过大量仿真测试，得出以下经验关系：
- 采样周期Ts：20-50ms（低于20ms求解器可能无法收敛）
- 预测时域N：建议8-15步（对应1-2秒预测范围）
- 控制时域Nu：通常取N/2
在代码中这样设置：
matlab复制Ts = 0.03; % 30ms采样周期
N = 10; % 预测步数
Nu = 5; % 控制步数
3.2 代价函数权重调整
代价函数采用经典的二次型形式：
matlab复制Q = diag([10 10 20 1 1 0.1 0.1 0.1 0 0 0 0]); % 状态权重
R = 0.1*eye(4); % 控制量权重
调试中发现z轴权重需要比其他位置高2-3倍，因为：
- 高度控制对电机转速变化最敏感
- 地面效应会导致低空飞行时出现升力波动
4. 实时优化实现技巧
4.1 QP求解器选择
对比测试了三种求解方案：
| 求解器 | 平均求解时间 | 内存占用 | 适用场景 |
|---|---|---|---|
| quadprog | 15ms | 中等 | 仿真环境 |
| OSQP | 8ms | 较低 | 嵌入式部署 |
| CVXGEN生成代码 | 3ms | 高 | 高性能飞控 |
最终选择OSQP作为默认求解器，因其在Raspberry Pi 4上也能稳定运行。
4.2 热启动优化
利用上一时刻的解作为初始猜测，可减少30-50%的迭代次数：
matlab复制if ~isempty(last_sol)
solver.warm_start('x', last_sol.x, 'y', last_sol.y);
end
5. 轨迹跟踪实验结果
测试了三种典型轨迹：
- 阶跃响应：超调<5%，稳定时间1.2s
- 圆形轨迹：跟踪误差<0.3m（速度1m/s时）
- 八字轨迹：最大误差出现在转弯处约0.5m
关键性能指标对比：
| 指标 | MPC | PID | LQR |
|---|---|---|---|
| 超调量 | <5% | 15-20% | 8-10% |
| 抗风扰能力 | ★★★★☆ | ★★☆☆☆ | ★★★☆☆ |
| 计算负载 | 较高 | 低 | 中等 |
6. 实际部署中的坑与技巧
- 电机混控处理：
matlab复制% 正确的推力分配矩阵
B = [1 1 1 1; % 总推力
0 -L 0 L; % roll力矩
L 0 -L 0; % pitch力矩
-c c -c c]; % yaw力矩
常见错误是忽略yaw轴力矩系数c（实测c≈0.05-0.1）
- 传感器延迟补偿：
matlab复制% 预测状态补偿
x_actual = x_predicted + (current_time - sensor_timestamp)*dxdt;
- 紧急降落逻辑：
当求解器连续3次失败时，自动切换到：
matlab复制u_emergency = [m*g/4; 0; 0; 0]; % 保持水平姿态缓降
7. 代码架构建议
我的项目采用分层结构：
code复制├── main_mpc.m # 主循环
├── model/ # 动力学模型
│ ├── nonlinear.m # 非线性模型
│ └── linearized.m # 线性化模型
├── controller/ # 控制算法
│ ├── mpc_design.m # 权重设计
│ └── qp_solver.m # 优化求解
└── trajectory/ # 轨迹生成
├── circle_ref.m # 圆形轨迹
└── lemniscate.m # 八字轨迹
关键接口设计原则：
- 采样周期严格定时（使用MATLAB的timer）
- 控制器输入输出采用结构体封装：
matlab复制ctrl_in = struct('x',x,'ref',ref,'t',t);
ctrl_out = solver(ctrl_in);
这个项目从仿真到实机飞行大约耗时3个月，最大的收获是认识到MPC参数调试需要系统化的方法。我总结了一个调试流程：先调高度控制→再调水平位置→最后调姿态响应。每次只调整1-2个参数，通过自动化脚本批量测试不同参数组合，最终在Crazyflie 2.1无人机上实现了厘米级轨迹跟踪。