机器人关节伺服PID怎么自整定：PSO粒子群20粒子100代七步落地（STM32）

机器人关节的伺服调试，难点从来不在 PID 公式，而在于参数空间同时受超调、力矩饱和、齿隙与摩擦约束，靠手感试凑既慢又不可复现。结论先给三条：第一，用 Ziegler-Nichols（ZN）法算出的一组参数做"中心"，把搜索区间收缩到中心值的 ±90%（α=0.9），再用粒子群（PSO）寻优，既保留 ZN 的合理内核，又把搜索空间压到可控范围；第二，同一被控对象上，ZN 的阶跃超调为 51.84%，用 ITAE 适应度寻优后可降到 2.31%，5% 调节时间从 2.34s 缩到 0.83s；第三，PSO 是离线工具——20 粒子、100 代、每次 2000 步仿真，跑在电脑或上位机上，STM32 只负责固化结果与在线增益调度，不要把上万次迭代塞进 20kHz 的电流环中断。

一、关节 PID 难在哪：三个不像教科书的地方

教科书里的二阶对象是定常的，真实关节不是。第一，惯量随姿态变化：六轴机器人手臂前伸与回收时，折算到电机轴的等效惯量可能相差数倍，同一组 K_p 在回收位姿下刚好，前伸位姿就发飘。第二，非线性摩擦与齿隙：谐波减速器的启动力矩比运行力矩高，低速爬行时积分项容易积起来，一过零点就"窜"。第三，力矩饱和：关节输出力矩有硬上限，误差大时积分仍在累加，退饱和瞬间必然超调。

这三件事决定了：整定不是"算一组最优参数"，而是"在一个受限空间里找一组在多个工况下都不坏的参数"。这也是为什么手工试凑如此常见——它其实是人在做多目标折中。问题在于，人的折中不可复现，换个人、换台样机，结果就不同。

整定方法	典型耗时	依赖条件	超调表现	可复现性	适用场景
手工试凑	2~3 天	调试者经验、示波器与阶跃工装	看手感，常在 10%~30%	差，换人即变	单台样机、非标设备
ZN 临界比例度法	约 10 分钟	能测出临界增益 Ku 与振荡周期 Tu	偏大，论文对象 1 达 51.84%	好	快速拿到"能跑"的初值
继电器反馈自整定	5~15 分钟	控制器需支持继电器实验	中等，仍按单一指标	好	温控、流量等慢过程
PSO 离线寻优	20~40 分钟	有可仿真的对象模型或录波数据	可按适应度压到 2%~5%	好，随机种子固定即可复现	批量关节、需固化为参数表

这张表给出的结论很直接：ZN 不是用来交付的，是用来"定心"的；PSO 也不是万能的，它必须有一个像样的初值区间，否则 20 个粒子在三维空间里撒开，100 代根本收不回来。

二、PSO 只有五个旋钮

粒子群的做法是：每个粒子代表一组 (K_p, K_i, K_d)，它记住自己走过的最好位置 pBest，同时跟着全群最好位置 gBest 走。第 i 个粒子第 d 维的速度与位置按下面两式更新：

V(id,n+1) = W * V(id,n)
          + c1 * rand() * ( P(id,n) - X(id,n) )   // 认知项：自己的历史最优
          + c2 * rand() * ( P(gd,n) - X(id,n) )   // 社会项：全群历史最优

X(id,n+1) = X(id,n) + V(id,n+1)

式中的 W 是惯性权重，c1、c2 是加速因子，rand() 是 [0,1] 上的随机数；每维速度还要被 V_max 限幅，防止粒子一步飞出搜索区间。真正需要调的只有五个量，论文给出的取值与关节场景的建议如下：

参数	论文取值	关节场景建议	调大后的影响
粒子数 m	20	20~30（三维问题足够）	更稳但单代耗时线性增加
惯性权重 W	1.2 线性递减到 0.1	0.9 递减到 0.4，前期全局、后期精修	前期探索强，后期收敛慢
加速因子 c1 / c2	2 / 2	c1=1.6、c2=2.0（更信群体）	c2 过大易早熟收敛到局部
最大速度 Vmax	取该维搜索区间宽度	区间宽度的 20%~30%	过大跳过最优，过小爬行
迭代次数	100	60~120，配合"贴边重开"	收益递减，超过 150 代基本无改善

