用车轮编码器实现差速驱动里程计

差速机器人有两个独立驱动的轮子，之间相隔一个轮距。两个轮子走同样的距离，机器人就直行；其中一个走得更远，底盘就沿一条圆弧运动。里程计把这些小运动随时间积分起来以估计位姿：水平位置 x、垂直位置 y、航向 θ。

这个估计是局部的、累积的。它从一个选定的位姿出发，仅靠轮子编码器无法自我纠正。轮径、轮距、时序和打滑带来的小误差会不断累积，所以目标是一份界限被量过的、有用的短期估计——而不是完美的真值。

先定一套坐标约定

写方程之前先选坐标系。一个常见的二维约定是：

• x 从起始位姿朝前。
• y 朝左。
• 正 θ 为逆时针。
• 机器人前进时，左右轮的距离为正。

内部用弧度存角度。只在显示时换算成度。如果有助于调试可以把航向归一化到有界范围，但正弦和余弦对无界角度同样适用。

设 dL 和 dR 为自上次更新以来左右轮走过的距离，b 为两个轮子接地点之间的有效距离。

把计数变成米

上面的一切都假设 dL 和 dR 已经是米。编码器不产生米，它产生计数。两者之间隔着一次换算，弄错了就会让本文后面算出的每个数字都整体缩放。

countsPerWheelTurn = PPR x edgesDecoded x gearRatio
metresPerCount     = (pi x wheelDiameter) / countsPerWheelTurn

只有在你对两个通道的两个边沿都做解码——完整正交解码——时，edgesDecoded 才是 4。只解一个通道的上升沿得到 1，而且还会丢掉方向信息，于是机器人分不出倒车和前进。

最后一行是让人意外的那一行。用扎带把开槽盘固定在轮子上很便宜，但 10 mm 的量化意味着一次 200 mm 的移动只知道到大约 5%，而再多标定也补不回传感器从未采集到的信息。正交编码器教程完整覆盖了解码这一侧。

分辨率是精度的下限，不是精度的承诺。如果轮径错了 2%，73 um 的计数毫无意义。

计算前进和转动

轴心的位移是两个轮子行程的平均值：

dCenter = (dR + dL) / 2

航向按两者的差变化：

dTheta = (dR - dL) / b

行程相等则 dTheta = 0。行程大小相等、方向相反，则 dCenter = 0，机器人绕轴心原地旋转。

对于小步更新，在中点航向上施加平移既准确，又避免了对直线情形写特例：

headingMid = theta + dTheta / 2
x = x + dCenter * cos(headingMid)
y = y + dCenter * sin(headingMid)
theta = theta + dTheta

这种中点积分对嵌入式控制器来说足够简单，而且循环跑得勤时表现良好。精确的圆弧积分对大步长有用，但它修不好错误的轮子测量。

实现一个可重复的更新

保留原始的编码器累计值，把它们的差换算成米。不要每个循环都重置硬件计数器；用快照能让回绕和诊断都更容易。

struct Pose { float x, y, theta; };

void updateOdometry(Pose& pose, float dLeft, float dRight, float trackWidth) {
  const float dCenter = 0.5f * (dLeft + dRight);
  const float dTheta = (dRight - dLeft) / trackWidth;
  const float headingMid = pose.theta + 0.5f * dTheta;

  pose.x += dCenter * cosf(headingMid);
  pose.y += dCenter * sinf(headingMid);
  pose.theta += dTheta;
}

在原子地读完两个编码器之后再调用它。左右快照应当代表几乎同一瞬间；两者之间隔太久，会在快速运动中造出虚假的转向。

如果别的子系统需要速度，把增量除以实测流逝时间即可。位姿积分本身用的是距离，所以循环时间的抖动不会直接改变行驶距离的估计。

读两个编码器而不丢计数

上面那个函数是纯算术。喂对它才是嵌入式里程计真正会坏的地方，而三个细节承担了几乎全部风险。

**在同一个瞬间给两个计数器拍快照。**你的循环读它们的时候，中断处理程序还在继续累加。先读左轮，干一堆活，再读右轮，就会把中间发生的运动算到唯一一个轮子上——而方程会忠实地把它解释成一个转弯。

**永远不要重置硬件计数器。**自由运行的累计值加上记住的上一个值就给出了增量，而且原始计数仍然保留给诊断用。

**用无符号算术计算增量。**C++ 里有符号溢出是未定义行为；下面这套转换写法是定义良好的、不花任何代价，而且跨过回绕点依然正确。

volatile int32_t leftCount = 0, rightCount = 0;   // written only by the ISRs

void isrLeft()  { leftCount  += digitalRead(L_CHAN_B) ? +1 : -1; }
void isrRight() { rightCount += digitalRead(R_CHAN_B) ? +1 : -1; }

constexpr float METRES_PER_COUNT = (3.14159265f * 0.065f) / 2800.0f;   // 72.9 um
constexpr float TRACK_WIDTH      = 0.152f;                             // calibrated, not measured

