做无人机姿态控制的人，几乎都体验过那种“调PID调到失去耐心”的状态。姿态环增益大了抖，小了飘；模型里转动惯量差个20%，仿真和真机完全两回事；再来一阵侧风，悬停直接跑偏。我一开始看到“动态反演 + 扩展状态观测器ESO + 自适应”这种组合时，第一反应是：这又是在哪篇论文里飘着的华丽组合？但当我真正照着论文把Simulink模型搭出来、把m脚本跑起来，才发现这套方案的实际价值比想象中高太多。它不仅把“模型不准”和“外部扰动”两个问题分而治之，而且结构清晰，调参思路明确，非常适合做无人机、机器人、航天器等非线性姿态系统的控制器开发或者算法复现。这篇文章我会把整套控制器的设计思路、公式细节、Simulink搭建过程、m脚本初始化方法，以及我踩过的调参坑一次性讲透，照着做你也能复现出来。

1. 先理清楚：这套控制器到底想干掉什么问题

1.1 姿态控制里真正难缠的三类不确定性

无人机姿态控制之所以难，并不是因为“非线性”本身有多吓人，而是因为真实系统里同时存在好几类不确定性。第一类是模型参数不确定性，最常见的就转动惯量算不准，机架焊接误差、载荷变化、电池位置移动都会让惯量估计值偏掉百分之二三十；第二类是外部扰动，阵风、洗流、螺旋桨效率下降，全都表现为额外力矩，而且往往是时变的；第三类是建模误差，比如电机电调延迟、桨叶挥舞动力学、机架弹性模态，这些在简化模型里根本不存在，但它们真实存在，会在高频段把控制器“咬”一口。

这三类不确定性同时出现时，传统PID的局限性就很明显了：PID本质上是在“不知道模型”的情况下靠误差信号压制扰动，为了压制慢变扰动需要高积分增益，但高积分增益和相位滞后又会限制整体带宽，最终往往导致“抗得了风扰就稳不住姿态，稳得住姿态就压不住振动”。所以我在复现这套方案时最深的感受是：它解决的问题不是某一个具体工况，而是把上述三类不确定性拆分到不同模块里，各管一段，这才是这个控制器的灵魂。

1.2 为什么把动态反演、ESO和自适应焊在一起

标题里提到的三个词，其实代表了三种完全不同的控制哲学。动态反演，也叫动态逆，本质是反馈线性化的一种工程化实现：我们知道无人机姿态动力学里有一大堆陀螺力矩、交叉耦合项，这些项虽然非线性，但可以建模，那就干脆构造一个逆模型把它们精确抵消掉，将系统“掰”成几个独立的积分器；扩展状态观测器ESO则继承了自抗扰控制的思路，把所有建模误差和外部扰动合在一起，当作一个扩张出来的状态去实时估计，然后在前向通道里直接补偿；自适应环节负责的是“在线更新模型中的慢变参数”。

把三者焊在一起的理由也很直白：动态逆负责把非线性部分消掉，但它要求模型准确，模型不准逆得就不干净；ESO能吃掉模型不准造成的残差和外部扰动，但观测器带宽有限，面对快速变化的扰动会力不从心；自适应恰恰能在线修正惯性参数这类慢变偏差，从源头减轻ESO的负担。换句话说，三者的分工是“动态逆做主干，ESO兜底，自适应治本”，互补性非常强，这也是这个控制器鲁棒性的来源。

1.3 整体信号流：从角度指令到力矩输出

整套控制器的信号流可以分成四层去看。最外层是姿态环，输入期望姿态角，比如期望俯仰角30度，输出期望角速度；第二层是角速度环，也是整个控制器的核心，它接收期望角速度和实测角速度的误差，结合动态逆模型计算控制力矩；第三层是ESO，它实时观测角速度通道里的总扰动，把估计值反馈到角速度环进行补偿；第四层是自适应模块，在线输出慢变参数的修正量，叠加到力矩指令上。四个模块按“姿态指令 → 期望角速度 → 控制力矩 → 实际角速度/姿态”的链路串起来，最终输出的就是油门机构需要的三个力矩指令。

关键是，这个结构里的每个模块都能独立调试、单独验证。我复现时就是先把姿态环用纯PD跑通，再往上加ESO，最后加自适应，每加一层都能明显看到效果，这种循序渐进的方式比直接丢一个黑盒模型要稳妥得多。

