最近我把这套基于动态反演和扩展状态观测器（ESO）的无人机鲁棒反馈线性化自适应姿态控制器从头到尾复现了一遍。拿到这个项目标题的时候，我的第一反应是：又是一个把几个控制名词叠在一起的“缝合怪”。但等我把公式推完、把Simulink模型跑通之后，我承认这套设计是有一套内在逻辑的，并不是表面看起来那样堆砌。
它能解决什么问题？传统反馈线性化控制强依赖精确模型，模型一偏性能就明显下滑；ESO专门负责在线估计总扰动并做前馈补偿；自适应模块进一步处理控制通道增益漂移这类“老顽固”。三者叠在一起的姿态控制器，对参数不确定和外部风扰都有不错的容忍度。如果你正在做飞控算法，或者想把一篇论文里的方案真正跑通看曲线，这篇复现笔记应该能帮你省下不少折腾时间。
1. 方案选型：动态反演、ESO和自适应各解决什么问题
1.1 动态反演：把非线性拉直成一阶积分器
动态反演（Dynamic Inversion）本质上是一种“基于模型做反向计算”的控制思想。先写出系统微分方程，再把控制输入当成未知数解出来，让系统对外呈现出一个期望的简单动态。
放在无人机姿态控制里，这个思路特别好用。四旋翼姿态动力学的非线性主要来自两个地方：角速度耦合项 ω × (Jω)，以及欧拉角运动学变换里的三角函数。如果把这些项当作已知信息去参与控制量计算，配合一个相对准确的转动惯量矩阵 J，就能把“角加速度实际是多少”和“我施加了多大力矩”这条关系强行拉平。控制逻辑一下子清爽：你想要多大的角加速度，控制器就算出对应力矩，中间的非线性全部被对消掉，被控对象看起来像几个积分器串联。
但动态反演的软肋也很明显：它对模型的信任有点过头。模型参数与真实参数不一致，控制器里算出来的耦合项和实际不匹配，外界还有风场扰动完全没有模型，任何一项都会让非线性对消得不彻底。这一部分残留误差会直接作用在系统上，这也是单纯用动态反演做无人机控制器，总会出现“仿真很好，实际一飞就抖”的原因。所以要引入ESO这类在线观测手段来收拾残局。
1.2 ESO：把一个额外状态装进观测器，专吞扰动
ESO全称Extended State Observer，扩展状态观测器。它比普通状态观测器多了一维状态，专门用来估计系统的“总扰动”。这里“总扰动”是个打包概念：外部风扰力矩、未建模动态、参数偏差、甚至传感器偏置，全都可以塞进去。
为什么这种打包有意义？因为从控制器视角看，它能知道的信息只有两种：我给了多少输入，输出发生了什么变化。任何不在“我预期模型”里的响应偏差，都可以视为总扰动的贡献。ESO利用控制输入和测量输出，实时反推总扰动，然后把估计值送到控制前馈里抵消掉。这是典型的“边测边补”思路，有点像我开车时感觉方向盘偏了一点，就下意识修正方向——不是事先找到所有原因，而是直接补偿那个让我跑偏的总效果。
在这个项目里，ESO放在内环角速度回路中。用它的原因很直接：动态反演残留误差、外部阵风力矩、参数偏差，最后都会以“角速度响应偏离预期”这种形式体现出来。ESO守在这个位置，刚好能把这一堆说不清道不明的东西一网打尽。
1.3 自适应在这里不是摆设
很多人觉得有了动态反演加ESO，自适应就是锦上添花。实际复现之后我的看法变了：自适应解决的是一个很具体且容易被忽略的问题——ESO前馈通道的控制增益失配。
以四旋翼模型为例，滚转角速度通道的理论模型是 Ix·ṗ = τx + ...，控制器里用到的是标称惯量 Ix0。如果真实惯量和标称值偏差较大，观测器里用的 b0 = 1/Ix0 与实际通道增益对不上，ESO估计出的扰动就会带着系统性偏差，补偿效果自然打折。自适应律在线修正这个增益，把修正后的数值同时回传给控制器和ESO，整条回路才算真正闭合。
理论上当然可以做更复杂的在线惯量辨识，但工程上复杂度高不少，收益未必明显。我的建议是先把“ESO补偿加通道增益自适应”这个组合在仿真里跑稳，再考虑要不要上更重的辨识算法。
2. 核心原理推导：从姿态方程到控制律
2.1 无人机姿态模型：刚体动力学与欧拉角运动学
这一步最值得花时间，很多复现失败都是因为模型符号写反了。我采用刚体假设，忽略旋翼摆振和机体弹性形变，四旋翼姿态动力学在机体坐标系下写成：
[ J\dot{\omega} + \omega \times (J\omega) = \tau + d_{ext} ]
其中 ω = [p, q, r]^T 是机体角速度，J = diag(Ix, Iy, Iz) 是转动惯量矩阵，τ 是控制力矩，d_ext 是外部扰动力矩。这个式子本身不复杂，角速度叉乘自己产生的陀螺力矩项，是姿态动力学中最主要的非线性来源之一。
姿态运动学用欧拉角表示，滚转 φ、俯仰 θ、偏航 ψ：
[ \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \ 0 & \cos\phi & -\sin\phi \ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix} p \ q \ r \end{bmatrix} ]
这个矩阵在俯仰角接近 ±90°时会奇异，所以仿真机动范围要控制住，否则数值会直接爆炸。我在Simulink里加了一个角度限幅模块，把俯仰角限制在 ±60°以内，算是一个保险措施。
还有一个容易踩的坑：欧拉角速率是惯性系下的量，而角速度和力矩都是机体系下的量，中间要通过这个矩阵变换。新手搭模型经常在不经意间把矩阵转置写反。验证方法其实很简单：给一个纯滚转速率 p，理论上应该只产生 φ̇，如果输出里出现明显的 θ̇ 或 ψ̇，那矩阵就写错了。
2.2 动态反演控制律的设计流程
控制律按内外环拆分，这是无人机姿态控制的事实标准。外环把姿态误差变成期望角速度，内环把角速度误差变成力矩。
外环控制器先取最简单的P结构：
[ \omega_{des} = K_{att} \cdot e_{att} ]
其中 e_att 是姿态角误差向量，K_att 是对角增益矩阵，通常取 2.0 到 3.0（1/s量级）。如果要做轨迹跟踪，可以把期望角速度前馈加进来；阶跃指令场景用纯P已经够了。
内环是动态反演的主场。先把动力学方程改写成便于对消的形式：
[ \dot{\omega} = -J^{-1}(\omega \times J\omega) + J^{-1}\tau + d_{total} ]
d_total 是等效到角加速度域的总扰动。设内环期望角加速度为：
[ \dot{\omega}{des} = K_v (\omega{des} - \omega) ]
动态反演直接解出控制力矩：
[ \tau = J(\dot{\omega}{des} - \hat{d}{total}) + \omega \times (J\omega) ]
第一项让闭环呈现出近似一阶积分器特性，第二项把已知非线性项对消，第三项用ESO估计出的扰动做前馈补偿。这个式子就是整个控制器的核心，后续所有模块都围绕它展开。
K_v 的选择直接决定角速度环的响应速度，我习惯按期望闭环带宽 ω_rate 来整定，K_v = ω_rate。工程上内环比外环快 3 到 5 倍比较合理，姿态缓慢跟踪时才不容易振荡。
2.3 线性ESO的参数配置：带宽设定与观测方程
把内环某个角速度通道独立出来，以滚转角速度 p 通道为例：
[ \dot{p} = f_p(\omega) + b_0 \tau_x + d_p ]
f_p 是已知模型项，在这个通道里是 (-\frac{I_z - I_y}{I_x}qr)；b0 = 1/Ix 是标称通道增益；d_p 是集总扰动。线性ESO构造如下：
[ \begin{aligned} \dot{z}_1 &= z_2 + f_p + b_0 \tau_x - \beta_1(z_1 - p) \ \dot{z}_2 &= -\beta_2(z_1 - p) \end{aligned} ]
z1 跟踪角速度 p，z2 跟踪总扰动 d_p。如果取观测器带宽为 ωo，增益配置为：
[ \beta_1 = 2\omega_o, \quad \beta_2 = \omega_o^2 ]
这样观测误差的动态特征方程是 s² + 2ωo s + ωo² = 0，两个极点都配置在 ωo 处。滚转、俯仰、偏航三个角速度通道各配一个相同结构的ESO即可。
带宽怎么取？太低了扰动估计慢，补偿滞后明显；太高了测量噪声会被放大。我实测下来的经验值：在仿真步长 1ms、内环带宽 6 rad/s 的前提下，ωo 取 30 rad/s 就很稳，大约 4 到 6 倍内环带宽。如果传感器噪声明显，可以先给角速度测量加一阶低通滤波，再把 ωo 降到 20 rad/s 附近。
2.4 自适应通道增益辨识：如何不破坏ESO的估计
前面说ESO里的 b0 要尽量准，那怎么在线修？这里用最直观的梯度自适应律。还是拿滚转通道举例，定义角速度跟踪误差 e = p_des - p。把待估计的通道增益写成标称值的倍数：
[ b = k \cdot b_0 ]
其中 k 是无量纲倍数，初始设为 1 表示完全信任标称值。自适应律更新这个倍数，工程上做成归一化的形式：
[ \dot{\hat{k}} = \gamma \cdot e \cdot \frac{\tau_x}{\tau_{nom}} \cdot \text{Proj} ]
γ 是自适应增益，τ_nom 是控制力矩的参考标尺，用于把数值拉到同一量级。这个律的直观含义是：如果角速度误差和控制输入方向持续同号，说明增益估计偏了，持续修正。为了防止漂移到不合理范围，投影算子或直接限幅必不可少：
[ \hat{k} = \text{sat}_{[0.5,\ 2.0]}(\hat{k}) ]
允许通道增益在标称值一半到两倍之间浮动。为什么用限幅？自适应律在持续激励不足时很容易发散，纯仿真里尤其常见——系统只在某一个小角度范围机动，k̂ 会因为数值原因越跑越远。加了限幅，即使激励不足，它也只会缓慢走到边界而不是爆炸。
这里必须强调一下分工：ESO负责估计“当前时刻的总扰动”，自适应负责修正“通道增益模型”。两者叠加，动态反演对消的那一步才能始终保持在比较可信的状态。
3. Simulink与m脚本实战：从零搭建可复现工程
3.1 项目文件结构：谁负责什么
拿到项目文件后，别急着双击打开 .slx 模型，先把脚本和模型分类理清楚。我复现后的常用工程结构是：
params_init.m：定义所有参数，控制器增益，ESO带宽，仿真时长，状态初值quad_attitude_model.slx：Simulink模型，包含被控对象、控制器、ESO、自适应模块run_sim.m：调用sim()函数跑仿真plot_results.m：绘制姿态响应、角速度、控制力矩、扰动估计曲线
m脚本和Simulink的分工很明确：脚本管参数和数据处理，模型管信号流。这样以后改参数、跑批量仿真、出对比图都会顺很多。我会额外留一个compare_sweep.m，跑不同ESO带宽的批量仿真，这对理解参数影响特别有帮助。
3.2 被控对象子系统：六自由度姿态模型搭建
被控对象是前面推导的六状态刚体姿态模型。在Simulink里既可以用积分器、增益、乘法器手搭，也可以直接在MATLAB Function里写微分方程右端函数。我强烈推荐后者，调试效率和数值一致性都更好。
核心代码就几行：
function dstate = plantModel(state, tau, d_ext) phi = state(1); theta = state(2); psi = state(3); p = state(4); q = state(5); r = state(6); Jx = 0.0050; Jy = 0.0050; Jz = 0.0090; J = diag([Jx, Jy, Jz]); omega = [p; q; r]; omega_dot = -J \ cross(omega, J*omega) + J \ (tau + d_ext); phi_dot = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta); theta_dot = q*cos(phi) - r*sin(phi); psi_dot = q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta); dstate = [phi_dot; theta_dot; psi_dot; omega_dot]; end注意这里的参数值只是便于演示，正式工程建议通过MATLAB Function的附加参数从工作区传入。d_ext 是外部扰动力矩向量，仿真里我习惯在第5秒用一个阶跃信号注入。要验证ESO的性能，这个干扰一定要加，不然全程无扰，你根本看不出扰动补偿的贡献在哪里。
3.3 控制器子系统：内外环怎么衔接
控制器子系统接收三路信号：期望姿态角、实际姿态角、实际角速度，输出控制力矩向量。外环先算期望角速度：
omega_des = K_att * (ref_att - att);滚转角误差和俯仰角误差直接做差，偏航角误差要 wrap 到 [-pi, pi]，防止 10 度和 350 度之间出现“假大误差”。这个细节特别坑，偏航大角度机动时最容易暴露。
内环把期望角速度与实际角速度做差，乘上角速度环增益：
omega_dot_des = K_rate * (omega_des - omega);建议给 omega_dot_des 加限幅，比如限制在 ±10 rad/s²，否则大阶跃指令会瞬间产生过大的力矩尖峰，数值上容易触发欧拉角奇异，甚至直接导致仿真发散。
最后是核心控制律，加ESO扰动补偿和动态反演项：
tau = J * (omega_dot_des - d_hat) + cross(omega, J * omega);用信号线搭建的话，这几个模块对应的是增益 K_att、增益 K_rate、矩阵乘法和cross模块、以及来自ESO的 d_hat 信号。我的搭建习惯是从外到内逐级推进：先只接被控对象和动态反演，跑通以后再接ESO，最后接自适应。每加一级就跑一次仿真，千万不要一下子画完整张图再排错。
3.4 ESO与自适应模块：MATLAB Function实现细节
ESO我建议写成离散形式，放MATLAB Function模块里，而不是用连续积分器搭。离散形式更好控制时序，也能避免Simulink里常见的代数环问题。一个滚转通道的ESO函数如下：
function [z1, z2] = esoRoll(z1_prev, z2_prev, p_meas, tau_x, q_meas, r_meas, dt, omega_o, Ix, Iy, Iz) beta1 = 2 * omega_o; beta2 = omega_o^2; f_known = -(Iz - Iy)/Ix * q_meas * r_meas; b0 = 1 / Ix; e = z1_prev - p_meas; z1 = z1_prev + dt * (z2_prev + f_known + b0*tau_x - beta1*e); z2 = z2_prev + dt * (-beta2*e); end三个通道结构完全一样，区别只在 f_known 的具体形式。由于角速度三通道天然耦合，f_known 要用到实际角速度 q 和 r，这在Simulink里会形成一个反馈关系，但只进观测器，不会形成纯代数环。
自适应模块用一个持续变量记录增益倍数 k_hat：
% 简化归一化自适应律，tau_nom 是参考力矩标尺，比如 0.05 N·m function k_hat_next = adaptGain(e_w, tau_x, k_hat_prev, gamma, tau_nom, dt) update = gamma * e_w * (tau_x / tau_nom) * dt; rough = k_hat_prev + update; k_hat_next = min(max(rough, 0.5), 2.0); ende_w 的单位是 rad/s，tau_x 是力矩，tau_nom 是参考标尺。要做归一化的原因很简单，如果不先统一量纲，gamma 很难选出一个对整个仿真都适用的值。我通常会先用 gamma = 0.1 试跑，看 k_hat 收敛速度，太慢就加大，出现抖动就减小。
3.5 m脚本：参数初始化、仿真调用与后处理绘图
参数初始化脚本把所有变量写到工作区，方便日后批量跑。核心参数见下表：
| 参数 | 符号 | 数值 | 备注 |
|---|---|---|---|
| 滚转/俯仰惯量 | Ix, Iy | 0.005 kg·m² | 小型四旋翼量级 |
| 偏航惯量 | Iz | 0.009 kg·m² | 比滚转俯仰偏大 |
| 姿态外环增益 | K_att | 2.5 | 姿态角到角速度 |
| 角速度内环增益 | K_rate | 6.0 | 角速度环带宽 |
| ESO带宽 | omega_o | 30 | 内环带宽的5倍 |
| 自适应增益 | gamma | 0.1 | 归一化后试跑调整 |
| 仿真步长 | Ts | 1e-3 s | 固定步长 |
| 仿真时长 | T_end | 30 s | 便于观察扰动后恢复 |
| 扰动注入时间 | t_dist | 5 s | 阶跃扰动力矩 |
初始化之后，run_sim.m 里一行调用就够了：
out = sim('quad_attitude_model.slx'); logsout = out.logsout;我会在Simulink里开启信号记录，把姿态角、角速度、控制力矩、ESO的 z2 全部记录到 logsout，然后统一在 plot_results.m 里处理。验证图一般包含四张子图：姿态角阶跃响应、角速度响应、控制力矩曲线、ESO扰动估计与真实扰动的对比。这套后处理脚本做成模板之后，换参数、出图、写报告都特别快。
4. 调试实录：复现过程中最容易踩的坑
4.1 Simulink代数环与步长引起的发散
这套控制环路里最典型的代数环路径是：控制器输出 τ → 被控对象 → 角速度测量 → ESO → d_hat → 控制器。如果ESO的离散时序不对，或者没有用上一时刻的状态做更新，Simulink会直接报代数环错误，或者给出一个看起来能跑但姿态纹丝不动的结果。
解决办法很明确：ESO模块里使用 z1_prev、z2_prev 这样的离散状态更新，而不是在当前时刻解算跨越多个模块的隐式方程。固定步长建议 1ms。ESO带宽 30 rad/s 时一个振荡周期大约 0.21 秒，1ms 的采样间隔已经留出了足够的余量。
4.2 ESO初始值、噪声放大与带宽取舍
我对ESO初值的印象特别深。如果 z1(0)=0、z2(0)=0，但实际角速度初值是 0.5 rad/s，仿真前 0.5 秒会有一段明显的观测暂态。控制器会把这段观测误差当成扰动去补偿，产生一个不真实的力矩尖峰。处理办法很简单：初始化脚本里把 z1 的初值设成与实际角速度一致，z2 设成 0。
噪声放大是另一个高频问题。ESO本质上是带微分作用的观测器，带宽一旦高于系统等效采样率的五分之一，测量噪声会直接漏进扰动估计，然后被当成“真扰动”补偿掉。我在模型里给角速度测量加了一个截止频率 50 rad/s 的一阶滤波器再接进ESO。代价是滤波器会引入相位滞后，所以滤波截止频率不能取得比ESO带宽低太多，否则补偿相位就乱了。
4.3 自适应律发散：投影算子与死区处理
自适应律在仿真里最容易出问题的两种情形：持续激励不足时 k_hat 慢慢漂移，以及误差在零点附近抖动时 k_hat 来回震荡。除了限幅，死区也是一个有效手段：
if abs(e_w) < 0.01 update = 0; else update = gamma * e_w * (tau_x / tau_nom) * dt; end这个死区范围 0.01 rad/s，能让仿真稳定性显著提升。实际飞控里这个死区必不可少，因为传感器噪声会让角速度误差永远非零，不加死区增益倍数会一直乱跳。
这里再提醒一点：限幅范围 [0.5, 2.0] 不是拍脑袋选的。如果真实惯量与标称值偏差超过这个范围，说明模型标称值本身就不合格，应该回头重新校准模型，而不是指望自适应把所有问题都扛下来。
4.4 如何验证复现成功：几条可靠的判断曲线
复现控制器最怕跑出和预期不符的曲线，却说不清楚是参数有出入还是实现有bug。我整理了几条判断标准：
- 姿态阶跃响应在 2 秒内收敛，稳态误差小于 0.5°，超调量小于 10%
- 角速度偏差在机动结束后平滑归零，没有高频抖振
- 控制力矩在扰动注入前后有明显对应变化，恢复时间在 2 秒以内
- ESO 的 z2 输出在无扰阶段接近 0，有扰阶段能跟踪真实扰动量级，曲线平滑
- k_hat 收敛到真实通道增益附近，而不是一直压在限幅边界上
如果前两条满足但第四条完全对不上，说明ESO很可能只观测到了测量噪声，要么信号接线错了，要么带宽设置太高。如果 k_hat 一直压在边界，说明真实惯量离标称值太远，或者自适应增益太小，需要回到 m 脚本重新整定参数。
4.5 一点个人体会
这次复现给我最大的感受是：这类听起来很高级的组合控制器，本质上是在解决一个很朴素的矛盾——模型永远不精确，但控制律又必须依赖模型作为基准。动态反演把模型里可信的部分用到极致，ESO把模型里不可信的部分在线估计出来，自适应负责让模型部分不至于越漂越偏。三者配合，才能在“依赖模型”和“容忍不确定性”之间取得一个工程上能用的平衡。
如果你准备从头搭一遍，我给的建议是：不要把所有模块一次性建完再调。先只搭被控对象和动态反演，用真实惯量跑通；然后故意把惯量参数设偏 20%，观察性能恶化；再接入ESO和自适应，对比偏前、偏中、偏后三条曲线。这个逐级添加的过程，比任何公式推导都能帮你建立对这套控制器直观的理解。实际飞控调试，本质上也是这样一个逐级叠加补偿的过程。
