基于ADRC的FOC位置环控制

ADRC介绍

自抗 扰 控 制 技 术 是 近 几 年 以来 在 非 线 性 PID 控 制 的基 础 上 发 展 而 来 的一 种 改 进 的 新 型 非 线 性 控 制 技 术 ，该算法在PID的基础上，引多了“安排过渡过程”和同时合理“提取微分信号”的方法,并加多了扩张状态观测器对系统的未知模型和外部扰动总和作用量进行实时估计和补偿，自抗扰控 主 要 包 括 跟 踪 微 分 器 (Tracking Differentiator，TD)、扩 张状 态观 测器 (ExtendedState Observer，ESO)和 非 线性状 态 误差 反馈 (Nonlinear StatesErrorFeed．Back，NLSEF)三 部分 。

1.非线性跟踪微分器

x1为输入v的过渡信号
x2为输入v的微分的过渡信号
h为计算步长槽。越大滤波效果更好，但跟踪信号的相位损失越大。
r为跟踪速度因子，值越大跟踪速度越快
fhan为离散域最速控制综合函数
α为非线性因子
跟踪微分器可以使给定的输入信号有一个缓冲的过称，减少超调现象，使经过跟踪微分器后的输出有一个爬升的过称。fhan为最速控制函数。
跟踪微分器是一个由误差提取误差微分的过称，同时能让控制量变得缓和。

2.非线性ESO观测器

ESO是一种通过对不确定转改对象的观测器形式的跟踪微分器进行改造而获得的状态观测器。 过将ESO观测出的对象模型的内、外扰动的实时作用量补偿到控制中,可将不确定非线性系统动态地转化为积分器串联型结构系统。
z1为系统输入的观测值
z2为系统输入的微分的观测值
z3为系统总扰动
fe fe1为非线性扰动
fal为非线性函数
β_01 β_02β_03为可调因子
为滤波因子，该参数越大，滤波效果越好

非线性反馈函数，用非线性反馈代替线性反馈，提高了收敛速度和抗扰动能力。
传统观测器使用误差积分，但是作用速度慢且会引起超调。
ESO将系统编程积分串联形式，将误差在控制量中补偿掉，是ADRC的灵魂所在。

3.非线性状态误差反馈控制律原理(NLSEF)

非线性状态误差反馈控制律采用了一种独立于对象模型的非线性控制器结构。NLSEF自动检测系统扰动，并给与补偿分量，若ESO的速度足够快，那么这个补偿分量就能精确地反映出系统的扰动情况。被控对象被化为积分串联型后，就可构造出理想控制器，这完全归功于该分量的补偿作用。补偿分量作用的实质是一种抗扰作用。
e1是偏差误差 e2是微分误差 u0是未经补偿的控制量 u是经过扰动补偿后的控制量

4.调参流程：

1.调TD

先单独调TD，r的数量级一般为10的5次方，h的数量级一般为10的负三次方。设置完r和h后，设定一个过滤变化的数据输入，将x1和输入的数据绘出，观察是否能够跟踪上。
TD跟踪效果（粗调）

2.调ESO

调完TD后，在调ESO时，需要先将NLSEF的参数大致调一下，这是由于ESO中的公式与NLSEF相关联。将alpha1和alpha2调为0.5和1.5，belta1和belta2设置为1。然后开始调ESO的数据，ESO主要用来观测输入量和外部扰动，以控制某类电机的速度举例，ESO则用来观测速度和电机受到的扰动。将电机的实际速度和设定输入设置进入函数，显示实际速度和观测速度z1，观察观测效果，z1是否能够跟踪电机的实际速度，给电机添加扰动，观察z3是否正确变化，b一般设为1或2。
ESO观测速度值与真实速度值（黄色为真实值）

3.调NLSEF

先调alpha1和alpha2，0<alpha1<1<alpha2，belta1和belta2即PID的P和D，根据需要的控制效果进行调整。
NLSEF控制效果（黄色为设定值10）

5.基于ADRC的位置环设计

在对ADRC调参完成后，将其应用在位置环中。控制框图如图所示。

ADRC框图

基于ADRC的位置环控制框图

ADRC控制角度效果（设定值8000）

5.ADRC代码

#include "MC_ADRC.h"
#include "MC_Globals.h"
#include "math.h"
#include "stdlib.h"
#include "uploading.h"
#include "MC_PID_regulators.h"
#include "stm32f40x_as5048.h"


//参数区，这11个就是需要用户整定的参数
/****************TD**********/
float r = 200000,//快速跟踪因子
      h = 0.0003;//滤波因子,系统调用步长

/**************ESO**********/
float b       = 2,//系统系数
      delta   = 0.0021,//delta为fal（e，alpha，delta）函数的线性区间宽度
      belta01 = 30,//扩张状态观测器反馈增益1
			belta02 = 300,//扩张状态观测器反馈增益2
			belta03 = 10000;//扩张状态观测器反馈增益3
	  
