串级 PID：云台的角度环和速度环

对象：云台和转动的底盘#

云台偏航轴还是第 1 篇的 GimbalAxis，扭矩指令的 GM6020：

$\psi$ 是云台相对世界的角度，$\tau$ 是电机力矩（限幅 $\pm1$ N·m），$\omega_c$ 是底盘的角速度。最后一项是轴承的粘性耦合：底盘转起来，轴承会把云台往同一个方向拖。参数 $J = 0.02$ kg·m²、$b = 0.002$、$b_f = 0.02$ 全是我取的量级，不是实测。令 $\omega = \dot\psi$，$b_t = b + b_f = 0.022$，速度子系统是

对云台来说，$\omega_c$ 就是一个力矩扰动 $d = b_f\omega_c$：底盘到 6 rad/s 时是 $0.02\times6 = 0.12$ N·m，占力矩上限的 12%。这个扰动是一段斜坡：我让 $\omega_c$ 从 1.0 s 到 1.5 s 线性升到 6 rad/s，之后保持。

测量有两路：角度 $\psi + n_a$，$\sigma_a = 10^{-4}$ rad；陀螺 $\dot\psi + n_g$，$\sigma_g = 0.01$ rad/s。噪声是 Python 用 numpy 种子 7 生成、写进数据 CSV 的，三种原生语言读同一份。这两个数字的出处要交代清楚：卡尔曼系列第 4 篇的陀螺仿真用的是 0.005 rad/s，那是“典型消费级 MEMS 的量级”；我这里取了它的 2 倍，是为了让噪声看得见，不是那一篇的结论。角度的 $10^{-4}$ rad（0.0057°）是我假设的姿态估计精度，也不是那一篇测出来的。

单环：三个增益摆三个极点#

先写单环，它是对照组。用第 5 篇的 Pid，只开 P 和 I，D 用陀螺：陀螺读数本身就是角度的导数，不需要对带噪声的角度再差分一次。控制律是

陀螺项通过 Pid.update 的 ff 参数送进去（$\mathrm{ff} = -k_{dg}\dot\psi_{gyro}$），它进入限幅前的 $v$，所以抗积分饱和照样看得见它。代入对象，特征多项式是

要的极点是一个实极点 $-p$ 加一个 $\zeta = 0.7$ 的共轭对，即 $J(s+p)(s^2 + 2\zeta\omega s + \omega^2)$。对系数得到

取 $p = \omega = \omega_o = \omega_i/5 = 12$ rad/s（$\omega_i = 60$ rad/s 是后面串级内环的带宽，这样两边“外环带宽”相同，对比才公平）：$k_{dg} = 0.554$，$k_p = 6.912$，$k_i = 34.56$。极点是 $-12$ 和 $-8.40\pm8.57\mathrm{j}$（report 打印的）。

这个单环有一个第 2 篇已经讲过的毛病：闭环传递函数 $T(s) = (k_ps + k_i)/(\ldots)$ 有一个零点在 $-k_i/k_p = -5$ rad/s，比极点慢，所以阶跃会超调，不管阻尼多好。我会在结果里看到这一点。

串级：内环管速度，外环管角度#

串级把这三个增益拆成两层。内环是速度 PI，设定值是 $\omega_{ref}$，测量是陀螺：

被控对象 $\omega/\tau = 1/(Js + b_t)$ 是一阶的，和第 2 篇的电机速度环同一个形状，所以直接套那一篇的极点配置，闭环传递函数是

令 $\zeta = 0.7$、$\omega_i = 60$ rad/s：$k_{i,i} = J\omega_i^2 = 72$，$k_{p,i} = 2\zeta\omega_iJ - b_t = 1.658$。这个 PI 有一个零点在 $-k_{i,i}/k_{p,i} = -43.4$ rad/s，比 $\omega_i$ 还低一点，稍后会看到它对外环有用。

外环是一个纯 P，输入角度误差，输出内环的速度设定值：$\omega_{ref} = k_o(r - \psi)$，$k_o = \omega_i/\text{比值}$。比值就是“内环比外环快多少倍”，是这一篇的主要旋钮。整条链路是：

虚线是这一篇最后一节才用到的：内环把它有没有饱和告诉外环。

外环看到的对象不是云台，而是“内环加一个积分器”：$L_o(s) = k_o\,T_i(s)/s$。如果把 $T_i$ 粗略当成一阶的 $1/(s/\omega_i + 1)$，外环的特征方程是 $s^2 + \omega_i s + \omega_i k_o = 0$，阻尼比 $\zeta_o = \tfrac12\sqrt{\omega_i/k_o} = \tfrac12\sqrt{\text{比值}}$，比值到 4 就是临界阻尼；相角裕度是 $90^\circ - \arctan(\omega_c/\omega_i)$：内环对外环来说就是一个滞后，比值越大，这个滞后在外环穿越频率处吃掉的相位越少。我不信一个近似，所以 report 用第 3 篇的 freq 在精确的 $T_i$ 上也算一遍（延迟取 ZOH 的半个采样周期）。

