Field-Oriented Drive: Make Forward Mean Forward

The problem you will have within ten seconds

Build a mecanum robot, pick up the controller, push the stick forward. The robot drives away from you. Good.

Now spin it 180° and push the stick forward again. The robot drives at you, because forward means forward to the robot and the robot is now facing you. Every driver’s brain does the mirror-image translation for a second or two, gets it wrong, and the robot hits something.

That is bad enough on a differential-drive robot, where at least the robot always moves along its own nose. On a holonomic base it is much worse, because now the robot can be pointing one way and moving another, and there is no visual cue at all for which way “sideways” has ended up. Ten seconds of spinning and nobody in the room can drive it.

The fix is one rotation matrix.

Two frames, one transform

There are two ways to interpret the stick:

• Robot-oriented (robot-centric, robot-relative). The stick is a command in the robot’s own frame — base_link, if you speak ROS. Push forward, the robot drives along its nose.
• Field-oriented (field-centric, field-relative). The stick is a command in the floor’s frame. Push forward, the robot moves north, whichever way its nose happens to point.

The drivetrain only ever understands the robot frame — the mecanum kinematics are written in it. So field-oriented control means rotating the driver’s command into the robot frame before the kinematics see it.

If the robot’s heading is θ (measured counter-clockwise from the field’s x axis), rotate the command by −θ:

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

Minus theta, not theta. Getting the sign wrong gives a robot that seems fine while pointing forward and drives in exactly the wrong direction once it has turned 90°, which is a maddening ten minutes.

Rotation is not touched. ω means the same thing in both frames — a spin is a spin — so it passes straight through.

The whole thing, in code

#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
}

Six lines, and it is the difference between a robot people can drive and one they cannot. Note that driveMecanum() never learns about any of this — field orientation is a hat you put on the command, not a change to the drivetrain.

Getting θ

You need a yaw angle, and there are three ways to get one, in increasing order of how long they stay honest.

Yaw from a gyro is an integral, so bias becomes drift linearly:

// 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;

Two details that matter more than the sensor choice:

• Measure the bias every boot, with the robot still. Average 500 samples over a second or two before anything moves. A raw MPU6050 with no bias removal drifts tens of degrees a minute; with it, single digits.
• A deadband on the rate, not on the angle. Deadbanding the angle loses slow real turns. Deadbanding the rate only throws away the noise floor, which is exactly what is integrating into drift.

An accelerometer cannot help here. It gives you roll and pitch by watching gravity, which is why a complementary filter fixes tilt beautifully — but gravity is parallel to the yaw axis, so it says nothing about which way you are facing. Yaw needs a magnetometer or it needs to drift.

The reset, which is not optional

Two things force a manual zero:

1. Drift. After a few minutes the robot’s idea of north and the driver’s have separated, and the fix has become the problem.
2. Startup orientation. The IMU’s zero is wherever the robot was pointing when it powered on, which is rarely where the driver is standing.

So keep an offset and a button:

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"
}

The convention everyone converges on: point the robot away from the driver, press the button, drive. It takes half a second and it fixes both problems at once. Every serious teleoperated holonomic robot has this button, and every team that skipped it added it after the first match.

Field-oriented for autonomy, too

This is not only a driving aid. Any autonomous behaviour whose command is naturally expressed on the floor wants the same transform:

• “Move 300 mm toward the goal” — the goal is in field coordinates, so the command is.
• “Strafe to keep the camera target centred while orbiting it” — the orbit is field-frame, the spin is robot-frame, and field-oriented control is what lets you write both in the same line.
• Anything downstream of a path planner, which plans on a map and therefore in the field frame.

In ROS this is the odom → base_link transform doing the same job, which is why TF2 exists and why cmd_vel is explicitly defined in base_link. Field-oriented control is the hand-rolled version of a lookup you would otherwise ask TF2 for.

Seeing it

The mecanum drive simulator has a single manoeuvre that makes the point without any words: hold the stick forward, keep spinning, and switch the frame.

Same stick, same wheels, same kinematics. The only difference is six lines of trigonometry.

Troubleshooting

Next

Field orientation makes the command mean the right thing. Closed-loop wheel velocity control makes the wheels actually deliver it — on a holonomic base the two are a pair, because a robot whose motors droop unequally is off heading before the IMU gets a say.