从 PID 到 ADRC：原理、公式推导、C 语言实现与电机调参指南

本文面向第一次接触 ADRC 的读者，从闭环控制和 PID 开始，逐步推导一阶、二阶 LADRC，解释 TD、ESO、误差反馈和扰动补偿，并给出适合 STM32 的 C 语言实现、参数整定顺序以及电机上机注意事项。

摘要

ADRC（Active Disturbance Rejection Control，自抗扰控制）的核心不是“换一种 PID”，而是把未知负载、摩擦、模型误差和外部扰动统一看成总扰动，利用扩张状态观测器（ESO）实时估计，再在控制输出中主动补偿。

如果只记住复杂的fhan()或几组参数，而没有理解对象阶数、b0、采样周期和 ESO 状态含义，那么 ADRC 很容易变成一段“能编译但不会调”的代码。本文希望解决的正是这个问题：不仅给出公式，还要说明公式怎样落到代码、参数错了会出现什么现象，以及怎样从零复现一个最小可用的 LADRC。

关键词：ADRC、LADRC、TD、ESO、STM32、电机控制、PID、扰动观测器

一、先从最基本的电机闭环说起

以电机位置控制为例，最基本的闭环是：

目标位置 set | v 控制器 PID/ADRC ---> 电流或转矩指令 u ---> 驱动器和电机 ---> 实际位置 y ^ | |_______________________________________________| 编码器反馈

控制器每隔固定时间执行一次：读取目标和反馈，计算输出，再把输出发送给电机驱动器。

假设目标位置是1000，编码器反馈是800，那么位置误差为：

e = set - feedback = 1000 - 800 = 200

PID 和 ADRC 都会利用这个误差产生控制输出，但二者处理未知负载的方式不同。

二、PID 如何处理负载，ADRC 又有什么不同

PID 的基本形式是：

u=Kpe+Ki∫edt+Kddedtu=Kp​e+Ki​∫edt+Kd​dtde​

• 比例项根据当前误差输出，误差越大，输出越大；
• 积分项累积历史误差，用于消除恒定负载造成的静差；
• 微分项观察误差变化趋势，用于增加阻尼、抑制超调。

当负载突然增加时，PID 的典型过程是：

负载增加 -> 电机偏离目标 -> 误差增大 -> PID增加输出

也就是说，PID 通常要等误差出现后，再通过比例和积分进行补救。

ADRC 的思路更直接：除了纠正跟踪误差，还建立一个观测器，估计“系统中没有被模型解释的那部分作用”，再主动补偿它。

目标值 | v TD 跟踪微分器 | v 误差反馈控制 ------+ | ESO 状态观测器 --->+-- 扰动补偿 --> 输出 u ^ | 电机反馈

因此可以先记住一句话：

ADRC = 目标轨迹安排 + 状态/扰动观测 + 误差反馈 + 总扰动补偿。

不过，ADRC 不是“自动调参控制器”。b0、控制周期、控制器带宽和观测器带宽设置错误时，它同样会振荡、饱和甚至反向失控。

三、先确定控制对象是一阶还是二阶

这是使用 ADRC 前最容易被忽略的一步。

3.1 一阶速度模型

如果控制输入是电流/转矩指令，被控量是电机速度，可以把低频动态近似为：

y˙=f+b0uy˙​=f+b0​u

其中：

• yy：电机速度；
• uu：电流或转矩指令；
• b0b0​：控制输入对速度变化率的作用强度估计；
• ff：负载、摩擦、参数误差等总扰动。

此时可使用二阶 ESO：

z1 ≈ 速度 y z2 ≈ 总扰动 f

3.2 二阶位置模型

如果被控量是位置，则常写成：

y¨=f+b0uy¨​=f+b0​u

令：

x1=y,x2=y˙,x3=fx1​=y,x2​=y˙​,x3​=f

得到：

x˙1=x2x˙2=x3+b0ux˙3≈0x˙1​x˙2​x˙3​​=x2​=x3​+b0​u≈0​

此时使用三阶 ESO：

z1 ≈ 位置 z2 ≈ 速度 z3 ≈ 总扰动

这两个结构不能仅靠修改变量名互换。速度环中z2可以是总扰动，而二阶位置环中z2是速度、z3才是总扰动。阅读任何 ADRC 代码时，都应先从 ESO 方程判断它实现的是哪一种对象模型。

四、TD：把突变目标变成可跟踪轨迹

