1. 项目概述：为什么把四种算法放在同一条轨迹上“极限施压”

无人机控制算法仿真，最怕的不是算法本身跑不通，而是模型建得“太干净”。我以前见过很多同学的项目，仿真里全是理想条件：没有噪声、没有风扰、轨迹平滑得像教科书上的示意图，跑出来的控制效果当然好看，但一到真机试飞就原形毕露。所以看到这个项目标题的对比设计——剧烈轨迹、测量噪声、外部干扰三重重压之下，同时拉出几何控制、微分平坦、INDI、NMPC四种主流算法同台竞技——我觉得这就是典型“仿真不够狠，实飞就翻车”的反向思维，也是这个项目最有价值的地方。

欠驱动无人机说的是四旋翼这种只有4个电机输入、却有6个自由度输出的系统，平动和转动之间存在强耦合，控制难度远高于全驱动机械臂。处理这种欠驱动特性，不同学派给出了完全不同的思路：几何控制直接在SE(3)李群上设计误差，微分平坦把轨迹规划代数化，INDI把问题转化成增量形式的抗扰控制，NMPC则靠滚动优化硬扛非线性约束。这四种算法平时各说各话，能凑到同一套仿真框架里对比，本身就需要解决大量工程细节——坐标系统一、参数初始化、控制频率匹配、扰动注入方式，每一步都有可能让对比结果失真。

本文适合四类读者：正在做无人机毕设或竞赛的本科生，需要快速理解几种主流控制框架差异的研究生，准备把仿真算法往实机上迁移的工程师，以及纯粹想看看“同一台无人机在不同控制器下到底有什么表现”的爱好者。我不打算逐行贴完整的MATLAB代码——那样篇幅根本铺不开，而是把四种算法的建模思路、关键实现环节、扰动注入方法和对比结果讲透，配合核心代码片段，让你能自己复现整个对比实验。

这个项目最终交付的是一套完整的MATLAB仿真工程，包含四旋翼非线性模型、参考轨迹生成器、噪声与干扰注入模块、四种控制器、离线对比分析脚本。我的建议是直接把它当成“赛前摸底测试”来看待：如果一种算法在剧烈轨迹+高噪声+强风扰下还能保持厘米级跟踪误差，它才有资格被考虑装到飞控里。

2. 核心难点拆解：欠驱动模型怎么建，扰动怎么加才能“伤筋动骨”

2.1 欠驱动四旋翼模型的三个关键坑

四旋翼动力学模型，教科书上通常写成平移和旋转两组方程。平移部分遵循牛顿第二定律，旋转部分遵循欧拉方程，两组方程通过旋转矩阵耦合在一起。看起来简单，但实际建模时至少有三个坑。

第一个坑是坐标系的约定。仿真里如果使用NED（北东地）坐标系，z轴是向下的，重力加速度要写成正数；如果使用ENU（东北天）坐标系，z轴向上，重力加速度为负。两种约定下，控制器的符号逻辑完全不同。MATLAB的Aerospace Toolbox默认偏NED，机器人学的通用习惯是ENU，两个工具箱混用时非常容易出符号错位。我建议整个仿真统一使用ENU坐标系，因为后面NMPC的优化求解器和几何控制的旋转矩阵误差定义都更贴近机器人学惯例。

第二个坑是旋转矩阵的更新方式。四元数和旋转矩阵的微分方程虽然数学上等价，但数值积分时的误差特性不一样。用欧拉角做积分在俯仰角接近90度时会出现万向节锁，而旋转矩阵直接积分会出现非正交化的漂移，时间一长矩阵就不再是合法的旋转矩阵。比较稳妥的做法是每步积分之后做一次Gram-Schmidt正交化，或者直接用四元数积分再转回矩阵。在剧烈轨迹下，飞行器姿态变化非常快，这个细节会明显影响姿态解算精度。

第三个坑是电机执行器模型。很多初版仿真把电机抽象成一阶惯性环节，这没有问题，但时间常数选多大很讲究。太大会让响应慢的电机跟不上高频控制指令，仿真里表现为高频抖振；太小则过于理想，真机上电机根本达不到这样的响应速度。比较合理的电机时间常数范围是0.02到0.05秒之间，对应小型无刷电机搭配电子调速器的实际带宽。

