先说一个我观察到的现象网上下载的四旋翼MPC轨迹跟踪工程包不少Matlab脚本加Simulink模型全套都有但真正能自己跑出稳定8字形轨迹的很少。大部分人拿到代码按下运行飞机不是起点抖几下落下来就是飞到一半发散去最后只能把功劳归咎于MPC参数没调好。其实问题往往不是MPC理论本身而是模型搭建、内外环接口、调度变量处理这些细节没理清。这篇文章我想把位置姿态双闭环线性参数变化MPC控制8字形轨迹这条技术路线完整拆开讲一遍。它适合正在做无人机轨迹跟踪仿真、需要复现8字形飞行、或者课程课题里被LPV-MPC这几个字卡住的同学。我会从模型为什么要这样建、MPC为什么要分两环、LPV调度变量到底怎么选一直讲到Matlab代码骨架和实际调参时踩过的坑。目标是让任何有一定控制理论基础的人能照着这条思路把仿真自己搭起来而不是当个代码搬运工。1. 为什么8字形轨迹必须用位置姿态双闭环架构1.1 8字形轨迹对控制器提出的真实挑战先看8字形轨迹本身有什么特别的。很多教程喜欢用圆轨迹或者直线轨迹做演示因为曲率恒定或为零控制器压力小。但8字形轨迹不一样它的曲率是连续变化的而且拐弯处方向急剧反转飞行器需要持续改变横滚角和俯仰角来产生足够的水平加速度。以一个常见参数方程为例参考位置可以写成x_ref(t) A * sin(omega * t) y_ref(t) A * sin(2 * omega * t) z_ref(t) h其中A是幅度omega决定飞行速度。当t让sin项和sin(2t)项的组合发生变化时轨迹在xy平面划出一个躺倒的8字。这个轨迹的特点是方向变化快、加速度方向频繁切换、位置回路和姿态回路强耦合。控制器如果只有简单的PID往往在拐弯处出现明显的跟踪滞后而MPC的优势在于能预判未来几拍的需求提前输出控制量。但这还不够。8字形轨迹中飞行器始终处于倾斜姿态横滚角和俯仰角不是小量。此时四旋翼的位置动态和姿态动态互相影响如果只用一组固定的线性模型去设计MPC效果会很差。这就引出了两个问题模型怎么描述这种姿态变化导致系统动态变化的特性以及控制器怎么分层。1.2 单层MPC为什么不现实有人会想既然MPC这么强为什么不把位置、速度、姿态角、角速度全部塞进一个MPC里用一个14维状态向量直接跟踪参考轨迹理论上是可行的通用预测控制(MPC/NMPC)就能做但这在实际仿真和工程落地中有几个现实障碍。第一个障碍是预测时域与采样时间的冲突。姿态回路需要快速更新采样时间通常在0.01秒量级位置回路需要看远的未来预测时域可能要拉到2到3秒。如果单层MPC采样时间取0.01秒预测时域取100步那么决策变量维度会爆炸。即使Matlab能算每个控制周期内QP求解的实时性也难以保证。第二个障碍是系统非线性的处理。四旋翼的位置回路本质上是平移加旋转耦合在非线性轨迹上单一线性模型根本不够用。如果上非线性MPC优化问题的规模更大对初值敏感仿真时也更容易发散。第三个障碍更实际双闭环结构可以利用时间尺度分离原则把快回路和慢回路分开设计、分开调试出现问题时能定位到具体环节。单层MPC一旦跟踪效果不好你根本不知道是位置预测偏了还是姿态执行不到位。所以在轨迹跟踪控制里内外环加MPC的组合是更加成熟、可维护的方案。1.3 双闭环时间尺度分离的设计逻辑双闭环的思路很直接外环位置回路输出期望的姿态角和总拉力内环姿态回路负责快速跟踪这些期望值。位置回路关心的是飞机该往哪个方向加速姿态回路关心的是飞机怎么摆出这个姿态。二者通过姿态角参考接口衔接。时间尺度分离是这种结构的理论基础。姿态回路的时间常数比位置回路小一个数量级以上所以外环可以把内环近似看成理想的执行器——我给一个期望姿态角内环能很快跟上。反过来内环设计时不需要过多考虑位置回路它只负责把角速度控制好。这样两个MPC可以独立设计外环采样周期取大一点内环取小一点计算负担分散整机调试难度也降低。在后面的章节里我会一步步展开但先记住设计目标外环MPC负责把位置误差和速度误差拉小输出期望滚转角、俯仰角和总升力内环MPC负责跟踪姿态指令输出三个轴的力矩。8字形轨迹能否跟踪好80%的功劳在外环的设计20%的坑在内环的执行。2. 先理顺四旋翼数学模型从牛顿欧拉到LPV表示2.1 位置回路模型三个轴上的积分器链与推力分解设计MPC之前先得把被控对象的数学模型整理清楚。四旋翼的位置回路本质上是牛顿第二定律在惯性系下的投影。忽略空气阻力时位置方程可以写成m * ddot(P) F_total * R * e3 - m * g * e3其中P是位置向量F_total是总拉力大小R是机体系到惯性系的旋转矩阵e3是竖直单位向量。这个方程说明一件事无人机往前飞不是靠往前推实现的而是靠机体倾斜让总拉力的方向指向前方从而产生水平方向的加速度分量。加速度的来源是推力方向的改变所以外环MPC的输出表面上是期望加速度实际执行靠的是期望姿态角期望拉力。在悬停点附近做小角度线性化时旋转矩阵近似为常值矩阵位置回路退化成三个解耦的二阶积分器。这种情况下线性MPC就够用。但8字形轨迹上横滚角和俯仰角经常达到20到30度甚至更大此时必须考虑姿态角变化对位置加速度分配的影响。因此在位置回路MPC的预测模型里至少要把期望加速度到期望姿态角的转换这个环节正确处理。多数工程做法是先由MPC求出期望加速度向量再通过姿态解算公式反解出对应的期望横滚角、俯仰角和总拉力。2.2 姿态回路模型内环响应要足够快且能应对角速度耦合姿态回路的模型相对复杂一些。刚体姿态动力学可以写成J * dot(omega) -omega × (J * omega) tau其中omega是机体角速度J是转动惯量矩阵tau是控制力矩。这里有一个经常被忽略的项omega × (J * omega)也就是陀螺力矩。大角速度机动时这项的贡献不容小觑。如果仿真里做大幅度转弯而内环MPC模型里没有这一项控制器就会为了补偿而输出抖动。姿态运动学可以用欧拉角或者四元数描述。在MPC预测模型里如果我们限制姿态角变化范围不太大比如滚转角不超过30度用欧拉角表示是直观且够用的。小角度假设下欧拉角速率近似等于体轴角速度姿态回路也退化成三个近似解耦的通道。但同样地8字形轨迹上这个假设会失效尤其是偏航角在持续变化时通道之间会出现明显的交叉耦合。这就是姿态环需要LPV-MPC的深层原因系统的状态方程系数矩阵会随着姿态角变化而变化用一组固定的A和B矩阵无法准确描述大范围机动下的系统动态。2.3 为什么要换成线性参数变化视角线性参数变化(LPV)模型的思路可以这样理解把非线性系统看成一族线性系统每个工作点对应一组线性模型系统的实际动态由当前工作点决定而工作点由一组调度变量来刻画。调度变量一般是可测或可计算的状态量比如当前姿态角、当前总推力、当前空速等。对四旋翼来说最自然的调度变量是姿态角和总拉力。为什么因为位置回路的动态主要受推力方向和重力方向的夹角影响姿态回路的动态主要受当前角速度和转动惯量张量影响。推力不同角加速度对控制力矩的响应快慢也不同。把这些参数纳入调度变量后模型的精度显著提升。具体到状态空间形式LPV模型可以写作dot(x) A(σ(t)) * x B(σ(t)) * u其中σ(t)是调度向量。在每个采样时刻MPC根据当前的σ(t)选取或在预计算的工作点之间插值模型矩阵然后构建预测模型和QP问题。这个随着工作点变化而切换模型的机制就是LPV-MPC与普通线性时不变MPC的本质区别。调度变量不需要很多通常一到两个核心量就够了太多反而增加在线计算负担和插值误差。3. LPV-MPC设计中的三个关键决策3.1 调度变量选什么不是越多越好我在实际搭建时第一个困惑就是调度变量到底选哪些。一开始想把姿态角、角速度、总推力全部当成调度变量结果模型矩阵搜了一堆在线插值逻辑变得很复杂控制效果却没有显著提升。后来想明白了调度变量要选对系统动态影响最大、变化缓慢、且容易获取的量。对姿态回路来说总推力F_total是首选调度变量之一。因为当总推力不同时同样的控制力矩产生的角加速度响应速度不一样。另一个调度变量是当前的横滚角和俯仰角——它们决定了旋转矩阵的瞬时状态和陀螺力矩的大小。至于角速度变化太快直接把它作为调度变量会让模型矩阵剧烈跳变反而不利于MPC的预测稳定性更适合放在状态里处理。一个实用的原则是先做包线分析。在设定的8字形轨迹飞行包线内记录飞行过程中姿态角、推力的范围然后在这个范围内选取几个典型工作点做线性化。工作点之间用线性插值就够不需要无数个点。通常推力取3到4个点、姿态角取2到3个点组合起来10个以内的模型矩阵已经能覆盖绝大部分飞行场景。3.2 预测模型离散化与QP问题的构建MPC的设计核心是把连续模型离散化然后在每个采样时刻求解一个带约束的二次规划(QP)问题。离散化我推荐用零阶保持器。采样周期Ts内认为调度变量σ保持不变也就是每个控制周期内系统是线性时不变的这样离散化后的状态转移矩阵可以根据当前σ在线计算。具体到Matlab可以用c2d函数来获取连续模型对应的离散模型再把离散模型转换成预测矩阵。构建QP问题时每个控制周期内要做几件事根据当前状态x0比如当前位置、速度和期望姿态误差构建未来N步的预测方程把状态序列表示成初始状态和输入序列的线性组合把代价函数写成J (U - U_ref) * H * (U - U_ref) 松弛项其中U是控制序列把输入约束和状态约束写成不等式A_ineq * U b_ineq调用quadprog或OSQP求解。算法实现时最省事的方式是预先构建预测矩阵的稀疏结构在线只更新与当前调度变量相关的部分。Matlab自带的MPC Toolbox也能直接创建LPV控制器但对LPV机制的理解是必要的否则出了约束冲突或数值问题很难定位是参数问题还是模型矩阵构造问题。一个值得注意的点是松弛变量。仿真开始阶段状态通常不是参考轨迹的起始点或者约束不可行如果没有松弛变量QP问题可能直接无解整体仿真就会中断。加上松弛变量之后即使短暂违反约束控制器也能平滑过渡到可行区域。传统上人们会在代价函数里加一项ρ * ε²这个ρ既是约束罚权重也是调试中很有用的旋钮。3.3 约束、代价函数与松弛变量的处理策略每个MPC都会有代价函数设计。双闭环架构下外环和内环的代价函数权重需要分开设置。外环位置回路状态误差是位置误差和速度误差控制量是期望姿态角和拉力。我通常把位置误差权重设成速度误差权重的几倍以上因为8字形轨迹对位置精度要求高而速度误差已经被位置导数和参考速度隐含约束了。控制量权重不要取得太大否则控制器过于保守拐弯处跟不上轨迹但太小又会出现期望姿态角长时间饱和的问题。这里需要在仿真里反复试没有一劳永逸的公式。内环姿态回路状态误差是姿态角误差和角速度误差控制量是三个轴的力矩。姿态回路一定要快且稳所以姿态角误差权重取较高值角速度权重次之控制力矩的惩罚适当小一些保证内环响应速度足够快。如果内环响应太慢外环给的姿态角参考会产生持续滞后最终表现为位置跟踪的整体滞后。约束方面四旋翼主要的物理约束是总拉力有上下限电机饱和三轴力矩也有最大限制姿态角需要限制在一定范围内防止控制器索要不切实际的姿态角。此外我还会给外环的期望姿态角变化率加一个隐式约束——通过控制量权重大小来控制它别变化太快而不是直接硬限幅这样轨迹更平滑。状态约束要注意一点8字形轨迹经过大曲率段时位置误差在实际中的需求非常紧张如果把位置误差硬约束设置过小会频繁触发约束激活导致QP求解时间变长甚至无解。如果做的是纯仿真可以适当放宽如果是后续移植到真机才需要严格收紧。4. Matlab仿真落地的完整流程与代码骨架4.1 总体程序架构与文件划分写Matlab代码前先把工程文件划分清楚。我习惯这样组织project/ ├── main_simulation.m % 主仿真脚本 ├── trajectory_ref.m % 8字形参考轨迹生成 ├── quadrotor_model.m % 非线性四旋翼模型仿真被控对象 ├── mpc_outer_position.m % 外环位置LPV-MPC ├── mpc_inner_attitude.m % 内环姿态LPV-MPC ├── attitude_solver.m % 期望加速度到期望姿态角的解算 └── visualize_results.m % 结果可视化与误差统计main_simulation.m负责整体流程初始化参数、循环递推、在每个采样周期调用两个MPC控制器和四旋翼模型。不用Simulink而用纯脚本的好处是调试方便每一行命令都能断点检查尤其当你需要调试MPC内部的QP问题时脚本比Simulink直观得多。4.2 8字形参考轨迹的生成与参数选择轨迹生成很简单但参数选择有讲究。参考轨迹我采用omega 0.5; % 角频率单位rad/s A 4; % 轨迹幅度单位m h 2; % 期望高度单位m t 0:dt_sim:t_final; p_ref [A * sin(omega * t); A * sin(2 * omega * t); h * ones(1, length(t))];这里omega、A、h三个参数直接决定了轨迹的难度。如果A太大或者omega太大轨迹需要的向心加速度很大对应的期望倾斜角会超过姿态约束仿真很容易发散。建议从A2、omega0.3开始确保最大期望倾斜角小于20度再逐步加大难度。参考速度可以由位置指令差分得到也可以用解析式求导解析式更平滑。有一个容易忽视的点8字形在中心交叉处y方向的加速度方向会突然反转。如果参考速度在这一点不连续控制器会受到一个很大的瞬态冲击。因此轨迹参数方程的时间导数必须连续单纯用线性插值产生的点列会产生阶梯状速度导致MPC不断修正参考信号表现就是轨迹跟踪毛刺多。4.3 外环与内环的接口设计从期望加速度到期望姿态角两个MPC之间的接口是整个仿真最容易写乱的地方必须一步到位说清楚。外环MPC的输出我建议设计为期望惯性加速度a_des而不是直接输出姿态角。原因是位置回路的动态在惯性系下更直观、更容易设计权重。得到a_des之后再用姿态解算公式把它翻译成期望姿态角。具体解算公式如下% 期望加速度为 a_des [ax; ay; az] % 期望偏航角 psi_ref 由轨迹规划给定 % 期望推力大小 F_des m * norm(a_des [0; 0; g]); % 期望横滚角 phi_des atan2(ax * sin(psi_ref) - ay * cos(psi_ref), az g); % 期望俯仰角 theta_des atan2(ax * cos(psi_ref) ay * sin(psi_ref), az g);这段公式的意义是总推力方向必须与期望加速度方向对齐同时力矩必须平衡重力。如果换成直接输出姿态角的方案外环模型就掺入了姿态解算的非线性LPV模型更难写所以推荐加速度接口显式解算的方式。内环姿态MPC接收phi_des、theta_des、psi_ref作为参考输入输出体轴力矩tau_x、tau_y、tau_z再通过电机的混控矩阵分配到四个电机。混控矩阵其实就是一个固定的线性变换由四旋翼的臂长、扭矩系数等决定在悬停附近是常值。4.4 外环MPC的核心代码骨架这里给出外环MPC的核心伪代码不是完整代码但足以帮助你理解流程function [F_des, phi_des, theta_des] mpc_outer_position(p_now, v_now, p_ref_seq, v_ref_seq, psi_ref, params) % 状态: x [p_x - p_ref; p_y - p_ref; p_z - p_ref; v_x - v_ref; v_y - v_ref; v_z - v_ref] % 控制增量: u [a_x_des; a_y_des; a_z_des] 的增量形式 x0 [p_now(1) - p_ref_seq(1,1); p_now(2) - p_ref_seq(2,1); p_now(3) - p_ref_seq(3,1); v_now(1) - v_ref_seq(1,1); v_now(2) - v_ref_seq(2,1); v_now(3) - v_ref_seq(3,1)]; % 构建预测方程矩阵 A_pred, B_pred基于离散化模型 % ... % 构建QP问题 H B_pred * Q * B_pred R; f (A_pred * x0 - ref_seq) * Q * B_pred; % 求解 options optimoptions(quadprog, Display, off); U quadprog(H, f, A_ineq, b_ineq, [], [], lb, ub, [], options); % 取第一步控制增量 a_des U(1:3) [0; 0; params.g]; % 姿态解算 F_des params.m * norm(a_des); phi_des atan2(a_des(1) * sin(psi_ref) - a_des(2) * cos(psi_ref), a_des(3) params.g); theta_des atan2(a_des(1) * cos(psi_ref) a_des(2) * sin(psi_ref), a_des(3) params.g); end实际实现中矩阵构建和QP求解都封装在函数内部外部只关心状态输入和输出结果。这里用quadprog最简单如果追求速度可以用OSQP但对仿真来说quadprog完全够用。内环姿态MPC的结构与外环几乎一样只是状态换成姿态角和角速度误差控制量换成三轴力矩预测模型换成姿态回路LPV模型。为了避免重复造轮子可以写一个通用的MPC核心函数传入状态空间矩阵、权重矩阵和约束返回第一个控制增量然后分别在外环和内环调用它。4.5 结果可视化与误差统计仿真跑完最直观的是画三维轨迹对比。参考轨迹用虚线实际轨迹用实线这样一眼就能看出跟踪误差的位置和幅度。我常用的可视化输出包括三维轨迹对比图和xy平面投影位置误差随时间变化曲线三个轴分开展示姿态角跟踪曲线重点是期望滚转角、俯仰角和实际值的对比控制输出曲线包括总拉力和三轴力矩检查是否频繁饱和。误差统计方面我最关心的是位置误差的均方根值(RMS)和最大误差。8字形轨迹跟踪效果好不好不需要看华丽的动画就看RMS误差这个数字。如果RMS误差在0.1米以内姿态角误差在2度以内这个控制器基本合格。5. 调参与避坑仿真中真正卡住人的细节5.1 采样时间与预测时域的匹配问题双闭环结构下采样时间的匹配是第一道坎。内环姿态回路的采样时间我建议取0.01秒外环位置回路的采样时间可以放宽到0.05秒甚至0.1秒。这样做的好处是每个控制周期内QP问题的规模可控且时间尺度分离假设更成立。预测时域方面外环要能看见未来1秒到2秒的轨迹变化所以预测时域N取20到40步外环采样时间0.05秒比较合适。内环姿态回路不需要看那么远因为姿态本身是快变量预测N取10到20步就够了。如果内环预测时域太长不仅计算量大而且控制器会对远期的参考过于敏感容易抖动。我遇到的一个典型现象是内环姿态MPC的预测时域设到40步以后横滚角的响应出现明显的超调哪怕权重没变。这是因为未来太远的姿态参考信息对当前决策的影响很小反而引入了高频误差成分。缩短预测时域后响应立刻干净。5.2 姿态参考解算的奇异与滤波陷阱姿态解算公式在滚转角接近±90度时会出问题但8字形轨迹通常不会飞那么大角度。真正容易踩的坑是外环MPC在误差较大时会产生一个很大的期望加速度导致解算出的期望姿态角超出约束范围。举个例子仿真开始时飞机在原点参考轨迹从(0, 0)出发如果初始状态没有严格对齐外环为了尽快追上参考会产生一个很大的水平加速度指令解算出的滚转角可能瞬间达到60度甚至90度。此时内环姿态MPC即使全力跟踪也要花很长时间而且姿态角约束被硬生生突破。我的处理办法是给外环MPC的期望加速度加上限幅同时把姿态解算的中间变量做一次饱和处理。具体来说解算之前先检查F_des是否在电机推力范围内如果超限就缩放到最大推力对应方向检查phi_des、theta_des是否超出内环能接受的包线如果超出就裁剪到边界值。这样虽然名义上牺牲了一点跟踪速度但保证了内环的稳定性。另外解算出的姿态角信号不能直接给内环当参考最好先经过一个低通滤波器或者速率限制器。8字形轨迹中期望姿态角本身变化很快但如果外环MPC因为数值扰动输出抖动姿态角参考会携带高频噪声内环MPC跟踪这种噪声会浪费很多控制能量导致电机指令怪异。5.3 调度参数突变带来的控制量跳变LPV-MPC和普通线性MPC最大的区别在于模型矩阵随调度变量变化。调度变量的突变会直接导致MPC预测模型跳变进而让最优控制序列跳变最终表现为控制量抖一下。最容易触发这个问题的场景是8字形轨迹上总推力需求在转弯处明显上升如果调度变量是总推力那么每个控制周期计算出的离散模型都会变化。变化本身是正常的但如果调度变量变化速率过快或者工作点插值逻辑不够平滑控制量就会出现毛刺。我的做法是给调度变量加一个速率限制。具体到代码每次更新调度变量时限制它与上一时刻的差值在一定范围内。比如推力调度变量的变化速率不超过每秒最大值的20%这样模型矩阵变化是平缓的MPC的解也是连续过度的。还有一种更精细的做法是用前馈加反馈调度变量的前馈值由轨迹规划给出来反馈修正只用于消除实际误差这样调度变量既平滑又准确。5.4 权重整定的实操顺序MPC的权重参数少则四五个多则七八个挨个试会很痛苦。分享一下我调试双闭环MPC权重的实际操作顺序。第一步先调内环。把外环断开给内环一组阶跃姿态角指令从小幅度开始逐渐加大。调节内环的姿态角误差权重和角速度误差权重让姿态响应时间在0.1到0.3秒之间超调小于10%。内环确定前绝对不要去动外环否则外环解算出问题会使内环误以为跟踪失败。第二步接上外环但先用简单直线轨迹调试。外环权重可以先参考经验值位置误差权重为速度误差权重的3到5倍控制增量的惩罚权重可以从很小的值开始。如果轨迹跟踪出现振荡增大控制增量权重如果跟踪滞后明显增大位置误差权重。这个振荡就加重滞后就轻罚的调整口诀适用于绝大多数MPC调试场景。第三步换8字形轨迹。此时重点看拐弯段的跟踪性能。如果拐弯处误差持续偏大通常是外环预测时域不够长或者期望加速度限幅太紧如果拐弯时姿态角响应异常那就是内环预测时域或姿态权重问题。权重调整没有标准答案但有一套可复现的路径。我在实际项目里一般两三个小时能调出一组可用参数其中一半时间花在内环上。回到开头说的问题很多人下载了MPC代码跑不出效果其实不是MPC这个工具不行而是没有把模型和接口的细节吃透。双闭环架构中的姿态解算、调度变量的平滑、时间尺度分离的采样周期这些东西看起来零碎却决定了一个仿真项目能不能跑稳。个人经验是调试这类系统时不要一上来就追求8字形大轨迹先在小轨迹、小速度下把各个环节打通。每一个环节稳定可靠后再逐步加大幅度和速度。速度太快时你会感受到调度变量频繁变化带来的压力速度太慢时又感受不到LPV相比线性定常MPC的优势。找到一个缓慢大姿态的中间工况才是锻炼LPV-MPC识别和调参能力最好的场景。希望这篇拆解能帮正在做类似仿真的朋友少走弯路。按文章的思路把模型、接口、代码骨架一个个搭起来你也能跑出平滑稳定的8字形轨迹。到时候你回头再看那些跑不通的工程包应该就能一眼看出问题出在哪里了。 
长河君 · 2026/10/20 22:57:24 · 阅读 1.2k 四旋翼LPV-MPC双闭环8字形轨迹跟踪仿真详解
先说一个我观察到的现象网上下载的四旋翼MPC轨迹跟踪工程包不少Matlab脚本加Simulink模型全套都有但真正能自己跑出稳定8字形轨迹的很少。大部分人拿到代码按下运行飞机不是起点抖几下落下来就是飞到一半发散去最后只能把功劳归咎于MPC参数没调好。其实问题往往不是MPC理论本身而是模型搭建、内外环接口、调度变量处理这些细节没理清。这篇文章我想把位置姿态双闭环线性参数变化MPC控制8字形轨迹这条技术路线完整拆开讲一遍。它适合正在做无人机轨迹跟踪仿真、需要复现8字形飞行、或者课程课题里被LPV-MPC这几个字卡住的同学。我会从模型为什么要这样建、MPC为什么要分两环、LPV调度变量到底怎么选一直讲到Matlab代码骨架和实际调参时踩过的坑。目标是让任何有一定控制理论基础的人能照着这条思路把仿真自己搭起来而不是当个代码搬运工。1. 为什么8字形轨迹必须用位置姿态双闭环架构1.1 8字形轨迹对控制器提出的真实挑战先看8字形轨迹本身有什么特别的。很多教程喜欢用圆轨迹或者直线轨迹做演示因为曲率恒定或为零控制器压力小。但8字形轨迹不一样它的曲率是连续变化的而且拐弯处方向急剧反转飞行器需要持续改变横滚角和俯仰角来产生足够的水平加速度。以一个常见参数方程为例参考位置可以写成x_ref(t) A * sin(omega * t) y_ref(t) A * sin(2 * omega * t) z_ref(t) h其中A是幅度omega决定飞行速度。当t让sin项和sin(2t)项的组合发生变化时轨迹在xy平面划出一个躺倒的8字。这个轨迹的特点是方向变化快、加速度方向频繁切换、位置回路和姿态回路强耦合。控制器如果只有简单的PID往往在拐弯处出现明显的跟踪滞后而MPC的优势在于能预判未来几拍的需求提前输出控制量。但这还不够。8字形轨迹中飞行器始终处于倾斜姿态横滚角和俯仰角不是小量。此时四旋翼的位置动态和姿态动态互相影响如果只用一组固定的线性模型去设计MPC效果会很差。这就引出了两个问题模型怎么描述这种姿态变化导致系统动态变化的特性以及控制器怎么分层。1.2 单层MPC为什么不现实有人会想既然MPC这么强为什么不把位置、速度、姿态角、角速度全部塞进一个MPC里用一个14维状态向量直接跟踪参考轨迹理论上是可行的通用预测控制(MPC/NMPC)就能做但这在实际仿真和工程落地中有几个现实障碍。第一个障碍是预测时域与采样时间的冲突。姿态回路需要快速更新采样时间通常在0.01秒量级位置回路需要看远的未来预测时域可能要拉到2到3秒。如果单层MPC采样时间取0.01秒预测时域取100步那么决策变量维度会爆炸。即使Matlab能算每个控制周期内QP求解的实时性也难以保证。第二个障碍是系统非线性的处理。四旋翼的位置回路本质上是平移加旋转耦合在非线性轨迹上单一线性模型根本不够用。如果上非线性MPC优化问题的规模更大对初值敏感仿真时也更容易发散。第三个障碍更实际双闭环结构可以利用时间尺度分离原则把快回路和慢回路分开设计、分开调试出现问题时能定位到具体环节。单层MPC一旦跟踪效果不好你根本不知道是位置预测偏了还是姿态执行不到位。所以在轨迹跟踪控制里内外环加MPC的组合是更加成熟、可维护的方案。1.3 双闭环时间尺度分离的设计逻辑双闭环的思路很直接外环位置回路输出期望的姿态角和总拉力内环姿态回路负责快速跟踪这些期望值。位置回路关心的是飞机该往哪个方向加速姿态回路关心的是飞机怎么摆出这个姿态。二者通过姿态角参考接口衔接。时间尺度分离是这种结构的理论基础。姿态回路的时间常数比位置回路小一个数量级以上所以外环可以把内环近似看成理想的执行器——我给一个期望姿态角内环能很快跟上。反过来内环设计时不需要过多考虑位置回路它只负责把角速度控制好。这样两个MPC可以独立设计外环采样周期取大一点内环取小一点计算负担分散整机调试难度也降低。在后面的章节里我会一步步展开但先记住设计目标外环MPC负责把位置误差和速度误差拉小输出期望滚转角、俯仰角和总升力内环MPC负责跟踪姿态指令输出三个轴的力矩。8字形轨迹能否跟踪好80%的功劳在外环的设计20%的坑在内环的执行。2. 先理顺四旋翼数学模型从牛顿欧拉到LPV表示2.1 位置回路模型三个轴上的积分器链与推力分解设计MPC之前先得把被控对象的数学模型整理清楚。四旋翼的位置回路本质上是牛顿第二定律在惯性系下的投影。忽略空气阻力时位置方程可以写成m * ddot(P) F_total * R * e3 - m * g * e3其中P是位置向量F_total是总拉力大小R是机体系到惯性系的旋转矩阵e3是竖直单位向量。这个方程说明一件事无人机往前飞不是靠往前推实现的而是靠机体倾斜让总拉力的方向指向前方从而产生水平方向的加速度分量。加速度的来源是推力方向的改变所以外环MPC的输出表面上是期望加速度实际执行靠的是期望姿态角期望拉力。在悬停点附近做小角度线性化时旋转矩阵近似为常值矩阵位置回路退化成三个解耦的二阶积分器。这种情况下线性MPC就够用。但8字形轨迹上横滚角和俯仰角经常达到20到30度甚至更大此时必须考虑姿态角变化对位置加速度分配的影响。因此在位置回路MPC的预测模型里至少要把期望加速度到期望姿态角的转换这个环节正确处理。多数工程做法是先由MPC求出期望加速度向量再通过姿态解算公式反解出对应的期望横滚角、俯仰角和总拉力。2.2 姿态回路模型内环响应要足够快且能应对角速度耦合姿态回路的模型相对复杂一些。刚体姿态动力学可以写成J * dot(omega) -omega × (J * omega) tau其中omega是机体角速度J是转动惯量矩阵tau是控制力矩。这里有一个经常被忽略的项omega × (J * omega)也就是陀螺力矩。大角速度机动时这项的贡献不容小觑。如果仿真里做大幅度转弯而内环MPC模型里没有这一项控制器就会为了补偿而输出抖动。姿态运动学可以用欧拉角或者四元数描述。在MPC预测模型里如果我们限制姿态角变化范围不太大比如滚转角不超过30度用欧拉角表示是直观且够用的。小角度假设下欧拉角速率近似等于体轴角速度姿态回路也退化成三个近似解耦的通道。但同样地8字形轨迹上这个假设会失效尤其是偏航角在持续变化时通道之间会出现明显的交叉耦合。这就是姿态环需要LPV-MPC的深层原因系统的状态方程系数矩阵会随着姿态角变化而变化用一组固定的A和B矩阵无法准确描述大范围机动下的系统动态。2.3 为什么要换成线性参数变化视角线性参数变化(LPV)模型的思路可以这样理解把非线性系统看成一族线性系统每个工作点对应一组线性模型系统的实际动态由当前工作点决定而工作点由一组调度变量来刻画。调度变量一般是可测或可计算的状态量比如当前姿态角、当前总推力、当前空速等。对四旋翼来说最自然的调度变量是姿态角和总拉力。为什么因为位置回路的动态主要受推力方向和重力方向的夹角影响姿态回路的动态主要受当前角速度和转动惯量张量影响。推力不同角加速度对控制力矩的响应快慢也不同。把这些参数纳入调度变量后模型的精度显著提升。具体到状态空间形式LPV模型可以写作dot(x) A(σ(t)) * x B(σ(t)) * u其中σ(t)是调度向量。在每个采样时刻MPC根据当前的σ(t)选取或在预计算的工作点之间插值模型矩阵然后构建预测模型和QP问题。这个随着工作点变化而切换模型的机制就是LPV-MPC与普通线性时不变MPC的本质区别。调度变量不需要很多通常一到两个核心量就够了太多反而增加在线计算负担和插值误差。3. LPV-MPC设计中的三个关键决策3.1 调度变量选什么不是越多越好我在实际搭建时第一个困惑就是调度变量到底选哪些。一开始想把姿态角、角速度、总推力全部当成调度变量结果模型矩阵搜了一堆在线插值逻辑变得很复杂控制效果却没有显著提升。后来想明白了调度变量要选对系统动态影响最大、变化缓慢、且容易获取的量。对姿态回路来说总推力F_total是首选调度变量之一。因为当总推力不同时同样的控制力矩产生的角加速度响应速度不一样。另一个调度变量是当前的横滚角和俯仰角——它们决定了旋转矩阵的瞬时状态和陀螺力矩的大小。至于角速度变化太快直接把它作为调度变量会让模型矩阵剧烈跳变反而不利于MPC的预测稳定性更适合放在状态里处理。一个实用的原则是先做包线分析。在设定的8字形轨迹飞行包线内记录飞行过程中姿态角、推力的范围然后在这个范围内选取几个典型工作点做线性化。工作点之间用线性插值就够不需要无数个点。通常推力取3到4个点、姿态角取2到3个点组合起来10个以内的模型矩阵已经能覆盖绝大部分飞行场景。3.2 预测模型离散化与QP问题的构建MPC的设计核心是把连续模型离散化然后在每个采样时刻求解一个带约束的二次规划(QP)问题。离散化我推荐用零阶保持器。采样周期Ts内认为调度变量σ保持不变也就是每个控制周期内系统是线性时不变的这样离散化后的状态转移矩阵可以根据当前σ在线计算。具体到Matlab可以用c2d函数来获取连续模型对应的离散模型再把离散模型转换成预测矩阵。构建QP问题时每个控制周期内要做几件事根据当前状态x0比如当前位置、速度和期望姿态误差构建未来N步的预测方程把状态序列表示成初始状态和输入序列的线性组合把代价函数写成J (U - U_ref) * H * (U - U_ref) 松弛项其中U是控制序列把输入约束和状态约束写成不等式A_ineq * U b_ineq调用quadprog或OSQP求解。算法实现时最省事的方式是预先构建预测矩阵的稀疏结构在线只更新与当前调度变量相关的部分。Matlab自带的MPC Toolbox也能直接创建LPV控制器但对LPV机制的理解是必要的否则出了约束冲突或数值问题很难定位是参数问题还是模型矩阵构造问题。一个值得注意的点是松弛变量。仿真开始阶段状态通常不是参考轨迹的起始点或者约束不可行如果没有松弛变量QP问题可能直接无解整体仿真就会中断。加上松弛变量之后即使短暂违反约束控制器也能平滑过渡到可行区域。传统上人们会在代价函数里加一项ρ * ε²这个ρ既是约束罚权重也是调试中很有用的旋钮。3.3 约束、代价函数与松弛变量的处理策略每个MPC都会有代价函数设计。双闭环架构下外环和内环的代价函数权重需要分开设置。外环位置回路状态误差是位置误差和速度误差控制量是期望姿态角和拉力。我通常把位置误差权重设成速度误差权重的几倍以上因为8字形轨迹对位置精度要求高而速度误差已经被位置导数和参考速度隐含约束了。控制量权重不要取得太大否则控制器过于保守拐弯处跟不上轨迹但太小又会出现期望姿态角长时间饱和的问题。这里需要在仿真里反复试没有一劳永逸的公式。内环姿态回路状态误差是姿态角误差和角速度误差控制量是三个轴的力矩。姿态回路一定要快且稳所以姿态角误差权重取较高值角速度权重次之控制力矩的惩罚适当小一些保证内环响应速度足够快。如果内环响应太慢外环给的姿态角参考会产生持续滞后最终表现为位置跟踪的整体滞后。约束方面四旋翼主要的物理约束是总拉力有上下限电机饱和三轴力矩也有最大限制姿态角需要限制在一定范围内防止控制器索要不切实际的姿态角。此外我还会给外环的期望姿态角变化率加一个隐式约束——通过控制量权重大小来控制它别变化太快而不是直接硬限幅这样轨迹更平滑。状态约束要注意一点8字形轨迹经过大曲率段时位置误差在实际中的需求非常紧张如果把位置误差硬约束设置过小会频繁触发约束激活导致QP求解时间变长甚至无解。如果做的是纯仿真可以适当放宽如果是后续移植到真机才需要严格收紧。4. Matlab仿真落地的完整流程与代码骨架4.1 总体程序架构与文件划分写Matlab代码前先把工程文件划分清楚。我习惯这样组织project/ ├── main_simulation.m % 主仿真脚本 ├── trajectory_ref.m % 8字形参考轨迹生成 ├── quadrotor_model.m % 非线性四旋翼模型仿真被控对象 ├── mpc_outer_position.m % 外环位置LPV-MPC ├── mpc_inner_attitude.m % 内环姿态LPV-MPC ├── attitude_solver.m % 期望加速度到期望姿态角的解算 └── visualize_results.m % 结果可视化与误差统计main_simulation.m负责整体流程初始化参数、循环递推、在每个采样周期调用两个MPC控制器和四旋翼模型。不用Simulink而用纯脚本的好处是调试方便每一行命令都能断点检查尤其当你需要调试MPC内部的QP问题时脚本比Simulink直观得多。4.2 8字形参考轨迹的生成与参数选择轨迹生成很简单但参数选择有讲究。参考轨迹我采用omega 0.5; % 角频率单位rad/s A 4; % 轨迹幅度单位m h 2; % 期望高度单位m t 0:dt_sim:t_final; p_ref [A * sin(omega * t); A * sin(2 * omega * t); h * ones(1, length(t))];这里omega、A、h三个参数直接决定了轨迹的难度。如果A太大或者omega太大轨迹需要的向心加速度很大对应的期望倾斜角会超过姿态约束仿真很容易发散。建议从A2、omega0.3开始确保最大期望倾斜角小于20度再逐步加大难度。参考速度可以由位置指令差分得到也可以用解析式求导解析式更平滑。有一个容易忽视的点8字形在中心交叉处y方向的加速度方向会突然反转。如果参考速度在这一点不连续控制器会受到一个很大的瞬态冲击。因此轨迹参数方程的时间导数必须连续单纯用线性插值产生的点列会产生阶梯状速度导致MPC不断修正参考信号表现就是轨迹跟踪毛刺多。4.3 外环与内环的接口设计从期望加速度到期望姿态角两个MPC之间的接口是整个仿真最容易写乱的地方必须一步到位说清楚。外环MPC的输出我建议设计为期望惯性加速度a_des而不是直接输出姿态角。原因是位置回路的动态在惯性系下更直观、更容易设计权重。得到a_des之后再用姿态解算公式把它翻译成期望姿态角。具体解算公式如下% 期望加速度为 a_des [ax; ay; az] % 期望偏航角 psi_ref 由轨迹规划给定 % 期望推力大小 F_des m * norm(a_des [0; 0; g]); % 期望横滚角 phi_des atan2(ax * sin(psi_ref) - ay * cos(psi_ref), az g); % 期望俯仰角 theta_des atan2(ax * cos(psi_ref) ay * sin(psi_ref), az g);这段公式的意义是总推力方向必须与期望加速度方向对齐同时力矩必须平衡重力。如果换成直接输出姿态角的方案外环模型就掺入了姿态解算的非线性LPV模型更难写所以推荐加速度接口显式解算的方式。内环姿态MPC接收phi_des、theta_des、psi_ref作为参考输入输出体轴力矩tau_x、tau_y、tau_z再通过电机的混控矩阵分配到四个电机。混控矩阵其实就是一个固定的线性变换由四旋翼的臂长、扭矩系数等决定在悬停附近是常值。4.4 外环MPC的核心代码骨架这里给出外环MPC的核心伪代码不是完整代码但足以帮助你理解流程function [F_des, phi_des, theta_des] mpc_outer_position(p_now, v_now, p_ref_seq, v_ref_seq, psi_ref, params) % 状态: x [p_x - p_ref; p_y - p_ref; p_z - p_ref; v_x - v_ref; v_y - v_ref; v_z - v_ref] % 控制增量: u [a_x_des; a_y_des; a_z_des] 的增量形式 x0 [p_now(1) - p_ref_seq(1,1); p_now(2) - p_ref_seq(2,1); p_now(3) - p_ref_seq(3,1); v_now(1) - v_ref_seq(1,1); v_now(2) - v_ref_seq(2,1); v_now(3) - v_ref_seq(3,1)]; % 构建预测方程矩阵 A_pred, B_pred基于离散化模型 % ... % 构建QP问题 H B_pred * Q * B_pred R; f (A_pred * x0 - ref_seq) * Q * B_pred; % 求解 options optimoptions(quadprog, Display, off); U quadprog(H, f, A_ineq, b_ineq, [], [], lb, ub, [], options); % 取第一步控制增量 a_des U(1:3) [0; 0; params.g]; % 姿态解算 F_des params.m * norm(a_des); phi_des atan2(a_des(1) * sin(psi_ref) - a_des(2) * cos(psi_ref), a_des(3) params.g); theta_des atan2(a_des(1) * cos(psi_ref) a_des(2) * sin(psi_ref), a_des(3) params.g); end实际实现中矩阵构建和QP求解都封装在函数内部外部只关心状态输入和输出结果。这里用quadprog最简单如果追求速度可以用OSQP但对仿真来说quadprog完全够用。内环姿态MPC的结构与外环几乎一样只是状态换成姿态角和角速度误差控制量换成三轴力矩预测模型换成姿态回路LPV模型。为了避免重复造轮子可以写一个通用的MPC核心函数传入状态空间矩阵、权重矩阵和约束返回第一个控制增量然后分别在外环和内环调用它。4.5 结果可视化与误差统计仿真跑完最直观的是画三维轨迹对比。参考轨迹用虚线实际轨迹用实线这样一眼就能看出跟踪误差的位置和幅度。我常用的可视化输出包括三维轨迹对比图和xy平面投影位置误差随时间变化曲线三个轴分开展示姿态角跟踪曲线重点是期望滚转角、俯仰角和实际值的对比控制输出曲线包括总拉力和三轴力矩检查是否频繁饱和。误差统计方面我最关心的是位置误差的均方根值(RMS)和最大误差。8字形轨迹跟踪效果好不好不需要看华丽的动画就看RMS误差这个数字。如果RMS误差在0.1米以内姿态角误差在2度以内这个控制器基本合格。5. 调参与避坑仿真中真正卡住人的细节5.1 采样时间与预测时域的匹配问题双闭环结构下采样时间的匹配是第一道坎。内环姿态回路的采样时间我建议取0.01秒外环位置回路的采样时间可以放宽到0.05秒甚至0.1秒。这样做的好处是每个控制周期内QP问题的规模可控且时间尺度分离假设更成立。预测时域方面外环要能看见未来1秒到2秒的轨迹变化所以预测时域N取20到40步外环采样时间0.05秒比较合适。内环姿态回路不需要看那么远因为姿态本身是快变量预测N取10到20步就够了。如果内环预测时域太长不仅计算量大而且控制器会对远期的参考过于敏感容易抖动。我遇到的一个典型现象是内环姿态MPC的预测时域设到40步以后横滚角的响应出现明显的超调哪怕权重没变。这是因为未来太远的姿态参考信息对当前决策的影响很小反而引入了高频误差成分。缩短预测时域后响应立刻干净。5.2 姿态参考解算的奇异与滤波陷阱姿态解算公式在滚转角接近±90度时会出问题但8字形轨迹通常不会飞那么大角度。真正容易踩的坑是外环MPC在误差较大时会产生一个很大的期望加速度导致解算出的期望姿态角超出约束范围。举个例子仿真开始时飞机在原点参考轨迹从(0, 0)出发如果初始状态没有严格对齐外环为了尽快追上参考会产生一个很大的水平加速度指令解算出的滚转角可能瞬间达到60度甚至90度。此时内环姿态MPC即使全力跟踪也要花很长时间而且姿态角约束被硬生生突破。我的处理办法是给外环MPC的期望加速度加上限幅同时把姿态解算的中间变量做一次饱和处理。具体来说解算之前先检查F_des是否在电机推力范围内如果超限就缩放到最大推力对应方向检查phi_des、theta_des是否超出内环能接受的包线如果超出就裁剪到边界值。这样虽然名义上牺牲了一点跟踪速度但保证了内环的稳定性。另外解算出的姿态角信号不能直接给内环当参考最好先经过一个低通滤波器或者速率限制器。8字形轨迹中期望姿态角本身变化很快但如果外环MPC因为数值扰动输出抖动姿态角参考会携带高频噪声内环MPC跟踪这种噪声会浪费很多控制能量导致电机指令怪异。5.3 调度参数突变带来的控制量跳变LPV-MPC和普通线性MPC最大的区别在于模型矩阵随调度变量变化。调度变量的突变会直接导致MPC预测模型跳变进而让最优控制序列跳变最终表现为控制量抖一下。最容易触发这个问题的场景是8字形轨迹上总推力需求在转弯处明显上升如果调度变量是总推力那么每个控制周期计算出的离散模型都会变化。变化本身是正常的但如果调度变量变化速率过快或者工作点插值逻辑不够平滑控制量就会出现毛刺。我的做法是给调度变量加一个速率限制。具体到代码每次更新调度变量时限制它与上一时刻的差值在一定范围内。比如推力调度变量的变化速率不超过每秒最大值的20%这样模型矩阵变化是平缓的MPC的解也是连续过度的。还有一种更精细的做法是用前馈加反馈调度变量的前馈值由轨迹规划给出来反馈修正只用于消除实际误差这样调度变量既平滑又准确。5.4 权重整定的实操顺序MPC的权重参数少则四五个多则七八个挨个试会很痛苦。分享一下我调试双闭环MPC权重的实际操作顺序。第一步先调内环。把外环断开给内环一组阶跃姿态角指令从小幅度开始逐渐加大。调节内环的姿态角误差权重和角速度误差权重让姿态响应时间在0.1到0.3秒之间超调小于10%。内环确定前绝对不要去动外环否则外环解算出问题会使内环误以为跟踪失败。第二步接上外环但先用简单直线轨迹调试。外环权重可以先参考经验值位置误差权重为速度误差权重的3到5倍控制增量的惩罚权重可以从很小的值开始。如果轨迹跟踪出现振荡增大控制增量权重如果跟踪滞后明显增大位置误差权重。这个振荡就加重滞后就轻罚的调整口诀适用于绝大多数MPC调试场景。第三步换8字形轨迹。此时重点看拐弯段的跟踪性能。如果拐弯处误差持续偏大通常是外环预测时域不够长或者期望加速度限幅太紧如果拐弯时姿态角响应异常那就是内环预测时域或姿态权重问题。权重调整没有标准答案但有一套可复现的路径。我在实际项目里一般两三个小时能调出一组可用参数其中一半时间花在内环上。回到开头说的问题很多人下载了MPC代码跑不出效果其实不是MPC这个工具不行而是没有把模型和接口的细节吃透。双闭环架构中的姿态解算、调度变量的平滑、时间尺度分离的采样周期这些东西看起来零碎却决定了一个仿真项目能不能跑稳。个人经验是调试这类系统时不要一上来就追求8字形大轨迹先在小轨迹、小速度下把各个环节打通。每一个环节稳定可靠后再逐步加大幅度和速度。速度太快时你会感受到调度变量频繁变化带来的压力速度太慢时又感受不到LPV相比线性定常MPC的优势。找到一个缓慢大姿态的中间工况才是锻炼LPV-MPC识别和调参能力最好的场景。希望这篇拆解能帮正在做类似仿真的朋友少走弯路。按文章的思路把模型、接口、代码骨架一个个搭起来你也能跑出平滑稳定的8字形轨迹。到时候你回头再看那些跑不通的工程包应该就能一眼看出问题出在哪里了。