麦克纳姆轮运动学：四个轮子，三个运动轴

一个滚子决定一切

把一个麦克纳姆轮从机器人上拆下来拿在手里，它像普通轮子一样绕轴转。轮缘上装着八九个桶形小滚子，每个有自己的小轴、都能自由转动，且都与轮轴成 45°。

现在问这个轮子能给地面施加什么力。

它不能沿滚子的滚动方向推——滚子一转，什么都传不出去。它只能沿滚子轴线推，也就是滚子拒绝滚动的那个方向。所以轮子的地面作用力被锁在地板上一条 45° 线上，加再大电机扭矩也换不了这条线。

机制的全部就是这个，下面全是记账。

约束，一次一个轮子

用常用机器人坐标系：x 朝前，y 朝机器人左方，ω 逆时针。这是 ROS 约定，用别的以后会和每个库打架。

轮子 i 在该坐标系中的位置为 (xᵢ, yᵢ)，滚子旋向 sᵢ = ±1 表示它的滚子沿两条 45° 线中的哪一条。轮缘速度 vᵢ（轮胎处线速度，单位 m/s，不是弧度）与底盘速度的关系为：

vᵢ = vx + sᵢ·vy + (sᵢ·xᵢ − yᵢ)·ω

三项，每项一句话：

• vx——无论旋向如何，每个轮子都全额贡献前进运动，这就是麦克纳姆底盘前进开起来完全正常的原因。
• sᵢ·vy——侧移让一半轮子正转、一半反转，旋向决定哪一半。
• (sᵢ·xᵢ − yᵢ)·ω——力臂。注意它不只是轮距，滚子旋向在里面，所以装错角的轮子连旋转带侧移一起毁。

从轮缘速度到真正发给电机的指令，除以轮半径：ωwheel = vᵢ / r，单位 rad/s。

四个轮子：矩阵

四个轮子的行叠起来，就是 4×3 矩阵 A，底盘速度 ξ = (vx, vy, ω)：

v = A · ξ

标准布局——左前配右后一种旋向，右前配左后另一种——记 k = lx + ly，其中 lx 为半轴距（前后）、ly 为半轮距（左右），则：

也就是人人都在写的四行：

float fl = vx - vy - K * omega;
float fr = vx + vy + K * omega;
float rl = vx + vy - K * omega;
float rr = vx - vy + K * omega;

用你已知的麦克纳姆机器人四种行为验算一下：

算例

底盘 lx = 75 mm、ly = 85 mm，97 mm 轮（r = 48.5 mm），指令 vx = 0.30 m/s、vy = 0.20 m/s、ω = 0.50 rad/s。则 k = 0.160 m。

看起来不极端的指令，让一个轮子比另一个快 29 倍。麦克纳姆底盘一辈子都在让四个电机干差别巨大的活，所以“四个给一样 PWM 还能走直线”在这里不是有效测试。

饱和：缩放，绝不截幅

指令超出电机能力，总要放弃点什么，只有两种选择，而且不等价。

float peak = max(max(fabs(fl), fabs(fr)), max(fabs(rl), fabs(rr)));
if (peak > V_MAX) {            // SCALE: keep the direction, lose the speed
  float s = V_MAX / peak;
  fl *= s; fr *= s; rl *= s; rr *= s;
}

对比直接给每个电机写 constrain(fl, -255, 255) 的做法：截幅。截幅保住速度、扔掉方向。假设轮子极限 0.50 m/s，指令 0.60 m/s 前进、0.15 侧移、0.8 rad/s 旋转：

最阴险的是截幅在人们常测的指令上看起来没事。纯斜行让两个轮子跑 2、两个跑 0，零本来就在限内，截幅和缩放结果一样，缺陷就藏起来了。它之后会在路径跟随器底下现形：机器人莫名其妙向弯道外偏。在麦克纳姆模拟器同一趟跑里对比两种限幅，差别是一条看得见的曲线。

反过来：里程计

正运动学是同一个矩阵反着读。但 A 是 4×3——四个测量、三个未知数——没有逆，只有最小二乘解：

ξ = (Aᵀ A)⁻¹ Aᵀ v