2.2 剧烈轨迹的生成：要比“标准测试轨迹”刁钻得多

标题里说的“剧烈轨迹”，我实际实现时用了三段式拼接的8字形螺旋上升轨迹。8字形保证位置跟踪在水平面内不断改变方向，螺旋上升部分叠加大幅度的升降变化，考验系统的耦合跟踪能力。

轨迹生成的核心参数是最大加速度和最大加加速度（jerk）。我设定最大加速度为8米每平方秒，大约是1.6g的过载水平，这已经超过大多数实际飞行任务的机动强度；加加速度限制在80米每三次方秒，让轨迹在转折处产生剧烈但连续的变化。这样生成的轨迹，速度方向变化频率高，姿态角需求频繁震荡，特别适合暴露控制器的动态响应短板。

轨迹表达式我直接用参数方程生成位置参考，再用有限差分或解析求导得到速度和加速度。需要注意，几何控制和NMPC能用到位移、速度和加速度三元组，但INDI和微分平坦需要更高阶的参考量（加加速度和加加加速度），这些导数必须预先生成并存储为查找表，不能在仿真循环里现算——现算会引入数值噪声，把控制品质的对比结果搞脏。

2.3 噪声与干扰：白噪声、有色噪声、阵风、机体质心偏移怎么注入

测量噪声如果只加“白噪声”三个字就完了，那是严重偷懒。实际传感器噪声有色且相关：GPS位置测量有米级漂移且更新率低，IMU加速度计和陀螺仪有零偏和高频噪声，磁力计受电机磁场干扰。我按传感器类型分别注入噪声：位置测量叠加均值为0、标准差0.1米的高斯白噪声，外加幅值0.05米的低频正弦漂移来模拟GPS缓慢漂移；陀螺仪噪声标准差0.02弧度每秒，加速度计噪声标准差0.1米每二次方秒。噪声通过随机数生成器注入，固定随机种子保证四种算法面对完全一样的噪声序列，这是对比公平性的基础。

外部干扰我设置了两种类型。第一种是常值风扰，在某个时间段内施加到机体上的恒定外力，模拟侧风场景；第二种是突变阵风，用梯形波函数生成短时高强度干扰，模拟穿越建筑物风场边缘。干扰直接叠加到平动力方程和旋转动力学方程的广义力上，相当于验证控制器对外部扰动力的抑制能力。

更有意思的一组实验是参数失配拒动——把真实模型的质量在某个时刻突然改成标称值的1.2倍，再在后面恢复。这个实验能有效测试控制器的鲁棒性：NMPC如果模型里用的还是标称质量，预测误差就会增大，但反馈校正又能拉回来；INDI由于依赖增量控制，对模型参数的敏感度相对较低。这种参数大跳变的扰动在实际中对应载荷投掷、燃油消耗、电池电压下降等场景，比单纯加个外力更接近工程实际。

3. 算法选型与原理：四种控制方案的底层逻辑差异

3.1 几何控制：在SE(3)流形上做误差反馈

几何控制的核心思想是直接在SO(3)旋转矩阵或SE(3)齐次变换矩阵上定义姿态误差，不在欧拉角或四元数上做中间转换。它的理论优势在于避免参数化奇异点，轨迹跟踪的几何结构更清晰。

实际实现中，姿态误差用矩阵对数映射计算，形式是$$\Psi(R_e) = \frac{1}{2} (R_d^T R - R^T R_d)^{\vee}$$，其中$R_e$是期望姿态与实际姿态之间的误差矩阵，$\vee$表示从反对称矩阵提取对应的三维向量。这个误差项被直接引入姿态控制器设计，配合位置控制器的虚拟力指令，构成完整的几何跟踪控制律。

