Odometría y estimación de posición

01 · Punto de partida

Saber dónde estoy sin mirar

La odometría estima la pose (x, y, θ) de un robot integrando su propio movimiento: se parte de una pose conocida y se suman, paso a paso, los desplazamientos medidos por los encoders de las ruedas. En navegación se llama dead reckoning, «estima a ciegas»: el robot no observa el entorno, solo su cuenta.

Es la pieza que convierte las órdenes de los temas anteriores en un robot que «sabe» dónde está. Un seguidor de línea no la necesita; una navegación punto a punto (tema 28), sí.

Una advertencia desde el principio: la odometría nunca mejora sola. Es exacta para movimientos cortos y se degrada con la distancia. Saber cuánto, y qué hacer al respecto, es el contenido del tema.

02 · La medición

De cuentas a distancia

El encoder entrega cuentas enteras (tema 9). Con CPR cuentas por vuelta de la rueda y radio r, cada cuenta corresponde a un desplazamiento fijo del punto de contacto:

Con r = 3,25 cm y CPR = 1440, k = 0,01418 cm por cuenta, y una vuelta de rueda equivale a 20,42 cm. Si el encoder va en el motor y hay un reductor, las cuentas por vuelta de rueda son las del encoder multiplicadas por la relación (tema 18).

Dos ideas importantes sobre la resolución. Primero, la odometría acumula cuentas y no velocidades: aunque en cada intervalo corto se cuente una fracción de cuenta, el total se conserva, y el error por cuantización queda acotado a una cuenta por rueda, sin importar el tiempo. Segundo, un encoder de baja resolución sigue siendo útil para odometría (a diferencia de la medición de velocidad del tema 18), aunque limita la resolución de la pose: con 48 cuentas por vuelta, cada cuenta son 0,43 cm.

03 · La cuenta

Actualizar la pose

En cada intervalo se miden los incrementos de las dos ruedas. Con la cinemática del tema 26 (en términos de desplazamientos, no de velocidades):

y se actualiza la pose proyectando el avance con la orientación a mitad del intervalo, la mejor aproximación simple (tema 26):

La orientación se normaliza a (−π, π] después de cada paso (tema 20). Observá que la fórmula no usa el tiempo: la odometría depende de cuánto giraron las ruedas, no de cuándo. El período de actualización solo afecta la calidad de la aproximación del arco: cuanto más corto, mejor (20 ms es razonable).

Un ejemplo con intervalos de 20 ms: si en un intervalo la rueda izquierda suma 100 cuentas y la derecha 140, ΔsL = 1,418 cm, ΔsR = 1,985 cm, Δs = 1,702 cm y Δθ = 0,568/15 = 0,0378 rad (2,17°). Desde la pose cero, queda en (1,701; 0,032) con 2,17° de orientación.

La odometría usa el modelo del robot: los valores de b, r y CPR que le damos. Si el robot real no coincide con ese modelo, la pose estimada se aparta de la real sin ningún aviso: el encoder cuenta exactamente lo que giró la rueda, pero ese giro no se traduce exactamente en el desplazamiento que el modelo supone.

04 · El problema de fondo

Por qué el error se acumula

La literatura (Borenstein y colaboradores) separa los errores en dos familias:

En un suelo liso y un robot bien tratado, los errores sistemáticos suelen dominar, y son los que podemos eliminar. Pero el efecto de cualquier error es mayor de lo que parece, por una razón geométrica: un error de orientación se convierte en error de posición.

Si las ruedas tienen una diferencia relativa de diámetros ε, el robot que cree ir recto curva con radio cercano a b/ε. Tras recorrer una distancia d:

Con ε = 1 % y b = 15 cm: a 2 m, la orientación se aparta 7,6° y el desvío es de 13 cm. A 5 m, 19° y 83 cm. A 10 m, 38° y 3,3 m. La orientación crece linealmente con la distancia y el error de posición crece con su cuadrado. Por eso un error de 1 % en un diámetro, que parece insignificante, vuelve inutilizable la estimación en recorridos largos.

