传递函数、极点与根轨迹

拉普拉斯变换：把微分方程变成代数方程#

函数 $f(t)$（$t \ge 0$）的拉普拉斯变换定义为

$s$ 是复数。我在这里只需要三条性质，每一条都值得自己推一遍。

微分变成乘以 $s$。 分部积分：

初值为零时，时域里的 $d/dt$ 就是 $s$ 域里的乘以 $s$。这一条的用处是把 $J\dot\omega + b\omega = K_t i$ 这种常微分方程变成 $(Js + b)\,\Omega(s) = K_t I(s)$，求解变成除法。

两个基本变换。 单位阶跃 $1(t)$ 的变换是 $\int_0^\infty e^{-st}dt = 1/s$；指数 $e^{-at}$ 的变换是 $1/(s + a)$。有了它们和线性性质，反变换基本靠部分分式，把 $F(s)$ 拆成这两种形式的和。

终值定理。 如果 $sF(s)$ 的极点全在左半平面（$f$ 真的收敛），那么 $f(\infty) = \lim_{s \to 0} sF(s)$。推导：对 $\dot f$ 的变换令 $s \to 0$，左边是 $\int_0^\infty \dot f\,dt = f(\infty) - f(0)$，右边是 $\lim sF(s) - f(0)$，两边消掉 $f(0)$。条件不能省：对不稳定的系统这个公式仍然会给你一个漂亮的有限数，只是和真实响应毫无关系。后面的 Routh 判据就是用来先回答“它到底收敛吗”。

用终值定理把第 1 篇的静差重算一遍，当作对工具的检验。纯 P 控制的速度环，开环是 $G(s) = K_t/(Js + b)$，闭环 $T = K_tK_p/(Js + b + K_tK_p)$，阶跃 $R = r/s$，所以

$e_{ss} = r - \omega(\infty) = b\,r/(b + K_tK_p)$，和第 1 篇令导数为零得到的结果一致（没有负载的情形）。

传递函数：每个对象写成一个分式#

初值为零时，输出的变换除以输入的变换叫传递函数。这一篇和后面几篇用到的对象是（参数沿用第 1 篇，新增的两个我标明）：

MotorSpeedLag 多出来的一阶因子是电流环：电调不会瞬间把电流拉到指令值，我把它建成 $\tau_c\,di/dt = i_{cmd} - i$。DialPosition 是拨弹盘上的 M3508，状态是角度、角速度和电流，输出是角度。它的 $J = 0.005$ 和 $\tau_c = 0.002$ s 都是我取的数（量级合理，没有实测），所以下面所有关于它的数字都应该读成“这个形状”，不是“你车上的数”。

PID 控制器是 $C(s) = K_p + K_i/s + K_ds$。把控制器和对象首尾相连，开环 $L(s) = C(s)G(s)$，闭环有两个值得命名的函数：

$T$ 管设定值到输出，$S$ 管误差。第 1 篇的负载 $\tau_d$ 从对象输入端进来，等价于电流扰动 $\tau_d/K_t$，所以 $Y = T\,R - S\,G\,\tau_d/K_t$。两个函数的分母都是 $1 + L$ 的分子，也就是特征多项式，它的根叫闭环极点。系统的所有自由运动都是极点的组合：每个极点 $p$ 贡献一项 $e^{pt}$。（类比一下琴弦：拨的位置和力度只决定每个音的音量，响的永远是弦自己的音高，极点就是系统的音高。）这一篇所有的事归结为一句话：极点在哪，响应就长什么样；$K_p$、$K_i$、$K_d$ 的作用是把极点搬到我要的地方。

二阶原型：阶跃响应的完整推导#

很多闭环最后都化成同一个形状，我把它叫原型：

$\omega_n$ 是固有频率，$\zeta$ 是阻尼比。$0 < \zeta < 1$ 时极点是一对共轭复数 $-\sigma \pm j\omega_d$，$\sigma = \zeta\omega_n$，$\omega_d = \omega_n\sqrt{1 - \zeta^2}$。单位阶跃响应的变换是 $Y_0 = T_0/s$，拆成部分分式：

后两项是 $e^{-\sigma t}\cos\omega_d t$ 和 $e^{-\sigma t}\sin\omega_d t$ 的变换，所以

对 $t$ 求导，$\dot y_0(t) = \dfrac{\omega_n}{\sqrt{1-\zeta^2}}\,e^{-\sigma t}\sin\omega_d t$（这也是原型的脉冲响应）。令它为零，第一个解是 $\omega_d t = \pi$：

