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

本文面向第一次接触 ADRC 的读者从闭环控制和 PID 开始逐步推导一阶、二阶 LADRC解释 TD、ESO、误差反馈和扰动补偿并给出适合 STM32 的 C 语言实现、参数整定顺序以及电机上机注意事项。摘要ADRCActive 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 200PID 和 ADRC 都会利用这个误差产生控制输出但二者处理未知负载的方式不同。二、PID 如何处理负载ADRC 又有什么不同PID 的基本形式是uKpeKi∫edtKddedtuKp​eKi​∫edtKd​dtde​比例项根据当前误差输出误差越大输出越大积分项累积历史误差用于消除恒定负载造成的静差微分项观察误差变化趋势用于增加阻尼、抑制超调。当负载突然增加时PID 的典型过程是负载增加 - 电机偏离目标 - 误差增大 - PID增加输出也就是说PID 通常要等误差出现后再通过比例和积分进行补救。ADRC 的思路更直接除了纠正跟踪误差还建立一个观测器估计“系统中没有被模型解释的那部分作用”再主动补偿它。目标值 | v TD 跟踪微分器 | v 误差反馈控制 ------ | ESO 状态观测器 ----- 扰动补偿 -- 输出 u ^ | 电机反馈因此可以先记住一句话ADRC 目标轨迹安排 状态/扰动观测 误差反馈 总扰动补偿。不过ADRC 不是“自动调参控制器”。b0、控制周期、控制器带宽和观测器带宽设置错误时它同样会振荡、饱和甚至反向失控。三、先确定控制对象是一阶还是二阶这是使用 ADRC 前最容易被忽略的一步。3.1 一阶速度模型如果控制输入是电流/转矩指令被控量是电机速度可以把低频动态近似为y˙fb0uy˙​fb0​u其中yy电机速度uu电流或转矩指令b0b0​控制输入对速度变化率的作用强度估计ff负载、摩擦、参数误差等总扰动。此时可使用二阶 ESOz1 ≈ 速度 y z2 ≈ 总扰动 f3.2 二阶位置模型如果被控量是位置则常写成y¨fb0uy¨​fb0​u令x1y,x2y˙,x3fx1​y,x2​y˙​,x3​f得到x˙1x2x˙2x3b0ux˙3≈0x˙1​x˙2​x˙3​​x2​x3​b0​u≈0​此时使用三阶 ESOz1 ≈ 位置 z2 ≈ 速度 z3 ≈ 总扰动这两个结构不能仅靠修改变量名互换。速度环中z2可以是总扰动而二阶位置环中z2是速度、z3才是总扰动。阅读任何 ADRC 代码时都应先从 ESO 方程判断它实现的是哪一种对象模型。四、TD把突变目标变成可跟踪轨迹目标位置从0突然跳到1000时直接送入控制器可能产生很大的瞬时输出。TDTracking Differentiator跟踪微分器用于生成平滑目标及其变化率原始目标0 ---------------- 1000 TD位置v10 - 1 - 3 - 6 - ... - 1000 TD速度v20 - 增大 - 保持 - 减小 - 0TD 状态更新可以理解为基础运动学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 曲线替代以降低调试复杂度。五、ESOADRC 的核心以二阶位置模型为例y¨fb0uy¨​fb0​u线性扩张状态观测器可以写成ez1−yz˙1z2−β1ez˙2z3−β2eb0uz˙3−β3eez˙1​z˙2​z˙3​​z1​−yz2​−β1​ez3​−β2​eb0​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 每周期都做两件事根据模型和上一周期输出预测状态用位置估计误差z1-y同时修正z1、z2、z3。例如外部负载突然增加电机实际加速度小于模型预测值位置预测会逐渐偏离编码器反馈。ESO 将这部分无法由b0*u解释的差异推入z3最终把它看作总扰动。5.2 带宽参数化令 ESO 三个极点都位于 −ω0−ω0​有(sω0)3s33ω0s23ω02sω03(sω0​)3s33ω0​s23ω02​sω03​因此beta1 3.0f * w0; beta2 3.0f * w0 * w0; beta3 w0 * w0 * w0;w0越大观测越快但编码器量化噪声、通信抖动和机械振动也越容易进入z2、z3。ESO 带宽不是越大越好。六、误差反馈与扰动补偿二阶位置 LADRC 使用e1v1−z1,e2v2−z2e1​v1​−z1​,e2​v2​−z2​基础控制作用为u0Kpe1Kde2u0​Kp​e1​Kd​e2​选择控制器带宽 ωcωc​Kpωc2,Kd2ωcKp​ωc2​,Kd​2ωc​对应期望闭环特征多项式(sωc)2s22ωcsωc2(sωc​)2s22ωc​sωc2​6.1 标准扰动补偿公式如何得到对象模型是y¨fb0uy¨​fb0​u希望补偿后y¨≈u0y¨​≈u0​ESO 给出 z3≈fz3​≈f于是u0z3b0uu0​z3​b0​u移项得到uu0−z3b0ub0​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); #endif7.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 sTD 和 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.4w0ESO 带宽可以先从w0 ≈ 3 × wc开始。w0太小z1、z2和扰动估计明显滞后w0太大z2、z3会放大编码器噪声输出可能高频抖动。9.5rTD 跟踪快慢在本文使用的fhan()实现中fh被限制在[-r,r]因此r可以近似理解为目标轨迹允许的最大加速度。r太小 - 目标轨迹本身很慢 r太大 - 接近直接阶跃输出容易饱和和冲击9.6out_max输出能力边界调试初期必须保守设置。即使驱动器允许±16384也不代表第一次上电就应该开放到最大范围。当输出长期饱和时理论关系 y¨≈u0y¨​≈u0​ 不再成立。此时继续增大wc通常只会让饱和更严重。十、推荐调参顺序不要同时修改所有参数。可靠的顺序是明确控制位置还是速度选择正确对象阶数统一位置、速度、时间和输出的单位测量真实控制周期h和周期抖动设置保守out_max准备急停和机械限位用小输出确认控制方向和反馈方向粗略辨识b0的符号与量级使能前把z1、TD 初值与当前反馈对齐使用较小wc建立稳定闭环令w0从约3wc开始调节r使目标轨迹符合机构允许的加速度每次按约10%~20%提高wc通过负载阶跃观察恢复速度和z3噪声再调整w0最后才逐步扩大输出限幅。10.1 现象与参数定位现象优先检查一使能就反向加速b0符号、反馈方向、输出方向上电瞬间打满输出ESO/TD 初始状态未对齐、h或b0错误整体响应慢且没有饱和wc可能太小td_v1本身跟目标很慢r可能太小目标阶跃时长期饱和r/wc太大、限幅过小或b0不准z1明显落后编码器反馈w0可能太小z2/z3高频抖动w0太大、编码器量化或机械振动明显加载后恢复慢w0太小、b0不准或输出能力不足电机高频鸣叫优先降低w0再检查wc和反馈噪声低频来回摆动wc过大、系统延迟或机械柔性十一、理想模型仿真ESO 能否真的估计扰动使用虚拟二阶电机x˙1x2,x˙2fbtrueux˙1​x2​,x˙2​fbtrue​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得到部分结果时间真实位置真实速度z1z2z3输出0.500 s0.44001.67220.44171.6698-0.035-5.4121.999 s0.99870.00550.99880.00550.000-0.0482.100 s0.9084-1.64190.9292-0.5349-3.86721.9712.500 s0.61190.58160.61390.6341-19.60445.8284.000 s0.99860.00620.99860.0062-20.00039.947最终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控制算法不应绕过安全状态机直接向电机发送指令。十四、常见误区误区 1w0越大抗扰越强更大的w0确实能提高观测速度但也会放大噪声。高频抖动的z3会通过扰动补偿直接进入输出。误区 2有 ESO 就不需要确认b0ESO 可以把模型误差并入总扰动但b0符号错误可能直接破坏负反馈量级严重错误也会造成输出过大或响应过慢。误区 3位置 ADRC 可以直接拿去控制速度必须重新判断对象阶数。三阶 ESO 的z2是速度、z3是扰动一阶速度对象通常使用二阶 ESO。误区 4仿真稳定就能直接上机仿真常常忽略延迟、抖动、噪声、死区、摩擦、限流和结构柔性。实机必须从低输出开始并保留 PID 基线和急停。误区 5参数不合适就同时修改所有参数同时修改b0、wc、w0、r会失去因果判断。一次只改一个参数并保存每次实验波形。总结学习 ADRC 最容易走偏的地方是把注意力全部放在复杂的fhan()和参数表上。真正决定能否独立实现和调参的是下面这条逻辑链先确定对象阶数 - 写出 yfb0u 或 yfb0u - 根据阶数设计 ESO - 用带宽法得到 beta 和 Kp/Kd - 推导 u(u0-z)/b0 - 离散化并使用上一周期实际输出 - 限幅、记录、仿真 - 最后才进入低输出实机测试ADRC 的价值不在于让模型彻底消失而在于把无法准确建模的部分统一为总扰动并通过观测与补偿降低控制器对精确模型的依赖。当你能够不看源码写出 ESO 更新方程能从 y′′fb0uy′′fb0​u 推导控制律能根据z1/z2/z3/out波形定位h、b0、wc、w0、r的问题时才算真正掌握了 ADRC而不是只会调用一个函数。下一篇文章讲点什么呢不太清楚每天被小登就要气死了