03.1｜从一轴动力学推到可运行的离散 PID

学习问题：已知当前速度、目标速度与推进器能力，怎样每隔一个采样周期算出不会无限积累的控制需求？本课输入是一个轴上的速度观测与参考，输出是有界加速度命令。先在水平、姿态近似固定、低速的小范围工作点推导，再讨论怎样接回六自由度系统。位置、速度都选 NED 的同一水平轴；在此局部假设下它与选定机体轴重合。

需要补的数学只读这些： 导数、积分与一拍递推。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

先用一个动作理解反馈，再谈三个增益

目标速度 $v_d=0.5\,\mathrm{m/s}$，实际速度 $v=0.3\,\mathrm{m/s}$，误差 $e=v_d-v=0.2\,\mathrm{m/s}$。如果正命令使正方向加速，那么正比例增益给出的正命令会缩小误差，这才叫负反馈。“负”不是说命令总为负，而是动作在抵消偏差。若接线、轴向或电机映射反了，数学上的正增益也可能变成物理上的正反馈。

积分不是“把目标记住”，而是累积误差来形成持久补偿。误差归零时，积分停止增长，不一定归零；保持的非零推力可能正好抵抗水阻。导数也不是预测未来的魔法，而是使用当前变化趋势；差分除以很小的周期会放大测量抖动，所以后面必须推导低通。

偏置与扰动要分开诊断。 海流推动的是实际艇体；速度传感器偏置改变的是控制器看到的读数。若读数恒定偏高 $0.1\,\mathrm{m/s}$，控制器可能把读数调到目标，却让真实速度低于目标。增加积分不能辨别这种情况，需额外测量或校准。控制性能不能只在同一个有偏反馈上自证。

微练习： 目标从 $0.3$ 跳到 $0.5\,\mathrm{m/s}$，真实速度这一瞬间没变。本课对测量求导的项会立即跳变吗？比例项呢？

从力平衡开始，而不是从三个旋钮开始

Fossen 的模型把惯性、科氏、阻尼与恢复力分别组织。沿一个固定轴、忽略轴间项，并把工作点附近阻力线性化，就得到下面的标量模型；有效质量包含附加质量。这里不是把整艇质量直接当作全部惯性，而是对该运动方向建立等效参数。TB03-S1

这一式解决什么。 它给控制器一个“输入改变后速度会怎样变化”的因果对象。$m\dot v$ 是改变动量所需的力，$dv$ 是与速度反向的线性阻力，把阻力放在左侧使 $d>0$ 直接表示耗能；$F$ 是受控输入，$w$ 是未被控制的同轴外力。$m$ 必须正，才能除以它而不改变反馈方向。取 $m=20\,\mathrm{kg}$、$d=10\,\mathrm{kg/s}$、$F=6\,\mathrm N$、$w=2\,\mathrm N$、$v=0.2\,\mathrm{m/s}$，左侧阻力为 $2\,\mathrm N$，故 $\dot v=(6+2-2)/20=0.3\,\mathrm{m/s^2}$。实现里先确定输出究竟是牛顿、加速度还是归一化命令，再选增益单位；它们不能共用一个未定义的 output。

第二行逐项除以正数质量，因此输入变为加速度。若前端输出牛顿，PID增益要相应乘质量；本课始终使用加速度输出。设误差为目标减反馈。比例项在速度偏小时产生正推力；积分项记住长期偏差；导数项根据变化趋势提前制动。为避免目标阶跃产生导数冲击，下面对测量而不是目标求导。

为什么既有积分又有微分。 在常扰动下，纯比例反馈只有留下非零误差才能保持抵消扰动的输入；积分把过去误差累积成输出，即使误差最后为零，$I$ 也能停留在非零平衡值。$K_I e$ 的单位是 $\mathrm{m/s^3}$，因此积分一段时间才得到加速度。导数项的 $K_D$ 无量纲；$D_f$ 不是误差而是速度变化率。$u_{\rm ff}$ 与 $u_c$ 都是加速度，前者描述事先知道的目标运动需求，后者还包含反馈修正。例：$e=0.1$、$K_P=2$、$I=0.05$、$K_D=0.1$、$D_f=0.2$、$u_{\rm ff}=0.03$，各项按 SI 代入，得到 $u_c=0.26\,\mathrm{m/s^2}$。这个逐项加法应成为调试日志的第一张表，而不是只记录总输出。

