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实现部分_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控制器输出限幅_z1
:ESO系统输入的观测值_z2
:ESO系统输入的微分的观测值_z3
:ESO系统总扰动- 控制器误差 = 目标值 - z1
- 控制微分误差 = -z2
- 状态观测误差 = z1 - 测量值
fal
函数:Fal函数实际上是对控制工程界的一个经验知识:“大误差,小增益;小误差,大增益”的数学拟合,具有快速收敛的特性。sign
函数:符号函数reset_eso
函数:将当前测量值赋给z1,z2和z3置零,实现ESO初始化- 模式1:
- 非线性控制率:
- 输出限幅:
- 状态估计:
- 模式2:
- 非线性控制率:
- 输出限幅:
- 状态估计:
- 日志记录:
定义几个重要参数(
AP_GROUPINFO
),除控制模式参数为AP_Int8
类型,其余参数均为AP_Float
类型:ESO内部参数:
ADRC参数更新:
ADRC函数定义:
针对于ADRC控制模式变量来选择对应的控制率
超过阈值的输出限制在阈值处,其他的输出乘以对应的系数并输出
定义两个变量,
beta1
和beta2
,代表各扩张状态观测器的反馈增益,更新后的
_z1
、_z2
、_z3
变量用于更新之前的PID值与模式1的一致
这里与模式1多了个
beta3
变量,该变量调节扩张观测器的跟踪速度target actual error
Loading...