这就是第 1 篇一直在用的超调公式。调节时间看包络线：$e^{-\sigma t}/\sqrt{1-\zeta^2}$ 降到 0.02 以下的时间大约是 $4/\sigma$，所以 $t_s \approx 4/(\zeta\omega_n)$（一条经验公式，$\zeta$ 很小或接近 1 时会偏）。$\zeta = 1$ 是临界阻尼，$y_0 = 1 - (1 + \omega_n t)e^{-\omega_n t}$，没有超调；$\zeta > 1$ 是两个实极点，也没有超调，只是更慢。上升时间没有这么干净的闭式，我让仿真去量。

极点配置：把 $K_p$、$K_d$ 变成算出来的数#

云台。 力矩指令的云台，$J\ddot\psi = \tau - (b + b_f)\dot\psi$。我这次不用第 1 篇的误差微分，而是用角速度反馈：$\tau = K_p(r - \psi) - K_d\dot\psi_{meas}$（这样设定值的跳变不会被微分成脉冲，第 5 篇会专门处理这件事）。代入：

这恰好是原型，没有任何零点。对照 $2\zeta\omega_n = (b + b_f + K_d)/J$ 和 $\omega_n^2 = K_p/J$：

注意括号里是 $b + b_f$：底盘不动时，轴承耦合 $b_f$ 对云台就是一个额外的阻尼。只减 $b$ 的话，得到的 $\zeta$ 会比目标少一点点。$\omega_n = 20$ rad/s、$J = 0.02$ 时 $K_p = 8$，$\zeta = 0.2,\ 0.4,\ 0.7,\ 1$ 对应 $K_d = 0.1380,\ 0.2980,\ 0.5380,\ 0.7780$。

速度环 PI。 $u = K_p e + K_i\int e\,dt$，对象 $K_t/(Js + b)$。特征多项式是 $s(Js + b) + K_t(K_ps + K_i)$：

$\zeta = 0.7$、$\omega_n = 10$ rad/s 给出 $K_p = 1.367$、$K_i = 10$；$\omega_n = 30$ rad/s 给出 $K_p = 4.167$、$K_i = 90$。

闭环零点：PI 为什么超调#

极点对了不代表响应是原型，因为分子还没看。PI 的闭环传递函数是

分子 $K_ps + K_i$ 在 $s = -z$ 处为零，这是闭环零点。它的效果很容易算：$T_{PI} = (1 + s/z)\,T_0$，所以 $Y_{PI} = Y_0 + sY_0/z$，而 $sY_0$ 正好是 $\dot y_0$ 的变换（$y_0(0) = 0$），时域里就是

原型的 $\dot y_0$ 在到达峰值之前一直是正的，所以零点把一份“速度”加到了位置上，让输出更早、更猛地冲向设定值，超调自然变大。$z$ 越靠近 $\omega_n$ 影响越大。

如果我只想要积分器给的极点、不想要零点，办法是让比例项只作用在测量值上：$u = -K_p y + K_i\int(r - y)\,dt$，我叫它 I-P。这时 $U = -K_pY + K_i(R - Y)/s$，代进对象得 $T_{IP} = K_tK_i/(Js^2 + (b + K_tK_p)s + K_tK_i)$，分子是常数，正好是原型，超调只由 $\zeta$ 决定。代价是设定值进来的路径少了 $K_p$ 那一份，响应会慢。第 5 篇会把这个做成一个参数 $b$（设定值权重，$b = 1$ 是 PI，$b = 0$ 是 I-P）。

先把第 1 篇的账算清楚：$K_p = 1$、$K_i = 10$ 时 $\omega_n = 10$、$\zeta = 0.517$、$z = 10$。原型公式给 15.02%，带上这个零点是 27.94%，第 1 篇实测 28.02%。三个数对得上，那 13 个百分点就是零点。

Routh–Hurwitz：三阶系统稳定的条件#

拨弹盘用 P 控制 $u = K e$，$e = r - \theta$，特征多项式是 $s(Js + b)(\tau_cs + 1) + KK_t$：

一般的三阶 $a_3s^3 + a_2s^2 + a_1s + a_0$（系数全正），它在稳定与不稳定之间的边界上有一对极点正好落在虚轴上。令 $s = j\omega$：

