四旋翼无人机的姿态控制看着是老话题，但真到了动手用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 = v2
- v2_dot = -r²·(v1 - ref) - 2·r·v2
其中r是跟踪速度系数，r越大过渡越快但噪声也被放大，r太小会拖慢响应。整定思路很简单：先根据期望姿态角的变化幅度设定一个“多少秒内到达”的目标，反推r的初值，再微调。fhan函数虽然有最优快速性，但参数语义暧昧，而且离散化实现容易出现数值敏感问题，线性TD对不追求极致性能的项目完全够用，调试友好得多。
2.3 扩张状态观测器ESO为什么能“自动补偿扰动”
ESO是ADRC的灵魂。它的思想比较直观：既然系统方程是x¨ = f + b0·u，f既包含外部扰动又包含内部耦合和未建模动态，那我能不能构造一个状态观测器，把x、xdot以及f一起估计出来？只要f被估出来了，控制量里减掉它，系统就变成了一个接近“积分串联型”的标准对象，剩下的控制就非常简单了。
以二阶姿态系统为例，对应的三阶扩张状态观测器连续形式为：
- eo = z1 - y
- z1_dot = z2 - β1·fal(eo, α1, δ)
- z2_dot = z3 - β2·fal(eo, α2, δ) + b0·u
- z3_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模型里的变量自动从基础工作区读取。常用的四旋翼物理参数参考值为：
| 参数 | 参考值 | 说明 |
|---|---|---|
| m | 1.2 kg | 飞行器总质量 |
| g | 9.8 m/s² | 重力加速度 |
| l | 0.25 m | 机臂长度 |
| Ix, Iy | 0.012 kg·m² | 横滚/俯仰转动惯量 |
| Iz | 0.024 kg·m² | 偏航转动惯量 |
| tau | 0.015 s | 电机一阶延迟时间常数 |
ADRC控制器参数初值我会给一套能跑通的基准组合。以横滚通道为例，TD的r取10，ESO带宽ω₀取30，此时β1=90、β2=2700、β3=27000，δ取0.01，NLSEF的k1取45、k2取12，b0按照物理关系估算，也就是期望控制量和实际加速度之间的关系系数，我用的模型里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量级错误是另一个高发问题。如果被控对象的真实输入增益是20，ESO里假定b0=1，那么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、k2，ESO没调好之前，你觉得自适应律有问题、其实是估计错了扰动。
给一套横滚通道的起始经验值：TD的r=8~12，ESO带宽ω₀=25~40，NLSEF的k1=40~60、k2=10~18，b0按物理估算±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这套“建模不精也能抗扰”的思路，本身就是一套非常好用的方法论。
