上车之前：float32、定点数与 CAN 量化

乘法#

两个 Q16.16 数的乘积有 32 位小数，要右移 16 位才回到 Q16.16。但乘积本身要 64 位：$k_p$ 的 Q16.16 表示是 $18.614\times65536=1.22\times10^6$（21 位），误差 2000 rpm 是 $1.31\times10^8$（27 位），乘起来 48 位，int32 装不下。所以：

加 $2^{15}$ 再向下取整等于四舍五入（平局向 $+\infty$），不加的话每次乘法都向 $-\infty$ 偏半个最低位，积分起来就是一个固定的漂移。右移用算术移位，负数的取整就是向 $-\infty$。加法也要饱和：$p+I$ 在 $k_pe$ 超过 32768 raw 时会溢出 int32。这里 $k_pe=18.6\times2000=37228$ raw，确实会超，饱和成 32768 raw，但它比输出的钳位 16384 大，所以最后决定输出的还是钳位，积分器的抗饱和判断（v 和 u 的大小关系）也不受影响。

Python 的整数是任意精度的，不会溢出，所以参考实现必须主动模拟 int32 的饱和，否则它描述的就不是硬件。python/pidlab/pid_q.py 的基本操作：

1
def sat32(x: int) -> int:
2
    return INT32_MIN if x < INT32_MIN else (INT32_MAX if x > INT32_MAX else x)
3

4

5
def to_q(x: float) -> int:
6
    """Real to Q16.16: round half away from zero, saturate to int32."""
7
    return sat32(int(round_half_away(x * ONE)))
8

9

10
def qmul(a: int, b: int) -> int:
11
    """(a * b) in Q16.16: int64 product, round half up, saturate."""
12
    return sat32((a * b + (1 << (FRAC - 1))) >> FRAC)

to_q 用“四舍五入、平局远离零”（和第 4 篇起所有舍入的约定一样，Python 内置的 round 是银行家舍入，不能用）；而 qmul 里的舍入是“平局向上”，两处不同是有意的：前者是一次性的转换，后者每个采样都发生，必须便宜。

update#

make_config 把三个增益提前换成 Q16.16：$k_p$、$k_it_s$、$k_d/t_s$，之后每个采样里没有除法。update 就是第 5 篇的 CLAMP 规则换成整数：输出被钳位、并且误差把积分往钳位的方向推的时候，积分器不动。

1
    def update(self, r_q: int, y_q: int) -> int:
2
        """One sample; returns the int16 command."""
3
        c = self.cfg
4
        e = sat32(r_q - y_q)
5
        if not self.primed:
6
            self.prev_e = e
7
            self.primed = True
8
        p = qmul(c.kp_q, e)
9
        d = qmul(c.kd_over_ts_q, sat32(e - self.prev_e))
10
        self.prev_e = e
11
        v = sat32(sat32(p + self.integral) + d)
12
        u = min(max(v, c.u_min_q), c.u_max_q)
13
        self.saturated = u != v
14
        if not (self.saturated and (e > 0) == (v > u) and e != 0):
15
            self.integral = sat32(self.integral + qmul(c.ki_ts_q, e))
16
        return (u + (ONE >> 1)) >> FRAC

每个加减法都过一次 sat32，包括 $e=r-y$：两个 int32 相减也能溢出。最后一行把 Q16.16 的输出舍入成 int16 的命令。注意这一步：无论控制器内部用什么格式，C620 收到的命令都是整数，这一步后面会成为 Q16.16 残余误差的来源。

数据和仿真循环#

数据由 Python 生成，原生程序只读，不用任何随机数发生器。三份：ch10_stall 是 2000 rpm 的阶跃加一个从 1 s 开始、每秒 0.9 N·m、到 3.7 N·m 为止的缓升负载（20 s）；ch10_quant 是同样的阶跃加 1.5 s 处 1 N·m 的负载（3 s）；ch10_jitter 的设定值在 1 s 处从 2000 降到 1000 rpm，2 s 处加 2 N·m 的负载，并带有 $w_k$（numpy 种子 10）。write_data 里最关键的两行是阶跃和缓升：

1
    step = lambda t: 2000.0 * (t >= 0.05)  # noqa: E731
2
    # load ramps to 3.7 N*m in 4.1 s, then holds
3
    ramp = lambda t: np.clip(0.9 * (t - 1.0), 0.0, 3.7)  # noqa: E731
4
    write_csv(DATA / "ch10_stall.csv", table(20000, step, ramp))