void odometryTick(Pose& pose) {
  int32_t nowLeft, nowRight;
  noInterrupts();                      // one instant, both wheels
  nowLeft  = leftCount;
  nowRight = rightCount;
  interrupts();

  static int32_t prevLeft = 0, prevRight = 0;
  // Unsigned subtraction wraps in a defined way; the cast back recovers the
  // signed delta even across the 2^32 boundary.
  const int32_t dLeftCounts  = (int32_t)((uint32_t)nowLeft  - (uint32_t)prevLeft);
  const int32_t dRightCounts = (int32_t)((uint32_t)nowRight - (uint32_t)prevRight);
  prevLeft  = nowLeft;
  prevRight = nowRight;

  updateOdometry(pose,
                 dLeftCounts  * METRES_PER_COUNT,
                 dRightCounts * METRES_PER_COUNT,
                 TRACK_WIDTH);
}

每计数 73 um 时，一个有符号 32 位计数器大约要走 157 km 才回绕，所以对一台桌面机器人来说回绕只是理论问题。不过为了在这件事上正确而多花两次转换转换仍然值得，而在 16 位计数器上同样的写法是必需的——它每 2.4 m 就回绕。

手动过一遍转弯

假设左轮走了 0.20 m，右轮走了 0.30 m，轮距为 0.25 m：

• dCenter = 0.25 m
• dTheta = (0.30 - 0.20) / 0.25 = 0.4 rad
• 从航向零算起，中点航向是 0.2 rad
• dx ≈ 0.25 cos(0.2) = 0.245 m
• dy ≈ 0.25 sin(0.2) = 0.050 m

机器人最终落在原前进轴稍偏左的位置，并转过约 22.9°。像这样手算核对一遍，能在代码上硬件之前抓住符号和单位的错误。

标定那两个主导参数

有效轮径控制距离标度。开一段长的、量过的直线，比较报告距离与实际距离，把直径标度乘上 实际 / 报告。如果机器人明明下发相同的指令却走弯，就两侧分别标定。

有效轮距控制旋转标度。下发若干整圈原地旋转，用外部参考量出实际的航向变化量，再调整轮距。多转几圈会放大误差，也让测量变得容易。轮胎擦滑意味着这个有效值可能与用尺子量出的轮子间距不同。

两个参数都保持可配置。地面、轮胎压缩、负载和磨损都会改变它们；硬件模型需要留标定的旋钮。

知道误差长得多快

标定并不能消除误差，它只是把系数缩小。知道剩下的东西有多大，就知道在需要外部手段来修正之前，这份估计还能信多久。

正方形那个数字来自积分一个 1 m 的方形、四次 90 度原地转弯，而除了轮距之外一切都是精确的。机器人最终仍然离起点 8.7 cm。

把最后一行和前两行放在一起读。**积分的数学永远不是你主导的误差。**它比标定项低三个数量级，比打滑低四个数量级——这正是提高循环频率去追精度是个错误的优化方向的原因，也是轮子打滑那一页对你的最终位姿比本文任何内容都更重要的原因。

还要注意系统误差和随机误差的行为完全不同。系统性的擦滑会线性累积，值得标定掉。随机的擦滑则按转弯次数的平方根增长，所以 50 次转弯的代价约为 6 度而不是 45 度——很烦人，但靠调参是修不掉的。

选一个更新频率

两次采样之间的圆弧用中点航向上的一条弦来近似，所以误差取决于每一步的航向变化量。对一个 dTheta 弧度的步长，弦大约短了 dTheta^2 / 24：

选 50 Hz，然后别再想它。把省下来的注意力花在同步两次编码器读取上，这件事在任何频率下都比频率本身更重要：左右快照之间 1 ms 的错位，在两轮速度差 0.5 m/s 时，每一轮都会凭空造出 0.5 mm 的轮子距离差。

加陀螺仪，但别搞混坐标系

陀螺仪给出的短期航向变化，常常比两个轮子编码器之差更好。你可以用陀螺仪航向做 theta，同时仍然用两轮平均距离做平移，平移则施加在旧航向与新航向的平均值上。

这并不能让位置变成绝对的。陀螺仪有偏置，轮子距离仍然会打滑。需要一个全局传感器——相机路标、激光定位，或者另一个外部参考——来修正累积的位姿。

用 ROS 的术语说，一个连续的局部估计天然属于 odom 坐标系。定位系统可以更新 map → odom，而不必让局部平滑的 odom → base_link 变换发生跳变。

把漂移量出来，而不是藏起来

跑三个可重复的测试：

1. 前进再后退，回到起点。
2. 转几圈，回到原来的航向。
3. 走一个大正方形，把终止位姿和起点标记作比较。

记录编码器增量、算出的位姿、电池电压和地面材质。每个测试两个方向都做一遍。稳定的标度误差提示需要标定；随方向变化的误差提示有背隙或不对称；随地面变化的误差提示有打滑。

故障排查

在循迹模拟器里试试转向的后果，再把局部运动估计和 SLAM 基础里的建图概念作比较。对于最直接依赖这份估计的任务——在整个地面上平行地走完一垄垄——见覆盖路径规划和覆盖模拟器。