ADRC自抗扰控制自学笔记(包含simulink仿真)(转载)

他这里让我很好理解了跟踪微分器

非线性PID(准确说是非线性PD,所以可以看到输入是两根线而不是三根线)  也就是说传统PID是线性的?但是不是那些系统是非线性的,比如无人机,平衡车,只是小幅度里近似为线性的。

下面这个来自于:自抗扰控制理论(一)ADRC的原理 - 知乎

他这里确实让我对ADRC的理解更进了一个层面。现在对ADRC有了比较清晰的认识,并不神秘并不高深,其实还好。还是很好理解的。

下面这个来自这篇自抗扰控制理论(一)ADRC的原理 - 知乎

扩张状态观测器(ESO)

(1)功能
估计系统内外扰动的实时作用值,并在反馈中给予补偿,用补偿的方法消除扰动的影响,从而具有抗干扰的作用。

所以它的输入是从被控对象的前后两个输入,这样也好理解。

摘自:https://blog.csdn.net/zouxu634866/article/details/106287879#comments_12978720

ADRC自抗扰控制自学笔记(包含simulink仿真)

总被蚊子叮的小旭 2020-05-22 17:59:36 1856 收藏 28

分类专栏: 控制

版权

ADRC控制中包含三个主要的部分:

跟踪微分器,非线性状态反馈(非线性组合),扩张观测器。

在这里插入图片描述
ADRC特点:

继承了经典PID控制器的精华,对被控对象的数学模型几乎没有任何要求,又在其基础上引入了基于现代控制理论的状态观测器技术,将抗干扰技术融入到了传统PID控制当中去,最终设计出了适合在工程实践中广泛应用的全新控制器。

本博客将从0开始,逐一介绍每一个部分,最后在合起来实现ADRC,每个部分都将介绍其公式原理和仿真实验。


一、跟踪微分器(TD)

这是一个单输入双输出的模块,作用有两个:

  • 避免输入量不要有跳变,便于实际系统实时跟踪。因为传统的pid有个问题,就是当跟踪像阶跃信号这种突变信号时超调和上升时间共存的现象,所以我们的思路就是对输入的信号进行平滑处理,也就是避免其出现突变。
  • 过滤高频噪声

所以输出1就是处理过的信号,第二个信号是输出1的微分,输出1和2都将用于下一环节,这里不介绍。

先摆出他的输出效果图,输入为阶跃信号:
在这里插入图片描述

说明:蓝色为处理后的阶跃信号,显然就好很多,没有那么突变。黄色为微分。

TD公式:
在这里插入图片描述

在这里插入图片描述

公式不难理解,接下来,我将对TD进行simulink仿真,其中fst函数我用的脚本写的:

在这里插入图片描述

在这里插入图片描述

友情提示:离散差分方程建模和连续系统微分方程建模一样,先找准输出y(k),再找准y(k-1)…,然后他们之间用单位延迟连接,最后在这基础上连其他东西。

hfst函数模块:

 
  1. function out = hfst(u1,u2,r,h)

  2. d=r*h;

  3. d0=h*d;

  4. y=u1+h*u2;

  5. a0=sqrt(d*d+8*r*abs(y));

  6. a=0;

  7. out1=0;

  8. if abs(y)>d0

  9. a=u2+(a0-d)/2*sign(y);

  10. end

  11. if abs(y)<=d0

  12. a=u2+y/h;

  13. end

  14. if abs(a)>d

  15. out1=-r*sign(a);

  16. end

  17. if abs(a)<=d

  18. out1=-r*a/d;

  19. end

  20. out=out1;

  21. end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