V_max 取"区间宽度"这一条值得注意：粒子最快一步可横穿整个搜索空间。在区间已被 ZN 收缩到 ±90% 的前提下，这既保证探索能力，又不会让粒子乱撞。

三、适应度函数决定你得到什么

PSO 只会"往你给它的指标更小的方向走"。指标选错，参数一定偏。论文用了两个经典积分指标：

IAE  = ∫ |e(t)| dt          // 绝对误差积分：偏向"平稳"
ITAE = ∫ t * |e(t)| dt      // 时间加权绝对误差积分：重罚"拖尾"

两者的差别在关节上非常明显。IAE 对早期大误差和后期小误差一视同仁，寻优结果往往"起步柔和、尾巴拖长"；ITAE 用时间加权，越晚的误差罚得越重，因此会把参数往"快速收敛"方向推。点位运动（PTP）最怕的是到位慢，所以关节位置环建议用 ITAE；但如果关节要跟连续轨迹（如涂胶、打磨），过快的响应会放大齿隙与柔性振动，此时用"ITAE + 超调惩罚 + 力矩变化率惩罚"的加权形式更稳。

适应度函数	优化倾向	论文对象 1 超调	论文对象 1 上升时间	关节适用性
IAE	误差面积最小，响应偏柔	4.66%	0.53s	轨迹跟踪、需抑制振动
ITAE	重罚拖尾，收敛快	2.31%	0.62s	点位运动、节拍优先
ITAE + 超调/力矩惩罚	多目标折中	可按权重压到 1% 级	略慢于纯 ITAE	大惯量、长臂展关节

注意表里那个反直觉的结果：ITAE 的超调比 IAE 更小，但上升时间更长。原因是 ITAE 不允许"先冲过头再慢慢回来"，它逼着系统用更平缓的上升换取更短的调节时间。这也说明"超调小"和"上升快"在 PID 里天然是矛盾的，选指标本质上是在选你更在意哪一个。

四、用 ZN 定心，把搜索空间缩到 ±90%

论文最关键的一步不是算法，而是搜索空间的构造：以 ZN 法得到的 K′_p、K′_i、K′_d 为中心，向两侧各扩展 α 倍，α 取 [0,1] 内的数：

(1 - α) * K'p ≤ Kp ≤ (1 + α) * K'p
(1 - α) * K'i ≤ Ki ≤ (1 + α) * K'i
(1 - α) * K'd ≤ Kd ≤ (1 + α) * K'd      // 论文取 α = 0.9

以论文对象 1 的 ZN 结果 K_p=4.605、K_i=5.904、K_d=0.847 为例，α=0.9 时三个参数的区间分别变成 [0.46, 8.75]、[0.59, 11.22]、[0.085, 1.61]。这个区间比"凭经验给个 0~100"小了两个数量级，20 个粒子撒进去，覆盖率足够，收敛也快。

工程上还要加一条判据：寻优结果贴在区间边界上，说明真实最优在区间外。此时不要直接采用边界值，而应以该边界解为新中心再开一轮。这个"贴边重开"在关节上很常见——ZN 初值按"1/4 衰减"整定，对大惯量关节往往偏保守。

五、论文实测：超调 51.84% 是怎么降到 2.31% 的

论文用两个对象做了 20 次随机仿真，参数为：粒子数 20、W 从 1.2 递减到 0.1、c1=c2=2、V_max 取区间宽度、迭代 100 代、α=0.9。两个对象分别是：

对象 1：G(s) = 1 / [ (1 + s) * (1 + s/6)^3 ]
对象 2：G(s) = 0.33 / (1 + 18.5s + 134s^2) * e^(-18.25s)

对象 1 是一个带三个高频极点的一阶惯性串联环节，对象 2 则带 18.25s 纯延迟——后者对应工业里常见的"大滞后"对象，在关节上可以类比为"指令下发到力矩建立"的通信与控制链延迟。寻优得到的参数见表：

对象	参数组	Kp	Ki	Kd
对象 1	ZN	4.605	5.904	0.847
	PSO + IAE	5.973	1.967	1.694
	PSO + ITAE	4.692	1.811	1.317
对象 2	ZN	2.901	0.08	26.43
	PSO + IAE	2.383	0.086	26.656
	PSO + ITAE	1.962	0.079	18.379

参数本身看不出好坏，性能指标才有说服力：

