四旋翼无人机的姿态控制看着是老话题但真到了动手用Simulink做一套基于自抗扰控制ADRC的仿真时很多人会发现一个尴尬的局面文献读了一大堆模型却不知道从哪里下手MATLAB程序写出来了仿真跑两步就发散ADRC三个字谁都能聊两句但问到扩张状态观测器到底怎么接进Simulink反馈环就卡住了。这篇把我从搭建四旋翼动力学模型、设计ADRC姿态控制器、编写MATLAB初始化脚本到最终调参完整的项目过程整理了出来内容包括可以直接参考的Simulink模型架构、核心代码片段、参数整定顺序以及一份够用的参考文献研读路线。适合正在做毕业设计、课程项目、竞赛准备或者想把四旋翼姿态控制仿真真正落地的人。1. 项目的核心思路与方案选型1.1 四旋翼姿态控制为什么值得做仿真四旋翼是一个典型的欠驱动、强耦合、非线性系统。六个自由度三个位置、三个姿态只有四个控制输入四个电机的转速姿态通道之间还存在陀螺效应和交叉耦合。更麻烦的是实际飞行中还要面对阵风、机体重心偏移、电机响应延迟这些说不清楚的外部扰动。把这些因素全部写进数学模型再推导精确控制律工程上不现实科研上也容易陷入“模型越建越复杂、控制器越来越不实用”的怪圈。仿真平台的意义就在这里先用一个足够贴近物理特性的被控对象模型把控制算法放在理想可控的环境里验证确认算法结构没有方向性问题再去真机上调参。Simulink天然适合做这种工作它把动力学方程、控制器逻辑、观测器结构全部可视化信号流向一眼就能看明白出问题也方便分段排查。做完姿态控制仿真后再扩展位置外环、路径规划、甚至多机编队底子都是同一套。1.2 为什么选ADRC而不是传统PID很多入门方案会用经典PID做姿态控制。PID的优势是简单、参数少、直观但它本质上是基于误差驱动的反馈控制当系统存在明显耦合、参数摄动和外部扰动时PID的控制品质会显著下降积分项抗扰的同时带来超调和相位滞后微分项对测量噪声天生敏感。实际调四旋翼PID的时候横滚俯仰偏航三个通道互相牵扯调完一个通道发现另一个通道又偏了这种体验做过的人都知道。ADRC的核心思路是用扩张状态观测器把“未建模动态 内部耦合 外部扰动”统一看成系统总扰动在控制量里主动补偿掉。它不需要精确的被控对象模型只需要知道控制输入到被控输出的增益估计值b0这正是四旋翼姿态控制最需要的特性。相比滑模控制的抖振问题、LQR对模型精度的依赖、反步法对虚拟控制律求导的繁琐ADRC在工程实现难度和抗扰能力之间取得了非常好的平衡。我在项目里同时试过串级PID和LADRC线性自抗扰同样加阵风扰动的情况下ADRC的恢复速度和稳态精度明显占优。1.3 ADRC控制器整体架构速览一个完整的ADRC姿态控制器由三部分组成跟踪微分器TD、扩张状态观测器ESO和非线性状态误差反馈NLSEF。TD负责给期望姿态角安排一个平滑的过渡过程解决阶跃指令突变导致的超调问题ESO是核心它实时估计系统状态和总扰动NLSEF根据状态误差计算初步控制量最后用ESO估计的扰动做前馈补偿。四旋翼姿态控制通常把控制器按横滚roll、俯仰pitch、偏航yaw三个通道分开设计每个通道一个独立的ADRC。这是因为姿态动力学经过坐标变换后三个通道的主控制量相对解耦剩余的耦合、陀螺效应全部交给ESO当扰动吃掉。这种“单通道独立设计 整体扰动补偿”的思路在工程上非常实用也是大部分参考文献采用的方案路线。2. 基础原理动力学模型与ADRC数学表达2.1 四旋翼动力学模型的建立要点仿真被控对象我采用的是简化刚体动力学模型假设机体坐标系下三个轴的转动惯量分别为Ix、Iy、Iz四个电机提供的推力和力矩经过混控矩阵映射为滚转力矩、俯仰力矩和偏航力矩。姿态动力学方程可以写成横滚角加速度phi¨ ((Iy - Iz)/Ix)·θdot·ψdot (l·U2)/Ix d_phi俯仰角加速度theta¨ ((Iz - Ix)/Iy)·φdot·ψdot (l·U3)/Iy d_theta偏航角加速度psi¨ ((Ix - Iy)/Iz)·φdot·θdot U4/Iz d_psi其中l是机臂长度U2、U3、U4是经过混控后的等效力矩控制量d_phi、d_theta、d_psi是外部扰动。在实际Simulink被控对象模型中我还会在U2等控制量后面串联一个一阶惯性环节模拟电机响应延迟时间常数取0.01s到0.02s之间这样仿真结果更接近真实飞行。这里有一个关键选择仿真被控对象要建模到多精我的建议是“建模完整、控制只看姿态”。位置回路可以后续再建姿态控制验证阶段先把角运动方程跑准电机一阶延迟加上去外加扰动注入通道留好。模型过度复杂会拖慢仿真速度而且模型不准反而会误导控制器设计。2.2 跟踪微分器TD有什么用TD的核心作用是把跳变的期望指令变成一条平滑的期望轨迹。如果直接把阶跃信号作为期望角给到控制器ESO和控制律要瞬间响应一个不可导的突变前期必然出现大的超调和猛烈的控制量冲击。设置了TD之后期望角自己和它的导数都被安排成连续信号控制过程的“瞬态”变得可控。工程上我推荐用线性TD而不是最速控制综合函数fhan。线性TD的数学形式为v1_dot v2v2_dot -r²·(v1 - ref) - 2·r·v2其中r是跟踪速度系数r越大过渡越快但噪声也被放大r太小会拖慢响应。整定思路很简单先根据期望姿态角的变化幅度设定一个“多少秒内到达”的目标反推r的初值再微调。fhan函数虽然有最优快速性但参数语义暧昧而且离散化实现容易出现数值敏感问题线性TD对不追求极致性能的项目完全够用调试友好得多。2.3 扩张状态观测器ESO为什么能“自动补偿扰动”ESO是ADRC的灵魂。它的思想比较直观既然系统方程是x¨ f b0·uf既包含外部扰动又包含内部耦合和未建模动态那我能不能构造一个状态观测器把x、xdot以及f一起估计出来只要f被估出来了控制量里减掉它系统就变成了一个接近“积分串联型”的标准对象剩下的控制就非常简单了。以二阶姿态系统为例对应的三阶扩张状态观测器连续形式为eo z1 - yz1_dot z2 - β1·fal(eo, α1, δ)z2_dot z3 - β2·fal(eo, α2, δ) b0·uz3_dot -β3·fal(eo, α3, δ)这里z1观测姿态角z2观测角速度z3观测扩张状态也就是总扰动f。观测误差eo驱动整个观测器修正β1、β2、β3是观测器增益。经典实现里使用的非线性函数fal定义为当|e|≤δ时fal e/δ^(1-α)否则fal |e|^α·sign(e)。实际调试中我发现一个很实用的小技巧先用线性ESO即fal直接退化成eα1把项目跑通因为线性ESO只有三个增益参数整定规律和观测器带宽强相关β1 3ω₀、β2 3ω₀²、β3 ω₀³即可ω₀就是观测器带宽。等系统稳定运行后再决定要不要把fal的非线性段加上用来加快收敛、提高小误差情况下的估计精度。2.4 非线性误差反馈NLSEF与最终控制率NLSEF部分完成的是“误差到初步控制量”的映射。将TD输出的期望角和期望角速度与ESO估计的当前状态做差得到状态误差e1和e2然后带入非线性组合u0 k1·fal(e1, α1, δ) k2·fal(e2, α2, δ)最终的控制量做扰动补偿u (u0 - z3) / b0这里除以b0的操作就是把估计出来的总扰动从控制通道里去除让系统动态近似变成u0直接驱动积分串联对象。α1、α2一般取0.5和0.25附近这样小误差时增益增大、大误差时增益相对平缓兼顾快速性和稳定性。整套公式在Simulink里实现时我建议直接用MATLAB Function块把TD、ESO、NLSEF写成一个完整的控制函数输入期望角、测量角和b0参数输出控制量。这样比纯框图方案更容易修改算法结构也比S-Function方式少了很多文件管理的麻烦。3. Simulink仿真的整体架构搭建3.1 顶层模型布局从指令到响应Simulink模型我习惯按照信号流方向横着排最左边是期望姿态角指令源中间是三个姿态通道的ADRC控制器再右是被控对象动力学模型最右是示波器和工作区记录模块。模型内部通过Goto/From标签传递信号避免连线拉得又长又乱。期望姿态角我通常用Step阶跃模块默认值0阶跃值为20度约0.35弧度启动时间1秒。如果希望三通道分别加不同指令就用三个Step模块再合并成向量。偏航通道我一般给的指令量小一些比如10度因为偏航响应受电机差速限制给太大指令初期控制量会很激烈。顶层被控对象模块的输入是三个ADRC控制器输出的U2、U3、U4输出则是三个姿态角。为了观察扰动补偿效果我还在被控对象的每个通道上加了一个扰动注入端口用一个Signal Builder生成长度为1秒左右的方波脉冲在仿真运行到第10秒时注入这样可以直观看到ADRC在扰动下的“抗扰曲线”。3.2 四旋翼动力学被控对象的实现方式被控对象有两种常见实现方式一是直接用积分器、增益、加法器搭出来的Simulink框图二是用Level-2 MATLAB S-Function写一套状态方程。我的经验是两套都值得做。S-Function版本适合核心验证。把姿态动力学方程写成状态空间形式x [φ, θ, ψ, φdot, θdot, ψdot]六个状态输入是[U2、U3、U4、扰动向量]基于欧拉方程计算状态导数。这样写的好处是被控对象和控制器完全隔离换控制器算法不影响被控对象代码做对照实验非常方便。框图版本适合看信号内部细节。比如想把陀螺效应耦合项单独引出来看它到底对姿态响应产生了多大影响用S-Function就很难直接看到而框图版本里这些项本身就是独立的信号线拉出来接示波器就行。我在项目里先做S-Function被控对象调通ADRC之后再搭框图版本目的是做耦合效果可视化展示。3.3 姿态通道的ADRC封装设计每一个姿态通道的ADRC控制器我封装成一个独立的子系统对外接口统一是期望角、实测角、控制量三个端口内部包含TD、ESO、NLSEF三块。子系统内部用变量名关联基础工作区的参数比如TD的r、ESO的β1β2β3和δ、NLSEF的k1k2、补偿增益b0。封装的好处显而易见三个通道各用一份参数。横滚俯仰参数大致相同偏航因为转动惯量不同以及执行效率差异b0和控制增益通常要单独整定。如果之后打算做串级ADRC外层位置环内层姿态环内层姿态控制器已经封装好直接在这个子系统的输入前面接一个外环控制器就行改造量很小。4. MATLAB程序设计初始化脚本与核心代码4.1 物理参数与控制器参数的初始化所有参数我习惯集中放在一个init_params.m脚本里每次改完参数直接运行Simulink模型里的变量自动从基础工作区读取。常用的四旋翼物理参数参考值为参数参考值说明m1.2 kg飞行器总质量g9.8 m/s²重力加速度l0.25 m机臂长度Ix, Iy0.012 kg·m²横滚/俯仰转动惯量Iz0.024 kg·m²偏航转动惯量tau0.015 s电机一阶延迟时间常数ADRC控制器参数初值我会给一套能跑通的基准组合。以横滚通道为例TD的r取10ESO带宽ω₀取30此时β190、β22700、β327000δ取0.01NLSEF的k1取45、k2取12b0按照物理关系估算也就是期望控制量和实际加速度之间的关系系数我用的模型里b0大约为20。这套参数不是拍脑袋来的。ESO带宽和TD速度存在匹配关系观测器带宽至少要达到TD过渡过程频率的3到5倍否则观测器跟不上期望轨迹。NLSEF的k1对应自然频率的平方量级45对应约6.7rad/s的自然频率k2取12对应阻尼因子约0.9响应形态是临界阻尼略欠工程上既能快速收敛又不会剧烈超调。4.2 ESO观测器的离散化实现Simulink的MATLAB Function块内默认运行在连续时间框架可以直接写连续微分方程形式。但如果要做嵌入式部署或者想验证离散化算法的效果就需要把ESO离散化。我用欧拉法做离散化采样周期h与Simulink固定步长保持一致比如1ms代码如下function [z1, z2, z3] eso_discrete(z1_prev, z2_prev, z3_prev, y, u, b0, h, beta1, beta2, beta3, delta, alpha) eo z1_prev - y; fal1 fal_fun(eo, alpha, delta); z1 z1_prev h * (z2_prev - beta1 * fal1); z2 z2_prev h * (z3_prev - beta2 * fal1 b0 * u); z3 z3_prev h * (-beta3 * fal1); end function f fal_fun(e, alpha, delta) if abs(e) delta f e / (delta^(1 - alpha)); else f sign(e) * (abs(e)^alpha); end end离散化的一个坑是当采集周期h变大时观测器很快变得不稳定所以固定步长不能随便加大。我在调试中从1e-4步长开始稳定后逐步加到大步长验证极限一般到5e-3就快到临界了所以需要注意数据采样频率和实际部署的控制周期保持匹配。如果用连续时间MATLAB Function块则不需要这么多状态变量直接在函数内使用persistent变量保存z1、z2、z3的积分状态用微分方程的形式更新代码如下function u adrc_controller(y_ref, y_meas, t) persistent z1 z2 z3 v1 v2 if isempty(z1) z1 0; z2 0; z3 0; v1 0; v2 0; end r 10; b0 20; w0 30; beta1 3 * w0; beta2 3 * w0^2; beta3 w0^3; delta 0.01; k1 45; k2 12; % TD v1_dot v2; v2_dot -r * r * (v1 - y_ref) - 2 * r * v2; % ESO eo z1 - y_meas; fal1 fal_fun(eo, 0.5, delta); z1_dot z2 - beta1 * fal1; z2_dot z3 - beta2 * fal1 b0 * u; z3_dot -beta3 * fal1; % NLSEF e1 v1 - z1; e2 v2 - z2; u0 k1 * fal_fun(e1, 0.5, delta) k2 * fal_fun(e2, 0.25, delta); u (u0 - z3) / b0; % 状态更新 v1 v1 0.001 * v1_dot; v2 v2 0.001 * v2_dot; z1 z1 0.001 * z1_dot; z2 z2 0.001 * z2_dot; z3 z3 0.001 * z3_dot; end注意这段代码是示意性的连续近似写法Simulink里最好把0.001步长改成变步长求解器下的连续积分方式否则固定步长限制会影响仿真精度。4.3 仿真结果的可视化分析数据记录我使用To Workspace模块变量名设为tout和yout格式选择Timeseries。仿真结束后在主脚本里直接画出姿态角响应、控制量曲线以及ESO的扰动估计曲线和注入的实际扰动的对比。扰动对比是验证ADRC最直观的环节如果ESO工作正常z3曲线应该能“贴”住注入扰动的形状相位延迟越小越好。处理曲线时我习惯同时画两条曲线一条是期望角TD输出一条是实际角。如果已经调到“实际角贴着期望角走”说明控制器工作正常如果两条线之间有明显稳态偏差多半是ESO的z3没有收敛或者b0估计偏差太大如果实际角震荡明显大概率是观测器带宽过大导致噪声放大或NLSEF增益过高。5. 参考文献整理与研读路线5.1 自抗扰控制理论与MATLAB仿真入门文献第一个必读方向是ADRC奠基者的原始理论文章。自抗扰控制的概念由国内学者提出相关奠基性论文详细阐述了从PID到ADRC的理论逻辑包含TD、ESO、NLSEF三大件的完整设计思路和稳定性分析。这篇文献虽然年代比较久但它是整个ADRC理论体系的源头里面很多关于“为什么PID不能满足高性能抗扰需求”的论述直到今天读起来仍然没有过时。第二个必读方向是ADRC的系统性综述。控制领域有几位学者把ADRC做了非常系统的总结和推广包括线性自抗扰控制LADRC的频域设计方法、参数整定公式以及和PID的结构对比。这类综述文章信息量很大适合在看完奠基文献之后建立整体坐标系明确哪些问题已经有理论支撑、哪些问题学术界还在争论中。第三个方向是MATLAB/ Simulink动力学仿真参考书。四旋翼ADRC仿真的技术短板往往不是控制理论本身而是不熟悉Simulink建模规范和状态空间建模方法。建议找一本把Simulink建模从基本模块讲到S-Function编写的参考书不需要全读重点看状态方程建模、子系统封装、数据记录三章就够用。5.2 四旋翼ADRC应用方向的代表作应用层面的文献重点找两类。第一类是专门讨论四旋翼姿态ADRC控制的学位论文这类论文通常会把动力学模型推导、控制器设计、仿真分析和实物实验全套走一遍非常适合作为项目模板参考。看学位论文重点看两处一是控制器的Simulink实现结构二是调参的详细过程学位论文篇幅长作者一般会交代很多实操细节这是期刊论文给不了的。第二类是讨论前馈补偿和传感器噪声抑制问题的刊物文章。四旋翼ADRC的实际性能上限往往不是控制器的抗扰能力而是加速度计和陀螺仪的噪声水平因为ESO要同时估计状态和扰动测量噪声会被观测器增益放大。近几年的改进方法包括级联扩张状态观测器、降阶ESO、基于IMU预积分的前馈结构等。这类文章逻辑清晰、形式短小适合读完之后做算法升级。英文部分主动检索的标题方向包括“active disturbance rejection control quadrotor attitude”“linear active disturbance rejection control UAV”等组合关键词重点关注那些建立了严格稳定性分析并给出完整仿真曲线的文章这类文章仿真条件描述详尽方便复现对照。5.3 高效的文献阅读顺序建议我给一个已经验证过非常有效的阅读顺序先用半小时快速浏览入门综述知道ADRC能干什么不能干什么然后精读奠基者原版论文把ESO公式和符号体系弄熟接着读一篇Simulink建模教程类文献把模型的“硬骨架”搭好之后再读报名报道性学位论文对照它给出的实例数据调整自己的初参最后再根据自己调试中遇到的具体问题精准检索针对性的期刊论文。不要把参考文献留到代码调完才整理应该在选型阶段就开始读奠基文献和综述在建模阶段读Simulink教程在调参阶段读应用论文每一个阶段需要的文献类型不同。项目做完之后你会发现真正被频繁翻阅的文献也就五六篇其余大多是查个公式就放下所以没必要一开始贪多求全。6. 常见问题、调参经验与避坑实录6.1 仿真发散和模型初始化问题仿真一开始就输出NaN或者数值爆炸几乎都是三类问题ESO初值和被控对象状态初值不一致、控制器内部存在代数环、b0符号或数值量级错误。ESO初值问题最隐蔽控制器开始运行时z1初始值设为0但如果被控对象初始姿态也是0看起来一致实际运行后若指令一进来观测器就出现很大的瞬态偏差。解决办法是让ESO的z1初始值等于被控对象初始输出值z2初始值等于被控对象初始角速度z3初始值等于0。b0量级错误是另一个高发问题。如果被控对象的真实输入增益是20ESO里假定b01那么z3估计的总扰动会把真实的控制作用当成扰动去补偿控制量会不断漂移直至发散。检查b0是否合理的简单方法是控制器先不接ESO的补偿项等价于去掉z3只保留反馈控制如果系统还能稳定且控制量不过大说明b0方向正确如果控制量反向振荡优先排查b0符号和量级。6.2 ESO跟踪性能差的处理思路ESO的z1跟踪实测姿态角时如果出现明显滞后优先提高观测器带宽ω₀但代价是噪声放大。我在调参时发现一个规律观测器带宽和被控对象自然频率的比值维持在20到40之间比较合适太低则抗扰能力差太高则噪声问题压不住。如果带宽已经很高但z3仍不收敛问题往往出在b0偏差上——b0偏差30%z3的估计结果就会明显偏斜。如果ESO在扰动突变瞬间跟踪速度慢可以考虑改用串联形式的观测器结构即角速度环和角度环各自拥有独立的ESO。这类改进在四旋翼实物系统上非常常见因为内环采样频率更高更适合独立调整观测器带宽也能规避测量噪声在高带宽下的放大问题。6.3 实战调参顺序与经验参数我建议的参数整定顺序是TD参数先整定因为期望轨迹决定了系统响应的“基线形态”然后ESO的三个β参数先保证观测器能跟上真实状态最后整定NLSEF增益。切忌一上来就疯狂调k1、k2ESO没调好之前你觉得自适应律有问题、其实是估计错了扰动。给一套横滚通道的起始经验值TD的r8~12ESO带宽ω₀25~40NLSEF的k140~60、k210~18b0按物理估算±20%范围调整。这套参数下姿态阶跃响应约1.5秒稳定超调轻微。偏航通道由于转动惯量大b0要适当调小k1和k2可以保持同一量级实测下来偏航的响应速度会略慢一些这是正常现象。6.4 典型问题速查表现象可能原因处理办法仿真秒级发散b0符号/量级错误断开z3补偿先确认反馈方向姿态响应大超调TD过渡速度过快降低r让期望轨迹变化更缓稳态有微小振荡NLSEF增益过高或δ过小减小k1/k2增大δ扰动下恢复慢ESO带宽不足增大ω₀并确认b0偏差在20%内高频抖动ESO带宽过大或采样步长过大降低ω₀减小固定步长z3无法跟踪真实扰动b0估计偏差过大修正b0后再调ESO增益三通道耦合时乱跳缺少前馈补偿提高ESO带宽或改用级联ESO这套速查表是我自己在项目里一遍遍对出来的对照调试比逐项猜要高效得多。6.5 项目扩展的几个方向姿态ADRC跑通之后往上加位置外环是基于同样的思路往下做真机移植则需要把离散化步长和实际控制频率对齐。如果想让模型更贴近真实可以给测量环节叠加高斯白噪声模块默认噪声功率设置大一点观察ESO是否还能保持稳定这是考验控制器鲁棒性非常实用的测试方式。更强的进阶方向是将ADRC与模糊控制或神经网络结合让b0根据飞行状态自适应调节这类组合方案也是控制领域不太容易过时的研究方向。我个人在实际操作中的体会是四旋翼ADRC仿真项目最大的瓶颈从来不是理论公式的推导而是Simulink建模规范和调参经验的积累。第一次搭模型花了整整一天踩了代数环和初值不一致两个大坑第二次只用了半天就完成了三通道ADRC的全部搭建和基本调参。所以如果你现在被仿真发散卡住不要太自我怀疑把ESO和TD初值检查一遍把b0单独拿出来单测大概率就能跑通。这个项目做完之后你会发现后面无论是做其他飞行器算法还是写学位论文都变得顺了很多因为ADRC这套“建模不精也能抗扰”的思路本身就是一套非常好用的方法论。