Un error en el ancho de vía actúa sobre los giros: un 2 % de error en b hace que cada giro de 90° sea 2 % mayor o menor, y esa desviación angular se arrastra durante todo el recorrido posterior. Además, el robot no tiene forma de saber que se equivoca: la odometría informa con la misma seguridad una pose correcta y una errónea.

05 · Una prueba reveladora

El cuadrado: ver los errores

Una prueba clásica (UMBmark, de la Universidad de Michigan) consiste en ordenar al robot que recorra un cuadrado de lado conocido, por ejemplo 1 m, y medir dónde termina respecto de donde empezó. Como los encoders cuentan exactamente lo ordenado, la odometría «cree» que cerró el cuadrado; la distancia real entre inicio y final es el error acumulado.

Lo valioso es repetir la prueba en ambos sentidos, horario y antihorario:

• Un error de ancho de vía hace que los giros sean mayores o menores de lo ordenado. En cada sentido el error de orientación tiene signo opuesto.
• Un error de diámetros curva el robot siempre hacia el mismo lado, en cualquier sentido. El error de orientación tiene el mismo signo en los dos sentidos.

Con los dos sentidos se identifica cuál de los errores domina y se estima su tamaño. Además, los dos errores pueden compensarse en un sentido y sumarse en el otro, con lo cual una sola prueba puede dar la falsa impresión de que el robot está bien calibrado. El laboratorio 2 permite explorarlo.

Con b +2 % y diámetros +1 %, un cuadrado de 1 m en sentido antihorario termina a unos 9,6 cm del origen, pero en sentido horario a 28 cm. Un robot «bien» en un sentido puede estar mal en el otro.

06 · Corregir lo corregible

Calibración

Dos pruebas sencillas permiten estimar los dos errores sistemáticos. Hacelas sobre una superficie lisa, a velocidad moderada y con las baterías cargadas.

1. Recta larga (diferencia de diámetros). Ordená avanzar en línea recta una distancia d (2 o 3 m) y medí el desvío lateral y. El radio es R = d²/(2y) y la diferencia relativa de diámetros es ε ≈ b/R. Con d = 200 cm y y = 13 cm: R ≈ 1538 cm, ε ≈ 0,97 %. Se corrige escalando el radio (o el factor k) de una rueda: kR ← kR·(1 − ε/2) y kL ← kL·(1 + ε/2), con el signo que acerque el desvío a cero.
2. Giros en el lugar (ancho de vía). Ordená varias vueltas completas (por ejemplo 10) y medí el ángulo real, con una marca en el suelo. El ancho efectivo es bef = b · θordenado / θreal. Si ordena 360° y gira 340°, bef = 15 · 360/340 ≈ 15,9 cm.

Orden recomendado: primero la recta (diámetros) y después el giro (ancho de vía), porque un error de diámetros contamina la prueba de giro. Luego, repetí el cuadrado en ambos sentidos como verificación: los errores deberían reducirse y, sobre todo, dejar de depender del sentido.

Cada calibración vale para esas condiciones: un cambio de ruedas, de carga, de batería o de suelo puede alterarla. Anotá los valores y la fecha. Y no esperes llegar a cero: queda el error no sistemático, que es el que fija el límite de lo que la odometría puede dar.

07 · Alcance y complementos

Hasta dónde llega la odometría

Una estimación razonable de un robot educativo bien calibrado, en suelo liso, es del orden de 1 a 3 % de la distancia recorrida en posición y unos pocos grados por vuelta en orientación. Eso sirve para recorridos de varios metros, no para decenas de metros, y menos todavía si el suelo resbala.

Estrategias para extender su utilidad:

• Mantener los recorridos cortos entre correcciones. Navegar de punto a punto sin acumular distancia (tema 28).
• Corregir la orientación con otro sensor. El giróscopo o la brújula de una IMU (tema 29) atacan justo el error que más se amplifica. Es la mejora más rentable.
• Reiniciar la pose en referencias conocidas: una pared (se alinea el robot contra ella), una marca en el suelo, una estación de carga.
• Evitar lo que provoca deslizamiento: aceleraciones suaves (tema 17), ruedas con buen agarre.
• Fusionar con mediciones externas (distancias a paredes, balizas, cámara), cuando el proyecto lo justifique. La técnica general, que se ve en cursos de estimación, combina la predicción odométrica con las observaciones ponderando su incertidumbre.

La odometría sigue siendo la base: es la única fuente continua, barata y rápida de la pose. Las demás la corrigen de a ratos.

08 · Código real

Detalles de implementación

• Contar con interrupciones. Los pulsos de un encoder a velocidad media llegan miles de veces por segundo; no se pueden leer por sondeo en un loop() ocupado. Las rutinas de interrupción deben ser cortas: solo actualizan el contador (tema 9).
• Lectura atómica. En un microcontrolador de 8 bits, un long se lee en varios ciclos; hay que desactivar las interrupciones un instante al copiarlo (noInterrupts()), o se puede leer un valor a medias.
• Signo del conteo. Verificá que girar una rueda hacia adelante suma cuentas en las dos; si no, invertí el signo de esa rueda. Un signo erróneo parece un error de odometría gigante.
• Mismo reloj, mismos intervalos. Leé ambos contadores en el mismo instante para que los incrementos correspondan al mismo intervalo.
• Precisión numérica. Un float de 32 bits tiene unos 7 dígitos: sumar 0,3 cm a una posición de 100 000 cm ya pierde resolución. Para recorridos de metros sobra; para kilómetros, usá otra representación.
• Una pose, un dueño. Solo la rutina de odometría escribe la pose; el resto la lee. Si otra parte del código la pone a cero (por ejemplo, al reiniciar en una referencia), que sea una operación explícita.
• Unidades consistentes. cm y radianes en todo el programa, convirtiendo a grados solo para mostrar.

Con estos cuidados, la rutina completa son unas veinte líneas. Lo costoso no es el código: es la calibración y la disciplina de medir.

09 · Código para la placa

Odometría en Arduino

Supuestos que debés verificar: UNO R3; encoder izquierdo con canales A y B en D2 (interrupción 0) y D4; encoder derecho en D3 (interrupción 1) y D5; decodificación x2 con 1440 cuentas por vuelta de rueda; b = 15 cm y r = 3,25 cm medidos en tu robot. No necesita motores: podés girar las ruedas con la mano.

// Odometría de un robot diferencial con dos encoders en cuadratura (decodificación x2).
// Probala sin motores: girá las ruedas con la mano y mirá la pose por el monitor serie.
const byte PIN_IZQ_A = 2, PIN_IZQ_B = 4;     // canal A en una interrupción externa
const byte PIN_DER_A = 3, PIN_DER_B = 5;
const int SIGNO_IZQ = 1, SIGNO_DER = 1;      // -1 si la rueda gira hacia adelante y el conteo baja
const float B = 15.0;                        // distancia entre ruedas (cm) - medila
const float R_RUEDA = 3.25;                  // radio de rueda (cm) - medilo
const float CUENTAS_VUELTA = 1440.0;         // cuentas por vuelta de rueda con esta decodificación
const float CM_POR_CUENTA = TWO_PI * R_RUEDA / CUENTAS_VUELTA;
const unsigned long PERIODO_MS = 20;

volatile long cuentasIzq = 0, cuentasDer = 0;
long previaIzq = 0, previaDer = 0;
float x = 0, y = 0, th = 0;                  // pose estimada (cm, cm, rad)
unsigned long ultimo = 0;

void contarIzq() {
  cuentasIzq += (digitalRead(PIN_IZQ_A) == digitalRead(PIN_IZQ_B)) ? -SIGNO_IZQ : SIGNO_IZQ;
}
void contarDer() {
  cuentasDer += (digitalRead(PIN_DER_A) == digitalRead(PIN_DER_B)) ? -SIGNO_DER : SIGNO_DER;
}