这里的积分状态本身已经带有输出单位，不是裸的误差积分。所有加项都是加速度，负的测量导数意味着艇还在加速时减少推力。导数滤波器不能只写成“先求差再平滑”而不交代时间：对滤波微分方程用后向欧拉，将当前导数写成相邻测量之差，再移项。

把移项过程写完整。 原方程两边乘 $T$，得到 $\tau_fD_{f,k}-\tau_fD_{f,k-1}+TD_{f,k}=v_k-v_{k-1}$。把两个当前状态合并为 $(\tau_f+T)D_{f,k}$，再除以 $\tau_f+T$，便出现旧状态系数 $\tau_f/(\tau_f+T)$ 和测量差系数 $1/(\tau_f+T)$。这正对应代码 alpha*deriv + (measured-last)/(tf+dt)，不是漏乘了 $1-\alpha$：因为 $(1-\alpha)/T=1/(\tau_f+T)$。量纲也验证了这一点，速度差除以时间才是加速度。后向欧拉把滤波器的当前值放入衰减项，使任意正 $T$ 下该独立滤波状态的极点都落在单位圆内；这不等于完整闭环可以任意慢采样。

由于正时间常数使 $\alpha$ 位于零与一之间，滤波自身稳定。取采样周期 0.02 s、时间常数 0.08 s，$\alpha$ 为 0.8；测量速度从 0.10 到 0.12 m/s，旧导数为零，新导数为 0.2 $\mathrm{m/s^2}$，而未滤波差分为 1 $\mathrm{m/s^2}$。低通减噪同时引入相位滞后，不能把截止频率降得很低却继续沿用原来的高带宽设计。

饱和后，积分器到底应记住什么

如果目标速度很高，实际推进器已到上限，但正误差还在，普通积分仍会不断增大。目标回落后，控制器必须先消耗这笔积累才能反向制动。抗饱和不是删除积分，而是让积分状态跟踪真实可执行输入。Åström 与 Rundqwist 的经典文献给出了回算与条件积分的思路。TB03-S2

为什么回算系数必须带时间单位。 残差 $u_a-u_c$ 已经是加速度，而 $\dot I$ 是加速度每秒，故要除以 $T_t$；若直接把残差相加，就把量纲不同的量混在一起。只看持续饱和且其余输入近似冻结的短时间段，$u_c=I+c$，于是 $\dot I=K_Ie+(u_a-c-I)/T_t$；积分状态向 $u_a-c+T_tK_Ie$ 回归，误差衰减率为 $1/T_t$。显式欧拉离散的该回算子系统极点是 $1-T/T_t$，要求 $0<T/T_t<2$ 才稳定，因此“回算越快越好”并不成立。真正的执行值若晚一拍才知道，应给请求与执行记录同一时间戳再作差，不能拿当前请求减上一拍执行。

上限饱和时实际值小于请求值，回算项为负，恰好阻止正向积累；下限饱和时相反。若把残差写成请求减实际，就会加重饱和。手算：误差 0.4 m/s，比例增益 2 $\mathrm{s^{-1}}$，积分状态 0.7 $\mathrm{m/s^2}$，导数与前馈为零，请求为 1.5，执行上限为 1；积分增益 1 $\mathrm{s^{-2}}$、回算时间 0.5 s、周期 0.1 s，积分增量是 −0.06 $\mathrm{m/s^2}$，新积分为 0.64。普通积分却会增加到 0.74。

判断的是继续积累的方向。 在正增益 $K_I>0$ 下，积分的增长方向与 $e$ 一致：上限饱和加正误差才应冻结；上限饱和但负误差能帮助退饱和，不能冻结。下限情况完全对称。式中“其他情况”还包含没有饱和的正常积分。边界等号要和实现约定一致，可以在恰好等于限值时先允许积分、下一拍再检查，也可以用带容差的方向判断；数值测试应覆盖两种误差符号、两个饱和方向和恰好等于边界。积分绝对限幅只限制 $I$ 自身范围，不能代替这个方向判断。

这是另一种条件积分：仅当误差继续把请求推向更深饱和时冻结；若误差反向，则允许积分退出饱和。它与回算不是同一算法。更关键的是执行反馈的来源：若水平局部限幅后还有深度、姿态和偏航争用推进器，最终实际加速度可能更小，局部 anti-windup 只能处理自己知道的那一层。

