TD非线性跟踪微分器的C代码实现

一、何为跟踪微分器

TD即time delay,具有时间延迟作用，跟踪微分器会提供两个信号：一个是过渡信号，另一个是过渡信号的微分信号。

过渡信号：即起过渡作用的信号。这里的过渡是连续的，显示在图像上的则是一条光滑曲线。

二、使用跟踪微分器的原因

本人用PID好长一段时间了，由于噪音对系统的影响，我对单独的PID控制效果不满意，考虑ADRC，但是ADRC需要的参数也太多，而调ADRC的参并不容易，一个系统调好了，另一个系统也要换个参数调，TD是内嵌于ADRC算法的一项，且调参简单，因此我采用了ADRC中的TD。

三、在STM32上的C代码实现

1、.c文件

/*跟踪微分信号器，简称TD（time delay）算法*/
//对信号起到过渡作用//
/*使用步骤*/
/*
定义
TD_conctrol ABCD[4];

初始化
for(uint i=0;i<4;i++)
create_TD(&ABCD[i],parameter);

进行输入和计算
TD_Run(&ABCD[i],given_value);

获取输出结果
x=TD_get_R1(&ABCD[i]);
xx=TD_get_R2(&ABCD[i]);
单环PID采用ABCD[i].R1作为输入，双环PID采用R1为外环输入，R2为内环输入

参数整定
只需改变parameter,    parameter越大,收敛速度越快，反之亦然
*/

#include "td.h"
#include "struct_typedef.h"

float fst(float x1, float x2, float r, float h)//期望  跟踪速度  最大跟踪速度  控制的时间间隔
{

float y, d, a0, a1, a, sy, sa, result;
	y = x1 + h * x2;
	d = r * h*h;
	d = (d==0) ? 0.000001f : d;
	a0 = sqrtf(d*(d + 8.f * fabsf(y)));
	a1 = 0.5f *(a0 - d)* sign(y) + h * x2;
	sy = 0.5f *(sign(y+d)- sign(y-d));
	a = (h * x2 + y - a1)* sy + a1;
	sa = 0.5f *(sign(a+d)- sign(a-d));
	result = -r *((a / d - sign(a))* sa + sign(a));

	return result;
}



static void TD_cal(TD_conctrol * TD)   //计算得出预估的跟踪速度 预估的跟踪加速度 当前跟踪值 当前跟踪速度  
{
	TD->td.V1 = TD->td.R2;
	TD->td.V2 = fst(TD->td.R1-TD->Target, TD->td.R2, TD->td.R, TD->Time.Dtime);
	TD->td.R1 += TD->Time.Dtime * TD->td.V1;
	TD->td.R2 += TD->Time.Dtime * TD->td.V2;
}


static void getTimeStamp(TD_conctrol * TD)  
{
	TD->Time.Time_p = TD->Time.Time_n;
	TD->Time.Time_n = HAL_GetTick();
	TD->Time.Dtime = (TD->Time.Time_n - TD->Time.Time_p)/ timeradio;
}

static void TD_param_init(TD_conctrol * TD, float R)
{
	memset(TD, 0, sizeof(TD_conctrol));
	TD->td.R = R;
}

static void TD_inputStatus(TD_conctrol * TD, float target)
{
	TD->Lastnot0_target = TD->Target==0 ? TD->Lastnot0_target : TD->Target;
	TD->Target = target;
	getTimeStamp(TD);
}

void TD_Run(TD_conctrol * TD, float target)
{
	TD_inputStatus(TD, target);
	TD_cal(TD);

}

static void reset_TD(TD_conctrol * TD, float R)
{
	TD->td.R = R;
}

void create_TD(TD_conctrol* TD,float R)
{
	reset_TD(TD,R);
	TD_param_init(TD,R);

}

float TD_get_R1(TD_conctrol * TD)
{
	return TD->td.R1;
}

float TD_get_R2(TD_conctrol * TD)
{
	return TD->td.R2;
}

2、.h文件

#include <math.h>
#include <string.h>
#include <stdint.h>
#include "stm32f4xx_hal.h"

#define timeradio 1000.f
#define sign(x) ((x)>0?1:((x)<0?-1:1))
/*****************************/
typedef struct 
{
	float Target; //目标值
	float Lastnot0_target;  // 最后一次非零的目标值

	struct 
	{
		float R1;  // 跟踪微分器当前跟踪值
		float R2;  // 跟踪微分器当前跟踪速度
		float V1;  // 跟踪微分器预估的跟踪速度
		float V2;  // 跟踪微分器预估的跟踪加速度
		float R;   // *跟踪微分器最大跟踪速度
	}td;  // 跟踪微分器子结构

   struct 
	{
		uint32_t Time_p;  // 上次控制的时刻
		uint32_t Time_n;  // 本次控制的时刻
		float Dtime;      // 两次控制之间的时间
	}Time;


}TD_conctrol;

	
static void getTimeStamp(TD_conctrol * TD); // 记录当前的时间戳
static void TD_cal(TD_conctrol * TD);  // 计算跟踪微分器部分
static void TD_inputStatus(TD_conctrol * TD, float target);  // 输入当前状态
void TD_Run(TD_conctrol * TD, float target);  //输入目标值运行
static void reset_TD(TD_conctrol * TD, float new_R);      	// 重设跟踪微分器参数
static void TD_param_init(TD_conctrol * TD, float R);
float TD_get_R1(TD_conctrol * TD);
float TD_get_R2(TD_conctrol * TD);
void create_TD(TD_conctrol* TD,float R);

#endif

四、参考文献

跟踪微分器http://t.csdnimg.cn/zBU4X

【ADRC】跟踪微分器http://t.csdnimg.cn/NzMTdADRC的C代码开源https://github.com/zzqzzqzzq2002/ThisisADRC