所以边界是 $a_2a_1 = a_3a_0$，稳定条件是 $a_2a_1 > a_3a_0$。这就是 Routh 阵列第一列第三项 $(a_2a_1 - a_3a_0)/a_2$ 为正的条件（阵列的前两行是 $a_3, a_1$ 和 $a_2, a_0$，第三行 $(a_2a_1 - a_3a_0)/a_2$，第四行 $a_0$，第一列不变号就稳定）。代入拨弹盘：

数值是 $K_{crit} = 16.7333$ A/rad，穿越频率 $\omega_c = 31.623$ rad/s。K 超过它，一对极点跑到右半平面，响应是一个越来越大的振荡。

但这个结论有一个前提：连续时间。我的控制器是 1 kHz 的采样系统，输出要保持一个周期。第 1 篇说过零阶保持平均相当于 $t_s/2$ 的延迟，这个延迟对稳定边界的影响，我这次用精确的离散模型算出来，不用近似。拨弹盘的状态方程 $\dot x = Ax + Bu$，保持 $u$ 一个周期后 $x_{k+1} = A_dx_k + B_du_k$，其中

$u_k = K(r - \theta_k)$ 时，$A_d - B_dK\,[1\ 0\ 0]$ 的特征值模长是 1 的地方就是真正的边界，等效的增长率是 $\ln|\lambda|_{max}/t_s$。这一步我用 scipy.linalg.expm（第 9 篇会手写）和 numpy 求特征值，不用手写的参考，因为它只是用来解释仿真，不是四种语言要对齐的东西。

根轨迹：极点作为 $K$ 的函数#

画根轨迹的传统是用一套规则手画。我不画，我直接算：对 400 个对数间隔的 $K$（0.01 到 1000），每个求一次特征多项式的根。定义很简单：控制器是 $K$ 乘一个固定的 $N_c(s)$ 时，特征多项式是 $D(s) + K\,N(s)$（$D$ 是对象分母，$N$ 是对象分子乘 $N_c$ 的分子），根随 $K$ 连续移动，画出来就是根轨迹。$K \to 0$ 时根趋向 $D$ 的根（开环极点），$K \to \infty$ 时趋向 $N$ 的根（开环零点）或无穷远。

两条要算的轨迹，都是拨弹盘：

• P： $D + KK_t$，开环极点在 $0$、$-b/J = -2$ 和 $-1/\tau_c = -500$。
• PD： 控制器 $K(1 + s/20)$，多了一个零点 $-20$，特征多项式是 $J\tau_cs^3 + (J + b\tau_c)s^2 + (b + KK_t/20)s + KK_t$。

PD 的 Routh 条件可以直接写出来：$(J + b\tau_c)(b + KK_t/20) > J\tau_c KK_t$，整理成

方括号里是 $0.00502/20 - 0.00001 = 2.41\times 10^{-4} > 0$，所以对任何 $K > 0$ 都成立：这个 PD 对任何增益都稳定。 一个零点就把边界拆掉了，这是后面反复出现的主题：D 项给相位超前，系统才敢用高增益。

数值求根：Durand–Kerner#

三阶多项式有求根公式，但我不想写它：公式在有重根时数值很差，也不好推广到后面的四阶和五阶。我用 Durand–Kerner 迭代：把多项式化成首一，同时维护 $n$ 个根的猜测 $z_1, \dots, z_n$，每个按

更新（用最新的 $z_j$，也就是原地更新）。初值是 $R\,(0.4 + 0.9j)^k$，$k = 0, \dots, n-1$，$R = 1 + \max|c_i|$ 是 Cauchy 界（所有根的模长都小于它）；这些点落在一条螺线上，互不相同，不会出现分母为零。收敛判据是所有更新量都小于 $10^{-14}(1 + |z|)$，最多 1000 轮。

之后做 3 步 Newton 抛光。重根附近 $p'$ 接近零，Newton 一步可能飞出去，所以我加了保护：只在 $|p|$ 变小时才接受这一步。最后做两件事保证四种语言的输出可以逐列对比：虚部小于 $10^{-9}\max(1, |z|)$ 的根强制成实数；然后按实部从小到大排序，实部相差小于 $10^{-9}(1 + \max|\mathrm{Re}|)$ 的当作并列，并列的按虚部从小到大。共轭对因此永远是“负虚部在前”。