浮点版本的仿真循环，就是第 5 篇的 Pid、一个带电流环的 MotorSpeedLag、和上面的单位换算。每个采样先把真实转速换算成转子 rpm，需要的话取整成“CAN 反馈”，然后（实测模式下）把 cfg.ts 设成上一个周期，更新，需要的话把命令取整，最后用真实的周期推进对象（RK4，10 个子步）：

1
    for k in range(len(data["t"])):
2
        r, d, w = data["r"][k], data["d"][k], data["w"][k]
3
        ts_k = TS * (1.0 + JITTER * w)
4
        ytrue = x[0] * RPM_PER_RADS
5
        y = round_half_away(ytrue) if case.quant else ytrue
6
        if case.jitter == 2:
7
            pid.cfg.ts = prev
8
        u = pid.update(r, y)
9
        if case.quant or case.quant_u:
10
            u = round_half_away(u)
11
        t = t_act if case.jitter else data["t"][k]
12
        rows.append([t, r, y, u, ytrue, pid.integral, float(pid.saturated)])
13
        x = advance(plant.deriv, x, [u * AMP_PER_RAW, d], ts_k, SUBSTEPS)
14
        t_act += ts_k
15
        prev = ts_k

t 列在抖动算例里是累加的真实时间。quant_u 是只在 Python 里用的一个开关，只取整命令、不取整反馈，后面解释它为什么存在。

Q16.16 的循环只多了两件事：把 r、y 用 to_q 转成 Q16.16 再交给 PidQ，以及输出已经是整数命令，直接乘 AMP_PER_RAW 喂给对象。

C 的循环是同一个形状。pid.cfg.ts = prev 就是“实测周期”：

1
    if (c->jitter == 2)
2
      pid.cfg.ts = prev;
3
    u = pl_pid_update(&pid, row[cr], y, 0, false);
4
    if (c->quant)
5
      u = PL_ROUND(u);

C++ 的版本是函数模板，T 是 double 或 float，RunAll<float>() 用的就是第 5 篇原封不动的 Pid<float>；Rust 的 Real 在 f32 特性下是 f32，同一份源码编译出两个版本，只有定点部分（run_fixed）永远用整数。三个 float32 构建的命令是：

1
make -C c BUILD=../build/c32 EXTRA_CFLAGS=-DPL_USE_FLOAT ONLY=ch10
2
cd rust && cargo build --release --features f32 --bin ch10 --target-dir target32

C++ 不需要单独构建，同一个程序里 RunAll<double>() 和 RunAll<float>() 各跑一遍。文件名里 float32 的是 _f32_c.csv、_f32_cpp.csv、_f32_rs.csv。

增益在 double 里算：三种语言的 raw_gains 都用 double 做算术，输入是对象自己的参数（float32 构建里这些参数本身就是 float32），然后才转成 pl_real、T 或 Real，这样三种语言从同一个数起步。

只在 Python 里存在的一个函数#

原生的 float32 构建里控制器和对象都是 float32，对象自己的状态也会舍入，两件事混在一起。所以我在 Python 里加了 run_f32_controller：控制器是 numpy 的 float32，对象仍是 float64。

1
    kp, ki = (np.float32(v) for v in raw_gains())
2
    ts, lim = np.float32(TS), np.float32(RAW_FULL)
3
    ...
4
        y = np.float32(ytrue)
5
        v = kp * (r - y) + integral
6
        u = min(max(v, -lim), lim)
7
        e = r - y
8
        if not (u != v and e * (v - u) > 0):
9
            integral = integral + ki * ts * e

这一段是第 5 篇 CLAMP 规则的 float32 版，所有算术都在 np.float32 里。后面讲结果时会看到，为什么要把这两件事分开。

对得上吗：Q16.16 逐位一致，float32 也是#

24 个比较文件全部通过。三个 fixed_* 算例的容差是 0，每种语言的每一列，包括 r_q、y_q、u、integral_q、sat，最大差异都是 0（所以三种语言的 Q16.16 和 Python 的整数参考实现逐位一致）。五个 double 算例和 Python 的最大差异也是 0.000e+00，比 $10^{-9}$ 的容差好得多（这五个算例没有三角函数，Rust 的 libm 和 glibc 的差异不会出现）。

float32 的构建没有 Python 的 float32 参考，所以我做了两件事：三种语言的 float32 输出两两比较，以及和 Python 的 double 参考比较。C、C++、Rust 的 float32 输出逐位一致（我把跨语言的容差设成 $10^{-4}$，断言是“不超过 $10^{-4}$”，实际是 0）。