void leerCuentas(long &izq, long &der) {
  noInterrupts();
  izq = cuentasIzq;
  der = cuentasDer;
  interrupts();
}

void actualizarPose() {
  long izq, der;
  leerCuentas(izq, der);
  const float dsIzq = (izq - previaIzq) * CM_POR_CUENTA;
  const float dsDer = (der - previaDer) * CM_POR_CUENTA;
  previaIzq = izq; previaDer = der;

  const float ds = (dsDer + dsIzq) / 2.0;          // avance del punto medio del eje
  const float dth = (dsDer - dsIzq) / B;           // giro
  x += ds * cos(th + dth / 2.0);                   // orientación a mitad de intervalo
  y += ds * sin(th + dth / 2.0);
  th += dth;
  while (th > PI) th -= TWO_PI;
  while (th <= -PI) th += TWO_PI;
}

void imprimirPose() {
  Serial.print("x="); Serial.print(x, 2);
  Serial.print(" y="); Serial.print(y, 2);
  Serial.print(" theta="); Serial.print(th * RAD_TO_DEG, 1);
  Serial.println(" grados (estimada)");
}

void setup() {
  pinMode(PIN_IZQ_A, INPUT_PULLUP); pinMode(PIN_IZQ_B, INPUT_PULLUP);
  pinMode(PIN_DER_A, INPUT_PULLUP); pinMode(PIN_DER_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_IZQ_A), contarIzq, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_DER_A), contarDer, CHANGE);
  Serial.begin(115200);
  Serial.println("R: poner la pose en cero; cualquier otra tecla: ver la pose");
  ultimo = millis();
}

void loop() {
  const unsigned long ahora = millis();
  if (ahora - ultimo >= PERIODO_MS) {
    ultimo = ahora;
    actualizarPose();
  }
  if (Serial.available() > 0) {
    const char c = toupper(Serial.read());
    if (c == 'R') { x = 0; y = 0; th = 0; Serial.println("Pose en cero"); }
    else if (c != '\n' && c != '\r') imprimirPose();
  }
}

Comprobaciones con las ruedas giradas a mano: una vuelta completa de ambas hacia adelante da x=20.42 y=0.00 theta=0.0; una vuelta de la rueda derecha solo (la izquierda quieta) da una pose cercana a x≈7.3 y≈5.9 theta≈78 grados (el robot gira alrededor de la rueda izquierda); giradas en sentidos opuestos la pose casi no se traslada y cambia la orientación. Si al avanzar las dos hacia adelante algún conteo baja, invertí SIGNO_IZQ o SIGNO_DER.

Cuando integres esto con los lazos de velocidad del tema 18, el mismo par de contadores sirve para ambas cosas: velocidad (diferencias en un intervalo corto) y pose (acumulado). Mantené la rutina de pose con su propio período y no la mezcles con el lazo de control si tienen períodos distintos.

Recordá que esta pose es una estimación: nada en el programa la verifica. Mostrala siempre como tal, y no tomes decisiones críticas de seguridad basadas únicamente en ella.

10 · Exploración interactiva

Laboratorios: de cuentas a pose y el cuadrado

Con 14 y 26 cm/s durante 4 s, el robot da media vuelta y termina cerca de (−1,5; 50,0) cm con 183° de orientación. Con 1440 cuentas la diferencia con la pose real es de 0,01 cm; con 360, 0,04 cm; con 48, 0,24 cm. Un encoder de baja resolución cuesta poco en odometría porque las cuentas se acumulan y el error queda acotado a una cuenta.

Con los valores iniciales (b +2 %, diámetros +1 %, antihorario), el robot termina a 9,6 cm del origen con 7,9° de error. Cambiá el sentido a horario: sube a 28 cm y 22°. Poné solo el ancho de vía (diámetros 0): el error de orientación cambia de signo con el sentido (∓7,1°) y el de posición es igual en ambos. Poné solo diámetros: la orientación se desvía lo mismo (+15,3°) en los dos sentidos. Subí las vueltas a 4 con los valores iniciales: 38 cm de error tras 16 m.