几何控制的调试重点在增益矩阵的选取。我使用的是对角增益矩阵，角速度增益取角度增益的2倍平方根关系来逼近临界阻尼，这是源于二阶系统极点配置的经验关系。但在剧烈轨迹下，如果角速度增益偏小，会出现姿态滞后引起的明显轨迹偏移；偏大又会引入噪声放大。需要反复调参，我最后固定为位置增益$K_p=16$、速度增益$K_v=8$、姿态增益$K_R=80$、角速度增益$K_\Omega=18$，这个组合在仿真中表现比较均衡。

让我觉得麻烦的是几何控制的鲁棒性问题。它本质上是基于模型准确性的反馈控制，一旦有较大的外力扰动或者模型失配，积分环节又不足，稳态误差就会显现。所以在零均值噪声下它表现很好，但加了常值风扰之后，位置跟踪会稳定地偏移一个固定角度，需要用足够大的积分增益去压住，但积分增益过大又会带来超调。

3.2 微分平坦控制：把轨迹规划变“代数题”

微分平坦的核心是先分析系统的平坦输出，把轨迹规划从解微分方程变成代数映射，再逆解出系统输入。四旋翼的平坦输出可以直接选为位置和偏航角，共4个量，而这恰好与4个控制输入匹配。这正是四旋翼被称为微分平坦系统的原因——用位置和偏航角就能代数重构出全部状态和控制指令。

实现微分平坦的关键在于：给定位置轨迹$p(t)$和偏航角$\psi(t)$，先由平移动力学反解出期望加速度、推力方向和总推力，再由偏航角解出完整的姿态，最后用姿态误差驱动内环控制。往理论深处走一步，推力方向由加速度确定，加速度来自轨迹的二阶导数，所以参考轨迹至少要C2连续；而如果后续要用到加加速度相关的扰动补偿，就需要C3连续。这也解释了为什么剧烈轨迹的生成必须对加加速度做约束——如果轨迹不可微足够多次，微分平坦性就无从谈起。

调试微分平坦算法时，一个容易踩坑的地方是解析雅可比的求取。当偏航角参考不为零时，位置参考与偏航角之间存在耦合，因为机体推力轴的投影被偏航方向影响。必须把旋转矩阵的构造用解析表达式写出来，而不是每次都调用数值微分，这样生成的控制指令才足够平滑。

从对比结果来看，微分平坦算法在无扰动条件下表现最好——因为它本质上就是在执行一条精心设计的平坦输出轨迹，完全没有跟踪误差的积累过程。但问题也随之而来：微分平坦需要精确的模型反解，外部干扰一旦破坏了模型匹配性，它的抗扰能力就明显弱于INDI。

3.3 INDI：用增量控制把非线性问题“局部线性化”

INDI的思路和传统非线性动态逆完全不同。传统的NDI先把系统的非线性动态精确求逆来抵消非线性项，计算量大且对模型误差敏感；INDI不直接求逆整个动力学，而是在当前工作点做增量形式的动态逆，利用上一时刻的输入和状态来计算当前所需增量。这样只需知道当前状态和输入，并不需要精确的全局模型——这相当于做“局部泰勒展开”而不是“全局求逆”，天然具备对模型失配的鲁棒性。

四旋翼的INDI实现重点在角加速度估计。INDI的核心公式是$\dot{\omega} \approx \dot{\omega}_{ref} + G^{-1}(u - u_0)$，其中$G$是输入到角加速度的增益矩阵，$u_0$和$\dot{\omega}_0$是上一时刻的输入和角加速度。角加速度不能直接量测，需要通过陀螺仪差分或状态观测器估计。差分会对噪声非常敏感，这是我实际调试中最头疼的部分。

我最终采用了在Simulink里对陀螺仪角速度信号做微分并级联二阶低通滤波器的方法。滤波器的截止频率要反复调整：太高则噪声放大严重，INDI的增量指令抖动明显；太低则相位滞后，导致控制发散。试了多组参数后，我选择截止频率30赫兹，既能有效滤除高频噪声，也能跟上需要30赫兹以上带宽的剧烈轨迹。

INDI在风扰阶跃实验中表现是所有算法里最好的，这也是它近年成为飞行控制研究热点的原因。因为增量形式天然对常值扰动具有积分效应——常值外力造成的角加速度偏差会被增量控制持续修正，直到偏差为零。它的弱点在于对测量噪声比较敏感，因为增量$\dot{\omega}_0$本身由噪声较大的差分信号估计而来。在高噪声实验中，它的推力指令高频分量最显著，轻微牺牲了平滑性。

