Controllo orientato al campo: avanti resta avanti

Il problema che incontrerai entro dieci secondi

Costruisci un robot mecanum, prendi il comando e spingi lo stick in avanti. Il robot si allontana da te. Bene.

Ora ruotalo di 180° e spingi di nuovo lo stick in avanti. Il robot avanza verso di te, perché avanti significa avanti per il robot e il robot ora è rivolto verso di te. Per uno o due secondi, il cervello di ogni pilota esegue la traduzione speculare, sbaglia e il robot colpisce qualcosa.

È già abbastanza grave su un robot a trazione differenziale, dove almeno il robot si muove sempre lungo il proprio muso. Su una base olonomica è molto peggio, perché ora il robot può essere rivolto in una direzione e muoversi in un’altra, senza alcun indizio visivo che mostri dove sia finito “di lato”. Dieci secondi di rotazione e nessuno nella stanza riesce a controllarlo.

La soluzione è una matrice di rotazione.

Due sistemi di riferimento, una trasformazione

Ci sono due modi per interpretare lo stick:

• Orientato al robot (robot-centrico, relativo al robot). Lo stick è un comando nel sistema di riferimento del robot stesso — base_link, se usi ROS. Spingi in avanti e il robot si muove lungo il proprio muso.
• Orientato al campo (field-centrico, relativo al campo). Lo stick è un comando nel sistema di riferimento del pavimento. Spingi in avanti e il robot si sposta verso nord, ovunque sia rivolto il suo muso.

La trazione comprende soltanto il sistema di riferimento del robot: la cinematica mecanum è scritta in quel sistema. Il controllo orientato al campo significa quindi ruotare il comando del pilota nel sistema di riferimento del robot prima che la cinematica lo veda.

Se l’orientamento del robot è θ (misurato in senso antiorario rispetto all’asse x del campo), ruota il comando di −θ:

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

Meno theta, non theta. Se sbagli il segno, ottieni un robot che sembra corretto mentre punta in avanti e si muove nella direzione esattamente opposta dopo aver ruotato di 90°, un problema che può far impazzire per dieci minuti.

La rotazione non viene toccata. ω significa la stessa cosa in entrambi i sistemi di riferimento — una rotazione è una rotazione — quindi passa senza modifiche.

Tutto, in codice

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

Sei righe, e sono la differenza tra un robot che le persone possono controllare e uno che non possono. Nota che driveMecanum() non saprà mai nulla di tutto questo: l’orientamento al campo è un cappello che metti sul comando, non una modifica alla trazione.

Come ottenere θ

Ti serve un angolo di orientamento. Ci sono tre modi per ottenerlo, in ordine crescente di quanto tempo rimangono affidabili.

L’orientamento derivato da un giroscopio è un integrale, quindi il bias diventa deriva in modo lineare:

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

Due dettagli contano più della scelta del sensore:

• Misura il bias a ogni avvio, con il robot fermo. Fai la media di 500 campioni in uno o due secondi prima di qualsiasi movimento. Un MPU6050 grezzo senza rimozione del bias deriva di decine di gradi al minuto; con la rimozione, di un numero di una sola cifra.
• Applica una zona morta alla velocità angolare, non all’angolo. Applicarla all’angolo fa perdere le rotazioni reali lente. Applicarla alla velocità scarta soltanto il rumore di fondo, che è esattamente ciò che viene integrato e diventa deriva.

Un accelerometro non può aiutarti qui. Fornisce rollio e beccheggio osservando la gravità, motivo per cui un filtro complementare corregge egregiamente l’inclinazione, ma la gravità è parallela all’asse di orientamento, quindi non dice nulla sul verso in cui sei rivolto. L’orientamento richiede un magnetometro, oppure deve derivare.

Il reset, che non è opzionale

Due fattori impongono uno zero manuale:

1. Deriva. Dopo pochi minuti l’idea di nord del robot e quella del pilota si separano e la correzione diventa il problema.
2. Orientamento iniziale. Lo zero della IMU corrisponde alla direzione in cui il robot puntava all’accensione, che raramente è quella da cui si trova il pilota.

Mantieni quindi un offset e un pulsante:

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

La convenzione verso cui tutti convergono: punta il robot in direzione opposta al pilota, premi il pulsante e guida. Ci vuole mezzo secondo e risolve entrambi i problemi in una volta. Ogni robot olonomico teleoperato serio ha questo pulsante e ogni squadra che lo ha omesso lo ha aggiunto dopo la prima partita.

Controllo orientato al campo anche per l’autonomia

Non è solo un aiuto alla guida. Qualsiasi comportamento autonomo il cui comando sia espresso naturalmente rispetto al pavimento richiede la stessa trasformazione:

• “Muoviti di 300 mm verso l’obiettivo” — l’obiettivo è nelle coordinate del campo, quindi lo è anche il comando.
• “Sposta lateralmente per mantenere centrato il bersaglio della telecamera mentre lo si orbita” — l’orbita è nel riferimento del campo, la rotazione in quello del robot e il controllo orientato al campo ti permette di scrivere entrambe sulla stessa riga.
• Qualsiasi elemento a valle di un pianificatore di percorsi, che pianifica su una mappa e quindi nel riferimento del campo.

In ROS è la trasformazione odom → base_link a svolgere lo stesso compito: per questo esiste TF2 e perché cmd_vel è definito esplicitamente in base_link. Il controllo orientato al campo è la versione implementata manualmente di una ricerca che altrimenti chiederesti a TF2.

Come si vede

Il simulatore della trazione mecanum ha una sola manovra che rende il punto senza bisogno di parole: tieni lo stick in avanti, continua a ruotare e cambia sistema di riferimento.

Stesso stick, stesse ruote, stessa cinematica. L’unica differenza sono sei righe di trigonometria.

Risoluzione dei problemi

Prossimo passo

L’orientamento al campo fa sì che il comando significhi la cosa giusta. Il controllo ad anello chiuso della velocità delle ruote fa sì che le ruote lo eroghino davvero: su una base olonomica i due aspetti formano una coppia, perché un robot i cui motori cedono in modo diseguale ha un orientamento errato prima che la IMU possa dire la sua.