Cinemática del robot diferencial

01 · Punto de partida

Del tipo de chasis al modelo

En el tema 25 elegimos la configuración diferencial por su simplicidad y por su capacidad de girar sobre el lugar. Ahora la modelamos. La cinemática relaciona velocidades sin preocuparse por las fuerzas: qué velocidades de rueda producen qué movimiento del chasis, y al revés.

La necesitamos en las dos direcciones, igual que en el brazo (temas 21 y 22):

• Directa: con las velocidades medidas de las dos ruedas, saber a qué velocidad avanza y gira el robot. Es la base de la odometría (tema 27).
• Inversa: con la velocidad de avance y de giro que pide el controlador (seguidor de línea, navegación), calcular qué velocidad debe tener cada rueda. Es lo que hace que un algoritmo de alto nivel pueda ordenar «avanzá a 20 cm/s y girá a 1 rad/s» a dos motores.

Todo en esta unidad usa un robot con b = 15 cm entre ruedas, ruedas de 3,25 cm de radio y una velocidad máxima de 30 cm/s por rueda. Son valores de ejemplo: medí los tuyos.

02 · Notación

El modelo y su notación

Usamos el marco del robot del tema 20: origen en el punto medio del eje que une las ruedas, eje x hacia adelante, eje y hacia la izquierda y ángulos positivos antihorarios. La pose en el mundo es (x, y, θ).

Las variables son:

• b: distancia entre los puntos de contacto de las dos ruedas (ancho de vía efectivo).
• r: radio de las ruedas. La velocidad de una rueda sobre el suelo es v = r·ωrueda.
• vL y vR: velocidades lineales de los puntos de contacto de las ruedas izquierda y derecha (positivas hacia adelante).
• v y ω: velocidad lineal del punto medio del eje y velocidad angular del chasis.

La hipótesis central es que las ruedas ruedan sin deslizar. Es la que hace válidas las fórmulas y la que el mundo real viola un poco; la retomamos al final.

03 · Medir y deducir

De las ruedas al robot

El punto medio del eje se mueve a la velocidad promedio de las dos ruedas. Y la diferencia entre ellas, repartida sobre la distancia que las separa, es la velocidad angular:

El signo sale de la convención: si la rueda derecha va más rápida que la izquierda, el robot gira hacia la izquierda, antihorario, ω > 0. Con vL = 12 cm/s y vR = 27 cm/s y b = 15 cm: v = 19,5 cm/s y ω = 1,0 rad/s.

Esta es la cinemática que se usa con los encoders: se mide el desplazamiento de cada rueda en un intervalo, se divide por el tiempo para obtener vL y vR y se calculan v y ω (sección 8). Integrando en el tiempo se obtiene la pose, que es la odometría del próximo tema.

04 · Ordenar el movimiento

Del robot a las ruedas

Despejando las dos ecuaciones anteriores:

Es la forma en que un algoritmo de alto nivel «habla» con los motores. Un seguidor de línea decide, por ejemplo, v = 15 cm/s y ω = −1 rad/s (girar a la derecha) y esta fórmula devuelve vL = 22,5 cm/s y vR = 7,5 cm/s. Separar la decisión (qué movimiento) de la ejecución (qué velocidad por rueda) permite cambiar el robot sin tocar el algoritmo, y es la misma idea de capas del proyecto del tema 24.

El término ω·b/2 es la velocidad extra que gana la rueda exterior y pierde la interior. Cuanto más ancho el robot, más velocidad diferencial hace falta para el mismo giro.

05 · La curva que describe

Radio de curvatura y casos especiales

Si v y ω se mantienen constantes, el robot recorre una circunferencia cuyo radio, medido desde el punto medio del eje hasta el centro instantáneo de rotación (CIR), es:

El CIR está sobre el eje de las ruedas, hacia el lado de la rueda más lenta. Los casos que conviene reconocer a primera vista:

Con v = 20 cm/s y ω = 1 rad/s, el radio es 20 cm. Con v = 0 y ω = 2 rad/s, el robot gira sobre su eje: vL = −15 y vR = +15 cm/s.

La relación entre curvatura y velocidades es lo que gobierna los seguidores de línea (tema 31): el algoritmo calcula una corrección de giro y la cinemática la convierte en una diferencia de velocidades.

06 · Dónde está el robot

Integrar la pose

Con la velocidad y el giro se obtiene cómo cambia la pose en el marco del mundo:

El avance se proyecta en el marco del mundo con la orientación actual (tema 20). En un programa hay que pasar a pasos de tiempo Δt. Tres formas de hacerlo, de menos a más precisa:

• Euler: x ← x + v·cos θ·Δt, y ← y + v·sen θ·Δt, θ ← θ + ω·Δt. Usa la orientación del inicio del paso. Es la más simple y acumula un error que crece con el paso y con la velocidad.
• Punto medio: igual, pero con cos(θ + ω·Δt/2) y sen(θ + ω·Δt/2). Usa la orientación a mitad de paso y reduce mucho el error.
• Arco exacto (si v y ω son constantes en el paso y ω ≠ 0): x ← x + (v/ω)·[sen(θ+ω·Δt) − sen θ], y ← y − (v/ω)·[cos(θ+ω·Δt) − cos θ], θ ← θ + ω·Δt. Si ω = 0, se vuelve a la recta.

Con v = 20 cm/s, ω = 1 rad/s durante 3 s, el robot termina en (2,82; 39,80) cm con orientación 172°. Con Euler y pasos de 20 ms, el error es de 0,40 cm; con 100 ms, 2,0 cm; con 500 ms, 10 cm. Con el punto medio los errores son 0,001, 0,017 y 0,42 cm. Con arcos es cero, a menos de redondeo.

En un robot real, v y ω cambian en cada paso y se obtienen de los encoders. Entonces ninguna fórmula es exacta, pero la de arcos (o la del punto medio) sigue siendo la mejor práctica: es barata y evita un error sistemático. El tema 27 la usa.

07 · La realidad impone límites

Límites de los motores

Cada rueda tiene una velocidad máxima Vmáx. El conjunto de pares (v, ω) realizables es una región: las dos ecuaciones de la inversa deben cumplir |vL| ≤ Vmáx y |vR| ≤ Vmáx, es decir:

Con Vmáx = 30 cm/s y b = 15 cm: puede avanzar a 30 cm/s en recta, o girar a 4 rad/s sobre su eje, o combinaciones. Pedir v = 25 cm/s y ω = 2 rad/s da vR = 40 cm/s: no se puede. Hay que decidir qué sacrificar:

No hay una opción correcta para todos los casos. Para seguir una línea o una trayectoria, escalar es lo más seguro: conserva la forma del camino, que es lo que el algoritmo calculó. Recortar rueda por rueda cambia la curva sin avisar, y es una causa frecuente de seguidores que se salen de la pista en los tramos rápidos. Priorizar el giro conviene para evitar un choque.

Además de la máxima, existe una velocidad mínima útil: por debajo de cierto PWM, el motor no arranca por la zona muerta (tema 13). Una velocidad pedida muy baja puede no producir movimiento.

08 · Hacia el hardware

De cm/s a rpm y a cuentas de encoder

Los motores no entienden de cm/s. La cadena de conversiones, en el sentido de la salida, es:

La última función sale de calibrar cada motor (tema 13) o, mejor, de un lazo cerrado de velocidad con encoder (tema 18), en el que la referencia es justamente la velocidad de la rueda calculada aquí. La inversa de este esquema es la medición: de las cuentas del encoder a la velocidad lineal de la rueda,

Con r = 3,25 cm y CPR = 1440 cuentas por vuelta de rueda, cada cuenta equivale a 0,0142 cm. En un período de 20 ms, una cuenta corresponde a 0,71 cm/s: ésa es la resolución con la que se puede medir la velocidad (tema 18). A 20 cm/s la rueda suma unas 28 cuentas por período.

Con vR = 27,5 cm/s la rueda gira a 27,5 / 3,25 = 8,46 rad/s, unas 81 rpm. Son velocidades modestas: lo habitual es un motor con reductor, que gira mucho más rápido internamente.

09 · Los números del modelo

Dónde falla el modelo

