ADRC
优化fhan
《自抗扰控制入门》
自抗扰死忠粉
![在这里插入图片描述](https://img-blog.csdnimg.cn/20191221224958533.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MzI4ODEzMg==,size_16,color_FFFFFF,t_70)
ADRC.H
#ifndef _ADRC_H_
#define _ADRC_H_
typedef struct
{
float x1;
float x2;
float r;
float h;
uint16 N0;
float h0;
float fh;
float fst;
float z1;
float z2;
float z3;
float e;
float y;
float fe;
float fe1;
float beta_01;
float beta_02;
float beta_03;
float b;
float e0;
float e1;
float e2;
float u0;
float u;
float b0;
float beta_0;
float beta_1;
float beta_2;
float alpha1;
float alpha2;
float zeta;
float h1;
uint16 N1;
float c;
}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
ADRC.C
#include "ADRC.h"
Fhan_Data ADRC_Pitch_Controller;
Fhan_Data ADRC_Roll_Controller;
const float ADRC_Unit[3][16]=
{
{300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 2.0, 0.0010, 5, 5, 0.8, 1.5, 50, 0},
{300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 2.0, 0.0010, 5, 5, 0.8, 1.5, 50, 0},
{300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 1.2, 0.0005, 5, 5, 0.8, 1.5, 50, 0},
};
float Constrain_Float(float amt, float low, float high){
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
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];
}
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)
{
float d=0,a0=0,y=0,a1=0,a2=0,a=0;
float x1_delta=0;
x1_delta=fhan_Input->x1-expect_ADRC;
fhan_Input->h0=fhan_Input->N0*fhan_Input->h;
d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;
a0=fhan_Input->h0*fhan_Input->x2;
y=x1_delta+a0;
a1=sqrt(d*(d+8*ABS(y)));
a2=a0+Sign_ADRC(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;
fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;
}
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;
}
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->h);
fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,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->b*fhan_Input->u);
fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1);
}
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{
float temp_e2=0;
temp_e2=Constrain_Float(fhan_Input->e2,-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_ADRC(fhan_Input,expect_ADRC);
fhan_Input->y=feedback_ADRC;
ESO_ADRC(fhan_Input);
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;
Nolinear_Conbination_ADRC(fhan_Input);
fhan_Input->u=Constrain_Float(fhan_Input->u0,-200,200);
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)