3.4 NMPC：滚动优化里藏着“上帝视角”

NMPC不像前三者那样做显式反馈控制，而是每一步都解决一个有限时域的最优控制问题：给定当前状态、参考轨迹和动态模型，在预测时域内求解未来的控制输入序列，只取第一步执行，下个时刻重新求解。它的优势是把约束和预测能力统一纳入一个优化框架——推力有限幅、姿态角限幅、控制增量限幅都可以直接作为优化问题的约束条件。

实现NMPC最核心的模块是预测模型和优化求解器。我对系统做了离散化处理，用内部碰撞模型作为预测模型，采样时间取30毫秒，预测时域取20步，控制时域取10步。代价函数包含状态跟踪误差项、控制量惩罚项和终端惩罚项，用SQP（序列二次规划）求解。每个采样周期的平均求解时间大约12毫秒，在模型预测误差可接受范围内能实时运行。

NMPC的调参自由度远高于前三种算法，每个权重系数都影响系统行为：位置误差权重决定跟踪精度，控制惩罚项决定执行器磨损，预测时域长度决定“看得多远”。我调试中印象最深的一次是把预测时域从20步加到30步，期望系统“看得更远”从而减少超调，结果适得其反——因为模型预测在大扰动下失配严重，越远的预测误差越大，反而扰乱优化决策。预测时域不是越长越好，要结合模型准确性来权衡。

从仿真时间上看，NMPC的计算量是其他三种算法的好几倍，这是它的主要工程代价。但在模型准确、约束复杂、需要在线避障或推力约束的飞行任务中，它对系统的预测能力又无可替代。

4. 仿真环境搭建与关键实现

4.1 仿真主循环结构与数据流设计

我建议整个仿真不用Simulink，而是用纯MATLAB脚本加类封装的形式搭建。这样做的考虑是，后续做参数扫掠实验（比如同时改变噪声强度和风扰大小）时，脚本循环调用比Simulink拖模型更灵活，数据记录也更容易统一管理。

主循环的结构大致如下：

• 初始化阶段：定义四旋翼物理参数（质量、惯量、电机时间常数），初始化状态向量，生成参考轨迹和干扰序列。
• 主循环阶段：每个步长依次执行传感器模型更新、控制器计算、执行器模型更新、刚体动力学积分。
• 数据记录阶段：每个步长把时间、状态、参考值、控制输入、跟踪误差、代价指标存入结构化数组中。
• 离线分析阶段：计算RMS位置误差、最大偏差、控制能量消耗等指标，绘制各对比曲线。

