我最早在模拟四旋翼轨迹跟踪时采用的是在悬停点线性化的单个模型。模型跑出来的闭环响应看着挺正常——小角度机动、低速飞行都跟得上一旦给定一个快速爬升配合大角度转向的3D轨迹偏差就肉眼可见地膨胀。后来把控制结构换成“LPV建模加上MPC预测控制”之后这条轨迹才算真正被按住。这篇文章就围绕这个组合展开为什么线性变参数模型能弥补固定线性模型的不足模型预测控制又如何在其中扮演执行者的角色以及整套系统在Matlab里怎么一步步落地。如果你正在做四旋翼的3D轨迹跟踪仿真或者被“非线性模型难处理、线性模型又不准”两头卡住这篇内容应该能给你一条明确的路。我会把LPV建模思路、MPC控制器设计、Matlab代码的组织方式以及我自己调参时踩过的几个坑尽量一次讲透。1. 从悬停点线性化到LPV模型四旋翼轨迹跟踪的痛点在哪1.1 固定线性模型的失效场景大机动跟踪为什么崩大多数入门级的四旋翼控制仿真都会在“悬停状态”附近做小扰动线性化得到一组定常的状态空间矩阵(A, B)然后用LQR或者线性MPC控制。这在悬停点附近没问题因为姿态角、线速度、角速度都足够小小扰动假设基本成立。但轨迹跟踪本质上是在让无人机不断离开悬停点。尤其当参考轨迹里出现螺旋爬升、快速转弯、俯仰角超过15°这样的机动时问题就来了动力学中的耦合项比如旋转矩阵引起的重力分解变化角速度和惯量叉乘带来的陀螺力矩在姿态角变大后不再是“小量”。固定矩阵A中的每个线性化系数只在那个点附近才准确。模型一旦失配MPC内部做预测时就会“以为”系统响应是某个样子实际响应却是另一个样子。预测不准滚动优化自然给出错误的控制量。最终表现就是位置跟踪有明显的稳态误差或相位滞后严重时还会出现小幅度振荡看起来像是系统“没吃饱劲”。我用同一个螺旋上升轨迹做过对比。固定线性模型MPC在爬升段的z方向误差能达到0.8米左右x-y平面在转向处也有持续0.3到0.5米的偏差。这个水平对仿真来说已经属于“不可用了”。1.2 另一种极端非线性MPC为什么难推广既然固定线性模型不够最直接的想法是上非线性MPCNMPC每个控制周期都直接对非线性模型做预测和优化。理论上这最准确但实际上很麻烦。NMPC每一步都要解一个非线性优化问题在Matlab里用fmincon这类工具往往要几百毫秒甚至几秒。仿真里能等真机上根本等不了。其次非线性优化不保证每次都能在指定时间内收敛你还需要处理局部极小值、初值猜测等问题。对于四旋翼这种状态维度12维起步、还有输入约束的系统NMPC的工程实现成本完全不低。于是LPV模型作为折中方案进入视野它在不下沉到非线性规划的前提下用一组“线性模型族”去逼近整个飞行包线内的系统动态。每个模型都来自某个平衡点附近的小扰动线性化然后通过调度变量在线选择或插值当前最合适的线性模型。这让我既能继续使用线性MPC的高效QP求解流程又能在不同速度、不同姿态条件下得到相对可信的预测模型。1.3 LPV在工程视角下的真实定位严格来说LPV有一整套基于线性矩阵不等式的稳定性理论。但在工程实现中我更多把它当成“带调度机制的增益调度控制”来用重点不在于证明闭环全局稳定而在于“当前模型足够贴近真实”。这和固定线性模型比是多了一个模型库和调度器和非线性MPC比是少了一大批在线求解负担。代价是需要提前做配平计算、网格化、以及调度变量的选择。这些工作一次性做在前面后面每次运行只是一些矩阵插值计算量完全可以接受。2. 四旋翼的LPV建模调度变量怎么选、平衡族怎么铺2.1 四旋翼动力学回顾哪些项是真正非线性的做LPV建模之前建议先把四旋翼的刚体模型写成标准形式。我用的状态通常取12维位置x, y, z线速度vx, vy, vz姿态角φ滚转、θ俯仰、ψ偏航角速度p, q, r输入是总推力u1和三个轴的力矩u2、u3、u4。在不考虑气动阻尼、旋翼进动等二次效应的前提下核心非线性主要来自三处一是旋转矩阵R(φ, θ, ψ)导致的推力矢量分解它让机体推力沿地面坐标系的三个轴都有分量而且和姿态角呈三角函数关系。二是姿态运动学中欧拉角速率与机体角速度之间的非线性映射。三是刚体欧拉方程中的角速度叉乘项ω × Iω会在快速翻滚时产生耦合力矩。这三处非线性决定了固定线性模型在大机动时的短板也决定了LPV模型的调度变量应该往哪些量上选。2.2 调度变量的选择策略别让网格维度爆炸LPV建模的核心是选定一组“调度变量”ρ在ρ的空间里布置若干配平点每个配平点对应一个线性模型。调度变量选得好不好直接影响模型族的覆盖质量。常见的做法是选和系统非线性最相关、又容易测量的量。四旋翼里最自然的选择是合速度V、爬升角γ和偏航角ψ。比如V sqrt(vx² vy² vz²)范围从0到6 m/sγ asin(vz / V)典型范围 -20°到20°ψ 在某些方案里可以固定为参考偏航做轨迹跟踪时通常假设ψ恒定或按参考轨迹给定为什么用合速度而不是三个速度分量主要是网格维度问题。如果调度变量取[vx, vy, vz]每个维度取5个点那就是125个配平点每个配平点都要做一次数值线性化后面插值也会更复杂。取[V, γ]或[V, γ, ψ]能大幅缩小网格规模。如果参考轨迹的偏航变化不剧烈ψ可以直接从参考轨迹取不作为在线调度变量这样在线阶段只需根据V和γ插值。这个取舍需要在文章里明确说一句模型族越细分跟踪精度越高但离线建模、在线插值、控制器求解的负担都上去了。建议从粗网格开始确认闭环稳定后再加密。2.3 配平计算LPV模型族的灵魂步骤配平点不是随便挑一个状态就能线性化它必须满足“在给定调度变量下系统处于平衡状态”。对四旋翼来说通常指速度恒定线加速度为零角速度为零姿态角恒定在这些条件下根据力和力矩平衡可以解出每个配平点对应的稳态控制输入。我在Matlab里习惯分两步走。第一步给定V和γ反推速度分量V 3.0; gamma deg2rad(10); vx V * cos(gamma); vy 0; vz V * sin(gamma);第二步忽略空气阻力时稳态条件是推力矢量与重力平衡。设期望偏航ψ_ref 0则可以从旋转矩阵反推稳态俯仰角θ̄和滚转角φ̄进而得到稳态总推力% 由加速度需求反推姿态无风环境 % 设期望加速度为0则合力应抵消重力 theta_bar atan2(vx * 0 ... , g); % 实际上是atan(vx_dot / g)形式匀速时趋于0 phi_bar atan2(vy * cos(theta_bar), g); u1_bar m * g / (cos(phi_bar) * cos(theta_bar));注意如果参考轨迹本身带加速度比如螺旋轨迹有向心加速度严格来说配平点应该包含这个向心加速度项。但实际工程中有一个很实用的处理方式调度变量依旧按速度来选配平按匀速条件解而把加速度导致的模型失配交给MPC的滚动优化去补偿。因为MPC每个周期都在重新优化轻微的模型偏差不会造成不可控的结果这比追求“全工况精确配平”要简单得多。配平之后在每个配平点附近用数值差分法对非线性函数f(x, u)求雅可比矩阵得到A(ρ)和B(ρ)。手工推导四旋翼的12维雅可比矩阵极其容易出错我用的是A zeros(12, 12); B zeros(12, 4); for i 1:12 x_perturb x_bar; x_perturb(i) x_perturb(i) h; f_plus nonlinear_dynamics(x_perturb, u_bar); f_minus nonlinear_dynamics(x_bar, u_bar); A(:, i) (f_plus - f_minus) / (2 * h); end这里h取1e-6左右效果还不错。数值雅可比虽然看起来“不够优雅”但对LPV模型族这种一次性离线计算来说稳定性远大于手推公式的优雅。2.4 在线生成当前模型最近邻还是线性插值离线把所有网格点上的(A, B)算好并存储之后运行时根据当前调度变量ρ(k)选出当前模型。两种常见做法最近邻取网格中离当前ρ最近的点直接返回其(A, B)。优点是快缺点是模型切换时会有跳变可能引起控制量轻微抖振。线性插值在网格矩形内对A和B逐元素插值。Matlab里可以用interpn或者自己写几个线性插值式子。工程上我更推荐插值因为平滑的模型变化能直接减少调度切换引起的瞬态。如果调度变量维度是2比如V和γ插值代码非常简单V_grid 0:1:6; gamma_grid deg2rad(-20:10:20); A_interp zeros(12, 12); for i 1:12 for j 1:12 A_interp(i,j) interpn(V_grid, gamma_grid, A_store(i,j,:,:), V_now, gamma_now, linear); end end不过这里要注意网格边界之外不要外插否则模型可能变得不物理。我习惯把调度变量做饱和处理让它始终落进网格范围。3. MPC控制器怎么搭预测方程、QP目标函数与约束的组装逻辑3.1 离散化与预测方程展开有了当前时刻的连续模型(A, B)第一步是离散化。这里必须强调一点不要用简单的欧拉近似至少用零阶保持ZOH离散化。四旋翼的动力学并不算特别刚但当预测时域拉长到几十步时欧拉法积累的离散误差会直接影响预测准确度。Matlab里直接用c2d就能得到Ad和Bdsys_c ss(A, B, eye(12), zeros(12, 4)); sys_d c2d(sys_c, Ts, zoh); Ad sys_d.A; Bd sys_d.B;然后就是标准的模型预测控制器推导。设预测时域为Np控制时域为Nc输入维度为Nu四旋翼是4状态维度为Nx这里12。定义从当前时刻k开始的未来状态序列X [x(k1|k); x(k2|k); ...; x(kNp|k)]U [u(k); u(k1); ...; u(kNc-1)]递推线性状态方程可以得到X PHI * x(k) G * U其中PHI是Np·Nx行、Nx列的矩阵由Ad的幂次堆积而成G是Np·Nx行、Nc·Nu列的矩阵描述每一时刻的控制量对未来所有预测点的影响。这段代码建议在仿真前一次性写好注意矩阵尺寸不要写错。PHI zeros(Np * Nx, Nx); for i 1:Np PHI((i-1)*Nx1:i*Nx, :) Ad^i; end G zeros(Np * Nx, Nc * Nu); for i 1:Np for j 1:min(i, Nc) G((i-1)*Nx1:i*Nx, (j-1)*Nu1:j*Nu) Ad^(i-j) * Bd; end end3.2 目标函数设计位置、速度、姿态各占多少权重MPC的目标函数可以写成状态误差加权与控制量加权之和J (X - X_ref)^T Qbar (X - X_ref) U^T Rbar U其中X_ref是未来Np步的参考状态序列。Qbar是分块对角矩阵每个对角块是QRbar同理每个对角块是R。实际设计Q和R的时候要知道四旋翼的控制层级到底该关心什么。我常用的权重矩阵是对角阵大致设置思路是状态量量级特征权重参考位置 x, y, z米级0.8 ~ 1.2线速度 vx, vy, vz米/秒级0.3 ~ 0.5姿态角 φ, θ, ψ弧度级0.1 ~ 0.2角速度 p, q, r弧度/秒级0.05 ~ 0.1为什么位置权重不是远大于姿态权重因为四旋翼是一个欠驱动的系统你要改变水平位置必须先倾斜机体产生水平推力分量。如果位置权重过大、姿态权重过小控制器会试图非常快地改变姿态角从而让姿态角快速冲到约束边界甚至产生振荡。反之姿态权重稍微留一点调节空间控制器会更平滑地“引导”机体倾斜跟踪更稳。这个道理类似于开车位置目标是“去某地”姿态是“轮子方向”你不可能同时要求车瞬间到位还保证方向一点都不偏只能在权重上做折中。3.3 约束怎么配置才合理四旋翼MPC里最核心的约束是输入幅值约束总推力u1上下限取决于电机推力和机体质量。仿真里我通常设成0.4·m·g到1.6·m·g之间留出足够的机动余量。力矩u2、u3、u4根据电机饱和边界设定一般取总推力上限相应放缩。另外还必须考虑姿态角约束尤其是φ和θ。这不仅是为了保护电机也是因为欧拉角姿态模型在±90°附近会出问题。把φ和θ限制在±30°以内既能保证模型准确也符合大多数实际飞行器的安全限制。把这些约束改写成标准形式A_ineq * U ≤ b_ineq也就是把每个时刻、每个输入的状态约束写成线性不等式。如果你的MPC还要做“输出约束”比如位置不能越过某个盒子边界那就要把PHI和G代入输出方程得到带状态矩阵约束的完整不等式组。我第一次实现时只约束了输入和姿态已经足够解决大多数跟踪问题。3.4 QP问题组装与quadprog调用将目标函数展开并合并同类项之后QP的标准形式是min 0.5 · U^T · H · U f^T · U s.t. A_ineq · U ≤ b_ineq其中H 2 * (G^T * Qbar * G Rbar) f 2 * (PHI * x(k) - X_ref)^T * Qbar * G注意f是行向量还是列向量取决于你后续怎么调quadprog。我在Matlab里习惯统一转成列向量f f(:); options optimoptions(quadprog, Display, off, Algorithm, active-set); u_seq quadprog(H, f, A_ineq, b_ineq, [], [], [], [], [], options); u_opt u_seq(1:Nu);有一个很关键的小经验如果你发现quadprog报“Hessian is not positive definite”或者求解特别慢第一件事检查Rbar是否太小第二件事检查G矩阵是不是有零列某些控制输入在预测时域内没起作用。Rbar基本权值不要设成0给一个很小的正值比如1e-4可以避免很多数值麻烦。4. Matlab仿真的代码骨架与关键函数实现4.1 仿真框架用非线性模型当“真实对象”让LPV-MPC当“控制器”闭环仿真最容易犯的错误是把LPV模型同时用于预测和控制然后又用同一个模型去“假装”真实无人机。这样仿真结果再好也只是在验证离散化不是验证控制器。正确做法是保持两个模型分离真实对象四维非线性模型用Runge-Kutta积分模拟无人机的真实动态。预测模型LPV模型由调度变量在线生成只用于MPC控制器内部的预测。二者的失配才是你在实际工程中一定会遇到的所以在仿真阶段就必须让这条失配存在。我的代码组织如下run_mpc_lpv_simulation.m % 主脚本跑完出图和指标 quadrotor_dynamics.m % 非线性动力学用于真实对象 lpv_model_library.m % 离线生成模型库 get_lpv_model.m % 在线根据调度变量插值返回A, B mpc_controller.m % 输入当前状态和参考序列输出控制量 reference_trajectory.m % 生成3D参考轨迹主循环的逻辑很简单x_true x0; % 初始状态 for k 1:N_sim t_now (k-1) * Ts; [x_ref_seq, ~] reference_trajectory(t_now, Np, Ts); rho compute_scheduling_variables(x_true); [A_lpv, B_lpv] get_lpv_model(rho); u_opt mpc_controller(x_true, x_ref_seq, A_lpv, B_lpv, params); x_true rk4_step(quadrotor_dynamics, x_true, u_opt, Ts); record(k, :) [t_now, x_true, u_opt]; end4.2 mpc_controller函数核心源码解读控制器函数不需要很长但每一步的推导都很重要。下面这段是我精简之后还能直接跑通的主干function u_opt mpc_controller(x_current, x_ref_seq, A_c, B_c, params) Ts params.Ts; Np params.Np; Nc params.Nc; Nx params.Nx; Nu params.Nu; Qbar params.Qbar; Rbar params.Rbar; % 零阶保持离散化 sys_d c2d(ss(A_c, B_c, eye(Nx), zeros(Nx, Nu)), Ts, zoh); Ad sys_d.A; Bd sys_d.B; % 预测矩阵 PHI zeros(Np*Nx, Nx); G zeros(Np*Nx, Nc*Nu); for i 1:Np PHI((i-1)*Nx1:i*Nx, :) Ad^i; end for i 1:Np for j 1:min(i, Nc) G((i-1)*Nx1:i*Nx, (j-1)*Nu1:j*Nu) Ad^(i-j) * Bd; end end % 参考序列 x_ref_vec reshape(x_ref_seq, Np*Nx, 1); % QP目标 H 2 * (G * Qbar * G Rbar); f (x_ref_vec - PHI * x_current) * Qbar * G; f f(:); % 输入饱和约束 U_min repmat(params.u_min, Nc, 1); U_max repmat(params.u_max, Nc, 1); % 姿态角约束用状态不等式实现 % 这里只写输入约束姿态约束需要构造A_ineq A_ineq []; b_ineq []; options optimoptions(quadprog, Display, off, Algorithm, active-set); u_seq quadprog(H, f, A_ineq, b_ineq, [], [], U_min, U_max, [], options); u_opt u_seq(1:Nu); end注意如果要做姿态角约束需要把姿态状态作为输出的线性组合加上状态预测方程X PHI·x G·U整理成对U的不等式。我第一次做的时候漏了这步导致姿态角在仿真中冲到很大的角度跟踪效果和稳定性都变得很差。4.3 参考轨迹生成螺旋爬升和8字轨迹的公式3D轨迹跟踪的验证参考轨迹要足够“刁钻”才有说服力。我常用两类螺旋爬升轨迹能同时检验水平转向和持续爬升function [pos_ref, vel_ref, yaw_ref] spiral_ref(t) R 4.0; omega 0.4; vz 0.6; x R * cos(omega * t); y R * sin(omega * t); z vz * t; dx -R * omega * sin(omega * t); dy R * omega * cos(omega * t); dz vz * ones(size(t)); pos_ref [x, y, z]; vel_ref [dx, dy, dz]; yaw_ref zeros(size(t)); % 保持机头朝前 end8字轨迹则重点检验连续变向能力用利萨如曲线生成x A * sin(omega_x * t); y B * sin(2 * omega_x * t);我建议在仿真里把“位置参考”和“速度参考”一起发到MPC的目标函数里。如果只给位置参考控制器在每一个预测步只知道“该往哪去”不知道“该以什么速度去”结果就是明显的追踪滞后。把参考轨迹解析求导得到速度信号之后跟踪性能会立刻改善。4.4 调度变量的计算与平滑调度变量的计算是整个LPV-MPC实现中最容易被忽视的环节。我踩过一个很大的坑直接用当前的瞬时速度去算调度变量结果无人机做微小幅度的速度波动时调度变量抖得很厉害线性模型频繁在相邻网格间跳变控制器输出也带上高频抖动。解决办法是给调度变量加一阶低通滤波V_filtered alpha * V_measured (1 - alpha) * V_filtered_prev; gamma_filtered alpha * gamma_measured (1 - alpha) * gamma_filtered_prev;alpha取0.3到0.5左右比较合适。滤波后的调度变量变化平缓模型切换平滑控制量抖振明显减少。这个细节如果不处理你会在曲线上看到非常“毛刺”的控制输出还不好定位原因。5. 仿真轨迹与调参避坑我在实测中遇到的那几个坑5.1 预测时域和控制时域怎么配对预测时域Np和控制时域Nc是MPC里最影响行为的两个参数。Np太短控制器“看不远”遇到轨迹弯道时才临时反应滞后明显Np太长计算量成倍上升而且远处的预测受模型失配影响大反而让控制质量下降。我常用的比例是Np 20到40步Nc 5到8步。Ts取0.05秒时Np30意味着看未来1.5秒。这个长度对四旋翼轨迹跟踪来说比较够用。Nc不需要和Np一样大因为控制量通道本来就不多后面时间的控制量对目标函数的影响可以通过G矩阵体现没必要全部作为变量。有一次我把Nc调到和Np一样大30quadprog的求解时间直接翻了约5倍跟踪效果却没有明显提升。从那以后我都固定采用“用小Nc控制、大Np预测”的配置。5.2 那一次QP无解软约束救了我的仿真在轨迹跟踪过程中如果参考轨迹要求的速度或姿态变化超过了模型实际能力加上姿态角约束后QP可能出现无解的情况。我第一次遇到时quadprog直接返回了一个空的U仿真在一个时间点上爆掉后面全是NaN。最直接的处理方法是加软约束。把姿态角和输入的硬约束放宽改成目标函数里的惩罚项J 原目标 λ · (超出量)^2思路是当控制器发现约束无解时不再强行要求所有约束都必须满足而是允许“稍微超出一点姿态角极限”同时用代价函数惩罚这种超出。这样QP始终有解且超限程度可控。在代码里做软约束需要引入松弛变量H矩阵和f向量都要对应扩充。这个改动不算大复杂度主要在组装矩阵时细心。仿真中一旦加了软约束后续几乎没有再出现过求解失败。5.3 固定模型与LPV-MPC的量化对比跑完一组螺旋轨迹后我把固定线性MPC和LPV-MPC的结果放在同一张图上对比差异非常直观。指标固定线性MPCLPV-MPC最大位置误差水平面0.62米0.18米最大高度误差0.81米0.22米平均控制量变化率较高带抖振较低平滑quadprog平均求解时间约12ms约18ms注意LPV-MPC每步还要做一次模型选择和插值所以求解时间略多。但换来的是整体跟踪精度提升3到5倍这个代价完全值得。5.4 调度网格边界附近的“伪失配”现象还有一个经验值得单独提一下。当我让无人机跟踪一个幅度较大的轨迹调度变量跑到网格边界附近时插值模型会偏向边界值内部模型和外部真实性的差距会增大。最典型的表现是控制器输出没有明显抖动但位置误差突然从0.1米级别跳到0.3米级别。这说明当前模型族的覆盖范围已经不够了。解决方式有两种一是把网格范围扩大二是把轨迹幅度缩小到网格覆盖范围内。我一直建议先按参考轨迹的预计包线来设计网格再去跑完整仿真而不是默认一个“足够大”的范围。网格太密、范围太大离线建模的时间还不算什么在线插值的负担和存储量都会增加得不偿失。5.5 从仿真到实际飞行的三点提醒这套方法在仿真里跑通之后如果计划迁移到真机有三件事提前想清楚调度变量的测量噪声。仿真里状态是干净的真机上速度估计会带噪声调度变量必须先做滤波否则LPV模型切换会非常敏感。模型失配的界限。LPV模型覆盖的是标称动力学真机上的电池电压下降、电机温度漂移、气动阻尼变化都不会进模型。MPC的鲁棒性有一定的余量但前提是不超出约束边界。求解周期的确定。Matlab里的quadprog在仿真环境没问题真机上建议把同一套QP问题导出成C代码并实测最坏情况下的求解时长。如果最坏情况超过控制周期就需要考虑缩短Np或换成嵌入式友好的求解器。我个人在实际操作中的体会是LPV-MPC真正难的地方不是MPC的公式推导而是“如何让预测模型在不同飞行条件下都够用”的工程判断。调度变量选什么、网格怎么铺、插值怎么平滑、约束怎么留余量这些决定仿真结果天花板的选择往往藏在细节里需要反复调试才会形成手感。如果你在Matlab里复现这套流程后发现误差比预期大先从调度变量的范围和滤波系数查起多半能找到原因。