我原先担心重根附近 Durand–Kerner 的精度只有 $\sqrt\epsilon \approx 10^{-8}$ 量级，四种语言在根轨迹的分叉点附近会差出一点，打算给这两个文件把容差放宽到 $10^{-6}$。结果下面会看到：没用上。

阶跃响应指标#

仿真的响应是一串采样值，不是公式，所以指标要有离散的定义。阶跃在第 $k_0$ 个采样点施加，起始值取 $y[k_0]$，目标值 $r$，归一化 $z_i = (y_i - y[k_0])/(r - y[k_0])$，时间从 $k_0$ 起算：

• 上升时间： $z$ 第一次达到 0.1 和 0.9 的时刻（相邻两个采样点之间线性插值）之差。
• 超调： $\max(0, \max z - 1)\times 100\%$。
• 峰值时间： 第一个满足 $z \ge \max z - 10^{-9}$ 的采样点。直接取 argmax 的话，没有超调的响应在末尾会因为最后一位舍入而挑出不同的“峰”，四种语言可能挑得不一样；加一个 $10^{-9}$ 的容差，有超调的响应不受影响（峰顶是平的，差 $10^{-9}$ 对应不到一个采样点），单调的响应也有了确定的答案。
• 2% 调节时间： 最后一次 $|z - 1| > 0.02$ 的采样点之后的第一个时刻。
• IAE： $\sum|r - y_i|\,t_s$，矩形积分。

多项式#

多项式就是系数列表，最高次在前，和 numpy.roots 同一个约定。需要的运算很少：乘、加、数乘、求值。poly_add 要把常数项对齐，这样“三阶特征多项式加上一个常数 $KK_t$”可以直接写成 poly_add(plant, [k * kt])：

1
def poly_add(a: Sequence[float], b: Sequence[float]) -> list[float]:
2
    """Sum of two polynomials; the constant terms line up."""
3
    n = max(len(a), len(b))
4
    out = [0.0] * n
5
    for i, ai in enumerate(a):
6
        out[n - len(a) + i] += ai
7
    for i, bi in enumerate(b):
8
        out[n - len(b) + i] += bi
9
    return out
10

11

12
def poly_eval(a: Sequence[float], z: complex) -> complex:
13
    """Horner evaluation at a complex point."""
14
    acc = complex(0.0, 0.0)
15
    for ai in a:
16
        acc = acc * z + ai
17
    return acc

求值用 Horner 法，输入点是复数，因为求根的时候每个猜测都是复数。poly_mul 是两层循环的卷积，poly_scale 是列表推导，没什么好解释的。

Durand–Kerner 求根#

上一节的算法翻译成 Python，主体就是两个嵌套循环加一步 Newton。先看 Durand–Kerner 的那一段：

1
    c = [ai / a[0] for ai in a]
2
    radius = 1.0 + max(abs(ci) for ci in c[1:])
3
    seed = complex(0.4, 0.9)
4
    z = [radius * seed**k for k in range(n)]
5
    for _ in range(MAX_ITER):
6
        done = True
7
        for i in range(n):
8
            den = complex(1.0, 0.0)
9
            for j in range(n):
10
                if j != i:
11
                    den *= z[i] - z[j]
12
            if den == 0:
13
                continue
14
            step = poly_eval(c, z[i]) / den
15
            z[i] -= step
16
            if abs(step) > DK_TOL * (1.0 + abs(z[i])):
17
                done = False
18
        if done:
19
            break

这里的 n 是次数。seed**k 就是螺线上的第 $k$ 个点；z[i] -= step 在循环里原地更新，所以后面的根用到的是已经更新过的 z[j]，和上一节写的“用最新的 $z_j$”一致。

重根附近 $p'$ 接近零，Newton 抛光那一步我只在 $|p|$ 变小时才接受：

1
def _newton(a: Sequence[float], da: Sequence[float], z: complex) -> complex:
2
    """One safeguarded Newton step: keep it only if |p| shrinks."""
3
    p = poly_eval(a, z)
4
    dp = poly_eval(da, z)
5
    if dp == 0:
6
        return z
7
    z_new = z - p / dp
8
    return z_new if abs(poly_eval(a, z_new)) < abs(p) else z

最后的规范化也是几行：虚部够小的强制成实数，然后用插入排序按实部排（并列时按虚部）。我没有用 sorted，而是写了插入排序，四种语言可以写成同一个样子，比较函数带着这个容差：

1
def _after(a: complex, b: complex, tie: float) -> bool:
2
    """True when a must come after b."""
