ADRC太难调？试试这个‘拆解版’：手把手教你用C语言实现非线性跟踪微分器(TD)

非线性跟踪微分器(TD)的C语言实战：从原理到嵌入式实现

在工业控制和信号处理领域，我们经常需要从带有噪声的传感器信号中提取出干净的测量值和其微分信号。传统的一阶或二阶滤波器虽然简单，但在快速性和平滑性之间往往难以兼顾。这就是非线性跟踪微分器(Tracking Differentiator, TD)大显身手的地方——它能在几乎无超调的情况下快速跟踪输入信号，同时给出高质量的微分估计。

1. 非线性跟踪微分器的核心原理

非线性跟踪微分器最早由韩京清教授提出，作为自抗扰控制(ADRC)的核心组件之一。但它的价值远不止于此——当我们将TD从ADRC框架中独立出来，它就变成了一个强大的通用信号处理工具。

TD的核心数学表达可以简化为一个二阶系统：

ẋ₁ = x₂ ẋ₂ = fst(x₁-v, x₂, r, h)

其中v是输入信号，x₁是跟踪信号，x₂是微分信号，r决定跟踪速度，h是步长。关键在于fst（最速控制综合函数）的非线性设计。

fst函数的精妙之处在于它能够根据跟踪误差的大小自适应调整控制力度：

• 当误差较大时，采用最大控制量加速跟踪
• 当接近目标时，自动减小控制量避免超调

这种特性使得TD相比线性滤波器具有显著优势：

2. 关键算法实现：fst函数解析

让我们深入分析TD的核心——fst函数的C语言实现：