串级为什么抗扰：等效 PID#

回到 17.5 倍。扰动力矩 $d$ 到角度 $\psi$ 的传递函数，单环和串级是同一个形状（令 $r = 0$，把控制律代进去）：

单环的 $(k_d, k_p, k_i)$ 就是 $(k_{dg}, k_p, k_i)$；串级的是等效增益

代入数字，比值 5（$k_o = 12$）：

两者最慢的极点几乎在同一个位置（$-12.00$ 和 $-11.48$），所以“外环带宽”确实是一样的；串级多出来的是一对 60 rad/s 量级的快极点，换来了 13.3 倍的 $k_p$ 和 25 倍的 $k_i$。对一段斜率为 $m$ 的力矩斜坡，这个 $s/(\ldots)$ 形式的闭环由终值定理给出稳态角度误差 $m/k_i$：

单环是 $0.24/34.56 = 6.944$ mrad，串级是 $0.24/864 = 0.278$ mrad。换个说法更直观：内环的 PI 先把速度误差压到 $m/k_{i,i} = 0.24/72 = 3.33$ mrad/s，外环再把它除以 $k_o = 12$，就是 0.278 mrad。扰动在到达角度之前，已经在速度这一层被消掉了大部分。 类比的话，这像工厂里班组先把小故障在车间内消化，不往上报给调度：车间的响应快，所以故障还没传远就被吃掉。

要把话说准：这不是说单环做不到。串级的 $(k_d, k_p, k_i) = (1.658, 91.9, 864)$ 直接塞进一个单环 PID，特征多项式和扰动响应完全一样。差别是单环用的是我选的“三个极点都放在 12 rad/s”的配方，三个增益被这个配方锁死了；串级的配方是“先把内环做到 60 rad/s，外环按比值再降”，它用同样的三个增益自由度，摆出了更刚的一组极点，并且让整定变成两个有物理含义的旋钮：内环带宽和比值。设定值通道也不同（外环 P 没有积分，没有那个 $-5$ rad/s 的零点）。

底盘角速度前馈。 底盘陀螺（底盘上本来就有）给出 $\omega_c$，所以扰动 $b_f\omega_c$ 是已知的，可以直接在内环的输出上抵消。符号要小心：云台静止（$\dot\psi = 0$）时，耦合项对云台的力矩是 $+b_f\omega_c$，所以前馈是 $\mathrm{ff} = -b_f\omega_c$。

带宽比：多快才算“快得够”#

比值太小，内环在外环穿越频率处的滞后就大，相角裕度掉；比值太大，外环慢得没必要，而且你把整条链路的响应速度扔掉了。我在 $\{1.5, 3, 5, 10\}$ 四个比值上扫：$k_o = 40, 20, 12, 6$ rad/s，内环都不动。

内环饱和：外环怎么知道#

云台要转一个大角度，比如 1.5 rad，外环给出 $k_o\times1.5 = 18$ rad/s 的速度设定值。力矩上限 1 N·m 对应的最大角加速度是 $1/0.02 = 50$ rad/s²，加速到 18 rad/s 至少要 0.36 s。这段时间里内环的力矩一直贴在上限，内环饱和了，而外环自己并没有饱和：外环输出的是速度设定值，我没有给它限幅。所以第 5 篇的条件积分（CLAMP）在外环看不出任何问题，如果外环带积分（$k_{i,o} = 0.2\,k_o^2 = 28.8$），它会一直在 1.5 rad 的误差上累加，最后速度设定值积出来一大块，角度冲过头。

解法在第 5 篇的 hold_integral 里：驱动把内环上一个采样的 saturated 标志传给外环，外环这一拍不更新积分：

延迟一拍是因为内环要在外环之后才更新；我猜 1 kHz 下一拍的差别可以忽略，但没有单独测。这个策略很粗：内环一饱和就整个冻结外环积分，不看误差方向；第 5 篇的条件积分只在“继续往饱和方向积”时才跳过。我这一篇没有比较两种策略。

C#

构造放在 make_loops，下标约定是 pid[0] 外环（或单环），pid[1] 内环。增益公式和 Python 一行对一行，这里只看内环那一段：

1
  kpi = 2.0 * ZETA * WN_I * g->j - (g->b + g->bf);
2
  kii = WN_I * WN_I * g->j;
3
  ko = WN_I / c->ratio;
4
  kio = c->outer_i ? OUTER_KI * ko * ko : 0.0;
5
  set_cfg(&cfg, ko, kio);