对象	指标	ZN	PSO + IAE	PSO + ITAE
对象 1	超调量 σ%	51.84	4.66	2.31
	上升时间 tr/s	0.46	0.53	0.62
	调节时间 ts/s（5%）	2.34	1.58	0.83
对象 2	超调量 σ%	11.24	6.21	2.65
	上升时间 tr/s	22.7	26.2	32.1
	调节时间 ts/s（5%）	103.2	64.4	55.1

三点解读。其一，ZN 的上升时间几乎总是最快，代价是超调失控——51.84% 在关节上意味着"到位冲过头半圈"，机械上不可接受。其二，ITAE 把超调压到 2% 级，同时把调节时间缩短一半以上，正是点位运动要的组合。其三，对象 2 延迟 18.25s，三种方法都做不短上升时间，但 ITAE 仍把调节时间从 103.2s 压到 55.1s——PSO 的收益不只在"超调"，更在"拖尾"。

六、柔性关节的"负调"：两阶段 PI/PD

带谐波减速器与力矩传感器的柔性关节，力矩到位置之间存在一个右半平面零点，阶跃响应会先朝反方向"凹"一下，这就是非最小相位系统的负调。负调靠单一 PID 压不住：压超调就加剧负调，压负调就拖慢响应。论文的做法是把控制器拆成两段——PI 负责压负调，PD 负责压超调与加快瞬态，再用平滑因子按误差大小调配权重：

U = S_PI * U_PI + S_PD * U_PD
S_PI = 1            ( |e| ≥ 1 )
S_PI = |e|^n        ( |e| < 1 )      // n 为平滑因子阶次
S_PD = 1 - S_PI

误差大时几乎全由 PI 接管（先别冲），误差进入 1 以内后逐渐交给 PD（快速收敛）。论文在三个非最小相位对象上，把两阶段 PI/PD 与文献中的 GA-SA 整定 PID 做了对比：

指标	对象 3（GA-SA → PI/PD）	对象 4（GA-SA → PI/PD）	对象 5（GA-SA → PI/PD）
超调 Os/%	1.29 → 0.29	1.92 → 0.04	1.28 → 0.01
负调 Us/%	3.47 → 3.27	1.14 → 2.08	8.35 → 7.35
上升时间 Tr/s	3.10 → 1.72	3.45 → 2.82	3.80 → 3.12
调节时间 Ts/s	4.35 → 3.11	5.03 → 4.22	6.22 → 5.50

平滑因子阶次 n 则是一个纯折中旋钮。以对象 3 为例：n=1 时超调 0.06%、调节 3.67s；n=3 时超调 0.24%、调节 3.25s；n=5 时超调 0.29%、调节 3.11s。负调始终保持在 3.27% 不变，变化只发生在"超调"与"快速性"之间。关节调试时可以这样用：先取 n=3 看整体手感，振动偏大就降到 1，节拍不够就升到 5。

七、STM32 上怎么落地：离线寻优 + 在线调度

分工要清楚：PSO 寻优在 PC 或上位机上做，输入是关节的辨识模型或实测阶跃录波，输出是一张 (K_p, K_i, K_d) 参数表；STM32 只做三件事——按工况查表、双线性插值、把参数送进定点 PID。下面这段代码是可以直接搬到 STM32F4 上的寻优内核（也可在 PC 上编译，把仿真模型换成录波回放）：

/* 关节简化模型：J*dω/dt = u - b*ω ，输出角度 θ */
#define J_EQ      0.0012f      /* 折算到电机轴的等效惯量 kg·m^2 */
#define B_VISC    0.0008f      /* 粘性阻尼 N·m·s/rad          */
#define U_MAX     3.0f         /* 关节力矩限幅 N·m             */
#define DT        0.00005f     /* 20kHz 电流环，仿真步长 50µs  */

typedef struct { float kp, ki, kd; } pid_t;

/* 一次阶跃仿真，返回 ITAE；同时把积分抗饱和做进去 */
static float sim_itae(pid_t g, int n)
{
    float th = 0.0f, w = 0.0f, integ = 0.0f, e_prev = 0.0f, itae = 0.0f;
    for (int k = 0; k < n; k++) {
        float e   = 1.0f - th;                 /* 单位阶跃 */
        float der = (e - e_prev) / DT;

        integ += e * DT;                       /* 先积分，再限幅 */
        float u = g.kp * e + g.ki * integ + g.kd * der;
        if (u > U_MAX) {                       /* 抗饱和：顶到限幅就回退积分 */
            u = U_MAX;
            integ -= e * DT;
        } else if (u < -U_MAX) {
            u = -U_MAX;
            integ -= e * DT;
        }

        w  += (u - B_VISC * w) / J_EQ * DT;
        th += w * DT;
        itae += (float)k * DT * fabsf(e);      /* 时间加权 */
        e_prev = e;
    }
    return itae;
}

