ADRC Ardupilot代码分析

本文分析了Ardupilot中的ADRC控制代码,重点介绍了ADRC控制器的ESO定义与实现、相关参数的更新、两种控制模式的非线性控制率、输出限幅和状态估计的计算方法,并提供了GitHub链接和参考论文。

type
status
date
slug
summary
tags
category
icon
password
记录一下自己对于Ardupilot ADRC控制代码的一些理解

GitHub链接

参考论文

ADRC UAV Paper Details:

修改细则

  • RC_Channel.cpp文件添加了ALT_RATE_CONTROL部分(高度变化率控制)
    • 针对于QUADPLANE的部分添加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:
      • 非线性控制率:
        • 输出限幅:
          • 超过阈值的输出限制在阈值处,其他的输出乘以对应的系数并输出
        • 状态估计:
          • 定义两个变量,beta1beta2,代表各扩张状态观测器的反馈增益,
            更新后的_z1_z2_z3变量用于更新之前的PID值
      • 模式2:
        • 非线性控制率:
          • 输出限幅:
            • 与模式1的一致
          • 状态估计:
            • 这里与模式1多了个beta3变量,该变量调节扩张观测器的跟踪速度
        • 日志记录:
          • target actual error
       
      Loading...