2. 控制器的完整设计逻辑与公式推导

2.1 姿态模型与符号约定

我先说清楚模型，因为整个控制器所有的公式都从这里来。姿态角用欧拉角表示，记作Ω = [φ, θ, ψ]^T，分别对应滚转、俯仰、偏航；本体系角速度用ω = [p, q, r]^T。在小角度或悬停飞行条件下，姿态运动学可以简化为Ω_dot ≈ W(Ω)ω，W是欧拉角到角速度的转换矩阵，该矩阵在俯仰角接近90度时会出现奇异，所以工程上做大机动时要换四元数，但做姿态稳定和中等幅度机动，欧拉角完全够用。

动力学方程是刚体姿态控制最核心的一行：

J · ω_dot = -ω × (J·ω) + τ + d

其中J是转动惯量矩阵，实际无人机通常近似为对角阵diag([Jx, Jy, Jz])；τ是三轴控制力矩；d是外部扰动力矩。方程右边的-ω × (J·ω)就是陀螺力矩，它是roll、pitch、yaw三个通道之间互相耦合的根源，也是“反馈线性化”最想要抵消掉的东西。如果忽略这个耦合项而直接对三个通道分别做PID，姿态在做大速率机动时就会出现明显的通道串扰。

2.2 动态反演：把非线性项“逆”掉的原理

反馈线性化的核心思想其实很简单：既然我们知道非线性项长什么样，那就把它算出来，再在控制输入里减去它，这样系统剩下的部分就是线性的。以角速度方程为例，如果J完全已知，控制力矩直接选成：

τ = ω × (J·ω) + J·ν

其中ν是伪控制输入，也就是“想让角速度变成的样子”。把这个表达式代回动力学方程后，陀螺力矩被精确抵消，系统退化成：

ω_dot = ν + J^{-1}·d

这时候三个角速度通道近似的变成了三个独立的积分器，控制器设计就退化成对积分器设计线性控制器，我可以直接用PD来设计ν：

ν = ω_dot_d + K_ω ·(ω_d - ω)

这里ω_d是期望角速度，ω_dot_d是期望角加速度前馈，K_ω是正定增益矩阵。这个处理就是标题里的“动态反演”，文献里叫dynamic inversion，也可以理解为反馈线性化的一种工程结构。它在思路上非常优雅，但前提是J准确，这个前提在实际系统中几乎不成立，所以必须引入后两个模块来兜底。

2.3 ESO的设计：把总扰动当成一个状态来估计

ESO的基本思想是，把系统方程里所有“不清楚”的部分合并成一项，叫作总扰动w，然后把w当作一个额外状态去观测。对于角速度通道，反馈线性化后的实际系统可以写成：

ω_dot = ν + w(ω, τ, d, t)

其中w包含了J失配导致的线性化残差、外部扰动、未建模动态的等效角加速度。ESO把这个w写成扩张状态x2，原来的ω写成x1，构造观测器：

z1_dot = z2 + β1·(ω - z1) + ν z2_dot = β2·(ω - z1)

z1是ω的估计值，z2就是总扰动w的估计值。β1和β2是观测器增益，工程上最常用的整定方法是带宽法——把观测器特征方程配置成二阶标准形式，取β1 = 2·ω_o，β2 = ω_o²，ω_o就是期望的观测器带宽。

最终在控制律里把z2减掉，也就是让伪控制ν = ν_nominal - z2，这样总扰动就在物理上被“中和”掉了。ESO的好处是不需要知道w的任何模型，只要扰动有界、观测器带宽足够，它就能把扰动逼出来。这也是这套方案比纯自适应控制更宽松的地方，很多快变的阵风扰动根本来不及被自适应参数学到，但ESO可以实时跟住。

2.4 自适应环节：在线辨识转动惯量参数

自适应模块负责把动态逆“逆不准”的根源修掉。回到动力学方程，如果把J的三个对角元看作未知参数向量θ = [Jx, Jy, Jz]^T，那么方程J·ω_dot + ω×(J·ω) = τ + d可以重新参数化成一个线性回归形式：

Y(ω, ω_dot_d)·θ

这里的Y是3×3回归矩阵，它不包含未知参数，只包含角速度和期望角加速度。经过推导，回归矩阵的具体形式是：

Y = [p_dot_d, -q·r, q·r; p·r, q_dot_d, -p·r; -p·q, p·q, r_dot_d]