或者,不用像上面的simulink+m,整个td都可以直接用m脚本写也可以,已经过验证效果一样:

 
  1. function [y1k,y2k] = fcn(u)

  2. persistent y1k_1 y2k_1

  3. h=0.01;

  4. delta=10;

  5. if isempty(y1k_1)

  6. y1k_1=0;

  7. end

  8. if isempty(y2k_1)

  9. y2k_1=0;

  10. end

  11. y1k=y1k_1+h*y2k_1;

  12. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%hfst计算内容

  13. d=delta*h;

  14. d0=h*d;

  15. y=y1k_1-u+h*y2k_1;

  16. a0=sqrt(d*d+8*delta*abs(y));

  17. a=0;

  18. out1=0;

  19. if abs(y)>d0

  20. a=y2k_1+(a0-d)/2*sign(y);

  21. end

  22. if abs(y)<=d0

  23. a=y2k_1+y/h;

  24. end

  25. if abs(a)>d

  26. out1=-delta*sign(a);

  27. end

  28. if abs(a)<=d

  29. out1=-delta*a/d;

  30. end

  31. out=out1;

  32. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  33. y2k=y2k_1+h*out;

  34. y1k_1=y1k;

  35. y2k_1=y2k;

  36. y1k=y1k;

  37. y2k=y2k;

  38. end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48

说明:TD模型涉及两个调参:δ和h,h为采样周期,delta决定跟踪快慢(δ越大,过滤后的输出越接近输入),一般的仿真模型r可以尽量大一些,在100~500范围内基本相同,即使再大效果也基本不会有大的提升,我这里delta为50,h=0.001。

C语言实现:

//ADRC最速跟踪微分器TD,改进的算法fhan
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)//安排ADRC过度过程
{
  float d=0,a0=0,y=0,a1=0,a2=0,a=0;
  float x1_delta=0;//ADRC状态跟踪误差项
  x1_delta=fhan_Input->x1-expect_ADRC;//用x1-v(k)替代x1得到离散更新公式
  fhan_Input->h0=fhan_Input->N0*fhan_Input->h;//用h0替代h,解决最速跟踪微分器速度超调问题
  d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;//d=rh^2;
  a0=fhan_Input->h0*fhan_Input->x2;//a0=h*x2
  y=x1_delta+a0;//y=x1+a0
  a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))])
  a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2;
  a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));
  fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)
    -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量
  fhan_Input->x1+=fhan_Input->h*fhan_Input->x2;//跟新最速跟踪状态量x1
  fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;//跟新最速跟踪状态量微分x2
}


二、非线性组合

这一部分对应第一张图中的非线性组合模块,这一模块为双输入单输出,输入的是两个误差,分别是指令信号差和指令信号微分的差,参考指令信号和参考指令信号的微分均由TD产生。

传统的pid或者pd控制就是比例、积分、微分的线性加权之和,但这种线性的组合不是最佳的,后来发现三者的非线性组合效果更好。最常用的就是pd形式的非线性组合
在这里插入图片描述
这里面涉及的调参有三个:β1,β2,δ,δ为h的整数倍。

实验:演示非线性pd相比传统pd的优越性

咱们先看传统的pd控制:
对象为:

 
  1. sys = tf([133],[1,25,0])

  2. dsys = c2d(sys,0.001,'z');

  3. [num,den]=tfdata(dsys,'v');

  • 1
  • 2
  • 3

在这里插入图片描述

效果:
在这里插入图片描述

显然效果不好。

再来看看非线性pd控制:

在这里插入图片描述

函数模块代码:

 
  1. function y =nonlinear_pd(e1,e2)

  2. alfa1=0.75;

  3. alfa2=1.5;

  4. delta=0.002;

  5. beta1=150;

  6. beta2=1;

  7. fal1=1;

  8. fal2=1;

  9. if abs(e1)<=delta

  10. fal1=e1/(delta^(1-alfa1));

  11. end

  12. if abs(e1)>delta

  13. fal1=(abs(e1))^(alfa1)*sign(e1);

  14. end

  15. if abs(e2)<=delta

  16. fal2=e2/(delta^(1-alfa2));

  17. end

  18. if abs(e2)>delta

  19. fal2=(abs(e2))^(alfa2)*sign(e2);

  20. end

  21. y=beta1*fal1+beta2*fal2;

  22. end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27