这个“逐位一致”不是一次就到的。第一次跑的时候 C 和另外两种语言差了 0.029（积分项），C++ 和 Rust 之间是 0。原因是 sim.c 里的 RK4 写的是 0.5 * h * k1[i] 和 h / 6.0：字面量是 double，在 float32 构建里整个表达式被悄悄提升成 double 再舍回 float，而 C++ 和 Rust 是纯 float 运算。我把这几个字面量改成了 0.5f、6.0f、2.0f（在 double 构建里它们是精确可表示的，所以 double 的结果不变）。freq.c 里的 PL_CABS(1.0 + cur) 在 float32 构建里同样编译不过，改成 (pl_real)1 + cur。这两处都是前面章节的文件，改完以后我重跑了第 1、3、9 篇，都通过；其余章节我没有重跑，只是论证了改动不会改变 double 的结果：0.5f、6.0f、2.0f 都是精确可表示的，(pl_real)1 在 double 构建里就是 1.0。逐位一致也有前提： x86-64 的基线指令集没有融合乘加，gcc 和 rustc 都不会把 a*b+c 合并成一条指令。如果换到有 FMA 的目标（据我所知 M4F 的 FPU 有融合乘加指令），我猜三种语言的结果会因为编译器的合并策略不同而分开，没有试过。

和 double 参考比，float32 在 stall 上的最大差异：转速 0.014 rpm、命令 0.40 raw、积分项 0.38 raw。quant_on 是个例外：转速差 1.0 rpm、命令差 23 raw，因为取整的分支（round(y) 的 0.5 平局）在 float32 和 double 里翻向了不同的一侧，两条轨迹从那个采样起走开了。这是取整这类不连续操作的通病：最后一位上的差异会被放大成一整个量化格。这里翻转发生在 float32 对 double 之间，不是语言之间，所以三种语言互相比较的时候不需要为这个算例放宽容差。

float32 的积分器停了，停在 1 mrpm#

左图是 $e_{\min}=\text{ulp}(I)/(2k_it_s)$ 的阶梯，三条线是三个 $k_it_s$：这篇用的 0.402、0.01、0.001。每次积分项跨过 2 的幂，误差带翻一倍。黑星是 float32 控制器（对象用 float64）在负载稳定后的实际残余误差。右图是整条回路都在 float32 或 Q16.16 里的积分项和 double 版本的差，大约 5.6 s 之后（负载缓升结束）就冻结了，不再变化。

数字（report 输出里的表）：

• 积分项到 10401 raw，超过 $10^4$，负载在最后 5 s 恒定。
• f64：残余误差 $1.1\times10^{-12}$ rpm，没有停滞。
• f32 ctl（float32 控制器，float64 对象）：残余误差 $1.041\times10^{-3}$ rpm，积分项 10401.31（f64 是 10401.29，差 0.02）。
• 公式 $e_{\min}$ 是 $1.214\times10^{-3}$ rpm，实测是它的 0.86 倍。公式是上限（积分器停在误差带的边缘以内），所以这个比值合理。
• float32 加法二分出来的误差带，和公式在三个 $k_it_s$ 上完全一致：0.4021 对 $1.214\times10^{-3}$，0.01 对 $4.883\times10^{-2}$，0.001 对 $4.883\times10^{-1}$ rpm。

所以结论是：float32 的积分器停滞是真的，大小和半个 ulp 的推导一致，但对这个速度环它是 $10^{-3}$ rpm 的事，在 2000 rpm 上不值得操心。 如果 $k_it_s$ 小两个数量级，误差带就是 0.5 rpm，开始有意义；我没有在 RM 的真实固件里核实 $k_it_s$ 通常是多大，所以“RM 代码里 $k_it_s$ 很小”只是我的猜测，不是结论。

这里有一个我差点说错的地方。原生的 float32 构建（控制器和对象都是 float32）残余误差是 $7.32\times10^{-4}$ rpm，积分项 10401.63，和 double 差 0.34；而 float32 控制器配 float64 对象的积分项只差 0.02。多出来的那 0.32 raw 来自对象：float32 的 w 在 10.9 rad/s 附近的 ulp 是 $9.5\times10^{-7}$，RK4 子步的增量 $h\,\dot w$ 小于一半就丢了，对象自己的状态也在停滞。真实的电机不会这样，所以用 float32 仿真去回答“真车上的 float32 控制器会怎样”会掺进仿真器自己的舍入。我把 f32 ctl 这一行加进了 report 就是为了把它们分开，上面的结论用的是它。

