麦克纳姆驱动模拟器：横移、打滑与漂移

控件

每一个控件在收到输入时都会重放整个机动，因此路径会跟着你拖动的滑块走。没有任何需要点击开始的操作。

左面板是地面视图。虚线是按指令执行的机动，实线是机器人实际走到的位置，点线是它自己的编码器以为它走到的位置。右面板是四个轮缘速度随时间的变化，并标出了电机的上限。一条平贴在那条线上的曲线，意味着那个轮子的电机已经力竭；打滑则体现在左面板上，表现为实线与点线之间的间隙。

原理

麦克纳姆轮只能沿滚子的轴线方向向地面施加力。在其他任何方向上，滚子只会空转，什么都传不出去。这些轴线与轮子成 45°，因此单个轮子的地面力被钉死在地面的某一条直线上，电机扭矩再大也改变不了它。

四条这样的直线、左右手交替排列，其和可以合成任意方向。把轮缘速度写成底盘旋转向量 ξ = (vx, vy, ω) 的函数，每个轮子得到一行：

vᵢ = vx + sᵢ·vy + (sᵢ·xᵢ − yᵢ)·ω        sᵢ = ±1, the roller hand

把这四行堆成一个矩阵 A，整套传动系统就是 v = A·ξ。由此得出三个推论，而实验台存在的意义就是让每一个都看得见：

**四个方程，三个未知数。**轮子全部咬住地面的刚性底盘无法产生任意的轮缘速度——多出来的那个方程恒为 FL + FR = RL + RR。这个校验的残差就是一个免费的打滑检测器，只靠编码器就能算出来。它对非对称打滑会报警——一个轮子悬空、一个轮子踩在湿处、一个轮子装错了角——而四个轮子一起打滑时它完全失明，这正是麦克纳姆里程计会悄悄退化的原因。

**附着力要付出 1/√2 的代价。**电机真正有用的驱动力沿着滚动方向，但实际的地面力是它的 √2 倍，方向则指向滚子轴线——而摩擦对这个矢量整体设了上限。因此单轮极限是 μN/√2，整机的加速度上限是：

a_max = μ·g / √2

在 μ = 0.85 时这是 5.9 m/s²，而同一台车用普通轮子是 8.3 m/s²。质量在两式里都被约掉了，所以加配重不改变任何事——这与推重物的机器人正好相反，在那里重量本身就是全部策略。

**对角线方向同样要付出 1/√2 的代价。**一个 45° 指令把 vx + vy 压到两个轮子上、另两个轮子为零，于是忙碌的那一对达到饱和，而机器人实际上只以 V_MAX / √2 的速度运动。更快的电机能抬高这个上限，但没有任何办法消除它。

完整推导（包含里程计的逆解）在麦克纳姆轮运动学里。

算法

每 5 ms 一步，按以下顺序。末尾的两次位姿积分是刻意分开的——这种分开本身就是里程计要教的一课。

1. 读取这次机动在此刻的摇杆输入。
2. 如果是场地坐标系，旋转到机器人坐标系：vx' = vx·cos θ + vy·sin θ、vy' = −vx·sin θ + vy·cos θ。机器人坐标系则完全跳过这一步。
3. 逆运动学——v = A·ξ，使用固件所相信的那个矩阵，而它永远是正确的那一个。装错的轮子不是固件能知道的事。
4. 限幅——按最吃力的轮子整体缩放这个矢量，或对每个轮子单独限幅。
5. 轮子滞后——每个轮缘以 0.1 s 的一阶响应追赶自己的目标，因为速度环虽好，但并非瞬时。
6. 轮子实际要什么——对轮缘速度做最小二乘正运动学，用的是实际造出来的那台机器人的矩阵。
7. 解出让底盘吻合所需的力，然后依次施加两个上限：电机的上限（扭矩本来就只有那么多）和地面的上限（每轮 μN/√2）。
8. **地面拒绝掉的那部分，改为让轮缘空转。**这份多余的力会把轮子推离它被指令的速度，这就是驱动轮打滑，也是编码器唯一能报告的打滑症状。
9. 用实际施加的力积分出真实位姿。
10. 从轮缘速度出发、经过固件的矩阵积分出里程计——因为编码器是装在轮子上的，不是装在地上的。