11 · Código ejecutable

Simular y calibrar en Python

Simulamos el cuadrado con y sin errores, en ambos sentidos, con el mismo generador pseudoaleatorio que el laboratorio, y repetimos las cuentas de la calibración con recta y giro.

import math

B, R_RUEDA, CPR = 15.0, 3.25, 1440
K_CM = 2 * math.pi * R_RUEDA / CPR            # cm por cuenta

def generador(semilla):
    a = [semilla & 0xFFFFFFFF]
    def siguiente():
        a[0] = (a[0] + 0x6D2B79F5) & 0xFFFFFFFF
        t = a[0]
        t = ((t ^ (t >> 15)) * (t | 1)) & 0xFFFFFFFF
        t ^= (t + (((t ^ (t >> 7)) * (t | 61)) & 0xFFFFFFFF)) & 0xFFFFFFFF
        return ((t ^ (t >> 14)) & 0xFFFFFFFF) / 4294967296
    return siguiente

def actualizar(x, y, th, ds_izq, ds_der, b):
    ds, dth = (ds_der + ds_izq) / 2, (ds_der - ds_izq) / b
    return x + ds * math.cos(th + dth / 2), y + ds * math.sin(th + dth / 2), th + dth

def cuadrado(lado, vueltas, e_b, e_d, sigma, sentido, semilla=1):
    """Pose REAL tras recorrer un cuadrado con órdenes calculadas con los parámetros nominales."""
    azar = generador(semilla)
    b_real = B * (1 + e_b)
    f_izq, f_der = 1 - e_d / 2, 1 + e_d / 2         # e_d: diferencia relativa de diámetros
    ruido = lambda: 1 + sigma * (2 * azar() - 1) * math.sqrt(3)
    x = y = th = 0.0
    giro = sentido * math.pi / 2 * B / 2             # recorrido de cada rueda en un giro de 90°
    for _ in range(4 * vueltas):
        for _ in range(round(lado / 2)):             # tramo recto en pasos de 2 cm
            x, y, th = actualizar(x, y, th, 2 * f_izq * ruido(), 2 * f_der * ruido(), b_real)
        n = max(1, round(abs(giro) / 2))              # giro de 90° en pasos de ~2 cm por rueda
        for _ in range(n):
            x, y, th = actualizar(x, y, th, -giro / n * f_izq * ruido(), giro / n * f_der * ruido(), b_real)
    return x, y, math.degrees(th) - 360 * sentido * vueltas

print(f"Parametro de conversion: {K_CM:.5f} cm por cuenta; una vuelta de rueda = {2 * math.pi * R_RUEDA:.2f} cm")
casos = [
    ("Sin errores", 0.0, 0.0, 0.0),
    ("b +2 %", 0.02, 0.0, 0.0),
    ("diametros +1 %", 0.0, 0.01, 0.0),
    ("b +2 % y diametros +1 %", 0.02, 0.01, 0.0),
    ("ademas deslizamiento 1 %", 0.02, 0.01, 0.01),
]
for nombre, e_b, e_d, sigma in casos:
    for sentido, etiqueta in ((1, "antihorario"), (-1, "horario")):
        x, y, dth = cuadrado(100, 1, e_b, e_d, sigma, sentido)
        print(f"{nombre:28s} {etiqueta:11s}: error=({x:6.2f}, {y:6.2f}) cm, {math.hypot(x, y):5.2f} cm, orientacion {dth:+5.2f} grados")
x, y, dth = cuadrado(100, 4, 0.02, 0.01, 0.0, 1)
print(f"Cuatro vueltas (1600 cm): error={math.hypot(x, y):.1f} cm, orientacion {dth:+.1f} grados")

