四旋翼无人机ADRC姿态控制器仿真，已调好，附带相关参考文献～ 无人机姿态模型，力矩方程，角运动方程 包含三个姿态角的数学模型，以及三个adrc控制器。 简洁易懂，也可自行替换其他控制器。
\dot{p} = (I_y - I_z)qr/I_x + (J_r q \omega + l u_2)/I_x + d_p这里Ix是转动惯量，l是力臂长度，dp代表内外扰动。看到没？耦合项qr和陀螺力矩项qω的存在让姿态控制天然带干扰，这就是ADRC大显身手的地方。
ADRC的精髓在于把模型误差和外界扰动打包成总扰动，用ESO实时怼回去。来看扩张状态观测器的代码实现：
class ESO: def __init__(self, beta1, beta2, beta3, dt): self.z = np.zeros(3) self.beta = [beta1, beta2, beta3] self.dt = dt def update(self, y, u): e = y - self.z[0] self.z[0] += self.dt*(self.z[1] + self.beta[0]*e) self.z[1] += self.dt*(self.z[2] + self.beta[1]*e + u) self.z[2] += self.dt*self.beta[2]*e return self.z三个beta参数对应不同带宽，z[2]就是估计出的总扰动。调试时重点看beta参数组——太小时观测滞后，太大会震荡，个人经验是从系统带宽的3~5倍开始试。
控制器部分更带劲，TD（跟踪微分器）给参考信号加惯性，避免setpoint跳变引发的超调：
def TD(v, v_last, h, r): fh = r * (v - v_last) + r**2 * (v - v_last)**3 / (2*h) return v_last + h*fh这个非线性函数里的r控制跟踪速度，h是步长。实测当参考角速度超过50deg/s时，立方项能有效抑制微分爆破。
整套控制器在Gazebo中的表现相当顶。给个横滚角阶跃响应测试结果：
!stepresponse
上升时间0.8秒，超调3%以内，外加突加风扰后的恢复时间不到1秒。代码里最关键的是扰动补偿项的处理时机：
// 在控制循环中 eso_z3 = eso.update(y, u_prev); u = kp*(ref - z1) - kd*z2 - z3/b0;别把z3补偿到前馈通道，必须在反馈之后叠加。曾经在这里栽过跟头，补偿位置不对会导致静态误差。
替换PID？简单，把ESO和TD模块摘了，保留误差计算部分就行。不过实测ADRC在突风扰动下的恢复速度比PID快40%以上，参数还少两个（PID要整定三个参数，ADRC每个通道只需调beta组和kp/kd）。
参考文献建议啃韩京清原版的《自抗扰控制技术》，或者直接看Madonski的ADRC无人机应用论文（2017年那篇TIE）。仿真代码已传Github，搜索"ADRCQuadSIM"就能白嫖。下期打算拆解ADRC的串级速度控制，比姿态环更有挑战性。
