自抗扰（ADRC）控制原理及控制器设计

https://blog.csdn.net/weixin_43487974/article/details/127163824?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2_defaultYuanLiJiHua_{Position-2-127163824-blog-122591196.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2}default_YuanLiJiHuaPosition-2-127163824-blog-122591196.pc_relevant_default&utm_relevant_index=5

//最速控制综合函数
FH_Float fhan(FH_Float x1, FH_Float x2, FH_Float ctrlError, int r, FH_Float h)
{
	FH_Float d = r*h;
	FH_Float d0 = d*h;
	FH_Float y = x1 - ctrlError + h*x2;
	FH_Float a0 = sqrt(d*d + 8*r*fabs(y));
	FH_Float a,f;
	if(fabs(y) <= d0)
		a = x2 + y/h;
	else
		a = x2 + 0.5*(a0 -d)*Sign(y);
	if(fabs(a) <= d)
		f = -r*a/d;
	else
		f = -r*Sign(a);
//	LogPrintf("fhan f=%f,a=%f,a0=%f,y=%f,pid=%f,x1=%f,x2=%f./n",f,a,a0,y,ctrlError,x1,x2);
	return f;		
}


//TD微分跟踪器
FH_Float TD_differential_track(struct PIDController_t* pidCtrller)
{
//	int r = 10; //调节系数
	struct PIDControllerParams_t* pidParams = (struct PIDControllerParams_t*)pidCtrller->base.params;
	int r = pidParams->td_r;
	FH_Float h = 0.02; //滤波因子 略大于步长(0.01)
	FH_Float x_1k = pidCtrller->td_x1;
	FH_Float x_2k = pidCtrller->td_x2;
	FH_Float rtn = fhan(x_1k,x_2k,pidCtrller->ctrlError,r,h);
	pidCtrller->td_x1 = x_1k + AgvSamplePeriod*x_2k;
	pidCtrller->td_x2 = x_2k + AgvSamplePeriod*rtn;
	pidCtrller->ctrlError = pidCtrller->td_x1;
 
}