03｜级联 PID：从位置误差到受限推力

两个不同的问题，用两个时间尺度处理

位置外环决定“希望移动多快”，速度内环决定“为了达到这个速度需要多大力”。直接把位置误差送到推力虽然也能形成反馈，却更难分别处理速度上限、海流扰动和执行器动态。外环常用 P 即可，内环的 I 用来消除恒定扰动下的速度偏差；不是每一层都需要完整 PID。

$${{\text{e}}_{\text{p}}}^{\text{n}}\text{=}{{\text{p}}_{\text{d}}}^{\text{n}}\text{−}{\text{p̂}}^{\text{n}}\text{，}{{\text{v}}_{\text{d}}}^{\text{n}}\text{= limit(}{{\text{v}}_{\text{ff}}}^{\text{n}}\text{+}{\text{K}}_{\text{x}}{{\text{e}}_{\text{p}}}^{\text{n}}\text{)，}{{\text{v}}_{\text{d}}}^{\text{b}}\text{=}{\text{R}}^{\text{T}}{{\text{v}}_{\text{d}}}^{\text{n}}$$

$${\text{e}}_{\text{v}}\text{=}{{\text{v}}_{\text{d}}}^{\text{b}}\text{−}{\text{v̂}}^{\text{b}}\text{，}{\text{τ}}_{\text{raw}}\text{=}{\text{K}}_{\text{v}}{\text{e}}_{\text{v}}\text{+ z −}{\text{K}}_{\text{d}}{\text{D}}_{\text{f}}\text{(}{\text{v̂}}^{\text{b}}\text{) +}{\text{τ}}_{\text{ff}}$$

v_ff 是轨迹本身的期望速度；τ_ff 可补偿期望加速度、已知阻尼或重浮力。反馈负责剩余误差。对测量而非设定值做微分，可避免参考阶跃造成的微分冲击；D_f 表示带滤波的微分，滤波越强也会增加延迟。

为什么限幅以后积分还会出问题

设速度误差持续为正，但推进器已到上限。积分状态仍不断增长，等机器人终于接近目标时，积分还会要求大推力，造成冲过头且恢复很慢。只限制最终输出并不会让积分“知道自己没被执行”。

$$\text{ż =}{\text{K}}_{\text{i}}{\text{e}}_{\text{v}}\text{+}{\text{K}}_{\text{aw}}\text{(}{\text{τ}}_{\text{achieved}}\text{−}{\text{τ}}_{\text{raw}}\text{)}$$

回算项把实际可实现广义力与原始需求的差送回积分器。若分配器削弱了某个轴，就把 Af 与 τ_raw 的差反馈回来，而不只看单台推进器是否触顶。z 按 N 或 N·m 保存时，K_aw 的单位是 1/s；这种量纲检查能抓出很多实现错误。

一步一步做出闭环

先只闭速度 P 环，再加入积分抵抗恒定外力，然后加入抗饱和。速度环行为清楚后再打开位置外环，并限制速度和加速度。最后加入模型前馈。每次只增加一个机制，才能看懂它改变了哪段响应。

算例：m=20 kg、d=8，速度比例增益 K_v=12 N·s/m，则无积分速度闭环时间常数是 m/(d+K_v)=1 s。位置外环 K_x=0.2 s⁻¹ 时，距离目标 1 m 对应 0.2 m/s；若距离是 5 m，仍通过限幅把速度限制在允许范围。临近目标时逐渐减速，比远处一味增大推力更容易控制。

小实验：让积分饱和自己显形

模拟位置从 0 跳到 1 m，速度限制 0.3 m/s、推力限制 ±10 N，并在中途加入恒定 3 N 外力。比较无积分、普通积分、回算积分三条响应。观察的不只是到达时间，还包括饱和解除后积分怎样回到合理值。把积分上限加大并不能解决饱和，反而会让储存的错误更大。