• Ancho de vía efectivo. Los neumáticos tienen un área de contacto y deslizan al girar; el b efectivo no es la distancia medida entre centros. Un error de 5 % en b da 5 % de error en ω, y se acumula en la orientación. Se calibra haciendo girar el robot en el lugar y comparando el ángulo ordenado con el medido: bef = b · θordenado / θreal. Si ordena 360° y gira 340°, bef = 15 · 360/340 ≈ 15,9 cm.
• Radios distintos. Si una rueda es 1 % mayor que la otra, con igual velocidad angular el robot curva con R ≈ b/0,01 ≈ 15 m. Tras 2 m recorridos, se desvía unos 13 cm. Se corrige midiendo el recorrido de cada rueda o calibrando.
• Deslizamiento. Aceleraciones fuertes, suelos resbaladizos o giros bruscos hacen que la rueda gire sin avanzar lo que corresponde (tema 17).
• Rueda loca. Su orientación inicial desvía el arranque (tema 25).
• Retardos y dinámica. Los motores no alcanzan la velocidad ordenada de inmediato; el modelo supone que sí.
• Cuantización de la medición. La velocidad medida tiene escalones de 1/(CPR·Δt) (tema 18).

Los errores sistemáticos —b y radios— se calibran. Los aleatorios —deslizamiento, retardos— se toleran o se corrigen con otros sensores (tema 29). Un modelo es una aproximación útil, no una descripción exacta del robot.

10 · Código para la placa

Cinemática diferencial en Arduino

Este sketch no necesita motores. Recibe una orden v ω (cm/s y rad/s), calcula las velocidades de las ruedas, aplica la política de escalado ante la saturación, informa las rpm y la pose tras 3 s integrando por arcos. Es el puente entre el algoritmo de navegación y los lazos de velocidad de las ruedas.

const float B = 15.0;          // distancia entre ruedas (cm)
const float R_RUEDA = 3.25;    // radio de rueda (cm)
const float V_MAX = 30.0;      // velocidad máxima de cada rueda en el suelo (cm/s)

// Ruedas -> robot
void directa(float vl, float vr, float &v, float &w) {
  v = (vr + vl) / 2.0;
  w = (vr - vl) / B;           // positivo = giro antihorario (a la izquierda)
}

// Robot -> ruedas
void inversa(float v, float w, float &vl, float &vr) {
  vl = v - w * B / 2.0;
  vr = v + w * B / 2.0;
}

// Si una rueda supera V_MAX, se escalan v y w por igual: se conserva el radio de curvatura.
float factorDeEscala(float vl, float vr) {
  const float mayor = max(fabs(vl), fabs(vr));
  return (mayor > V_MAX) ? V_MAX / mayor : 1.0;
}

float rpm(float vRueda) { return vRueda / R_RUEDA * 60.0 / TWO_PI; }

void procesar(float v, float w) {
  float vl, vr;
  inversa(v, w, vl, vr);
  const float k = factorDeEscala(vl, vr);
  Serial.print("pedido v="); Serial.print(v, 2); Serial.print(" w="); Serial.print(w, 2);
  Serial.print(" -> vl="); Serial.print(vl, 2); Serial.print(" vr="); Serial.print(vr, 2);
  if (fabs(w) < 1e-6) Serial.println(" | recta");
  else { Serial.print(" | radio="); Serial.print(v / w, 1); Serial.println(" cm"); }

  if (k < 1.0) {
    v *= k; w *= k;
    inversa(v, w, vl, vr);
    Serial.print("saturado: k="); Serial.print(k, 2);
    Serial.print(" -> v="); Serial.print(v, 2); Serial.print(" w="); Serial.print(w, 2);
    Serial.print(" vl="); Serial.print(vl, 2); Serial.print(" vr="); Serial.println(vr, 2);
  }
  Serial.print("rpm: izq="); Serial.print(rpm(vl), 0); Serial.print(" der="); Serial.println(rpm(vr), 0);

  // Pose tras 3 s, integrando por arcos cada 100 ms (exacto si v y w son constantes).
  float x = 0, y = 0, th = 0;
  const float dt = 0.1;
  for (int i = 0; i < 30; i++) {
    if (fabs(w) < 1e-6) { x += v * cos(th) * dt; y += v * sin(th) * dt; }
    else {
      x += v / w * (sin(th + w * dt) - sin(th));
      y -= v / w * (cos(th + w * dt) - cos(th));
      th += w * dt;
    }
  }
  Serial.print("pose a los 3 s: x="); Serial.print(x, 2); Serial.print(" y="); Serial.print(y, 2);
  Serial.print(" theta="); Serial.print(th * RAD_TO_DEG, 0); Serial.println(" grados");
}

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(100);
  Serial.println("Escribi 'v w' (cm/s, rad/s), por ejemplo: 20 1");
  procesar(20.0, 1.0);
}

void loop() {
  if (Serial.available() == 0) return;
  const float v = Serial.parseFloat();
  const float w = Serial.parseFloat();
  while (Serial.available() > 0) Serial.read();
  procesar(v, w);
}