注意这里用期望角加速度ω_dot_d代替了实际角速度导数，这样设计控制律时就不需要对测量信号求导，既避免了噪声放大，也让控制器中的回归矩阵可以离线计算。

自适应律按李雅普诺夫方法设计，最常用的形式是：

θ_hat_dot = Γ·Y^T·e_ω - σ·Γ·(θ_hat - θ_ref)

其中e_ω = ω_d - ω是角速度误差，Γ是自适应增益矩阵，一般取0.01到0.5之间的对角阵；σ是σ修正系数，用来防止参数漂移。有人可能担心自适应和ESO会“抢活干”，我的经验是：自适应增益设得小一点，它只负责收敛慢变参数，ESO观测器带宽设得高一点，它负责快速扰动，两者按频率分界，不会打架。

2.5 完整控制律与参数作用一览

把所有模块合在一起，完整控制律可以写成以下三步：外环根据姿态误差生成期望角速度，ω_d = W^{-1}·(Ω_dot_d + K_R·(Ω_d - Ω))；内环计算回归矩阵Y，用自适应参数θ_hat构造动态逆前馈；最终控制力矩为：

τ = Y(ω, ω_dot_d)·θ_hat + J_hat·K_ω·e_ω - J_hat·z2

这里J_hat对应θ_hat组成的对角矩阵。第一项是自适应前馈，负责把非线性动力学抵消掉；第二项是线性PD，负责把角速度误差压回零；第三项是ESO扰动补偿，负责把残余的总扰动直接抵消。

我整理了一张参数作用表，方便后面调参时对照：

3. Simulink与m脚本复现全记录

3.1 顶层模型怎么搭

我复现用的Simulink模型分五个主要模块：参考指令模块、姿态控制器子系统、ESO子系统、无人机动力学S函数、数据记录与显示模块。参考指令可以做成阶跃信号或者正弦信号发生器，我用的是From Workspace，在m脚本里直接生成时间序列，这样改工况特别方便，不用去Simulink里点信号源。

姿态控制器子系统内部并不复杂，主要干三件事：第一，计算姿态误差并生成期望角速度ω_d；第二，根据ω_d和实测ω计算角速度误差e_ω；第三，计算回归矩阵Y和控制力矩τ。我把核心计算全部写在MATLAB Function块里，而不是用一个个Gain和Sum模块拼，因为公式多了以后用原生模块搭极易出错，而且改公式要改半天；用函数块，参数从工作区读入，逻辑一目了然。

需要注意的是，动力学模块我建议用S-Function而不是纯模块积分，因为要写四元数更新、欧拉角转换、限幅等逻辑，纯模块拼起来非常臃肿。如果你不想写S-Function，也可以用积分器模块直接搭，但陀螺力矩耦合项、转换矩阵运算量不小，新手容易连错线。

3.2 ESO子系统实现要点

ESO子系统我单独拎出来说，因为它是整个模型里最容易搭错的地方。我的做法是：对三个角速度通道共用一个ESO函数块，输入为实测角速度ω和三轴控制力矩τ，输出为z1和z2。实现时最关键的一点是，ESO内部使用的是“经动态逆抵消后”的伪控制量ν，而不是直接用控制力矩τ。换句话说，ESO观测的和补偿的是“动态逆没逆干净的剩余误差”，这样才能避免双重积分导致观测器振荡。

具体函数可以这样写：

function [z1, z2] = eso_update(omega, nu, Ts, wo) persistent z1_p z2_p if isempty(z1_p) z1_p = omega; z2_p = zeros(3,1); end beta1 = 2*wo; beta2 = wo^2; e = omega - z1_p; z1_dot = z2_p + beta1*e + nu; z2_dot = beta2*e; z1_p = z1_p + Ts*z1_dot; z2_p = z2_p + Ts*z2_dot; z1 = z1_p; z2 = z2_p; end

如果你是在Simulink里用积分器模块搭，需要把z1和z2两个积分器连好，反馈回路别连错。我踩过的坑是把z2的积分器输入接到了z1上，结果观测器完全失去估计能力，输出乱跳。此外，z1的初值要设为实测角速度的初始值，否则启动瞬间会有一个很大的观测误差尖峰，这个尖峰经过补偿后会变成控制力矩突变，严重时直接导致仿真发散。

3.3 自适应模块和扰动补偿的实现