目标位置从0突然跳到1000时，直接送入控制器可能产生很大的瞬时输出。TD（Tracking Differentiator，跟踪微分器）用于生成平滑目标及其变化率：

原始目标：0 ---------------- 1000 TD位置v1：0 -> 1 -> 3 -> 6 -> ... -> 1000 TD速度v2：0 -> 增大 -> 保持 -> 减小 -> 0

TD 状态更新可以理解为基础运动学：

v1 = v1 + h * v2; v2 = v2 + h * fh;

其中：

• v1是平滑后的目标位置；
• v2是目标速度；
• fh是 TD 计算出的目标加速度；
• h是真实控制周期；
• r控制 TD 允许的加减速强度。

项目中的离散fhan()核心代码如下：

static float adrc_fhan(float position_error, float velocity, float r, float h) { float d = r * h; float d0 = h * d; float y = position_error + h * velocity; float a1 = sqrtf(d * d + 8.0f * r * fabsf(y)); float a; if (fabsf(y) <= d0) { a = velocity + y / h; } else if (y > 0.0f) { a = velocity + (a1 - d) / 2.0f; } else { a = velocity - (a1 - d) / 2.0f; } if (a > d) a = d; if (a < -d) a = -d; return -r * a / d; }

它的直观目标是：离目标较远时加速追赶，接近目标时根据当前位置和当前速度提前制动。

需要注意：h和r必须大于零，否则代码可能出现除零或无效平方根。TD 不是 ADRC 唯一可用的目标规划方式，工程中也可以先用斜坡、梯形速度规划或 S 曲线替代，以降低调试复杂度。

五、ESO：ADRC 的核心

以二阶位置模型为例：

y¨=f+b0uy¨​=f+b0​u

线性扩张状态观测器可以写成：

e=z1−yz˙1=z2−β1ez˙2=z3−β2e+b0uz˙3=−β3eez˙1​z˙2​z˙3​​=z1​−y=z2​−β1​e=z3​−β2​e+b0​u=−β3​e​

使用显式欧拉离散后：

e = z1 - feedback; z1_next = z1 + h * (z2 - beta1 * e); z2_next = z2 + h * (z3 - beta2 * e + b0 * u_last); z3_next = z3 + h * (-beta3 * e);

这里的u_last是上一周期实际发送的、经过限幅的控制指令。当前反馈是上一周期输出作用后的结果，因此观测器应使用上一周期输出解释当前测量。

5.1 ESO 为什么能估计没有传感器测量的速度和扰动

ESO 每周期都做两件事：

1. 根据模型和上一周期输出预测状态；
2. 用位置估计误差z1-y同时修正z1、z2、z3。

例如外部负载突然增加，电机实际加速度小于模型预测值，位置预测会逐渐偏离编码器反馈。ESO 将这部分无法由b0*u解释的差异推入z3，最终把它看作总扰动。

5.2 带宽参数化

令 ESO 三个极点都位于 −ω0−ω0​，有：

(s+ω0)3=s3+3ω0s2+3ω02s+ω03(s+ω0​)3=s3+3ω0​s2+3ω02​s+ω03​

因此：

beta1 = 3.0f * w0; beta2 = 3.0f * w0 * w0; beta3 = w0 * w0 * w0;

w0越大，观测越快，但编码器量化噪声、通信抖动和机械振动也越容易进入z2、z3。ESO 带宽不是越大越好。

六、误差反馈与扰动补偿

二阶位置 LADRC 使用：

e1=v1−z1,e2=v2−z2e1​=v1​−z1​,e2​=v2​−z2​

基础控制作用为：

u0=Kpe1+Kde2u0​=Kp​e1​+Kd​e2​

选择控制器带宽 ωcωc​：

Kp=ωc2,Kd=2ωcKp​=ωc2​,Kd​=2ωc​

对应期望闭环特征多项式：

(s+ωc)2=s2+2ωcs+ωc2(s+ωc​)2=s2+2ωc​s+ωc2​

6.1 标准扰动补偿公式如何得到

对象模型是：

y¨=f+b0uy¨​=f+b0​u

希望补偿后：

y¨≈u0y¨​≈u0​

ESO 给出 z3≈fz3​≈f，于是：

u0=z3+b0uu0​=z3​+b0​u

移项得到：

u=u0−z3b0u=b0​u0​−z3​​​

对应代码：