效果:

在这里插入图片描述

可见效果好多了,因此非线性pid有效果!!!!!!

当然,我们这里用的是非线性pd控制,我们也可以用论文中的非线性pid控制,同理,这一部分公式为:

在这里插入图片描述

这里不再演示了。


三、ESO扩张观测器

ESO是一个双输入单输出模块,输入的值为对象的输出以及对象的控制输入,见第一张图,而输出有三个,分别是对象输出的估计值、对象输出的估计值的一阶导数、对象输出的估计值的二阶导数。而对象输出的估计值、对象输出的估计值的一阶导数将反馈给最开始的跟踪微分器(TD),而对象输出的估计值的二阶导数将反馈给非线性组合的输出上用于弥补扰动。

一般观测器仅观测系统的状态,只有输出和输出的导数(速度)。但是这里对输出的导数的导数(加速度)也进行了观测,这里也就是所谓的扰动(即第一张图中的w),对扰动进行了观测。观测器的状态量也由此扩张了一维,因此叫做扩张观测器。

ESO的公式见下图:
在这里插入图片描述
simulink模型:
在这里插入图片描述

里面的代码如下:

 
  1. function [z1_k,z2_k,z3_k] = ESO(yk,uk)

  2. %%参数初始化

  3. persistent z1_k_1 z2_k_1 z3_k_1

  4. bata01=30;

  5. beta02=300;

  6. beta03=1000;

  7. b=5;

  8. h=0.001;

  9. alfa1=0.75;

  10. alfa2=1.5;

  11. delta=0.002;

  12. fal1=1;

  13. fal2=1;

  14. if isempty(z1_k_1)

  15. z1_k_1=0;

  16. end

  17. if isempty(z2_k_1)

  18. z2_k_1=0;

  19. end

  20. if isempty(z3_k_1)

  21. z3_k_1=0;

  22. end

  23. e1=z1_k_1-yk;

  24. z1_k=z1_k_1+h*(z2_k_1-bata01*e1);

  25. z1_k_1=z1_k; %%迭代更新z1_k_1

  26. %%计算fal函数

  27. if abs(e1)<=delta

  28. fal1=e1/(delta^(1-alfa1));

  29. end

  30. if abs(e1)>delta

  31. fal1=(abs(e1))^(alfa1)*sign(e1);

  32. end

  33. if abs(e1)<=delta

  34. fal2=e1/(delta^(1-alfa2));

  35. end

  36. if abs(e1)>delta

  37. fal2=(abs(e1))^(alfa2)*sign(e1);

  38. end

  39. z2_k=z2_k_1+h*(z3_k_1-beta02*fal1+b*uk);

  40. z2_k_1=z2_k;%%迭代更新z2_k_1

  41. z3_k=z3_k_1-h*beta03*fal2;

  42. z3_k_1=z3_k;%%迭代更新z3_k_1

  43. end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48

但要注意:因为ESO的公式里面已经加入了b*u,所以在搭最后的模型时第一张图中的b0就不要了,即不用再乘上b0,直接将补偿后的u输入ESO。同理,如果你要在similink模型里要显示用上b0这个比例系数,那么ESO里的公式里就要改为:z2_k=z2_k_1+h*(z3_k_1-beta02*fal1+uk);即去掉最后的uk前的系数b。


//状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3) ...
void ESO_ADRC(Fhan_Data *fhan_Input)
{
  fhan_Input->e=fhan_Input->z1-fhan_Input->y;//状态误差
  
  fhan_Input->fe=Fal_ADRC(fhan_Input->e,0.5,fhan_Input->N1*fhan_Input->h);//非线性函数,提取跟踪状态与当前状态误差
  fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,fhan_Input->N1*fhan_Input->h);
  
  /*************扩展状态量更新**********/
  fhan_Input->z1+=fhan_Input->h*(fhan_Input->z2-fhan_Input->beta_01*fhan_Input->e);
  fhan_Input->z2+=fhan_Input->h*(fhan_Input->z3
                                 -fhan_Input->beta_02*fhan_Input->fe
                                   +fhan_Input->b0*fhan_Input->u);
  //ESO估计状态加速度信号,进行扰动补偿,传统MEMS陀螺仪漂移较大,估计会产生漂移
  fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1);
}


