当前位置：首页 > news >正文

从PID到ADRC：非线性跟踪微分器(TD)的C语言实践与参数调优

news 2026/7/9 9:02:32

1. 从PID到ADRC：为什么需要非线性跟踪微分器

如果你用过PID控制器，肯定遇到过这样的烦恼：传感器信号稍微有点噪声，输出就开始抖个不停。我以前调试四轴飞行器时，陀螺仪数据哪怕只有5%的波动，电机转速就会像抽风一样忽快忽慢。这就是传统PID的硬伤——对噪声太敏感。

ADRC（自抗扰控制）像是PID的全面升级版，它用非线性跟踪微分器（TD）作为"前端处理器"。这个TD模块的神奇之处在于，它能从带噪声的输入信号中提取出光滑的过渡信号和干净的微分信号。我去年给工业机械臂做运动控制时实测过，同样的电机噪声条件下，经过TD处理的信号比直接差分求导的波动减少了70%。

但ADRC有个让人头疼的问题：参数太多。光是扩张状态观测器就有五六个参数要调。相比之下，TD模块只需要调一个参数R——这个参数直接决定跟踪速度。就像开车时只用一个油门踏板就能控制加速节奏，调参效率提升的不是一星半点。

2. TD的核心算法：fst()函数解剖

2.1 快速收敛的非线性函数

TD的核心是那个看起来有点吓人的fst()函数。别被它的数学表达式唬住，其实它的工作原理很像我们日常中的"渐进式刹车"：

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)); // 最终输出 }

这个函数的精妙之处在于它的非线性处理：

当误差较大时（|y|>d），输出接近±r，相当于全速跟踪
当接近目标时，自动切换为平滑制动模式
通过sign函数实现离散判断，但整体输出保持连续

2.2 参数h的隐藏技巧

虽然理论上h是固定时间步长，但在STM32实践中我发现个窍门：当系统负载波动大时，可以动态调整h来补偿计算延迟。比如在电机控制中，我用定时器中断实测计算耗时，动态修正h值：

void TIM2_IRQHandler() { uint32_t start = DWT->CYCCNT; TD_Run(&td_ctrl, target_value); uint32_t cycles = DWT->CYCCNT - start; td_ctrl.Time.Dtime = cycles / SystemCoreClock; // 实际耗时 }

3. STM32上的工程实现细节

3.1 内存优化策略

在资源受限的STM32F103上，我吃过内存不足的亏。后来优化出的结构体方案，比原版节省了20%内存：

typedef struct { union { float Target; struct { float R1; // 跟踪值 float R2; // 跟踪速度 }; }; uint16_t time_flag:1; // 用位域节省空间 float R; uint32_t last_tick; } Compact_TD;

3.2 中断安全设计

在电机控制这种实时性要求高的场景，TD计算必须放在中断服务例程(ISR)中。但浮点运算在Cortex-M3上可能引发上下文切换，我的解决方案是：

启用FPU（如果硬件支持）
使用__attribute__((section(".ramfunc")))将关键函数放在RAM执行
预计算常用值，减少实时计算量

__attribute__((section(".ramfunc"))) void TD_ISR_Handler(TD_conctrol* td) { static float precomp_r_h = 0; if(td->td.R != precomp_r_h) { precomp_r_h = td->td.R * td->Time.Dtime; } // 使用预计算值加速 }

4. 参数调优实战指南

4.1 快速确定R的初始值

经过十多个项目的积累，我总结出这个经验公式：

R_initial = (2π × 目标带宽)^2 / 10

比如需要跟踪1kHz的信号：

float target_bandwidth = 1000.0f; // Hz float R = powf(2*3.1415926f*target_bandwidth, 2) / 10.0f;

4.2 调试中的避坑指南

振荡问题：表现为输出在目标值附近来回跳动
- 现象：R值过大导致
- 解决方案：每次将R减半，直到振荡消失
响应迟钝：输出像老爷子散步一样慢悠悠
- 现象：R值过小或h值不准确
- 解决方案：先用示波器确认实际h值，再按20%步进增大R
阶跃响应过冲：像刹车不及撞上前车
- 现象：系统惯性大时容易出现
- 解决方案：加入软化因子R_actual = R * (1 - k*|error|)

4.3 自动调参方案

对于需要批量生产的设备，我开发了这套自动调参流程：

施加5%幅值的阶跃信号
用二分法搜索临界振荡点R_critical
取最终R = 0.6 × R_critical
验证在不同幅值(20%,50%,80%)下的响应

void auto_tune(TD_conctrol* td) { float R_low = 1.0f, R_high = 1000.0f; while(fabsf(R_high - R_low) > 1.0f) { float R_mid = (R_low + R_high)/2; td->td.R = R_mid; if(check_overshoot(td) > 0.1f) { R_high = R_mid; } else { R_low = R_mid; } } td->td.R = 0.6f * R_high; }

5. 进阶应用：多级TD串联

在无人机姿态控制中，我采用三级TD串联结构：

第一级：粗跟踪（R=1000）处理原始传感器数据
第二级：速度估计（R=500）输出平滑速度
第三级：加速度估计（R=200）提取干净加速度

这种结构比单级TD延迟增加约15%，但噪声抑制效果提升3倍以上。关键点在于级间耦合处理：

void cascade_TD(TD_conctrol* td1, TD_conctrol* td2, TD_conctrol* td3) { TD_Run(td1, raw_input); TD_Run(td2, TD_get_R1(td1)); TD_Run(td3, TD_get_R1(td2)); float accel = TD_get_R2(td3); // 最终加速度 }

实际测试数据对比：

方案	延迟(ms)	噪声抑制比
直接差分	2.1	1.0x
单级TD	3.8	4.7x
三级TD串联	5.2	15.3x

6. 与PID的协同工作模式

在四轴飞行器项目中，我摸索出两种混合控制方案：

模式A（串行结构）：

传感器 → TD → PID → 电机

优点：PID无需修改
缺点：TD延迟直接影响响应速度

模式B（并行结构）：

传感器 → TD → 前馈补偿 ↘ PID → 电机

优点：动态响应快
缺点：需要调整PID参数

实测发现模式B在高速机动时效果更优。关键是要处理好前馈增益：

void control_update() { TD_Run(&td, target_angle); float feedforward = 0.6f * TD_get_R2(&td); // 前馈系数 float pid_out = PID_Calculate(&pid, TD_get_R1(&td)); motor_output = pid_out + feedforward; }

最后分享一个血泪教训：千万别在调试初期就把TD和PID一起调。正确的步骤应该是：