最小 C++：每个状态只更新一次

// 教学实现，C++17；不连接硬件。
#include <algorithm>
#include <iostream>
struct PID {
  double kp=2, ki=1, kd=0.1, tf=0.08, tt=0.5, limit=1;
  double integral=0, deriv=0, last=0;
  bool initialized=false;
  double step(double target, double measured, double dt, double ff=0) {
    if(dt<=0) return 0;
    if(!initialized) { last=measured; initialized=true; }
    double alpha=tf/(tf+dt);
    deriv=alpha*deriv+(measured-last)/(tf+dt);
    double error=target-measured;
    double requested=kp*error+integral-kd*deriv+ff;
    double actual=std::clamp(requested,-limit,limit);
    integral+=dt*(ki*error+(actual-requested)/tt);
    last=measured;
    return actual;
  }
};
int main(){
  PID c; double v=0, dt=0.01;
  for(int k=0;k<2000;++k){
    double ref=k<1000 ? 2.0 : 0.0;
    double u=c.step(ref,v,dt);
    v+=dt*(-v+u+0.2);
    if(k%100==0) std::cout<<k*dt<<","<<v<<","<<u<<"\n";
  }
}

保存为 pid.cpp，用 g++ -std=c++17 pid.cpp -o pid 编译。前十秒目标超过该模型稳态可达速度，故持续饱和是预期现象；后十秒观察积分回退及速度回零。每次调用先用旧积分形成请求、再获得同一时刻的饱和输出、最后更新下一时刻积分，这避免代数循环。首次测量仅初始化历史，不能把开机非零速度误当巨大加速度。

读项目时把算法接在正确边界

在项目 AC_PID_Basic::update_all 中沿误差滤波、导数滤波、条件积分、前馈的顺序读；上面的回算教学版用来比较机制，不替代项目的条件积分。报告给出的速度环默认 $P=3$、$I=0.3$、$D=0$、$FF=0.1$、$IMAX=30$，需要与 cm/s 输入、$\mathrm{cm/s^2}$ 输出一起理解。输入输出同时从米变厘米时，P、I、D 的数值可以不变，而输出积分上限必须乘一百。误差滤波与导数滤波是两个不同状态。

手写练习：把上限例改为误差 −0.4、积分 −0.7，计算回算增量。

TB03-S1 · Thor I. Fossen — Fossen’s Marine Craft Model

从六自由度方程与惯性、阻尼、恢复力定义进入本课的一轴局部模型。

TB03-S2 · K. J. Åström, L. Rundqwist (1989) — Integrator Windup and How to Avoid It

重点阅读饱和时积分累积、回算与条件积分；本课数值与代码自行构造。 可配合作者论文公开PDF：https://cse.lab.imtlucca.it/~bemporad/teaching/controllodigitale/pdf/Astrom-ACC89.pdf。

从连续积分到一拍状态转移：明确“现在”和“下一拍”

积分微分式并不能唯一确定程序。对 $\dot I=K_Ie$ 在 $[t_k,t_{k+1}]$ 积分，有 $I_{k+1}-I_k=K_I\int_{t_k}^{t_{k+1}}e(t)\,dt$。如果周期内误差用左端当前采样保持，就得到前向欧拉 $I_{k+1}=I_k+TK_Ie_k$；若用两端平均，得到梯形积分 $I_{k+1}=I_k+TK_I(e_k+e_{k+1})/2$，但此刻尚未测到 $e_{k+1}$，因此在线计算要重排索引，或者等下一拍才更新。不能把连续式里所有点号一律替换成差分，再默认程序没有时序问题。

本课代码选择“输出使用 $I_k$，再更新 $I_{k+1}$”。令 $T=0.1\,\mathrm s$、$K_I=1\,\mathrm{s^{-2}}$、$e_k=0.4\,\mathrm{m/s}$，不饱和时积分增加 $0.04\,\mathrm{m/s^2}$；如果改为先积分再生成输出，当前输出立即多出 $0.04$，等效为另一种离散控制器。两种都可以设计，但参考答案、日志和测试必须统一顺序。读代码时先圈出状态写入行，标注它属于拍首还是拍末，比只搜索 kp 更重要。