四、完整的ADRC

这一节将把前面的组合起来构成一个完整的ADRC,也就是第一张图中的形式。搭好的结构如下:

在这里插入图片描述
注意点:
1.因为这是一个离散的模型,所以确保所有模块的采样时间一致
2.注意检查所有的求和模块的正负
3.注意上面第三节的黑色注意部分

开始仿真,报错了:
在这里插入图片描述

翻译过来就是说存在代数环的原因,这个问题我搜了百度:

https://wenku.baidu.com/view/c3be45de763231126fdb1143.html

这个问题很同意理解,我们知道,我们在求解反馈环的时候,首先反馈的初始值是为o的,也就是反馈系统的顺序是:我们先根据主路输入计算得到主路的输出(即得到反馈路的输入),在根据反馈路的输入计算出反馈路的输出(即反馈值),然后进行下一次循环。而在simulink中,他不像我们之前写的m脚本(我们自己写脚本就是从主路开始),他不知道首先应该计算主路还是首先计算反馈路,所以我们需要告诉他,解决办法就是在反馈路的输出端加上单位延迟,也就是告诉反馈路你等等,别太急,等主路先走。

其实我们还可以这样去理解代数环,将其理解为初始状态时反馈量没有初始值,我们以前的控制模型比如pid啥的,我们反馈的反馈量都会有一个初始值,而这个模型的ESO的输出作为反馈量是没有初始值的,所以他报错的原因还可以理解为反馈量没有初始值,所以我们就去给反馈量设一个初始值,我们就可以用memory模块(这个模块的作用就是有输入时输出=输入,没输入时输出保持原先状态,且当输入改变时输出才改变,否则一直保持输出不变),在memory模块里设置一个初始值,这样反馈量就有初始值了。而上面我们用的单位延迟模块就是一个memory模块,二者本来就是一样的。所以上面用单位延迟模块也可以这样理解。

修改后的simulink为:

在这里插入图片描述

或者(两者等价):

在这里插入图片描述


仿真后结果没报错,且结果令人满意:
在这里插入图片描述

其中黄色为初始阶跃信号,红色为经过td的信号,蓝色为控制输出。

 

/*----------------------------------------------------------------------------------------------------------------------/
*               本程序只供购买者学习使用,版权著作权属于无名科创团队,无名科创团队将飞控程序源码提供给购买者,
*               购买者要为无名科创团队提供保护,未经作者许可,不得将源代码提供给他人,不得将源代码放到网上供他人免费下载, 
*               更不能以此销售牟利,如发现上述行为,无名科创团队将诉之以法律解决!!!
-----------------------------------------------------------------------------------------------------------------------/
*               生命不息、奋斗不止;前人栽树,后人乘凉!!!
*               开源不易,且学且珍惜,祝早日逆袭、进阶成功!!!
*               学习优秀者,简历可推荐到DJI、ZEROTECH、XAG、AEE、GDU、AUTEL、EWATT、HIGH GREAT等公司就业
*               求职简历请发送:15671678205@163.com,需备注求职意向单位、岗位、待遇等
*               无名科创开源飞控QQ群:540707961
*               CSDN博客:http://blog.csdn.net/u011992534
*               优酷ID:NamelessCotrun无名小哥
*               B站教学视频:https://space.bilibili.com/67803559/#/video
*               客户使用心得、改进意见征集贴:http://www.openedv.com/forum.php?mod=viewthread&tid=234214&extra=page=1
*               淘宝店铺:https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2
*               百度贴吧:无名科创开源飞控
*               修改日期:2018/8/30
*               版本:慧飞者——V1.0.1
*               版权所有,盗版必究。
*               Copyright(C) 武汉科技大学无名科创团队 2017-2025
*               All rights reserved
----------------------------------------------------------------------------------------------------------------------*/
#include "Headfile.h"
#include "ADRC.h"
 