3
    if abs(a.real - b.real) <= tie:
4
        return a.imag > b.imag
5
    return a.real > b.real

poly_roots 在首项为零或次数小于 1 时抛 ValueError。

阶跃指标#

step_metrics 把上面五条定义直接翻成代码。先把响应归一化，再找各个时刻：

1
    y0 = y[k0]
2
    amp = r - y0
3
    z = [(y[i] - y0) / amp for i in range(k0, n)]
4
    t10 = _cross(z, 0.1, ts)
5
    t90 = _cross(z, 0.9, ts)
6
    zmax = max(z)
7
    k_peak = next(i for i, zi in enumerate(z) if zi >= zmax - PEAK_TOL)
8
    last_out = -1
9
    for i, zi in enumerate(z):
10
        if abs(zi - 1.0) > BAND:
11
            last_out = i

调节时间是 (last_out + 1) * ts，超调是 max(0, zmax - 1) * 100。线性插值的穿越时刻单独成一个函数：

1
def _cross(z: Sequence[float], level: float, ts: float) -> float:
2
    """First time z reaches level, linearly interpolated; NaN if never."""
3
    for i in range(1, len(z)):
4
        if z[i] >= level:
5
            frac = (level - z[i - 1]) / (z[i] - z[i - 1])
6
            return (i - 1 + frac) * ts
7
    return math.nan

没有穿过的时候返回 NaN 而不是抛异常，这样“没有到达 0.9”也能写进 CSV，四种语言的 NaN 在对比时算相等。

两个新对象#

MotorSpeedLag 的状态是 $[\omega, i]$，DialPosition 的状态是 $[\theta, \omega, i]$。接口和第 1 篇的对象一样，deriv(x, u) 和 output(x)，输入是 [指令, 负载]：

1
    def deriv(self, x, u):
2
        return [
3
            x[1],
4
            (self.kt * x[2] - self.b * x[1] - u[1]) / self.j,
5
            (u[0] - x[2]) / self.tau_c,
6
        ]

这是 DialPosition.deriv：角度的导数是角速度，角速度按 $J\dot\omega = K_ti - b\omega - \tau_d$，电流按 $\tau_c\dot i = i_{cmd} - i$。MotorSpeedLag.deriv 去掉第一行。它们是两个类而不是一个带开关的类：状态维数在 C、C++、Rust 里是编译期常数，一个对象不能有时是 1 维有时是 2 维。

驱动：实验表、设计公式和回路#

驱动 python/ch02_transfer_functions.py 还是 ref 和 report 两个子命令。每个实验是一个 Case，kind 说明控制器的种类，p1、p2 的含义随 kind 变，bw 是比例项里设定值的权重（1 是 PI，0 是 I-P）：

1
CASES = {
2
    "pd_z0.2": Case(PD, 0.2, PD_WN, 1.0, 0.0, 0.05),
3
    # ... pd_z0.4, pd_z0.7, pd_z1
4
    "pi_place_w10": Case(PI_PLACE, 0.7, 10.0, 1.0, 0.0, 5.0),
5
    "pi_place_w10_ip": Case(PI_PLACE, 0.7, 10.0, 0.0, 0.0, 5.0),
6
    # ... w30, w30_ip, w30_lag, pi_ch01, pi_ch01_ip
7
    "routh_0.7": Case(ROUTH, 0.7, 0.0, 1.0, 0.0, 0.05),
8
    # ... routh_0.9, routh_1.1
9
}

一共 14 组。design 就是上一节推出的两组公式，外加 Routh 临界增益：

1
    if c.kind == PD:
2
        g = GimbalAxis()
3
        kp = g.j * c.p2 * c.p2
4
        kd = 2.0 * c.p1 * c.p2 * g.j - (g.b + g.bf)
5
        return g, kp, kd
6
    # ...
7
    if c.kind == PI_PLACE:
8
        kp = (2.0 * c.p1 * c.p2 * m.j - m.b) / m.kt
9
        ki = c.p2 * c.p2 * m.j / m.kt
10
        return m, kp, ki
11

12

13
def critical_gain(d: DialPosition) -> float:
14
    """Routh limit of P control on the dial: (J + b tc) b / (J tc Kt)."""
15
    return (d.j + d.b * d.tau_c) * d.b / (d.j * d.tau_c * d.kt)

run_case 的循环顺序和第 1 篇完全一样（测量、控制、限幅、延迟线、保持、记录），区别在控制律那几行：

