Veldgeoriënteerde regeling: laat vooruit ook vooruit zijn

Het probleem dat je binnen tien seconden hebt

Bouw een mecanum-robot, pak de besturing, duw de stick naar voren. De robot rijdt van je weg. Goed.

Draai hem nu 180° en duw de stick opnieuw naar voren. De robot rijdt op je af, want vooruit betekent vooruit voor de robot, en de robot kijkt nu naar jou. Het brein van elke bestuurder doet die spiegelbeeldvertaling een seconde of twee, doet het verkeerd, en de robot raakt iets.

Op een robot met differentiële aandrijving is dat al erg genoeg, want daar beweegt de robot in elk geval langs zijn eigen neus. Op een holonome basis is het veel erger, want nu kan de robot de ene kant op wijzen en de andere kant op bewegen, en er is helemaal geen visueel teken dat aangeeft welke kant “zijwaarts” geworden is. Tien seconden draaien en niemand in de zaal kan hem besturen.

De oplossing is één rotatiematrix.

Twee frames, één transformatie

Er zijn twee manieren om de stick te interpreteren:

• Robotgeoriënteerd (robot-centric, robot-relatief). De stick is een commando in het eigen frame van de robot — base_link, als je ROS spreekt. Duw naar voren, de robot rijdt langs zijn neus.
• Veldgeoriënteerd (field-centric, veldrelatief). De stick is een commando in het frame van de vloer. Duw naar voren, de robot beweegt noord, hoe zijn neus ook toevallig staat.

De aandrijving begrijpt uitsluitend het robotframe — de mecanum-kinematica is daarin geschreven. Veldgeoriënteerde regeling betekent dus dat je het commando van de bestuurder in het robotframe roteert voordat de kinematica het ziet.

Is de richting van de robot θ (gemeten tegen de klok in vanaf de x-as van het veld), draai het commando dan met −θ:

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

Minus theta, niet theta. Met het verkeerde teken krijg je een robot die prima lijkt te doen zolang hij vooruit wijst, en precies de verkeerde kant op rijdt zodra hij 90° gedraaid heeft — wat tien minuten frustrerend is.

Rotatie wordt niet aangeraakt. ω betekent in beide frames hetzelfde — een draai is een draai — dus die gaat er onveranderd doorheen.

Het hele verhaal, 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
}

Zes regels, en dat is het verschil tussen een robot die mensen kunnen besturen en een die ze niet kunnen. Merk op dat driveMecanum() hier nooit iets van merkt — veldoriëntatie is een pet die je het commando opzet, geen wijziging van de aandrijving.

θ verkrijgen

Je hebt een yaw-hoek nodig, en er zijn drie manieren om die te krijgen, in oplopende volgorde van hoe lang ze betrouwbaar blijven.

Yaw uit een gyroscoop is een integraal, dus wordt bias lineaire 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;

Twee details die belangrijker zijn dan de keuze van de sensor:

• Meet de bias bij elke opstart, met de robot stilstaand. Middel 500 samples over een seconde of twee voordat er iets beweegt. Een ruwe MPU6050 zonder biascorrectie drijft tientallen graden per minuut; met correctie enkele graden.
• Een dode band op de snelheid, niet op de hoek. Een dode band op de hoek kost je langzame, echte draaibewegingen. Een dode band op de snelheid gooit alleen de ruisvloer weg, en dat is precies wat in drift wordt geïntegreerd.

Een accelerometer kan hier niet mee helpen. Die levert rol en pitch door naar de zwaartekracht te kijken, en daarom repareert een complementair filter de tilt zo mooi — maar de zwaartekracht staat evenwijdig aan de yaw-as, dus zegt hij niets over welke kant je op kijkt. Yaw heeft een magnetometer nodig, of het gaat driften.

De reset, die niet optioneel is

Twee dingen dwingen een handmatige nulstelling af:

1. Drift. Na een paar minuten zijn het idee van noord van de robot en dat van de bestuurder uit elkaar gedreven, en is het herstellen het probleem geworden.
2. Startrichting. Het nulpunt van de IMU is waar de robot naartoe wees toen hij aanging, en dat is zelden waar de bestuurder staat.

Hou dus een offset en een knop aan:

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

De afspraak waar iedereen op uitkomt: richt de robot van de bestuurder af, druk op de knop, rij. Dat kost een halve seconde en lost beide problemen tegelijk op. Elke serieuze op afstand bestuurde holonome robot heeft deze knop, en elk team dat hem oversloeg voegde hem toe na de eerste wedstrijd.

Veldgeoriënteerd werkt ook voor autonomie

Dit is niet alleen een rijhulpmiddel. Elk autonoom gedrag waarvan het commando vanzelf op de vloer wordt uitgedrukt wil dezelfde transformatie:

• “Beweeg 300 mm naar het doel toe” — het doel ligt in veldcoördinaten, dus het commando ook.
• “Strafe om het cameradoelwit gecentreerd te houden terwijl je eromheen cirkelt” — de cirkelbeweging is in het veldframe, de draai in het robotframe, en veldgeoriënteerde regeling is wat je in één regel kunt opschrijven.
• Alles wat volgt op een padplanner, die op een kaart plant en dus in het veldframe.

In ROS is dat de transform odom → base_link die hetzelfde werk doet, vandaar dat TF2 bestaat en dat cmd_vel expliciet in base_link is gedefinieerd. Veldgeoriënteerde regeling is de met de hand gebouwde versie van een opzoeking die je anders aan TF2 zou vragen.

Het zien gebeuren

De mecanum drive simulator heeft één manoeuvre dat het punt zonder woorden maakt: houd de stick naar voren, blijf draaien, en wissel van frame.

Zelfde stick, zelfde wielen, zelfde kinematica. Het enige verschil is zes regels trigonometrie.

Storingzoeken

Volgende stap

Veldoriëntatie zorgt dat het commando het juiste betekent. Gesloten-lusregeling van de wielsnelheid zorgt dat de wielen het ook echt leveren — op een holonome basis horen die twee bij elkaar, want een robot waarvan de motoren ongelijk inzakken is al van koers voordat de IMU er iets over kan zeggen.