记录一下自己对于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;
-
输出限幅:
超过阈值的输出限制在阈值处,其他的输出乘以对应的系数并输出
if(is_zero(_limit.get())){
output_limited = output;
}else{
output_limited = constrain_float(output * dmod,-_limit,_limit);
}
-
状态估计:
定义两个变量,beta1
和beta2
,代表各扩张状态观测器的反馈增益,更新之前的PID值
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;
-
模式2:
-
非线性控制率:
output = (kp * fal(e1,0.5f,_delta) + kd * fal(e2,0.25,_delta) - sigma * _z3)/_b0;
-
输出限幅:
与模式1的一致
-
状态估计:
这里与模式1多了个beta3
变量,该变量调节扩张观测器的跟踪速度
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;
-
日志记录:
target actual error
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)