float u0 = kp * e1 + kd * e2; float u = (u0 - z3) / b0;

6.2 一个容易被忽略的括号错误

下面两种写法并不等价：

u = (u0 - z3) / b0; /* 标准形式 */ u = u0 - z3 / b0; /* 只让扰动项除以b0 */

假设：

u0 = 40 z3 = -6 b0 = 0.5

标准形式得到：

u = (40-(-6))/0.5 = 92

代回对象：

y'' = -6 + 0.5×92 = 40

而第二种写法得到：

u = 40-(-6/0.5) = 52 y'' = -6 + 0.5×52 = 20

这会改变控制器实际带宽。审查项目源码时，应特别检查括号和b0的定义，而不是仅凭变量名判断实现正确。

七、从零实现一个最小二阶 LADRC

为了先验证 ESO 和扰动补偿，下面的最小实现暂时不包含 TD 和循环角度处理。目标位置与目标速度由外部直接提供。

7.1 数据结构

#ifndef LADRC2_H #define LADRC2_H #include <stdbool.h> typedef struct { float h; float b0; float out_max; float beta1; float beta2; float beta3; float kp; float kd; float z1; float z2; float z3; float out; } ladrc2_t; bool LADRC2_Init(ladrc2_t *ctrl, float h, float b0, float w0, float wc, float out_max, float initial_feedback); float LADRC2_Update(ladrc2_t *ctrl, float feedback, float target_position, float target_velocity); #endif

7.2 初始化

#include "ladrc2.h" #include <math.h> #include <stddef.h> #define LADRC_B0_MIN 1e-4f static float ladrc_clamp(float value, float limit) { if (value > limit) return limit; if (value < -limit) return -limit; return value; } bool LADRC2_Init(ladrc2_t *ctrl, float h, float b0, float w0, float wc, float out_max, float initial_feedback) { if (ctrl == NULL || h <= 0.0f || w0 <= 0.0f || wc <= 0.0f || out_max <= 0.0f || fabsf(b0) < LADRC_B0_MIN) { return false; } ctrl->h = h; ctrl->b0 = b0; ctrl->out_max = out_max; ctrl->beta1 = 3.0f * w0; ctrl->beta2 = 3.0f * w0 * w0; ctrl->beta3 = w0 * w0 * w0; ctrl->kp = wc * wc; ctrl->kd = 2.0f * wc; /* 上电时对齐当前位置，避免非零位置产生巨大观测误差 */ ctrl->z1 = initial_feedback; ctrl->z2 = 0.0f; ctrl->z3 = 0.0f; ctrl->out = 0.0f; return true; }

7.3 周期计算

float LADRC2_Update(ladrc2_t *ctrl, float feedback, float target_position, float target_velocity) { float e; float z1_next; float z2_next; float z3_next; float e1; float e2; float u0; float u; if (ctrl == NULL) return 0.0f; /* ESO：使用当前反馈和上一周期限幅后的实际指令 */ e = ctrl->z1 - feedback; z1_next = ctrl->z1 + ctrl->h * (ctrl->z2 - ctrl->beta1 * e); z2_next = ctrl->z2 + ctrl->h * (ctrl->z3 - ctrl->beta2 * e + ctrl->b0 * ctrl->out); z3_next = ctrl->z3 + ctrl->h * (-ctrl->beta3 * e); ctrl->z1 = z1_next; ctrl->z2 = z2_next; ctrl->z3 = z3_next; /* 位置和速度误差反馈 */ e1 = target_position - ctrl->z1; e2 = target_velocity - ctrl->z2; u0 = ctrl->kp * e1 + ctrl->kd * e2; /* 总扰动补偿 */ u = (u0 - ctrl->z3) / ctrl->b0; /* 保存真实发送值，下一周期ESO继续使用 */ ctrl->out = ladrc_clamp(u, ctrl->out_max); return ctrl->out; }

7.4 调用示例

static ladrc2_t motor_position_adrc; void MotorControl_Init(float current_position) { bool ok = LADRC2_Init(&motor_position_adrc, 0.001f, /* 真实控制周期：1 ms */ 0.5f, /* b0：需要实验辨识 */ 15.0f, /* ESO带宽 */ 5.0f, /* 控制器带宽 */ 500.0f, /* 调试阶段保守限幅 */ current_position); if (!ok) { /* 参数无效：禁止使能电机 */ } } void MotorControl_1ms(float position_feedback, float position_target) { float output = LADRC2_Update(&motor_position_adrc, position_feedback, position_target, 0.0f); Motor_SendCurrentCommand(output); }

