四旋翼无人机自抗扰控制算法研究 ADRC 轨迹跟踪 附带说明文档

飞控调试间里飘着咖啡香，小张盯着屏幕上抖得像筛糠的无人机轨迹曲线，第17次把咖啡杯捏得咯吱响。四旋翼在风洞实验室的强风里表演着醉拳，传统PID控制器的参数怎么调都像在打地鼠。这时候老工程师老王溜达过来，在轨迹图上画了个大圈："试试这个叫ADRC的'流氓算法'，专治各种不服。"

ADRC（Active Disturbance Rejection Control）这货确实像个控制界的"流氓头子"。传统控制算法遇到外部扰动就像乖学生遇到校霸，ADRC却自带"以暴制暴"的buff。核心思路就一句话：管你什么妖风邪气，我自抗扰控制器全给你收拾了。

看看这个扩张状态观测器的实现片段：

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):
        k1 = self._derivative(y, u)
        k2 = self._derivative(y + 0.5*self.dt*k1, u)
        k3 = self._derivative(y + 0.5*self.dt*k2, u)
        k4 = self._derivative(y + self.dt*k3, u)
        self.z += (k1 + 2*k2 + 2*k3 + k4) * self.dt / 6
        return self.z[2]  # 返回扰动估计值

    def _derivative(self, y, u):
        e = y - self.z[0]
        return np.array([
            self.z[1] + self.beta[0]*e,
            self.z[2] + self.beta[1]*e + u,
            self.beta[2]*e
        ])

这段代码堪称"流氓算法"的鹰犬爪牙。beta参数组就像三个打手，实时揪出系统里的各种扰动成分。特别注意导数计算里那个u的骚操作，把控制量也喂给观测器，相当于在敌方阵营安插了卧底。

实际飞行时，轨迹跟踪控制器得这么玩：

void ADRC_Controller::updateTrajectory(const Vector3d& target_pos) {
    // 轨迹微分跟踪器
    double delta = 0.005;  // 滤波因子
    tracking_diff = (target_pos - last_pos) / delta;
    last_pos = target_pos;

    // 非线性状态误差反馈
    double e1 = pos_est - target_pos.x();
    double e2 = vel_est - tracking_diff.x();
    
    // 非线性组合
    double u0 = -fal(e1, 0.5, delta) - fal(e2, 0.25, delta);
    
    // 扰动补偿
    control_output = u0 - disturbance_est / b0;
}

这个fal函数是个灵魂存在，名字听着像国骂，实际是种非线性函数。当误差大时它重拳出击，误差小时温柔抚摸，完美解决传统PID线性组合的"要么莽要么怂"问题。

在 Gazebo 仿真中加载风扰模型后，ADRC控制的无人机画出的轨迹就像用了飘柔——纵使妖风阵阵，我自顺滑如丝。对比PID控制那癫痫发作般的轨迹，ADRC的跟踪误差能压到前者的1/5以下。不过要注意b0这个补偿因子，调参不当会让无人机表演托马斯回旋——别问我是怎么知道的。

说明文档里必须用加粗字体警告：ADRC虽好，可不要贪杯！特别是ESO的beta参数，调大了观测器变疯狗，调小了变树懒。建议先用粒子群优化找参数范围，再手动微调，比相亲找对象还考验耐心。

某次户外实测，突遇妖风阵风达15m/s。看着ADRC控制的无人机在风浪中稳如老狗，而隔壁组的PID飞机早已开启"随波逐流"模式。那一刻，我仿佛听见ADRC在说："不是风动，不是幡动，是扰动在动，而我——不动。"