1
        e = r - y
2
        if c.kind == PD:
3
            u = k1 * e - k2 * x[1]  # rate feedback from the gyro
4
        elif c.kind == ROUTH:
5
            u = k1 * e
6
        else:
7
            u = k1 * (c.bw * r - y) + integ
8
        u = min(max(u, -plant.u_max), plant.u_max)
9
        rows.append([data["t"][k], r, y, u])
10
        if c.kind in (PI_PLACE, PI_GAINS):
11
            integ += k2 * TS * e

PD 用 x[1] 当“陀螺仪读数”做角速度反馈；c.bw * r - y 就是 I-P 和 PI 的唯一区别；积分在算完 u 之后才更新，这个顺序我会在结果里用到。根轨迹那边，特征多项式就是用 poly_mul、poly_add、poly_scale 拼出来的，和上面的数学一一对应：

1
def locus_poly(pd_zero: bool, k: float) -> list[float]:
2
    """Characteristic polynomial of the dial loop at gain k."""
3
    d = DialPosition()
4
    plant = poly_mul(poly_mul([1.0, 0.0], [d.j, d.b]), [d.tau_c, 1.0])
5
    ctrl = [1.0 / PD_ZONE, 1.0] if pd_zero else [1.0]
6
    return poly_add(plant, poly_scale(ctrl, k * d.kt))

report 里的对比、指标表、断言和画图在驱动的 report 子命令里，下面“结果”一节引用的数字都来自它。

C#

求根函数是 Python 版的逐行翻译。pl_cplx 的乘除由编译器处理，我没有手写。Durand–Kerner 循环：

1
  for (it = 0; it < MAX_ITER; it++) {
2
    bool done = true;
3

4
    for (i = 0; i < deg; i++) {
5
      pl_cplx den = 1.0;
6
      pl_cplx step;
7

8
      for (j = 0; j < deg; j++)
9
        if (j != i)
10
          den *= roots[i] - roots[j];
11
      if (den == 0.0)
12
        continue;
13
      step = pl_poly_eval(c, n, roots[i]) / den;
14
      roots[i] -= step;
15
      if (PL_CABS(step) > DK_TOL * (1.0 + PL_CABS(roots[i])))
16
        done = false;
17
    }
18
    if (done)
19
      break;
20
  }

返回值和出错都是内核风格：n < 2、n > PL_MAX_POLY 或首项为零时返回 -EINVAL，结果写进调用者给的 roots 数组。C 里只有 poly.c 用到复数，别处都是 pl_real。PL_FABS(PL_CREAL(...)) 看着啰嗦，但它是 float 构建（-DPL_USE_FLOAT）下 fabsf、crealf 都匹配的最简单写法，我编译过 float 版确认没有警告（第 10 篇才会真的跑它）。

C++#

PolyRoots 是一个函数模板，N 是系数个数，返回 std::optional<std::array<std::complex<T>, N - 1>>，根的个数在类型里：

1
template <class T, std::size_t N>
2
std::optional<std::array<std::complex<T>, N - 1>> PolyRoots(
3
    const std::array<T, N>& a) {
4
  static_assert(N >= 2, "need degree >= 1");
5
  constexpr std::size_t kDeg = N - 1;
6
  if (a[0] == T(0)) return std::nullopt;
7
  // ... normalize, Cauchy radius, spiral start
8
  for (int it = 0; it < internal::kMaxIter; ++it) {
9
    bool done = true;
10
    for (std::size_t i = 0; i < kDeg; ++i) {
11
      std::complex<T> den(1);
12
      for (std::size_t j = 0; j < kDeg; ++j) {
13
        if (j != i) den *= z[i] - z[j];
14
      }
15
      if (den == std::complex<T>(0)) continue;
16
      const std::complex<T> step = PolyEval(c, z[i]) / den;
17
      z[i] -= step;
18
      if (std::abs(step) > T(internal::kDkTol) * (T(1) + std::abs(z[i]))) {
19
        done = false;
20
      }
21
    }
22
    if (done) break;
23
  }
24
  // ... Newton polish, canonical sort
25
  return z;
26
}

std::complex 的运算符和 C 的 _Complex 行为一样，所以循环体几乎逐字相同。Newton 和比较函数放在 internal 命名空间里，不对外。

Rust#

没有现成的复数，所以先写 complex.rs。四则运算用运算符重载，这样 poly_roots 的循环读起来和 Python 一样。除法用的是教科书公式：

