从 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 每周期都做两件事：

例如外部负载突然增加，电机实际加速度小于模型预测值，位置预测会逐渐偏离编码器反馈。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.1 h：真实控制周期

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

h = 0.001f;

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

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

w0 × h <= 0.1~0.2

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

9.2 b0：对象输入增益估计

二阶位置模型中：

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

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

b0 ≈ 500/1000 = 0.5

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

9.3 wc：控制器带宽

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

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

调节时间 ≈ 5/wc

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

9.4 w0：ESO 带宽

可以先从：

w0 ≈ 3 × wc

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

9.5 r：TD 跟踪快慢

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

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

9.6 out_max：输出能力边界

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

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

十、推荐调参顺序

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

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，而不是只会调用一个函数。

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