Mecanum-wielkinematica: vier wielen, drie assen

Eén rol beslist alles

Haal één mecanum-wiel van de robot af en houd het vast. Het draait om zijn as net als elk ander wiel. Om de rand zitten acht of negen tonvormige rollen, elk op zijn eigen kleine as, elk vrij draaiend, en elk op 45° ten opzichte van de as van het wiel.

Vraag nu welke kracht dat wiel in de vloer kan duwen.

Het kan niet langs de rolrichting duwen—de rol draait gewoon weg en er wordt niets overgedragen. Het kan alleen langs de rolas duwen, de ene richting waarin de rol weigert te rollen. De grondkracht van het wiel zit dus vast aan één lijn op de vloer, op 45° ten opzichte van het chassis, en geen hoeveelheid motorkoppel verandert welke lijn dat is.

Dat is het hele mechanisme. Al het restje is boekhouding.

De randvoorwaarde, wiel voor wiel

Stel het gebruikelijke robotstelsel op: x vooruit, y naar links van de robot, ω tegen de klok in. Dit is de ROS-conventie, en iets anders gebruiken betekent vechten met elke bibliotheek die je ooit aanraakt.

Zet wiel i op positie (xᵢ, yᵢ) in dat stelsel en geef het een rolhand sᵢ = ±1 die aangeeft langs welke van de twee 45°-lijnen zijn rollen liggen. Dan hangt de randsnelheid vᵢ (in meter per seconde bij de band, niet in radialen) als volgt samen met de twist van het chassis:

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

Drie termen, en elke is een zin waard:

• vx — elk wiel draagt volledig bij aan de voorwaartse beweging, wat zijn hand ook is. Daarom voelt vooruit op een mecanum-basis volkomen normaal.
• sᵢ·vy — zijwaartse beweging stuurt de helft van de wielen de ene kant op en de andere helft de andere kant. De hand bepaalt welke helft.
• (sᵢ·xᵢ − yᵢ)·ω — de momentarm. Let op dat dit niet alleen de spoorbreedte is; de rolhand zit erin, en daarom verpest een wiel in de verkeerde hoek ook de draaiing, niet alleen de zijwaartse slag.

Ga je van randsnelheid naar wat je een motor daadwerkelijk opgeeft, deel dan door de wielstraal: ωwheel = vᵢ / r, in rad/s.

Vier wielen: de matrix

Stap die vier rijen op elkaar en je hebt een 4 × 3-matrix A, met de twist van het chassis ξ = (vx, vy, ω):

v = A · ξ

Voor de standaardopstelling—linksvoor en rechts achter één hand, rechtsvoor en linksachter de andere—schrijf je k = lx + ly, waarbij lx de halve wielbasis is (voor naar achter) en ly de halve spoorbreedte (links naar rechts). Dan:

En dat zijn de vier regels die iedereen opschrijft:

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

Toets het aan de vier dingen die je al weet dat een mecanumrobot doet:

Het uitgewerkte voorbeeld

Een basis met lx = 75 mm, ly = 85 mm, wielen van 97 mm (r = 48,5 mm), gevraagd om vx = 0,30 m/s, vy = 0,20 m/s, ω = 0,50 rad/s. Dus k = 0,160 m.

Negentwintig keer de snelheid op het ene wiel tegenover het andere, uit een commando dat er niet extreem uitziet. Een mecanum-aandrijflijn brengt het grootste deel van zijn leven door met vier motoren die allemaal wilde, onderling verschillende dingen doen, en daarom betekent “ze krijgen allemaal dezelfde PWM en hij rijdt rechtdoor” hier geen enkele test.

Verzadiging: schaal, klem nooit

Vraag je meer dan de motoren hebben, dan moet je iets opgeven. Er zijn precies twee keuzes en die zijn niet gelijkwaardig.

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

Tegenover wat er gebeurt als je op elke motor gewoon constrain(fl, -255, 255) schrijft: klemmen. Klemmen houdt de snelheid vast en gooit de richting weg. Vraag 0,60 m/s vooruit, 0,15 zijwaarts en 0,8 rad/s draaiing op een robot waarvan de wielen hooguit 0,50 m/s halen:

Het lastigste is dat klemmen er goed uitziet bij de commando’s waarop mensen testen. Een zuivere diagonale beweging vraagt twee wielen om 2 en twee om 0; die nullen zitten al binnen de grens, dus klemmen en schalen geven identieke getallen en de bug verschuilt zich. Hij komt later boven, onder een padvolger, als een robot die om een of andere onvindbare reden uit de bochten leunt. Bekijk beide begrenzers in dezelfde rit en het verschil is een zichtbare kromme.