这里面没有任何环节去寻找打滑或插入打滑。打滑就是第 7 步拒绝了某个力，而你看到的漂移则是第 10 步始终没有发现这件事。

引导实验

每一个实验只隔离一个变量。按顺序运行，它们会层层递进。

1. 你买麦克纳姆轮是为了这件事

加载能闭合的正方形。机器人向前开一米、向右开一米、向后、向左，回到距离起点 17 mm 的地方，而航向读数始终没有离开 0.0°。现在把机动切换到拐角转弯的正方形——同一个正方形，只是用差速驱动机器人不得不采用的方式来开。它也能闭合，误差是 4 mm。记住：这两种方式在这里都能用。

**观察要点：**横移正方形在两个轴上平移，而为此完全没有花掉任何航向。任何差速驱动都做不到这一点。

2. 把地面拿走

保持横移正方形，把地面附着从 0.85 拖到 0.25。然后对转弯正方形做同样的事。

**观察要点：**要看航向，不要看距离。两台车都在滑，也都没能回到原点；横移那台虽然偏了，指向却仍与起点完全一致，因为它从头到尾就没有指令过旋转。转弯那台则先丢掉航向，然后朝错误的方向走下一段，小小的打滑就这样变成了大大的误差。每一次旋转都是一次重新走失的机会，而全向底盘压根不给自己这种机会。

这个实验里的距离在不同设置下每次跑都会有出入，这是诚实的答案而不是 bug：一个四轮都在滑的机器人是一个粘滑系统，不会两次落在同一个位置。航向则没有这个问题。

3. 看着编码器说谎

加载同样的正方形、油腻的地面，看左面板上的两条线。点线（里程计）画出一个规整的正方形，实线却没有。机器人最终离原点 225 mm，而它自己的编码器却把它放在别处 133 mm 的地方——这期间有 1.27 m 的地面从轮子底下过了一遍，机器人却根本没动。

现在盯着编码器分歧这个读数。它全程都停在 0.000。

**观察要点：**这个免费的残差校验——在任何刚性底盘上 FL + FR 都应等于 RL + RR——能抓到一个轮子打滑，却对四个轮子一起打滑完全失明。这种失明正是麦克纳姆里程计悄悄失效的原因。给它装上 IMU，或者加死轮，别再相信驱动编码器给出的位置。

4. √2 的天花板

加载对角冲刺。摇杆速度是 0.60 m/s，轮子最高转速也是 0.60 m/s，所以这个指令看起来应该做得到。事实并非如此：整段运行中有 91% 的时间指令都被限幅，机器人的最高速度只有 0.424 m/s——这正是 0.60/√2 取到小数点后三位，精确吻合。

**观察要点：**右面板上有两条曲线平贴在零上，另两条则被钉在上限。一个 45° 指令把 vx + vy 压到两个轮子上、另两个轮子为零，于是忙碌的那一对在 V_MAX / √2 上饱和。提高轮子最高转速，上限会跟着一起抬高——而那个 71% 永远不会变。

5. 整体缩放与逐轮限幅

把机动切换到边转边走，把摇杆旋转推到 240 °/s，再把超出限值时在两种设置之间来回切换。路径的形状会变。

**观察要点：**整体缩放会放弃每个轴上的速度，但保住了方向。逐轮限幅保住了前进速度，却丢掉了 27% 的旋转量——机器人整段只转过 92°，而不是 127°——所以它的弯曲方式和被要求的并不一样。再回到对角冲刺，试同样的开关：什么都不会发生，因为空闲的轮子本来就在限值以内。这个 bug 之所以能活过测试，就是这个原因。

6. 把一个轮子装反

加载一个轮子装反了，或者在图上按 W。代码一个字都没改。