自适应模块的核心就是参数更新律。我用一个MATLAB Function块实现，输入是e_ω、ω、ω_dot_d，输出是θ_hat。函数内部先构造回归矩阵Y，再计算θ_hat_dot，并用积分器累积。为了抑制参数漂移，我加了σ修正项，实测下来比纯积分形式的自适应律稳定很多，尤其在参考指令长时间保持不变的情况下，纯积分式自适应参数会缓慢漂移，加了σ修正后参数最终会停在真实值附近的有界区域内。

function theta_hat = adapt_update(e_w, w, wd_dot, Gamma, sigma, theta_ref, Ts) persistent theta_p if isempty(theta_p) theta_p = theta_ref; end Y = [wd_dot(1), -w(2)*w(3), w(2)*w(3); w(3)*w(1), wd_dot(2), -w(3)*w(1); -w(1)*w(2), w(1)*w(2), wd_dot(3)]; theta_dot = Gamma * (Y' * e_w) - sigma * Gamma * (theta_p - theta_ref); theta_p = theta_p + Ts * theta_dot; theta_hat = theta_p; end

关于σ修正还有个细节：θ_ref我直接取标称惯量J0的对角元素，也就是初始估计值，这样即使出现参数漂移，自适应也会把估计值拉回标称值附近，不至于让前馈力矩失控。补偿环节则比较简单，在控制力矩表达式里直接减去J_hat * z2即可，对应前面公式的第三项。

3.4 m脚本初始化参数详解

运行模型之前，所有参数都要在m脚本里准备好。我的初始化脚本大致是这样：

%% 无人机物理参数 J0 = diag([0.02, 0.021, 0.035]); % 标称转动惯量 Jreal = diag([0.025, 0.027, 0.045]); % 真实惯量，用于模拟失配 Jinv = inv(Jreal); %% 控制器增益 K_R = diag([10, 10, 10]); % 姿态外环 K_w = diag([6, 6, 6]); % 角速度内环 wo = 50; % ESO观测器带宽 Gamma = diag([0.05, 0.05, 0.05]); % 自适应增益 sigma = 0.1; %% 参考指令 t_end = 10; dt_sim = 1e-4; t = 0:dt_sim:t_end; phi_d = zeros(size(t)); theta_d = 30*pi/180 * ones(size(t)); % 俯仰阶跃30度 psi_d = zeros(size(t)); %% 运行仿真 simOut = sim('drone_attitude_ctrl.slx');

这里我要强调两点。第一，真实惯量Jreal在初始化脚本里被定义但并不会传给Simulink控制器，它只进动力学S-Function，用来模拟“控制器认为的模型”和“飞机真实的模型”之间的偏差，这是验证鲁棒性最重要的手段。第二，仿真步长我取1e-4秒，定步长求解器，因为ESO带宽到50 rad/s后，离散化步长如果太大，观测器的高频段会发生失真，出现类似“数值颤振”的现象。

仿真跑完后，我用后处理脚本直接在工作区绘图，把姿态角误差、角速度、三轴力矩、ESO估计值z2、自适应参数θ_hat五组曲线全部画出来。其中最有意思的是θ_hat的收敛曲线：在俯仰阶跃30度、惯量失配25%的情况下，三轴参数大约在3到5秒内收敛到接近真实值，而且整个过程中俯仰角超调一直压在5%以内，这个结果比纯PID或纯ADRC都要漂亮。

3.5 仿真结果中值得注意的细节

复现成功后，有一个现象值得你特别留意：ESO估计出来的z2并不是平滑的，它在每个阶跃瞬间都会出现一个比较明显的尖峰，这个尖峰对应的是期望角速度突变引起的惯性力突变，物理上确实存在，不是数值错误。但是，如果把z2直接全通量补偿进力矩指令，会看到控制力矩在阶跃瞬间有冲击，这在真实无人机上会导致舵机/电机瞬时过载。

所以工程上有两个处理手段：一是对z2的低频部分做高保真补偿，高频部分做限幅补偿，也就是给补偿项串一个一阶低通或者速率限制器；二是把姿态指令改成斜坡或滤波后的平滑信号，从源头避免阶跃突变。我实测下来，两者结合效果最好，姿态响应几乎没有损失，但力矩尖峰能被压掉一半以上。这也是复现论文方案和落地到真机之间最关键的一步。

4. 调参与排错实录

4.1 ESO带宽不是越高越好

很多第一次调ESO的人，包括我，都会本能地觉得ω_o越高扰动估计越快越准。实际一跑就发现，ω_o取太高时，z2会开始剧烈振荡，整个仿真画面都在抖。原因很简单：ESO的观测器本质上是跟随系统的状态，ω_o越高，它对测量噪声的放大越严重，尤其飞控里角速度测量通常有中高频噪声，ESO会把噪声当成总扰动去补偿，结果就是“越补越颤”。

我的经验是，对姿态角速度环，ω_o取控制器带宽的5到10倍就够。比如角速度环期望带宽是10 rad/s左右，ω_o取50到80 rad/s，仿真里效果很好；再往上加，曲线会开始毛糙，性能提升却很小。另外，仿真阶段有噪声可以，但真机上还要考虑传感器采样延迟，通常真机上的ω_o要取比仿真更低一些，给传感器滤波留出裕量。

4.2 自适应增益：收敛速度和振荡的拉锯

自适应增益Γ是另一个容易让人上头又翻车的参数。Γ太小，参数要十几秒甚至更久才能收敛，自适应形同虚设；Γ太大，参数学习曲线会振，而且更重要的是，它会把动态逆前馈项变得“神经质”，导致控制力矩高频波动。

我建议的整定方法是：先设Γ=0也就是关闭自适应，用带ESO的反馈线性化把系统调稳，再从小到大逐步增加Γ。每加一档，跑一次阶跃响应，观察两个指标：一是角速度误差是否在1秒内压到零附近，二是θ_hat曲线是否平滑。如果θ_hat在收敛过程中出现等幅振荡，就把Γ降一半。另外，σ修正不能省，它是整个自适应模块防爆的保险丝。

4.3 代数环与仿真步长的坑

这个方案在Simulink里最容易遇到的仿真层面的问题是代数环。由于控制器里的动态逆需要用到当前控制力矩τ，而τ又是由当前测量状态计算出来的，如果模型里直接把τ反馈到某个用于计算τ的模块中，Simulink就会检测到代数环。解决思路有三个：第一，在动力学模型内部给力矩加一个一阶惯性环节模拟执行器延迟，比如电机响应时间常数取0.02秒，这样τ_cmd和τ_actual就分开了，代数环自然消失；第二，在ESO函数块里用上一时刻的τ值进行估计，相当于加入一拍延迟；第三，避免在ESO函数内部直接调用当前的τ输出作为输入，改用控制器的指令值。

仿真步长方面，我的底线是1e-4秒定步长。如果电脑性能有限必须用变步长，建议把求解器误差限设得严格一点，否则ESO的离散化误差会和自适应更新律互相叠加，出现“看着挺稳，一换电脑结果变了”的复现问题。

4.4 常见问题速查表

我把整个复现过程中遇到的和同行反馈过的问题整理成了一个速查表，方便你直接对照排查：

4.5 我的调试顺序建议

最后给你一个我反复验证过的调试顺序，按这个顺序走，能少走很多弯路。第一步，先关闭自适应和ESO补偿，只保留姿态外环和角速度内环PD，把系统调到一个能稳定但不一定有多好的状态；第二步，打开ESO，从ω_o = 20 rad/s开始往上加，观察z2是否有估计到真实扰动，同时看姿态误差是否有改善；第三步，调整ESO补偿通道的滤波参数，把高频抖动压下去；第四步，再打开自适应，从小到大调Γ，直到参数收敛曲线平滑稳定；第五步，把Jreal故意调成和J0不一样，作为鲁棒性测试的最终关卡。

我个人实际测试的体会是，这套控制器在转动惯量失配30%以内、常值风扰不超过最大力矩的20%时，都能保持姿态角误差小于2度。比起纯PID，它的优势主要体现在强扰动和模型失配场景下的一致性表现；比起纯ADRC，多出的自适应环节确实能提高稳态精度。如果你后续想把它往真机上移植，只需要把连续形式的ESO和自适应律改成离散递推形式，采样频率做到200Hz以上即可，其余结构和参数可以直接沿用。

这个方案还有一个很好的扩展方向：把欧拉角模型换成四元数后，可以兼容大姿态角机动，甚至倒飞动作；把回归矩阵从对角惯量扩展到包含重心偏移参数，就能处理载荷不平衡的问题。如果你正在找一套既能学术研究又能工程落地的姿态控制方案，我建议你亲手复现一遍这套组合，收获非常大。