做无人机轨迹跟踪有一阵子了最开始用的是常规的PID串级控制调出来效果也算能飞但一旦把参考轨迹换成三维空间里那种连续转弯、高度起伏比较剧烈的航线跟踪误差就变得很难看姿态还会出现明显的滞后。后来把方案改成 MPC模型预测控制配合 LPV线性变参数建模才算是把这个问题真正压下去。这篇就把这套基于 Matlab 的实现思路、模型推导要点和调试经验整理出来给正在做四旋翼轨迹跟踪研究或者准备复现类似控制方案的同学一个参考。先说清楚这套方案解决的核心问题是什么。四旋翼无人机在飞行过程中姿态和位置之间是强耦合的非线性关系用单一工作点线性化出来的模型做 MPC只能在平衡点附近的小范围内有效一旦轨迹大幅度变化模型失配就会导致预测不准。LPV 的思路是把非线性系统表示成一组随调度变量连续变化的线性模型簇再用 MPC 在每个时刻根据当前调度变量实时选取对应的预测模型这样既保留了线性预测控制的计算效率又能在更大包线内保持模型精度非常适合做三维轨迹跟踪这种需要频繁改变飞行状态的任务。1. 为什么是 MPC-LPV单一线性模型根本扛不住三维轨迹很多人在做无人机轨迹跟踪时第一步想到的就是在悬停点附近做线性化然后基于这个线性模型设计 LQR 或者 MPC。这个方法在二维平面小扰动的情况下确实够用小修小补地跟踪一条近乎直线的轨迹没什么大问题但放到三维轨迹里就完全不是一回事了。三维轨迹跟踪的难点在于无人机的运动状态一直在变。爬升时速度方向改变、转弯时滚转和俯仰角持续变化、加减速时推力和阻力不断重新配平这些动态特性会让系统的工作点沿着轨迹持续移动。如果你只在悬停点做一次线性化得到的 A 矩阵和 B 矩阵只代表那一个点的局部动态特性轨迹稍微离开这个区域模型误差就会迅速放大MPC 的预测输出就会偏离真实响应。我实际对比过两组仿真数据。用单一工作点线性化的 MPC 去跟踪一条包含螺旋爬升和俯冲段的参考轨迹在平飞段跟踪误差还能控制在十厘米以内但到了螺旋爬升段最大误差直接超过了半米而且误差出现明显的周期性振荡。换用 LPV 建模之后同一套 MPC 框架、同样的权重参数最大误差被压到了十五厘米以内。这个对比让我确认了一件事三维轨迹跟踪问题里模型结构比控制器本身的复杂程度更关键模型覆盖不了飞行包线再强的优化算法也救不回来。LPV 系统的优势就是它把非线性模型组织成一组带调度变量的线性系统簇调度变量可以是速度、姿态角、角速度这些反映当前飞行状态的关键量。你不用在每个时刻重新做非线性模型的 Jacobian 展开只需要根据调度变量的当前值在预先算好的线性模型之间进行插值或者直接在线更新矩阵就能得到一个足够准确的局部线性模型。这个特性让 LPV 非常契合 MPC 的滚动优化结构因为 MPC 本身就是在每个采样时刻基于当前模型做预测模型更新得越准预测就越可靠。从计算量的角度讲LPV 也占了很大便宜。全非线性 MPC 需要对非线性优化问题做迭代求解就算用高效的求解器在嵌入式平台上跑起来也非常吃力。而 LPV-MPC 的本质仍然是一堆线性矩阵运算加一个标准 QP二次规划问题计算量稳定可控实时性有保障。对于还要跑 Matlab 仿真验证算法的研究阶段来说这个特性尤其重要迭代速度会快很多。2. 四旋翼的动力学模型到底该怎么写成 LPV 形式做 LPV 建模之前先把四旋翼的数学模型梳理清楚。四旋翼的完整动力学分为位置环和姿态环两部分位置环描述无人机质心在三维空间中的运动姿态环描述机体坐标系相对惯性坐标系的转动。轨迹跟踪最关心的是位置环但位置环的控制输入实际上是姿态角所以两个环路必须联合建模。位置环的动力学方程可以直接从牛顿第二定律写出[ m\ddot{p} R(\Phi) F_T F_d mg ]其中 (p [x, y, z]^T) 是无人机在惯性系中的位置(m) 是质量(R(\Phi)) 是由欧拉角 (\Phi [\phi, \theta, \psi]^T) 构成的旋转矩阵(F_T) 是机体坐标系下四个旋翼产生的总推力(F_d) 是气动阻力(g) 是重力加速度。把旋转矩阵展开位置环的加速度就显式地依赖于滚转角和俯仰角[ \ddot{x} \frac{1}{m}(cos\psi sin\theta cos\phi sin\psi sin\phi) T ][ \ddot{y} \frac{1}{m}(sin\psi sin\theta cos\phi - cos\psi sin\phi) T ][ \ddot{z} \frac{1}{m}(cos\theta cos\phi) T - g ]注意这里的 (T) 是四个旋翼的总推力它的方向始终沿着机体坐标系的 (z_b) 轴。从这些方程可以看出位置加速度和姿态角之间存在明显的三角非线性关系这就是轨迹跟踪问题非线性的主要来源。在做 LPV 建模之前先定义好状态向量。我用的状态向量是[ x [x, y, z, v_x, v_y, v_z, \phi, \theta, \psi]^T ]控制输入选的是[ u [T, \dot{\phi}, \dot{\theta}, \dot{\psi}]^T ]这里的 (T) 是总推力(\dot{\phi})、(\dot{\theta})、(\dot{\psi}) 是期望的姿态角速率。之所以把姿态角速率作为控制输入而不是直接把姿态角当作输入是为了把系统的相对阶降下来让位置环和姿态环都能写进同一个线性模型里。实际控制器输出的还是推力以及姿态角姿态角速率在这里相当于中间变量到了仿真环节通过一个限幅器或者姿态控制器转换成最终的电机指令。把非线性项 (\sin\phi)、(\cos\phi)、(\sin\theta)、(\cos\theta)、(\sin\psi)、(\cos\psi) 这些统一用一组参数来表示就能得到 LPV 模型的雏形。具体做法是定义调度变量[ \rho [\phi, \theta, \psi, v_x, v_y, v_z]^T ]然后把状态方程写成[ \dot{x}(t) A(\rho(t)) x(t) B(\rho(t)) u(t) ]其中 (A(\rho)) 和 (B(\rho)) 随调度变量连续变化。注意这里不能简单地把 (\cos\phi) 直接塞进矩阵就完事因为 LPV 结构要求在状态模型的系数矩阵中调度变量不能和状态量相乘后再次形成非线性项也就是说需要保证系统是关于状态和控制输入仿射的。对于四旋翼这个模型位置方程中的姿态角是作为状态量直接出现的控制输入通过三角函数进入需要经过重新整理才能得到严格的 LPV 形式。一种常用的处理方法是做小角度近似把 (\sin\phi \approx \phi)、(\cos\phi \approx 1) 这种代换用在模型中。这样虽然会损失一些大角度下的精度但能大大简化模型结构方便构造 LPV 矩阵。对于轨迹跟踪来说一般飞行的姿态角都不会超过正负三十度小角度近似带来的误差完全在 MPC 鲁棒性可以接受的范围内。另一种精度更高的做法是引入扩展状态或者采用准 LPVquasi-LPV结构把调度变量设置为系统状态本身或其函数。quasi-LPV 的好处在于模型仍然是精确的不需要做小角度近似但代价是调度变量的变化率需要在 MPC 设计时作为额外约束来处理。我在仿真中采用的是准 LPV 形式这样模型精度高而调度变量就是直接可测的状态量也不需要额外的估计器。准 LPV 模型写出来之后还需要做离散化才能接入 MPC。离散化方法用零阶保持器ZOH采样时间我选的是 (T_s 0.05s)也就是 20Hz 的控制频率。对于四旋翼这种动态系统20Hz 足够覆盖主要的运动模态同时留给 MPC 优化求解的时间也充裕。每个采样时刻根据当前的调度变量 (\rho(t_k)) 计算 (A(\rho)) 和 (B(\rho))然后做离散化得到 (A_d) 和 (B_d)这个矩阵就直接给 MPC 滚动优化用。在 Matlab 里实现离散化这一步有个小技巧。先用符号表达式把 (A(\rho)) 和 (B(\rho)) 的完整形式推导出来然后把调度变量作为函数的参数传入在循环里数值化计算矩阵再调用c2d函数完成离散化。直接用数值方法在每个采样时刻重算矩阵也很快符号推导的价值在于方便检查和验证模型结构是否正确。初期做验证时建议先用符号推导把矩阵形式确定下来后面跑大仿真时再改成数值计算版本能快不少。2.1 调度变量的选择与覆盖范围调度变量的选择直接决定 LPV 模型对真实系统的逼近能力这也是我在实践里最花时间的一个环节。如果调度变量选少了光靠增益调度无法覆盖系统的非线性变化模型在某些飞行段仍然会失配。如果选多了模型矩阵的计算和管理复杂度上升MPC 在每个时刻需要更新的参数变多实时性会受影响。我在模型里最终选取了六个调度变量三个欧拉角 (\phi)、(\theta)、(\psi) 和三个线速度 (v_x)、(v_y)、(v_z)。欧拉角直接决定了旋转矩阵的取值是模型非线性最核心的来源。线速度则影响气动阻力项的系数在高速飞行时阻力不可忽略这个影响会变得明显。六个调度变量的组合起来基本覆盖了四旋翼在常规轨迹跟踪任务中的整个飞行包线。调度变量的取值范围需要提前界定。我根据实际飞行约束把滚转角限制在正负三十度俯仰角限制在正负三十度偏航角不做限制但按照轨迹需要连续变化。速度范围则是根据任务设定最大飞行速度 (2m/s)最大爬升速度 (1m/s)。这些范围在仿真中是作为约束写入 MPC 的同时也决定了 LPV 模型在什么范围内有效。如果实际任务要求更大的飞行包线调度变量的范围要相应扩大这时小角度近似就不再适用必须用准 LPV 的完整形式。2.2 模型验证开环响应对比是建模是否准确的唯一标准LPV 模型建好之后一定不要急着接入 MPC先做开环验证。我最常用的验证方法是把 LPV 模型和原始非线性模型放在同一个仿真环境下输入相同的推力指令和姿态角指令对比两者的状态响应曲线。如果 LPV 模型在正常飞行范围内的响应和原始非线性模型基本一致说明建模成立后面 MPC 的工作就是在这个准确的模型基础上展开。有一个很容易被忽略的细节开环验证时不要只验证平飞这一个姿态。故意给模型一个较大的姿态角指令和速度指令看 LPV 模型还能不能跟上非线性模型。如果偏差明显说明调度变量选取或者模型简化出了问题回去检查之前先确认一下是不是小角度近似带来的误差超出预期。如果确实是近似误差太大就要把假设放松采用更高精度的模型结构。我自己的经验是LPV 模型和真实非线性模型的开环响应误差控制在百分之五以内MPC 的跟踪效果才会有保证。这个指标不是一个理论推导出来的标准而是我在多组实验中的经验总结大家可以作为参考。3. MPC 在三维轨迹跟踪中的具体设计预测模型、代价函数与约束MPC 的核心思想是在每个采样时刻基于当前模型预测未来一段时间内的系统输出然后通过求解一个带约束的优化问题得到最优控制序列只执行第一个控制量下一时刻重复这个过程这就是滚动优化。在轨迹跟踪场景里MPC 的任务是让预测位置轨迹尽量贴近参考轨迹同时保持控制量平滑、不超出执行机构的物理限制。具体来说需要设计三个要素预测模型、代价函数、约束条件。预测模型直接使用上一节建立的 LPV 离散模型。设当前时刻为 (t_k)预测时域为 (N_p)控制时域为 (N_c)那么在预测时域内的状态演化可以写成[ x(ki1|k) A_d(k) x(ki|k) B_d(k) u(ki|k) ]注意这里的 (A_d(k)) 和 (B_d(k)) 是当前时刻根据调度变量算出来的固定矩阵在预测时域内保持不变。这种做法的假设是调度变量在未来一小段时间内变化不大这在采样周期为 50ms 的情况下是合理的。如果飞行状态变化特别剧烈可以增加一个调度变量的预测步骤在每个时刻先根据参考轨迹预测未来的调度变量变化然后更新预测模型。这样做会显著增加计算负担我实测下来对于常规轨迹跟踪、采样频率 20Hz 的情况固定调度变量已经足够所以默认方案里没有加这个预测扩展。代价函数的设计直接决定跟踪性能和控制品质。我用的代价函数包含三项[ J \sum_{i1}^{N_p} | \hat{y}(ki|k) - y_{ref}(ki|k) |Q^2 \sum{i0}^{N_c-1} | u(ki|k) |R^2 \sum{i0}^{N_c-1} | \Delta u(ki|k) |_{S}^2 ]第一项是预测轨迹与参考轨迹之间的位置误差(Q) 是权重矩阵对三个方向分别设置权重。第二项是控制量本身的幅值惩罚(R) 是控制幅值权重。第三项是控制增量惩罚(S) 是控制增量权重用来抑制控制量的剧烈变化可以理解为四旋翼电机响应和姿态环执行能力的平滑度约束。权重矩阵的调参经验后面会专门说这里先给出一个初始参考值[ Q diag(10, 10, 15), \quad R diag(0.01, 0.01, 0.01, 0.01), \quad S diag(0.1, 0.1, 0.1, 0.1) ]位置权重比控制幅值权重高两个数量级这样才能保证跟踪精度优先。(z) 轴的权重比其他两个轴大是因为多旋翼在垂直方向上的推力和重力是直接耦合的响应上比水平方向迟钝一些权重加大可以补偿这个差距。R 和 S 的相对大小决定了控制量是偏激进还是偏保守这两个值需要放到仿真里不断调。约束条件也是 MPC 设计中不能忽视的一部分。我设置了以下几组约束总推力上限(T_{min} \leq T \leq T_{max})这是由电机饱和特性决定的姿态角限幅(|\phi| \leq 30^\circ)(|\theta| \leq 30^\circ)姿态角速率限幅(|\dot{\phi}| \leq 60^\circ/s) 等控制增量限幅约束每个时刻的控制量变化率在求解 QP 问题时这些约束会以不等式形式加入优化问题。约束设置过紧会导致优化问题无解或者跟踪性能严重下降设置过松又起不到保护执行机构的作用。我建议先把约束放开跑一遍仿真看一下实际控制量的峰值和变化率然后基于这些数据把约束设置在略高于真实需求的位置这样既能保证安全又不会过度限制控制器的发挥空间。求解 QP 问题可以选择 Matlab 自带的quadprog也可以接第三方求解器比如OSQP或者Gurobi。用quadprog的好处是 Matlab 自带、不用额外配置缺点是求解速度一般。在 20Hz 采样率下quadprog处理我这里规模的问题状态 9 维、控制 4 维、预测时域 20 步耗时大约十毫秒以内完全够用。等后面要做实时实验再换 OSQP这类嵌入式友好型求解器在代码生成方面有更好的支持。% 求解当前时刻的优化问题 [H, f, Aineq, bineq] buildQPProblem(x0, Ad, Bd, rho, refTraj, Q, R, S, Np, Nc, constraints); u_opt quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options); u_actual u_opt(1:nu); % 只取第一个控制量有一个需要特别提醒的地方QP 求解器的初始点不要随便给上一时刻的最优解是当前时刻最好的 warm start可以显著减少迭代次数提升实时性。在 Matlab 中做仿真时这个优化可能感觉不明显但换成嵌入式实时环境时这个差别会非常显著习惯了 warm start 之后做移植会顺利很多。4. Matlab 代码实现从模型导入到 MPC 核心循环的完整骨架整个 MPC-LPV 轨迹跟踪的 Matlab 代码实现我按照模块化的思路分成几个部分每个模块独立测试最后串联成完整的仿真闭环。完整的目录结构大致如下simulation/ ├── model/ │ ├── quadrotor_dynamics.m # 非线性四旋翼模型用于真实仿真 │ ├── lpv_model.m # LPV 模型矩阵计算 │ └── generate_reference.m # 三维参考轨迹生成 ├── mpc/ │ ├── mpc_solve.m # 求解 QP 问题 │ ├── build_qp_problem.m # 构建 QP 矩阵 │ └── constraint_setup.m # 约束定义 ├── utils/ │ └── discretize_lpv.m # LPV 模型离散化 └── main_simulation.m # 主仿真脚本模型文件和 MPC 求解文件分离的好处是后面如果要切换成其他控制方法做对比只需要替换 mpc 目录下的核心函数模型部分完全不需要改动。做研究对比时的可维护性很重要不然每次换方法都要改一大片代码容易引入低级错误。4.1 LPV 模型矩阵计算的代码逻辑lpv_model.m的输入是当前状态输出是连续时间下的 (A(\rho)) 和 (B(\rho)) 矩阵。核心代码逻辑如下function [A, B] lpv_model(x) phi x(7); theta x(8); psi x(9); A zeros(9, 9); % 位置微分与速度的关系 A(1, 4) 1; A(2, 5) 1; A(3, 6) 1; % 速度微分与姿态角的耦合 A(4, 7) disp_force_x / m; % 由 cos/sin 函数计算得到 A(4, 8) disp_force_x / m; % ... 其他项 A(5, 7) disp_force_y / m; B zeros(9, 4); B(4, 1) (cos(phi)*sin(theta)*cos(psi) sin(phi)*sin(psi)) / m; B(5, 1) (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) / m; B(6, 1) cos(phi)*cos(theta) / m; B(6, 4) -g; % 重力补偿项 % ... 姿态环相关项 end这里有几个细节需要特别留意。第一位置环状态方程中 (x)、(y) 和 (z) 的导数直接等于 (v_x)、(v_y)、(v_z)体现在 (A) 矩阵的对应位置为 1。第二速度方程中姿态角的耦合项需要把三角函数的具体数值算出来填进去而不是保留符号变量。第三(B) 矩阵中推力对应的那一列必须包含姿态角的三角函数这是把推力方向从机体系转到惯性系的关键。4.2 MPC 预测矩阵的构建技巧MPC 求解的关键一步是把预测时域内的所有状态表达式写成统一的矩阵形式这样可以一次性表达整个预测过程交给 QP 求解器处理。设 (X [x(k1|k); x(k2|k); ...; x(kN_p|k)])(U [u(k|k); u(k1|k); ...; u(kN_c-1|k)])通过递推可以得到[ X \Phi x(k) \Psi U ]其中 (\Phi) 由 (A_d) 的幂次构成(\Psi) 由 (A_d) 和 (B_d) 的组合构成。这两个矩阵的构建在 Matlab 中可以用循环完成但要注意矩阵维度的匹配。Phi zeros(Np*nx, nx); Psi zeros(Np*nx, Nc*nu); temp Ad; for i 1:Np Phi((i-1)*nx1:i*nx, :) temp; if i Nc for j 1:i Psi((i-1)*nx1:i*nx, (j-1)*nu1:j*nu) Ad^(i-j) * Bd; end end temp temp * Ad; end有了 (\Phi) 和 (\Psi) 之后代价函数就可以写成关于 (U) 的二次型QP 问题的 (H) 矩阵和 (f) 向量可以直接推导出来。这个推导过程尽量写成独立的函数方便后面修改权重矩阵或者预测时域时只需要改参数、不动代码结构。4.3 参考轨迹的生成方式三维轨迹跟踪的仿真需要一条有代表性的参考轨迹。我常用的生成函数支持螺旋线、8字曲线、自定义航路点插值三种模式。螺旋线适合验证连续爬升和转弯性能8字曲线适合测试横侧向机动航路点插值适合模拟实际任务中的点对点飞行。% 螺旋上升轨迹 t 0:Ts:T_total; R 2.0; % 螺旋半径 H 1.5; % 总爬升高度 omega 0.5; % 角速度 ref_x R * cos(omega*t); ref_y R * sin(omega*t); ref_z H * t / T_total;参考轨迹生成后还要生成对应的参考速度。MPC 的代价函数中如果只跟踪位置不跟踪速度会出现一种现象位置误差很小但速度波动大控制量频繁变化。所以我在代价函数里额外加入了速度参考项把参考速度也一起跟踪这样轨迹跟踪的平滑性会好很多。4.4 主仿真循环的完整流程主仿真脚本的循环逻辑是所有模块的粘合剂。每个采样时刻的流程包括更新调度变量、计算 LPV 模型矩阵、离散化、构建 QP 问题、求解、施加第一个控制量、更新非线性模型状态、记录仿真数据。这里贴出核心代码骨架x x0; % 初始化状态 u zeros(4, 1); for k 1:size(ref_traj, 1) % 1. 当前状态提取 x_curr x(:, k); % 2. 计算 LPV 模型并离散化 [Ac, Bc] lpv_model(x_curr); [Ad, Bd] discretize_lpv(Ac, Bc, Ts); % 3. 提取当前参考轨迹段 ref_segment ref_traj(k : kNp, :); % 4. 构建 QP 并求解 [H, f, Aineq, bineq, lb, ub] build_qp_problem(Ad, Bd, x_curr, ref_segment); u_opt quadprog(H, f, Aineq, bineq, [], [], lb, ub); % 5. 提取第一个控制量并施加到系统 u_actual u_opt(1:4); u(:, k1) u_actual; % 6. 用非线性模型模拟下一步状态 x(:, k1) quadrotor_dynamics(x_curr, u_actual, Ts); end这段循环对性能的影响点在于第 2 步和第 4 步。第 2 步每个时刻都要重算模型矩阵并做离散化如果直接调用c2d会比较慢建议预分配矩阵空间或者对离散化函数做向量化处理。第 4 步的 QP 求解是最大的时间开销如果使用quadprog设置好优化选项、启用 warm start 可以显著提速。这些细节看似微小实际仿真时长从几十秒到几百秒的差别往往就出在这些地方。5. 仿真调参与结果评价权重矩阵、预测时域和约束的整定心得MPC 的参数整定是这个项目里最耗时、也最考验经验的环节。参数之间相互影响只调一个参数很难收敛到好的结果。下面按优先级从高到低说明我常用的调参顺序和分析方法。5.1 先调位置权重锁定跟踪精度的基调位置误差权重 (Q) 是最先要确定的参数因为它直接决定控制器的核心行为。我把 (Q) 的初始值设成对角矩阵三个方向的比例按照任务的优先级设置。如果任务是精确跟踪三维轨迹(x)、(y)、(z) 三个方向应该分配相近的权重但 (z) 方向我会稍微高一些补偿推力重力耦合带来的迟钝。调 (Q) 的方法很简单先把其他参数放到一个保守的水平然后只改变 (Q) 的值观察位置误差的变化。误差太大就加大 (Q)误差满足要求但控制量抖动明显就适当减小 (Q)。有一个比较反直觉的现象是(Q) 并不是越大越好。(Q) 太大会让控制器对误差的反应过于激进产生控制量振荡反而导致误差增大甚至引发姿态饱和。一个非常有效的辅助工具是绘制误差的功率谱密度图。如果误差中某个频率成分特别突出说明控制器在该频率附近的响应有问题顺着频率可以反推是哪部分参数设置不当。比如高频抖动很常见的原因是位置权重和姿态角速率约束配合不当这时需要检查约束是否过紧或者控制增量惩罚是否过小。5.2 预测时域与控制时域的匹配关系预测时域 (N_p) 是 MPC 最重要的结构性参数。(N_p) 反映了控制器眼光有多远太短了预测不到轨迹的弯道太长了不仅计算量大还会引入过多未来的不确定性。我测试下来 (N_p) 取 20 到 30 之间比较合适对应 1 到 1.5 秒的预测范围。四旋翼的轨迹跟踪任务通常不会有过快的动态要求1 秒出头的预测范围足够覆盖主要的机动需求。控制时域 (N_c) 表示优化变量中自由控制量的个数一般远小于 (N_p)。(N_c) 越大控制器越自由但计算量也越大。我常用的配置是 (N_c 5) 到 (8)再往上对性能的提升非常有限反而有可能让控制量过度变化。一个值得尝试的做法是把 (N_c) 取小但把控制增量惩罚 (S) 也减小让前几步的控制动作更果断剩下几步交给后续的滚动优化去修正。我在一个共同指导的项目里让 A 同学对比了 (N_p 10)、20、30 三组的跟踪效果结果很有意思(N_p10) 时跟踪螺旋爬升轨迹明显滞后误差有周期性波动(N_p20) 时误差显著下降(N_p30) 时误差没有进一步减小但计算时间增加了大约四成。这个对比说明预测时域并不是越长越好存在一个性价比最高的区间超出之后多出的计算量是纯浪费。5.3 约束的松紧平衡约束设置是最容易走极端的一环。我自己早期犯过的一个错误是把推力上限设置得过于贴近实际最大推力结果优化问题经常无解仿真直接崩溃。后来才明白约束要留一点余量不是在边缘线上跳舞。实际做的时候先把约束设得很松跑完看控制量的实际分布然后根据分布把约束收紧到比真实需求略高一点的位置这样既不会触发无效约束又能保证飞行安全。姿态角约束的松紧会直接影响跟踪中的姿态饱和现象。如果轨迹要求无人机快速转向但姿态角约束过紧位置环就会够不到参考轨迹误差持续累积。这种情况下有两种处理方式一是适当放松姿态角约束二是降低参考轨迹的机动性。从实际工程角度看应该先检查参考轨迹是否合理再做约束放松的决定不能无条件加大约束。控制增量的约束也很重要它决定了输出指令的平稳度。四旋翼的执行机构是电机电机转速不能瞬间跳变控制增量约束就对应了这个物理限制。设置太松会导致指令剧烈变化电机响应跟不上形成高频振荡设置太紧则会让控制器反应迟钝。我通常会先不加这个约束跑一次记录控制增量的标准差然后以大约三倍标准差为限设置约束这个经验值在多数情况下都能得到不错的效果。5.4 结果评价不能只看 RMSE做轨迹跟踪研究的同学很容易只盯着位置 RMSE 这一个指标。这个指标当然重要但只有它远远不够。我实际评估一套 MPC-LPV 方案时至少会看三组数据位置追踪误差的时间序列图和三维轨迹对比图控制量的时间序列重点看是否频繁饱和、是否抖动姿态角的时间序列重点看是否超出约束、是否出现振荡一个常见的情况是位置 RMSE 很小但控制量一直在高频抖动姿态角的波动幅度很大。这种情况说明控制器在硬顶误差长期运行会对执行机构造成损坏真机飞行几乎不可能用这种控制律。所以我在仿真阶段就要求自己必须同时把位置误差、控制量质量、姿态角变化三方面的数据都放在同一张图上检查只有三个维度都正常方案才算真正靠谱。6. 从仿真走向实飞前的几个隐患调度变量跳变与模型失配理论上 MPC-LPV 在仿真中表现很好但离真机应用还有一段距离有几个隐患必须在仿真阶段就要提前意识到并采取措施。最大的隐患是调度变量的跳变问题。LPV 调度变量来自状态反馈而状态估计在真实系统中会带有噪声噪声会导致调度变量在相邻采样时刻之间发生快速跳变进而导致模型矩阵 (A(\rho)) 和 (B(\rho)) 也快速变化。这种模型矩阵的跳变会让 MPC 的预测输出变得不稳定反映在控制量上就是高频抖振。缓解这个问题的方法主要有两种。一种是在调度变量进入模型计算之前加低通滤波把高频噪声滤掉。另一种是在 MPC 的预测模型中加入调度变量的变化率约束限制模型矩阵在相邻时刻的变化幅度。第一种方法实现简单效果也足够好我在仿真中加了一个截止频率为 5Hz 的一阶低通滤波器来处理调度变量模型矩阵的跳变明显被抑制控制量的抖动幅度也减小了很多。第二个隐患是模型失配主要是未建模动态带来的误差。LPV 模型在建模过程中做了不少简化比如忽略了气动阻力的非线性、忽略了电机动态的延迟、忽略了柔性结构的影响。在仿真中这些被忽略的部分用的是理想模型但在实际飞行中它们会真实存在并干扰控制性能。增强鲁棒性的一个有效手段是在 MPC 的输出端加一个前馈补偿项或者引入干扰观测器来估计未建模动态然后在控制量中将其补偿掉。我在某一次的仿真对比中特意加入了参数不确定性比如把模型中的质量和转动惯量与真实值偏差百分之二十然后观察 MPC 的表现。结果位置误差比标称情况下增大了一倍左右但整体跟踪趋势没有崩说明 LPV-MPC 在这一类参数偏差下具备一定的鲁棒性。如果偏差继续加大到百分之三十以上控制效果就开始明显稳定恶化这时候单纯靠 MPC 的反馈已经不够必须加入自适应或者鲁棒控制的机制。第三个隐患是 QP 求解失败的情形。当约束设置不合理或者调度变量出现异常值时QP 求解器可能会返回无解或者不收敛的标志如果不做处理直接结束程序整个仿真就崩溃了。我在代码里加了求解失败的保护逻辑如果 QP 求解器报告问题就直接沿用上一时刻的最优控制量并在日志中记录一次fallback 事件。通过统计 fallback 次数可以间接评估约束设置的合理性。一个健康的仿真中fallback 次数应该为零一旦出现就需要回头检查参数。7. 扩展思路从 MPC-LPV 还能往哪些方向走MPC-LPV 这个方案本身已经能解决三维轨迹跟踪的大部分问题但如果想继续往深了做有几个方向和这个框架天然兼容延展起来比推倒重来要顺得多。第一个扩展方向是显式地处理调度变量的预测。目前方案假设预测时域内调度变量不变这在机动平缓时没有问题。如果换成高速机动、大角度姿态变化的场景这个假设会带来明显的预测误差。改进方法是在预测时域内建立调度变量的参考轨迹然后在构建 QP 问题时把每个时刻对应的模型矩阵都算出来形成一个时变的预测模型。带来的计算量增加是可接受的但实现复杂度会上一个台阶。第二个方向是和学习方法的结合。LPV 建模的精度取决于模型结构而真实系统的未建模动态总是存在的。可以考虑用高斯过程回归或者神经网络在线学习模型残差把学到的东西叠加到 LPV 模型的预测输出上形成一个物理模型数据模型的混合预测框架。这个方向我在一个模拟项目里做过初步验证效果比纯物理模型好不少但稳定性和理论保证方面还有很多问题要解决属于前沿探索性的工作。第三个方向是编队控制。如果多架无人机要协同跟踪可以建立在每个单机 MPC-LPV 控制器的基础上在代价函数中加入队形保持的耦合项通过分布式优化实现编队。每架飞机只需要和邻居交换预测的姿态和位置信息计算量增加可控而且每架飞机的控制器内部结构不需要大幅改动这个扩展的工程难度相对友好。从我个人这段时间做 MPC-LPV 四旋翼轨迹跟踪的体会来说这个方案的定位很适合作为从线性控制向非线性控制过渡的中间站。它比纯线性控制能覆盖更大的飞行包线又比全非线性 MPC 简单、可控、容易调试。如果说做基本功的时间成本LPV 建模、模型验证、MPC 参数整定这几块确实都不轻松但每一步踩下去都能让人对四旋翼动力学和预测控制的理解深一层投入的时间完全值得。如果你正在做类似的方向建议先把非线性模型和 LPV 模型的开环对比做扎实再碰 MPC这一关过了后面就是一马平川。