**观察要点：**机器人做的已经完全不是被指令的事了，而这一次残差读数确实会报警——又大又稳，而不是一阵一阵的。留意哪一列有确切数字、哪一列没有：当一个轮子与另外三个较劲时，机器人是在持续滑动的，所以它的最终位姿是混乱的。残差是唯一存活下来的数值，这正是它值得放上遥测行的原因。这是在没有 IMU、也没有外部参考的真实机器人上唯一读得出的症状。也看看画出来的机器人：被镜像那个角的剖面线方向反了，四条滚子线也不再构成一个 X。

7. 一键切换到场地坐标系

加载边转边走时的场地坐标系，然后按 O 切回机器人坐标系，再按 O 切回来。

**观察要点：**在机器人坐标系下，摇杆保持前推的同时旋转，画出来的是一朵玫瑰线：机器人走了 2.34 m 的路径，终点离起点 0.30 m——13% 的力气花在了有意义的地方。在场地坐标系下，同样的摇杆走 2.03 m，终点也正好在 2.03 m 之外：一条笔直的直线，100%，而机身全程都在自转。分开这两者的只是六行三角函数，但它决定的是一台人能开的机器人和一台没人能开的机器人之间的差别。

你应该观察到什么

把以上七点合在一起看：

• **前进是免费的；横移和对角不是。**每个轮子都全额贡献于前进运动，而对角运动只用到其中一半。
• **两个根号二的代价，来自同一个 45°。**一个是对角方向的速度，另一个是所有方向上的附着力。两者都换不来更大的电机。
• **质量会被约掉。**不管机器人多重，加速度上限都是 μg/√2，这跟推重物的机器人正好相反。
• **地面差的时候，全向胜过差速，**因为误差正是从那些旋转里来的。
• **里程计就是代价。**它不是调参问题，也不是换个更好的编码器的问题——轮子打滑本来就是这套传动系统的工作方式。

硬件落地

这个实验台值这二十分钟，主要是为了实验 6。在这里把轮子装反不花任何代价，在台架上却要花掉半个工作日，而且这是你最可能亲手造出来的那个故障。

源代码

整套传动系统就是这两个函数。几何参数只存在于一个地方。

const float LX = 0.075f;      // half the wheelbase, m (wheel centre to wheel centre / 2)
const float LY = 0.085f;      // half the track, m
const float K  = LX + LY;
const float R  = 0.0485f;     // wheel radius, m
const float V_MAX = 0.60f;    // rim speed the motors can actually hold, m/s

// Inverse kinematics + saturation. vx forward, vy LEFT, omega counter-clockwise.
void driveMecanum(float vx, float vy, float omega) {
  float w[4] = {
    vx - vy - K * omega,      // front left
    vx + vy + K * omega,      // front right
    vx + vy - K * omega,      // rear left
    vx - vy + K * omega,      // rear right
  };

  float peak = 0;
  for (int i = 0; i < 4; i++) peak = max(peak, fabsf(w[i]));
  if (peak > V_MAX) {                    // SCALE, never clip
    float s = V_MAX / peak;
    for (int i = 0; i < 4; i++) w[i] *= s;
  }

  for (int i = 0; i < 4; i++) wheelLoop[i].target = w[i];   // hand to the velocity loops
}

// Forward kinematics for odometry, and the free diagnostic.
float updateOdometry(const float rim[4], float dt) {
  float vx    = ( rim[0] + rim[1] + rim[2] + rim[3]) * 0.25f;
  float vy    = (-rim[0] + rim[1] + rim[2] - rim[3]) * 0.25f;
  float omega = (-rim[0] + rim[1] - rim[2] + rim[3]) * 0.25f / K;

  theta += omega * dt;
  poseX += (vx * cosf(theta) - vy * sinf(theta)) * dt;
  poseY += (vx * sinf(theta) + vy * cosf(theta)) * dt;

  // Four equations, three unknowns. On any rigid, gripping chassis this is zero.
  // Large and steady => a wheel is in the wrong corner. Bursty => something slipped.
  return (rim[0] + rim[1]) - (rim[2] + rim[3]);
}

那四行的推导，以及残差为什么有效，都在麦克纳姆轮运动学里。让 wheelLoop[i].target 真正有意义，靠的是闭环轮速控制。至于把这东西造出来，那就是麦克纳姆全向机器人。