四旋翼的MPC轨迹跟踪最近不少朋友在问。我刚把一个基于Matlab的模型预测控制MPC仿真项目完整跑通把四旋翼的动力学模型、MPC优化问题构建、QP求解器调用到轨迹跟踪效果调优整个过程都过了一遍。这里把从零开始实现的思路、代码结构、遇到的那些坑一次性说清楚给正在搞无人机控制或者准备入门MPC的同学做个参考。这个项目核心是用MPC控制器取代传统PID让四旋翼在满足姿态和执行机构约束的前提下准确跟踪三维空间中的参考轨迹。适合已经了解四旋翼运动学、有一定Matlab和Simulink基础但对MPC还停留在理论阶段的读者。我会尽量少讲空泛的公式推导多说怎么把MPC“落到代码里”以及实测中哪些参数最影响跟踪性能。1. 为什么轨迹跟踪选MPC而不是继续堆PID很多做四旋翼控制的第一反应是PID简单、鲁棒工程上也非常成熟。但轨迹跟踪场景下PID有两个很尴尬的问题一是耦合强四旋翼本身就是一个强耦合、欠驱动系统横滚、俯仰和升力共同决定水平加速度PID通常只能线性化解耦后再串联调节参数整定依赖大量实验二是不会“看未来”PID只根据当前误差作用遇到高速大机动轨迹时会产生明显滞后跟复杂曲线时经常出现“切弯不到位”或“过冲”。MPC的核心思想是滚动优化——在每个控制周期内利用当前状态预测未来一段时间的输出在满足约束的前提下求解一个有限时域优化问题然后只执行第一步控制量下一周期重新滚动。这意味着MPC天然具备“前瞻性”能把轨迹未来的弯曲趋势纳入当前决策同时把姿态角限幅、执行机构饱和、速度限制等约束直接放进优化问题而不是靠外部限幅器硬截断。我用一个具体例子说明差异。让四旋翼跟踪一条“8”字形三维轨迹PID在圆弧段最快也只能以较小的倾斜角跟随一旦参考加速度超过机体当前姿态能提供的极限跟踪误差就开始发散MPC则会在进入圆弧前提前预倾斜把误差控制在很小的范围内。这就是“预测”带来的本质优势。当然MPC不是万能的。它对模型精度敏感计算量比PID大一个量级调节参数也比PID复杂。但在轨迹跟踪这个任务上MPC的模型预测能力正好击中痛点所以很多科研项目和前沿开源飞控都在往这个方向走。这个项目里我们选用的是线性时变MPCLTV-MPC在四旋翼悬停点附近做线性化滚动更新线性化点以平衡精度和计算量适合在Matlab环境里仿真验证。2. 四旋翼动力学模型与MPC预测模型构建2.1 从牛顿欧拉到状态空间表达式四旋翼的常用建模方式是牛顿-欧拉法。把机体视为刚体六个自由度包含位置x, y, z和姿态角滚转φ、俯仰θ、偏航ψ。控制输入是总推力T和三个轴的力矩τφ、τθ、τψ最终通过四个电机的转速合成。位置方程可以简化表达为[ \ddot{P} R(\phi,\theta,\psi) \cdot \frac{T}{m} \cdot e_z - g e_z ]旋转方程用欧拉角形式写在小角度假设下可以近似为三个独立的二阶惯性环节。MPC设计中很少直接用非线性模型做优化那样代价太高普遍做法是在工作点线性化。悬停状态下姿态角接近零速度接近零把平移和旋转方程分别展开成状态空间方程。状态量我取12维[ X [x,y,z,\dot{x},\dot{y},\dot{z},\phi,\theta,\psi,p,q,r]^T ]前六维是位置和线速度后六维是姿态角和角速度。输入量U [T, τφ, τθ, τψ]共四维。输出取位置和偏航角即可因为水平位置由姿态间接控制不需要全部输出。线性化之后得到[ \dot{X} A X B U B_d (X_{ref}) ]其中参考轨迹带来的额外项合并到B矩阵里处理。关键点在于A和B不是常数矩阵——它们依赖当前的姿态和速度。MPC滚动更新时每个周期都要重新计算线性化矩阵这也是“线性时变”的含义。2.2 离散化与约束规范化连续模型必须离散化才能交给MPC。Matlab里最稳妥的方式是使用c2d函数不要自己用欧拉法近似因为欧拉法在采样时间稍大时会放大误差。我用的采样周期是10ms也就是100Hz控制频率离散化方法选零阶保持器zohTs 0.01; sys_d c2d(ss(A, B, eye(12), zeros(12,4)), Ts, zoh); [Ad, Bd] deal(sys_d.A, sys_d.B);约束处理是整个MPC工程化最容易出错的部分。四旋翼的约束大致分三类执行机构饱和每个电机的推力有限映射到总推力T和力矩τ的上下限同时还要保证四个电机都不超限这意味着T和几个τ之间是耦合约束不能简单各自限幅。姿态角约束为了安全或避免线性化失效一般把滚转和俯仰角限制在±30°以内。这个约束实现起来很直接就是对状态变量限幅。加加速度/推力变化率约束电机响应有物理极限所以相邻周期控制量变化ΔU也要限制。在Matlab的MPC设计中约束可以统一写到控制器对象里。用模型预测控制工具箱时直接设置mpcobj.OutputVariables(1).Min -2; mpcobj.OutputVariables(1).Max 2; mpcobj.ManipulatedVariables(1).Min 0.5*m*g; mpcobj.ManipulatedVariables(1).Max 1.5*m*g; mpcobj.ManipulatedVariablesRate(1).Min -0.2; mpcobj.ManipulatedVariablesRate(1).Max 0.2;如果是自己写QP公式则约束写成(A_{ineq} \cdot \Delta U \le b_{ineq})的形式。推荐先从工具箱跑通再自己实现QP这样能快速对照验证。2.3 MPC优化问题的数学构造MPC每一步要解决这样一个问题预测时域N内求一组输入增量ΔU使预测输出与参考轨迹的偏差最小同时控制增量不能太大。目标函数通常写成[ J \sum_{k0}^{N-1} | y_{k} - y_{ref,k} |Q^2 | \Delta u_k |R^2 | u_k - u{ref} |{S}^2 ]Q矩阵惩罚输出偏差R矩阵惩罚控制增量S矩阵惩罚控制量偏离稳态值。这个表达式看起来不难真正实现时最花时间的反而是参考轨迹的构建——参考轨迹必须既包含位置参考还要包含对应的速度、加速度参考这样MPC才能形成“前馈反馈”的效果。我见过很多仿真模型姿态振荡就是因为只给了位置参考没有给速度参考导致MPC一直在追赶运动目标产生纯滞后。推导预测方程是其中一步关键操作。把离散状态空间方程迭代N步可以得到一个大的矩阵形式预测状态向量(\tilde{X} A_{aug} X_k B_{aug} \Delta U)预测输出(\tilde{Y} C_{aug} \tilde{X} D_{aug} \Delta U)把(\tilde{Y})代入目标函数J就变成了关于ΔU的二次型[ J \frac{1}{2} \Delta U^T H \Delta U f^T \Delta U ]其中H只依赖Q、R和系统矩阵f依赖当前状态与参考轨迹。约束也全部转换成关于ΔU的线性不等式。这样就把MPC转化成了标准的二次规划问题Matlab里可以用quadprog或工具箱自带的QP求解器求解。3. Matlab代码实现分层拆解3.1 整体文件结构与主循环框架建议把项目分成几个模块不要把所有代码堆在一个脚本里。我当时的文件结构是这样的quad_mpc/ ├─ main_sim.slx % Simulink仿真模型 ├─ init_params.m % 全局参数初始化 ├─ mpc_controller.m % MPC控制器回调函数 ├─ quad_model.m % 四旋翼非线性模型用于被控对象 ├─ reference_traj.m % 参考轨迹生成 ├─ plot_results.m % 结果可视化 └─ run_compare.m % PID与MPC对比实验主循环不用Simulink也可以纯Matlab脚本逐周期仿真更直观尤其适合调试MPC算法本身。核心循环框架如下for k 1:N_sim % 1. 获取当前状态真实状态或测量噪声叠加 x_k x_history(:, k) noise; % 2. 生成参考轨迹未来N步 ref_seq reference_traj(t_k : Ts : t_k N*Ts); % 3. 线性化并更新MPC预测模型 [A_k, B_k] linearize_quad(x_k); % 4. 求解QP得到最优控制增量 [U_opt, exitflag] solve_mpc_qp(x_k, ref_seq, A_k, B_k); % 5. 取第一步控制量输入到非线性模型 u_k U_opt(:,1); % 6. 用非线性模型推进仿真Runge-Kutta或欧拉 x_history(:,k1) nonlin_step(x_k, u_k, Ts); end注意事项第3步线性化矩阵不建议每个周期都重新计算一次除非轨迹变化很快。对于大多数轨迹跟踪可以每隔5个采样周期更新一次A、B矩阵能明显减少计算时间而不损失性能。实测10ms周期下纯Matlab循环能做到接近实时但加上Simulink后反而更慢因为Simulink每个步长都要调用外部函数所以纯脚本更适合算法验证。3.2 参考轨迹生成与前瞻数据准备轨迹跟踪的前提是有一条平滑可跟踪的参考轨迹。如果参考轨迹本身带有突变再好的MPC也追不上。我在项目里用了两种典型参考螺旋上升轨迹(x 2\cos(0.2t))(y 2\sin(0.2t))(z 0.5t)主要验证三维位置跟踪和姿态协调。“8”字形轨迹(x 2\sin(0.3t))(y 1.5\sin(0.6t))(z 1.5)验证连续转弯和反向运动。生成参考轨迹时一定要通过数值微分同时产生速度参考和加速度参考并把它们输出成结构体数组方便MPC按预测时域索引。如果只存位置后续MPC的前馈项会缺失控制效果会大打折扣。这里给出一个简单的参考生成示例function ref reference_traj(t) if nargin 1, t 0; end omega 0.3; ref.pos [2*sin(omega*t); 1.5*sin(2*omega*t); 1.5]; ref.vel [2*omega*cos(omega*t); 3*omega*cos(2*omega*t); 0]; ref.acc [-2*omega^2*sin(omega*t); -6*omega^2*sin(2*omega*t); 0]; end在MPC每个周期需要把参考序列按预测时域N提取好。提取时注意如果参考轨迹是时间函数那么预测时刻是(t_k i*Ts)不要错误使用固定步长索引。3.3 QP求解器选择与热启动策略Matlab里求解MPC的QP有两条路一是用MPC工具箱二是不依赖工具箱手写quadprog调用。我更推荐手写quadprog原因在于能完全掌控约束矩阵和权重变化并且方便观察QP求解失败的原因。调用方式如下options optimoptions(quadprog, ... Algorithm, interior-point-convex, ... Display, off, ... MaxIterations, 200, ... OptimalityTolerance, 1e-6); [U_opt, fval, exitflag, output] quadprog(H, f, Aineq, bineq, ... Aeq, beq, lb, ub, U_init, options);这里有几个经验热启动。把上一周期求解得到的(U_opt)作为下一周期的初始猜测(U_init)不仅收敛更快而且可以避免因为起始点离可行域太远导致的数值问题。轨迹平滑时我实测热启动能把平均求解时间缩短约40%。最优容差不要太严。MPC本身有反馈修正1e-6的精度已经足够设置到1e-10反而可能延缓收敛尤其是在线实时仿真时没必要。当exitflag为负数比如-2或-3时说明问题不可行。这时候不要只报错要规划一个降级策略。我常用的做法是两个一是把权重矩阵临时缩小某个惩罚项的权重这样可以扩大可行域二是把约束松弛比如允许姿态角约束从±30°临时放宽到±35°。在仿真中这种降级很有用真实飞行中也能防止控制器“宕机”。3.4 Simulink与纯脚本的选择建议Simulink做MPC的优点是可视化强可以直观接入四旋翼模型缺点是当MPC控制器内部包含复杂迭代或自定义QP时仿真步长容易不稳定。如果你的重心是验证MPC算法本身我建议先用纯脚本实现。等你确认算法没问题了再移植到Simulink做更复杂的故障注入或硬件在环测试。纯脚本实现的非线性被控对象我建议用四阶Runge-Kutta不要用欧拉法。因为线性MPC每一步预测都基于近似模型如果被控对象本身用欧拉法积分误差反而叠加。代码可以这样写function x_next nonlin_step(x, u, dt) k1 quad_dyn(x, u); k2 quad_dyn(x 0.5*dt*k1, u); k3 quad_dyn(x 0.5*dt*k2, u); k4 quad_dyn(x dt*k3, u); x_next x (dt/6)*(k1 2*k2 2*k3 k4); end这里的quad_dyn返回状态导数具体实现对每个状态量分别计算。写完后建议先用悬停工况做一个自检给任意小扰动看系统能否收敛回悬停状态。这个测试能暴露模型正负号错误、转动惯量设置不合理等问题。4. 参数整定让MPC从振荡发散到精准跟踪4.1 Q、R矩阵的实际含义与初值估计MPC参数整定和PID整定有相似逻辑但更直观。Q矩阵惩罚状态偏差R惩罚控制量变化。四旋翼轨迹跟踪中Q矩阵通常这么设置位置偏差权重100到1000。位置是我们最关心的所以要给高权重。速度偏差权重10到100。速度权重太低会导致跟踪出现相位滞后太高则会让控制器过于激进。姿态角权重1到10。姿态角是间接控制量不需要惩罚得太狠否则会限制机体的机动能力。角速度权重0.01到1。角速度通常只起阻尼作用权重可以很小。R矩阵惩罚控制量变化一般设成0.1到10之间的对角阵。R太小时控制增量不受限制容易导致四旋翼电机转速剧烈抖动整个控制量呈现高频串扰R太大时控制器过于保守跟踪误差变大且响应变慢。有一个实用的初值公式先把位置权重设为1然后按“每个量的单位量级”折算其他权重。比如位置单位是米速度单位是米/秒。如果典型速度为1m/s控制周期0.01s那么速度偏差引起的等效位移偏差约0.01m重要程度只有位置偏差的1%所以速度权重应在位置权重的0.01到0.1倍之间。这个思路比盲目试参更靠谱。4.2 预测时域和控制时域的选择预测时域Np决定了MPC“看到多远”。Np太小预测失去意义MPC退化成近似PIDNp太大计算量上升而且预测误差累积控制精度反而下降。我测试下来对于1Hz左右的参考轨迹频率Np取20到30比较合适对应预测0.2-0.3秒。如果你的轨迹更平缓可以适当加大Np到40跟踪会更平滑。控制时域Nc一般远小于Np表示未来多久内的控制量能自由变化之后保持恒定。Nc取3到5已足够再增大会增加很多决策变量却几乎不改善性能。在我的项目里Np25Nc4QP的决策变量维度是4×416个quadprog求解非常快平均只需要2ms左右。这两个参数的调整规律如下参数增大时效果减小时效果常见范围Np预测更远跟踪更平滑但计算量增大响应变快但会振荡20-40Nc控制更灵活但计算量剧增控制更保守噪声影响小3-8采样周期Ts计算压力减小但性能下降性能提升但计算压力增大5ms-20ms4.3 约束权重和松弛因子的微妙关系MPC的一个特点是约束是“硬”的一旦不可行就报错。真实工程里其实希望约束可以被适度违反只要代价足够大。于是就需要给约束加松弛因子。在Matlab工具箱里可以设置ECR等约束容差来调节权重手写QP则可以在成本函数中加入松弛变量并给一个很大的惩罚系数比如1e6。这里有个经验约束惩罚系数不是越大越好太大会导致约束几乎不可违反和硬约束无异一旦遇到微小数值扰动就可能出现不可行问题太小的松弛惩罚又会让控制器总是试图避开约束跟踪精度受损。我建议设置成Q矩阵中最大元素的10倍左右这样在关键场景下约束被严格限制但偶尔有微小超限也能被“容忍”。姿态角约束和推力约束的耦合问题值得一提。四旋翼如果滚转角达到30°水平加速度分量就是(g \tan 30° ≈ 5.7m/s²)。如果参考轨迹要求6m/s²的加速度那么约束严格设30°会导致无解。这时有两种解决办法一是放宽姿态角约束到35°二是接受位置跟踪误差并降低位置权重。实际应用中我一般先根据最大参考加速度反推最小姿态角约束再留出5°到8°的余量这个做法可以避免大量试错。5. 仿真结果分析与常见问题排查5.1 悬停收敛与轨迹跟踪结果先给结论MPC在轨迹跟踪上表现明显优于调好的PID但前提是参数正确。我的仿真里螺旋上升轨迹跟踪的均方根位置误差PID大约在0.15mMPC可以做到0.03m以内。姿态角在整个过程中保持在±25°以内电机推力也没有长时间饱和。一个值得注意的细节是跟踪的相位特性。PID跟踪正弦轨迹时会有一个固定相位滞后滞后量随频率增加而增大MPC几乎没有滞后因为控制器在预测区间内已经看到了未来几个周期的轨迹。这在“8”字形轨迹中表现特别明显MPC在转弯前大约提前3个采样周期就开始滚转而PID要到进入弯道才开始响应。但MPC并非完美。我观察到在轨迹曲率变化剧烈的点比如“8”字形交叉点MPC的预测模型如果更新不及时会出现瞬间误差尖峰。解决方案是这里的参考加速度比较大需要提高速度参考权重或者把线性化矩阵的更新频率提高到每3个周期一次。5.2 常见失控现象与排查方法我把调试过程中遇到的几个典型问题和解决过程列出来这对实际动手很有参考价值。现象一仿真刚开始状态疯狂发散几秒内位置跑到飞起。原因通常是A矩阵或B矩阵设置错误特别是控制输入的方向或符号反了。排查方法令所有状态为零输入一个固定的正推力观察是否向上加速。这个简单测试花不了1分钟能拦住大半低级错误。现象二跟踪全程误差稳定但总是差一个固定偏差。这多半是参考速度或参考加速度没有正确传给MPC导致MPC认为目标静止或匀速。检查参考序列索引注意参考序列维度是否与状态向量匹配。现象三收敛后小幅震荡但没有任何约束被激活。这是权重整定问题大概率是R太小或位置权重太大。处理方法把R增大5倍把位置权重减小2倍然后再看震荡是否减弱。不要一次改动太多参数建议一次调一个数记录响应的变化。我调试时的习惯是用一个多维数组保存每次参数对应的误差指标再统一对比看趋势。现象四QP求解经常报不可行偶尔还报出NaN。绝大部分情况下是约束条件自相矛盾。比如推力上限小于悬停所需推力那么系统在重力补偿附近就不可能满足约束MPC当然求不出可行解。解决方法是先检查各约束上下限是否物理自洽。四旋翼悬停需要的推力是(mg)推力上限至少要比它大20%~30%才能留出机动裕度。5.3 与PID对比实验的公平性说明很多论文在对比MPC和PID时往往只给PID调一套不太好的参数然后得出MPC更强的结论。这种对比没有说服力。我在项目里做了公平对比先把PID调到位用Ziegler-Nichols法加手工微调让其在简单阶跃响应中表现良好然后在同一平台上跑复杂轨迹控制频率保持一致。这样对比出来的MPC优势才可信。不过在调PID的过程中我也有新体会MPC对于模型误差的鲁棒性其实和PID差不多并不会天生更强。MPC的优势在于约束处理与前瞻但如果模型偏差太大预测就没有意义。仿真中如果四旋翼质量比标称值大20%MPC的位置误差会从0.03m上升到0.08m而PID也会从0.15m上升到0.2m左右。这说明MPC对模型参数的依赖并不比PID更脆弱但确实需要在线辨识或自适应机制来进一步补偿。6. 后续扩展与一些个人经验整个项目跑完之后我觉得MPC做四旋翼轨迹跟踪真正难的不是公式和代码而是“模型与实际问题的对应关系”。你只有把物理约束一项项翻译成数学表达把权重矩阵一个个对应到实际控制要求MPC才会从“看起来科学”变成“真的好用”。一个比较适合深入的方向是加入干扰观测器。仿真中加恒定风力干扰后纯MPC会出现一个稳态偏移因为预测模型里没有这个扰动。我在成本函数里额外增加一个干扰补偿项通过扩张状态观测器估计干扰力并在MPC预测模型中加以修正效果很好。这个扩展不会改变MPC的大结构主要是修改状态方程中的扰动向并修改QP中的参考序列。另外可以尝试把四旋翼的电机动力学加入MPC预测模型。大多数MPC模型把控制量直接当作推力或力矩但实际电机响应有延迟和惯性忽略这个环节在高速机动时会让MPC的预测偏乐观。加入一阶惯性环节后虽然状态量增加但可以看到更真实的控制效果。如果你打算把这套代码往实际飞控上移植要注意几点求解器要换成更高效的开源库比如嵌入了OSQP或自定义的qpOASES线性化矩阵要提前缓存避免在控制中断里做大量矩阵运算控制频率不能低于50Hz否则MPC的预测优势会被离散误差抵消。纯Matlab仿真只是第一步要真正飞起来还需要做代码生成和硬件在环测试。最后分享一个非常小的调试技巧。我在仿真里总会把MPC每一步的最优控制量记录下来然后在图里对比控制量参考值和实际输出值。如果发现控制量一直在边界附近振荡说明约束太紧或采样时间不足如果控制量变化很微弱则说明R权重过大控制器太懒。这两个方向性的判断比看误差曲线更快定位问题。这个项目做到现在最大的感受是MPC让四旋翼控制从“事后修正”变成了“提前规划”这种控制思路的转变带来的性能提升非常明显。后面我会继续把MPC与深度强化学习的结合做一次对比实验看看数据驱动方法能不能进一步补充模型失配时的鲁棒性。到时候有了新结论再来分享。