6
  ret = pl_pid_init(&pid[0], &cfg);
7
  if (ret)
8
    return ret;
9
  set_cfg(&cfg, kpi, kii);
10
  cfg.u_min = -g->u_max;
11
  cfg.u_max = g->u_max;
12
  return pl_pid_init(&pid[1], &cfg);

闭环里的接线，hold 在内环更新之前读：

1
    if (c->kind == LOOP_SINGLE) {
2
      u = pl_pid_update(&pid[0], row[cr], angle, -kdg * gyro,
3
            false);
4
      sat = pid[0].saturated;
5
    } else {
6
      /* the inner flag still holds the last sample's value */
7
      bool hold = c->hold && pid[1].saturated;
8
      pl_real ff = c->ff ? -g.bf * row[cw] : 0.0;
9

10
      wref = pl_pid_update(&pid[0], row[cr], angle, 0.0,
11
               hold);
12
      u = pl_pid_update(&pid[1], wref, gyro, ff, false);
13
      sat = pid[1].saturated;
14
    }

完整应用：c/apps/ch07.c。

C++#

外环配置先按单环填好（带 $\pm1$ N·m 限幅），串级时整个换成无限幅的新配置；Create 返回 std::optional，inner->saturated() 是访问器：

1
  if (c.cascade) {
2
    const double ko = kWnI / c.ratio;
3
    outer_cfg = {ko, c.outer_i ? kOuterKi * ko * ko : 0.0, 0.0, kTs};
4
    outer_cfg.u_min = -inf;
5
    outer_cfg.u_max = inf;
6
  }
7
  auto outer = pidlab::Pid<double>::Create(outer_cfg);
8
  auto inner = pidlab::Pid<double>::Create(inner_cfg);
9
  if (!outer || !inner) return false;
10
  // ...
11
    } else {
12
      // the inner flag still holds the last sample's value
13
      const bool hold = c.hold && inner->saturated();
14
      const double ff = c.ff ? -g.bf * (*wc)[k] : 0.0;
15
      wref = outer->Update((*r)[k], angle, 0.0, hold);
16
      u = inner->Update(wref, gyro, ff);
17
      sat = inner->saturated();
18
    }

完整应用：cpp/apps/ch07.cc。

Rust#

outer_cfg 和 C++ 一样先按单环填限幅，串级时用一个默认无限幅的 PidConfig::new 整个替换：

1
    if c.cascade {
2
        let ko = WN_I / c.ratio;
3
        let ki = if c.outer_i { OUTER_KI * ko * ko } else { 0.0 };
4
        outer_cfg = PidConfig::new(ko, ki, 0.0, TS);
5
    }
6
    let mut outer = Pid::new(outer_cfg).map_err(|e| format!("{e:?}"))?;
7
    let mut inner = Pid::new(inner_cfg).map_err(|e| format!("{e:?}"))?;
8
    // ...
9
        if c.cascade {
10
            // the inner flag still holds the last sample's value
11
            let hold = c.hold && inner.saturated();
12
            let ff = if c.ff { -g.bf * wc[k] } else { 0.0 };
13
            wref = outer.update(r[k], angle, 0.0, hold);
14
            u = inner.update(wref, gyro, ff, false);
15
            sat = inner.saturated();
16
        }

完整应用：rust/src/bin/ch07.rs。

单环对串级：阶跃#

0.2 rad 的阶跃，单环超调 29.89%，上升时间 0.095 s，调节时间 0.513 s；串级（比值 5）超调 0.08%，上升时间 0.186 s，调节时间 0.354 s。单环的超调主要来自上面说的 $-5$ rad/s 零点，不是阻尼不足：把阶跃缩小到 5 mrad，不碰力矩限幅，report 另算了一遍，单环超调是 33.95%，比限幅下的还大 4 个百分点（限幅砍掉了一点峰值）。相角裕度 65.9° 一点也没预报这个超调：相角裕度说的是回路的鲁棒性，不是设定值响应的形状，第 2 篇的教训。

下图的下半部分是力矩：两种回路的第一拍都撞上了 $\pm1$ N·m 限幅，因为 0.2 rad 对单环是 $6.912\times0.2 = 1.38$ N·m，对串级是 $12\times0.2\times1.658 = 3.98$ N·m。单环贴了 14 ms，串级（比值 5）贴了 31 ms。这也意味着这一篇所有 0.2 rad 的阶跃响应都包含了饱和，线性分析要靠小阶跃单独验证。

底盘转动#

这是实验的主角。三条曲线的峰值：

