De cmd_vel a velocidades de rueda: tracción diferencial en ROS 2

Introducción

Todo lo que está por encima de las ruedas en un robot ROS 2 —teleop, navegación, tu propio controlador de atraque— habla el mismo idioma: un geometry_msgs/msg/Twist en /cmd_vel, diciendo «ve así de rápido adelante y gira así de rápido». Todo lo que está por debajo de las ruedas habla ciclos de trabajo PWM.

Este tutorial es la pieza intermedia, y es lo bastante corto para parecer trivial. Dos líneas de álgebra convierten uno en otro. El otro 90 % de un nodo de tracción funcional es lo que esas dos líneas hacen mal cuando al robot se le pide algo que no puede dar.

Qué dice realmente un Twist

Solo dos de sus seis números importan a un robot de tracción diferencial:

• linear.x —velocidad adelante en metros por segundo, a lo largo del morro del robot.
• angular.z —velocidad de guiñada en radianes por segundo, positiva en sentido antihorario vista desde arriba.

Los otros cuatro describen movimiento que un robot de dos ruedas no puede hacer. linear.y es lateral, que solo una base holonómica puede hacer; ignóralo en lugar de aproximarlo. Cambia las dos líneas de abajo por la matriz mecanum de cuatro filas y linear.y empieza a significar algo. Ambos valores se expresan en el propio marco base_link del robot, y ambos están en unidades SI —un robot que toma linear.x como porcentaje parecerá que funciona hasta que se encuentra con una pila de navegación real.

Las dos líneas de cinemática

Sea b la separación de ruedas y v, ω las velocidades lineal y angular comandadas. La velocidad en el suelo de cada rueda es la velocidad adelante del robot más o menos la contribución del giro:

v_left  = v − ω · b / 2
v_right = v + ω · b / 2

Convierte cada una a velocidad angular de rueda dividiendo por el radio de rueda r:

ω_wheel = v_wheel / r        [rad/s]

Esa es toda la conversión. Girar en el sitio es v = 0, lo que da dos velocidades de rueda iguales y opuestas; conducir recto es ω = 0, lo que da dos idénticas. La proporción entre ellas fija el radio de giro, R = v / ω, y preservar esa proporción es de lo que trata el resto de este tutorial.

Mide b conduciendo, no con una regla

La separación de ruedas es el parámetro del que hereda todo lo demás, y la distancia entre los centros de tus ruedas no lo es.

Los neumáticos son deformables, contactan el suelo en un parche en lugar de una línea, y ambos deslizan lateralmente en cada giro. El valor que hace verdaderas las ecuaciones —la anchura de vía efectiva— está típicamente unos puntos porcentuales lejos de la física, y siempre en la dirección que hace que tu robot subgire.

Calíbrala directamente:

1. Comanda una rotación pura, v = 0, ω = 1.0 rad/s, durante exactamente 10 segundos.
2. El robot debería haber girado 10 radianes —una vuelta completa más unos 213°. Marca el rumbo inicial y mide dónde se detuvo realmente.
3. Escala: b_new = b_old × (turned / commanded).

Repite en ambas direcciones y promedia, porque una transmisión asimétrica da respuestas distintas en cada sentido. Dos iteraciones suelen bastar. Este mismo número es el que tu odometría usa para integrar la posición, así que calibrarlo aquí arregla dos cosas a la vez.

Maneja la saturación sin cambiar el giro

Un motor tiene una velocidad máxima. Pide v = 0.5 m/s con un giro fuerte en un robot cuyas ruedas llegan a 0.55 m/s, y el objetivo de una rueda queda por encima de lo que puede hacer.

La respuesta obvia es recortar esa rueda. No lo hagas. Recortar una rueda cambia la diferencia entre las ruedas, y la diferencia es el giro —así que el robot conduce en silencio un arco más abierto que el comandado. Un seguidor de ruta que pide una curva concreta recibe otra curva, corrige, satura de nuevo y serpentea.

Escala en cambio ambas ruedas por el mismo factor. La proporción sobrevive, así que el arco sobrevive; el robot simplemente lo recorre más despacio:

peak = max(abs(left), abs(right))
if peak > MAX_WHEEL_SPEED:
    scale = MAX_WHEEL_SPEED / peak
    left *= scale
    right *= scale

Tres líneas, y convierte «forma equivocada» en «forma correcta, más lenta» —que es casi siempre el compromiso que quieres en un robot que sigue algo.

Conoce la envolvente que realmente tienes

El escalado salva el arco, pero merece la pena saber de antemano qué comandos lo dispararán, porque ese límite es una propiedad de tu robot y se le puede contar a un planificador.