Fhan_Data ADRC_Pitch_Controller;
Fhan_Data ADRC_Roll_Controller;
const float ADRC_Unit[4][16]=
{
  /*TD跟踪微分器   改进最速TD,h0=N*h      扩张状态观测器ESO           扰动补偿     非线性组合*/
  /*  r     h      N                  beta_01   beta_02    beta_03     b0     beta_0  beta_1     beta_2     N1     C    alpha1  alpha2  zeta       b*/
  {1000000 ,0.005 , 5,               300,      10000,      100000,      0.5,    0.85,   1.5,      0.0003,       2,    5,    0.9,   1.2,    0.03,    0.1},
  {1000000 ,0.005 , 5,               300,      10000,      100000,      0.5,    0.85,   1.5,      0.0003,       2,    5,    0.9,   1.2,    0.03,    0.1},
  {300000  ,0.005 , 3,               300,      4000,      10000,     100,   0.2,    1.2,      0.0010,       5,      5,    0.8,   1.5,    0.03,    0.05},
};
 
 
float Constrain_Float(float amt, float low, float high){
  return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
 
Butter_Parameter ADRC_Div_LPF_Parameter={
  //200---20hz
  1,    -1.14298050254,   0.4128015980962,
  0.06745527388907,   0.1349105477781,  0.06745527388907
};
float ADRC_LPF(float curr_inputer,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{
  /* 加速度计Butterworth滤波 */
  /* 获取最新x(n) */
  Buffer->Input_Butter[2]=curr_inputer;
  /* Butterworth滤波 */
  Buffer->Output_Butter[2]=
    Parameter->b[0] * Buffer->Input_Butter[2]
      +Parameter->b[1] * Buffer->Input_Butter[1]
	+Parameter->b[2] * Buffer->Input_Butter[0]
          -Parameter->a[1] * Buffer->Output_Butter[1]
            -Parameter->a[2] * Buffer->Output_Butter[0];
  /* x(n) 序列保存 */
  Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
  Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
  /* y(n) 序列保存 */
  Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
  Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
  return (Buffer->Output_Butter[2]);
}
 
 
 
 
 
int16_t Sign_ADRC(float Input)
{
  int16_t output=0;
  if(Input>1E-6) output=1;
  else if(Input<-1E-6) output=-1;
  else output=0;
  return output;
}
 
int16_t Fsg_ADRC(float x,float d)
{
  int16_t output=0;
  output=(Sign_ADRC(x+d)-Sign_ADRC(x-d))/2;
  return output;
}
 
 
void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2)
{
  fhan_Input1->r=ADRC_Unit[0][0];
  fhan_Input1->h=ADRC_Unit[0][1];
  fhan_Input1->N0=(uint16)(ADRC_Unit[0][2]);
  fhan_Input1->beta_01=ADRC_Unit[0][3];
  fhan_Input1->beta_02=ADRC_Unit[0][4];
  fhan_Input1->beta_03=ADRC_Unit[0][5];
  fhan_Input1->b0=ADRC_Unit[0][6];
  fhan_Input1->beta_0=ADRC_Unit[0][7];
  fhan_Input1->beta_1=ADRC_Unit[0][8];
  fhan_Input1->beta_2=ADRC_Unit[0][9];
  fhan_Input1->N1=(uint16)(ADRC_Unit[0][10]);
  fhan_Input1->c=ADRC_Unit[0][11];
  
  fhan_Input1->alpha1=ADRC_Unit[0][12];
  fhan_Input1->alpha2=ADRC_Unit[0][13];
  fhan_Input1->zeta=ADRC_Unit[0][14];
  fhan_Input1->b=ADRC_Unit[0][15];
  
  fhan_Input2->r=ADRC_Unit[1][0];
  fhan_Input2->h=ADRC_Unit[1][1];
  fhan_Input2->N0=(uint16)(ADRC_Unit[1][2]);
  fhan_Input2->beta_01=ADRC_Unit[1][3];
  fhan_Input2->beta_02=ADRC_Unit[1][4];
  fhan_Input2->beta_03=ADRC_Unit[1][5];
  fhan_Input2->b0=ADRC_Unit[1][6];
  fhan_Input2->beta_0=ADRC_Unit[1][7];
  fhan_Input2->beta_1=ADRC_Unit[1][8];
  fhan_Input2->beta_2=ADRC_Unit[1][9];
  fhan_Input2->N1=(uint16)(ADRC_Unit[1][10]);
  fhan_Input2->c=ADRC_Unit[1][11];
  
  fhan_Input2->alpha1=ADRC_Unit[1][12];
  fhan_Input2->alpha2=ADRC_Unit[1][13];
  fhan_Input2->zeta=ADRC_Unit[1][14];
  fhan_Input2->b=ADRC_Unit[1][15];
}
 
 
 
//ADRC最速跟踪微分器TD,改进的算法fhan
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)//安排ADRC过度过程
{
  float d=0,a0=0,y=0,a1=0,a2=0,a=0;
  float x1_delta=0;//ADRC状态跟踪误差项
  x1_delta=fhan_Input->x1-expect_ADRC;//用x1-v(k)替代x1得到离散更新公式
  fhan_Input->h0=fhan_Input->N0*fhan_Input->h;//用h0替代h,解决最速跟踪微分器速度超调问题
  d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;//d=rh^2;
  a0=fhan_Input->h0*fhan_Input->x2;//a0=h*x2
  y=x1_delta+a0;//y=x1+a0
  a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))])
  a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2;
  a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));
  fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)
    -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量
  fhan_Input->x1+=fhan_Input->h*fhan_Input->x2;//跟新最速跟踪状态量x1
  fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;//跟新最速跟踪状态量微分x2
}
 
 
//原点附近有连线性段的连续幂次函数
float Fal_ADRC(float e,float alpha,float zeta)
{
  int16 s=0;
  float fal_output=0;
  s=(Sign_ADRC(e+zeta)-Sign_ADRC(e-zeta))/2;
  fal_output=e*s/(powf(zeta,1-alpha))+powf(ABS(e),alpha)*Sign_ADRC(e)*(1-s);
  return fal_output;
}
 
 
 
 
/************扩张状态观测器********************/
//状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3) ...
void ESO_ADRC(Fhan_Data *fhan_Input)
{
  fhan_Input->e=fhan_Input->z1-fhan_Input->y;//状态误差
  
  fhan_Input->fe=Fal_ADRC(fhan_Input->e,0.5,fhan_Input->N1*fhan_Input->h);//非线性函数,提取跟踪状态与当前状态误差
  fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,fhan_Input->N1*fhan_Input->h);
  
  /*************扩展状态量更新**********/
  fhan_Input->z1+=fhan_Input->h*(fhan_Input->z2-fhan_Input->beta_01*fhan_Input->e);
  fhan_Input->z2+=fhan_Input->h*(fhan_Input->z3
                                 -fhan_Input->beta_02*fhan_Input->fe
                                   +fhan_Input->b0*fhan_Input->u);
  //ESO估计状态加速度信号,进行扰动补偿,传统MEMS陀螺仪漂移较大,估计会产生漂移
  fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1);
}
 
 
/************非线性组合****************/
/*
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{
float d=0,a0=0,y=0,a1=0,a2=0,a=0;
float Sy=0,Sa=0;//ADRC状态跟踪误差项
 
fhan_Input->h1=fhan_Input->N1*fhan_Input->h;
 
d=fhan_Input->r*fhan_Input->h1*fhan_Input->h1;
a0=fhan_Input->h1*fhan_Input->c*fhan_Input->e2;
y=fhan_Input->e1+a0;
a1=sqrt(d*(d+8*ABS(y)));
a2=a0+Sign_ADRC(y)*(a1-d)/2;
 
Sy=Fsg_ADRC(y,d);
a=(a0+y-a2)*Sy+a2;
Sa=Fsg_ADRC(a,d);
fhan_Input->u0=-fhan_Input->r*((a/d)-Sign_ADRC(a))*Sa-fhan_Input->r*Sign_ADRC(a);
 
//a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));
 
//fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)
//                -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量
}
*/
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{
  float temp_e2=0;
  //temp_e2=Constrain_Float(fhan_Input->e2,-3000,3000);
  temp_e2=Constrain_Float(fhan_Input->e2_lpf,-3000,3000);
  fhan_Input->u0=fhan_Input->beta_1*Fal_ADRC(fhan_Input->e1,fhan_Input->alpha1,fhan_Input->zeta)
    +fhan_Input->beta_2*Fal_ADRC(temp_e2,fhan_Input->alpha2,fhan_Input->zeta);
  
}
 
 
void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback_ADRC)
{
  fhan_Input->Last_TD_Input=fhan_Input->TD_Input;
  fhan_Input->TD_Input=expect_ADRC;
  fhan_Input->TD_Input_Div=(fhan_Input->TD_Input-fhan_Input->Last_TD_Input)/fhan_Input->h;
  
  fhan_Input->Last_ESO_Input=fhan_Input->ESO_Input;
  fhan_Input->ESO_Input=feedback_ADRC;
  fhan_Input->ESO_Input_Div=(fhan_Input->ESO_Input-fhan_Input->Last_ESO_Input)/fhan_Input->h;
  
  /*自抗扰控制器第1步*/
  /********
      **
      **
      **
      **
      **
  ********/
  /*****
  安排过度过程,输入为期望给定,
  由TD跟踪微分器得到:
  过度期望信号x1,过度期望微分信号x2
  ******/
  Fhan_ADRC(fhan_Input,expect_ADRC);
  
  /*自抗扰控制器第2步*/
  /********
          *
          *
  *  * *  *
  *
  *
  ********/
  /************系统输出值为反馈量,状态反馈,ESO扩张状态观测器的输入*********/
  fhan_Input->y=feedback_ADRC;
  /*****
  扩张状态观测器,得到反馈信号的扩张状态:
  1、状态信号z1;
  2、状态速度信号z2;
  3、状态加速度信号z3。
  其中z1、z2用于作为状态反馈与TD微分跟踪器得到的x1,x2做差后,
  经过非线性函数映射,乘以beta系数后,
  组合得到未加入状态加速度估计扰动补偿的原始控制量u
  *********/
  ESO_ADRC(fhan_Input);//低成本MEMS会产生漂移,扩展出来的z3此项会漂移,目前暂时未想到办法解决,未用到z3
  /*自抗扰控制器第3步*/
  /********
         **
      **
     **
      **
        **
  ********/
  /********状态误差反馈率***/
  fhan_Input->e0+=fhan_Input->e1*fhan_Input->h;//状态积分项
  fhan_Input->e1=fhan_Input->x1-fhan_Input->z1;//状态偏差项
  fhan_Input->e2=fhan_Input->x2-fhan_Input->z2;//状态微分项
  fhan_Input->e2_lpf=ADRC_LPF(fhan_Input->e2,
                              &fhan_Input->ADRC_LPF_Buffer,
                              &ADRC_Div_LPF_Parameter);//巴特沃斯低通后得到的微分项,20hz
  /********线性组合*******/
  /*
  fhan_Input->u0=//fhan_Input->beta_0*fhan_Input->e0
  +fhan_Input->beta_1*fhan_Input->e1
  +fhan_Input->beta_2*fhan_Input->e2;
  */
  Nolinear_Conbination_ADRC(fhan_Input);
  /**********扰动补偿*******/
  fhan_Input->u=fhan_Input->u0//-fhan_Input->z3/fhan_Input->b;
    -fhan_Input->z3/fhan_Input->b0;
  //fhan_Input->u+=Constrain_Float(fhan_Input->beta_0*fhan_Input->e0,-150,150);
  //由于MEMS传感器漂移比较严重,当beta_03取值比较大时,长时间z3漂移比较大,目前不加入扰动补偿控制量
  fhan_Input->u=Constrain_Float(fhan_Input->u0,-450,450);//不加入扰动补偿
  //fhan_Input->u=Constrain_Float(fhan_Input->u,-450,450);//加入扰动补偿
}
 