串级把峰值砍到 1/17.5，加上底盘角速度前馈砍到 1/44.1。“区间”是 1.0 s 到 3.0 s。斜坡误差公式的预测：单环 6.944 mrad，仿真峰值是它的 1.03 倍；串级预测 0.278 mrad，仿真是 1.47 倍。串级这边偏大，我猜是因为 0.5 s 的斜坡对它的两个快极点够长，但角度测量噪声（$10^{-4}$ rad，也就是 0.1 mrad）已经占了峰值的相当一部分，我没有把噪声和斜坡响应拆开验证。两个回路的公式比值是 25 倍（$k_i$ 之比），仿真峰值的比值是 17.5 倍，差的那部分我猜是噪声，加上 0.5 s 的斜坡对串级的快极点并没有长到稳态。

代价。 稳态时三个回路都要用约 0.12 N·m 的力矩顶住 $b_f\omega_c$，所以最后 0.5 s 力矩的 RMS 差不多：单环 0.12008、串级 0.12116 N·m。把 0.12 在平方和意义下减掉，粗算噪声分量是单环约 0.004、串级约 0.017 N·m，差 3.8 倍。这是我对打印数字的心算，report 没有直接打噪声分量。串级的内环增益 $k_{p,i} = 1.658$ 把陀螺噪声放大了 3 倍（对比单环的 $k_{dg} = 0.554$），这部分是实打实的代价。

带宽比#

数字来自 report 的两张表。先是 0.2 rad 阶跃（含饱和）和线性分析：

再是 5 mrad 的小阶跃（outer_margins 的相角裕度配上一次不饱和的仿真，这一张表只有 Python 跑，没有跨语言对比）：单环 33.946%，比值 1.5 是 4.608%，比值 3、5、10 都是 0.000%。

几个结论：

• 比值 1.5 是唯一出问题的。 相角裕度 61.7°，不饱和时超调 4.6%，被限幅打断之后（138 ms 贴着上限）超调 15.6%，三倍多。比值 3 以上，线性超调是 0，相角裕度 85° 以上。
• 比值 3 以后是在用速度换不必要的裕度。 相角裕度从 85.4° 到 88.7° 到 89.6°，只多了 4.2°，上升时间从 0.123 s 到 0.186 s 到 0.363 s，慢了 2.95 倍。在这些假设下，这个对象用比值 3 到 5 就够了；“内环快十倍”的口诀在这里是浪费。
• 一阶近似偏保守。 比值 1.5 时低 2.7°，比值 3 时低 13.6°。我猜是因为内环 PI 的零点（$-43.4$ rad/s）在外环穿越频率附近带来了超前，一阶近似里没有它，我没有把零点拿掉重算来验证。这是我没想到的：粗近似不是“偏乐观”而是“偏悲观”。
• 大阶跃下比值 5 到 10 的超调都是 0.07% 到 0.08%，差别在噪声量级，report 没有在这上面断言。

内环饱和#

外环 PI，$k_o = 12$、$k_{i,o} = 28.8$，1.5 rad 阶跃：

冻结外环积分把超调从 46.41% 降到 1.59%（29 倍），积分峰值从 7.329 降到 0.379 rad/s（19 倍），调节时间从 1.240 s 缩到 0.329 s（3.8 倍）。内环饱和的时间也短了，467 ms 对 302 ms：外环不再把速度设定值积到离谱的高度，少饱和了 165 ms。report 把两种饱和分开数了：outer_aw_off 先贴着 $+1$ N·m 229 ms，再贴着 $-1$ N·m 反向刹车 238 ms；outer_aw_on 是 182 ms 和 120 ms，刹车的那一段短了一半。

这些数字的边界#

上面的比较有几处要打折，我逐条列出来：

• 前馈的 44 倍是上限。 前馈用的是精确的 $\omega_c$，没有给底盘陀螺加噪声和延迟。真车上的底盘陀螺有噪声，而且和云台陀螺不是同一颗，这 44 倍应当打折，打多少折我没测。
• 单环不是“最好的单环”。 我选的是三个极点都在 12 rad/s 的配方，它被锁在低增益里；前面已经推过，串级的等效增益塞进单环就是同一个扰动响应。这一篇比较的是“两种整定配方”，不是“单环这种结构”对“串级这种结构”。
• 单环和串级的 $\omega_c$ 不能直接比。 单环的开环断在对象输入，串级的断在速度设定值，两者的穿越频率是两个不同回路的量。表里放在一起只是为了省地方。
• 饱和混进了线性分析。 0.2 rad 的阶跃对所有回路都撞限幅，所以阶跃响应的比较要和 5 mrad 的线性表一起看。
• 冻结策略没有和条件积分比较。 见前面“内环饱和：外环怎么知道”一节。
• 噪声和参数是假设。 $J$、$b$、$b_f$ 和两个噪声标准差都是我取的量级，没有测过。一台真的云台，$b_f$ 很可能比这小一个量级，那样底盘转动的扰动就没有这里这么大。

实验清单#