/**************NLSEF*******/
float alpha0 = 0.4,//
			alpha1 = 0.8,//
      alpha2 = 1.2,//

			belta0 = 0,//跟踪输入信号增益
      belta1 = -1.8,//跟踪输入信号增益
      belta2 = -0.02;//跟踪微分信号增益
	  

	  
/*****************************fhan函数*********************************/
float fhan(float x1,float x2,float r,float h)
{
 /*****************************第一套************************/
  float d    = 0,
        a    = 0,
				a0   = 0,
				a1   = 0,
				a2   = 0,
				y    = 0,
				fhan = 0;
  
  d    =  r*h*h;
  a0   =  h*x2;
  y    =  x1+a0;
  a1   =  sqrtf(d*(d+8*fabsf(y)));
  a2   =  a0 + LSign(y)*(a1 - d)/2.0;
  a    =  (a0+y)*(LSign(y+d)-LSign(y-d))/2.0  + a2*(1-(LSign(y+d)-LSign(y-d))/2.0);
  fhan = -r*(a/d)*(LSign(y+d)-LSign(y-d))/2.0 - r*LSign(a)*(1-(LSign(a+d)-LSign(a-d))/2.0);
	
 /******************************第二套**********************/
//	float deltaa  =0,
//				deltaa0 =0,
//	      y       =0,
//	      a0      =0,
//	      a       =0,
//	      fhan    =0;
//	
//	deltaa = r*h;
//	deltaa0 = deltaa*h;
//	y=fx1+fx2*h;
//	a0 = sqrtf(deltaa*deltaa+8*r*fabsf(y));
//	if(fabsf(y)<=deltaa0)
//		a=fx2+y/h;
//	else
//		a=fx2+0.5*(a0-deltaa)*sign(y);
//	if(fabsf(a)<=deltaa)
//		fhan = -r*a/deltaa;
//	else
//		fhan = -r*sign(a);
	
  return fhan;
}
/************************************sign函数***************************/

int LSign(float x)
{
	if(x>0)
	{
		return 1 ;
	}	
	else if(x<0)
	{
	return -1;
	}	
	else
	{
			return 0;
	}
	
}
/*******************************fal函数**********************************/
float fal(float e,float alpha,float delta)
{
  float result = 0,fabsf_e = 0;
  
  fabsf_e = fabsf(e);
  
  if(delta>=fabsf_e)
    result = e/powf(delta,1.0-alpha);
  else //if(delta<fabsf_e)
    result = powf(fabsf_e,alpha)*LSign(e);
 
 return result;     
}


//中间变量区，不需要用户管理以及赋值
/****************TD*******************/
float x1 = 0,//跟踪输入
      x2 = 0,//跟踪输入的微分
/****************ESO******************/
      e  = 0,//误差
			z1 = 0,//跟踪反馈值
			z2 = 0,//跟踪反馈值的而微分
			z3 = 0,//跟踪系统的扰动（总扰动）
/**************NLSEF******************/
      u = 0;//输出值
/********************************ADRC************************************/
float ADRC(float v,float y)
{
  float u0 = 0,
				e0 = 0,
        e1 = 0,
				e2 = 0;
/******************************TD****************************************/
  x1 = x1 + h*x2;
  x2 = x2 + h*fhan(x1-v,x2,r,h);
	
	
	
/******************************ESO***************************************/
	
	
  e = z1 - y;
  z1 = z1 + h*(z2-belta01*e);
  z2 = z2 + h*(z3-belta02*fal(e,0.5,delta)+b*u);
  z3 = z3 + h*(-belta03*fal(e,0.25,delta));
	
	
	
/******************限幅，ADRC正常的话不会达到限幅条件********************/
  if(z1>=30000) z1=30000;
  if(z1<=-30000) z1 = -30000;
  if(z2>=30000) z2=30000;
  if(z2<=-30000) z2 = -30000;
  if(z3>=30000) z3=30000;
  if(z3<=-30000) z3 = -30000;
/******************************NLSEF*************************************/

	e0  = e0+e1;
  e1 = x1 - z1;
  e2 = x2 - z2;
  
//  u0 = belta1*fal(e1,alpha1,delta) + belta2*fal(e2,alpha2,delta);//其中0<alpha1<1<alpha2
	u0 = belta1*fal(e1,alpha1,delta) + belta2*fal(e2,alpha2,delta)+belta0*fal(e0,alpha0,delta);//其中0<alpha1<1<alpha2
  
  u = u0 - z3/b;
  
	Pack_Send_data(u0,0,v,z1,z2,z3,0xA1);
	
  return u0;
}

获取源码：公众号【程序员DeRozan】回复1207