Conducción orientada al campo: que adelante signifique adelante

El problema que tendrás en diez segundos

Construye un robot mecanum, toma el mando, empuja la palanca hacia adelante. El robot se aleja de ti. Bien.

Ahora gíralo 180° y empuja la palanca hacia adelante otra vez. El robot conduce hacia ti, porque adelante significa adelante para el robot y el robot ahora te mira de frente. El cerebro de cada conductor hace la traducción en espejo durante uno o dos segundos, se equivoca y el robot choca con algo.

Eso ya es bastante malo en un robot diferencial, donde al menos el robot siempre se mueve a lo largo de su propio morro. En una base holonómica es mucho peor, porque ahora el robot puede apuntar en una dirección y moverse en otra, y no hay ninguna pista visual de dónde ha quedado el «lateral». Diez segundos girando y nadie en la sala puede conducirlo.

La solución es una matriz de rotación.

Dos marcos, una transformación

Hay dos formas de interpretar la palanca:

• Orientado al robot (céntrico en el robot, relativo al robot). La palanca es una orden en el propio marco del robot — base_link, si hablas ROS. Empuja adelante, el robot avanza a lo largo de su morro.
• Orientado al campo (céntrico en el campo, relativo al campo). La palanca es una orden en el marco del suelo. Empuja adelante, el robot se mueve al norte, apunte donde apunte su morro.

La transmisión solo entiende el marco del robot — la cinemática mecanum está escrita en él. Así que el control orientado al campo significa rotar la orden del conductor al marco del robot antes de que la cinemática la vea.

Si el rumbo del robot es θ (medido en sentido antihorario desde el eje x del campo), rota la orden −θ:

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

Menos theta, no theta. Equivocarse de signo da un robot que parece ir bien apuntando al frente y conduce exactamente en dirección contraria cuando ha girado 90°, lo que supone diez minutos enloquecedores.

La rotación no se toca. ω significa lo mismo en ambos marcos — un giro es un giro — así que pasa directamente.

Todo el sistema, en código

#include <math.h>

// stickX: adelante en la palanca, positivo = alejándose del conductor (campo +x)
// stickY: izquierda en la palanca, positivo = a la izquierda del conductor (campo +y)
// stickW: giro, positivo = sentido antihorario
void driveFieldOriented(float stickX, float stickY, float stickW) {
  float theta = imuYawRadians() - headingOffset;   // ver "el reinicio" más abajo

  float c = cosf(theta);
  float s = sinf(theta);
  float vx =  stickX * c + stickY * s;
  float vy = -stickX * s + stickY * c;

  driveMecanum(vx, vy, stickW);   // la cinemática inversa, sin cambios
}

Seis líneas, y marcan la diferencia entre un robot que la gente puede conducir y uno que no. Observa que driveMecanum() nunca se entera de nada de esto: la orientación al campo es una capa sobre el comando, no una modificación de la transmisión.

Cómo obtener θ

Necesitas un ángulo de guiñada (yaw), y hay tres formas de conseguirlo, en orden creciente de duración de su precisión:

La guiñada obtenida de un giroscopio es una integral, por lo que el sesgo se convierte linealmente en deriva:

// Integra la velocidad de guiñada, con el sesgo medido mientras estaba inmóvil.
float rate = gyroZ() - gyroBiasZ;             // rad/s
if (fabsf(rate) < 0.004f) rate = 0.0f;        // banda muerta: elimina el último residuo de deriva
yaw += rate * dt;

Dos detalles que importan más que la elección del sensor:

• Mide el sesgo en cada arranque, con el robot quieto. Promedia 500 muestras durante uno o dos segundos antes de que nada se mueva. Un MPU6050 en bruto sin corrección de sesgo deriva decenas de grados por minuto; con ella, apenas dígitos simples.
• Una banda muerta en la velocidad angular, no en el ángulo. Aplicar banda muerta al ángulo descarta giros lentos reales. Aplicarla a la velocidad angular solo elimina el suelo de ruido, que es justo lo que se integra produciendo deriva.

Un acelerómetro no puede ayudar aquí. Proporciona balanceo (roll) y cabeceo (pitch) midiendo la gravedad, por lo que un filtro complementario corrige la inclinación de maravilla; pero la gravedad es paralela al eje de guiñada, así que no dice nada sobre hacia dónde estás encarado. La guiñada necesita un magnetómetro o asumirá deriva.

El reinicio (cero manual), que no es opcional

Dos motivos obligan a tener un botón de puesta a cero:

1. Deriva. Tras unos minutos la noción de norte del robot y la del conductor se han separado, y la solución se convierte en el problema.
2. Orientación al arranque. El cero del IMU es dondequiera que estuviese apuntando el robot al encenderse, lo que casi nunca coincide con la posición del conductor.

Por tanto, guarda un desfase y un botón:

float headingOffset = 0.0f;

void resetFieldHeading() {          // vincúlalo a un botón accesible sin mirar
  headingOffset = imuYawRadians();  // "adondequiera que apuntes ahora es adelante"
}

La convención en la que todos convergen: apunta el robot en dirección contraria al conductor, pulsa el botón, conduce. Tarda medio segundo y soluciona ambos problemas a la vez. Todo robot holonómico serio teledirigido incluye este botón, y todos los equipos que lo omitieron lo añadieron tras el primer combate.

Orientado al campo también para autonomía

Esto no es solo una ayuda a la conducción manual. Cualquier comportamiento autónomo cuya orden se exprese de forma natural en el suelo requiere la misma transformación:

• “Moverse 300 mm hacia el objetivo”: el objetivo está en coordenadas del campo, por lo que el comando también.
• “Desplazarse lateralmente para mantener centrado el blanco de la cámara mientras se orbita a su alrededor”: la órbita está en el marco del campo, el giro en el del robot, y el control orientado al campo es lo que permite escribir ambos en la misma línea.
• Cualquier proceso aguas abajo de un planificador de trayectorias, que planifica sobre un mapa y por tanto en el marco del campo.

En ROS esto equivale a la transformación odom → base_link cumpliendo la misma función, que es por lo que existe TF2 y por lo que cmd_vel se define explícitamente en base_link. El control orientado al campo es la versión manual de una búsqueda de coordenadas que de otro modo pedirías a TF2.

Cómo visualizarlo

El simulador de tracción mecanum tiene una maniobra simple que lo ilustra sin palabras: mantén la palanca hacia adelante, sigue girando sobre ti mismo y cambia de marco.

Misma palanca, mismas ruedas, misma cinemática. La única diferencia son seis líneas de trigonometría.

Solución de problemas

Siguiente paso

La orientación al campo hace que el comando signifique lo correcto. El control de velocidad de rueda en bucle cerrado hace que las ruedas realmente lo cumplan: en una base holonómica ambos forman una pareja indispensable, ya que un robot cuyos motores decaen de forma desigual se desvía del rumbo antes de que el IMU pueda intervenir.