推荐的一拍数据流是：读取同一时刻的参考与反馈 → 检查有限值及正采样周期 → 初始化或更新测量导数 → 用旧积分形成请求 → 限幅/分配得到本拍已知执行量 → 计算回算或条件积分 → 保存历史测量与下一拍积分。状态至少包括历史测量、滤波导数、积分值和首次采样标志。改变控制模式时，要决定这些状态是复位还是无扰切换；目标切换但不说明积分初值，会让所谓“同样参数”产生完全不同的瞬态。

如果目标是无扰切换，可从期望切换瞬间输出 $u_{\rm keep}$ 反算积分初值 $I_0=u_{\rm keep}-K_Pe_0+K_DD_{f,0}-u_{{\rm ff},0}$。每项仍为加速度；这个初值只保证切换瞬间请求连续，并不保证接下来的动态稳定。若执行器已经饱和，还应让初值和可达输出一致，避免刚切换就携带一大笔不可执行的积分。

原始代码精读：上游 AC_PID 与本课教学实现的边界

以下是 ArduPilot 项目、ArduSub-4.1.1 标签所对应提交 66c674c1927d5d31401e6b4bc48b8eb3ee7e066e 的原始片段，按 GPL-3.0-or-later 提供；许可证原文。这是公开上游 AC_PID，不是项目报告中的定制 AC_PID_Basic。对照原始实现时，正是这些差别值得解释，而不是把教学代码贴上原函数名字。

AC_PID.cpp 第153–155行：导数计算与低通：

float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);

第一行对已经滤波的误差差分，而本课教学版对测量差分，所以它可能受目标变化影响；不能把它解释成天然的“测量微分”。第二行是增量型低通。若把源码权重记作 $\lambda$，则它等价于 $D_k=(1-\lambda)D_{k-1}+\lambda D_{{\rm raw},k}$。本课式（3.3）中的 $\alpha$ 是旧值权重，因此 $\lambda=1-\alpha$；两个变量都叫 $\alpha$ 并不意味着数值应相等。

AC_PID.cpp 第240–242行：条件积分及积分上限 中的更新原句为：

_integrator += ((float)_error * _ki) * _dt;
_integrator = constrain_float(_integrator, -_kimax, _kimax);

更新之前还有条件：没有限幅标志，或者积分与误差异号时才允许进入这两行。它利用 limit 与当前积分符号限制积分继续增长；和式（3.5）显式检查请求超上界/下界的教学规则存在信息差异。第一行对应矩形积分，第二行限制的是积分输出，不是整个控制输出。这段上游实现不是式（3.4）的回算，因为它没有请求与执行之差除以跟踪时间常数的支路。完整 update_all 还包含目标与误差滤波，以及对比例和微分项的变化率修正；理解片段之后应回到完整函数核对实际数据流。

自己写，而不是复制：一拍 PID 判题任务

打开 C++ 在线练习：PID 单步与状态更新。先读题目约定：该题练的是误差导数低通与条件积分，不是本页采用测量导数与回算的完整仿真。题目的输入直接给误差 $e$，导数项以正号加入输出；本页先输入测量 $v$，再以负号加入测量导数。固定目标时 $\dot e=-\dot v$，二者对应；目标改变时则不等价。把输入参数的单位、状态更新顺序和返回的 $u,I,D$ 写在纸上，再写函数。至少自造四种用例：恒定测量使导数逐渐衰减；固定目标时误差导数与负测量导数等价；目标变化时误差导数会产生冲击；正向饱和且正误差停止继续积累；误差反向时允许退饱和。完整参考代码默认折叠，先用公开用例定位是符号、量纲还是时序问题，再展开比较。

与判题合同核对：同一阶跃，两种导数给出不同结果

假设目标瞬间增加 $0.2\,\mathrm{m/s}$，测量暂时不变，旧滤波导数为零，周期 $T=0.02\,\mathrm s$、滤波时间 $\tau_f=0.08\,\mathrm s$。练习采用误差差分，因此新误差导数为 $0.2/(0.08+0.02)=2\,\mathrm{m/s^2}$；若 $K_D=0.1$，输出立即增加 $0.2\,\mathrm{m/s^2}$。本页采用测量差分，此瞬间导数增量为零。这不是哪个实现计算错，而是两种控制律对目标阶跃的响应不同。实现题目时遵守题目的状态更新、条件积分与误差导数约定；理解工程选择时再比较是否需要避免目标导数冲击。