四旋翼的ADRC姿态控制总给人一种“玄学调参“的错觉，其实从模型到代码落地，整个过程比想象中有意思得多。咱先甩出核心公式——滚转通道的角加速度方程

四旋翼无人机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

// 在控制循环中 eso_z3 = eso.update(y, u_prev); u = kp*(ref - z1) - kd*z2 - z3/b0;