这段代码是学习和仿真的最小核心，不等于可直接上真实机器人。实际工程还需要角度展开、故障状态机、通信超时、软硬限位、急停和电流保护。

八、完整 ADRC 周期的执行顺序

加入 TD 和循环角度处理后，一个周期通常按以下顺序运行：

1. 保存目标和反馈 2. 对循环角度做最短路径展开 3. TD更新v1、v2 4. ESO使用反馈和上一周期输出更新z1、z2、z3 5. 计算e1、e2和u0 6. 使用z3进行扰动补偿 7. 对最终输出限幅 8. 保存实际输出，供下一周期ESO使用

如果编码器范围为0~8191，反馈为8190、目标为10，直接相减得到-8180，但最短路径其实是正向跨零12个计数。因此循环量需要展开：

float error = target - feedback; if (error >= 4096.0f) error -= 8192.0f; else if (error < -4096.0f) error += 8192.0f;

速度不是循环量，速度环不应使用这种角度跨零逻辑。

九、六个关键参数如何理解

9.1h：真实控制周期

h不是用来改善响应的调参旋钮，而是算法时间基准。任务每1 ms调用一次，就应设置：

h = 0.001f;

实际周期是1 ms，代码却填写0.01 s，TD 和 ESO 每周期积分量会放大十倍。上机前必须测量实际周期和抖动。

显式欧拉实现可以先保守检查：

w0 × h <= 0.1~0.2

这不是通用稳定性证明，但适合作为初始筛查。

9.2b0：对象输入增益估计

二阶位置模型中：

b0≈Δ加速度Δ控制指令b0​≈Δ控制指令Δ加速度​

例如控制指令增加1000，测得加速度增加约500 count/s²：

b0 ≈ 500/1000 = 0.5

首先要确认符号。正向指令应产生正向加速度，否则反馈方向、输出方向或b0符号至少有一项错误。方向错误时必须立即停机，不能通过增大带宽“调回来”。

9.3wc：控制器带宽

wc增大 -> Kp、Kd增大 -> 响应更快 -> 输出需求和延迟敏感性增加

二阶临界阻尼响应的调节时间可以粗略估计为：

调节时间 ≈ 5/wc

希望约1 s稳定时，可以从wc≈5 rad/s的量级开始，再根据执行器能力调整。

9.4w0：ESO 带宽

可以先从：

w0 ≈ 3 × wc

开始。w0太小，z1、z2和扰动估计明显滞后；w0太大，z2、z3会放大编码器噪声，输出可能高频抖动。

9.5r：TD 跟踪快慢

在本文使用的fhan()实现中，fh被限制在[-r,r]，因此r可以近似理解为目标轨迹允许的最大加速度。

r太小 -> 目标轨迹本身很慢 r太大 -> 接近直接阶跃，输出容易饱和和冲击

9.6out_max：输出能力边界

调试初期必须保守设置。即使驱动器允许±16384，也不代表第一次上电就应该开放到最大范围。

当输出长期饱和时，理论关系 y¨≈u0y¨​≈u0​ 不再成立。此时继续增大wc通常只会让饱和更严重。

十、推荐调参顺序

不要同时修改所有参数。可靠的顺序是：

1. 明确控制位置还是速度，选择正确对象阶数；
2. 统一位置、速度、时间和输出的单位；
3. 测量真实控制周期h和周期抖动；
4. 设置保守out_max，准备急停和机械限位；
5. 用小输出确认控制方向和反馈方向；
6. 粗略辨识b0的符号与量级；
7. 使能前把z1、TD 初值与当前反馈对齐；
8. 使用较小wc建立稳定闭环；
9. 令w0从约3wc开始；
10. 调节r，使目标轨迹符合机构允许的加速度；
11. 每次按约10%~20%提高wc；
12. 通过负载阶跃观察恢复速度和z3噪声，再调整w0；
13. 最后才逐步扩大输出限幅。

10.1 现象与参数定位

十一、理想模型仿真：ESO 能否真的估计扰动

使用虚拟二阶电机：

x˙1=x2,x˙2=f+btrueux˙1​=x2​,x˙2​=f+btrue​u

取：

h = 0.001 s b_true = 0.5 b0 = 0.5 wc = 5 w0 = 15 out_max= 500