Q16.16 对 float32：不是更准，是误差换了个地方#

负载恒定之后三种格式的残余误差。float32（整条回路）是一条水平线：积分器停了，误差不动。Q16.16 是锯齿：它的最大误差是 $1.129\times10^{-3}$ rpm，比 float32 大，但它不是积分器的问题。Q16.16 的积分项分辨率是 $1.5\times10^{-5}$ raw，比 float32 在 10401 附近的 $9.8\times10^{-4}$ 细 64 倍，积分器根本不会停。锯齿来自最后一步：命令取整成 int16。我在 Python 里把 double 版本的命令也取整成整数（不动反馈，不用定点），残余误差是 $1.124\times10^{-3}$ rpm，和 Q16.16 的 $1.129\times10^{-3}$ 差不到 0.5%。report 里断言了这两个数在 10% 以内。右图是 Q16.16 的命令减去 double 命令，只在 $-0.29$ 和 $+0.71$ raw 之间跳，就是取整的两侧。

所以“定点数更准”是错的，更准确的说法是：定点数的分辨率是常数，float32 的分辨率随数的大小变；这个对象里积分项是大数（1 万以上），定点数的分辨率反而更细，但输出最后要变成 int16，这一步无论用哪种格式都躲不掉。

CAN 量化：不是抖动，是 0.85 秒一个的锯齿#

指标（report 输出）：

• 负载稳定后（2 到 3 s）速度误差的均方根：quant_off $7.96\times10^{-4}$ rpm，quant_on $0.298$ rpm，是 374 倍。
• 这 1 s 里命令变了多少次：quant_off 997 次（每个采样都在微调，峰峰值 0.1 raw），quant_on 只有 4 次（峰峰值 37 raw：2.001 s 和 2.002 s 是负载瞬态的尾巴，2.849 s 和 2.850 s 是下面要说的翻档毛刺和恢复）。

我原本以为量化会让命令抖，实际是反过来的：量化让命令几乎不动，让速度慢慢漂。 图里 quant_on 的误差从 $+0.45$ rpm 一路线性掉到 $-0.50$ rpm，斜率 1.12 rpm/s（我拟合出来是 1.125），到 2.85 s 反馈的整数 rpm 翻了一档，比例项一步把命令拉低了一个 $k_p\times1\text{ rpm}=18.6$ raw（图里的毛刺，3010 对 3029），然后回到 3028，误差跳回去，下一轮锯齿开始。

斜率可以对上：命令是整数 3029，而需要的电流大约对应 3028.5，差半个 raw。半个 raw 的转矩误差是 $0.5\times\frac{20}{16384}\times0.3=1.83\times10^{-4}$ N·m，除以 $J=0.03$ 得 $6.1\times10^{-3}$ rad/s²，乘 183.4 rpm/(rad/s) 得 1.119 rpm/s。拟合出来的 1.125 rpm/s 对上了。这个对象的转子 rpm 量化（1 rpm）和命令量化（半个 raw）合起来产生了一个 0.85 s 周期、$\pm0.5$ rpm 幅度的极限环，在 2000 rpm 上是 0.025%。在这个 $k_p$ 下，它不是抖，是慢速的锯齿；$k_p$ 再大几倍，每次翻档的毛刺就会更明显。这是一个设定值和负载都是常数的算例，我没有扫 $k_p$。

抖动：实测周期只改善了 0.53%#

指标：

• 设定值降到 1000 rpm 之后那 1 s 的 IAE：标称 30.947 rpm·s，实测 30.782 rpm·s，差 0.53%。
• 2 到 3 s（负载 2 N·m 加上以后）：标称 14.796，实测 14.991；也就是说实测并不总是更好，这一段它多了 1.3%。
• 2.5 s 起的稳态里，命令的标准差 0.0335（标称）对 0.0324（实测），误差的标准差 $2.16\times10^{-3}$ 对 $2.18\times10^{-3}$ rpm。

所以前面手算的结论成立：差是一个零均值噪声项，大小取决于误差的大小，设定值跳变的时候最大（图右，最多约 8 rpm 的转速差，占 2000 到 1000 的跳变的 0.8%）。对这个对象，±20% 的周期抖动用不着补偿。 这不是说周期抖动永远不用管：这里的抖动是独立同分布、零均值的，真实的抖动（中断被别的任务延迟）往往有偏、有相关，我没有试。