标准布局下 AᵀA 是对角阵，整体坍缩成三行，Arduino 都放得下：

vx    = (fl + fr + rl + rr) / 4.0;
vy    = (-fl + fr + rl - rr) / 4.0;
omega = (-fl + fr - rl + rr) / (4.0 * K);

每个节拍结合航向积分，就是航位推算：

float dt = (now - last) * 1e-6f;
theta += omega * dt;
x += (vx * cosf(theta) - vy * sinf(theta)) * dt;
y += (vx * sinf(theta) + vy * cosf(theta)) * dt;

效果和底下的假设一样好——轮子不打滑。而麦克纳姆轮打滑是设计使然，所以侧向里程计是全车最不可信的数。

没人用的第四方程

四个方程，三个未知数，多出来的方程不是浪费——是整个传动链上唯一的免费诊断。

机器人是刚体且每轮都抓地时，四个轮缘速度不能任意。矩阵前两行相加与后两行相加都是 2·vx，所以：

FL + FR = RL + RR

恒成立，精确成立，刚体麦克纳姆底盘的任何运动都满足。所以每循环算一次残差：

float residual = (fl + fr) - (rl + rr);   // should be ~0 forever

非零说明四个编码器描述的运动是刚体做不出来的。原因按可能性排序：轮子离地、轮子压到湿滑处、滚子卡死、联轴器打滑、编码器某通道坏——或轮子装错角，此时残差大而恒定，而非阵发。

诚实说出它看不见的：如果四个轮子同时同向打滑，残差保持为零，机器人悄无声息地斜飘出航线。这正是麦克纳姆里程计悄无声息失效而非大声报错的原因，认真的做法是加 IMU 或无动力随动轮，而不是信驱动编码器。

45° 的两个后果

都从矩阵里掉出来，谁也买不掉。

斜行限于 1/√2。 45° 指令把 vx + vy 压在两个轮子上，另两个为 0，忙的那对到限时机器人速度只有直线极速的 V_MAX / √2 ≈ 71%。更大的电机抬高天花板，但去不掉它。

牵引也限于 1/√2，原因不同。 电机有效驱动力 F 沿滚动方向，而实际地面作用力沿滚子轴线为 F·√2，摩擦限制的是那个。所以每轮有效推力上限 μN/√2，整车加速度上限为：

a_max = μ·g / √2

μ = 0.85 时是 5.9 m/s²，同样机器人用普通轮是 8.3。质量在两边都约掉了，所以压重没用——和相扑机器人重量即一切的教训正好相反。

在固件里

几何放一处，绝不手写两遍。

struct Mecanum {
  float lx, ly, r;                 // half wheelbase, half track, wheel radius (m)
  float k() const { return lx + ly; }

  // twist -> four wheel angular velocities, rad/s
  void inverse(float vx, float vy, float w, float out[4]) const {
    out[0] = (vx - vy - k() * w) / r;   // FL
    out[1] = (vx + vy + k() * w) / r;   // FR
    out[2] = (vx + vy - k() * w) / r;   // RL
    out[3] = (vx - vy + k() * w) / r;   // RR
  }

  // four measured wheel angular velocities -> twist, plus the residual
  float forward(const float in[4], float &vx, float &vy, float &w) const {
    float fl = in[0] * r, fr = in[1] * r, rl = in[2] * r, rr = in[3] * r;
    vx = (fl + fr + rl + rr) * 0.25f;
    vy = (-fl + fr + rl - rr) * 0.25f;
    w  = (-fl + fr - rl + rr) * 0.25f / k();
    return (fl + fr) - (rl + rr);       // the free diagnostic
  }
};

lx 和 ly 量轮心到轮心再对半——不是车体板，车体板通常更宽。k 错了机器人照样走，但旋转和平移互相串：原地转会漂移，纯侧移会画弧。原地转还漂移，先怀疑 k 再怀疑代码。

故障排查

下一步

用会打滑的机器人验算这些数：麦克纳姆模拟器把这些方程开进摩擦模型，看里程计和地面怎么吵架。然后让轮子真正转出你算的速度——车轮速度闭环——再用场定向驱动让“前进”指地面前进而非车体前进。