Achteruit: odometrie

Voorwaartse kinematica is dezelfde matrix de andere kant op gelezen. Maar A is 4 × 3—vier metingen, drie onbekenden— dus er is geen inverse. Er is een oplossing in de kleinste kwadraten:

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

Voor de standaardopstelling valt AᵀA diagonaal uit en klapt het hele spul in tot drie regels die je op een Arduino kunt zetten:

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

Integreer dat door de koers van de robot per tick en je hebt dead reckoning:

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;

Wat werkt precies zo goed als de aanname eronder—dat de wielen niet slippen. Op een mecanum-basis is slip het ontwerppunt van het wiel, dus behandel zijwaartse odometrie als het minst betrouwbare getal op de robot.

De vierde vergelijking die niemand gebruikt

Vier vergelijkingen, drie onbekenden. Die overgebleven vergelijking is geen afval—het is de enige gratis diagnose van de hele aandrijflijn.

Is de robot star en grijpt elk wiel, dan kunnen de vier randsnelheden niet willekeurig zijn. Tel de eerste twee rijen van de matrix bij elkaar op en de laatste twee, en beide komen uit op 2·vx, dus:

FL + FR = RL + RR

altijd, exact, voor elke beweging die een starre mecanum-chassis kan uitvoeren. Bereken dus de restterm in elke lus:

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

Niet nul betekent dat de vier encoders een beweging beschrijven die geen vast voorwerp kan maken. Oorzaken, ruwweg op volgorde van waarschijnlijkheid: een wiel van de grond, een wiel op een natte plek, een vastgelopen rol, een slippende koppeling, een dood encoderkanaal—of een wiel in de verkeerde hoek gemonteerd, wat de restterm groot en constant maakt in plaats van schokkerig.

Wees eerlijk over wat hij niet kan zien: slippen alle vier de wielen op dezelfde manier op hetzelfde moment, dan blijft de restterm op nul en glijdt de robot stilzwijgend zijwaarts uit de koers. Die blindheid is precies waarom mecanum-odometrie stilletjes misgaat in plaats van luid, en waarom serieuze bouws een IMU of niet-aangedreven dode wielen toevoegen in plaats van de aandrijfencoders te vertrouwen.

De twee gevolgen van 45°

Beide vallen uit de matrix, en geen van beide is weg te praten.

Diagonale beweging is begrensd op 1/√2. Een commando op 45° zet vx + vy op twee wielen en 0 op de andere twee, dus het bezette paar raakt de snelheidsgrens terwijl de robot pas V_MAX / √2 ≈ 71% van zijn topsnelheid rechtuit beweegt. Snellere motoren verhogen het plafond; ze halen het niet weg.

Tractie is óók begrensd op 1/√2, maar om een andere reden. De nuttige aandrijfkracht F van de motor zit langs de rolrichting, maar de werkelijke grondkracht is F·√2 langs de rolas, en wrijving begrenst dat. Per wiel is de nuttige kracht dus begrensd op μN/√2, en het versnellingsplafond van de robot is:

a_max = μ·g / √2

Bij μ = 0,85 is dat 5,9 m/s², waar dezelfde robot op gewone wielen 8,3 haalt. De massa van de robot valt in beide weg, dus ballast helpt niet—een les die precies omgekeerd loopt aan die van een sumorobot, waar gewicht de hele strategie is.

In firmware

Zet de geometrie op één plek en laat haar nooit twee keer intypen.

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

Meet lx en ly van wielmiddelpunt tot wielmiddelpunt en halveer daarna—niet de chassisplaat, die meestal breder is. Een verkeerde k houdt de robot niet tegen om te rijden; hij laat draaiing en translatie in elkaar overvloeien, waardoor een zuiver draaicommando kruipt en een zuivere zijwaartse slag bocht. Kruipt je robot terwijl hij op de plek draait, verdenk dan eerst k en pas daarna de code.

Problemen oplossen

Volgende stap

Reken de getallen door op een robot die kan slippen: de mecanum drive simulator voert precies deze vergelijkingen in een wrijvingsmodel, zodat je ziet hoe de odometrie en de vloer het oneens zijn. Laat de wielen daarna ook echt de berekende snelheden vasthouden—gesloten-luswiel-snelheidsregeling—en laat “vooruit” op de vloer vooruit betekenen in plaats van op de robot, met veldgeoriënteerde aandrijving.