计时：主机上的数，不是 MCU 上的数#

每个程序做 $10^6$ 次更新，重复 5 次，取中位数（纳秒/次），输入是一张 4096 点的带噪声测量表：

这张图能读出的东西很少。语言之间的差（C++ 比 C 快 2 倍多）大于格式之间的差；我猜原因是 C++ 的 Pid 是头文件里的模板，可以内联进测试循环，而 C 的 pl_pid_update 在另一个编译单元里，没有链接时优化，每次是一个真正的函数调用；这是猜测，我没有看汇编。格式之间的差更没有规律：Q16.16 在 C 里最快，在 Rust 里最慢。在 x86-64 上 float32 和 float64 的标量乘加延迟本来就差不多，所以 float32 没有变快也不奇怪。这些数不能支持“定点比浮点快”或者“float32 比 double 快”。 另外这是 5 次中位数、一台笔记本、没有锁频，噪声本身也不小。

在 M4F 上有一个更有意义的数：目标文件的大小。

1
$ arm-none-eabi-size build/arm/pid.o build/arm/pid_q.o
2
   text     data      bss      dec      hex  filename
3
    556        0        0      556      22c  build/arm/pid.o
4
   1332        0        0     1332      534  build/arm/pid_q.o

float32 的 pid.o 是 556 字节，Q16.16 的 pid_q.o 是 1332 字节，更大。M4F 有硬件的单精度 FPU，浮点乘加是一条指令；定点这边每次乘法要处理 64 位中间值、舍入和饱和，指令更多。脚本最后数到的软件双精度函数（__aeabi_d*）是 0 个，所以 -Wdouble-promotion -Werror 的意义是实打实的：这两个文件里没有任何一处悄悄用了 double。这回答了开头的问题：在有单精度 FPU 的 C 板上，float32 的 Pid 比 Q16.16 版本更小，我没有测它更快还是更慢（那需要真板子）。定点数的价值在没有 FPU 的核（Cortex-M0、M3 一类）上，那些我没有编译过，所以这是一个推测。

没有按直觉发生的事#

• float32 积分器停滞的大小比我预想的小得多。 我预料它会成为这篇最显眼的问题，但在这个速度环上它是 $10^{-3}$ rpm。我没有为了让它更明显去降低 $k_i$，那样就不是我手上这个速度环了；二分表里给了 $k_it_s$ 小一到两个数量级时的数字。
• 原生 float32 构建里对象自己也在舍入。 所以停滞的数字取自 Python 的 f32 ctl（控制器 float32，对象 float64），而不是原生构建的 $7.3\times10^{-4}$。这个函数只在 Python 里存在，原生程序不做这个组合。
• Q16.16 的残余误差来自 int16 命令，不来自定点。 用 double 加整数命令复现了同样的数字（$1.124\times10^{-3}$ 对 $1.129\times10^{-3}$）。这个实验不在原生程序里，只在 report 里，靠 Python 的 quant_u 开关。
• CAN 量化的“抖”不存在，存在的是锯齿。 我预想命令会抖，实测命令几乎不动。我只测了一个 $k_p$。
• 周期抖动的补偿只有 0.53%。 一个算例，一个种子（10）。2 到 3 s 段补偿反而差了 1.3%。
• 三种语言的 float32 逐位一致依赖 x86-64 没有 FMA。 换到有 FMA 的目标，我不保证。

哪些参数我核实过#

M3508 的转矩常数 0.3 N·m/A、减速比 3591/187、空载 482 rpm 和额定 3 N·m 在数据手册里有；C620 的 $\pm16384\leftrightarrow\pm20$ A 和 rpm 反馈在手册里有（镜像文件）；C 板是 STM32F407 来自 C 板手册的框图（PDF 转文字后读到的，图本身我没有看）。没有核实的：数据手册里转矩常数对应哪根轴、反馈转速字段是否有符号、负载惯量 0.03 kg·m²（我取的量级）、电流环时间常数 1 ms（我取的量级）。C 的 pid.o 和 pid_q.o 在 M4F 上的体积是真实的编译结果，但没有在真板子上跑过。

实验清单#

所有文件都在 pid-lab 仓库的 tutorial-v1 标签里。下面的命令都在仓库根目录、nix develop 里运行。

Python：

C、C++、Rust：

脚本和 Makefile：