ADRC Ardupilot代码分析

记录一下自己对于Ardupilot ADRC控制代码的一些理解

GitHub链接

ADRC: Active Disturbance Rejection Control by MichelleRos · Pull Request #20243 · ArduPilot/ardupilot

参考论文

ADRC UAV Paper Details:

Jiachi_Zou_Thesis.2018.ADRC.UAV.Github.pdf

修改细则

• RC_Channel.cpp文件添加了ALT_RATE_CONTROL部分（高度变化率控制）

• wscript文件添加AP_ADRC部分

• AC_AttitudeControl.h文件新增高度控制函数，AC_AttitudeControl_Multi.cpp函数添加几个参数，并在AC_AttitudeControl_Multi.h函数中声明：

  • _pid2_rate_roll
  • _pid2_rate_pitch
  • _pid2_rate_yaw

  以上三个参数在AC_AttitudeControl_Multi类创建的时候初始化

• AP_ADRC.h函数为ADRC控制器的ESO定义部分，AP_ADRC.cpp函数为ADRC控制器的ESO实现部分

  定义几个重要参数（AP_GROUPINFO），除控制模式参数为AP_Int8类型，其余参数均为AP_Float类型：

  • _wc: ADRC control bandwidth(rad/s) → ADRC控制器带宽
  • _wo: ADRC ESO bandwidth(rad/s) → ADRC ESO带宽
  • _b0: ADRC control input gain → ADRC控制器输入增益
  • _delta: ADRC control linear zone length → ADRC控制器线性区间长度
  • _order: ADRC control model order → ADRC控制模式
  • _limit: ADRC control output limit → ADRC控制器输出限幅

  ESO内部参数：

  • _z1：ESO系统输入的观测值
  • _z2：ESO系统输入的微分的观测值
  • _z3：ESO系统总扰动

  ADRC参数更新：

  • 控制器误差 = 目标值 - z1
  • 控制微分误差 = -z2
  • 状态观测误差 = z1 - 测量值

  ADRC函数定义：

  • fal函数：Fal函数实际上是对控制工程界的一个经验知识：“大误差，小增益；小误差，大增益”的数学拟合，具有快速收敛的特性。
  • sign函数：符号函数
  • reset_eso函数：将当前测量值赋给z1，z2和z3置零，实现ESO初始化

  针对于ADRC控制模式变量来选择对应的控制率（待补充）

  • 模式1：

    • 非线性控制率：

      output = (_wc * fal(e1,0.5f,_delta) - sigma * _z2)/_b0;
      1

    • 输出限幅：

      超过阈值的输出限制在阈值处，其他的输出乘以对应的系数并输出

      // Limit output
      if(is_zero(_limit.get())){
      	output_limited = output;
      }else{
      	output_limited = constrain_float(output * dmod,-_limit,_limit);
      }
      1
      2
      3
      4
      5
      6

    • 状态估计：

      定义两个变量，beta1和beta2，代表各扩张状态观测器的反馈增益，更新之前的PID值

      // State estimation
      float fe = fal(e,0.5,_delta);
      float beta1 = 2 * _wo;
      float beta2 = _wo * _wo;
      _z1 = _z1 + _dt * (_z2 - beta1*e + _b0 * output_limited);
      _z2 = _z2 + _dt * (-beta2 * fe);
      
      _pid_info.P      = _z1;
      _pid_info.I      = _z2;
      _pid_info.D      = _z3;
      _pid_info.FF     = output_limited;
      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11

  • 模式2：

    • 非线性控制率：

      output = (kp * fal(e1,0.5f,_delta) + kd * fal(e2,0.25,_delta) - sigma * _z3)/_b0;
      1

    • 输出限幅：

      与模式1的一致

    • 状态估计：

      这里与模式1多了个beta3变量，该变量调节扩张观测器的跟踪速度

      // State estimation
      float beta1 = 3 * _wo;
      float beta2 = 3 * _wo * _wo;
      float beta3 = _wo * _wo * _wo;
      float fe  = fal(e,0.5,_delta);
      float fe1 = fal(e,0.25,_delta);
      _z1  = _z1 + _dt * (_z2 - beta1 * e);
      _z2  = _z2 + _dt * (_z3 - beta2 * fe + _b0 * output_limited);
      _z3  = _z3 + _dt * (- beta3 * fe1);
      
      _pid_info.P      = _z1;
      _pid_info.I      = _z2;
      _pid_info.D      = _z3;
      _pid_info.FF     = output_limited;
      1
      2
      3
      4
      5
      6
      7
      8
      9
      10
      11
      12
      13
      14

  • 日志记录：

    target actual error