PSO 主循环只需要维护三样东西：每个粒子的位置、速度、pBest，以及一个全局 gBest。更新时先算适应度，再比 pBest/gBest，最后按第二节的两式推进并做 V_max 与区间双重限幅。注意 fabsf 与 float 运算在 M4 上有单周期 FPU，20 粒子 × 100 代 × 2000 步约 1.2×10⁷ 次递推，按每步十余个周期估算在 1 秒量级——完全可以在台架上跑完，不必上电脑。

真正跑在关节里的，是下面这套"查表 + 插值"的轻量逻辑：按关节电流与温度分 3 档，档间线性插值，切换时对积分项做限幅继承。参数表放 Flash 末页，出厂标定写入，OTA 可整表替换。

typedef struct { float i_lo, i_hi, kp, ki, kd; } gain_seg_t;

/* 3 段增益表：空载 / 额定 / 重载 */
static const gain_seg_t gain_tbl[3] = {
    { 0.0f, 2.0f, 0.85f, 12.0f, 0.0060f },
    { 2.0f, 6.0f, 0.62f,  9.5f, 0.0045f },
    { 6.0f, 9.0f, 0.48f,  7.0f, 0.0032f },
};

void gain_schedule(float i_now, pid_t *out)
{
    if (i_now <= gain_tbl[0].i_hi) { *out = (pid_t){ gain_tbl[0].kp,
        gain_tbl[0].ki, gain_tbl[0].kd }; return; }
    for (int k = 1; k < 3; k++) {
        if (i_now <= gain_tbl[k].i_hi) {
            float t = (i_now - gain_tbl[k].i_lo) /
                      (gain_tbl[k].i_hi - gain_tbl[k].i_lo);
            out->kp = gain_tbl[k-1].kp + t * (gain_tbl[k].kp - gain_tbl[k-1].kp);
            out->ki = gain_tbl[k-1].ki + t * (gain_tbl[k].ki - gain_tbl[k-1].ki);
            out->kd = gain_tbl[k-1].kd + t * (gain_tbl[k].kd - gain_tbl[k-1].kd);
            return;
        }
    }
    *out = (pid_t){ gain_tbl[2].kp, gain_tbl[2].ki, gain_tbl[2].kd };
}

表里的数值只表示结构，实际值必须由你的关节模型寻优得到——直接抄别人的 K_p 是关节调试最常见的坑。另外两点：微分项要带一阶低通（截止频率取电流环带宽的 1/10），否则编码器量化噪声会被放大成力矩抖动；参数切换要保留原积分量、只换比例与微分，否则重载切换会出现力矩台阶。

八、七步落地清单

步骤	做什么	验收判据
1　建对象模型	辨识等效惯量 J、阻尼 b、延迟 Td，或直接录一段阶跃响应做回放	仿真阶跃与实测曲线上升段吻合
2　跑 ZN 定心	测临界增益 Ku 与振荡周期 Tu，算出 K′p、K′i、K′d	闭环能稳定运行，允许超调大
3　定搜索空间	按 α=0.9 收缩，检查区间是否包含手感值	区间宽度可比"0~100"小两个数量级
4　选适应度	点位用 ITAE，轨迹用 IAE 或加权 ITAE	指标与工艺节拍一致
5　离线寻优	20 粒子、100 代、Vmax=区间宽度，固定随机种子	20 次重启结果离散度小
6　固化成表	按电流/温度分 3 档写入 Flash，档间线性插值	全工况无超调、无爬行
7　极限验证	空载/满载/低温/急停复位各跑 50 次点位	到位误差与节拍均满足工艺

本文基于《基于粒子群算法的 PID 控制器参数自整定》（王介生、王金城、王伟，大连理工大学自动化系，《控制与决策》2005 年第 20 卷第 1 期）等资料整理优化，并结合机器人关节伺服的工程实践进行了系统化梳理。文中对象 1、对象 2 及三个非最小相位对象的参数与性能数据均引自该文仿真结果。