用编码器实现车轮速度的闭环控制

PWM 是一个电压，不是一个速度

analogWrite(motorPin, 180) 并没有在要一个速度。它要的是大约 70% 的电池电压，而那个电压产生出什么速度，取决于轮子上的负载、那个减速箱里的摩擦、剩余电量，以及电刷的温度。

这就是为什么一台两个电机相同、PWM 相同的机器人会走弧线。它们从来就不相同——一个减速箱更紧，一副电刷更新，同样电压下它们会差 3–8%。三米下来就是一个看得见的弯。

在差速机器人上你可以用一个微调常数把它糊过去。在麦克纳姆底盘上你不能，因为逆运动学交给四个轮子的目标天差地别——在那边那个算例里，同一条指令给一个轮子 0.020 m/s、给另一个 0.580 m/s——而决定机器人往哪去的正是这些比例。一个慢了 5% 的轮子不会让机器人稍微慢一点；它会让机器人去别的地方。

在每个轮子实测的速度上闭合这个环，就是修掉这件事的办法，而且对任何轮子多于两个的机器人、或者任何需要准确到达的场合，都是同一个修法。

量出你真正拥有的速度

从正交编码器计数开始。换算是固定的几何：

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

对 97 mm 轮子配 12 CPR 电机编码器和 30:1 减速箱：12 × 30 × 4 = 每转 1440 个计数，而 2π × 0.0485 / 1440 = 每计数 212 µm。

然后是人人都会搞错的那部分：

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

在 100 Hz 和 0.05 m/s 下，那是每个采样一个计数。你的速度信号是 0、212、0、212 µm——一个 ±100% 纹波的方波，而 D 项会把它放大成一台嗡嗡响的电机。三条出路，一般你最想要的是第一条：

1. **采样比控制慢。**在 20 ms（50 Hz）上算速度，环也按 50 Hz 跑。每个窗口的计数更多、量化更少，而且没有滤波延迟。
2. 测边沿之间的时间，而不是一段时间里的边沿数。低速时极好，高速时别扭，而且需要中断时间戳。
3. **对速度低通。**最便宜，而它的代价是相位裕量——那恰恰是你最缺的东西。

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount 和那对 noInterrupts() 括号不是可选项：在 AVR 上一次 32 位读是好几条指令，而一个 ISR 落在它中间，就会交给你一个从来没成立过的数。

先前馈，后 PID

本能反应是伸手去拿 PID。先把那容易的 90% 做完。

直流电机的稳态速度与所加电压接近线性，所以如果你知道想要的速度，就能算出大部分 PWM：

pwm ≈ kS·sign(v) + kV·v

• kV——斜率。买 1 m/s 要多少 PWM。
• kS——截距。轮子还没动之前，仅为了克服静摩擦就需要的那份 PWM。

十分钟量出两个量：

1. 把机器人垫高，让轮子空转。
2. 慢慢提高 PWM，直到轮子刚刚开始转。那就是 kS，一般是 255 之里的 25–45。
3. 把 PWM 一档一档提到 100、150、200、250，每档记下稳态速度。
4. 做一次直线拟合。斜率是 kV，而且它外推回去应该大致落在你的 kS 上。

现在这个环只需要纠正那个估计与现实之间的误差——误差很小，所以增益可以很小，所以整件事又稳又安静：

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

没有前馈，积分器每次目标变化时都得从零开始建立整条指令。那很慢、会超调，也是那么多手工调过的电机环最后都配上一个巨大的 kI、机器人一顿猛冲的原因。

控制器

用 P 和 I。不用 D。

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

为什么不用微分：D 项要对速度信号求导，而速度信号本身已经是编码器计数的一次差。你等于求导两次，也就是把量化噪声放大两次，而且轮子上几乎根本没有东西可供它阻尼。一个带 PI 和良好前馈的速度环是标准答案，而加上 D 是把一个本来好用的环弄糟的更常见做法之一。（位置环不一样——见级联控制。）

**抗饱和是那一段你跳不过去的东西。**把机器人按在墙上、同时施加一个速度指令，天真的积分器会在几秒内冲到满量程。一松手机器人就窜出去。条件积分，一个 if，就修好了。

按正确的顺序整定

1. **只用前馈。**kP = kI = 0。下发 0.3 m/s，量一量你得到多少。落在约 10% 以内说明 kV 是对的。在碰任何别的东西之前先把它修好。
2. **加 kP 直到嗡嗡响，然后退回三分之一。**你在听一种随增益出现、随增益消失的啸叫。
3. **加 kI 直到稳态误差消失。**前馈正确的轮子需要的量很少。从每秒 kP/10 起步，慢慢加。
4. 在负载下检查，而不是在垫高的架子上。整件事的意义就在于那个被要求得更多的轮子。
5. 四个一起测，开一条对角线。那是轮子目标差距最大的指令，也是调不足的轮子最先露出来的地方。

对第 2、3 步建立直觉，PID 模拟器比机器人快得多；而调参指南里有通用流程。

采样率

环路频率必须远高于它要控制的那个东西。一台小型减速直流电机的速度以大约 50–150 ms 的时间常数稳定；那个时间常数里取十个采样是常规规则，所以是50–200 Hz。

把它跑在固定调度上，而不是“loop() 有多快就多快”，因为 dt 在速度估计和积分器里都直接出现：

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

那个带符号比较就是 millis() 回绕陷阱在 micros() 形式下的同一件事，只不过它咬人的时间是 71 分钟而不是 49 天。

买到什么，实测数字

同一台机器人、同一条指令、同一地面：

最后一行是被低估的那一行。闭环知道自己什么时候失效了：一个输出被钉在 255、却仍然够不到目标的轮子正在报告一件有用的事，你可以把它放到遥测线上、拒绝那条指令，或者把整台机器人慢下来以保持比例诚实。

故障排查

下一步

轮子现在能交付被要求的那个速度了。用场地坐标驾驶把整车指向驾驶员真正想去的地方，用运动曲线喂它平滑的目标而不是阶跃。然后在麦克纳姆驱动模拟器里看看速度环能救什么、救不了什么——一个被完美保持住的轮速，在地面松手时仍然毫无意义。