for k = 1:length(t_vec) % 传感器测量（带噪声） pos_meas = pos(k,:)' + noise_pos(k,:)'; vel_meas = vel(k,:)' + noise_vel(k,:)'; att_meas = att(k,:)' + noise_att(k,:)'; % 控制器输出 [u_ref, debug_info] = controller_handle(pos_meas, vel_meas, att_meas, ref_seq(:,k), dt); % 电机响应 thrust_raw = exec_model(u_ref, motor_state(k,:), inhibit_time); % 刚体动力学积分 [pos_next, vel_next, att_next, omega_next] = quad_dynamics(pos(k,:)', vel(k,:)', att(k,:)', omega(k,:)', thrust_raw, disturbance(k,:)', params); % 更新状态 pos(k+1,:) = pos_next'; vel(k+1,:) = vel_next'; att(k+1,:) = att_next'; omega(k+1,:) = omega_next'; % 记录调试数据 log_data(k) = collect_logs(u_ref, debug_info, ...); end

主循环的时间步长选择1毫秒，比控制频率更高以保证动力学积分稳定性。控制频率根据各算法特点设置，几何控制和微分平坦为200赫兹，INDI为250赫兹，NMPC由于计算开销为33赫兹。这个频率差异本身就会反映在最终的轨迹跟踪精度上，所以对比时必须注明各算法的更新频率，否则结果不可比。

用纯脚本实现的另一个好处是方便单元测试。每种控制器封装成独立的函数句柄，输入输出接口一致，不需要改主循环代码就能切换算法。这让对比实验变得更加公平，因为我只需改变一行代码就能保证四种算法经历完全一样的状态演化过程。

4.2 参数整定与控制器调优过程

参数整定是整个仿真中最耗时的一环。我的做法不是一次性把所有参数调到最优，而是分轮逐步逼近：每轮只调一个算法的一组参数，保证该算法在中等难度（没有噪声和干扰的8字形轨迹）下表现良好，然后固定参数进入高难度测试。否则一边调参一边增加扰动，问题来源互相混淆，根本无法定位参数问题。

对几何控制和微分平坦，我先用线性化的姿态环和位置环模型计算增益矩阵的初值，再代入非线性模型做闭环微调。姿态环的角速度增益初值按$$K_\Omega = 2\sqrt{K_R}$$来设定，这样响应接近临界阻尼。位置环的速度增益则按$$K_v = 2\sqrt{K_p}$$的关系给出，再根据实际超调情况微调。这类二阶系统极点配置的经验关系能大幅压缩调参时间。

INDI的调试重点在角加速度滤波器的截止频率和增益矩阵$G$。我会先把增益矩阵设为单位对角矩阵，让系统跑起来，再通过阶跃响应观察振动频率来判断增益的正误。角加速度估计用Simulink等效模型离线测试，观察滤波后的角加速度信号是否滞后于真值超过一个控制周期。

NMPC的权重初值来自对数量级的估算：位置误差以米计，速度误差以米每秒计，控制量以牛顿计，将它们归一化到相近数量级后再做精细调节。预测时域长度从15步起步，根据半实物仿真中的实时性约束逐步调整。权重系数的最终值，我经过三轮迭代确定：位置误差权重800、速度误差权重100、控制量权重0.02、控制增量权重0.01，终端权重与预测时域内的积分权重保持一致性。

调参过程中我最想提醒的一点是：不要同时在所有条件下做联合优化。每轮只给算法设置一个难度旋钮，比如第一轮只加噪声，第二轮只加阶跃风扰，第三轮才加剧烈轨迹；等每轮表现都稳定了，再所有条件叠加做终极测试。我最初试过一上来就全条件叠加，结果四种算法全都发散，根本无法定位是控制器的问题还是参考轨迹的问题。

4.3 场景矩阵设计：噪声、风扰、参数失配如何组合

为了说明四种算法在不同压力下的差异，我设计了三个递进式的对比场景。

场景一：剧烈轨迹+零噪声+零干扰。这是理想条件下的能力上限测试。预期结果是微分平坦和NMPC表现最好，因为它们分别有反向推演和预测优势，而几何控制可能出现轻微滞后，INDI应当接近几何控制。

场景二：剧烈轨迹+测量噪声。加噪声后，微分平坦的性能下降最明显，因为它需要高阶导数，噪声会被差分放大。INDI也会因为角加速度估计受噪声影响而出现一些抖振，但总体还能维持跟踪。几何控制和NMPC由于有明确的积分作用或预测滤波，对零均值噪声更宽容。

场景三：剧烈轨迹+测量噪声+外部干扰和参数失配。这个组合最接近真实飞行环境。NMPC由于模型失配引入的预测误差，表现会下降，但其优化框架能利用约束重新规划，保持在可接受范围内；INDI得益于增量反馈，对参数失配和常值风扰的抗抑制效果最显著；几何控制则需要靠积分项硬扛，会出现可见的稳态偏移；微分平坦没有显式反馈校正机制，误差可能持续积累。

最终实验统一使用相同的随机种子和扰动序列，从曲线中可以看到，微分平坦在纯净条件下领先，但到了场景三，INDI的轨迹包络明显更紧密，NMPC次之，几何控制第三，微分平坦垫底。

5. 仿真结果对比与核心结论

5.1 四组实验的量化指标对比

仿真完成后，我计算了三个量化指标：位置跟踪的RMS误差（单位米），最大瞬时误差（单位米），以及控制能量消耗（定义为控制信号平方的积分）。下面对不同算法的结果汇总如下：

这组数据值得仔细解读。在纯净条件下，微分平坦确实以0.09米的最小RMS误差领先，说明它的轨迹执行力最强。但加上噪声后，它的RMS误差直接跳到0.38米，增加了四倍多，这是因为微分平坦对加速度及加加速度参考的高阶依赖放大了噪声。INDI则从0.14米增至0.29米，增幅小于微分平坦，主要归功于增量控制天然对测量突变不敏感。几何控制的抗噪性能好于预期，得益于积分项的过滤作用。NMPC在噪声下表现最佳——它的预测模型起到了隐式滤波的效果，RMS只有0.19米。

场景三的结果最能说明问题。常值风扰和参数失配同时作用时，INDI的RMS误差为0.26米，竟然比场景二还小，这是因为增量控制在扰动下修正得更积极，把稳态偏差彻底压掉了，同时也说明它在纯噪声下还有改进空间。NMPC表现中规中矩，0.35米的误差说明模型失配确实影响预测效果，但优化器仍然能通过约束重新找到可行解。几何控制受积分饱和影响，出现明显滞后，误差增大。微分平坦因为开环反解指令缺乏反馈校正，误差最大。

5.2 时域曲线中直观可见的差异

只看表格可能不够直观，需要再结合时域曲线来读。

在位置曲线对比中，微分平坦在轨迹转弯处的跟踪最贴紧，几乎没有超调；几何控制则呈现明显的相位滞后，转弯越大滞后越明显。INDI在风扰注入的瞬间会产生一个微小偏移，但很快被增量修正拉回，像弹簧一样。NMPC的位置曲线最为平滑，因为它每一步都在优化未来的轨迹，所以不会出现剧烈突变。

在控制输入曲线中，NMPC的推力曲线有明显的提前预判——在轨迹到达转折点之前就开始改变推力方向，因为预测时域让它“看到”了未来的转折。几何控制的推力曲线则相对平直，因为反馈控制只在偏差出现后才能反应。INDI的推力曲线在噪声环境下高频分量明显，低速滤波器的噪声被增量放大，但幅度仍然在可接受范围内。

姿态误差曲线更能揭示算法本质。NMPC的姿态误差虽然存在，但被限制在紧凑的区间内，因为控制量约束和预测优化在同时起作用。INDI的姿态误差变化最猛，但收敛速度快，几乎每个扰动之后都能恢复到零误差。几何控制的姿态误差暴露了转动惯量在剧烈机动力下的影响，会出现一个固定偏置。

5.3 对仿真结果客观公平性的说明

需要提醒的是，这个对比并非严格意义上的“最优公平”——每一种算法的最优参数都没有做全局搜索，只做了基于经验和启发式的局部寻优。如果花更多时间做响应曲面法或贝叶斯优化调参，每一种算法的数据结果都能进一步提升。特别是NMPC，它的权重系数和预测时域对性能影响很大，我在仿真里还有很大的调参潜力没有挖掘。

不过，这不是研究的弱点，反而是它的价值所在。实际工程中，没有人在一条赛道上给你无限时间调参，而是要求“在有限时间内达到工程可用的控制品质”。这个仿真框架的意义，正是模拟了这样的现实约束。

6. 踩坑记录与仿真调试避坑指南

6.1 高频振荡：几何控制和NMPC在剧烈轨迹下的共同Bug

仿真刚开始时，几何控制在8字形轨迹大幅度转弯处出现了明显的高频振荡，位置误差曲线呈现周期性的齿状波动。排查发现，问题不在控制器增益，而在参考轨迹的时间导数离散计算上。主循环里使用的参考速度是相邻时刻位置差分除以步长，这种数值微分在1毫秒步长下引入了极大的随机误差，被控制器的微分项放大后造成振荡。

正确做法是参考轨迹的所有导数都在生成轨迹时用解析表达式直接计算，或者用中心差分法配合适当低通滤波，而不是简单的前向差分。我在参考轨迹生成脚本里用符号微分求出了速度和加速度的解析表达式，问题立即消失。

6.2 INDI的角加速度估计滞后导致发散

INDI实现过程中最危险的一次失败是仿真在第6秒突然发散，无人机状态直接飞到NaN。检查发现，角加速度信号用陀螺仪数据差分后没有做滤波，而剧烈轨迹中角速度变化极快，差分信号噪声大到完全淹没了真实角加速度，导致增量控制量出现巨大毛刺。

解决办法是引入二阶低通滤波器并加前馈补偿。滤波后的角加速度虽然滞后于真实值约一个控制周期，但由于INDI本身是增量控制，每个周期只修正一小部分偏差，滞后并不会导致发散。关键是要检查滤波后的信号噪声水平是否低于控制量阈值的10%，如果不满足就进一步降低截止频率。

6.3 NMPC的初猜测问题与SQP求解失败

NMPC在噪声场景下会出现间歇性的SQP求解失败，具体表现是优化器返回NaN或产生违反约束的解，导致控制量跳变。排查发现，问题来自每次优化迭代的初猜测。我把上一次解算出的最优控制序列作为下一次优化的初猜测，这在扰动小时完全可行，但大扰动下状态突变，旧的控制序列与当前状态严重不匹配，SQP在有限迭代次数内无法收敛。

解决方案是为每个预测时域内的初始控制量增加一个前馈项。具体来说，初猜测的第一段用当前参考轨迹对应的解析控制律给一个值，后面的段落用上一次的最优解平移一步来初始化。这样即使状态突变，初始猜测也相对合理，求解成功率从不足90%提升到接近100%。

6.4 参数失配实验中最容易忽略的坑

做质量突变实验时，几何控制出现了持续振荡，但微分平坦却没有。我一开始以为是几何控制的积分增益不够，后来才发现问题出在质量突变的时间点刚好落在轨迹的一个急剧转弯处——两个大扰动叠加在一起，任何反馈控制器都会出现短暂的动态失调。

这个现象提示了一个实验设计的教训：参数失配实验最好避开剧烈轨迹的转折点，或者将失配发生在轨迹相对平稳的段。如果一定需要做“最恶劣情况”叠加，则要把失配发生时间设定为轨迹的最大曲率时刻，并在论文里明确说明这种情况已经逼近物理可行性的极限。

7. 仿真工程的可扩展方向

这个仿真框架跑通之后，扩展空间其实相当大。

一个非常自然的延伸是把NMPC的预测模型换成学习模型（例如高斯过程回归），这样它就能利用飞行数据在线修正预测偏差。另一个方向是针对INDI的角加速度估计引入无迹卡尔曼滤波或滑模观测器，替代目前的低通滤波器差分方案，可能会大幅提升它在高噪声环境下的表现。

如果要做半实物仿真，这个架构也能直接复用——控制器不变，把动力学模型替换成真实飞控的传感器信号来源，通过Pixhawk或定制硬件在环平台接入MATLAB。最难的部分是系统延迟的建模和补偿，需要调整各算法的输入信号延迟参数。这比从零开始搭建半实物系统要省很多功夫。

如果目标期刊或会议论文，这个对比实验结果加上不同扰动场景的收敛性分析，已经具备可投稿的工作量。建议在论文中重点说明仿真参数的公平性设定和扰动注入的物理依据。

8. 一点个人经验

这套仿真框架我断续调了大概三周，从最初的模型拼凑到最后一键跑完所有场景，最大的感受不是MATLAB代码难写，而是对比实验的公平性设计远比算法本身更费心思。你给某个算法稍微多调几下参数，它在对比时的成绩可能就从“居后”变成“领先”，这种结果说服力不强。所以我的建议是，在做这类多算法对比时，先把所有算法在中等难度场景下各自调到“合格线”，然后用同一套扰动序列高强度测试，全程不做优化调参，最后的结果才经得起同行推敲。

如果你打算用这个项目做毕设或论文支撑，我最后再分享一个小技巧：在实验结果部分，除了RMS误差表格，多放一组“误差概率密度曲线”。它比单纯看均值更能体现算法在极端扰动下的失效概率，审稿人非常买账。祝你们跑道顺利。