# Calibración simple a partir de dos pruebas
d, desvio = 200.0, 13.0                              # recta de 2 m con 13 cm de desvío lateral
radio = d ** 2 / (2 * desvio)
print(f"Prueba en recta: radio={radio:.0f} cm -> diferencia de diametros = b/R = {B / radio * 100:.2f} %")
ordenado, medido = 360.0, 340.0                      # giro en el lugar
print(f"Giro en el lugar: b efectivo = {B * ordenado / medido:.2f} cm (nominal {B} cm)")

Sin errores el cuadrado cierra. Con b +2 % el error es de 8,84 cm en ambos sentidos, con orientación de signo opuesto (∓7,06°). Con diámetros +1 %, 18,2 y 19,5 cm y la misma orientación (+15,28°). Combinados: 9,6 cm antihorario y 28,4 cm horario. Con 1 % de deslizamiento aleatorio: 8,2 y 30,0 cm. Cuatro vueltas (16 m): 38 cm y 32° de error. La calibración da 0,97 % de diferencia de diámetros y un ancho efectivo de 15,88 cm.

Probá poner e_b = 0.0 y e_d = 0.0 con sigma = 0.02: el error es aleatorio y distinto para cada semilla. Cambiá la semilla y observá la dispersión. Luego aplicá a mano una corrección que anule uno de los errores sistemáticos y verificá que el cuadrado vuelve a cerrar.

12 · Método y diagnóstico

Pruebas y diagnóstico

1. Con las ruedas en el aire, girá cada una exactamente una vuelta y comprobá que suma las cuentas esperadas y que el desplazamiento calculado es 2π·r.
2. Verificá los signos de los dos conteos y la correspondencia izquierda–derecha.
3. Con el robot apoyado, empujalo a mano 1 m en recta y comparalo con la pose estimada.
4. Calibrá primero los diámetros con la recta larga y después el ancho de vía con giros en el lugar.
5. Repetí el cuadrado en ambos sentidos y registrá el error de posición y de orientación en cada uno.
6. Probá con distintas velocidades y aceleraciones: si el error cambia mucho, hay deslizamiento.
7. Anotá los valores de calibración, la fecha y las condiciones (suelo, batería, carga).

13 · Práctica guiada

Predecí antes de comprobar

1. Distancia por cuenta

Con r = 3,25 cm y CPR = 1440, ¿cuántos cm vale una cuenta, una vuelta de rueda y 1000 cuentas?

2. Un paso de odometría

En un intervalo, la rueda izquierda suma 100 cuentas y la derecha 140. Con b = 15 cm y k = 0,01418 cm, ¿cuánto valen Δs y Δθ?

3. Ancho de vía efectivo

Se ordenan 10 giros completos en el lugar (3600°) y el robot gira 3440° según la marca del suelo. ¿Cuánto vale el ancho de vía efectivo?

4. Diámetros desiguales

Una rueda es 1 % mayor que la otra. ¿Qué error de orientación y qué desvío lateral produce una recta de 5 m, con b = 15 cm?

5. Identificar el error

En un cuadrado en sentido antihorario el robot termina con +8° de error de orientación; en el sentido horario, con −8°. ¿Qué error sistemático domina?

6. Estimar el diámetro

En una recta de 3 m el robot se desvía 20 cm. ¿Qué radio describe y cuánto vale la diferencia de diámetros con b = 15 cm?

7. Resolución de la pose

Con CPR = 48 y r = 3,25 cm, ¿qué distancia representa una cuenta? ¿Cuál es el error máximo de distancia de una rueda por cuantización?

14 · Síntesis

Ideas para recordar

¿Listo para el tema 28?

Antes de avanzar, planificá la calibración de tu robot: qué pruebas harás, con qué instrumentos, qué error aceptarás. Estimá cuánto error de posición tendrías al recorrer 3 m con un 1 % de diferencia de diámetros sin calibrar.

En el próximo tema usamos la pose estimada para navegar punto a punto: avanzar una distancia, girar un ángulo y llegar a coordenadas.