Iguala v_left o v_right a la velocidad máxima de la rueda y despeja. La máxima velocidad de guiñada disponible a una velocidad adelante dada es:

w_max(v) = 2 * (V_max - |v|) / b

Para el robot del código de abajo —b = 0.152 m, ruedas que llegan a 0.55 m/s:

La forma es lo útil: la velocidad y la tasa de giro se intercambian linealmente, y el último 10 % de velocidad adelante cuesta casi toda tu dirección. Un robot comandado a 0,5 m/s tiene el 12 % de la autoridad de guiñada que tenía parado, que es justo por qué un seguidor de ruta que satura tiende a hacerlo en una curva.

Dos consecuencias sobre las que actuar. Pon tus límites reales en el planificador en lugar de dejar que los descubra saturando —en Nav2 eso es max_vel_x y max_vel_theta, y poner max_vel_x al total de 0,55 m/s garantiza que el controlador pedirá arcos que el robot no puede conducir. Y al sintonizar pure pursuit, recuerda que un adelanto lo bastante corto para exigir un radio cerrado limitará tu velocidad en silencio.

Aplica rampa al comando, o las ruedas deslizan

Un Twist es un escalón. Navegación publica 0,0 y luego 0,35 m/s en mensajes consecutivos, y un comando en escalón pide a los motores tanta aceleración como pueden dar —que en un robot ligero es más de lo que los neumáticos pueden transmitir.

Lo que se rompe no es el movimiento, es la odometría. Una rueda que desliza gira sin que el robot se mueva, los encoders lo cuentan igual, y la estimación de pose gana un error que ningún filtro posterior puede distinguir del movimiento real. La página de tracción pone un techo medido a esto: un robot con el 55 % de su peso sobre el eje motriz logra unos 1,6 m/s2 sobre baldosa polvorienta antes de que las ruedas rompan.

Limita la aceleración por debajo de ese techo, en el temporizador donde dt es conocido y constante:

MAX_ACCEL = 1.0        # m/s^2 at the wheel, comfortably under the traction limit

def slew(target, current, dt):
    step = MAX_ACCEL * dt
    return max(current - step, min(current + step, target))

# in tick(), before sending:
self.out_left = slew(self.left, self.out_left, 0.02)
self.out_right = slew(self.right, self.out_right, 0.02)

A 1,0 m/s2 un arranque desde parado llega a velocidad máxima en 0,55 s, añadiendo 20 mm/s por tic de 50 Hz. Eso es lo bastante lento para mantener los neumáticos agarrados y lo bastante rápido para que ningún humano llame lento al robot.

Aplica rampa a ambas ruedas con el mismo límite, no con la misma fracción. Limitar la aceleración de cada rueda por separado distorsiona la proporción durante la rampa por la misma razón que el recorte por rueda —el giro sale mal durante el primer medio segundo de cada movimiento, que es justo cuando un controlador de atraque es más sensible.

Un nodo de tracción completo

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

WHEEL_RADIUS = 0.0325        # m
WHEEL_SEPARATION = 0.152     # m — calibrated by driving, see above
MAX_WHEEL_SPEED = 0.55       # m/s at full duty, measured
CMD_TIMEOUT = 0.4            # s


class DiffDrive(Node):
    def __init__(self):
        super().__init__('diff_drive')
        self.create_subscription(Twist, 'cmd_vel', self.on_cmd, 10)
        self.create_timer(0.02, self.tick)          # 50 Hz output
        self.left = self.right = 0.0
        self.last_cmd = self.get_clock().now()

    def on_cmd(self, msg):
        v, w = msg.linear.x, msg.angular.z
        left = v - w * WHEEL_SEPARATION / 2.0
        right = v + w * WHEEL_SEPARATION / 2.0

        peak = max(abs(left), abs(right))
        if peak > MAX_WHEEL_SPEED:                  # keep the arc, lose the speed
            left *= MAX_WHEEL_SPEED / peak
            right *= MAX_WHEEL_SPEED / peak

        self.left, self.right = left, right
        self.last_cmd = self.get_clock().now()

    def tick(self):
        age = (self.get_clock().now() - self.last_cmd).nanoseconds * 1e-9
        if age > CMD_TIMEOUT:                       # nobody is driving — stop
            self.left = self.right = 0.0
        send_wheel_targets(self.left / WHEEL_RADIUS,
                           self.right / WHEEL_RADIUS)   # rad/s

Dos decisiones estructurales que merece la pena nombrar.

Salida por temporizador, no en la retrollamada. Los motores se comandan a 50 Hz fijos sin importar cada cuánto llegue cmd_vel. Un publicador que tartamudea —porque el Wi-Fi hipa, o un planificador tuvo un ciclo largo— ya no tartamudea las ruedas.