1
impl Div for Complex {
2
    type Output = Complex;
3
    fn div(self, o: Complex) -> Complex {
4
        let den = o.re * o.re + o.im * o.im;
5
        Complex::new(
6
            (self.re * o.re + self.im * o.im) / den,
7
            (self.im * o.re - self.re * o.im) / den,
8
        )
9
    }
10
}

于是 poly_roots 里的核心循环和前面三个版本几乎一模一样：

1
    for _ in 0..MAX_ITER {
2
        let mut done = true;
3
        for i in 0..deg {
4
            let mut den = Complex::ONE;
5
            for j in 0..deg {
6
                if j != i {
7
                    den *= z[i] - z[j];
8
                }
9
            }
10
            if den == Complex::ZERO {
11
                continue;
12
            }
13
            let step = poly_eval(c, z[i]) / den;
14
            z[i] -= step;
15
            if step.abs() > DK_TOL * (1.0 + z[i].abs()) {
16
                done = false;
17
            }
18
        }
19
        if done {
20
            break;
21
        }
22
    }

poly_roots 要求调用者给一个足够长的 &mut [Complex]，这是在没有分配器的情况下把“返回一组根”表达出来的方式；次数不对返回 PolyError::Degenerate，缓冲区或系数个数超限返回 PolyError::Size。

二阶原型：公式和仿真#

云台 PD，$\omega_n = 20$ rad/s，$\zeta$ 取 0.2、0.4、0.7、1：

1
$ make -s ch02
2
... (trimmed)
3
Gimbal PD, wn = 20 rad/s: simulation vs second-order formulas
4
 zeta     Kp      Kd    Mp %   sim %   tp (s)      sim  4/(z wn)   sim ts    rise
5
  0.2   8.00  0.1380   52.66   53.49   0.1603   0.1600     1.000    0.988  0.0598
6
  0.4   8.00  0.2980   25.38   25.75   0.1714   0.1700     0.500    0.419  0.0726
7
  0.7   8.00  0.5380    4.60    4.61   0.2200   0.2180     0.286    0.297  0.1056
8
  1.0   8.00  0.7780    0.00    0.00               n/a     0.200    0.292  0.1674
9
(Kd = 2 zeta wn J - (b + bf), b + bf = 0.022)

左图的实线是仿真，黑色虚线是上一节的闭式 $y_0(t)$，两者几乎叠在一起，右图是对应的极点：$\zeta$ 越小，极点越靠近虚轴，越往上（频率不变，所有极点都在半径为 20 的圆上）。数字上看：$\zeta = 0.2$ 时公式给 52.66%，仿真 53.49%，差 0.83 个百分点；峰值时间 0.1603 s 对 0.1600 s。$\zeta = 0.7$ 时公式 4.60%，仿真 4.61%。差距随 $\zeta$ 变小而变大，而且永远是仿真更大，我认为这是零阶保持的延迟（大约 $t_s/2$）在悄悄削掉一点相位，等效地减小了阻尼。我没有单独验证这个解释，但第 3 篇会把这个延迟变成相位裕度，到时候能直接算。

$\zeta = 1$ 的调节时间（0.292 s）比 $\zeta = 0.7$（0.297 s）还短，不是哪里算错了：$\zeta = 0.7$ 有 4.6% 的超调，要进 2% 的带子得先冲出去再绕回来，临界阻尼从下面单调爬进去，反而更早进带子。$4/(\zeta\omega_n)$ 对 $\zeta = 0.7$ 的预测是 0.286 s（实测 0.297 s），对 $\zeta = 1$ 是 0.200 s（实测 0.292 s），偏得很多：这个公式本来就只是包络线的粗略估计，$\zeta$ 接近 1 时没有振荡包络可言。

极点配置与闭环零点#

速度环 PI 用公式 $K_p = (2\zeta\omega_n J - b)/K_t$、$K_i = \omega_n^2J/K_t$ 配到 $\zeta = 0.7$，每个 $\omega_n$ 各跑一次 PI 和一次 I-P（_ip 后缀）。pi_ch01 和 pi_ch01_ip 是第 1 篇的增益，最后两行是对它的核对：

1
$ make -s ch02
2
... (trimmed)
3
Speed PI vs I-P: closed-loop zero at -Ki/Kp
4
             case      Kp      Ki     wn   zeta    zero  proto %  +zero %   sim %  sat u
