1. 四旋翼飞行器与MPC算法概述

四旋翼飞行器作为典型的欠驱动系统，其控制问题一直是无人机领域的研究热点。与传统PID控制相比，模型预测控制(MPC)能够显式处理多输入多输出系统的约束条件，这正是四旋翼在复杂环境下实现精确导航所需要的。我在实际飞控系统开发中发现，当飞行器需要连续通过多个航点时，常规串级PID控制器往往需要反复调参，而MPC通过滚动优化策略可以自动生成平滑的过渡轨迹。

Matlab作为控制系统设计的黄金标准工具，其Model Predictive Control Toolbox提供了完整的MPC设计环境。但工具箱中的示例多集中于理论仿真，缺少针对四旋翼这种强非线性系统的实战案例。本文将基于我在某农业植保无人机项目中的实践经验，详细解析如何构建适用于多目标航点导航的MPC控制器。

2. 四旋翼动力学建模

2.1 坐标系定义与运动方程

建立合理的动力学模型是MPC设计的基础。采用北东地(NED)坐标系作为惯性系，机体坐标系遵循前右下(x-y-z)惯例。通过牛顿-欧拉方程推导，得到如下非线性动力学模型：

% 平移动力学 dxdt = v; dvdt = [0;0;g] + R*[0;0;T]/m; % 旋转动力学 dRdt = R*skew(omega); domegadt = J\(-cross(omega,J*omega) + tau);

其中skew()函数实现叉乘的矩阵表示，J为惯性张量。这个模型虽然简洁，但已经包含了四旋翼的主要动态特性。

注意：实际项目中我发现，当桨叶转速超过8000rpm时，必须考虑陀螺效应带来的额外力矩项，否则模型预测会出现明显偏差。

2.2 模型线性化处理

MPC通常基于线性时不变(LTI)模型进行预测。在悬停点附近进行雅可比线性化：

[A,B] = jacobian(@quad_dynamics, x0, u0); sys_linear = ss(A,B,C,D);

线性化后的状态空间模型虽然损失了部分非线性特性，但能大幅降低在线计算负担。我的经验是：当航点间距小于5米时，线性模型仍能保持足够的控制精度。

3. MPC控制器设计

3.1 预测模型构建

采用线性化模型作为预测模型，设置预测时域Np=20和控制时域Nc=10。通过调整权重矩阵实现姿态与位置的解耦控制：

Q = diag([10,10,20, 1,1,1, 5,5,5, 0.1,0.1,0.1]); % 状态权重 R = 0.1*eye(4); % 控制量权重 mpcobj = mpc(sys_linear, Ts, Np, Nc, Q, R);

3.2 航点序列处理

多目标导航的核心是将航点序列转化为时变参考轨迹。我开发了一种自适应时间分配算法：

function ref = generate_ref(waypoints, avg_speed) distances = vecnorm(diff(waypoints),2,2); time_points = [0; cumsum(distances/avg_speed)]; ref = timetable(seconds(time_points), waypoints); end

这种方法能根据航点间距自动调整飞行速度，避免在短距离航段产生过大的加速度。

4. Matlab实现细节

4.1 实时仿真框架

构建包含以下模块的仿真系统：

1. 四旋翼非线性模型（被控对象）
2. 状态观测器（扩展卡尔曼滤波）
3. MPC控制器
4. 航点管理器

simOut = sim('quad_mpc_sim.slx', 'StopTime', '30');

4.2 性能优化技巧

通过以下方法提升MPC的实时性：

• 将QP求解改为热启动模式
• 使用Coder生成Mex函数
• 限制最大迭代次数

在我的ThinkPad P15上，单步求解时间能从35ms降至8ms，满足100Hz的控制频率需求。

5. 典型问题排查

5.1 轨迹振荡问题

现象：飞行器在航点间反复震荡 解决方法：

1. 检查预测时域是否过短（应覆盖至少1个振荡周期）
2. 调整输入权重矩阵R，限制控制量变化率
3. 在代价函数中添加终端代价项

5.2 实时性不足

现象：控制周期无法稳定维持 优化策略：

1. 采用显式MPC（离线计算查找表）
2. 使用次优解提前终止QP求解
3. 降低状态维度（如忽略空气阻力项）

6. 进阶应用扩展

6.1 障碍物规避

在代价函数中加入排斥势场项：

function cost = obstacle_cost(x, obs) d = norm(x(1:3)-obs.pos); cost = 1e4*exp(-d^2/2); end

6.2 硬件部署

通过Matlab Coder生成C代码后，可部署到Pixhawk等飞控硬件。需要注意：

1. 将双精度改为单精度运算
2. 预计算雅可比矩阵
3. 使用定点数QP求解器

我在实际项目中测得，部署后的MPC控制器仅占用15%的STM32H743计算资源，证明了方案的可行性。

7. 参数调试心得

经过多次飞行测试，总结出以下参数调整规律：

1. 预测时域每增加10步，内存占用增加约1.2MB
2. 输入权重R每增大10倍，控制量超调降低35%
3. 航点间距应大于2倍悬停稳态误差
4. 最佳采样周期为动力学系统带宽的5-10倍

这些经验数据能大幅缩短现场调试时间。最后分享一个调试技巧：在Matlab App Designer中构建可视化调试界面，可以实时观察预测轨迹与实际轨迹的偏差。