Feldorientierte Fahrt: Vorwärts soll vorwärts heißen

Das Problem, das du in zehn Sekunden hast

Einen Mecanum-Roboter bauen, den Controller aufnehmen, den Steuerknüppel nach vorne drücken. Der Roboter fährt von dir weg. Gut.

Ihn nun 180° drehen und den Knüppel erneut nach vorne drücken. Der Roboter fährt auf dich zu – denn vorwärts heißt vorwärts für den Roboter, und der Roboter zeigt nun auf dich. Jedes Fahrergehirn rechnet die Spiegelbildübersetzung eine, zwei Sekunden um, liegt falsch, und der Roboter trifft etwas.

An einem Differenzialroboter ist das schlimm genug, wo der Roboter sich wenigstens immer entlang seiner eigenen Nase bewegt. An einer holonomen Basis ist es weit schlimmer – denn nun kann der Roboter woandershin zeigen als er fährt, und es gibt gar keinen optischen Hinweis, wohin „seitwärts“ geraten ist. Zehn Sekunden Drehen und niemand im Raum kann ihn fahren.

Die Lösung ist eine Rotationsmatrix.

Zwei Frames, eine Transformation

Den Knüppel gibt es in zwei Deutungen:

• Roboterorientiert (roboterzentrisch, roboterrelativ). Der Knüppel ist ein Befehl im roboteigenen Frame – base_link, wenn man ROS spricht. Nach vorne drücken, der Roboter fährt entlang seiner Nase.
• Feldorientiert (feldzentrisch, feldrelativ). Der Knüppel ist ein Befehl im Frame des Bodens. Nach vorne drücken, der Roboter bewegt sich nach Norden – egal wohin seine Nase gerade zeigt.

Der Antriebsstrang versteht immer nur den Roboterframe – die Mecanum-Kinematik ist darin geschrieben. Feldorientierte Regelung heißt also: Den Fahrbefehl in den Roboterframe drehen, bevor die Kinematik ihn sieht.

Ist der Roboterkurs θ (gegen den Uhrzeigersinn ab der Feld-x-Achse gemessen), den Befehl um −θ drehen:

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

Minus Theta, nicht Theta. Falsches Vorzeichen liefert einen Roboter, der geradeaus zeigend in Ordnung scheint und ab 90° Drehung exakt falschherum fährt – was zehrende zehn Minuten sind.

Drehung bleibt unberührt. ω heißt in beiden Frames dasselbe – ein Drall ist ein Drall –, läuft also direkt durch.

Das Ganze, 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
}

Sechs Zeilen – und der Unterschied zwischen einem Roboter, den Leute fahren können, und einem, den sie nicht können. Beachte: driveMecanum() erfährt davon nie etwas – Feldorientierung ist ein Hut, den man dem Befehl aufsetzt, keine Änderung am Antriebsstrang.

θ bekommen

Man braucht einen Gierwinkel – und es gibt drei Wege dorthin, aufsteigend nach Ehrlichkeitsdauer.

Gier aus einem Gyro ist ein Integral – Bias wird also linear zu Drift:

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

Zwei Details, die mehr zählen als die Sensorwahl:

• Den Bias bei jedem Start messen, bei stillem Roboter. 500 Messwerte über ein, zwei Sekunden mitteln, bevor sich etwas bewegt. Ein roher MPU6050 ohne Biasabzug driftet Dutzende Grad pro Minute; mit Abzug einstellige.
• Ein Totband auf die Rate, nicht auf den Winkel. Totband auf den Winkel verliert langsame echte Drehungen. Totband auf die Rate wirft nur das Rauschfundament weg – exakt das, was sich zu Drift integriert.

Ein Beschleunigungssensor kann hier nicht helfen. Er liefert Roll und Nick per Schwerkraftbeobachtung – weshalb ein Komplementärfilter Neigung wunderschön richtet –, doch Schwerkraft liegt parallel zur Gierachse, sagt also nichts darüber, wohin man zeigt. Gier braucht ein Magnetometer oder muss driften.

Der Reset, der nicht optional ist

Zwei Dinge erzwingen eine manuelle Null:

1. Drift. Nach wenigen Minuten haben sich die Nordvorstellung des Roboters und die des Fahrers getrennt – und die Lösung ist zum Problem geworden.
2. Startorientierung. Die Null der IMU liegt dort, wohin der Roboter beim Einschalten zeigte – selten dort, wo der Fahrer steht.

Also einen Offset und einen Knopf behalten:

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

Die Konvention, auf die alle konvergieren: Den Roboter vom Fahrer weg zeigen lassen, den Knopf drücken, fahren. Dauert eine halbe Sekunde und behebt beide Probleme auf einmal. Jeder ernsthafte teleoperierte holonome Roboter hat diesen Knopf – und jedes Team, das ihn ausließ, rüstete ihn nach dem ersten Match nach.

Feldorientiert auch für Autonomie

Das ist nicht nur eine Fahrhilfe. Jedes autonome Verhalten, dessen Befehl sich natürlicherweise auf dem Boden ausdrückt, will dieselbe Transformation:

• „300 mm zum Ziel bewegen“ – das Ziel liegt in Feldkoordinaten, der Befehl also auch.
• „Seitwärts strafen, um das Kameraziel mittig zu halten, während man es umkreist“ – die Umlaufbahn ist feldframe, der Drall roboterframe, und feldorientierte Regelung schreibt beides in dieselbe Zeile.
• Alles nachgelagert zu einem Pfadplaner, der auf einer Karte plant und daher im Feldframe.

In ROS ist das der odom → base_link-Transform, der denselben Job tut – weshalb es TF2 gibt und weshalb cmd_vel explizit in base_link definiert ist. Feldorientierte Regelung ist die handgerollte Version eines Lookups, den man sonst TF2 fragen würde.

Es sehen

Der Mecanum-Antriebssimulator hat ein einziges Manöver, das den Punkt ohne Worte macht: Den Knüppel vorne halten, weiterdrehen, und den Frame umschalten.

Derselbe Knüppel, dieselben Räder, dieselbe Kinematik. Der einzige Unterschied sind sechs Zeilen Trigonometrie.

Fehlersuche

Weiter

Feldorientierung lässt den Befehl das Richtige heißen. Geregelte Raddrehzahlregelung lässt die Räder es wirklich liefern – an einer holonomen Basis sind beide ein Paar: Denn ein Roboter, dessen Motoren ungleich einbrechen, ist vom Kurs, bevor die IMU mitreden darf.