float fst(float x1, float x2, float r, float h) { float y = x1 + h * x2; float d = r * h * h; d = (d == 0) ? 0.000001f : d; // 避免除以零 float a0 = sqrtf(d * (d + 8.f * fabsf(y))); float a1 = 0.5f * (a0 - d) * sign(y) + h * x2; float sy = 0.5f * (sign(y + d) - sign(y - d)); float a = (h * x2 + y - a1) * sy + a1; float sa = 0.5f * (sign(a + d) - sign(a - d)); return -r * ((a / d - sign(a)) * sa + sign(a)); }

这个看似复杂的函数实际上实现了智能的Bang-Bang控制：

1. 计算预测位置y = x1 + h*x2
2. 确定控制边界d = r*h²（与最大加速度相关）
3. 根据位置误差y的大小选择控制策略：
   • 当|y| > d时，采用最大控制量（Bang-Bang控制）
   • 当|y| ≤ d时，采用平滑过渡策略

参数调节指南：

• r：决定跟踪速度，典型值在10-1000之间
  • 增大r→ 加快跟踪速度但可能引入高频噪声
  • 减小r→ 更平滑但响应变慢
• h：应与实际采样时间一致（如STM32中常用1ms）

3. 完整的嵌入式实现方案

下面我们构建一个完整的TD模块，适合在STM32等嵌入式平台使用。首先定义数据结构：

typedef struct { float Target; // 当前目标值 float LastNonZeroTarget; // 最后非零目标（用于零输入处理） struct { float R1; // 跟踪值 float R2; // 微分值 float V1; // 内部状态1 float V2; // 内部状态2 float R; // 速度因子 } td; struct { uint32_t TimePrev; // 上次时间戳 uint32_t TimeNow; // 当前时间戳 float DeltaTime; // 时间间隔 } Time; } TD_Controller;

关键操作函数实现：

void TD_Run(TD_Controller* td, float target) { // 更新输入状态 td->LastNonZeroTarget = (target == 0) ? td->LastNonZeroTarget : target; td->Target = target; // 计算时间差（毫秒） td->Time.TimePrev = td->Time.TimeNow; td->Time.TimeNow = HAL_GetTick(); td->Time.DeltaTime = (td->Time.TimeNow - td->Time.TimePrev) / 1000.0f; // 核心计算 td->td.V1 = td->td.R2; td->td.V2 = fst(td->td.R1 - td->Target, td->td.R2, td->td.R, td->Time.DeltaTime); // 更新状态 td->td.R1 += td->Time.DeltaTime * td->td.V1; td->td.R2 += td->Time.DeltaTime * td->td.V2; }

使用流程：

1. 初始化控制器：

   TD_Controller td; create_TD(&td, 50.0f); // 初始化，设置r=50

2. 在主循环中调用：

   while(1) { float sensor_value = read_sensor(); TD_Run(&td, sensor_value); float filtered_value = TD_get_R1(&td); // 获取滤波后信号 float differential = TD_get_R2(&td); // 获取微分信号 delay_ms(1); }

4. 实际应用案例：编码器信号处理

假设我们有一个带噪声的编码器信号，需要同时获取位置和速度信息。传统方法可能需要两个独立的滤波器，而使用TD可以一举两得。

配置参数建议：

• 对于100Hz的编码器信号：
  • r = 100-500（根据机械系统刚性调整）
  • 采样时间h = 0.01s（与编码器读取周期一致）

性能对比测试：

测试条件：输入为叠加了高斯白噪声的阶跃信号

调试技巧：

1. 初始调试时，可以先给一个阶跃输入，观察响应：
   • 如果出现振荡，适当减小r
   • 如果响应太慢，适当增大r
2. 实际应用中，可以将r设计为可在线调节参数，方便现场调试
3. 对于特别高频的噪声，可以在TD前加一个简单的一阶预滤波

// 简单的预滤波实现 float pre_filter(float new_value) { static float filtered = 0; filtered = 0.8f * filtered + 0.2f * new_value; return filtered; }

5. 进阶应用：多阶TD与复合控制

对于更高要求的应用，我们可以使用串级TD结构：

// 二阶TD结构 typedef struct { TD_Controller td1; // 第一级 TD_Controller td2; // 第二级 } TwoStageTD; void TwoStageTD_Run(TwoStageTD* ttd, float target) { TD_Run(&ttd->td1, target); TD_Run(&ttd->td2, TD_get_R1(&ttd->td1)); // 最终输出 float position = TD_get_R1(&ttd->td2); float velocity = TD_get_R2(&ttd->td1) + TD_get_R2(&ttd->td2); float acceleration = TD_get_R2(&ttd->td2); }

这种结构特别适用于：

• 需要更高阶微分信号的应用
• 对噪声特别敏感的环境
• 要求超调量严格为零的精密控制

参数调节策略：

• 第一级TD：较大的r值，快速跟踪
• 第二级TD：较小的r值，平滑输出
• 典型比例：r1 = 3 * r2

6. 常见问题与解决方案

Q1：TD输出出现高频振荡怎么办？

• 可能原因：r值过大或采样时间h不准确
• 解决方案：
  1. 逐步减小r直到振荡消失
  2. 检查实际采样间隔是否与h设置一致
  3. 考虑加入预滤波

Q2：如何应对输入信号的突变？

• 解决方案：实现动态r调整

  // 根据误差动态调整r float error = fabs(td->Target - td->td.R1); td->td.R = base_r * (1 + error * adaptive_factor);

Q3：在资源受限的MCU上如何优化？

• 优化技巧：
  1. 使用查表法替代sqrtf()和fabsf()
  2. 将浮点运算转换为定点运算
  3. 适当降低采样频率

// 快速近似平方根实现（精度与速度的折衷） float fast_sqrt(float x) { union { float f; uint32_t i; } u; u.f = x; u.i = 0x5F3759DF - (u.i >> 1); return x * u.f * (1.5f - 0.5f * x * u.f * u.f); }

7. 性能优化与特殊场景处理

实时性优化： 对于高动态应用，可以采用以下优化策略：

1. 时间戳优化：

// 使用定时器中断确保精确采样 void TIMx_IRQHandler(void) { static uint32_t last_tick = 0; uint32_t current_tick = HAL_GetTick(); float h = (current_tick - last_tick) / 1000.0f; last_tick = current_tick; TD_Run_With_Custom_Step(&td, sensor_read(), h); }

2. 抗积分饱和处理： 当系统长时间处于饱和状态时，需要特殊处理：

void TD_Run_With_AntiWindup(TD_Controller* td, float target) { // 正常TD计算 TD_Run(td, target); // 抗饱和处理 if(fabs(td->td.R2) > max_speed) { td->td.R2 = sign(td->td.R2) * max_speed; } }

特殊输入处理：

1. 零输入保持：

float TD_Get_Smart_R1(TD_Controller* td) { return (td->Target == 0) ? td->LastNonZeroTarget : td->td.R1; }

2. 输入突变检测：

void TD_Run_With_Change_Detection(TD_Controller* td, float target) { static float last_target = 0; float change_rate = fabs(target - last_target) / td->Time.DeltaTime; if(change_rate > threshold) { // 突变时临时提高跟踪速度 float original_r = td->td.R; td->td.R *= 3.0f; TD_Run(td, target); td->td.R = original_r; } else { TD_Run(td, target); } last_target = target; }