在0.2 s将位置目标从0改为1，在2.0 s突然加入恒定扰动f=-20，得到部分结果：

最终：

z1 ≈ 真实位置 z2 ≈ 真实速度 z3 ≈ -20 u ≈ 40

因为：

0 = -20 + 0.5×u u = 40

这说明理想条件下 ESO 能估计并补偿恒定扰动。但仿真没有包含编码器量化、通信延迟、采样抖动、机械间隙、驱动器限流和参数变化，因此不能把“理想模型收敛”直接等同于“实机已经可用”。

十二、PID 和 ADRC 应该怎样选择

ADRC 并不意味着 PID 过时。

PID 的优势是结构简单、计算量低、工程经验丰富、故障定位直观。对象变化不大、传感器噪声明显、控制要求不高时，一个带限幅和抗积分饱和的 PID 往往更容易落地。

ADRC 更适合以下场景：

• 负载变化明显；
• 精确模型难以获得；
• 希望显式观察总扰动；
• 执行器和采样频率能够支持较高带宽；
• 有能力记录z1/z2/z3/out并进行系统调试。

二者的差异可以概括为：

PID积分补偿：依靠误差长期积累，间接形成负载补偿 ADRC扰动补偿：利用ESO估计总扰动，直接从输出中抵消

实际项目中最稳妥的方法不是一开始删除 PID，而是保留同一套目标、反馈、限幅和日志接口，让 PID 与 ADRC 在相同条件下进行阶跃、负载突变和噪声测试。

十三、上真实电机前的安全清单

在 STM32 上调用 ADRC 前，至少确认：

• 控制任务周期与h一致，并测量最坏周期抖动；
• 每个电机使用独立控制器实例；
• 正输出、正反馈和b0符号定义一致；
• 使能前将 ESO 和目标轨迹状态对齐到当前反馈；
• 输出限幅远低于硬件危险区间，并逐级放开；
• 有机械软限位、驱动器故障处理、通信超时和急停；
• 记录set/fdb/v1/v2/z1/z2/z3/out；
• 驱动器实际限流值与软件out_max的含义一致；
• 先空载、低输出、短时间测试，再逐步增加负载；
• 发现反向加速、持续饱和或高频振荡时立即禁止输出。

对于真实机器人关节，还应增加状态机：

INIT -> READY -> ENABLED -> FAULT / E_STOP

控制算法不应绕过安全状态机直接向电机发送指令。

十四、常见误区

误区 1：w0越大，抗扰越强

更大的w0确实能提高观测速度，但也会放大噪声。高频抖动的z3会通过扰动补偿直接进入输出。

误区 2：有 ESO 就不需要确认b0

ESO 可以把模型误差并入总扰动，但b0符号错误可能直接破坏负反馈，量级严重错误也会造成输出过大或响应过慢。

误区 3：位置 ADRC 可以直接拿去控制速度

必须重新判断对象阶数。三阶 ESO 的z2是速度、z3是扰动；一阶速度对象通常使用二阶 ESO。

误区 4：仿真稳定就能直接上机

仿真常常忽略延迟、抖动、噪声、死区、摩擦、限流和结构柔性。实机必须从低输出开始，并保留 PID 基线和急停。

误区 5：参数不合适就同时修改所有参数

同时修改b0、wc、w0、r会失去因果判断。一次只改一个参数，并保存每次实验波形。

总结

学习 ADRC 最容易走偏的地方，是把注意力全部放在复杂的fhan()和参数表上。真正决定能否独立实现和调参的，是下面这条逻辑链：

先确定对象阶数 -> 写出 y'=f+b0u 或 y''=f+b0u -> 根据阶数设计 ESO -> 用带宽法得到 beta 和 Kp/Kd -> 推导 u=(u0-z)/b0 -> 离散化并使用上一周期实际输出 -> 限幅、记录、仿真 -> 最后才进入低输出实机测试

ADRC 的价值不在于让模型彻底消失，而在于把无法准确建模的部分统一为总扰动，并通过观测与补偿降低控制器对精确模型的依赖。

当你能够不看源码写出 ESO 更新方程，能从 y′′=f+b0uy′′=f+b0​u 推导控制律，能根据z1/z2/z3/out波形定位h、b0、wc、w0、r的问题时，才算真正掌握了 ADRC，而不是只会调用一个函数。

下一篇文章讲点什么呢，不太清楚，每天被小登就要气死了