El vigilante no es opcional. Sin él, el último comando antes de un enlace caído es el comando que el robot sigue ejecutando, y el último comando suele ser «adelante». Un robot que se detiene cuando deja de que le digan qué hacer es la diferencia entre un error y un agujero en la pared. Los tutoriales de control inalámbrico argumentan lo mismo por la misma razón.

Convertir velocidad de rueda en ciclo de trabajo

El nodo de arriba entrega rad/s por rueda. Algo tiene que hacerlo realidad, y hay dos formas honestas.

Lazo abierto. Mapea velocidad a ciclo linealmente y acepta el error. Los motores tienen una banda muerta —por debajo de un 15–20 % de ciclo zumban y no giran— así que el mapa tiene que empezar por encima de ella:

int dutyFor(float radPerSec) {
  if (fabsf(radPerSec) < 0.05f) return 0;                 // genuinely stopped
  float u = fabsf(radPerSec) / MAX_RAD_PER_SEC;           // 0..1
  int duty = DEADBAND + (int)((255 - DEADBAND) * u);
  return (radPerSec > 0) ? duty : -duty;
}

Esto está bien para teleop y mal para cualquier cosa que mida dónde fue. El ciclo es una petición de par, no de velocidad: el mismo ciclo da distinta velocidad cuesta arriba, sobre alfombra o con la batería floja.

Lazo cerrado. Pon encoders en cuadratura en las ruedas y ejecuta un lazo PID por rueda con rad/s como consigna. Ahora la capa de tracción entrega la velocidad pedida, y la banda muerta, la carga y la batería se vuelven problema del controlador en lugar del tuyo.

En un robot basado en Pi la división habitual es ejecutar el nodo ROS en la Pi y el PID por rueda en un Arduino por serie, a unos cientos de hercios. Linux no es el sitio para un lazo que no debe tener fluctuación.

Pruébalo antes de que el robot pueda hacer daño

Todo el nodo se puede probar desde la línea de comandos con las ruedas en el aire. Haz eso primero; un error de signo en cinemática descubierto en el banco es una curiosidad, y el mismo error descubierto al borde de una mesa es una reparación.

# Straight ahead at 0.2 m/s, republished at 10 Hz
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.2}, angular: {z: 0.0}}' -r 10

# Pure rotation, counter-clockwise at 1 rad/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.0}, angular: {z: 1.0}}' -r 10

# Who is on the topic, and with which message type
ros2 topic info /cmd_vel --verbose

Cuatro comprobaciones, en orden:

1. Comando recto, ambas ruedas igual de dirección y velocidad. Si difieren, el cableado o la convención de signos están mal antes de que intervenga ninguna matemática.
2. angular.z positivo, rueda izquierda atrás. REP 103 dice que la guiñada positiva es antihoraria vista desde arriba, así que la rueda izquierda debe ir en reversa. Equivocarse aquí hace que cada pila de navegación gire alejándose de su meta.
3. Mata el publicador con Ctrl-C. Las ruedas deben detenerse dentro de CMD_TIMEOUT. Cronométralo —un vigilante que nunca se ejercita es un vigilante que debes suponer roto.
4. Comanda más allá de la envolvente, digamos linear.x: 0.5 con angular.z: 3.0, y confirma que ambas ruedas escalan en lugar de recortar una. La proporción entre ambas velocidades de rueda debería quedar igual que en el comando no saturado.

Solo entonces ponlo en el suelo, y conduce la calibración de rotación de la sección de arriba como primera prueba real.

¿Twist o TwistStamped?

Merece saberse antes de que te cueste una tarde: las partes más nuevas del ecosistema ROS 2 —diff_drive_controller entre ellas— han pasado a geometry_msgs/msg/TwistStamped, que envuelve el mismo twist en una cabecera con marca de tiempo e id de marco.

El fallo es silencioso. Un nodo de teleop publicando Twist plano y un controlador suscrito a TwistStamped son, para DDS, dos topics distintos que resulta que comparten nombre. Nada da error; el robot simplemente nunca se mueve. Comprueba con ros2 topic info /cmd_vel --verbose, que imprime el tipo en cada extremo.

Cuando algo sale mal

Una vez que la capa de tracción es honesta, todo lo de arriba se vuelve más fácil: la misma interfaz /cmd_vel sirve a teleop, un controlador de atraque y una pila de navegación completa sin que ninguno sepa cuáles son tus motores. Un seguidor de ruta es exactamente tal productor: convierte una ruta en una velocidad lineal y una angular y las entrega por esta misma interfaz.