我一直觉得四旋翼轨迹跟踪这类题目最能拉开差距的不是控制器写得花不花哨而是“你有没有把控制问题想清楚”。前阵子把一套仿真从PID切到MPC模型预测控制第一眼看到跟踪螺旋轨迹的效果确实比我想象中好尤其是高速转弯那段MPC会提前减速并规划好姿态而不是等误差出来了再去“救火”。这篇文章就围绕“四旋翼飞行器UAV_MPC模型预测轨迹跟踪控制研究Matlab代码实现”这个主题把我从建模、控制器设计、代码实现到调参踩坑的完整过程写了一遍。内容适合正在做课程设计、毕业课题或者想从PID/LQR往MPC方向过渡的工程师我会尽量把每个“为什么这么做”讲透不搞只有公式没有逻辑的堆砌。1. 为什么轨迹跟踪这题MPC比PID和LQR更适合当主角先说个这些年做控制仿真的体会轨迹跟踪难不在“稳定”而在“既要跟得快、又要跟得稳、还不想突破执行器极限”。PID是把当前误差放大成控制量天然是反应式的等误差出来了才动作遇到高速变化参考时总有一股说不出来的滞后感。LQR则是基于无限时域的线性最优控制能处理多变量耦合但要把输入饱和、姿态限制这类约束优雅地写进去非常麻烦。MPC的核心差异在于它每次控制都在“向前看一段未来”用模型预测未来行为再在线求解一段最优控制序列最后只执行第一步。这个机制决定了它更适合轨迹跟踪这种带前瞻性的任务。1.1 轨迹跟踪的本质不是捏住误差而是提前规划未来运动看一个直观的例子四旋翼要沿一条圆形轨迹飞行速度恒定但切线方向不断变化。PID控制下横滚、俯仰角指令通常基于位置误差和速度误差生成只有在系统已经偏离参考轨迹之后控制器才会生成对应的倾斜角修正这个“滞后”在高频转弯区域会被放大成可见的切向误差。MPC不一样它在当前时刻就知道未来几秒钟的参考轨迹是一条圆弧于是可以在进入弯道之前就开始分配横向加速度像司机看到弯道会提前打方向盘一样。另一个点轨迹不只是一串位置点它往往还伴随期望速度、期望加速度。MPC的预测模型可以把位置误差、速度误差、甚至加速度前馈统一放进代价函数里去权衡这在PID里通常需要额外设计前馈通道否则跟踪带宽上不去。所以轨迹跟踪这个场景本质上是“带预见性的最优控制问题”MPC天生就站在这个问题的正解位置上。1.2 PID/LQR在四旋翼上的两个短板约束问题和前瞻问题先说约束。四旋翼有一个很现实的上限油门不能无限大姿态角不能随便倾斜到90度电机的角加速度也有限制。PID在设计时如果想不让执行器饱和要么把增益调得很保守要么在外面加一堆限幅逻辑。限幅看起来简单但限幅后的闭环行为实际上是不可分析的工程上经常出现“仿真无事发生实飞一限幅就抖”的情况。LQR本身是无约束最优控制设计时根本不考虑输入饱和如果强行把输出压到饱和边界线性最优的假设就被破坏了。再说前瞻。LQR虽然能给出状态反馈矩阵但它假设的是无限时域也就是“今天和明天的控制策略完全一样”可轨迹跟踪的参考是时变的天然不满足这个前提。虽然可以用LQR加前馈、加轨迹线性化来凑但每一次参考变化都要重新计算增益复杂度一点不低。相比之下MPC把“当前时刻往前看Np步”写成一个有限时域优化问题参考轨迹可以直接以序列形式塞进去前瞻能力是内置的不是靠额外模块补出来的。1.3 四旋翼MPC的两种主流路线全量NMPC与级联线性MPC刚接触MPC的人很容易一头扎进全量非线性模型预测控制NMPC也就是把位置、速度、姿态角、角速度一共12个状态全部放进预测模型控制量直接就是四个电机的推力或者期望力矩。这种路线理论最完整但有两个现实问题第一每个控制周期要求解一个非线性优化问题计算量明显高第二姿态动力学和位置动力学时间尺度差了一个量级放在同一个优化问题里数值刚度会让求解器非常痛苦。工程里我更推荐另一条路线级联MPC。外环用一个线性/轻度非线性MPC做位置控制输出期望加速度内环用姿态控制器去跟踪期望姿态角姿态环可以用PID也可以再用一层MPC。四旋翼本身的物理特性决定了姿态响应比位置响应快得多这种级联结构完全符合系统的时间尺度分离特性而且外环MPC只需要用到位置、速度状态模型简单、求解快Matlab实现起来特别顺手。后面我讲的整套代码就是基于这条路线先把外环位置MPC跑通再考虑要不要升级到NMPC。2. 先把模型放到台面上从牛顿-欧拉方程到能进MPC的简化模型MPC是一个“模型越准控制越好”的算法但这里的“准”不是越复杂越好而是“预测模型要抓住主导动态同时计算量可控”。四旋翼全动力学模型虽然完整直接塞进MPC里往往不是最优选择。先说完整的动力学长什么样再说怎么简化这样你能理解为什么最终代码里用的是那个简化版本。2.1 从牛顿-欧拉方程到12维状态每一步的物理含义四旋翼的常见状态取12维位置3个分量、速度3个分量、姿态角3个分量或者四元数4个分量、角速度3个分量。如果设位置为p [x,y,z]^T速度为v [vx,vy,vz]^T姿态角为Φ [φ,θ,ψ]^T角速度为ω [p,q,r]^T那么刚体运动学方程可以写成p_dot v v_dot g * e3 (R(Φ) * [0;0;T]) / m Φ_dot W(Φ) * ω J * ω_dot τ - ω × (J * ω)这里的R(Φ)是从机体坐标系到世界坐标系的旋转矩阵e3是竖直方向的单位向量T是总推力τ是三个轴的力矩W(Φ)是欧拉角速率与机体角速度之间的变换矩阵包含了欧拉角的奇异项。每条方程的物理含义都很直接位置微分是速度速度微分由重力、推力、空气阻力共同决定姿态角微分由角速度通过变换矩阵得到角速度微分由力矩、转动惯量和陀螺力矩共同决定。2.2 为什么MPC里常用“平动双积分近似姿态响应一阶近似”如果你把上面12个状态全部作为预测模型每个控制周期里要同时优化推力、三个力矩变量数量多姿态块的强非线性还会让优化时间变长。实际中四旋翼的姿态内环可以做得很强典型姿态闭环带宽可以做到位置外环带宽的5到10倍以上所以位置MPC完全可以“假装”姿态已经稳定只把期望加速度作为控制量再通过一个一阶惯性环节近似姿态响应。我的做法是外环MPC状态只取[x,y,z,vx,vy,vz]和上一时刻的期望加速度或姿态角控制量u [ax_cmd, ay_cmd, az_cmd]^T是期望加速度。在预测模型里位置和速度的更新用双积分p(k1) p(k) Ts * v(k) v(k1) v(k) Ts * (u(k) - g * e3)然后再用一个姿态响应近似把u对应的实际加速度延迟一阶a_actual(k1) alpha * a_actual(k) (1 - alpha) * u(k)alpha由姿态环闭环时间常数决定一般取 0.8到0.95之间。这个简化不是拍脑袋它的意义在于把“推力大小”和“姿态方向”这两个物理量从预测模型里抽离成加速度层的问题MPC只负责回答“我这Np步要怎样的加速度”至于“多大油门、什么角度”交给内环去算职责分离。2.3 离散化处理零阶保持与RK4的选择MPC是数字控制连续方程必须离散化。最常用的组合是预测模型用相对简单的欧拉离散或零阶保持离散真实仿真模型用四阶龙格库塔RK4。这样故意制造一点模型失配反而能检验控制器的鲁棒性。采样时间Ts我一般取0.02~0.05秒对应控制频率20~50Hz。如果你用fmincon做非线性优化Ts0.02、预测时域Np30意味着每个周期要计算60步的预测轨迹计算量需要提前评估速度要求不高时可以把Ts放到0.05预测时域缩短到20步。3. MPC控制器的三块拼图代价函数、约束和滚动优化怎么搭配所有MPC都绕不开三件事怎么定义“好坏”、哪些量可以动、哪些量不能越界、以及怎么让这个优化在线滚动起来。这三个设计点直接决定跟踪精度和执行器损耗之间的平衡。3.1 代价函数怎么写输出误差、输入变化量与参考权重之间的权衡我用的代价函数是这一形式J sum_{i1}^{Np} (e_i^T * Q * e_i) sum_{i1}^{Nc} (u_i^T * R * u_i du_i^T * Rd * du_i)其中e_i [p_pred(i)-p_ref(i); v_pred(i)-v_ref(i)]是预测状态与参考轨迹在预测时域内的误差u_i是期望加速度du_i u_i - u_{i-1}是控制增量。Q的维度跟状态一致R、Rd对应控制量和控制增量都是对角矩阵。权重矩阵的调法有规律可循。位置误差项的权重Q_pos决定跟踪精度小了会看到轨迹偏离大了会看到加速过猛速度误差的权重Q_vel我一般给位置权重的一半左右太大容易产生超调R用来避免控制量过大但对于轨迹跟踪R不能给得太大否则会出现“为了省力而不去追误差”的情况Rd才是抑制抖振的关键它管的是控制量变化得快不快适当增大Rd比增大Q更有效。我常用的初始值是Q_pos diag([1,1,3])Q_vel diag([0.5,0.5,1])R 0.01*IRd 0.1*I然后根据仿真现象再调。3.2 约束都约束在哪儿油门饱和、姿态角限制与轨迹偏差走廊MPC的约束分为输入约束和状态约束两类。输入约束最直接期望加速度的水平分量不能超过由最大倾斜角决定的上限竖直分量要落在油门可提供的范围内。如果最大允许倾斜角是30度那么水平加速度上限大约是g*tan(30°) ≈ 5.67 m/s²这个值可以直接写成线性不等式约束-5.67 a_xy 5.67 a_z_min a_z a_z_max状态约束也是可以加的比如限定高度不能低于某值、不能高于某值或者给位置误差一个走廊范围。注意一点硬的输出约束在轨迹比较复杂时经常导致优化问题不可行因为预测轨迹可能来不及在约束内转弯。我的处理方法是把这些状态约束做成软约束也就是在代价函数里加一个惩罚项J_soft J rho * epsilon^2epsilon是允许约束违反的松弛变量rho是惩罚系数。这样求解器永远不会因为“预测越限”而直接罢工但越得多、代价就越高控制器会尽量避开受限区域实际调试体验比硬约束好得多。3.3 滚动优化的实现流程预测、求解、只执行第一步在每个控制周期k完整的滚动优化步骤如下获取当前状态x_k既可以是仿真里的真值也可以是估计状态。截取参考轨迹从k到kNp-1的序列包括位置参考和速度参考。将初始状态代入预测模型把决策变量u_k,...,u_{kNc-1}映射成预测位置序列。求解带约束的优化问题得到最优控制序列。只将第一个控制量u_k发给内环控制器。到下一个采样时刻状态更新后重复第1步。这里最反直觉的一点是明明算出来Np步的控制量为什么只执行第一步因为模型不完美、外部有扰动预测轨迹一定会逐渐偏离真实状态。如果傻乎乎把Np步全部执行完误差会越来越大。每步滚动、每步重新优化的过程相当于一个“不断修正的闭环规划器”这也是MPC被称为“反馈控制”的原因——MPC不是开环最优控制而是反馈最优控制。3.4 Matlab求解器选型quadprog、fmincon还是MPC Toolbox在Matlab里写MPC你面对的求解器选择直接影响代码写法和运行速度求解方式适用场景优点缺点quadprog线性模型、线性约束求解快适合实时性较高的仿真必须把问题化成二次规划标准形式fmincon非线性预测模型模型自由度大能处理非线性约束求解慢可能陷入局部最优MPC Toolbox标准线性MPC上手最快自带参数化界面定制化空间小依赖工具箱CasADi外部工具箱非线性MPC支持自动微分求解效率高需要额外安装学习曲线略陡我的建议是如果只是入门或者做课程验证先用quadprog跑通“线性MPC位置控制”的完整链路因为你的预测模型只要写成双积分近似代价函数是二次型约束是线性不等式问题就是一个标准QP代码结构清晰调试容易。等你对MPC的逻辑有感觉了再考虑用fmincon或者CasADi上非线性预测模型。别一上来就开大优化问题一旦变成非凸你会花掉大量时间在“为什么解不出来”上。4. Matlab代码实现主循环、优化求解和参考轨迹逐段拆解这一节把核心代码结构直接拆开讲。完整工程文件里会包含初始化脚本、参考轨迹函数、MPC求解函数、真实动力学更新函数和绘图脚本这里重点说几个容易卡住的地方。4.1 总代码框架初始化、主循环、可视化三段式结构主程序框架大致是这样一个循环%% 初始化 p struct(m,1.2,g,9.81,Ts,0.02,Np,30,Nc,10); p.Q blkdiag(eye(3), 0.5*eye(3)); p.R 0.01*eye(3); p.Rd 0.1*eye(3); p.a_max 5.67; % 水平加速度上限对应30度最大倾角 p.az_min -2; p.az_max 8; % 竖直加速度范围 x_state zeros(6,1); % [x,y,z,vx,vy,vz] N_steps 5000; x_history zeros(6,N_steps); u_history zeros(3,N_steps); for k 1:N_steps t_k (k-1)*p.Ts; % 截取未来Np步参考轨迹 ref_seq gen_reference(t_k, p.Np, p.Ts); % 求解MPC得到第一个控制量 u_cmd solve_mpc_linear(x_state, ref_seq, p); % 保存数据 u_history(:,k) u_cmd; x_history(:,k) x_state; % 真实模型更新RK4故意和预测模型不完全一样 x_state dynamics_rk4(x_state, u_cmd, p.Ts, p); end这里有一个小设计参考轨迹生成函数一次性返回未来Np步的序列而不是只返回当前点的参考这样可以省去主循环里频繁拼接、切片的开销也方便后面调试预测时域长度。4.2 代价函数与QP约束矩阵的代码写法线性MPC用quadprog时核心工作是构造标准形式min 0.5 * U * H * U f * U s.t. A_ineq * U b_ineq lb U ub其中U [a_x(k); a_y(k); a_z(k); ...; a_x(kNc-1); ...]是一个3*Nc维向量。通过双积分模型可以把预测位置、预测速度写成U的线性函数代入代价函数后展开就能得到H和f。代码大致是这样function u_opt solve_mpc_linear(x0, ref_seq, p) Nc p.Nc; Nu 3*Nc; H zeros(Nu); f zeros(Nu,1); % 构造状态递推矩阵根据双积分模型 A eye(6); Ad zeros(6,3); A(1:3,4:6) p.Ts*eye(3); Ad(4:6,:) p.Ts*eye(3); % 对每个预测步累加代价矩阵省略展开细节 [H, f] build_cost_quadratic(A, Ad, x0, ref_seq, p); % 输入约束 lb [-p.a_max; -p.a_max; p.az_min] * ones(Nc,1); ub [ p.a_max; p.a_max; p.az_max] * ones(Nc,1); % 控制增量约束如果走线性不等式则在这里写A_ineq opts optimoptions(quadprog,Display,off); [U_opt, ~, flag] quadprog(H, f, [], [], [], [], lb, ub, [], opts); if flag 0 % 宽松约束后再求一次 lb(:) -1e3; ub(:) 1e3; U_opt quadprog(H1e-6*eye(Nu), f, [],[],[],[],lb,ub,[],opts); end u_opt reshape(U_opt, 3, Nc); u_opt u_opt(:,1); end如果选fmincon路线代码就朴素一些用函数句柄把代价函数和约束函数传进去即可但要注意fmincon对初值敏感建议把上一时刻的最优解平移一拍作为当前初值也就是“热启动”能明显减少迭代次数。4.3 状态更新与参考轨迹生成螺旋线的参考信号怎么给参考轨迹生成这部分很多人容易忽略速度参考。位置MPC同时跟踪位置和速度会让轨迹跟踪更平滑。螺旋线参考可以这样生成function ref gen_reference(t, Np, Ts) r 3.0; omega 0.4; z_spd 0.5; n (0:Np-1)*Ts t; x r * cos(omega * n); y r * sin(omega * n); z 0.8 z_spd * n; vx -r * omega * sin(omega * n); vy r * omega * cos(omega * n); vz z_spd * ones(size(n)); ref [x; y; z; vx; vy; vz]; end注意这里参考值的单位要保持一致位置用米速度用米每秒。如果你是分层结构参考轨迹的加速度也应当生成出来外环MPC可以把参考加速度作为前馈项加进代价函数减少跟踪滞后不过那会增加变量维度初次复现可以先不做。4.4 仿真与运行时间优化雅可比矩阵、解析梯度能省多少时间在Matlab里做MPC仿真最容易发现的问题就是“仿真比实际慢非常多”。这里的关键不是算法复杂而是优化求解器在数值求导上花了大量时间。fmincon默认用有限差分法估计梯度一个3*Nc维决策变量就要反复调用几十次代价函数。如果你把代价函数写成可解析的形式或者用optimoptions里的SpecifyObjectiveGradient选项提供梯度运行时间通常能下降30%到60%。quadprog不存在这个问题因为二次型的目标函数天然就是解析的这也是我推荐先走QP路线的原因之一。另一个实用技巧是降维如果位置环只控制水平位置竖向用单独的高度控制器决策变量就少1/3。控制时域Nc也不一定非要等于预测时域Np我先用Nc10, Np30让控制器对远期轨迹有预见但只优化前10步的控制量后面10步控制量保持最后一个值计算量大幅下降跟踪效果几乎不受影响。5. 仿真实验与参数影响从悬停抗扰到3D螺旋线跟踪代码跑通之后真正花时间的其实是一轮又一轮的参数实验。这里分享我记录的一组典型现象。5.1 实验设计与评价指标位置误差包络、姿态限幅、执行器输出我习惯于三类评价指标位置跟踪的均方根误差RMSE和最大瞬时误差、控制量是否频繁触达约束边界、控制增量的波动程度。仿真对比场景包括悬停小扰动、水平圆轨迹和3D螺旋线轨迹。以3D螺旋线为例在一组典型参数下Np30, Nc10, Q_pos1, Q_vel0.5, Rd0.1水平方向的位置RMSE大约能控制在0.08米以内竖直方向因为有独立的油门通道通常更小约0.03米。最大瞬时误差出现在进入螺旋线的起始段因为初始状态和参考轨迹之间有一段位置差MPC需要先“追上”参考这段误差不可避免但控制器不会为此产生剧烈振荡而是用接近约束上限的加速度尽快收敛整体行为是合理的。5.2 预测时域Np和权重Q/R分别怎么影响效果预测时域Np的影响非常直观它代表控制器“看得多远”。Np10时控制器只看到未来0.2秒如果Ts0.02进入弯道前几乎没有提前动作跟踪误差明显转弯处还会出现超调Np30时控制器能提前0.6秒规划转弯更平滑。但Np不是越大越好Np50时计算耗时从10几毫秒涨到几十毫秒而且远期参考的变化会被过大的不确定性抹平对当前控制质量贡献不大。权重方面我把两个对比结果列成一张表方便你参考调整方向现象说明Q_pos增大位置误差减小但加速度峰值升高跟踪精度和控制幅值之间的直接trade-offQ_vel增大超调减少但收敛变慢速度误差权重过高会让控制器“不敢追”位置误差R增大控制量整体变小跟踪滞后尽量避免开场就给大RRd增大控制量更平滑初始误差收敛边慢对抖振抑制效果最明显5.3 模型失配和噪声下的韧性测试MPC的运行效果最终要看模型的鲁棒性。我在真实仿真模型里加入两类异常一是把实际质量调成预测模型质量的1.1倍二是加入标准差为0.1的测量噪声和常值风扰。结果不出意料稳态精度稍微下降水平RMSE从0.08米变到0.12米左右但控制器没有发散。这里有一个经验如果模型失配导致固定偏移单纯靠MPC的反馈修正能压住一部分但压不干净更稳妥的办法是在预测模型中加一个扰动状态的增广比如把恒定风扰作为附加状态估计出来再补偿到预测方程里。我的代码里为了演示清晰只做了反馈修正如果你要做更深入的研究增广状态是非常有价值的下一步。6. 我在这套实现中踩过的坑和留给后来者的几条建议最后这部分是我最想说的因为网上能找到的MPC教程大多止步于“能跑”但真正折磨人的全是在跑通之后的事情。6.1 求解器陷入局部最优与不可行解的处理fmincon处理非线性MPC时最难缠的就是生成的优化问题不可行或者解出来一个看起来很怪的控制量。我的排查顺序是先看约束是不是给得太紧比如水平加速度上限3 m/s²但参考轨迹要求4 m/s²的加速度那必然不可行再看代价函数里有没有除以0的隐患最后看初值。初值问题很阴险每次都从全零初值出发前几拍因为跟踪误差大优化会跑到奇怪的地方后面还回不来。改成“上一时刻最优解平移一拍作为当前初值”之后这个问题几乎消失。6.2 控制频率与计算延迟仿真也不算账就完了Matlab仿真里默认“求解完立刻执行”但真实系统里控制量总有延迟。哪怕放着不做实飞我也建议你在仿真中加入“求解耗时”的模型假设求解耗时t_solve那么在k时刻应该用k-1时刻解出来的第三拍控制量来顶住而不是空等。实际操作中我通常把控制周期Ts设成预期求解耗时的3倍以上比如quadprog线性MPC在15毫秒解完我就把Ts设0.05秒留出充分余量。6.3 别再把欧拉角奇异带进预测模型姿态表示的选择建议如果你用的是全量非线性MPC姿态表示这一关早晚要过。欧拉角在俯仰角接近90度时会遇到万向锁变换矩阵直接退化预测模型瞬变。解决思路是预测模型里改用旋转矩阵或四元数表示姿态。但说实话对于大部分轨迹跟踪任务四旋翼很少做极限倒飞姿态此时级联结构里位置环根本不碰欧拉角只需要在把期望加速度转成期望姿态角时用到一点三角函数完全避开了奇异问题。所以能用级联就用级联这是我在工程上最实际的一句建议。6.4 如果下一步要做真实飞行器先改这几点这套Matlab仿真离实飞还有几步路要走。第一状态必须是估计值而不是真值外环MPC通常假设位置和速度完全已知实飞时要接视觉里程计、IMU融合或者RTK估计滞后必须建模第二执行器模型要考虑电机响应延迟和推力映射曲线尤其油门小角度附近的非线性会让MPC预测偏差变大第三传输掉线和时钟不同步会让主循环的参考轨迹切片产生跳变代码里需要加上超时保护和时间戳对齐。把这些补上之后MPC的工程价值才真正体现出来。最后再讲一个我自己的体会MPC之所以难不是算法本身有多玄而是它把所有控制问题都变成了“建模调参求解”的组合问题。你不需要一开始就追求大预测时域和复杂模型先把线性MPC的位置环跟一套内环PID跑通再慢慢把约束、扰动补偿、非线性预测模型加进来。我自己就是这样一步步过来的中间踩坑无数但每次把一个问题想透后面换任何控制对象都能很快上手。希望这篇记录能让你少走几段弯路。