场地坐标驾驶：让“前进”真正意味着前进

你会在十秒内撞上的问题

搭一台麦克纳姆机器人，拿起手柄，摇杆前推。机器人开走了。好。

现在把它转 180°，再推一次摇杆。机器人朝你开过来，因为“前进”的意思是相对于机器人自己的前方，而它现在正对着你。每个驾驶员的大脑都会做一两秒的镜像换算，然后做错，机器人就撞上东西了。

在差速机器人上这已经够糟了——至少机器人永远沿自己的车头方向移动。在全向底盘上要糟得多，因为现在机器人可以指着一个方向、朝另一个方向移动，而且“横向”最后落在哪一边完全没有视觉线索。转上十秒，屋里没人能开它。

解法是一个旋转矩阵。

两个坐标系，一次变换

摇杆有两种解释方式：

• 车坐标（以机器人为中心、以机器人为参考）。摇杆是机器人自己坐标系里的指令——用 ROS 的话说就是 base_link。前推，机器人沿车头方向开。
• 场地坐标（以场地为中心、以场地为参考）。摇杆是地面坐标系里的指令。前推，机器人向北移动，不管它的车头此刻指在哪。

传动永远只懂车坐标——麦克纳姆运动学就是用它写的。所以场地坐标控制的含义是：在运动学看到之前，把驾驶员的指令旋转到车坐标里。

设机器人航向为 θ（从场地 x 轴逆时针量起），把指令旋转 −θ：

vx_robot = vx_field · cos(θ) + vy_field · sin(θ)
vy_robot = −vx_field · sin(θ) + vy_field · cos(θ)

是负的 theta，不是正的 theta。符号弄错会得到这样一台机器人：朝前指时看着正常，转过 90° 之后开的正好是反方向——这会浪费你恼火的十分钟。

旋转量本身不动。ω 在两个坐标系里是同一个意思——转就是转——所以它直接穿过。

整件事，代码形式

#include <math.h>

// stickX: forward on the stick, positive = away from the driver (field +x)
// stickY: left on the stick,    positive = to the driver's left (field +y)
// stickW: twist, positive = counter-clockwise
void driveFieldOriented(float stickX, float stickY, float stickW) {
  float theta = imuYawRadians() - headingOffset;   // see "the reset" below

  float c = cosf(theta);
  float s = sinf(theta);
  float vx =  stickX * c + stickY * s;
  float vy = -stickX * s + stickY * c;

  driveMecanum(vx, vy, stickW);   // the inverse kinematics, unchanged
}

六行代码，这就是“人能开的车”和“人开不了的车”之间的区别。注意 driveMecanum() 完全不知道这件事——场地朝向是戴在指令上的一顶帽子，而不是对传动的改动。

θ 从哪来

你需要一个偏航角，有三种拿到它的方式，按“能保持诚实多久”递增排列：

从陀螺仪得到偏航是一次积分，所以偏置会线性地变成漂移：

// Integrate yaw rate, with the bias you measured while it sat still.
float rate = gyroZ() - gyroBiasZ;             // rad/s
if (fabsf(rate) < 0.004f) rate = 0.0f;        // deadband: kill the last of the creep
yaw += rate * dt;

有两个细节比传感器选型更重要：

• **每次上电都在机器人静止时测偏置。**在动之前用一两秒平均 500 个采样。一颗不做偏置消除的原始 MPU6050 每分钟漂几十度；做了之后是个位数。
• **死区要加在角率上，不是加在角度上。**给角度加死区会丢掉缓慢的真实转弯。给角率加死区只是扔掉噪声底，而这恰恰就是正在被积分成漂移的东西。

加速度计在这里帮不上忙。它靠看重力给出横滚和俯仰，这正是互补滤波能把倾角修好的原因——但重力与偏航轴平行，所以它对“你面朝哪”一个字也说不出来。偏航要么用磁力计，要么就接受漂移。

归零，这一步不能省

有两件事逼着你做一次手动置零：

1. **漂移。**几分钟后，机器人心里的“北”和驾驶员心里的“北”已经分开了，于是修正本身变成了问题。
2. **启动朝向。**IMU 的零位是上电那一刻机器人指的方向，而驾驶员很少正站在那儿。

所以留一个偏移量和一个按键：

float headingOffset = 0.0f;

void resetFieldHeading() {          // bind to a button the driver can hit blind
  headingOffset = imuYawRadians();  // "whatever you are pointing at is now forward"
}

大家最后都收敛到同一个约定：把机器人指向远离驾驶员的方向，按下按键，开车。这花半秒钟，却一次修好两个问题。每一台认真做的遥操作全向机器人都有这个按键，而每一个跳过它的队伍都在第一场比赛之后补上了它。

自主行为同样需要场地坐标

这不只是开车时的辅助。任何指令天然在地面上表达的自主行为，都想要同一个变换：

• “朝目标移动 300 mm”——目标在场地里，所以指令也在场地里。
• “边绕飞边横移，让相机目标保持居中”——绕飞是场地坐标的，自转是车坐标的，而场地坐标控制正是让你把两者写进同一行的原因。
• 任何位于路径规划器下游的东西——它在地图上规划，因此就在场地坐标里。

在 ROS 里这是 odom → base_link 变换在做同一件事，这也正是 TF2存在的理由，也是 cmd_vel 被明确定义在 base_link 里的理由。场地坐标控制，就是你否则会向 TF2 索取的那个查表的手写版本。

亲眼看见它

麦克纳姆驱动模拟器里有一个动作，不用一个词就能说明这件事：按住摇杆前推，同时持续自转，然后切换坐标系。

同一根摇杆、同一套餐子、同一套运动学。唯一的区别是六行三角函数。

故障排查

下一步

场地朝向让指令变成正确的意思。闭环轮速控制则让轮子真的把它交付出来——在全向底盘上这两件事是一对，因为一台电机下垂不均的机器人，在 IMU 还没来得及说话之前就已经偏离航向了。