Al iniciar muestra el caso 20 1: vl=12.50 vr=27.50 | radio=20.0 cm, rpm: izq=37 der=81 y pose a los 3 s: x=2.82 y=39.80 theta=172 grados. Con 25 2 la orden satura: k=0.75 -> v=18.75 w=1.50 vl=7.50 vr=30.00, rpm: izq=22 der=88 y la pose termina en x=-12.22 y=15.13 theta=258 grados.

Los resultados son los de un robot ideal. En un robot real, las dos velocidades de rueda se pasan como referencias a sus lazos de control con encoder (tema 18), y las velocidades medidas alimentan la misma cinemática en sentido directo para estimar la pose (tema 27). Como en temas anteriores, parseFloat() espera con un tiempo límite, aceptable aquí porque el sketch solo calcula.

11 · Exploración interactiva

Laboratorios: cinemática e integración

Con v = 20 cm/s y ω = 1 rad/s salen v_L = 12,5 y v_R = 27,5 cm/s, radio 20 cm. Pasá a v = 0 y ω = 2: giro sobre el eje con ∓15 cm/s. Poné v = 25 y ω = 2 y alterná las políticas: «escalar» da radio 12,5 cm; «recortar», 15 cm; «priorizar el giro», 7,5 cm con la rueda izquierda detenida. En «Ruedas → robot», dejá una rueda en 0 y la otra en 30: gira alrededor de la detenida, con radio 7,5 cm.

Con v = 20, ω = 1 y 3 s, el final exacto es (2,8; 39,8) cm. Con 100 ms, Euler se aparta 2,0 cm; con 500 ms, 10 cm; con 20 ms, 0,4 cm. El punto medio queda por debajo de 0,5 cm incluso a 500 ms. El error de Euler crece con el paso y con la velocidad.

12 · Código ejecutable

Comprobar el modelo en Python

Repetimos las conversiones, la política de escalado y la comparación de métodos de integración, para verificar el modelo antes de llevarlo al microcontrolador.

import math

B = 15.0            # distancia entre ruedas (cm)
R_RUEDA = 3.25      # radio de rueda (cm)
V_MAX = 30.0        # velocidad máxima de cada rueda en el suelo (cm/s)

def directa(vl, vr):
    return (vr + vl) / 2, (vr - vl) / B               # v (cm/s), omega (rad/s)

def inversa(v, w):
    return v - w * B / 2, v + w * B / 2               # vl, vr

def escalar(v, w):
    vl, vr = inversa(v, w)
    k = min(1.0, V_MAX / max(abs(vl), abs(vr), 1e-9))
    return v * k, w * k, k                            # conserva el radio de curvatura

def rpm(v):
    return v / R_RUEDA * 60 / (2 * math.pi)

for v, w in ((20, 1.0), (0, 2.0), (25, 2.0)):
    vl, vr = inversa(v, w)
    radio = "recta" if abs(w) < 1e-9 else f"{v / w:.1f} cm"
    print(f"v={v:4.1f} cm/s w={w:3.1f} rad/s -> vl={vl:5.2f} vr={vr:5.2f} cm/s; radio={radio}; "
          f"rpm=({rpm(vl):.0f}, {rpm(vr):.0f})")
vs, ws, k = escalar(25, 2.0)
print(f"Escalado por saturacion: k={k:.2f} -> v={vs:.2f} cm/s, w={ws:.2f} rad/s, radio={vs / ws:.1f} cm (antes {25 / 2.0:.1f})")
print(f"Prioridad al giro: vr=30 -> v={30 - 2.0 * B / 2:.0f} cm/s, w=2.0 rad/s, radio={(30 - 2.0 * B / 2) / 2.0:.1f} cm")

# Integración de la pose: v = 20 cm/s y w = 1 rad/s durante 3 s
def integrar(v, w, T, dt):
    n = round(T / dt)
    euler = [0.0, 0.0, 0.0]
    medio = [0.0, 0.0, 0.0]
    arco = [0.0, 0.0, 0.0]
    for _ in range(n):
        euler[0] += v * math.cos(euler[2]) * dt
        euler[1] += v * math.sin(euler[2]) * dt
        euler[2] += w * dt
        medio[0] += v * math.cos(medio[2] + w * dt / 2) * dt
        medio[1] += v * math.sin(medio[2] + w * dt / 2) * dt
        medio[2] += w * dt
        arco[0] += v / w * (math.sin(arco[2] + w * dt) - math.sin(arco[2]))
        arco[1] -= v / w * (math.cos(arco[2] + w * dt) - math.cos(arco[2]))
        arco[2] += w * dt
    return euler, medio, arco

