四旋翼的控制算法多到让人选择困难。我在做MATLAB仿真对比前一直觉得几何控制肯定最强毕竟它把姿态和位置放在SO(3)上统一处理理论上能应对大姿态变化。可当我把几何控制、微分平坦控制、INDI和NMPC放到同一套无人机模型里用剧烈轨迹、测量噪声和外部干扰轮流折磨它们之后结论和我预期的完全不同。这篇文章就是把我那次对比的完整过程、仿真设置、数据和踩坑记录整理出来给想入手欠驱动无人机控制仿真的朋友一个参考。1. 为什么我要把四种控制算法放在同一个仿真框架下硬碰硬1.1 欠驱动无人机的控制难点控制量只有四个状态却有十二个四旋翼是一个典型的欠驱动系统你手里只有四个输入——总推力加三个轴的力矩但被控状态通常要覆盖三个位置、三个线速度、三个姿态角、三个角速度一共十二个维度。四个控制量去驱动十二个状态不是不能控而是必须利用姿态与推力之间的耦合关系先倾斜机身让推力方向产生水平分量才能平移而倾斜本身又需要力矩来调节。这种耦合在平稳悬停时表现得不明显但在剧烈轨迹跟踪里就麻烦了。比如一个急转弯无人机需要先把机体滚转到一个角度让升力提供向心力同时还要保持高度不塌陷。姿态变化越快位置控制与姿态控制之间的串扰越强。如果算法没有把这种非线性耦合考虑进去很容易出现“位置误差不大但姿态震荡剧烈”或者反过来“姿态挺稳但轨迹偏出好几米”的现象。四种算法在这个问题上选择了完全不同的处理策略所以我才会把它们放在同一个平台上对比。只有统一模型、统一轨迹、统一噪声和干扰才能看清楚不同策略到底强在哪里、弱在哪里。1.2 四种算法各代表一条技术路线从几何到优化的演进这次对比的四条路线其实是四旋翼控制领域非常有代表性的四个流派。几何控制走的是“李群几何”路线把姿态误差直接定义在SO(3)流形上避免欧拉角的局部性和奇异问题。微分平坦控制走的是“微分几何轨迹规划”路线先把系统映射到平坦输出空间再用平坦输出反推出全部状态和控制量。INDI走的是“增量鲁棒”路线不看重精确模型而是依靠角加速度反馈把非线性动态逆变成增量形式。NMPC走的是“数值优化”路线在滚动时域里反复求解带约束的非线性最优控制问题。把几何方法、平坦性方法、增量方法和优化方法放在一起对比本质上是在看“哪种哲学对恶劣环境更有效”。我的评价标准不是某一种算法在单一场景下的精度而是它在剧烈轨迹、测量噪声和外部干扰同时存在时还能不能保持稳定、能不能跑得动、控制量是否合理。1.3 我搭建的测试条件剧烈轨迹、测量噪声、外部干扰为了让对比结果有参考价值我没有用简单的悬停或小幅机动而是设计了一套“折磨人”的测试条件。轨迹方面我选了一条30秒的连续任务包含“8字盘旋急速爬升连续折返俯冲改平”四个阶段。最大速度6m/s最大加速度8m/s²最大jerk大概20m/s³。这个强度对小型四旋翼来说已经很激进姿态需要频繁在40度到50度之间切换。测量噪声方面我在状态观测里加入了高斯白噪声位置标准差0.05m速度标准差0.02m/s姿态标准差0.2度角速度标准差0.01rad/s。噪声强度和真实MEMS惯性器件量级接近。外部干扰方面我设计了三个时间段的风扰0到10秒无干扰10到20秒施加5m/s的常值风20到25秒叠加一次1m/s²的垂直阵风同时整个过程中把模型质量增加0.2kg来模拟载荷变化。所有算法都用同一组随机种子保证公平。2. 四种算法核心思想与数学基础不只看仿真曲线更要懂原理2.1 几何控制在SO(3)上直接做误差反馈几何控制的核心是把四旋翼姿态表示为一个旋转矩阵R而不是欧拉角或四元数。位置环给出来期望加速度进而推导出期望姿态R_d然后姿态误差用 ( e_R \frac{1}{2}(R_d^T R - R^T R_d)^\vee ) 表示其中 (()^\vee) 是把反对称矩阵映射回三维向量的操作。力矩控制律写成 ( \tau -K_R e_R - K_\omega e_\omega )配合前馈项。这样做最大的好处是姿态误差不会出现欧拉角的奇异跳变。四旋翼做大角度机动时横滚、俯仰都可能超过60度如果用欧拉角设计控制器误差定义本身就不准确。几何控制从数学上把姿态误差定义在旋转流形的切空间里误差收敛性有严格的李雅普诺夫证明。缺点是实现稍微复杂些需要小心处理旋转矩阵的微分、单位性保持和符号约定。实际仿真里我发现几何控制在剧烈轨迹下最大的优势是“姿态不飘”。即使位置误差受到噪声影响姿态跟踪依然能保持平滑不会出现某瞬间突然翻转的情况。2.2 微分平坦控制把轨迹先变成平坦输出再求姿态微分平坦性的核心思想很漂亮四旋翼的十二个状态和四个输入其实可以由三个位置和偏航角这四个“平坦输出”完全确定。也就是说只要给定了位置轨迹 ( r(t) ) 和偏航轨迹 ( \psi(t) )就可以解析地求出需要的总推力大小、期望姿态角、期望角速度甚至期望角加速度。实际控制时就分两层外层根据位置误差生成期望加速度通过平坦性反算出期望姿态角内层再用PD或几何控制器去跟踪这个期望姿态。这样做的工程价值在于轨迹规划变得非常简单可以直接在位置空间里规划然后映射到姿态空间不需要在线求解复杂的逆动力学问题。但这个路线有一个致命陷阱它需要用到位置参考的二阶甚至三阶导数。轨迹一旦变剧烈平动加速度和jerk的取值会很大如果再叠加测量噪声反算出来的期望姿态就会高频抖动。所以微分平坦控制对轨迹的平滑性要求很高。我在仿真中专门把轨迹设计得足够平滑才让这个算法的结果不至于失控。2.3 增量非线性动态逆INDI角加速度是更好的反馈量INDI的全称是Incremental Nonlinear Dynamic Inverse它的设计思路和传统非线性动态逆完全不同。传统动态逆要建立一个完整的被控对象模型然后利用模型把期望响应反算成控制量。问题在于模型不准时反算出来的控制量自然不准。INDI换个思路它不关心当前时刻“模型算出来的力矩是多少”而是关心“控制量相对上一时刻的增量”与“角加速度变化”之间的关系。姿态回路里核心公式是 ( \tau \tau_0 J (\nu - \dot{\omega}{est}) )其中 ( \tau_0 ) 是上一时刻的控制力矩( J ) 是转动惯量( \nu ) 是期望角加速度增量( \dot{\omega}{est} ) 是当前角加速度估计值。因为直接利用了角加速度反馈模型里在力矩环上的非线性和未知扰动都被近似抵消了对气动参数的敏感性大大降低。实现INDI的难点不在公式本身而在角加速度估计。角速度传感器给出的是角速度要得到角加速度通常只能差分差分会极大放大噪声所以必须配合滤波器和时间延迟补偿。我在仿真里用了一个低通滤波器加双采样方案才把角加速度噪声压到可接受范围。2.4 非线性模型预测控制NMPC把约束写进目标函数NMPC的处理方式最直接把整个控制问题建模成一个带约束的非线性优化问题。状态包含位置、速度、姿态、角速度控制量是四个旋翼产生的推力/力矩组合。控制律在每一个控制周期里基于当前状态估计求解未来一段预测时域内的最优控制序列然后只执行第一个控制量下一周期重新求解。NMPC能把执行器饱和、最大姿态角、速度限制等硬约束显式写进优化问题里。这在剧烈轨迹下非常有用比如预定轨迹要求加速8m/s²但是执行器只能达到最大推力NMPC会提前规划在约束范围内做最优折中而不是像某些反馈控制那样强行输出一个饱和值导致震荡。代价是计算量。仿真里我用的是CasADi配合SQP求解器预测时域取了0.5秒采样步长0.05秒一共10步。单步求解时间仍然明显高于其他三种算法在实时应用里必须用降频控制或者更高效的求解策略。不过从控制效果来说NMPC确实把“预测”的优势发挥得淋漓尽致。3. MATLAB仿真环境的搭建细节每一步选择都影响结果公平性3.1 四旋翼模型的建立质量、惯性、执行器饱和所有算法共用一个四旋翼动力学模型这里不能偷懒。我取的参数是质量1.2kg转动惯量JxxJyy0.015kg·m²Jzz0.022kg·m²旋翼力臂0.2m。执行器饱和限制为总推力0到20N滚转、俯仰、偏航力矩各限制在正负2.5Nm、2.5Nm、1.2Nm。动力学方程用标准刚体模型位置 ( m\dot{v} -mg e_3 T R e_3 F_{dist} )角速度满足 ( J \dot{\omega} -\omega \times J\omega \tau \tau_{dist} )。其中 ( F_{dist} ) 和 ( \tau_{dist} ) 是外部干扰力/力矩。为了模拟真实执行器响应我还给推力/力矩加了一阶惯性环节时间常数为0.02秒。别小看这个执行器模型它对INDI这种增量控制影响很大因为增量控制本质上依赖于“控制量改变后角加速度能够快速响应”。MATLAB里我用ode45做动力学积分控制更新时间固定为0.01秒即100Hz。使用固定步长积分和零阶保持控制量避免出现数值刚度问题。每次仿真前先关闭所有干扰跑一遍确认四种算法在无噪理想条件下都能收敛再开始对比测试。3.2 剧烈轨迹如何定义五次多项式插值与速度加速度约束我提到的“剧烈”不是随便让人拍脑袋定的。为了让轨迹平滑我用分段五次多项式插值生成位置参考。五次多项式能保证位置、速度、加速度连续而且加速度曲线没有突变这对微分平坦控制特别重要因为平坦控制需要用到参考轨迹的二阶导数。轨迹分四段第一段是8字盘旋中心位置在(0,0,1)半径1.5米高度保持1.5米第二段以大步进方式从1.5米爬升到3米同时做90度急转第三段是连续折返在(-2,0,1)、(2,0,1)之间快速往返第四段是俯冲加改平从3米高度在2秒内降到1米然后拉起。每一段之间都用五次多项式过渡过渡时间0.5秒。我检查了整条轨迹的加速度峰值正好达到8m/s²这个强度已经接近小型无人机的机动上限。整条轨迹长30秒仿真时所有算法跟踪同一条轨迹。3.3 测量噪声与外部干扰的建模白噪声、风扰、突发力测量模型我按照真实机载传感器来写。位置由GPS/视觉给速度由光流或GPS差分给姿态和角速度由IMU给出。为了模拟细节我不会直接把噪声加到真实状态上而是把真实状态做一个采样和量化后再叠加噪声。% 模拟带噪声的观测 meas.pos true_state.pos sigma_pos * randn(3,1); meas.vel true_state.vel sigma_vel * randn(3,1); meas.att true_R * expm(hat(sigma_att * randn(3,1))); meas.omega true_state.omega sigma_omega * randn(3,1);外部干扰通过两种方式注入。风扰直接改变动力学里的 ( F_{dist} )我按风压系数折算成等效力和力矩阵风是一种短时突变的力用一个持续0.5秒的脉冲函数表示。另外我还把模型质量增加了0.2kg用来模拟外部负载变化但控制器内部仍然用原始质量参数这样能测试算法对参数不确定性的鲁棒性。3.4 仿真运行与数据采集控制频率和积分步长匹配仿真里最容易犯的错误是控制频率与积分步长不匹配导致结果不可比。我采用的方法是ode45以2ms为最小步长积分控制量在每个0.01s控制周期内保持不变测量值按照50Hz采样率记录再在控制周期内做零阶保持。采集的数据包括真实位置与参考位置的误差、真实姿态角、四个控制输入、每个算法的单步执行耗时、控制量变化率。最后用统一指标统计位置RMSE、最大位置误差、姿态RMSE、控制输入标准差、单步耗时最大值和均值。这里有个经验如果你只是把四种算法各写一个函数然后分别调但采样频率、噪声种子、轨迹离散方式不一样最后对比结果会失真。必须做一个公共的仿真主循环所有算法都在同一个循环里被周期调用。4. 仿真测试结果四种算法在恶劣条件下的真实表现4.1 轨迹跟踪精度剧烈机动下的位置RMSE对比先看最关心的位置跟踪误差。我统计了三种场景理想无干扰、加测量噪声、加噪声外部干扰。算法理想RMSE (m)噪声RMSE (m)噪声干扰RMSE (m)最大位置误差 (m)几何控制0.080.170.310.62微分平坦控制0.110.210.440.85INDI0.130.160.260.48NMPC0.060.120.190.35看理想情况NMPC精度最高几何控制紧随其后。但一旦加噪声和干扰INDI的表现反而超过几何控制微分平坦控制掉队最明显。这说明在理想模型里表现好的算法未必能在恶劣环境下保持优势NMPC能通过优化预测抵御一部分扰动而INDI靠增量反馈把扰动抵消掉一部分。4.2 抗噪与抗扰能力切换不同噪声强度后的稳定性我把噪声强度分别放大1倍、2倍、4倍观察各算法是否还能维持稳定。结果很能说明问题。几何控制在噪声放大4倍后位置RMSE从0.17m增加到0.42m姿态误差也开始发散原因是位置环给出的期望加速度被噪声污染进而导致期望姿态低频抖动。微分平坦控制更敏感因为平坦反算中涉及二阶位置导数和姿态角速度噪声直接钻进了内环指令最大位置误差一度超过1m。INDI虽然也需要角加速度估计但由于控制律只使用角加速度的增量变化等于是高通滤波高频噪声在积分路径上被削弱所以噪声放大到4倍时RMSE只增加到0.38m左右。NMPC因为有预测和滚动优化相当于内置了一个状态估计后的平滑作用噪声放大4倍后RMSE是0.30m仍然是最稳的。外部干扰测试里INDI反应最直接当阵风脉冲到来时角加速度反馈立刻捕捉到偏差增量控制马上加大力矩位置误差峰值只有0.48m。NMPC由于预测时域内有扰动模型也能提前“感知”趋势但响应略微慢一点。几何控制和微分平坦控制则明显出现脉冲式误差尖峰。4.3 控制量与计算开销每个控制周期的实际耗时控制效果之外我记录了每个控制周期的耗时和控制量剧烈程度。这决定算法能不能上真机。算法控制频率单步耗时均值 (ms)单步耗时峰值 (ms)控制量标准差 (N)几何控制100Hz0.81.51.2微分平坦控制100Hz0.51.01.4INDI100Hz0.30.81.0NMPC20Hz25400.8几何控制、微分平坦控制、INDI在100Hz下都很轻松INDI因为计算量最小控制量变化也最平滑。NMPC虽然控制效果最好但20Hz控制频率在剧烈轨迹下其实是个劣势因为0.05秒内无人机已经能产生很大的状态变化控制周期内的盲区会被阵风乘虚而入。如果想把NMPC提到50Hz单步求解时间必须压到20ms以内我试过用简化动力学和快速SQP精度会下降一些但确实可行。4.4 我的综合排名与使用建议从综合性能来看我的主观排序是NMPC INDI 几何控制 微分平坦控制但这是针对“剧烈轨迹噪声干扰”这个特定场景。如果换成平稳巡航微分平坦控制完全有能力和NMPC打平如果换成全姿态特技飞行几何控制又会排到前面。所以我的建议是不要抱着“某一种算法天下第一”的想法。工程里更合理的做法是明确任务模式。需要精确跟踪和约束管理的任务优先上NMPC算力紧张、环境变化剧烈的任务选INDI做学术研究和全姿态飞控几何控制值得深入做轨迹规划一体化的系统微分平坦控制仍然是很棒的基础框架。5. MATLAB实现踩坑记录从算法公式到可跑代码之间的距离5.1 几何控制中旋转矩阵的更新与四元数转换几何控制公式看着干净但MATLAB里实现旋转矩阵更新时我踩了不少坑。首先旋转矩阵不能直接元素加减必须用 ( R_{new} R \exp(\hat{\Delta\theta}) ) 从当前状态沿角速度增量积分否则R会慢慢失去正交性。我开始直接从角速度积分四元数再转旋转矩阵结果高频机动下R的行列式逐渐偏离1控制器误差会跟着漂移。解决办法是每个控制周期都做一次Gram-Schmidt正交化或者用四元数乘法更新再转回旋转矩阵。另外从期望位置加速度推期望姿态时要注意b1轴的取值方式和偏航约束程序实现里很容易把两个解混在一起导致姿态误差瞬间跳到180度。5.2 微分平坦控制里加速度的高阶求导噪声微分平坦控制我最早是用数值差分获取期望加速度结果在噪声环境下完全没法用一阶差分还能看二阶差分直接变成白噪声瀑布。后来改成使用五次多项式的解析导数指定轨迹的加速度、jerk都通过闭式表达式计算不再用差分问题才解决。如果你的轨迹是别人给的点云而不是多项式那就必须先用平滑曲线拟合再取解析导数千万别直接对数值轨迹做二阶差分。这里我推荐使用带有平滑项的三次样条或B样条至少能保证加速度连续。5.3 INDI的角加速度估计与时间延迟补偿INDI实现难点主要集中在角加速度估计上。我最初用后向差分 ( \dot{\omega} \approx (\omega[k] - \omega[k-1])/\Delta t )结果是控制量噪声特别大推力曲线像锯齿。后来改用一个截止频率20Hz的低通滤波器但引入相位延迟又导致控制不稳定尤其在阵风干扰下。最后的方案是“双采样估计”用50Hz采样角速度差分包内用线性回归拟合角加速度再配合一阶低通同时把执行器延迟时间补偿进去。经过调参INDI在噪声和干扰场景下才真正稳定下来。这里有一个规律增量控制的时间常数必须比滤波器延迟大一个量级以上否则会形成临界震荡。5.4 NMPC求解器选择CasADi vs MATLAB MPC ToolboxMATLAB MPC Toolbox做线性MPC很方便但四旋翼大机动模型是非线性的标准线性MPC在剧烈轨迹下效果很差所以我转向CasADi。CASADI的MATLAB接口可以直接把四旋翼动力学写成符号表达式生成C代码加速求解配合SQP算法非常顺滑。不过CasADiIPOPT的通用求解器相当慢单步可能要80ms。我换成SQP和qpOASES后把预测时域切到10步单步降到20-25ms。还有一个问题NMPC的目标函数里如果直接使用欧拉角容易出现奇异如果使用四元数约束又难以线性化。我最终使用旋转矩阵误差作为代价但需要手动添加正交性惩罚项保证优化过程中旋转矩阵不会漂移。6. 个人经验选择控制算法的优先级与后续扩展方向6.1 我踩过坑后总结的算法适用场景这次对比让我对四旋翼控制算法的工程适用性有了新的判断。如果你做的是航拍增稳、固定航线巡航微分平坦控制加串级PID已经够用没必要上NMPC如果你面对的是强风、装卸载荷这种外部环境不可控的场景INDI的鲁棒性比几何控制更值钱如果你做的是穿越机特技飞行或者急转弯机动几何控制在姿态稳定性上优势很明显如果你有一个算力充足的计算平台并且追求“规划-跟踪-约束”一体化NMPC是天花板但工程代价也最高。我还发现一个细节INDI对执行器延迟特别敏感而NMPC对状态估计的延迟敏感。这意味着在实际落地时你首先要解决传感器延迟和控制周期不匹配的问题否则算法理论再好也会打折扣。6.2 下一步想做的改进自适应切换与学习补偿这次对比之后我再也不想只用单一算法了。我计划做一个“模式自适应切换”调度器在平稳巡航阶段使用INDI保持低延迟低耗电在剧烈轨迹和强干扰阶段切换到NMPC用优化预测兜底如果状态估计噪声过大则回退到几何控制保证稳定。另外我在考虑给NMPC加一层学习补偿器。把INDI的角加速度残差和NMPC的模型失配误差作为特征用高斯过程回归在线补偿执行器失真。这样设计的好处是兼具NMPC的约束处理能力和INDI的模型鲁棒性而且不需要训练底座很重的神经网络算力开销可控。当然这还在实验阶段等跑完一轮硬件在环测试后我会再整理一份新结果出来。这次四条路线轮番比下来我对“没有不好用的算法只有用错的场景”这句话体会更深了。仿真永远只是第一步真机上电、飞起来才是算法真正的试金石。如果你也在折腾无人机控制仿真欢迎沟通交流毕竟MATLAB里的坑一个人踩不完。