├── ADRC.c └── ADRC.h /ADRC.c: -------------------------------------------------------------------------------- 1 | #include "Headfile.h" 2 | #include "ADRC.h" 3 | /*----------------------------------------------------------------------------------------------------------------------/ 4 | * 本程序只供购买者学习使用，版权著作权属于无名科创团队， 5 | * 无名科创团队将飞控程序源码提供给购买者， 6 | * 购买者要为无名科创团队提供保护， 7 | * 未经作者许可，不得将源代码提供给他人 8 | * 不得将源代码放到网上供他人免费下载， 9 | * 更不能以此销售牟利，如发现上述行为， 10 | * 无名科创团队将诉之以法律解决！！！ 11 | -----------------------------------------------------------------------------------------------------------------------/ 12 | * 生命不息、奋斗不止；前人栽树，后人乘凉！！！ 13 | * 开源不易，且学且珍惜，祝早日逆袭、进阶成功！！！ 14 | -----------------------------------------------------------------------------------------------------------------------/ 15 | * 无名科创开源飞控 V1.1 武汉科技大学 By.YuYi 16 | * CSDN博客: http://blog.csdn.net/u011992534 17 | * 优酷ID：NamelessCotrun无名小哥 18 | * 无名科创开源飞控QQ群：540707961 19 | * https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2 20 | * 百度贴吧:无名科创开源飞控 21 | * 修改日期:2017/10/30 22 | * 版本：V1.1 23 | * 版权所有，盗版必究。 24 | * Copyright(C) 武汉科技大学无名科创团队 2017-2019 25 | * All rights reserved 26 | ----------------------------------------------------------------------------------------------------------------------*/ 27 | Fhan_Data ADRC_Pitch_Controller; 28 | Fhan_Data ADRC_Roll_Controller; 29 | const float ADRC_Unit[3][16]= 30 | { 31 | /*TD跟踪微分器 改进最速TD,h0=N*h 扩张状态观测器ESO 扰动补偿 非线性组合*/ 32 | /* r h N beta_01 beta_02 beta_03 b0 beta_0 beta_1 beta_2 N1 C alpha1 alpha2 zeta b*/ 33 | {300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 2.0, 0.0010, 5, 5, 0.8, 1.5, 50, 0}, 34 | {300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 2.0, 0.0010, 5, 5, 0.8, 1.5, 50, 0}, 35 | {300000 ,0.005 , 3, 300, 4000, 10000, 0.001, 0.002, 1.2, 0.0005, 5, 5, 0.8, 1.5, 50, 0}, 36 | }; 37 | 38 | 39 | float Constrain_Float(float amt, float low, float high){ 40 | return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); 41 | } 42 | 43 | int16_t Sign_ADRC(float Input) 44 | { 45 | int16_t output=0; 46 | if(Input>1E-6) output=1; 47 | else if(Input<-1E-6) output=-1; 48 | else output=0; 49 | return output; 50 | } 51 | 52 | int16_t Fsg_ADRC(float x,float d) 53 | { 54 | int16_t output=0; 55 | output=(Sign_ADRC(x+d)-Sign_ADRC(x-d))/2; 56 | return output; 57 | } 58 | 59 | 60 | void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2) 61 | { 62 | fhan_Input1->r=ADRC_Unit[0][0]; 63 | fhan_Input1->h=ADRC_Unit[0][1]; 64 | fhan_Input1->N0=(uint16)(ADRC_Unit[0][2]); 65 | fhan_Input1->beta_01=ADRC_Unit[0][3]; 66 | fhan_Input1->beta_02=ADRC_Unit[0][4]; 67 | fhan_Input1->beta_03=ADRC_Unit[0][5]; 68 | fhan_Input1->b0=ADRC_Unit[0][6]; 69 | fhan_Input1->beta_0=ADRC_Unit[0][7]; 70 | fhan_Input1->beta_1=ADRC_Unit[0][8]; 71 | fhan_Input1->beta_2=ADRC_Unit[0][9]; 72 | fhan_Input1->N1=(uint16)(ADRC_Unit[0][10]); 73 | fhan_Input1->c=ADRC_Unit[0][11]; 74 | 75 | fhan_Input1->alpha1=ADRC_Unit[0][12]; 76 | fhan_Input1->alpha2=ADRC_Unit[0][13]; 77 | fhan_Input1->zeta=ADRC_Unit[0][14]; 78 | fhan_Input1->b=ADRC_Unit[0][15]; 79 | 80 | fhan_Input2->r=ADRC_Unit[1][0]; 81 | fhan_Input2->h=ADRC_Unit[1][1]; 82 | fhan_Input2->N0=(uint16)(ADRC_Unit[1][2]); 83 | fhan_Input2->beta_01=ADRC_Unit[1][3]; 84 | fhan_Input2->beta_02=ADRC_Unit[1][4]; 85 | fhan_Input2->beta_03=ADRC_Unit[1][5]; 86 | fhan_Input2->b0=ADRC_Unit[1][6]; 87 | fhan_Input2->beta_0=ADRC_Unit[1][7]; 88 | fhan_Input2->beta_1=ADRC_Unit[1][8]; 89 | fhan_Input2->beta_2=ADRC_Unit[1][9]; 90 | fhan_Input2->N1=(uint16)(ADRC_Unit[1][10]); 91 | fhan_Input2->c=ADRC_Unit[1][11]; 92 | 93 | fhan_Input2->alpha1=ADRC_Unit[1][12]; 94 | fhan_Input2->alpha2=ADRC_Unit[1][13]; 95 | fhan_Input2->zeta=ADRC_Unit[1][14]; 96 | fhan_Input2->b=ADRC_Unit[1][15]; 97 | } 98 | 99 | 100 | 101 | //ADRC最速跟踪微分器TD，改进的算法fhan 102 | void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)//安排ADRC过度过程 103 | { 104 | float d=0,a0=0,y=0,a1=0,a2=0,a=0; 105 | float x1_delta=0;//ADRC状态跟踪误差项 106 | x1_delta=fhan_Input->x1-expect_ADRC;//用x1-v(k)替代x1得到离散更新公式 107 | fhan_Input->h0=fhan_Input->N0*fhan_Input->h;//用h0替代h，解决最速跟踪微分器速度超调问题 108 | d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;//d=rh^2; 109 | a0=fhan_Input->h0*fhan_Input->x2;//a0=h*x2 110 | y=x1_delta+a0;//y=x1+a0 111 | a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))]) 112 | a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2; 113 | a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d)); 114 | fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d) 115 | -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量 116 | fhan_Input->x1+=fhan_Input->h*fhan_Input->x2;//跟新最速跟踪状态量x1 117 | fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;//跟新最速跟踪状态量微分x2 118 | } 119 | 120 | 121 | //原点附近有连线性段的连续幂次函数 122 | float Fal_ADRC(float e,float alpha,float zeta) 123 | { 124 | int16 s=0; 125 | float fal_output=0; 126 | s=(Sign_ADRC(e+zeta)-Sign_ADRC(e-zeta))/2; 127 | fal_output=e*s/(powf(zeta,1-alpha))+powf(ABS(e),alpha)*Sign_ADRC(e)*(1-s); 128 | return fal_output; 129 | } 130 | 131 | 132 | 133 | 134 | /************扩张状态观测器********************/ 135 | //状态观测器参数beta01=1/h beta02=1/(3*h^2) beta03=2/(8^2*h^3) ... 136 | void ESO_ADRC(Fhan_Data *fhan_Input) 137 | { 138 | fhan_Input->e=fhan_Input->z1-fhan_Input->y;//状态误差 139 | 140 | fhan_Input->fe=Fal_ADRC(fhan_Input->e,0.5,fhan_Input->h);//非线性函数，提取跟踪状态与当前状态误差 141 | fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,fhan_Input->h); 142 | 143 | /*************扩展状态量更新**********/ 144 | fhan_Input->z1+=fhan_Input->h*(fhan_Input->z2-fhan_Input->beta_01*fhan_Input->e); 145 | fhan_Input->z2+=fhan_Input->h*(fhan_Input->z3 146 | -fhan_Input->beta_02*fhan_Input->fe 147 | +fhan_Input->b*fhan_Input->u); 148 | //ESO估计状态加速度信号，进行扰动补偿，传统MEMS陀螺仪漂移较大，估计会产生漂移 149 | fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1); 150 | } 151 | 152 | 153 | /************非线性组合****************/ 154 | /* 155 | void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input) 156 | { 157 | float d=0,a0=0,y=0,a1=0,a2=0,a=0; 158 | float Sy=0,Sa=0;//ADRC状态跟踪误差项 159 | 160 | fhan_Input->h1=fhan_Input->N1*fhan_Input->h; 161 | 162 | d=fhan_Input->r*fhan_Input->h1*fhan_Input->h1; 163 | a0=fhan_Input->h1*fhan_Input->c*fhan_Input->e2; 164 | y=fhan_Input->e1+a0; 165 | a1=sqrt(d*(d+8*ABS(y))); 166 | a2=a0+Sign_ADRC(y)*(a1-d)/2; 167 | 168 | Sy=Fsg_ADRC(y,d); 169 | a=(a0+y-a2)*Sy+a2; 170 | Sa=Fsg_ADRC(a,d); 171 | fhan_Input->u0=-fhan_Input->r*((a/d)-Sign_ADRC(a))*Sa-fhan_Input->r*Sign_ADRC(a); 172 | 173 | //a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d)); 174 | 175 | //fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d) 176 | // -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量 177 | } 178 | */ 179 | void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input) 180 | { 181 | float temp_e2=0; 182 | temp_e2=Constrain_Float(fhan_Input->e2,-3000,3000); 183 | fhan_Input->u0=fhan_Input->beta_1*Fal_ADRC(fhan_Input->e1,fhan_Input->alpha1,fhan_Input->zeta) 184 | +fhan_Input->beta_2*Fal_ADRC(temp_e2,fhan_Input->alpha2,fhan_Input->zeta); 185 | 186 | } 187 | 188 | 189 | void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback_ADRC) 190 | { 191 | /*自抗扰控制器第1步*/ 192 | /******** 193 | ** 194 | ** 195 | ** 196 | ** 197 | ** 198 | ********/ 199 | /***** 200 | 安排过度过程，输入为期望给定， 201 | 由TD跟踪微分器得到： 202 | 过度期望信号x1，过度期望微分信号x2 203 | ******/ 204 | Fhan_ADRC(fhan_Input,expect_ADRC); 205 | 206 | /*自抗扰控制器第2步*/ 207 | /******** 208 | * 209 | * 210 | **** 211 | * 212 | * 213 | ********/ 214 | /************系统输出值为反馈量，状态反馈，ESO扩张状态观测器的输入*********/ 215 | fhan_Input->y=feedback_ADRC; 216 | /***** 217 | 扩张状态观测器，得到反馈信号的扩张状态： 218 | 1、状态信号z1； 219 | 2、状态速度信号z2； 220 | 3、状态加速度信号z3。 221 | 其中z1、z2用于作为状态反馈与TD微分跟踪器得到的x1,x2做差后， 222 | 经过非线性函数映射，乘以beta系数后， 223 | 组合得到未加入状态加速度估计扰动补偿的原始控制量u 224 | *********/ 225 | ESO_ADRC(fhan_Input);//低成本MEMS会产生漂移，扩展出来的z3此项会漂移，目前暂时未想到办法解决，未用到z3 226 | /*自抗扰控制器第3步*/ 227 | /******** 228 | ** 229 | ** 230 | ** 231 | ** 232 | ** 233 | ********/ 234 | /********状态误差反馈率***/ 235 | fhan_Input->e0+=fhan_Input->e1*fhan_Input->h;//状态积分项 236 | fhan_Input->e1=fhan_Input->x1-fhan_Input->z1;//状态偏差项 237 | fhan_Input->e2=fhan_Input->x2-fhan_Input->z2;//状态微分项， 238 | /********线性组合*******/ 239 | /* 240 | fhan_Input->u0=//fhan_Input->beta_0*fhan_Input->e0 241 | +fhan_Input->beta_1*fhan_Input->e1 242 | +fhan_Input->beta_2*fhan_Input->e2; 243 | */ 244 | Nolinear_Conbination_ADRC(fhan_Input); 245 | /**********扰动补偿*******/ 246 | //fhan_Input->u=fhan_Input->u0 247 | // -fhan_Input->z3/fhan_Input->b0; 248 | //由于MEMS传感器漂移比较严重，当beta_03取值比较大时，长时间z3漂移比较大，目前不加入扰动补偿控制量 249 | fhan_Input->u=Constrain_Float(fhan_Input->u0,-200,200); 250 | } 251 | 252 | -------------------------------------------------------------------------------- /ADRC.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADRC_H_ 2 | #define _ADRC_H_ 3 | /*----------------------------------------------------------------------------------------------------------------------/ 4 | * 本程序只供购买者学习使用，版权著作权属于无名科创团队， 5 | * 无名科创团队将飞控程序源码提供给购买者， 6 | * 购买者要为无名科创团队提供保护， 7 | * 未经作者许可，不得将源代码提供给他人 8 | * 不得将源代码放到网上供他人免费下载， 9 | * 更不能以此销售牟利，如发现上述行为， 10 | * 无名科创团队将诉之以法律解决！！！ 11 | -----------------------------------------------------------------------------------------------------------------------/ 12 | * 生命不息、奋斗不止；前人栽树，后人乘凉！！！ 13 | * 开源不易，且学且珍惜，祝早日逆袭、进阶成功！！！ 14 | -----------------------------------------------------------------------------------------------------------------------/ 15 | * 无名科创开源飞控 V1.1 武汉科技大学 By.YuYi 16 | * CSDN博客: http://blog.csdn.net/u011992534 17 | * 优酷ID：NamelessCotrun无名小哥 18 | * 无名科创开源飞控QQ群：540707961 19 | * https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2 20 | * 百度贴吧:无名科创开源飞控 21 | * 修改日期:2017/10/30 22 | * 版本：V1.1 23 | * 版权所有，盗版必究。 24 | * Copyright(C) 武汉科技大学无名科创团队 2017-2019 25 | * All rights reserved 26 | ----------------------------------------------------------------------------------------------------------------------*/ 27 | typedef struct 28 | { 29 | /*****安排过度过程*******/ 30 | float x1;//跟踪微分期状态量 31 | float x2;//跟踪微分期状态量微分项 32 | float r;//时间尺度 33 | float h;//ADRC系统积分时间 34 | uint16 N0;//跟踪微分器解决速度超调h0=N*h 35 | 36 | float h0; 37 | float fh;//最速微分加速度跟踪量 38 | /*****扩张状态观测器*******/ 39 | /******已系统输出y和输入u来跟踪估计系统状态和扰动*****/ 40 | float z1; 41 | float z2; 42 | float z3;//根据控制对象输入与输出，提取的扰动信息 43 | float e;//系统状态误差 44 | float y;//系统输出量 45 | float fe; 46 | float fe1; 47 | float beta_01; 48 | float beta_02; 49 | float beta_03; 50 | float b; 51 | 52 | 53 | /**********系统状态误差反馈率*********/ 54 | float e0;//状态误差积分项 55 | float e1;//状态偏差 56 | float e2;//状态量微分项 57 | float u0;//非线性组合系统输出 58 | float u;//带扰动补偿后的输出 59 | float b0;//扰动补偿 60 | 61 | /*********第一种组合形式*********/ 62 | float beta_0;//线性 63 | float beta_1;//非线性组合参数 64 | float beta_2;//u0=beta_1*e1+beta_2*e2+(beta_0*e0); 65 | /*********第二种组合形式*********/ 66 | float alpha1;//u0=beta_1*fal(e1,alpha1,zeta)+beta_2*fal(e2,alpha2,zeta) 67 | float alpha2;//0