void ADRC_Integrate_Reset(Fhan_Data *fhan_Input)  {fhan_Input->e0=0.0f;}
 
#ifndef _ADRC_H_
#define _ADRC_H_
/*----------------------------------------------------------------------------------------------------------------------/
        *               本程序只供购买者学习使用,版权著作权属于无名科创团队,
        *               无名科创团队将飞控程序源码提供给购买者,
        *               购买者要为无名科创团队提供保护,
        *               未经作者许可,不得将源代码提供给他人
        *               不得将源代码放到网上供他人免费下载,
        *               更不能以此销售牟利,如发现上述行为,
        *               无名科创团队将诉之以法律解决!!!
-----------------------------------------------------------------------------------------------------------------------/
        *               生命不息、奋斗不止;前人栽树,后人乘凉!!!
        *               开源不易,且学且珍惜,祝早日逆袭、进阶成功!!!
-----------------------------------------------------------------------------------------------------------------------/
	*		无名科创开源飞控 V1.1	武汉科技大学  By.YuYi
	*		CSDN博客: http://blog.csdn.net/u011992534
	*               优酷ID:NamelessCotrun无名小哥
	*               无名科创开源飞控QQ群:540707961
        *               https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2
        *               百度贴吧:无名科创开源飞控
        *               修改日期:2017/10/30
        *               版本:V1.1
        *               版权所有,盗版必究。
        *               Copyright(C) 武汉科技大学无名科创团队 2017-2019
        *               All rights reserved
----------------------------------------------------------------------------------------------------------------------*/
typedef struct
{
/*****安排过度过程*******/
float x1;//跟踪微分期状态量
float x2;//跟踪微分期状态量微分项
float r;//时间尺度
float h;//ADRC系统积分时间
uint16 N0;//跟踪微分器解决速度超调h0=N*h
 
float h0;
float fh;//最速微分加速度跟踪量
/*****扩张状态观测器*******/
/******已系统输出y和输入u来跟踪估计系统状态和扰动*****/
float z1;
float z2;
float z3;//根据控制对象输入与输出,提取的扰动信息
float e;//系统状态误差
float y;//系统输出量
float fe;
float fe1;
float beta_01;
float beta_02;
float beta_03;
 
/**********系统状态误差反馈率*********/
float e0;//状态误差积分项
float e1;//状态偏差
float e2;//状态量微分项
float u0;//非线性组合系统输出
float u;//带扰动补偿后的输出
 
 
/*********第一种组合形式*********/
float beta_0;//线性
float beta_1;//非线性组合参数
float beta_2;//u0=beta_1*e1+beta_2*e2+(beta_0*e0);
/*********第二种组合形式*********/
float alpha1;//u0=beta_1*fal(e1,alpha1,zeta)+beta_2*fal(e2,alpha2,zeta)
float alpha2;//0<alpha1<1<alpha2
float zeta;//线性段的区间长度
/*********第三种组合形式*********/
float h1;//u0=-fhan(e1,e2,r,h1);
uint16 N1;//跟踪微分器解决速度超调h0=N*h
/*********第四种组合形式*********/
float c;//u0=-fhan(e1,c*e2*e2,r,h1);
float b0;//扰动补偿
}Fhan_Data;
 
 
 
void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2);
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC);
void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback);
 
extern Fhan_Data ADRC_Pitch_Controller,ADRC_Roll_Controller;
#endif
 

  • 10
    点赞
  • 24
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值