5
     pi_place_w10   1.367   10.00  10.00  0.700   -7.32     4.60    20.06   20.29     no
6
  pi_place_w10_ip   1.367   10.00  10.00  0.700   -7.32     4.60    20.06    4.70     no
7
     pi_place_w30   4.167   90.00  30.00  0.700  -21.60     4.60    20.70   21.48    yes
8
  pi_place_w30_ip   4.167   90.00  30.00  0.700  -21.60     4.60    20.70    4.92     no
9
          pi_ch01   1.000   10.00  10.00  0.517  -10.00    15.02    27.94   28.29     no
10
       pi_ch01_ip   1.000   10.00  10.00  0.517  -10.00    15.02    27.94   15.30     no
11

12
             case  rise (s)  settle (s)      IAE   sim %
13
     pi_place_w10    0.0868       0.488   0.4784   20.29
14
  pi_place_w10_ip    0.2111       0.597   0.8070    4.70
15
     pi_place_w30    0.0280       0.161   0.1633   21.48
16
  pi_place_w30_ip    0.0694       0.199   0.2703    4.92
17
 pi_place_w30_lag    0.0267       0.158   0.1678   22.58
18
          pi_ch01    0.0953       0.745   1.2680   28.29
19
       pi_ch01_ip    0.1659       0.791   1.7011   15.30
20
ch01 gains (Kp=1, Ki=10): PI 28.29 %, I-P 15.30 %, prototype formula 15.02 %, with the zero 27.94 %
21
pi_ch01 with the integrator updated before u (ch01's order): 28.02 %

极点配置的 $\zeta$ 是 0.7，不带零点的原型应该给 4.60%。PI 的实测是 20.29%（$\omega_n = 10$）和 21.48%（$\omega_n = 30$），零点公式预测 20.06% 和 20.70%，仿真比公式高 0.23 和 0.78 个百分点，同样是采样保持的方向。零点把 4.6% 变成了 20%，是 4.4 倍。 I-P 的实测是 4.70% 和 4.92%，和原型 4.60% 只差 0.10 和 0.32。第 1 篇的增益也是同一个故事：PI 28.29%，零点公式 27.94%，I-P 15.30%，原型 15.02%。

我把这件事看成一笔交易：I-P 把超调压到原型水平，上升时间从 0.0868 s 变成 0.2111 s（2.4 倍），IAE 从 0.4784 变成 0.8070（1.7 倍）。零点是在拿超调换速度。哪个对，取决于这个速度环后面接什么；如果它是串级里的内环（第 7 篇），超调往往不是问题，慢才是。

债还清了，但有三处要交代。第一，第 1 篇报告的是 28.02%，这里是 28.29%，差 0.27。不是实验不稳：这一篇的驱动在算完 $u$ 之后才更新积分，第 1 篇的 PidBasic 是先更新积分再算 $u$，积分项等于提前了一个采样点。我在 report 里把 pi_ch01 按第 1 篇的顺序重跑了一次，输出最后一行的 28.02% 就是它，和第 1 篇对上了。第二，pi_place_w30 的 $u$ 在第一个采样点就是 $K_p r = 4.167\times 5 = 20.83$ A，超过 20 A 的限幅，表里 sat u 标 yes。限幅只持续了几个采样点，所以 21.48% 里有一小部分不是线性公式能解释的，我没有把它拆出来。第三，pi_place_w30_lag 是同一个控制器接到带 1 ms 电流环的对象上：超调 22.58%，比没有电流环的 21.48% 多 1.1 个百分点，上升时间 0.0267 s。这就是电流环的代价，也是后面几篇都要带着 MotorSpeedLag 的原因。

Routh：临界增益比算出来的低 20%#

拨弹盘 P 控制，增益取 Routh 临界增益的 0.7、0.9、1.1 倍：

1
$ make -s ch02
2
... (trimmed)
3
Routh: K_crit = 16.7333 A/rad, crossing at w = sqrt(b/(J tau_c)) = 31.623 rad/s
4
exact 1 kHz ZOH loop: critical gain 13.3776 A/rad = 0.7995 K_crit
5
      case       K   pole re  ZOH pred  sim slope  max|u|
6
 routh_0.7  11.713   -0.2983   -0.1236    -0.1236    0.59
7
 routh_0.9  15.060   -0.0993   +0.1247    +0.1247    0.95
8
 routh_1.1  18.407   +0.0991   +0.3721    +0.3721    1.86