ardupilot 上实现ADRC内环角速度控制
时间：2023-07-29 15:37:05
目录
文章目录
- 目录
- 摘要
- 0.写之前
- 1.关键参考公式
-
- 1.首先上传关键模型图**
- 2.使用的关键公式：
- 2.上传代码
-
- 1.非线性ADRC
- 2.线性ADRC
- 3.移植细节
- 4.飞行视频
- 5.难点
- 6.遗留问题
摘要
本节主要记录自己ardupilot上移植adrc欢迎批评指正，飞行测试不太好，欢迎交流学习1812927205！
0.写之前
我在网上找到了很多信息，发现大部分都是模拟和理论知识很少有东西有参考价值，有些可以飞行，但代码完全混乱，不知道为什么要写ADRC公式(没有任何理论或工程依据)，决定写你遇到的所有过程，一起学习和成长！关于adrc建议看韩老先生的资料，大致知道是什么意思！深入了解后期使用。废话不多说！
1.关键参考公式
参考公式全部来自韩老先生的书，按照书中板书的公式。
重点把握ADRC: 跟踪微分器（TD）、非线性误差反馈（NSLF）、扩展状态观测器（ESO）
1.首先上传关键模型图**
2.使用的关键公式：
1.TD:
2.ESO:
3.NSLF:
2.上传代码
代码主要有两种:一种是非线性的ADRC和线性ADRC仅供参考！
1.非线性ADRC
.cpp文件
/******************************************************************************************************************** ********************************************************************************************************************* *文件名称：AC_ADRC.cpp *功能描述：实现adrc控制库函数 *修改作者： *修改时间：2022-7-13 *修改内容： *注：自抗扰控制器应涉及三个部分： * 1.安排过渡过程的部分 * 2.扩展状态观测器涉及 * 3.涉及误差反馈 * 本文主要研究二阶自抗扰算法 * PID 误差修正误差主要控制 * adrc在经典反馈控制结构的基础上，主要由跟踪微分器组成（Tracking differentiator）， * 误差反馈控制率（Feedback control law）扩展观测器（Extended state observer）组成。 * TD:跟踪微分器-Tracking differentiator, * 它分为线性和非线性，本质上是一种低通滤波器，作用是平滑指令，减少闭环传输函数的过度调整（将闭环传输函数的峰值降低到0dB以下）。 * NLSEF:误差反馈控制率-Feedback control law * 和经典反馈控制结构中的反馈控制器一样，目的是让误差等于0，有线组合和各种非线组合，最常见的是LADRC中的PD控制器， * 偶尔也有例如fal函数里类似dynamic gain处理(增益在误差范围内保持不变，随后增益减少或增加，即小误差大增益或小误差小增益)。 * ESO:扩张观测器-Extended state observer * 在系统模型为纯积分串联的假设的基础上，将所有其他项目(忽略的模型与外部扰动(噪声)统一为总扰动，并设计龙贝格观测器观测。 * ESO:扩张观测器-Extended state observer * 在系统模型为纯积分串联的假设的基础上，将所有其他项目(忽略的模型与外部扰动(噪声)统一为总扰动，并设计龙贝格观测器观测。 * 其中对于TD和ESO其实这两部分比较容易调整，很容易得到好的效果。难调参数主要在非线性组合部分。 * 常见的有以下四种误差反馈率。我试过前三种，效果都不错。 * TD: r h h0 * ESO: B01、B02、B03和观测器带宽w0. * 非线性反馈：（beta1、beta2）用kp和kd代替，b。 * 对于TD，一般仿真模型r可以尽可能大，在100~500范围内基本相同，即使效果再大，也不会有大的提升。h和h理论上可以相同，即模拟模型中的模拟步长。 * h0 和h1 预测时间通常在这里h1比滤波因子h0稍大1-1.5倍数 * _b扰动估值为01z3的补偿，这里参数b0是决定补偿强度的补偿因素作为可调参数 * ESO三个参数与观测器带宽有关，依次设置为3w0、3w0^2、 w0^3能满足要求。 * 因此，只有四个参数需要调整：kp kd w0 b。此时可以控制变量。 * 基本规律如下： * b调整时间越小，调整时间越短，但过小会导致震荡。 * w调整时间越小，冲击范围越小。 * Kp调整时间越短，震荡越大。 * kd稳定后可调效果不明显。 *ADRC参数整定经验 TD:有两个参数r、h r速度越大，速度越好，但容易超调和引起振荡 h静态误差越大，刚开始带来的超调越小，初始误差越小；但是会导致上升太慢，快速性不好 ESO:有6个参数bata01;beta02;beta03;b;T=0.0015;alpha1;alpha2;delta_Eso; 一般alpha1=0.5;alpha2=0.25;delta_Eso=0.01；是固定参数，只需调整其他三个参数： bata01和1/h同一数量级，过大会带来振荡甚至发散; beta02过小会带来发散，过大会产生高频噪声； beta03会议产生振荡；过小会降低跟踪速度； Nolinear_PD:三个参数：Kp、Kd、delt Kp误差越大，但速度越快 Kd速度越大，速度越快，但过度会议会产生振荡 delt值基本不影响输出，但是一般在0.01~0.1之间选取，会议产生振荡 最终参数： r = 30;h =0.0025;Kp = 500;Kd = 25;delt = 0.01; bata01=100;beta02=300;beta03=1500;alpha1=0.5;alpha2=0.25;delta_Eso=0.01;b=5;T=0.0015; ********************************************************************************************************************************** **********************************************************************************************************************************/ #include#include "AC_ADRC.h" const AP_Param::GroupInfo AC_ADRC::var_info[] = { AP_GROUPINFO_FLAGS("ENB", 0 , AC_ADRC , _enable , 0 , AP_PARAM_FLAG_ENABLE ) , //TD参数 AP_GROUPINFO ( "H0" , 1 , AC_ADRC , _h0 , 0.025 ) , //h0=10h AP_GROUPINFO ( "H1" , 2 , AC_ADRC , _h , 0.0025 ) , //滤波因子,系统调用步长 AP_GROUPINFO ( "R0" , 3 , AC_ADRC , _r0 , 5.0 ) , //快速跟踪因子 //ESO参数 AP_GROUPINFO ( "B" , 4 , AC_ADRC , _b , 0.2 ) , //系统系数b AP_GROUPINFO ( "BT0" , 5 , AC_ADRC , _beta0 , 10 ) , //扩张状态观测器反馈增益1 AP_GROUPINFO ( "BT1" , 6 , AC_ADRC , _beta1 , 300 ) , //扩张状态观测器反馈增益2 AP_GROUPINFO ( "BT2" , 7 , AC_ADRC , _beta2 , 1000 ) , //扩张状态观测器反馈增益3 AP_GROUPINFO ( "DETA" , 8 , AC_ADRC , _delta , 0.0125 ) , //delta为fal（e，alpha，delta）函数的线性区间宽度 delta取值范围在5h<=delta<=10h,h为ADRC控制周期 AP_GROUPINFO ( "P" , 9 , AC_ADRC , _kp , 0.12 ) , //跟踪输入信号增益 AP_GROUPINFO ( "D" , 10 , AC_ADRC , _kd , 0.001 ) , //跟踪微分信号增益 AP_GROUPINFO ( "A1" , 11 , AC_ADRC , _alfa1 , 0.75 ) , AP_GROUPINFO ( "A2" , 12 , AC_ADRC , _alfa2 , 1.5 ) , AP_GROUPEND } ; /********************************************************************************************************************* *主要的参数记录 * TD： * h0:根据仿真可以看出,参数h0的扩大起着很好的滤波效果，因此我们将把参数h0称作跟踪微分器的滤波因子：表示滤波因子 * h0选择适当大于步长h的参数 * h:步长一般选择周期值 * _r0:表示速度因子，若速度因子r小，则TD的输出比较光滑地跟踪参考输入,若比较大，更接近原始输入，有棱有角,具体选择多大的速度因子 * 取决与受控对象的承受能力和可提供的控制能力没对象的承受能力和可提供的控制能力大,速度因子可以取大200，适当大的参数r跟踪微分效果是很好 * ESO：状态观测器参数beta01=1/h beta02=1/(3*h^2) beta03=2/(8^2*h^3) * _beta0 ：3w 40 0.1 * _beta1 ：3w*w 533 0.3 * _beta2 ：w*w*w 2000 1 * _b： 0.1 参数b即便是状态的函数或时变参数，但其变化范围不很大，大于0.5就会很差 * _delta: * * NSL: * _kp * _kd * _alfa1： 0<_alfa1<1<_alfa2 * _alfa2： * ********************************************************************************************************************/ /******************************************************************************************************************** *函数原型：AC_ADRC() *函数功能：构造函数 *修改日期：2022-7-13 *修改作者： *备注信息： **********************************************************************************************************/ AC_ADRC :: AC_ADRC ( float h0 , float h , float r0 , //TD float b , float beta0 , float beta1 , float beta2 , float delta , //ESO float kp , float kd , float a1 , float a2 ) //: { //加载默认参数 load parameter values from eeprom AP_Param :: setup_object_defaults ( this , var_info ) ; //需要设定的TD参数 _h0 = h0 ; _h = h ; _r0 = r0 ; //需要设定的ESO参数 _b = b ; _beta0 = beta0 ; _beta1 = beta1 ; _beta2 = beta2 ; _delta = delta ; if (_b <= 0 ) { _b = 1 ; } //NSL _kp = kp ; _kd = kd ; _alfa1 = a1 ; _alfa2 = a2 ; //初始值 _z [ 0 ] = 0 ; _z [ 1 ] = 0 ; _z [ 2 ] = 0 ; _v1 = _v2 = 0.0f ; _e1 = _e2 = 0.0f ; _eso_error = 0.0f ; _disturb_u = _disturb = 0.0f ; _u0 = 0 ; _u = 0 ; } /******************************************************************************************************** *函数原型：float AC_ADRC::adrc_constrain(float val, float min, float max) *函数功能：限制函数 *修改日期：2022-7-13 *修改作者： *备注信息： **********************************************************************************************************/ float AC_ADRC :: adrc_constrain ( float val , float min , float max ) { return (val < min ) ? min : ( (val > max ) ? max : val ) ; } /******************************************************************************************************** *函数原型：void AC_ADRC::adrc_td(float in) *函数功能：构造函数 *修改日期：2022-7-13 *修改作者： *备注信息：TD:跟踪微分器-Tracking differentiator, * 分为线性和非线性两种，本质上是一种低通滤波器,作用是平滑指令以及减少闭环传递函数的超调（将闭环传递函数的峰值压低到0dB以下）。 * fh=fhan(x1(k)-v(k),x2(k),r,h0) * 主要参考韩京清的教材；fhan为最优控制函数，主要参考：2.6.25 和2.6.26 * 这里一般预报时间h1比滤波因子h0稍大1-1.5倍数 **********************************************************************************************************/ void AC_ADRC :: adrc_td ( float in ) { _v1 += _h * _v2 ; //td_x1=v1; v1(t+1) = v1(t) + T * v2(t) _v2 += _h * fhan (_v1 - in , _v2 , _r0 , _h0 ) ; //td_x2=v2 v2(t+1) = t2(t) + T fst2(v1(t) - in, v2(t), r, h) } /******************************************************************************************************** *函数原型：float AC_ADRC::fsg(float x,float d) *函数功能：构造函数 *修改日期：2022-7-13 *修改作者： *备注信息：符号函数的另外转换形式 **********************************************************************************************************/ float AC_ADRC :: fsg ( float x , float d ) { float value ; value = ( sign_adrc (x +d ) - sign_adrc (x -d ) ) * 0.5f ; return (value ) ; } /******************************************************************************************************** *函数原型：float AC_ADRC::fhan(float x1, float x2, float r, float h) *函数功能：最优控制函数,这个函数的实现参考2.7.24公式，教材107页 *修改日期：2022-7-13 *修改作者： *备注信息：h：表示步长 **********************************************************************************************************/ float AC_ADRC :: fhan ( float x1 , float x2 , float r , float h ) { float y = 0 ; float a0 = 0 ,a1 ,a2 ; float a = 0 ; float fh = 0 ; float d = 0 ; float sa ; d = r * h * h ; //d=r*h d0=h*d //中间变量a0 a0 = h * x2 ; //a0=h*x2 //计算出y y = x1 + a0 ; //y=x1+a0 //计算出教材中的a1 a1 = sqrtf (d * (d + 8.0 * fabsf (y ) ) ) ; //a1=sqrtf(d*(d+8*(|y|))) //计算出a2 a2 = a0 + sign_adrc (y ) * (a1 -d ) * 0.5f ; //a2=a0+sign(y)*(a1-d)/2 //最终计算得到a0 a = (a0 + y - a2 ) * fsg (y ,d ) + a2 ; //a=(a0+y-a2)*fsg(y,d)+a2 sa = fsg (a ,d ) ; //fsg(a,d)=(sign(a+d)-sign(a-d))/2 fh = -r * (a / d - sign_adrc (a ) ) * sa - r * sign_adrc (a ) ; //fhan=-r*fsg(a,d)-r*sign(a)(1-fsg(a,d)) return (fh ) ; } /******************************************************************************************************** *函数原型：float AC_ADRC::fal(float e,float alfa,float delta) *函数功能：构造函数 *修改日期：2022-7-13 *修改作者： *备注信息：原点附近具有线性段的连续的幂次函数 **********************************************************************************************************/ float AC_ADRC :: fal ( float e , float alfa , float delta ) { float f = 0.0f ; float s = 0 ; s = fsg (e ,delta ) ; //fal(e,a,d) f =e *s / ( powf (delta , 1 -alfa ) ) + powf ( fabsf (e ) ,alfa ) * sign_adrc (e ) * ( 1 -s ) ; return (f ) ; } /******************************************************************************************************** *函数原型：float AC_ADRC::adrc_eso(float v,float y,float u,float T,float MAX) *函数功能：非线性观测器 *修改日期：2022-7-13 *修改作者： *备注信息：公式4.7.4 4.3.19 *备注信息：ESO输入量 Y和U *备注信息：其中_beta0=3w _beta1=3w*w _beta2=w*w*w **********************************************************************************************************/ void AC_ADRC :: adrc_eso ( float y , float u , float h , float MAX ) { float fe ,fe1 ; //得到误差值 _eso_error = _z [ 0 ] - y ; //其中y是输入 //调用原点附近具有线性段的连续的幂次函数 fe = fal (_eso_error , 0.5f , _delta ) ; fe1 = fal (_eso_error , 0.25f , _delta ) ; _z [ 0 ] += h * (_z [ 1 ] - _beta0 *_eso_error ) ; _z [ 1 ] += h * (_z [ 2 ] - _beta1 * fe + _b * u ) ; //u也是输入，x1对应的是x0的导数 _z [ 2 ] += -h * _beta2 * fe1 ; //对应总扰动 } /******************************************************************************************************** *函数原型：float AC_ADRC::adrc_nsl(float v,float y,float u,float T,float MAX) *函数功能：非线性状态反馈 *修改日期：2022-7-13 *修改作者： *备注信息：参考公式：5.2.1 *备注信息：kd=2wc *备注信息：kp=wc*wc wc=w0/(3-10) **********************************************************************************************************/ float AC_ADRC :: adrc_nsl ( float MAX ) { //得到误差e1 _e1 = _v1 - _z [ 0 ] ; //得到误差e2 _e2 = _v2 - _z [ 1 ] ; //公式5.2.1第二个公式,要求0<_alfa1<1<_alfa2 _u0 = _kp * fal (_e1 , _alfa1 , _delta ) + _kd * fal (_e2 ,_alfa2 ,_delta ) ; //0<_alfa1<1<_alfa2 if (_b <= 0.0f ) { _b = 1.0f ; } //得到扰动估值值z3的补偿，这里参数b0是决定补偿强弱的“补偿因子”作为可调参数 _disturb_u = _z [ 2 ] / _b ; //最终得到控制量信息 _u =_u0 -_disturb_u ; //对控制量信息进行限制 _u = adrc_constrain (_u , -MAX , MAX ) ; return (_u ) ; } /******************************************************************************************************** *函数原型：float AC_ADRC::control_adrc(float v,float y,float u,float MAX) *函数功能：adrc控制函数 *修改日期：2022-7-13 *修改作者： *备注信息：v输入值 y反馈值 **********************************************************************************************************/ float AC_ADRC :: control_adrc ( float v , float y , float u , float MAX ) { //定义输出数据 float output_u = 0 ; if (_enable == 0 ) { return 0 ; } else { //安排过渡过程,根据设定值V安排过渡过程V1并提取器微分信号V2 //根据对象的输出和输入信号y，u估计出对象的状态x1 x2和作用于对象的综合扰动x3 //状态误差的非线性反馈律。系统的状态误差是指e1=v1-z1 e2=v2-z2，误差反馈律是根据误差e1,e2来决定的控制纯积分器串联型对象的控制规律u0 //对误差反馈控制量u0用扰动估计值Z3的补偿来决定最终控制量 //其中参数b0是决定补偿强弱的“补偿因子” adrc_td (v ) ; //微分跟踪器TD //记录控制器的数据值 target =v ; //输入值 actual =y ; //实际 disu =u ; //返回的值 adrc_eso (y , u , _h , MAX ) ; //状态观测器ESO //数据输出出去 output_u = adrc_nsl (MAX ) ; //非线性误差反馈 return output_u ; } } /************************************************************************************************************************* * file_end *************************************************************************************************************************/
.h文件
#ifndef AC_ADRC_H_
#define AC_ADRC_H_
#include 