v, w, T = 20.0, 1.0, 3.0
exacto = (v / w * math.sin(w * T), v / w * (1 - math.cos(w * T)))
print(f"Posicion exacta tras {T:.0f} s: ({exacto[0]:.2f}, {exacto[1]:.2f}) cm, orientacion {math.degrees(w * T):.0f} grados")
for dt in (0.02, 0.1, 0.5):
    resultados = integrar(v, w, T, dt)
    errores = [math.hypot(p[0] - exacto[0], p[1] - exacto[1]) for p in resultados]
    print(f"dt={dt * 1000:3.0f} ms: error Euler={errores[0]:.2f} cm; punto medio={errores[1]:.3f} cm; por arcos={errores[2]:.1e} cm")

Para (20 cm/s; 1 rad/s): ruedas a 12,5 y 27,5 cm/s, radio 20 cm. En el lugar, con ω = 2: ∓15 cm/s. Para (25; 2): satura; escalando, k = 0,75 y radio 12,5 cm; priorizando el giro, radio 7,5 cm. A los 3 s la posición exacta es (2,82; 39,80) cm, y los errores de Euler son 0,40 cm (20 ms), 2,00 cm (100 ms) y 10,01 cm (500 ms), contra 0,001, 0,017 y 0,419 cm del punto medio.

Probá cambiar B y observar el efecto en las velocidades de rueda para un mismo giro, o V_MAX para ver cuándo satura. Cambiá la duración a 6,28 s (una vuelta completa) y notá que el error de Euler baja: el camino es casi cerrado, pero esto no ocurre en general.

13 · Método y diagnóstico

Pruebas y diagnóstico

1. Con las ruedas en el aire, ordená velocidades conocidas y medí con el encoder o contando vueltas; comprobá vL y vR.
2. Medí b y r con calibre o regla y anotá su incertidumbre.
3. Ordená una recta de 2 m a velocidad moderada y medí la desviación lateral; si hay una curva sistemática, hay una diferencia de radios o de motores.
4. Ordená un giro de 360° en el lugar y medí el ángulo real; ajustá b efectivo.
5. Ordená un círculo de radio conocido y comparalo con el trazo del robot.
6. Probá pedidos que saturan y verificá que la política elegida se comporta como se espera.
7. Verificá los signos: una rueda con el sentido invertido hace que el giro y el avance se intercambien.

14 · Práctica guiada

Predecí antes de comprobar

1. De robot a ruedas

Con b = 15 cm, ¿qué velocidades de rueda corresponden a v = 15 cm/s y ω = −1 rad/s? ¿Hacia dónde gira?

2. De ruedas a robot

Si v_L = 10 cm/s y v_R = 25 cm/s, ¿cuánto valen v, ω y el radio?

3. Giro alrededor de una rueda

Con v_L = 0 y v_R = 30 cm/s, ¿cuánto valen v, ω y R? ¿Dónde está el centro?

4. Saturación

Con V_máx = 30 cm/s, el pedido es v = 25 y ω = 2. Escalando por igual, ¿cuánto valen v, ω y el radio resultantes?

5. A rpm

Una rueda de radio 3,25 cm debe avanzar a 27,5 cm/s. ¿Cuántas rpm?

6. Resolución del encoder

Con CPR = 1440, r = 3,25 cm y un período de 20 ms, ¿cuántas cuentas se esperan a 20 cm/s y qué velocidad representa una cuenta?

7. Una rueda más grande

La rueda derecha es 1 % mayor que la izquierda y ambas giran igual. ¿Qué radio describe el robot y cuánto se desvía en 2 m?

15 · Síntesis

Ideas para recordar

¿Listo para el tema 27?

Antes de avanzar, medí b y r de tu robot (o definí los de un modelo), calculá las velocidades de rueda para un círculo de 30 cm de radio a 15 cm/s y la resolución de velocidad de tu encoder. Pensá cuánto error de orientación acumularías en 10 m con un error del 2 % en b.

En el próximo tema usamos esta cinemática en sentido directo: odometría y estimación de posición, acumulando pasos de encoder y analizando sus errores.